Abstract：For the pose estimation based on RGB-D cameras, the pose estimation uncertainties of the ICP (iterative closest point) and PnP (perspective-n-point) algorithms are analyzed according to the maximum likelihood estimation and covariance propagation law, and the differences between the two algorithms in practical applications are explored from the perspective of data association. In addition, the influence of different parametrization forms of feature observation on the localization accuracy of visual odometer is compared, and a parametrization method is proposed. Based on the difference and connection between ICP and PnP algorithms, a switching strategy for the two PnP algorithms in the RGB-D SLAM (simultaneous localization and mapping) system is proposed, and different weights are assigned to each error term in the optimization problem according to the uncertainty of the observation. Experiments on two public datasets show that compared with the mainstream RGB-D SLAM algorithms, the proposed algorithm has higher localization accuracy and robustness in a variety of scenes with low texture, fast camera movement or dynamic objects. Meanwhile, the time efficiency of the proposed algorithm is about 10% higher than ORB-SLAM2 algorithm.
[1] Klein G, Murray D. Parallel tracking and mapping for small AR workspaces[C]//IEEE and ACM International Symposium on Mixed and Augmented Reality. Piscataway, USA:IEEE, 2007:225-234. [2] Mur-Artal R, Montiel J M M, Tardos J D. ORB-SLAM:A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163. [3] Engel J, Schöps T, Cremers D. LSD-SLAM:Large-scale direct monocular SLAM[M]//Lecture Notes in Computer Science, Vol.8690. Berlin, Germany:Springer, 2014:834-849. [4] Engel J, Koltun V, Cremers D. Direct sparse odometry[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2017, 40(3):611-625. [5] 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. [6] Kerl C, Sturm J, Cremers D. Dense visual SLAM for RGB-D cameras[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA:IEEE, 2013:21002106. [7] Newcombe R A, Izadi S, Hilliges O, et al. KinectFusion:Realtime dense surface mapping and tracking[C]//10th IEEE International Symposium on Mixed and Augmented Reality. Piscataway, USA:IEEE, 2011:127-136. [8] Endres F, Hess J, Sturm J, et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics, 2014, 30(1):177187. [9] Whelan T, Salas-Moreno R F, Glocker B, et al. ElasticFusion:Real-time dense SLAM and light source estimation[J]. International Journal of Robotics Research, 2016, 35(14):1697-1716. [10] Liu H M, Li C, Chen G J, et al. Robust keyframe-based dense SLAM with an RGB-D camera[DB/OL]. (2017-11-14)[202002-03]. https://arxiv.org/abs/1711.05166. [11] Besl P J, McKay N D. Method for registration of 3-D shapes[C]//Proceedings of the SPIE, Vol.1611, Sensor Fusion IV:Control Paradigms and Data Structures. Bellingham, USA:SPIE, 1992:586-606. [12] Mur-Artal R, Tardós J D. ORB-SLAM2:An open-source SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5):1255-1262. [13] Gomez-Ojeda R, Zhang Z C, Gonzalez-Jimenez J, et al. Learning-based image enhancement for visual odometry in challenging HDR environments[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2018:805-811. [14] Lu Y, Song D. Robust RGB-D odometry using point and line features[C]//IEEE International Conference on Computer Vision. Piscataway, USA:IEEE, 2015:3934-3942. [15] Ma L, Kerl C, Stückler J, et al. CPA-SLAM:Consistent planemodel alignment for direct RGB-D SLAM[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2016:1285-1291. [16] Kim P, Coltin B, Jin K H. Linear RGB-D SLAM for planar environments[C]//Lecture Notes in Computer Science, Vol.11208. Berlin, Germany:Springer, 2018:333-348. [17] Civera J, Davison A J, Montiel J M M. Inverse depth parametrization for monocular SLAM[J]. IEEE Transactions on Robotics, 2008, 24(5):932-945. [18] Hartley R, Zisserman A. Multiple view geometry in computer vision[M]. Cambridge, UK:Cambridge University Press, 2003. [19] Lepetit V, Moreno-Noguer F, Fua P. EPnP:An accurate O(n) solution to the PnP problem[J]. International Journal of Computer Vision, 2009, 81(2). DOI:10.1007/s11263-008-0152-6. [20] Penate-Sanchez A, Andrade-Cetto J, Moreno-Noguer F. Exhaustive linearization for robust camera pose and focal length estimation[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2013, 35(10):2387-2400. [21] Horn R A, Johnson C R. Matrix analysis[M]. Cambridge, UK:Cambridge University Press, 2012. [22] Smisek J, Jancosek M, Pajdla T. 3D with Kinect[M]//Consumer Depth Cameras for Computer Vision. London, UK:Springer, 2013:3-25. [23] 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. [24] Handa A, Whelan T, McDonald J, et al. A benchmark for RGBD visual odometry, 3D reconstruction and SLAM[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2014:1524-1531. [25] Kim P, Coltin B, Kim H J. Low-drift visual odometry in structured environments by decoupling rotational and translational motion[C]//IEEE International Conference on Robotics and Automation. Piscataway, USA:IEEE, 2018:7247-7253. [26] Whelan T, Kaess M, Fallon M, et al. Kintinuous:Spatially extended KinectFusion[R/OL]. Cambridge, USA:MIT, 2012.[2020-02-01]. https://dspace.mit.edu/handle/1721.1/71756.
