diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp index a3669c5c16c426eee51e24aa8d946081f0f12400..c717323570c1bd5051dcc12bbf050a5b92a4f972 100644 --- a/src/processor/processor_odom_icp.cpp +++ b/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());