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