From cc741426f2acdf2e65d4083e10986466a6389275 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Wed, 15 Dec 2021 11:48:24 +0100
Subject: [PATCH] Fixed covariance rotation bug
---
src/collision_manager_alg_node.cpp | 5 ++++-
1 file changed, 4 insertions(+), 1 deletion(-)
diff --git a/src/collision_manager_alg_node.cpp b/src/collision_manager_alg_node.cpp
index af20bda..536e18b 100644
--- a/src/collision_manager_alg_node.cpp
+++ b/src/collision_manager_alg_node.cpp
@@ -484,11 +484,14 @@ bool CollisionManagerAlgNode::get_base_link_new_pose_from_icp(const Eigen::Vecto
tf_map_new_blink = tf_map_new_laser*(_front? this->tf_blink_front_laser: this->tf_blink_rear_laser).inverse();
_blink_pose << tf_map_new_blink.getOrigin().x(), tf_map_new_blink.getOrigin().y(), tf2::getYaw(tf_map_new_blink.getRotation());
- Eigen::Quaternion<double> rot(tf_map_laser.getOrigin().w(), tf_map_laser.getOrigin().x(), tf_map_laser.getOrigin().y(), tf_map_laser.getOrigin().z());
+ Eigen::Quaternion<double> rot(tf_map_laser.getRotation().w(), tf_map_laser.getRotation().x(), tf_map_laser.getRotation().y(), tf_map_laser.getRotation().z());
Eigen::Transform<double,3,Eigen::Affine> map_laser_rot(rot);
_blink_cov = map_laser_rot*_covariances;
+ for (unsigned int i=0; i<9; i++)
+ _blink_cov(i) = std::fabs(_blink_cov(i))*(_covariances(i) < 0? -1: 1);
+
if (this->config_.debug)
{
if (_front)
--
GitLab