Draft: Resolve "SensorBase: extrinsic and intrinsic initialization and prior"

Closes #381

include/core/sensor/sensor_base.h

@@ -65,7 +65,13 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
 }                                                                                                                               \
 
 
-
+struct ParamsStateBlock
+{
+    Eigen::VectorXd state;
+    std::string prior_mode;
+    Eigen::VectorXd sigma;
+    bool dynamic;
+};
 
 /** \brief base struct for intrinsic sensor parameters
  *
@@ -74,8 +80,103 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
 struct ParamsSensorBase: public ParamsBase
 {
     std::string prefix = "sensor/";
+
+    int dim;
+
+    std::unordered_map<char, ParamsStateBlock> state_blocks;
+
+    ParamsSensorBase() = default;
+    ParamsSensorBase(std::string _unique_name, const ParamsServer& _server) :
+        ParamsBase(_unique_name, _server)
+    {
+        dim = _server.getParam<int>("problem/dimension");
+        if (dim != 2 and dim != 3)
+        {
+            WOLF_ERROR("ParamsSensorBase: parameter 'problem/dimension' should be 2 or 3. Value provided: ", dim);
+            throw std::runtime_error("ParamsSensorBase wrong 'problem/dimension'");
+        }
+
+        // P
+        state_blocks.emplace('P',ParamsStateBlock());
+        if (dim == 2)
+        {
+            state_blocks['P'].state         = _server.getParam<Eigen::Vector2d> (prefix + _unique_name + "/P/state");
+            state_blocks['P'].prior_mode    = _server.getParam<std::string>     (prefix + _unique_name + "/P/prior_mode");
+            state_blocks['P'].dynamic       = _server.getParam<bool>            (prefix + _unique_name + "/P/is_dynamic");
+            if (state_blocks['P'].prior_mode == "factor")
+                state_blocks['P'].sigma     = _server.getParam<Eigen::Vector2d> (prefix + _unique_name + "/P/sigma");
+        }
+        else if (dim == 3)
+        {
+            state_blocks['P'].state         = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/P/state");
+            state_blocks['P'].prior_mode    = _server.getParam<std::string>     (prefix + _unique_name + "/P/prior_mode");
+            state_blocks['P'].dynamic       = _server.getParam<bool>            (prefix + _unique_name + "/P/is_dynamic");
+            if (state_blocks['P'].prior_mode == "factor")
+                state_blocks['P'].sigma     = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/P/sigma");
+        }
+        assert((state_blocks['P'].prior_mode == "initial_guess" || state_blocks['P'].prior_mode == "fix" || state_blocks['P'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
+
+        // O
+        state_blocks.emplace('O',ParamsStateBlock());
+        if (dim == 2)
+        {
+            state_blocks['O'].state         = _server.getParam<Eigen::Vector1d> (prefix + _unique_name + "/O/state");
+            state_blocks['O'].prior_mode    = _server.getParam<std::string>     (prefix + _unique_name + "/O/prior_mode");
+            state_blocks['O'].dynamic       = _server.getParam<bool>            (prefix + _unique_name + "/O/is_dynamic");
+            if (state_blocks['O'].prior_mode == "factor")
+                state_blocks['O'].sigma     = _server.getParam<Eigen::Vector1d> (prefix + _unique_name + "/O/sigma");
+        }
+        else if (dim == 3)
+        {
+            state_blocks['O'].state         = _server.getParam<Eigen::Vector4d> (prefix + _unique_name + "/O/state");
+            state_blocks['O'].prior_mode    = _server.getParam<std::string>     (prefix + _unique_name + "/O/prior_mode");
+            state_blocks['O'].dynamic       = _server.getParam<bool>            (prefix + _unique_name + "/O/is_dynamic");
+            if (state_blocks['O'].prior_mode == "factor")
+                state_blocks['O'].sigma     = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/O/sigma");
+        }
+        assert((state_blocks['O'].prior_mode == "initial_guess" || state_blocks['O'].prior_mode == "fix" || state_blocks['O'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
+
+        // I
+        try
+        {
+            state_blocks.emplace('I',ParamsStateBlock());
+            state_blocks['I'].state         = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/I/state");
+            state_blocks['I'].prior_mode    = _server.getParam<std::string>     (prefix + _unique_name + "/I/prior_mode");
+            state_blocks['I'].dynamic       = _server.getParam<bool>            (prefix + _unique_name + "/I/is_dynamic");
+            if (state_blocks['I'].prior_mode == "factor")
+                state_blocks['I'].sigma     = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/I/sigma");
+            assert((state_blocks['I'].prior_mode == "initial_guess" || state_blocks['I'].prior_mode == "fix" || state_blocks['I'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
+        }
+        catch(...)
+        {
+            WOLF_INFO("ParamsSensorBase: Prior for the intrinsics not provided for sensor ", _unique_name);
+        }
+    }
+
     ~ParamsSensorBase() override = default;
-    using ParamsBase::ParamsBase;
+
+    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 "";
@@ -108,6 +209,17 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         void setProblem(ProblemPtr _problem) override final;
 
     public:
+        /** \brief Constructor with params struct
+         *
+         * Constructor with parameter struct
+         * \param _tp Type of the sensor  (types defined at wolf.h)
+         * \param _params ParamsSensorBasePtr pointer to params struct
+         *
+         **/
+        SensorBase(const std::string& _type,
+                   ParamsSensorBasePtr _params,
+                   const int& _intrinsics_size);
+
         /** \brief Constructor with noise size
          *
          * Constructor with parameter vector