diff --git a/CMakeLists.txt b/CMakeLists.txt index 96f0efa8a76ebcfd5da19b986309d49324f28ebf..5d60969f803754dbcaccb4516f04dbcbe535e6d3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -215,6 +215,7 @@ SET(HDRS_SENSOR include/core/sensor/sensor_base.h # include/core/sensor/sensor_diff_drive.h # include/core/sensor/sensor_motion_model.h + include/core/sensor/sensor_odom.h include/core/sensor/sensor_odom_2d.h # include/core/sensor/sensor_odom_3d.h # include/core/sensor/sensor_pose.h @@ -323,6 +324,7 @@ SET(SRCS_SENSOR src/sensor/sensor_base.cpp # src/sensor/sensor_diff_drive.cpp # src/sensor/sensor_motion_model.cpp + src/sensor/sensor_odom.cpp src/sensor/sensor_odom_2d.cpp # src/sensor/sensor_odom_3d.cpp # src/sensor/sensor_pose.cpp diff --git a/include/core/sensor/sensor_odom.h b/include/core/sensor/sensor_odom.h new file mode 100644 index 0000000000000000000000000000000000000000..3bb0eff610255c21b9f80e3d6c3031a429f7fbf1 --- /dev/null +++ b/include/core/sensor/sensor_odom.h @@ -0,0 +1,123 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +#ifndef SENSOR_ODOM_H_ +#define SENSOR_ODOM_H_ + +//wolf includes +#include "core/sensor/sensor_base.h" +#include "core/utils/params_server.h" + +namespace wolf { + +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom); + +struct ParamsSensorOdom : public ParamsSensorBase +{ + double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation + double k_disp_to_rot; ///< ratio of displacement variance to rotation, for odometry noise calculation + double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation + double min_disp_var; + double min_rot_var; + + ParamsSensorOdom() = default; + ParamsSensorOdom(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_disp_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_rot"); + k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot"); + min_disp_var = _server.getParam<double>(prefix + _unique_name + "/min_disp_var"); + min_rot_var = _server.getParam<double>(prefix + _unique_name + "/min_rot_var"); + } + ~ParamsSensorOdom() override = default; + std::string print() const override + { + return ParamsSensorBase::print() + "\n" + + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n" + + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n" + + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n" + + "min_disp_var: " + std::to_string(min_disp_var) + "\n" + + "min_rot_var: " + std::to_string(min_rot_var) + "\n"; + } +}; + +WOLF_PTR_TYPEDEFS(SensorOdom); + +class SensorOdom : public SensorBase +{ + protected: + SizeEigen dim_; + ParamsSensorOdomPtr params_odom_; + + public: + SensorOdom(const std::string& _unique_name, + const SizeEigen& _dim, + ParamsSensorOdomPtr _params, + const Priors& _priors); + + SensorOdom(const std::string& _unique_name, + const SizeEigen& _dim, + ParamsSensorOdomPtr _params, + const ParamsServer& _server); + + WOLF_SENSOR_CREATE(SensorOdom, ParamsSensorOdom); + + ~SensorOdom() override = default; + + double getDispVarToDispNoiseFactor() const; + double getDispVarToRotNoiseFactor() const; + double getRotVarToRotNoiseFactor() const; + double getMinDispVar() const; + double getMinRotVar() const; + + Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override; + +}; + +inline double SensorOdom::getDispVarToDispNoiseFactor() const +{ + return params_odom_->k_disp_to_disp; +} + +inline double SensorOdom::getDispVarToRotNoiseFactor() const +{ + return params_odom_->k_disp_to_rot; +} + +inline double SensorOdom::getRotVarToRotNoiseFactor() const +{ + return params_odom_->k_rot_to_rot; +} + +inline double SensorOdom::getMinDispVar() const +{ + return params_odom_->min_disp_var; +} + +inline double SensorOdom::getMinRotVar() const +{ + return params_odom_->min_rot_var; +} + +} // namespace wolf + +#endif // SENSOR_ODOM_2d_H_ diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h index b386829fec38d7b7e8fe96f1785b2a02e807561d..117245f896f4362bba30c740bd7b5974588d0282 100644 --- a/include/core/sensor/sensor_odom_2d.h +++ b/include/core/sensor/sensor_odom_2d.h @@ -92,8 +92,6 @@ class SensorOdom2d : public SensorBase /** * Compute covariance of odometry given the motion data. * - * NOTE: This is a helper function for the user, not called automatically anywhere. - * * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion: * - a linear dependence with total displacement * - a linear dependence with total rotation diff --git a/include/core/sensor/sensor_odom_3d.h b/include/core/sensor/sensor_odom_3d.h index 2b94194e2e1cb37fe33a34008d0647e7130e43c9..f1977980b7187128302dc2ed7425168ca12a0c68 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) { @@ -57,6 +55,7 @@ struct ParamsSensorOdom3d : public ParamsSensorBase min_disp_var = _server.getParam<double>(prefix + _unique_name + "/min_disp_var"); min_rot_var = _server.getParam<double>(prefix + _unique_name + "/min_rot_var"); } + ~ParamsSensorOdom3d() override = default; std::string print() const override { return ParamsSensorBase::print() + "\n" @@ -66,7 +65,6 @@ struct ParamsSensorOdom3d : public ParamsSensorBase + "min_disp_var: " + std::to_string(min_disp_var) + "\n" + "min_rot_var: " + std::to_string(min_rot_var) + "\n"; } - ~ParamsSensorOdom3d() override = default; }; WOLF_PTR_TYPEDEFS(SensorOdom3d); @@ -81,9 +79,17 @@ class SensorOdom3d : public SensorBase double min_rot_var_; public: - SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& params); - SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3dPtr params); - WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom3d, 7); + SensorOdom3d(const std::string& _unique_name, + const SizeEigen& _dim, + ParamsSensorOdom2dPtr _params, + const Priors& _priors); + + SensorOdom3d(const std::string& _unique_name, + const SizeEigen& _dim, + ParamsSensorOdom3dPtr _params, + const ParamsServer& _server); + + WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom3dPtr); ~SensorOdom3d() override; @@ -96,8 +102,6 @@ class SensorOdom3d : public SensorBase /** * Compute covariance of odometry given the motion data. * - * NOTE: This is a helper function for the user, not called automatically anywhere. - * * computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion: * - a minimal value for displacement * - a minimal value for rotation @@ -111,7 +115,7 @@ class SensorOdom3d : public SensorBase * * See implementation for details. */ - Matrix6d computeCovFromMotion (const VectorXd& _data) const; + Matrix6d computeNoiseCov (const VectorXd& _data) const override; }; diff --git a/src/sensor/sensor_odom.cpp b/src/sensor/sensor_odom.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c78ae346e835e8588554953e4938885525957565 --- /dev/null +++ b/src/sensor/sensor_odom.cpp @@ -0,0 +1,107 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// +//--------LICENSE_END-------- +#include "core/sensor/sensor_odom.h" + +namespace wolf { + +SensorOdom::SensorOdom(const std::string& _unique_name, + const SizeEigen& _dim, + ParamsSensorOdomPtr _params, + const Priors& _priors) : + SensorBase("SensorOdom", + _unique_name, + _dim, + _params, + _priors), + dim_(_dim), + params_odom_(_params) +{ + assert(dim_ == 2 or dim_ == 3); +} + +SensorOdom::SensorOdom(const std::string& _unique_name, + const SizeEigen& _dim, + ParamsSensorOdomPtr _params, + const ParamsServer& _server) : + SensorBase("SensorOdom", + _unique_name, + _dim, + _params, + _server), + dim_(_dim), + params_odom_(_params) +{ + assert(dim_ == 2 or dim_ == 3); +} + +Eigen::MatrixXd SensorOdom::computeNoiseCov(const Eigen::VectorXd & _data) const +{ + double d; // displacement + double r; // rotation + + // 2D + if (dim_ ==2) + { + assert(_data.size() == 2 or _data.size() == 3); + + // data = [forward_x, rotation_z] 2D + if (_data.size() == 2) + { + d = fabs(_data(0)); + r = fabs(_data(1)); + } + else + { + d = _data.head<2>().norm(); + r = fabs(_data(2)); + } + } + // 3D + else + { + assert(_data.size() == 6 or _data.size() == 7); + + d = _data.head<3>().norm(); + if (_data.size() == 6) + r = _data.tail<3>().norm(); + else + r = 2 * acos(_data(6)); // arc cos of real part of quaternion + } + + // variances + double dvar = params_odom_->min_disp_var + params_odom_->k_disp_to_disp * d; + double rvar = params_odom_->min_rot_var + params_odom_->k_disp_to_rot * d + params_odom_->k_rot_to_rot * r; + + // return + if (dim_ == 2) + return (Vector2d() << dvar, rvar).finished().asDiagonal(); + else + return (Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal(); +} + +} + +// Register in the FactorySensor +#include "core/sensor/factory_sensor.h" +namespace wolf { +WOLF_REGISTER_SENSOR(SensorOdom); +} // namespace wolf