Improving slam_rtabmap.launch

[matlabbe]

Describe the bug
The slam_rtabmap.launch file can be improved. Here are some changes I propose (I was about to open a pull request but it seems we have to open an Issue before):

• Added IMU support (map is now aligned with gravity by default).
• Updated depth rectification interpolation method to NearestNeighbor instead of Linear. This avoids a lot of noisy points in the 3D point cloud. This would also fix image_proc bug for rectified depth images #103 at the same time.
• Added IR mode when setting color_enabled to false. This mode can be useful when mapping in the dark.
• Using depth registered to RGB camera instead of RGB registered to depth camera. This is useful for proper texturing of the 3D mesh when exporting the map.
• Use 720P instead of 1536P so that default parameters of rtabmap can work better.
• The RGB image has to be rectified, because of that, it adds black borders to RGB image. Those black borders would not affect visual odometry or loop closure detection, but it is affecting 3D point cloud reconstruction. I added this note: When using color camera, to avoid black borders in point clouds in rtabmapviz, set ROI ratios in Preferences->3D rendering to "0.05 0.05 0.05 0.05" under Map and Odom columns.. The black borders won't appear anymore.

Desktop (please complete the following information):

• OS: Ubuntu
• Version 18.04
• Package version : 6d575ce

Proposed file

<!--
Copyright (c) Microsoft Corporation. All rights reserved.
Licensed under the MIT License.
-->

<launch>

  <!-- If color_enabled is false, IR camera is used (may work better in dark areas). -->
  <!-- When using color camera, to avoid black borders in point clouds 
       in rtabmapviz, set ROI ratios in Preferences->3D rendering 
       to "0.05 0.05 0.05 0.05" under Map and Odom columns. -->
  <arg name="color_enabled" default="true"/> 

  <param name="robot_description"
    command="xacro $(find azure_kinect_ros_driver)/urdf/azure_kinect.urdf.xacro" />

  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- Start the K4A sensor driver -->
  <group ns="k4a" >

    <!-- Spawn a nodelet manager -->
    <node pkg="nodelet" type="nodelet" name="manager" args="manager" output="screen">
      <param name="num_worker_threads" value="16" />
    </node>

    <!-- Spawn an image_proc/rectify nodelet to rectify the RGB image -->
    <node pkg="nodelet" type="nodelet" name="rectify_rgb"
          args="load image_proc/rectify manager --no-bond"
          respawn="true">
      <remap from="image_mono"  to="rgb/image_raw" />
      <remap from="image_rect"  to="rgb/image_rect" />
    </node>
    
    <!-- Spawn an image_proc/rectify nodelet to rectify the depth image -->
    <node unless="$(arg color_enabled)" pkg="nodelet" type="nodelet" name="rectify_depth"
          args="load image_proc/rectify manager --no-bond"
          respawn="true">
      <param name="interpolation"  value="0" />
      <remap from="image_mono"  to="depth/image_raw" />
      <remap from="image_rect"  to="depth/image_rect" />
    </node>
    
    <!-- Spawn an image_proc/rectify nodelet to rectify the ir image -->
    <node unless="$(arg color_enabled)" pkg="nodelet" type="nodelet" name="rectify_ir"
          args="load image_proc/rectify manager --no-bond"
          respawn="true">
      <remap from="image_mono"  to="ir/image_raw" />
      <remap from="image_rect"  to="ir/image_rect" />
    </node>
    
    <node pkg="nodelet" type="nodelet" name="k4a_ros_bridge"
          args="load Azure_Kinect_ROS_Driver/K4AROSBridgeNodelet manager --no-bond"
          respawn="true">
      <param name="depth_enabled"     value="true" />
      <param name="depth_mode"        value="WFOV_2X2BINNED" />
      <param name="color_enabled"     value="$(arg color_enabled)" />
      <param name="color_resolution"  value="720P" />
      <param name="fps"               value="30" />
      <param name="point_cloud"       value="false" />
      <param name="rgb_point_cloud"   value="false" />
      <param name="required"          value="true" />
      <param name="imu_rate_target"   value="100" />
      <param name="rescale_ir_to_mono8"   value="true" />
    </node>

  </group>
  
  <!-- compute IMU quaternion -->
  <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
    <param name="use_mag" value="false" />
    <param name="publish_tf" value="false" />
    <param name="world_frame" value="enu" />
    <remap from="/imu/data_raw" to="/k4a/imu" />
  </node>
  
  <!-- Start rtabmap_ros node -->
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg     if="$(arg color_enabled)" name="rgb_topic"          value="/k4a/rgb/image_rect" />
    <arg     if="$(arg color_enabled)" name="depth_topic"        value="/k4a/depth_to_rgb/image_raw" />
    <arg     if="$(arg color_enabled)" name="camera_info_topic"  value="/k4a/rgb/camera_info" />
    <arg unless="$(arg color_enabled)" name="rgb_topic"          value="/k4a/ir/image_rect" />
    <arg unless="$(arg color_enabled)" name="depth_topic"        value="/k4a/depth/image_rect" />
    <arg unless="$(arg color_enabled)" name="camera_info_topic"  value="/k4a/ir/camera_info" />
    <arg name="approx_sync" value="true" />
    <arg name="frame_id"    value="camera_base" />
    <arg      if="$(arg color_enabled)" name="args" value="--delete_db_on_start --GFTT/MinDistance 7 --Vis/CorGuessWinSize 40 --Optimizer/GravitySigma 0.3" />
    <arg unless ="$(arg color_enabled)" name="args" value="--delete_db_on_start --Optimizer/GravitySigma 0.3" />
    <arg name="wait_imu_to_init"   value="true"/>
  </include>

</launch>

[ooeygui]

Thank you for the investigation and suggested fixes! These all sound like wonderful improvements.