Fix Issue with Quaternion Angular Distance (#1473)

Original issue here:
#1381

- Add new implementation for calculating angular distance between
  quaternions suggested by issue author, but include additional step of
  normalizing quaternions prior to the dot product operation
- Add gmock test to verify that new implementation works even with small
  angular distances, e.g. 1.0e-6
- Decrease tolerance on expected values of angle distance calculations
  in similar tests, for parity

Author: chen-harrison

rviz_default_plugins/src/rviz_default_plugins/displays/odometry/quaternion_helper.hpp

@@ -40,11 +40,12 @@ namespace rviz_default_plugins
 
 float ogreQuaternionAngularDistance(Ogre::Quaternion first, Ogre::Quaternion second)
 {
-  Ogre::Quaternion product = first * Ogre::Quaternion(second.w, -second.x, -second.y, -second.z);
-  float imaginary_norm =
-    sqrtf(powf(product.x, 2.0f) + powf(product.y, 2.0f) + powf(product.z, 2.0f));
+  first.normalise();
+  second.normalise();
+  float dot = first.Dot(second);
+  dot = std::clamp(dot, -1.0f, 1.0f);
 
-  return 2.0f * atan2f(imaginary_norm, sqrtf(powf(product.w, 2.0f)));
+  return 2.0f * acosf(fabs(dot));
 }
 
 }  // namespace rviz_default_plugins

rviz_default_plugins/test/rviz_default_plugins/displays/odometry/quaternion_helper_test.cpp

@@ -49,16 +49,21 @@ TEST(QuaternionHelper, ogreQuaternionHelper_returns_angle) {
   Ogre::Vector3 axis(1, 2, 3);
   axis.normalise();
   Ogre::Quaternion quaternion1;
-  quaternion1.FromAngleAxis(Ogre::Radian(0), axis);
+  quaternion1.FromAngleAxis(Ogre::Radian(0.0f), axis);
   Ogre::Quaternion quaternion2;
   quaternion2.FromAngleAxis(Ogre::Radian(0.5f), axis);
   Ogre::Quaternion quaternion3;
   quaternion3.FromAngleAxis(Ogre::Radian(1.2f), axis);
+  Ogre::Quaternion quaternion4;
+  quaternion4.FromAngleAxis(Ogre::Radian(1.0e-6f), axis);
 
   EXPECT_THAT(
     rviz_default_plugins::ogreQuaternionAngularDistance(quaternion1, quaternion2),
-    FloatNear(0.5f, 0.001f));
+    FloatNear(0.5f, 1.0e-6f));
   EXPECT_THAT(
     rviz_default_plugins::ogreQuaternionAngularDistance(quaternion1, quaternion3),
-    FloatNear(1.2f, 0.001f));
+    FloatNear(1.2f, 1.0e-6f));
+  EXPECT_THAT(
+    rviz_default_plugins::ogreQuaternionAngularDistance(quaternion1, quaternion4),
+    FloatNear(1.0e-6f, 1.0e-6f));
 }