Spatial Regularization
In this section we derive the minimum energy filter for joint camera motion and disparity estimation which was discussed in section 5.3. We recall the
A.5. Supplemental Material of Chapter 5 (left-trivialized) Hamiltonian (5.33) which includes the spatial regularizer. It is given through H(x, µ, t) := −hµ, f(x(t)i − 1 2kvecg(µ)k 2 R−1 +X z∈Ω φdata 12kyz(t)− hz(x(t))k 2 Qz + λφreg 1 2k∇zdi(z)k 2 . (A.86) Here, x = (E, v, di)∈ G consists of the camera motion E ∈ SE3, v ∈ R6 and
the disparity map di ∈ (0, 1)|Ω|.
Computation of the total time derivative of the necessary condition (5.24) again leads to a differential equation for the optimal state (cf. (A.87)).
˙x∗(t) = x(t)−D2H(x∗(t), 0, t)− Z(x∗(t), t)−1◦ x−1(D1H(x∗(t), 0, t))
(A.87) where Z : g → g∗ is the left-trivialized Hessian of the value function given
through
Z(x, t)◦ η = x−1HessV(x(t), t, x(t
0))[xη], η∈ g . (A.88)
The differential of the Hamiltonian regarding the second component is D2H(x∗, 0, t) = −f(x∗). We continue with the differential of the Hamilto-
nian regarding the first component. As the differential of the data term was already derived in (A.71), we only calculate the differential of the regularizer, which we denote for x = (E, v, di) by
Φ(x) := λX
z∈Ω
φreg 1
2k∇zdi(z)k
2 . (A.89)
The image domain Ω is discrete, therefore we require a discretization of the spatial gradient ∇zdi(z). For this reason we introduce difference matrices
which can be expressed with help of Kronecker products. If n1 denotes the
number of rows and n2 denotes the number of columns of the image domain
Ω, we have
D1 :=1n2 ⊗ ˜D
1, D
2 := ˜D2⊗ 1n1, (A.90)
where ˜D1 ∈ {−1, 0, 1}n1×n1 and ˜D2 ∈ {−1, 0, 1}(n2−1)×n2 are given through
˜ D1ij := 1, j = i + 1, i < n1, −1, j = i, i < n1, 0, otherwise , , D˜ij2 := 1, j = i + 1, i < n2, −1, j = i, i < n2, 0, otherwise .
As an example the difference matrices of a 3× 4 image are given through D1 = −1 1 0 0 0 0 0 0 0 0 0 0 0 −1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0−1 1 0 0 0 0 0 0 0 0 0 0 0 −1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0−1 1 0 0 0 0 0 0 0 0 0 0 0 −1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0−1 1 0 0 0 0 0 0 0 0 0 0 0 −1 1 0 0 0 0 0 0 0 0 0 0 0 0 ∈ {−1, 0, 1}3·4×3·4 D2 = −1 0 0 1 0 0 0 0 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 0 0 0 0 −1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ∈ {−1, 0, 1}3·4×3·4
The discrete gradient ∇zdi(z) can now be expressed as follows
∇zdi(z) =(D(D1di)p(z) 2di)p(z)
, (A.91)
where di is the vector representation (stacked column wise) of the inverse
depth map (di(z))z∈Ω and p(z) denotes position (index) of z in the vector
di, i.e. (di)p(z) = di(z). The derivative of (A.89) can be calculated regarding
di(˜z) : ∂ ∂di(˜z) Φ(x) = λ ∂ ∂di(˜z) X z∈Ω φreg 12k∇zdi(z)k 2 =λX z∈Ω ∂ ∂di(˜z) 1 2k (D 1di)p(z) (D2di)p(z) k2 + νr αr − ναr r =λX z∈Ω αr 12k (D 1di)p(z) (D2di)p(z) k2+ νr αr−1 · (D1di)p(z)(D1ep(˜z))p(z)+ (D2di)p(z)(D2ep(˜z))p(z) :=(Gregdi (x))p(˜z) (A.92)
Stacking these partial derivatives into a vector results in the gradient regard- ing the depth, i.e. DdiΦ(x) =: Greg(x). By using the previously calculated
expressions GE from (A.69) and Gdi from (A.70) and combining them with
the expression (A.92) we find the differential of the expanded Hamiltonian in (A.86) which reads for x = (E, v, di)∈ G
D1H(x, 0, t) = TIdLx GE(x), 0, TId(0,1)|Ω|L∗di Gdi(x) + G
reg
A.5. Supplemental Material of Chapter 5 For the second-order optimal filter it remains to compute the Hessian of the Hamiltonian regarding the first component D1H(x, 0, t) which was partially
done within the last section for the data term. Therefore, we continue sepa- rately with the regularizer Φ in (A.89). This expression does not depend on the camera motion E ∈ SE3 and the velocity v ∈ R6. Thus, we only require
to calculate the mixed derivatives of Φ since the geometry of the disparity group (0, 1)|Ω| is trivial (the corresponding Christoffel-symbols are all zero).
∂2 ∂di(ˆz)∂di(˜z) Φ(x) = ∂ ∂di(ˆz) λX z∈Ω αr 12k (D 1di)p(z) (D2di)p(z) k2+ ν r αr−1 · (D1di)p(z)(D1ep(˜z))p(z)+ (D2di)p(z)(D2ep(˜z))p(z) =λX z∈Ω αr(αr− 1) 12k (D 1di)p(z) (D2di)p(z) k2+ ν r αr−2 · (D1di)p(z)(D1ep(˜z))p(z)+ (D2di)p(z)(D2ep(˜z))p(z) · (D1di)p(z)(D1ep(ˆz))p(z)+ (D2di)p(z)(D2ep(ˆz))p(z) + αr 12k (D 1di)p(z) (D2di)p(z) k2+ νr αr−1 · (D1ep(ˆz))p(z)(D1ep(˜z))p(z)+ (D2ep(ˆz))p(z)(D2ep(˜z))p(z) =:(Hreg(x))p(ˆz)p(˜z).
Adding the Hessian of the regularization term to the Hessian of the data term, which was calculated above, finally results in the Hessian of the Hamiltonian regarding the first component. All the other expressions which are required for the minimum energy filter with spatial regularization were already calcu- lated in appendix A.5.3. By adding the corresponding gradients and Hessians of the regularizers we finally obtain the minimum energy filter for the prob- lem of joint monocular disparity map and camera motion estimation which also includes a spatial regularizer. This completes the calculations.
Bibliography
[1] P. A. Absil, R. Mahony, and R. Sepulchre. Optimization Algorithms on Matrix Manifolds. Princeton University Press, 2008.
[2] A. A. Agrachev and Y. Sachkov. Control Theory from the Geometric Viewpoint, volume 2. Springer, 2004.
[3] A. P. Aguiar and J.P. Hespanha. Minimum-Energy State Estimation for Systems with Perspective Outputs. Automatic Control, IEEE Transac- tions on, 51(2):226–241, Feb 2006.
[4] M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp. A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking. Signal Processing, IEEE Transactions on, 50(2):174–188, 2002.
[5] M. Athans and P. Falb. Optimal Control. An Introduction to the Theory and Its Applications. McGraw-Hill, 1966.
[6] H. Badino, A. Yamamoto, and T. Kanade. Visual Odometry by Multi- Frame Feature Integration. In Computer Vision Workshops (ICCVW), 2013 IEEE International Conference on, pages 222–229. IEEE, 2013. [7] A. Bain and D. Crisan. Fundamentals of Stochastic Filtering. Springer,
2009.
[8] F. Becker, F. Lenzen, J. H. Kappes, and C. Schn¨orr. Variational Recur- sive Joint Estimation of Dense Scene Structure and Camera Motion from Monocular High Speed Traffic Sequences. IJCV, 105:269–297, 2013. [9] F. Bellavia, M. Fanfani, F. Pazzaglia, and C. Colombo. Robust Selective
Stereo SLAM without Loop Closure and Bundle Adjustment. In Image Analysis and Processing–ICIAP 2013, pages 462–471. Springer, 2013.
[10] P. Benner, J. R. Li, and T. Penzl. Numerical Solution of Large-Scale Lyapunov Equations, Riccati Equations, and Linear-Quadratic Opti- mal Control Problems. Numerical Linear Algebra with Applications, 15(9):755–777, 2008.
[11] P. Benner and J. Saak. Numerical Solution of Large and Sparse Contin- uous Time Algebraic Matrix Riccati and Lyapunov Equations: A State of The Art Survey. GAMM-Mitteilungen, 36(1):32–52, 2013.
[12] J. Berger, F. Lenzen, F. Becker, A. Neufeld, and C. Schn¨orr. Second- Order Recursive Filtering on the Rigid-Motion Lie Group SE(3) Based on Nonlinear Observations, 2015. ArXiv, preprint.
[13] J. Berger, A. Neufeld, F. Becker, F. Lenzen, and C. Schn¨orr. Second Order Minimum Energy Filtering on SE(3) with Nonlinear Measurement Equations. In Scale Space and Variational Methods in Computer Vision, pages 397–409. Springer, 2015.
[14] J. Berger and C. Schn¨orr. Joint Recursive Monocular Filtering of Cam- era Motion and Disparity Map. In 38th German Conference on Pattern Recognition, 2016. in press.
[15] S. Bonnabel. Left-Invariant Extended Kalman Filter and Attitude Esti- mation. In IEEE conference on decision and control, pages 1027–1032, 2007.
[16] G. Bourmaud and R. M´egret. Robust Large Scale Monocular Visual SLAM. In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, pages 1638–1647, 2015.
[17] G. Bourmaud, R. M´egret, M. Arnaudon, and A. Giremus. Continuous- Discrete Extended Kalman Filter on Matrix Lie Groups Using Concen- trated Gaussian Distributions. Journal of Mathematical Imaging and Vision, 51(1):209–228, 2015.
[18] D. Brigo, B. Hanzon, and F. Le Gland. Approximate Nonlinear Fil- tering by Projection on Exponential Manifolds of Densities. Bernoulli, 5(3):495–534, 1999.
[19] D. Bump. Lie Groups. Springer, 2004.
[20] Y. Chikuse. Statistics on Special Manifolds, volume 174. Springer Sci- ence & Business Media, 2012.
Bibliography [21] G. S. Chirikjian. Stochastic Models, Information Theory, and Lie Groups, Volume 2: Analytic Methods and Modern Applications, vol- ume 2. Springer Science & Business Media, 2011.
[22] J. L. Crassidis and F. L. Markley. Unscented Filtering for Space- craft Attitude Estimation. Journal of guidance, control, and dynamics, 26(4):536–542, 2003.
[23] R. Dahlhaus and J. C. Neddermeyer. Online Spot Volatility-Estimation and Decomposition with Nonlinear Market Microstructure Noise Mod- els. Journal of Financial Econometrics, 12(1):174–212, 2014.
[24] F. Daum. Nonlinear Filters: Beyond the Kalman Filter. IEEE A&E Systems Magazin, 20(8, Part 2):57–69, 2005.
[25] F. Daum and J. Huang. Curse of Dimensionality and Particle Filters. In Aerospace Conference, 2003.
[26] A. J. Davison, I. D. Reid, N. D. Molton, and O. Stasse. MonoSLAM: Real-Time Single Camera SLAM. PAMI, 29(6):1052–1067, 2007. [27] L. Dieci and T. Eirola. Positive Definiteness in the Numerical Solution of
Riccati Differential Equations. Numerische Mathematik, 67(3):303–313, 1994.
[28] P. Doll´ar. Piotr’s Computer Vision Matlab Toolbox (PMT). http://vision.ucsd.edu/ pdollar/toolbox/doc/index.html.
[29] A. Doucet, N. Freitas, and N. Gordon. Sequential Monte Carlo Methods in Practice, chapter An Introduction to Sequential Monte Carlo Meth- ods, pages 3–14. Springer New York, New York, NY, 2001.
[30] J. Engel, T. Sch¨ops, and D. Cremers. LSD-SLAM: Large-Scale Direct Monocular SLAM. In Computer Vision–ECCV 2014, pages 834–849. Springer, 2014.
[31] J. Engel, J. Sturm, and D. Cremers. Semi-Dense Visual Odometry for a Monocular Camera. In ICCV, pages 1449–1456. IEEE, 2013.
[32] L. C. Evans. Partial Differential Equations. Graduate studies in math- ematics. American Mathematical Society, 2010.
[33] R. J. Fitzgerald. Divergence of the Kalman Filter. Automatic Control, IEEE Transactions on, 16(6):736–747, 1971.
[34] W. H. Fleming and R. W. Rishel. Deterministic and Stochastic Optimal Control, volume 1. Springer Science & Business Media, 1982.
[35] P. Frogerais, J. Bellanger, and L. Senhadji. Various Ways to Compute the Continuous-Discrete Extended Kalman Filter. Automatic Control, IEEE Transactions on, 57:1000–1004, 2012.
[36] A. Geiger, P. Lenz, and R. Urtasun. Are we ready for Autonomous Driv- ing? the KITTI Vision Benchmark Suite. In Conference on Computer Vision and Pattern Recognition (CVPR), 2012.
[37] A. Geiger, J. Ziegler, and C. Stiller. Stereoscan: Dense 3D Reconstruc- tion in Real-Time. In Intelligent Vehicles Symposium (IV), IEEE, 2011. [38] N.J. Gordon, D. J Salmond, and A. F. M. Smith. Novel Approach to Nonlinear/Non-Gaussian Bayesian State Estimation. In Radar and Signal Processing, IEE Proceedings F, volume 140, 1993.
[39] E. Hairer, C. Lubich, and G. Wanner. Geometric Numerical Integra- tion: Structure-Preserving Algorithms for Ordinary Differential Equa- tions. Springer, 2006.
[40] B.C. Hall. Lie Groups, Lie Algebras, and Representations: An Elemen- tary Introduction, volume 222. Springer, 2015.
[41] R. Hartley. In Defense of The Eight-Point Algorithm. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 19(6):580–593, 1997. [42] S. Hauberg, F. Lauze, and K. Pedersen. Unscented Kalman Filtering on
Riemannian Manifolds. Journal of Mathematical Imaging and Vision, 46(1):103–120, 2013.
[43] J. Hilgert and K.-H. Neeb. Structure and Geometry of Lie Groups. Springer Science & Business Media, 2011.
[44] H. Hirschm¨uller. Stereo Processing by Semiglobal Matching and Mutual Information. PAMI, 30(2):328–341, 2008.
[45] E. P. Hsu. Stochastic Analysis on Manifolds, volume 38. American Mathematical Soc., 2002.
[46] A. H. Jazwinski. Stochastic Processes and Filtering Theory. Courier Corporation, 2007.
Bibliography [47] S. J. Julier and J. K. Uhlmann. New Extension of the Kalman Filter to Nonlinear Systems. In AeroSense’97, pages 182–193. International Society for Optics and Photonics, 1997.
[48] V. Jurdjevic. Geometric Control Theory. Cambridge university press, 1997.
[49] R. E. Kalman. A New Approach to Linear Filtering and Prediction Problems. Journal of Fluids Engineering, 82(1):35–45, 1960.
[50] A. J. Krener. The Convergence of the Minimum Energy Estimator. In New Trends in Nonlinear Dynamics and Control and their Applications. Springer, 2003.
[51] H. J. Kushner. On the Differential Equations Satisfied by Conditional Probablitity Densities of Markov Processes, with Applications. SIAM Journal on Control and Optimization, 2(1):106, 1964.
[52] H. J. Kushner. Approximations to Optimal Nonlinear Filters. Automatic Control, IEEE Transactions on, 12(5):546–556, 1967.
[53] J. Kwon, M. Choi, F. C. Park, and C. Chun. Particle Filtering on the Euclidean Group: Framework and Applications. Robotica, 25(6):725– 737, 2007.
[54] J. Kwon and K. M. Lee. Monocular SLAM with Locally Planar Landmarks via Geometric Rao-Blackwellized Particle Filtering on Lie Groups. In CVPR 2010, pages 1522–1529. IEEE, 2010.
[55] P. Lancaster and L. Rodman. Existence and Uniqueness Theorems for the Algebraic Riccati Equation. International Journal of Control, 32(2):285–309, 1980.
[56] F. L. Markley. Attitude Error Representations for Kalman Filtering. Journal of guidance, control, and dynamics, 26(2):311–317, 2003. [57] M. I. Miller, A. Srivastava, and U. Grenander. Conditional-
Mean Estimation via Jump-Diffusion Processes in Multiple Target Tracking/Recognition. Signal Processing, IEEE Transactions on, 43(11):2678–2690, 1995.
[58] R. E. Mortensen. Maximum-Likelihood Recursive Nonlinear Filtering. J. Opt. Theory Appl., 2(6):386–394, 1968.
[59] R. M. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC press, 1994.
[60] B. Nasihatkon, R. Hartley, and J. Trumpf. A Generalized Projective Reconstruction Theorem and Depth Constraints for Projective Factor- ization. International Journal of Computer Vision, pages 1–28, 2015. [61] A. Neufeld, J. Berger, F. Becker, F. Lenzen, and C. Schn¨orr. Estimating
Vehicle Ego-Motion and Piecewise Planar Scene Structure from Optical Flow in a Continuous Framework. In GCPR, 2015.
[62] D. Nist´er. An Efficient Solution to the Five-Point Relative Pose Prob- lem. Pattern Analysis and Machine Intelligence, IEEE Transactions on, 26(6):756–770, 2004.
[63] D. Ocone and E. Pardoux. A Lie Algebraic Criterion for Non-Existence of Finite Dimensionally Computable Filters. In Stochastic Partial Dif- ferential Equations and Applications II, pages 197–204. Springer, 1989. [64] K. B. Petersen and M. S. Pedersen. The Matrix Cookbook. Technical
University of Denmark, 7:15, 2008.
[65] M. Pizzoli, C. Forster, and D. Scaramuzza. REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time. In Proc. IEEE Int. Conf. on Robotics and Automation, 2014.
[66] L. S. Pontryagin, V. Boltyanskii, R. Gamkrelidze, and E. Mishchenko. The Mathematical Theory of Optimal Processes. Interscience Publish- ers, Inc., 1962.
[67] E. T. Psota, J. Kowalczuk, M. Mittek, and L. C. Perez. MAP Disparity Estimation Using Hidden Markov Trees. In ICCV, pages 2219–2227, 2015.
[68] Q. Rentmeesters, P. A. Absil, P. Van Dooren, K. Gallivan, and A. Sri- vastava. An Efficient Particle Filtering Technique on the Grassmann Manifold. In Acoustics Speech and Signal Processing (ICASSP), 2010 IEEE International Conference on, pages 3838–3841. IEEE, 2010. [69] J. Revaud, P. Weinzaepfel, Z. Harchaoui, and C. Schmid. EpicFlow:
Edge-Preserving Interpolation of Correspondences for Optical Flow. In CVPR, 2015.
[70] A. Saccon, A. P. Aguiar, and J. Hauser. Lie Group Projection Operator Approach: Optimal Control on t so (3). In CDC, pages 6973–6978, 2011.
Bibliography [71] A. Saccon, J. Hauser, and A. P. Aguiar. Optimal Control on Lie Groups: The Projection Operator Approach. Automatic Control, IEEE Trans- actions on, 58(9):2230–2245, Sept 2013.
[72] A. Saccon, J. Trumpf, R. Mahony, and A. P. Aguiar. Second-Order- Optimal Filters on Lie groups. In CDC, 2013.
[73] A. Saccon, J. Trumpf, R. Mahony, and A. P. Aguiar. Second-Order- Optimal Minimum-Energy Filters on Lie Groups. IEEE TAC, PP(99):1– 1, 2015.
[74] S. S¨arkk¨a, J. Hartikainen, and A. Solin. EKF/UKF Toolbox for Matlab V1.3, 2011.
[75] T. Sch¨on, F. Gustafsson, and P. J. Nordlund. Marginalized Particle Filters for Mixed Linear/Nonlinear State-Space Models. IEEE Transac- tions on Signal Processing, 53(7):2279–2289, July 2005.
[76] J. M. Selig. Lie Groups and Lie Algebras in Robotics. In Computational Noncommutative Algebra and Applications, pages 101–125. Springer, 2004.
[77] H. W. Sorenson. On The Development of Practical Nonlinear Filters. Information Sciences, 7:253 – 270, 1974.
[78] P. Sturm and B. Triggs. A Factorization Based Algorithm for Multi- Image Projective Structure and Motion. In ECCV, pages 709–720, Cam- bridge, England, 1996. Springer.
[79] F. Tompkins and P. J. Wolfe. Bayesian Filtering on the Stiefel Manifold. In Computational Advances in Multi-Sensor Adaptive Processing, 2007. CAMPSAP 2007. 2nd IEEE International Workshop on, pages 261–264. IEEE, 2007.
[80] B. Triggs, P. F. McLauchlan, R. I Hartley, and A. W. Fitzgibbon. Bundle AdjustmentA Modern Synthesis. In Vision Algorithms: Theory and Practice, pages 298–372. Springer, 2000.
[81] A. Tyagi and J. W. Davis. A Recursive Filter for Linear Systems on Riemannian Manifolds. In CVPR, pages 1–8. IEEE, 2008.
[82] C. Vogel, K. Schindler, and S. Roth. 3D Scene Flow Estimation with a Piecewise Rigid Scene Model. IJCV, 115(1):1–28, 2015.
[83] P. Weinzaepfel, J. Revaud, Z. Harchaoui, and C. Schmid. Deepflow: Large Displacement Optical Flow with Deep Matching. In ICCV, pages 1385–1392, 2013.
[84] G. Welch and G. Bishop. An Introduction to the Kalman Filter, 1995. [85] T. Whelan, M. Kaess, J. J. Leonard, and J. McDonald. Deformation-
Based Loop Closure for Large Scale Dense RGB-D SLAM. In Intelligent Robots and Systems (IROS), 2013 IEEE/RSJ International Conference on, pages 548–555. IEEE, 2013.
[86] K. C. Wolfe, M. Mashner, and G. Chirikjian. Bayesian Fusion on Lie Groups. J. Algebraic Statistics, 2(1):75–97, 2011.
[87] J. Zamani, M.and Trumpf and R. Mahony. Minimum-Energy Filtering for Attitude Estimation. Automatic Control, IEEE Transactions on, 58(11):2917–2921, 2013.
[88] M. Zamani, J. Trumpf, and M. Mahoney. A Second Order Minimum- Energy Filter on the Special Orthogonal Group. In Proc. ACC, 2012. [89] M. ˇZefran, V. Kumar, and C. Croke. Metrics and Connections for Rigid-
Body Kinematics. The International Journal of Robotics Research, 18(2):242–1, 1999.