diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index de4d764cd387c659d19261c2da9a059c4566d7c4..0ae859483afd1b78fb2fac5723cb4c656f5aee40 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -70,12 +70,20 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker
class ProcessorOdomICP : public ProcessorTracker
{
protected:
- laserscanutils::icpOutput origin_last_;
- laserscanutils::icpOutput origin_incoming_;
- laserscanutils::icpOutput last_incoming_;
+ // Useful sensor stuff
+ SensorLaser2DPtr sensor_laser_; // casted pointer to parent
+ laserscanutils::LaserScanParams laser_scan_params_;
+
+ // ICP algorithm
std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
+ // temporary and carry-on transformations
+ laserscanutils::icpOutput trf_origin_last_;
+ laserscanutils::icpOutput trf_origin_incoming_;
+ laserscanutils::icpOutput trf_last_incoming_;
+
+
public:
ProcessorOdomICP(ProcessorParamsOdomICPPtr _params);
virtual ~ProcessorOdomICP();
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index d0f038a085ad3ce4ed84afc7bc2e79bb5f4d53de..caecbc842aed8f87dbcb9ac2a129cf1da42e2933 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -21,9 +21,9 @@ ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params):
icp_tools_ptr_ = std::make_shared<ICP>();
// Frame transforms
- origin_last_.res_covar = Eigen::Matrix3s::Identity();
- origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
- last_incoming_.res_covar = Eigen::Matrix3s::Identity();
+ trf_origin_last_.res_covar = Eigen::Matrix3s::Identity();
+ trf_origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
+ trf_last_incoming_.res_covar = Eigen::Matrix3s::Identity();
}
ProcessorOdomICP::~ProcessorOdomICP()
@@ -37,12 +37,14 @@ unsigned int ProcessorOdomICP::processKnown()
if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) //FIRST_TIME
{
- CaptureLaser2DPtr origin_ptr = std::static_pointer_cast<CaptureLaser2D>(origin_ptr_);
- CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
- SensorLaser2DPtr laser_sen = std::static_pointer_cast<SensorLaser2D>(this->getSensor());
-
- laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams();
- origin_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), origin_ptr->getScan(), scan_params, this->icp_params_, origin_last_.res_transf); // Check order
+ CaptureLaser2DPtr origin_ptr = std::static_pointer_cast<CaptureLaser2D>(origin_ptr_);
+ CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
+
+ trf_origin_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(),
+ origin_ptr->getScan(),
+ this->laser_scan_params_,
+ this->icp_params_,
+ this->trf_origin_last_.res_transf); // Check order
}
return 0;
@@ -56,11 +58,12 @@ unsigned int ProcessorOdomICP::processNew(const int& _max_features)
// XXX: Dynamic or static? JS: static is OK.
CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
CaptureLaser2DPtr last_ptr = std::static_pointer_cast<CaptureLaser2D>(last_ptr_);
- SensorLaser2DPtr laser_sen = std::static_pointer_cast<SensorLaser2D >(this->getSensor());
-
- laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams();
- last_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), last_ptr->getScan(), scan_params, this->icp_params_, t_identity);
+ trf_last_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(),
+ last_ptr->getScan(),
+ this->laser_scan_params_,
+ this->icp_params_,
+ t_identity);
return 0;
}
@@ -74,23 +77,23 @@ bool ProcessorOdomICP::voteForKeyFrame()
inline bool ProcessorOdomICP::voteForKeyFrameDistance()
{
- if (origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist)
+ if (trf_origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist)
{
std::cout << "Distance True" << '\n';
}
- return (origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist);
+ return (trf_origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist);
}
inline bool ProcessorOdomICP::voteForKeyFrameAngle()
{
- if (fabs(origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle)
+ if (fabs(trf_origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle)
{
std::cout << "Angle True" << '\n';
}
- return (fabs(origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle);
+ return (fabs(trf_origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle);
}
inline bool ProcessorOdomICP::voteForKeyFrameTime()
@@ -107,17 +110,17 @@ inline bool ProcessorOdomICP::voteForKeyFrameTime()
inline bool ProcessorOdomICP::voteForKeyFrameMatchQuality()
{
- if (origin_incoming_.error > proc_params_->vfk_min_error || origin_incoming_.nvalid < proc_params_->vfk_max_points)
+ if (trf_origin_incoming_.error > proc_params_->vfk_min_error || trf_origin_incoming_.nvalid < proc_params_->vfk_max_points)
{
std::cout << "Quality True" << '\n';
}
- return (origin_incoming_.error > proc_params_->vfk_min_error || origin_incoming_.nvalid < proc_params_->vfk_max_points) ;
+ return (trf_origin_incoming_.error > proc_params_->vfk_min_error || trf_origin_incoming_.nvalid < proc_params_->vfk_max_points) ;
}
void ProcessorOdomICP::advanceDerived()
{
- origin_last_ = origin_incoming_;
+ trf_origin_last_ = trf_origin_incoming_;
}
void ProcessorOdomICP::establishFactors()
@@ -139,7 +142,7 @@ void ProcessorOdomICP::resetDerived()
Eigen::Rotation2D<Scalar> w_R_ro = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2));
Eigen::Rotation2D<Scalar> r_R_s = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0));
Eigen::Rotation2D<Scalar>& ro_R_so = r_R_s;
- Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(origin_last_.res_transf(2));
+ Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(trf_origin_last_.res_transf(2));
// Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*r_R_s*so_R_sl*r_R_s.inverse();
// ro_R_so = rl_R_sl = r_R_s
@@ -149,26 +152,27 @@ void ProcessorOdomICP::resetDerived()
// Translation composition
Eigen::Vector2s w_p_w_ro = origin_ptr_->getFrame()->getState().head(2);
Eigen::Vector2s w_p_ro_so = w_R_ro * origin_ptr_->getSensorP()->getState();
- Eigen::Vector2s w_p_so_sl = w_R_ro * ro_R_so * origin_last_.res_transf.head(2);
+ Eigen::Vector2s w_p_so_sl = w_R_ro * ro_R_so * trf_origin_last_.res_transf.head(2);
Eigen::Vector2s w_p_sl_rl = w_R_rl * (-last_ptr_->getSensorP()->getState());
Eigen::Vector2s w_p_w_rl = w_p_w_ro + w_p_ro_so + w_p_so_sl + w_p_sl_rl;
Eigen::Vector3s curr_state;
curr_state.head(2) = w_p_w_rl;
- curr_state(2) = origin_ptr_->getFrame()->getState()(2) + origin_last_.res_transf(2);
+ curr_state(2) = origin_ptr_->getFrame()->getState()(2) + trf_origin_last_.res_transf(2);
last_ptr_->getFrame()->setState(curr_state);
std::cout << "[KEY FRAME DONE: ]" << '\n';
std::cout << "Current state" << last_ptr_->getFrame()->getState() << '\n';
- origin_last_ = last_incoming_;
+ trf_origin_last_ = trf_last_incoming_;
}
}
void ProcessorOdomICP::configure(SensorBasePtr _sensor)
{
- icp_params_.sigma = std::static_pointer_cast<SensorLaser2D>(getSensor())->getScanParams().range_std_dev_;
+ sensor_laser_ = std::static_pointer_cast<SensorLaser2D>(getSensor());
+ laser_scan_params_ = sensor_laser_->getScanParams();
}
void ProcessorOdomICP::preProcess()
@@ -209,7 +213,7 @@ FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
{
CaptureLaser2DPtr capture_laser = std::static_pointer_cast<CaptureLaser2D>(_capture_laser);
return FeatureBase::emplace<FeatureICPAlign>(capture_laser,
- origin_last_);
+ trf_origin_last_);
}
FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)