3D map with RTAB, SICK, IMU and Husky A200

edit

I'm continuing my work on 3D map building as specified in my old topic.

Finally, I mounted my sensors (IMU, SICK and RealSense R200) on a mobile robot (Husky A200) an I'm doing some acquisitions outdoor; in particular, the IMU is mounted on the center of the robot frame, the SICK laser is mounted 10 centimeters at the top of the IMU sensor and the RealSense is mounted 40 centimeters on the top of the laser sensor.

Now, I have the /odometry/filtered topic which is a combination of wheels encoders and IMU data. I added the laser information in this way:

  <arg name="subscribe_scan"          default="true"/>
  <arg name="scan_topic"              default="/scan"/>

inside the rgbd_mapping.launch file and I run the RTAB algorithm with these commands:

$ roscore
$ rosparam set use_sim_time true
$ roslaunch rtabmap_ros rgbd_mapping.launch rgb_topic:=/camera/color/image_raw depth_registered_topic:=/camera/depth/image_raw camera_info_topic:=/camera/color/camera_info compressed:=true frame_id:=base_link rtabmap_args:="--delete_db_on_start" estimation:=1
$ rosbag play --clock test1.bag (the bag contains both realsense and laser data)

How can I add the filtered odometry which contains also the IMU data? Is there anything which I have to change/add in the rgbd_mapping.launch file?

Moreover, I tried to check the tf and frames for all sensors and I cannot see the laser frame. Moreover, the real sense frame is not a child of base_link.

How can I fix this frame problem? This is the frames.pdf file. The frame tree is suitable for the RTAB algorithm?

What happens if the robot has to face terrain disparity? The /odometry/filtered based on IMU sensor is able to compensate the disparity by adjusting the laser and the real sense data according with the tilt angle?

This is my rgbd_mapping.launch file:

<launch>
  <!-- Your RGB-D sensor should be already started with "depth_registration:=true".
        Examples:
           $ roslaunch freenect_launch freenect.launch depth_registration:=true 
           $ roslaunch openni2_launch openni2.launch depth_registration:=true -->

  <!-- Choose visualization -->
  <arg name="rviz"                    default="false" />
  <arg name="rtabmapviz"              default="true" /> 

  <!-- Corresponding config files -->
  <arg name="rtabmapviz_cfg"          default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
  <arg name="rviz_cfg"                default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />

  <arg name="frame_id"                default="camera_link"/>   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="time_threshold"          default="0"/>             <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
  <arg name="optimize_from_last_node" default="false"/>         <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="rtabmap_args"            default=""/>              <!-- delete_db_on_start, udebug -->
  <arg name="launch_prefix"           default=""/>              <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->

  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_registered_topic"  default="/camera/depth_registered/image_raw" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="compressed"              default="false"/>
  <arg name="convert_depth_to_mm"     default="true"/>

  <arg name="subscribe_scan"          default="true"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>

  <arg name ...