WIP

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,71 @@ 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");
+        }
+
+        // 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");
+        }
+
+        // I
+        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");
+    }
+
     ~ParamsSensorBase() override = default;
-    using ParamsBase::ParamsBase;
+
     std::string print() const override
     {
         return "";
@@ -108,6 +177,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

include/core/sensor/sensor_diff_drive.h

@@ -26,7 +26,7 @@ struct ParamsSensorDiffDrive : public ParamsSensorBase
     double          ticks_cov_factor;
 
     ParamsSensorDiffDrive() = default;
-    ParamsSensorDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server)
+    ParamsSensorDiffDrive(std::string _unique_name, const ParamsServer& _server)
       : ParamsSensorBase(_unique_name, _server)
     {
         radius_left                = _server.getParam<double>(prefix + _unique_name + "/radius_left");

src/sensor/sensor_base.cpp

@@ -9,6 +9,71 @@ namespace wolf {
 
 unsigned int SensorBase::sensor_id_count_ = 0;
 
+SensorBase::SensorBase(const std::string& _type,
+                       ParamsSensorBasePtr _params,
+                       const int& _intrinsics_size) :
+        NodeBase("SENSOR", _type),
+        HasStateBlocks(""),
+        hardware_ptr_(),
+        sensor_id_(++sensor_id_count_), // simple ID factory
+        last_capture_(nullptr)
+{
+    for (auto sb_pair : _params->state_blocks)
+    {
+        const char& key = sb_pair.first;
+        const ParamsStateBlock& sb_param = sb_pair.second;
+
+        if (key == 'I' and _intrinsics_size == 0)
+            continue;
+
+        addStateBlock(key,
+                      (key != 'O' ?
+                              std::make_shared<StateBlock>(sb_param.state,sb_param.prior_mode == "fix") :
+                              (_params->dim == 2 ?
+                                      std::static_pointer_cast<StateBlock>(std::make_shared<StateAngle>(sb_param.state, sb_param.prior_mode == "fix")) :
+                                      std::static_pointer_cast<StateBlock>(std::make_shared<StateQuaternion>(sb_param.state,sb_param.prior_mode == "fix")) ) ),
+                      sb_param.dynamic);
+
+      if (sb_param.prior_mode == "factor")
+          addPriorParameter(key,
+                            sb_param.state,
+                            sb_param.sigma.asDiagonal());
+    }
+
+    /*// P
+    addStateBlock('P',
+                  std::make_shared<StateBlock>(_params->state_blocks.at('P').state,
+                                               _params->state_blocks.at('P').prior_mode == "fix"),
+                  _params->state_blocks.at('P').dynamic);
+
+    if (_params->state_blocks.at('P').prior_mode == "factor")
+        addPriorP(_params->state_blocks.at('P').state, _params->state_blocks.at('P').sigma.asDiagonal());
+
+    // O
+    addStateBlock('O',
+                  (_params->dim == 2 ?
+                          std::make_shared<StateAngle>(_params->state_blocks.at('O').state,
+                                                       _params->state_blocks.at('O').prior_mode == "fix") :
+                          std::make_shared<StateQuaternion>(_params->state_blocks.at('O').state,
+                                                            _params->state_blocks.at('O').prior_mode == "fix") ),
+                  _params->o_dynamic);
+
+    if (_params->state_blocks.at('O').prior_mode == "factor")
+        addPriorO(_params->p_state, _params->p_cov);
+
+    // I
+    if (_intrinsics_size != 0)
+    {
+        addStateBlock('I',
+                      std::make_shared<StateBlock>(_params->state_blocks.at('I').state,
+                                                   _params->state_blocks.at('I').prior_mode == "fix"),
+                      _params->state_blocks.at('I').dynamic);
+
+        if (_params->state_blocks.at('I').prior_mode == "factor")
+            addPriorI(_params->state_blocks.at('I').state, _params->state_blocks.at('I').sigma.asDiagonal());
+    }*/
+}
+
 SensorBase::SensorBase(const std::string& _type,
                        StateBlockPtr _p_ptr,
                        StateBlockPtr _o_ptr,