RTABMAP WITH GAZEBO PLUGINS - RGBD Odometry drift in z and y

[rodrigo55]

Hi,

I'm trying to implement visual odometry with a gazebo plugin for a depth and a color camera. Here is my plugin (libgazebo_ros_openni_kinect.so):

 
  <gazebo reference="camera_0_depth_frame">
    <sensor type="depth" name="depth_realsense">
      <update_rate>30.0</update_rate>
      <camera name="__default__">
        <horizontal_fov>1.38998</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>5</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.05</stddev>
        </noise>
      </camera>
      <plugin name="realsense_link_controller" filename="libgazebo_ros_openni_kinect.so">
        <baseline>0.05</baseline>
        <alwaysOn>true</alwaysOn>
        <updateRate>30.0</updateRate>
        <cameraName>camera0</cameraName>
        <depthImageTopicName>/camera0/depth/image_raw</depthImageTopicName>
        <depthImageInfoTopicName>/camera0/depth/camera_info</depthImageInfoTopicName>
        <pointCloudTopicName>/camera0/depth/points</pointCloudTopicName>
        <frameName>depth_cloud_frame</frameName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <pointCloudCutoffMax>5.0</pointCloudCutoffMax>
        <hackBaseline>0.0</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
        <CxPrime>0</CxPrime>
        <Cx>315.91</Cx>
        <Cy>245.70</Cy>
        <focalLength>383.78</focalLength>
      </plugin>
    </sensor>
  </gazebo>

 
  <gazebo reference="camera_0_color_frame">
    <sensor type="camera" name="color_realsense">
      <update_rate>30.0</update_rate>
      <camera name="__default__">
        <horizontal_fov>1.211956</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>5</far>
        </clip>
      </camera>
      <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>30.0</updateRate>
        <cameraName>camera0</cameraName>
        <imageTopicName>/camera0/color/image_raw</imageTopicName>
        <cameraInfoTopicName>/camera0/color/camera_info</cameraInfoTopicName>
        <frameName>camera_0_color_frame</frameName>
        <hackBaseline>0.0</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
        <CxPrime>0</CxPrime>
        <Cx>644.31</Cx>
        <Cy>365.17</Cy>
        <focalLength>923.95</focalLength>
      </plugin>
    </sensor>
  </gazebo>

My thinking is that I can fuse this visual odometry with robot_localization along with wheel odometry published (odom frame published by this already) by gazebo and another IMU gazebo plugin. This is my tf tree:



As for rtabmap, I've been trying to make it work but it shows strange behaviors when computing the visual odometry. I've added an optical rotation because the odometry seemed to be going forward in the z axis instead of the x axis:

 
  <arg name="use_static_transform"    value="true"/>
  <arg name="pi_2"                    value="1.5707963267948966"/>
  <arg name="optical_rotate"          value="0 0 0 -$(arg pi_2) 0 -$(arg pi_2)"/>
 
  <node if="$(arg use_static_transform)" pkg="tf" type="static_transform_publisher" name="camera_optical_tf"
    args="$(arg optical_rotate) ugv_base_link camera_0_link 20"/>

However, once rotated, there is still a drift in z and y:




I really don't understand what I'm doing wrong. I have a feeling that it might be an issue with the gazebo plugin so I wanted to ask if there was experience working with RTABMAP and gazebo, and which plugins have worked well.

I'm using the example launch for rtabmap, and here are the relevant bits:

 
  <arg name="stereo"          default="false"/>

 
  <arg name="rtabmapviz"              default="true" />
  <arg name="rviz"                    default="false" />

 
  <arg name="localization"            default="false"/>

 
  <arg name="use_sim_time"            default="true"/>
 

 
  <arg name="cfg"                     default="" /> 
  <arg name="gui_cfg"                 default="~/.ros/rtabmap_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />

  <arg name="frame_id"                default="ugv_base_link"/>     
  <arg name="odom_frame_id"           default=""/>               
  <arg name="map_frame_id"            default="map"/>
  <arg name="ground_truth_frame_id"   default=""/>     
  <arg name="ground_truth_base_frame_id" default=""/> 
  <arg name="namespace"               default="rtabmap"/>
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="queue_size"              default="10"/>
  <arg name="wait_for_transform"      default="0.2"/>
  <arg name="args"                    default=""/>             
  <arg name="rtabmap_args"            default="$(arg args)"/>   
  <arg name="launch_prefix"           default=""/>             
  <arg name="output"                  default="screen"/>       
  <arg name="publish_tf_map"          default="true"/>

-----------------------

  <arg name="visual_odometry"          default="true"/>         
  <arg name="icp_odometry"             default="false"/>         
  <arg name="odom_topic"               default="ugv_odom"/>         
  <arg name="vo_frame_id"              default="$(arg odom_topic)"/> 
  <arg name="publish_tf_odom"          default="false"/>
  <arg name="odom_tf_angular_variance" default="1"/>             
  <arg name="odom_tf_linear_variance"  default="1"/>             
  <arg name="odom_args"                default=""/>             
  <arg name="odom_sensor_sync"         default="false"/>
  <arg name="odom_guess_frame_id"        default=""/>
  <arg name="odom_guess_min_translation" default="0"/>
  <arg name="odom_guess_min_rotation"    default="0"/>
  <arg name="imu_topic"                default="/imu/data"/>         
  <arg name="wait_imu_to_init"         default="false"/>
er_rtabmap.launch


I'm really at a loss here and any help would be appreciated, I really want to make RTAB work with my localization setup.

Thank you!

[rodrigo55]

I've found the issue.

It turns out that my depth and color images coming from the gazebo plugin were sized differently, so what I did is used a node called depth_image_proc that registers the depth image onto the color one, making them the same size.