Rtabmap with 9dof Razor IMU + ZED camera

[abhishek]

Hi Mathieu,


I have a 9dof RAZOR-IMU. We want to utilize it for more accurate odometry computation while with RTABMAPS's RGB-D method  for ZED Camera.

We are using the ros pkg: https://github.com/KristofRobot/razor_imu_9dof to get the /imu data as below:

header:
  seq: 158
  stamp:
    secs: 1490853212
    nsecs:  84635972
  frame_id: base_imu_link
orientation:
  x: -0.168721903131
  y: 0.0422059879943
  z: -0.78545025713
  w: 0.593986083638
orientation_covariance: [0.0025, 0.0, 0.0, 0.0, 0.0025, 0.0, 0.0, 0.0, 0.0025]
angular_velocity:
  x: -0.05
  y: -0.05
  z: 0.02
angular_velocity_covariance: [0.02, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.02]
linear_acceleration:
  x: 1.92212921875
  y: -2.39251078125
  z: 8.78632921875
linear_acceleration_covariance: [0.04, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.04]


I have put the IMU sensor on top of zed's right camera.  so that imu's  X-axis is forward, Y-axis towards left & Z-axis in upward direction. Below is the sensor_fusion.launch file, just like given here :

<launch> 

 
 
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_imu_link" args=" 0.0 0 0.0 0 0.0 0.0  /camera_link /base_imu_link 100" />
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_camera_link_rgb" args=" 0 0.0 0 -1.5707963267948966 0.0 -1.5707963267948966  /camera_link  /zed_initial_frame 100" />
 
     
  <arg name="frame_id"                default="camera_link" />
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth/depth_registered" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="imu_topic"               default="/imu" />
  <arg name="imu_ignore_acc"          default="true" />
  <arg name="imu_remove_gravitational_acceleration" default="true" />

 
  <arg name="localization"      default="false"/>
  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/> 
 
  <group ns="rtabmap">
   
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/vo"/>

     
     
     
     

     
     
       
     
    </node>

   
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
     
 
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/odometry/filtered"/>
     
       

     
       
     
     
     
    </node>
  </group>

 
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

     
     
     

     
     
     

     

     
       

     
      <rosparam param="odom0_config">[true, true, false,
                                      false, false, false,
                                      true, true, true,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam     if="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     false,  false,  false] </rosparam>
      <rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true] </rosparam> 
     
     
     

     
     

     

     

     
     
       

     
      <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.0025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.004, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.001, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.002, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.001, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</rosparam>

     
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

    </node>


 
</launch>


 I start  Zed camera, IMU sensor & launch the above sensor_fusion.launch file. After this, I start the  RTABmap with RGB-D, such that visual_odometry option is false in the rtabmap.launch file & it uses /odometry/filtered.

roslaunch rtabmap_ros rtabmap.launch rtabmap_args:="--delete_db_on_start" depth_topic:=/camera/depth/depth_registered


Result: The point cloud map appears shaking a lot(drifting.) even if the camera+imu is stable. As in below image:
 


Am I missing something ? Is there anything wrong in tf or something?
Can you please suggest.

Thanks,
Abhishek

[matlabbe]

Hi,

put your code text between < raw > tags. It seems the launch file is not fully shown.

You can try with Vis/EstimationType=1, which gives smoother visual odometry. For the ekf filter, you can also increment the queue_size of each sensor to make sure to process them all.

<param name="odom0_queue_size" value="50"/>
<param name="imu0_queue_size" value="50"/>

I've been playing with my ZED and Brick imu with this launch file:

<launch> 

  <arg name="frame_id"                default="camera_link" />
  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
  <arg name="depth_topic"             default="/camera/depth/depth_registered" />
  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
  <arg name="imu_topic"               default="/imu/data" />
  <arg name="imu_ignore_acc"          default="true" />
  <arg name="imu_remove_gravitational_acceleration" default="false" />

  <arg name="rtabmap_args"  default="--delete_db_on_start --Vis/EstimationType 1 --Kp/FeatureType 6 --Odom/ResetCountdown 2"/>  

<group ns="camera">
   <node pkg="zed_wrapper" type="zed_wrapper_node" name="zed_wrapper_node"/>
</group>

<node pkg="imu_brick" type="imu_brick_node" name="imu_brick">
      <param name="frame_id"                       value="imu_link"/>
      <param name="period_ms"                      value="10"/>
      <param name="uid"              type="string" value="6KwwD7"/>
      <param name="cov_orientation"  type="double" value="0.0005"/>
      <param name="cov_velocity"     type="double" value="0.00025"/>
      <param name="cov_acceleration" type="double" value="0.1"/>
      <param name="remove_gravitational_acceleration" type="bool" value="true"/>
</node>

<node pkg="tf" type="static_transform_publisher" name="camera_imu" args=" -0.025 0.0 0.015 0 0 0  $(arg frame_id) imu_link 100" />
<node pkg="tf" type="static_transform_publisher" name="camera_zed" args=" 0 0 0 -1.5707963267948966 0 -1.5707963267948966  $(arg frame_id) zed_initial_frame 100" />
<node pkg="tf" type="static_transform_publisher" name="zed_left" args=" 0 0 0 0 0 0 zed_current_frame ZED_left_camera 100" />

  
  <group ns="rtabmap">
    <!-- Visual Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/vo"/>

      <param name="frame_id"               type="string" value="$(arg frame_id)"/>
      <param name="publish_tf"             type="bool"   value="false"/>
      <param name="publish_null_when_lost" type="bool"   value="false"/>
      <param name="guess_from_tf"          type="bool"   value="false"/>
    </node>

    <!-- SLAM -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
 
      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
      <remap from="depth/image"     to="$(arg depth_topic)"/>
      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
      <remap from="odom"            to="/odometry/filtered"/>      
    </node>
  </group>

  <!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

      <param name="frequency" value="50"/>
      <param name="sensor_timeout" value="0.1"/>
      <param name="two_d_mode" value="false"/>

      <param name="odom_frame" value="odom"/>
      <param name="base_link_frame" value="$(arg frame_id)"/>
      <param name="world_frame" value="odom"/>

      <param name="transform_time_offset" value="0.0"/>

      <param name="odom0" value="/vo"/>
      <param name="imu0" value="$(arg imu_topic)"/> 

      <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="odom0_config">[true, true, true,
                                      false, false, false,
                                      true, true, true,
                                      false, false, false,
                                      false, false, false]</rosparam>

      <rosparam     if="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     false,  false,  false] </rosparam>
      <rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
                                     false, false, false,
                                     true,  true,  true,
                                     false, false, false,
                                     true,  true,  true,
                                     true,  true,  true] </rosparam>  
      
      <param name="odom0_differential" value="true"/>
      <param name="imu0_differential" value="false"/>

      <param name="odom0_relative" value="true"/>
      <param name="imu0_relative" value="false"/>

      <param name="imu0_remove_gravitational_acceleration" value="$(arg imu_remove_gravitational_acceleration)"/>

      <param name="print_diagnostics" value="true"/>

      <!-- ======== ADVANCED PARAMETERS ======== -->
      <param name="odom0_queue_size" value="50"/>
      <param name="imu0_queue_size" value="50"/> 

      <!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
           vroll, vpitch, vyaw, ax, ay, az. -->
      <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0.0025, 0,    0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.004, 0,    0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.001, 0,    0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 0,    0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.002, 0,    0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.001, 0,    0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</rosparam>

      <!-- The values are ordered as x, y,
           z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

</node>
 
</launch>

The biggest problem is how to handle when odometry gets lost, this is when there is a lot of drift in position (the angle is very accurate though, thx to IMU that cannot get lost). Note that I've put the IMU over the left camera (roughly aligned), as the rgb image is the left camera. Probably the TF between the camera and imu should be also precise to get good fusion. I'm not an expert in sensor fusion, you may find more help on ROS answers on that subject.

cheers,
Mathieu