diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 1a9ca45aa5cf2334f45b4b4d894f25692b93af03..884efeb41230ccd255336938fb445fc9acfd327e 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 "";