bug fixed in odom_icp

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);