diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 0fdcac98b7c6c317b29f2a6dd718ae155db6e018..94897853c9d3011d7b847fb7bd47088c9b4dbdee 100644
--- a/include/core/sensor/sensor_base.h
+++ b/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
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index a734043ce8a94776831d3ed43819d0c321935e16..8d017da3f40e61bb0896937a4e687fbfba5e7fe0 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/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);
diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h
index 2b94194e2e1cb37fe33a34008d0647e7130e43c9..cc1c13f873f31e3781104dc249d3409b2c3470a2 100644
--- a/include/core/sensor/sensor_odom_3d.h
+++ b/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)
{
diff --git a/include/core/sensor/sensor_pose.h b/include/core/sensor/sensor_pose.h
index e653fe458d39107c143da729a006c727df3959e3..e9b853e81bd6452c16d60fd24c633bc807701781 100644
--- a/include/core/sensor/sensor_pose.h
+++ b/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)
{
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0e649ec5cce5c11ca522c21bab70c67855922839..3d2453078416f8506039e796aa65819a4be1b5a1 100644
--- a/src/sensor/sensor_base.cpp
+++ b/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)
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index b44561ff402bef4971b211cfa2eae43724457fc3..09d90d753440edb5e34e7c698906c112adae8387 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/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) :
diff --git a/src/sensor/sensor_pose.cpp b/src/sensor/sensor_pose.cpp
index 87f2880b8f3e58e1898470572e630375bd792d8c..fcb765c4363eef3d9a294bbec642298e275cbe06 100644
--- a/src/sensor/sensor_pose.cpp
+++ b/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) :