Skip to content
Snippets Groups Projects
Commit 98fc7ca6 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

adapting to measurements in factors

parent ab3eb138
No related branches found
No related tags found
3 merge requests!30Release after RAL,!29After 2nd RAL submission,!19Resolve "Copying measurement and sqrtinfo in FactorBase (multi-threading)"
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
#include "core/factor/factor_autodiff.h" #include "core/factor/factor_autodiff.h"
#include "laser/feature/feature_polyline_2d.h" #include "laser/feature/feature_polyline_2d.h"
#include "laser/landmark/landmark_polyline_2d.h" #include "laser/landmark/landmark_polyline_2d.h"
#include "core/math/covariance.h"
namespace wolf { namespace wolf {
...@@ -19,9 +20,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> ...@@ -19,9 +20,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
unsigned int feature_point_id_; unsigned int feature_point_id_;
int landmark_point_id_; int landmark_point_id_;
StateBlockPtr point_state_ptr_; 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: public:
...@@ -44,19 +42,32 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> ...@@ -44,19 +42,32 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
_lmk_ptr->getPointStateBlock(_lmk_point_id)), _lmk_ptr->getPointStateBlock(_lmk_point_id)),
feature_point_id_(_ftr_point_id), feature_point_id_(_ftr_point_id),
landmark_point_id_(_lmk_point_id), landmark_point_id_(_lmk_point_id),
point_state_ptr_(_lmk_ptr->getPointStateBlock(_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))
{ {
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl; //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; //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 ~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 virtual std::string getTopology() const override
{ {
return std::string("LMK"); return std::string("LMK");
...@@ -103,27 +114,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2> ...@@ -103,27 +114,6 @@ class FactorPoint2d: public FactorAutodiff<FactorPoint2d, 2,2,1,2,1,2>
*/ */
template <typename T> 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; 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> template<typename T>
......
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
#include "core/factor/factor_autodiff.h" #include "core/factor/factor_autodiff.h"
#include "laser/feature/feature_polyline_2d.h" #include "laser/feature/feature_polyline_2d.h"
#include "laser/landmark/landmark_polyline_2d.h" #include "laser/landmark/landmark_polyline_2d.h"
#include "core/math/covariance.h"
namespace wolf { namespace wolf {
...@@ -19,9 +20,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, ...@@ -19,9 +20,7 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
unsigned int feature_point_id_; unsigned int feature_point_id_;
StateBlockPtr point_state_ptr_; StateBlockPtr point_state_ptr_;
StateBlockPtr point_aux_state_ptr_; StateBlockPtr point_aux_state_ptr_;
Eigen::VectorXd measurement_; ///< the measurement vector
Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
Eigen::MatrixXd measurement_sqrt_information_; ///< the squared root information matrix
public: public:
...@@ -48,20 +47,33 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, ...@@ -48,20 +47,33 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
landmark_point_aux_id_(_lmk_point_aux_id), landmark_point_aux_id_(_lmk_point_aux_id),
feature_point_id_(_ftr_point_id), feature_point_id_(_ftr_point_id),
point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)),
point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_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))
{ {
//std::cout << "FactorPointToLine2d" << std::endl; //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; //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_)); 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 ~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 virtual std::string getTopology() const override
{ {
return std::string("GEOM"); return std::string("GEOM");
...@@ -99,27 +111,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1, ...@@ -99,27 +111,6 @@ class FactorPointToLine2d: public FactorAutodiff<FactorPointToLine2d, 1,2,1,2,1,
template <typename T> 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; 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> template<typename T>
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment