From 317fca21f43facb1a9eb1b27e97d72427a2c2147 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 11:47:34 +0100 Subject: [PATCH] added implementation of addParameterPrior() --- src/sensor_base.cpp | 20 ++++++++++++++++++++ src/sensor_base.h | 2 +- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index e16a01fb3..088e84d5e 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 354e1474f..0fb7d8eb8 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; -- GitLab