ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

amcl estimation quality

asked 2015-09-21 23:43:13 -0500

dan gravatar image

I would like to know how much to trust the amcl output pose. One thought is to see how well the laser data correlates with obstacles in the cost map, another is to calculate the variance of the particle locations. Is there some standard way to estimate the quality of the amcl pose?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2015-09-22 09:55:51 -0500

updated 2015-09-22 11:45:54 -0500

AMCL publishes a PoseWithCovarianceStamped where the covariance represents the spread within the entire filter. This tells you something about how confident the filter is about its estimate, but not directly how good the estimate is. If you want to know how good a particular pose is, you'll have to apply the sensor model to the pose. AMCL does this in amcl_laser.cpp. If you want to modify AMCL, you can probably just call pf_get_cluster_stats on the pose and look at the weight.


pf_get_cluster_stats doesn't give variance on a particular pose, the weight is an estimate of how closely the pose matches the current laser scan based on the map, normalized (I think) by the weights of all the particles.

When you say "see how well the laser data correlates with obstacles in the cost map" this is exactly what is happening, except it's using the static map, not costmap.

edit flag offensive delete link more


I tried using the covariance in the PoseWithCovarianceStamped output from AMCL, but, as you say, it seems to only the overall spread. I looked at pf_get_cluster_stats and do see how that could give the variance on a particular pose, but am unclear on how to relate that to how well the ........

dan gravatar image dan  ( 2015-09-22 11:24:49 -0500 )edit

laser data matches the costmap for a particular pose. What about integrating the costmap weights that are at the locations of the radar returns? Unfortunately, that fails when the pose is only slightly offset, where the laser returns appear next to, but not right on, objects such as long walls.

dan gravatar image dan  ( 2015-09-22 11:30:43 -0500 )edit

Updated my answer with some more info based on your comments.

Dan Lazewatsky gravatar image Dan Lazewatsky  ( 2015-09-22 11:46:11 -0500 )edit

OK, I will give that a try. Can you point me to a place in the amcl code where I could apply pf_get_cluster_stats? I looked at amcl_laser.cpp and see that around line 410 the laser hits are compared with the obstacle location, but the details of how to call pf_get_cluster_stats eludes me.

dan gravatar image dan  ( 2015-09-22 20:44:10 -0500 )edit

You actually don't even need to call it yourself. in amcl_node.cpp, I would create a float64 publisher, and somewhere around line 1217, publish max_weight (or maybe max_weight * number of particles).

Dan Lazewatsky gravatar image Dan Lazewatsky  ( 2015-09-23 14:31:48 -0500 )edit

see followup question here:

dan gravatar image dan  ( 2015-10-06 10:15:48 -0500 )edit

Question Tools



Asked: 2015-09-21 23:43:13 -0500

Seen: 2,740 times

Last updated: Sep 22 '15