Abstract:In order to solve the pose estimation problem of mobile robots in visually degraded environments, a real-time, robust and low-drift depth vision SLAM (simultaneous localization and mapping) method is proposed by utilizing both dense range flow and sparse geometry features. The proposed method is mainly composed of three optimization layers, namely range flow based visual odometry layer, ICP (iterative closest point) based pose optimization layer and pose graph based optimization layer. In range flow based visual odometry layer, the constraint equation of range variation is used to solve fast 6 DOF (degree of freedom) frame-to-frame pose estimation of camera. Then, local drifts are reduced by applying local map in ICP based pose optimization layer. After that, the loop closure constraints are built by extracting and matching sparse geometric features from depth information and a pose graph is constructed for global pose optimization in pose graph based optimization layer. Performances of the proposed method are tested on TUM datasets and in real-world scenes. Finally, experiment results show that our front-end algorithm outperforms the state-of-the-art depth vision methods, and our back-end algorithm can robustly construct loop closures constraints and reduce the global drift caused by front-end pose estimation.
[1] Prakhya S M, Liu B, Lin W, et al. Sparse depth odometry:3D keypoint based pose estimation from dense depth data[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2015:4216-4223.
[2] Jaimez M, Gonzalez-Jimenez J. Fast visual odometry for 3-D range sensors[J]. IEEE Transactions on Robotics, 2015, 31(4):809-822.
[3] Newcombe R A, Izadi S, Hilliges O, et al. KinectFusion:Real-time dense surface mapping and tracking[C]//IEEE International Symposium on Mixed and Augmented Reality. Piscataway, USA:IEEE, 2012:127-136.
[4] Forster C, Pizzoli M, Scaramuzza D. SVO:Fast semi-direct monocular visual odometry[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2014:15-22.
[5] Fischler M A, Bolles R C. Random sample consensus[J]. Communications of the ACM, 1981, 24(6). DOI:10.1145/358669. 358692.
[6] Tykkala T, Audras C, Comport A I. Direct iterative closest point for real-time visual odometry[C]//IEEE International Conference on Computer Vision Workshops. Piscataway, USA:IEEE, 2016:2050-2056.
[7] Mur-Artal R, Montiel J M M, Tardós J D. ORB-SLAM:A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163.
[8] Engel J, Schops T, Cremers D. LSD-SLAM:Large-scale direct monocular SLAM[M]//Lecture Notes in Computer Science, Vol.8690. Berlin, Germany:Springer-Verlag, 2014:834-849.
[9] Kerl C, Sturm J, Cremers D. Robust odometry estimation for RGB-D cameras[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2013:3748-3754.
[10] Jaimez M, Kerl C, Gonzalez-Jimenez J, et al. Fast odometry and scene flow from RGB-D cameras based on geometric clustering[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2017:3992-3999.
[11] Kerl C, Sturm J, Cremers D. Dense visual SLAM for RGB-D cameras[C]//IEEE/RSJ International Conference on IntelligentRobots and Systems. Piscataway, USA:IEEE, 2014:2100-2106.
[12] Endres F, Hess J, Sturm J, et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics, 2017, 30(1):177-187.
[13] Whelan T, Kaess M, Johannsson H, et al. Real-time large-scale dense RGB-D SLAM with volumetric fusion[J]. International Journal of Robotics Research, 2015, 34(4/5):598-626.
[14] Besl P J, McKay N D. A method for registration of 3-D shapes[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2):239-256.
[15] Segal A V, Haehnel D, Thrun S. Generalized-ICP[C]//Robotics:Science and Systems. Cambridge, USA:MIT Press, 2009. DOI:10.15607/RSS.2009.V.021.
[16] Magnusson M, Lilienthal A, Duckett T. Scan registration for autonomous mining vehicles using 3D-NDT[J]. Journal of Field Robotics, 2007, 24(10):803-827.
[17] Yousif K, Bab-Hadiashar A, Hoseinnezhad R. Real-time RGB-D registration and mapping in texture-less environments using ranked order statistics[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2014:2654-2660.
[18] Horn B K P, Harris J G. Rigid body motion from range image sequences[J]. CVGIP Image Understanding, 1991, 53(1):1-13.
[19] Spies H, Jahne B, Barron J L. Range flow estimation[J]. Computer Vision and Image Understanding, 2002, 85(3):209-231.
[20] Gonzalez J. Recovering motion parameters from a 2D range image sequence[C]//International Conference on Pattern Recognition. Piscataway, USA:IEEE, 1996:433-440.
[21] Fang Z, Yang S, Jain S, et al. Robust autonomous flight in constrained and visually degraded environments[J]. Journal of Field Robotics, 2017, 34(1):25-52.
[22] Schmiedel T, Einhorn E, Gross H M. IRON:A fast interest point descriptor for robust NDT-map matching and its application to robot localization[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2015:3144-3151.
[23] Kümmerle R, Grisetti G, Strasdat H, et al. g2o:A general framework for graph optimization[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2011:3607-3613.
[24] Sturm J, Engelhard N, Endres F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2012:573-580.