rtabmap mapping

[yyz]

Mathieu,
I have a problem with mapping, when the machine goes forward, the map like this


when the machine goes back, the map like this


and sometimes the map disappear and remapping

I don't know why,
this is the raw image:


Have a nice day,
Laraine

[matlabbe]

what sensors are you using? wheel odometry, Kinect, LiDAR? which launch files are you using?

What I see is that odometry is drifting and no loop closures have been found to correct the map. If you have a single camera for mapping, loop closures cannot be found when traveling in different direction the same corridor.

[yyz]

I use only a Kinect.

my launch file

<launch>
 
     
    <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"/>
     
        <include file="$(find freenect_launch)/launch/freenect.launch">
                <arg name="depth_registration" value="true"/>               
        </include>
       

    <node pkg="tf" type="static_transform_publisher" name="camera" args="0.25 0 0.12 0 0 0 base_link camera_link 100"/>
       
   
    <node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan">
                <remap from="image"        to="/camera/depth_registered/image_raw"/>
                <remap from="camera_info"  to="/camera/depth_registered/camera_info"/>
         
        </node>   
       
   
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen">
      <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
      <remap from="odom"            to="/odom"/>

     
     
         
           
     
     
     </node>
       
     
  <group ns="rtabmap">
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
     
     
           
     
     

      <remap from="odom"    to="/odom"/>
      <remap from="scan"    to="/scan"/>
      <remap from="mapData" to="mapData"/>

      <remap from="rgb/image"       to="/camera/rgb/image_rect_color"/>
      <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
      <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>

             
     
      <remap from="grid_map"  to="/rtabmap/grid_map"/>

     
                 
     
 
                         
         
       
       
     
         
     
     
     
     
     
     
     

     

     
     
           
 
     
     
     
         
     
     
     
           

     
     
     
     
    </node>
  </group>
       
      <remap from="map"                   to="/rtabmap/grid_map"/>       
     
          <remap from="scan"                  to="/scan"/>
          <remap from="obstacles_cloud"       to="/obstacles_cloud"/>
          <remap from="ground_cloud"          to="/ground_cloud"/>
         
          <node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen">
           
                <rosparam file="$(find rtabmap_nav)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/>
                <rosparam file="$(find rtabmap_nav)/config/costmap_common_params.yaml" command="load" ns="local_costmap"/>
                <rosparam file="$(find rtabmap_nav)/config/local_costmap_params.yaml" command="load" ns="local_costmap"/>
                <rosparam file="$(find rtabmap_nav)/config/global_costmap_params.yaml" command="load" ns="global_costmap"/>
                <rosparam file="$(find rtabmap_nav)/config/base_local_planner_params.yaml" command="load"/>
          </node> 

    <group ns="camera">
     <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
     
         

      <remap from="rgb/image_in"        to="rgb/image_rect_color"/>
      <remap from="depth/image_in"      to="depth_registered/image_raw"/>
      <remap from="rgb/camera_info_in"  to="depth_registered/camera_info"/>
   
      <remap from="rgb/image_out"       to="data_resized_image"/>
      <remap from="depth/image_out"     to="data_resized_image_depth"/>
      <remap from="rgb/camera_info_out" to="data_resized_camera_info"/>
    </node>

   
   
    <node pkg="nodelet" type="nodelet" name="points_xyz_planner" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
      <remap from="depth/image"        to="data_resized_image_depth"/>
      <remap from="depth/camera_info"  to="data_resized_camera_info"/>
      <remap from="cloud"              to="cloudXYZ"/>

     
     
     
    </node>
 
    <node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection camera_nodelet_manager">
      <remap from="cloud"     to="cloudXYZ"/>
      <remap from="obstacles" to="/obstacles_cloud"/>
      <remap from="ground"    to="/ground_cloud"/>

                     
     
     
     
     
     
    </node>
   </group>
 

</launch>

what sensors should I use if I want to  loop closures can be found when traveling in different direction the same corridor.

[matlabbe]

Hi,

The most robust, easiest to integrate and with lowest computation cost is a LiDAR (with >180 deg of field of view). If you have a very good computer, you may try with a second kinect looking backward.

If you look at this video from this demo, the LiDAR is used to re-localize on the map when travelling back the same corridor (from 0:45).

In contrast, this video has laser scan localization disabled, so the map is corrected only at the very end (when the camera is looking at the same direction than old locations in the map).

cheers,
Mathieu

[yyz]

Hi,Mathieu

I don't have a LiDAR.And I think I need the effect of the second video.Do you have a reference launch file?
And I also have some questions.
1) You said that my odometry is drifting before.I used a visual odometery ,not wheel odometry.What should I do if I want to make my odometry
not drift.

2)You said that no loop closures have been found to correct the map.I don't know how to use it.Can you give me some examples?

Thank you very much.

Have a nice day,
Laraine

[matlabbe]

Hi,

I don't have a specified launch file for the second video, if your robot doesn't subscribe to a laser scan topic, the default parameters should produce the same result. If subscribed to a laser scan, to disable local scan matching, set parameter "RGBD/ProximityBySpace" to false or "RGBD/ProximityPathMaxNeighbors" to 0.

1) Odometry will always drift, more or less depending on the sensor(s) used and the approach to estimate it.

2) Loop closure detection is automatic. See at 1:42 on the second video. Loop closures happen when two images are detected as the same location, then the map is corrected with the new constraint. See this paper about loop closure detection.

cheers,
Mathieu

[yyz]

Hi,Mathieu

I have got a better map.But when navigating,the machine flicked forward.
However, when the machine turns,the machine runs smoothly.I send the speed every 30ms.

And sometimes the machine drag racing.This is my video.https://youtu.be/bDPwOR0qOYU
Have a nice day,
Laraine

[matlabbe]

It is maybe your robot controller. You should have some safety in your robot controller to avoid high acceleration. I assume you are sending command velocity (geometry_msgs/Twist) with teleop joystick node or are you using move_base?

[yyz]

I am using move_base and my base_local_planner_params file:

TrajectoryPlannerROS:

 max_vel_x: 0.5
 min_vel_x: 0.24

 #max_rotational_vel: 0.15
 #min_in_place_ratational_vel: 0.01
 min_in_place_vel_theta: 0.25
 max_vel_theta: 0.5
 min_vel_theta: -0.5
 acc_lim_theta: 4
 acc_lim_x: 0.75
 acc_lim_y: 0.75
 holonomic_robot: true


 xy_goal_tolerance: 0.25
 yaw_goal_tolerance: 0.25
 latch_xy_goal_tolerance: true

 sim_time: 1.5
 sim_granularity: 0.025
 angular_sim_granularity: 0.05
 vx_samples: 12
 vtheta_samples: 20

 meter_scoring: true

 pdist_scale: 0.7
 gdist_scale: 0.8
 occdist_scale: 0.01
 publish_cost_grid_pc: false

 controller_frequency: 10

 NavfnROS:
  allow_unknown: true
  visualize_potential: false