Refactor odometry calculation

include/laser/processor/processor_odom_icp.h

@@ -146,6 +146,7 @@ struct ParamsProcessorOdomIcp : public ParamsProcessorTracker
 
 class ProcessorOdomIcp : public ProcessorTracker, public IsMotion
 {
+    void computeOdometry(Eigen::Isometry2d, Eigen::Isometry2d, Eigen::Isometry2d);
     protected:
         // Useful sensor stuff
         SensorLaser2dPtr sensor_laser_; // casted pointer to parent

src/processor/processor_odom_icp.cpp

@@ -176,7 +176,18 @@ inline bool ProcessorOdomIcp::voteForKeyFrameMatchQuality() const
     }
     return vote ;
 }
-
+void ProcessorOdomIcp::computeOdometry(Eigen::Isometry2d map_T_sl, Eigen::Isometry2d so_T_sl, Eigen::Isometry2d so_T_si)
+{
+    double x, y, theta;
+    x = so_T_sl.translation().x();
+    y = so_T_sl.translation().y();
+    theta = Eigen::Rotation2Dd(so_T_sl.linear()).angle();
+    WOLF_INFO("DEBUG ICP LAST TRF ", x, " ", y, " ", theta);
+    Isometry2d map_T_si = map_T_sl * so_T_sl.inverse() * so_T_si;
+    odometry_['P'] = map_T_si.translation();
+    odometry_['O'] = Vector1d(Rotation2Dd(map_T_si.rotation()).angle());
+    WOLF_INFO("DEBUG ICP ODOM ", odometry_.vector("PO").transpose());
+}
 void ProcessorOdomIcp::advanceDerived()
 {
     using namespace Eigen;
@@ -193,15 +204,13 @@ void ProcessorOdomIcp::advanceDerived()
     auto       so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
     Isometry2d  w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
     Vector3d x_inc; x_inc << w_T_ri.translation() , Rotation2Dd(w_T_ri.rotation()).angle();
-
+    auto       so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
+    auto map_T_sl = laser::trf2isometry(x_inc);
+    computeOdometry(map_T_sl, so_T_sl, so_T_si);
     // Put the state of incoming in last
     last_ptr_->getFrame()->setState(x_inc, "PO", {2,1});
 
     // computing odometry
-    auto       so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
-    Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
-    odometry_['P'] = sl_T_si.translation();
-    odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
 
     // advance transforms
     trf_origin_last_ = trf_origin_incoming_;
@@ -240,10 +249,8 @@ void ProcessorOdomIcp::resetDerived()
 
         // computing odometry
         auto       so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
-        Isometry2d sl_T_si = so_T_sl.inverse() * so_T_si;
-        odometry_['P'] = sl_T_si.translation();
-        odometry_['O'] = Vector1d(Rotation2Dd(sl_T_si.rotation()).angle());
-
+        auto map_T_sl = laser::trf2isometry(x_current);
+        computeOdometry(map_T_sl, so_T_sl, so_T_si);
         // advance transforms
         trf_origin_last_ = trf_last_incoming_;
     }