From 6d9c4c2fba5f06391a562d81bb0b9c25218ee9a0 Mon Sep 17 00:00:00 2001
From: Idril Geer <igeer@iri.upc.edu>
Date: Tue, 9 Feb 2021 17:21:04 +0100
Subject: [PATCH] hotfix: Fix motion covariance in SensorOdom2d and
SensorOdom3d
---
include/core/sensor/sensor_odom_2d.h | 2 +-
src/sensor/sensor_odom_2d.cpp | 4 ++--
src/sensor/sensor_odom_3d.cpp | 2 +-
3 files changed, 4 insertions(+), 4 deletions(-)
diff --git a/include/core/sensor/sensor_odom_2d.h b/include/core/sensor/sensor_odom_2d.h
index ca06c7c01..0859f15bc 100644
--- a/include/core/sensor/sensor_odom_2d.h
+++ b/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;
};
diff --git a/src/sensor/sensor_odom_2d.cpp b/src/sensor/sensor_odom_2d.cpp
index a3fe8ee9a..c853426d8 100644
--- a/src/sensor/sensor_odom_2d.cpp
+++ b/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();
}
}
diff --git a/src/sensor/sensor_odom_3d.cpp b/src/sensor/sensor_odom_3d.cpp
index 5954f505c..b261029d6 100644
--- a/src/sensor/sensor_odom_3d.cpp
+++ b/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
--
GitLab