diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 723a198faa4156e39758ec20b080dc944f2b88b7..c9d40a28e705bfd6e5bb4ce43cd13188498a8313 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,10 +140,12 @@ 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 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'");
@@ -152,11 +155,13 @@ unsigned int ProcessorOdomIcp::processKnown()
                                                      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 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;
 }
@@ -173,10 +178,10 @@ 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_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();
     }
     else if (params_odom_icp_->initial_guess != "zero")
       throw std::runtime_error("unknown value for param 'initial_guess'. Should be 'odom', 'state' or 'zero'");
@@ -186,11 +191,12 @@ unsigned int ProcessorOdomIcp::processNew(const int& _max_features)
                                                 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;