From f90dbf9355f00ead41e6511419076663408ca44e Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@iri.upc.edu>
Date: Fri, 21 Oct 2022 12:25:47 +0200
Subject: [PATCH] ProcessorOdomIcp hotfix
---
src/processor/processor_odom_icp.cpp | 40 +++++++++++++++-------------
1 file changed, 22 insertions(+), 18 deletions(-)
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index b9d82f90e..c9d40a28e 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -140,25 +140,28 @@ unsigned int ProcessorOdomIcp::processKnown()
// update extrinsics (if necessary)
updateExtrinsicsIsometries();
- auto ro_T_ri = computeIsometry2d(odom_origin_, odom_incoming_);
- auto so_T_si = ro_T_so_.inverse() * ro_T_ri * rl_T_sl_;
+ auto rl_T_ri = computeIsometry2d(odom_last_, odom_incoming_);
+ auto sl_T_si = rl_T_sl_.inverse() * rl_T_ri * rl_T_sl_;
+ auto so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
+ auto so_T_si = so_T_sl * sl_T_si;
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(origin_ptr->getScan(),
- incoming_ptr->getScan(),
+ trf_origin_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
+ origin_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);
+ WOLF_DEBUG("ProcessorOdomIcp::processKnown:");
+ WOLF_DEBUG("odom_origin: ", odom_origin_.transpose());
+ WOLF_DEBUG("odom_incoming: ", odom_incoming_.transpose());
+ WOLF_DEBUG("trf_origin_last: ", trf_origin_last_.res_transf.transpose());
+ WOLF_DEBUG("initial guess: ", initial_guess.transpose());
+ WOLF_DEBUG("ICP transform: ", trf_origin_incoming_.res_transf.transpose(), "\n\tvalid: ", trf_origin_incoming_.valid, "\n\terror: ", trf_origin_incoming_.error);
+ WOLF_DEBUG("ICP cov: \n", trf_origin_incoming_.res_covar);
}
return 0;
}
@@ -175,7 +178,7 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
// update extrinsics (if necessary)
updateExtrinsicsIsometries();
- auto rl_T_ri = computeIsometry2d(odom_incoming_, odom_last_);
+ auto rl_T_ri = computeIsometry2d(odom_last_, odom_incoming_);
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();
@@ -183,16 +186,17 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
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(last_ptr->getScan(),
- incoming_ptr->getScan(),
+ trf_last_incoming_ = icp_tools_ptr_->align(incoming_ptr->getScan(),
+ last_ptr->getScan(),
this->laser_scan_params_,
params_odom_icp_->icp_params,
initial_guess);
- WOLF_DEBUG("ProcessorOdomIcp::processNew odom_incoming_: ", odom_incoming_.transpose());
- WOLF_DEBUG("ProcessorOdomIcp::processNew odom_last_: ", odom_last_.transpose());
- WOLF_DEBUG("ProcessorOdomIcp::processNew initial guess: ", initial_guess.transpose());
- WOLF_DEBUG("ProcessorOdomIcp::processNew ICP transform: ", trf_origin_incoming_.res_transf.transpose());
- WOLF_DEBUG("ProcessorOdomIcp::processNew ICP cov: \n", trf_origin_incoming_.res_covar);
+ WOLF_DEBUG("ProcessorOdomIcp::processNew:");
+ WOLF_DEBUG("odom_incoming: ", odom_incoming_.transpose());
+ WOLF_DEBUG("odom_last: ", odom_last_.transpose());
+ WOLF_DEBUG("initial guess: ", initial_guess.transpose());
+ WOLF_DEBUG("ICP transform: ", trf_last_incoming_.res_transf.transpose(), "\n\tvalid: ", trf_last_incoming_.valid, "\n\terror: ", trf_last_incoming_.error);
+ WOLF_DEBUG("ICP cov: \n", trf_last_incoming_.res_covar);
//trf_last_incoming_.valid = trf_last_incoming_.valid && trf_last_incoming_.error / trf_last_incoming_.nvalid < 5e-2;
--
GitLab