diff --git a/include/laser/factor/factor_point_2d.h b/include/laser/factor/factor_point_2d.h index 395ef5e844c48a0d761d413c31f4c2a47c9d7f63..5cf97ff2bdb1cbc32c385d85cfb6bce3d340a2f5 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: @@ -33,6 +31,7 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorPoint2d,2,2,1,2,1,2>("FactorPoint2d", + _ftr_ptr, nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, @@ -44,15 +43,21 @@ 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 + + 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 ~FactorPoint2d() = default; @@ -103,27 +108,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 f2aee9c6701ba9083524dc51537374f7b4b562b2..1fad54e85f41e393ad1e6e04f775437229f158db 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: @@ -31,6 +30,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,2,2>("FactorPointToLine2d", + _ftr_ptr, nullptr, nullptr, nullptr, @@ -48,16 +48,23 @@ 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 + + // 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 ~FactorPointToLine2d() = default; @@ -99,27 +106,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>