search

ros-planning / navigation

ROS Navigation stack. Code for finding where the robot is and how it can get somewhere else.
2.36k stars 1.8k forks source link

AMCL, problem with clustering in west direction #27

Open corot opened 11 years ago

corot commented 11 years ago

While evaluating kidnap recovery through global_localization service, I have realized that almost never the best hypothesis points west-ward. After googling a bit, I found this in a player-stage project mailing list:

http://player-stage-gazebo.10965.n7.nabble.com/AMCL-problem-with-clustering-in-west-direction-td2731.html

Looks like an old, never solved problem.

forouher commented 11 years ago

Interesting. Did you notice real-world consequences from this? E.g. wrong/bad localization in some scenario?

corot commented 11 years ago

Not really; only when trying global localization. I suppose this is because on global localization is normally the only moment when there are particles pointing to any heading "competing" to be the best hypothesis

VorpalBlade commented 5 years ago

I ran into this separately while looking into how AMCL works. It should be reasonably easy to fix in the function pf_kdtree_cluster_node in pf_kdtree.cpp, which neighbor "buckets" from the KLD kd-tree algorithm are found by looking at the 3^3 buckets around the current one. There would simply have to be some wrap-around logic for nkey[2] (based on self->size[2] to determine when -pi/+pi are reached).

That should fix the problem.