work on advanceDerived()

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);
 
 
 

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