diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index d50a728160c9d1d393fd552501a1d3f58a5dbe8f..0f785cdd3d8178a5fb5b194841e76d46547f58cf 100644
--- a/include/core/capture/capture_diff_drive.h
+++ b/include/core/capture/capture_diff_drive.h
@@ -32,7 +32,7 @@ public:
                          const SensorBasePtr& _sensor_ptr,
                          const Eigen::VectorXs& _data,
                          const Eigen::MatrixXs& _data_cov,
-                         FrameBasePtr _origin_frame_ptr,
+                         CaptureBasePtr _capture_origin_ptr,
                          StateBlockPtr _p_ptr = nullptr,
                          StateBlockPtr _o_ptr = nullptr,
                          StateBlockPtr _intr_ptr = nullptr);
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index 4ea8cb172cd8503788125d441c79569a9cf15f75..7fa7e38eb1dbad2579222c2dff247b9614322594 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -52,7 +52,7 @@ class CaptureMotion : public CaptureBase
                       const Eigen::VectorXs& _data,
                       SizeEigen _delta_size,
                       SizeEigen _delta_cov_size,
-                      FrameBasePtr _origin_frame_ptr);
+                      CaptureBasePtr _capture_origin_ptr);
 
         CaptureMotion(const std::string & _type,
                       const TimeStamp& _ts,
@@ -61,7 +61,7 @@ class CaptureMotion : public CaptureBase
                       const Eigen::MatrixXs& _data_cov,
                       SizeEigen _delta_size,
                       SizeEigen _delta_cov_size,
-                      FrameBasePtr _origin_frame_ptr,
+                      CaptureBasePtr _capture_origin_ptr,
                       StateBlockPtr _p_ptr = nullptr,
                       StateBlockPtr _o_ptr = nullptr,
                       StateBlockPtr _intr_ptr = nullptr);
@@ -96,9 +96,9 @@ class CaptureMotion : public CaptureBase
         MatrixXs getDeltaPreintCov(const TimeStamp& _ts) const;
         virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const;
 
-        // Origin frame
-        FrameBasePtr getOriginFrame();
-        void setOriginFrame(FrameBasePtr _frame_ptr);
+        // Origin frame and capture
+        CaptureBasePtr getOriginCapture();
+        void setOriginCapture(CaptureBasePtr _capture_origin_ptr);
 
         // member data:
     private:
@@ -106,7 +106,7 @@ class CaptureMotion : public CaptureBase
         Eigen::MatrixXs data_cov_;          ///< Motion data covariance
         Eigen::VectorXs calib_preint_;      ///< Calibration parameters used during pre-integration
         MotionBuffer buffer_;               ///< Buffer of motions between this Capture and the next one.
-        FrameBaseWPtr origin_frame_ptr_;    ///< Pointer to the origin frame of the motion
+        CaptureBaseWPtr capture_origin_ptr_;    ///< Pointer to the origin capture of the motion
 };
 
 inline const Eigen::VectorXs& CaptureMotion::getData() const
@@ -158,14 +158,14 @@ inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const
     return _delta + _delta_error;
 }
 
-inline FrameBasePtr CaptureMotion::getOriginFrame()
+inline wolf::CaptureBasePtr CaptureMotion::getOriginCapture()
 {
-    return origin_frame_ptr_.lock();
+    return capture_origin_ptr_.lock();
 }
 
-inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr)
+inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr)
 {
-    origin_frame_ptr_ = _frame_ptr;
+    capture_origin_ptr_ = _capture_origin_ptr;
 }
 
 inline VectorXs CaptureMotion::getCalibrationPreint() const
diff --git a/include/core/capture/capture_odom_2D.h b/include/core/capture/capture_odom_2D.h
index 6b6258cea271baf716324a3888a2725557fd4027..06a507aa8368474460e93b7b6d64c059180d41d4 100644
--- a/include/core/capture/capture_odom_2D.h
+++ b/include/core/capture/capture_odom_2D.h
@@ -23,13 +23,13 @@ class CaptureOdom2D : public CaptureMotion
         CaptureOdom2D(const TimeStamp& _init_ts,
                       SensorBasePtr _sensor_ptr,
                       const Eigen::VectorXs& _data,
-                      FrameBasePtr _origin_frame_ptr = nullptr);
+                      CaptureBasePtr _capture_origin_ptr = nullptr);
 
         CaptureOdom2D(const TimeStamp& _init_ts,
                       SensorBasePtr _sensor_ptr,
                       const Eigen::VectorXs& _data,
                       const Eigen::MatrixXs& _data_cov,
-                      FrameBasePtr _origin_frame_ptr = nullptr);
+                      CaptureBasePtr _capture_origin_ptr = nullptr);
 
         virtual ~CaptureOdom2D();
 
diff --git a/include/core/capture/capture_odom_3D.h b/include/core/capture/capture_odom_3D.h
index c7e0eecb59e5fc500c5a67c07e1557d65681f5cb..cc810448af0a68d7287483b0afe538bda0e0d2b2 100644
--- a/include/core/capture/capture_odom_3D.h
+++ b/include/core/capture/capture_odom_3D.h
@@ -22,14 +22,14 @@ class CaptureOdom3D : public CaptureMotion
     public:
         CaptureOdom3D(const TimeStamp& _init_ts,
                       SensorBasePtr _sensor_ptr,
-                      const Eigen::Vector6s& _data,
-                      FrameBasePtr _origin_frame_ptr = nullptr);
+                      const Eigen::VectorXs& _data,
+                      CaptureBasePtr _capture_origin_ptr = nullptr);
 
         CaptureOdom3D(const TimeStamp& _init_ts,
                       SensorBasePtr _sensor_ptr,
-                      const Eigen::Vector6s& _data,
+                      const Eigen::VectorXs& _data,
                       const Eigen::MatrixXs& _data_cov,
-                      FrameBasePtr _origin_frame_ptr = nullptr);
+                      CaptureBasePtr _capture_origin_ptr = nullptr);
 
         virtual ~CaptureOdom3D();
 
diff --git a/include/core/capture/capture_velocity.h b/include/core/capture/capture_velocity.h
index 98bc3b3e395af54388f03777dfe876bf6a8a8429..f665c181311494bf0bbc154b3f1e030c7f1ecad2 100644
--- a/include/core/capture/capture_velocity.h
+++ b/include/core/capture/capture_velocity.h
@@ -35,14 +35,14 @@ public:
                   const SensorBasePtr& _sensor_ptr,
                   const Eigen::VectorXs& _velocity,
                   SizeEigen _delta_size, SizeEigen _delta_cov_size,
-                  FrameBasePtr _origin_frame_ptr);
+                  CaptureBasePtr _capture_origin_ptr);
 
   CaptureVelocity(const TimeStamp& _ts,
                   const SensorBasePtr& _sensor_ptr,
                   const Eigen::VectorXs& _velocity,
                   const Eigen::MatrixXs& _velocity_cov,
                   SizeEigen _delta_size, SizeEigen _delta_cov_size,
-                  FrameBasePtr _origin_frame_ptr,
+                  CaptureBasePtr _capture_origin_ptr,
                   StateBlockPtr _p_ptr = nullptr,
                   StateBlockPtr _o_ptr = nullptr,
                   StateBlockPtr _intr_ptr = nullptr);
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 1192230afa33cbb5814f6fc599e1df17c8290b9b..6c8bf6a34605dd5da4c9d4766f619b3c7cbc6941 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -56,7 +56,7 @@ class ProcessorDiffDrive : public ProcessorOdom2D
                                                 const MatrixXs& _data_cov,
                                                 const VectorXs& _calib,
                                                 const VectorXs& _calib_preint,
-                                                const FrameBasePtr& _frame_origin) override;
+                                                const CaptureBasePtr& _capture_origin) override;
         virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
         virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
                                             CaptureBasePtr _capture_origin) override;
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 0de934bca5dd2ef91eb77522c9db20a5eadfbc19..9bc5027fd8e8cbcc0729aed9b897765b21862ec7 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -418,7 +418,7 @@ class ProcessorMotion : public ProcessorBase
                                                 const MatrixXs& _data_cov,
                                                 const VectorXs& _calib,
                                                 const VectorXs& _calib_preint,
-                                                const FrameBasePtr& _frame_origin) = 0;
+                                                const CaptureBasePtr& _capture_origin_ptr) = 0;
 
         /** \brief emplace a feature corresponding to given capture and add the feature to this capture
          * \param _capture_motion: the parent capture
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 2957ec7de8cd0647a45fc1f72c15629b9f42a089..a4f6e3358fc0f71745a3df1ce15cf48cc880a7f6 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -80,7 +80,7 @@ class ProcessorOdom2D : public ProcessorMotion
                                                 const MatrixXs& _data_cov,
                                                 const VectorXs& _calib,
                                                 const VectorXs& _calib_preint,
-                                                const FrameBasePtr& _frame_origin) override;
+                                                const CaptureBasePtr& _capture_origin_ptr) override;
         virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
         virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
                                                     CaptureBasePtr _capture_origin) override;
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index 1512b996e89a5e5d5c7c6bc294d1dfa495219d41..22f4903c209d11bf31f0e08f302c0ce7a6d8d77f 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -99,7 +99,7 @@ class ProcessorOdom3D : public ProcessorMotion
                                                 const MatrixXs& _data_cov,
                                                 const VectorXs& _calib,
                                                 const VectorXs& _calib_preint,
-                                                const FrameBasePtr& _frame_origin) override;
+                                                const CaptureBasePtr& _capture_origin) override;
 
         virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
 
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index 9367347e07da6a0f9aaa0708c5db3b939a0deba3..47b0b7886ee9a123e39a5b8cb90576d8a2562ea6 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -49,10 +49,10 @@ class SensorDiffDrive : public SensorBase
 {
     public:
         SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
-                                 const IntrinsicsDiffDrivePtr& _intrinsics);
+                        const IntrinsicsDiffDrivePtr& _intrinsics);
         WOLF_SENSOR_CREATE(SensorDiffDrive, IntrinsicsDiffDrive, 3);
         virtual ~SensorDiffDrive();
-        IntrinsicsDiffDriveConstPtr getParams() const {return params_diff_drive_;}
+        IntrinsicsDiffDriveConstPtr getParams() const;
 
         Scalar getRadiansPerTick() const
         {
@@ -65,6 +65,11 @@ class SensorDiffDrive : public SensorBase
 
 };
 
+inline wolf::IntrinsicsDiffDriveConstPtr SensorDiffDrive::getParams() const
+{
+    return params_diff_drive_;
+}
+
 } /* namespace wolf */
 
 #endif /* SENSOR_SENSOR_DIFF_DRIVE_H_ */
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index 02d9eab183156528ffe991b5f01a7867fd5d5568..cc81850c8447040d21f7e6cffa4f079c5884ca47 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.cpp
@@ -9,7 +9,7 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
                                    const SensorBasePtr& _sensor_ptr,
                                    const Eigen::VectorXs& _data,
                                    const Eigen::MatrixXs& _data_cov,
-                                   FrameBasePtr _origin_frame_ptr,
+                                   CaptureBasePtr _capture_origin_ptr,
                                    StateBlockPtr _p_ptr,
                                    StateBlockPtr _o_ptr,
                                    StateBlockPtr _intr_ptr) :
@@ -20,7 +20,7 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
                 _data_cov,
                 3,
                 3,
-                _origin_frame_ptr,
+                _capture_origin_ptr,
                 _p_ptr,
                 _o_ptr,
                 _intr_ptr)
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 47d26a131c25cffb0014ca5472ad744070929eaf..a2330905ad8be9ff5f871f8e5fc0c11a01b9ccde 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -16,12 +16,12 @@ CaptureMotion::CaptureMotion(const std::string & _type,
                              const Eigen::VectorXs& _data,
                              SizeEigen _delta_size,
                              SizeEigen _delta_cov_size,
-                             FrameBasePtr _origin_frame_ptr) :
+                             CaptureBasePtr _capture_origin_ptr) :
         CaptureBase(_type, _ts, _sensor_ptr),
         data_(_data),
         data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
         buffer_(),
-        origin_frame_ptr_(_origin_frame_ptr)
+        capture_origin_ptr_(_capture_origin_ptr)
 {
     //
 }
@@ -33,7 +33,7 @@ CaptureMotion::CaptureMotion(const std::string & _type,
                              const Eigen::MatrixXs& _data_cov,
                              SizeEigen _delta_size,
                              SizeEigen _delta_cov_size,
-                             FrameBasePtr _origin_frame_ptr,
+                             CaptureBasePtr _capture_origin_ptr,
                              StateBlockPtr _p_ptr ,
                              StateBlockPtr _o_ptr ,
                              StateBlockPtr _intr_ptr ) :
@@ -41,7 +41,7 @@ CaptureMotion::CaptureMotion(const std::string & _type,
                 data_(_data),
                 data_cov_(_data_cov),
                 buffer_(),
-                origin_frame_ptr_(_origin_frame_ptr)
+                capture_origin_ptr_(_capture_origin_ptr)
 {
     //
 }
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp
index 1e073b2810e92eaa7c19affe4dc0bb4ad9ef74fe..36826540a27804e14c61102260bbec24a36df58f 100644
--- a/src/capture/capture_odom_2D.cpp
+++ b/src/capture/capture_odom_2D.cpp
@@ -13,8 +13,8 @@ namespace wolf
 CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
                              SensorBasePtr _sensor_ptr,
                              const Eigen::VectorXs& _data,
-                             FrameBasePtr _origin_frame_ptr):
-        CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _origin_frame_ptr)
+                             CaptureBasePtr _capture_origin_ptr):
+        CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr)
 {
     //
 }
@@ -23,8 +23,8 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
                              SensorBasePtr _sensor_ptr,
                              const Eigen::VectorXs& _data,
                              const Eigen::MatrixXs& _data_cov,
-                             FrameBasePtr _origin_frame_ptr):
-        CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _origin_frame_ptr)
+                             CaptureBasePtr _capture_origin_ptr):
+        CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr)
 {
     //
 }
diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp
index d3236a2ab89c312320d0f3c9a6caf830733d3ef2..84f263d2cfbd6c97e647b8504322352696d4332d 100644
--- a/src/capture/capture_odom_3D.cpp
+++ b/src/capture/capture_odom_3D.cpp
@@ -12,19 +12,19 @@ namespace wolf
 
 CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
                              SensorBasePtr _sensor_ptr,
-                             const Eigen::Vector6s& _data,
-                             FrameBasePtr _origin_frame_ptr):
-        CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _origin_frame_ptr)
+                             const Eigen::VectorXs& _data,
+                             CaptureBasePtr _capture_origin_ptr):
+        CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
 {
     //
 }
 
 CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
                              SensorBasePtr _sensor_ptr,
-                             const Eigen::Vector6s& _data,
+                             const Eigen::VectorXs& _data,
                              const Eigen::MatrixXs& _data_cov,
-                             FrameBasePtr _origin_frame_ptr):
-        CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _origin_frame_ptr)
+                             CaptureBasePtr _capture_origin_ptr):
+        CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr)
 {
     //
 }
diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp
index 0785e8bae4918c1f8406bc379123dd56f4120730..00a6b796bbba9135ee8fbba1150ff353c29b72ec 100644
--- a/src/capture/capture_velocity.cpp
+++ b/src/capture/capture_velocity.cpp
@@ -6,9 +6,9 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
                                  const SensorBasePtr& _sensor_ptr,
                                  const Eigen::VectorXs& _velocity,
                                  SizeEigen _delta_size, SizeEigen _delta_cov_size,
-                                 FrameBasePtr _origin_frame_ptr) :
+                                 CaptureBasePtr _capture_origin_ptr) :
   CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity,
-                _delta_size, _delta_cov_size, _origin_frame_ptr)
+                _delta_size, _delta_cov_size, _capture_origin_ptr)
 {
   //
 }
@@ -18,12 +18,12 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
                                  const Eigen::VectorXs& _velocity,
                                  const Eigen::MatrixXs& _velocity_cov,
                                  SizeEigen _delta_size, SizeEigen _delta_cov_size,
-                                 FrameBasePtr _origin_frame_ptr,
+                                 CaptureBasePtr _capture_origin_ptr,
                                  StateBlockPtr _p_ptr,
                                  StateBlockPtr _o_ptr,
                                  StateBlockPtr _intr_ptr) :
   CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity, _velocity_cov,
-                _delta_size, _delta_cov_size, _origin_frame_ptr,
+                _delta_size, _delta_cov_size, _capture_origin_ptr,
                 _p_ptr, _o_ptr, _intr_ptr)
 {
   //
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 37a841abeeaf8caf6daf56f12d3f00689bd3aa6d..49a9fab6bf8450592dd78e8b6ac69fd4bfe7a0a3 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -939,8 +939,12 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
                     if (C->isMotion())
                     {
                         auto CM = std::static_pointer_cast<CaptureMotion>(C);
-                        if (CM->getOriginFrame())
-                            cout << " -> OF" << CM->getOriginFrame()->id();
+                        if (auto OC = CM->getOriginCapture())
+                        {
+                            cout << " -> OC" << OC->id();
+                            if (auto OF = OC->getFrame())
+                                cout << " ; OF" << OF->id();
+                        }
                     }
 
                     cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index d0afacb45a6fb235e6d01e7c893ffc612449e6c0..d4605c6f0ab27ef247dd64885de922eece4b0fc6 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -130,14 +130,14 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
                                                     const MatrixXs& _data_cov,
                                                     const VectorXs& _calib,
                                                     const VectorXs& _calib_preint,
-                                                    const FrameBasePtr& _frame_origin)
+                                                    const CaptureBasePtr& _capture_origin)
 {
     auto cap_motion = CaptureBase::emplace<CaptureDiffDrive>(_frame_own,
                                                              _ts,
                                                              _sensor,
                                                              _data,
                                                              _data_cov,
-                                                             _frame_origin);
+                                                             _capture_origin);
     cap_motion->setCalibration      (_calib);
     cap_motion->setCalibrationPreint(_calib_preint);
 
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 2d6d5db588913577dc7e0a88c7989b6ffc69ccda..d54036d2d3a9b4a00c80daeceddb836920daed10 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -52,10 +52,10 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
     _capture_source->getBuffer().get().push_front(motionZero(t_zero));
 
     // Update the existing capture
-    _capture_source->setOriginFrame(_keyframe_target);
+    _capture_source->setOriginCapture(_capture_target);
 
     // Get optimized calibration params from 'origin' keyframe
-    VectorXs calib_preint_optim = _capture_source->getOriginFrame()->getCaptureOf(getSensor())->getCalibration();
+    VectorXs calib_preint_optim = _capture_source->getOriginCapture()->getCalibration();
 
     // Write the calib params into the capture before re-integration
     _capture_source->setCalibrationPreint(calib_preint_optim);
@@ -123,12 +123,10 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             FrameBasePtr keyframe_from_callback = pack->key_frame;
 
             // find the capture whose buffer is affected by the new keyframe
-            auto existing_capture   = findCaptureContainingTimeStamp(keyframe_from_callback->getTimeStamp()); // k
+            auto capture_existing   = findCaptureContainingTimeStamp(keyframe_from_callback->getTimeStamp()); // k
 
-            // Find the frame acting as the capture's origin
-            auto keyframe_origin    = existing_capture->getOriginFrame(); // i
-
-            auto capture_origin     = std::static_pointer_cast<CaptureMotion>(keyframe_origin->getCaptureOf(getSensor()));
+            // Find the capture acting as the buffer's origin
+            auto capture_origin     = capture_existing->getOriginCapture();
 
             // Get calibration params for preintegration from origin, since it has chances to have optimized values
             VectorXs calib_origin   = capture_origin->getCalibration();
@@ -142,31 +140,31 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                                                                  getSensor()->getNoiseCov(),
                                                                  calib_origin,
                                                                  calib_origin,
-                                                                 keyframe_origin);
+                                                                 capture_origin);
 
             // split the buffer
             // and give the part of the buffer before the new keyframe to the capture for the KF callback
-            splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+            splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
 
             // create motion feature and add it to the capture
-            auto new_feature = emplaceFeature(capture_for_keyframe_callback);
+            auto feature_new = emplaceFeature(capture_for_keyframe_callback);
 
             // create motion factor and add it to the feature, and constrain to the other capture (origin)
-            emplaceFactor(new_feature, keyframe_origin->getCaptureOf(getSensor()) );
+            emplaceFactor(feature_new, capture_origin );
 
             // modify existing feature and factor (if they exist in the existing capture)
-            if (!existing_capture->getFeatureList().empty())
+            if (!capture_existing->getFeatureList().empty())
             {
-                auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
+                auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
 
                 // Modify existing feature --------
-                existing_feature->setMeasurement          (existing_capture->getBuffer().get().back().delta_integr_);
-                existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
+                feature_existing->setMeasurement          (capture_existing->getBuffer().get().back().delta_integr_);
+                feature_existing->setMeasurementCovariance(capture_existing->getBuffer().get().back().delta_integr_cov_);
 
                 // Modify existing factor --------
                 // Instead of modifying, we remove one ctr, and create a new one.
-                auto fac_to_remove  = existing_feature->getFactorList().back(); // there is only one factor!
-                auto new_ctr        = emplaceFactor(existing_feature, capture_for_keyframe_callback);
+                auto fac_to_remove  = feature_existing->getFactorList().back(); // there is only one factor!
+                auto new_ctr        = emplaceFactor(feature_existing, capture_for_keyframe_callback);
                 fac_to_remove       ->remove();  // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
             }
             break;
@@ -197,7 +195,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
              *                      k
              *    * ========= * === * === *
              *    x ----------o ----n ----l
-             *     \____f____/
+             *     \____f____/ \_f_/
              *
              * Trajectory after the KF, and after reset
              *
@@ -210,11 +208,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             // extract pack elements
             FrameBasePtr keyframe_from_callback = pack->key_frame;
 
-            auto & existing_capture = last_ptr_;
-
-            // Find the frame acting as the capture's origin
-            auto keyframe_origin    = existing_capture->getOriginFrame();
+            auto & capture_existing = last_ptr_;
 
+            // Find the capture acting as the buffer's origin
             auto & capture_origin   = origin_ptr_;
 
             // Get calibration params for preintegration from origin, since it has chances to have optimized values
@@ -229,17 +225,17 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                                                                 getSensor()->getNoiseCov(),
                                                                 calib_origin,
                                                                 calib_origin,
-                                                                keyframe_origin);
+                                                                capture_origin);
 
             // split the buffer
             // and give the part of the buffer before the new keyframe to the capture for the KF callback
-            splitBuffer(existing_capture, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
+            splitBuffer(capture_existing, ts_from_callback, keyframe_from_callback, capture_for_keyframe_callback);
 
             // create motion feature and add it to the capture
             auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
 
             // create motion factor and add it to the feature, and constrain to the other capture (origin)
-            emplaceFactor(feature_for_keyframe_callback, keyframe_origin->getCaptureOf(getSensor()) );
+            emplaceFactor(feature_for_keyframe_callback, capture_origin );
 
             // reset processor origin
             origin_ptr_ = capture_for_keyframe_callback;
@@ -270,10 +266,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
          *    + : last frame
          *    x : any capture
          *    o : origin capture
-         *    l : last capture
+         *    l : last capture      -> we'll make a KF here
          *    i : incoming capture
-         *    e : existing capture  -> new KF splits its buffer in two parts
-         *    n : new capture       -> part of the split buffer will be given to this
+         *    n : new capture       ->
          *  --- : buffer history
          *
          * Trajectory before the KF
@@ -282,6 +277,12 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
          *    x ----------o ----------l   i
          *     \____f____/
          *
+         * Trajectory after creating KF at last
+         *
+         *    * ========= * ========= * = +
+         *    x ----------o ----------l --n
+         *     \____f____/ \____f____/
+         *
          * Trajectory after creating KF at last and reset
          *
          *    * ========= * ========= * = +
@@ -302,27 +303,27 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         auto factor         = emplaceFactor(key_feature, origin_ptr_);
 
         // create a new frame
-        auto new_frame      = getProblem()->emplaceFrame(NON_ESTIMATED,
+        auto frame_new      = getProblem()->emplaceFrame(NON_ESTIMATED,
                                                         getCurrentState(),
                                                         getCurrentTimeStamp());
         // create a new capture
-        auto new_capture    = emplaceCapture(new_frame,
+        auto capture_new    = emplaceCapture(frame_new,
                                              getSensor(),
                                              key_frame->getTimeStamp(),
                                              Eigen::VectorXs::Zero(data_size_),
                                              getSensor()->getNoiseCov(),
                                              last_ptr_->getCalibration(),
                                              last_ptr_->getCalibration(),
-                                             key_frame);
+                                             last_ptr_);
         // reset the new buffer
-        new_capture->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ;
+        capture_new->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ;
 
         // reset derived things
         resetDerived();
 
-        // Update pointers
+        // Reset pointers
         origin_ptr_     = last_ptr_;
-        last_ptr_       = new_capture;
+        last_ptr_       = capture_new;
 
         // callback to other processors
         getProblem()->keyFrameCallback(key_frame, shared_from_this(), params_motion_->time_tolerance);
@@ -347,8 +348,8 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
     if (capture_motion)  // We found a CaptureMotion whose buffer contains the time stamp
     {
         // Get origin state and calibration
-        VectorXs state_0          = capture_motion->getOriginFrame()->getState();
-        CaptureBasePtr cap_orig   = capture_motion->getOriginFrame()->getCaptureOf(getSensor());
+        CaptureBasePtr cap_orig   = capture_motion->getOriginCapture();
+        VectorXs state_0          = cap_orig->getFrame()->getState();
         VectorXs calib            = cap_orig->getCalibration();
 
         // Get delta and correct it with new calibration params
@@ -411,7 +412,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
                                getSensor()->getNoiseCov(),
                                getSensor()->getCalibration(),
                                getSensor()->getCalibration(),
-                               _origin_frame);
+                               origin_ptr_);
 
     // clear and reset buffer
     getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index dda3af4ddaa964537b95d85eac1a8cccf1fb7167..0f094674afb3c902dbb3bac9e983929a170f1839 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -127,9 +127,9 @@ CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own,
                                                  const MatrixXs& _data_cov,
                                                  const VectorXs& _calib,
                                                  const VectorXs& _calib_preint,
-                                                 const FrameBasePtr& _frame_origin)
+                                                 const CaptureBasePtr& _capture_origin_ptr)
 {
-    auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin);
+    auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
     cap_motion->setCalibration(_calib);
     cap_motion->setCalibrationPreint(_calib_preint);
 
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 27659e3eeb6deca00ed09794cc1059f53b007aba..c2821646896d2573f7bcaa1ed828f63c0d7813d2 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -194,9 +194,9 @@ CaptureMotionPtr ProcessorOdom3D::emplaceCapture(const FrameBasePtr& _frame_own,
                                                  const MatrixXs& _data_cov,
                                                  const VectorXs& _calib,
                                                  const VectorXs& _calib_preint,
-                                                 const FrameBasePtr& _frame_origin)
+                                                 const CaptureBasePtr& _capture_origin)
 {
-    auto cap_motion = CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _frame_origin);
+    auto cap_motion = CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
     cap_motion->setCalibration(_calib);
     cap_motion->setCalibrationPreint(_calib_preint);
 
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 369e32919a84d20ab3e7396b67bfcb5e68b514c5..d19ab8aa2e4d2ba5f9146bbdad2d45e5d855afc2 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -12,13 +12,13 @@ namespace wolf
 {
 
 SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
-                                                   const IntrinsicsDiffDrivePtr& _intrinsics) :
-                                                           SensorBase("DIFF DRIVE",
-                                                                      std::make_shared<StateBlock>(_extrinsics.head(2), true),
-                                                                      std::make_shared<StateAngle>(_extrinsics(2), true),
-                                                                      std::make_shared<StateBlock>(3, false),
-                                                                      2),
-                                                                      params_diff_drive_(_intrinsics)
+                                 const IntrinsicsDiffDrivePtr& _intrinsics) :
+                                                                   SensorBase("DIFF DRIVE",
+                                                                              std::make_shared<StateBlock>(_extrinsics.head(2), true),
+                                                                              std::make_shared<StateAngle>(_extrinsics(2), true),
+                                                                              std::make_shared<StateBlock>(3, false),
+                                                                              2),
+                                                                   params_diff_drive_(_intrinsics)
 {
     radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
     getIntrinsic()->setState(Eigen::Vector3s(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index f8901a4d9e75edc815913289b697d2a39dd455fe..ce15f0b0343b3a58321f0971f1b85f817628988e 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -123,7 +123,7 @@ TEST(Emplace, EmplaceDerived)
     // LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
     auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
     auto cov = Eigen::MatrixXs::Identity(2,2);
-    auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, frm);
+    auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, nullptr);
     // auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
     auto m = Eigen::Matrix<Scalar,9,6>();
     for(int i = 0; i < 9; i++)
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 6a9e0ddb95cc05413d8c960912256bb0ff908b04..7302ae4833a94a7aeb0ed9ed304b4ea3026b453c 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -83,9 +83,9 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
                                                 const MatrixXs& _data_cov,
                                                 const VectorXs& _calib,
                                                 const VectorXs& _calib_preint,
-                                                const FrameBasePtr& _frame_origin)
+                                                const CaptureBasePtr& _capture_origin)
         {
-            return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _frame_origin);
+            return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
         }
 
         virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own)
@@ -456,7 +456,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
     data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
     data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
 
-    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
 
     for (int n = 0; n < N; n++)
     {
@@ -588,7 +588,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
     data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
     data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
 
-    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
 
     for (int n = 0; n < N; n++)
     {
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 3c15a924d782d331db4b6ef268a2ea123b27db7d..f697a948afecf8f83b8275220f30497bd6e75130 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -78,9 +78,9 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
                                                 const MatrixXs& _data_cov,
                                                 const VectorXs& _calib,
                                                 const VectorXs& _calib_preint,
-                                                const FrameBasePtr& _frame_origin)
+                                                const CaptureBasePtr& _capture_origin)
         {
-            return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _frame_origin);
+            return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
         }
 
         virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own)
@@ -318,7 +318,7 @@ TEST_F(ProcessorDiffDriveTest, process)
     data(0) = 0.25*intr->ticks_per_wheel_revolution / N;
     data(1) = 0.50*intr->ticks_per_wheel_revolution / N;
 
-    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
 
     WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
     for (int n = 1; n <= N; n++)
@@ -347,7 +347,7 @@ TEST_F(ProcessorDiffDriveTest, linear)
     data(0) = 100.0 ; // one turn of the wheels
     data(1) = 100.0 ;
 
-    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
 
     C->process();
     WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
@@ -372,7 +372,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
     data(0) = -20.0 ; // one fifth of a turn of the left wheel, in reverse
     data(1) =  20.0 ; // one fifth of a turn of the right wheel, forward --> we'll turn left --> positive angle
 
-    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0);
+    auto C = std::make_shared<CaptureDiffDrive>(t, sensor, data, data_cov, F0->getCaptureList().front());
 
     C->process();
     WOLF_TRACE("t = ", t, "; x = ", processor->getCurrentState().transpose());
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index 93bf18911e2daadcabdd5ad7df66a9161fdb679c..0977b8f87f3ba2fab515098a2afd0942f8fb6714 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -140,9 +140,7 @@ TEST_F(ProcessorMotion_test, splitBuffer)
 
     TimeStamp        t_target = 8.5;
     FrameBasePtr     F_target = problem->emplaceFrame(KEY, t_target);
-    CaptureBasePtr   C_last   = processor->getLast();
-    CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(C_last);
-    FrameBasePtr     F_origin = C_source->getOriginFrame();
+    CaptureMotionPtr C_source = std::dynamic_pointer_cast<CaptureMotion>(processor->getLast());
     CaptureMotionPtr C_target = CaptureBase::emplace<CaptureMotion>(F_target,
                                                                     "ODOM 2D",
                                                                     t_target,
@@ -150,7 +148,7 @@ TEST_F(ProcessorMotion_test, splitBuffer)
                                                                     data,
                                                                     3,
                                                                     3,
-                                                                    F_origin);
+                                                                    nullptr);
 
     processor->splitBuffer(C_source,
                            t_target,