WIP

src/processor/processor_odom_icp.cpp

@@ -183,7 +183,8 @@ void ProcessorOdomIcp::computeOdometry(Eigen::Isometry2d map_T_sl, Eigen::Isomet
     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;
+    // 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());
     WOLF_INFO("DEBUG ICP ODOM ", odometry_.vector("PO").transpose());