diff --git a/include/laser/processor/processor_odom_icp.h b/include/laser/processor/processor_odom_icp.h
index e06ddbc550852c400748504d228c35a908c60d03..56da0c09150ff9927cc0d706ebdcb6d49b53ad0b 100644
--- a/include/laser/processor/processor_odom_icp.h
+++ b/include/laser/processor/processor_odom_icp.h
@@ -69,11 +69,9 @@ struct ProcessorParamsOdomICP : public ProcessorParamsTracker
 class ProcessorOdomICP : public ProcessorTracker
 {
     protected:
-        Eigen::Vector3s t_origin_last_;
-        Eigen::Vector3s t_origin_incoming_;
-        Eigen::Vector3s t_last_incoming_;
-
-        laserscanutils::icpOutput icp_match_res_;
+        laserscanutils::icpOutput origin_last_;
+        laserscanutils::icpOutput origin_incoming_;
+        laserscanutils::icpOutput last_incoming_;
 
         std::shared_ptr<laserscanutils::ICP> icp_tools_ptr_;
 
@@ -99,7 +97,7 @@ class ProcessorOdomICP : public ProcessorTracker
 
         virtual void preProcess() override;
 
-        FeatureBasePtr emplaceFeature(CaptureLaser2DPtr _capture_laser);
+        FeatureBasePtr emplaceFeature(CaptureBasePtr _capture_laser);
 
         FactorBasePtr emplaceFactor(FeatureBasePtr _feature);
 
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index 7974ced1087c195fb06ff207f437b655e00853f9..e96debd03fcf2bcbf6f2453f7c25e9fe50989421 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -6,12 +6,13 @@ using namespace laserscanutils;
 ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params):
                     ProcessorTracker("ODOM ICP", _params)
 {
-    t_origin_incoming_ << 0, 0, 0;
-    t_origin_last_     << 0, 0, 0;
-    t_last_incoming_   << 0, 0, 0;
-
     proc_params_ = _params;
 
+    origin_last_.res_covar = Eigen::Matrix3s::Identity();
+    origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
+    last_incoming_.res_covar = Eigen::Matrix3s::Identity();
+
+
     icp_params_.use_point_to_line_distance = proc_params_->use_point_to_line_distance;
     icp_params_.max_correspondence_dist = proc_params_->max_correspondence_dist;
     icp_params_.max_iterations = proc_params_->max_iterations;
@@ -31,17 +32,14 @@ unsigned int ProcessorOdomICP::processKnown()
 {
     // Match the incoming with the origin, passing the transform from origin to last as initialization
 
-    if (processing_step_ == RUNNING_WITH_PACK || processing_step_ == RUNNING_WITHOUT_PACK)
+    if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK) //FIRST_TIME
     {
-        // XXX: Dynamic or static?
         CaptureLaser2DPtr origin_ptr = std::static_pointer_cast<CaptureLaser2D>(origin_ptr_);
         CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
         SensorLaser2DPtr laser_sen = std::static_pointer_cast<SensorLaser2D>(this->getSensor());
 
         laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams();
-        icp_match_res_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), origin_ptr->getScan(), scan_params, this->icp_params_, t_origin_last_);
-
-        t_origin_incoming_ = icp_match_res_.res_transf;
+        origin_incoming_ = icp_tools_ptr_->matchPC(incoming_ptr->getScan(), origin_ptr->getScan(), scan_params, this->icp_params_, origin_last_.res_transf); // Check order
     }
 
     return 0;
@@ -56,12 +54,10 @@ unsigned int ProcessorOdomICP::processNew(const int& _max_features)
     CaptureLaser2DPtr incoming_ptr = std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_);
     CaptureLaser2DPtr last_ptr = std::static_pointer_cast<CaptureLaser2D>(last_ptr_);
     SensorLaser2DPtr laser_sen = std::static_pointer_cast<SensorLaser2D>(this->getSensor());
-    laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams();
 
-    icp_match_res_ =  icp_tools_ptr_->matchPC(incoming_ptr->getScan(), last_ptr->getScan(), scan_params, this->icp_params_, t_identity);
-
-    t_origin_last_ = icp_match_res_.res_transf;
+    laserscanutils::LaserScanParams scan_params = laser_sen->getScanParams();
 
+    last_incoming_ =  icp_tools_ptr_->matchPC(incoming_ptr->getScan(), last_ptr->getScan(), scan_params, this->icp_params_, t_identity);
     return 0;
 }
 
@@ -75,23 +71,23 @@ bool ProcessorOdomICP::voteForKeyFrame()
 
 inline bool ProcessorOdomICP::voteForKeyFrameDistance()
 {
-  if (icp_match_res_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist)
+  if (origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist)
   {
     std::cout << "Distance True" << '\n';
   }
 
-    return (icp_match_res_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist);
+    return (origin_incoming_.res_transf.head<2>().norm() > proc_params_->vfk_min_dist);
 }
 
 inline bool ProcessorOdomICP::voteForKeyFrameAngle()
 {
 
-  if (fabs(icp_match_res_.res_transf(2)) > proc_params_->vfk_min_angle)
+  if (fabs(origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle)
   {
     std::cout << "Angle True" << '\n';
   }
 
-    return (fabs(icp_match_res_.res_transf(2)) > proc_params_->vfk_min_angle);
+    return (fabs(origin_incoming_.res_transf(2)) > proc_params_->vfk_min_angle);
 }
 
 inline bool ProcessorOdomICP::voteForKeyFrameTime()
@@ -108,52 +104,61 @@ inline bool ProcessorOdomICP::voteForKeyFrameTime()
 
 inline bool ProcessorOdomICP::voteForKeyFrameMatchQuality()
 {
-  if  (icp_match_res_.error > proc_params_->vfk_min_error || icp_match_res_.nvalid < proc_params_->vfk_max_points)
+  if  (origin_incoming_.error > proc_params_->vfk_min_error || origin_incoming_.nvalid < proc_params_->vfk_max_points)
   {
     std::cout << "Quality True" << '\n';
   }
-    return (icp_match_res_.error > proc_params_->vfk_min_error || icp_match_res_.nvalid < proc_params_->vfk_max_points) ;
+    return (origin_incoming_.error > proc_params_->vfk_min_error || origin_incoming_.nvalid < proc_params_->vfk_max_points) ;
 }
 
 
 void ProcessorOdomICP::advanceDerived()
 {
-    t_origin_last_ = t_origin_incoming_;
+    origin_last_ = origin_incoming_;
 }
 
 void ProcessorOdomICP::establishFactors()
 {
-  CaptureLaser2DPtr last_ptr = std::static_pointer_cast<CaptureLaser2D>(last_ptr_);
-  auto ftr_ptr = emplaceFeature(last_ptr);
+  auto ftr_ptr = emplaceFeature(last_ptr_);
   emplaceFactor(ftr_ptr);
 }
 
 void ProcessorOdomICP::resetDerived()
 {
     // Using processing_step_ to ensure that origin, last and incoming are different
-    if (processing_step_ == RUNNING_WITH_PACK || processing_step_ == RUNNING_WITHOUT_PACK)
+    if (processing_step_ != FIRST_TIME_WITH_PACK && processing_step_ != FIRST_TIME_WITHOUT_PACK)
     {
+        // Notation explanation:
+        // x1_R_x2: Rotation from frame x1 to frame x2
+        // x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
+
         // Rotation composition
-        Eigen::Rotation2D<Scalar> r_world_origin  = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2));
-        Eigen::Rotation2D<Scalar> r_robot_sensor  = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0));
-        Eigen::Rotation2D<Scalar> r_sorigin_slast = Eigen::Rotation2D<Scalar>(t_origin_last_(2));
+        Eigen::Rotation2D<Scalar> w_R_ro  = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2));
+        Eigen::Rotation2D<Scalar> r_R_s   = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0));
+        Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(origin_last_.res_transf(2));
 
-        Eigen::Rotation2D<Scalar> r_world_last = r_world_origin*r_robot_sensor*r_sorigin_slast*r_robot_sensor.inverse();
+        // Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*r_R_s*so_R_sl*r_R_s.inverse();
+        // ro_R_so = rl_R_sl = r_R_s
+        // Planar rotations are commutative
+        Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*so_R_sl;
 
         // Translation composition
-        Eigen::Vector2s p_world_origin_w        = origin_ptr_->getFrame()->getState().head(2);
-        Eigen::Vector2s p_robotorigin_sensor_w  = r_world_origin*origin_ptr_->getSensorP()->getState();
-        Eigen::Vector2s p_sorigin_slast_w       = r_world_origin*r_robot_sensor*t_origin_last_.head(2);
-        Eigen::Vector2s p_sensor_robotlast_w    = r_world_last*-origin_ptr_->getSensorP()->getState();
+        Eigen::Vector2s w_p_w_ro = origin_ptr_->getFrame()->getState().head(2);
+        Eigen::Vector2s w_p_ro_so = w_R_ro*origin_ptr_->getSensorP()->getState();
+        Eigen::Vector2s w_p_so_sl = w_R_ro*r_R_s*origin_last_.res_transf.head(2);
+        Eigen::Vector2s w_p_sl_rl = w_R_rl*(-last_ptr_->getSensorP()->getState());
 
-        Eigen::Vector2s p_world_last_w = p_world_origin_w + p_robotorigin_sensor_w
-                                          + p_sorigin_slast_w + p_sensor_robotlast_w;
+        Eigen::Vector2s w_p_w_rl = w_p_w_ro + w_p_ro_so + w_p_so_sl + w_p_sl_rl;
 
         Eigen::Vector3s curr_state;
-        curr_state.head(2) = p_world_last_w;
-        curr_state(2) = origin_ptr_->getFrame()->getState()(2) + t_origin_last_(2);
+        curr_state.head(2) = w_p_w_rl;
+        curr_state(2) = origin_ptr_->getFrame()->getState()(2) + origin_last_.res_transf(2);
 
         last_ptr_->getFrame()->setState(curr_state);
+        std::cout << "[KEY FRAME DONE: ]" << '\n';
+        std::cout << "Current state" << last_ptr_->getFrame()->getState() << '\n';
+
+        origin_last_ = last_incoming_;
     }
 }
 
@@ -196,10 +201,11 @@ ProcessorBasePtr ProcessorOdomICP::createAutoConf(const std::string& _unique_nam
     return prc_ptr;
 }
 
-FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureLaser2DPtr _capture_laser)
+FeatureBasePtr ProcessorOdomICP::emplaceFeature(CaptureBasePtr _capture_laser)
 {
-    return FeatureBase::emplace<FeatureICPAlign>(_capture_laser,
-                                                icp_match_res_);
+    CaptureLaser2DPtr capture_laser = std::static_pointer_cast<CaptureLaser2D>(_capture_laser);
+    return FeatureBase::emplace<FeatureICPAlign>(capture_laser,
+                                                origin_last_);
 }
 
 FactorBasePtr ProcessorOdomICP::emplaceFactor(FeatureBasePtr _feature)