Lie Group Modelling for an EKF-Based Monocular SLAM Algorithm
- the filtering approach: this is classically based on a Kalman filter and its variants (such as the extended Kalman filter, i.e., EKF). It provides an estimate of the state and its associated covariance matrix at each epoch.
- the number of detected landmarks in each image can be high (≥500), making the map huge for big environments. This impacts directly the size of the state vector, which increases dramatically, making a filter-based method with 2D features not well-suited for long-term navigation;
- each measurement must be associated to each detection with a data association algorithm . This step remains a challenge and false matching can cause the filter to diverge. Moreover, it can be difficult to treat it from a computational point of view.
1.1. Formulating VSLAM on Lie Groups
- We can rewrite a compact camera model, allowing us to overcome the high non-linearities created by rotation matrices parametrization;
- The analytical development of quantities of interest, such as Jacobian matrices, are intrinsic to LGs and are consequently less difficult to compute and to implement.
- Theoretical: we define a new matrix state containing the camera pose and the pattern transformations on , which constitutes the map. Consequently, the associated state space is ( direct products of ), where K is the number of current patterns;
- Algorithmic: the newly detected patterns are initialized by a transformation, obtained by minimizing a criterion through a Gauss–Newton algorithm on LGs.
2. The Euclidean Approach
2.1. Coded Patterns
- a rotation matrix between the local frame and a world frame, ;
- the position of the origin of the local frame expressed in this world frame, ;
- the distance L between the center of two ellipses;
- an ID coded by the circular bar-codes.
2.2. Camera Observation Model
- Euler angles: the unknown variables are six angles , such as:
- Quaternion: In this case and can be written as a complex combination of quaternion coefficients with the form .
- the pose of the camera ;
- the set of parameters of the patterns , where K is the number of detected patterns,
2.3. Why EKF-SLAM on Lie Groups?
- the camera observation model can be written in a compact way and depends “linearly” on the rotation matrix. Consequently, the approximation at order 1 is more valid;
- The Jacobian of the observation model can be computed analytically by considering the geometrical structure of the rotation matrices and does not depend on any parametrization.
3. Background on Lie Groups
3.1. Elementary Notions
- Due to its group structure, G has an internal composition law which is associative. Furthermore, there is an identity element defined, so that each element of the group has an inverse;
- Due to its manifold structure, it is possible to compute the derivative and the integral of two elements, or the inverse of one element.
3.2. Non-Commutativity and Jacobian
3.2.2. Jacobian on LGs
3.3. Lie Groups of Interest
3.3.1. The Special Orthogonal LG
3.3.2. The Special Euclidean LG
3.4. Camera Observation Model on LGs
4. SLAM Filtering Problem on Lie Groups
4.1. Gaussian Distribution on LGs
4.2. Unknown State and Evolution Model
- is a dynamic evolution function (where corresponds to the dimension of );
- is a control input;
- , where is a white Gaussian noise on , with covariance matrix .
4.3. Principle of LG-EKF
4.3.1. The Prediction Step
4.3.2. The Update Step
4.4. Initialization of Pattern on
- corresponds to the correlation between the new pattern and the previous state;
- is the covariance associated to the new pattern j.
4.4.1. Computation of
4.4.2. Computation of
5.1. Experiments with Simulated Data
- The quality of can be evaluated by using two intrinsic metrics on LGs, which enables us to compute the rotation error directly on :
- a RMSE (Root Mean Square Error) metric, which can be seen as a generalization of the classical Euclidean RMSE. The classical distance is substituted by the geodesic distance on :
- a mean RPE (Relative Pose Error) metric to evaluate the relative pose error between two consecutive epochs for several realizations of the algorithm.
- As the position parameter is Euclidean, the quality of its estimator is determined with the classical Euclidean RPE and RMSE.
5.1.1. Case 1: L Perfectly Known
- Firstly, the LG modelling takes into account the intrinsic properties of the unknown state belonging to . Consequently, the indeterminacy of the Euler angle estimates is deleted;
- Secondly, through this modelling, the observation model is freed from the non-linearity introduced by the cosine and sine functions of the rotation matrix in the Euclidean modelling. In this way, the approximation of the covariance estimation error is more precise.
5.1.2. Case 2: L Is Misspecified
5.2. Experiment with Real Data
5.2.1. Experiment Setup
5.2.2. Obtained Results
Institutional Review Board Statement
Informed Consent Statement
Data Availability Statement
Conflicts of Interest
Appendix A. Computation of C(k−1)
Appendix A.1. Case of from (69)
Appendix A.2. Case of from (78)
Appendix B. Computation of J(k)
Appendix B.1. Computation of
- the classical derivative product rule according to ;
- the fact that , where is the lth vector basis of Lie algebra .
Appendix B.2. Computation of
Appendix C. Expression of
Appendix C.1. Case of SO(3)
Appendix C.2. Case of SE(3)
Appendix C.3. Case of G
- Engel, J.; Schöps, T.; Cremers, D. LSD-SLAM: Large-Scale Direct Monocular SLAM. In Computer Vision—ECCV 2014, Proceedings of the 13th European Conference, Zurich, Switzerland, 6–12 September 2014; Springer International Publishing: Cham, Switzerland, 2014; pp. 834–849. [Google Scholar]
- Khairuddin, A.R.; Talib, M.S.; Haron, H. Review on simultaneous localization and mapping (SLAM). In Proceedings of the IEEE International Conference on Control System, Computing and Engineering, Penang, Malaysia, 27–29 November 2015; pp. 85–90. [Google Scholar]
- Triggs, B.; McLauchlan, P.; Hartley, R.; Fitzgibbon, A. Bundle Adjustment—A Modern Synthesis. In Proceedings of the International Workshop on Vision Algorithms, Corfu, Greece, 21–22 September 1999. [Google Scholar]
- Mouragnon, E.; Lhuillier, M.; Dhome, M.; Dekeyser, F.; Sayd, P. 3D reconstruction of complex structures with bundle adjustment: An incremental approach. In Proceedings of the 2006 IEEE International Conference on Robotics and Automation, Orlando, FL, USA, 15–19 May 2006; pp. 3055–3061. [Google Scholar]
- Guivant, J.; Nebot, E. Optimization of the simultaneous localization and map-building algorithm for real-time implementation. IEEE Trans. Robot. Autom. 2001, 17, 242–257. [Google Scholar] [CrossRef][Green Version]
- Harris, C.; Stephens, M. A combined corner and edge detector. In Proceedings of the Fourth Alvey Vision Conference, Manchester, UK, 31 August–2 September 1988; pp. 147–151. [Google Scholar]
- Rosten, E.; Drummond, T. Machine learning for high-speed corner detection. In Proceedings of the European Conference on Computer Vision, Graz, Austria, 7–13 May 2006. [Google Scholar]
- Bowman, S.L.; Atanasov, N.; Daniilidis, K.; Pappas, G.J. Probabilistic data association for semantic SLAM. In Proceedings of the 2017 IEEE International Conference on Robotics and Automation (ICRA), Singapore, 29 May–3 June 2017; pp. 1722–1729. [Google Scholar]
- Lim, H.; Lee, Y.S. Real-time single camera SLAM using fiducial markers. In Proceedings of the ICCAS-SICE, Fukuoka, Japan, 18–21 August 2009; pp. 177–182. [Google Scholar]
- Houben, S.; Droeschel, D.; Behnke, S. Joint 3D laser and visual fiducial marker based SLAM for a micro aerial vehicle. In Proceedings of the IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems (MFI), Baden-Baden, Germany, 19–21 September 2016; pp. 609–614. [Google Scholar]
- Kalaitzakis, M.; Cain, B.; Carroll, S.; Ambrosi, A.; Whitehead, C.; Vitzilaios, N. Fiducial Markers for Pose Estimation Overview, Applications and Experimental Comparison of the ARTag, AprilTag, ArUco and STag Markers. J. Intell. Robot. Syst. 2021, 101, 71. [Google Scholar] [CrossRef]
- Fiala, M. ARTag, a fiducial marker system using digital techniques. In Proceedings of the 2005 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, San Diego, CA, USA, 20–25 June 2005; Volume 2, pp. 590–596. [Google Scholar]
- Pilté, M.; Bonnabel, S.; Barbaresco, F. Tracking the Frenet-Serret frame associated to a highly maneuvering target in 3D. In Proceedings of the 2017 IEEE 56th Annual Conference on Decision and Control (CDC), Melbourne, VIC, Australia, 12–15 December 2017; pp. 1969–1974. [Google Scholar]
- Mika, D.; Jozwik, J. Lie groups methods in blind signal processing. Sensors 2020, 20, 440. [Google Scholar] [CrossRef] [PubMed][Green Version]
- Bourmaud, G.; Giremus, A.; Berthoumieu, Y.; Mégret, R. Discrete Extended Kalman Filter on Lie groups. In Proceedings of the EUSIPCO, Marrakech, Morocco, 9–13 September 2013. [Google Scholar]
- Bonnabel, S.; Martin, P.; Salaün, E. Invariant Extended Kalman Filter: Theory and application to a velocity-aided attitude estimation problem. In Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference, Shanghai, China, 15–18 December 2009; pp. 1297–1304. [Google Scholar]
- Wang, J.; Zhang, C.; Wu, J.; Liu, M. An Improved Invariant Kalman Filter for Lie Groups Attitude Dynamics with Heavy-Tailed Process Noise. Machines 2021, 9, 182. [Google Scholar] [CrossRef]
- Brossard, M.; Bonnabel, S.; Condomines, J.P. Unscented Kalman filtering on Lie groups. In Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 24–28 September 2017; pp. 2485–2491. [Google Scholar]
- Ćesić, J.; Marković, I.; Bukal, M.; Petrović, I. Extended information filter on matrix Lie groups. Automatica 2017, 82, 226–234. [Google Scholar] [CrossRef]
- Marjanovic, G.; Solo, V. An engineer’s guide to particle filtering on matrix Lie groups. In Proceedings of the 2016 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP), Shanghai, China, 20–25 March 2016; pp. 3969–3973. [Google Scholar]
- Brossard, M.; Bonnabel, S.; Barrau, A. Invariant Kalman Filtering for Visual Inertial SLAM. In Proceedings of the 2018 21st International Conference on Information Fusion (FUSION), Cambridge, UK, 10–13 July 2018; pp. 2021–2028. [Google Scholar]
- Lenac, K.; Marković, I.; Petrović, I. Exactly sparse delayed state filter on Lie groups for long-term pose graph SLAM. Int. J. Robot. Res. 2018, 37, 585–610. [Google Scholar] [CrossRef]
- Bourmaud, G.; Mégret, R. Robust large scale monocular visual SLAM. In Proceedings of the 2015 IEEE Conference on Computer Vision and Pattern Recognition (CVPR), Boston, MA, USA, 7–12 June 2015; pp. 1638–1647. [Google Scholar]
- Hui, C.; Shiwei, M. Visual SLAM based on EKF filtering algorithm from omnidirectional camera. In Proceedings of the 2013 IEEE 11th International Conference on Electronic Measurement Instruments, Harbin, China, 16–19 August 2013; Volume 2, pp. 660–663. [Google Scholar]
- Selig, J.M. Geometrical Methods in Robotics; Springer: Berlin/Heidelberg, Germany, 1996. [Google Scholar]
- Solà, J.; Deray, J.; Atchuthan, D. A micro Lie theory for state estimation in robotics. arXiv 2018, arXiv:1812.01537. [Google Scholar]
- Faraut, J. Analysis on Lie Groups: An Introduction; Cambridge University Press: Cambridge, UK, 2008. [Google Scholar]
- Chirikjian, G. Information-Theoretic Inequalities On Unimodular Lie Groups. J. Geom. Mech. 2010, 2, 119–158. [Google Scholar] [CrossRef] [PubMed]
- Koks, D. A Roundabout Route to Geometric Algebra. In Explorations in Mathematical Physics: The Concepts Behind an Elegant Language; Springer Science: Berlin/Heidelberg, Germany, 2006; Chapter 4; pp. 147–184. [Google Scholar]
- Said, S.; Bombrun, L.; Berthoumieu, Y.; Manton, J.H. Riemannian Gaussian Distributions on the Space of Symmetric Positive Definite Matrices. IEEE Trans. Inf. Theory 2017, 63, 2153–2170. [Google Scholar] [CrossRef][Green Version]
- Pennec, X. Intrinsic statistics on Riemannian manifolds: Basic tools for geometric measurements. J. Math. Imaging Vis. 2006, 25, 127–155. [Google Scholar] [CrossRef][Green Version]
- Barfoot, T.; Furgale, P.T. Associating Uncertainty With Three-Dimensional Poses For Use In Estimation Problem. IEEE Trans. Robot. 2014, 30, 679–693. [Google Scholar] [CrossRef]
- Sola, J.; Monin, A.; Devy, M.; Lemaire, T. Undelayed initialization in bearing only SLAM. In Proceedings of the 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems, Edmonton, AB, Canada, 2–6 August 2005; pp. 2499–2504. [Google Scholar]
- Bishop, C. Pattern Recognition and Machine Learning; Springer: Berlin/Heidelberg, Germany, 2006. [Google Scholar]
- Bourmaud, G.; Mégret, R.; Giremus, A.; Berthoumieu, Y. From Intrinsic Optimization to Iterated Extended Kalman Filtering on Lie Groups. J. Math. Imaging Vis. 2016, 55, 284–303. [Google Scholar] [CrossRef][Green Version]
- Huynh, D.Q. Metrics for 3D Rotations: Comparison and Analysis. J. Math. Imaging Vis. 2009, 35, 155–164. [Google Scholar] [CrossRef]
- Chauchat, P.; Vilà-Valls, J.; Chaumette, E. Robust Linearly Constrained Invariant Filtering for a Class of Mismatched Nonlinear Systems. IEEE Control Syst. Lett. 2022, 6, 223–228. [Google Scholar] [CrossRef]
- Ozyesil, O.; Voroninski, V.; Basri, R.; Singer, A. A Survey of Structure from Motion. Acta Numerica 2017, 26, 305–364. [Google Scholar] [CrossRef]
- Zitová, B.; Flusser, J. Image registration methods: A survey. Image Vis. Comput. 2003, 21, 977–1000. [Google Scholar] [CrossRef][Green Version]
|RMSE position (m)||0.298||0.427|
|RMSE orientation (rad)||0.0048||0.0055|
|RPE position (m)||0.0172||0.0205|
|RPE orientation (rad)|
|RMSE position (m)||0.758||1.257|
|RMSE orientation (rad)||0.0158||0.065|
|RPE position (m)||0.0398||0.0924|
|RPE orientation (rad)|
Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.
© 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).
Share and Cite
Labsir, S.; Pages, G.; Vivet, D. Lie Group Modelling for an EKF-Based Monocular SLAM Algorithm. Remote Sens. 2022, 14, 571. https://doi.org/10.3390/rs14030571
Labsir S, Pages G, Vivet D. Lie Group Modelling for an EKF-Based Monocular SLAM Algorithm. Remote Sensing. 2022; 14(3):571. https://doi.org/10.3390/rs14030571Chicago/Turabian Style
Labsir, Samy, Gaël Pages, and Damien Vivet. 2022. "Lie Group Modelling for an EKF-Based Monocular SLAM Algorithm" Remote Sensing 14, no. 3: 571. https://doi.org/10.3390/rs14030571