From 816da2c44440a4ee95727bbf70a45c6407996f28 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 27 Jul 2021 09:27:13 +0200
Subject: [PATCH] wip
---
include/core/sensor/sensor_base.h | 22 ++++++++++++++++++++++
1 file changed, 22 insertions(+)
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 1a9ca45aa..884efeb41 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -155,6 +155,28 @@ struct ParamsSensorBase: public ParamsBase
~ParamsSensorBase() override = default;
+ void getStateBlockPrior(const ParamsServer& _server, const std::string& _unique_name, const char _key, bool _mandatory = false)
+ {
+ try
+ {
+ state_blocks.emplace(_key,ParamsStateBlock());
+ state_blocks[_key].state = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/" + std::to_string(_key) + "/state");
+ state_blocks[_key].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/" + std::to_string(_key) + "/prior_mode");
+ state_blocks[_key].dynamic = _server.getParam<bool> (prefix + _unique_name + "/" + std::to_string(_key) + "/is_dynamic");
+ if (state_blocks[_key].prior_mode == "factor")
+ state_blocks[_key].sigma = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/" + std::to_string(_key) + "/sigma");
+ assert((state_blocks[_key].prior_mode == "initial_guess" or
+ state_blocks[_key].prior_mode == "fix" or
+ state_blocks[_key].prior_mode == "factor") and
+ "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
+ }
+ catch(...)
+ {
+ if (_mandatory)
+ throw std::runtime_error("Prior for state block " + std::to_string(_key) + " not found");
+ }
+ }
+
std::string print() const override
{
return "";
--
GitLab