Fix Swapped TFs Axes #2684

SamerKhshiboun · 2023-04-05T16:50:50Z

Fixing issues #2500 #2637 #2583
These issues related to static TFs and Extrinsic

Background and Terminology:

Point of view: Imagine we are standing behind of the camera, and looking forward.
ROS2 Coordinates System (X: Forward, Y:Left, Z: Up). Everything related to TFs should be calculated in this coordination system.
Camera Optical Coordinates System (X: Right, Y: Down, Z: Forward). Everything related to extrinsics should be calculated in this coordination system.
Static TF msg expresses a transform from coordinate frame header.frame_id (parent) to the coordinate frame child_frame_id Reference
Extrinsic from sensor A to sensor B means the position and orientation of sensor A relative to sensor B
As you see, TF(A->B) translation should be the inversion of Extrinsic(A->B)
In a previous PR, we fixed the Extrinsic calculation, which made the /camera/extrinsics/<A_TO_B> correct.
For example, depth_to_color, in D435i:
If we look from behind of the D435i, extrinsic from depth to color, means, where is the depth in relative to the color.
If we just look at X coordinates (again, from behind), we can say that Depth is on the right of RGB (color) sensor by 0.0148m (1.48cm), which is correct.

administrator@perclnx466 ~/ros2_humble $ ros2 topic echo /camera/extrinsics/depth_to_color
rotation:
...
translation:
- 0.01485931035131216
- 0.0010161789832636714
- 0.0005317096947692335
---

The problem we are trying to fix in this PR, is that when publishing this data as TF message, in the ROS2 coordinates system, we forgot to invert the extrinsic data. Please read the top of this description again if you still confused why we need to invert extrinsic data when we are moving from Optical coordinates system to ROS2 coordinates system.

Attaching rviz2 screenshots that describe the change.

Before the fix:

You can see that INFRA1 is in the right side, and INFRA2 is in the left, while we are looking from behind of the camera, and this is wrong !

After the fix:

guoqingzh · 2023-04-06T07:02:02Z

@SamerKhshiboun how about IMU extrinsic ? Are they following the Camera Optical Coordinate System as well ?

Gilaadb · 2023-04-09T11:21:09Z

realsense2_camera/src/base_realsense_node.cpp

+    // Invert x,y,z of extrinsic translation before sending this as TF translation
+    // because extrinsic from A to B is the position of A relative to B
+    // while TF from A to B is the transformation to be done on A to get to B
+    float3 trans{-ex.translation[0], -ex.translation[1], -ex.translation[2]};


What about the rotation?
I think a more correct way to handle it would be to replace line #906: ex = base_profile.get_extrinsics_to(profile);
with: ex = profile.get_extrinsics_to(base_profile);

In fact, I see it was that way before a PR from september 2022: 61406af

Maybe that PR was a mistake or did it solve a real issue?

After discussing this offline, I understood why the change was introduced in the first place. Samer will check if the "get_extrinsics_to()" function call is "heavy" and if not, we might have one for publishing to the extrinsic topic and another one with opposite order of frames to publish to the tf topic.

We will have to explain it well in the code, hard to understand why extrinsic msg and static_tf should behave differently.

A quick example on D435i why we needed the change of #2477:

Before the fix of PR #2477:

rotation: - 0.9999583959579468 - -0.008895229548215866 - 0.0020131953060626984 - 0.008895332925021648 - 0.9999604225158691 - -4.254872692399658e-05 - -0.0020127370953559875 - 6.045500049367547e-05 - 0.9999979734420776 translation: - -0.0148666612803936 - -0.0008839939837343991 - -0.0005615801201201975

After the fix of PR #2477:

rotation: - 0.9999583959579468 - 0.008895332925021648 - -0.0020127370953559875 - -0.008895229548215866 - 0.9999604225158691 - 6.045500049367547e-05 - 0.0020131953060626984 - -4.254872692399658e-05 - 0.9999979734420776 translation: - 0.01485931035131216 - 0.0010161789832636714 - 0.0005317096947692335

To make sure which result is the right one, all we need is to compare it to rs-enumare-devices -c while running the same camera. Somewhere in the output we get this:

Extrinsic from "Depth" To "Color" : Rotation Matrix: 0.999958 -0.00889523 0.0020132 0.00889533 0.99996 -4.25487e-05 -0.00201274 6.0455e-05 0.999998 Translation Vector: 0.0148593103513122 0.00101617898326367 0.000531709694769233

And that makes sense with the new result after the fix.
If we "imagine" that color sensor is (0,0,0), then Depth is on the right of this sensor by 0.01485 m (~1.5 cm), and this is the result that should be published in the relevant topic of the
/camera/extrinsics/depth_to_color

Now, the next explanation is to elaborate more why we need in addition to calculate extrinsic in the opposite way as @Gilaadb suggest.

The first extrinsic we calculated above, was used to publish the depth_to_color topic.
As I described, and as you see the results, it shows where is "Depth" in the eyes of the "Color".

To calculate TF from depth to color, we need the something opposite: how to go from Depth To Color ?
We can manipulate the Extrinsic result we already calculated above, and do some inversion, to get the right direction for TFs, but as Gilad suggested, it may also be complicated a little bit, especially with the ROTATION matrix, so he suggested to calculated two things, instead of inverting the rotation-matrix/translation:

ex1 = base_profile.get_extrinsics_to(profile); // will be used for depth_to_color topic
ex2 = profile.get_extrinsics_to(base_profile); // will be used for the Depth To Color TF information

@Gilaadb ,

After a deeper investigation and testing, I would prefer calculating the extrinsic only once, publish it to the extrinsic topic, and then to manipulate it (invert Matrix and Translation-Vector) for TF topics.

I tested with ex1 and ex2, and it looks like it add some time to fetch the extrinsic in both ways, as it goes and enumerate on all extrinsic of the camera (twice).
But I learned from that, that also the Rotation Matrix should be flipped (inverted), so I will fix tomorrow and update the code.

so for example, we get today:

x: 0.004447687417268753 y: -2.575120197434444e-05 z: -0.0010064936941489577 w: 0.9999895691871643

but it should be:

rotation: x: -0.004447687417268753 y: 2.575120197434444e-05 z: 0.0010064936941489577 w: 0.9999895691871643

so, we have to invert X,Y,Z also in the Rotation, and I'm on it to find the best way for doing that.

SamerKhshiboun · 2023-04-09T22:55:36Z

Pushed a new commit that fixes the Matrix rotation also.

I just saw something weird to me... maybe related to calibration info inside the camera
from rs-enumerate-device

Extrinsic from "Color"    To      "Depth" :
 Rotation Matrix:
   0.999958         0.00889533      -0.00201274
  -0.00889523       0.99996          6.0455e-05
   0.0020132       -4.25487e-05      0.999998

 Translation Vector: -**0.0148666612803936**  **-0.000883993983734399**  **-0.000561580120120198**


Extrinsic from "Depth"    To      "Color" :
 Rotation Matrix:
   0.999958        -0.00889523       0.0020132
   0.00889533       0.99996         -4.25487e-05
  -0.00201274       6.0455e-05       0.999998

 Translation Vector: **0.0148593103513122**  **0.00101617898326367**  **0.000531709694769233**

why does there are small differences between some translation vector values ? (except they are inverted)
for example TranslationVector1.y = -0.000883993983734399
while TranslationVector2.y = 0.00101617898326367

Gilaadb · 2023-04-10T07:02:35Z

why does there are small differences between some translation vector values ? (except they are inverted) for example TranslationVector1.y = -0.000883993983734399 while TranslationVector2.y = 0.00101617898326367

That's because we also need to account for the rotation. The correct way to calculate it is to invert the matrix (calculated in matlab):
A =

0.999958000000000 0.008895330000000 -0.002012740000000 -0.014866661280394
-0.008895230000000 0.999960000000000 0.000060455000000 -0.000883993983734
0.002013200000000 -0.000042548700000 0.999998000000000 -0.000561580120120
0 0 0 1.000000000000000

B = inv(A)

B =

0.999958820147486 -0.008895233841980 0.002013198903423 0.014859316312756
0.008895341189744 0.999960869629048 -0.000042548710445 0.001016179522653
-0.002012742637003 0.000060455040734 0.999997945213466 0.000531709645056
0 0 0 1.000000000000000

…ation matrix

Nir-Az · 2023-04-10T08:36:03Z

realsense2_camera/src/base_realsense_node.cpp

@@ -865,9 +865,13 @@ void BaseRealSenseNode::publish_static_tf(const rclcpp::Time& t,
    msg.header.stamp = t;
    msg.header.frame_id = from;
    msg.child_frame_id = to;
+
+    // Convert x,y,z (taken from camera extrinsics)
+    // from optical cooridnates to ros coordinates
    msg.transform.translation.x = trans.z;


Let's verify if IMU coordinates need to change to match this (like in PR #2680 )

After a deeper check, and after review with @Gilaadb, no need for the PR #2680, since all IMU frames and IMU optical frames are looking good.

SamerKhshiboun · 2023-04-10T09:02:44Z

Ok, so I used two extrinsic at the end, as it being called only once for each stream in our code.
This saves dealing with rotation matrix calculations/inversion

SamerKhshiboun · 2023-04-10T14:24:05Z

waiting for @guoqingzh review to confirm this PR code.

guoqingzh · 2023-04-10T15:36:22Z

realsense2_camera/src/base_realsense_node.cpp

+    // so, we need to calculate extrinsics in two opposite ways, one for extrinsic topic
+    // and the second is for transformation topic (TF)
+    rs2_extrinsics normal_ex;  // used to for extrinsics topic
+    rs2_extrinsics tf_ex; // used for TF


Feel like a proper way is to define a class called rs2_ros_tf , the class should have a constructor which takes rs2_extrinsics as input , the conversion should be done inside the constructor. Current fix defines both normal_ex and tf_ex as rs2_extrinsics but the tf_ex is actually ros tf

The problem is that doing the conversion/transformation inside the class you are suggesting, requires inverting the rotation matrix, and I found it complex to do that.
Instead, I called the extrinsic API of librealsense twice, in two opposite ways, and that solved the problem.

@guoqingzh can you please first verify you agree with the outputs?
We focus on Extrinsics + static tf of the video + IMU streams.
Any coding suggestions are always welcome but should not block this PR.

guoqingzh

have to block the PR, because I think the static_tf for IMU is wrong. And also I don't understand the transformation in line 934

guoqingzh · 2023-04-10T18:25:08Z

realsense2_camera/src/base_realsense_node.cpp

@@ -934,8 +949,6 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile&
        publish_static_tf(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID);
        publish_static_tf(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID);


I think this is wrong. The IMU data published by libRS is already aligned with depth CS (x-right, y-down, z-front) according to the code-> https://github.com/IntelRealSense/librealsense/blob/master/src/proc/motion-transform.cpp#L91
The rotation from IMU_FRAME_ID to IMU_OPTICAL_FRAME_ID should be identity matrix IMO

@Nir-Az @SamerKhshiboun notified you set IMU frame_id to IMU_OPTICAL_FRAME_ID by default (BaseRealSenseNode::ImuMessage_AddDefaultValues()), so i think it is ok as well

guoqingzh · 2023-04-10T18:27:09Z

realsense2_camera/src/base_realsense_node.cpp

-    float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]};
+    // publish static TF
+    auto Q = rotationMatrixToQuaternion(tf_ex.rotation);
+    Q = quaternion_optical * Q * quaternion_optical.inverse();


what are you trying to achieve with this line ? qpq-1 is meant for transformation a vector from CS A to CS B but you have Q in the middle, which is a rotation quaternion. I don't understand the math here. You have relative rotation from tf_ex already, why you need to rotate it again with the quaternion_optical ? Isn't that the tf_ex already assume optical frame ?

Actually I did not add this line, it was there for a couple of years.
see this commit:
1cd63b6

and it was working all the time.
I don't know yet if it necessary or not.

@guoqingzh This added by PR #383 , please review it

Could be that Q here is acctually P from the original equation

@Nir-Az As your screenshot told, the expression rotates vector quaternion (not rotation quaternion). I read the original PR#383, I think the original author is trying to do a rotation combination here. In our code, Q is optical to optical , which can't be used directly in ros tf so he has to rotate first from ros to optical (quaternion_optical inverse), then apply Q, after that apply quaternion_optical again to rotate from optical back to ros, i think it is ok

guoqingzh

IMU messgae is associated with IMU optical frame and quaternion_opticalQquaternion_optical.inv() is a rotation combination rather than vector transformation. I will approve this

Nir-Az

Excellent work

SamerKhshiboun requested review from ev-mp, MartyG-RealSense, Nir-Az, Gilaadb and benlev April 5, 2023 16:50

SamerKhshiboun force-pushed the invert_axes_of_tfs branch from a5a62e4 to 149ebf3 Compare April 5, 2023 17:17

SamerKhshiboun changed the title ~~invert translation~~ Fix Swapped TFs Axes Apr 5, 2023

invert translation

81b49ee

SamerKhshiboun force-pushed the invert_axes_of_tfs branch from 149ebf3 to 81b49ee Compare April 5, 2023 17:19

SamerKhshiboun marked this pull request as ready for review April 5, 2023 17:20

Nir-Az requested a review from guoqingzh April 6, 2023 07:02

This was referenced Apr 6, 2023

Static transforms are inverted #2637

Closed

Static TFs appear inverted #2583

Closed

Gilaadb reviewed Apr 9, 2023

View reviewed changes

fix matrix rotation

1e08819

calculate extriniscs twice in two opposite ways to save inverting rot…

d171559

…ation matrix

Nir-Az reviewed Apr 10, 2023

View reviewed changes

Nir-Az removed request for benlev, ev-mp and MartyG-RealSense April 10, 2023 08:43

SamerKhshiboun mentioned this pull request Apr 10, 2023

fix imu topic frame_id with came_name ns #2680

Closed

SamerKhshiboun added 3 commits April 10, 2023 17:12

Merge branch 'ros2-development' into invert_axes_of_tfs

3c35930

Merge branch 'ros2-development' into invert_axes_of_tfs

daba7e5

update readme

ebacd30

guoqingzh suggested changes Apr 10, 2023

View reviewed changes

guoqingzh approved these changes Apr 11, 2023

View reviewed changes

Nir-Az approved these changes Apr 12, 2023

View reviewed changes

Nir-Az merged commit 54c7519 into IntelRealSense:ros2-development Apr 12, 2023

MartyG-RealSense mentioned this pull request Apr 12, 2023

Why are the axes swapped when publishing static tf? #2500

Closed

SamerKhshiboun deleted the invert_axes_of_tfs branch May 9, 2023 10:47

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix Swapped TFs Axes #2684

Fix Swapped TFs Axes #2684

SamerKhshiboun commented Apr 5, 2023 •

edited

Loading

guoqingzh commented Apr 6, 2023

Gilaadb Apr 9, 2023

Gilaadb Apr 9, 2023

Nir-Az Apr 9, 2023

SamerKhshiboun Apr 9, 2023 •

edited

Loading

SamerKhshiboun Apr 9, 2023 •

edited

Loading

SamerKhshiboun Apr 9, 2023 •

edited

Loading

SamerKhshiboun commented Apr 9, 2023

Gilaadb commented Apr 10, 2023

Nir-Az Apr 10, 2023

SamerKhshiboun Apr 10, 2023

SamerKhshiboun commented Apr 10, 2023

SamerKhshiboun commented Apr 10, 2023

guoqingzh Apr 10, 2023

SamerKhshiboun Apr 10, 2023

Nir-Az Apr 10, 2023

guoqingzh left a comment

guoqingzh Apr 10, 2023

guoqingzh Apr 11, 2023

guoqingzh Apr 10, 2023 •

edited

Loading

SamerKhshiboun Apr 10, 2023

Nir-Az Apr 10, 2023 •

edited

Loading

guoqingzh Apr 11, 2023 •

edited

Loading

guoqingzh left a comment •

edited

Loading

Nir-Az left a comment

		@@ -934,8 +949,6 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile&
		publish_static_tf(transform_ts_, zero_trans, zero_rot_quaternions, FRAME_ID(sip), IMU_FRAME_ID);
		publish_static_tf(transform_ts_, zero_trans, quaternion_optical, IMU_FRAME_ID, IMU_OPTICAL_FRAME_ID);

Fix Swapped TFs Axes #2684

Fix Swapped TFs Axes #2684

Conversation

SamerKhshiboun commented Apr 5, 2023 • edited Loading

Background and Terminology:

Before the fix:

After the fix:

guoqingzh commented Apr 6, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SamerKhshiboun Apr 9, 2023 • edited Loading

Choose a reason for hiding this comment

SamerKhshiboun Apr 9, 2023 • edited Loading

Choose a reason for hiding this comment

SamerKhshiboun Apr 9, 2023 • edited Loading

Choose a reason for hiding this comment

SamerKhshiboun commented Apr 9, 2023

Gilaadb commented Apr 10, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SamerKhshiboun commented Apr 10, 2023

SamerKhshiboun commented Apr 10, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

guoqingzh left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

guoqingzh Apr 10, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Nir-Az Apr 10, 2023 • edited Loading

Choose a reason for hiding this comment

guoqingzh Apr 11, 2023 • edited Loading

Choose a reason for hiding this comment

guoqingzh left a comment • edited Loading

Choose a reason for hiding this comment

Nir-Az left a comment

Choose a reason for hiding this comment

SamerKhshiboun commented Apr 5, 2023 •

edited

Loading

SamerKhshiboun Apr 9, 2023 •

edited

Loading

SamerKhshiboun Apr 9, 2023 •

edited

Loading

SamerKhshiboun Apr 9, 2023 •

edited

Loading

guoqingzh Apr 10, 2023 •

edited

Loading

Nir-Az Apr 10, 2023 •

edited

Loading

guoqingzh Apr 11, 2023 •

edited

Loading

guoqingzh left a comment •

edited

Loading