ProcessorOdomIcp hotfix

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;