From bca1a75a7544aa49a6ee185f4c4ba748a1aaa5e3 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 6 Feb 2019 12:20:29 +0100
Subject: [PATCH] documented addParameterPrior()
---
src/sensor_base.cpp | 6 +++++-
src/sensor_base.h | 20 +++++++++++++++++---
2 files changed, 22 insertions(+), 4 deletions(-)
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 1ea1ed3d5..ae29d7491 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -145,12 +145,13 @@ void SensorBase::unfixIntrinsics()
updateCalibSize();
}
-void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx = 0, int _size = -1)
+void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
{
assert(std::find(state_block_vec_.begin(),state_block_vec_.end(),_sb) != state_block_vec_.end() && "adding prior to unknown state block");
assert(_x.size() == _cov.rows() && _x.size() == _cov.cols() && "covariance and prior dimension should be the same");
assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
assert(_size == -1 || _size == _x.size());
+ assert(params_prior_map_.find(_sb) == params_prior_map_.end() && "this parameter has already a prior");
// create feature
auto ftr_prior = std::make_shared<FeatureBase>(_x,_cov);
@@ -163,6 +164,9 @@ void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::Vector
ftr_prior->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(_sb));
else
ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size));
+
+ // store feature in params_prior_map_
+ params_prior_map_[_sb] = ftr_prior;
}
void SensorBase::registerNewStateBlocks()
diff --git a/src/sensor_base.h b/src/sensor_base.h
index 0fb7d8eb8..b97c65976 100644
--- a/src/sensor_base.h
+++ b/src/sensor_base.h
@@ -51,7 +51,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
Eigen::VectorXs noise_std_; // std of sensor noise
Eigen::MatrixXs noise_cov_; // cov matrix of noise
- FeatureBaseList params_prior_list_; // Priors (value and covariance) of extrinsic & intrinsic parameters
+ std::map<StateBlockPtr,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (multimap
public:
/** \brief Constructor with noise size
@@ -93,7 +93,6 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const bool _intr_dyn = false);
virtual ~SensorBase();
- void remove();
unsigned int id();
@@ -136,7 +135,21 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void unfixExtrinsics();
void fixIntrinsics();
void unfixIntrinsics();
- void addParameterPrior(const StateBlockPtr& _sb, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx = 0, int _size = -1);
+ /** \brief Add an absolute constraint to a parameter
+ *
+ * Add an absolute constraint to a parameter
+ * \param _sb state block of the parameter to be constrained
+ * \param _x prior value
+ * \param _cov covariance
+ * \param _start_idx state segment starting index (not used in quaternions)
+ * \param _size state segment size (-1: whole state) (not used in quaternions)
+ *
+ **/
+ void addParameterPrior(const StateBlockPtr& _sb,
+ const Eigen::VectorXs& _x,
+ const Eigen::MatrixXs& _cov,
+ unsigned int _start_idx = 0,
+ int _size = -1);
SizeEigen getCalibSize() const;
Eigen::VectorXs getCalibration() const;
@@ -201,6 +214,7 @@ inline StateBlockPtr SensorBase::getStateBlockPtrStatic(unsigned int _i) const
inline void SensorBase::setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr)
{
+ assert((params_prior_map_.find(state_block_vec_[_i]) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute constraint");
state_block_vec_[_i] = _sb_ptr;
}
--
GitLab