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.process.FloatProcessor;
012import imagingbook.lib.ij.IjUtils;
013import imagingbook.lib.math.Matrix;
014import imagingbook.pub.geometry.mappings.linear.ProjectiveMapping;
015
016/**
017 * Lucas-Kanade (forward-additive) matcher, as described in Simon Baker and Iain Matthews, 
018 * "Lucas-Kanade 20 Years On: A Unifying Framework: Part 1", CMU-RI-TR-02-16 (2002)
019 * Also called the "forward-additive" algorithm.
020 * This version assumes that the the origin of R is at its center!
021 *  @author Wilhelm Burger
022 *  @version 2014/02/08
023 */
024public class LucasKanadeForwardMatcher extends LucasKanadeMatcher {
025        
026        
027        private int n;                                                          // number of warp parameters
028        private FloatProcessor Ix, Iy;                          // gradient of the search image
029        private double qmag = Double.MAX_VALUE;         // magnitude of parameter difference vector
030        private double sqrError = Double.MAX_VALUE;     // squared sum of differences between I and R
031        private double[][][] S;                                         // S[u][u][n] = the steepest descent image for dimension n at pos. u,v (same size as R)
032
033        /**
034         * Creates a new matcher of type {@link LucasKanadeForwardMatcher}.
035         * @param I the search image (of type {@link FloatProcessor}).
036         * @param R the reference image (of type {@link FloatProcessor})
037         * @param params a parameter object of type  {@link LucasKanadeMatcher.Parameters}.
038         */
039        public LucasKanadeForwardMatcher(FloatProcessor I, FloatProcessor R, Parameters params) {
040                super(I, R, params);
041        }
042        
043        public LucasKanadeForwardMatcher(FloatProcessor I, FloatProcessor R) {
044                super(I, R, new Parameters());
045        }
046
047        @Override
048        public boolean hasConverged() {
049                return (qmag < params.tolerance);
050        }
051
052        @Override
053        public double getRmsError() {
054                return Math.sqrt(sqrError);
055        }
056
057        private void initializeMatch(ProjectiveMapping Tinit) {
058                n = Tinit.getWarpParameterCount();
059                Ix = gradientX(I);
060                Iy = gradientY(I);
061                iteration = 0;
062        }
063
064        @Override
065        public ProjectiveMapping iterateOnce(ProjectiveMapping Tp) {
066                if (iteration < 0) {
067                        initializeMatch(Tp);
068                }
069                iteration = iteration + 1;
070                double[][] H = new double[n][n];        // n x n cumulated Hessian matrix
071                double[] dp = new double[n];            // n-dim vector \delta_p = 0
072                sqrError = 0;
073
074                if (params.showSteepestDescentImages) {
075                        S = new double[wR][hR][];               // S[u][v] holds a double vector of length n
076                }
077                
078                // for all positions (u,v) in R do
079                for (int u = 0; u < wR; u++) {
080                        for (int v = 0; v < hR; v++) {
081                                double[] x = {u - xc, v - yc};  // position w.r.t. the center of R
082                                double[] xT = Tp.applyTo(x);            // warp x -> x'
083
084                                double gx = Ix.getInterpolatedValue(xT[0], xT[1]);      // interpolate the gradient at pos. xw
085                                double gy = Iy.getInterpolatedValue(xT[0], xT[1]);
086                                double[] G = new double[] {gx, gy};                             // interpolated gradient vector
087
088                                // Step 4: Evaluate the Jacobian of the warp W at position x
089                                double[][] J = Tp.getWarpJacobian(x);
090
091                                // Step 5: compute the steepest descent image:
092                                double[] sx = Matrix.multiply(G, J); // I_steepest = gradI(xy) * dW/dp(xy)  is a n-dim vector
093
094                                if (params.showSteepestDescentImages && iteration == 1) {
095                                        S[u][v] = sx;
096                                }
097
098                                // Step 6: Update the Hessian matrix
099                                double[][] Hx = Matrix.outerProduct(sx, sx);
100                                H = Matrix.add(H, Hx);
101
102                                // Step 7: Compute sum_x [gradI*dW/dp]^T (R(x)-I(W(x;p))] = Isteepest^T * Exy]
103                                double d = R.getf(u, v) - I.getInterpolatedValue(xT[0], xT[1]);
104                                sqrError = sqrError + d * d;
105
106                                dp = Matrix.add(dp, Matrix.multiply(d, sx));
107                        }
108                }
109
110                if (params.showHessians && iteration == 1) {
111                        IjUtils.createImage("H", H).show();
112                        IjUtils.createImage("Hi", Matrix.inverse(H)).show();
113                }
114
115                // Step 8: Compute delta_p using Equation 10
116                //                      double[][] Hi = Matrix.invert(H);
117                //                      if (Hi == null) {
118                //                              IJ.log("could not invert Hessian matrix!");
119                //                              return null;
120                //                      }
121                // Step 9: update the parameter vector
122                //                      double[] dp = Matrix.multiply(Hi, S);
123
124                // Step 8/9 (alternative)
125                double[] qopt = Matrix.solve(H, dp);
126                if (qopt == null) {
127                        // IJ.log("singular Hessian!");
128                        return null;
129                }
130                
131                double[] p = Matrix.add(Tp.getWarpParameters(), qopt);
132                Tp.setWarpParameters(p);
133
134                qmag = Matrix.normL2squared(qopt);
135
136                if (params.showSteepestDescentImages && iteration == 1) {
137                        showSteepestDescentImages(S);
138                }
139
140                return Tp;
141        }
142}