How to localize in a previously created map using ZED, Laser and odom in RTABMap_ros?

asked 2018-05-03 05:07:15 -0600

shiveshkhaitan
23 ●4 ●6 ●9

updated 2018-05-04 00:39:08 -0600

I am able to create a rtabmap using zed, laser and odom according to Kinect + Odometry + 2D laser in this tutorial http://wiki.ros.org/rtabmap_ros/Tutor...

But this tutorial does not tell how to localise once we start the run again. This tutorial http://wiki.ros.org/rtabmap_ros/Tutor...

tells how to localise but that way is not working with the previous Kinect + Odom +2D Laser approach. How do I localize?

This is what my launch file launches

 <!-- Localization-only mode -->
     <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"/>

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

      <param name="frame_id" type="string" value="base_link"/>
      <param name="subscribe_depth" type="bool" value="true"/>
      <param name="subscribe_scan" type="bool" value="true"/>
      <remap from="rgb/image" to="/zed/left/image_rect_color"/>
      <remap from="depth/image" to="/zed/depth/depth_registered"/>
      <remap from="rgb/camera_info" to="/zed/left/camera_info"/>
      <remap from="/rtabmap/odom" to="/odom"/>
      <remap from="/rtabmap/scan" to="/scan"/>
      <param name="queue_size" type="int" value="1"/>

      <remap from="grid_map" to="map"/>
      <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>
      <param name="RGBD/ProximityBySpace"     type="string" value="true"/>
      <param name="RGBD/AngularUpdate"        type="string" value="0.01"/>
      <param name="RGBD/LinearUpdate"         type="string" value="0.01"/>
      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>
      <param name="Optimizer/Slam2D"          type="string" value="true"/>
      <param name="Reg/Strategy"              type="string" value="1"/> 
      <param name="Reg/Force3DoF"             type="string" value="true"/>
      <param name="Vis/MinInliers"            type="string" value="5"/>
      <param name="Vis/InlierDistance"        type="string" value="0.1"/>
      <param name="Rtabmap/TimeThr"           type="string" value="700"/>
      <param name="Mem/RehearsalSimilarity"   type="string" value="0.45"/>
       <!-- localization mode -->
       <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
       <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>
</node>

This is what I get when I set localization to true.

[ WARN] (2018-05-04 10:58:12.928) Rtabmap.cpp:990::process() Update map correction based on last       localization saved in database! correction = xyz=-39.871330,14.157864,0.835705 rpy=-0.038996,-0.008449,-0.733314, nearest id = 173 of last pose = xyz=-10.255625,18.225515,0.453622 rpy=0.039121,0.003428,2.411816, odom = xyz=19.276752,22.848824,0.346089 rpy=0.000095,-0.004879,-3.137862

[ WARN] (2018-05-04 10:58:13.163) Rtabmap.cpp:1913::process() Rejected loop closure 9 -> 174: Cannot compute transform (converged=false var=1.000000)

[ WARN] [1525411693.255677825, 4.199000000]: Many occupancy grids should be loaded (~167), this may take a while to update the map(s)...

[ WARN] (2018-05-04 10:58:13.800) MapsManager.cpp:497::updateMapCaches() Occupancy grid for location 94 should be added to global map (e..g, a ROS node is subscribed to any occupancy grid output) but it cannot be found in memory. For convenience, the occupancy grid is regenerated. Make sure parameter "RGBD/CreateOccupancyGrid" is true to avoid this warning for the next locations added to map. For older ...

(more)

answered 2018-05-03 11:13:44 -0600

matlabbe

6409 ●11 ●96 ●80

updated 2018-05-04 08:33:37 -0600

Hi,

Based on the robot mapping demo, which is similar to your setup, localization is set like this with custom.launch:

<launch>
  <!-- Localization-only mode -->
  <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"/>

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

    ...

    <!-- localization mode -->
    <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
    <param if="$(arg localization)" name="Mem/InitWMWithAllNodes" type="string" value="true"/>
  </node>
</launch>

Launch in mapping mode (delete previous database):

$ roslaunch custom.launch

Launch in localization mode:

$ roslaunch custom.launch localization:=true

EDIT: By default (with version >=0.17.0), on localization mode the robot will assume that it is restarting at the same place where it stopped mapping previously. If the robot is still moved when offline, the first pose in the map will then be wrong, but while moving, if the robot is able to localize visually (loop closure), it will replace the robot at the right location. If you don't want that the robot assumes that it is starting at the last location of the previous session (i.e., robot restarts always at different locations each run) you can add this parameter:

<param name="RGBD/SavedLocalizationIgnored" type="string" value="true"/>

In this mode, the map will not appear until the robot can localize on the previous map.

cheers,
Mathieu

edit flag offensive delete link

Comments

I edited my file according to your answer. The new launch file is as above. The problem is that when I set it to localization in the second run it loads the map and starts from exactly the same place where it was shut in the first run and doesnt localize.

shiveshkhaitan ( 2018-05-04 00:02:22 -0600 )edit

Updated answer!

matlabbe ( 2018-05-04 08:34:36 -0600 )edit

Thank you.. Works for me.. But the result from localization is not that great.. It can hardly localize if the robot is moved more than 0.5 meters.

shiveshkhaitan ( 2018-05-07 19:47:32 -0600 )edit

Do you mean 0.5 meters from the nearest node in the map? Yes, if point of view is different, it will be more challenging to localize. As you have a lidar, proximity detections should help to localize after a visual loop closure is found.

matlabbe ( 2018-05-08 10:48:21 -0600 )edit

hi, it seems this parameter has changed into RGBD/StartAtOrigin. I changed this parameter into true in my parameters.h file, but again i have the error. Just to note that i donot have camera, and the main sensor that i use is 3D Lidar. so how can i solve the problem of kidnapped robot? (it might happen that we move the robot after closing rtabmap, so this will raise an error in location of the vehicle)

thanks

Delbina ( 2021-07-13 05:56:20 -0600 )edit

Without a camera, it cannot resolve the kidnapped robot problem. You can use /rtabmap/initialpose topic to set an initial pose manually. This can be used with 2D pose estimate button from RVIZ by remapping the correct topic in the tool settings.

matlabbe ( 2021-07-29 08:15:00 -0600 )edit

add a comment

How to localize in a previously created map using ZED, Laser and odom in RTABMap_ros?

1 Answer

Comments

Question Tools

Stats

Related questions

How to localize in a previously created map using ZED, Laser and odom in RTABMap_ros? edit

1 Answer

Comments

Question Tools

Stats

Related questions

How to localize in a previously created map using ZED, Laser and odom in RTABMap_ros?