diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index c012b9839644d5ce53d347c79f16401c97ce82f2..723a198faa4156e39758ec20b080dc944f2b88b7 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -258,9 +258,9 @@ void ProcessorOdomIcp::advanceDerived()
 
     odom_last_ = odom_incoming_;
 
-    // init odometry_
-    if (odometry_.empty())
-        odometry_ = getProblem()->stateZero("PO");
+    // init odometry
+    if (getOdometry().empty())
+        setOdometry(getProblem()->stateZero("PO"));
 
     // update extrinsics (if necessary)
     updateExtrinsicsIsometries();
@@ -271,12 +271,13 @@ void ProcessorOdomIcp::advanceDerived()
     Isometry2d rl_T_ri = rl_T_sl_ * so_T_sl.inverse() * so_T_si * rl_T_sl_.inverse();
     
     // 2D
+    auto odometry = getOdometry();
     if (getProblem()->getDim() == 2)
     {
-        auto    m_T_rl = laser::trf2isometry(odometry_['P'], odometry_['O']);
+        auto    m_T_rl = laser::trf2isometry(odometry['P'], odometry['O']);
         auto    m_T_ri = m_T_rl * rl_T_ri;
-        odometry_['P'] = m_T_ri.translation();
-        odometry_['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
+        odometry['P'] = m_T_ri.translation();
+        odometry['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
     }
     // 3D
     else
@@ -285,10 +286,11 @@ void ProcessorOdomIcp::advanceDerived()
 
         convert2dTo3d(rl_T_ri, rl_T3d_ri);
         
-        SE3::compose(odometry_['P'], odometry_['O'], 
+        SE3::compose(odometry['P'], odometry['O'], 
                      rl_T3d_ri.head<3>(), rl_T3d_ri.tail<4>(), 
-                     odometry_['P'], odometry_['O']);
+                     odometry['P'], odometry['O']);
     }
+    setOdometry(odometry);
 
     // advance transforms
     trf_origin_last_ = trf_origin_incoming_;
@@ -299,24 +301,25 @@ void ProcessorOdomIcp::resetDerived()
     // Using processing_step_ to ensure that origin, last and incoming are different
     if (processing_step_ != FIRST_TIME_WITH_KEYFRAME && processing_step_ != FIRST_TIME_WITHOUT_KEYFRAME)
     {
-        // init odometry_
-        if (odometry_.empty())
-            odometry_ = getProblem()->stateZero("PO");
+        // init odometry
+        if (getOdometry().empty())
+            setOdometry(getProblem()->stateZero("PO"));
 
         // update extrinsics (if necessary)
         updateExtrinsicsIsometries();
 
         // computing odometry
+        auto odometry = getOdometry();
         auto       so_T_si = laser::trf2isometry(trf_origin_incoming_.res_transf);
         auto       so_T_sl = laser::trf2isometry(trf_origin_last_.res_transf);
         Isometry2d rl_T_ri = rl_T_sl_ * so_T_sl.inverse() * so_T_si * rl_T_sl_.inverse();
         // 2D
         if (getProblem()->getDim() == 2)
         {
-            auto m_T_rl = laser::trf2isometry(odometry_['P'], odometry_['O']);
+            auto m_T_rl = laser::trf2isometry(odometry['P'], odometry['O']);
             auto m_T_ri = m_T_rl * rl_T_ri;
-            odometry_['P'] = m_T_ri.translation();
-            odometry_['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
+            odometry['P'] = m_T_ri.translation();
+            odometry['O'] = Vector1d(Rotation2Dd(m_T_ri.rotation()).angle());
         }
         // 3D
         else
@@ -325,10 +328,11 @@ void ProcessorOdomIcp::resetDerived()
 
             convert2dTo3d(rl_T_ri, rl_T3d_ri);
             
-            SE3::compose(odometry_['P'], odometry_['O'], 
+            SE3::compose(odometry['P'], odometry['O'], 
                          rl_T3d_ri.head<3>(), rl_T3d_ri.tail<4>(), 
-                         odometry_['P'], odometry_['O']);
+                         odometry['P'], odometry['O']);
         }
+        setOdometry(odometry);
 
         // advance transforms
         trf_origin_last_ = trf_last_incoming_;