diff --git a/demos/hello_wolf/factor_bearing.h b/demos/hello_wolf/factor_bearing.h index 6975a798db580e1c127b0215609f49904b29ae40..f5c5dd95887add0562377c360e7087a8f975665d 100644 --- a/demos/hello_wolf/factor_bearing.h +++ b/demos/hello_wolf/factor_bearing.h @@ -68,9 +68,8 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, // 3. Get the expected bearing of the point T bearing = atan2(point_r(1), point_r(0)); - // 4. Get the measured range-and-bearing to the point, and its covariance + // 4. Get the measured range-and-bearing to the point Matrix<T, 2, 1> range_bearing = getMeasurement(); - Matrix<T, 2, 2> range_bearing_cov = getMeasurementCovariance(); // 5. Get the bearing error by comparing against the bearing measurement T er = range_bearing(1) - bearing; @@ -80,8 +79,7 @@ inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1, er -= T(2*M_PI); // 6. Compute the residual by scaling according to the standard deviation of the bearing part - T sigma = sqrt(range_bearing_cov(1,1)); - *_res = er / sigma; + *_res = er * T(getMeasurementSquareRootInformationUpper()(1,1)); return true; } diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h index 6436100b70dc72d24e5332c693792ae11f567330..82feb706d6012dbd35ff78aa09206ff3b5458f49 100644 --- a/include/core/factor/factor_base.h +++ b/include/core/factor/factor_base.h @@ -52,7 +52,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list - ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer list + ProcessorBaseWPtr processor_ptr_; ///< Processor pointer + Eigen::VectorXd measurement_; ///< the measurement vector + Eigen::MatrixXd measurement_sqrt_information_upper_;///< the squared root information matrix ///< ProcessorBase pointer list virtual void setProblem(ProblemPtr) final; public: @@ -124,17 +126,17 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa **/ virtual std::vector<unsigned int> getStateSizes() const = 0; - /** \brief Returns a reference to the feature measurement + /** \brief Returns a reference to the measurement **/ virtual const Eigen::VectorXd& getMeasurement() const; - /** \brief Returns a reference to the feature measurement covariance + /** \brief Returns a reference to the measurement square root information **/ - virtual const Eigen::MatrixXd& getMeasurementCovariance() const; + virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const; - /** \brief Returns a reference to the feature measurement square root information + /** \brief Update the measurement and its square root information according to the feature **/ - virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const; + virtual void updateMeasurementAndSquareRootInformationUpper(); /** \brief Returns a pointer to the feature constrained from **/ @@ -254,7 +256,6 @@ std::shared_ptr<classType> FactorBase::emplace(FeatureBasePtr _ftr_ptr, T&&... a return ctr; } - inline unsigned int FactorBase::id() const { return factor_id_; @@ -307,7 +308,6 @@ inline LandmarkBasePtr FactorBase::getLandmarkOther() const return landmark_other_list_.front().lock(); } - inline ProcessorBasePtr FactorBase::getProcessor() const { return processor_ptr_.lock(); @@ -318,5 +318,15 @@ inline void FactorBase::setProcessor(const ProcessorBasePtr& _processor_ptr) processor_ptr_ = _processor_ptr; } +inline const Eigen::VectorXd& FactorBase::getMeasurement() const +{ + return measurement_; +} + +inline const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpper() const +{ + return measurement_sqrt_information_upper_; +} + } // namespace wolf #endif diff --git a/include/core/factor/factor_block_difference.h b/include/core/factor/factor_block_difference.h index d3558ef21aa6e6577fc54d26015db3f9e3193312..2c089128b6aa6e0190786accdf3130d73ac8e517 100644 --- a/include/core/factor/factor_block_difference.h +++ b/include/core/factor/factor_block_difference.h @@ -129,8 +129,8 @@ inline Eigen::VectorXd FactorBlockDifference::evaluateResiduals(const std::vecto { // Check measurement and cov sizes assert(_st_vector.size() == 2 && "Wrong number of state blocks!"); - assert(getMeasurement().size() == getMeasurementCovariance().cols()); - assert(getMeasurement().size() == getMeasurementCovariance().rows()); + assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().cols()); + assert(getMeasurement().size() == getMeasurementSquareRootInformationUpper().rows()); assert(getMeasurement().size() == sb1_constrained_size_); assert(getMeasurement().size() == sb2_constrained_size_); diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h index da1f68b8400cb94f87d1cc5ee460fbf8440b7bee..c343dfe94748a3ba33963ad7fca306ba1d54e4ec 100644 --- a/include/core/feature/feature_base.h +++ b/include/core/feature/feature_base.h @@ -191,11 +191,6 @@ inline const Eigen::MatrixXd& FeatureBase::getMeasurementSquareRootInformationUp return measurement_sqrt_information_upper_; } -inline void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas) -{ - measurement_ = _meas; -} - inline const Eigen::VectorXd& FeatureBase::getExpectation() const { return expectation_; diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index 0c9286337d850796006c208a67aba4c2775e137e..92cd31c4586f3e549cba372374bef58ff286fe34 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -139,22 +139,13 @@ void FactorBase::remove(bool viral_remove_empty_parent) } } -const Eigen::VectorXd& FactorBase::getMeasurement() const +void FactorBase::updateMeasurementAndSquareRootInformationUpper() { - assert(getFeature() != nullptr && "calling getMeasurement before linking with a feature"); - return getFeature()->getMeasurement(); -} - -const Eigen::MatrixXd& FactorBase::getMeasurementCovariance() const -{ - assert(getFeature() != nullptr && "calling getMeasurementCovariance before linking with a feature"); - return getFeature()->getMeasurementCovariance(); -} - -const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpper() const -{ - assert(getFeature() != nullptr && "calling getMeasurementSquareRootInformationUpper before linking with a feature"); - return getFeature()->getMeasurementSquareRootInformationUpper(); + if (getFeature()) + { + measurement_ = getFeature()->getMeasurement(); + measurement_sqrt_information_upper_ = getFeature()->getMeasurementSquareRootInformationUpper(); + } } CaptureBasePtr FactorBase::getCapture() const @@ -249,11 +240,6 @@ bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other) const return false; } -//void FactorBase::setApplyLossFunction(const bool _apply) -//{ -// apply_loss_function_ = _apply; -//} - void FactorBase::link(FeatureBasePtr _ftr_ptr) { assert(!is_removing_ && "linking a removed factor"); @@ -264,16 +250,19 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr) { WOLF_WARN("Linking with nullptr"); return; - - // if frame, should not be not-key - if (getCapture() and getFrame()) - assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame."); } + // copy measurement_ and measurement_sqrt_information_upper_ + measurement_ = _ftr_ptr->getMeasurement(); + measurement_sqrt_information_upper_ = _ftr_ptr->getMeasurementSquareRootInformationUpper(); + // link with feature _ftr_ptr->addFactor(shared_from_this()); this->setFeature(_ftr_ptr); + // if frame, should be key + if (getCapture() and getFrame()) + assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame."); // set problem ( and register factor ) WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM."); diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index 86d56b3e46bf20390a7083d695b0151b910d17ee..fa7180f2de59a1263d937bee6a0c7b2e1ecc8c36 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -131,6 +131,11 @@ void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov) // compute square root information upper matrix measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse()); + + // call all factors to update their measurements and square root info + WOLF_INFO_COND(!factor_list_.empty(), "Calling getMeasurementSquareRootInformationUpper() for all factors of this feature"); + for (auto fac : factor_list_) + fac->updateMeasurementAndSquareRootInformationUpper(); } void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info) @@ -145,6 +150,21 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info) // compute square root information upper matrix measurement_sqrt_information_upper_ = computeSqrtUpper(_meas_info); + + // call all factors to update their measurements and square root info + WOLF_INFO_COND(!factor_list_.empty(), "Calling getMeasurementSquareRootInformationUpper() for all factors of this feature"); + for (auto fac : factor_list_) + fac->updateMeasurementAndSquareRootInformationUpper(); +} + +void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas) +{ + measurement_ = _meas; + + // call all factors to update their measurements and square root info + WOLF_INFO_COND(!factor_list_.empty(), "Calling getMeasurementSquareRootInformationUpper() for all factors of this feature"); + for (auto fac : factor_list_) + fac->updateMeasurementAndSquareRootInformationUpper(); } void FeatureBase::setProblem(ProblemPtr _problem)