3D mappping using D435 ,not able to get a proper map

[tom haverford]

Hello Mathieu, I added the d435 to the turtlebot3 package and tried to do 3d mapping. But I am not able to get a correct map. I used teleop to move around the gazebo world. The problems I am facing are the walls are misaligned and are created multiple times as shown below.

This is the gazebo world

The steps I followed are :

1) Spawn the turtlebot3 in the gazebo world 2)Launch Teleop 3) Launch the Rtabmapping This is the launch file I am using.

<launch>
  <!-- Arguments for launch file with defaults provided -->
  <arg name="database_path" default="$(find rtabmap_ros)/maps/rtabmap.db"/>
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <!--arg name="rgb_topic" default="/camera/color/image_raw"/>
  <arg name="depth_topic" default="/camera/depth/image_raw"/>
  <arg name="camera_info_topic" default="/camera/color/camera_info"/-->
  <arg name="rate"            default="1.0"/>
  <arg name="open_rviz" default="true"/>
  <arg name="move_forward_only" default="false"/>

  <param name="use_sim_time" type="bool" value="True"/>
  
  <!-- Localization-only mode -->
  <arg name="localization"      default="true"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>



  <!-- Mapping Node -->
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <!-- Basic RTAB-Map Parameters -->
      <param name="database_path" type="string" value="$(arg database_path)"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="wait_for_transform" type="bool" value="true"/>
      <param name="odom_frame_id" type="string" value="/odom"/>
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan" type="bool" value="true"/>

      <!-- use actionlib to send goals to move_base --> 
      <param name="use_action_for_goal" type="bool" value="true"/>
      <remap from="move_base"            to="/move_base"/>

 
      <param name="odom_tf_linear_variance"  type="double" value="0.001"/>
      <param name="odom_tf_angular_variance" type="double" value="0.001"/>

      <!-- RTAB-Map Inputs -->
      <remap from="scan" to="/scan"/>
      <remap from="rgb/image" to="/camera/color/image_raw"/>
      <remap from="depth/image" to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
      <remap from="odom"            to="/odom"/>      
      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>

      <remap from="grid_map" to="/map"/>  
      <param name="queue_size" type="int" value="100"/>



      <!-- RTAB-Map's parameters -->
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
      <param name="RGBD/AngularUpdate"        type="string" value="0.05"/>
      <param name="RGBD/LinearUpdate"         type="string" value="0.05"/>
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="Reg/Force3DoF"             type="string" value="true"/>
      <param name="Reg/Strategy"              type="string" value="1"/> <!-- 1=ICP -->
      <param name="Vis/MinInliers"            type="string" value="5"/>
      <param name="Vis/InlierDistance"        type="string" value="0.1"/>
      <param name="Kp/MaxDepth"               type="string" value="1.75"/>
      <param name="Vis/MaxDepth"              type="string" value="1.75"/>
      <param name="Rtabmap/TimeThr"           type="string" value="700"/>
      <param name="Rtabmap/DetectionRate"     type="string" value="$(arg rate)" />
      <param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
      <param name="Grid/MaxObstacleHeight"    type="string" value="1.7" />
      <param name="Grid/NoiseFilteringRadius" type="string" value="0.05"/>
      <param name="Grid/NoiseFilteringMinNeighbors" type="string" value="5"/>
      <param name="RGBD/ProximityByTime"      type="string" value="false"/> 

      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>  
      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> 
      <param name="Reg/Strategy"              type="string" value="1"/>     
      <param name="Vis/MinInliers"        type="string" value="12"/>   
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> 
      <param name="RGBD/OptimizeMaxError"     type="string" value="3"/>	    
      <param name="Grid/FromDepth"            type="string" value="false"/> 
      <param name="Mem/STMSize"               type="string" value="30"/>    
      <param name="RGBD/LocalRadius"          type="string" value="5"/>    
      <param name="Icp/CorrespondenceRatio"   type="string" value="0.4"/>   

      <!-- Rate (Hz) at which new nodes are added to map -->
      <param name="Rtabmap/DetectionRate" type="string" value="1"/>

      <!-- localization mode -->
      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
    </node>

    <!-- visualization with rtabmapviz -->
    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz"
          args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan" type="bool" value="true"/>
      <param name="frame_id" type="string" value="base_footprint"/>
      <param name="wait_for_transform" type="bool" value="true"/>
      <remap from="scan" to="/scan"/>
      <remap from="rgb/image" to="/camera/color/image_raw"/>
      <remap from="depth/image" to="/camera/depth/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
      <remap from="odom"            to="/odom"/>
      <param name="rgb/image_transport"   type="string" value="compressed"/>
      <param name="depth/image_transport" type="string" value="compressedDepth"/>
      
      <param name="queue_size" type="int" value="10"/>
    </node>
    <include file="$(find turtlebot3_navigation)/launch/move_base.launch">
      <arg name="model" value="$(arg model)" />
      <arg name="move_forward_only" value="$(arg move_forward_only)"/>
    </include>

    <group if="$(arg open_rviz)"> 
    <node pkg="rviz" type="rviz" name="rviz" required="true"
          args="-d $(find turtlebot3_navigation)/rviz/turtlebot3_navigation.rviz"/>
    </group>
   
    
  </group>
</launch>

Realsense Plugin

<gazebo>
      <plugin name="${topics_ns}" filename="librealsense_gazebo_plugin.so">
        <prefix>${camera_name}</prefix>
        <depthUpdateRate>10.0</depthUpdateRate>
        <colorUpdateRate>10.0</colorUpdateRate>
        <infraredUpdateRate>10.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>4.0</rangeMaxDepth>
        <pointCloud>${publish_pointcloud}</pointCloud>
        <pointCloudTopicName>depth/color/points</pointCloudTopicName>
        <pointCloudCutoff>0.2</pointCloudCutoff>
        <pointCloudCutoffMax>4.0</pointCloudCutoffMax>
      </plugin>
    </gazebo>

Is the range and pointcloud cutoff enough? Can you give me suggestions on how I can improve getting a clearer map and what are the changes that I should make to correct the alignment. I tried launching and teleoping multiple times with changes in launch file, but seem to be getting the incorrect map each time. Any help would be of great help to me in moving forward! Thanks!

[matlabbe]

Your scan map (cyan points) is almost perfect. The problem is coming from camera versus lidar synchronization. If you subscribe to scan, I recommend to use rgbd_sync nodelet to pre-sync exactly camera topics before rtabmap. See this example: http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot#Kinect_.2B-_Odometry_.2B-_2D_laser

You can also try with odom_sensor_sync=true for rtabmap node.

cheers,
Mathieu

[tom haverford]

Hello Mathieu,

I have use the rgbd  sync nodelet and also made the odom_sensor_sync as 'true'. Compared to the above image the results are better. But the map still needs to be improved, for example if there are tables or desks in the environment, they are mapped a bit hazy and also not all the features are mapped.

Below are some of the things I tried after reading multiple posts.

1) In the 3D rendering section changed the maximum depth to 4.0m.
2) Set the odom topic so that the rtabmap uses the odometry from the wheel encoders.
3) Set subsrcibe_scan to 'true' and subscribe_depth to 'false'

Does using the  imu data from the d45i give more accurate robust map?Also I set icp odometry and visual odometry as false in the rtabmap.launch, should I set these also to true?  I am using the d435i along with a RpiLidar.
Do you have any suggestions on how to improve the map accuracy and what parameters should I mainly take into account to make the map more robust.

[matlabbe]

Do you have a resulting database to share?

If you do 2D mapping with (Reg/Force3DoF=true), IMU won't add much accuracy. Based on your cyan scan map in your above post, the map is already pretty accurate (from lidar point of view).

[tom haverford]

Hey,

This the rtabmap.db link : https://www.dropbox.com/s/88rystj7yshczaj/rtabmap.db?dl=0

[matlabbe]

There is something wrong with the depth registration on RGB camera. Is the depth camera registered to RGB camera? On left is the same RGB and depth image, we can see that they don't match. The resulting point cloud from the camera doesn't match with the scan.


What is used to simulate the RGB-D camera? Normally you could find a camera plugin that can provide the depth already registered to RGB camera.

[tom haverford]

I am using the D435 gazebo plugin. Here is the file. The Depth is mentioned at Line 87 and the RGB is defined at Line 21. Can the issue be due to the update rate difference, for Depth it is 90(Line 105) and for RGB it is 30 ( Line 40).

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137

<robot xmlns:xacro="http://ros.org/wiki/xacro"> <xacro:macro name="gazebo_d435" params="camera_name reference_link topics_ns depth_optical_frame color_optical_frame infrared1_optical_frame infrared2_optical_frame" > <!-- 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>1</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>1280</width> <height>720</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>${85.2*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>${85.2*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>60.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.1</rangeMinDepth> <rangeMaxDepth>15.0</rangeMaxDepth> <pointCloud>true</pointCloud> <pointCloudTopicName>depth/points</pointCloudTopicName> <pointCloudCutoff>0.5</pointCloudCutoff> </plugin> </gazebo> </xacro:macro> </robot>

I also created a new map with a lot of features in it. This is the link:

http://https://www.dropbox.com/s/88rystj7yshczaj/rtabmap.db?dl=0

Any suggestions would be of great help, trying to solve it for the past one week.

Thanks!

[matlabbe]

Hi,

Maybe you didn't feed the correct depth image to rtabmap. From this demo: https://gitioc.upc.edu/rostutorials/realsense_gazebo_plugin#run-the-point-cloud-demo

roslaunch realsense_gazebo_plugin realsense.launch

The color and depth should be already registered. Make sure the frame_id in depth's header is "color":

rostopic echo /realsense_plugin/camera/depth/image_raw/header
seq: 8659
stamp: 
  secs: 213
  nsecs:  38000000
frame_id: "color"

[tom haverford]

Hello Mathieu,

You are right! The depth and rgb image does not align. Here is the difference between the depth and rgb image using the d435 plugin.




The header got by rostopic echo / camera/ depth/ image_raw/ header

seq: 11121
stamp:
  secs: 722
  nsecs: 479000000
frame_id: "realsense_d435_depth_optical_frame"

Should I change the frame_id to color? Also,any suggestions on how to align the depth and rgb image?

[matlabbe]

Can you add the link to the D435 gazebo plugin you are using? The realsense link I've found in my previous post doesn't have d435 stuff.

[tom haverford]

Here is the link to the gazebo plugin for d435:
https://github.com/pal-robotics/realsense_gazebo_plugin

[matlabbe]

Hi,

The depth image is indeed not registered to color camera. We can register depth image to another camera with the depth_image_proc/register nodelet or by projecting the point cloud and reproject it using rtabmap_ros/pointcloud_to_depthimage. The second approach lets us adjust the resolution of the output depth image to be more dense.

There is my launch file (we should follow those instructions, I just copied the modified urdf over the one of the official realsense2_description package) launching both approaches:

<launch>

  <arg name="urdf" default="$(find realsense2_description)/urdf/test_d435_camera.urdf.xacro"/> 

  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="headless" value="false"/>
    <arg name="gui" value="true"/>
    <arg name="verbose" value="true"/>
  </include>
  
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(arg urdf)' " />
  <node pkg="gazebo_ros" type="spawn_model" name="spawn_model" args="-urdf -param /robot_description -model drone"/>
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>
  
  <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" />

  <node pkg="nodelet" type="nodelet" name="register"
        args="load depth_image_proc/register nodelet_manager">
    <remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
    <remap from="depth/camera_info" to="/camera/depth/camera_info"/>
    <remap from="depth/image_rect"  to="/camera/depth/image_raw"/>
  </node>
  
  
  <node pkg="nodelet" type="nodelet" name="points_xyz" 
        args="load rtabmap_ros/point_cloud_xyz nodelet_manager">
    <remap from="depth/image"       to="/camera/depth/image_raw"/>
    <remap from="depth/camera_info" to="/camera/depth/camera_info"/>
    <remap from="cloud"             to="/camera/points" />

    <param name="decimation" type="int" value="1"/>
  </node>
  
  <node pkg="nodelet" type="nodelet" name="pointcloud_to_depthimage" 
        args="load rtabmap_ros/pointcloud_to_depthimage nodelet_manager">
    <remap from="camera_info"     to="/camera/color/camera_info"/>
    <remap from="cloud"           to="/camera/points" />
    <remap from="image"           to="/camera/depth_registered" />

    <param name="approx" type="bool" value="true"/>
    <param name="fixed_frame_id" type="string" value="base_link"/>
    <param name="decimation" type="int" value="8"/>
    <param name="fill_holes_size" type="int" value="0"/>
  </node>
  
</launch>

The resulting registered depth images (top-left is gazebo, top-right is the color image, middle-right is the registered depth using depth_proc nodelet, bottom-right is the registered depth with rtabmap_ros and bottomleft is the raw depth image not registered):


cheers,
Mathieu

[BalajiRavi]

Hello Mathieu,
I am facing similar problem, I am using the same Gazebo plugin from pal robotics for D435i. But the topic aligned depth image is not being published and how to add them or configure. Kindly help me out