A Sliding Window Filter for Incremental SLAM
This work develops a sliding window filter for incremental simultaneous localization and mapping (SLAM) that focuses computational resources on accurately estimating the immediate spatial surroundings using a sliding time window of the most recent sensor measurements. Ideally, we would like a constant time algorithm that closely approximates the all-time maximum-likelihood estimate as well as the minimum variance Cramer Rao lower bound (CRLB) - that is we would like an estimator that achieves some notion of statistical optimality (quickly converges), efficiency (quickly reduces uncertainty) and consistency (avoids over-confidence). To this end we give a derivation of the SLAM problem from the Gaussian non-linear least squares optimization perspective.We find that this results in a simple, yet general, take on the SLAM problem; we think this is a useful contribution.
Our approach is inspired by the results from the photogrammetry community, dating back to the late 1950’s , and later derivatives like Mikhail’s least squares treatment , the Variable state dimension filter(VSDF) , visual odometry( VO) , modern bundle adjustment(BA) [10, 3] and of course extended Kalman filter (EKF) SLAM .
We apply the sliding window filter to SLAM with stereo vision and inertial measurements. Experiments show that the best approximate method comes close to matching the performance of the optimal estimator while attaining constant time complexity - empirically, it is often the case that the difference in their performance is indistinguishable.
KeywordsExtend Kalman Filter Bundle Adjustment Sparsity Pattern Visual Odometry Slide Time Window
Unable to display preview. Download preview PDF.
- 1.Brown, D.: A solution to the general problem of multiple station analytical stereotriangulation. Tech. rep., RCP-MTP Data Reduction Technical Report No. 43, Patrick Air Force Base, Flordia (also designated as AFMTC 58-8) (1958)Google Scholar
- 2.Dennis, J.J., Schnabel, R.B.: Numerical Methods for Unconstrained Optimization and Nonlinear Equations. Society for Industrial & Applied Mathematics (1996)Google Scholar
- 3.Engels, C., Stewenius, H., Nister, D.: Bundle adjustment rules. In: Photogrammetric Computer Vision (2006)Google Scholar
- 5.McLauchlan, P.F.: The variable state dimension filter applied to surface-based structure from motion. Tech. rep., University of Surrey (1999)Google Scholar
- 6.Mikhail, E.M.: Observations and Least Squares. Rowman & Littlefield (1983)Google Scholar
- 7.Nister, D., Naroditsky, O., Bergen, J.: Visual odometry. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, pp. 652-659. Washington, DC (2004)Google Scholar
- 8.Olson, C.F., Matthies, L.H., Schoppers, M., Maimone, M.W.: Stereo ego-motion improvements for robust rover navigation. In: Proceedings of the IEEE Conference on Robotics and Automation, pp. 1099-1104. Washington, DC (2001)Google Scholar
- 9.Smith, R.C., Self, M., Cheeseman, P.: Estimating uncertain spatial relationships in robotics. In: I.J. Cox, G.T. Wilfong (eds.) Autonomous Robot Vehicles, pp. 167-193. Springer-Verlag (1990)Google Scholar
- 10.Triggs, B., McLauchlan, P., Hartley, R., Fitzgibbon, A.: Bundle adjustment - A modern synthesis. In: W. Triggs, A. Zisserman, R. Szeliski (eds.) Vision Algorithms: Theory and Practice, LNCS, pp. 298-375. Springer Verlag (2000)Google Scholar