AMCL, problem with clustering in west direction

[ros-planning]

AMCL, problem with clustering in west direction

[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.