Draft: Resolve "Implementation of new nodes creation"

Closes #454, #318, #364, #449, #381, #376, #208, #485

include/core/sensor/sensor_base.h

@@ -27,10 +27,12 @@ namespace wolf{
 class HardwareBase;
 class ProcessorBase;
 class StateBlock;
+class ParamsServer;
 }
 
 //Wolf includes
 #include "core/common/wolf.h"
+#include "core/state_block/prior.h"
 #include "core/common/node_base.h"
 #include "core/common/time_stamp.h"
 #include "core/common/params_base.h"
@@ -91,12 +93,18 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
  */
 struct ParamsSensorBase: public ParamsBase
 {
-    std::string prefix = "sensor/";
+    Eigen::VectorXd noise_std;
+
+    ParamsSensorBase() = default;
+    ParamsSensorBase(std::string prefix, const wolf::ParamsServer& _server)
+    {
+        noise_std = _server.getParam<Eigen::VectorXd>(prefix + "/noise_std");
+    }
     ~ParamsSensorBase() override = default;
-    using ParamsBase::ParamsBase;
+
     std::string print() const override
     {
-        return "";
+        return "noise_std: " + std::to_string(noise_std) + "\n";
     }
 };
 
@@ -124,6 +132,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         Eigen::MatrixXd noise_cov_; // cov matrix of noise
 
         void setProblem(ProblemPtr _problem) override final;
+        virtual void loadParams(ParamsSensorBasePtr _params);
+        virtual void loadPriors(Priors _priors, SizeEigen _dim);
 
     public:
 
@@ -131,57 +141,28 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
          *
          * Constructor with parameter vector
          * \param _tp Type of the sensor  (types defined at wolf.h)
-         * \param _priors list of the priors of the sensor states
+         * \param _priors priors of the sensor states
          * \param _params params struct
          *
          **/
         SensorBase(const std::string& _type,
-                   std::list<Prior> _priors,
+                   const SizeEigen& _dim,
+                   const Priors& _priors,
                    ParamsSensorBasePtr _params);
 
-        /** \brief Constructor with noise size
-         *
-         * Constructor with parameter vector
-         * \param _tp Type of the sensor  (types defined at wolf.h)
-         * \param _p_ptr StateBlock pointer to the sensor position
-         * \param _o_ptr StateBlock pointer to the sensor orientation
-         * \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
-         * \param _noise_size dimension of the noise term
-         * \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
-         * \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
-         * \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving)
-         *
-         **/
-        SensorBase(const std::string& _type,
-                   StateBlockPtr _p_ptr,
-                   StateBlockPtr _o_ptr,
-                   StateBlockPtr _intr_ptr,
-                   const unsigned int _noise_size,
-                   const bool _p_dyn = false,
-                   const bool _o_dyn = false,
-                   const bool _intr_dyn = false);
-
-        /** \brief Constructor with noise std vector
+        /** \brief Constructor with ParamServer and keys
          *
          * Constructor with parameter vector
          * \param _tp Type of the sensor  (types defined at wolf.h)
-         * \param _p_ptr StateBlock pointer to the sensor position
-         * \param _o_ptr StateBlock pointer to the sensor orientation
-         * \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed).
-         * \param _noise_std standard deviations of the noise term
-         * \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
-         * \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
-         * \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving)
+         * \param _server parameter server
+         * \param _keys keys of the states of the derived sensor
          *
          **/
         SensorBase(const std::string& _type,
-                   StateBlockPtr _p_ptr,
-                   StateBlockPtr _o_ptr,
-                   StateBlockPtr _intr_ptr,
-                   const Eigen::VectorXd & _noise_std,
-                   const bool _p_dyn = false,
-                   const bool _o_dyn = false,
-                   const bool _intr_dyn = false);
+                   const SizeEigen& _dim,
+                   const std::string& _unique_name,
+                   const ParamsServer& _server,
+                   std::string _keys);
 
         ~SensorBase() override;