diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index 0ae859483afd1b78fb2fac5723cb4c656f5aee40..5c6c16e25c2db253a23a52b07184997df8ca94a1 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -70,7 +70,6 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker
class ProcessorOdomICP : public ProcessorTracker
{
protected:
-
// Useful sensor stuff
SensorLaser2DPtr sensor_laser_; // casted pointer to parent
laserscanutils::LaserScanParams laser_scan_params_;
@@ -83,31 +82,31 @@ class ProcessorOdomICP : public ProcessorTracker
laserscanutils::icpOutput trf_origin_incoming_;
laserscanutils::icpOutput trf_last_incoming_;
+ public:
+ laserscanutils::icpParams icp_params_;
+ ProcessorParamsOdomICPPtr proc_params_;
public:
ProcessorOdomICP(ProcessorParamsOdomICPPtr _params);
virtual ~ProcessorOdomICP();
-
- laserscanutils::icpParams icp_params_;
- ProcessorParamsOdomICPPtr proc_params_;
+ static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr);
+ static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr);
+ virtual void configure(SensorBasePtr _sensor) override;
protected:
+ virtual void preProcess() override;
+
virtual unsigned int processKnown() override;
+ virtual unsigned int processNew(const int& _max_features) override;
virtual bool voteForKeyFrame() override;
virtual void advanceDerived() override;
-
- virtual unsigned int processNew(const int& _max_features) override;
-
- virtual void establishFactors() override;
-
virtual void resetDerived() override;
- virtual void preProcess() override;
+ virtual void establishFactors() override;
FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
-
FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
inline bool voteForKeyFrameDistance();
@@ -115,11 +114,6 @@ class ProcessorOdomICP : public ProcessorTracker
inline bool voteForKeyFrameTime();
inline bool voteForKeyFrameMatchQuality();
- public:
- virtual void configure(SensorBasePtr _sensor) override;
-
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr);
- static ProcessorBasePtr createAutoConf(const std::string& _unique_name, const ParamsServer& _server, const SensorBasePtr sensor_ptr);
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 5098018960752bd65352baacfadc76fd5df42f3d..5d17f0bbbc9f9f41b22a3d63dc47df542ac7b17e 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -31,6 +31,21 @@ ProcessorOdomICP::~ProcessorOdomICP()
}
+void ProcessorOdomICP::preProcess()
+{
+
+}
+
+
+
+void ProcessorOdomICP::configure(SensorBasePtr _sensor)
+{
+ sensor_laser_ = std::static_pointer_cast<SensorLaser2D>(_sensor);
+ laser_scan_params_ = sensor_laser_->getScanParams();
+}
+
+
+
unsigned int ProcessorOdomICP::processKnown()
{
// Match the incoming with the origin, passing the transform from origin to last as initialization
@@ -120,20 +135,40 @@ inline bool ProcessorOdomICP::voteForKeyFrameMatchQuality()
void ProcessorOdomICP::advanceDerived()
{
- trf_origin_last_ = trf_origin_incoming_;
-}
+ using namespace Eigen;
-void ProcessorOdomICP::establishFactors()
-{
- auto ftr_ptr = emplaceFeature(last_ptr_);
- emplaceFactor(ftr_ptr);
+ WOLF_TRACE("========================== ADVANCE =================================");
+ WOLF_TRACE("========================== ADVANCE =================================");
+ WOLF_TRACE("========================== ADVANCE =================================");
+ // overwrite last frame's state
+
+ Isometry2ds w_T_ro = Translation2ds(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Ds(origin_ptr_->getFrame()->getState()(2));
+ Isometry2ds ro_T_so = Translation2ds(origin_ptr_->getSensorP()->getState()) * Rotation2Ds(origin_ptr_->getSensorO()->getState()(0));
+
+ // incoming
+
+ Isometry2ds so_T_si = Translation2ds(trf_origin_incoming_.res_transf.head(2)) * Rotation2Ds(trf_origin_incoming_.res_transf(2));
+ Isometry2ds w_T_ri = w_T_ro * ro_T_so * so_T_si * ro_T_so.inverse();
+ Vector3s x_inc; x_inc << w_T_ri.translation() , Rotation2Ds(w_T_ri.rotation()).angle();
+
+ WOLF_TRACE("x_inc ", x_inc.transpose());
+
+ // Put the state of incoming in last
+ last_ptr_->getFrame()->setState(x_inc);
+
+ trf_origin_last_ = trf_origin_incoming_;
}
void ProcessorOdomICP::resetDerived()
{
+
+ WOLF_TRACE("========================== RESET =================================");
+
// Using processing_step_ to ensure that origin, last and incoming are different
if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK)
{
+ WOLF_TRACE("========================== RESET =================================");
+ WOLF_TRACE("========================== RESET =================================");
// Notation explanation:
// x1_R_x2: Rotation from frame x1 to frame x2
// x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
@@ -141,7 +176,7 @@ void ProcessorOdomICP::resetDerived()
// Rotation composition
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>& ro_R_so = r_R_s; // just a ref for bettter chaining trf. names
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();
@@ -161,6 +196,8 @@ void ProcessorOdomICP::resetDerived()
curr_state.head(2) = w_p_w_rl;
curr_state(2) = origin_ptr_->getFrame()->getState()(2) + trf_origin_last_.res_transf(2);
+ WOLF_TRACE("x_last ", curr_state.transpose());
+
last_ptr_->getFrame()->setState(curr_state);
std::cout << "[KEY FRAME DONE: ]" << '\n';
std::cout << "Current state" << last_ptr_->getFrame()->getState() << '\n';
@@ -169,17 +206,28 @@ void ProcessorOdomICP::resetDerived()
}
}
-void ProcessorOdomICP::configure(SensorBasePtr _sensor)
+void ProcessorOdomICP::establishFactors()
{
- sensor_laser_ = std::static_pointer_cast<SensorLaser2D>(getSensor());
- laser_scan_params_ = sensor_laser_->getScanParams();
+ auto ftr_ptr = emplaceFeature(last_ptr_);
+ emplaceFactor(ftr_ptr);
}
-void ProcessorOdomICP::preProcess()
+FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
{
+ CaptureLaser2DPtr capture_laser = std::static_pointer_cast<CaptureLaser2D>(_capture_laser);
+ return FeatureBase::emplace<FeatureICPAlign>(capture_laser,
+ trf_origin_last_);
+}
+FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)
+{
+ return FactorBase::emplace<FactorOdom2D>(_feature, _feature, origin_ptr_->getFrame(),
+ shared_from_this());
}
+
+
+/// FACTORY METHODS -- to be replaced by macro after PR !313 addressing issue #248 is merged.
ProcessorBasePtr ProcessorOdomICP::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
{
ProcessorOdomICPPtr prc_ptr;
@@ -209,19 +257,6 @@ ProcessorBasePtr ProcessorOdomICP::createAutoConf(const std::string& _unique_nam
return prc_ptr;
}
-FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
-{
- CaptureLaser2DPtr capture_laser = std::static_pointer_cast<CaptureLaser2D>(_capture_laser);
- return FeatureBase::emplace<FeatureICPAlign>(capture_laser,
- trf_origin_last_);
-}
-
-FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)
-{
- return FactorBase::emplace<FactorOdom2D>(_feature, _feature, origin_ptr_->getFrame(),
- shared_from_this());
-}
-
// Register in the SensorFactory