001/*******************************************************************************
002 * Permission to use and distribute this software is granted under the BSD 2-Clause
003 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause).
004 * Copyright (c) 2016-2023 Wilhelm Burger. All rights reserved.
005 * Visit https://imagingbook.com for additional details.
006 ******************************************************************************/
007package imagingbook.calibration.zhang;
008
009import imagingbook.common.geometry.basic.Pnt2d;
010import org.apache.commons.math3.analysis.MultivariateMatrixFunction;
011import org.apache.commons.math3.analysis.MultivariateVectorFunction;
012
013import java.util.Arrays;
014
015/**
016 * Nonlinear optimizer based on the Levenberg-Marquart method, where the Jacobian matrix is calculated numerically
017 * (i.e., by estimating the first partial derivatives from finite differences). The advantage is that the calculation of
018 * the Jacobian is independent of the calibration model, while performance and runtime are similar to the analytic
019 * version (see {@link NonlinearOptimizerAnalytic}).
020 *
021 * @author WB
022 */
023public class NonlinearOptimizerNumeric extends NonlinearOptimizer {
024        
025        NonlinearOptimizerNumeric(Pnt2d[] modelPts, Pnt2d[][] obsPts) {
026                super(modelPts, obsPts);
027        }
028        
029        @Override
030        MultivariateVectorFunction makeValueFun() {
031                return new ValueFun();
032        }
033
034        @Override
035        MultivariateMatrixFunction makeJacobianFun() {
036                return new JacobianFun();
037        }
038
039        private class JacobianFun implements MultivariateMatrixFunction {
040
041                /**
042                 * Calculates a "stacked" Jacobian matrix with 2MN rows and K = 7 + 6M columns (for M views with N points each,
043                 * K parameters). For example, with M = 5 views and N = 256 points each, J is of size 2560 × 37. Each pair of
044                 * rows in the Jacobian corresponds to one point. THIS VERSION only calculates single blocks of the Jacobian!
045                 */
046                @Override
047            public double[][] value(double[] params) {
048                        final int K = params.length;
049                double[][] J = new double[2 * M * N][K];        // the Jacobian matrix (initialized to zeroes!)
050                double[] refValues = new double[2 * M * N];     // values obtained with undisturbed parameters 
051                
052                double[] a = Arrays.copyOfRange(params, 0, camParLength);       // camera parameters
053                Camera camOrig = new Camera(a);
054                
055                // Step 0: calculate all 2MN reference output values (for undisturbed parameters)
056               
057                for (int r = 0, i = 0; i < M; i++) {    // for all views, r = row
058                        int m = camParLength + viewParLength * i;
059                                double[] w = Arrays.copyOfRange(params, m, m + viewParLength);
060                                ViewTransform view = new ViewTransform(w);
061                        for (int j = 0; j < N; j++) {   // for all model points: calculate reference values
062                                double[] uv = camOrig.project(view, modelPts[j]);
063                                refValues[r + 0] = uv[0];
064                                refValues[r + 1] = uv[1];
065                                r = r + 2;
066                        }                
067                }
068                
069                // Step 1: calculate the leftmost (green) block of J associated with camera intrinsics
070                
071                for (int k = 0; k < a.length; k++) {    // for all camera parameters
072                        double ak = a[k];                                       // keep original parameter value        
073                        double delta = estimateDelta(ak);
074                        a[k] = a[k] + delta;            // modify parameter s_k
075                        Camera camMod = new Camera(a);  // modified camera
076                        
077                        for (int r = 0, i = 0; i < M; i++) {    // for all views, r = row
078                                int m = camParLength + i * viewParLength;
079                                double[] w = Arrays.copyOfRange(params, m, m + viewParLength);
080                                ViewTransform view = new ViewTransform(w);
081                                for (int j = 0; j < N; j++) {   // for all model points: calculate disturbed value
082                                        Pnt2d Pj = modelPts[j];
083                                        double[] uvMod = camMod.project(view, Pj);
084                                        J[r + 0][k] = (uvMod[0] - refValues[r + 0]) / delta;   // dX
085                                        J[r + 1][k] = (uvMod[1] - refValues[r + 1]) / delta;   // dY
086                                        r = r + 2;
087                                }    
088                        }
089                        a[k] = ak;      // return parameter s_k to original
090                }
091                
092                // Step 2: calculate the diagonal blocks, one for each view
093                
094                for (int i = 0; i < M; i++) {   // for all views/blocks
095                        final int start = camParLength + i * viewParLength;
096                        double[] w = Arrays.copyOfRange(params, start, start + viewParLength);
097                        final int c = a.length + i * w.length;          // leftmost matrix column of block i
098                        for (int k = 0; k < w.length; k++) {    // for all parameters in w
099                                double wk = w[k];                                       // keep original parameter w_k
100                                double delta = estimateDelta(wk);
101                                w[k] =  w[k] + delta;                           // modify parameter w_k
102                                ViewTransform view = new ViewTransform(w);
103                                int r = 2 * i * N;      // row
104                                for (int j = 0; j < N; j++) {           // for all model points: calculate disturbed value
105                                        Pnt2d Pj = modelPts[j];
106                                        double[] uvMod = camOrig.project(view, Pj);
107                                        J[r + 0][c + k] = (uvMod[0] - refValues[r + 0]) / delta;   // dX
108                                        J[r + 1][c + k] = (uvMod[1] - refValues[r + 1]) / delta;   // dY
109                                        r = r + 2;
110                                } 
111                                w[k] = wk; // w[k] - DELTA;             // return parameter w_k to original
112                        }
113                }
114                
115//                      long endtime = System.nanoTime();
116//                      System.out.println("time diff = " + (endtime - starttime) + " ns");
117//              System.out.println(NonlinearOptimizerNumeric.class.getSimpleName() + 
118//                              ": Jacobian inverse condition number = " + MathUtil.inverseConditionNumber(J));
119                return J;
120            }
121                
122                 // THIS VERSION calculates all entries of the Jacobian (NOT USED)!
123            
124            @Deprecated
125            @SuppressWarnings("unused")
126                public double[][] value(double[] params, boolean dummy) {
127                //long starttime = System.nanoTime();
128                //System.out.println("getJacobianMatrix - NUMERICAL");
129                // M = number of views, N = number of model points
130                double[][] J = new double[2 * M * N][params.length];    // the Jacobian matrix
131                double[] refValues = new double[2 * M * N];     // function values obtained with undisturbed parameters 
132                
133                double[] s = Arrays.copyOfRange(params, 0, camParLength);
134                Camera cam = new Camera(s);
135                
136                // Step 0: calculate all 2MN reference output values (for undisturbed parameters)
137                
138                for (int row = 0, m = 0; m < M; m++) {  // for all views
139                        int start = camParLength + m * viewParLength;
140                                double[] w = Arrays.copyOfRange(params, start, start + viewParLength);
141                                ViewTransform view = new ViewTransform(w);
142                        for (int j = 0; j < N; j++) {   // for all model points: calculate reference values
143                                double[] uv = cam.project(view, modelPts[j]);
144                                refValues[row + 0] = uv[0];
145                                refValues[row + 1] = uv[1];
146                                row = row + 2;
147                        }                
148                }
149                
150                // Step 1: calculate all entries of the Jacobian (brute force!)
151                
152                for (int k = 0; k < params.length; k++) {       // for ALL parameters
153                        int col = k;
154                        // calculate step width and modify the parameter vector c:
155                        double pk = params[k];
156                        double delta = estimateDelta(pk);
157                        params[k] = params[k] + delta;          // modify parameter c_k
158                        
159                        double[] smod = Arrays.copyOfRange(params, 0, camParLength);
160                        Camera camMod = new Camera(smod);       // modified camera
161                        
162                        for (int row = 0, m = 0; m < M; m++) {  // for all views
163                                int start = camParLength + m * viewParLength;
164                                double[] w = Arrays.copyOfRange(params, start, start + viewParLength);
165                                ViewTransform view = new ViewTransform(w);
166                                for (int n = 0; n < N; n++) {   // for all model points: calculate disturbed value
167                                        Pnt2d Pj = modelPts[n];
168                                        double[] uvMod = camMod.project(view, Pj);
169                                        J[row + 0][col] = (uvMod[0] - refValues[row + 0]) / delta;   // du
170                                        J[row + 1][col] = (uvMod[1] - refValues[row + 1]) / delta;   // dv      
171                                        row = row + 2;
172                                }    
173                        }
174                        params[k] = pk;
175                }
176               
177                long endtime = System.nanoTime();
178//                      System.out.println("time diff = " + (endtime - starttime) + " ns");
179//                      System.out.println(this.getClass().getSimpleName() + 
180//                              ": Jacobian inverse condition number = " + MathUtil.inverseConditionNumber(J));
181                return J;
182            }
183
184        }
185
186        /**
187         * Returns a positive delta value adapted to the magnitude of the parameter x
188         *
189         * @param x
190         * @return
191         */
192    private double estimateDelta(double x) {
193        final double eps = 1.5e-8;      // = sqrt(2.2 * 10^{-16})
194        double dx = eps * Math.max(Math.abs(x), 1); // dx >= eps
195        // avoid numerical truncation problems (add and subtract again) - 
196        // not sure if this survives the compiler !?
197        double tmp = x + dx;
198        return tmp - x;
199    }
200        
201}
202
203/*
204
205Benchmarks for calculating the Jacobian:
206
207Numeric/Block
208time diff = 3767805 ns
209time diff = 2845680 ns
210time diff = 3000871 ns
211time diff = 2911613 ns
212time diff = 3284195 ns
213time diff = 1581763 ns
214time diff = 1582074 ns
215
216Numeric/Full
217time diff = 9383902 ns
218time diff = 5680786 ns
219time diff = 3523978 ns
220time diff = 5784972 ns
221time diff = 3406730 ns
222time diff = 3463022 ns
223time diff = 3566275 ns
224
225Analytic
226time diff = 7060397 ns
227time diff = 1106239 ns
228time diff = 1095976 ns
229time diff = 1035330 ns
230time diff = 1041550 ns
231time diff = 1043416 ns
232time diff = 1073895 ns
233
234
235
236
237*/