Multithreaded SLAM mapping

[motters]

I've now made the DP-SLAM algorithm multithreaded, so that it scales
well across multiple CPU cores. Testing on a dual core system shows
that I'm getting a good amount of concurrency. Although this may not
be highly useful right now, since most of my computers are only
running on single core processors, it will allow the system to perform
better and be far more robust in the future. On a quad core system
instead of evaluating a few hundred hypothetical poses at each time
step a couple of thousand could be evaluated, giving a consequent
increase in reliability.

In the multithreaded system multiple grid maps are created and updated
in parallel. The score for the best path within each map is compared,
and the map with the highest score then becomes the most probable
world. A possible future enhancement is that I could turn this into a
kind of genetic algorithm. So, if the best path score within a map
falls a long way behind the top performer it could be replaced by one
of the better maps, with some small amount of mutation added. That
way the system is constantly striving to improve itself.