setting align_depth causes misaligned coloured point cloud, similar but not the same issue as 2595

[elvintoh82]

I'm running ROS Humble inside docker where I also apt install ros-humble-realsense2-*
My setup:

When I run the ros2 launch realsense2_camera rs_launch.py with the following arguments:

enable_depth ==> true
pointcloud.enable ==> true
align_depth.enable ==> true

Note that there is a green colour silicon mat in real life, it is perfectly aligned to the edge of the table in real life(you can also see the urdf model of the table on rviz). However, on rviz where the pointcloud is displayed live, the points of the green mat is not aligned with the edge of the urdf table.

The purpose of me putting the brown box is to show that this problem is NOT related to issue #2595.

I notice that the pointcloud that is published is always misaligned by around 4-5cm to the right. Note that I am not referring to an issue where the depth information is not aligned to the rgb information that has been raised previously by issue #2595. This is a different problem. I know for a fact that this pointcloud is misaligned because I mount the D455 camera onto the robot end effector, and that the robot is mounted on a known position of the table.

Here we can see that the green mats' points are now properly aligned to the edge of the urdf table.

Furthermore, once I change the argument align_depth.enable ==> false. The problem of the misaligned pointcloud disappears.
Suggestions might include just leaving the align_depth.enable argument as false, but I desire the following topics:

1. depth image[that is aligned to the rgb image]
2. rgb image
3. pointcloud

Could someone please look into this? Thanks.

---

Required Info
Camera Model	D455
Firmware Version	5.15.1
Operating System & Version	Ubuntu 22
Platform	PC
Librealsense SDK Version	2.54.2
ROS Distro	Humble
RealSense ROS Wrapper Version	4.54.1-1jammy

Issue Description

The pointcloud that is published when using align_depth.enable==>true is misaligned to real physical space. This is NOT the same as issue #2595 although the title description is the same.

[MartyG-RealSense]

Hi @elvintoh82 You are correct that the recommendation would usually be to not enable align_depth if you are using the pointcloud, since the pointcloud will map depth and color together anyway. I appreciate that you require the aligned topics though.

Is there any improvement if you have align_depth = true and set the pointcloud to use the infrared stream to texture the pointcloud instead of the color stream using the pointcloud.stream_filter parameter?

ros2 launch realsense2_camera rs_launch.py align_depth.enable:=true pointcloud.enable:=true depth_module.profile:=1280x720x30 rgb_camera.profile:=1280x720x30 pointcloud.stream_filter:=1

[elvintoh82]

Thanks @MartyG-RealSense , I will go and try to change the texture types and see if it usable, and also see if it will influence the behaviour of the pointcloud alignment or not.

By the way, I tried searching for the available options to the other argument pointcloud.stream_filter ==> to see what are all the values available and what they would represent, but I can't seem to find any documentation regarding it (specifically pointcloud.stream_filter, not just filters in general). I even tried searching the code base for this repo for the text "pointcloud.stream_filter" and only 3 hits came back, none of which actually gave the list of options/descriptions.

Regardless, I do hope that realsense would be able to address this bug where the pointcloud gets misaligned whenever the align_depth is enabled.

Thanks!

[elvintoh82]

Looking at other issues related to align_depth.enable, I found one possibly related issue (but theirs was regarding RGBD instead of point cloud i think).

#2810 (comment)

Do you reckon it is a related problem?

Conceptually, I would think that a published pointcloud WITHOUT align_depth(to colour) should have a frame_id as "depth_optical_frame".

Whereas a published pointcloud WITH align_depth(to colour) should have a frame_id as "color_optical_frame". Is this a valid concept?

I will go and check what is the pointcloud's frame_id tomorrow.

[MartyG-RealSense]

Thanks very much @elvintoh82

A fix for this alignment / pointcloud issue - #2775 - was implemented in the ROS2 wrapper on 27 June 2023. That was the same day that wrapper 4.54.1 was released and the fix is included in the release notes for 4.54.1. So because you have wrapper 4.54.1 installed then this particular cause of mis-alignment should already have been addressed.

Regarding pointcloud.stream_filter, my understanding is that 0 is no texture, 1 is infrared and 2 is color.

[elvintoh82]

So i did some test today. Tried to use
pointcloud.stream_filter ==> 1 pointcloud.stream_index_filter ==> 0(the default which was given in the launch file)
but it says "[WARN] [1710913035.317315962] [D455.D455]: No stream match for pointcloud chosen texture Process - Depth", and does not publish any pointcloud at all.

Once I changed it to
pointcloud.stream_filter ==> 0 pointcloud.stream_index_filter ==> 0
It was able to publish a textureless pointcloud, but it was still misaligned as per the original issue.
.

[elvintoh82]

I have also checked on who the frame_ids are.
align_depth.enable ==> true, the frame_id is "color_optical_frame"
align_depth.enable ==> false, the frame_id is "depth_optical_frame"
This indeed implies that the wrapper 4.54.1 has been implemented, just that something must have went wrong.

For now.
I think I will try hijacking the pointcloud topic (when align_depth.enable==>true) and change the frame_id to "depth_optical_frame", then republish it to another temporary pointcloud topic and see how it behaves.

[MartyG-RealSense]

Thanks so much for your detailed feedback. Please do let me know how your frame_id edit works out. Good luck!

[elvintoh82]

Thanks so much for your detailed feedback. Please do let me know how your frame_id edit works out. Good luck!

Hi I managed to do hijack the pointcloud topic and changed the frame_id form color_optical_frame to depth_optical_frame and then republish it out (using another temp topic name). It works properly and that pointcloud looks aligned and proper in RVIZ now.
Hopefully there is enough information for your team to work on resolving it subsequently.

Thanks!

[MartyG-RealSense]

I have highlighted your fix to my Intel RealSense colleagues on the ROS team. Thanks for sharing!

[MartyG-RealSense]

My colleagues on the ROS team confirmed that a later PR fix had already been made in September 2023 at #2868 to fix the misalignment issue. The fix is in the 2.54.1 wrapper if it is built from source code but is not yet in the version installed from the ROS server package. So you should build the wrapper from source code to obtain the fixed version.

[elvintoh82]

Thanks for getting back on the implemented fix. I shall find a chance to build it from source and try it, although i don't foresee I have much opportunity to do that anytime soon.

[MartyG-RealSense]

Thanks very much. The alternative would be to wait for the next ROS wrapper release and install the wrapper from ROS server package.

[MartyG-RealSense]

Hi @elvintoh82 Do you require further assistance with this case, please? Thanks!

[elvintoh82]

Hi @elvintoh82 Do you require further assistance with this case, please? Thanks!

At this stage, I'm fine already thanks. I'll just wait for the next update to the apt install method instead. Meanwhile, I'l also just using the hijack method to overcome the cosmetic problem. Much Thanks! @MartyG-RealSense

[MartyG-RealSense]

You are very welcome, @elvintoh82 - thanks very much for the update!