Inconsistent Point Cloud Data Between Gazebo Simulation and Real-World RealSense D405

[FadiGEBRAYEL]

Hello,

I am currently using the Intel RealSense D405 camera in a Gazebo simulation environment to replicate real-world scenarios. The purpose of my work is to scan a specific scene and emulate what a robot would see in real life using the camera in Gazebo. However, I am encountering significant inconsistencies between the point cloud data captured by the D405 camera in the simulation and the point cloud data captured in real life.

I am using the following xacro files for the camera setup:

_d405.gazebo.xacro

<xacro:macro name="gazebo_d405" params="camera_name reference_link topics_ns depth_optical_frame color_optical_frame infrared1_optical_frame infrared2_optical_frame publish_pointcloud:=true" >

<!-- Load parameters to model's main link-->
<xacro:property name="deg_to_rad" value="0.01745329251994329577" />
<gazebo reference="${reference_link}">
  <self_collide>0</self_collide>
  <enable_wind>0</enable_wind>
  <kinematic>0</kinematic>
  <gravity>0</gravity>
  <!--<mu>1</mu>-->
  <mu2>1</mu2>
  <fdir1>0 0 0</fdir1>
  <!--<slip1>0</slip1>
  <slip2>0</slip2>-->
  <kp>1e+13</kp>
  <kd>1</kd>
  <!--<max_vel>0.01</max_vel>
  <min_depth>0</min_depth>-->
  <sensor name="${camera_name}color" type="camera">
    <camera name="${camera_name}">
      <horizontal_fov>${69.4*deg_to_rad}</horizontal_fov>
      <image>
        <width>640</width>
        <height>480</height>
        <format>RGB_INT8</format>
      </image>
      <clip>
        <near>0.1</near>
        <far>100</far>
      </clip>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.007</stddev>
      </noise>
    </camera>
    <always_on>1</always_on>
    <update_rate>30</update_rate>
    <visualize>1</visualize>
  </sensor>
  <sensor name="${camera_name}ired1" type="camera">
    <camera name="${camera_name}">
      <horizontal_fov>${84*deg_to_rad}</horizontal_fov>
      <image>
        <width>1280</width>
        <height>720</height>
        <format>L_INT8</format>
      </image>
      <clip>
        <near>0.1</near>
        <far>100</far>
      </clip>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.05</stddev>
      </noise>
    </camera>
    <always_on>1</always_on>
    <update_rate>90</update_rate>
    <visualize>0</visualize>
  </sensor>
  <sensor name="${camera_name}ired2" type="camera">
    <camera name="${camera_name}">
      <horizontal_fov>${84*deg_to_rad}</horizontal_fov>
      <image>
        <width>1280</width>
        <height>720</height>
        <format>L_INT8</format>
      </image>
      <clip>
        <near>0.1</near>
        <far>100</far>
      </clip>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.05</stddev>
      </noise>
    </camera>
    <always_on>1</always_on>
    <update_rate>90</update_rate>
    <visualize>0</visualize>
  </sensor>
  <sensor name="${camera_name}depth" type="depth">
    <camera name="${camera_name}">
      <horizontal_fov>${85.2*deg_to_rad}</horizontal_fov>
      <image>
        <width>1280</width>
        <height>720</height>
      </image>
      <clip>
        <near>0.1</near>
        <far>100</far>
      </clip>
      <noise>
        <type>gaussian</type>
        <mean>0.0</mean>
        <stddev>0.100</stddev>
      </noise>
    </camera>
    <always_on>1</always_on>
    <update_rate>90</update_rate>
    <visualize>0</visualize>
  </sensor>
</gazebo>

<gazebo>
  <plugin name="${topics_ns}" filename="librealsense_gazebo_plugin.so">
    <prefix>${camera_name}</prefix>
    <depthUpdateRate>30.0</depthUpdateRate>
    <colorUpdateRate>30.0</colorUpdateRate>
    <infraredUpdateRate>30.0</infraredUpdateRate>
    <depthTopicName>depth/image_raw</depthTopicName>
    <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
    <colorTopicName>color/image_raw</colorTopicName>
    <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
    <infrared1TopicName>infra1/image_raw</infrared1TopicName>
    <infrared1CameraInfoTopicName>infra1/camera_info</infrared1CameraInfoTopicName>
    <infrared2TopicName>infra2/image_raw</infrared2TopicName>
    <infrared2CameraInfoTopicName>infra2/camera_info</infrared2CameraInfoTopicName>
    <colorOpticalframeName>${color_optical_frame}</colorOpticalframeName>
    <depthOpticalframeName>${depth_optical_frame}</depthOpticalframeName>
    <infrared1OpticalframeName>${infrared1_optical_frame}</infrared1OpticalframeName>
    <infrared2OpticalframeName>${infrared2_optical_frame}</infrared2OpticalframeName>
    <rangeMinDepth>0.2</rangeMinDepth>
    <rangeMaxDepth>10.0</rangeMaxDepth>
    <pointCloud>${publish_pointcloud}</pointCloud>
    <pointCloudTopicName>depth/color/points</pointCloudTopicName>
    <pointCloudCutoff>0.25</pointCloudCutoff>
    <pointCloudCutoffMax>9.0</pointCloudCutoffMax>
  </plugin>
</gazebo>

</xacro:macro>

_d405.urdf.xacro

<xacro:macro name="sensor_d405" params="name:=camera topics_ns:=camera parent *origin publish_pointcloud:=true">
<xacro:property name="M_PI" value="3.1415926535897931" />

<!-- The following values are approximate, and the camera node
 publishing TF values with actual calibrated camera extrinsic values -->
<xacro:property name="d405_cam_depth_to_infra1_offset" value="0.0"/>
<xacro:property name="d405_cam_depth_to_infra2_offset" value="-0.018"/>
<xacro:property name="d405_cam_depth_to_color_offset" value="0.0"/>

<!-- The following values model the aluminum peripherial case for the
d405 camera, with the camera joint represented by the actual
peripherial camera tripod mount -->
<xacro:property name="d405_cam_width" value="0.042"/>
<xacro:property name="d405_cam_height" value="0.042"/>
<xacro:property name="d405_cam_depth" value="0.023"/>
<xacro:property name="d405_cam_mount_from_center_offset" value="0.01465"/>
<!-- glass cover is 0.1 mm inwards from front aluminium plate -->
<xacro:property name="d405_glass_to_front" value="0.1e-3"/>
<!-- see datasheet Revision 017, Table. 4-16 page 97 -->
<xacro:property name="d405_zero_depth_to_glass" value="3.7e-3"/>
<!-- convenience precomputation to avoid clutter-->
<xacro:property name="d405_mesh_x_offset" value="${d405_cam_mount_from_center_offset-d405_glass_to_front-d405_zero_depth_to_glass}"/>

<!-- The following offset is relative to the physical d405 camera peripherial
camera tripod mount -->
<xacro:property name="d405_cam_depth_px" value="${d405_cam_mount_from_center_offset}"/>
<xacro:property name="d405_cam_depth_py" value="0.009"/>
<xacro:property name="d405_cam_depth_pz" value="${d405_cam_height/2}"/>

<material name="${name}_aluminum">
<color rgba="0.5 0.5 0.5 1"/>
</material>

<!-- camera body, with origin at bottom screw mount -->
<joint name="${name}_joint" type="fixed">
  <xacro:insert_block name="origin" />
  <parent link="${parent}"/>
  <child link="${name}_bottom_screw_frame" />
</joint>
<link name="${name}_bottom_screw_frame"/>

<joint name="${name}_link_joint" type="fixed">
  <origin xyz="${d405_mesh_x_offset} ${d405_cam_depth_py} ${d405_cam_depth_pz}" rpy="0 0 0"/>
  <parent link="${name}_bottom_screw_frame"/>
  <child link="${name}_link" />
</joint>

<link name="${name}_link">
  <visual>
    <!-- the mesh origin is at front plate in between the two infrared camera axes -->
    <origin xyz="${d405_zero_depth_to_glass + d405_glass_to_front} ${-d405_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
    <geometry>
        <mesh filename="file://$(find realsense2_description)/meshes/d405.stl" scale="0.001 0.001 0.001" />
    </geometry>
    <material name="${name}_aluminum"/>
  </visual>
  <collision>
    <origin xyz="${d405_zero_depth_to_glass-d405_cam_depth/2} ${-d405_cam_depth_py} 0" rpy="0 0 0"/>
    <geometry>
      <box size="${d405_cam_depth} ${d405_cam_width} ${d405_cam_height}"/>
    </geometry>
  </collision>
  <inertial>
    <!-- The following are not reliable values, and should not be used for modeling -->
    <mass value="0.072" />
    <origin xyz="0 0 0" />
    <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257" />
  </inertial>
</link>

<!-- camera depth joints and links -->
<joint name="${name}_depth_joint" type="fixed">
  <origin xyz="0 0 0" rpy="0 0 0"/>
  <parent link="${name}_link"/>
  <child link="${name}_depth_frame" />
</joint>
<link name="${name}_depth_frame"/>

<joint name="${name}_depth_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="${name}_depth_frame" />
  <child link="${name}_depth_optical_frame" />
</joint>
<link name="${name}_depth_optical_frame"/>

<!-- camera left IR joints and links -->
<joint name="${name}_infra1_joint" type="fixed">
  <origin xyz="0 ${d405_cam_depth_to_infra1_offset} 0" rpy="0 0 0" />
  <parent link="${name}_link" />
  <child link="${name}_infra1_frame" />
</joint>
<link name="${name}_infra1_frame"/>

<joint name="${name}_infra1_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="${name}_infra1_frame" />
  <child link="${name}_infra1_optical_frame" />
</joint>
<link name="${name}_infra1_optical_frame"/>

<!-- camera right IR joints and links -->
<joint name="${name}_infra2_joint" type="fixed">
  <origin xyz="0 ${d405_cam_depth_to_infra2_offset} 0" rpy="0 0 0" />
  <parent link="${name}_link" />
  <child link="${name}_infra2_frame" />
</joint>
<link name="${name}_infra2_frame"/>

<joint name="${name}_infra2_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="${name}_infra2_frame" />
  <child link="${name}_infra2_optical_frame" />
</joint>
<link name="${name}_infra2_optical_frame"/>

<!-- camera color joints and links -->
<joint name="${name}_color_joint" type="fixed">
  <origin xyz="0 ${d405_cam_depth_to_color_offset} 0" rpy="0 0 0" />
  <parent link="${name}_link" />
  <child link="${name}_color_frame" />
</joint>
<link name="${name}_color_frame"/>

<joint name="${name}_color_optical_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
  <parent link="${name}_color_frame" />
  <child link="${name}_color_optical_frame" />
</joint>
<link name="${name}_color_optical_frame"/>

<!-- Realsense Gazebo Plugin -->
<xacro:gazebo_d405 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared1_optical_frame="${name}_left_ir_optical_frame" infrared2_optical_frame="${name}_right_ir_optical_frame" publish_pointcloud="${publish_pointcloud}"/>

</xacro:macro>

test_d405_camera.urdf.xacro

Could you please provide guidance on how to better match the simulated point cloud data with the real-world data?
Thank you for your assistance

ros version: noetic

[MartyG-RealSense]

Hi @FadiGEBRAYEL Thanks very much for your patience as I considered your question.

I note that you are using ROS1 Noetic.

I see a possivble cause of discrepancy between your real-world pointcloud and your Gazebo one. In your xacro, you have the scale set as '0.001'.

<mesh filename="file://$(find realsense2_description)/meshes/d405.stl" scale="0.001 0.001 0.001" />

However, the D405 model differs from most of the other RealSense 400 Series models in that its default depth scale is 0.01 because it is designed to specialize in close-range depth sensing.