Problem with stereo outdoor mapping using rtabmap_ros

edit

Thx for the bag. The rendering problem is that in rtabmap_ros/MapCloud RVIZ display, the default decimation is 4. Set it to 1 or 2. Set also maximum depth to 30 meters instead of default 4 meters. You may also notice points under the road caused by the reflection on the car.

For the odometry lost problem, try this launch file based on yours (I included rtabmap.launch for convenience to avoid duplicating parameters between rtabmap and stereo_odometry nodes):

<launch>

   <!--
      Demo of outdoor stereo mapping. 
      From bag: 
      $ rosbag record 
            /stereo_camera/left/image_raw_throttle/compressed 
            /stereo_camera/right/image_raw_throttle/compressed 
            /stereo_camera/left/camera_info_throttle 
            /stereo_camera/right/camera_info_throttle 
            /tf

      $ roslaunch rtabmap demo_stereo_outdoor.launch
      $ rosbag play -.-clock stereo_oudoorA.bag
   -->

  <!-- Arguments -->
  <arg name="camera" default="/stereo"/>
  <arg name="frameid_robot" default="base_link"/>
  <!--OR: base_footprint -->
  <arg name="frameid_camera" default="stereo_camera"/>

  <!-- yaw, pitch, rooll -->
  <!-- Transformada entre base_link e IMU -->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_imu" args="0 0 0.3 0 0 0 base_link imu_frame 30"/>

<node pkg="tf" type="static_transform_publisher" name="base_link_to_base" args="0 0 0 0 0 0 base_footprint base_link 30"/>
  <!-- Transformada entre base_link e GPS-->
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_gps" args="0 0 0.5 0 0 0 base_link gps 30"/> 

  <!-- TF: "base_link" to "camera_link" [Rotate the camera frame.] -->
  <arg name="pi/2" value="1.5707963267948966"/>
  <arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0.0 -$(arg pi/2)"/>
  <node pkg="tf" type="static_transform_publisher" name="camera_to_gimbal" args="$(arg optical_rotate) stereo_camera_base $(arg frameid_camera) 30"/>

  <!-- Tranformada entre o Gimbal e Base_link -->
  <arg name="Gimbal_rotate" value="0 0 1.75 0 0.19 0"/>
  <node pkg="tf" type="static_transform_publisher" name="Gimbal_baselink" args="$(arg Gimbal_rotate) $(arg frameid_robot) stereo_camera_base 30"/>

  <!-- Tranformada entre o Camera Frame e Leftoptical -->
  <node pkg="tf" type="static_transform_publisher" name="left_optical2stereo_camera" args="0 0 0 0 0 0 stereo_camera left_optical 30"/>


   <param name="use_sim_time" type="bool" value="True"/>

   <!-- Just to uncompress images for stereo_image_rect -->
   <node name="republish_left"  type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
   <node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />

   <!-- Run the ROS package stereo_image_proc for image rectification -->
   <group ns="/stereo_camera" >
      <node pkg="nodelet" type="nodelet" name="stereo_nodelet"  args="manager"/>

      <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
         <remap from="left/image_raw"    to="left/image_raw_throttle_relay"/>
         <remap from="left/camera_info"  to="left/camera_info_throttle"/>
         <remap from="right/image_raw"   to="right/image_raw_throttle_relay"/>
         <remap from="right/camera_info" to="right/camera_info_throttle"/>
         <param name="disparity_range" value="128"/>
      </node>
   </group>

   <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
    <arg name="frame_id" value="base_footprint"/>
    <arg name="stereo" value="true"/>
    <arg name="approx_sync" value="true"/>
    <arg name="left_image_topic"        value="/stereo_camera/left/image_rect_color" />
    <arg name="right_image_topic"       value="/stereo_camera/right/image_rect" /> 
    <arg name="left_camera_info_topic"  value="/stereo_camera/left/camera_info_throttle" />
    <arg name="right_camera_info_topic" value="/stereo_camera/right/camera_info_throttle" />
    <arg name="rtabmap_args" value="--delete_db_on_start --Vis/EstimationType 1 --Vis/MaxDepth 0 --GFTT/QualityLevel 0.00001 --Stereo/MinDisparity 0 --Stereo/MaxDisparity 64 --Vis/RoiRatios '0 0 0 .2' --Kp/RoiRatios '0 0 0 .2' --Odom/GuessMotion true --Vis/MinInliers 10 --Vis/BundleAdjustment 1 ...