diff --git a/src/constraint_analytic.cpp b/src/constraint_analytic.cpp
index f6d5ab4f10132ebffd11a3a448a28c6ff22de236..9fc6a2f8187d871bf9501e8ba583ce942ce5e42d 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -47,108 +47,24 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const LandmarkBasePtr
     resizeVectors();
 }
 
-const std::vector<Scalar*> ConstraintAnalytic::getStateScalarPtrVector()
+std::vector<Scalar*> ConstraintAnalytic::getStateScalarPtrVector() const
 {
     assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
 
-    switch (state_ptr_vector_.size())
-    {
-        case 1:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr()});
-        }
-        case 2:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr()});
-        }
-        case 3:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr(),
-                                             state_ptr_vector_[2]->getPtr()});
-        }
-        case 4:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr(),
-                                             state_ptr_vector_[2]->getPtr(),
-                                             state_ptr_vector_[3]->getPtr()});
-        }
-        case 5:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr(),
-                                             state_ptr_vector_[2]->getPtr(),
-                                             state_ptr_vector_[3]->getPtr(),
-                                             state_ptr_vector_[4]->getPtr()});
-        }
-        case 6:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr(),
-                                             state_ptr_vector_[2]->getPtr(),
-                                             state_ptr_vector_[3]->getPtr(),
-                                             state_ptr_vector_[4]->getPtr(),
-                                             state_ptr_vector_[5]->getPtr()});
-        }
-        case 7:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr(),
-                                             state_ptr_vector_[2]->getPtr(),
-                                             state_ptr_vector_[3]->getPtr(),
-                                             state_ptr_vector_[4]->getPtr(),
-                                             state_ptr_vector_[5]->getPtr(),
-                                             state_ptr_vector_[6]->getPtr()});
-        }
-        case 8:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr(),
-                                             state_ptr_vector_[2]->getPtr(),
-                                             state_ptr_vector_[3]->getPtr(),
-                                             state_ptr_vector_[4]->getPtr(),
-                                             state_ptr_vector_[5]->getPtr(),
-                                             state_ptr_vector_[6]->getPtr(),
-                                             state_ptr_vector_[7]->getPtr()});
-        }
-        case 9:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr(),
-                                             state_ptr_vector_[2]->getPtr(),
-                                             state_ptr_vector_[3]->getPtr(),
-                                             state_ptr_vector_[4]->getPtr(),
-                                             state_ptr_vector_[5]->getPtr(),
-                                             state_ptr_vector_[6]->getPtr(),
-                                             state_ptr_vector_[7]->getPtr(),
-                                             state_ptr_vector_[8]->getPtr()});
-        }
-        case 10:
-        {
-            return std::vector<Scalar*>({state_ptr_vector_[0]->getPtr(),
-                                             state_ptr_vector_[1]->getPtr(),
-                                             state_ptr_vector_[2]->getPtr(),
-                                             state_ptr_vector_[3]->getPtr(),
-                                             state_ptr_vector_[4]->getPtr(),
-                                             state_ptr_vector_[5]->getPtr(),
-                                             state_ptr_vector_[6]->getPtr(),
-                                             state_ptr_vector_[7]->getPtr(),
-                                             state_ptr_vector_[8]->getPtr(),
-                                             state_ptr_vector_[9]->getPtr()});
-        }
-    }
+    std::vector<Scalar*> state_scalar_ptr_vector(state_ptr_vector_.size());
+
+    for (auto i = 0; i < state_scalar_ptr_vector.size(); i++)
+        state_scalar_ptr_vector[i] = state_ptr_vector_[i]->getPtr();
 
-    return std::vector<Scalar*>(0); //Not going to happen
+    return state_scalar_ptr_vector;
 }
 
-const std::vector<StateBlockPtr> ConstraintAnalytic::getStateBlockPtrVector() const
+std::vector<StateBlockPtr> ConstraintAnalytic::getStateBlockPtrVector() const
 {
     return state_ptr_vector_;
 }
 
-const std::vector<unsigned int> ConstraintAnalytic::getStateSizes() const
+std::vector<unsigned int> ConstraintAnalytic::getStateSizes() const
 {
     return state_block_sizes_vector_;
 }
diff --git a/src/constraint_analytic.h b/src/constraint_analytic.h
index 356b6556443612a2250727cef0951c4a1cb476ca..279394b652498a4001579516e507159bd6fa3205 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -103,21 +103,21 @@ class ConstraintAnalytic: public ConstraintBase
          * Returns a vector of pointers to the state blocks in which this constraint depends
          *
          **/
-        virtual const std::vector<Scalar*> getStateScalarPtrVector() override;
+        virtual std::vector<Scalar*> getStateScalarPtrVector() const override;
 
         /** \brief Returns a vector of pointers to the states
          *
          * Returns a vector of pointers to the state in which this constraint depends
          *
          **/
-        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
+        virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
 
         /** \brief Returns a vector of sizes of the state blocks
          *
          * Returns a vector of sizes of the state blocks
          *
          **/
-        virtual const std::vector<unsigned int> getStateSizes() const;
+        virtual std::vector<unsigned int> getStateSizes() const override;
 
         /** \brief Returns the constraint residual size
          *
diff --git a/src/constraint_autodiff.h b/src/constraint_autodiff.h
index 2b7c2ddc0e98404acd7da5bccecb95c6e62b5792..3e00d4d4c7a2d8a365ca2516c90d1025c14b5004 100644
--- a/src/constraint_autodiff.h
+++ b/src/constraint_autodiff.h
@@ -274,7 +274,7 @@ class ConstraintAutodiff: public ConstraintBase
          * Returns a vector of pointers to the state blocks in which this constraint depends
          *
          **/
-        virtual const std::vector<Scalar*> getStateScalarPtrVector()
+        virtual std::vector<Scalar*> getStateScalarPtrVector() const
         {
             return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                          state_ptrs_[1]->getPtr(),
@@ -294,7 +294,7 @@ class ConstraintAutodiff: public ConstraintBase
          * Returns a vector of pointers to the state in which this constraint depends
          *
          **/
-        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+        virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
         {
             return state_ptrs_;
         }
@@ -302,7 +302,7 @@ class ConstraintAutodiff: public ConstraintBase
         /** \brief Returns a vector of the states sizes
          *
          **/
-        virtual const std::vector<unsigned int> getStateSizes() const
+        virtual std::vector<unsigned int> getStateSizes() const
         {
             return state_block_sizes_;
         }
@@ -554,7 +554,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                         state_ptrs_[1]->getPtr(),
@@ -568,12 +568,12 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
@@ -808,7 +808,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                         state_ptrs_[1]->getPtr(),
@@ -821,12 +821,12 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
@@ -1050,7 +1050,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                         state_ptrs_[1]->getPtr(),
@@ -1062,12 +1062,12 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
@@ -1280,7 +1280,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                         state_ptrs_[1]->getPtr(),
@@ -1291,12 +1291,12 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
@@ -1497,7 +1497,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                         state_ptrs_[1]->getPtr(),
@@ -1507,12 +1507,12 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
@@ -1702,7 +1702,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                         state_ptrs_[1]->getPtr(),
@@ -1711,12 +1711,12 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
@@ -1895,7 +1895,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                         state_ptrs_[1]->getPtr(),
@@ -1903,12 +1903,12 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
@@ -2076,19 +2076,19 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr(),
                                         state_ptrs_[1]->getPtr()
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
@@ -2245,18 +2245,18 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
 //               std::cout << jacobians_[i] << std::endl << std::endl;
        }
 
-       virtual const std::vector<Scalar*> getStateScalarPtrVector()
+       virtual std::vector<Scalar*> getStateScalarPtrVector() const
        {
            return std::vector<Scalar*>({state_ptrs_[0]->getPtr()
                                         });
        }
 
-       virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const
+       virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
        {
            return state_ptrs_;
        }
 
-       virtual const std::vector<unsigned int> getStateSizes() const
+       virtual std::vector<unsigned int> getStateSizes() const
        {
            return state_block_sizes_;
        }
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 1fb15abab7408458304d8051bbd2471d181fa4d7..bd5cd88f0fec4b8cfa48bcbb0c0e9692c14fefc2 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -73,15 +73,15 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
 
         /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
          **/
-        virtual const std::vector<Scalar*> getStateScalarPtrVector() = 0;
+        virtual std::vector<Scalar*> getStateScalarPtrVector() const = 0;
 
         /** \brief Returns a vector of pointers to the states in which this constraint depends
          **/
-        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
+        virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
 
         /** \brief Returns a vector of the states sizes
          **/
-        virtual const std::vector<unsigned int> getStateSizes() const = 0;
+        virtual std::vector<unsigned int> getStateSizes() const = 0;
 
         /** \brief Returns a reference to the feature measurement
          **/
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 968ab5fb971d9baf56561fc7efdf0375a4159cc2..bb6d1f86f074ed913a583ee7cc3e322780b497e5 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -33,11 +33,11 @@ class ConstraintEpipolar : public ConstraintBase
 
         /** \brief Returns a vector of scalar pointers to the first element of all state blocks involved in the constraint
          **/
-        virtual const std::vector<Scalar*> getStateScalarPtrVector() override {return std::vector<Scalar*>(0);}
+        virtual std::vector<Scalar*> getStateScalarPtrVector() const override {return std::vector<Scalar*>(0);}
 
         /** \brief Returns a vector of pointers to the states in which this constraint depends
          **/
-        virtual const std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+        virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
 
         /** \brief Returns the constraint residual size
          **/
@@ -45,7 +45,7 @@ class ConstraintEpipolar : public ConstraintBase
 
         /** \brief Returns the constraint states sizes
          **/
-        virtual const std::vector<unsigned int> getStateSizes() const{return std::vector<unsigned int>({1});}
+        virtual std::vector<unsigned int> getStateSizes() const{return std::vector<unsigned int>({1});}
 
     public:
         static wolf::ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,