wrong optical frame transform in v2

[christian-rauch]

System Environment

• Hardware Info: Please provide system hardware details, such as:
  • CPU architecture (e.g., x86_64, arm64)
  • CPU model (e.g., 13th Gen Intel® Core™ i7-13700 × 24, NVIDIA Jetson Orin)
  • Memory size (e.g., 16GB, 32GB)
  • GPU model if available (e.g., NVIDIA RTX 3060, Integrated GPU)
• Ubuntu Version: Please provide the ubuntu version you are using (e.g., ubuntu20.04, ubuntu22.04, etc.)
• ROS Version: Please provide the version of ROS you are using (e.g., ROS Noetic, ROS 2 Foxy, etc.)
• Camera Model: Femto Bolt
• Firmware Version: latest
• Branch: v2-main
• Commit: Please provide the commit ID you are using (e.g., e2493a2, 7834949)

Description

The transform between the camera_base_link and camera_color_optical_frame are different between v1 and v2:

ros2 launch orbbec_camera femto_bolt.launch.py
ros2 run tf2_ros tf2_echo camera_link camera_color_optical_frame

For v1.5.8:

- Translation: [-0.002, -0.032, -0.001]
- Rotation: in Quaternion (xyzw) [-0.525, 0.526, -0.475, 0.472]
- Rotation: in RPY (radian) [-1.675, -0.002, -1.574]
- Rotation: in RPY (degree) [-95.957, -0.116, -90.167]
- Matrix:
 -0.003 -0.104  0.995 -0.002
 -1.000 -0.002 -0.003 -0.032
  0.002 -0.995 -0.104 -0.001
  0.000  0.000  0.000  1.000

v2.3.4, v2.4.7:

- Translation: [-0.002, -0.032, -0.001]
- Rotation: in Quaternion (xyzw) [-0.500, 0.500, -0.500, 0.500]
- Rotation: in RPY (radian) [-1.571, -0.000, -1.571]
- Rotation: in RPY (degree) [-90.000, -0.000, -90.000]
- Matrix:
 -0.000  0.000  1.000 -0.002
 -1.000 -0.000 -0.000 -0.032
  0.000 -1.000  0.000 -0.001
  0.000  0.000  0.000  1.000

For v2.5.4 and later:

- Translation: [-0.002, -0.032, -0.001]
- Rotation: in Quaternion (xyzw) [-0.474, 0.473, -0.524, 0.527]
- Rotation: in RPY (radian) [-1.467, 0.002, -1.568]
- Rotation: in RPY (degree) [-84.043, 0.099, -89.822]
- Matrix:
  0.003  0.104  0.995 -0.002
 -1.000  0.002  0.003 -0.032
 -0.002 -0.995  0.104 -0.001
  0.000  0.000  0.000  1.000

Note the different rotation and how the signs are switches in the rotation matrix between v1.5.8 and v2.5.4.

For v2, this results in the following wrong transforms:

This results in the following observation of a table plane that is parallel to the camera:

[christian-rauch]

4586c4e introduces a new parameter enable_publish_extrinsic, which is false by default. Setting this to true:

ros2 launch orbbec_camera femto_bolt.launch.py enable_publish_extrinsic:=True

brings back the "nominal" extrinsics:

$ ros2 run tf2_ros tf2_echo camera_link camera_color_optical_frame
[INFO] [1764769878.173682104] [tf2_echo]: Waiting for transform camera_link ->  camera_color_optical_frame: Invalid frame ID "camera_link" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [-0.002, -0.032, -0.001]
- Rotation: in Quaternion (xyzw) [-0.500, 0.500, -0.500, 0.500]
- Rotation: in RPY (radian) [-1.571, -0.000, -1.571]
- Rotation: in RPY (degree) [-90.000, -0.000, -90.000]
- Matrix:
 -0.000  0.000  1.000 -0.002
 -1.000 -0.000 -0.000 -0.032
  0.000 -1.000  0.000 -0.001
  0.000  0.000  0.000  1.000

The meaning of this parameter is quite confusing, as some kind of extrinsics are published in any case. It is just that with enable_publish_extrinsic:=True some kind of "nominal"/"idealised" axis-aligned rotation is published, whereas enable_publish_extrinsic:=False, published non-nominal and incorrect extrinsics. I assume that the extrinsics from v1.5.8 are the correct ones.

[christian-rauch]

The issue lies with rotationMatrixToQuaternion:

OrbbecSDK_ROS2/orbbec_camera/src/utils.cpp

Lines 200 to 208 in 03c487e

	tf2::Quaternion rotationMatrixToQuaternion(const float rotation[9]) {
	Eigen::Matrix3f m;
	// We need to be careful about the order, as RS2 rotation matrix is
	// column-major, while Eigen::Matrix3f expects row-major.
	m << rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6],
	rotation[7], rotation[8];
	Eigen::Quaternionf q(m);
	return {q.x(), q.y(), q.z(), q.w()};
	}

The statement "Eigen::Matrix3f expects row-major" is false. Please see the Eigen documentation about storage order:

If the storage order is not specified, then Eigen defaults to storing the entry in column-major. This is also the case if one of the convenience typedefs (Matrix3f, ArrayXXd, etc.) is used.

I guess the documentation cannot be more explicit: Matrix3f is using column-major.

Setting this matrix correctly via correctly in "column-major":

  Eigen::Matrix3f m;
  m << rotation[0], rotation[3], rotation[6],
       rotation[2], rotation[4], rotation[7],
       rotation[3], rotation[5], rotation[8];

Also gets you the correct transformations:

$ ros2 run tf2_ros tf2_echo camera_link camera_color_optical_frame
[INFO] [1764776508.059768980] [tf2_echo]: Waiting for transform camera_link ->  camera_color_optical_frame: Invalid frame ID "camera_link" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [-0.002, -0.032, -0.001]
- Rotation: in Quaternion (xyzw) [0.526, -0.525, 0.474, -0.473]
- Rotation: in RPY (radian) [-1.675, -0.002, -1.571]
- Rotation: in RPY (degree) [-95.957, -0.135, -90.033]
- Matrix:
 -0.001 -0.104  0.995 -0.002
 -1.000 -0.002 -0.001 -0.032
  0.002 -0.995 -0.104 -0.001
  0.000  0.000  0.000  1.000

[christian-rauch]

However, with this fix, the camera is just pointing 5deg in the opposite direction, which still looks way off:

The nominal axis-aligned rotation still provides the best results.

[ob-yalian]

@christian-rauch Thank you for your detailed feedback.

1. First, regarding the tilt between the color and depth coordinate frames: this is caused by the physical installation of the IR sensor in the Femto Bolt camera, which has an inherent tilt. In earlier versions, the camera_link frame of the Femto Bolt followed the IR sensor’s tilt. Later, we added special handling in the code: when the device is identified as a Femto Bolt module and the color stream is enabled, the camera_link frame is rotated to align with the color coordinate frame.

2. Regarding the column-major issue, this is simply an error in our code comments. The extrinsic matrix returned by our SDK is already in column-major order, so no code changes are required.

3. The parameter enable_publish_extrinsic affects the TF publishing result, which is not the intended behavior. This issue will be fixed in the next release. As a temporary workaround, please set enable_publish_extrinsic to true.

[christian-rauch]

In earlier versions, the camera_link frame of the Femto Bolt followed the IR sensor’s tilt. Later, we added special handling in the code: when the device is identified as a Femto Bolt module and the color stream is enabled, the camera_link frame is rotated to align with the color coordinate frame.

Regardless of the other frames, the transformation between the colour and IR camera should always be the same. Do you have CAD drawings of the nominal camera frame poses?

[christian-rauch]

To add to this confusion, the OBExtrinsic is also defined as OBD2CTransform:
https://github.com/orbbec/OrbbecSDK_v2/blob/v2.5.5/include/libobsensor/h/ObTypes.h#L440-L443

That suggests that the type and all functions producing this, are supposed to work with Depth-to-Colour (D2C) transformations. There is no OBC2DTransform. Is the rotation matrix in rot[9] row-major or column-major? What is the coordinate frame convention for the optical camera frames? Is this using OpenCV's convention (x-right, y-down, z-forward)? It would be useful to document information such as this.

Btw, the comment says "Transformation matrix in millimeters" for the translation vector :-)

Using depthProfile->getExtrinsicTo(colorProfile); in the ob_coordinate_transform example, I get

extrinsicD2C: 0.99461, 0.00171772, 0.00309976, -0.00202972, 0.994613, 0.103636, -0.00290505, -0.103641, 0.99461, -32.3073, -0.988853, 2.37924
extrinsicC2D: 0.99461, -0.00202972, -0.00290505, 0.00171772, 0.994613, -0.103641, 0.00309976, 0.103636, 0.99461, 32.1381, 1.28561, -2.16379

[ob-yalian]

@christian-rauch I apologize for the confusion in my previous response. Let me clarify regarding row-major vs column-major order.

Our SDK returns the rotation matrix in row-major order. For example, if the rotation matrix is:

[0, 1, 2]
[3, 4, 5]
[6, 7, 8]

Our SDK's rot array is stored as: [0, 1, 2, 3, 4, 5, 6, 7, 8] (row by row).

Therefore, the code:

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

is correct because:

1. The << operator fills the matrix by logical rows
2. Eigen's column-major storage only affects the internal memory layout, not the logical filling order
   So the assignment works correctly.

Regarding the optical camera coordinate frame, we use OpenCV's convention (x-right, y-down, z-forward). This is documented here: coordinate_and_tf

Regarding the other issues you mentioned, we will actively review and address them accordingly.

[ob-yalian]

@christian-rauch Currently, in the ROS2 wrapper packages (main and v2-main branches), as well as in the corresponding ROS1 wrapper, the transformation between the color and IR (depth) frames is consistent.

The difference observed in v1.5.8 is caused by a bug in that version’s rotationMatrixToQuaternion() implementation. In v1.5.8, the rotation matrix was incorrectly constructed as:

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

[christian-rauch]

@christian-rauch I apologize for the confusion in my previous response. Let me clarify regarding row-major vs column-major order.

Our SDK returns the rotation matrix in row-major order. For example, if the rotation matrix is:

[0, 1, 2]
[3, 4, 5]
[6, 7, 8]

Our SDK's rot array is stored as: [0, 1, 2, 3, 4, 5, 6, 7, 8] (row by row).

Thanks for the clarification.

Regarding the optical camera coordinate frame, we use OpenCV's convention (x-right, y-down, z-forward). This is documented here: coordinate_and_tf

Is this convention only used by the ROS 2 nodes or also by getExtrinsicTo? My understanding is that depthProfile->getExtrinsicTo(colorProfile) returns the transformation from the depth to the colour frame.

With the transformation from depth to colour:

extrinsicD2C:
0.99461, 0.00171772, 0.00309976, -32.3073
-0.00202972, 0.994613, 0.103636, -0.988853
-0.00290505, -0.103641, 0.99461, 2.37924
0, 0, 0, 1

and the OpenCV coordinate frame conventions, the colour sensor frame should be on the negative x-side, i.e. the colour sensor should be left of the depth sensor when looking into the same direction as z and the x-axes levelled on the horizon.

But according to the drawing https://github.com/orbbec/OrbbecHardware/blob/main/Femto%20Bolt/01_2D%20PDF/Femto%20Bolt.pdf, the colour camera is right of the depth camera, hence, by the OpenCV coordinate convention, the translation part of the depth-to-colour transformation should point in positive x direction.

Is OBExtrinsic using a different coordinate convention? Or am I misinterpreting the getExtrinsicTo?

[ob-yalian]

The OpenCV optical frame convention (x-right, y-down, z-forward) is used consistently across the SDK and ROS 2. This also applies to getExtrinsicTo.

depthProfile->getExtrinsicTo(colorProfile) returns the transform from the depth optical frame to the color optical frame:

p_color = T_depth_to_color * p_depth

As a concrete example, if we take the depth frame origin:

p_depth = [0, 0, 0, 1]^T

then:

p_color = T_depth_to_color * p_depth = [-32.3, -1.0, 2.38, 1]^T

The -32 mm on the x-axis means that the depth origin is located at -32 mm in the color optical frame. This is consistent with the drawing.

[christian-rauch]

depthProfile->getExtrinsicTo(colorProfile) returns the transform from the depth optical frame to the color optical frame:

p_color = T_depth_to_color * p_depth

As a concrete example, if we take the depth frame origin:

p_depth = [0, 0, 0, 1]^T

Well, I now see that we are just using different notations. I am following the notation by Barfoot (http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser24.pdf#page=20, page xx):

According to this notation, the transformation matrix $$\mathbf{T}_{dc}$$ maps coordinates from the colour frame, $$\underrightarrow{\mathcal{F}}_c$$, to the depth frame, $$\underrightarrow{\mathcal{F}}_d$$:

$$\mathbf{p}_d = \mathbf{T}_{dc} \cdot \mathbf{p}_c$$

With:

$$\mathbf{p}_c = \left[ 0, 0, 0, 1 \right]^\top$$

the transformation gives you the position of the colour camera origin in the depth camera frame:

$$\mathbf{p}_d = \mathbf{T}_{dc} \cdot \begin{bmatrix} 0, 0, 0, 1 \end{bmatrix}^\top$$

Or in other words, $$\mathbf{T}_{dc}$$ describes the pose of the colour camera frame within the depth camera frame.

In this regard, and with your notation above, depthProfile->getExtrinsicTo(colorProfile) returns $\mathbf{T}_{cd}$, not T_depth_to_color, or $\mathbf{T}_{dc}$, respectively.

I would really appreciate, if all those things (raw/column major, notation, ...) could be documented properly. That would make debugging the transformations much easier.