Abstract
Visual odometry (VO) is an important problem studied in robotics and computer vision in which the relative camera motion is computed through visual information. In this work, we propose to reduce the error accumulation of a dual stereo VO system (4 cameras) computing 6 degrees of freedom poses by fusing two independent stereo odometry with a nonlinear optimization. Our approach computes two stereo odometries employing the LIBVISO2 algorithm and later merge them by using image correspondences between the stereo pairs and minimizing the reprojection error with graph-based bundle adjustment. Experiments carried out on the KITTI odometry datasets show that our method computes more accurate estimates (measured as the Relative Positioning Error) in comparison to the traditional stereo odometry (stereo bundle adjustment). In addition, the proposed method has a similar or better odometry accuracy compared to ORB-SLAM2 and UCOSLAM algorithms.
Article PDF
Similar content being viewed by others
Explore related subjects
Discover the latest articles, news and stories from top researchers in related subjects.Avoid common mistakes on your manuscript.
Availability of data, code and materials
Data, code and other materials are available from the GitHub of Natalnet Associate Labs at: https://github.com/Natalnet
References
Macario Barros, A., Michel, M., Moline, Y., Corre, G., Carrel, F.: A comprehensive survey of visual slam algorithms. Robotics 11(1) (2022). https://doi.org/10.3390/robotics11010024
Cadena, C., Carlone, L., Carrillo, H., Latif, Y., Scaramuzza, D., Neira, J., Reid, I., Leonard, J.J.: Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age. Trans. Rob. 32(6), 1309–1332 (2016). https://doi.org/10.1109/TRO.2016.2624754
Thrun, S., Burgard, W., Fox, D.: Probabilistic Robotics (Intelligent Robotics and Autonomous Agents). The MIT Press, USA (2005)
Trucco, E., Verri, A.: Introductory Techniques for 3-D Computer Vision. Prentice Hall, USA (1998)
Ortiz-Fernandez, L.E., Cabrera-Ávila, E.V., Gonçalves, L.M.G.: Depth data error modeling of the ZED 3D vision sensor from stereolabs. Electron. Lett. Comput. Vision Image Anal. 17(1) (2018). https://doi.org/10.5565/rev/elcvia.1084
Scaramuzza, D., Fraundorfer, F.: Visual odometry [tutorial]. IEEE Robot. Autom. Mag. 18(4), 80–92 (2011). https://doi.org/10.1109/MRA.2011.943233
Fraundorfer, F., Scaramuzza, D.: Visual odometry : Part ii: Matching, robustness, optimization, and applications. IEEE Robot. Automat. Mag. 19(2), 78–90 (2012). https://doi.org/10.1109/MRA.2012.2182810
Nister, D., Naroditsky, O., Bergen, J.: Visual odometry. In: Proceedings of the 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, 2004. CVPR 2004., vol. 1, p. (2004). https://doi.org/10.1109/CVPR.2004.1315094
Klein, G., Murray, D.: Parallel tracking and mapping for small ar workspaces. In: 2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality, pp. 225–234 (2007). https://doi.org/10.1109/ISMAR.2007.4538852
Pire, T., Fischer, T., Castro, G., De-Cristóforis, P., Civera, J., Jacobo-Berlles, J.: S-ptam: Stereo parallel tracking and mapping. Robot. Auton. Syst. 93, 27–42 (2017). https://doi.org/10.1016/j.robot.2017.03.019
Mur-Artal, R., Tardós, J.D.: Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras. IEEE Trans. Rob. 33(5), 1255–1262 (2017). https://doi.org/10.1109/TRO.2017.2705103
Muñoz-Salinas, R., Medina-Carnicer, R.: UcoSLAM: Simultaneous localization and mapping by fusion of keypoints and squared planar markers. Pattern Recognition, 107193 (2020)
Taketomi, T., Uchiyama, H., Ikeda, S.: Visual slam algorithms: a survey from 2010 to 2016. IPSJ Trans. Comput. Vision Appl. 9(1), 16 (2017). https://doi.org/10.1186/s41074-017-0027-2
Borenstein, J., Feng, L.: Measurement and correction of systematic odometry errors in mobile robots. IEEE Trans. Robot. Autom. 12(6), 869–880 (1996). https://doi.org/10.1109/70.544770
Cabrera-Avila, E.V., Ortiz-Fernandez, L.E., da Silva, B.M.F.d., Clua, E.W.G., Gonçalves, L.M.G.: A versatile method for depth data error estimation in RGB-D sensors. Sensors 18(9) (2018). https://doi.org/10.3390/s18093122
Silva, B.M.F., Gonçalves, L.M.G.: A fast visual odometry and mapping system for rgb-d cameras. In: 2014 Joint Conference on Robotics: SBR-LARS Robotics Symposium and Robocontrol, pp. 55–60 (2014). https://doi.org/10.1109/SBR.LARS.Robocontrol.2014.35
Leutenegger, S., Lynen, S., Bosse, M., Siegwart, R., Furgale, P.: Keyframe-based visual-inertial odometry using nonlinear optimization. Internat. J. Robot. Res. 34(3), 314–334 (2015). https://doi.org/10.1177/0278364914554813
Merfels, C., Stachniss, C.: Pose fusion with chain pose graphs for automated driving. In: 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3116–3123 (2016). https://doi.org/10.1109/IROS.2016.7759482
Mo, S., Zhou, Z., Du, S., Xiang, C., Kuang, C., Wu, J.: Implementation of mixed sequential kalman filters for vision-aided GNSS/INS integrated navigation system. In: Sun, J., Yang, C., Guo, S. (eds.) China Satellite Navigation Conference (CSNC) 2018 Proceedings, pp. 629–641. Springer, Singapore (2018)
Qin, T., Cao, S., Pan, J., Shen, S.: A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors (2019)
Mascaro, R., Teixeira, L., Hinzmann, T., Siegwart, R., Chli, M.: GOMSF: Graph-optimization based multi-sensor fusion for robust UAV pose estimation. In: 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1421–1428 (2018). https://doi.org/10.1109/ICRA.2018.8460193
Lynen, S., Achtelik, M.W., Weiss, S., Chli, M., Siegwart, R.: A robust and modular multi-sensor fusion approach applied to MAV navigation. In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 3923–3929 (2013). https://doi.org/10.1109/IROS.2013.6696917
Qin, T., Li, P., Shen, S.: Vins-mono: A robust and versatile monocular visual-inertial state estimator. IEEE Trans. Rob. 34(4), 1004–1020 (2018). https://doi.org/10.1109/TRO.2018.2853729
Ortiz-Fernandez, L.E., Cabrera-Avila, E.V., Silva, B.M.F.d., Gonçalves, L.M.G.: Smart artificial markers for accurate visual mapping and localization. Sensors 21(2) (2021). https://doi.org/10.3390/s21020625
Quigley, M., Conley, K., Gerkey, B.P., Faust, J., Foote, T., Leibs, J., Wheeler, R., Ng, A.Y.: ROS: An open-source robot operating system. In: Workshops at the IEEE international conference on robotics and automation (2009)
Mouragnon, E., Lhuillier, M., Dhome, M., Dekeyser, F., Sayd, P.: Real time localization and 3d reconstruction. In: 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR’06), vol. 1, pp. 363–370 (2006). https://doi.org/10.1109/CVPR.2006.236
Grisetti, G., Kümmerle, R., Stachniss, C., Burgard, W.: A tutorial on graph-based slam. IEEE Intell. Transp. Syst. Mag. 2(4), 31–43 (2010). https://doi.org/10.1109/MITS.2010.939925
Kümmerle, R., Grisetti, G., Strasdat, H., Konolige, K., Burgard, W.: G2o: A general framework for graph optimization. In: 2011 IEEE International Conference on Robotics and Automation, pp. 3607–3613 (2011). https://doi.org/10.1109/ICRA.2011.5979949
Geiger, A., Lenz, P., Urtasun, R.: Are we ready for autonomous driving? the KITTI vision benchmark suite. In: Conference on Computer Vision and Pattern Recognition (CVPR) (2012)
Cabrera-Ávila, E.V.: Windowed optimization for stereo visual odometry fusion. PhD thesis, Universidade Federal do Rio Grande do Norte (2022). https://repositorio.ufrn.br/handle/123456789/47206
Cabrera-Avila, E.V., Silva, B.M.F., Gonçalves, L.M.G.: Sliding windowed optimization algorithm for fusion of redundant stereo visual odometries. In: 2022 Latin American Robotics Symposium (LARS), 2022 Brazilian Symposium on Robotics (SBR), and 2022 Workshop on Robotics in Education (WRE), pp. 300–305 (2022). https://doi.org/10.1109/LARS/SBR/WRE56824.2022.9995979
Lowe, D.G.: Distinctive image features from scale-invariant keypoints. Int. J. Comput. Vision 60(2), 91–110 (2004). https://doi.org/10.1023/B:VISI.0000029664.99615.94
Bay, H., Ess, A., Tuytelaars, T., Van Gool, L.: Speeded-up robust features (surf). Computer Vision and Image Understanding 110(3), 346–359 (2008). https://doi.org/10.1016/j.cviu.2007.09.014. Similarity Matching in Computer Vision and Multimedia
Rublee, E., Rabaud, V., Konolige, K., Bradski, G.: ORB: An efficient alternative to SIFT or SURF. In: 2011 International Conference on Computer Vision, pp. 2564–2571 (2011). https://doi.org/10.1109/ICCV.2011.6126544
Aqel, M.O.A., Marhaban, M.H., Saripan, M.I., Ismail, N.B.: Review of visual odometry: types, approaches, challenges, and applications. Springerplus 5(1), 1897 (2016). https://doi.org/10.1186/s40064-016-3573-7
Wu, R., Gao, Y.: Research on underwater complex scene slam algorithm based on image enhancement. Sensors 22(21) (2022). https://doi.org/10.3390/s22218517
Hartley, R., Zisserman, A.: Multiple View Geometry in Computer Vision, 2nd edn. Cambridge University Press, USA (2003)
Grisetti, G., Guadagnino, T., Aloise, I., Colosi, M., Della Corte, B., Schlegel, D.: Least squares optimization: From theory to practice. Robotics 9(3) (2020). https://doi.org/10.3390/robotics9030051
Se, S., Lowe, D., Little, J.: Mobile robot localization and mapping with uncertainty using scale-invariant visual landmarks. Int. J. Robot. Res. 21(8), 735–758 (2002). https://doi.org/10.1177/027836402761412467
Bloesch, M., Omari, S., Hutter, M., Siegwart, R.: Robust visual inertial odometry using a direct EKF-based approach. In: 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 298–304 (2015). https://doi.org/10.1109/IROS.2015.7353389
Shen, S., Mulgaonkar, Y., Michael, N., Kumar, V.: Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft MAV. In: 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 4974–4981 (2014). https://doi.org/10.1109/ICRA.2014.6907588
Nabavi-Chashmi, S.-Y., Asadi, D., Ahmadi, K.: Image-based uav position and velocity estimation using a monocular camera. Control. Eng. Pract. 134, 105460 (2023). https://doi.org/10.1016/j.conengprac.2023.105460
Strasdat, H., Montiel, J.M.M., Davison, A.J.: Real-time monocular slam: Why filter? In: 2010 IEEE International Conference on Robotics and Automation, pp. 2657–2664 (2010). https://doi.org/10.1109/ROBOT.2010.5509636
Michot, J., Bartoli, A., Gaspard, F.: Bi-objective bundle adjustment with application to multi-sensor SLAM (2010)
Zheng, F., Tang, H., Liu, Y.: Odometry-vision-based ground vehicle motion estimation with SE(2)-constrained SE(3) poses. IEEE Trans. Cybernet. 49(7), 2652–2663 (2019). https://doi.org/10.1109/TCYB.2018.2831900
Mur-Artal, R., Tardós, J.D.: Visual-inertial monocular SLAM with map reuse. IEEE Robot. Automat. Lett. 2(2), 796–803 (2017). https://doi.org/10.1109/LRA.2017.2653359
Liu, H., Chen, M., Zhang, G., Bao, H., Bao, Y.: ICE-BA: Incremental, consistent and efficient bundle adjustment for visual-inertial SLAM. In: 2018 IEEE/CVF Conference on Computer Vision and Pattern Recognition, pp. 1974–1982 (2018). https://doi.org/10.1109/CVPR.2018.00211
Agarwal, S., Mierle, K., Team, T.C.S.: Ceres Solver. https://github.com/ceres-solver/ceres-solver
Dellaert, F., Kaess, M.: Factor graphs for robot perception. Found. Trends® in Robot. 6(1-2), 1–139 (2017). https://doi.org/10.1561/2300000043
Jurić, A., Kendeš, F., Marković, I., Petrović, I.: A comparison of graph optimization approaches for pose estimation in slam. In: 2021 44th International Convention on Information, Communication and Electronic Technology (MIPRO), pp. 1113–1118 (2021). https://doi.org/10.23919/MIPRO52101.2021.9596721
Triggs, B., McLauchlan, P.F., Hartley, R.I., Fitzgibbon, A.W.: Bundle adjustment – a modern synthesis. In: Triggs, B., Zisserman, A., Szeliski, R. (eds.) Vision algorithms: theory and practice, pp. 298–372. Springer, Berlin, Heidelberg (2000)
Kazik, T., Kneip, L., Nikolic, J., Pollefeys, M., Siegwart, R.: Real-time 6D stereo visual odometry with non-overlapping fields of view. In: 2012 IEEE Conference on computer vision and pattern recognition, pp. 1529–1536 (2012). https://doi.org/10.1109/CVPR.2012.6247843
Mhiri, R., Vasseur, P., Mousset, S., Boutteau, R., Bensrhair, A.: Visual odometry with unsynchronized multi-cameras setup for intelligent vehicle application. In: 2014 IEEE Intelligent vehicles symposium proceedings, pp. 1339–1344 (2014). https://doi.org/10.1109/IVS.2014.6856533
Geiger, A., Ziegler, J., Stiller, C.: Stereoscan: Dense 3D reconstruction in real-time. In: Intelligent vehicles symposium (IV) (2011)
Lourakis, M.I.A., Argyros, A.A.: Sba: A software package for generic sparse bundle adjustment. ACM Trans. Math. Softw. 36(1) (2009). https://doi.org/10.1145/1486525.1486527
Kitt, B., Geiger, A., Lategahn, H.: Visual odometry based on stereo image sequences with RANSAC-based outlier rejection scheme. In: 2010 IEEE Intelligent vehicles symposium, pp. 486–492 (2010). https://doi.org/10.1109/IVS.2010.5548123
Fischler, M.A., Bolles, R.C.: Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography. Commun. ACM 24(6), 381–395 (1981). https://doi.org/10.1145/358669.358692
Marquardt, D.W.: An algorithm for least-squares estimation of nonlinear parameters. J. Soc. Ind. Appl. Math. 11(2), 431–441 (1963)
Chen, Y., Davis, T.A., Hager, W.W.: Algorithm 887: Cholmod, supernodal sparse cholesky factorization and update/downdate. ACM Trans. Math. Softw. 1–14 (2008)
Davis, T.A.: Direct Methods for Sparse Linear Systems (Fundamentals of Algorithms 2). Society for Industrial and Applied Mathematics, USA (2006)
Kümmerle, R., Steder, B., Dornhege, C., Ruhnke, M., Grisetti, G., Stachniss, C., Kleiner, A.: On measuring the accuracy of SLAM algorithms. Auton. Robot. 27(4), 387 (2009). https://doi.org/10.1007/s10514-009-9155-6
Acknowledgements
We thank the Brazilian National Research Council (CNPq) for the grants of Elizabeth V. Cabrera-Avila and Luiz M. G. Gonçalves. This manuscript is a compilation of the full thesis by Elizabeth Viviana Cabrera-Ávila, freely available as a technical report from our Graduate Program Library (https://sisbi.ufrn.br/biblioteca/bczm). This repository works as a preprint server for hosting the university graduate works.
Funding
This work is partially supported by Universidade Federal do Rio Grande do Norte, Natal, Brazil and by CNPq Brazil under grant 311789/2021-8.
Author information
Authors and Affiliations
Contributions
All authors have made substantial contributions to the conception or design of the work; the analysis, acquisition, and assembly of material and equipment; drafted the work and revised it critically for important intellectual content; and approved the version to be submitted, as described next. Viviana E. Cabrera-Ávila: Conceptualization, Methodology, Formal analysis and investigation, Writing - original draft preparation. Bruno M. F. da Silva: Conceptualization, Methodology, Formal analysis and investigation, Writing - review and editing, Funding acquisition, Resources, Supervision. Luiz M. G. Gonçalves: Conceptualization, Methodology, Formal analysis and investigation, Writing - original draft preparation, Writing - review and editing, Funding acquisition, Resources, Supervision. In addition, all authors agree to be accountable for all aspects of the work in ensuring that questions related to the accuracy or integrity of any part of the work are appropriately investigated and resolved.
Corresponding author
Ethics declarations
Conflicts of interest
The authors declare that there are no conflicts of interests nor competing interests for this work.
Ethics approval
Not applicable
Consent to participate
Not applicable
Consent for publication
Not applicable
Additional information
Publisher's Note
Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.
Rights and permissions
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article’s Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article’s Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecommons.org/licenses/by/4.0/.
About this article
Cite this article
Cabrera-Ávila, E.V., da Silva, B.M.F. & Gonçalves, L.M.G. Nonlinearly Optimized Dual Stereo Visual Odometry Fusion. J Intell Robot Syst 110, 56 (2024). https://doi.org/10.1007/s10846-024-02069-4
Received:
Accepted:
Published:
DOI: https://doi.org/10.1007/s10846-024-02069-4