AMCL, problem with clustering in west direction

[corot]

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]

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

[corot]

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]

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.