Misaligned point clouds from driver and depth_image_proc

[SDADEEC]

Required Info
Camera Model	D455
Firmware Version	5.13.0.50
Operating System & Version	Linux (Ubuntu 20.04)
Kernel Version (Linux Only)	5.15.0-113-generic
Platform	Intel NUC
Librealsense SDK Version	2.50.0
Language	C++
Segment	Robot
ROS Distro	ROS1 noetic
RealSense ROS Wrapper Version	RealSense ROS v2.3.2

Issue Description

Hi, I'm trying to compare the point clouds generated from the driver by setting enable_pointcloud to true, and from the rgb-d images using depth_image_proc for D455 in ROS 1.

I found there are some misalignments in the point clouds generated from the two methods (1~3 cm). In the screenshot below, the red cloud is from the driver and the white cloud is from the rgbd images.

Here is my driver launch file:

<?xml version="1.0"?>

<launch>
  <arg name="serial_no"           default="030422250130"/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="d455"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>

  <arg name="depth_width"         default="848"/>
  <arg name="depth_height"        default="480"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="infra_width"        default="848"/>
  <arg name="infra_height"       default="480"/>
  <arg name="enable_infra"        default="true"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>
  <arg name="infra_rgb"           default="false"/>

  <arg name="color_width"         default="848"/>
  <arg name="color_height"        default="480"/>
  <arg name="enable_color"        default="true"/>

  <arg name="depth_fps"           default="15"/> <!--15, 5-->
  <arg name="infra_fps"           default="15"/> <!--15, 5-->
  <arg name="color_fps"           default="15"/> <!--15, 5-->
  <arg name="gyro_fps"            default="400"/>
  <arg name="accel_fps"           default="250"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="true"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>

  <arg name="enable_sync"           default="true"/>
  <arg name="align_depth"           default="true"/>

  <arg name="filters"               default=""/>
  <arg name="clip_distance"         default="-2"/>
  <arg name="linear_accel_cov"      default="0.01"/>
  <arg name="initial_reset"         default="true"/>
  <!-- "none" is best for logging. "copy" for online with vilens -->
  <arg name="reconnect_timeout"         default="6.0"/>
  <arg name="wait_for_device_timeout"   default="-1.0"/>
  <arg name="unite_imu_method"      default="copy"/> 

  <arg name="publish_tf"          default="false"/>
  <arg name="tf_publish_rate"     default="0"/> <!-- 0 - static transform -->

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra"             value="$(arg enable_infra)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>
      <arg name="infra_rgb"                value="$(arg infra_rgb)"/>

      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="reconnect_timeout"        value="$(arg reconnect_timeout)"/>
      <arg name="wait_for_device_timeout"  value="$(arg wait_for_device_timeout)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>
    </include>
  </group>

  <!-- lower png_level from realsense camera, so that png compression can be real time. -->
  <!-- default (1) can only be published at 5Hz. range of png_level is 1-9 -->
  <node pkg="dynamic_reconfigure" type="dynparam" name="dynparam_$(arg camera)_png_level" output="screen"
    args="set /$(arg camera)/aligned_depth_to_color/image_raw/compressedDepth png_level 1" >
  </node>
  <!-- higher jpeg from realsense camera, so that jpeg_quality compression can be real time. -->
  <node pkg="dynamic_reconfigure" type="dynparam" name="dynparam_$(arg camera)_color_jpg_level" output="screen"
    args="set /$(arg camera)/color/image_raw/compressed jpeg_quality 90" >
  </node>
  <!-- higher jpeg from realsense camera, so that jpeg_quality compression can be real time. -->
  <node pkg="dynamic_reconfigure" type="dynparam" name="dynparam_$(arg camera)_infra1_jpg_level" output="screen"
    args="set /$(arg camera)/infra1/image_rect_raw/compressed jpeg_quality 90" >
  </node>
  <!-- higher jpeg from realsense camera, so that jpeg_quality compression can be real time. -->
  <node pkg="dynamic_reconfigure" type="dynparam" name="dynparam_$(arg camera)_infra2_jpg_level" output="screen"
    args="set /$(arg camera)/infra2/image_rect_raw/compressed jpeg_quality 90" >
  </node>
  <!-- turn off IR emitter to make feature tracking possible on the IR images -->
  <node pkg="dynamic_reconfigure" type="dynparam" name="dynparam_$(arg camera)_ir_emitter" output="screen"
    args="set /$(arg camera)/stereo_module emitter_enabled 1" >
  </node>
  <!-- have the highest accuracy depth estimation (recommended for robotics applications) -->
  <node pkg="dynamic_reconfigure" type="dynparam" name="dynparam_$(arg camera)_depth_accuracy" output="screen"
    args="set /$(arg camera)/stereo_module visual_preset 3" >
  </node>

  <!-- turn laser power to maximum-->
  <node pkg="dynamic_reconfigure" type="dynparam" name="dynparam_$(arg camera)_laser_power" output="screen"
    args="set /$(arg camera)/stereo_module laser_power 360" >
  </node>

</launch>

And the launch file to generate point clouds from rgbd images

<launch>
  <node pkg="nodelet" type="nodelet" name="nodelet_manager_d455" args="manager" />

  <node pkg="nodelet" type="nodelet" name="depth2cloud_d455"
        args="load depth_image_proc/point_cloud_xyzrgb nodelet_manager_d455">
    <remap from="rgb/camera_info" to="/d455/color/camera_info"/>
    <remap from="rgb/image_rect_color" to="/d455/color/image_raw"/>
    <remap from="depth_registered/image_rect" to="/d455/aligned_depth_to_color/image_raw"/>
    <remap from="depth_registered/points" to="/d455/points_raw"/>
  </node>
</launch>

I noticed d455 has some distortions in the camera_info (1580) which is different from other cameras like d435i (1430). Is that mean the driver internally performs some rectification then generate the point clouds? If so, how to properly rectify the rgbd images and how to generate the identical point clouds from them compared to the clouds from driver?

[MartyG-RealSense]

Hi @SDADEEC A difference between the rs_camera and rs_rgbd launches is that by default rs_camera generates an unordered pointcloud by default, whilst rs_rgbd generates an ordered pointcloud.

You can set the rs_camera pointcloud to an ordered one to see whether it brings the results closer to those of rs_rgbd's ordered cloud.

roslaunch realsense2_camera rs_camera.launch ordered_pc:=true

[SDADEEC]

Hi @MartyG-RealSense, thanks very much for the suggestion. I set the point cloud from the driver to be ordered, but it looks like the rgbd cloud is still not well aligned with the driver cloud (red cloud is from the driver and the white cloud is from the rgbd images.):

I've checked the two clouds messages, the left is the driver cloud and the right is the rgbd cloud. I noticed the length from the two point clouds in the "data" field are not equal. Do you have any idea on why this is happening?

For rs_rgbd.lauch, I found that the way to generate the point cloud from rgbd images is to use the rectified color image and raw aligned-to-color depth image, which is aligned to the raw color image. Is this the correct way to generate the point cloud? Do we need to rectify the raw aligned-to-color depth image, then use the rectified depth image together with the rectified color image to generate the point cloud?

[MartyG-RealSense]

Usually when aligning depth to color in the ROS wrapper, you do not need to consider the configuration of the depth and color streams. You simply set the align_depth parameter to True and aligned-image topics are automatically published.

You do not need to perform rectification on images, and would not be able to do so as the rectification process takes place automatically in the camera hardware before the depth and color frames even reach the computer.

Actually, in the rs_camera.launch file you do not need to set align_depth to true when publishing a pointcloud (unless you need to use the aligned topics for something too) as depth and color will be mapped together anyway when the pointcloud filter is enabled. So you could be aligning the images twice by enabling align_depth.

Is the rs_camera pointcloud more accurate if you set align_depth to false? You can test this without editing the launch file by adding align_depth:=false to the roslaunch instruction, as the command will then override the setting stored in the launch file.

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud align_depth:=false

[SDADEEC]

Hi @MartyG-RealSense, I'm not sure how to check whether the point cloud published by the driver, or the one from rgbd images is more accurate.

My case is that I want to record the point cloud published by the driver. But it is too big in size, I decide to record the rgbd images instead and recover the point cloud from them (i.e., one color image topic and one aligned_depth_to_color depth image topic). In that case, I would like to make sure the point clouds from the rgbd images are identical to the point clouds from the driver. However, using the above launch file, I still get some misaligned point clouds.

Actually, in the rs_camera.launch file you do not need to set align_depth to true when publishing a pointcloud (unless you need to use the aligned topics for something too) as depth and color will be mapped together anyway when the pointcloud filter is enabled. So you could be aligning the images twice by enabling align_depth.

What I found is that in rs_rgbd.lauch, the /camera/color/image_raw topic would be rectified and it's actually the /camera/color/image_rect_color that is being used for the point cloud generation.

From what you said, is that mean when I set filters:=pointcloud or enable_pointcloud:=true, the /camera/color/image_raw topic would be automatically rectified? And I should use the /camera/color/image_raw instead for the point cloud generation in the rs_rgbd.lauch?

[MartyG-RealSense]

When the pointcloud filter is enabled, by default the pointcloud is based upon the /camera/aligned_depth_to_color/image_raw topic. It uses this aligned topic without you needing to set align_depth to true.

This is reflected in your launch file for generating pointclouds from RGBD images.

<remap from="depth_registered/image_rect" to="/d455/aligned_depth_to_color/image_raw"/>

You are welcome to try experimenting with remapping to a different color topic, but the default is likely to be the best choice.

[SDADEEC]

Is that mean for /camera/aligned_depth_to_color/image_raw, the depth image is transformed to color image frame and also undistorted?

[MartyG-RealSense]

Both the depth and color images have distortion models applied to them, so there should be some distortion incorporated into the aligned image because the original images had it.

[SDADEEC]

OK, in that case, for a pixel (u,v) in the /camera/aligned_depth_to_color/image_raw, does the depth correspond to the same pixel location in the /camera/color/image_raw topic?

[MartyG-RealSense]

When depth-color alignment is performed, the field of view size of the depth sensor is resized to match the field of view size of the RGB color sensor, and the depth coordinates are mapped onto matching RGB coordinates. Also, after alignment the 0,0,0 origin point of depth changes from the centerline of the left IR sensor to the centerline of the RGB sensor.

[SDADEEC]

OK, I see. Thanks for the explanation. So the coordinates of /camera/aligned_depth_to_color/image_raw are matched to the coordinates of /camera/color/image_raw.

But this raises a question, in realsense2_camera/launch/rs_rgbd.launch, the block to generate point clouds from rgbd images is shown below when align_depth is set to true:

<group if="$(eval depth_registered_processing and hw_registered_processing)">
      <!-- Publish registered XYZRGB point cloud with hardware registered input (ROS Realsense depth alignment) -->
      <node pkg="nodelet" type="nodelet" name="points_xyzrgb_hw_registered"
            args="load depth_image_proc/point_cloud_xyzrgb $(arg manager) $(arg bond)" respawn="$(arg respawn)">
        <remap from="rgb/image_rect_color"        to="$(arg rgb)/image_rect_color" />
        <remap from="rgb/camera_info"             to="$(arg rgb)/camera_info" />
        <remap from="depth_registered/image_rect" to="$(arg depth_registered)/image_raw" />
        <remap from="depth_registered/points"     to="$(arg depth_registered_pub)/points" />
      </node>
</group>

The depth image topic is still using the /camera/aligned_depth_to_color/image_raw topic, but the color image topic is using /camera/color/image_rect_color instead of /camera/color/image_raw.

Is this correct? Is /camera/color/image_rect_color the same as /camera/color/image_raw for d455?

[MartyG-RealSense]

At https://github.com/IntelRealSense/realsense-ros/issues/1029 a RealSense ROS user asks a similar question about these color topics and the creator of the RealSense ROS1 wrapper advises that the /camera/color/image_rect_color topic is a product of the rs_rgbd.launch file. So it should only be published when rs_rgbd.launch is used.

[MartyG-RealSense]

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

[SDADEEC]

I think I got what I wanted. Thanks so much for the help.

[MartyG-RealSense]

You are very welcome. I'm pleased that I could help. Thanks very much for the update!