ZED X Downward-Facing Camera — IMU Gimbal Lock in RPY

Software ROS2
1

Hi team,

I’m using a ZED X camera mounted facing straight down on a ground vehicle (ROS 2 Humble). I wanted to share an issue I ran into with IMU orientation data

When subscribing to /zed/zed_node/imu/data and converting the quaternion to roll/pitch/yaw using the standard ZYX formula, I was seeing:

Roll:  -31.2°   Pitch:  89.2°   Yaw:  -31.7°
Roll:  -36.1°   Pitch:  89.2°   Yaw:  -36.7°
Roll:  -25.4°   Pitch:  89.2°   Yaw:  -26.0°

Roll and Yaw were nearly identical and oscillating wildly between -25° and -39°, while Pitch was locked at ~89°..

angular_velocity:
x: -0.0037372470899413083
y: 0.0068473275281483535
z: -0.0007464583917411008
angular_velocity_covariance:

  • 5.105498603087797e-06

  • 0.0

  • 0.0

  • 0.0

  • 0.00010884288259745603

  • 0.0

  • 0.0

  • 0.0

  • 7.027000590363156e-06
    linear_acceleration:
    x: -9.861936569213867
    y: -0.05002943426370621
    z: 0.037796176970005035

    these are zed imu output and thye camera is facing downwards ..

    is there any way i can convert quaternion to rpy without the gimbal lock issue ??

    please help with the issue …

2

Hi,

Thank you for reaching out !

In ROS 2 Humble, the usual C++ path to do such a transformation without the gimbal lock issue is to convert the geometry_msgs::msg::Quaternion into tf2::Quaternion, normalize it, then use tf2::Matrix3x3(q).getRPY(...):

#include <geometry_msgs/msg/quaternion.hpp>#include <tf2/LinearMath/Quaternion.hpp>#include <tf2/LinearMath/Matrix3x3.hpp>#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>void quatToRPY(const geometry_msgs::msg::Quaternion & q_msg, double & roll, double & pitch, double & yaw){ tf2::Quaternion q; tf2::fromMsg(q_msg, q); q.normalize(); // recommended tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);}

Hope that helps !

Best

Stereolabs Support

1 Like