Egomotion Estimation and Reconstruction with Kalman Filters and GPS Integration

  • Haokun GengEmail author
  • Hsiang-Jen Chien
  • Radu Nicolescu
  • Reinhard Klette
Conference paper
Part of the Lecture Notes in Computer Science book series (LNCS, volume 9256)


This paper presents an approach for egomotion estimation over stereo image sequences combined with extra GPS data. The accuracy of the estimated motion data is tested with 3D roadside reconstruction. Our proposed method follows the traditional flowchart of many visual odometry algorithms: it firstly establishes the correspondences between the keypoints of every two frames, then it uses the depth information from the stereo matching algorithms, and it finally computes the best description of the cameras’ motion. However, instead of simply using keypoints from consecutive frames, we propose a novel technique that uses a set of augmented and selected keypoints, which are carefully tracked by a Kalman filter fusion. We also propose to use the GPS data for each key frame in the input sequence, in order to reduce the positioning errors of the estimations, so that the drift errors could be corrected at each key frame. Finally, the overall growth of the build-up errors can be bounded within a certain range. A least-squares process is used to minimise the reprojection error and to ensure a good pair of translation and rotation measures, frame by frame. Experiments are carried out for trajectory estimation, or combined trajectory and 3D scene reconstruction, using various stereo-image sequences.


Egomotion estimation Visual odometry Kalman filter GPS input Roadside reconstruction 


Unable to display preview. Download preview PDF.

Unable to display preview. Download preview PDF.


  1. 1.
    Badino, H., Franke, U., Rabe, C., Gehrig, S.: Stereo vision-based detection of moving objects under strong camera motion. In: Proceedings of Computer Vision Theory and Applications, vol. 2, pp. 253–260 (2006)Google Scholar
  2. 2.
    Badino, H., Kanade, T.: A head-wearable short-baseline stereo system for the simultaneous estimation of structure and motion. In: Proceedings of the IAPR Conference on Machine Vision Applications, pp. 185–189 (2011)Google Scholar
  3. 3.
    Badino, H., Yamamoto, A., Kanade, T.: Visual odometry by multi-frame feature integration. In: Proceedings of ICCV Workshop on Computer Vision for Autonomous Driving, pp. 222–229 (2013)Google Scholar
  4. 4.
    Besl, P., McKay, N.D.: A method for registration of 3-d shapes. Proceedings of Pattern Analysis and Machine Intelligence 14, 239–256 (1992)CrossRefGoogle Scholar
  5. 5.
    Demirdjian, D., Darrell, T.: Motion estimation from disparity images. Proceedings of ICCV 1, 213–218 (2001)Google Scholar
  6. 6.
    Franke, U., Rabe, C., Badino, H., Gehrig, S.K.: 6D-Vision: fusion of stereo and motion for robust environment perception. In: Kropatsch, W.G., Sablatnig, R., Hanbury, A. (eds.) DAGM 2005. LNCS, vol. 3663, pp. 216–223. Springer, Heidelberg (2005) CrossRefGoogle Scholar
  7. 7.
    Geiger, A., Lenz, P., Stiller, C., Urtasun, R.: Vision meets robotics: The kitti dataset. International Journal of Robotics Research 32(11), 1231–1237 (2013)CrossRefGoogle Scholar
  8. 8.
    Julier, S., Uhlmann, J.: Unscented filtering and nonlinear estimation. Proceedings of the IEEE 92(3), 401–422 (2004)CrossRefGoogle Scholar
  9. 9.
    Kitt, B., Geiger, A., Lategahn, H.: Visual odometry based on stereo image sequences with ransac-based outlier rejection scheme. In: Proceedings of Intelligent Vehicles Symposium, pp. 486–492 (2010)Google Scholar
  10. 10.
    Klette, R.: Concise Computer Vision. Springer, London (2014) CrossRefzbMATHGoogle Scholar
  11. 11.
    Maimone, M., Cheng, Y., Matthies, L.: Two years of visual odometry on the mars exploration rovers. Journal of Field Robotics 24, 169–186 (2007)CrossRefGoogle Scholar
  12. 12.
    Matthies, L.: Dynamic stereo vision. Ph.D. dissertation, Carnegie Mellon University (1989)Google Scholar
  13. 13.
    Matthies, L., Shafer, S.A.: Error modeling in stereo navigation. IEEE Journal of Robotics and Automation 3, 239–250 (1987)CrossRefGoogle Scholar
  14. 14.
    Milella, A., Siegwart, R.: Stereo-based ego-motion estimation using pixel tracking and iterative closest point. In: Proceedings of IEEE International Conference on Computer Vision Systems, pp. 21–21 (2006)Google Scholar
  15. 15.
    Olson, C., Matthies, L., Schoppers, M., Maimone, M.: Stereo ego-motion improvements for robust rover navigation. In: Proceedings of IEEE International Conference on Robotics and Automation, vol. 2, pp. 1099–1104 (2001)Google Scholar
  16. 16.
    Rublee, E., Rabaud, V., Konolige, K., Bradski, G.: Orb: An efficient alternative to sift or surf. In: Proceedings of ICCV, pp. 2564–2571 (2011)Google Scholar
  17. 17.
    Scaramuzza, D., Fraundorfer, F.: Visual odometry tutorial. Robotics Automation Magazine 18(4), 80–92 (2011)CrossRefGoogle Scholar
  18. 18.
    Shakernia, O., Vidal, R., Sastry, S.: Omnidirectional egomotion estimation from back-projection flow. In: Proceedings of CVPR Workshop, vol. 7, pp. 82–82 (2003)Google Scholar
  19. 19.
    Sibley, G., Sukhatme, G.S., Matthies, L.: The iterated sigma point kalman filter with applications to long range stereo. In: Proceedings of Robotics: Science and Systems (2006)Google Scholar
  20. 20.
    Song, Z., Klette, R.: Robustness of point feature detection. In: Wilson, R., Hancock, E., Bors, A., Smith, W. (eds.) CAIP 2013, Part II. LNCS, vol. 8048, pp. 91–99. Springer, Heidelberg (2013) CrossRefGoogle Scholar
  21. 21.
    Tian, T.Y., Tomasi, C., Heeger, D.J.: Comparison of approaches to egomotion computation. In: Proceedings of CVPR, pp. 315–320 (1996)Google Scholar

Copyright information

© Springer International Publishing Switzerland 2015

Authors and Affiliations

  • Haokun Geng
    • 1
    Email author
  • Hsiang-Jen Chien
    • 2
  • Radu Nicolescu
    • 1
  • Reinhard Klette
    • 2
  1. 1.Department of Computer ScienceThe University of AucklandAucklandNew Zealand
  2. 2.School of EngineeringAuckland University of TechnologyAucklandNew Zealand

Personalised recommendations