diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5f0bf77316d12028cc0aa6fe4e152a57c7691a03..184a42ee5a574e28f6d15e24b50b3a61b1fa1c67 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -358,6 +358,7 @@ SET(SRCS_LANDMARK
   src/landmark/landmark_base.cpp
   )
 SET(SRCS_PROCESSOR
+  src/processor/is_motion.cpp
   src/processor/motion_buffer.cpp
   src/processor/processor_base.cpp
   src/processor/processor_diff_drive.cpp
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/problem/problem.h b/include/core/problem/problem.h
index 0fe1cd5b6a18c007859440563eb7a3475eb432c3..69e2392494c8d1c8441feff04f112c05423b8911 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -50,6 +50,7 @@ class Problem : public std::enable_shared_from_this<Problem>
     friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
     friend ProcessorBase;
     friend ProcessorMotion;
+    friend IsMotion;
 
 
     protected:
diff --git a/include/core/processor/is_motion.h b/include/core/processor/is_motion.h
index e586883ed2e199cc46ec68e94f2173a31cf631fa..1488e1a85d2f605949c966c17d0c322ea78882aa 100644
--- a/include/core/processor/is_motion.h
+++ b/include/core/processor/is_motion.h
@@ -22,7 +22,6 @@ class IsMotion
 {
     public:
 
-
         virtual ~IsMotion();
 
         // Queries to the processor:
@@ -30,16 +29,17 @@ class IsMotion
         virtual void    getTimeStamp      ( TimeStamp& _ts) const = 0;
         virtual bool    getState ( VectorComposite& _state) const = 0;
         virtual bool    getState ( const TimeStamp& _ts,
-                                            VectorComposite& _state) const = 0;
-
+                                   VectorComposite& _state) const = 0;
+        // implemented here
         TimeStamp       getTimeStamp() const;
-        // Get composite states
         VectorComposite getState() const;
         VectorComposite getState(const TimeStamp& _ts) const;
 
         const StateStructure& getStateStructure() const {return state_structure_;};
         void            setStateStructure(const StateStructure& _state_structure){state_structure_ = _state_structure;};
 
+        void addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr);
+   
     protected:
         StateStructure state_structure_; ///< The structure of the state vector (to retrieve state blocks from frames)
 
@@ -56,13 +56,6 @@ inline IsMotion::~IsMotion()
 {
 }
 
-//inline Eigen::VectorXd IsMotion::getCurrentState() const
-//{
-//    Eigen::VectorXd x;
-//    getCurrentState(x);
-//    return x;
-//}
-
 inline TimeStamp IsMotion::getTimeStamp() const
 {
     TimeStamp ts;
diff --git a/src/processor/is_motion.cpp b/src/processor/is_motion.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1bfd06a5e9669f76ef48c49a559cade0dc7845f0
--- /dev/null
+++ b/src/processor/is_motion.cpp
@@ -0,0 +1,9 @@
+#include "core/processor/is_motion.h"
+#include "core/problem/problem.h"
+
+using namespace wolf;
+
+void IsMotion::addToProblem(ProblemPtr _prb_ptr, IsMotionPtr _motion_ptr)
+{
+    _prb_ptr->addProcessorIsMotion(_motion_ptr);
+}
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index 7483e4c7f6dcb8fed2f2621c16fcd2b26b604612..722ff1eaaf8056f6dbcbc510985adb8d5249d277 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -102,7 +102,8 @@ void ProcessorTracker::processCapture(CaptureBasePtr _incoming_ptr)
         	// No-break case only for debug. Next case will be executed too.
             PackKeyFramePtr pack = buffer_pack_kf_.selectPack( incoming_ptr_, params_tracker_->time_tolerance);
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITH_PACK: KF" , pack->key_frame->id() , " callback unpacked with ts= " , pack->key_frame->getTimeStamp() );
-        } // @suppress("No break at end of case")
+        }
+        // Fall through
         case SECOND_TIME_WITHOUT_PACK :
         {
             WOLF_DEBUG( "PT ", getName(), " SECOND_TIME_WITHOUT_PACK" );
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 3862e263cb747b443e77ef113e9d85efa529f3a5..b2fc65a6acea79636728737980f8d18818524ee8 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -137,7 +137,9 @@ TEST_F(BufferPackKeyFrameTest, selectPack)
             {
                 PackKeyFramePtr packQ = pack_kf_buffer.selectPack(16, q[iq]);
                 if (packQ!=nullptr)
+                {
                     ASSERT_EQ(packQ->key_frame->getTimeStamp(),res(ip1*6+ip2*3+iq));
+                }
             }
             pack_kf_buffer.clear();
         }
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