ogreQuaternionAngularDistance does not properly handle invalid quaternion input

[zhihaoshang]

Operating System:

Linux shangzh-VMware-Virtual-Platform 6.11.0-19-generic #19~24.04.1-Ubuntu SMP PREEMPT_DYNAMIC Mon Feb 17 11:51:52 UTC 2 x86_64 x86_64 x86_64 GNU/Linux

ROS version or commit hash:

ros 2 jazzy

RMW implementation (if applicable):

No response

RMW Configuration (if applicable):

No response

Client library (if applicable):

rviz

'ros2 doctor --report' output

ros2 doc --report

   NETWORK CONFIGURATION
inet         : 127.0.0.1
inet4        : ['127.0.0.1']
inet6        : ['::1']
netmask      : 255.0.0.0
device       : lo
flags        : 73<LOOPBACK,UP,RUNNING>
mtu          : 65536
inet         : 192.168.148.137
inet4        : ['192.168.148.137']
ether        : 00:0c:29:be:c8:19
inet6        : ['fe80::20c:29ff:febe:c819%ens33']
netmask      : 255.255.255.0
device       : ens33
flags        : 4163<UP,RUNNING,MULTICAST,BROADCAST>
mtu          : 1500
broadcast    : 192.168.148.255

   PLATFORM INFORMATION
system           : Linux
platform info    : Linux-6.11.0-19-generic-x86_64-with-glibc2.39
release          : 6.11.0-19-generic
processor        : x86_64

   QOS COMPATIBILITY LIST
compatibility status    : No publisher/subscriber pairs found

   RMW MIDDLEWARE
middleware name    : rmw_fastrtps_cpp

   TOPIC LIST
topic               : none
publisher count     : 0
subscriber count    : 0

Steps to reproduce issue

Environment

OS Version: Ubuntu 24.04
rviz version: ros2 jazzy
Compiler name and version number: Ubuntu clang version 18.1.3
Source or binary build?
source build
build options: --mixin asan-gcc

Description

The function rviz_default_plugins::ogreQuaternionAngularDistance does not validate the input quaternions. When invalid quaternions are passed in (e.g., quaternions with zero norm such as (0, 0, 0, 0)), the function performs the computation and silently returns 0.0f without throwing any exception or error.

This behavior may conceal logical errors in upstream code and negatively impacts the robustness and safety of the system.

Test Case

#include <gmock/gmock.h>
#include <OgreMatrix3.h>
#include <OgreQuaternion.h>
#include <OgreVector.h>
#include "../../../../src/rviz_default_plugins/displays/odometry/quaternion_helper.hpp"
using namespace ::testing;  // NOLINT

TEST(QuaternionHelper, ogreQuaternionAngularDistance_handles_zero_product_w) {
  Ogre::Quaternion quaternion1(0.0f, 0.0f, 0.0f, 0.0f);
  Ogre::Quaternion quaternion2(0.0f, 0.0f, 0.0f, 0.0f);
  EXPECT_THROW(
    rviz_default_plugins::ogreQuaternionAngularDistance(quaternion1, quaternion2),
    std::runtime_error);
}

Output

[==========] Running 1 test from 1 test suite.
[----------] Global test environment set-up.
[----------] 1 test from QuaternionHelper
[ RUN      ] QuaternionHelper.ogreQuaternionAngularDistance_handles_zero_product_w
/home/shangzh/ros2_jazzy/src/ros2/rviz/rviz_default_plugins/test/rviz_default_plugins/displays/odometry/quaternion_helper_test.cpp:11: Failure
Expected: rviz_default_plugins::ogreQuaternionAngularDistance(quaternion1, quaternion2) throws an exception of type std::runtime_error.
  Actual: it throws nothing.

[  FAILED  ] QuaternionHelper.ogreQuaternionAngularDistance_handles_zero_product_w (0 ms)
[----------] 1 test from QuaternionHelper (0 ms total)

[----------] Global test environment tear-down
[==========] 1 test from 1 test suite ran. (0 ms total)
[  PASSED  ] 0 tests.
[  FAILED  ] 1 test, listed below:
[  FAILED  ] QuaternionHelper.ogreQuaternionAngularDistance_handles_zero_product_w

 1 FAILED TEST

Expected behavior

When the function receives invalid quaternions (e.g., with zero or near-zero norm), it should detect the invalid input and throw a std::runtime_error to notify the caller that the input is not valid.

Actual behavior

Even when both input quaternions are (0.0f, 0.0f, 0.0f, 0.0f), the function completes execution and returns 0.0f, without any warning or exception. This violates basic input validation practices and misleads the user into believing the computation was successful.

Additional information

Suggested Fix

float ogreQuaternionAngularDistance(Ogre::Quaternion first, Ogre::Quaternion second)
{
  const float epsilon = 1e-6f;
  if (first.norm() < epsilon || second.norm() < epsilon) {
    throw std::runtime_error("Invalid quaternion: norm is zero or too small");
  }

  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));

  return 2.0f * atan2f(imaginary_norm, sqrtf(powf(product.w, 2.0f)));
}