diff --git a/demos/solver/test_SPQR.cpp b/demos/solver/test_SPQR.cpp
index b5f159c731741dabe4ed00bbe8eaa0e7968be7f3..56df88821b3441b9e1526f713741ffbfc49acedc 100644
--- a/demos/solver/test_SPQR.cpp
+++ b/demos/solver/test_SPQR.cpp
@@ -27,8 +27,8 @@
  */
 
 #include <iostream>
-#include <eigen3/Eigen/SPQRSupport>
-#include <eigen3/Eigen/CholmodSupport>
+#include <Eigen/SPQRSupport>
+#include <Eigen/CholmodSupport>
 #include "SuiteSparseQR.hpp"
 
 using namespace Eigen;
diff --git a/demos/solver/test_ccolamd.cpp b/demos/solver/test_ccolamd.cpp
index 98f496f26b5de691a38384d42b3349d62fc8b20e..126fa7f869f203188a3c589bf68fced8f4e593ff 100644
--- a/demos/solver/test_ccolamd.cpp
+++ b/demos/solver/test_ccolamd.cpp
@@ -43,9 +43,9 @@
 #include "solver/ccolamd_ordering.h"
 
 // eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
+#include <Eigen/OrderingMethods>
+#include <Eigen/CholmodSupport>
+#include <Eigen/SparseLU>
 
 using namespace Eigen;
 using namespace wolf;
diff --git a/demos/solver/test_ccolamd_blocks.cpp b/demos/solver/test_ccolamd_blocks.cpp
index 14bb174286235f063a375cca4e28f1e4dfeca537..2d82633432d7620f108b8ef8532c4d08f0a17b9f 100644
--- a/demos/solver/test_ccolamd_blocks.cpp
+++ b/demos/solver/test_ccolamd_blocks.cpp
@@ -37,9 +37,9 @@
 #include <queue>
 
 // eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
+#include <Eigen/OrderingMethods>
+#include <Eigen/CholmodSupport>
+#include <Eigen/SparseLU>
 
 // ccolamd
 #include "solver/ccolamd_ordering.h"
diff --git a/demos/solver/test_iQR.cpp b/demos/solver/test_iQR.cpp
index 53a37cef1a62ec1ffb281a6fdbd741fc73cb59f8..ba4ae194a00c6e9c8a78affc1a54fc02b1f52bb1 100644
--- a/demos/solver/test_iQR.cpp
+++ b/demos/solver/test_iQR.cpp
@@ -44,9 +44,9 @@
 #include <queue>
 
 // eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
-#include <eigen3/Eigen/SPQRSupport>
+#include <Eigen/OrderingMethods>
+#include <Eigen/SparseQR>
+#include <Eigen/SPQRSupport>
 
 // ccolamd
 #include "solver/ccolamd_ordering.h"
diff --git a/demos/solver/test_iQR_wolf.cpp b/demos/solver/test_iQR_wolf.cpp
index a8df237237669b54aa837d32f35bf8c156fa33eb..4ffc4a0bffada8742a20dd9d00b220c7544be156 100644
--- a/demos/solver/test_iQR_wolf.cpp
+++ b/demos/solver/test_iQR_wolf.cpp
@@ -38,8 +38,8 @@
 #include <queue>
 
 // eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
+#include <Eigen/OrderingMethods>
+#include <Eigen/SparseQR>
 #include <Eigen/SPQRSupport>
 
 // ccolamd
diff --git a/demos/solver/test_incremental_ccolamd_blocks.cpp b/demos/solver/test_incremental_ccolamd_blocks.cpp
index 740407a9a560f0efdbdb8267ee7925c22fbf12bf..f0eabf66cde8d920265f3af6a5b4e8a994fa5858 100644
--- a/demos/solver/test_incremental_ccolamd_blocks.cpp
+++ b/demos/solver/test_incremental_ccolamd_blocks.cpp
@@ -37,9 +37,9 @@
 #include <queue>
 
 // eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
+#include <Eigen/OrderingMethods>
+#include <Eigen/CholmodSupport>
+#include <Eigen/SparseLU>
 
 // ccolamd
 #include "solver/ccolamd_ordering.h"
diff --git a/demos/solver/test_permutations.cpp b/demos/solver/test_permutations.cpp
index 0765d8ae77996a72b027a62380804243aa0aedfa..6724ea83f167bbacbfceed245a01b7c5cfdeea60 100644
--- a/demos/solver/test_permutations.cpp
+++ b/demos/solver/test_permutations.cpp
@@ -37,7 +37,7 @@
 #include <queue>
 
 // eigen includes
-#include <eigen3/Eigen/OrderingMethods>
+#include <Eigen/OrderingMethods>
 
 using namespace Eigen;
 
diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h
index 9c2e6422e84a8747ec8a468f7ba7c1fca1089394..314a31bb92f722eed3743778ceaa388ace44f1a4 100644
--- a/include/core/ceres_wrapper/sparse_utils.h
+++ b/include/core/ceres_wrapper/sparse_utils.h
@@ -22,7 +22,7 @@
 #pragma once
 
 // eigen includes
-//#include <eigen3/Eigen/Sparse>
+//#include <Eigen/Sparse>
 
 namespace wolf
 {
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 8a366ebccad93c0f26f0368238e84f2e93b6ce51..9a782ab6e936f6dd79746061c5d68fb7bb1ffa72 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -148,17 +148,17 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         bool isConstrainedBy(const FactorBaseConstPtr& _factor) const;
 
         template <class C>
-        CaptureBaseConstPtr getCaptureOfType() const;
+        std::shared_ptr<const C> getCaptureOfType() const;
         template <class C>
-        CaptureBasePtr getCaptureOfType();
+        std::shared_ptr<C> getCaptureOfType();
 
         CaptureBaseConstPtr getCaptureOfType(const std::string& type) const;
         CaptureBasePtr getCaptureOfType(const std::string& type);
 
         template <class C>
-        CaptureBaseConstPtrList getCapturesOfType() const;
+        std::list<std::shared_ptr<const C>> getCapturesOfType() const;
         template <class C>
-        CaptureBasePtrList getCapturesOfType();
+        std::list<std::shared_ptr<C>> getCapturesOfType();
 
         CaptureBaseConstPtrList getCapturesOfType(const std::string& type) const;
         CaptureBasePtrList getCapturesOfType(const std::string& type);
@@ -299,40 +299,52 @@ inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
 }
 
 template <class C>
-inline CaptureBaseConstPtr FrameBase::getCaptureOfType() const
+inline std::shared_ptr<const C> FrameBase::getCaptureOfType() const
 {
     for (auto capture_ptr : getCaptureList())
-        if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
-            return capture_ptr;
+    {
+        auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
+        if (cap_derived)
+            return cap_derived;
+    }
     return nullptr;
 }
 
 template <class C>
-inline CaptureBasePtr FrameBase::getCaptureOfType()
+inline std::shared_ptr<C> FrameBase::getCaptureOfType()
 {
     for (auto capture_ptr : getCaptureList())
-        if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
-            return capture_ptr;
+    {
+        auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
+        if (cap_derived)
+            return cap_derived;
+    }
     return nullptr;
 }
 
 template <class C>
-inline CaptureBaseConstPtrList FrameBase::getCapturesOfType() const
+inline std::list<std::shared_ptr<const C>> FrameBase::getCapturesOfType() const
 {
-    CaptureBaseConstPtrList captures;
+    std::list<std::shared_ptr<const C>> captures;
     for (auto capture_ptr : getCaptureList())
-        if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
-            captures.push_back(capture_ptr);
+    {
+        auto cap_derived = std::dynamic_pointer_cast<const C>(capture_ptr);
+        if (cap_derived)
+            captures.push_back(cap_derived);
+    }
     return captures;
 }
 
 template <class C>
-inline CaptureBasePtrList FrameBase::getCapturesOfType()
+inline std::list<std::shared_ptr<C>> FrameBase::getCapturesOfType()
 {
-    CaptureBasePtrList captures;
+    std::list<std::shared_ptr<C>> captures;
     for (auto capture_ptr : getCaptureList())
-        if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
-            captures.push_back(capture_ptr);
+    {
+        auto cap_derived = std::dynamic_pointer_cast<C>(capture_ptr);
+        if (cap_derived)
+            captures.push_back(cap_derived);
+    }
     return captures;
 }
 
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index 343247a93f07350eacb9296bbfdbf201962db4a9..7c1365a1ec0a8d82c2aef1e3390613ac10ee8851 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -92,7 +92,7 @@ class MotionBuffer : public std::list<Motion>
         const Motion& getMotion(const TimeStamp& _ts) const;
         void getMotion(const TimeStamp& _ts, Motion& _motion) const;
         void split(const TimeStamp& _ts, MotionBuffer& _oldest_buffer);
-        void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0);
+        void print(bool show_data = 0, bool show_delta = 0, bool show_delta_int = 0, bool show_jacs = 0, bool show_covs = 0);
 };
 
 } // namespace wolf
\ No newline at end of file
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 8b81af6a87015310996aa535239a38b9c3f5a103..725c33ef81b950811a9a4f60cf7aec58f63afe4e 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -55,30 +55,30 @@ class ProcessorDiffDrive : public ProcessorOdom2d
 
     protected:
         // Motion integration
-        void computeCurrentDelta(const Eigen::VectorXd& _data,
-                                         const Eigen::MatrixXd& _data_cov,
-                                         const Eigen::VectorXd& _calib,
-                                         const double _dt,
-                                         Eigen::VectorXd& _delta,
-                                         Eigen::MatrixXd& _delta_cov,
-                                         Eigen::MatrixXd& _jacobian_calib) const override;
-        CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
-                                                const SensorBasePtr& _sensor,
-                                                const TimeStamp& _ts,
-                                                const VectorXd& _data,
-                                                const MatrixXd& _data_cov,
-                                                const VectorXd& _calib,
-                                                const VectorXd& _calib_preint,
-                                                const CaptureBasePtr& _capture_origin) override;
-        FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
-                                            CaptureBasePtr _capture_origin) override;
-        VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
-        void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+      void computeCurrentDelta(const Eigen::VectorXd& _data,
+                               const Eigen::MatrixXd& _data_cov,
+                               const Eigen::VectorXd& _calib,
+                               const double           _dt,
+                               Eigen::VectorXd&       _delta,
+                               Eigen::MatrixXd&       _delta_cov,
+                               Eigen::MatrixXd&       _jacobian_calib) const override;
+
+      CaptureMotionPtr emplaceCapture(const FrameBasePtr&   _frame_own,
+                                      const SensorBasePtr&  _sensor,
+                                      const TimeStamp&      _ts,
+                                      const VectorXd&       _data,
+                                      const MatrixXd&       _data_cov,
+                                      const VectorXd&       _calib,
+                                      const VectorXd&       _calib_preint,
+                                      const CaptureBasePtr& _capture_origin) override;
+      void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+      VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+      void     setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
 
     protected:
-        ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
-        double radians_per_tick_;
+      ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
+      double                      radians_per_tick_;
 
 };
 
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index dbc86ff01abcb16fa27605781326fb66e30a62b3..0d68a216211c1169e62c0daedf8056952524bb24 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -468,16 +468,17 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
                                                 const VectorXd& _calib_preint,
                                                 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
-         */
-        virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) = 0;
 
-        /** \brief emplace a factor and link it in the wolf tree
-         * \param _feature_motion: the parent feature
-         * \param _frame_origin: the frame constrained by this motion factor
+        /** \brief emplace the features and factors corresponding to given capture and link them to the capture
+         * \param _capture_own: the parent capture
+         * \param _capture_origin: the capture constrained by this motion factor
+         * 
+         * Typical factors to add for a ProcessorMotionDerived can be:
+         *   - A preintegrated motion factor -- this is the main factor
+         *   - A calibration drift factor -- only for dynamic sensor calibration parameters
+         *   - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot models
          */
-        virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
+        virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0;
 
         virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
 
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index e5dd0d822f34922f8afeba03ee71ab85d7b8d68c..f1b04fe357035477f51f51c26b39b5f9eb88b599 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -62,44 +62,42 @@ class ProcessorOdom2d : public ProcessorMotion
         bool voteForKeyFrame() const override;
 
     protected:
-        void computeCurrentDelta(const Eigen::VectorXd& _data,
-                                 const Eigen::MatrixXd& _data_cov,
-                                 const Eigen::VectorXd& _calib,
-                                 const double _dt,
-                                 Eigen::VectorXd& _delta,
-                                 Eigen::MatrixXd& _delta_cov,
-                                 Eigen::MatrixXd& _jacobian_calib) const override;
-        void deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                            const Eigen::VectorXd& _delta2,
-                            const double _Dt2,
-                            Eigen::VectorXd& _delta1_plus_delta2) const override;
-        void deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                            const Eigen::VectorXd& _delta2,
-                            const double _Dt2,
-                            Eigen::VectorXd& _delta1_plus_delta2,
-                            Eigen::MatrixXd& _jacobian1,
-                            Eigen::MatrixXd& _jacobian2) const override;
-        void statePlusDelta(const VectorComposite& _x,
-                            const Eigen::VectorXd& _delta,
-                            const double _Dt,
-                            VectorComposite& _x_plus_delta) const override;
-        Eigen::VectorXd deltaZero() const override;
-        Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
-                                     const Eigen::VectorXd& delta_step) const override;
-
-        CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
-                                        const SensorBasePtr& _sensor,
-                                        const TimeStamp& _ts,
-                                        const VectorXd& _data,
-                                        const MatrixXd& _data_cov,
-                                        const VectorXd& _calib,
-                                        const VectorXd& _calib_preint,
-                                        const CaptureBasePtr& _capture_origin_ptr) override;
-        FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
-                                            CaptureBasePtr _capture_origin) override;
-        VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
-        void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+      void            computeCurrentDelta(const Eigen::VectorXd& _data,
+                                          const Eigen::MatrixXd& _data_cov,
+                                          const Eigen::VectorXd& _calib,
+                                          const double           _dt,
+                                          Eigen::VectorXd&       _delta,
+                                          Eigen::MatrixXd&       _delta_cov,
+                                          Eigen::MatrixXd&       _jacobian_calib) const override;
+      void            deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                     const Eigen::VectorXd& _delta2,
+                                     const double           _Dt2,
+                                     Eigen::VectorXd&       _delta1_plus_delta2) const override;
+      void            deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                     const Eigen::VectorXd& _delta2,
+                                     const double           _Dt2,
+                                     Eigen::VectorXd&       _delta1_plus_delta2,
+                                     Eigen::MatrixXd&       _jacobian1,
+                                     Eigen::MatrixXd&       _jacobian2) const override;
+      void            statePlusDelta(const VectorComposite& _x,
+                                     const Eigen::VectorXd& _delta,
+                                     const double           _Dt,
+                                     VectorComposite&       _x_plus_delta) const override;
+      Eigen::VectorXd deltaZero() const override;
+      Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+                                   const Eigen::VectorXd& delta_step) const override;
+
+      CaptureMotionPtr emplaceCapture(const FrameBasePtr&   _frame_own,
+                                      const SensorBasePtr&  _sensor,
+                                      const TimeStamp&      _ts,
+                                      const VectorXd&       _data,
+                                      const MatrixXd&       _data_cov,
+                                      const VectorXd&       _calib,
+                                      const VectorXd&       _calib_preint,
+                                      const CaptureBasePtr& _capture_origin_ptr) override;
+      void     emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+      VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
+      void     setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
 
     protected:
         ParamsProcessorOdom2dPtr params_odom_2d_;
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 8118344916d696f262fc3227dab735e2b56a4cc7..a06085407e85a45268e9336890d6ec73c8e8760f 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -81,47 +81,45 @@ class ProcessorOdom3d : public ProcessorMotion
 
     public:
         // Motion integration
-        void computeCurrentDelta(const Eigen::VectorXd& _data,
-                                         const Eigen::MatrixXd& _data_cov,
-                                         const Eigen::VectorXd& _calib,
-                                         const double _dt,
-                                         Eigen::VectorXd& _delta,
-                                         Eigen::MatrixXd& _delta_cov,
-                                         Eigen::MatrixXd& _jacobian_calib) const override;
-        void deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                            const Eigen::VectorXd& _delta2,
-                            const double _Dt2,
-                            Eigen::VectorXd& _delta1_plus_delta2) const override;
-        void deltaPlusDelta(const Eigen::VectorXd& _delta1,
-                            const Eigen::VectorXd& _delta2,
-                            const double _Dt2,
-                            Eigen::VectorXd& _delta1_plus_delta2,
-                            Eigen::MatrixXd& _jacobian1,
-                            Eigen::MatrixXd& _jacobian2) const override;
-        void statePlusDelta(const VectorComposite& _x,
-                            const Eigen::VectorXd& _delta,
-                            const double _Dt,
-                            VectorComposite& _x_plus_delta) const override;
-        Eigen::VectorXd deltaZero() const override;
-        Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
-                                     const Eigen::VectorXd& delta_step) const override;
+      void            computeCurrentDelta(const Eigen::VectorXd& _data,
+                                          const Eigen::MatrixXd& _data_cov,
+                                          const Eigen::VectorXd& _calib,
+                                          const double           _dt,
+                                          Eigen::VectorXd&       _delta,
+                                          Eigen::MatrixXd&       _delta_cov,
+                                          Eigen::MatrixXd&       _jacobian_calib) const override;
+      void            deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                     const Eigen::VectorXd& _delta2,
+                                     const double           _Dt2,
+                                     Eigen::VectorXd&       _delta1_plus_delta2) const override;
+      void            deltaPlusDelta(const Eigen::VectorXd& _delta1,
+                                     const Eigen::VectorXd& _delta2,
+                                     const double           _Dt2,
+                                     Eigen::VectorXd&       _delta1_plus_delta2,
+                                     Eigen::MatrixXd&       _jacobian1,
+                                     Eigen::MatrixXd&       _jacobian2) const override;
+      void            statePlusDelta(const VectorComposite& _x,
+                                     const Eigen::VectorXd& _delta,
+                                     const double           _Dt,
+                                     VectorComposite&       _x_plus_delta) const override;
+      Eigen::VectorXd deltaZero() const override;
+      Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+                                   const Eigen::VectorXd& delta_step) const override;
 
-        bool voteForKeyFrame() const override;
-        CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
-                                                const SensorBasePtr& _sensor,
-                                                const TimeStamp& _ts,
-                                                const VectorXd& _data,
-                                                const MatrixXd& _data_cov,
-                                                const VectorXd& _calib,
-                                                const VectorXd& _calib_preint,
-                                                const CaptureBasePtr& _capture_origin) override;
+      bool             voteForKeyFrame() const override;
 
-        FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
+      CaptureMotionPtr emplaceCapture(const FrameBasePtr&   _frame_own,
+                                      const SensorBasePtr&  _sensor,
+                                      const TimeStamp&      _ts,
+                                      const VectorXd&       _data,
+                                      const MatrixXd&       _data_cov,
+                                      const VectorXd&       _calib,
+                                      const VectorXd&       _calib_preint,
+                                      const CaptureBasePtr& _capture_origin) override;
+      void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
 
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
-                                            CaptureBasePtr _capture_origin) override;
-        VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
-        void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+      VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
+      void     setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
 
     protected:
         ParamsProcessorOdom3dPtr params_odom_3d_;
diff --git a/include/core/solver_suitesparse/ccolamd_ordering.h b/include/core/solver_suitesparse/ccolamd_ordering.h
index eee05794ae2aa545f1ccdd628ce628d99d2b005c..e1187b9d9580c0e9516b473723f1d8364ee2f040 100644
--- a/include/core/solver_suitesparse/ccolamd_ordering.h
+++ b/include/core/solver_suitesparse/ccolamd_ordering.h
@@ -25,9 +25,9 @@
 #include <iostream>
 
 // Eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/CholmodSupport>
-#include <eigen3/Eigen/SparseLU>
+#include <Eigen/OrderingMethods>
+#include <Eigen/CholmodSupport>
+#include <Eigen/SparseLU>
 
 // ccolamd
 #include "ccolamd.h"
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index f715761d8514009682066365cf67c59c72e227e3..06b87b641fcec83e1f7d3f29f5e62ecae9cb1171 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.h
@@ -38,8 +38,8 @@
 #include "solver/cost_function_sparse.h"
 
 // eigen includes
-#include <eigen3/Eigen/OrderingMethods>
-#include <eigen3/Eigen/SparseQR>
+#include <Eigen/OrderingMethods>
+#include <Eigen/SparseQR>
 #include <Eigen/StdVector>
 #include "core/factor/factor_pose_2d.h"
 
diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h
index 4c0f01633c0a591d62c25daafd3029e10caccfee..fffb34f3abf69e82923d0037d29a58e7c0b503ee 100644
--- a/include/core/solver_suitesparse/sparse_utils.h
+++ b/include/core/solver_suitesparse/sparse_utils.h
@@ -22,7 +22,7 @@
 #pragma once
 
 // eigen includes
-#include <eigen3/Eigen/Sparse>
+#include <Eigen/Sparse>
 
 namespace wolf
 {
diff --git a/prova.txt b/prova.txt
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index f61c1b1f9ef7ba4d4f3609fed41fedef53e3cbb8..6afd1c5c76c9c838e3072711b69ca199da0a48b9 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -144,14 +144,14 @@ void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before
 }
 
 
-void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs)
+void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, bool show_jacs, bool show_covs)
 {
     using std::cout;
     using std::endl;
 
-    if (!show_data && !show_delta && !show_delta_int && !show_jacs)
+    if (!show_data && !show_delta && !show_delta_int && !show_jacs && !show_covs)
     {
-        cout << "Buffer state [" << this->size() << "] : <";
+        cout << "Buffer size: " << this->size() << " ; time stamps: <";
         for (Motion mot : *this)
             cout << " " << mot.ts_;
         cout << " >" << endl;
@@ -165,17 +165,20 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
             if (show_data)
             {
                 cout << "   data: " << mot.data_.transpose() << endl;
-                cout << "   data cov: \n" << mot.data_cov_ << endl;
+                if (show_covs)
+                    cout << "   data cov: \n" << mot.data_cov_ << endl;
             }
             if (show_delta)
             {
                 cout << "   delta: " << mot.delta_.transpose() << endl;
-                cout << "   delta cov: \n" << mot.delta_cov_ << endl;
+                if (show_covs)
+                    cout << "   delta cov: \n" << mot.delta_cov_ << endl;
             }
             if (show_delta_int)
             {
                 cout << "   delta integrated: " << mot.delta_integr_.transpose() << endl;
-                cout << "   delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
+                if (show_covs)
+                    cout << "   delta integrated cov: \n" << mot.delta_integr_cov_ << endl;
             }
             if (show_jacs)
             {
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 3e1ea99bb600d4a94fa67e58db8a8146ed3fd4ce..9c4a4e6926e1699f3faf4686952636873046783f 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -167,28 +167,22 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
     return cap_motion;
 }
 
-FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
+void ProcessorDiffDrive::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
 {
-    auto key_feature_ptr = FeatureBase::emplace<FeatureDiffDrive>(_capture_motion,
-                                                               _capture_motion->getBuffer().back().delta_integr_,
-                                                               _capture_motion->getBuffer().back().delta_integr_cov_,
-                                                               _capture_motion->getCalibrationPreint(),
-                                                               _capture_motion->getBuffer().back().jacobian_calib_);
-
-    return key_feature_ptr;
+    auto feature = FeatureBase::emplace<FeatureDiffDrive>(_capture_own,
+                                                          _capture_own->getBuffer().back().delta_integr_,
+                                                          _capture_own->getBuffer().back().delta_integr_cov_,
+                                                          _capture_own->getCalibrationPreint(),
+                                                          _capture_own->getBuffer().back().jacobian_calib_);
+
+    auto ftr_motion = std::static_pointer_cast<FeatureMotion>(feature);
+    FactorBase::emplace<FactorDiffDrive>(ftr_motion,
+                                         ftr_motion,
+                                         _capture_origin,
+                                         shared_from_this(),
+                                         params_prc_diff_drv_selfcal_->apply_loss_function);
 }
 
-FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
-                                                CaptureBasePtr _capture_origin)
-{
-    auto ftr_motion = std::static_pointer_cast<FeatureMotion>(_feature);
-    auto fac_odom = FactorBase::emplace<FactorDiffDrive>(ftr_motion,
-                                                         ftr_motion,
-                                                         _capture_origin,
-                                                         shared_from_this(),
-                                                         params_prc_diff_drv_selfcal_->apply_loss_function);
-    return fac_odom;
-}
 
 } /* namespace wolf */
 
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index d47ea9b91149949c0f94dfa757746deec525982c..8549943aa71439543fdb74e0a66922342f8b80d0 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -58,11 +58,13 @@ ProcessorMotion::ProcessorMotion(const std::string& _type,
         calib_preint_(_calib_size),
         jacobian_delta_preint_(delta_cov_size_, delta_cov_size_),
         jacobian_delta_(delta_cov_size_, delta_cov_size_),
-        jacobian_calib_(delta_cov_size_, calib_size_)
+        jacobian_calib_(delta_cov_size_, calib_size_),
+        jacobian_delta_calib_(delta_cov_size_, calib_size_)
 {   
     jacobian_delta_preint_          .setIdentity(delta_cov_size_,delta_cov_size_);                                    // dDp'/dDp, dDv'/dDv, all zeros
     jacobian_delta_                 .setIdentity(delta_cov_size_,delta_cov_size_);                                           //
     jacobian_calib_                 .setZero(delta_cov_size_,calib_size_);
+    jacobian_delta_calib_           .setZero(delta_cov_size_,calib_size_);
     unmeasured_perturbation_cov_ =
               params_motion_->unmeasured_perturbation_std
             * params_motion_->unmeasured_perturbation_std
@@ -109,8 +111,7 @@ void ProcessorMotion::mergeCaptures(CaptureMotionPtr cap_prev,
     // emplace new feature and factor (if origin has frame)
     if (cap_prev->getOriginCapture() and cap_prev->getOriginCapture()->getFrame())
     {
-        auto new_feature = emplaceFeature(cap_target);
-        emplaceFactor(new_feature, cap_prev->getOriginCapture());
+        emplaceFeaturesAndFactors(cap_prev->getOriginCapture(), cap_target);
     }
 }
 
@@ -302,11 +303,9 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             // and give the part of the buffer before the new keyframe to the capture for the KF callback
             splitBuffer(capture_existing, timestamp_from_callback, capture_for_keyframe_callback);
 
-            // create motion feature and add it to the capture
-            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(feature_new, capture_origin );
+            // // create motion feature and add it to the capture
+            // // create motion factor and add it to the feature, and constrain to the other capture (origin)
+            emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback);
 
             // modify existing feature and factor (if they exist in the existing capture)
             if (!capture_existing->getFeatureList().empty())
@@ -316,9 +315,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
 
                 assert(capture_existing->getFeatureList().empty());// there was only one feature!
 
-                auto new_feature_existing = emplaceFeature(capture_existing);
-
-                emplaceFactor(new_feature_existing, capture_for_keyframe_callback);
+                emplaceFeaturesAndFactors(capture_for_keyframe_callback, capture_existing);
             }
 
             break;
@@ -396,10 +393,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
             splitBuffer(capture_existing, timestamp_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, capture_origin );
+            emplaceFeaturesAndFactors(capture_origin, capture_for_keyframe_callback);
 
             // reset processor origin
             origin_ptr_ = capture_for_keyframe_callback;
@@ -467,10 +462,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
         keyframe            ->link(getProblem());
 
         // create motion feature and add it to the key_capture
-        auto key_feature    = emplaceFeature(last_ptr_);
-
         // create motion factor and link it to parent feature and other frame (which is origin's frame)
-        auto factor         = emplaceFactor(key_feature, origin_ptr_);
+        emplaceFeaturesAndFactors(origin_ptr_, last_ptr_);
 
         // create a new frame
         auto frame_new      = std::make_shared<FrameBase>(getTimeStamp(),
@@ -985,23 +978,23 @@ FrameBasePtr ProcessorMotion::computeProcessingStep()
             if (checkTimeTolerance(keyframe_from_callback,
                                    incoming_ptr_))
             {
-                WOLF_DEBUG("First time with a KF compatible.")
+                WOLF_DEBUG("PM ", getName(), ": First time with a KF compatible.")
                 processing_step_ = FIRST_TIME_WITH_KF_ON_INCOMING;
             }
             else if (keyframe_from_callback->getTimeStamp() < incoming_ptr_->getTimeStamp())
             {
-                WOLF_DEBUG("First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
+                WOLF_DEBUG("PM ", getName(), ": First time with a KF too old. It seems the prior has been set before receiving the first capture of this processor.")
                 processing_step_ = FIRST_TIME_WITH_KF_BEFORE_INCOMING;
             }
             else
             {
-                WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+                WOLF_DEBUG("PM ", getName(), ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
                 processing_step_ = FIRST_TIME_WITH_KF_AFTER_INCOMING;
             }
         }
         else
         {
-            WOLF_DEBUG("First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
+            WOLF_DEBUG("PM ", getName(), ": First time with a KF newer than the first capture. It only can happen if prior mode is 'nothing'")
             processing_step_ = FIRST_TIME_WITHOUT_KF;
         }
 
@@ -1047,7 +1040,7 @@ TimeStamp ProcessorMotion::getTimeStamp ( ) const
         origin_ptr_->isRemoving() or
         not last_ptr_)
     {
-        WOLF_DEBUG("Processor has no time stamp. Returning a non-valid timestamp equal to 0");
+        WOLF_DEBUG("PM proc \"", getName(), "\" has no time stamp. Returning a non-valid timestamp equal to 0");
         return TimeStamp::Invalid();
     }
 
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index c6c0e34c4ae47a507a235d990810f52745a3db04..84c916b90d68f4aef67a502ec795118e26c10808 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -167,29 +167,17 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
     return cap_motion;
 }
 
-FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
+void ProcessorOdom2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
 {
-    auto fac_odom = FactorBase::emplace<FactorRelativePose2d>(_feature,
-                                                              _feature,
-                                                              _capture_origin->getFrame(),
-                                                              shared_from_this(),
-                                                              params_->apply_loss_function,
-                                                              TOP_MOTION);
-    return fac_odom;
-}
-
-FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
-    Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_;
+    Eigen::MatrixXd covariance = _capture_own->getBuffer().back().delta_integr_cov_;
     makePosDef(covariance);
 
-    FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
-                                                                       "ProcessorOdom2d",
-                                                                       _capture_motion->getBuffer().back().delta_integr_,
-                                                                       covariance);
-    return key_feature_ptr;
-}
+    FeatureBasePtr feature = FeatureBase::emplace<FeatureBase>(
+        _capture_own, "ProcessorOdom2d", _capture_own->getBuffer().back().delta_integr_, covariance);
 
+    FactorBase::emplace<FactorRelativePose2d>(
+        feature, feature, _capture_origin->getFrame(), shared_from_this(), params_->apply_loss_function, TOP_MOTION);
+}
 
 
 } /* namespace wolf */
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 26a2a8d19b8c1156443638263bc92c68c9220ff4..b9a53c2a3ae875a068ad926bb5e680378cf5f726 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -58,9 +58,8 @@ void ProcessorOdom3d::computeCurrentDelta(const Eigen::VectorXd& _data,
     if (_data.size() == (long int)6)
     {
         // rotation in vector form
-        _delta.head<3>()    = _data.head<3>();
-        Eigen::Map<Eigen::Quaterniond> q(_delta.data() + 3);
-        q                   = v2q(_data.tail<3>());
+        _delta.head<3>() = _data.head<3>();
+        _delta.tail<4>() = v2q(_data.tail<3>()).coeffs();
     }
     else
     {
@@ -217,15 +216,6 @@ CaptureMotionPtr ProcessorOdom3d::emplaceCapture(const FrameBasePtr& _frame_own,
     return cap_motion;
 }
 
-FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
-    FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
-                                                                       "ProcessorOdom3d",
-                                                                       _capture_motion->getBuffer().back().delta_integr_,
-                                                                       _capture_motion->getBuffer().back().delta_integr_cov_);
-    return key_feature_ptr;
-}
-
 Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_preint,
                                                const Eigen::VectorXd& delta_step) const
 {
@@ -234,15 +224,16 @@ Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_prei
     return delta_corrected;
 }
 
-FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+
+void ProcessorOdom3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
 {
-    auto fac_odom = FactorBase::emplace<FactorRelativePose3d>(_feature_motion,
-                                                              _feature_motion,
-                                                              _capture_origin->getFrame(),
-                                                              shared_from_this(),
-                                                              params_->apply_loss_function,
-                                                              TOP_MOTION);
-    return fac_odom;
+    Eigen::MatrixXd covariance = _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_;
+
+    FeatureBasePtr feature = FeatureBase::emplace<FeatureBase>(
+        _capture_own, "ProcessorOdom3d", _capture_own->getBuffer().back().delta_integr_, covariance);
+        
+    FactorBase::emplace<FactorRelativePose3d>(
+        feature, feature, _capture_origin->getFrame(), shared_from_this(), params_->apply_loss_function, TOP_MOTION);
 }
 
 VectorXd ProcessorOdom3d::getCalibration (const CaptureBaseConstPtr _capture) const
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 0892276de37c33ad0390f2a01b38e14400375f29..94e30fd1f20d1526a9580f7b1a2a5a94083dfede 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -112,7 +112,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             // Process info
             // TrackerFeature:  We only process new features in Last, here last = nullptr, so we do not have anything to do.
             // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
-            processKnown();
+            if (not getProblem()->getMap()->getLandmarkList().empty())
+                processKnown();
 
             // Issue KF callback with new KF
             getProblem()->keyFrameCallback(keyframe, shared_from_this());
@@ -148,14 +149,16 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_KEYFRAME" );
 
             // Make a NON KEY Frame to hold incoming capture
-            FrameBasePtr frame = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
-                                                             getProblem()->getFrameTypes(),
-                                                             getProblem()->getState());
-            incoming_ptr_->link(frame);
+            FrameBasePtr keyframe = std::make_shared<FrameBase>(incoming_ptr_->getTimeStamp(),
+                                                                getProblem()->getFrameTypes(),
+                                                                getProblem()->getState());
+            incoming_ptr_->link(keyframe);
 
             // Process info
             // TrackerLandmark: If we have been given a map, all landmarks in the map are known. Process them.
-            processKnown();
+            if (not getProblem()->getMap()->getLandmarkList().empty())
+                processKnown();
+            
             // Both Trackers:  We have a last_ Capture with not enough features, so populate it.
             processNew(params_tracker_->max_new_features);
 
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0db3b19e06f4c0c359009afaeaed21c29dc99e85..820f1b9968a890d028763d69f2210fee5fed3fb4 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -554,8 +554,18 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
         {
             auto sb = getStateBlockDynamic(key);
             if (sb)
-                _stream << _tabs << "  " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
-                        << std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl;
+            {
+                _stream << _tabs << "  " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", "
+                        << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] = ( " << std::setprecision(3)
+                        << sb->getState().transpose() << " )";
+                if (params_prior_map_.count(key))
+                {
+                    auto ftr = params_prior_map_.at(key);
+                    _stream << " ; mean = ( " << ftr->getMeasurement().transpose() << " )";
+                    _stream << " , std = ( " << ftr->getMeasurementCovariance().diagonal().array().sqrt().transpose() << " )";
+                }
+                _stream << " @ " << sb << std::endl;
+            }
         }
     }
     else if (_metric)
@@ -570,7 +580,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
         {
             auto sb = getStateBlockDynamic(key);
             if (sb)
-                _stream << "    " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
+                _stream << "    " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << ", " << (isStateBlockDynamic(key) ? "Dyn" : "Sta") << "] @ " << sb;
         }
         _stream << std::endl;
     }
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index ed4aeede54f33e274dbd6cc9c52837f22be2869c..411a057635d27c7ac164536525a34ae28caf8268 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -108,16 +108,16 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
             return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
         }
 
-        FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
-        {
-            return Base::emplaceFeature(_capture_own);
-        }
-
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
-                                            CaptureBasePtr _capture_origin) override
-        {
-            return Base::emplaceFactor(_feature_motion, _capture_origin);
-        }
+        // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
+        // {
+        //     return Base::emplaceFeature(_capture_own);
+        // }
+
+        // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+        //                                     CaptureBasePtr _capture_origin) override
+        // {
+        //     return Base::emplaceFactor(_feature_motion, _capture_origin);
+        // }
 
         ParamsProcessorDiffDrivePtr getParams()
         {
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 47cc6069c7bcb90dc4f1ea2307cef2d95b0622ff..345818253dfeede026481d1665f0dcef680c35e2 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -98,16 +98,16 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
             return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
         }
 
-        FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
-        {
-            return Base::emplaceFeature(_capture_own);
-        }
-
-        FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
-                                            CaptureBasePtr _capture_origin) override
-        {
-            return Base::emplaceFactor(_feature_motion, _capture_origin);
-        }
+        // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
+        // {
+        //     return Base::emplaceFeature(_capture_own);
+        // }
+
+        // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+        //                                     CaptureBasePtr _capture_origin) override
+        // {
+        //     return Base::emplaceFactor(_feature_motion, _capture_origin);
+        // }
 
         ParamsProcessorDiffDrivePtr getParams()
         {
diff --git a/test/gtest_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp
index 73f873272b402fcf226b4ae3b2eaff746bd3072a..16fb0d7c554372b4a0a98d71f75dbe474819c695 100644
--- a/test/gtest_yaml_conversions.cpp
+++ b/test/gtest_yaml_conversions.cpp
@@ -24,7 +24,7 @@
 #include "core/utils/utils_gtest.h"
 #include <yaml-schema-cpp/yaml_conversion.hpp>
 #include <yaml-cpp/yaml.h>
-#include <eigen3/Eigen/Dense>
+#include <Eigen/Dense>
 #include <iostream>
 
 using namespace Eigen;