Localization accuracy and some questions...

[g.bartoli]

Hi Mathieu,
we are currently making some tests with the Wheel Odometry + Asus Xtion setup.
I prepared the mapping and localization launch files for both phases and everything seems to work fine. All the processes run on the robot except the teleoperation that is running from another desktop computer.

Now we would like to measure the real localization accuracy within our environment, so I have some questions... I'll try be as concise as possible! ;)

1) Is the visual odometry fused with wheel odometry during SLAM or is it ignored? I noticed that point clouds are aligned only using wheel odometry and then corrected only when loop closures are detected, is this true? In this configuration the "Odometry lost" error is impossible, right?

2) Is the "base_link" tf frame the actual (corrected) robot pose? It seems that the tf coordinates do not change after graph optimizations, only the map is changed (I think it's correct, because it avoids rapid changes in estimated robot position during navigation). Does "RGBD/OptimizeFromGraphEnd" change this behaviour?

3) What is the difference between "proj_map" and "cloud_map" published topics? It seems that the former is not optimized and not correctly aligned with the current robot pose. I noticed this in RViz by using the MapCloud plugin and the "Download map" action versus just adding the visualization of PointCloud2 topic.

4) Does the "reset" service actually resets odometry and try to relocalize or it just deletes the cached maps and starts from scratch?

5) During the localization mode, I noticed that when the robot is stationary and the map has been already optimized (the main closures has been successfully found), the graph still "oscillates" a bit, like the TORO optimization is continually finding local minima and never stabilizes going back and forth between two or three almost stable configurations. You talked about this change you made, but how can this problem be mitigated? I tried to increment TORO iterations and error threshold, but it still very unstable, maybe disable Robust Graph Optimization?

6) When a navigation goal will be set up while the map will be optimized, will the goal position accordingly be corrected?

Furthermore, I'm thinking about this setup for measuring metric accuracy:

1) Do a full teleoperated mapping session and save the database
2) Bring the robot in a new location and put some markers on the ground for reference
3) Restart in localization mode and wait for the robot to be relocalized within the map
4) Take note (measure in RViz?) of the distance of the robot base from some known physical landmarks
5) Teleoperate the robot to explore the environment and then come back to the initial (referenced) position
6) Measure again the distance from the known physical landmarks and evaluate difference against point (4)

What do you think about this? Is this a correct workflow or do you have some suggestions?

Many thanks,
Guido

[matlabbe]

Hi Guido,

1) Visual odometry is not used if you connected directly wheel odometry to rtabmap node. In this case, "Odometry Lost" will never happen (if your wheel odometry always send non-null values), but slipping wheels can happen without "noticing" it! If you want to fuse wheel odometry with visual odometry, you should do that with an external node outputting the odometry fused. See robot_pose_ekf for an example of such node (note that if you use this node, you will need to convert "geometry_msgs/PoseWithCovarianceStamped" output to "nav_msgs/Odometry").

2) On graph optimizations, only /map -> /odom changes, not /odom -> /base_link. If you want the corrected pose, you have to transform /base_link in /map frame. "RGBD/OptimizeFromGraphEnd" changes this behavior: If true, /map -> /odom will be always identity (no correction) as it is the whole map that is corrected from the latest odometry pose. The effect is that the map will "drift" over time, but it will be always valid relativity to the latest odometry pose. Particularly useful when we have multiple sessions to avoid jumping between session origins on localization.

3) Do you mean "mapData" vs "cloud_map"? "proj_map" is the 2D projection of the "cloud_map" on the ground to create an occupancy grid map. They ("proj_map", "grid_map" and "cloud_map") should be aligned together. "mapData" (used by MapCloud Plugin) is a lightweight topic sending only the latest node data with the graph. The MapCloud plugin incrementally builds the map on the client side. "cloud_map" is pretty heavyweight for network usage as the whole 3D point cloud of the map is published. "cloud_map" may be used locally on the robot for 3D navigation.

4) "reset" deletes the whole database (not odometry). "reset_odom" or "reset_odom_to_pose" reset odometry only. If odometry is reset to Identity (by default), rtabmap will detect it and automatically create a new map in its database.

5) I've found that in some cases, setting "RGBD/OptimizeVarianceIgnored=true" results in better maps. If you have the referenced change (version 0.10.10), the graph should be optimized only once. Then depending to which node the robot is localizing, the map may oscillate a little even if the robot is stationary.

6) If you send the goal directly to move_base, the goal will not change on optimization. If you send the goal to rtabmap (topic /rtabmap/goal, see documentation), rtabmap will update the goal if there are optimizations (e.g., the goal will "follow" the map).

7) The accuracy will depend on the capability of the robot to localize. While you are using only an Asus Xtion, the robot may only relocalize if it is "seeing" the same images of the nodes created in the map (camera in the same direction), otherwise it would be pure odometry. While mapping, you can move in many directions to increase the number of nodes in the map with different orientations.

Have great experiments!
cheers

[g.bartoli]

matlabbe wrote

1) If you want to fuse wheel odometry with visual odometry, you should do that with an external node outputting the odometry fused. See robot_pose_ekf for an example of such node (note that if you use this node, you will need to convert "geometry_msgs/PoseWithCovarianceStamped" output to "nav_msgs/Odometry").

Ok, that is a nice solution. Once the navigation stack will be ready, we will try this approach if the odometry is too erroneus. How can rtabmap visual odometry be used with the other processes already running inside the rtabmap node? Do I need to start two rtabmap nodes?

matlabbe wrote

2) On graph optimizations, only /map -> /odom changes, not /odom -> /base_link. If you want the corrected pose, you have to transform /base_link in /map frame. "RGBD/OptimizeFromGraphEnd" changes this behavior:

So, setting it to false does also update base_link position after optimizations?

matlabbe wrote

3) Do you mean "mapData" vs "cloud_map"? "proj_map" is the 2D projection of the "cloud_map" on the ground to create an occupancy grid map. They ("proj_map", "grid_map" and "cloud_map") should be aligned together. "mapData" (used by MapCloud Plugin) is a lightweight topic sending only the latest node data with the graph. The MapCloud plugin incrementally builds the map on the client side. "cloud_map" is pretty heavyweight for network usage as the whole 3D point cloud of the map is published. "cloud_map" may be used locally on the robot for 3D navigation.

Ok, I understood, thanks.

matlabbe wrote

4) "reset" deletes the whole database (not odometry). "reset_odom" or "reset_odom_to_pose" reset odometry only. If odometry is reset to Identity (by default), rtabmap will detect it and automatically create a new map in its database.

Ok, now I saw that visual odometry (RGBD or stereo) is an independent node from rtabmap, it receives "reset_odom" service, right? So, when rtabmap starts from a launch file does it also load a separate visual odometry node?

matlabbe wrote

5) I've found that in some cases, setting "RGBD/OptimizeVarianceIgnored=true" results in better maps. If you have the referenced change (version 0.10.10), the graph should be optimized only once. Then depending to which node the robot is localizing, the map may oscillate a little even if the robot is stationary.

6) If you send the goal directly to move_base, the goal will not change on optimization. If you send the goal to rtabmap (topic /rtabmap/goal, see documentation), rtabmap will update the goal if there are optimizations (e.g., the goal will "follow" the map).

7) The accuracy will depend on the capability of the robot to localize. While you are using only an Asus Xtion, the robot may only relocalize if it is "seeing" the same images of the nodes created in the map (camera in the same direction), otherwise it would be pure odometry. While mapping, you can move in many directions to increase the number of nodes in the map with different orientations.

Ok, thanks!

[matlabbe]

If your wheel odometry is very erroneous, robot_pose_ekf may not be able to filter that. You don't have to start two rtabmap nodes. The visual odometry is an independent node. For example:

wheel_odometry
       \
      "odometry"
         \
        robot_pose_ekf_node  ----- "pose" -----> convert_pose_to_odom_node --"odometry" ---> rtabmap
         /
      "odometry"
       /
rgbd_odometry

Setting "RGBD/OptimizeFromGraphEnd=false" doesn't change /map->/odom, so /base_link is not affected.

Yes, the visual odometry node is independent of the rtabmap node. For convenience, the rgbd_mapping.launch launches rgbd_odometry, rtabmap and rtabmapviz (or rviz) nodes. If you only need rtabmap node, I recommend to use the node directly in your launch files instead (following these examples depending on your robot configuration).

cheers

[g.bartoli]

I'm trying your suggested approach, but it seems that my ROS Indigo cannot install the robot_pose_ekf package...

$ sudo apt-get install ros-indigo-robot_pose_ekf
E: Unable to locate package ros-indigo-robot_pose_ekf

Also following the official wiki, does not give results:

$ rosdep install robot_pose_ekf
ERROR: Rosdep cannot find all required resources to answer your query
Missing resource robot_pose_ekf

Robot_Pose_EKF seems to be supported in Indigo and I followed the standard procedure to install ROS Indigo, other components from the Navigation Stack are working (move_base, costmap_2d, ...). Do you known if I am missing something? Do I have to manually download the code from git and compile by myself?

Thanks,
Guido

[g.bartoli]

UPDATE:

I found that the package "robot_pose_ekf" is inside the "navigation" package, so I installed it with "sudo apt-get install ros-indigo-navigation". After that, "robot_pose_ekf" was available.
However, the "navigation" package, among other packages, also installed "global-planner" and "dwa-local-planner", but I thought that they were already installed, since I'm using them in the navigation stack with "move_base" together with RTab-Map.

[matlabbe]

The azimut configuraiton is using TrajectoryPlannerROS, not DWAPlannerROS. I don't know exactly what is the difference between the packages as TrajectoryPlannerROS has "dwa" parameter, here some info referring to dwa_local_planner.

cheers