diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index 8621a0eac7a781bb08abe419bc71fc517e886bce..72ed561c556275804b89b8bb9a54c876506d1926 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -152,7 +152,7 @@ inline MotionBuffer& CaptureMotion::getBuffer()
 
 inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const
 {
-    return getBuffer().get().back().jacobian_calib_;
+    return getBuffer().back().jacobian_calib_;
 }
 
 inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
@@ -193,7 +193,7 @@ inline void CaptureMotion::setCalibrationPreint(const VectorXd& _calib_preint)
 
 inline VectorXd CaptureMotion::getDeltaPreint() const
 {
-    return getBuffer().get().back().delta_integr_;
+    return getBuffer().back().delta_integr_;
 }
 
 inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
@@ -203,7 +203,7 @@ inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
 
 inline MatrixXd CaptureMotion::getDeltaPreintCov() const
 {
-    return getBuffer().get().back().delta_integr_cov_;
+    return getBuffer().back().delta_integr_cov_;
 }
 
 inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index 14e3660fa43bfb8c7892a4f225b5edf38592bb4d..03fc5fbb407c1f2c6914f871ede6834701af2bab 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -46,19 +46,19 @@ class FactorAutodiff : public FactorBase
         std::vector<StateBlockPtr> state_ptrs_;
 
         static const std::vector<unsigned int> jacobian_locations_;
-        std::array<WolfJet, RES>* residuals_jets_;
-        std::array<WolfJet, B0>* jets_0_;
-        std::array<WolfJet, B1>* jets_1_;
-        std::array<WolfJet, B2>* jets_2_;
-        std::array<WolfJet, B3>* jets_3_;
-        std::array<WolfJet, B4>* jets_4_;
-        std::array<WolfJet, B5>* jets_5_;
-        std::array<WolfJet, B6>* jets_6_;
-        std::array<WolfJet, B7>* jets_7_;
-        std::array<WolfJet, B8>* jets_8_;
-        std::array<WolfJet, B9>* jets_9_;
-        std::array<WolfJet, B10>* jets_10_;
-        std::array<WolfJet, B11>* jets_11_;
+        mutable std::array<WolfJet, RES> residuals_jets_;
+        mutable std::array<WolfJet, B0> jets_0_;
+        mutable std::array<WolfJet, B1> jets_1_;
+        mutable std::array<WolfJet, B2> jets_2_;
+        mutable std::array<WolfJet, B3> jets_3_;
+        mutable std::array<WolfJet, B4> jets_4_;
+        mutable std::array<WolfJet, B5> jets_5_;
+        mutable std::array<WolfJet, B6> jets_6_;
+        mutable std::array<WolfJet, B7> jets_7_;
+        mutable std::array<WolfJet, B8> jets_8_;
+        mutable std::array<WolfJet, B9> jets_9_;
+        mutable std::array<WolfJet, B10> jets_10_;
+        mutable std::array<WolfJet, B11> jets_11_;
 
     public:
         /** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
@@ -84,20 +84,7 @@ class FactorAutodiff : public FactorBase
                        StateBlockPtr _state10Ptr,
                        StateBlockPtr _state11Ptr) :
             FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-            state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr}),
-            residuals_jets_(new std::array<WolfJet, RES>),
-            jets_0_(new std::array<WolfJet, B0>),
-            jets_1_(new std::array<WolfJet, B1>),
-            jets_2_(new std::array<WolfJet, B2>),
-            jets_3_(new std::array<WolfJet, B3>),
-            jets_4_(new std::array<WolfJet, B4>),
-            jets_5_(new std::array<WolfJet, B5>),
-            jets_6_(new std::array<WolfJet, B6>),
-            jets_7_(new std::array<WolfJet, B7>),
-            jets_8_(new std::array<WolfJet, B8>),
-            jets_9_(new std::array<WolfJet, B9>),
-            jets_10_(new std::array<WolfJet, B10>),
-            jets_11_(new std::array<WolfJet, B11>)
+            state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr,_state11Ptr})
         {
             // asserts for null states
             assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -116,46 +103,33 @@ class FactorAutodiff : public FactorBase
             // initialize jets
             unsigned int last_jet_idx = 0;
             for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+               jets_0_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+               jets_1_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+               jets_2_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+               jets_3_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B4; i++)
-               (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+               jets_4_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B5; i++)
-               (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+               jets_5_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B6; i++)
-               (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+               jets_6_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B7; i++)
-               (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+               jets_7_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B8; i++)
-               (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+               jets_8_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B9; i++)
-               (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+               jets_9_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B10; i++)
-               (*jets_10_)[i] = WolfJet(0, last_jet_idx++);
+               jets_10_[i] = WolfJet(0, last_jet_idx++);
             for (unsigned int i = 0; i < B11; i++)
-               (*jets_11_)[i] = WolfJet(0, last_jet_idx++);
+               jets_11_[i] = WolfJet(0, last_jet_idx++);
         }
 
         virtual ~FactorAutodiff()
         {
-            delete jets_0_;
-            delete jets_1_;
-            delete jets_2_;
-            delete jets_3_;
-            delete jets_4_;
-            delete jets_5_;
-            delete jets_6_;
-            delete jets_7_;
-            delete jets_8_;
-            delete jets_9_;
-            delete jets_10_;
-            delete jets_11_;
-            delete residuals_jets_;
         }
 
         virtual JacobianMethod getJacobianMethod() const
@@ -196,30 +170,30 @@ class FactorAutodiff : public FactorBase
                 updateJetsRealPart(param_vec);
 
                 // call functor
-                (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                  jets_1_->data(),
-                                                  jets_2_->data(),
-                                                  jets_3_->data(),
-                                                  jets_4_->data(),
-                                                  jets_5_->data(),
-                                                  jets_6_->data(),
-                                                  jets_7_->data(),
-                                                  jets_8_->data(),
-                                                  jets_9_->data(),
-                                                  jets_10_->data(),
-                                                  jets_11_->data(),
-                                                  residuals_jets_->data());
+                (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                  jets_1_.data(),
+                                                  jets_2_.data(),
+                                                  jets_3_.data(),
+                                                  jets_4_.data(),
+                                                  jets_5_.data(),
+                                                  jets_6_.data(),
+                                                  jets_7_.data(),
+                                                  jets_8_.data(),
+                                                  jets_9_.data(),
+                                                  jets_10_.data(),
+                                                  jets_11_.data(),
+                                                  residuals_jets_.data());
 
                 // fill the residual array
                 for (unsigned int i = 0; i < RES; i++)
-                    residuals[i] = (*residuals_jets_)[i].a;
+                    residuals[i] = residuals_jets_[i].a;
 
                 // fill the jacobian matrices
                 for (unsigned int i = 0; i<n_blocks; i++)
                     if (jacobians[i] != nullptr)
                         for (unsigned int row = 0; row < RES; row++)
-                            std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                      (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                            std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                      residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                       jacobians[i] + row * state_block_sizes_.at(i));
             }
             return true;
@@ -232,29 +206,29 @@ class FactorAutodiff : public FactorBase
         {
             // update jets real part
             for (unsigned int i = 0; i < B0; i++)
-                (*jets_0_)[i].a = parameters[0][i];
+                jets_0_[i].a = parameters[0][i];
             for (unsigned int i = 0; i < B1; i++)
-                (*jets_1_)[i].a = parameters[1][i];
+                jets_1_[i].a = parameters[1][i];
             for (unsigned int i = 0; i < B2; i++)
-                (*jets_2_)[i].a = parameters[2][i];
+                jets_2_[i].a = parameters[2][i];
             for (unsigned int i = 0; i < B3; i++)
-                (*jets_3_)[i].a = parameters[3][i];
+                jets_3_[i].a = parameters[3][i];
             for (unsigned int i = 0; i < B4; i++)
-                (*jets_4_)[i].a = parameters[4][i];
+                jets_4_[i].a = parameters[4][i];
             for (unsigned int i = 0; i < B5; i++)
-                (*jets_5_)[i].a = parameters[5][i];
+                jets_5_[i].a = parameters[5][i];
             for (unsigned int i = 0; i < B6; i++)
-                (*jets_6_)[i].a = parameters[6][i];
+                jets_6_[i].a = parameters[6][i];
             for (unsigned int i = 0; i < B7; i++)
-                (*jets_7_)[i].a = parameters[7][i];
+                jets_7_[i].a = parameters[7][i];
             for (unsigned int i = 0; i < B8; i++)
-                (*jets_8_)[i].a = parameters[8][i];
+                jets_8_[i].a = parameters[8][i];
             for (unsigned int i = 0; i < B9; i++)
-                (*jets_9_)[i].a = parameters[9][i];
+                jets_9_[i].a = parameters[9][i];
             for (unsigned int i = 0; i < B10; i++)
-                (*jets_10_)[i].a = parameters[10][i];
+                jets_10_[i].a = parameters[10][i];
             for (unsigned int i = 0; i < B11; i++)
-                (*jets_11_)[i].a = parameters[11][i];
+                jets_11_[i].a = parameters[11][i];
         }
 
         /** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
@@ -271,23 +245,23 @@ class FactorAutodiff : public FactorBase
             updateJetsRealPart(_states_ptr);
 
             // call functor
-            (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                              jets_1_->data(),
-                                              jets_2_->data(),
-                                              jets_3_->data(),
-                                              jets_4_->data(),
-                                              jets_5_->data(),
-                                              jets_6_->data(),
-                                              jets_7_->data(),
-                                              jets_8_->data(),
-                                              jets_9_->data(),
-                                              jets_10_->data(),
-                                              jets_11_->data(),
-                                              residuals_jets_->data());
+            (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                              jets_1_.data(),
+                                              jets_2_.data(),
+                                              jets_3_.data(),
+                                              jets_4_.data(),
+                                              jets_5_.data(),
+                                              jets_6_.data(),
+                                              jets_7_.data(),
+                                              jets_8_.data(),
+                                              jets_9_.data(),
+                                              jets_10_.data(),
+                                              jets_11_.data(),
+                                              residuals_jets_.data());
 
             // fill the residual vector
             for (unsigned int i = 0; i < RES; i++)
-                residual_(i) = (*residuals_jets_)[i].a;
+                residual_(i) = residuals_jets_[i].a;
 
             // fill the jacobian matrices
             for (unsigned int i = 0; i<n_blocks; i++)
@@ -297,8 +271,8 @@ class FactorAutodiff : public FactorBase
                 // Fill Jacobian rows from jets
                 if (!state_ptrs_[i]->isFixed())
                     for (unsigned int row = 0; row < RES; row++)
-                        std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                  (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                        std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                  residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                   Ji.data() + row * state_block_sizes_.at(i));
                 // add to the Jacobians vector
                 jacobians_.push_back(Ji);
@@ -371,19 +345,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
-       std::array<WolfJet, B3>* jets_3_;
-       std::array<WolfJet, B4>* jets_4_;
-       std::array<WolfJet, B5>* jets_5_;
-       std::array<WolfJet, B6>* jets_6_;
-       std::array<WolfJet, B7>* jets_7_;
-       std::array<WolfJet, B8>* jets_8_;
-       std::array<WolfJet, B9>* jets_9_;
-       std::array<WolfJet, B10>* jets_10_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
+       mutable std::array<WolfJet, B3> jets_3_;
+       mutable std::array<WolfJet, B4> jets_4_;
+       mutable std::array<WolfJet, B5> jets_5_;
+       mutable std::array<WolfJet, B6> jets_6_;
+       mutable std::array<WolfJet, B7> jets_7_;
+       mutable std::array<WolfJet, B8> jets_8_;
+       mutable std::array<WolfJet, B9> jets_9_;
+       mutable std::array<WolfJet, B10> jets_10_;
 
    public:
 
@@ -407,19 +380,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
                       StateBlockPtr _state9Ptr,
                       StateBlockPtr _state10Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>),
-           jets_3_(new std::array<WolfJet, B3>),
-           jets_4_(new std::array<WolfJet, B4>),
-           jets_5_(new std::array<WolfJet, B5>),
-           jets_6_(new std::array<WolfJet, B6>),
-           jets_7_(new std::array<WolfJet, B7>),
-           jets_8_(new std::array<WolfJet, B8>),
-           jets_9_(new std::array<WolfJet, B9>),
-           jets_10_(new std::array<WolfJet, B10>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr,_state10Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -437,44 +398,32 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B3; i++)
-              (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+              jets_3_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B4; i++)
-              (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+              jets_4_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B5; i++)
-              (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+              jets_5_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B6; i++)
-              (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+              jets_6_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B7; i++)
-              (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+              jets_7_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B8; i++)
-              (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+              jets_8_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B9; i++)
-              (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+              jets_9_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B10; i++)
-              (*jets_10_)[i] = WolfJet(0, last_jet_idx++);
+              jets_10_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete jets_3_;
-           delete jets_4_;
-           delete jets_5_;
-           delete jets_6_;
-           delete jets_7_;
-           delete jets_8_;
-           delete jets_9_;
-           delete jets_10_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -509,29 +458,29 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                 jets_1_->data(),
-                                                 jets_2_->data(),
-                                                 jets_3_->data(),
-                                                 jets_4_->data(),
-                                                 jets_5_->data(),
-                                                 jets_6_->data(),
-                                                 jets_7_->data(),
-                                                 jets_8_->data(),
-                                                 jets_9_->data(),
-                                                 jets_10_->data(),
-                                                 residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                 jets_1_.data(),
+                                                 jets_2_.data(),
+                                                 jets_3_.data(),
+                                                 jets_4_.data(),
+                                                 jets_5_.data(),
+                                                 jets_6_.data(),
+                                                 jets_7_.data(),
+                                                 jets_8_.data(),
+                                                 jets_9_.data(),
+                                                 jets_10_.data(),
+                                                 residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -541,27 +490,27 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
            for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i].a = parameters[3][i];
+               jets_3_[i].a = parameters[3][i];
            for (unsigned int i = 0; i < B4; i++)
-               (*jets_4_)[i].a = parameters[4][i];
+               jets_4_[i].a = parameters[4][i];
            for (unsigned int i = 0; i < B5; i++)
-               (*jets_5_)[i].a = parameters[5][i];
+               jets_5_[i].a = parameters[5][i];
            for (unsigned int i = 0; i < B6; i++)
-               (*jets_6_)[i].a = parameters[6][i];
+               jets_6_[i].a = parameters[6][i];
            for (unsigned int i = 0; i < B7; i++)
-               (*jets_7_)[i].a = parameters[7][i];
+               jets_7_[i].a = parameters[7][i];
            for (unsigned int i = 0; i < B8; i++)
-               (*jets_8_)[i].a = parameters[8][i];
+               jets_8_[i].a = parameters[8][i];
            for (unsigned int i = 0; i < B9; i++)
-               (*jets_9_)[i].a = parameters[9][i];
+               jets_9_[i].a = parameters[9][i];
            for (unsigned int i = 0; i < B10; i++)
-               (*jets_10_)[i].a = parameters[10][i];
+               jets_10_[i].a = parameters[10][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -575,22 +524,22 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             jets_3_->data(),
-                                             jets_4_->data(),
-                                             jets_5_->data(),
-                                             jets_6_->data(),
-                                             jets_7_->data(),
-                                             jets_8_->data(),
-                                             jets_9_->data(),
-                                             jets_10_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             jets_3_.data(),
+                                             jets_4_.data(),
+                                             jets_5_.data(),
+                                             jets_6_.data(),
+                                             jets_7_.data(),
+                                             jets_8_.data(),
+                                             jets_9_.data(),
+                                             jets_10_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -600,8 +549,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,B10,0> : public Fact
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -660,18 +609,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
-       std::array<WolfJet, B3>* jets_3_;
-       std::array<WolfJet, B4>* jets_4_;
-       std::array<WolfJet, B5>* jets_5_;
-       std::array<WolfJet, B6>* jets_6_;
-       std::array<WolfJet, B7>* jets_7_;
-       std::array<WolfJet, B8>* jets_8_;
-       std::array<WolfJet, B9>* jets_9_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
+       mutable std::array<WolfJet, B3> jets_3_;
+       mutable std::array<WolfJet, B4> jets_4_;
+       mutable std::array<WolfJet, B5> jets_5_;
+       mutable std::array<WolfJet, B6> jets_6_;
+       mutable std::array<WolfJet, B7> jets_7_;
+       mutable std::array<WolfJet, B8> jets_8_;
+       mutable std::array<WolfJet, B9> jets_9_;
 
    public:
 
@@ -694,18 +642,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
                       StateBlockPtr _state8Ptr,
                       StateBlockPtr _state9Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>),
-           jets_3_(new std::array<WolfJet, B3>),
-           jets_4_(new std::array<WolfJet, B4>),
-           jets_5_(new std::array<WolfJet, B5>),
-           jets_6_(new std::array<WolfJet, B6>),
-           jets_7_(new std::array<WolfJet, B7>),
-           jets_8_(new std::array<WolfJet, B8>),
-           jets_9_(new std::array<WolfJet, B9>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -722,41 +659,30 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B3; i++)
-              (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+              jets_3_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B4; i++)
-              (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+              jets_4_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B5; i++)
-              (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+              jets_5_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B6; i++)
-              (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+              jets_6_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B7; i++)
-              (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+              jets_7_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B8; i++)
-              (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+              jets_8_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B9; i++)
-              (*jets_9_)[i] = WolfJet(0, last_jet_idx++);
+              jets_9_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete jets_3_;
-           delete jets_4_;
-           delete jets_5_;
-           delete jets_6_;
-           delete jets_7_;
-           delete jets_8_;
-           delete jets_9_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -790,28 +716,28 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                 jets_1_->data(),
-                                                 jets_2_->data(),
-                                                 jets_3_->data(),
-                                                 jets_4_->data(),
-                                                 jets_5_->data(),
-                                                 jets_6_->data(),
-                                                 jets_7_->data(),
-                                                 jets_8_->data(),
-                                                 jets_9_->data(),
-                                                 residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                 jets_1_.data(),
+                                                 jets_2_.data(),
+                                                 jets_3_.data(),
+                                                 jets_4_.data(),
+                                                 jets_5_.data(),
+                                                 jets_6_.data(),
+                                                 jets_7_.data(),
+                                                 jets_8_.data(),
+                                                 jets_9_.data(),
+                                                 residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -821,25 +747,25 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
            for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i].a = parameters[3][i];
+               jets_3_[i].a = parameters[3][i];
            for (unsigned int i = 0; i < B4; i++)
-               (*jets_4_)[i].a = parameters[4][i];
+               jets_4_[i].a = parameters[4][i];
            for (unsigned int i = 0; i < B5; i++)
-               (*jets_5_)[i].a = parameters[5][i];
+               jets_5_[i].a = parameters[5][i];
            for (unsigned int i = 0; i < B6; i++)
-               (*jets_6_)[i].a = parameters[6][i];
+               jets_6_[i].a = parameters[6][i];
            for (unsigned int i = 0; i < B7; i++)
-               (*jets_7_)[i].a = parameters[7][i];
+               jets_7_[i].a = parameters[7][i];
            for (unsigned int i = 0; i < B8; i++)
-               (*jets_8_)[i].a = parameters[8][i];
+               jets_8_[i].a = parameters[8][i];
            for (unsigned int i = 0; i < B9; i++)
-               (*jets_9_)[i].a = parameters[9][i];
+               jets_9_[i].a = parameters[9][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -853,21 +779,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             jets_3_->data(),
-                                             jets_4_->data(),
-                                             jets_5_->data(),
-                                             jets_6_->data(),
-                                             jets_7_->data(),
-                                             jets_8_->data(),
-                                             jets_9_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             jets_3_.data(),
+                                             jets_4_.data(),
+                                             jets_5_.data(),
+                                             jets_6_.data(),
+                                             jets_7_.data(),
+                                             jets_8_.data(),
+                                             jets_9_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -877,8 +803,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9,0,0> : public Factor
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -937,17 +863,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
-       std::array<WolfJet, B3>* jets_3_;
-       std::array<WolfJet, B4>* jets_4_;
-       std::array<WolfJet, B5>* jets_5_;
-       std::array<WolfJet, B6>* jets_6_;
-       std::array<WolfJet, B7>* jets_7_;
-       std::array<WolfJet, B8>* jets_8_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
+       mutable std::array<WolfJet, B3> jets_3_;
+       mutable std::array<WolfJet, B4> jets_4_;
+       mutable std::array<WolfJet, B5> jets_5_;
+       mutable std::array<WolfJet, B6> jets_6_;
+       mutable std::array<WolfJet, B7> jets_7_;
+       mutable std::array<WolfJet, B8> jets_8_;
 
    public:
 
@@ -969,17 +894,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
                       StateBlockPtr _state7Ptr,
                       StateBlockPtr _state8Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>),
-           jets_3_(new std::array<WolfJet, B3>),
-           jets_4_(new std::array<WolfJet, B4>),
-           jets_5_(new std::array<WolfJet, B5>),
-           jets_6_(new std::array<WolfJet, B6>),
-           jets_7_(new std::array<WolfJet, B7>),
-           jets_8_(new std::array<WolfJet, B8>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -995,38 +910,28 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B3; i++)
-              (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+              jets_3_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B4; i++)
-              (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+              jets_4_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B5; i++)
-              (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+              jets_5_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B6; i++)
-              (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+              jets_6_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B7; i++)
-              (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+              jets_7_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B8; i++)
-              (*jets_8_)[i] = WolfJet(0, last_jet_idx++);
+              jets_8_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete jets_3_;
-           delete jets_4_;
-           delete jets_5_;
-           delete jets_6_;
-           delete jets_7_;
-           delete jets_8_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -1059,27 +964,27 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                        jets_1_->data(),
-                                                        jets_2_->data(),
-                                                        jets_3_->data(),
-                                                        jets_4_->data(),
-                                                        jets_5_->data(),
-                                                        jets_6_->data(),
-                                                        jets_7_->data(),
-                                                        jets_8_->data(),
-                                                        residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                        jets_1_.data(),
+                                                        jets_2_.data(),
+                                                        jets_3_.data(),
+                                                        jets_4_.data(),
+                                                        jets_5_.data(),
+                                                        jets_6_.data(),
+                                                        jets_7_.data(),
+                                                        jets_8_.data(),
+                                                        residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -1089,23 +994,23 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
            for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i].a = parameters[3][i];
+               jets_3_[i].a = parameters[3][i];
            for (unsigned int i = 0; i < B4; i++)
-               (*jets_4_)[i].a = parameters[4][i];
+               jets_4_[i].a = parameters[4][i];
            for (unsigned int i = 0; i < B5; i++)
-               (*jets_5_)[i].a = parameters[5][i];
+               jets_5_[i].a = parameters[5][i];
            for (unsigned int i = 0; i < B6; i++)
-               (*jets_6_)[i].a = parameters[6][i];
+               jets_6_[i].a = parameters[6][i];
            for (unsigned int i = 0; i < B7; i++)
-               (*jets_7_)[i].a = parameters[7][i];
+               jets_7_[i].a = parameters[7][i];
            for (unsigned int i = 0; i < B8; i++)
-               (*jets_8_)[i].a = parameters[8][i];
+               jets_8_[i].a = parameters[8][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1119,20 +1024,20 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             jets_3_->data(),
-                                             jets_4_->data(),
-                                             jets_5_->data(),
-                                             jets_6_->data(),
-                                             jets_7_->data(),
-                                             jets_8_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             jets_3_.data(),
+                                             jets_4_.data(),
+                                             jets_5_.data(),
+                                             jets_6_.data(),
+                                             jets_7_.data(),
+                                             jets_8_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -1142,8 +1047,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0,0,0> : public FactorB
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -1202,16 +1107,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
-       std::array<WolfJet, B3>* jets_3_;
-       std::array<WolfJet, B4>* jets_4_;
-       std::array<WolfJet, B5>* jets_5_;
-       std::array<WolfJet, B6>* jets_6_;
-       std::array<WolfJet, B7>* jets_7_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
+       mutable std::array<WolfJet, B3> jets_3_;
+       mutable std::array<WolfJet, B4> jets_4_;
+       mutable std::array<WolfJet, B5> jets_5_;
+       mutable std::array<WolfJet, B6> jets_6_;
+       mutable std::array<WolfJet, B7> jets_7_;
 
    public:
 
@@ -1232,16 +1136,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
                       StateBlockPtr _state6Ptr,
                       StateBlockPtr _state7Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>),
-           jets_3_(new std::array<WolfJet, B3>),
-           jets_4_(new std::array<WolfJet, B4>),
-           jets_5_(new std::array<WolfJet, B5>),
-           jets_6_(new std::array<WolfJet, B6>),
-           jets_7_(new std::array<WolfJet, B7>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -1256,35 +1151,26 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B3; i++)
-              (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+              jets_3_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B4; i++)
-              (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+              jets_4_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B5; i++)
-              (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+              jets_5_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B6; i++)
-              (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+              jets_6_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B7; i++)
-              (*jets_7_)[i] = WolfJet(0, last_jet_idx++);
+              jets_7_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete jets_3_;
-           delete jets_4_;
-           delete jets_5_;
-           delete jets_6_;
-           delete jets_7_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -1316,26 +1202,26 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                 jets_1_->data(),
-                                                 jets_2_->data(),
-                                                 jets_3_->data(),
-                                                 jets_4_->data(),
-                                                 jets_5_->data(),
-                                                 jets_6_->data(),
-                                                 jets_7_->data(),
-                                                 residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                 jets_1_.data(),
+                                                 jets_2_.data(),
+                                                 jets_3_.data(),
+                                                 jets_4_.data(),
+                                                 jets_5_.data(),
+                                                 jets_6_.data(),
+                                                 jets_7_.data(),
+                                                 residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -1345,21 +1231,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
            for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i].a = parameters[3][i];
+               jets_3_[i].a = parameters[3][i];
            for (unsigned int i = 0; i < B4; i++)
-               (*jets_4_)[i].a = parameters[4][i];
+               jets_4_[i].a = parameters[4][i];
            for (unsigned int i = 0; i < B5; i++)
-               (*jets_5_)[i].a = parameters[5][i];
+               jets_5_[i].a = parameters[5][i];
            for (unsigned int i = 0; i < B6; i++)
-               (*jets_6_)[i].a = parameters[6][i];
+               jets_6_[i].a = parameters[6][i];
            for (unsigned int i = 0; i < B7; i++)
-               (*jets_7_)[i].a = parameters[7][i];
+               jets_7_[i].a = parameters[7][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1373,19 +1259,19 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             jets_3_->data(),
-                                             jets_4_->data(),
-                                             jets_5_->data(),
-                                             jets_6_->data(),
-                                             jets_7_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             jets_3_.data(),
+                                             jets_4_.data(),
+                                             jets_5_.data(),
+                                             jets_6_.data(),
+                                             jets_7_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -1395,8 +1281,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0,0,0> : public FactorBa
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -1455,15 +1341,14 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
-       std::array<WolfJet, B3>* jets_3_;
-       std::array<WolfJet, B4>* jets_4_;
-       std::array<WolfJet, B5>* jets_5_;
-       std::array<WolfJet, B6>* jets_6_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
+       mutable std::array<WolfJet, B3> jets_3_;
+       mutable std::array<WolfJet, B4> jets_4_;
+       mutable std::array<WolfJet, B5> jets_5_;
+       mutable std::array<WolfJet, B6> jets_6_;
 
    public:
 
@@ -1483,15 +1368,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
                       StateBlockPtr _state5Ptr,
                       StateBlockPtr _state6Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>),
-           jets_3_(new std::array<WolfJet, B3>),
-           jets_4_(new std::array<WolfJet, B4>),
-           jets_5_(new std::array<WolfJet, B5>),
-           jets_6_(new std::array<WolfJet, B6>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -1505,32 +1382,24 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B3; i++)
-              (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+              jets_3_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B4; i++)
-              (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+              jets_4_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B5; i++)
-              (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+              jets_5_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B6; i++)
-              (*jets_6_)[i] = WolfJet(0, last_jet_idx++);
+              jets_6_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete jets_3_;
-           delete jets_4_;
-           delete jets_5_;
-           delete jets_6_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -1561,25 +1430,25 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                        jets_1_->data(),
-                                                        jets_2_->data(),
-                                                        jets_3_->data(),
-                                                        jets_4_->data(),
-                                                        jets_5_->data(),
-                                                        jets_6_->data(),
-                                                        residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                        jets_1_.data(),
+                                                        jets_2_.data(),
+                                                        jets_3_.data(),
+                                                        jets_4_.data(),
+                                                        jets_5_.data(),
+                                                        jets_6_.data(),
+                                                        residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -1589,19 +1458,19 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
            for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i].a = parameters[3][i];
+               jets_3_[i].a = parameters[3][i];
            for (unsigned int i = 0; i < B4; i++)
-               (*jets_4_)[i].a = parameters[4][i];
+               jets_4_[i].a = parameters[4][i];
            for (unsigned int i = 0; i < B5; i++)
-               (*jets_5_)[i].a = parameters[5][i];
+               jets_5_[i].a = parameters[5][i];
            for (unsigned int i = 0; i < B6; i++)
-               (*jets_6_)[i].a = parameters[6][i];
+               jets_6_[i].a = parameters[6][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1615,18 +1484,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             jets_3_->data(),
-                                             jets_4_->data(),
-                                             jets_5_->data(),
-                                             jets_6_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             jets_3_.data(),
+                                             jets_4_.data(),
+                                             jets_5_.data(),
+                                             jets_6_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -1636,8 +1505,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0,0,0> : public FactorBas
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -1696,14 +1565,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
-       std::array<WolfJet, B3>* jets_3_;
-       std::array<WolfJet, B4>* jets_4_;
-       std::array<WolfJet, B5>* jets_5_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
+       mutable std::array<WolfJet, B3> jets_3_;
+       mutable std::array<WolfJet, B4> jets_4_;
+       mutable std::array<WolfJet, B5> jets_5_;
 
    public:
 
@@ -1722,14 +1590,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
                       StateBlockPtr _state4Ptr,
                       StateBlockPtr _state5Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>),
-           jets_3_(new std::array<WolfJet, B3>),
-           jets_4_(new std::array<WolfJet, B4>),
-           jets_5_(new std::array<WolfJet, B5>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -1742,29 +1603,22 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B3; i++)
-              (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+              jets_3_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B4; i++)
-              (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+              jets_4_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B5; i++)
-              (*jets_5_)[i] = WolfJet(0, last_jet_idx++);
+              jets_5_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete jets_3_;
-           delete jets_4_;
-           delete jets_5_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -1794,24 +1648,24 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                        jets_1_->data(),
-                                                        jets_2_->data(),
-                                                        jets_3_->data(),
-                                                        jets_4_->data(),
-                                                        jets_5_->data(),
-                                                        residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                        jets_1_.data(),
+                                                        jets_2_.data(),
+                                                        jets_3_.data(),
+                                                        jets_4_.data(),
+                                                        jets_5_.data(),
+                                                        residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -1821,17 +1675,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
            for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i].a = parameters[3][i];
+               jets_3_[i].a = parameters[3][i];
            for (unsigned int i = 0; i < B4; i++)
-               (*jets_4_)[i].a = parameters[4][i];
+               jets_4_[i].a = parameters[4][i];
            for (unsigned int i = 0; i < B5; i++)
-               (*jets_5_)[i].a = parameters[5][i];
+               jets_5_[i].a = parameters[5][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -1845,17 +1699,17 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             jets_3_->data(),
-                                             jets_4_->data(),
-                                             jets_5_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             jets_3_.data(),
+                                             jets_4_.data(),
+                                             jets_5_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -1865,8 +1719,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0,0,0> : public FactorBase
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -1920,13 +1774,12 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
-       std::array<WolfJet, B3>* jets_3_;
-       std::array<WolfJet, B4>* jets_4_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
+       mutable std::array<WolfJet, B3> jets_3_;
+       mutable std::array<WolfJet, B4> jets_4_;
 
    public:
 
@@ -1944,13 +1797,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
                       StateBlockPtr _state3Ptr,
                       StateBlockPtr _state4Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>),
-           jets_3_(new std::array<WolfJet, B3>),
-           jets_4_(new std::array<WolfJet, B4>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -1962,26 +1809,20 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B3; i++)
-              (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+              jets_3_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B4; i++)
-              (*jets_4_)[i] = WolfJet(0, last_jet_idx++);
+              jets_4_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete jets_3_;
-           delete jets_4_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -2010,23 +1851,23 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                        jets_1_->data(),
-                                                        jets_2_->data(),
-                                                        jets_3_->data(),
-                                                        jets_4_->data(),
-                                                        residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                        jets_1_.data(),
+                                                        jets_2_.data(),
+                                                        jets_3_.data(),
+                                                        jets_4_.data(),
+                                                        residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -2036,15 +1877,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
            for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i].a = parameters[3][i];
+               jets_3_[i].a = parameters[3][i];
            for (unsigned int i = 0; i < B4; i++)
-               (*jets_4_)[i].a = parameters[4][i];
+               jets_4_[i].a = parameters[4][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2058,16 +1899,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             jets_3_->data(),
-                                             jets_4_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             jets_3_.data(),
+                                             jets_4_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -2077,8 +1918,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0,0,0> : public FactorBase
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -2132,12 +1973,11 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
-       std::array<WolfJet, B3>* jets_3_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
+       mutable std::array<WolfJet, B3> jets_3_;
 
    public:
 
@@ -2154,12 +1994,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
                       StateBlockPtr _state2Ptr,
                       StateBlockPtr _state3Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>),
-           jets_3_(new std::array<WolfJet, B3>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -2170,23 +2005,18 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B3; i++)
-              (*jets_3_)[i] = WolfJet(0, last_jet_idx++);
+              jets_3_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete jets_3_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -2214,22 +2044,22 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                 jets_1_->data(),
-                                                 jets_2_->data(),
-                                                 jets_3_->data(),
-                                                 residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                 jets_1_.data(),
+                                                 jets_2_.data(),
+                                                 jets_3_.data(),
+                                                 residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -2239,13 +2069,13 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
            for (unsigned int i = 0; i < B3; i++)
-               (*jets_3_)[i].a = parameters[3][i];
+               jets_3_[i].a = parameters[3][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2259,15 +2089,15 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             jets_3_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             jets_3_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -2277,8 +2107,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0,0,0> : public FactorBase
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -2336,11 +2166,10 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
-       std::array<WolfJet, B2>* jets_2_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
+       mutable std::array<WolfJet, B2> jets_2_;
 
    public:
 
@@ -2356,11 +2185,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
                       StateBlockPtr _state1Ptr,
                       StateBlockPtr _state2Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>),
-           jets_2_(new std::array<WolfJet, B2>)
+           state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -2370,20 +2195,16 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B2; i++)
-              (*jets_2_)[i] = WolfJet(0, last_jet_idx++);
+              jets_2_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete jets_2_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -2410,21 +2231,21 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                        jets_1_->data(),
-                                                        jets_2_->data(),
-                                                        residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                        jets_1_.data(),
+                                                        jets_2_.data(),
+                                                        residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -2434,11 +2255,11 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
            for (unsigned int i = 0; i < B2; i++)
-               (*jets_2_)[i].a = parameters[2][i];
+               jets_2_[i].a = parameters[2][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2452,14 +2273,14 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             jets_2_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             jets_2_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -2469,8 +2290,8 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0,0,0> : public FactorBase
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -2528,10 +2349,9 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
-       std::array<WolfJet, B1>* jets_1_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
+       mutable std::array<WolfJet, B1> jets_1_;
 
    public:
 
@@ -2546,10 +2366,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
                       StateBlockPtr _state0Ptr,
                       StateBlockPtr _state1Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr,_state1Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>),
-           jets_1_(new std::array<WolfJet, B1>)
+           state_ptrs_({_state0Ptr,_state1Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -2558,17 +2375,14 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            for (unsigned int i = 0; i < B1; i++)
-              (*jets_1_)[i] = WolfJet(0, last_jet_idx++);
+              jets_1_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete jets_1_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -2594,20 +2408,20 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                 jets_1_->data(),
-                                                 residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                 jets_1_.data(),
+                                                 residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -2617,9 +2431,9 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
            for (unsigned int i = 0; i < B1; i++)
-               (*jets_1_)[i].a = parameters[1][i];
+               jets_1_[i].a = parameters[1][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2633,13 +2447,13 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             jets_1_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             jets_1_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -2649,8 +2463,8 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0,0,0> : public FactorBase
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
@@ -2708,9 +2522,8 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
        std::vector<StateBlockPtr> state_ptrs_;
 
        static const std::vector<unsigned int> jacobian_locations_;
-       std::array<WolfJet, RES>* residuals_jets_;
-
-       std::array<WolfJet, B0>* jets_0_;
+       mutable std::array<WolfJet, RES> residuals_jets_;
+       mutable std::array<WolfJet, B0> jets_0_;
 
    public:
 
@@ -2724,9 +2537,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
                       FactorStatus _status,
                       StateBlockPtr _state0Ptr) :
            FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
-           state_ptrs_({_state0Ptr}),
-           residuals_jets_(new std::array<WolfJet, RES>),
-           jets_0_(new std::array<WolfJet, B0>)
+           state_ptrs_({_state0Ptr})
        {
            // asserts for null states
            assert(_state0Ptr  != nullptr && "nullptr state block");
@@ -2734,14 +2545,12 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
            // initialize jets
            unsigned int last_jet_idx = 0;
            for (unsigned int i = 0; i < B0; i++)
-              (*jets_0_)[i] = WolfJet(0, last_jet_idx++);
+              jets_0_[i] = WolfJet(0, last_jet_idx++);
            state_ptrs_.resize(n_blocks);
        }
 
        virtual ~FactorAutodiff()
        {
-           delete jets_0_;
-           delete residuals_jets_;
        }
 
        virtual JacobianMethod getJacobianMethod() const
@@ -2766,19 +2575,19 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
                updateJetsRealPart(param_vec);
 
                // call functor
-               (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                                 residuals_jets_->data());
+               (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                                 residuals_jets_.data());
 
                // fill the residual array
                for (unsigned int i = 0; i < RES; i++)
-                   residuals[i] = (*residuals_jets_)[i].a;
+                   residuals[i] = residuals_jets_[i].a;
 
                // fill the jacobian matrices
                for (unsigned int i = 0; i<n_blocks; i++)
                    if (jacobians[i] != nullptr)
                        for (unsigned int row = 0; row < RES; row++)
-                           std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                     (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                           std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                     residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                      jacobians[i] + row * state_block_sizes_.at(i));
            }
            return true;
@@ -2788,7 +2597,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
        {
            // update jets real part
            for (unsigned int i = 0; i < B0; i++)
-               (*jets_0_)[i].a = parameters[0][i];
+               jets_0_[i].a = parameters[0][i];
        }
 
        virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
@@ -2802,12 +2611,12 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
            updateJetsRealPart(_states_ptr);
 
            // call functor
-           (*static_cast<FacT const*>(this))(jets_0_->data(),
-                                             residuals_jets_->data());
+           (*static_cast<FacT const*>(this))(jets_0_.data(),
+                                             residuals_jets_.data());
 
            // fill the residual vector
            for (unsigned int i = 0; i < RES; i++)
-               residual_(i) = (*residuals_jets_)[i].a;
+               residual_(i) = residuals_jets_[i].a;
 
            // fill the jacobian matrices
            for (unsigned int i = 0; i<n_blocks; i++)
@@ -2817,8 +2626,8 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0,0,0> : public FactorBase
                // Fill Jacobian rows from jets
                if (!state_ptrs_[i]->isFixed())
                    for (unsigned int row = 0; row < RES; row++)
-                       std::copy((*residuals_jets_)[row].v.data() + jacobian_locations_.at(i),
-                                 (*residuals_jets_)[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
+                       std::copy(residuals_jets_[row].v.data() + jacobian_locations_.at(i),
+                                 residuals_jets_[row].v.data() + jacobian_locations_.at(i) + state_block_sizes_.at(i),
                                  Ji.data() + row * state_block_sizes_.at(i));
                // add to the Jacobians vector
                jacobians_.push_back(Ji);
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index a5b41ebfe3ebf77f269148d42781b7dd96cd36a2..548fcf6e1e4f99b9d020784b226f4f1df75e4b98 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -72,30 +72,16 @@ struct Motion
  *   - If the query time stamp does not match any time stamp in the buffer,
  *     the returned motion-integral or delta-integral is the one immediately before the query time stamp.
  */
-class MotionBuffer{
+class MotionBuffer : public std::list<Motion>
+{
     public:
         MotionBuffer() ;
-        std::list<Motion>& get();
-        const std::list<Motion>& get() const;
         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);
-
-    private:
-        std::list<Motion> container_;
 };
 
-inline std::list<Motion>& MotionBuffer::get()
-{
-    return container_;
-}
-
-inline const std::list<Motion>& MotionBuffer::get() const
-{
-    return container_;
-}
-
 } // namespace wolf
 
 #endif /* SRC_MOTIONBUFFER_H_ */
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 00d5698016447ede93473c94fc36b2a57179beeb..b12d92f10a430810150040fe9277346d2f63c620 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -326,6 +326,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
 
         virtual bool permittedAuxFrame() final;
 
+        virtual void setProblem(ProblemPtr) override;
+
     public:
         /**\brief notify a new keyframe made by another processor
          *
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 74d834693858cfb2fb86d9b30e3755ccc198a6be..d55c279fc3fd9ce5d083529b41d3448191f1f395 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -147,7 +147,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
     protected:
         ParamsProcessorMotionPtr params_motion_;
         ProcessingStep processing_step_;        ///< State machine controlling the processing step
-        virtual void setProblem(ProblemPtr) override;
 
     // This is the main public interface
     public:
@@ -171,7 +170,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          * \param _x the returned state vector
          */
         virtual void getCurrentState(Eigen::VectorXd& _x) const override;
-        virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().get().back().ts_; }
+        virtual void getCurrentTimeStamp(TimeStamp& _ts) const override { _ts = getBuffer().back().ts_; }
         using IsMotion::getCurrentState;
         using IsMotion::getCurrentTimeStamp;
 
@@ -530,12 +529,12 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
 
 inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
 {
-    return getBuffer().get().back().delta_integr_cov_;
+    return getBuffer().back().delta_integr_cov_;
 }
 
 inline Motion ProcessorMotion::getMotion() const
 {
-    return getBuffer().get().back();
+    return getBuffer().back();
 }
 
 inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
@@ -548,7 +547,7 @@ inline Motion ProcessorMotion::getMotion(const TimeStamp& _ts) const
 
 inline double ProcessorMotion::updateDt()
 {
-    return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().get().back().ts_;
+    return dt_ = incoming_ptr_->getTimeStamp() - getBuffer().back().ts_;
 }
 
 inline const MotionBuffer& ProcessorMotion::getBuffer() const
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 9d4b96c3b9a096089b9ad051b4cf8a48510860c1..65a78728260a755e3b9ddbc370d7d8bc623a3160 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -228,15 +228,12 @@ void CaptureBase::move(FrameBasePtr _frm_ptr)
     WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame");
     WOLF_WARN_COND(_frm_ptr == nullptr, "moving a capture to a null FrameBasePtr");
 
+    assert((this->getFrame() == nullptr || this->getFrame()->isKey()) && "Forbidden: moving a capture already linked to a KF");
+    assert((_frm_ptr == nullptr || _frm_ptr->isKey()) && "Forbidden: moving a capture to a non-estimated frame");
+
     // Unlink
     if (this->getFrame())
     {
-        if (this->getFrame()->isKey())
-        {
-            WOLF_ERROR("moving a capture linked to a KF");
-            return;
-        }
-
         // unlink from previous non-key frame
         this->getFrame()->removeCapture(shared_from_this());
         this->setFrame(nullptr);
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index f9cda1f9ad32920b7c66c435f8fdffee1277d279..aa7d4f23a633a01090c9a0d042e46ff153613bd6 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -54,8 +54,8 @@ CaptureMotion::~CaptureMotion()
 Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const
 {
     VectorXd calib_preint    = getCalibrationPreint();
-    VectorXd delta_preint    = getBuffer().get().back().delta_integr_;
-    MatrixXd jac_calib       = getBuffer().get().back().jacobian_calib_;
+    VectorXd delta_preint    = getBuffer().back().delta_integr_;
+    MatrixXd jac_calib       = getBuffer().back().jacobian_calib_;
     VectorXd delta_error     = jac_calib * (_calib_current - calib_preint);
     VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
     return   delta_corrected;
@@ -110,8 +110,8 @@ void CaptureMotion::printHeader(int _depth, bool _constr_by, bool _metric, bool
             }
         }
 
-    _stream << _tabs << "  " << "buffer size  :  " << getBuffer().get().size() << std::endl;
-    if ( _metric && ! getBuffer().get().empty())
+    _stream << _tabs << "  " << "buffer size  :  " << getBuffer().size() << std::endl;
+    if ( _metric && ! getBuffer().empty())
     {
         _stream << _tabs << "  " << "delta preint : (" << getDeltaPreint().transpose() << ")" << std::endl;
         if (hasCalibration())
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 0f65ddb4dcd0587fff4cc9cae0d0f6491c10f437..ee45aa68570db221c37b2d686c26aab948d6f40e 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -256,12 +256,17 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
     {
         WOLF_WARN("Linking with nullptr");
         return;
+
+        // if frame, should not be not-key
+        if (getCapture() and getFrame())
+            assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
     }
 
     // link with feature
     _ftr_ptr->addFactor(shared_from_this());
     this->setFeature(_ftr_ptr);
 
+
     // set problem ( and register factor )
     WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
     this->setProblem(_ftr_ptr->getProblem());
@@ -270,7 +275,11 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
     for (auto& frm_ow : frame_other_list_)
     {
         auto frame_other = frm_ow.lock();
-        if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
+        if(frame_other != nullptr)
+        {
+            assert(frame_other->isKey() && "Forbidden: linking a factor with a non-key frame_other.");
+            frame_other->addConstrainedBy(shared_from_this());
+        }
     }
     for (auto& cap_ow : capture_other_list_)
     {
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 5d0a595673da23a73efd69d6b1c12d0734f1172b..52383b14cbf0cb7c355f416ef96a20e726bd711b 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -64,17 +64,17 @@ void Motion::resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_
 
 MotionBuffer::MotionBuffer()
 {
-    container_.clear();
+    this->clear();
 }
 
 const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
 {
-    //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
-    auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+    //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+    auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
     {
         return m.ts_ <= _ts;
     });
-    if (previous == container_.rend())
+    if (previous == this->rend())
         // The time stamp is older than the buffer's oldest data.
         // We could do something here, and throw an error or something, but by now we'll return the first valid data
         previous--;
@@ -84,12 +84,12 @@ const Motion& MotionBuffer::getMotion(const TimeStamp& _ts) const
 
 void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
 {
-    //assert((container_.front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
-    auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+    //assert((this->front().ts_ <= _ts) && "Query time stamp out of buffer bounds");
+    auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
     {
         return m.ts_ <= _ts;
     });
-    if (previous == container_.rend())
+    if (previous == this->rend())
         // The time stamp is older than the buffer's oldest data.
         // We could do something here, but by now we'll return the last valid data
         previous--;
@@ -99,25 +99,25 @@ void MotionBuffer::getMotion(const TimeStamp& _ts, Motion& _motion) const
 
 void MotionBuffer::split(const TimeStamp& _ts, MotionBuffer& _buffer_part_before_ts)
 {
-    assert((container_.front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
-    assert((container_.back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
+    assert((this->front().ts_ <= _ts) && "Error: Query time stamp is smaller than the buffer's first tick");
+    assert((this->back().ts_ >= _ts) && "Error: Query time stamp is greater than the buffer's last tick");
 
-    auto previous = std::find_if(container_.rbegin(), container_.rend(), [&](const Motion& m)
+    auto previous = std::find_if(this->rbegin(), this->rend(), [&](const Motion& m)
     {
         return m.ts_ <= _ts;
     });
-    if (previous == container_.rend())
+    if (previous == this->rend())
     {
         // The time stamp is more recent than the buffer's most recent data:
         // return an empty buffer as the _oldest_buffer
-        _buffer_part_before_ts.get().clear();
+        _buffer_part_before_ts.clear();
     }
     else
     {
         // Transfer part of the buffer
-        _buffer_part_before_ts.container_.splice(_buffer_part_before_ts.container_.begin(),
-                                                 container_,
-                                                 container_.begin(),
+        _buffer_part_before_ts.splice(_buffer_part_before_ts.begin(),
+                                                 *this,
+                                                 this->begin(),
                                                  (previous--).base());
     }
 }
@@ -130,15 +130,15 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b
 
     if (!show_data && !show_delta && !show_delta_int && !show_jacs)
     {
-        cout << "Buffer state [" << container_.size() << "] : <";
-        for (Motion mot : container_)
+        cout << "Buffer state [" << this->size() << "] : <";
+        for (Motion mot : *this)
             cout << " " << mot.ts_;
         cout << " >" << endl;
     }
     else
     {
         print(0,0,0,0);
-        for (Motion mot : container_)
+        for (Motion mot : *this)
         {
             cout << "-- Motion (" << mot.ts_ << ")" << endl;
             if (show_data)
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 101bd5abb500ff95feec86e3c8969ab4f82301d5..6f28aef4d3393d1baf4a652fa4a06052ba9abb82 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -104,6 +104,23 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
         WOLF_WARN("Linking with a nullptr");
     }
 }
+
+void ProcessorBase::setProblem(ProblemPtr _problem)
+{
+    std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
+    assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+
+    if (_problem == nullptr or _problem == this->getProblem())
+        return;
+
+    NodeBase::setProblem(_problem);
+
+    // adding processor is motion to the processor is motion vector
+    auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this());
+    if (is_motion_ptr)
+        getProblem()->addProcessorIsMotion(is_motion_ptr);
+}
+
 /////////////////////////////////////////////////////////////////////////////////////////
 
 void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const double& _time_tolerance)
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 6993332c3e23dec79c7697abe6f56a1feb531ce7..55841ede92a1bb5dbf4d49bc190b53ab3392cb64 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -148,10 +148,10 @@ FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_moti
 {
     auto key_feature_ptr = FeatureBase::emplace<FeatureMotion>(_capture_motion,
                                                                "ProcessorDiffDrive",
-                                                               _capture_motion->getBuffer().get().back().delta_integr_,
-                                                               _capture_motion->getBuffer().get().back().delta_integr_cov_,
+                                                               _capture_motion->getBuffer().back().delta_integr_,
+                                                               _capture_motion->getBuffer().back().delta_integr_cov_,
                                                                _capture_motion->getCalibrationPreint(),
-                                                               _capture_motion->getBuffer().get().back().jacobian_calib_);
+                                                               _capture_motion->getBuffer().back().jacobian_calib_);
 
     return key_feature_ptr;
 }
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 080f54afce83a7fe4f8c7b1e5cd02f0e69dbb581..817dfb367f6aacee7d8be66231f5d2e7e4d56c29 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -66,8 +66,8 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
     _capture_source->getBuffer().split(_ts_split, _capture_target->getBuffer());
 
     // start with empty motion
-    TimeStamp t_zero = _capture_target->getBuffer().get().back().ts_; // last tick of previous buffer is zero tick of current buffer
-    _capture_source->getBuffer().get().push_front(motionZero(t_zero));
+    TimeStamp t_zero = _capture_target->getBuffer().back().ts_; // last tick of previous buffer is zero tick of current buffer
+    _capture_source->getBuffer().push_front(motionZero(t_zero));
 
     // Update the existing capture
     _capture_source->setOriginCapture(_capture_target);
@@ -207,8 +207,8 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                 auto feature_existing = capture_existing->getFeatureList().back(); // there is only one feature!
 
                 // Modify existing feature --------
-                feature_existing->setMeasurement          (capture_existing->getBuffer().get().back().delta_integr_);
-                feature_existing->setMeasurementCovariance(capture_existing->getBuffer().get().back().delta_integr_cov_);
+                feature_existing->setMeasurement          (capture_existing->getBuffer().back().delta_integr_);
+                feature_existing->setMeasurementCovariance(capture_existing->getBuffer().back().delta_integr_cov_);
 
                 // Modify existing factor --------
                 // Instead of modifying, we remove one ctr, and create a new one.
@@ -367,7 +367,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
                                              last_ptr_->getCalibration(),
                                              last_ptr_);
         // reset the new buffer
-        capture_new->getBuffer().get().push_back( motionZero(key_frame->getTimeStamp()) ) ;
+        capture_new->getBuffer().push_back( motionZero(key_frame->getTimeStamp()) ) ;
 
         // reset derived things
         resetDerived();
@@ -422,7 +422,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
         _x.resize( state_0.size() );
 
         // Compose on top of origin state using the buffered time stamp, not the query time stamp
-        double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
+        double dt = motion.ts_ - capture_motion->getBuffer().front().ts_;
         statePlusDelta(state_0, delta, dt, _x);
     }
     else
@@ -482,7 +482,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
                                origin_ptr_);
     last_frame_ptr_ = new_frame_ptr;
     // clear and reset buffer
-    getBuffer().get().push_back(motionZero(_origin_frame->getTimeStamp()));
+    getBuffer().push_back(motionZero(_origin_frame->getTimeStamp()));
 
     // Reset derived things
     resetDerived();
@@ -507,7 +507,7 @@ void ProcessorMotion::integrateOneStep()
                         jacobian_delta_calib_);
 
     // integrate the current delta to pre-integrated measurements, and get Jacobians
-    deltaPlusDelta(getBuffer().get().back().delta_integr_,
+    deltaPlusDelta(getBuffer().back().delta_integr_,
                    delta_,
                    dt_,
                    delta_integrated_,
@@ -518,17 +518,17 @@ void ProcessorMotion::integrateOneStep()
     if ( hasCalibration() )
     {
         jacobian_calib_.noalias()
-            = jacobian_delta_preint_ * getBuffer().get().back().jacobian_calib_
+            = jacobian_delta_preint_ * getBuffer().back().jacobian_calib_
             + jacobian_delta_ * jacobian_delta_calib_;
     }
 
     // Integrate covariance
     delta_integrated_cov_.noalias()
-            = jacobian_delta_preint_ * getBuffer().get().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
+            = jacobian_delta_preint_ * getBuffer().back().delta_integr_cov_ * jacobian_delta_preint_.transpose()
             + jacobian_delta_        * delta_cov_                                 * jacobian_delta_.transpose();
 
     // push all into buffer
-    getBuffer().get().emplace_back(incoming_ptr_->getTimeStamp(),
+    getBuffer().emplace_back(incoming_ptr_->getTimeStamp(),
                                    incoming_ptr_->getData(),
                                    incoming_ptr_->getDataCovariance(),
                                    delta_,
@@ -550,15 +550,15 @@ void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
     jacobian_calib_      .setZero();
 
     // check that first motion in buffer is zero!
-    assert(_capture_ptr->getBuffer().get().front().delta_integr_     == delta_integrated_     && "Buffer's first motion is not zero!");
-    assert(_capture_ptr->getBuffer().get().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
-    assert(_capture_ptr->getBuffer().get().front().jacobian_calib_   == jacobian_calib_       && "Buffer's first motion is not zero!");
+    assert(_capture_ptr->getBuffer().front().delta_integr_     == delta_integrated_     && "Buffer's first motion is not zero!");
+    assert(_capture_ptr->getBuffer().front().delta_integr_cov_ == delta_integrated_cov_ && "Buffer's first motion is not zero!");
+    assert(_capture_ptr->getBuffer().front().jacobian_calib_   == jacobian_calib_       && "Buffer's first motion is not zero!");
 
     // Iterate all the buffer
-    auto motion_it      = _capture_ptr->getBuffer().get().begin();
+    auto motion_it      = _capture_ptr->getBuffer().begin();
     auto prev_motion_it = motion_it;
     motion_it++;
-    while (motion_it != _capture_ptr->getBuffer().get().end())
+    while (motion_it != _capture_ptr->getBuffer().end())
     {
         // get dt
         const double dt = motion_it->ts_ - prev_motion_it->ts_;
@@ -621,7 +621,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
         {
             // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
             capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
-            if (_ts < frame->getTimeStamp() and !capture_motion->getBuffer().get().empty() and _ts >= capture_motion->getBuffer().get().front().ts_)
+            if (_ts < frame->getTimeStamp() and !capture_motion->getBuffer().empty() and _ts >= capture_motion->getBuffer().front().ts_)
             {
                 // Found time stamp satisfying rule 3 above !! ==> break for loop
                 break;
@@ -671,7 +671,7 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
         PackKeyFramePtr pack = buffer_pack_kf_.selectFirstPackBefore(last_ptr_, params_motion_->time_tolerance);
 
         // ignore "future" KF to avoid MotionBuffer::split() error
-        if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().get().back().ts_)
+        if (pack && pack->key_frame->getTimeStamp() > last_ptr_->getBuffer().back().ts_)
             pack = nullptr;
 
         if (pack)
@@ -701,24 +701,6 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
     return nullptr;
 }
 
-void ProcessorMotion::setProblem(ProblemPtr _problem)
-{
-    std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
-    assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
-
-    if (_problem == nullptr or _problem == this->getProblem())
-        return;
-
-    NodeBase::setProblem(_problem);
-
-    // set the origin
-    if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr)
-        this->setOrigin(this->getProblem()->getLastKeyFrame());
-    
-    // adding processor is motion to the processor is motion vector
-    getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this()));
-}
-
 bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
 {
   return true;
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 04265e964c385bdbe591e84c650f8c7d99c36eeb..931efc2e3ea522f4afde3a1647e329d26507859d 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -98,24 +98,24 @@ void ProcessorOdom2d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::Vec
 bool ProcessorOdom2d::voteForKeyFrame() const
 {
     // Distance criterion
-    if (getBuffer().get().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
+    if (getBuffer().back().delta_integr_.head<2>().norm() > params_odom_2d_->dist_traveled)
     {
         WOLF_DEBUG("PM", id(), " ", getType(), " votes per distance");
         return true;
     }
-    if (getBuffer().get().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
+    if (getBuffer().back().delta_integr_.tail<1>().norm() > params_odom_2d_->angle_turned)
     {
         WOLF_DEBUG("PM", id(), " ", getType(), " votes per angle");
         return true;
     }
     // Uncertainty criterion
-    if (getBuffer().get().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
+    if (getBuffer().back().delta_integr_cov_.determinant() > params_odom_2d_->cov_det)
     {
         WOLF_DEBUG("PM", id(), " ", getType(), " votes per state covariance");
         return true;
     }
     // Time criterion
-    if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
+    if (getBuffer().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2d_->max_time_span)
     {
         WOLF_DEBUG("PM", id(), " ", getType(), " votes per time");
         return true;
@@ -151,12 +151,12 @@ FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBas
 
 FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
 {
-    Eigen::MatrixXd covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_;
+    Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_;
     makePosDef(covariance);
 
     FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
                                                                        "ProcessorOdom2d",
-                                                                       _capture_motion->getBuffer().get().back().delta_integr_,
+                                                                       _capture_motion->getBuffer().back().delta_integr_,
                                                                        covariance);
     return key_feature_ptr;
 }
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 457394c9213b39a69da4a01f74f2804db723a573..d8f2781f4667989b155ad65e5640a01e4aa52db3 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -161,20 +161,20 @@ void ProcessorOdom3d::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::Vec
 
 bool ProcessorOdom3d::voteForKeyFrame() const
 {
-    //WOLF_DEBUG( "Time span   : " , getBuffer().get().back().ts_ - getBuffer().get().front().ts_ );
-    //WOLF_DEBUG( " last ts : ", getBuffer().get().back().ts_);
-    //WOLF_DEBUG( " first ts : ", getBuffer().get().front().ts_);
-    //WOLF_DEBUG( "BufferLength: " , getBuffer().get().size() );
+    //WOLF_DEBUG( "Time span   : " , getBuffer().back().ts_ - getBuffer().front().ts_ );
+    //WOLF_DEBUG( " last ts : ", getBuffer().back().ts_);
+    //WOLF_DEBUG( " first ts : ", getBuffer().front().ts_);
+    //WOLF_DEBUG( "BufferLength: " , getBuffer().size() );
     //WOLF_DEBUG( "DistTraveled: " , delta_integrated_.head(3).norm() );
     //WOLF_DEBUG( "AngleTurned : " , 2.0 * acos(delta_integrated_(6)) );
     // time span
-    if (getBuffer().get().back().ts_ - getBuffer().get().front().ts_ > params_odom_3d_->max_time_span)
+    if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_odom_3d_->max_time_span)
     {
         WOLF_DEBUG( "PM: vote: time span" );
         return true;
     }
     // buffer length
-    if (getBuffer().get().size() > params_odom_3d_->max_buff_length)
+    if (getBuffer().size() > params_odom_3d_->max_buff_length)
     {
         WOLF_DEBUG( "PM: vote: buffer size" );
         return true;
@@ -217,8 +217,8 @@ FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
 {
     FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
                                                                        "ProcessorOdom3d",
-                                                                       _capture_motion->getBuffer().get().back().delta_integr_,
-                                                                       _capture_motion->getBuffer().get().back().delta_integr_cov_);
+                                                                       _capture_motion->getBuffer().back().delta_integr_,
+                                                                       _capture_motion->getBuffer().back().delta_integr_cov_);
     return key_feature_ptr;
 }
 
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index b0309585c0a4a1a16b2ce0582669e2e75cf41d68..cb9421c5ed18c64af24816a7ba14c13d92ea3471 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -35,7 +35,8 @@ set(SRC_DUMMY
   dummy/processor_tracker_feature_dummy.cpp
   dummy/processor_tracker_landmark_dummy.cpp
   )
-add_library(dummy ${SRC_DUMMY})
+add_library(dummy SHARED ${SRC_DUMMY})
+target_link_libraries(dummy ${PROJECT_NAME})
 ################# ADD YOUR TESTS BELOW ####################
 #                                                         #
 #           ==== IN ALPHABETICAL ORDER! ====              #
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 8374c1d2475d86e1c4fa08b046541c80226770f2..68712f969a298f5986b93378480f1a66e03cafe1 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -348,8 +348,8 @@ TEST(FactorAutodiff, AutodiffDummy12)
 
 TEST(FactorAutodiff, EmplaceOdom2d)
 {
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -379,8 +379,8 @@ TEST(FactorAutodiff, ResidualOdom2d)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -423,8 +423,8 @@ TEST(FactorAutodiff, JacobianOdom2d)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -501,8 +501,8 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(NON_ESTIMATED, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 57b817eb8e93bbb3093975bbbd9d4390e9f54180..2accaa608175f73ffe6d056e54180013d98c3a07 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -70,9 +70,14 @@ TEST(FrameBase, LinksToTree)
     intrinsics_odo.k_rot_to_rot = 1;
     auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
     auto F1 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
-    auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 2, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    auto F2 = FrameBase::emplaceKeyFrame<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
     auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr);
-    auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+    auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+    p->link(S);
+    //auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
     auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
     auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false);
 
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 987c713a245df58dba2c32e9edde9505bcd15d56..3c74ede52ce7ec57a153cdcf416b9c84597cadd9 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -42,8 +42,8 @@ TEST(MotionBuffer, QueryTimeStamps)
 {
     MotionBuffer MB;
 
-    MB.get().push_back(m0);
-    MB.get().push_back(m1);
+    MB.push_back(m0);
+    MB.push_back(m1);
     TimeStamp t;
 
     t.set(-1); // t is older than m0.ts_ -> return m0
@@ -66,10 +66,10 @@ TEST(MotionBuffer, getMotion)
 {
     MotionBuffer MB;
 
-    MB.get().push_back(m0);
+    MB.push_back(m0);
     ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
 
-    MB.get().push_back(m1);
+    MB.push_back(m1);
     ASSERT_EQ(MB.getMotion(t0).delta_, m0.delta_);
     ASSERT_EQ(MB.getMotion(t1).delta_, m1.delta_);
     ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
@@ -80,11 +80,11 @@ TEST(MotionBuffer, getDelta)
 {
     MotionBuffer MB;
 
-    MB.get().push_back(m0);
+    MB.push_back(m0);
 
     ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
 
-    MB.get().push_back(m1);
+    MB.push_back(m1);
 
     ASSERT_EQ(MB.getMotion(t0).delta_integr_, m0.delta_integr_);
     ASSERT_EQ(MB.getMotion(t1).delta_integr_, m1.delta_integr_);
@@ -94,19 +94,19 @@ TEST(MotionBuffer, Split)
 {
     MotionBuffer MB;
 
-    MB.get().push_back(m0);
-    MB.get().push_back(m1);
-    MB.get().push_back(m2);
-    MB.get().push_back(m3);
-    MB.get().push_back(m4); // put 5 motions
+    MB.push_back(m0);
+    MB.push_back(m1);
+    MB.push_back(m2);
+    MB.push_back(m3);
+    MB.push_back(m4); // put 5 motions
 
     MotionBuffer MB_old;
 
     TimeStamp t = 1.5; // between m1 and m2
     MB.split(t, MB_old);
 
-    ASSERT_EQ(MB_old.get().size(), (unsigned int) 2);
-    ASSERT_EQ(MB    .get().size(), (unsigned int) 3);
+    ASSERT_EQ(MB_old.size(), (unsigned int) 2);
+    ASSERT_EQ(MB    .size(), (unsigned int) 3);
 
     ASSERT_EQ(MB_old.getMotion(t1).ts_, t1);
     ASSERT_EQ(MB_old.getMotion(t2).ts_, t1); // last  ts is t1
@@ -118,11 +118,11 @@ TEST(MotionBuffer, Split)
 // {
 //     MotionBuffer MB;
 // 
-//     MB.get().push_back(m0);
-//     MB.get().push_back(m1);
-//     MB.get().push_back(m2);
-//     MB.get().push_back(m3);
-//     MB.get().push_back(m4); // put 5 motions
+//     MB.push_back(m0);
+//     MB.push_back(m1);
+//     MB.push_back(m2);
+//     MB.push_back(m3);
+//     MB.push_back(m4); // put 5 motions
 // 
 //     Eigen::MatrixXd cov = MB.integrateCovariance();
 //     ASSERT_NEAR(cov(0), 0.04, 1e-8);
@@ -133,11 +133,11 @@ TEST(MotionBuffer, Split)
 // {
 //     MotionBuffer MB;
 // 
-//     MB.get().push_back(m0);
-//     MB.get().push_back(m1);
-//     MB.get().push_back(m2);
-//     MB.get().push_back(m3);
-//     MB.get().push_back(m4); // put 5 motions
+//     MB.push_back(m0);
+//     MB.push_back(m1);
+//     MB.push_back(m2);
+//     MB.push_back(m3);
+//     MB.push_back(m4); // put 5 motions
 // 
 //     Eigen::MatrixXd cov = MB.integrateCovariance(t2);
 //     ASSERT_NEAR(cov(0), 0.02, 1e-8);
@@ -147,11 +147,11 @@ TEST(MotionBuffer, Split)
 // {
 //     MotionBuffer MB;
 // 
-//     MB.get().push_back(m0);
-//     MB.get().push_back(m1);
-//     MB.get().push_back(m2);
-//     MB.get().push_back(m3);
-//     MB.get().push_back(m4); // put 5 motions
+//     MB.push_back(m0);
+//     MB.push_back(m1);
+//     MB.push_back(m2);
+//     MB.push_back(m3);
+//     MB.push_back(m4); // put 5 motions
 // 
 //     Eigen::MatrixXd cov = MB.integrateCovariance(t1, t3);
 //     ASSERT_NEAR(cov(0), 0.03, 1e-8);
@@ -164,9 +164,9 @@ TEST(MotionBuffer, print)
 {
     MotionBuffer MB;
 
-    MB.get().push_back(m0);
-    MB.get().push_back(m1);
-    MB.get().push_back(m2);
+    MB.push_back(m0);
+    MB.push_back(m1);
+    MB.push_back(m2);
 
     MB.print();
     MB.print(0,0,0,0);
diff --git a/wolf_scripts/sed_rename.sh b/wolf_scripts/sed_rename.sh
new file mode 100755
index 0000000000000000000000000000000000000000..53d20f2bba1f57a7c4569d1dd27b8d5ec09974ea
--- /dev/null
+++ b/wolf_scripts/sed_rename.sh
@@ -0,0 +1,28 @@
+#!/bin/bash
+#$1 flag telling whether we are in test mode or not
+
+modify=$1
+apply=false
+
+if [[ "$modify" = true || "$modify" = t ]]; then
+    read -p "Are you sure? " -n 1 -r
+    echo    # (optional) move to a new line
+    if [[ $REPLY =~ ^[Yy]$ ]]
+    then
+        apply=true
+        echo "Modifying..."
+    fi
+fi
+
+string="StringToBeReplaced"
+replace="ReplacementString"
+
+for file in $(find include/ src/ test/ demos/ -type f); do
+    if [ "$apply" = true ]; then
+        # echo "APPLYING"
+        sed -Ei 's%'$string'%'$replace'%g' $file
+    else
+        # echo "NOT APPLYING"
+        sed -En 's%'$string'%'$replace'%gp' $file
+    fi
+done