hotfix: Fix motion covariance in SensorOdom2d and SensorOdom3d

include/core/sensor/sensor_odom_2d.h

@@ -78,7 +78,7 @@ class SensorOdom2d : public SensorBase
          *
          * See implementation for details.
          */
-        Matrix3d computeCovFromMotion (const VectorXd& _data) const;
+        Matrix2d computeCovFromMotion (const VectorXd& _data) const;
 
 
 };

src/sensor/sensor_odom_2d.cpp

@@ -34,7 +34,7 @@ double SensorOdom2d::getRotVarToRotNoiseFactor() const
     return k_rot_to_rot_;
 }
 
-Eigen::Matrix3d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
+Eigen::Matrix2d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
 {
     double d = fabs(_data(0));
     double r = fabs(_data(1));
@@ -42,7 +42,7 @@ Eigen::Matrix3d SensorOdom2d::computeCovFromMotion (const VectorXd& _data) const
     double dvar = k_disp_to_disp_ * d;
     double rvar = k_rot_to_rot_   * r;
 
-    return (Matrix3d() << dvar, dvar, rvar).finished();
+    return (Vector2d() << dvar, rvar).finished().asDiagonal();
 }
 
 }

src/sensor/sensor_odom_3d.cpp

@@ -50,7 +50,7 @@ Eigen::Matrix6d SensorOdom3d::computeCovFromMotion (const VectorXd& _data) const
     double dvar = min_disp_var_ + k_disp_to_disp_ * d;
     double rvar = min_rot_var_  + k_disp_to_rot_  * d + k_rot_to_rot_ * r;
 
-    return (Matrix6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished();
+    return (Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
 }
 
 } // namespace wolf