Rtabmap mapping done but can't do relocalization

[zelda]

Hello Matlabbe
First of all, I really thank you for the great work. I am following the instructions from your site and am being fascinated.

However, I am stuck now.. ToT
I was following this instruction, http://wiki.ros.org/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot
Steps until Mapping was fine. I could build 3d map. Also, When I launch with localization:=true option, it works.
I visualized it  with $roslaunch rtabmap_ros demo_turtlebot_rviz.launch and downloaded the point clouds from
rtabmap.db, via checking boxes 1)Download map, 2)Download graph consequently.

Here comes the problem. First of all, downloaded 3d graph doesn't seem to match with turtlebot's current position and
direction. It is shown in  the picture.(corridor of map and corridor of point clouds have different direction). Should i always
set the turtlebot's position and direction to correct initial state?

At first I thought this would be solved if relocalization is done. But Most important problem here. My Turtlebot can't find
loop closure. I moved it via teleop, rotated around the exact place I used for mapping, but nothing happens.

I don't know why. I wanted to check .db file but I don't know how to.
1) Is there possibility that pictures during the mapping is not saved to .db file?
It is quite out-of-pint story, but when I made data_throttled topics following instructions here http://official-rtab-map-forum.206.s1.nabble.com/How-can-I-record-data-in-ROS-for-rtabmap-td74.html
and record it, and then check .bag with rosbag info, I couldn't find data_throttled topics in there, diffent with bag files I downloaded on your tutorial site. So I am always suspicious that whether pictures are being saved to .db file.
2)It is just one corridor way. Should it be effected with Loop closure?

The picture is when I started with localization option, visualize with rviz and download point cloud, without finding loop closure.
Thanks for reading.

[zelda]

Also, is there way to check the 2D map drawn in the .db file??

[matlabbe]

1) You may only have to click "Download map" checkbox to get the map (it includes "Download graph").

2) Is it the local costmap shown? A relocation is needed to correctly align the robot on the map. There is no initial state needed. Note that relocation will happen only when the robot revisits an area in the same orientation (i.e., camera facing the same direction as the traversal during mapping) and that enough visual features are matching. A nice example can be seen on this video on Pi Robot: https://www.youtube.com/watch?v=j8OYlSZcqMc

3) You can browse data saved in *.db by using rtabmap-databaseViewer tool:

$ rtabmap-databaseViewer ~/.ros/rtabmap.db

If the 3D map is constructed, images are saved (point clouds are regenerated by RGB and depth images saved in the database). To see the 2D map, open Graph View in database viewer.

cheers

[zelda]

Yes I impressively watched  https://www.youtube.com/watch?v=j8OYlSZcqMc
Actually, I already saw the video and was following it, but it didn't work as the same so I was uploading the
question.

1) Yes I read that I only have to click "Download map" to get the map from .db.
    But if I click it, it just get gray, and then turn bright within some time, nothing changing. At that point,
    if I click "Donwload graph", the point clouds appear. Am I doing something wrong?

2) Yes the picture shown is "Downloaded point cloud" and "local costmap".
Do I have lack of understading about the loop closure? I thought that in the statement

"robot revisits an area in the same orientation (i.e., camera facing the same direction as the traversal during mapping) and that enough visual features are matching",

combination of spot and orientation could be any where if I went during the mapping, so to relocalize,
I traversed the same course as when mapping, stop at one of that points and rotate like the one at movie, but relocaize didn't occur. I don't know why.

The reason I am asking about the initial state is this. As it is shown on the picture, corridor direction of local costmap is 90degree rotated from point cloud. If this problem disappears naturally, I wouldn't mind, but if it is reason of 'not getting relocalization', this is why I am wondering about this.

p.s1) When I am working with localization:=true, is there way to call the roslaunch mapping with selecting filename.db option?
p.2) Can I do the mapping process with priorly made .db file? Which means can I edit the pre-built rtab map?
p.s3) I really appreciate about the database tool! It's cool.

Thank you!

[zelda]

I checked the database. and it was like this(the picture!)

The image in Rviz is the image that robot is currently seeing. So I think that is quite similar situation,
but relocalization doesn't occur. I tried quite other places but none.

Thank you.

[matlabbe]

Hi,

Images like that (a lot of visual features) should trigger a loop closure. You can add rtabmap_ros/Info display in RVIZ and look for statistic called "Loop/Highest_hypothesis_value". When the value is >=0.11, a loop closure is detected. If this value is over 0.11 and relocation still not working, look in the terminal for possible warnings like "Loop closure rejected because ...".


1) When you click on "Download map", all images from the database are downloaded to RVIZ. If you are on WIFI and the map is large, the RVIZ message box may turn gray, just wait. This updates the MapCloud's cache with the old map(s). However, the clouds may disappear (but it is still in RVIZ cache) if rtabmap is running on the robot and that no relocation happened yet (empty graph is sent to RVIZ each second). On loop closure, the 3D map should reappear (with corresponding 2D occupancy grid) as now the new session is linked to old map(s).

p.s1) Yes, the argument is "database_path" (default is "~/.ros/rtabmap.db").

p.2) RTAB-Map supports multi-session mapping. If you omit "--delete_db_on_start" argument, rtabmap will load the database and starts a new map. On loop closure with a location in a previous maps in the database, it will merge the new map with the previous one. The localization mode does exactly the same, but it doesn't create a new map (i.e., the map published will be empty until a loop closure is found).

p.s3) great!

[zelda]

 
 
 
  Hello, I checked the Loop/Highest_hypothesis_value, and the value was over 0.11 some times. At the terminal, I got the message[ WARN] (2016-05-09 09:05:02.008) Rtabmap.cpp:1755::process() Rejected loop closure 151 -> 316: Cannot compute transform (cor=67 corrRatio=0.104687/0.300000) bunch of them. What should it be??Thanks for the other replies. (1), ps1), ps2)) They are wonderful :).
 
  edit)
Checking the above parts of the terminal , I also get these messages.
 
[ WARN] (2016-05-09 09:04:22.162) Rtabmap.cpp:1755::process() Rejected loop closure 124 -> 284: Not enough inliers 0/5 between 124 and 284

I also get this message frequently.

[ WARN] [1462752260.934984879]: rtabmap: Could not get transform from base_footprint to camera_rgb_optical_frame after 0.100000 second!

[matlabbe]

Hi,

1) The problem is mainly the first warning. Well, it is not super clear, but it means that ICP has under 30% correspondences when registering the fake laser scans from the kinect. You can either disable ICP refining or decrease the acceptance threshold. I think you are on version 0.10 as the default value is 30% (on 0.11 it is 20%), I'll give corresponding parameters for 0.10 and 0.11:

<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">

   <!-- Disable ICP refining -->
   <!-- v0.10 --> <param name="LccIcp/Type"  type="string" value="0"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
   <!-- v0.11 --> <param name="Reg/Strategy" type="string" value="0"/> <!-- 0=Vis, 1=Icp, 2=VisIcp -->

   <!-- Decrease ICP correspondence ratio to 15% for example -->
   <!-- v0.10 --> <param name="LccIcp2/CorrespondenceRatio" type="string" value="0.15"/>
   <!-- v0.11 --> <param name="Icp/CorrespondenceRatio"     type="string" value="0.15"/>

</node>

2) For the second warning, this is a correct one, there are just not enough visual correspondences to compute a good transformation between the two point of view, so the loop closure is rejected.

3) The third warning could be fixed following the "almost hidden" comment at the bottom of the tutorial page:

If there are many TF warnings, you can try to increase robot_state_publisher's publishing frequency in turtlebot_bringup/launch/includes/robot.launch.xml from 5 to 10 Hz.

cheers,
Mathieu

[zelda]

Hello, Really Thank you!

Change of launch file as you said worked!
Now I could successfully re-localize(Hooray!)

But I directly met another problems...
First of all, I changed the frequency at that file of location to 10Hz, but the TF warnings didn't disappear(keeps coming out). I changed the file of Turtlebot, because I launch mapping at turtlebot.

Second, 2D navigation doesn't work. Would it be due to first reason? It just gets new plan but never move or rotate. In hope that it would help for finding the problem, I will write all the warnings I got.

[ WARN] (2016-05-09 11:14:58.281) Rtabmap.cpp:904::process() Odometry is reset (identity pose detected). Increment map id to 1! (This appears quite a lot only when I start mapping).

[ WARN] (2016-05-09 11:20:16.382) util3d_registration.cpp:173::transformFromXYZCorrespondences() RANSAC refineModel: Refinement failed: got an empty set of inliers!
(According to your prior response that not enough inlier is not a problem, I thought this won't be also, but just in case).

[pcl::IterativeClosestPoint::computeTransformation] Not enough correspondences found. Relax your threshold parameters. Would it be help?

For last, when I relocalize, the map and 3d points appear. That's great but sometimes, within time(or if I go somewhere), point clouds and global map far from current robot position disappears. Is there way to preserve them??
(I once saw a global Error : new features found not in global costmap? (I'm not sure about the statement) (At first there isn't global error)).
(Sometimes, it even happens like this. When re-localization occurs, not all the 2D map and 3d point clouds appear.
I checked that the database file have entire point cloud and map of my lab with the tool rtabmap-databaseViewer, but
when re-localization occurs, not entire map appeared.)(Is there limit of memory or something?)

I am really appreciating your help. Couldn't thank more.

-----------------------Original Message-----------------------From: "matlabbe [via Official RTAB-Map Forum]" <[e-mail]>To: zelda <[e-mail]>Sent date: 2016-05-09 10:28:15 GMT +0900 (Asia/Seoul)Subject: RE: Re: Rtabmap mapping done but can't do relocalization
 
 
   Hi,
 
  1) The problem is mainly the first warning. Well, it is not super clear, but it means that ICP has under 30% correspondences when registering the fake laser scans from the kinect. You can either disable ICP refining or decrease the acceptance threshold. I think you are on version 0.10 as the default value is 30% (on 0.11 it is 20%), I'll give corresponding parameters for 0.10 and 0.11:
 
 
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap">

   
     
     

   
    
    

</node>

  2) For the second warning, this is a correct one, there are just not enough visual correspondences to compute a good transformation between the two point of view, so the loop closure is rejected.
 
  3) The third warning could be fixed following the "almost hidden" comment at the bottom of
  the tutorial page:
 
 
   
   
     If there are many TF warnings, you can try to increase robot_state_publisher's publishing frequency in turtlebot_bringup/launch/includes/robot.launch.xml from 5 to 10 Hz.
     
   
   cheers,
  Mathieu
   
   
   

[matlabbe]

Hi,

For Autonomous Navigation section, you can look if move_base is publishing velocity when dropping a goal in RVIZ:

$ rostopic echo /mobile_base/commands/velocity

If not look for move_base/planning errors on the terminal. Sometimes move_base would say that it cannot plan if there is a problem with receiving sensor messages. This topic should be somewhat connected to base controller of turtlebot.

Warnings:
You can show the TF tree to see actual frequency of each frame (to make sure that they are really at 10 Hz with the change):

$ rosrun tf view_frames
$ evince frames.pdf

If they are really at 10 Hz, maybe the wait_for_transform parameter is two close to 10 Hz (set to 0.1 s). You can increase wait_for_transform to 0.2.

a) "Odometry is reset (identity pose detected)." Until the robot is moving, it may appear a lot on start. This is fixed in recent versions.

b) "Refinement failed: got an empty set of inliers!" Yes, it is a normal warning as mentioned in previous post.

c) "Not enough correspondences found. Relax your threshold parameters" This is related to local loop closure detection using ICP. Actually, it is a warning coming from PCL library. Not the issue here for navigation.

For part of the map disappearing, it is related to memory management approach of RTAB-Map (explained in details in the papers). You can disable memory management to keep all point clouds, however map update time will not be bounded.

<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap">
   <param name="Rtabmap/TimeThr" type="string" value="0"/> <!-- 0 means memory management disabled -->
</node>

rtabmap sends always nodes contained in the current map of the Working Memory. If the Working Memory contains all nodes of the global map, then the full global map should be shown. However, when memory management is enabled and the time limit is reached, not all nodes would be in Working Memory (some are transferred in Long-Term Memory) so some parts may be missing in the published map. Note however that revisiting close areas of this missing parts would make them retrieved back into Working Memory, so publishing them.

When setting RTAB-Map in localization mode, you may safely set:

<!-- localization mode -->
<param     if="$(arg localization)" name="Rtabmap/TimeThr"       type="string" value="0"/>
<param unless="$(arg localization)" name="Rtabmap/TimeThr"       type="string" value="700"/>

<param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>

<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/> 

The above disables memory management in localization mode, set fixed memory and make sure that all nodes from Long-Term Memory (database) are in Working Memory.

cheers,
Mathieu

[zelda]

Thanks for the reviews.

1. Yes, when I set 2D navigation and do the $rostopic echo /mobile_base/commands/velocity,
it publishes the veloicity.(They are 0, because it is not moving).

I can't see any errors when doing the navigation.. It is just not moving..
(I usually wated errors/warnings at demo_turtlebot_mapping.launch terminal.
 I see some  [ERROR] [1462843004.190470220]: Kobuki : Timed out while waiting for serial data stream [/mobile_base].
 errors on minimal.launch terminal. Would it be problem?)

2. Oh actually it didn't change. Average of the frames connected to base_link is near 5Hz. Most recent transform is
about 0.145sec old. Hmm why??
(Add. I tried several times changing the robot.launch.xml, both Turtlebot and WorkStation, but the frequencies are still around 5Hz. I checked the similar files like mobile_base.launch.xml, but there weren't any frequnecy setting option like robot.launch.xml)

[matlabbe]

Hi,

1. Make sure you don't have a teleop node sending the velocity topic at the same time. Are you able to move the robot by teleoperation? EDIT: just saw on turtlebot tutorial that teleop and move_base can be used at the same time. Well, if you are able to teleoperate, move_base should be able to work too.

2. Try to increase wait_for_transform argument. Did you do "$ rosrun tf view_frames" on the workstation or on the robot?

[zelda]

2. I did $rosrun tf view_frames ont both workstation and robot, but the result was same..   Where I could find wait_for_transfrom argument?? (And is it abnormal for the frequency not changing despite I changed the 5.0 in robot.launch.xml to 10.0?)1. I was doing the teleop at the same time. When I drop 2D navigation goal, it doesn't move, but (teleop node was simultaneously turned on) if I do the teleop, it moves.Whoo.. I would be so happy if the robot moves when I do the 2D navigation, kk

[matlabbe]

The wait_for_transform parameter is here. For "robot.launch.xml", maybe it is not the one used by your robot. Try to track down where the robot_state_publisher is created in your launch files.

Make sure the goal topic sent is correctly sent to move_base.

If it can help, I tried again the Mapping and Navigation Tutorial (with robot.launch.xml frequency set to 10 Hz). I don't have the robot, so I should follow the last section:
1)

$ roslaunch turtlebot_bringup minimal.launch

I have 10 Hz frames:


2)

$ export TURTLEBOT_3D_SENSOR=kinect
$ roslaunch rtabmap_ros demo_turtlebot_mapping.launch args:="--delete_db_on_start" rgbd_odometry:=true
$ roslaunch rtabmap_ros demo_turtlebot_rviz.launch

3) Drop a 2D nav goal in RVIZ, show the local planner path for debugging (hide the robot model to see it, the green line below!)

$ rostopic echo /mobile_base/commands/velocity
---
linear: 
  x: 0.12500000298
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 0.347368433601
---
linear: 
  x: 0.0500000029802
  y: 0.0
  z: 0.0
angular: 
  x: 0.0
  y: 0.0
  z: 0.378947376264
...

Well, if I had a robot, it think it would move as the velocity values are sent.

cheers

[zelda]

Well, before checking others, I want to be sure with publish frequency.

I am currently using Turtlebot, with Kobuki base. I use "$roslaunch turtlebot_bringup minimal.launch" to start. (In Turtlebot)

turtlebot_bringup package is located in /opt/ros/indigo/share, so I go to there and check to minimal.launch file.

It is in /opt/ros/indigo/share/turtlebot_bringup/launch, and contains

<include file="$(find turtlebot_bringup)/launch/includes/robot.launch.xml">
    <arg name="base" value="$(arg base)" />
    <arg name="stacks" value="$(arg stacks)" />
    <arg name="3d_sensor" value="$(arg 3d_sensor)" />
  </include>
  <include file="$(find turtlebot_bringup)/launch/includes/mobile_base.launch.xml">
    <arg name="base" value="$(arg base)" />
    <arg name="serialport" value="$(arg serialport)" />
  </include>
  <include file="$(find turtlebot_bringup)/launch/includes/netbook.launch.xml">
    <arg name="battery" value="$(arg battery)" />
  </include>

So I changed the file /opt/ros/indigo/share/turtlebot_bringup/launch/robot.launch.xml,
value 5.0 to 10.0(I did all of them in Turtlebot)(But now I also changed the value of WS).

But After saving, closing all the terminals and turning back, I get
* /robot_state_publisher/publish_frequency: 5.0
at the launching.

Is there way to check where that parameter is getting used?
Is there any build or make that I had to do?

[zelda]

Oh my god 2D navigation started to work when I turned off the teleop node which was turned on with it.(Used to Re-localize)

That was the problem.. actually you previously said about it but I didn't check.. sorry. I thought
"EDIT: just saw on turtlebot tutorial that teleop and move_base can be used at the same time." meant that
I could use both of them at the same time without problem, but I think I misunderstood.

Your pictures were big help for debugging. At first I found strange when just with mapping and rviz nodes, 2d navigation
path were normally set. With teleop node, turtlebot never moves but when I do the ctrl+C on teleop node, it moves immediately.

Now all of my concerns with this topic have disappeared! I was concerned about the publish_frequency because that warning
could be the reason for 2D navigation not working well, but it wasn't so now it is not sever concern to me.

Really, thank you for your help :)

[matlabbe]

Great!

Maybe on my test the teleop node was not running because I wasn't on the robot. Good to know! I updated the wiki.

cheers,
Mathieu

[zelda]

Thank you! Pleasure that the content was updated there:)