001/*******************************************************************************
002 * This software is provided as a supplement to the authors' textbooks on digital
003 *  image processing published by Springer-Verlag in various languages and editions.
004 * Permission to use and distribute this software is granted under the BSD 2-Clause 
005 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause). 
006 * Copyright (c) 2006-2016 Wilhelm Burger, Mark J. Burge. All rights reserved. 
007 * Visit http://imagingbook.com for additional details.
008 *******************************************************************************/
009package imagingbook.pub.lucaskanade;
010
011import ij.IJ;
012import ij.process.FloatProcessor;
013import imagingbook.lib.ij.IjUtils;
014import imagingbook.lib.math.Matrix;
015import imagingbook.pub.geometry.mappings.linear.ProjectiveMapping;
016
017
018/**
019 * Inverse Compositional Matcher, as described in Simon Baker and Iain Matthews, 
020 * "Lucas-Kanade 20 Years On: A Unifying Framework: Part 1", CMU-RI-TR-02-16 (2002)
021 *  @author Wilhelm Burger
022 *  @version 2014/02/08
023 */
024public class LucasKanadeInverseMatcher extends LucasKanadeMatcher {
025        
026        private int n;                                  // number of warp parameters
027        private float[][] Rx, Ry;               // gradient of reference image
028        private double[][] Hi;                  // inverse of cumulated Hessian matrix
029        private double[][][] S;                 // S[u][u][n] = the steepest descent image for dimension n at pos. u,v (same size as R)
030
031        private double qmag = Double.MAX_VALUE;         // magnitude of parameter difference vector
032        private double sqrError = Double.MAX_VALUE;     // squared sum of differences between I and R
033        
034        /**
035         * Creates a new matcher of type {@link LucasKanadeInverseMatcher}.
036         * @param I the search image (of type {@link FloatProcessor}).
037         * @param R the reference image (of type {@link FloatProcessor})
038         * @param params a parameter object of type  {@link LucasKanadeMatcher.Parameters}.
039         */
040        public LucasKanadeInverseMatcher(FloatProcessor I, FloatProcessor R, Parameters params) {
041                super(I, R, params);
042        }
043        
044        public LucasKanadeInverseMatcher(FloatProcessor I, FloatProcessor R) {
045                super(I, R, new Parameters());
046        }
047        
048        private void initializeMatch(ProjectiveMapping Tinit) {
049                n = Tinit.getWarpParameterCount();
050                S = new double[wR][hR][];                       // S[u][v] holds a double vector of length n
051                Rx = gradientX(R).getFloatArray();      // gradient of R
052                Ry = gradientY(R).getFloatArray();
053                double[][] H = new double[n][n];        // cumulated Hessian matrix of size n x n (initially zero)
054                
055                ProjectiveMapping Tp = Tinit.duplicate();
056                
057                for (int u = 0; u < wR; u++) {                  // for all coordinates (u,v) in R do
058                        for (int v = 0; v < hR; v++) {
059                                double[] x = {u - xc, v - yc};  // position w.r.t. the center of R
060                                double[] gradR = {Rx[u][v], Ry[u][v]};
061
062                                double[][] J = Tp.getWarpJacobian(x);
063                                
064                                double[] sx = Matrix.multiply(gradR, J);                // column vector of length n (6)
065                                
066                                S[u][v] = sx;                                                                   // keep for use in main loop
067                                double[][] Hxy = Matrix.outerProduct(sx, sx);
068                                
069                                H = Matrix.add(H, Hxy);                                                 // cumulate the Hessian
070                        }
071                }
072                
073                Hi = Matrix.inverse(H);                                                                 // inverse of Hessian
074                if (Hi == null) {
075                        IJ.log("singular Hessian!");
076                        throw new RuntimeException("could not invert Hessian");
077                }
078                
079                iteration = 0;
080                
081                if (params.showSteepestDescentImages) 
082                        showSteepestDescentImages(S);
083                if (params.showHessians) {
084                        IjUtils.createImage("H", H).show();
085                        IjUtils.createImage("Hi", Matrix.inverse(H)).show();
086                }
087        }
088        
089        @Override
090        public ProjectiveMapping iterateOnce(ProjectiveMapping Tp) {
091                if (iteration < 0) {
092                        initializeMatch(Tp);
093                }
094                iteration = iteration + 1;
095                double[] dp = new double[n];    // n-dim vector \delta_p = 0
096                sqrError = 0;
097                
098                // for all positions (u,v) in R do
099                for (int u = 0; u < wR; u++) {
100                        for (int v = 0; v < hR; v++) {
101                                
102                                // get coordinate relative to center of R
103                                double[] x = {u - xc, v - yc};
104                                
105                                // warp I to I' (onto R)
106                                double[] xT = Tp.applyTo(x);            // warp from x -> x'
107
108                                // calculate pixel difference d for pos. (u,v)
109                                double d = I.getInterpolatedValue(xT[0], xT[1]) - R.getf(u, v);
110                                sqrError = sqrError + d * d;
111                                
112                                // multiply the pixel difference d with the corresponding steepest descent image sx
113                                // and sum into dp:
114                                double[] sx = S[u][v];
115                                dp = Matrix.add(dp, Matrix.multiply(d, sx));
116                        }
117                }
118                
119                // estimate the parameter difference vector qopt:
120                double[] qopt = Matrix.multiply(Hi, dp);
121                if (params.debug) IJ.log("qopt  = " + Matrix.toString(qopt));
122                
123                // measure the magnitude of the difference vector:
124                qmag = Matrix.normL2squared(qopt);
125                
126                // Calculate the warp parameters p', such that T_p'(x) = T_p (T^-1_q (x), for any point x.
127                ProjectiveMapping Tqopt = new ProjectiveMapping();
128                Tqopt.setWarpParameters(qopt);
129                ProjectiveMapping Tqopti = Tqopt.invert();
130                Tp = Tqopti.concat(Tp);
131                return Tp;
132        }
133        
134        @Override
135        public boolean hasConverged() {
136                return (qmag < params.tolerance);
137        }
138        
139        @Override
140        public double getRmsError() {
141                return Math.sqrt(sqrError);
142        }
143        
144}