added implementation of addParameterPrior()

src/sensor_base.cpp

 #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()
 {

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;