From 98fc7ca62f8a1ac6be84da058f17be96799fab69 Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Tue, 2 Jun 2020 22:36:29 +0200 Subject: [PATCH] adapting to measurements in factors --- include/laser/factor/factor_point_2d.h | 50 ++++++++----------- .../laser/factor/factor_point_to_line_2d.h | 49 ++++++++---------- 2 files changed, 40 insertions(+), 59 deletions(-) diff --git a/include/laser/factor/factor_point_2d.h b/include/laser/factor/factor_point_2d.h index 395ef5e84..f85c588f6 100644 --- a/include/laser/factor/factor_point_2d.h +++ b/include/laser/factor/factor_point_2d.h @@ -5,6 +5,7 @@ #include "core/factor/factor_autodiff.h" #include "laser/feature/feature_polyline_2d.h" #include "laser/landmark/landmark_polyline_2d.h" +#include "core/math/covariance.h" namespace wolf { @@ -19,9 +20,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> unsigned int feature_point_id_; int landmark_point_id_; StateBlockPtr point_state_ptr_; - Eigen::VectorXd measurement_; ///< the measurement vector - Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix - Eigen::MatrixXd measurement_sqrt_information_; ///< the squared root information matrix public: @@ -44,19 +42,32 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> _lmk_ptr->getPointStateBlock(_lmk_point_id)), feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), - point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), - measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), - measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2)) + point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)) { //std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl; //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl; - Eigen::LLT<Eigen::MatrixXd> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A - Eigen::MatrixXd measurement_sqrt_covariance = lltOfA.matrixU(); - measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition } virtual ~FactorPoint2d() = default; + virtual void updateMeasurementAndSquareRootInformationUpper() override + { + if (not getFeature()) + return; + + // cast + auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature()); + + // measurement + measurement_ = ftr->getPoints().col(feature_point_id_); + + // covariance (ensuring symmetry) + Eigen::Matrix2d measurement_covariance = ftr->getPointsCov().middleCols(feature_point_id_*2,2).selfadjointView<Eigen::Upper>(); + + // square root information upper matrix + measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance.inverse()); + } + virtual std::string getTopology() const override { return std::string("LMK"); @@ -103,27 +114,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> */ 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 a reference to the feature measurement - **/ - virtual const Eigen::VectorXd& getMeasurement() const override - { - return measurement_; - } - - /** \brief Returns a reference to the feature measurement covariance - **/ - virtual const Eigen::MatrixXd& getMeasurementCovariance() const override - { - return measurement_covariance_; - } - - /** \brief Returns a reference to the feature measurement square root information - **/ - virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const override - { - return measurement_sqrt_information_; - } }; template<typename T> diff --git a/include/laser/factor/factor_point_to_line_2d.h b/include/laser/factor/factor_point_to_line_2d.h index f2aee9c67..34cbe485d 100644 --- a/include/laser/factor/factor_point_to_line_2d.h +++ b/include/laser/factor/factor_point_to_line_2d.h @@ -5,6 +5,7 @@ #include "core/factor/factor_autodiff.h" #include "laser/feature/feature_polyline_2d.h" #include "laser/landmark/landmark_polyline_2d.h" +#include "core/math/covariance.h" namespace wolf { @@ -19,9 +20,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, unsigned int feature_point_id_; StateBlockPtr point_state_ptr_; StateBlockPtr point_aux_state_ptr_; - Eigen::VectorXd measurement_; ///< the measurement vector Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix - Eigen::MatrixXd measurement_sqrt_information_; ///< the squared root information matrix public: @@ -48,20 +47,33 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), - point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), - measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), - measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2)) + point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)) { //std::cout << "FactorPointToLine2d" << std::endl; //std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl; assert(_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_)); - Eigen::LLT<Eigen::MatrixXd> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A - Eigen::MatrixXd measurement_sqrt_covariance = lltOfA.matrixU(); - measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition } virtual ~FactorPointToLine2d() = default; + virtual void updateMeasurementAndSquareRootInformationUpper() override + { + if (not getFeature()) + return; + + // cast + auto ftr = std::static_pointer_cast<FeaturePolyline2d>(getFeature()); + + // measurement + measurement_ = ftr->getPoints().col(feature_point_id_); + + // covariance (ensuring symmetry) + measurement_covariance_ = ftr->getPointsCov().middleCols(feature_point_id_*2,2).selfadjointView<Eigen::Upper>(); + + // square root information upper matrix + measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse()); + } + virtual std::string getTopology() const override { return std::string("GEOM"); @@ -99,27 +111,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, 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 a reference to the feature measurement - **/ - virtual const Eigen::VectorXd& getMeasurement() const override - { - return measurement_; - } - - /** \brief Returns a reference to the feature measurement covariance - **/ - virtual const Eigen::MatrixXd& getMeasurementCovariance() const override - { - return measurement_covariance_; - } - - /** \brief Returns a reference to the feature measurement square root information - **/ - virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const override - { - return measurement_sqrt_information_; - } }; template<typename T> -- GitLab