diff --git a/src/constraint_AHP.h b/src/constraint_AHP.h
index 917c560454d7929f3d12968df6c0f279b1209892..f39c097b196d40a70d1151af5b7ff256c7d6850c 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 FeatureBasePtr& _ftr_ptr,
+ const LandmarkAHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
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,25 +53,28 @@ 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,
- bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE);
+ static ConstraintAHPPtr create(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkAHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ ConstraintStatus _status = CTR_ACTIVE);
};
-inline ConstraintAHP::ConstraintAHP(FeatureBasePtr _ftr_ptr,
- LandmarkAHPPtr _landmark_ptr,
+inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr& _ftr_ptr,
+ const LandmarkAHPPtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
ConstraintStatus _status) :
ConstraintSparse<2, 3, 4, 3, 4, 4>(CTR_AHP,
_landmark_ptr->getAnchorFrame(),
nullptr,
_landmark_ptr,
+ _processor_ptr,
_apply_loss_function, _status,
_ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr(),
_ftr_ptr->getCapturePtr()->getFramePtr()->getOPtr(),
@@ -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 FeatureBasePtr& _ftr_ptr,
+ const LandmarkAHPPtr& _lmk_ahp_ptr,
+ const ProcessorBasePtr& _processor_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>(_ftr_ptr, _lmk_ahp_ptr, _processor_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..87714278a757ecf03bcad1cebfb21a3f7f35472d 100644
--- a/src/constraint_analytic.cpp
+++ b/src/constraint_analytic.cpp
@@ -13,20 +13,22 @@ 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 FrameBasePtr& _frame_ptr, const ProcessorBasePtr& _processor_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, _frame_ptr, nullptr, nullptr, _processor_ptr, _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 FeatureBasePtr& _feature_ptr, const ProcessorBasePtr& _processor_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, nullptr, _feature_ptr, nullptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
@@ -34,23 +36,17 @@ ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, FeatureBasePtr _featu
}
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
+ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const LandmarkBasePtr& _landmark_ptr, const ProcessorBasePtr& _processor_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, nullptr, nullptr, _landmark_ptr, _processor_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..6c9ddc7f95ea104c93a9383f4425e0d159fcf8d0 100644
--- a/src/constraint_analytic.h
+++ b/src/constraint_analytic.h
@@ -40,67 +40,77 @@ class ConstraintAnalytic: public ConstraintBase
* Constructor of category CTR_FRAME
*
**/
- ConstraintAnalytic(ConstraintType _tp, FrameBasePtr _frame_ptr, bool _apply_loss_function, ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr = nullptr,
- StateBlockPtr _state2Ptr = nullptr,
- StateBlockPtr _state3Ptr = nullptr,
- StateBlockPtr _state4Ptr = nullptr,
- StateBlockPtr _state5Ptr = nullptr,
- StateBlockPtr _state6Ptr = nullptr,
- StateBlockPtr _state7Ptr = nullptr,
- StateBlockPtr _state8Ptr = nullptr,
- StateBlockPtr _state9Ptr = nullptr );
+ ConstraintAnalytic(ConstraintType _tp,
+ const FrameBasePtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr = nullptr,
+ StateBlockPtr _state2Ptr = nullptr,
+ StateBlockPtr _state3Ptr = nullptr,
+ StateBlockPtr _state4Ptr = nullptr,
+ StateBlockPtr _state5Ptr = nullptr,
+ StateBlockPtr _state6Ptr = nullptr,
+ StateBlockPtr _state7Ptr = nullptr,
+ StateBlockPtr _state8Ptr = nullptr,
+ StateBlockPtr _state9Ptr = nullptr );
/** \brief Constructor of category CTR_FEATURE
*
* Constructor of category CTR_FEATURE
*
**/
- ConstraintAnalytic(ConstraintType _tp, FeatureBasePtr _feature_ptr, bool _apply_loss_function, ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr = nullptr,
- StateBlockPtr _state2Ptr = nullptr,
- StateBlockPtr _state3Ptr = nullptr,
- StateBlockPtr _state4Ptr = nullptr,
- StateBlockPtr _state5Ptr = nullptr,
- StateBlockPtr _state6Ptr = nullptr,
- StateBlockPtr _state7Ptr = nullptr,
- StateBlockPtr _state8Ptr = nullptr,
- StateBlockPtr _state9Ptr = nullptr ) ;
+ ConstraintAnalytic(ConstraintType _tp,
+ const FeatureBasePtr& _feature_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr = nullptr,
+ StateBlockPtr _state2Ptr = nullptr,
+ StateBlockPtr _state3Ptr = nullptr,
+ StateBlockPtr _state4Ptr = nullptr,
+ StateBlockPtr _state5Ptr = nullptr,
+ StateBlockPtr _state6Ptr = nullptr,
+ StateBlockPtr _state7Ptr = nullptr,
+ StateBlockPtr _state8Ptr = nullptr,
+ StateBlockPtr _state9Ptr = nullptr );
/** \brief Constructor of category CTR_LANDMARK
*
* Constructor of category CTR_LANDMARK
*
**/
- ConstraintAnalytic(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr = nullptr,
- StateBlockPtr _state2Ptr = nullptr,
- StateBlockPtr _state3Ptr = nullptr,
- StateBlockPtr _state4Ptr = nullptr,
- StateBlockPtr _state5Ptr = nullptr,
- StateBlockPtr _state6Ptr = nullptr,
- StateBlockPtr _state7Ptr = nullptr,
- StateBlockPtr _state8Ptr = nullptr,
- StateBlockPtr _state9Ptr = nullptr ) ;
-
- virtual ~ConstraintAnalytic();
+ ConstraintAnalytic(ConstraintType _tp,
+ const LandmarkBasePtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, ConstraintStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr = nullptr,
+ StateBlockPtr _state2Ptr = nullptr,
+ StateBlockPtr _state3Ptr = nullptr,
+ StateBlockPtr _state4Ptr = nullptr,
+ StateBlockPtr _state5Ptr = nullptr,
+ StateBlockPtr _state6Ptr = nullptr,
+ StateBlockPtr _state7Ptr = nullptr,
+ StateBlockPtr _state8Ptr = nullptr,
+ StateBlockPtr _state9Ptr = nullptr );
+
+ 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 +124,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 +160,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..368448a7413f621a3de5d9abba5f37303b2c1411 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -16,12 +16,16 @@ 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 FrameBasePtr& _frame_other_ptr, const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr, const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, ConstraintStatus _status) :
NodeBase("CONSTRAINT", "Base"),
feature_ptr_(),
is_removing_(false),
@@ -31,16 +35,12 @@ 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;
}
-ConstraintBase::~ConstraintBase()
-{
-// std::cout << "destructed -c" << id() << std::endl;
-}
-
void ConstraintBase::remove()
{
if (!is_removing_)
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 039f3e085812edff29afa459724439652a25f82a..ba85eaef7f605dd8930b01e62b04730e919d51d0 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -32,18 +32,25 @@ 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 = false,
+ ConstraintStatus _status = CTR_ACTIVE);
/** \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 FrameBasePtr& _frame_other_ptr, const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE);
+
+ virtual ~ConstraintBase() = default;
- virtual ~ConstraintBase();
void remove();
unsigned int id() const;
@@ -120,6 +127,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 +235,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..41f7f3f3a93a56cc2f580a30107add1192de3964 100644
--- a/src/constraint_container.h
+++ b/src/constraint_container.h
@@ -18,8 +18,12 @@ 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 FeatureBasePtr& _ftr_ptr,
+ const LandmarkContainerPtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_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, _processor_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 +33,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()
{
@@ -115,24 +116,26 @@ class ConstraintContainer: public ConstraintSparse<3,2,1,2,1>
return true;
}
- /** \brief Returns the jacobians computation method
- *
- * Returns the jacobians computation method
- *
- **/
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
-
- public:
- static ConstraintBasePtr create(FeatureBasePtr _feature_ptr, 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);
- }
+ /** \brief Returns the jacobians computation method
+ *
+ * Returns the jacobians computation method
+ *
+ **/
+ virtual JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+
+ public:
+ static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr)
+ {
+ 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), _processor_ptr, corner);
+ }
};
diff --git a/src/constraint_corner_2D.h b/src/constraint_corner_2D.h
index 364c6d5445a8a61e79fed978559070f96c451756..6a5ea8a06aa76d3523dfcacfea4acc542578df9d 100644
--- a/src/constraint_corner_2D.h
+++ b/src/constraint_corner_2D.h
@@ -13,41 +13,42 @@ 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())
- {
- setType("CORNER 2D");
- }
-
- virtual ~ConstraintCorner2D()
- {
- //std::cout << "deleting ConstraintCorner2D " << id() << std::endl;
- }
-
- LandmarkCorner2DPtr getLandmarkPtr()
- {
- return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
- }
-
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
- const T* const _landmarkO, T* _residuals) const;
-
- /** \brief Returns the jacobians computation method
+ ConstraintCorner2D(const FeatureBasePtr _ftr_ptr,
+ const LandmarkCorner2DPtr _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function = false,
+ ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintSparse<3,2,1,2,1>(CTR_CORNER_2D, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
+ {
+ setType("CORNER 2D");
+ }
+
+ virtual ~ConstraintCorner2D() = default;
+
+ LandmarkCorner2DPtr getLandmarkPtr()
+ {
+ return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
+ }
+
+ template <typename T>
+ bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
+ const T* const _landmarkO, T* _residuals) const;
+
+ /** \brief Returns the jacobians computation method
*
* Returns the jacobians computation method
*
**/
- virtual JacobianMethod getJacobianMethod() const
- {
- return JAC_AUTO;
- }
-
- public:
- static ConstraintBasePtr create(FeatureBasePtr _feature_ptr, NodeBasePtr _correspondant_ptr)
- {
- return std::make_shared<ConstraintCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr) );
- }
+ virtual JacobianMethod getJacobianMethod() const override
+ {
+ return JAC_AUTO;
+ }
+
+public:
+ static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr = nullptr)
+ {
+ return std::make_shared<ConstraintCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr), _processor_ptr);
+ }
};
diff --git a/src/constraint_epipolar.h b/src/constraint_epipolar.h
index 03f27be071ef16fc55822a1ee4b720016b156909..199888a9a2751c428e4102cc77ec6a14b69f7f76 100644
--- a/src/constraint_epipolar.h
+++ b/src/constraint_epipolar.h
@@ -11,44 +11,49 @@ 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 FeatureBasePtr& _feature_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ 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 FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr);
};
-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 FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, ConstraintStatus _status) :
+ ConstraintBase(CTR_EPIPOLAR, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _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 FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<ConstraintEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr));
+ return std::make_shared<ConstraintEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
}
} // namespace wolf
diff --git a/src/constraint_fix.h b/src/constraint_fix.h
index b42cc7aa4e7e99aec2320342b0ef25d51537c70b..77bfe34330a429f5f660f658647674e8cb33f1bc 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_fix.h
@@ -26,11 +26,7 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
// std::cout << "created ConstraintFix " << std::endl;
}
- virtual ~ConstraintFix()
- {
-// std::cout << "destructed ConstraintFix " << std::endl;
- //
- }
+ virtual ~ConstraintFix() = default;
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
@@ -40,7 +36,7 @@ class ConstraintFix: public ConstraintSparse<3,2,1>
* Returns the jacobians computation method
*
**/
- virtual JacobianMethod getJacobianMethod() const
+ virtual JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
diff --git a/src/constraint_fix_3D.h b/src/constraint_fix_3D.h
index 4044d30ee050f29d8ad2ec47d9686bb2d56712d3..bd5cb3750984c4d56535b338db339bfc96d44f31 100644
--- a/src/constraint_fix_3D.h
+++ b/src/constraint_fix_3D.h
@@ -23,15 +23,13 @@ class ConstraintFix3D: public ConstraintSparse<6,3,4>
{
setType("FIX3D");
}
- virtual ~ConstraintFix3D()
- {
- //
- }
+
+ virtual ~ConstraintFix3D() = default;
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
- virtual JacobianMethod getJacobianMethod() const
+ virtual JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
diff --git a/src/constraint_fix_bias.h b/src/constraint_fix_bias.h
index 5e2fe71e605f876366c81b9c440e6750b8637d53..3ccdc2377e45f96de99016ae02380ea1ba50966f 100644
--- a/src/constraint_fix_bias.h
+++ b/src/constraint_fix_bias.h
@@ -28,11 +28,7 @@ class ConstraintFixBias: public ConstraintSparse<6,3,3>
// std::cout << "created ConstraintFixBias " << std::endl;
}
- virtual ~ConstraintFixBias()
- {
- // std::cout << "destructed ConstraintFixBias " << std::endl;
- //
- }
+ virtual ~ConstraintFixBias() = default;
template<typename T>
bool operator ()(const T* const _ab, const T* const _wb, T* _residuals) const;
@@ -42,7 +38,7 @@ class ConstraintFixBias: public ConstraintSparse<6,3,3>
* Returns the jacobians computation method
*
**/
- virtual JacobianMethod getJacobianMethod() const
+ virtual JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h
index 0f3f011c5207f51de33be7d0a6795e3284593bb1..874cba0f5271ab046af4e651c28d9594eb450c59 100644
--- a/src/constraint_gps_2D.h
+++ b/src/constraint_gps_2D.h
@@ -21,10 +21,7 @@ class ConstraintGPS2D : public ConstraintSparse<2, 2>
setType("GPS FIX 2D");
}
- virtual ~ConstraintGPS2D()
- {
- //
- }
+ virtual ~ConstraintGPS2D() = default;
template<typename T>
bool operator ()(const T* const _x, T* _residuals) const;
@@ -34,7 +31,7 @@ class ConstraintGPS2D : public ConstraintSparse<2, 2>
* Returns the jacobians computation method
*
**/
- virtual JacobianMethod getJacobianMethod() const
+ virtual JacobianMethod getJacobianMethod() const override
{
return JAC_AUTO;
}
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..7ae0c07b7408ff6dbdb86d88a137ae39cb33ce07 100644
--- a/src/constraint_imu.h
+++ b/src/constraint_imu.h
@@ -19,10 +19,12 @@ 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 FeatureIMUPtr& _ftr_ptr, const FrameIMUPtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ 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 +55,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 +100,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 FeatureIMUPtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr);
private:
/// Preintegrated delta
@@ -138,9 +141,11 @@ 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,
- ConstraintStatus _status) :
- ConstraintSparse<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3>(CTR_IMU, _frame_ptr, nullptr, nullptr, _apply_loss_function, _status,
+inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
+ const FrameIMUPtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_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, _processor_ptr, _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 +175,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 +312,9 @@ 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 FeatureIMUPtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<ConstraintIMU>(_feature_ptr, std::static_pointer_cast<FrameIMU>(_correspondant_ptr));
+ return std::make_shared<ConstraintIMU>(_feature_ptr, std::static_pointer_cast<FrameIMU>(_correspondant_ptr), _processor_ptr);
}
diff --git a/src/constraint_odom_2D.h b/src/constraint_odom_2D.h
index 99f66652c7d46ea427bf71e361ba29cdc7490e82..2314f9179d774c7f582889c8b69a463490afff9d 100644
--- a/src/constraint_odom_2D.h
+++ b/src/constraint_odom_2D.h
@@ -15,18 +15,17 @@ 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 FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintSparse<3, 2, 1, 2, 1>(CTR_ODOM_2D, _frame_ptr, nullptr, nullptr, _processor_ptr, _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 +33,15 @@ 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 FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<ConstraintOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr) );
+ return std::make_shared<ConstraintOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
}
};
diff --git a/src/constraint_odom_2D_analytic.h b/src/constraint_odom_2D_analytic.h
index 88ba510ae125ade012702c75c66eaf007baea5dd..9612a7bcb719668be8c8274b84a830ff17d62c36 100644
--- a/src/constraint_odom_2D_analytic.h
+++ b/src/constraint_odom_2D_analytic.h
@@ -13,16 +13,17 @@ 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 FeatureBasePtr& _ftr_ptr,
+ const FrameBasePtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false,
+ ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintRelative2DAnalytic(_ftr_ptr, CTR_ODOM_2D, _frame_ptr, _processor_ptr, _apply_loss_function, _status)
{
setType("ODOM 2D ANALYTIC");
}
- virtual ~ConstraintOdom2DAnalytic()
- {
- //
- }
+ virtual ~ConstraintOdom2DAnalytic() = default;
// /** \brief Returns the constraint residual size
// *
@@ -103,10 +104,11 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
public:
- static wolf::ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
- NodeBasePtr _correspondant_ptr)
+ static wolf::ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr)
{
- return std::make_shared<ConstraintOdom2DAnalytic>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr) );
+ return std::make_shared<ConstraintOdom2DAnalytic>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
}
};
diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index 564f06b1b6614f8433ae1657f74303869c12133a..2062bbba75e10762a694637977df63ade038bee6 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -20,10 +20,15 @@ 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 FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ 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,12 +74,16 @@ 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 FeatureBasePtr& _ftr_current_ptr,
+ const FrameBasePtr& _frame_past_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
ConstraintStatus _status) :
ConstraintSparse<6, 3, 4, 3, 4>(CTR_ODOM_3D, // type
_frame_past_ptr, // frame other
nullptr, // feature other
nullptr, // landmark other
+ _processor_ptr, // processor
_apply_loss_function,
_status,
_ftr_current_ptr->getFramePtr()->getPPtr(), // current frame P
@@ -86,11 +95,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..9a092e1e045d96839e2c7ccaf02bda706cba2262 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,15 @@ 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 FeaturePolyline2DPtr& _ftr_ptr,
+ const LandmarkPolyline2DPtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_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, _processor_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 +40,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..55eb97f4d5e9f04707f5447a839fc5a93da7d53a 100644
--- a/src/constraint_point_to_line_2D.h
+++ b/src/constraint_point_to_line_2D.h
@@ -25,8 +25,12 @@ 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 FeaturePolyline2DPtr& _ftr_ptr,
+ const LandmarkPolyline2DPtr& _lmk_ptr,
+ const ProcessorBasePtr& _processor_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, _processor_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 +40,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..f9b6fdc9d70234321f5c8c9a57b6d3b98c1258d6 100644
--- a/src/constraint_relative_2D_analytic.h
+++ b/src/constraint_relative_2D_analytic.h
@@ -17,36 +17,39 @@ 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 FeatureBasePtr& _ftr_ptr, const ConstraintType& _tp, const FrameBasePtr& _frame_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintAnalytic(_tp, _frame_ptr, _processor_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 FeatureBasePtr& _ftr_ptr, const ConstraintType& _tp, const FeatureBasePtr& _ftr_other_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintAnalytic(_tp, _ftr_other_ptr, _processor_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 FeatureBasePtr& _ftr_ptr, const ConstraintType& _tp, const LandmarkBasePtr& _landmark_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr,
+ bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintAnalytic(_tp, _landmark_ptr, _processor_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 +84,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..3ba6ecac2a267d09c6dc7826b853f5c9456c8682 100644
--- a/src/constraint_sparse.h
+++ b/src/constraint_sparse.h
@@ -59,7 +59,11 @@ 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 FrameBasePtr& _frame_other_ptr, const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, ConstraintStatus _status,
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr = nullptr,
StateBlockPtr _state2Ptr = nullptr,
@@ -71,28 +75,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 +168,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 FrameBasePtr& _frame_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ ConstraintStatus _status,
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr,
StateBlockPtr _state2Ptr,
@@ -180,40 +185,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, _frame_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_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/examples/test_constraint_AHP.cpp b/src/examples/test_constraint_AHP.cpp
index dc2d89fb9aa871693d5e618259b74615571cb656..91a2ba19e7f3aae58cbefca746a9dc4f9fcd8641 100644
--- a/src/examples/test_constraint_AHP.cpp
+++ b/src/examples/test_constraint_AHP.cpp
@@ -43,7 +43,7 @@ int main()
// PROCESSOR
// one-liner API
- wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_ORB.yaml");
+ ProcessorBasePtr processor_ptr = wolf_problem_ptr_->installProcessor("IMAGE LANDMARK", "ORB", "PinHole", wolf_root + "/src/examples/processor_image_ORB.yaml");
// create the current frame
@@ -118,7 +118,7 @@ int main()
// Create the constraint
- ConstraintAHPPtr constraint_ptr = std::make_shared<ConstraintAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark));
+ ConstraintAHPPtr constraint_ptr = std::make_shared<ConstraintAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark), processor_ptr);
feat_point_image_ptr->addConstraint(constraint_ptr);
std::cout << "Constraint AHP created" << std::endl;
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 78ab782bac8b1b14cb8d2615a6abca2446219ea4..052b528865d927611356ae38b80f682f48ffdbc5 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -159,11 +159,11 @@ int main(int argc, char** argv)
std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
// Constraints------------------
- ConstraintAHPPtr ctr_0 = ConstraintAHP::create(feat_0, lmk_1 );
+ ConstraintAHPPtr ctr_0 = ConstraintAHP::create(feat_0, lmk_1, nullptr);
feat_0->addConstraint(ctr_0);
- ConstraintAHPPtr ctr_1 = ConstraintAHP::create(feat_1, lmk_1 );
+ ConstraintAHPPtr ctr_1 = ConstraintAHP::create(feat_1, lmk_1, nullptr);
feat_1->addConstraint(ctr_1);
- ConstraintAHPPtr ctr_2 = ConstraintAHP::create(feat_2, lmk_1 );
+ ConstraintAHPPtr ctr_2 = ConstraintAHP::create(feat_2, lmk_1, nullptr);
feat_2->addConstraint(ctr_2);
// Projections----------------------------
@@ -210,9 +210,9 @@ int main(int argc, char** argv)
std::cout << "Landmark 2: " << lmk_2->point().transpose() << std::endl;
// New constraints from kf3 and kf4
- ConstraintAHPPtr ctr_3 = ConstraintAHP::create(feat_3, lmk_2 );
+ ConstraintAHPPtr ctr_3 = ConstraintAHP::create(feat_3, lmk_2, nullptr);
feat_3->addConstraint(ctr_3);
- ConstraintAHPPtr ctr_4 = ConstraintAHP::create(feat_4, lmk_2 );
+ ConstraintAHPPtr ctr_4 = ConstraintAHP::create(feat_4, lmk_2, nullptr);
feat_4->addConstraint(ctr_4);
Eigen::Vector2s pix_3 = ctr_3->expectation();
diff --git a/src/landmark_polyline_2D.cpp b/src/landmark_polyline_2D.cpp
index 951990a867ce6c49ff825af1d281e5bf9aece716..e0f2a6cfbf12b13af7bfe291f08d7a5848dfd60d 100644
--- a/src/landmark_polyline_2D.cpp
+++ b/src/landmark_polyline_2D.cpp
@@ -248,6 +248,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
new_ctr_ptr = std::make_shared<ConstraintPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+ ctr_point_ptr->getProcessor(),
ctr_point_ptr->getFeaturePointId(),
_remain_id,
ctr_point_ptr->getApplyLossFunction(),
@@ -261,6 +262,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+ ctr_point_ptr->getProcessor(),
ctr_point_ptr->getFeaturePointId(),
_remain_id,
ctr_point_ptr->getLandmarkPointAuxId(),
@@ -270,6 +272,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
else if (ctr_point_ptr->getLandmarkPointAuxId() == _remove_id)
new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
+ ctr_point_ptr->getProcessor(),
ctr_point_ptr->getFeaturePointId(),
ctr_point_ptr->getLandmarkPointId(),
_remain_id,
diff --git a/src/processor_image_feature.h b/src/processor_image_feature.h
index a8f1a1c4b092022350424a41194659fc41a00172..ac1e5786d4c7961cbbf41db32633b5bf2e7866f7 100644
--- a/src/processor_image_feature.h
+++ b/src/processor_image_feature.h
@@ -185,7 +185,7 @@ inline bool ProcessorImageFeature::voteForKeyFrame()
inline ConstraintBasePtr ProcessorImageFeature::createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
{
- ConstraintEpipolarPtr const_epipolar_ptr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr);
+ ConstraintEpipolarPtr const_epipolar_ptr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
// _feature_ptr->addConstraint(const_epipolar_ptr);
// _feature_other_ptr->addConstrainedBy(const_epipolar_ptr);
return const_epipolar_ptr;
diff --git a/src/processor_image_landmark.cpp b/src/processor_image_landmark.cpp
index 0c060b034bd326521e0a85971b8965615d24db50..ff9136c351250ba4e6ef72957adb9f5d82adc045 100644
--- a/src/processor_image_landmark.cpp
+++ b/src/processor_image_landmark.cpp
@@ -292,7 +292,7 @@ ConstraintBasePtr ProcessorImageLandmark::createConstraint(FeatureBasePtr _featu
LandmarkAHPPtr landmark_ahp = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
- ConstraintAHPPtr constraint_ptr = ConstraintAHP::create(_feature_ptr, landmark_ahp, true);
+ ConstraintAHPPtr constraint_ptr = ConstraintAHP::create(_feature_ptr, landmark_ahp, shared_from_this(), true);
return constraint_ptr;
}
diff --git a/src/processor_imu.h b/src/processor_imu.h
index 55109586c593c607c7d8fcafc699f58f05a5306d..90ed5acc82df1a6130b45749cd934d63eb61c31c 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>(ftr_imu, frm_imu, shared_from_this());
_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..be2654cf032ae7ab1484e162470e883ecb10b7f7 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>(_feature_motion, _frame_origin, shared_from_this());
_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..1a806888cff43bdbb3d84684258aca9ec641f48a 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>(_feature_motion, _frame_origin, shared_from_this());
_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..dbf96c0f75131ae9fc5935259ada7a088563bcd6 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>(_feature_other_ptr, landmark_ptr, shared_from_this()));
}
// 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>(_feature_ptr, landmark_ptr, shared_from_this());
}
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..5eb244498fff834ebad6951f975ed802ff4595dd 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>(_feature_ptr, _feature_other_ptr, shared_from_this());
// _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..80acb3c30ce066391143ac2c658741defee6398a 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>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>((_landmark_ptr)), shared_from_this());
}
} // namespace wolf
diff --git a/src/processor_tracker_landmark_dummy.cpp b/src/processor_tracker_landmark_dummy.cpp
index 60b22ac304b86a5dae72e78d59bfa54b88116ec5..71051458bfdd9ef5b5f4013984e20de93c4370b8 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>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr), shared_from_this());
}
} //namespace wolf
diff --git a/src/processor_tracker_landmark_polyline.cpp b/src/processor_tracker_landmark_polyline.cpp
index 74a37dac155e1280514dce655d7fc5963cc8c6ad..db749cfb1e5c7ac443604846ddbba76c73f2770e 100644
--- a/src/processor_tracker_landmark_polyline.cpp
+++ b/src/processor_tracker_landmark_polyline.cpp
@@ -816,7 +816,8 @@ 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>(polyline_feature, polyline_landmark, shared_from_this(),
+ ftr_point_id, lmk_point_id, lmk_next_point_id));
//std::cout << "constraint added" << std::endl;
}
@@ -828,7 +829,8 @@ 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>(polyline_feature, polyline_landmark, shared_from_this(),
+ ftr_point_id, lmk_point_id, lmk_prev_point_id));
//std::cout << "constraint added" << std::endl;
}
@@ -840,7 +842,8 @@ 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>(polyline_feature, polyline_landmark, shared_from_this(),
+ ftr_point_id, lmk_point_id));
//std::cout << "constraint added" << std::endl;
}
}
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 0b01f569fcfc9c6640ee5f81e6fe6a8d844f7c98..9f5f51f7a02255d99fc87b944f933a43a268b784 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -68,10 +68,6 @@ target_link_libraries(gtest_local_param ${PROJECT_NAME})
wolf_add_gtest(gtest_motion_buffer gtest_motion_buffer.cpp)
target_link_libraries(gtest_motion_buffer ${PROJECT_NAME})
-# Problem class test
-wolf_add_gtest(gtest_problem gtest_problem.cpp)
-target_link_libraries(gtest_problem ${PROJECT_NAME})
-
# Rotation test
wolf_add_gtest(gtest_rotation gtest_rotation.cpp)
target_link_libraries(gtest_rotation ${PROJECT_NAME})
@@ -140,6 +136,10 @@ target_link_libraries(gtest_odom_3D ${PROJECT_NAME})
# ROI test
IF(OpenCV_FOUND)
+ # Problem class test
+ wolf_add_gtest(gtest_problem gtest_problem.cpp)
+ target_link_libraries(gtest_problem ${PROJECT_NAME})
+
wolf_add_gtest(gtest_roi_ORB gtest_roi_ORB.cpp)
target_link_libraries(gtest_roi_ORB ${PROJECT_NAME})
ENDIF(OpenCV_FOUND)
\ No newline at end of file
diff --git a/src/test/gtest_constraint_odom_3D.cpp b/src/test/gtest_constraint_odom_3D.cpp
index 6bbac96bd12ec52c082097e7f8a958192da5f2c0..39afa2989534989c9f7c787e356ce91e9b98dc37 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>(fea1, frm0, nullptr))); // 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..a5f57c9a0b1b16409666fea0bdd539369954fc53 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>(feat_imu, std::static_pointer_cast<FrameIMU>(frm_imu), processor_ptr_);
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..6ccc9ab0a48cb6966b8d70e22069a61cbce8805c 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>(f, F2, p);
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..59f549cd20c60fe03e5949583a6ce92b6f0fedff 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>(f1, F0, nullptr));
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>(f2, F1, nullptr));
F1->addConstrainedBy(c2);
ASSERT_TRUE(Pr->check(0));