Misaligned rgbd point cloud when align_depth is enabled

[hello-chintan]

Hey team,

I am using a D435i camera on my robot to accomplish various tasks. I am using source built up to date version of realsenser-ros drivers along with ROS 2 Galactic and Humble. Upon enabling the align_depth parameter while launching the camera driver in ROS 2 and visualizing in RViz, I am observing a peculiar behavior which results in the /camera/depth/color/points topic to have an offset with respect to the actual object. This behavior is not observed when the align_depth parameter is disabled.

I am attaching images to make things more clear:

Figure 1: Point cloud offset with respect to robot model when align_depth parameter is enabled

Figure 2: No offset between point cloud and robot model when align_depth parameter is disabled

I also tested this to compare the behavior in ROS Noetic, but a similar effect was not observed when enabling the align_depth parameter. My issue seems to be similar in nature to #2595. I'd appreciate help to resolve this.

Thank you,
Chintan Desai

[MartyG-RealSense]

Hi @hello-chintan Yes, it does sound similar to the ROS2 case at #2595 which had an internal report created by Intel about it so that it could be investigated.

Until that issue is resolved, I believe that the only course of action that you can take is to set align_depth.enable to false like you are already doing.

[Arun-Prasad-V]

Hi @hello-chintan ,

A pull request #2775 related to this issue has been merged recently.

Could you please check and confirm you have the latest RealSense ROS2 source code and observing this issue with that?

[hello-chintan]

Hey @Arun-Prasad-V, I can confirm that I am using the latest realsense-ros drivers and still seeing the misaligned point cloud issue.

[Arun-Prasad-V]

Hi @hello-chintan,
Thanks for the confirmation. Could you please share the command and the input params you are using?

[hello-chintan]

Sure, @Arun-Prasad-V.

I am using a modified launch file to set the various parameters while invoking the rs_launch.py launch file.

[Arun-Prasad-V]

Hi @hello-chintan,
Could you please share the input json file as well?

[hello-chintan]

Hey @Arun-Prasad-V. Sure, we are using the HighAccuracyPreset.json file.

[MartyG-RealSense]

Hi @hello-chintan Do you require further assistance with this case, please? Thanks!

[hello-chintan]

Hey, @MartyG-RealSense. We do not have a resolution for the underlying issue apart from the temporary patch to unset the align_depth.enable parameter. Is the issue still being investigated?

[Arun-Prasad-V]

Hi @hello-chintan,
We still couldn't reproduce this issue. Could you please share few more details regarding the setup?

• FW version
• LibRealsense2 SDK version or commit ID if using source
• Realsense ROS2 version or commit ID
• OS & Version

[hello-chintan]

Certainly, @Arun-Prasad-V. Pasting the values here as reported by the camera launch file at startup:

• Device FW version: 5.13.0.50
• Running with LibRealSense v2.54.1
• RealSense ROS v4.54.1
• Ubuntu 22.04 with ROS 2 Humble

I'm also attaching the full terminal output for your reference:

[INFO] [launch]: All log files can be found below /home/stretch-2001/.ros/log/2023-07-25-11-34-54-384357-ubuntu22-33700
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [realsense2_camera_node-1]: process started with pid [33701]
[realsense2_camera_node-1] [INFO] [1690310094.975934944] [camera.camera]: RealSense ROS v4.54.1
[realsense2_camera_node-1] [INFO] [1690310094.976075270] [camera.camera]: Built with LibRealSense v2.51.1
[realsense2_camera_node-1] [INFO] [1690310094.976106384] [camera.camera]: Running with LibRealSense v2.54.1
[realsense2_camera_node-1] [WARN] [1690310094.976128329] [camera.camera]: ***************************************************
[realsense2_camera_node-1] [WARN] [1690310094.976146377] [camera.camera]: ** running with a different librealsense version **
[realsense2_camera_node-1] [WARN] [1690310094.976163064] [camera.camera]: ** than the one the wrapper was compiled with!   **
[realsense2_camera_node-1] [WARN] [1690310094.976185024] [camera.camera]: ***************************************************
[realsense2_camera_node-1] [INFO] [1690310095.005909860] [camera.camera]: Device with serial number 036522071164 was found.
[realsense2_camera_node-1] 
[realsense2_camera_node-1] [INFO] [1690310095.006012155] [camera.camera]: Device with physical ID /sys/devices/pci0000:00/0000:00:1c.4/0000:02:00.0/0000:03:02.0/0000:3a:00.0/usb4/4-1/4-1:1.0/video4linux/video0 was found.
[realsense2_camera_node-1] [INFO] [1690310095.006041682] [camera.camera]: Device with name Intel RealSense D435I was found.
[realsense2_camera_node-1] [INFO] [1690310095.006439784] [camera.camera]: Device with port number 4-1 was found.
[realsense2_camera_node-1] [INFO] [1690310095.006483650] [camera.camera]: Device USB type: 3.2
[realsense2_camera_node-1] [INFO] [1690310095.007997132] [camera.camera]: getParameters...
[realsense2_camera_node-1] [INFO] [1690310095.008329970] [camera.camera]: JSON file is not provided
[realsense2_camera_node-1] [INFO] [1690310095.008360250] [camera.camera]: Device Name: Intel RealSense D435I
[realsense2_camera_node-1] [INFO] [1690310095.008373329] [camera.camera]: Device Serial No: 036522071164
[realsense2_camera_node-1] [INFO] [1690310095.008384786] [camera.camera]: Device physical port: /sys/devices/pci0000:00/0000:00:1c.4/0000:02:00.0/0000:03:02.0/0000:3a:00.0/usb4/4-1/4-1:1.0/video4linux/video0
[realsense2_camera_node-1] [INFO] [1690310095.008396866] [camera.camera]: Device FW version: 5.13.0.50
[realsense2_camera_node-1] [INFO] [1690310095.008407602] [camera.camera]: Device Product ID: 0x0B3A
[realsense2_camera_node-1] [INFO] [1690310095.008417506] [camera.camera]: Sync Mode: Off
[realsense2_camera_node-1] [INFO] [1690310095.020879556] [camera.camera]: Set ROS param depth_module.profile to default: 848x480x30
[realsense2_camera_node-1] [WARN] [1690310095.022923550] [camera.camera]: Could not set param: rgb_camera.power_line_frequency with 3 Range: [0, 2]: parameter 'rgb_camera.power_line_frequency' could not be set: Parameter {rgb_camera.power_line_frequency} doesn't comply with integer range.
[realsense2_camera_node-1] [INFO] [1690310095.028969526] [camera.camera]: Set ROS param rgb_camera.profile to default: 1280x720x30
[realsense2_camera_node-1] [INFO] [1690310095.029536002] [camera.camera]: Set ROS param gyro_fps to default: 200
[realsense2_camera_node-1] [INFO] [1690310095.029594240] [camera.camera]: Set ROS param accel_fps to default: 63
[realsense2_camera_node-1] [INFO] [1690310095.038375927] [camera.camera]: Stopping Sensor: Depth Module
[realsense2_camera_node-1] [INFO] [1690310095.042132347] [camera.camera]: Starting Sensor: Depth Module
[realsense2_camera_node-1] [INFO] [1690310095.052994826] [camera.camera]: Open profile: stream_type: Depth(0), Format: Z16, Width: 848, Height: 480, FPS: 30
[realsense2_camera_node-1] [INFO] [1690310095.053469871] [camera.camera]: Stopping Sensor: RGB Camera
[realsense2_camera_node-1] [INFO] [1690310095.056853449] [camera.camera]: Starting Sensor: RGB Camera
[realsense2_camera_node-1] [INFO] [1690310095.068973372] [camera.camera]: Open profile: stream_type: Color(0), Format: RGB8, Width: 1280, Height: 720, FPS: 30
[realsense2_camera_node-1] [INFO] [1690310095.071473080] [camera.camera]: RealSense Node Is Up!
[realsense2_camera_node-1] [WARN] [1690310095.183927339] [camera.camera]: 
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[realsense2_camera_node-1] [INFO] [1690310115.542898443] [rclcpp]: signal_handler(signum=2)
[realsense2_camera_node-1] [INFO] [1690310115.643042402] [camera.camera]: Stop Sensor: Depth Module
[realsense2_camera_node-1] [INFO] [1690310115.643205951] [camera.camera]: Close Sensor. 
[realsense2_camera_node-1] [INFO] [1690310115.650419134] [camera.camera]: Close Sensor - Done. 
[realsense2_camera_node-1] [INFO] [1690310115.650558503] [camera.camera]: Stop Sensor: RGB Camera
[realsense2_camera_node-1] [INFO] [1690310115.650628129] [camera.camera]: Close Sensor. 
[realsense2_camera_node-1] [INFO] [1690310116.604877073] [camera.camera]: Close Sensor - Done. 
[INFO] [realsense2_camera_node-1]: process has finished cleanly [pid 33701]

[MartyG-RealSense]

Hi @hello-chintan Do you require further advice from @Arun-Prasad-V about this case, please? Thanks!

[hello-chintan]

Hi @MartyG-RealSense. Yes, I do.

@Arun-Prasad-V, were you able to recreate the issue? Apart from the camera configuration, what does your physical setup look like to see the difference in alignment? Maybe I can suggest improvements.

[MartyG-RealSense]

@Arun-Prasad-V Could you assist @hello-chintan with their question in the above comment, please? Thanks!

[Arun-Prasad-V]

Hi @hello-chintan ,
I couldn't recreate this issue yet. @SamerKhshiboun is going to try with a different setup. We will check and get back soon.
Thanks!

[hello-chintan]

I appreciate y'all for looking into this! Thanks!

[robin-read]

Hello,

I too have been seeing this problem, and I wanted to share what I have found, and how I replicate the issue. Fundamentally, I think that the frame_id of the point cloud published by the realsense2_camera package is incorrect when the align_depth.enable is set to true. I can see that the header reports the frame_id to be in the camera_color_optical_frame, while I suspect that that data itself is in the camera_depth_optical_frame and thus it is rendered incorrectly in RViz.

To probe this a bit further, I have run RViz with the raw point cloud from the realsense2_camera package, but also created a point cloud from the RGB and Registered Depth image using the depth_image_proc package. I have done this for both a DS415 and DS435i and I see a consistent behaviour.

<launch>
    <arg name="color_res" default="640x480"/>
    <arg name="depth_res" default="640x480"/>
    <arg name="fps" default="30"/>
    <arg name="order_pc" default="false" description="Whether to order the point cloud"/>

    <include file="$(find-pkg-share realsense2_camera)/launch/rs_launch.py">
        <arg name="pointcloud.enable" value="true"/>
        <arg name="rgb_camera.profile" value="$(var color_res)x$(var fps)"/>
        <arg name="depth_module.profile" value="$(var depth_res)x$(var fps)"/>
        <arg name="align_depth.enable" value="true"/>
        <arg name="pointcloud.ordered_pc" value="$(var order_pc)"/>
    </include>

    <node_container pkg="rclcpp_components" exec="component_container" name="container" namespace="">
        <composable_node pkg="depth_image_proc" plugin="depth_image_proc::PointCloudXyzrgbNode" name="point_cloud_xyzrgb_node" namespace="/camera">
            <remap from="rgb/camera_info" to="/camera/color/camera_info" />
            <remap from="rgb/image_rect_color" to="/camera/color/image_raw" />
            <remap from="depth_registered/image_rect" to="/camera/aligned_depth_to_color/image_raw" />
            <remap from="points" to="/camera/depth_registered/points" />
        </composable_node>
    </node_container>

</launch>

Point Clouds from DS415 camera

Point Cloud from DS435i camera

White Point Cloud is from the realsense2_camera package, and the red Point Cloud is from the depth_image_proc package.

Looking at the code on the ros2-development branch, I think that one problematic line is here - it may be a good start point. I think, but haven't double checked, that the data in the pc argument is not in the assumed frame and thus line 817 is setting an incorrect frame_id value.

Setup Specifics:

• Ubuntu 22.04 with ROS2 Humble
• RealSense ROS v4.54.1
• Built with LibRealSense v2.54.1
• Running with LibRealSense v2.54.1
• DS435 & DS415 Device FW version: 5.13.0.50

[Arun-Prasad-V]

@robin-read , Thanks for the detailed explanation and the suggestions. We will check and get back asap.

cc: @SamerKhshiboun

[Arun-Prasad-V]

Thanks @robin-read. I was able to reproduce the issue with the steps you have mentioned.
A related PR #2868 has been created.

@hello-chintan, @robin-read,
While we review and merge it, if possible please test from your side as well.

[hello-chintan]

@Arun-Prasad-V PR #2868 fixed the issue for me. I see that the point cloud is now aligned to the object irrespective of the align_depth.enable parameter being set or unset. With this, now the behavior of the driver is also similar to what's observed in Noetic.

@robin-read, @Arun-Prasad-V thanks for fixing it! Waiting for it to be mainlined!

[robin-read]

Looks good from my end too. Any idea when this will be mainlined and apt will be updated?

[SamerKhshiboun]

Looks good from my end too. Any idea when this will be mainlined and apt will be updated?

We are planning to release debian packages by end of year.
Anyway, you can build from source and use the code, and if you are looking for private debians, you can create one from our code, follow the steps by this link: https://docs.ros.org/en/foxy/How-To-Guides/Building-a-Custom-Debian-Package.html

If you need help, let me know.

[MartyG-RealSense]

@hello-chintan and @robin-read Do you require further assistance with this case, please? Thanks!

[hello-chintan]

The issue is resolved for me. Thank you!

[MartyG-RealSense]

@hello-chintan It's great to hear that your problem is resolved. Thanks very much for the update!

[MartyG-RealSense]

Case closed due to solution achieved and no further comments received.

[Tiryoh]

I also encountered the same issue, but I have confirmed that the problem has been resolved by PR #2868. Thanks!