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