setting align_depth causes misaligned coloured point cloud

[christian-rauch]

When I set the parameter align_depth.enable to True, the coloured point cloud will have misaligned colour, i.e. the colour will not be correctly registered to the points.

Consider the following ROS2 launch file:

launch:
- node_container:
    pkg: rclcpp_components
    exec: component_container
    name: camera_container
    namespace: realsense
    composable_node:
    -   pkg: realsense2_camera
        plugin: realsense2_camera::RealSenseNodeFactory
        name: camera
        param:
        -   name: rgb_camera.profile
            value: 640x480x30
        -   name: "align_depth.enable"
            value: True
        -   name: pointcloud.enable
            value: True
        -   name: pointcloud.stream_filter
            value: 2 # RS2_STREAM_COLOR

The launch file will start the node, set VGA resolution @ 30 fps (rgb_camera.profile), align the colour with the depth (align_depth.enable) and publish a coloured point cloud (pointcloud).

With

        -   name: "align_depth.enable"
            value: True

the point cloud is shown in RViz2 with misaligned colour: rs_not_aligned.zip. The image shows the point cloud of a RealSense box. It is clearly visible that the white background is registered to the majority of the box points in the foreground.

When set to False or removed, the colour is correctly aligned with the points: rs_aligned.zip

It looks like the colour is registered twice before it is assigned to the points.

[MartyG-RealSense]

Hi @christian-rauch If the pointcloud filter is enabled then alignment should automatically be used without having to set align_depth to true.

[christian-rauch]

Hi @christian-rauch If the pointcloud filter is enabled then alignment should automatically be used without having to set align_depth to true.

I am sure that in the default configuration, the node will align depth and colour. If I don't use align_depth, then the alignment in the point cloud is correct. But I am additionally setting align_depth because I also need the aligned depth images on topic /aligned_depth_to_color/image_raw. Enabling both somehow registers the images twice as if the point cloud registration would use the already registered image from /aligned_depth_to_color/image_raw instead of the original depth image.

@MartyG-RealSense In summary: I want to subscribe to the registered depth images on /aligned_depth_to_color/image_raw and the registered point cloud on /depth/color/points. This is not possible at the moment.

[MartyG-RealSense]

What happens if you set align_depth to true and change the pointcloud texture configuration by setting pointcloud.stream_filter to 0

[christian-rauch]

What happens if you set align_depth to true and change the pointcloud texture configuration by setting pointcloud.stream_filter to 0

Then I won't get colour.

With pointcloud.stream_filter set to 0

ros2 topic echo /depth/color/points --field fields

gives

[sensor_msgs.msg.PointField(name='x', offset=0, datatype=7, count=1), sensor_msgs.msg.PointField(name='y', offset=4, datatype=7, count=1), sensor_msgs.msg.PointField(name='z', offset=8, datatype=7, count=1)]

[MartyG-RealSense]

I have highlighted this issue to my Intel RealSense colleagues on the ROS team. Thanks very much for your patience.

[cardboardcode]

+1. Encountered the same issue of misaligned colored pointcloud after setting aligned_depth to true as well.

See below for the full report.

Environment 🌲

1. Ubuntu 22.04 [Operating System]
2. realsense-lib 2.51.1 [Driver]
3. ROS2 Humble [ROS2 Distribution]
4. Realsense D455 [Camera Depth Module - Hardware]

Steps To Reproduce 📖

1. Connect the Camera Hardware (Eg. Realsense D455) to your local workstation via a USB3.0 port.

2. Set up the ROS2 workspace with realsense_ros ROS2 package:

cd $HOME
mkdir -p official_realsense_ws/src
cd ~/official_realsense_ws/src
# Download the realsense_ros repository - branch ros2_development
git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-development
cd ~/official_realsense_ws/
source /opt/ros/humble/setup.bash
colcon build

3. Run realsense_ros ROS2 package once colcon build has completed:

cd ~/official_realsense_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true \
align_depth.enable:=true

4. Create the following .rviz configuration file by creating a debug.rviz file:

cd ~/official_realsense_ws
mkdir rviz2
touch debug.rviz
nano debug.rviz
# Copy and paste the content below into `debug.rviz`

Click here for debug.rviz file content.

```txt Panels: - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /Image1 - /PointCloud21 Splitter Ratio: 0.5 Tree Height: 578 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 Offset: X: 0 Y: 0 Z: 0 Plane: XY Plane Cell Count: 10 Reference Frame: Value: true - Class: rviz_default_plugins/Image Enabled: true Max Value: 1 Median window: 5 Min Value: 0 Name: Image Normalize Range: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /camera/color/image_raw Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 Value: true Axis: Z Channel Name: intensity Class: rviz_default_plugins/PointCloud2 Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: PointCloud2 Position Transformer: XYZ Selectable: true Size (Pixels): 2 Size (m): 0.009999999776482582 Style: Points Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /camera/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Fixed Frame: camera_color_optical_frame Frame Rate: 30 Name: root Tools: - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /initialpose - Class: rviz_default_plugins/SetGoal Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /goal_pose - Class: rviz_default_plugins/PublishPoint Single click: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /clicked_point Transformation: Current: Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit Distance: 1.0558407306671143 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0.005386230535805225 Y: 0.02611066773533821 Z: 0.5844396352767944 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: true Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 1.1253976821899414 Target Frame: Value: Orbit (rviz) Yaw: 3.883587121963501 Saved: ~ Window Geometry: Displays: collapsed: false Height: 1016 Hide Left Dock: false Hide Right Dock: false Image: collapsed: false QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002cd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000310000000cb0000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004c70000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: false Width: 1848 X: 72 Y: 27 ```

6. Open rviz2 to display the PointCloud result captured after executing Step 6.

cd ~/official_realsense_ws
source /opt/ros/humble/setup.bash
rviz2 -d rviz2/debug.rviz

The rviz GUI should now have appeared.

Verify

Actual Behaviour ❌

You should see the following result after executing Step 6:

The above pointcloud is deduced to be misaligned when comparing it to the Expected Behaviour.

Expected Behaviour ✔️

To obtain the expected behaviour, please run realsense_ros without align_depth.enable:=true:

cd ~/official_realsense_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true

You should see the following result:

Both Actual Behaviour and Expected Behaviour are done with the following controlled Setup and Object:

Setup

Object

The object in question is a water bottle with a yellow lid.

Conclusion 🔍

By comparing the difference in results between Actual Behaviour and Expected Behaviour, it would seem that running without align_depth.enable:=true already has align_depth set to true.

Setting align_depth.enable:=true therefore seems to cause the color to be registered twice unnecessarily.

[MartyG-RealSense]

Thanks very much @cardboardcode for your report.

[cardboardcode]

@MartyG-RealSense
The report has been fully elaborated. Hope it helps.

[MartyG-RealSense]

After discussion between my Intel RealSense colleagues, an official internal report has been created about this issue so that it can be investigated further by the RealSense ROS development team at a future date. If other RealSense ROS users experience this issue then they are encouraged to report it on this discussion.

This issue should be kept open whilst the internal report is active.

[SamerKhshiboun]

Hi @christian-rauch, Can you please tell me which camera model are you using ? is it D455 as @cardboardcode reported above ?

Also I see that #2585 is related to this issue, specially at this comment: #2585 (comment)
I'm investigating this and will update asap.

Thanks,
Samer

[christian-rauch]

Can you please tell me which camera model are you using ? is it D455 as @cardboardcode reported above ?

Yes, it is the D455.

[MartyG-RealSense]

Hi @SamerKhshiboun Do you have an update about this case that you can provide, please? Thanks!