Use ParamsSensorBase struct in sensors

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

include/core/sensor/sensor_odom_2d.h

@@ -32,26 +32,25 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2d);
 
 struct ParamsSensorOdom2d : public ParamsSensorBase
 {
-        ~ParamsSensorOdom2d() override = default;
 
         double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
         double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
-    ParamsSensorOdom2d()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
-    ParamsSensorOdom2d(std::string _unique_name, const ParamsServer& _server):
-        ParamsSensorBase(_unique_name, _server)
-    {
-        k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
-        k_rot_to_rot   = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
-    }
-    std::string print() const override
-    {
-        return ParamsSensorBase::print()                               + "\n"
-                + "k_disp_to_disp: "    + std::to_string(k_disp_to_disp)    + "\n"
-                + "k_rot_to_rot: "      + std::to_string(k_rot_to_rot)      + "\n";
-    }
+
+        ParamsSensorOdom2d() = default;
+
+        ParamsSensorOdom2d(std::string _unique_name, const ParamsServer& _server):
+            ParamsSensorBase(_unique_name, _server)
+        {
+            k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
+            k_rot_to_rot   = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
+        }
+        ~ParamsSensorOdom2d() override = default;
+        std::string print() const override
+        {
+            return ParamsSensorBase::print()                               + "\n"
+                    + "k_disp_to_disp: "    + std::to_string(k_disp_to_disp)    + "\n"
+                    + "k_rot_to_rot: "      + std::to_string(k_rot_to_rot)      + "\n";
+        }
 };
 
 WOLF_PTR_TYPEDEFS(SensorOdom2d);

include/core/sensor/sensor_odom_3d.h

@@ -44,10 +44,8 @@ struct ParamsSensorOdom3d : public ParamsSensorBase
         double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
         double min_disp_var;
         double min_rot_var;
-    ParamsSensorOdom3d()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
+    ParamsSensorOdom3d() = default;
+
     ParamsSensorOdom3d(std::string _unique_name, const ParamsServer& _server):
         ParamsSensorBase(_unique_name, _server)
     {

include/core/sensor/sensor_pose.h

@@ -41,10 +41,8 @@ struct ParamsSensorPose : public ParamsSensorBase
 {
     double std_p = 1;  // m
     double std_o = 1;  // rad
-    ParamsSensorPose()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
+    ParamsSensorPose() = default;
+
     ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
         ParamsSensorBase(_unique_name, _server)
     {

src/sensor/sensor_base.cpp

@@ -42,16 +42,14 @@ SensorBase::SensorBase(const std::string& _type,
         HasStateBlocks(""),
         hardware_ptr_(),
         sensor_id_(++sensor_id_count_), // simple ID factory
-        noise_std_(_noise_size),
-        noise_cov_(_noise_size, _noise_size),
         last_capture_(nullptr)
 {
     assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
     assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
     assert((_intr_ptr or not _intr_dyn) and "Trying to set dynamic intrinsics state block without providing an intrinsics state block. It is required anyway.");
 
-    noise_std_.setZero();
-    noise_cov_.setZero();
+    params_.noise_std.setZero(_noise_size);
+    params_.noise_cov.setZero(_noise_size,_noise_size);
 
     if (_p_ptr)
         addStateBlock('P', _p_ptr, _p_dyn);
@@ -76,8 +74,6 @@ SensorBase::SensorBase(const std::string& _type,
         HasStateBlocks(""),
         hardware_ptr_(),
         sensor_id_(++sensor_id_count_), // simple ID factory
-        noise_std_(_noise_std),
-        noise_cov_(_noise_std.size(), _noise_std.size()),
         last_capture_(nullptr)
 {
     setNoiseStd(_noise_std);
@@ -219,13 +215,13 @@ void SensorBase::registerNewStateBlocks() const
 }
 
 void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) {
-    noise_std_ = _noise_std;
-    noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
+    params_.noise_std = _noise_std;
+    params_.noise_cov = _noise_std.array().square().matrix().asDiagonal();
 }
 
 void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) {
-    noise_std_ = _noise_cov.diagonal().array().sqrt();
-    noise_cov_ = _noise_cov;
+    params_.noise_std = _noise_cov.diagonal().array().sqrt();
+    params_.noise_cov = _noise_cov;
 }
 
 void SensorBase::setLastCapture(CaptureBasePtr cap)

src/sensor/sensor_odom_3d.cpp

@@ -44,8 +44,8 @@ SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSe
 {
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
 
-    noise_cov_ = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
-    setNoiseCov(noise_cov_); // sets also noise_std_
+    Matrix6d noise_cov = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
+    setNoiseCov(noise_cov); // sets also noise_std_
 }
 
 SensorOdom3d::SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3dPtr _intrinsics) :

src/sensor/sensor_pose.cpp

@@ -41,8 +41,8 @@ SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensor
 {
     assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3d.");
 
-    noise_cov_ = (Eigen::Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal();
-    setNoiseCov(noise_cov_); // sets also noise_std_
+    Vector6d noise_std = (Eigen::Vector6d() << std_p_, std_p_, std_p_, std_o_, std_o_, std_o_).finished();
+    setNoiseStd(noise_std); // sets also noise_cov_
 }
 
 SensorPose::SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr _intrinsics) :