How does localization mode work？

[zzzzzhw]

Hi

I am using rtabmap with kinect v2.

Seeing that the RTB has a localization mode, I want to try to create a map first, then load the map and localization in the map, can i do like this in rtabmap?

Also, i want to learn about how does localization mode work? Could you tell me which file in the source code i shoul check?

Thanks a lot for your work!

[matlabbe]

Hi,

Yes, you can do localization in a map created previously by rtabmap. See http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping#Localization_mode

The code is the same than loop closure detection. See Bayes Filter update section in Rtabmap.cpp: https://github.com/introlab/rtabmap/blob/ccbc802fe6354fe8593975384170ddf04976a5aa/corelib/src/Rtabmap.cpp#L1415-L1653

cheers,
Mathieu

[zzzzzhw]

Hi，Mathieu

Thanks a lot, i will check this.

Another problem is that how can I get the trajectory after loop closer optimization? I want to evaluate the result.

(the blue line in the map after loop detection)

 I tried to subscribe the /rtabmap/odom topic and plot the trajectory, but it is the trajectory before optimization.

cheers!

[matlabbe]

To get the blue line, subscribe to /rtabmap/mapGraph:

$ rostopic echo /rtabmap/mapGraph

To get the odometry pose corrected, you can use TF and get /map -> /base_link (i.e., the base frame):

$ rosrun tf tf_echo map base_link

For convenience, you can also do File->"Export poses" in rtabmapviz if you only need the poses afterwards (you may need to pause to get "Export poses" option enabled).

cheers,
Mathieu

[zzzzzhw]

Hi，thanks for your replay!

I have tried to "Export poses" and got it.

However, i wanted to set the update rate to 5.0Hz. After mapping,  when i did "Export poses", there was an error:



How can i solve this problem?

[matlabbe]

rtabmapviz may have missed some updates at 5 Hz. Update rtabmapviz cache by doing "Edit->Download all clouds (update cache)" and try again.

cheers,
Mathieu

[zzzzzhw]

Thx a lot！

Now, i am facing with another problem.

Using two kinect v2, I have build a map. I want to use this map do localization.

I checked here you refered；
http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping#Localization_mode

and changed something in my launch file:

<launch>

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

 
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera1_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0 -1.5707963267948966 /camera1_base_link /camera1_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="camera_base_to_camera2_tf"
      args="0.0 0.0 0.0  -1.5707963267948966 0.0 -1.5707963267948966 /camera2_base_link /camera2_link 100" />
 


 

     <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
      args="0.0 0.0 0.0 0.0 -0.17 0.0 /base_link /camera1_base_link 100" />
  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
      args="-0.1375 -0.233 0  -1.570796327 -0.4 -0.04  /base_link /camera2_base_link 100" />

   
   
   
    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync nodelet_manager">
      <remap from="rgb/image"       to="/camera1/qhd/image_color_rect"/>
      <remap from="depth/image"     to="/camera1/qhd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/camera1/qhd/camera_info"/>
      <remap from="rgbd_image"      to="/camera1/rgbd_image"/>
     
    </node>


    <node pkg="nodelet" type="nodelet" name="rgbd_sync2" args="load rtabmap_ros/rgbd_sync nodelet_manager">
   
   
      <remap from="rgb/image"       to="/camera2/qhd/image_color_rect"/>
      <remap from="depth/image"     to="/camera2/qhd/image_depth_rect"/>
      <remap from="rgb/camera_info" to="/camera2/qhd/camera_info"/>
      <remap from="rgbd_image"      to="/camera2/rgbd_image"/>
     
    </node>

       
  <group ns="rtabmap">
 
   
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
   
     
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
       
         
         
         
           
           

           
           
           
    </node>
 
   

    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen">
   
     
     
 
     
     
     
     
      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
     

     
       
 
 
         
         
         
    </node>
 
 
   
    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
   
     
     
     
     
     
     


      <remap from="rgbd_image0"       to="/camera1/rgbd_image"/>
      <remap from="rgbd_image1"       to="/camera2/rgbd_image"/>
    </node>
 
  </group>

  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>

</launch>


Then,I want to do Edit->"Download map" to download the map from the rtabmap node, but some errors occur:

/rtabmap/rtabmapviz subscribed to (approx sync):
   /rtabmap/odom,
   /camera1/rgbd_image,
   /camera2/rgbd_image,
   /rtabmap/odom_info
[ INFO] [1545574934.549221003]: rtabmapviz started.
[ INFO] [1545574939.090858246]: visual_odometry: paused!
[ERROR] [1545574939.092854262]: Can't call "pause" service
[ERROR] [1545574941.783595237]: Can't call "resume" service
[ INFO] [1545574941.785371218]: visual_odometry: resumed!
[ WARN] [1545574951.344341305]: Can't call "get_map_data" service
[ERROR] (2018-12-23 22:22:31.345) MainWindow.cpp:3754::processRtabmapEvent3DMap() Map received with code error 1!
[ INFO] [1545574951.357153149]: visual_odometry: paused!
[ERROR] [1545574951.357875625]: Can't call "pause" service

How can i solve this problem?

[matlabbe]

Not sure why rtabmapviz cannot call rtabmap services. Can you show the rqt_graph? Which rtabmap version are you using?

For the localization mode, when starting from a launch file, as shown in Advanced section of the referred tutorial:

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
   ...
   <param name="Mem/IncrementalMemory" type="string" value="false"/>
</node>

This will make rtabmap loading the previously created database saved at "~/.ros/rtabmap.db" and fix the memory (Mem/IncrementalMemory=false), or in other world doing only localization (no mapping).

Can you edit your post and add the "raw" xml tags around the code, not sure if we see the whole code here. For example, to use rgbd_image inputs, you should set subscribe_rgbd to true like those lines lines are missing:

<param name="subscribe_depth"  type="bool"   value="false"/>
<param name="subscribe_rgbd"   type="bool"   value="true"/>
<param name="rgbd_cameras"    type="int"    value="2"/>
<param name="frame_id" type="string" value="base_link"/>

cheers,
Mathieu

[zzzzzhw]

localization_with_map.launch

sorry, this is my launch file.

I want to load a map file, then do localizaiton in this map(use rosbag).

When i set use_sim_time to "False",I could load the map file.

But, i want to test with a rosbag, when i set use_sim_time "False", rtabmap can not receive my data.

How can i solve this problem?

Thanks!

[matlabbe]

How did you record the rosbag? which topics? what is the output of:

$ rosbag info [BAGNAME].bag

use_sim_time should be true when using a rosbag, make sure to add "--clock" when doing rosbag play:

$ roslaunch localization_with_map.launch localization:=true use_sim_time:=true
$ rosbag play --clock [BAGNAME].bag

Make sure you already have a map in ~/.ros/rtabmap.db before trying localization mode. Are you able to make a map with the rosbag (not in localization mode)?

$ roslaunch localization_with_map.launch localization:=false use_sim_time:=true
$ rosbag play --clock [BAGNAME].bag