diff --git a/src/processor_capture_holder.cpp b/src/processor_capture_holder.cpp
index cc34d1d2f42a271f57d818fbfb0ade5cc8088109..398f829aa397888878ed7a0ca4fee73080740315 100644
--- a/src/processor_capture_holder.cpp
+++ b/src/processor_capture_holder.cpp
@@ -90,7 +90,7 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time
 //    {
 //      // go to the previous motion capture
 //      if (capture_ptr == last_ptr_)
-//        capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
+//        capture_ptr = origin_ptr_;
 //      else if (capture_ptr->getOriginFramePtr() == nullptr)
 //        return nullptr;
 //      else
diff --git a/src/processor_motion.cpp b/src/processor_motion.cpp
index 63cf1e0747917255b71ad285d4584b5f34a2b77e..b9d6d3218072c8cf8aa23af26c54b5bd560c2480 100644
--- a/src/processor_motion.cpp
+++ b/src/processor_motion.cpp
@@ -307,7 +307,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
         {
             // go to the previous motion capture
             if (capture_ptr == last_ptr_)
-                capture_ptr = std::static_pointer_cast<CaptureMotion>(origin_ptr_);
+                capture_ptr = origin_ptr_;
             else if (capture_ptr->getOriginFramePtr() == nullptr)
                 return nullptr;
             else
diff --git a/src/processor_motion.h b/src/processor_motion.h
index bc12469728c66b6d9e57d0b4ed789cde7b04a923..2c0c776ae17fb4c4fefaf6426027e843f5ca4591 100644
--- a/src/processor_motion.h
+++ b/src/processor_motion.h
@@ -400,7 +400,7 @@ class ProcessorMotion : public ProcessorBase
         CaptureMotionPtr getCaptureMotionContainingTimeStamp(const TimeStamp& _ts);
 
     public:
-        virtual CaptureBasePtr getOriginPtr();
+        virtual CaptureMotionPtr getOriginPtr();
         virtual CaptureMotionPtr getLastPtr();
         virtual CaptureMotionPtr getIncomingPtr();
 
@@ -412,7 +412,7 @@ class ProcessorMotion : public ProcessorBase
         Size delta_size_;       ///< the size of the deltas
         Size delta_cov_size_;   ///< the size of the delta covariances matrix
         Size calib_size_;       ///< size of the extra parameters (TBD in derived classes)
-        CaptureBasePtr origin_ptr_;
+        CaptureMotionPtr origin_ptr_;
         CaptureMotionPtr last_ptr_;
         CaptureMotionPtr incoming_ptr_;
 
@@ -534,7 +534,7 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
     );
 }
 
-inline CaptureBasePtr ProcessorMotion::getOriginPtr()
+inline CaptureMotionPtr ProcessorMotion::getOriginPtr()
 {
     return origin_ptr_;
 }
diff --git a/src/sensors/sensor_diff_drive.cpp b/src/sensors/sensor_diff_drive.cpp
index 52e4177c7c6896c7da6b421194ffbc8278eb8e00..0931b700ce658b486823fa930e3f1c454f87be5e 100644
--- a/src/sensors/sensor_diff_drive.cpp
+++ b/src/sensors/sensor_diff_drive.cpp
@@ -103,24 +103,22 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
 // problem->installSensor() return a SensorBasePtr.
 //bool SensorDiffDrive::addCapture(CaptureBasePtr _capture_ptr)
 //{
-//  std::shared_ptr<CaptureMotion> capture_ptr = std::static_pointer_cast<CaptureMotion>(_capture_ptr);
-
 //  if (intrinsics_.data_is_position_)
 //  {
-//    Eigen::Vector2s data = capture_ptr->getData();
+//    Eigen::Vector2s data = _capture_ptr->getData();
 
 //    // dt is set to one as we are dealing with wheel position
 //    data = pose_inc_(data, intrinsics_.left_radius_, intrinsics_.right_radius_,
 //                     intrinsics_.separation_, 1);
 
-//    capture_ptr->setData(data);
+//    _capture_ptr->setData(data);
 
 //    Eigen::Matrix2s data_cov;
 //    data_cov << 0.00001, 0, 0, 0.00001; // Todo
 
 //    computeDataCov(data, data_cov);
 
-//    capture_ptr->setDataCovariance(data_cov);
+//    _capture_ptr->setDataCovariance(data_cov);
 //  }
 
 //  /// @todo tofix
diff --git a/src/test/gtest_constraint_IMU.cpp b/src/test/gtest_constraint_IMU.cpp
index 3accb792b20b23f5f7d5e6b31fbf0a7dfd7fd662..4c4c85369af796d32bcb6d9f0d207957a7006905 100644
--- a/src/test/gtest_constraint_IMU.cpp
+++ b/src/test/gtest_constraint_IMU.cpp
@@ -979,9 +979,9 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
 
                 sensor_odo->process(capture_odo);
 
-//                WOLF_TRACE("Jac calib: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getJacobianCalib().row(0));
+//                WOLF_TRACE("Jac calib: ", processor_imu->getOriginPtr()->getJacobianCalib().row(0));
 //                WOLF_TRACE("orig calib: ", processor_imu->getOriginPtr()->getCalibration().transpose());
-//                WOLF_TRACE("orig calib preint: ", std::static_pointer_cast<CaptureMotion>(processor_imu->getOriginPtr())->getCalibrationPreint().transpose());
+//                WOLF_TRACE("orig calib preint: ", processor_imu->getOriginPtr()->getCalibrationPreint().transpose());
 
                 //prepare next odometry measurement
                 quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
diff --git a/src/test/gtest_processor_IMU.cpp b/src/test/gtest_processor_IMU.cpp
index f8aac4c30823043472aa73614012bd55cedf4287..7d8e3e549fa7a6e7d105184a6799f24608c0c10b 100644
--- a/src/test/gtest_processor_IMU.cpp
+++ b/src/test/gtest_processor_IMU.cpp
@@ -437,7 +437,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
     // init things
     problem->setPrior(x0, P0, t, 0.01);
 
-    std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
+    problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
     problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
 //    WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
 
@@ -493,7 +493,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
     Vector6s bias; bias << abx,aby,0,  0,0,0;
     Vector3s acc_bias = bias.head(3);
 
-    std::static_pointer_cast<wolf::CaptureMotion>(problem->getProcessorMotionPtr()->getOriginPtr())->setCalibration(bias);
+    problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
     problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
 
     wolf::Scalar rate_of_turn = 5 * M_PI/180.0;