Stereo 3D-mapping jump

[aronlin1997]

Hi,

I used installed a stereo camera to a simulated husky robot and do the stereo-3D mapping. However, everytime when I start the mapping, the 2D map will always jump to a different location.

<launch>
      <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>
      <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
         <param name="disparity_range" value="128"/>
         <remap from="left/image_raw"    to="/multisense_sl/camera/left/image_raw"/>
         <remap from="left/camera_info"  to="/multisense_sl/camera/left/camera_info"/>
         <remap from="right/image_raw"   to="/multisense_sl/camera/right/image_raw"/>
         <remap from="right/camera_info" to="/multisense_sl/camera/right/camera_info"/>
         <param name="disparity_range" value="128"/>
      </node>

	<node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
      <remap from="left/image_rect"       to="/left/image_rect"/>
      <remap from="right/image_rect"      to="/right/image_rect"/>
      <remap from="left/camera_info"      to="/multisense_sl/camera/left/camera_info"/>
      <remap from="right/camera_info"     to="/multisense_sl/camera/right/camera_info"/>
      <remap from="odom"                  to="/stereo_odometry"/>

      <param name="frame_id" type="string" value="base_link"/>
      <param name="odom_frame_id" type="string" value="/odom"/>
      <param name="Rtabmap/DetectionRate" type="string" value="1"/>
      <param name="Rtabmap/LoopThr" type="string" value="1000"/>
      <param name="Odom/Strategy" type="string" value="0"/> <!-- 0=BOW, 1=OpticalFlow -->
      <param name="Odom/EstimationType" type="string" value="1"/> <!-- 3D->2D (PnP) -->
      <param name="Odom/MinInliers" type="string" value="15"/>
      <param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
      <param name="Odom/FilteringStrategy" type="string" value="2"/>
      <param name="OdomBow/NNDR" type="string" value="0.8"/>
      <param name="Odom/MaxFeatures" type="string" value="1000"/>
      <param name="OdomF2M/BundleAdjustmentMaxFrames" type="string" value="0"/>
      <param name="GFTT/MinDistance" type="string" value="10"/>
      <param name="GFTT/QualityLevel" type="string" value="0.00001"/> 
	</node>
   <!-- Backward compatibility launch file, use "rtabmap.launch rgbd:=false stereo:=true" instead -->

   <!-- Your camera should be calibrated and publishing rectified left and right 
        images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
        Example:
           $ roslaunch rtabmap_ros bumblebee.launch -->
 
  <!-- Choose visualization -->
  <arg name="rtabmapviz"              default="true" /> 
  <arg name="rviz"                    default="false" />
  
  <!-- Localization-only mode -->
  <arg name="localization"            default="false"/>
  
  <!-- Corresponding config files -->
  <arg name="rtabmapviz_cfg"          default="$(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
  <arg name="rviz_cfg"                default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
  
  <arg name="frame_id"                default="base_footprint"/>     <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
  <arg name="database_path"           default="~/.ros/rtabmap.db"/>
  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- delete_db_on_start, udebug -->
  <arg name="launch_prefix"           default=""/>
  <arg name="approx_sync"             default="false"/>         <!-- if timestamps of the input topics are not synchronized -->
  
  <arg name="stereo_namespace"        default="/stereo_camera"/>
  <arg name="left_image_topic"        default="/left/image_rect_color" />
  <arg name="right_image_topic"       default="/right/image_rect" />      <!-- using grayscale image for efficiency -->
  <arg name="left_camera_info_topic"  default="/multisense_sl/camera/left/camera_info" />
  <arg name="right_camera_info_topic" default="/multisense_sl/camera/right/camera_info" />
  <arg name="compressed"              default="false"/>
   
  <arg name="subscribe_scan"          default="false"/>         <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
  <arg name="scan_topic"              default="/scan"/>
   
  <arg name="subscribe_scan_cloud"    default="false"/>         <!-- Assuming 3D scan if set -->
  <arg name="scan_cloud_topic"        default="/scan_cloud"/>
  
  <arg name="visual_odometry"         default="false"/>          <!-- Generate visual odometry -->
  <arg name="odom_topic"              default="/stereo_odometry"/>         <!-- Odometry topic used if visual_odometry is false -->
  <arg name="odom_frame_id"           default=""/>              <!-- If set, TF is used to get odometry instead of the topic -->
  
  <arg name="namespace"               default="rtabmap"/>
  <arg name="wait_for_transform"      default="0.2"/>
  
  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="stereo"                  value="true"/>
    <arg name="rtabmapviz"              value="$(arg rtabmapviz)" /> 
    <arg name="rviz"                    value="$(arg rviz)" />
    <arg name="localization"            value="$(arg localization)"/>
    <arg name="gui_cfg"                 value="$(arg rtabmapviz_cfg)" />
    <arg name="rviz_cfg"                value="$(arg rviz_cfg)" />
  
    <arg name="frame_id"                value="$(arg frame_id)"/>
    <arg name="namespace"               value="$(arg namespace)"/>
    <arg name="database_path"           value="$(arg database_path)"/>
    <arg name="wait_for_transform"      value="$(arg wait_for_transform)"/>
    <arg name="rtabmap_args"            value="$(arg rtabmap_args)"/>  
    <arg name="launch_prefix"           value="$(arg launch_prefix)"/>          
    <arg name="approx_sync"             value="$(arg approx_sync)"/>

    <arg name="stereo_namespace"        value="$(arg stereo_namespace)"/>
    <arg name="left_image_topic"        value="$(arg left_image_topic)" />
    <arg name="right_image_topic"       value="$(arg right_image_topic)" />
    <arg name="left_camera_info_topic"  value="$(arg left_camera_info_topic)" />
    <arg name="right_camera_info_topic" value="$(arg right_camera_info_topic)" />
  
    <arg name="compressed"              value="$(arg compressed)"/>                                                                                
   
    <arg name="subscribe_scan"          value="$(arg subscribe_scan)"/>
    <arg name="scan_topic"              value="$(arg scan_topic)"/>
  
    <arg name="subscribe_scan_cloud"    value="$(arg subscribe_scan_cloud)"/>
    <arg name="scan_cloud_topic"        value="$(arg scan_cloud_topic)"/>
   
    <arg name="visual_odometry"         value="$(arg visual_odometry)"/>          
    <arg name="odom_topic"              value="$(arg odom_topic)"/>       
    <arg name="odom_frame_id"           value="$(arg odom_frame_id)"/>   
    <arg name="odom_args"               value="$(arg rtabmap_args)"/>
  </include>



</launch>

Attached is localization code. Could you please help me figure out why it jumps all the time?

[matlabbe]

Can you record the TF tree? maybe you have another node publishing odom or map frames.

rosrun tf view_frames

[aronlin1997]

Hi Attached is the TF from the above code

[matlabbe]

Can you share the database? maybe there is something wrong with camera calibration.