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,