diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index 20dde002124334b5c64cf8241cd3fbdbba92a3e0..e208e3e27563d18693a4116ea9c6006f34a68ac0 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -88,6 +88,7 @@ unsigned int ProcessorOdomIcp3d::processKnown()
 {
     if (origin_ptr_ && (incoming_ptr_ != origin_ptr_))
     {
+        origin_laser_ = std::static_pointer_cast<CaptureLaser3d>(origin_ptr_);
         pairAlign(origin_laser_->getPointCloud(),
                   incoming_laser_->getPointCloud(),
                   T_origin_last_,
@@ -102,11 +103,15 @@ unsigned int ProcessorOdomIcp3d::processKnown()
  */
 unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
 {
-    pairAlign(last_laser_->getPointCloud(),
-              incoming_laser_->getPointCloud(),
-              Eigen::Isometry3d::Identity(),
-              T_last_incoming_,
-              registration_solver_);
+    if (last_ptr_)
+    {
+        last_laser_ = std::static_pointer_cast<CaptureLaser3d>(last_ptr_);
+        pairAlign(last_laser_->getPointCloud(),
+                  incoming_laser_->getPointCloud(),
+                  Eigen::Isometry3d::Identity(),
+                  T_last_incoming_,
+                  registration_solver_);
+    }
     return 0;
 };
 
@@ -123,6 +128,10 @@ bool ProcessorOdomIcp3d::voteForKeyFrame() const
     {
         return true;
     }
+    // TODO:
+    //  - vote by distance
+    //  - vote by angle
+    //  - vote by nbr. of captures
     return false;
 };
 
@@ -156,13 +165,13 @@ TimeStamp ProcessorOdomIcp3d::getTimeStamp() const
 
 VectorComposite ProcessorOdomIcp3d::getState(const StateStructure& _structure) const
 {
-    VectorComposite   state_origin = origin_laser_->getFrame()->getState("PO");
+    VectorComposite   state_origin = origin_ptr_->getFrame()->getState("PO");
     Eigen::Isometry3d T_world_origin_robot =
         Eigen::Translation3d(state_origin.at('P')) * Eigen::Quaterniond(state_origin.at('O').data());
     Eigen::Isometry3d T_world_last_robot = T_world_origin_robot * T_robot_sensor_ * T_origin_last_ * T_sensor_robot_;
     VectorComposite   state_last;
-    state_last.at('P') = T_world_last_robot.translation();
-    state_last.at('O') = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs();
+    state_last['P'] = T_world_last_robot.translation();
+    state_last['O'] = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs();
     return state_last;
 }
 
@@ -177,16 +186,19 @@ VectorComposite ProcessorOdomIcp3d::getState(const TimeStamp& _ts, const StateSt
  */
 void ProcessorOdomIcp3d::establishFactors()
 {
-    // emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
-    Eigen::Vector7d measurement_of_motion;
-    measurement_of_motion.head(3) = T_origin_last_.translation();
-    measurement_of_motion.tail(4) = Eigen::Quaterniond(T_origin_last_.rotation()).coeffs();
-    auto feature =
-        FeatureBase::emplace<FeatureBase>(last_laser_, "FeatureBase", measurement_of_motion, transform_cov_);
-
-    // emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above
-    auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
-        feature, feature, origin_laser_->getFrame(), shared_from_this(), false, TOP_MOTION);
+    if (last_ptr_ != origin_ptr_) // skip if it's the same frame (it happens the SECOND_TIME)
+    {
+        // emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_
+        Eigen::Vector7d measurement_of_motion;
+        measurement_of_motion.head(3) = T_origin_last_.translation();
+        measurement_of_motion.tail(4) = Eigen::Quaterniond(T_origin_last_.rotation()).coeffs();
+        auto feature =
+            FeatureBase::emplace<FeatureBase>(last_ptr_, "FeatureBase", measurement_of_motion, transform_cov_);
+
+        // emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above
+        auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(
+            feature, feature, origin_ptr_->getFrame(), shared_from_this(), false, TOP_MOTION);
+    }
 };
 
 }  // namespace wolf