From 08409cd1649d4e002f6804505a461e75826d9928 Mon Sep 17 00:00:00 2001
From: Paul Gesel <paulgesel@gmail.com>
Date: Tue, 23 Jan 2024 19:28:33 -0700
Subject: [PATCH] normalize quaternions

Signed-off-by: Paul Gesel <paulgesel@gmail.com>
---
 test_tf2/test/test_convert.cpp            | 13 +++++----
 tf2_eigen/include/tf2_eigen/tf2_eigen.hpp | 32 ++++++++++++++---------
 2 files changed, 28 insertions(+), 17 deletions(-)

diff --git a/test_tf2/test/test_convert.cpp b/test_tf2/test/test_convert.cpp
index 432bbfe5d..6e15bba77 100644
--- a/test_tf2/test/test_convert.cpp
+++ b/test_tf2/test/test_convert.cpp
@@ -106,15 +106,18 @@ TEST(tf2Convert, kdlBulletROSConversions)
 
 TEST(tf2Convert, ConvertTf2Quaternion)
 {
-  const tf2::Quaternion tq(1, 2, 3, 4);
+  double epsilon = 1e-9;
+
+  tf2::Quaternion tq(1, 2, 3, 4);
+  tq.normalize();
   Eigen::Quaterniond eq;
   // TODO(gleichdick): switch to tf2::convert() when it's working
   tf2::fromMsg(tf2::toMsg(tq), eq);
 
-  EXPECT_EQ(tq.w(), eq.w());
-  EXPECT_EQ(tq.x(), eq.x());
-  EXPECT_EQ(tq.y(), eq.y());
-  EXPECT_EQ(tq.z(), eq.z());
+  EXPECT_NEAR(tq.w(), eq.w(), epsilon);
+  EXPECT_NEAR(tq.x(), eq.x(), epsilon);
+  EXPECT_NEAR(tq.y(), eq.y(), epsilon);
+  EXPECT_NEAR(tq.z(), eq.z(), epsilon);
 }
 
 TEST(tf2Convert, PointVectorDefaultMessagetype)
diff --git a/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp
index e388de69d..da5ee6057 100644
--- a/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp
+++ b/tf2_eigen/include/tf2_eigen/tf2_eigen.hpp
@@ -52,9 +52,11 @@ namespace tf2
 inline
 Eigen::Isometry3d transformToEigen(const geometry_msgs::msg::Transform & t)
 {
+  Eigen::Quaterniond quat(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z);
+  quat.normalize();
   return Eigen::Isometry3d(
     Eigen::Translation3d(t.translation.x, t.translation.y, t.translation.z) *
-    Eigen::Quaterniond(t.rotation.w, t.rotation.x, t.rotation.y, t.rotation.z));
+    quat);
 }
 
 /** \brief Convert a timestamped transform to the equivalent Eigen data type.
@@ -79,7 +81,8 @@ geometry_msgs::msg::TransformStamped eigenToTransform(const Eigen::Affine3d & T)
   t.transform.translation.y = T.translation().y();
   t.transform.translation.z = T.translation().z();
 
-  Eigen::Quaterniond q(T.linear());  // assuming that upper 3x3 matrix is orthonormal
+  Eigen::Quaterniond q(T.linear());
+  q.normalize();
   t.transform.rotation.x = q.x();
   t.transform.rotation.y = q.y();
   t.transform.rotation.z = q.z();
@@ -330,6 +333,7 @@ inline
 void fromMsg(const geometry_msgs::msg::Quaternion & msg, Eigen::Quaterniond & out)
 {
   out = Eigen::Quaterniond(msg.w, msg.x, msg.y, msg.z);
+  out.normalize();
 }
 
 /** \brief Apply a geometry_msgs TransformStamped to an Eigen-specific Quaterniond type.
@@ -466,13 +470,15 @@ geometry_msgs::msg::Vector3 toMsg2(const Eigen::Vector3d & in)
 inline
 void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Affine3d & out)
 {
+  Eigen::Quaterniond quat(
+    msg.orientation.w,
+    msg.orientation.x,
+    msg.orientation.y,
+    msg.orientation.z);
+  quat.normalize();
   out = Eigen::Affine3d(
     Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
-    Eigen::Quaterniond(
-      msg.orientation.w,
-      msg.orientation.x,
-      msg.orientation.y,
-      msg.orientation.z));
+    quat);
 }
 
 /** \brief Convert a Pose message transform type to a Eigen Isometry3d.
@@ -483,13 +489,15 @@ void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Affine3d & out)
 inline
 void fromMsg(const geometry_msgs::msg::Pose & msg, Eigen::Isometry3d & out)
 {
+  Eigen::Quaterniond quat(
+    msg.orientation.w,
+    msg.orientation.x,
+    msg.orientation.y,
+    msg.orientation.z);
+  quat.normalize();
   out = Eigen::Isometry3d(
     Eigen::Translation3d(msg.position.x, msg.position.y, msg.position.z) *
-    Eigen::Quaterniond(
-      msg.orientation.w,
-      msg.orientation.x,
-      msg.orientation.y,
-      msg.orientation.z));
+    quat);
 }
 
 /** \brief Convert a Eigen 6x1 Matrix type to a Twist message.