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

Show file tree
Hide file tree
Changes from all commits
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
4 changes: 3 additions & 1 deletion README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -300,7 +300,9 @@ The `/diagnostics` topic includes information regarding the device temperatures

- ROS2 Coordinate System: (X: Forward, Y:Left, Z: Up)
- Camera Optical Coordinate System: (X: Right, Y: Down, Z: Forward)
- References: [REP-0103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions) [REP-0105](https://www.ros.org/reps/rep-0105.html#coordinate-frames)
- References: [REP-0103](https://www.ros.org/reps/rep-0103.html#coordinate-frame-conventions), [REP-0105](https://www.ros.org/reps/rep-0105.html#coordinate-frames)
- All data published in our wrapper topics is optical data taken directly from our camera sensors.
- static and dynamic TF topics publish optical CS and ROS CS to give the user the ability to move from one CS to other CS.

<hr>

Expand Down
33 changes: 23 additions & 10 deletions 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 @@ -896,31 +900,42 @@ void BaseRealSenseNode::calcAndPublishStaticTransform(const rs2::stream_profile&

rclcpp::Time transform_ts_ = _node.now();

rs2_extrinsics ex;
// extrinsic from A to B is the position of A relative to B
// TF from A to B is the transformation to be done on A to get to B
// 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
Copy link

Choose a reason for hiding this comment

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

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

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

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.

Copy link
Collaborator

Choose a reason for hiding this comment

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

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


try
{
ex = base_profile.get_extrinsics_to(profile);
normal_ex = base_profile.get_extrinsics_to(profile);
tf_ex = profile.get_extrinsics_to(base_profile);
}
catch (std::exception& e)
{
if (!strcmp(e.what(), "Requested extrinsics are not available!"))
{
ROS_WARN_STREAM("(" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << ") -> (" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << "): " << e.what() << " : using unity as default.");
ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}});
ROS_WARN_STREAM("(" << rs2_stream_to_string(base_profile.stream_type()) << ", " << base_profile.stream_index() << ") -> (" << rs2_stream_to_string(profile.stream_type()) << ", " << profile.stream_index() << "): " << e.what() << " : using unity as default.");
normal_ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}});
tf_ex = normal_ex;
}
else
{
throw e;
}
}

auto Q = rotationMatrixToQuaternion(ex.rotation);
Q = quaternion_optical * Q * quaternion_optical.inverse();
// publish normal extrinsics e.g. /camera/extrinsics/depth_to_color
publishExtrinsicsTopic(sip, normal_ex);

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();
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{tf_ex.translation[0], tf_ex.translation[1], tf_ex.translation[2]};
publish_static_tf(transform_ts_, trans, Q, _base_frame_id, FRAME_ID(sip));

// Transform stream frame to stream optical frame
// Transform stream frame to stream optical frame and publish it
publish_static_tf(transform_ts_, zero_trans, quaternion_optical, FRAME_ID(sip), OPTICAL_FRAME_ID(sip));

if (profile.is<rs2::video_stream_profile>() && profile.stream_type() != RS2_STREAM_DEPTH && profile.stream_index() == 1)
Expand All @@ -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);
Copy link

Choose a reason for hiding this comment

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

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

Copy link

Choose a reason for hiding this comment

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

@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

}

publishExtrinsicsTopic(sip, ex);
}

void BaseRealSenseNode::SetBaseStream()
Expand Down