diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index e16a01fb375c4809b36be966d8c500b81aa536df..088e84d5e82383662cdac1397a5d35720d406911 100644 --- a/src/sensor_base.cpp +++ b/src/sensor_base.cpp @@ -1,5 +1,7 @@ #include "sensor_base.h" #include "state_block.h" +#include "constraint_block_absolute.h" +#include "constraint_quaternion_absolute.h" namespace wolf { @@ -167,7 +169,25 @@ 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) +{ + 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()); + + // create feature + auto ftr_prior = std::make_shared<FeatureBase>(_x,_cov); + + // set feature problem + ftr_prior->setProblem(getProblem()); + // create & add constraint absolute + if (std::dynamic_pointer_cast<StateQuaternion>(_sb)) + ftr_prior->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(_sb)); + else + ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size)); +} void SensorBase::registerNewStateBlocks() { diff --git a/src/sensor_base.h b/src/sensor_base.h index 354e1474f4919dfa4d1c921b673d6ddf6bb2d999..0fb7d8eb879f1183918fa446c22f12f6539fc33e 100644 --- a/src/sensor_base.h +++ b/src/sensor_base.h @@ -136,7 +136,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa void unfixExtrinsics(); void fixIntrinsics(); void unfixIntrinsics(); - void addFeaturePrior(); + 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;