Coordinate System Issue with camera_color_frame and camera_color_optical_frame in Femto Bolt 1.5.7

[wataru-okumura]

Thanks as always. I have a question about the camera_color_frame tf. I am using femto bolt 1.5.7, The coordinates of the camera_color_frame and camera_color_optical_frames appear to be incorrect.

In one terminal, run the following ros2 launch orbbec_camera femto_bolt.launch.py

In another terminal, launch rviz2 to display tf. rviz2

Specifically, I think the camera_color_frame is tilted 6 degrees above the camera_depth frame, However, when I check with rviz2, the color frame is tilted below the depth frame. Also, since the optical center is tilted downwards, the tip of the optical center is elevated, so the camera_depth_optical_frame should be above the camera_color_optical_frame, but rviz2 shows that the height of the optical center is the same. However, rviz2 shows that the height of the two frames are the same.

Can you please check if the coordinates of camera_color_frame and camera_color_optical_frame are correct?

[Danilrivero]

Checking the STL files for the camera there may be an issue as you state.

Accessing the extrinsics (depth_to_color) of the camera seems to imply this too:

Yaw (ψ): 0.3652 degrees
Pitch (θ): -0.0131 degrees
Roll (φ): -6.0724 degrees

Checking the tf_tree we have the following:

The _camera_depthframe may need to be the one actually rotated in relation to _cameralink, and _camera_colorframe contain a translation and positive roll value when checking the extrinsics. Correct me if wrong.

The frames have not changed in 1.58 either.

[kjjpc]

The issue may be caused by this function. https://github.com/orbbec/OrbbecSDK_ROS2/blob/main/orbbec_camera/src/utils.cpp#L200-L203

The function assumes the rotation matrix is column-major, but the extrinsic parameters obtained from Orbbec SDK is row-major. We should fix by following.

-  m << rotation[0], rotation[3], rotation[6], rotation[1], rotation[4], rotation[7], rotation[2],
-      rotation[5], rotation[8];
+  m << rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6],
+      rotation[7], rotation[8];

[Danilrivero]

The issue may be caused by this function. https://github.com/orbbec/OrbbecSDK_ROS2/blob/main/orbbec_camera/src/utils.cpp#L200-L203

The function assumes the rotation matrix is column-major, but the extrinsic parameters obtained from Orbbec SDK is row-major. We should fix by following.

-  m << rotation[0], rotation[3], rotation[6], rotation[1], rotation[4], rotation[7], rotation[2],
-      rotation[5], rotation[8];
+  m << rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6],
+      rotation[7], rotation[8];

Upon inspecting I have done as you state:

It seems like it could be solved by doing so. I do still feel that being objective to the STL file, the _baselink should be looking forward. depth would have the actual rotation in relation to it's parent (_baselink), and _colorframe only contain a translation in relation to it's parent (_baselink) once again.

I also wanted to note the fact that the axis are slightly unaligned as well, as stated in my last message:

Yaw (ψ): 0.3652 degrees Pitch (θ): -0.0131 degrees Roll (φ): -6.0724 degrees

I do not know if there is any reason for this.

@jian-dong could you possibly look into this?

[wataru-okumura]

@jian-dong Thanks for merging it into main branch. I will close the issue now that it is resolved.