[skip ci] new sensor_odom for both 2D and 3D

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

include/core/sensor/sensor_odom.h

+//--------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_

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

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;
 
 };
 

src/sensor/sensor_odom.cpp

+//--------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