diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c5205bf69a1caead2a989ba12ac32784ce4931b..6386280f537dfd796ae8e5c90529762e0591bc81 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -288,6 +288,7 @@ SET(HDRS_FACTOR include/core/factor/factor_block_absolute.h include/core/factor/factor_diff_drive.h include/core/factor/factor_feature_dummy.h + include/core/factor/factor_landmark_dummy.h include/core/factor/factor_odom_2D.h include/core/factor/factor_odom_2D_analytic.h include/core/factor/factor_odom_3D.h diff --git a/include/core/factor/factor_landmark_dummy.h b/include/core/factor/factor_landmark_dummy.h new file mode 100644 index 0000000000000000000000000000000000000000..95a5c483dc4dfd336880b64972f4ecad4fcb07bf --- /dev/null +++ b/include/core/factor/factor_landmark_dummy.h @@ -0,0 +1,68 @@ +#ifndef FACTOR_LANDMARK_DUMMY_H +#define FACTOR_LANDMARK_DUMMY_H + +#include "core/factor/factor_base.h" + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FactorLandmarkDummy); + +class FactorLandmarkDummy : public FactorBase +{ + public: + FactorLandmarkDummy(const FeatureBasePtr& _feature_ptr, + const LandmarkBasePtr& _landmark_other_ptr, + const ProcessorBasePtr& _processor_ptr = nullptr, + bool _apply_loss_function = false, + FactorStatus _status = FAC_ACTIVE); + + virtual ~FactorLandmarkDummy() = default; + + /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians + **/ + virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;}; + + /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr + **/ + virtual void evaluate(const std::vector<const Scalar*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override {}; + + /** \brief Returns the jacobians computation method + **/ + virtual JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;} + + /** \brief Returns a vector of pointers to the states in which this factor depends + **/ + virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);} + + /** \brief Returns the factor residual size + **/ + virtual unsigned int getSize() const override {return 0;} + + /** \brief Returns the factor states sizes + **/ + virtual std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>({1});} + + public: + static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, + const NodeBasePtr& _correspondant_ptr, + const ProcessorBasePtr& _processor_ptr = nullptr); + +}; + +inline FactorLandmarkDummy::FactorLandmarkDummy(const FeatureBasePtr& /*_feature_ptr*/, const LandmarkBasePtr& _landmark_other_ptr, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, FactorStatus _status) : + FactorBase("FEATURE DUMMY", nullptr, nullptr, nullptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status) +{ + // +} + +inline FactorBasePtr FactorLandmarkDummy::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, + const ProcessorBasePtr& _processor_ptr) +{ + return std::make_shared<FactorLandmarkDummy>(_feature_ptr, std::static_pointer_cast<LandmarkBase>(_correspondant_ptr), _processor_ptr); +} + +} // namespace wolf + +#endif // FACTOR_LANDMARK_DUMMY_H