Journal of Mathematical Imaging and Vision

, Volume 58, Issue 1, pp 102–129 | Cite as

Second-Order Recursive Filtering on the Rigid-Motion Lie Group \({\text {SE}}_{3}\) Based on Nonlinear Observations

  • Johannes Berger
  • Frank Lenzen
  • Florian Becker
  • Andreas Neufeld
  • Christoph Schnörr


Camera motion estimation from observed scene features is an important task in image processing to increase the accuracy of many methods, e.g., optical flow and structure-from-motion. Due to the curved geometry of the state space \({\text {SE}}_{3}\) and the nonlinear relation to the observed optical flow, many recent filtering approaches use a first-order approximation and assume a Gaussian a posteriori distribution or restrict the state to Euclidean geometry. The physical model is usually also limited to uniform motions. We propose a second-order optimal minimum energy filter that copes with the full geometry of \({\text {SE}}_{3}\) as well as with the nonlinear dependencies between the state space and observations., which results in a recursive description of the optimal state and the corresponding second-order operator. The derived filter enables reconstructing motions correctly for synthetic and real scenes, e.g., from the KITTI benchmark. Our experiments confirm that the derived minimum energy filter with higher-order state differential equation copes with higher-order kinematics and is also able to minimize model noise. We also show that the proposed filter is superior to state-of-the-art extended Kalman filters on Lie groups in the case of linear observations and that our method reaches the accuracy of modern visual odometry methods.


Minimum energy filter Lie group Recursive filtering Constant acceleration model Optimal control Visual odometry 



This work was supported by the DFG (German Research Foundation), Grant GRK 1653.


  1. 1.
    Absil, P.A., Mahony, R., Sepulchre, R.: Optimization Algorithms on Matrix Manifolds. Princeton University Press, Princeton (2008)CrossRefMATHGoogle Scholar
  2. 2.
    Agrachev, A.A., Sachkov, Y.: Control Theory from the Geometric Viewpoint, vol. 2. Springer, Berlin (2004)CrossRefMATHGoogle Scholar
  3. 3.
    Aguiar, A., Hespanha, J.: Minimum-energy state estimation for systems with perspective outputs. IEEE Trans. Autom. Control 51(2), 226–241 (2006). doi: 10.1109/TAC.2005.861686 MathSciNetCrossRefGoogle Scholar
  4. 4.
    Arulampalam, M.S., Maskell, S., Gordon, N., Clapp, T.: A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking. IEEE Trans. Signal Process. 50(2), 174–188 (2002)CrossRefGoogle Scholar
  5. 5.
    Athans, M., Falb, P.: Optimal Control. An Introduction to the Theory and Its Applications. McGraw-Hill, New York (1966)MATHGoogle Scholar
  6. 6.
    Badino, H., Yamamoto, A., Kanade, T.: Visual odometry by multi-frame feature integration. In: 2013 IEEE International Conference on Computer Vision Workshops (ICCVW), pp. 222–229. IEEE (2013)Google Scholar
  7. 7.
    Bain, A., Crisan, D.: Fundamentals of Stochastic Filtering. Springer, Berlin (2009)CrossRefMATHGoogle Scholar
  8. 8.
    Barfoot, T.D., Furgale, P.T.: Associating uncertainty with three-dimensional poses for use in estimation problems. IEEE Trans. Robot. 30(3), 679–693 (2014)CrossRefGoogle Scholar
  9. 9.
    Bellavia, F., Fanfani, M., Pazzaglia, F., Colombo, C.: Robust selective stereo SLAM without loop closure and bundle adjustment. In: Image Analysis and Processing—ICIAP 2013, pp. 462–471. Springer (2013)Google Scholar
  10. 10.
    Berger, J., Neufeld, A., Becker, F., Lenzen, F., Schnörr, C.: Second order minimum energy filtering on SE(3) with nonlinear measurement equations. In: Scale Space and Variational Methods in Computer Vision, pp. 397–409. Springer (2015)Google Scholar
  11. 11.
    Bonnabel, S.: Left-invariant extended Kalman filter and attitude estimation. In: IEEE Conference on Decision and Control, pp. 1027–1032 (2007)Google Scholar
  12. 12.
    Bourmaud, G., Mégret, R.: Robust large scale monocular visual SLAM. In: Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, pp. 1638–1647 (2015)Google Scholar
  13. 13.
    Bourmaud, G., Mégret, R., Arnaudon, M., Giremus, A.: Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions. J. Math. Imaging Vis. 51(1), 209–228 (2015)MathSciNetCrossRefMATHGoogle Scholar
  14. 14.
    Brigo, D., Hanzon, B., Le Gland, F.: Approximate nonlinear filtering by projection on exponential manifolds of densities. Bernoulli 5(3), 495–534 (1999)MathSciNetCrossRefMATHGoogle Scholar
  15. 15.
    Chikuse, Y.: Statistics on Special Manifolds, vol. 174. Springer, Berlin (2012)MATHGoogle Scholar
  16. 16.
    Chirikjian, G.S.: Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications, vol. 2. Springer, Berlin (2011)Google Scholar
  17. 17.
    Daum, F.: Nonlinear filters: beyond the Kalman filter. IEEE A&E Syst. Mag. 20(8, Part 2), 57–69 (2005)CrossRefGoogle Scholar
  18. 18.
    Daum, F., Huang, J.: Curse of dimensionality and particle filters. In: Aerospace Conference (2003). doi: 10.1109/AERO.2003.1235126
  19. 19.
    Dieci, L., Eirola, T.: Positive definiteness in the numerical solution of Riccati differential equations. Numer. Math. 67(3), 303–313 (1994)MathSciNetCrossRefMATHGoogle Scholar
  20. 20.
    Frogerais, P., Bellanger, J., Senhadji, L.: Various ways to compute the continuous-discrete extended Kalman filter. IEEE Trans. Autom. Control 57, 1000–1004 (2012). doi: 10.1109/TAC.2011.2168129 MathSciNetCrossRefGoogle Scholar
  21. 21.
    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)Google Scholar
  22. 22.
    Geiger, A., Ziegler, J., Stiller, C.: Stereoscan: Dense 3D reconstruction in real-time. In: Intelligent Vehicles Symposium (IV). IEEE (2011)Google Scholar
  23. 23.
    Gordon, N.J., Salmond, D.J., Smith, A.F.: Novel approach to nonlinear/non-Gaussian Bayesian state estimation. In: IEE Proceedings F-Radar and Signal Processing, vol. 140, pp. 107–113. IET (1993)Google Scholar
  24. 24.
    Hairer, E., Lubich, C., Wanner, G.: Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations. Springer, Berlin (2006)MATHGoogle Scholar
  25. 25.
    Hartley, R., et al.: In defense of the eight-point algorithm. IEEE Trans. Pattern Anal. Mach. Intell. 19(6), 580–593 (1997)CrossRefGoogle Scholar
  26. 26.
    Hauberg, S., Lauze, F., Pedersen, K.: Unscented Kalman filtering on Riemannian manifolds. J. Math. Imaging Vis. 46(1), 103–120 (2013)MathSciNetCrossRefMATHGoogle Scholar
  27. 27.
    Hsu, E.P.: Stochastic Analysis on Manifolds, vol. 38. American Mathematical Soc., Providence (2002)MATHGoogle Scholar
  28. 28.
    Jazwinski, A.H.: Stochastic Processes and Filtering Theory. Courier Corporation, Chelmsford (2007)MATHGoogle Scholar
  29. 29.
    Julier, S.J., Uhlmann, J.K.: New Extension of the Kalman Filter to Nonlinear Systems. In: AeroSense’97, pp. 182–193. International Society for Optics and Photonics (1997)Google Scholar
  30. 30.
    Jurdjevic, V.: Geometric Control Theory. Cambridge university press, Cambridge (1997)MATHGoogle Scholar
  31. 31.
    Kalman, R.E.: A new approach to linear filtering and prediction problems. J Fluids Eng. 82(1), 35–45 (1960)Google Scholar
  32. 32.
    Krener, A.J.: The convergence of the minimum energy estimator. In: New Trends in Nonlinear Dynamics and Control and their Applications. Springer (2003)Google Scholar
  33. 33.
    Kwon, J., Choi, M., Park, F.C., Chun, C.: Particle filtering on the Euclidean group: framework and applications. Robotica 25(6), 725–737 (2007)CrossRefGoogle Scholar
  34. 34.
    Kwon, J., Lee, K.M.: Monocular SLAM with locally planar landmarks via geometric Rao-Blackwellized particle filtering on Lie Groups. In: CVPR 2010, pp. 1522–1529. IEEE (2010)Google Scholar
  35. 35.
    Lancaster, P., Rodman, L.: Existence and uniqueness theorems for the algebraic Riccati equation. Int. J. Control 32(2), 285–309 (1980)MathSciNetCrossRefMATHGoogle Scholar
  36. 36.
    Markley, F.L.: Attitude error representations for Kalman filtering. J. Guid. Control Dyn. 26(2), 311–317 (2003)CrossRefGoogle Scholar
  37. 37.
    Miller, M.I., Srivastava, A., Grenander, U.: Conditional-mean estimation via jump-diffusion processes in multiple target tracking/recognition. IEEE Trans. Signal Process. 43(11), 2678–2690 (1995)CrossRefGoogle Scholar
  38. 38.
    Mortensen, R.E.: Maximum-likelihood recursive nonlinear filtering. J. Opt. Theory Appl. 2(6), 386–394 (1968)MathSciNetCrossRefMATHGoogle Scholar
  39. 39.
    Murray, R., Li, Z., Sastry, S.: A Mathematical Introduction to Robotic Manipulation. CRC Press, Boca Raton (1994)MATHGoogle Scholar
  40. 40.
    Nasihatkon, B., Hartley, R., Trumpf, J.: A generalized projective reconstruction theorem and depth constraints for projective factorization. Int. J. Comput. Vis. 115(2), 87–114 (2015)Google Scholar
  41. 41.
    Nistér, D.: An efficient solution to the five-point relative pose problem. IEEE Trans. Pattern Anal. Mach. Intell. 26(6), 756–770 (2004)CrossRefGoogle Scholar
  42. 42.
    Petersen, K.B., Pedersen, M.S., et al.: The matrix cookbook. Tech. Univ. Den. 7, 15 (2008)Google Scholar
  43. 43.
    Rentmeesters, Q., Absil, P.A., Van Dooren, P., Gallivan, K., Srivastava, A.: An efficient particle filtering technique on the Grassmann manifold. In: 2010 IEEE International Conference on Acoustics Speech and Signal Processing (ICASSP), pp. 3838–3841. IEEE (2010)Google Scholar
  44. 44.
    Saccon, A., Aguiar, A.P., Hauser, J.: Lie group projection operator approach: optimal control on T SO (3). In: CDC, pp. 6973–6978 (2011)Google Scholar
  45. 45.
    Saccon, A., Hauser, J., Aguiar, A.: Optimal control on Lie groups: the projection operator approach. IEEE Trans. Autom. Control 58(9), 2230–2245 (2013). doi: 10.1109/TAC.2013.2258817 MathSciNetCrossRefGoogle Scholar
  46. 46.
    Saccon, A., Trumpf, J., Mahony, R., Aguiar, A.P.: Second-order-optimal filters on Lie groups. In: CDC (2013)Google Scholar
  47. 47.
    Särkkä, S., Hartikainen, J., Solin, A.: EKF/UKF toolbox for Matlab V1.3 (2011).
  48. 48.
    Schön, T., Gustafsson, F., Nordlund, P.J.: Marginalized particle filters for mixed linear/nonlinear state-space models. IEEE Trans. Signal Process. 53(7), 2279–2289 (2005). doi: 10.1109/TSP.2005.849151 MathSciNetCrossRefGoogle Scholar
  49. 49.
    Selig, J.: Lie groups and Lie algebras in robotics. In: Computational Noncommutative Algebra and Applications, pp. 101–125. Springer (2004)Google Scholar
  50. 50.
    Sturm, P., Triggs, B.: A factorization based algorithm for multi-image projective structure and motion. In: ECCV, pp. 709–720. Springer, Cambridge, England (1996)Google Scholar
  51. 51.
    Tompkins, F., Wolfe, P.J.: Bayesian filtering on the Stiefel manifold. In: 2nd IEEE International Workshop on Computational Advances in Multi-sensor Adaptive Processing, 2007 (CAMPSAP 2007), pp. 261–264. IEEE (2007)Google Scholar
  52. 52.
    Triggs, B., McLauchlan, P.F., Hartley, R.I., Fitzgibbon, A.W.: Bundle adjustment—a modern synthesis. In: Vision Algorithms: Theory and Practice, pp. 298–372. Springer (2000)Google Scholar
  53. 53.
    Tyagi, A., Davis, J.W.: A recursive filter for linear systems on Riemannian manifolds. In: CVPR, pp. 1–8. IEEE (2008)Google Scholar
  54. 54.
    Vogel, C., Schindler, K., Roth, S.: 3D scene flow estimation with a Rigid motion prior. In: 2011 IEEE International Conference on Computer Vision (ICCV), pp. 1291–1298. IEEE (2011)Google Scholar
  55. 55.
    Wang, Y., Chirikjian, G.S.: Nonparametric second-order theory of error propagation on motion groups. Int. J. Robot. Res. 27(11–12), 1258–1273 (2008)CrossRefGoogle Scholar
  56. 56.
    Whelan, T., Kaess, M., Leonard, J.J., McDonald, J.: Deformation-based loop closure for large scale dense RGB-D SLAM. In: 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 548–555. IEEE (2013)Google Scholar
  57. 57.
    Wolfe, K., Mashner, M., Chirikjian, G.: Bayesian Fusion on Lie groups. J. Algebr. Stat. 2(1), 75–97 (2011)MathSciNetCrossRefMATHGoogle Scholar
  58. 58.
    Zamani, M., Trumpf, J., Mahony, R.: Minimum-energy filtering for attitude estimation. IEEE Trans. Autom. Control 58(11), 2917–2921 (2013)Google Scholar
  59. 59.
    Zamani, M., Trumpf, J., Mahoney, M.: A second order minimum-energy filter on the special orthogonal group. In: Proc. ACC (2012)Google Scholar
  60. 60.
    Žefran, M., Kumar, V., Croke, C.: Metrics and connections for rigid-body kinematics. Int. J. Robot. Res. 18(2), 242–251 (1999)Google Scholar
  61. 61.
    Zhang, C., Taghvaei, A., Mehta, P.G.: Feedback particle filter on matrix Lie groups (2015). arXiv:1510.01259

Copyright information

© Springer Science+Business Media New York 2016

Authors and Affiliations

  1. 1.Research Training Group 1653, Image and Pattern Analysis GroupHeidelberg University (HCI)HeidelbergGermany
  2. 2.Heidelberg Collaboratory for Image ProcessingHeidelberg University (HCI)HeidelbergGermany
  3. 3.Image and Pattern Analysis GroupHeidelberg University (HCI)HeidelbergGermany

Personalised recommendations