Simultaneous Localization and Mapping (SLAM) is a well studied problem in robotics since it is usually a prerequisite for autonomous navigation in unknown environments. An interesting variant of the general problem is the case of a robot equipped with bearing-only sensors, most popular of which are monocular cameras.
There is a wide variety of algorithms that have been proposed to solve the SLAM problem in general and Bearing-Only SLAM (BO-SLAM) in particular. Our goal with this project is to implement the various algorithms and compare their efficiency in accurately and robustly solving the BO-SLAM problem. In our setup we assume that the sensor is able to extract landmarks from its sensory input, so mapping the environment corresponds to estimating the landmark coordinates.
The algorithms we are considering are the following:
- Extended Kalman Filter (EKF): Assuming the motion and odometry models can be linearized and that the noise follows a white, Gaussian model. We are testing the following EKF approaches:
- The most straightforward approach assumes that an Extended Kalman Filter is used to simultaneously localize the robot and to locate the landmark coordinates.
- An improvement over the previous method can possibly be achieved by using multiple Kalman filters to independently track each landmark. Filters that do not agree with the measurements are pruned. When only one Kalman filter remains then the landmark is added in the global Kalman filter and the method continues as above.
- On-line Expectation Maximization: Iteratively solve the Localization and the Mapping subproblems separately by assuming that there is a solution to the sibling subproblem. Apply Particle Filters (PFs) to solve each one of the subproblems. We are testing the following variants of PFs:
- The Sampling Importance Sampling (SIS) approach with Resampling according to the Effective Sampling Size (ESS) criterion.
- The Sampling Importance Resampling (SIR) aka. Monte Carlo Localization (MCL).
- The Dual-MCL and Mixture-MCL approaches.
- The SIR approach with Sensor Bias.
- The SIR approach with Sensor Resetting.
- The SIR approach with Roughing.
- Rao-Blackwellized Particle Filters: By applying an approach from statistics called Rao-Blackwellization it is possible to show that we can get an algorithm for the SLAM problem that converges (Online E/M does not provably converge) and which has a better complexity than the EKF.
We will be soon reporting the results of our experimental comparison.
The above figures show an example of a simulated experiment. There are 20 landmarks dispersed in the environment and a mobile robot. In the left figure we can see the uncertainty in the landmark locations when the robot has just started moving. In the right figure the robot has executed its exploration path and the landmark positions have been estimated.