Using vision for Monte-Carlo localization

Question

From each step of my vision code I am able to get around 400 coordinates of where the robot thinks the walls are

I want to integrate this into Monte-Carlo observation step.

I'm storing the map of the maze as a set of Line segments. What would be a nice way to implement the sensor update, i.e. given the position (x,y) of the robot what is the probability that it is found there given the above described coordinates of the walls.

---

The main idea I currently have:

Transform points in polar coordinates. Then for each point (from vision output) compute a ray with this angle and find the first intersection with the maze. Now we have the predicted distance and real distance and we can compute the probability that this measurement is right.

The main drawback is that this is slow. For each point from vision output I have to iterate over all line segments to find the one with the closest intersection. The line segments number is around 50. So it gets to O(400*50*Particle number).

Answer 1

Wow, walls, mazes, casting rays. Does anyone have Wolfenstein/Doom deja vu?

Back in the day, these games had to solve similar problems, calculating the intersection between lines (of fire or movement) and the walls of the map. For Wolfenstein, it was fairly easy because the map was arranged in a grid, and they used Ray Casting.

For Doom, it was a little harder, since there was no grid. So they used something called a Binary Space Partitioning Tree (BSP Tree):

This recursively divides the map in a way that makes it easy to perform geometrical queries on it. In computer graphics, this would usually mean rendering the surfaces in such a way that there was no overlap, but it could also mean performing efficient collision detection.

The BSP Tree works well if your walls are defined by nice large lines/planes. However, if your walls consist of thousands point cloud samples or something, then the tree will be inappropriate.

In the case of point cloud (or tiny line cloud) data, I would recommend a quadtree. Again, this divides space recursively in such a way that you can easily find the set of points that are close to your robot, or close to some line.

Answer 2

The term you are looking for is "frustrum", and the algorithm you want is "frustrum culling".

Check out this answer for how one would implement culling in a 2D game.

If performance is still an issue, remember that you don't need to consider all 400 "possible wall coordinates" every time. If you try to determine your exact position in a single step, you're trying to outsmart the particle filter; you don't need to do this.

Think of it this way: your robot is not randomly teleporting from place to place -- it's in one position now, and it will be in a very similar position at the next time step or sensor reading. Just evaluate as many coordinates as you can in the available time (for each particle), then do it again; your particles will doing the work. Obviously, you will converge faster if you can evaluate more coordinates at each step... but there are diminishing returns to adding more accuracy to each particle. This simulation shows that using only 20-ish measurements will still give you decent results:

_{(source: washington.edu)}

I'd imagine your code looks like this:

foreach particle
    foreach coordinate
        foreach segment
            see if a ray through coordinate intersects segment

You may want to try structuring it like this:

while there is still time before the next sensor measurement
    pick a coordinate that's far away from the coordinates that have been tried already
    foreach particle
        foreach segment_in_frustrum
            see if a ray through coordinate intersects segment_in_frustrum

Answer 3

I have done this with a kinect and it wasn't slow.

So Give more details about:

1. Sensor output sample
2. the system you are running on

In monte carlo you have to traverse all the points one way or another to get a Posterior distribution from the Prior.

If the System has a GPU you can parallel process the whole array at once.

What you are saying is one of the Cons of Monte Carlo Localization it doesn't scale well for bigger problems specially for 3-dimensional spaces.

there are other continous models which scale better than it

Answer 4

I assume you're doing this inside a loop. Make sure that you look at your computations and PRECOMPUTE everything you can before the loop starts. When you're multiplying matrices and doing other math operations 400*50*particle# times, it adds up very quickly. I remember I was doing a calculation for a transform between frames, which boiled down to multiplying 3 matrices together, and doing this inside the loop instead of precomputing made the difference of 3 seconds of computation time per iteration of my algorithm.

Answer 5

What kind of CPU or GPU power do you have available? A GPU might be able to do a large chunk of those calculations in parallel, and all of them in a single calculate call, which would complete in a few milliseconds.

Also, with proper spatial partitioning, SIMD implementation, and using coherency where you know that close rays are likely to have the same result, you should be able to optimize this even on a regular CPU.