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-2023 Wilhelm Burger, Mark J. Burge. All rights reserved.
007 * Visit https://imagingbook.com for additional details.
008 ******************************************************************************/
009package imagingbook.common.image.matching.lucaskanade;
010
011import ij.IJ;
012import ij.process.FloatProcessor;
013import imagingbook.common.geometry.basic.Pnt2d;
014import imagingbook.common.geometry.basic.Pnt2d.PntDouble;
015import imagingbook.common.geometry.mappings.linear.ProjectiveMapping2D;
016import imagingbook.common.ij.IjUtils;
017import imagingbook.common.math.Matrix;
018
019/**
020 * Lucas-Kanade elastic "Inverse Compositional" matcher, as described in [1]. See Sec. 24.3 (Alg. 4.2) of [2] for
021 * additional details.
022 * <p>
023 * [1] Simon Baker and Iain Matthews, "Lucas-Kanade 20 Years On: A Unifying Framework: Part 1", CMU-RI-TR-02-16 (2002)
024 * <br>
025 * [2] W. Burger, M.J. Burge, <em>Digital Image Processing &ndash; An Algorithmic Introduction</em>, 3rd ed, Springer
026 * (2022).
027 *
028 * @author WB
029 * @version 2014/02/08
030 */
031public class LucasKanadeInverseMatcher extends LucasKanadeMatcher {
032        
033        private int n;                                  // number of warp parameters
034        private float[][] Rx, Ry;               // gradient of reference image
035        private double[][] Hi;                  // inverse of cumulated Hessian matrix
036        private double[][][] S;                 // S[u][u][n] = the steepest descent image for dimension n at pos. u,v (same size as R)
037
038        private double qmag = Double.MAX_VALUE;         // magnitude of parameter difference vector
039        private double sqrError = Double.MAX_VALUE;     // squared sum of differences between I and R
040        
041        /**
042         * Constructor.
043         * @param I the search image (of type {@link FloatProcessor}).
044         * @param R the reference image (of type {@link FloatProcessor})
045         * @param params a parameter object of type  {@link LucasKanadeMatcher.Parameters}.
046         */
047        public LucasKanadeInverseMatcher(FloatProcessor I, FloatProcessor R, Parameters params) {
048                super(I, R, params);
049        }
050
051        /**
052         * Constructor using default parameters.
053         * @param I the search image (of type {@link FloatProcessor}).
054         * @param R the reference image (of type {@link FloatProcessor})
055         */
056        public LucasKanadeInverseMatcher(FloatProcessor I, FloatProcessor R) {
057                this(I, R, new Parameters());
058        }
059        
060        private void initializeMatch(ProjectiveMapping2D Tinit) {
061                n = getParameters(Tinit).length;        // number of transformation parameters
062                S = new double[wR][hR][];                       // S[u][v] holds a double vector of length n
063                Rx = gradientX(R).getFloatArray();      // gradient of R
064                Ry = gradientY(R).getFloatArray();
065                double[][] H = new double[n][n];        // cumulated Hessian matrix of size n x n (initially zero)
066                
067                ProjectiveMapping2D Tp = Tinit;
068                
069                for (int u = 0; u < wR; u++) {                  // for all coordinates (u,v) in R do
070                        for (int v = 0; v < hR; v++) {
071//                              double[] x = {u - xc, v - yc};  // position w.r.t. the center of R
072                                Pnt2d x = PntDouble.from(u - xc, v - yc);       // position w.r.t. the center of R
073                                double[] gradR = {Rx[u][v], Ry[u][v]};
074
075                                double[][] J = Tp.getJacobian(x);
076                                
077                                double[] sx = Matrix.multiply(gradR, J);                // column vector of length n (6)
078                                
079                                S[u][v] = sx;                                                                   // keep for use in main loop
080                                double[][] Hxy = Matrix.outerProduct(sx, sx);
081                                
082                                H = Matrix.add(H, Hxy);                                                 // cumulate the Hessian
083                        }
084                }
085                
086                Hi = Matrix.inverse(H);                                                                 // inverse of Hessian
087                if (Hi == null) {
088                        IJ.log("singular Hessian!");
089                        throw new RuntimeException("could not invert Hessian");
090                }
091                
092                iteration = 0;
093                
094                if (params.showSteepestDescentImages) 
095                        showSteepestDescentImages(S);
096                if (params.showHessians) {
097                        IjUtils.createImage("H", H).show();
098                        IjUtils.createImage("Hi", Matrix.inverse(H)).show();
099                }
100        }
101        
102        @Override
103        public ProjectiveMapping2D iterateOnce(ProjectiveMapping2D Tp) {
104                if (iteration < 0) {
105                        initializeMatch(Tp);
106                }
107                iteration = iteration + 1;
108                double[] dp = new double[n];    // n-dim vector \delta_p = 0
109                sqrError = 0;
110                
111                // for all positions (u,v) in R do
112                for (int u = 0; u < wR; u++) {
113                        for (int v = 0; v < hR; v++) {
114                                
115                                // get coordinate relative to center of R
116                                Pnt2d x = PntDouble.from(u - xc, v - yc);
117                                
118                                // warp I to I' (onto R)
119                                Pnt2d xT = Tp.applyTo(x);               // warp from x -> x'
120
121                                // calculate pixel difference d for pos. (u,v)
122                                double d = I.getInterpolatedValue(xT.getX(), xT.getY()) - R.getf(u, v);
123                                sqrError = sqrError + d * d;
124                                
125                                // multiply the pixel difference d with the corresponding steepest descent image sx
126                                // and sum into dp:
127                                double[] sx = S[u][v];
128                                dp = Matrix.add(dp, Matrix.multiply(d, sx));
129                        }
130                }
131                
132                // estimate the parameter difference vector qopt:
133                double[] qopt = Matrix.multiply(Hi, dp);
134                if (params.debug) IJ.log("qopt  = " + Matrix.toString(qopt));
135                
136                // measure the magnitude of the difference vector:
137                qmag = Matrix.normL2squared(qopt);
138                
139                // Calculate the warp parameters p', such that T_p'(x) = T_p (T^-1_q (x), for any point x.
140                ProjectiveMapping2D Tqopt = toProjectiveMap(qopt);
141                ProjectiveMapping2D Tqopti = Tqopt.getInverse();
142                return Tqopti.concat(Tp);
143        }
144        
145        @Override
146        public boolean hasConverged() {
147                return (qmag < params.tolerance);
148        }
149        
150        @Override
151        public double getRmsError() {
152                return Math.sqrt(sqrError);
153        }
154        
155}