Rollback map transformation to odometry

include/laser/processor/processor_odom_icp.h

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

src/processor/processor_odom_icp.cpp

@@ -176,7 +176,7 @@ 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)
+void ProcessorOdomIcp::computeOdometry(Eigen::Isometry2d so_T_sl, Eigen::Isometry2d so_T_si)
 {
     double x, y, theta;
     x = so_T_sl.translation().x();
@@ -184,9 +184,9 @@ void ProcessorOdomIcp::computeOdometry(Eigen::Isometry2d map_T_sl, Eigen::Isomet
     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;
-    Isometry2d map_T_si = so_T_sl.inverse() * so_T_si;
-    odometry_['P'] = map_T_si.translation();
-    odometry_['O'] = Vector1d(Rotation2Dd(map_T_si.rotation()).angle());
+    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());
     WOLF_INFO("DEBUG ICP ODOM ", odometry_.vector("PO").transpose());
 }
 void ProcessorOdomIcp::advanceDerived()
@@ -206,8 +206,8 @@ void ProcessorOdomIcp::advanceDerived()
     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);
+    // auto map_T_sl = laser::trf2isometry(x_inc);
+    computeOdometry(so_T_sl, so_T_si);
     // Put the state of incoming in last
     last_ptr_->getFrame()->setState(x_inc, "PO", {2,1});
 
@@ -250,8 +250,8 @@ void ProcessorOdomIcp::resetDerived()
 
         // computing odometry
         auto       so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
-        auto map_T_sl = laser::trf2isometry(x_current);
-        computeOdometry(map_T_sl, so_T_sl, so_T_si);
+        // auto map_T_sl = laser::trf2isometry(x_current);
+        computeOdometry(so_T_sl, so_T_si);
         // advance transforms
         trf_origin_last_ = trf_last_incoming_;
     }