diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index 1ea1ed3d57c04c74b10ef6e2482fa72c545afcff..ae29d749135771d38ca051151eabde4ab14eee3b 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 0fb7d8eb879f1183918fa446c22f12f6539fc33e..b97c65976c5baebc7c1e5614e504e79c26c16343 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; }