fix extrinsics calculation

[SamerKhshiboun]

Tracked by LRS-476

Issue #2432

[remibettan]

LGTM

[Nir-Az]

LGTM

[schmidtp1]

i don't agree with this fix. the way "from - to" is used is ambiguous. in librealsense "extrinsics from A to B" refer to the transformation of a coordinates from/ expressed in frame A to coordinates expressed in frame B (which is the same as coordinate frame A expressed with respect to frame B). ROS, on the other hand, seems to refer with "to" rather to the kinematic tree (from "parent frame" to "child frame") which is exactly the opposite direction.

i think the documentation here is a bit misleading: http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html but you can easily test it by publishing:
$ ros2 run tf2_ros static_transform_publisher 0 0 1 0 0 0 1 frame_id child_frame_id
$ ros2 topic echo /tf_static
transforms:

• header:
  stamp:
  sec: 1665789714
  nanosec: 273191209
  frame_id: frame_id
  child_frame_id: child_frame_id
  transform:
  translation:
  x: 0.0
  y: 0.0
  z: 1.0
  rotation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0

and possibly inspecting in rviz.

to transform from "child frame" to "frame" you would add 1. from "frame" to "child frame" subtract 1. (as a simple example.)
please let me know if this makes sense, @SamerKhshiboun, @Nir-Az.

[Nir-Az]

i don't agree with this fix. the way "from - to" is used is ambiguous. in librealsense "extrinsics from A to B" refer to the transformation of a coordinates from/ expressed in frame A to coordinates expressed in frame B (which is the same as coordinate frame A expressed with respect to frame B). ROS, on the other hand, seems to refer with "to" rather to the kinematic tree (from "parent frame" to "child frame") which is exactly the opposite direction.

i think the documentation here is a bit misleading: http://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/TransformStamped.html but you can easily test it by publishing: $ ros2 run tf2_ros static_transform_publisher 0 0 1 0 0 0 1 frame_id child_frame_id $ ros2 topic echo /tf_static transforms:

• header:
  stamp:
  sec: 1665789714
  nanosec: 273191209
  frame_id: frame_id
  child_frame_id: child_frame_id
  transform:
  translation:
  x: 0.0
  y: 0.0
  z: 1.0
  rotation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0

and possibly inspecting in rviz. to transform from "child frame" to "frame" you would add 1. from "frame" to "child frame" subtract 1. (as a simple example.) please let me know if this makes sense, @SamerKhshiboun, @Nir-Az.

Thanks for the feedback @schmidtp1 .
We will take a look at your perspective and investigate this issue further.
Thanks.

[SamerKhshiboun]

Hi @schmidtp1 and thanks for you informative comment

I think you are right.
The documentation of TransformStamped is "This expresses a transform from coordinate frame header.frame_id to the coordinate frame child_frame_id"

And as I see in one of our static TFs message:

header: stamp: sec: 1680527160 nanosec: 457847686 frame_id: camera_depth_frame child_frame_id: camera_depth_optical_frame transform: translation: x: 0.0 y: -0.0 z: -0.0 rotation: x: -0.5 y: 0.4999999999999999 z: -0.5 w: 0.5000000000000001

while frame_id (parent) let it be the ROS2 depth coordinates
and child_frame_id (child) let it be the OPTICAL depth coordinates

so if we want to move from parent to child, which means from ROS2 coordinates to OPTICAL coordinates in this example,
we should use this rotation (+0.5, -0.5, +0.5) which is the opposite values of what we get today in our static TF published message.

I think that now we calculate the extrinsic in a right way (after the PR is merged) and publish them correctly to the corresponding topics (e.g., /camera/extrinsics/depth_to_color).
But as I see, we need to manipulate (turn around) these values before we publish them to the static TF topic, and that can be done easily in our code.

Adding also @Gilaadb, @benlev since they are using TFs and Extrinsic topic so they may can advice more.