Simultaneous localization and mapping for a mobile robot

1. A method of localizing a mobile robot, the method comprising:

receiving sensor data of a scene about the robot, the sensor data comprising a three-dimensional point cloud;

executing a particle filter having a set of particles, each particle having an associated variance occupancy grid map and a robot location hypothesis;

accumulating cloud points in cells of the variance occupancy grid map based on first and second coordinates of the cloud points, each cell accumulating a height variance based on a third coordinate of the accumulated cloud points;

updating the variance occupancy grid map associated with each particle based on the received sensor data;

assessing a weight for each particle based on the received sensor data;

selecting at least one particle based on its weight; and

determining a location of the robot based on the at least one selected particle.

2. The method of claim 1 , wherein receiving senor data comprises receiving two-dimensional image data and three-dimensional image data of a scene about the robot.

3. The method of claim 1 , wherein the particle filter comprises a Rao-Blackwellized particle filter.

4. The method of claim 1 , further comprising:

associating a ground plane occupancy grid map with each particle;

identifying ground plane cloud points;

fitting a ground plane to the ground plane cloud points; and

updating the ground plane occupancy grid map.

5. The method of claim 1 , further comprising:

receiving an image of the scene about the robot;

associating a feature map with each particle;

instantiating an image pyramid comprising a set of scaled images, each scaled image having a scale relative to the received image;

identifying at least one feature point in the scaled images; and

updating the feature map.

6. The method of claim 5 , further comprising calculating a Harris Corner Score to identify feature points associated with a corner feature of the scene.

7. The method of claim 6 , further comprising selecting feature points as candidate feature points that have at least one of a local maximum Harris Corner Score or a Harris Corner Score substantially equal to the local maximum Harris Corner Score in a threshold area.

8. The method of claim 7 , further comprising selecting feature points as candidate feature points that have at least one of a local maximum Harris Corner Score or a Harris Corner Score within about 20% of the local maximum Harris Corner Score within a 10 pixel radius of the feature point having the local maximum Harris Corner Score.

9. The method of claim 5 , further comprising selecting a feature point of a scaled image as a key point for tracking and producing a descriptor of that key point.

10. The method of claim 9 , further comprising identifying the key point in a subsequent image using the descriptor.

11. The method of claim 9 , wherein the descriptor comprises feature points within a threshold distance of the key point on the corresponding scaled image of the key point.

12. The method of claim 11 , further comprising:

sampling feature points of the descriptor;

recording a brightness level for each feature point; and

normalizing the brightness levels to have a mean of zero and a variance of one.

13. The method of claim 12 , further comprising blurring the scaled image before sampling the feature points.

14. The method of claim 12 , further comprising rotating the feature points by a threshold angle before sampling the feature points.

15. The method of claim 12 , further comprising sampling feature points within a threshold area of the scaled image about the key point.

16. The method of claim 12 , further comprising producing a descriptor for each feature point of a set of feature points.

17. The method of claim 16 , further comprising comparing feature descriptors of first and second images to identify a common key point.

18. The method of claim 17 , further comprising searching respective image pyramids of the first and second images to find the common feature point.

19. The method of claim 18 , further comprising searching within a threshold area of the scaled images of the images pyramids for the common feature point.

20. The method of claim 18 , further comprising determining the threshold area based on at least one of a known previous feature point location or a robot drive trajectory.