A Velocity-Based Rao-Blackwellized Particle Filter Approach to Monocular vSLAM

Morteza Farrokhsiar, Homayoun Najjaran
DOI: 10.4236/jilsa.2011.33013                

Abstract

This paper presents a modified Rao-Blackwellized Particle Filter (RBPF) approach for the bearing-only monocular SLAM problem. While FastSLAM 2.0 is known to be one of the most computationally efficient SLAM approaches; it is not applicable to certain formulations of the SLAM problem in which some of the states are not explicitly expressed in the measurement equation. This constraint impacts the versatility of the FastSLAM 2.0 in dealing with partially ob-servable systems, especially in dynamic environments where inclusion of higher order but unobservable states such as velocity and acceleration in the filtering process is highly desirable. In this paper, the formulation of an enhanced RBPF-based SLAM with proper sampling and importance weights calculation for resampling distributions is presented. As an example, the new formulation uses the higher order states of the pose of a monocular camera to carry out SLAM for a mobile robot. The results of the experiments on the robot verify the improved performance of the higher order RBPF under low parallax angles conditions.

Keywords

Filtering, Higher Order Filter, Rao-Blackwellized Particle Filter, Bearing-Only Systems, Visual SLAM

Conflicts of Interest

The authors declare no conflicts of interest.

References

[1]	R. Smith, M. Self and P. Cheeseman, “A Stochastic Map for Uncertain Spatial Relationships,” In: R. Bolles and B. Roth, Eds., The Fourth International Symposium of Robotics Research, The MIT Press, Cambridge, 1988, pp. 467-474.
[2]	D. Micucci, D. G. Sorrenti, F. Tisato and F. M. Marchese, “Localisation and World Modelling: An Architectural Perspective,” International Journal of Advanced Robotic Systems, Vol. 3, No. 1, 2006, pp. 79-84.
[3]	M. Csorba and H. F. Durrant-Whyte, “New Approach to Map Building Using Relative Position Estimates,” Navigation and Control Technologies for Unmanned Systems II, SPIE, Bellingham, 1997, pp. 115-125.
[4]	M. W. M. G. Dissanayake, et al., “A Solution to the Simultaneous Localization and Map Building (SLAM) Problem,” IEEE Transactions on Robotics and Automation, Vol. 17, No. 3, 2001, pp. 229-241. Udoi:10.1109/70.938381U
[5]	G. Dissanayake, S. B. Williams, H. Durrant-Whyte and T. Bailey, “Map Management for Efficient Simultaneous Localization and Mapping (SLAM),” Autonomous Robots, Vol. 12, No. 3, 2002, pp. 267-286. Udoi:10.1023/A:1015217631658U
[6]	S. Ahn, J. Choi, N. L. Doh and W. K. Chung, “A Practical Approach for EKF-SLAM in an Indoor Environment: Fusing Ultrasonic Sensors and Stereo Camera,” Autonomous Robots, Vol. 24, No.3, 2008, pp. 315-335. Udoi:10.1007/s10514-007-9083-2U
[7]	U. Frese, “Treemap: An O (log n) Algorithm for Indoor Simultaneous Localization and Mapping,” Autonomous Robots, Vol. 21, No. 2, 2006, pp. 103-122. Udoi:10.1007/s10514-006-9043-2U
[8]	S. Huang, Z. Wang, G. Dissanayake and U. Frese, “Iterated D-SLAM Map Joining: Evaluating Its Performance in Terms of Consistency, Accuracy and Efficiency,” Autonomous Robots, Vol. 27, No. 4, 2009, pp. 409-429. Udoi:10.1007/s10514-009-9153-8U
[9]	Y. F. Liu and S. Thrun, “Results for Outdoor-SLAM Using Sparse Extended Information Filters,” Proceedings of the 2003 IEEE International Conference on Robotics and Automation, Taipei, 2003, pp. 1227-1233.
[10]	M. Montemerlo, S. Thrun, D. Koller and B. Wegbreit, “Fast SLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem,” Proceedings of 18th National Conference on Artificial Intelligence (AAAI-02), Edmonton, 2002, pp. 593-598.
[11]	K. P. Murphy, “Bayesian Map Learning in Dynamic Environments,” Advances in Neural Information Processing Systems, Denver, 1999, pp. 1015-1021.
[12]	R. Kümmerle, B. Steder, C. Dornhege, M. Ruhnke, G. Grisetti, C. Stachniss and A. Kleiner, “On Measuring the Accuracy of SLAM Algorithms,” Autonomous Robots, Vol. 27, No. 4, 2009, pp. 387-407.
[13]	S. Thrun, W. Burgard and D. Fox, “Probabilistic Robotics,” The MIT press, Cambridge, 2006.
[14]	M. Montemerlo, S. Thrun, D. Koller and B. Wegbreit, “Fast SLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges,” International Joint Conference on Artificial Intelligence, Las Vegas, 2003, pp. 1151-1156.
[15]	A. J. Davison, “Real-Time Simultaneous Localisation and Mapping with a Single Camera,” Proceedings of Ninth IEEE International Conference on Computer Vision, Nice, 2003, pp. 1403-1410. Udoi:10.1109/ICCV.2003.1238654U
[16]	E. Eade, “Scalable Monocular SLAM,” 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, Vol. 1, New York, 2006, pp. 469-476.
[17]	N. M. Kwok and A. B. Rad, “A Modified Particle Filter for Simultaneous Localization and Mapping,” Journal of Intelligent and Robotic Systems, Vol. 46, No. 4, 2006, pp. 365-382. doi:10.1007/s10846-006-9066-0U
[18]	A. Cesetti, E. Frontoni, A. Mancini, P. Zingaretti and S. Longhi, “A Vision-Based Guidance System for UAV Navigation and Safe Landing Using Natural Landmarks,” Journal of Intelligent and Robotic Systems, Vol. 57, No. 1-4, 2010, pp. 233-257. Udoi:10.1007/s10846-009-9373-3U
[19]	T. Bailey and H. Durrant-Whyte, “Simultaneous Locali- sation and Mapping (SLAM): Part II-State of the Art,” Robotics and Automation Magazine, Vol. 13, 2006, pp. 108-117. Udoi:10.1109/MRA.2006.1678144U
[20]	Y. Bar-Shalom, X. R. Li, T. Kirubarajan and J. Wiley, “Estimation with Applications to Tracking and Navigation,” Wiley, New York, 2001. Udoi:10.1002/0471221279U
[21]	M. Farrokhsiar and H. Najjaran, “A Higher Order Rao-Blackwellized Particle Filter for Monocular vSLAM,” American Control Conference 2010 (ACC-2010), Baltimore, 2010, pp. 6987-6992.
[22]	J. M. M. Montiel, J. Civera and A. J. Davison, “Unified Inverse Depth Parametrization for Monocular SLAM,” Proceedings of Robotics: Science and Systems, 2006.
[23]	D. T?rnqvist, T. B. Sch?n, R. Karlsson and F. Gustafsson, “Particle Filter SLAM with High Dimensional Vehicle Model,” Journal of Intelligent and Robotic Systems, Vol. 55, No. 4-5, 2009, pp. 249-266. Udoi:10.1007/s10846-008-9301-yU
[24]	H. F. Durrant-Whyte and T. Bailey, “Simultaneous Localization and Mapping: Part I,” IEEE Robotics & Automation Magazine, Vol. 13, No. 2, 2006, pp. 99-110. Udoi:10.1109/MRA.2006.1638022U
[25]	D. G. Lowe, “Distinctive Image Features from Scale-Invariant Keypoints,” International Journal of Computer Vision, Vol. 60, No. 2, 2004, pp. 91-110. Udoi:10.1023/B:VISI.0000029664.99615.94U
[26]	S. Se, D. Lowe and J. Little, “Mobile Robot Localization and Mapping with Uncertainty Using Scale-Invariant Visual Landmarks,” The International Journal of Robotics Research, Vol. 21, No. 8, 2002, p. 735. Udoi:10.1177/027836402761412467U
[27]	F. Caballero, L. Merino, J. Ferruz and A. Ollero, “Unmanned Aerial Vehicle Localization Based on Monocular Vision and Online Mosaicking,” Journal of Intelligent and Robotic Systems, Vol. 55, No. 4-5, 2009, pp. 323-343. Udoi:10.1007/s10846-008-9305-7U
[28]	M. Montemerlo and S. Thrun, “FastSLAM, A Scalable Method for the Simultaneous Localization and Mapping Problem in Robotics,” Springer, Berlin, 2007.
[29]	A. J. Davison, I. D. Reid, N. D. Molton and O. Stasse, “MonoSLAM: Real-Time Single Camera SLAM,” IEEE Transactions on Pattern Analysis and Machine Intelligence, Vol. 29, No. 6, pp. 1052-1067. Udoi:10.1109/TPAMI.2007.1049U
[30]	J. Civera, A. J. Davison and J. M. M. Montiel, “Inverse Depth Parametrization for Monocular SLAM,” IEEE Transactions on Robotics, Vol. 24, No. 5, 2008, pp. 932-945. Udoi:10.1109/TRO.2008.2003276U
[31]	K. S. Fu, R. C. Gonzalez and C. S. G. Lee, “Robotics: Control, Sensing, Vision, and Intelligence,” McGraw-Hill, New York, 1987.
[32]	M. Farrokhsiar and H. Najjaran, “Rao-Blackwellized Particle Filter Approach to Monocular vSLAM with a Modified Initialization Scheme,” Proceedings of 5th ASME/IEEE International Conference on Mechatronic and Embedded Systems and Applications, San Diego, 2009, pp. 427-433.
[33]	J. Shi and C. Tomasi, “Good Features to Track,” Proceedings of 1994 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, Seattle, 1994, pp. 593-600.
[34]	J. Y. Bouguet, “A Release of a Camera Calibration Toolbox for Matlab,” 2008. Uhttp://www.vision.caltech.edu/bouguetj/calib_doc/U
[35]	M. P. Parsley and S. J. Julier, “Avoiding Negative Depth in Inverse Depth Bearing-Only SLAM,” Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, 2008, pp. 2066-2071.