From a677a084ef3f3c1c39dac4e556b4616a8922c805 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 20 Oct 2022 17:13:08 +0200
Subject: [PATCH] bug fixed in odom_icp
---
src/processor/processor_odom_icp.cpp | 26 ++++++++++++++------------
1 file changed, 14 insertions(+), 12 deletions(-)
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 723a198fa..b9d82f90e 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -19,6 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+#include "laser/internal/config.h"
#include "laser/processor/processor_odom_icp.h"
#include "laser/math/laser_tools.h"
#include "laser/capture/capture_laser_2d.h"
@@ -139,21 +140,22 @@ unsigned int ProcessorOdomIcp::processKnown()
// update extrinsics (if necessary)
updateExtrinsicsIsometries();
- auto ri_T_ro = computeIsometry2d(odom_incoming_, odom_origin_);
- auto si_T_so = rl_T_sl_.inverse() * ri_T_ro * ro_T_so_;
- initial_guess.head<2>() = si_T_so.translation();
- initial_guess(2) = Rotation2Dd(si_T_so.rotation()).angle();
+ auto ro_T_ri = computeIsometry2d(odom_origin_, odom_incoming_);
+ auto so_T_si = ro_T_so_.inverse() * ro_T_ri * rl_T_sl_;
+ initial_guess.head<2>() = so_T_si.translation();
+ initial_guess(2) = Rotation2Dd(so_T_si.rotation()).angle();
}
else if (params_odom_icp_->initial_guess != "zero")
throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
- trf_origin_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
- origin_ptr->getScan(),
+ trf_origin_incoming_ = icp_tools_ptr_->align(origin_ptr->getScan(),
+ incoming_ptr->getScan(),
this->laser_scan_params_,
params_odom_icp_->icp_params,
initial_guess);
WOLF_DEBUG("ProcessorOdomIcp::processKnown odom_origin_: ", odom_origin_.transpose());
WOLF_DEBUG("ProcessorOdomIcp::processKnown odom_incoming_: ", odom_incoming_.transpose());
+ WOLF_DEBUG("ProcessorOdomIcp::processKnown trf_origin_last_.res_transf: ", trf_origin_last_.res_transf.transpose());
WOLF_DEBUG("ProcessorOdomIcp::processKnown initial guess: ", initial_guess.transpose());
WOLF_DEBUG("ProcessorOdomIcp::processKnown ICP transform: ", trf_origin_incoming_.res_transf.transpose());
WOLF_DEBUG("ProcessorOdomIcp::processKnown ICP cov: \n", trf_origin_incoming_.res_covar);
@@ -173,16 +175,16 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
// update extrinsics (if necessary)
updateExtrinsicsIsometries();
- auto ri_T_rl = computeIsometry2d(odom_incoming_, odom_last_);
- auto si_T_sl = rl_T_sl_.inverse() * ri_T_rl * rl_T_sl_;
- initial_guess.head<2>() = si_T_sl.translation();
- initial_guess(2) = Rotation2Dd(si_T_sl.rotation()).angle();
+ auto rl_T_ri = computeIsometry2d(odom_incoming_, odom_last_);
+ auto sl_T_si = rl_T_sl_.inverse() * rl_T_ri * rl_T_sl_;
+ initial_guess.head<2>() = sl_T_si.translation();
+ initial_guess(2) = Rotation2Dd(sl_T_si.rotation()).angle();
}
else if (params_odom_icp_->initial_guess != "zero")
throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
- trf_last_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
- last_ptr->getScan(),
+ trf_last_incoming_ = icp_tools_ptr_->align(last_ptr->getScan(),
+ incoming_ptr->getScan(),
this->laser_scan_params_,
params_odom_icp_->icp_params,
initial_guess);
--
GitLab