wip

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 "";