A Robust Visual Inertial Odometry SLAM Considering Robot Self Dynamics

Abstract

In this paper, to deal with the dynamic SLAM problem, we investigate feature tracking and IMU preintegration in visual-inertial odometry (VIO) and design a robust SLAM framework that explicitly considers robot self-dynamics. We propose a self-dynamics and IMU-aided feature tracker to predict initial optical flow and an iterative refinement method that accounts for patch affine deformation and illumination changes, improving tracking accuracy and robustness. Furthermore, we introduce an SE₂(3)-based IMU preintegration that preserves state correlations and consistently encodes robot self-dynamics for subsequent optimisation. A VIO framework with preprocessing, optimisation and loop-closing threads is developed to validate the proposed self-dynamics–aware tracker and SE₂(3)-based preintegration. Experiments, including module tests and ablation studies, demonstrate improved feature tracking accuracy, IMU noise propagation and overall VIO performance when explicitly modelling robot self-dynamics.

Keywords

sensor fusion / SLAM / state estimation / visual-inertial odometry

Acknowledgements

This work was supported by the National Natural Science Foundation of China (Grant 62306185); the Guangdong Basic and Applied Basic Research Foundation (Grant No. 2024A1515012065); and the Shenzhen Science and Technology Programme (Grants JSGGKQTD20221101115656029, KJZD20230923113801004 and ZDCY20250901094531003).

Conflicts of Interest

Hong Liu is an Executive Editor-in-Chief for the journal, and was not involved in peer review process or the decision to publish this article. The authors declare that they have no conflict of interest.

Data Availability Statement

The data that support the findings of this study are available from the corresponding author upon reasonable request.

References

[1] S. Wang, M. N. Nikolić, T. L. Lam, Q. Gao, R. Ding, and T. Zhang, “Robot Manipulation Based on Embodied Visual Perception: A Survey,” CAAI Transactions on Intelligence Technology 10, no. 4 (2025): 945-958, https://doi.org/10.1049/cit2.70022.

[2] S. Baker and I. Matthews, “Lucas-Kanade 20 Years On: A Unifying Framework,” International Journal of Computer Vision 56, no. 3 (2004): 221-255, https://doi.org/10.1023/b:visi.0000011205.11775.fd.

[3] T. Qin, P. Li, and S. Shen, “Vins-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator,” IEEE Transactions on Robotics 34, no. 4 (2018): 1004-1020, https://doi.org/10.1109/tro.2018.2853729.

[4] X. Li and T. Zhang, “Dof-Lio: Lidar-Inertial Odometry With Lightweight Dynamic Object Filter,” in IEEE Transactions on Instrumentation and Measurement (2026).

[5] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-Manifold Preintegration for Real-Time Visual-Inertial Odometry,” IEEE Transactions on Robotics 33, no. 1 (2017): 1-21, https://doi.org/10.1109/tro.2016.2597321.

[6] A. Barrau and S. Bonnabel, “The Invariant Extended Kalman Filter as a Stable Observer,” IEEE Transactions on Automatic Control 62, no. 4 (2016): 1797-1812, https://doi.org/10.1109/tac.2016.2594085.

[7] M. Brossard, A. Barrau, P. Chauchat, and S. Bonnabel, “Associating Uncertainty to Extended Poses for on Lie Group Imu Preintegration With Rotating Earth,” IEEE Transactions on Robotics 38, no. 2 (2021): 998-1015, https://doi.org/10.1109/tro.2021.3100156.

[8] Y. Wang, Y. Ng, I. Sa, et al., Mavis: Multi-Camera Augmented Visual-Inertial Slam Using se2(3) Based Exact Imu Pre-Integration,” in IEEE International Conference on Robotics and Automation (ICRA), (2024), 1694-1700.

[9] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “Orb: An Efficient Alternative to Sift or Surf,” in Proc. IEEE International Conference on Computer Vision, (2011), 2564-2571.

[10] S. You, U. Neumann, and R. Azuma, “Hybrid Inertial and Vision Tracking for Augmented Reality Registration,” in Proceedings of IEEE Virtual Reality, (1999), 260-267.

[11] M. Hwangbo, J. Kim, and T. Kanade, “Inertial-Aided KLT Feature Tracking for a Moving Camera,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), (2009), 1909-1916.

[12] M. Hwangbo, J. Kim, and T. Kanade, “Gyro-Aided Feature Tracking for a Moving Camera: Fusion, Auto-Calibration and Gpu Implementation,” International Journal of Robotics Research 30, no. 14 (2011): 1755-1774, https://doi.org/10.1177/0278364911416391.

[13] H. Ravichandar and A. Dani, “Gyro-Aided Image-Based Tracking Using Mutual Information Optimization and User Inputs,” in 2014 IEEE International Conference on Systems, Man, and Cybernetics (SMC), (2014), 858-863.

[14] G. Yao, M. Williams, and A. Dani, “Gyro-Aided Visual Tracking Using Iterative Earth Mover’s Distance,” in 2016 19th International Conference on Information Fusion (FUSION), (2016), 2317-2323.

[15] L. Chermak, N. Aouf, M. Richardson, and G. Visentin, “Real-Time Smart and Standalone Vision/Imu Navigation Sensor,” Journal of Real-Time Image Processing 16, no. 4 (2019): 1189-1205, https://doi.org/10.1007/s11554-016-0613-z.

[16] M. Quan, B. Mu, and Z. Chai, “Imrl: An Improved Inertial-Aided Klt Feature Tracker,” in 2019 IEEE International Conference on Cybernetics and Intelligent Systems (CIS) and IEEE Conference on Robotics Automation and Mechatronics (RAM), 2019, 392-397.

[17] W. Huang and H. Liu, “A Robust Pixel-Aware Gyro-Aided KLT Feature Tracker for Large Camera Motions,” IEEE Transactions on Instrumentation and Measurement 71 (2021): 1-14, https://doi.org/10.1109/tim.2021.3129493.

[18] T. Lupton and S. Sukkarieh, “Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions,” IEEE Transactions on Robotics 28, no. 1 (2011): 61-76, https://doi.org/10.1109/tro.2011.2170332.

[19] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “Imu Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,” Technical Reports (2015).

[20] S. Shen, N. Michael, and V. Kumar, “Tightly-Coupled Monocular Visual-Inertial Fusion for Autonomous Flight of Rotorcraft Mavs,” in IEEE International Conference on Robotics and Automation (ICRA), (2015), 5303-5310.

[21] K. Eckenhoff, P. Geneva, and G. Huang, “Direct Visual-Inertial Navigation With Analytical Preintegration,” in IEEE International Conference on Robotics and Automation (ICRA), (2017), 1429-1435.

[22] K. Eckenhoff, P. Geneva, and G. Huang, “Closed-Form Preintegration Methods for Graph-based Visual-Inertial Navigation,” International Journal of Robotics Research 38, no. 5 (2019): 563-586, https://doi.org/10.1177/0278364919835021.

[23] Y. Yang, B. P. W. Babu, C. Chen, G. Huang, and L. Ren, “Analytic Combined Imu Integration (Aci 2) for Visual Inertial Navigation,” in IEEE International Conference on Robotics and Automation (ICRA), (2020), 4680-4686.

[24] J. Henawy, Z. Li, W. Yau, and G. Seet, “Accurate Imu Factor Using Switched Linear Systems for Vio,” IEEE Transactions on Industrial Electronics 68, no. 8 (2020): 7199-7208, https://doi.org/10.1109/tie.2020.3000097.

[25] A. Barrau and S. Bonnabel, “A Mathematical Framework for Imu Error Propagation With Applications to Preintegration,” in IEEE International Conference on Robotics and Automation (ICRA), (2020), 5732-5738.

[26] J.-Y. Bouguet, “Pyramidal Implementation of the Affine Lucas Kanade Feature Tracker Description of the Algorithm,” Intel Corporation 5, no. 1-10 (2001): 4, https://robots.stanford.edu/cs223b04/algo_affine_tracking.pdf.

[27] Y. Ryu, H. Roh, and M. Chung, “Video Stabilization for Robot Eye Using IMU-Aided Feature Tracker,” in ICCAS 2010, (2010), 1875-1878.

[28] P. Huber, “Robust Estimation of a Location Parameter,” in Breakthroughs in Statistics (Springer, 1992), 492-518.

[29] M. Calonder, V. Lepetit, C. Strecha, and P. Fua, “Brief: Binary Robust Independent Elementary Features,” in European Conference on Computer Vision, (2010), 778-792.

[30] M. Rafael 2017. DBoW3 dbow3, https://github.com/rmsalinas/DBow3.

[31] M. Burri, J. Nikolic, P. Gohl, et al., The EuRoC Micro Aerial Vehicle Datasets,” International Journal of Robotics Research 35, no. 10 (2016): 1157-1163, https://doi.org/10.1177/0278364915620033.

[32] C. Campos, R. Elvira, J. Rodríguez, J. Montiel, and J. Tardós, “Orb-slam3: An Accurate Open-Source Library for Visual, Visual-Inertial, and Multimap Slam,” IEEE Transactions on Robotics 37, no. 6 (2021): 1874-1890, https://doi.org/10.1109/tro.2021.3075644.

[33] J. Engel, V. Koltun, and D. Cremers, “Direct Sparse Odometry,” IEEE Transactions on Pattern Analysis and Machine Intelligence 40, no. 3 (2017): 611-625, https://doi.org/10.1109/tpami.2017.2658577.