diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index 917c560454d7929f3d12968df6c0f279b1209892..59461de96f0165360b4564ca232fe343ea0130dd 100644
--- a/src/constraint_AHP.h
+++ b/src/constraint_AHP.h
@@ -25,12 +25,13 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
 
     public:
 
-        ConstraintAHP(FeatureBasePtr    _ftr_ptr,
-                      LandmarkAHPPtr    _landmark_ptr,
+        ConstraintAHP(const ProcessorBasePtr& _processor_ptr,
+                      const FeatureBasePtr&   _ftr_ptr,
+                      const LandmarkAHPPtr&   _landmark_ptr,
                       bool              _apply_loss_function = false,
                       ConstraintStatus  _status = CTR_ACTIVE);
 
-        virtual ~ConstraintAHP();
+        virtual ~ConstraintAHP() = default;
 
         template<typename T>
         void expectation(const T* const _current_frame_p,
@@ -52,22 +53,25 @@ class ConstraintAHP : public ConstraintSparse<2, 3, 4, 3, 4, 4>
 
         /** \brief Returns the jacobians computation method
          **/
-        virtual JacobianMethod getJacobianMethod() const;
+        virtual JacobianMethod getJacobianMethod() const override;
 
 
         // Static creator method
-        static ConstraintAHPPtr create(FeatureBasePtr   _ftr_ptr,
-                                       LandmarkAHPPtr   _lmk_ahp_ptr,
+        static ConstraintAHPPtr create(const ProcessorBasePtr& _processor_ptr,
+                                       const FeatureBasePtr&   _ftr_ptr,
+                                       const LandmarkAHPPtr&   _lmk_ahp_ptr,
                                        bool             _apply_loss_function = false,
                                        ConstraintStatus _status              = CTR_ACTIVE);
 
 };
 
-inline ConstraintAHP::ConstraintAHP(FeatureBasePtr   _ftr_ptr,
-                                    LandmarkAHPPtr   _landmark_ptr,
+inline ConstraintAHP::ConstraintAHP(const ProcessorBasePtr& _processor_ptr,
+                                    const FeatureBasePtr&   _ftr_ptr,
+                                    const LandmarkAHPPtr&   _landmark_ptr,
                                     bool             _apply_loss_function,
                                     ConstraintStatus _status) :
         ConstraintSparse<2, 3, 4, 3, 4, 4>(CTR_AHP,
+                                           _processor_ptr,
                                            _landmark_ptr->getAnchorFrame(),
                                            nullptr,
                                            _landmark_ptr,
@@ -86,11 +90,6 @@ inline ConstraintAHP::ConstraintAHP(FeatureBasePtr   _ftr_ptr,
     distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getDistortionVector();
 }
 
-inline ConstraintAHP::~ConstraintAHP()
-{
-    //
-}
-
 inline Eigen::VectorXs ConstraintAHP::expectation() const
 {
     FrameBasePtr frm_current = getFeaturePtr()->getCapturePtr()->getFramePtr();
@@ -193,13 +192,14 @@ inline wolf::JacobianMethod ConstraintAHP::getJacobianMethod() const
     return JAC_AUTO;
 }
 
-inline wolf::ConstraintAHPPtr ConstraintAHP::create(FeatureBasePtr   _ftr_ptr,
-                                                    LandmarkAHPPtr   _lmk_ahp_ptr,
+inline wolf::ConstraintAHPPtr ConstraintAHP::create(const ProcessorBasePtr& _processor_ptr,
+                                                    const FeatureBasePtr&   _ftr_ptr,
+                                                    const LandmarkAHPPtr&   _lmk_ahp_ptr,
                                                     bool             _apply_loss_function,
                                                     ConstraintStatus _status)
 {
     // construct constraint
-    ConstraintAHPPtr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _lmk_ahp_ptr, _apply_loss_function, _status);
+    ConstraintAHPPtr ctr_ahp = std::make_shared<ConstraintAHP>(_processor_ptr, _ftr_ptr, _lmk_ahp_ptr, _apply_loss_function, _status);
 
     // link it to wolf tree <-- these pointers cannot be set at construction time
     _lmk_ahp_ptr->getAnchorFrame()->addConstrainedBy(ctr_ahp);
diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp
index 940d4a7f1b17648e9733dba8f447bacdfdbff865..6fb48b8951a8ae3ea22ed86ce517d8f78e57f077 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -13,20 +13,20 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, bool _apply_loss_func
     resizeVectors();
 }
 
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr, const FrameBasePtr& _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
                                        StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
                                        StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
-            ConstraintBase(_tp, _frame_ptr, nullptr, nullptr, _apply_loss_function, _status),
+            ConstraintBase(_tp, _processor_ptr, _frame_ptr, nullptr, nullptr, _apply_loss_function, _status),
             state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
                                _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
 {
     resizeVectors();
 }
 
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
                                        StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
                                        StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
-            ConstraintBase( _tp, nullptr, _feature_ptr, nullptr, _apply_loss_function, _status),
+            ConstraintBase( _tp, _processor_ptr, nullptr, _feature_ptr, nullptr, _apply_loss_function, _status),
             state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
                                _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
 {
@@ -34,23 +34,16 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FeatureBasePtr _featu
 }
 
 
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr, const LandmarkBasePtr& _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
                                        StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
                                        StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
-            ConstraintBase( _tp, nullptr, nullptr, _landmark_ptr, _apply_loss_function, _status),
+            ConstraintBase( _tp, _processor_ptr, nullptr, nullptr, _landmark_ptr, _apply_loss_function, _status),
             state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
                                _state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
 {
     resizeVectors();
 }
 
-
-ConstraintAnalytic::~ConstraintAnalytic()
-{
-    //
-}
-
-
 const std::vector<Scalar*> ConstraintAnalytic::getStateScalarPtrVector()
 {
     assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index 2ac52e0c2c1c3a458a4dcc256944c044ec4faf0b..025ffb67121cc2c359cf199feca785c5e5aacfb6 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -40,7 +40,8 @@ class ConstraintAnalytic: public ConstraintBase
          * Constructor of category CTR_FRAME
          *
          **/
-        ConstraintAnalytic(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
+        ConstraintAnalytic(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr,
+                           const FrameBasePtr& _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
                          StateBlockPtr _state0Ptr,
                          StateBlockPtr _state1Ptr = nullptr,
                          StateBlockPtr _state2Ptr = nullptr,
@@ -57,7 +58,8 @@ class ConstraintAnalytic: public ConstraintBase
          * Constructor of category CTR_FEATURE
          *
          **/
-        ConstraintAnalytic(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
+        ConstraintAnalytic(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr,
+                           const FeatureBasePtr& _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
                          StateBlockPtr _state0Ptr,
                          StateBlockPtr _state1Ptr = nullptr,
                          StateBlockPtr _state2Ptr = nullptr,
@@ -74,7 +76,8 @@ class ConstraintAnalytic: public ConstraintBase
          * Constructor of category CTR_LANDMARK
          *
          **/
-        ConstraintAnalytic(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
+        ConstraintAnalytic(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr,
+                           const LandmarkBasePtr& _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
                          StateBlockPtr _state0Ptr,
                          StateBlockPtr _state1Ptr = nullptr,
                          StateBlockPtr _state2Ptr = nullptr,
@@ -86,21 +89,21 @@ class ConstraintAnalytic: public ConstraintBase
                          StateBlockPtr _state8Ptr = nullptr,
                          StateBlockPtr _state9Ptr = nullptr ) ;
 
-        virtual ~ConstraintAnalytic();
+        virtual ~ConstraintAnalytic() = default;
 
         /** \brief Returns a vector of pointers to the state blocks
          *
          * Returns a vector of pointers to the state blocks in which this constraint depends
          *
          **/
-        virtual const std::vector<Scalar*> getStateScalarPtrVector();
+        virtual const std::vector<Scalar*> getStateScalarPtrVector() override;
 
         /** \brief Returns a vector of pointers to the states
          *
          * Returns a vector of pointers to the state in which this constraint depends
          *
          **/
-        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const;
+        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
 
         /** \brief Returns a vector of sizes of the state blocks
          *
@@ -114,7 +117,7 @@ class ConstraintAnalytic: public ConstraintBase
          * Returns the constraint residual size
          *
          **/
-        virtual unsigned int getSize() const = 0;
+//        virtual unsigned int getSize() const = 0;
 
         /** \brief Returns the residual evaluated in the states provided
          *
@@ -150,7 +153,7 @@ class ConstraintAnalytic: public ConstraintBase
          * Returns the jacobians computation method
          *
          **/
-        virtual JacobianMethod getJacobianMethod() const;
+        virtual JacobianMethod getJacobianMethod() const override;
 
     private:
         void resizeVectors();
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index db055687a49734f5860399ac2d96c727943024f0..86560b693677c97117bf5833c41dc8fa0d3e9a46 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -16,12 +16,15 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, bool _apply_loss_function, Co
     apply_loss_function_(_apply_loss_function),
     frame_other_ptr_(), // nullptr
     feature_other_ptr_(), // nullptr
-    landmark_other_ptr_() // nullptr
+    landmark_other_ptr_(), // nullptr
+    processor_ptr_() // nullptr
 {
 //    std::cout << "constructed        +c" << id() << std::endl;
 }
 
-ConstraintBase::ConstraintBase(ConstraintType _tp, FrameBasePtr _frame_other_ptr, FeatureBasePtr _feature_other_ptr, LandmarkBasePtr _landmark_other_ptr, bool _apply_loss_function, ConstraintStatus _status) :
+ConstraintBase::ConstraintBase(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr,
+                               const FrameBasePtr& _frame_other_ptr, const FeatureBasePtr& _feature_other_ptr,
+                               const LandmarkBasePtr& _landmark_other_ptr, bool _apply_loss_function, ConstraintStatus _status) :
     NodeBase("CONSTRAINT", "Base"),
     feature_ptr_(),
     is_removing_(false),
@@ -31,7 +34,8 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, FrameBasePtr _frame_other_ptr
     apply_loss_function_(_apply_loss_function),
     frame_other_ptr_(_frame_other_ptr),
     feature_other_ptr_(_feature_other_ptr),
-    landmark_other_ptr_(_landmark_other_ptr)
+    landmark_other_ptr_(_landmark_other_ptr),
+    processor_ptr_(_processor_ptr)
 {
 //    std::cout << "constructed        +c" << id() << std::endl;
 }
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 039f3e085812edff29afa459724439652a25f82a..4d2d8e4739cde7c9e9ec740f9865fb2549acdd75 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -32,16 +32,19 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
         FrameBaseWPtr frame_other_ptr_;                    ///< FrameBase pointer (for category CTR_FRAME)
         FeatureBaseWPtr feature_other_ptr_;                ///< FeatureBase pointer (for category CTR_FEATURE)
         LandmarkBaseWPtr landmark_other_ptr_;              ///< LandmarkBase pointer (for category CTR_LANDMARK)
+        ProcessorBaseWPtr processor_ptr_;
 
     public:
 
         /** \brief Constructor of category CTR_ABSOLUTE
          **/
-        ConstraintBase(ConstraintType _tp, bool _apply_loss_function, ConstraintStatus _status);
+        ConstraintBase(ConstraintType _tp,  bool _apply_loss_function, ConstraintStatus _status);
 
         /** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK)
          **/
-        ConstraintBase(ConstraintType _tp, FrameBasePtr _frame_other_ptr, FeatureBasePtr _feature_other_ptr, LandmarkBasePtr _landmark_other_ptr, bool _apply_loss_function, ConstraintStatus _status);
+        ConstraintBase(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr,
+                       const FrameBasePtr& _frame_other_ptr, const FeatureBasePtr& _feature_other_ptr,
+                       const LandmarkBasePtr& _landmark_other_ptr, bool _apply_loss_function, ConstraintStatus _status);
 
         virtual ~ConstraintBase();
         void remove();
@@ -120,6 +123,22 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
         LandmarkBasePtr getLandmarkOtherPtr() const;
         void setLandmarkOtherPtr(LandmarkBasePtr _lmk_o){landmark_other_ptr_ = _lmk_o;}
 
+        /**
+         * @brief getProcessor
+         * @return
+         */
+        ProcessorBasePtr getProcessor() const;
+
+        /**
+         * @brief setProcessor
+         * @param _processor_ptr
+         */
+        void setProcessor(const ProcessorBasePtr& _processor_ptr);
+
+        /**
+         * @brief getProblem
+         * @return
+         */
         ProblemPtr getProblem();
 
 };
@@ -212,5 +231,15 @@ inline LandmarkBasePtr ConstraintBase::getLandmarkOtherPtr() const
     return landmark_other_ptr_.lock();
 }
 
+inline ProcessorBasePtr ConstraintBase::getProcessor() const
+{
+  return processor_ptr_.lock();
+}
+
+inline void ConstraintBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
+{
+  processor_ptr_ = _processor_ptr;
+}
+
 } // namespace wolf
 #endif
diff --git a/src/constraint_container.h b/src/constraint_container.h
index 26f566ec53010e216c70197a507354df8efea1b7..d744b0794f5f0ce7ff471638572f1be39b0f511e 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -18,8 +18,10 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
 
 	public:
 
-	    ConstraintContainer(FeatureBasePtr _ftr_ptr, LandmarkContainerPtr _lmk_ptr, const unsigned int _corner, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-			ConstraintSparse<3,2,1,2,1>(CTR_CONTAINER, nullptr, nullptr, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
+      ConstraintContainer(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _ftr_ptr,
+                          const LandmarkContainerPtr& _lmk_ptr, const unsigned int _corner,
+                          bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+      ConstraintSparse<3,2,1,2,1>(CTR_CONTAINER, _processor_ptr, nullptr, nullptr, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
 			lmk_ptr_(_lmk_ptr),
 			corner_(_corner)
 		{
@@ -29,10 +31,7 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
             std::cout << "new constraint container: corner idx = " << corner_ << std::endl;
 		}
 
-		virtual ~ConstraintContainer()
-		{
-			//std::cout << "deleting ConstraintContainer " << id() << std::endl;
-		}
+    virtual ~ConstraintContainer() = default;
 
 		LandmarkContainerPtr getLandmarkPtr()
 		{
@@ -120,18 +119,20 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
          * Returns the jacobians computation method
          *
          **/
-        virtual JacobianMethod getJacobianMethod() const
+        virtual JacobianMethod getJacobianMethod() const override
         {
             return JAC_AUTO;
         }
 
 
     public:
-        static ConstraintBasePtr create(FeatureBasePtr _feature_ptr, NodeBasePtr _correspondant_ptr)
+        static ConstraintBasePtr create(const ProcessorBasePtr& _processor_ptr,
+                                        const FeatureBasePtr& _feature_ptr,
+                                        const NodeBasePtr& _correspondant_ptr)
         {
             unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
 
-            return std::make_shared<ConstraintContainer>(_feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), corner);
+            return std::make_shared<ConstraintContainer>(_processor_ptr, _feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), corner);
         }
 
 };
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index 364c6d5445a8a61e79fed978559070f96c451756..6bfd54aff47ec742375ec4c518478f77136e7c05 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -13,16 +13,15 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
 {
 	public:
 
-		ConstraintCorner2D(FeatureBasePtr _ftr_ptr, LandmarkCorner2DPtr _lmk_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-			ConstraintSparse<3,2,1,2,1>(CTR_CORNER_2D, nullptr, nullptr, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
+    ConstraintCorner2D(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr _ftr_ptr,
+                       const LandmarkCorner2DPtr _lmk_ptr, bool _apply_loss_function = false,
+                       ConstraintStatus _status = CTR_ACTIVE) :
+      ConstraintSparse<3,2,1,2,1>(CTR_CORNER_2D, _processor_ptr, nullptr, nullptr, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
 		{
             setType("CORNER 2D");
 		}
 
-        virtual ~ConstraintCorner2D()
-        {
-            //std::cout << "deleting ConstraintCorner2D " << id() << std::endl;
-        }
+        virtual ~ConstraintCorner2D() = default;
 
         LandmarkCorner2DPtr getLandmarkPtr()
 		{
@@ -38,15 +37,15 @@ class ConstraintCorner2D: public ConstraintSparse<3,2,1,2,1>
          * Returns the jacobians computation method
          *
          **/
-        virtual JacobianMethod getJacobianMethod() const
+        virtual JacobianMethod getJacobianMethod() const override
         {
             return JAC_AUTO;
         }
 
     public:
-        static ConstraintBasePtr create(FeatureBasePtr _feature_ptr, NodeBasePtr _correspondant_ptr)
+        static ConstraintBasePtr create(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr)
         {
-            return std::make_shared<ConstraintCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr) );
+            return std::make_shared<ConstraintCorner2D>(_processor_ptr, _feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr) );
         }
 
 };
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 03f27be071ef16fc55822a1ee4b720016b156909..cf2e41233bcef082aed772c484d6a10562fdda13 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -11,44 +11,47 @@ WOLF_PTR_TYPEDEFS(ConstraintEpipolar);
 class ConstraintEpipolar : public ConstraintBase
 {
     public:
-        ConstraintEpipolar(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE);
+        ConstraintEpipolar(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _feature_ptr,
+                           const FeatureBasePtr& _feature_other_ptr, bool _apply_loss_function = false,
+                           ConstraintStatus _status = CTR_ACTIVE);
 
-        virtual ~ConstraintEpipolar();
+        virtual ~ConstraintEpipolar() = default;
 
         /** \brief Returns the jacobians computation method
          **/
-        virtual JacobianMethod getJacobianMethod() const{return JAC_ANALYTIC;}
+        virtual JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
 
         /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
          **/
-        virtual const std::vector<Scalar*> getStateScalarPtrVector(){return std::vector<Scalar*>(0);}
+        virtual const std::vector<Scalar*> getStateScalarPtrVector() override {return std::vector<Scalar*>(0);}
 
         /** \brief Returns a vector of pointers to the states in which this constraint depends
          **/
-        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const{return std::vector<StateBlockPtr>(0);}
+        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
 
         /** \brief Returns the constraint residual size
          **/
-        virtual unsigned int getSize() const{return 0;}
+        virtual unsigned int getSize() const override {return 0;}
 
     public:
-        static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
-                NodeBasePtr _correspondant_ptr);
+        static wolf::ConstraintBasePtr create(const ProcessorBasePtr& _processor_ptr,
+                                              const FeatureBasePtr& _feature_ptr,
+                                              const NodeBasePtr& _correspondant_ptr);
 
 };
 
-inline ConstraintEpipolar::ConstraintEpipolar(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr, bool _apply_loss_function, ConstraintStatus _status) :
-        ConstraintBase(CTR_EPIPOLAR, nullptr, _feature_other_ptr, nullptr, _apply_loss_function, _status)
+inline ConstraintEpipolar::ConstraintEpipolar(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _feature_ptr,
+                                              const FeatureBasePtr& _feature_other_ptr, bool _apply_loss_function, ConstraintStatus _status) :
+        ConstraintBase(CTR_EPIPOLAR, _processor_ptr, nullptr, _feature_other_ptr, nullptr, _apply_loss_function, _status)
 {
     setType("EPIPOLAR");
 }
 
-inline ConstraintEpipolar::~ConstraintEpipolar(){}
-
-inline wolf::ConstraintBasePtr ConstraintEpipolar::create(FeatureBasePtr _feature_ptr, //
-        NodeBasePtr _correspondant_ptr)
+inline wolf::ConstraintBasePtr ConstraintEpipolar::create(const ProcessorBasePtr& _processor_ptr,
+                                                          const FeatureBasePtr& _feature_ptr,
+                                                          const NodeBasePtr& _correspondant_ptr)
 {
-    return std::make_shared<ConstraintEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr));
+    return std::make_shared<ConstraintEpipolar>(_processor_ptr, _feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr));
 }
 
 } // namespace wolf
diff --git a/src/constraint_gps_pseudorange_2D.h b/src/constraint_gps_pseudorange_2D.h
index b2e2a2cdd389af5b6608a95a4015dd11a956eb37..409dc35699f5e10a1dbe2c7702fb8106e9b0b2b6 100644
--- a/src/constraint_gps_pseudorange_2D.h
+++ b/src/constraint_gps_pseudorange_2D.h
@@ -27,7 +27,7 @@ WOLF_PTR_TYPEDEFS(ConstraintGPSPseudorange2D);
 class ConstraintGPSPseudorange2D : public ConstraintSparse<1, 2, 1, 3, 1, 3, 1>
 {
     public:
-        ConstraintGPSPseudorange2D(FeatureBasePtr _ftr_ptr, 
+        ConstraintGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr,
                                    bool _apply_loss_function = false, 
                                    ConstraintStatus _status = CTR_ACTIVE) :
             ConstraintSparse<1, 2, 1, 3, 1, 3, 1>(CTR_GPS_PR_2D,
@@ -47,10 +47,7 @@ class ConstraintGPSPseudorange2D : public ConstraintSparse<1, 2, 1, 3, 1, 3, 1>
             //std::cout << "ConstraintGPSPseudorange2D()  pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
         }
 
-        virtual ~ConstraintGPSPseudorange2D()
-        {
-            //std::cout << "deleting ConstraintGPSPseudorange2D " << id() << std::endl;
-        }
+        virtual ~ConstraintGPSPseudorange2D() = default;
 
         template<typename T>
         bool operator ()(const T* const _vehicle_p, const T* const _vehicle_o, const T* const _sensor_p,
@@ -62,7 +59,7 @@ class ConstraintGPSPseudorange2D : public ConstraintSparse<1, 2, 1, 3, 1, 3, 1>
         * Returns the jacobians computation method
         *
         **/
-        virtual JacobianMethod getJacobianMethod() const
+        virtual JacobianMethod getJacobianMethod() const override
         {
             return JAC_AUTO;
         }
diff --git a/src/constraint_gps_pseudorange_3D.h b/src/constraint_gps_pseudorange_3D.h
index e7d7c93e6ca773ac017263fca8c9d24a36ce4724..c87cb87933717f9ef30fc25097e012982710afa7 100644
--- a/src/constraint_gps_pseudorange_3D.h
+++ b/src/constraint_gps_pseudorange_3D.h
@@ -46,12 +46,7 @@ class ConstraintGPSPseudorange3D: public ConstraintSparse<1, 3, 4, 3, 1, 3, 4>
         }
 
 
-        virtual ~ConstraintGPSPseudorange3D()
-        {
-            //std::cout << "deleting ConstraintGPSPseudorange3D " << id() << std::endl;
-        }
-
-
+        virtual ~ConstraintGPSPseudorange3D() = default;
 
         template<typename T>
         bool operator ()(const T* const _vehicle_p, const T* const _vehicle_o, const T* const _sensor_p,
@@ -64,7 +59,7 @@ class ConstraintGPSPseudorange3D: public ConstraintSparse<1, 3, 4, 3, 1, 3, 4>
         * Returns the jacobians computation method
         *
         **/
-        virtual JacobianMethod getJacobianMethod() const
+        virtual JacobianMethod getJacobianMethod() const override
         {
             return JAC_AUTO;
         }
diff --git a/src/constraint_imu.h b/src/constraint_imu.h
index bb40f00b154ea2969ec6bc30b7ddad7548dec774..98dc60e1bd131cfc6b841be7adbc78702e4d3144 100644
--- a/src/constraint_imu.h
+++ b/src/constraint_imu.h
@@ -19,10 +19,11 @@ WOLF_PTR_TYPEDEFS(ConstraintIMU);
 class ConstraintIMU : public ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>
 {
     public:
-        ConstraintIMU(FeatureIMUPtr _ftr_ptr, FrameIMUPtr _frame_ptr, bool _apply_loss_function = false,
+        ConstraintIMU(const ProcessorBasePtr& _processor_ptr, const FeatureIMUPtr& _ftr_ptr,
+                      const FrameIMUPtr& _frame_ptr, bool _apply_loss_function = false,
                       ConstraintStatus _status = CTR_ACTIVE);
 
-        virtual ~ConstraintIMU();
+        virtual ~ConstraintIMU() = default;
 
         /* \brief : compute the residual from the state blocks being iterated by the solver.
             -> computes the expected measurement
@@ -53,7 +54,7 @@ class ConstraintIMU : public ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>
         bool getResiduals(const Eigen::MatrixBase<D1> & _p1, const Eigen::QuaternionBase<D2> & _q1, const Eigen::MatrixBase<D1> & _v1, const Eigen::MatrixBase<D1> & _ab1, const Eigen::MatrixBase<D1> & _wb1,
                         const Eigen::MatrixBase<D1> & _p2, const Eigen::QuaternionBase<D2> & _q2, const Eigen::MatrixBase<D1> & _v2, const Eigen::MatrixBase<D1> & _ab2, const Eigen::MatrixBase<D1> & _wb2, const Eigen::MatrixBase<D3> & _residuals) const;
 
-        virtual JacobianMethod getJacobianMethod() const;
+        virtual JacobianMethod getJacobianMethod() const override;
 
         /* Function expectation(...)
          * params :
@@ -98,7 +99,8 @@ class ConstraintIMU : public ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>
         }
 
     public:
-        static wolf::ConstraintBasePtr create(FeatureIMUPtr _feature_ptr, NodeBasePtr _correspondant_ptr);
+        static wolf::ConstraintBasePtr create(const ProcessorBasePtr& _processor_ptr, const FeatureIMUPtr& _feature_ptr,
+                                              const NodeBasePtr& _correspondant_ptr);
 
     private:
         /// Preintegrated delta
@@ -138,9 +140,10 @@ class ConstraintIMU : public ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>
         const Eigen::Matrix3s sqrt_W_r_dt_inv;
 };
 
-inline ConstraintIMU::ConstraintIMU(FeatureIMUPtr _ftr_ptr, FrameIMUPtr _frame_ptr, bool _apply_loss_function,
+inline ConstraintIMU::ConstraintIMU(const ProcessorBasePtr& _processor_ptr, const FeatureIMUPtr& _ftr_ptr,
+                                    const FrameIMUPtr& _frame_ptr, bool _apply_loss_function,
                                     ConstraintStatus _status) :
-        ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>(CTR_IMU, _frame_ptr, nullptr, nullptr, _apply_loss_function, _status,
+        ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>(CTR_IMU, _processor_ptr, _frame_ptr, nullptr, nullptr, _apply_loss_function, _status,
                                                     _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _frame_ptr->getVPtr(),
                                                     _frame_ptr->getAccBiasPtr(), _frame_ptr->getGyroBiasPtr(),
                                                     _ftr_ptr->getFramePtr()->getPPtr(),
@@ -170,11 +173,6 @@ inline ConstraintIMU::ConstraintIMU(FeatureIMUPtr _ftr_ptr, FrameIMUPtr _frame_p
     setType("IMU");
 }
 
-inline ConstraintIMU::~ConstraintIMU()
-{
-    //
-}
-
 template<typename T>
 inline bool ConstraintIMU::operator ()(const T* const _p1, const T* const _q1, const T* const _v1, const T* const _ab1, const T* const _wb1,
                                        const T* const _p2, const T* const _q2, const T* const _v2, const T* const _ab2, const T* const _wb2,
@@ -312,9 +310,10 @@ inline JacobianMethod ConstraintIMU::getJacobianMethod() const
     return JAC_AUTO;
 }
 
-inline wolf::ConstraintBasePtr ConstraintIMU::create(FeatureIMUPtr _feature_ptr, NodeBasePtr _correspondant_ptr)
+inline wolf::ConstraintBasePtr ConstraintIMU::create(const ProcessorBasePtr& _processor_ptr, const FeatureIMUPtr& _feature_ptr,
+                                                     const NodeBasePtr& _correspondant_ptr)
 {
-    return std::make_shared<ConstraintIMU>(_feature_ptr, std::static_pointer_cast<FrameIMU>(_correspondant_ptr));
+    return std::make_shared<ConstraintIMU>(_processor_ptr, _feature_ptr, std::static_pointer_cast<FrameIMU>(_correspondant_ptr));
 }
 
 
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index 99f66652c7d46ea427bf71e361ba29cdc7490e82..6810b3d08be0a0dc0435d75ecf364afadd4065c4 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -15,18 +15,15 @@ WOLF_PTR_TYPEDEFS(ConstraintOdom2D);
 class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
 {
     public:
-        ConstraintOdom2D(FeatureBasePtr _ftr_ptr, FrameBasePtr _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-                ConstraintSparse<3, 2, 1, 2, 1>(CTR_ODOM_2D, _frame_ptr, nullptr, nullptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+        ConstraintOdom2D(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _ftr_ptr,
+                         const FrameBasePtr& _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+                ConstraintSparse<3, 2, 1, 2, 1>(CTR_ODOM_2D, _processor_ptr, _frame_ptr, nullptr, nullptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
         {
             setType("ODOM 2D");
 //            std::cout << "created ConstraintOdom2D " << std::endl;
         }
 
-        virtual ~ConstraintOdom2D()
-        {
-//            std::cout << "destructed ConstraintOdom2D " << std::endl;
-            //
-        }
+        virtual ~ConstraintOdom2D() = default;
 
         template<typename T>
         bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
@@ -34,16 +31,16 @@ class ConstraintOdom2D : public ConstraintSparse<3, 2, 1, 2, 1>
 
         /** \brief Returns the jacobians computation method
          **/
-        virtual JacobianMethod getJacobianMethod() const
+        virtual JacobianMethod getJacobianMethod() const override
         {
             return JAC_AUTO;
         }
 
     public:
-        static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
-                                            NodeBasePtr _correspondant_ptr)
+        static wolf::ConstraintBasePtr create(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _feature_ptr,
+                                              const NodeBasePtr& _correspondant_ptr)
         {
-            return std::make_shared<ConstraintOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr) );
+            return std::make_shared<ConstraintOdom2D>(_processor_ptr, _feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr) );
         }
 
 };
diff --git a/src/constraint_odom_2D_analytic.h b/src/constraint_odom_2D_analytic.h
index 88ba510ae125ade012702c75c66eaf007baea5dd..0993e91497e06f14ae1cb7baa2a1fef4659f69d9 100644
--- a/src/constraint_odom_2D_analytic.h
+++ b/src/constraint_odom_2D_analytic.h
@@ -13,16 +13,15 @@ WOLF_PTR_TYPEDEFS(ConstraintOdom2DAnalytic);
 class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
 {
     public:
-        ConstraintOdom2DAnalytic(FeatureBasePtr _ftr_ptr, FrameBasePtr _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-            ConstraintRelative2DAnalytic(_ftr_ptr, CTR_ODOM_2D, _frame_ptr, _apply_loss_function, _status)
+        ConstraintOdom2DAnalytic(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _ftr_ptr,
+                                 const FrameBasePtr& _frame_ptr, bool _apply_loss_function = false,
+                                 ConstraintStatus _status = CTR_ACTIVE) :
+            ConstraintRelative2DAnalytic(_processor_ptr, _ftr_ptr, CTR_ODOM_2D, _frame_ptr, _apply_loss_function, _status)
         {
             setType("ODOM 2D ANALYTIC");
         }
 
-        virtual ~ConstraintOdom2DAnalytic()
-        {
-            //
-        }
+        virtual ~ConstraintOdom2DAnalytic() = default;
 
 //        /** \brief Returns the constraint residual size
 //         *
@@ -103,10 +102,11 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
 
 
     public:
-        static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
-                                            NodeBasePtr _correspondant_ptr)
+        static wolf::ConstraintBasePtr create(const ProcessorBasePtr& _processor_ptr,
+                                              const FeatureBasePtr& _feature_ptr, //
+                                              const NodeBasePtr& _correspondant_ptr)
         {
-            return std::make_shared<ConstraintOdom2DAnalytic>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr) );
+            return std::make_shared<ConstraintOdom2DAnalytic>(_processor_ptr, _feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr) );
         }
 
 };
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index 564f06b1b6614f8433ae1657f74303869c12133a..921eac4b0cb99607226fc2234e9d0b82de2e6d9f 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -20,10 +20,13 @@ WOLF_PTR_TYPEDEFS(ConstraintOdom3D);
 class ConstraintOdom3D : public ConstraintSparse<6,3,4,3,4>
 {
     public:
-        ConstraintOdom3D(FeatureBasePtr _ftr_current_ptr, FrameBasePtr _frame_past_ptr, bool _apply_loss_function = false,
+        ConstraintOdom3D(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _ftr_current_ptr,
+                         const FrameBasePtr& _frame_past_ptr, bool _apply_loss_function = false,
                          ConstraintStatus _status = CTR_ACTIVE);
-        virtual ~ConstraintOdom3D();
-        JacobianMethod getJacobianMethod() const {return JAC_AUTO;}
+
+        virtual ~ConstraintOdom3D() = default;
+
+        JacobianMethod getJacobianMethod() const override {return JAC_AUTO;}
 
         template<typename T>
                 bool operator ()(const T* const _p_current,
@@ -69,9 +72,11 @@ inline void ConstraintOdom3D::printRes (const  Eigen::Matrix<Scalar,6,1> & r) co
 }
 
 
-inline ConstraintOdom3D::ConstraintOdom3D(FeatureBasePtr _ftr_current_ptr, FrameBasePtr _frame_past_ptr, bool _apply_loss_function,
+inline ConstraintOdom3D::ConstraintOdom3D(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _ftr_current_ptr,
+                                          const FrameBasePtr& _frame_past_ptr, bool _apply_loss_function,
                                           ConstraintStatus _status) :
         ConstraintSparse<6, 3, 4, 3, 4>(CTR_ODOM_3D,        // type
+                                        _processor_ptr,     // processor
                                         _frame_past_ptr,    // frame other
                                         nullptr,            // feature other
                                         nullptr,            // landmark other
@@ -86,11 +91,6 @@ inline ConstraintOdom3D::ConstraintOdom3D(FeatureBasePtr _ftr_current_ptr, Frame
     //
 }
 
-inline ConstraintOdom3D::~ConstraintOdom3D()
-{
-    //
-}
-
 template<typename T>
 inline bool wolf::ConstraintOdom3D::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
                                                 const T* const _q_past, T* _expectation_dp, T* _expectation_dq) const
diff --git a/src/constraint_point_2D.h b/src/constraint_point_2D.h
index 7f9078f82637fb17418949a020cd2ce51a59020b..36b3ebe9dca61269de00046b77deeb55c3058af2 100644
--- a/src/constraint_point_2D.h
+++ b/src/constraint_point_2D.h
@@ -10,7 +10,9 @@ namespace wolf {
     
 WOLF_PTR_TYPEDEFS(ConstraintPoint2D);
     
-//class
+/**
+ * @brief The ConstraintPoint2D class
+ */
 class ConstraintPoint2D: public ConstraintSparse<2,2,1,2,1,2>
 {
     protected:
@@ -19,12 +21,13 @@ class ConstraintPoint2D: public ConstraintSparse<2,2,1,2,1,2>
         StateBlockPtr point_state_ptr_;
         Eigen::VectorXs measurement_;                   ///<  the measurement vector
         Eigen::MatrixXs measurement_covariance_;        ///<  the measurement covariance matrix
-        Eigen::MatrixXs measurement_sqrt_information_;        ///<  the squared root information matrix
+        Eigen::MatrixXs measurement_sqrt_information_;  ///<  the squared root information matrix
 
     public:
 
-		ConstraintPoint2D(FeaturePolyline2DPtr _ftr_ptr, LandmarkPolyline2DPtr _lmk_ptr, unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-			ConstraintSparse<2,2,1,2,1,2>(CTR_POINT_2D, nullptr, nullptr, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
+    ConstraintPoint2D(const ProcessorBasePtr& _processor_ptr, const FeaturePolyline2DPtr& _ftr_ptr,
+                      const LandmarkPolyline2DPtr& _lmk_ptr, unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+      ConstraintSparse<2,2,1,2,1,2>(CTR_POINT_2D, _processor_ptr, nullptr, nullptr, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
 			feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
 		{
 			//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
@@ -35,64 +38,80 @@ class ConstraintPoint2D: public ConstraintSparse<2,2,1,2,1,2>
             measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U  in the decomposition
 		}
 
-        virtual ~ConstraintPoint2D()
-        {
-            //std::cout << "deleting ConstraintPoint2D " << id() << std::endl;
-        }
+    virtual ~ConstraintPoint2D() = default;
 
-        LandmarkPolyline2DPtr getLandmarkPtr()
+    /**
+     * @brief getLandmarkPtr
+     * @return
+     */
+    LandmarkPolyline2DPtr getLandmarkPtr()
 		{
 			return std::static_pointer_cast<LandmarkPolyline2D>(landmark_other_ptr_.lock());
 		}
 
-        int getLandmarkPointId()
-        {
-            return landmark_point_id_;
-        }
-
-        unsigned int getFeaturePointId()
-        {
-            return feature_point_id_;
-        }
-
-        StateBlockPtr getLandmarkPointPtr()
-        {
-            return point_state_ptr_;
-        }
-
-		template <typename T>
-        bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const;
-
-        /** \brief Returns the jacobians computation method
+    /**
+     * @brief getLandmarkPointId
+     * @return
+     */
+    int getLandmarkPointId()
+    {
+      return landmark_point_id_;
+    }
+
+    /**
+     * @brief getFeaturePointId
+     * @return
+     */
+    unsigned int getFeaturePointId()
+    {
+      return feature_point_id_;
+    }
+
+    /**
+     * @brief getLandmarkPointPtr
+     * @return
+     */
+    StateBlockPtr getLandmarkPointPtr()
+    {
+      return point_state_ptr_;
+    }
+
+    /**
+     *
+     */
+    template <typename T>
+    bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const;
+
+    /** \brief Returns the jacobians computation method
          *
          * Returns the jacobians computation method
          *
          **/
-        virtual JacobianMethod getJacobianMethod() const
-        {
-            return JAC_AUTO;
-        }
+    virtual JacobianMethod getJacobianMethod() const override
+    {
+      return JAC_AUTO;
+    }
 
-        /** \brief Returns a reference to the feature measurement
+    /** \brief Returns a reference to the feature measurement
          **/
-        virtual const Eigen::VectorXs& getMeasurement() const
-        {
-            return measurement_;
-        }
+    virtual const Eigen::VectorXs& getMeasurement() const override
+    {
+      return measurement_;
+    }
 
-        /** \brief Returns a reference to the feature measurement covariance
+    /** \brief Returns a reference to the feature measurement covariance
          **/
-        virtual const Eigen::MatrixXs& getMeasurementCovariance() const
-        {
-            return measurement_covariance_;
-        }
+    virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
+    {
+      return measurement_covariance_;
+    }
 
-        /** \brief Returns a reference to the feature measurement square root information
+    /** \brief Returns a reference to the feature measurement square root information
          **/
-        virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationTransposed() const
-        {
-            return measurement_sqrt_information_;
-        }
+    virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationTransposed() const override
+    {
+      return measurement_sqrt_information_;
+    }
 };
 
 template<typename T>
diff --git a/src/constraint_point_to_line_2D.h b/src/constraint_point_to_line_2D.h
index c0c64d55a5e73730159683abf777bac4d2bb9e81..86bc2f997482afeb8f4e1ae6ae3b00bdaaa61d1c 100644
--- a/src/constraint_point_to_line_2D.h
+++ b/src/constraint_point_to_line_2D.h
@@ -25,8 +25,9 @@ class ConstraintPointToLine2D: public ConstraintSparse<1,2,1,2,1,2,2>
 
     public:
 
-		ConstraintPointToLine2D(FeaturePolyline2DPtr _ftr_ptr, LandmarkPolyline2DPtr _lmk_ptr, unsigned int _ftr_point_id, int _lmk_point_id,  int _lmk_point_aux_id, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-			ConstraintSparse<1,2,1,2,1,2,2>(CTR_POINT_TO_LINE_2D, nullptr, nullptr, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
+    ConstraintPointToLine2D(const ProcessorBasePtr& _processor_ptr, const FeaturePolyline2DPtr& _ftr_ptr,
+                            const LandmarkPolyline2DPtr& _lmk_ptr, unsigned int _ftr_point_id, int _lmk_point_id,  int _lmk_point_aux_id, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+      ConstraintSparse<1,2,1,2,1,2,2>(CTR_POINT_TO_LINE_2D, _processor_ptr, nullptr, nullptr, _lmk_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
 			landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
 		{
 			//std::cout << "ConstraintPointToLine2D" << std::endl;
@@ -36,74 +37,71 @@ class ConstraintPointToLine2D: public ConstraintSparse<1,2,1,2,1,2,2>
             measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U  in the decomposition
 		}
 
-        virtual ~ConstraintPointToLine2D()
-        {
-            //std::cout << "deleting ConstraintPoint2D " << id() << std::endl;
-        }
+    virtual ~ConstraintPointToLine2D() = default;
 
-        LandmarkPolyline2DPtr getLandmarkPtr()
-		{
-			return std::static_pointer_cast<LandmarkPolyline2D>( landmark_other_ptr_.lock() );
-		}
+    LandmarkPolyline2DPtr getLandmarkPtr()
+    {
+      return std::static_pointer_cast<LandmarkPolyline2D>( landmark_other_ptr_.lock() );
+    }
 
-        int getLandmarkPointId()
-        {
-            return landmark_point_id_;
-        }
+    int getLandmarkPointId()
+    {
+      return landmark_point_id_;
+    }
 
-        int getLandmarkPointAuxId()
-        {
-            return landmark_point_aux_id_;
-        }
+    int getLandmarkPointAuxId()
+    {
+      return landmark_point_aux_id_;
+    }
 
-        unsigned int getFeaturePointId()
-        {
-            return feature_point_id_;
-        }
+    unsigned int getFeaturePointId()
+    {
+      return feature_point_id_;
+    }
 
-        StateBlockPtr getLandmarkPointPtr()
-        {
-            return point_state_ptr_;
-        }
+    StateBlockPtr getLandmarkPointPtr()
+    {
+      return point_state_ptr_;
+    }
 
-        StateBlockPtr getLandmarkPointAuxPtr()
-        {
-            return point_state_ptr_;
-        }
+    StateBlockPtr getLandmarkPointAuxPtr()
+    {
+      return point_state_ptr_;
+    }
 
-		template <typename T>
-        bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const;
+    template <typename T>
+    bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const;
 
-        /** \brief Returns the jacobians computation method
+    /** \brief Returns the jacobians computation method
          *
          * Returns the jacobians computation method
          *
          **/
-        virtual JacobianMethod getJacobianMethod() const
-        {
-            return JAC_AUTO;
-        }
+    virtual JacobianMethod getJacobianMethod() const override
+    {
+      return JAC_AUTO;
+    }
 
-        /** \brief Returns a reference to the feature measurement
+    /** \brief Returns a reference to the feature measurement
          **/
-        virtual const Eigen::VectorXs& getMeasurement() const
-        {
-            return measurement_;
-        }
+    virtual const Eigen::VectorXs& getMeasurement() const override
+    {
+      return measurement_;
+    }
 
-        /** \brief Returns a reference to the feature measurement covariance
+    /** \brief Returns a reference to the feature measurement covariance
          **/
-        virtual const Eigen::MatrixXs& getMeasurementCovariance() const
-        {
-            return measurement_covariance_;
-        }
+    virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
+    {
+      return measurement_covariance_;
+    }
 
-        /** \brief Returns a reference to the feature measurement square root information
+    /** \brief Returns a reference to the feature measurement square root information
          **/
-        virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationTransposed() const
-        {
-            return measurement_sqrt_information_;
-        }
+    virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationTransposed() const override
+    {
+      return measurement_sqrt_information_;
+    }
 };
 
 template<typename T>
diff --git a/src/constraint_relative_2D_analytic.h b/src/constraint_relative_2D_analytic.h
index 43c736d2d2c0d2bb305ae28e5e35f6694284845d..d0ab361d67110795d5953f4aa5dd26343564c271 100644
--- a/src/constraint_relative_2D_analytic.h
+++ b/src/constraint_relative_2D_analytic.h
@@ -17,36 +17,36 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
 
         /** \brief Constructor of category CTR_FRAME
          **/
-        ConstraintRelative2DAnalytic(FeatureBasePtr _ftr_ptr, ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-            ConstraintAnalytic(_tp, _frame_ptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+        ConstraintRelative2DAnalytic(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _ftr_ptr,
+                                     const ConstraintType& _tp, const FrameBasePtr& _frame_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+            ConstraintAnalytic(_tp, _processor_ptr, _frame_ptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
         {
             //
         }
 
         /** \brief Constructor of category CTR_FEATURE
          **/
-        ConstraintRelative2DAnalytic(FeatureBasePtr _ftr_ptr, ConstraintType _tp, FeatureBasePtr _ftr_other_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-            ConstraintAnalytic(_tp, _ftr_other_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _ftr_other_ptr->getFramePtr()->getPPtr(), _ftr_other_ptr->getFramePtr()->getOPtr() )
+        ConstraintRelative2DAnalytic(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _ftr_ptr,
+                                     const ConstraintType& _tp, const FeatureBasePtr& _ftr_other_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+            ConstraintAnalytic(_tp, _processor_ptr, _ftr_other_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _ftr_other_ptr->getFramePtr()->getPPtr(), _ftr_other_ptr->getFramePtr()->getOPtr() )
         {
             //
         }
 
         /** \brief Constructor of category CTR_LANDMARK
          **/
-        ConstraintRelative2DAnalytic(FeatureBasePtr _ftr_ptr, ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
-            ConstraintAnalytic(_tp, _landmark_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _landmark_ptr->getPPtr(), _landmark_ptr->getOPtr())
+        ConstraintRelative2DAnalytic(const ProcessorBasePtr& _processor_ptr, const FeatureBasePtr& _ftr_ptr,
+                                     const ConstraintType& _tp, const LandmarkBasePtr& _landmark_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+            ConstraintAnalytic(_tp, _processor_ptr, _landmark_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _landmark_ptr->getPPtr(), _landmark_ptr->getOPtr())
         {
             //
         }
 
-        virtual ~ConstraintRelative2DAnalytic()
-        {
-            //
-        }
+        virtual ~ConstraintRelative2DAnalytic() = default;
 
         /** \brief Returns the constraint residual size
          **/
-        virtual unsigned int getSize() const
+        virtual unsigned int getSize() const override
         {
             return 3;
         }
@@ -81,7 +81,7 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
 
         /** \brief Returns the jacobians computation method
          **/
-        virtual JacobianMethod getJacobianMethod() const
+        virtual JacobianMethod getJacobianMethod() const override
         {
             return JAC_AUTO;
         }
diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h
index b80c07f348397b190faa4cd9f7321b14975fc25b..18a6ecd18be7e3ffb2d280026368b2ba2673e3ac 100644
--- a/src/constraint_sparse.h
+++ b/src/constraint_sparse.h
@@ -59,7 +59,10 @@ class ConstraintSparse: public ConstraintBase
 
         /** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK)
          **/
-        ConstraintSparse(ConstraintType _tp, FrameBasePtr _frame_other_ptr, FeatureBasePtr _feature_other_ptr, LandmarkBasePtr _landmark_other_ptr, bool _apply_loss_function, ConstraintStatus _status,
+        ConstraintSparse(ConstraintType _tp, const ProcessorBasePtr& _processor_ptr,
+                         const FrameBasePtr& _frame_other_ptr, const FeatureBasePtr& _feature_other_ptr,
+                         const LandmarkBasePtr& _landmark_other_ptr,
+                         bool _apply_loss_function, ConstraintStatus _status,
                          StateBlockPtr _state0Ptr,
                          StateBlockPtr _state1Ptr = nullptr,
                          StateBlockPtr _state2Ptr = nullptr,
@@ -71,28 +74,28 @@ class ConstraintSparse: public ConstraintBase
                          StateBlockPtr _state8Ptr = nullptr,
                          StateBlockPtr _state9Ptr = nullptr );
 
-        virtual ~ConstraintSparse();
+        virtual ~ConstraintSparse() = default;
 
         /** \brief Returns a vector of pointers to the state blocks
          *
          * Returns a vector of pointers to the state blocks in which this constraint depends
          *
          **/
-        virtual const std::vector<Scalar*> getStateScalarPtrVector();
+        virtual const std::vector<Scalar*> getStateScalarPtrVector() override;
 
         /** \brief Returns a vector of pointers to the states
          *
          * Returns a vector of pointers to the state in which this constraint depends
          *
          **/
-        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const;
+        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
 
         /** \brief Returns the residual size
          *
          * Returns the residual size
          *
          **/
-        virtual unsigned int getSize() const;
+        virtual unsigned int getSize() const override;
 
     private:
         void resizeVectors();
@@ -164,12 +167,13 @@ ConstraintSparse<RESIDUAL_SIZE,
                  BLOCK_6_SIZE,
                  BLOCK_7_SIZE,
                  BLOCK_8_SIZE,
-                 BLOCK_9_SIZE>::ConstraintSparse(ConstraintType     _tp,
-                                                 FrameBasePtr       _frame_other_ptr,
-                                                 FeatureBasePtr     _feature_other_ptr,
-                                                 LandmarkBasePtr    _landmark_other_ptr,
-                                                 bool               _apply_loss_function,
-                                                 ConstraintStatus   _status,
+                 BLOCK_9_SIZE>::ConstraintSparse(ConstraintType          _tp,
+                                                 const ProcessorBasePtr& _processor_ptr,
+                                                 const FrameBasePtr&     _frame_other_ptr,
+                                                 const FeatureBasePtr&   _feature_other_ptr,
+                                                 const LandmarkBasePtr&  _landmark_other_ptr,
+                                                 bool                    _apply_loss_function,
+                                                 ConstraintStatus        _status,
                                                  StateBlockPtr _state0Ptr,
                                                  StateBlockPtr _state1Ptr,
                                                  StateBlockPtr _state2Ptr,
@@ -180,40 +184,13 @@ ConstraintSparse<RESIDUAL_SIZE,
                                                  StateBlockPtr _state7Ptr,
                                                  StateBlockPtr _state8Ptr,
                                                  StateBlockPtr _state9Ptr ) :
-            ConstraintBase(_tp, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _apply_loss_function, _status),
+            ConstraintBase(_tp, _processor_ptr, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _apply_loss_function, _status),
             state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
             state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
         {
             resizeVectors();
         }
 
-
-template <const unsigned int RESIDUAL_SIZE,
-                unsigned int BLOCK_0_SIZE,
-                unsigned int BLOCK_1_SIZE,
-                unsigned int BLOCK_2_SIZE,
-                unsigned int BLOCK_3_SIZE,
-                unsigned int BLOCK_4_SIZE,
-                unsigned int BLOCK_5_SIZE,
-                unsigned int BLOCK_6_SIZE,
-                unsigned int BLOCK_7_SIZE,
-                unsigned int BLOCK_8_SIZE,
-                unsigned int BLOCK_9_SIZE>
-ConstraintSparse<RESIDUAL_SIZE,
-                 BLOCK_0_SIZE,
-                 BLOCK_1_SIZE,
-                 BLOCK_2_SIZE,
-                 BLOCK_3_SIZE,
-                 BLOCK_4_SIZE,
-                 BLOCK_5_SIZE,
-                 BLOCK_6_SIZE,
-                 BLOCK_7_SIZE,
-                 BLOCK_8_SIZE,
-                 BLOCK_9_SIZE>::~ConstraintSparse()
-{
-    //
-}
-
 template <const unsigned int RESIDUAL_SIZE,
                 unsigned int BLOCK_0_SIZE,
                 unsigned int BLOCK_1_SIZE,
diff --git a/src/landmark_polyline_2D.cpp b/src/landmark_polyline_2D.cpp
index 71111ff6a895736a2adfbe6138354ba70b7b8636..0ce07d302f701399cee7d1e14d17b06ee6f8466a 100644
--- a/src/landmark_polyline_2D.cpp
+++ b/src/landmark_polyline_2D.cpp
@@ -246,7 +246,8 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
 
             // If landmark point constrained -> new constraint
             if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
-                new_ctr_ptr = std::make_shared<ConstraintPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+                new_ctr_ptr = std::make_shared<ConstraintPoint2D>(ctr_point_ptr->getProcessor(),
+                                                                  std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
                                                                   std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
                                                                   ctr_point_ptr->getFeaturePointId(),
                                                                   _remain_id,
@@ -259,7 +260,8 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
 
             // If landmark point constrained -> new constraint
             if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
-                new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+                new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(ctr_point_ptr->getProcessor(),
+                                                                        std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
                                                                         std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
                                                                         ctr_point_ptr->getFeaturePointId(),
                                                                         _remain_id,
@@ -268,7 +270,8 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
                                                                         ctr_point_ptr->getStatus());
             // If landmark point is aux point -> new constraint
             else if (ctr_point_ptr->getLandmarkPointAuxId() == _remove_id)
-                new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+                new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(ctr_point_ptr->getProcessor(),
+                                                                        std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
                                                                         std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
                                                                         ctr_point_ptr->getFeaturePointId(),
                                                                         ctr_point_ptr->getLandmarkPointId(),
diff --git a/src/processor_imu.h b/src/processor_imu.h
index 55109586c593c607c7d8fcafc699f58f05a5306d..ac38e94e363d1d059dec03bd890feab221c20dc1 100644
--- a/src/processor_imu.h
+++ b/src/processor_imu.h
@@ -420,7 +420,7 @@ inline ConstraintBasePtr ProcessorIMU::emplaceConstraint(FeatureBasePtr _feature
 {
     FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
     FrameIMUPtr frm_imu = std::static_pointer_cast<FrameIMU>(_frame_origin);
-    ConstraintIMUPtr ctr_imu = std::make_shared<ConstraintIMU>(ftr_imu, frm_imu);
+    ConstraintIMUPtr ctr_imu = std::make_shared<ConstraintIMU>(shared_from_this(), ftr_imu, frm_imu);
 
     _feature_motion->addConstraint(ctr_imu);
     _frame_origin->addConstrainedBy(ctr_imu);
diff --git a/src/processor_odom_2D.h b/src/processor_odom_2D.h
index 88e7193fc7343b7e8c1b8c7b3aee6e4e18155243..5d5203550e1334e1aa801715fe673502a8a8a5a3 100644
--- a/src/processor_odom_2D.h
+++ b/src/processor_odom_2D.h
@@ -202,7 +202,7 @@ inline Eigen::VectorXs ProcessorOdom2D::deltaZero() const
 
 inline ConstraintBasePtr ProcessorOdom2D::emplaceConstraint(FeatureBasePtr _feature_motion, FrameBasePtr _frame_origin)
 {
-    ConstraintOdom2DPtr ctr_odom = std::make_shared<ConstraintOdom2D>(_feature_motion, _frame_origin);
+    ConstraintOdom2DPtr ctr_odom = std::make_shared<ConstraintOdom2D>(shared_from_this(), _feature_motion, _frame_origin);
     _feature_motion->addConstraint(ctr_odom);
     _frame_origin->addConstrainedBy(ctr_odom);
     return ctr_odom;
diff --git a/src/processor_odom_3D.h b/src/processor_odom_3D.h
index 125a98841f382b51e7e381119ff8b0fd44d08a9d..fe9250cc02a8e5c9b85ba1fd65bdc863e89e07de 100644
--- a/src/processor_odom_3D.h
+++ b/src/processor_odom_3D.h
@@ -138,7 +138,7 @@ inline Eigen::VectorXs ProcessorOdom3D::deltaZero() const
 inline ConstraintBasePtr ProcessorOdom3D::emplaceConstraint(FeatureBasePtr _feature_motion,
                                                            FrameBasePtr _frame_origin)
 {
-    ConstraintOdom3DPtr ctr_odom = std::make_shared<ConstraintOdom3D>(_feature_motion, _frame_origin);
+    ConstraintOdom3DPtr ctr_odom = std::make_shared<ConstraintOdom3D>(shared_from_this(), _feature_motion, _frame_origin);
     _feature_motion->addConstraint(ctr_odom);
     _frame_origin->addConstrainedBy(ctr_odom);
     return ctr_odom;
diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp
index 320b19ecb07a7073d72ca5bdafdcf6b3d6c30f12..0f536b6e3ad823c7e0bc4d42196bb80ae1b1e466 100644
--- a/src/processor_tracker_feature_corner.cpp
+++ b/src/processor_tracker_feature_corner.cpp
@@ -108,13 +108,13 @@ ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr
                                                           _feature_ptr->getMeasurement()(3));
 
         // Add landmark constraint to the other feature
-        _feature_other_ptr->addConstraint(std::make_shared<ConstraintCorner2D>(_feature_other_ptr, landmark_ptr));
+        _feature_other_ptr->addConstraint(std::make_shared<ConstraintCorner2D>(shared_from_this(), _feature_other_ptr, landmark_ptr));
     }
 
 //    std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
 //              << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl
 //              << " corresponding to landmark " << landmark_ptr->id() << std::endl;
-    return std::make_shared<ConstraintCorner2D>(_feature_ptr, landmark_ptr);
+    return std::make_shared<ConstraintCorner2D>(shared_from_this(), _feature_ptr, landmark_ptr);
 }
 
 void ProcessorTrackerFeatureCorner::extractCorners(CaptureLaser2DPtr _capture_laser_ptr,
diff --git a/src/processor_tracker_feature_dummy.h b/src/processor_tracker_feature_dummy.h
index ee52ca5b1266fc0c4aa9b8ad77b0d0837aab552c..2457003d06e8501ea56fdc454dd23829216f9b59 100644
--- a/src/processor_tracker_feature_dummy.h
+++ b/src/processor_tracker_feature_dummy.h
@@ -95,7 +95,7 @@ inline ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureB
 {
 //    std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
 //              << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl;
-    auto ctr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr);
+    auto ctr = std::make_shared<ConstraintEpipolar>(shared_from_this(), _feature_ptr, _feature_other_ptr);
 //    _feature_ptr->addConstraint(ctr);
 //    _feature_other_ptr->addConstrainedBy(ctr);
     return ctr;
diff --git a/src/processor_tracker_landmark_corner.h b/src/processor_tracker_landmark_corner.h
index c4469f418a48e51278add6274ff618ba62046a1e..90ea9d132b4b4f5acf48ff4fbf06c3e15b485e8e 100644
--- a/src/processor_tracker_landmark_corner.h
+++ b/src/processor_tracker_landmark_corner.h
@@ -221,7 +221,7 @@ inline ConstraintBasePtr ProcessorTrackerLandmarkCorner::createConstraint(Featur
 {
     assert(_feature_ptr != nullptr && _landmark_ptr != nullptr && "ProcessorTrackerLandmarkCorner::createConstraint: feature and landmark pointers can not be nullptr!");
 
-    return std::make_shared<ConstraintCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>((_landmark_ptr)));
+    return std::make_shared<ConstraintCorner2D>(shared_from_this(), _feature_ptr, std::static_pointer_cast<LandmarkCorner2D>((_landmark_ptr)));
 }
 
 } // namespace wolf
diff --git a/src/processor_tracker_landmark_dummy.cpp b/src/processor_tracker_landmark_dummy.cpp
index 60b22ac304b86a5dae72e78d59bfa54b88116ec5..cd0c796babffc762d0db2afb37bcbe585af820de 100644
--- a/src/processor_tracker_landmark_dummy.cpp
+++ b/src/processor_tracker_landmark_dummy.cpp
@@ -88,7 +88,7 @@ ConstraintBasePtr ProcessorTrackerLandmarkDummy::createConstraint(FeatureBasePtr
     std::cout << "\tProcessorTrackerLandmarkDummy::createConstraint" << std::endl;
     std::cout << "\t\tfeature " << _feature_ptr->getMeasurement() << std::endl;
     std::cout << "\t\tlandmark "<< _landmark_ptr->getDescriptor() << std::endl;
-    return std::make_shared<ConstraintCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr) );
+    return std::make_shared<ConstraintCorner2D>(shared_from_this(), _feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr) );
 }
 
 } //namespace wolf
diff --git a/src/processor_tracker_landmark_polyline.cpp b/src/processor_tracker_landmark_polyline.cpp
index 74a37dac155e1280514dce655d7fc5963cc8c6ad..90af880b19dc29f2cda51093b02227d4927f1f26 100644
--- a/src/processor_tracker_landmark_polyline.cpp
+++ b/src/processor_tracker_landmark_polyline.cpp
@@ -816,7 +816,7 @@ void ProcessorTrackerLandmarkPolyline::establishConstraints()
                 if (lmk_next_point_id > polyline_landmark->getLastId())
                     lmk_next_point_id -= polyline_landmark->getNPoints();
                 //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl;
-                last_feature->addConstraint(std::make_shared<ConstraintPointToLine2D>(polyline_feature, polyline_landmark, ftr_point_id, lmk_point_id, lmk_next_point_id));
+                last_feature->addConstraint(std::make_shared<ConstraintPointToLine2D>(shared_from_this(), polyline_feature, polyline_landmark, ftr_point_id, lmk_point_id, lmk_next_point_id));
                 //std::cout << "constraint added" << std::endl;
             }
 
@@ -828,7 +828,7 @@ void ProcessorTrackerLandmarkPolyline::establishConstraints()
                 if (lmk_prev_point_id < polyline_landmark->getFirstId())
                     lmk_prev_point_id += polyline_landmark->getNPoints();
                 //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
-                last_feature->addConstraint(std::make_shared<ConstraintPointToLine2D>(polyline_feature, polyline_landmark, ftr_point_id, lmk_point_id, lmk_prev_point_id));
+                last_feature->addConstraint(std::make_shared<ConstraintPointToLine2D>(shared_from_this(), polyline_feature, polyline_landmark, ftr_point_id, lmk_point_id, lmk_prev_point_id));
                 //std::cout << "constraint added" << std::endl;
             }
 
@@ -840,7 +840,7 @@ void ProcessorTrackerLandmarkPolyline::establishConstraints()
 				//std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl;
 				//std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl;
 				//std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl;
-                last_feature->addConstraint(std::make_shared<ConstraintPoint2D>(polyline_feature, polyline_landmark, ftr_point_id, lmk_point_id));
+                last_feature->addConstraint(std::make_shared<ConstraintPoint2D>(shared_from_this(), polyline_feature, polyline_landmark, ftr_point_id, lmk_point_id));
                 //std::cout << "constraint added" << std::endl;
             }
         }
diff --git a/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp
index 6bbac96bd12ec52c082097e7f8a958192da5f2c0..e2240cff3211bb9fdc2460fa72f75b541e16246e 100644
--- a/src/test/gtest_constraint_odom_3D.cpp
+++ b/src/test/gtest_constraint_odom_3D.cpp
@@ -45,7 +45,7 @@ FrameBasePtr frm1 = problem->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
 // Capture, feature and constraint from frm1 to frm0
 CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>(1, nullptr, delta, 7, 7, 6, 0));
 FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov));
-ConstraintOdom3DPtr ctr1 = std::static_pointer_cast<ConstraintOdom3D>(fea1->addConstraint(std::make_shared<ConstraintOdom3D>(fea1, frm0))); // create and add
+ConstraintOdom3DPtr ctr1 = std::static_pointer_cast<ConstraintOdom3D>(fea1->addConstraint(std::make_shared<ConstraintOdom3D>(nullptr, fea1, frm0))); // create and add
 ConstraintBasePtr dummy = frm0->addConstrainedBy(ctr1);
 
 ////////////////////////////////////////////////////////
diff --git a/src/test/gtest_feature_imu.cpp b/src/test/gtest_feature_imu.cpp
index e88b06988425d50ffa569740539bc977f90b1e90..13947686ee0454cead19a81271844b5a8df457f6 100644
--- a/src/test/gtest_feature_imu.cpp
+++ b/src/test/gtest_feature_imu.cpp
@@ -24,6 +24,7 @@ class FeatureIMU_test : public testing::Test
         wolf::FrameIMUPtr last_frame;
         wolf::FrameBasePtr origin_frame;
         Eigen::MatrixXs dD_db_jacobians;
+        wolf::ProcessorBasePtr processor_ptr_;
 
     //a new of this will be created for each new test
     virtual void SetUp()
@@ -39,7 +40,7 @@ class FeatureIMU_test : public testing::Test
         IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
         IntrinsicsIMUPtr sen_imu_params = std::make_shared<IntrinsicsIMU>();
         SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, sen_imu_params);
-        ProcessorBasePtr processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
+        processor_ptr_ = wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
 
     // Time and data variables
         TimeStamp t;
@@ -148,7 +149,7 @@ TEST_F(FeatureIMU_test, addConstraint)
     using namespace wolf;
     
     FrameIMUPtr frm_imu = std::static_pointer_cast<FrameIMU>(last_frame);
-    ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(feat_imu, std::static_pointer_cast<FrameIMU>(frm_imu));
+    ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(processor_ptr_, feat_imu, std::static_pointer_cast<FrameIMU>(frm_imu));
     feat_imu->addConstraint(constraint_imu);
     origin_frame->addConstrainedBy(constraint_imu);
 }
diff --git a/src/test/gtest_frame_base.cpp b/src/test/gtest_frame_base.cpp
index 30a55da5cca6ce1a1fde61972a41f1e567ed8e3d..bbb024b9cb3e40f34a608647e1f914786c55c824 100644
--- a/src/test/gtest_frame_base.cpp
+++ b/src/test/gtest_frame_base.cpp
@@ -12,6 +12,7 @@
 
 #include "../frame_base.h"
 #include "../sensor_odom_2D.h"
+#include "../processor_odom_2D.h"
 #include "../constraint_odom_2D.h"
 #include "../capture_motion.h"
 
@@ -75,9 +76,11 @@ TEST(FrameBase, LinksToTree)
     T->addFrame(F2);
     CaptureMotionPtr C = make_shared<CaptureMotion>(1, S, Vector3s::Zero(), 3, 3, 3, 0);
     F1->addCapture(C);
+    /// @todo link sensor & proccessor
+    ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>();
     FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
     C->addFeature(f);
-    ConstraintOdom2DPtr c = make_shared<ConstraintOdom2D>(f, F2);
+    ConstraintOdom2DPtr c = make_shared<ConstraintOdom2D>(p, f, F2);
     f->addConstraint(c);
 
     // c-by link F2 -> c not yet established
diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp
index f7ae588e38facc4f1d3315e8becff866a2d2eec8..58f98f4ae830b280872a1f80306db9e7c26aeb5b 100644
--- a/src/test/gtest_odom_2D.cpp
+++ b/src/test/gtest_odom_2D.cpp
@@ -136,7 +136,7 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
     FrameBasePtr        F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
     CaptureBasePtr      C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
     FeatureBasePtr      f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
-    ConstraintBasePtr   c1 = f1->addConstraint(std::make_shared<ConstraintOdom2D>(f1, F0));
+    ConstraintBasePtr   c1 = f1->addConstraint(std::make_shared<ConstraintOdom2D>(nullptr, f1, F0));
     F0->addConstrainedBy(c1);
 
     // KF2 and motion from KF1
@@ -145,7 +145,7 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
     FrameBasePtr        F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
     CaptureBasePtr      C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
     FeatureBasePtr      f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
-    ConstraintBasePtr   c2 = f2->addConstraint(std::make_shared<ConstraintOdom2D>(f2, F1));
+    ConstraintBasePtr   c2 = f2->addConstraint(std::make_shared<ConstraintOdom2D>(nullptr, f2, F1));
     F1->addConstrainedBy(c2);
 
     ASSERT_TRUE(Pr->check(0));