Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix Swapped TFs Axes #2684

Merged
merged 6 commits into from
Apr 12, 2023
Merged

Fix Swapped TFs Axes #2684

Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
81b49ee
invert translation
SamerKhshiboun Apr 5, 2023
1e08819
fix matrix rotation
SamerKhshiboun Apr 9, 2023
d171559
calculate extriniscs twice in two opposite ways to save inverting rot…
SamerKhshiboun Apr 10, 2023
3c35930
Merge branch 'ros2-development' into invert_axes_of_tfs
SamerKhshiboun Apr 10, 2023
daba7e5
Merge branch 'ros2-development' into invert_axes_of_tfs
SamerKhshiboun Apr 10, 2023
ebacd30
update readme
SamerKhshiboun Apr 10, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 8 additions & 1 deletion realsense2_camera/src/base_realsense_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

msg.transform.translation.y = -trans.x;
msg.transform.translation.z = -trans.y;

msg.transform.rotation.x = q.getX();
msg.transform.rotation.y = q.getY();
msg.transform.rotation.z = q.getZ();
Expand Down Expand Up @@ -917,7 +921,10 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile&
auto Q = rotationMatrixToQuaternion(ex.rotation);
Q = quaternion_optical * Q * quaternion_optical.inverse();
Copy link

@guoqingzh guoqingzh Apr 10, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 ?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Collaborator

@Nir-Az Nir-Az Apr 10, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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

Copy link

@guoqingzh guoqingzh Apr 11, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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


float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]};
// 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]};
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

@SamerKhshiboun SamerKhshiboun Apr 9, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

@SamerKhshiboun SamerKhshiboun Apr 9, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

@SamerKhshiboun SamerKhshiboun Apr 9, 2023 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@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.

publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip));

// Transform stream frame to stream optical frame
Expand Down