From 1ce45abd4aa1df957f5da077a38c4260451858c0 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 14:42:42 +0100
Subject: [PATCH] Allowing overwritting priors (only one prior per state block)
---
src/sensor_base.cpp | 5 ++++-
1 file changed, 4 insertions(+), 1 deletion(-)
diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp
index 7bdba4f0b..954f519ad 100644
--- a/src/sensor_base.cpp
+++ b/src/sensor_base.cpp
@@ -153,7 +153,6 @@ void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::Vector
assert((_size == -1 && _start_idx == 0) || (_size+_start_idx <= _sb->getSize()));
assert(_size == -1 || _size == _x.size());
assert(!(_size != -1 && _sb->hasLocalParametrization()) && "prior for a segment of the state only available withour local parameterization");
- assert(params_prior_map_.find(_sb) == params_prior_map_.end() && "this parameter has already a prior");
// set StateBlock state
if (_size == -1)
@@ -165,6 +164,10 @@ void SensorBase::addParameterPrior(const StateBlockPtr& _sb, const Eigen::Vector
_sb->setState(new_x);
}
+ // remove previous prior (if any)
+ if (params_prior_map_.find(_sb) == params_prior_map_.end())
+ params_prior_map_[_sb]->remove();
+
// create feature
FeatureBasePtr ftr_prior = std::make_shared<FeatureBase>("ABSOLUTE",_x,_cov);
--
GitLab