From 4307d83cd7a61857561d0fc98bd95697cb40d493 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 23 Sep 2020 16:02:49 +0200
Subject: [PATCH] WIP
---
src/processor/processor_odom_icp.cpp | 3 ++-
1 file changed, 2 insertions(+), 1 deletion(-)
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index a3669c5c1..c71732357 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());
--
GitLab