Resolve "Implement use of ParamsSensorBase in class SensorBase"

Closes #450 (closed)

include/core/sensor/sensor_base.h

@@ -95,6 +95,14 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
 struct ParamsSensorBase: public ParamsBase
 {
     std::string prefix = "sensor/";
+    Eigen::VectorXd noise_std; // std of sensor noise
+    Eigen::MatrixXd noise_cov; // cov matrix of noise
+    ParamsSensorBase(std::string _unique_name, const ParamsServer& _server):
+        ParamsBase(_unique_name, _server)
+    {
+        noise_std = _server.getParam<Eigen::VectorXd>(prefix + _unique_name + "/noise_std");
+        noise_cov = noise_std.array().square().matrix().asDiagonal();
+    }
     ~ParamsSensorBase() override = default;
     using ParamsBase::ParamsBase;
     std::string print() const override
@@ -121,8 +129,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
         std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
 
     protected:
-        Eigen::VectorXd noise_std_; // std of sensor noise
-        Eigen::MatrixXd noise_cov_; // cov matrix of noise
+        ParamsSensorBase params_;
 
         CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
 
@@ -336,12 +343,12 @@ inline bool SensorBase::isStateBlockDynamic(const char& _key) const
 
 inline Eigen::VectorXd SensorBase::getNoiseStd() const
 {
-    return noise_std_;
+    return params_.noise_std;
 }
 
 inline Eigen::MatrixXd SensorBase::getNoiseCov() const
 {
-    return noise_cov_;
+    return params_.noise_cov;
 }
 
 inline HardwareBasePtr SensorBase::getHardware() const