001/**
002 * Copyright (c) 2011, The University of Southampton and the individual contributors.
003 * All rights reserved.
004 *
005 * Redistribution and use in source and binary forms, with or without modification,
006 * are permitted provided that the following conditions are met:
007 *
008 *   *  Redistributions of source code must retain the above copyright notice,
009 *      this list of conditions and the following disclaimer.
010 *
011 *   *  Redistributions in binary form must reproduce the above copyright notice,
012 *      this list of conditions and the following disclaimer in the documentation
013 *      and/or other materials provided with the distribution.
014 *
015 *   *  Neither the name of the University of Southampton nor the names of its
016 *      contributors may be used to endorse or promote products derived from this
017 *      software without specific prior written permission.
018 *
019 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
020 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
021 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
022 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
023 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
024 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
025 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
026 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
027 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
028 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
029 */
030package org.openimaj.math.geometry.transforms;
031
032import java.util.List;
033
034import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory;
035import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum;
036import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer;
037import org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction;
038import org.apache.commons.math3.linear.Array2DRowRealMatrix;
039import org.apache.commons.math3.linear.ArrayRealVector;
040import org.apache.commons.math3.linear.RealMatrix;
041import org.apache.commons.math3.linear.RealVector;
042import org.apache.commons.math3.util.Pair;
043import org.openimaj.math.geometry.point.Point2d;
044import org.openimaj.math.matrix.MatrixUtils;
045import org.openimaj.util.pair.IndependentPair;
046
047import Jama.Matrix;
048
049/**
050 * Refinement of fundamental matrix estimates using non-linear optimisation
051 * (Levenberg-Marquardt) under different geometric distance/error assumptions.
052 * 
053 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
054 */
055public enum FundamentalRefinement {
056        /**
057         * Don't perform any refinement and just return the initial input matrix
058         */
059        NONE {
060                @Override
061                public Matrix refine(Matrix initial, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
062                        // just return the initial estimate
063                        return initial;
064                }
065
066                @Override
067                protected MultivariateJacobianFunction getFunctions(
068                                List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data, Parameterisation p)
069                {
070                        return null;
071                }
072        },
073        /**
074         * Minimise the symmetric epipolar distance
075         */
076        EPIPOLAR {
077                @Override
078                protected MultivariateJacobianFunction getFunctions(
079                                List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data, Parameterisation p)
080                {
081                        switch (p) {
082                        case F12:
083                                return new F12Epipolar(data);
084                        case F13:
085                                return new F13Epipolar(data);
086                        case F23:
087                                return new F23Epipolar(data);
088                        }
089                        return null;
090                }
091        },
092        /**
093         * Minimise the Sampson distance (the first-order estimate of geometric
094         * error)
095         */
096        SAMPSON {
097                @Override
098                protected MultivariateJacobianFunction getFunctions(
099                                List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data, Parameterisation p)
100                {
101                        switch (p) {
102                        case F12:
103                                return new F12Sampson(data);
104                        case F13:
105                                return new F13Sampson(data);
106                        case F23:
107                                return new F23Sampson(data);
108                        }
109                        return null;
110                }
111        };
112
113        protected abstract MultivariateJacobianFunction getFunctions(
114                        final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data, Parameterisation p);
115
116        /**
117         * Refine an initial guess at the homography that takes the first points in
118         * data to the second using non-linear Levenberg Marquardt optimisation. The
119         * initial guess would normally be computed using the direct linear
120         * transform ({@link TransformUtilities#homographyMatrixNorm(List)}).
121         * 
122         * @param initial
123         *            the initial estimate (probably from the DLT technique)
124         * @param data
125         *            the pairs of data points
126         * @return the optimised estimate
127         */
128        public Matrix refine(Matrix initial, List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
129                final double[] params = new double[8];
130                final Parameterisation p = Parameterisation.chooseOptimalParameterisation(initial, params);
131
132                final LevenbergMarquardtOptimizer lm = new LevenbergMarquardtOptimizer();
133                final RealVector start = new ArrayRealVector(params, false);
134
135                // target values are all zero as we're computing the distances ourselves
136                final RealVector observed = new ArrayRealVector(data.size());
137
138                final int maxEvaluations = 1000;
139                final int maxIterations = 1000;
140
141                final MultivariateJacobianFunction model = getFunctions(data, p);
142
143                final Optimum result = lm.optimize(LeastSquaresFactory.create(model,
144                                observed, start, null, maxEvaluations, maxIterations));
145
146                final Matrix improved = p.paramsToMatrix(result.getPoint().toArray());
147
148                // normalise
149                MatrixUtils.times(improved, 1.0 / improved.normInf());
150
151                return improved;
152        }
153
154        /**
155         * Parameterisations of the fundamental matrix that preserve the rank-2
156         * constraint by writing one of the rows as a weighted combination of the
157         * other two.
158         * 
159         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
160         * 
161         */
162        protected enum Parameterisation {
163                /**
164                 * Use the first two rows and estimate the third as a weighted
165                 * combination
166                 */
167                F12 {
168                        @Override
169                        void matrixToParams(double[][] Fv, double r, double s, double[] params) {
170                                matrixToParams(Fv[0], Fv[1], r, s, params);
171                        }
172
173                        @Override
174                        Matrix paramsToMatrix(double[] params) {
175                                return new Matrix(
176                                                new double[][] {
177                                                                { params[0], params[1], params[2] },
178                                                                { params[3], params[4], params[5] },
179                                                                { params[0] * params[6] + params[3] * params[7], params[1] * params[6] + params[4]
180                                                                                * params[7], params[2] * params[6] + params[5] * params[7] }
181                                                });
182                        }
183                },
184                /**
185                 * Use the first and last rows and estimate the middle one as a weighted
186                 * combination
187                 */
188                F13 {
189                        @Override
190                        void matrixToParams(double[][] Fv, double r, double s, double[] params) {
191                                matrixToParams(Fv[0], Fv[2], r, s, params);
192                        }
193
194                        @Override
195                        Matrix paramsToMatrix(double[] params) {
196                                return new Matrix(
197                                                new double[][] {
198                                                                { params[0], params[1], params[2] },
199                                                                { params[0] * params[6] + params[3] * params[7], params[1] * params[6] + params[4]
200                                                                                * params[7], params[2] * params[6] + params[5] * params[7] },
201                                                                { params[3], params[4], params[5] }
202                                                });
203                        }
204                },
205                /**
206                 * Use the last two rows and estimate the first as a weighted
207                 * combination
208                 */
209                F23 {
210                        @Override
211                        void matrixToParams(double[][] Fv, double r, double s, double[] params) {
212                                matrixToParams(Fv[1], Fv[2], r, s, params);
213                        }
214
215                        @Override
216                        Matrix paramsToMatrix(double[] params) {
217                                return new Matrix(
218                                                new double[][] {
219                                                                { params[0] * params[6] + params[3] * params[7], params[1] * params[6] + params[4]
220                                                                                * params[7], params[2] * params[6] + params[5] * params[7] },
221                                                                { params[0], params[1], params[2] },
222                                                                { params[3], params[4], params[5] }
223                                                });
224                        }
225                };
226
227                abstract Matrix paramsToMatrix(double[] params);
228
229                abstract void matrixToParams(double[][] Fv, double r, double s, double[] params);
230
231                void matrixToParams(double[] Fv1, double[] Fv2, double r, double s, double[] params) {
232                        params[0] = Fv1[0];
233                        params[1] = Fv1[1];
234                        params[2] = Fv1[2];
235                        params[3] = Fv2[0];
236                        params[4] = Fv2[1];
237                        params[5] = Fv2[2];
238                        params[6] = r;
239                        params[7] = s;
240                }
241
242                /**
243                 * Choose the optimal parameterisation of F that preserves the rank-2
244                 * constraint by re-writing one of the rows as a weighted combination of
245                 * the other two. The params vector will be filled accordingly.
246                 * 
247                 * @param F
248                 *            the initial Fundamental matrix
249                 * @param params
250                 *            the 8 dimensional params vector
251                 * @return the chosen parameterisation
252                 */
253                public static Parameterisation chooseOptimalParameterisation(Matrix F, double[] params) {
254                        final double[][] Fv = F.getArray();
255
256                        final FastSolveNormal3x2 f12 = FastSolveNormal3x2.solve(Fv[0], Fv[1], Fv[2]);
257                        final FastSolveNormal3x2 f13 = FastSolveNormal3x2.solve(Fv[0], Fv[2], Fv[1]);
258                        final FastSolveNormal3x2 f23 = FastSolveNormal3x2.solve(Fv[1], Fv[2], Fv[0]);
259
260                        if (f12.absDet + f13.absDet + f23.absDet == 0) {
261                                throw new IllegalArgumentException("F matrix is probably zero");
262                        }
263
264                        if (f12.absDet > f13.absDet) {
265                                if (f12.absDet > f23.absDet) {
266                                        F12.matrixToParams(Fv, f12.r, f12.s, params);
267                                        return F12;
268                                } else {
269                                        F23.matrixToParams(Fv, f23.r, f23.s, params);
270                                        return F23;
271                                }
272                        } else {
273                                if (f13.absDet > f23.absDet) {
274
275                                        return F13;
276                                } else {
277                                        F23.matrixToParams(Fv, f23.r, f23.s, params);
278                                        return F23;
279                                }
280                        }
281                }
282        }
283
284        protected static class FastSolveNormal3x2 {
285                double r;
286                double s;
287                double absDet;
288
289                /**
290                 * Solve the over-determined system [r1 r2]x = [r3] where r1, r2 and r3
291                 * are 3-dimensional column vectors and x is a 2 dimensional column
292                 * vector x=[r; s]. Least-squares solution is found by solving the
293                 * normal equations x=(A^T * A)^-1 *(A^T * b) where [r1 r2]=A and b=r3.
294                 * 
295                 * @param r1
296                 *            vector 1
297                 * @param r2
298                 *            vector 2
299                 * @param r3
300                 *            vector 3
301                 * @return the solution
302                 */
303                protected static FastSolveNormal3x2 solve(double[] r1, double[] r2, double[] r3) {
304                        final FastSolveNormal3x2 s = new FastSolveNormal3x2();
305
306                        // compute A^T * A, where A^T * A=[a b; b c]:
307                        final double a = r1[0] * r1[0] + r1[1] * r1[1] + r1[2] * r1[2];
308                        final double b = r1[0] * r2[0] + r1[1] * r2[1] + r1[2] * r2[2];
309                        final double c = r2[0] * r2[0] + r2[1] * r2[1] + r2[2] * r2[2];
310
311                        // compute A^T * b where A^T * b = [d; e]:
312                        final double d = r1[0] * r3[0] + r1[1] * r3[1] + r1[2] * r3[2];
313                        final double e = r2[0] * r3[0] + r2[1] * r3[1] + r2[2] * r3[2];
314
315                        // |det(A^T * A)|
316                        final double det = a * c - b * b;
317                        s.absDet = Math.abs(det);
318
319                        // x=(A^T * A)^-1 *(A^T * b) where x=[r;s]
320                        // recall for 2x2 matrix [a b; b c], [a b; b c]^-1 = 1/det([a b; b
321                        // c]) * [c -b; -b a]
322                        s.r = (c * d - b * e) / det;
323                        s.s = (-b * d + a * e) / det;
324
325                        return s;
326                }
327        }
328
329        protected abstract static class Base implements MultivariateJacobianFunction {
330                List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data;
331
332                public Base(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
333                        this.data = data;
334                }
335
336                @Override
337                public Pair<RealVector, RealMatrix> value(RealVector point) {
338                        final double[] params = point.toArray();
339                        return new Pair<RealVector, RealMatrix>(value(params), jacobian(params));
340                }
341
342                RealVector value(double[] params) {
343                        final double[] result = new double[data.size()];
344
345                        for (int i = 0; i < data.size(); i++) {
346                                final IndependentPair<? extends Point2d, ? extends Point2d> pair = data.get(i);
347                                final Point2d p1 = pair.firstObject();
348                                final Point2d p2 = pair.secondObject();
349                                final double x = p1.getX();
350                                final double y = p1.getY();
351                                final double X = p2.getX();
352                                final double Y = p2.getY();
353
354                                result[i] = computeValue(x, y, X, Y, params[0], params[1], params[2], params[3], params[4], params[5],
355                                                params[6], params[7]);
356                        }
357
358                        return new ArrayRealVector(result, false);
359                }
360
361                abstract double computeValue(double x, double y, double X, double Y, double f1, double f2, double f3, double f4,
362                                double f5, double f6, double r, double s);
363
364                RealMatrix jacobian(double[] params) {
365                        final double[][] result = new double[data.size()][];
366
367                        for (int i = 0; i < data.size(); i++) {
368                                final IndependentPair<? extends Point2d, ? extends Point2d> pair = data.get(i);
369                                final Point2d p1 = pair.firstObject();
370                                final Point2d p2 = pair.secondObject();
371                                final double x = p1.getX();
372                                final double y = p1.getY();
373                                final double X = p2.getX();
374                                final double Y = p2.getY();
375
376                                result[i] = computeJacobian(x, y, X, Y, params[0], params[1], params[2], params[3], params[4], params[5],
377                                                params[6], params[7]);
378                        }
379
380                        return new Array2DRowRealMatrix(result, false);
381                }
382
383                abstract double[] computeJacobian(double x, double y, double X, double Y, double f1, double f2,
384                                double f3, double f4, double f5, double f6, double r, double s);
385        }
386
387        /**
388         * Based on the following matlab: <code>
389         * <pre>
390         * % Based on Eqn 11.10 in H&Z ("Symmetric Epipolar Distance")
391         * syms f1 f2 f3 f4 f5 f6 real
392         * syms r s real
393         * syms x y X Y real
394         * % row 3 is parameterised
395         * f7 = r*f1 + s*f4;
396         * f8 = r*f2 + s*f5;
397         * f9 = r*f3 + s*f6;
398         * % build F
399         * F = [f1 f2 f3; f4 f5 f6; f7 f8 f9];
400         * % the symmetric epipolar distance and its analytic jacobian
401         * Fx = F*[x y 1]';
402         * FtX = F'*[X Y 1]';
403         * XFx = [X Y 1] * F * [x y 1]';
404         * d = XFx^2 * (( 1 / (Fx(1)^2 + Fx(2)^2)) + (1 / (FtX(1)^2 + FtX(2)^2)));
405         * J = jacobian(d, [f1 f2 f3 f4 f5 f6 r s]);
406         * % generate code
407         * ccode(d, 'file', 'ccode/f12_epi_value.c')
408         * ccode(J, 'file', 'ccode/f12_epi_jac.c')
409         * </pre>
410         * </code>
411         * 
412         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
413         */
414        protected static class F12Epipolar extends Base {
415                public F12Epipolar(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
416                        super(data);
417                }
418
419                @Override
420                double computeValue(double x, double y, double X, double Y, double f1, double f2, double f3, double f4,
421                                double f5, double f6, double r, double s)
422                {
423                        final double t2 = f3 + f1 * x + f2 * y;
424                        final double t3 = f6 + f4 * x + f5 * y;
425                        final double t6 = X * f1;
426                        final double t7 = Y * f4;
427                        final double t8 = f1 * r;
428                        final double t9 = f4 * s;
429                        final double t4 = t6 + t7 + t8 + t9;
430                        final double t10 = X * f2;
431                        final double t11 = Y * f5;
432                        final double t12 = f2 * r;
433                        final double t13 = f5 * s;
434                        final double t5 = t10 + t11 + t12 + t13;
435                        final double t14 = X * f3 + Y * f6 + f3 * r + f6 * s + t4 * x + t5 * y;
436                        final double t0 = (t14 * t14) * (1.0 / (t2 * t2 + t3 * t3) + 1.0 / (t4 * t4 + t5 * t5));
437
438                        return t0;
439                }
440
441                @Override
442                double[] computeJacobian(double x, double y, double X, double Y, double f1, double f2, double f3, double f4,
443                                double f5, double f6, double r, double s)
444                {
445                        final double[] A0 = new double[8];
446
447                        final double t4 = f1 * x;
448                        final double t5 = f2 * y;
449                        final double t2 = f3 + t4 + t5;
450                        final double t19 = f4 * x;
451                        final double t20 = f5 * y;
452                        final double t3 = f6 + t19 + t20;
453                        final double t8 = X * f1;
454                        final double t9 = Y * f4;
455                        final double t10 = f1 * r;
456                        final double t11 = f4 * s;
457                        final double t6 = t8 + t9 + t10 + t11;
458                        final double t12 = X * f2;
459                        final double t13 = Y * f5;
460                        final double t14 = f2 * r;
461                        final double t15 = f5 * s;
462                        final double t7 = t12 + t13 + t14 + t15;
463                        final double t26 = X * f3;
464                        final double t27 = Y * f6;
465                        final double t28 = f3 * r;
466                        final double t29 = f6 * s;
467                        final double t30 = t6 * x;
468                        final double t31 = t7 * y;
469                        final double t16 = t26 + t27 + t28 + t29 + t30 + t31;
470                        final double t17 = X + r;
471                        final double t18 = t2 * t2;
472                        final double t21 = t3 * t3;
473                        final double t22 = t18 + t21;
474                        final double t23 = t6 * t6;
475                        final double t24 = t7 * t7;
476                        final double t25 = t23 + t24;
477                        final double t32 = 1.0 / (t22 * t22);
478                        final double t33 = 1.0 / (t25 * t25);
479                        final double t34 = t16 * t16;
480                        final double t35 = 1.0 / t22;
481                        final double t36 = 1.0 / t25;
482                        final double t37 = t35 + t36;
483                        final double t38 = Y + s;
484                        A0[0] = -t34 * (t6 * t17 * t33 * 2.0 + t2 * t32 * x * 2.0) + t16 * t17 * t37 * x * 2.0;
485                        A0[1] = -t34 * (t7 * t17 * t33 * 2.0 + t2 * t32 * y * 2.0) + t16 * t17 * t37 * y * 2.0;
486                        A0[2] = t16 * t17 * t37 * 2.0 - t32 * t34 * (f3 * 2.0 + f1 * x * 2.0 + f2 * y * 2.0);
487                        A0[3] = -t34 * (t6 * t33 * t38 * 2.0 + t3 * t32 * x * 2.0) + t16 * t37 * t38 * x * 2.0;
488                        A0[4] = -t34 * (t7 * t33 * t38 * 2.0 + t3 * t32 * y * 2.0) + t16 * t37 * t38 * y * 2.0;
489                        A0[5] = t16 * t37 * t38 * 2.0 - t32 * t34 * (f6 * 2.0 + f4 * x * 2.0 + f5 * y * 2.0);
490                        A0[6] = -t33 * t34 * (f1 * t6 * 2.0 + f2 * t7 * 2.0) + t2 * t16 * t37 * 2.0;
491                        A0[7] = -t33 * t34 * (f4 * t6 * 2.0 + f5 * t7 * 2.0) + t3 * t16 * t37 * 2.0;
492
493                        return A0;
494                }
495        }
496
497        /**
498         * Based on the following matlab: <code>
499         * <pre>
500         * % Based on Eqn 11.10 in H&Z ("Symmetric Epipolar Distance")
501         * syms f1 f2 f3 f7 f8 f9 real
502         * syms r s real
503         * syms x y X Y real
504         * % row 2 is parameterised
505         * f4 = r*f1 + s*f7;
506         * f5 = r*f2 + s*f8;
507         * f6 = r*f3 + s*f9;
508         * % build F
509         * F = [f1 f2 f3; f4 f5 f6; f7 f8 f9];
510         * % the symmetric epipolar distance and its analytic jacobian
511         * Fx = F*[x y 1]';
512         * FtX = F'*[X Y 1]';
513         * XFx = [X Y 1] * F * [x y 1]';
514         * d = XFx^2 * (( 1 / (Fx(1)^2 + Fx(2)^2)) + (1 / (FtX(1)^2 + FtX(2)^2)));
515         * J = jacobian(d, [f1 f2 f3 f7 f8 f9 r s]);
516         * % generate code
517         * ccode(d, 'file', 'ccode/f13_epi_value.c')
518         * ccode(J, 'file', 'ccode/f13_epi_jac.c')
519         * </pre>
520         * </code>
521         * 
522         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
523         */
524        protected static class F13Epipolar extends Base {
525                public F13Epipolar(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
526                        super(data);
527                }
528
529                @Override
530                double computeValue(double x, double y, double X, double Y, double f1, double f2, double f3, double f7,
531                                double f8, double f9, double r, double s)
532                {
533                        final double t4 = f1 * r;
534                        final double t5 = f7 * s;
535                        final double t6 = t4 + t5;
536                        final double t12 = X * f1;
537                        final double t13 = Y * t6;
538                        final double t2 = f7 + t12 + t13;
539                        final double t7 = f2 * r;
540                        final double t8 = f8 * s;
541                        final double t9 = t7 + t8;
542                        final double t14 = X * f2;
543                        final double t15 = Y * t9;
544                        final double t3 = f8 + t14 + t15;
545                        final double t16 = f3 * r;
546                        final double t17 = f9 * s;
547                        final double t10 = t16 + t17 + t6 * x + t9 * y;
548                        final double t11 = f3 + f1 * x + f2 * y;
549                        final double t18 = f9 + X * f3 + t2 * x + t3 * y + Y * (t16 + t17);
550                        final double t0 = (t18 * t18) * (1.0 / (t2 * t2 + t3 * t3) + 1.0 / (t10 * t10 + t11 * t11));
551
552                        return t0;
553                }
554
555                @Override
556                double[] computeJacobian(double x, double y, double X, double Y, double f1, double f2, double f3, double f7,
557                                double f8, double f9, double r, double s)
558                {
559                        final double[] A0 = new double[8];
560
561                        final double t6 = f3 * r;
562                        final double t7 = f9 * s;
563                        final double t8 = f1 * r;
564                        final double t9 = f7 * s;
565                        final double t10 = t8 + t9;
566                        final double t11 = t10 * x;
567                        final double t12 = f2 * r;
568                        final double t13 = f8 * s;
569                        final double t14 = t12 + t13;
570                        final double t15 = t14 * y;
571                        final double t2 = t6 + t7 + t11 + t15;
572                        final double t4 = f1 * x;
573                        final double t5 = f2 * y;
574                        final double t3 = f3 + t4 + t5;
575                        final double t18 = X * f1;
576                        final double t19 = Y * t10;
577                        final double t16 = f7 + t18 + t19;
578                        final double t20 = X * f2;
579                        final double t21 = Y * t14;
580                        final double t17 = f8 + t20 + t21;
581                        final double t31 = X * f3;
582                        final double t32 = t16 * x;
583                        final double t33 = t17 * y;
584                        final double t34 = t6 + t7;
585                        final double t35 = Y * t34;
586                        final double t22 = f9 + t31 + t32 + t33 + t35;
587                        final double t23 = t16 * t16;
588                        final double t24 = t17 * t17;
589                        final double t25 = t23 + t24;
590                        final double t26 = t2 * t2;
591                        final double t27 = t3 * t3;
592                        final double t28 = t26 + t27;
593                        final double t29 = Y * r;
594                        final double t30 = X + t29;
595                        final double t36 = 1.0 / (t28 * t28);
596                        final double t37 = 1.0 / (t25 * t25);
597                        final double t38 = t22 * t22;
598                        final double t39 = 1.0 / t25;
599                        final double t40 = 1.0 / t28;
600                        final double t41 = t39 + t40;
601                        final double t42 = Y * s;
602                        final double t43 = t42 + 1.0;
603                        A0[0] = -t38 * (t36 * (t3 * x * 2.0 + r * t2 * x * 2.0) + t16 * t30 * t37 * 2.0) + t22 * t30 * t41 * x * 2.0;
604                        A0[1] = -t38 * (t36 * (t3 * y * 2.0 + r * t2 * y * 2.0) + t17 * t30 * t37 * 2.0) + t22 * t30 * t41 * y * 2.0;
605                        A0[2] = -t36 * t38 * (f3 * 2.0 + f1 * x * 2.0 + f2 * y * 2.0 + r * t2 * 2.0) + t22 * t30 * t41 * 2.0;
606                        A0[3] = -t38 * (t16 * t37 * t43 * 2.0 + s * t2 * t36 * x * 2.0) + t22 * t41 * t43 * x * 2.0;
607                        A0[4] = -t38 * (t17 * t37 * t43 * 2.0 + s * t2 * t36 * y * 2.0) + t22 * t41 * t43 * y * 2.0;
608                        A0[5] = t22 * t41 * t43 * 2.0 - s * t2 * t36 * t38 * 2.0;
609                        A0[6] = -t38 * (t37 * (Y * f1 * t16 * 2.0 + Y * f2 * t17 * 2.0) + t2 * t3 * t36 * 2.0) + t22 * t41
610                                        * (Y * f3 + Y * f1 * x + Y * f2 * y) * 2.0;
611                        A0[7] = -t38 * (t37 * (Y * f7 * t16 * 2.0 + Y * f8 * t17 * 2.0) + t2 * t36 * (f9 + f7 * x + f8 * y) * 2.0)
612                                        + t22 * t41 * (Y * f9 + Y * f7 * x + Y * f8 * y) * 2.0;
613
614                        return A0;
615                }
616        }
617
618        /**
619         * Based on the following matlab: <code>
620         * <pre>
621         * % Based on Eqn 11.10 in H&Z ("Symmetric Epipolar Distance")
622         * syms f4 f5 f6 f7 f8 f9 real
623         * syms r s real
624         * syms x y X Y real
625         * % row 1 is parameterised
626         * f1 = r*f4 + s*f7;
627         * f2 = r*f5 + s*f8;
628         * f3 = r*f6 + s*f9;
629         * % build F
630         * F = [f1 f2 f3; f4 f5 f6; f7 f8 f9];
631         * % the symmetric epipolar distance and its analytic jacobian
632         * Fx = F*[x y 1]';
633         * FtX = F'*[X Y 1]';
634         * XFx = [X Y 1] * F * [x y 1]';
635         * d = XFx^2 * (( 1 / (Fx(1)^2 + Fx(2)^2)) + (1 / (FtX(1)^2 + FtX(2)^2)));
636         * J = jacobian(d, [f4 f5 f6 f7 f8 f9 r s]);
637         * % generate code
638         * ccode(d, 'file', 'ccode/f23_epi_value.c')
639         * ccode(J, 'file', 'ccode/f23_epi_jac.c')
640         * </pre>
641         * </code>
642         * 
643         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
644         */
645        protected static class F23Epipolar extends Base {
646                public F23Epipolar(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
647                        super(data);
648                }
649
650                @Override
651                double computeValue(double x, double y, double X, double Y, double f4, double f5, double f6, double f7,
652                                double f8, double f9, double r, double s)
653                {
654                        final double t4 = f4 * r;
655                        final double t5 = f7 * s;
656                        final double t6 = t4 + t5;
657                        final double t12 = Y * f4;
658                        final double t13 = X * t6;
659                        final double t2 = f7 + t12 + t13;
660                        final double t7 = f5 * r;
661                        final double t8 = f8 * s;
662                        final double t9 = t7 + t8;
663                        final double t14 = Y * f5;
664                        final double t15 = X * t9;
665                        final double t3 = f8 + t14 + t15;
666                        final double t16 = f6 * r;
667                        final double t17 = f9 * s;
668                        final double t10 = t16 + t17 + t6 * x + t9 * y;
669                        final double t11 = f6 + f4 * x + f5 * y;
670                        final double t18 = f9 + Y * f6 + t2 * x + t3 * y + X * (t16 + t17);
671                        final double t0 = (t18 * t18) * (1.0 / (t2 * t2 + t3 * t3) + 1.0 / (t10 * t10 + t11 * t11));
672
673                        return t0;
674                }
675
676                @Override
677                double[] computeJacobian(double x, double y, double X, double Y, double f4, double f5, double f6, double f7,
678                                double f8, double f9, double r, double s)
679                {
680                        final double[] A0 = new double[8];
681
682                        final double t6 = f6 * r;
683                        final double t7 = f9 * s;
684                        final double t8 = f4 * r;
685                        final double t9 = f7 * s;
686                        final double t10 = t8 + t9;
687                        final double t11 = t10 * x;
688                        final double t12 = f5 * r;
689                        final double t13 = f8 * s;
690                        final double t14 = t12 + t13;
691                        final double t15 = t14 * y;
692                        final double t2 = t6 + t7 + t11 + t15;
693                        final double t4 = f4 * x;
694                        final double t5 = f5 * y;
695                        final double t3 = f6 + t4 + t5;
696                        final double t18 = Y * f4;
697                        final double t19 = X * t10;
698                        final double t16 = f7 + t18 + t19;
699                        final double t20 = Y * f5;
700                        final double t21 = X * t14;
701                        final double t17 = f8 + t20 + t21;
702                        final double t31 = Y * f6;
703                        final double t32 = t16 * x;
704                        final double t33 = t17 * y;
705                        final double t34 = t6 + t7;
706                        final double t35 = X * t34;
707                        final double t22 = f9 + t31 + t32 + t33 + t35;
708                        final double t23 = t16 * t16;
709                        final double t24 = t17 * t17;
710                        final double t25 = t23 + t24;
711                        final double t26 = t2 * t2;
712                        final double t27 = t3 * t3;
713                        final double t28 = t26 + t27;
714                        final double t29 = X * r;
715                        final double t30 = Y + t29;
716                        final double t36 = 1.0 / (t28 * t28);
717                        final double t37 = 1.0 / (t25 * t25);
718                        final double t38 = t22 * t22;
719                        final double t39 = 1.0 / t25;
720                        final double t40 = 1.0 / t28;
721                        final double t41 = t39 + t40;
722                        final double t42 = X * s;
723                        final double t43 = t42 + 1.0;
724                        A0[0] = -t38 * (t36 * (t3 * x * 2.0 + r * t2 * x * 2.0) + t16 * t30 * t37 * 2.0) + t22 * t30 * t41 * x * 2.0;
725                        A0[1] = -t38 * (t36 * (t3 * y * 2.0 + r * t2 * y * 2.0) + t17 * t30 * t37 * 2.0) + t22 * t30 * t41 * y * 2.0;
726                        A0[2] = -t36 * t38 * (f6 * 2.0 + f4 * x * 2.0 + f5 * y * 2.0 + r * t2 * 2.0) + t22 * t30 * t41 * 2.0;
727                        A0[3] = -t38 * (t16 * t37 * t43 * 2.0 + s * t2 * t36 * x * 2.0) + t22 * t41 * t43 * x * 2.0;
728                        A0[4] = -t38 * (t17 * t37 * t43 * 2.0 + s * t2 * t36 * y * 2.0) + t22 * t41 * t43 * y * 2.0;
729                        A0[5] = t22 * t41 * t43 * 2.0 - s * t2 * t36 * t38 * 2.0;
730                        A0[6] = -t38 * (t37 * (X * f4 * t16 * 2.0 + X * f5 * t17 * 2.0) + t2 * t3 * t36 * 2.0) + t22 * t41
731                                        * (X * f6 + X * f4 * x + X * f5 * y) * 2.0;
732                        A0[7] = -t38 * (t37 * (X * f7 * t16 * 2.0 + X * f8 * t17 * 2.0) + t2 * t36 * (f9 + f7 * x + f8 * y) * 2.0)
733                                        + t22 * t41 * (X * f9 + X * f7 * x + X * f8 * y) * 2.0;
734
735                        return A0;
736                }
737        }
738
739        /**
740         * Based on the following matlab: <code>
741         * <pre>
742         * % Based on Eqn 11.9 in H&Z ("First order geometric error (Sampson distance)")
743         * syms f1 f2 f3 f4 f5 f6 real
744         * syms r s real
745         * syms x y X Y real
746         * % row 3 is parameterised
747         * f7 = r*f1 + s*f4;
748         * f8 = r*f2 + s*f5;
749         * f9 = r*f3 + s*f6;
750         * % build F
751         * F = [f1 f2 f3; f4 f5 f6; f7 f8 f9];
752         * % the sampson distance and its analytic jacobian
753         * Fx = F*[x y 1]';
754         * FtX = F'*[X Y 1]';
755         * XFx = [X Y 1] * F * [x y 1]';
756         * d = XFx^2 / (Fx(1)^2 + Fx(2)^2 + FtX(1)^2 + FtX(2)^2);
757         * J = jacobian(d, [f1 f2 f3 f4 f5 f6 r s]);
758         * % generate code
759         * ccode(d, 'file', 'ccode/f12_sampson_value.c')
760         * ccode(J, 'file', 'ccode/f12_sampson_jac.c')
761         * </pre>
762         * </code>
763         * 
764         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
765         */
766        protected static class F12Sampson extends Base {
767                public F12Sampson(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
768                        super(data);
769                }
770
771                @Override
772                double computeValue(double x, double y, double X, double Y, double f1, double f2, double f3, double f4,
773                                double f5, double f6, double r, double s)
774                {
775                        final double t6 = X * f1;
776                        final double t7 = Y * f4;
777                        final double t8 = f1 * r;
778                        final double t9 = f4 * s;
779                        final double t2 = t6 + t7 + t8 + t9;
780                        final double t10 = X * f2;
781                        final double t11 = Y * f5;
782                        final double t12 = f2 * r;
783                        final double t13 = f5 * s;
784                        final double t3 = t10 + t11 + t12 + t13;
785                        final double t4 = f3 + f1 * x + f2 * y;
786                        final double t5 = f6 + f4 * x + f5 * y;
787                        final double t14 = X * f3 + Y * f6 + f3 * r + f6 * s + t2 * x + t3 * y;
788                        final double t0 = (t14 * t14) / (t2 * t2 + t3 * t3 + t4 * t4 + t5 * t5);
789
790                        return t0;
791                }
792
793                @Override
794                double[] computeJacobian(double x, double y, double X, double Y, double f1, double f2, double f3, double f4,
795                                double f5, double f6, double r, double s)
796                {
797                        final double[] A0 = new double[8];
798
799                        final double t2 = X * f1;
800                        final double t3 = Y * f4;
801                        final double t4 = f1 * r;
802                        final double t5 = f4 * s;
803                        final double t6 = t2 + t3 + t4 + t5;
804                        final double t12 = X * f2;
805                        final double t13 = Y * f5;
806                        final double t14 = f2 * r;
807                        final double t15 = f5 * s;
808                        final double t7 = t12 + t13 + t14 + t15;
809                        final double t8 = f1 * x;
810                        final double t9 = f2 * y;
811                        final double t10 = f3 + t8 + t9;
812                        final double t21 = f4 * x;
813                        final double t22 = f5 * y;
814                        final double t11 = f6 + t21 + t22;
815                        final double t25 = X * f3;
816                        final double t26 = Y * f6;
817                        final double t27 = f3 * r;
818                        final double t28 = f6 * s;
819                        final double t29 = t6 * x;
820                        final double t30 = t7 * y;
821                        final double t16 = t25 + t26 + t27 + t28 + t29 + t30;
822                        final double t17 = X + r;
823                        final double t18 = t6 * t6;
824                        final double t19 = t7 * t7;
825                        final double t20 = t10 * t10;
826                        final double t23 = t11 * t11;
827                        final double t24 = t18 + t19 + t20 + t23;
828                        final double t31 = 1.0 / (t24 * t24);
829                        final double t32 = t16 * t16;
830                        final double t33 = 1.0 / t24;
831                        final double t34 = Y + s;
832                        A0[0] = -t31 * t32 * (t6 * t17 * 2.0 + t10 * x * 2.0) + t16 * t17 * t33 * x * 2.0;
833                        A0[1] = -t31 * t32 * (t7 * t17 * 2.0 + t10 * y * 2.0) + t16 * t17 * t33 * y * 2.0;
834                        A0[2] = t16 * t17 * t33 * 2.0 - t31 * t32 * (f3 * 2.0 + f1 * x * 2.0 + f2 * y * 2.0);
835                        A0[3] = -t31 * t32 * (t6 * t34 * 2.0 + t11 * x * 2.0) + t16 * t33 * t34 * x * 2.0;
836                        A0[4] = -t31 * t32 * (t7 * t34 * 2.0 + t11 * y * 2.0) + t16 * t33 * t34 * y * 2.0;
837                        A0[5] = t16 * t33 * t34 * 2.0 - t31 * t32 * (f6 * 2.0 + f4 * x * 2.0 + f5 * y * 2.0);
838                        A0[6] = -t31 * t32 * (f1 * t6 * 2.0 + f2 * t7 * 2.0) + t10 * t16 * t33 * 2.0;
839                        A0[7] = -t31 * t32 * (f4 * t6 * 2.0 + f5 * t7 * 2.0) + t11 * t16 * t33 * 2.0;
840
841                        return A0;
842                }
843        }
844
845        /**
846         * Based on the following matlab: <code>
847         * <pre>
848         * % Based on Eqn 11.9 in H&Z ("First order geometric error (Sampson distance)")
849         * syms f1 f2 f3 f7 f8 f9 real
850         * syms r s real
851         * syms x y X Y real
852         * % row 2 is parameterised
853         * f4 = r*f1 + s*f7;
854         * f5 = r*f2 + s*f8;
855         * f6 = r*f3 + s*f9;
856         * % build F
857         * F = [f1 f2 f3; f4 f5 f6; f7 f8 f9];
858         * % the sampson distance and its analytic jacobian
859         * Fx = F*[x y 1]';
860         * FtX = F'*[X Y 1]';
861         * XFx = [X Y 1] * F * [x y 1]';
862         * d = XFx^2 / (Fx(1)^2 + Fx(2)^2 + FtX(1)^2 + FtX(2)^2);
863         * J = jacobian(d, [f1 f2 f3 f7 f8 f9 r s]);
864         * % generate code
865         * ccode(d, 'file', 'ccode/f13_sampson_value.c')
866         * ccode(J, 'file', 'ccode/f13_sampson_jac.c')
867         * </pre>
868         * </code>
869         * 
870         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
871         */
872        protected static class F13Sampson extends Base {
873                public F13Sampson(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
874                        super(data);
875                }
876
877                @Override
878                double computeValue(double x, double y, double X, double Y, double f1, double f2, double f3, double f7,
879                                double f8, double f9, double r, double s)
880                {
881                        final double t4 = f1 * r;
882                        final double t5 = f7 * s;
883                        final double t6 = t4 + t5;
884                        final double t8 = f2 * r;
885                        final double t9 = f8 * s;
886                        final double t10 = t8 + t9;
887                        final double t16 = f3 * r;
888                        final double t17 = f9 * s;
889                        final double t2 = t16 + t17 + t6 * x + t10 * y;
890                        final double t3 = f3 + f1 * x + f2 * y;
891                        final double t12 = X * f1;
892                        final double t13 = Y * t6;
893                        final double t7 = f7 + t12 + t13;
894                        final double t14 = X * f2;
895                        final double t15 = Y * t10;
896                        final double t11 = f8 + t14 + t15;
897                        final double t18 = f9 + X * f3 + t7 * x + t11 * y + Y * (t16 + t17);
898                        final double t0 = (t18 * t18) / (t2 * t2 + t3 * t3 + t7 * t7 + t11 * t11);
899
900                        return t0;
901                }
902
903                @Override
904                double[] computeJacobian(double x, double y, double X, double Y, double f1, double f2, double f3, double f7,
905                                double f8, double f9, double r, double s)
906                {
907                        final double[] A0 = new double[8];
908
909                        final double t2 = f1 * r;
910                        final double t3 = f7 * s;
911                        final double t4 = t2 + t3;
912                        final double t5 = f3 * r;
913                        final double t6 = f9 * s;
914                        final double t7 = t4 * x;
915                        final double t8 = f2 * r;
916                        final double t9 = f8 * s;
917                        final double t10 = t8 + t9;
918                        final double t11 = t10 * y;
919                        final double t12 = t5 + t6 + t7 + t11;
920                        final double t13 = f1 * x;
921                        final double t14 = f2 * y;
922                        final double t15 = f3 + t13 + t14;
923                        final double t16 = X * f1;
924                        final double t17 = Y * t4;
925                        final double t18 = f7 + t16 + t17;
926                        final double t20 = X * f2;
927                        final double t21 = Y * t10;
928                        final double t19 = f8 + t20 + t21;
929                        final double t30 = X * f3;
930                        final double t31 = t18 * x;
931                        final double t32 = t19 * y;
932                        final double t33 = t5 + t6;
933                        final double t34 = Y * t33;
934                        final double t22 = f9 + t30 + t31 + t32 + t34;
935                        final double t23 = Y * r;
936                        final double t24 = X + t23;
937                        final double t25 = t12 * t12;
938                        final double t26 = t15 * t15;
939                        final double t27 = t18 * t18;
940                        final double t28 = t19 * t19;
941                        final double t29 = t25 + t26 + t27 + t28;
942                        final double t35 = 1.0 / (t29 * t29);
943                        final double t36 = t22 * t22;
944                        final double t37 = 1.0 / t29;
945                        final double t38 = Y * s;
946                        final double t39 = t38 + 1.0;
947                        A0[0] = -t35 * t36 * (t18 * t24 * 2.0 + t15 * x * 2.0 + r * t12 * x * 2.0) + t22 * t24 * t37 * x * 2.0;
948                        A0[1] = -t35 * t36 * (t19 * t24 * 2.0 + t15 * y * 2.0 + r * t12 * y * 2.0) + t22 * t24 * t37 * y * 2.0;
949                        A0[2] = -t35 * t36 * (f3 * 2.0 + f1 * x * 2.0 + f2 * y * 2.0 + r * t12 * 2.0) + t22 * t24 * t37 * 2.0;
950                        A0[3] = -t35 * t36 * (t18 * t39 * 2.0 + s * t12 * x * 2.0) + t22 * t37 * t39 * x * 2.0;
951                        A0[4] = -t35 * t36 * (t19 * t39 * 2.0 + s * t12 * y * 2.0) + t22 * t37 * t39 * y * 2.0;
952                        A0[5] = t22 * t37 * t39 * 2.0 - s * t12 * t35 * t36 * 2.0;
953                        A0[6] = -t35 * t36 * (t12 * t15 * 2.0 + Y * f1 * t18 * 2.0 + Y * f2 * t19 * 2.0) + t22 * t37
954                                        * (Y * f3 + Y * f1 * x + Y * f2 * y) * 2.0;
955                        A0[7] = -t35 * t36 * (t12 * (f9 + f7 * x + f8 * y) * 2.0 + Y * f7 * t18 * 2.0 + Y * f8 * t19 * 2.0) + t22
956                                        * t37 * (Y * f9 + Y * f7 * x + Y * f8 * y) * 2.0;
957
958                        return A0;
959                }
960        }
961
962        /**
963         * Based on the following matlab: <code>
964         * <pre>
965         * % Based on Eqn 11.9 in H&Z ("First order geometric error (Sampson distance)")
966         * syms f4 f5 f6 f7 f8 f9 real
967         * syms r s real
968         * syms x y X Y real
969         * % row 1 is parameterised
970         * f1 = r*f4 + s*f7;
971         * f2 = r*f5 + s*f8;
972         * f3 = r*f6 + s*f9;
973         * % build F
974         * F = [f1 f2 f3; f4 f5 f6; f7 f8 f9];
975         * % the sampson distance and its analytic jacobian
976         * Fx = F*[x y 1]';
977         * FtX = F'*[X Y 1]';
978         * XFx = [X Y 1] * F * [x y 1]';
979         * d = XFx^2 / (Fx(1)^2 + Fx(2)^2 + FtX(1)^2 + FtX(2)^2);
980         * J = jacobian(d, [f4 f5 f6 f7 f8 f9 r s]);
981         * % generate code
982         * ccode(d, 'file', 'ccode/f23_sampson_value.c')
983         * ccode(J, 'file', 'ccode/f23_sampson_jac.c')
984         * </pre>
985         * </code>
986         * 
987         * @author Jonathon Hare (jsh2@ecs.soton.ac.uk)
988         */
989        protected static class F23Sampson extends Base {
990                public F23Sampson(List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data) {
991                        super(data);
992                }
993
994                @Override
995                double computeValue(double x, double y, double X, double Y, double f4, double f5, double f6, double f7,
996                                double f8, double f9, double r, double s)
997                {
998                        final double t4 = f4 * r;
999                        final double t5 = f7 * s;
1000                        final double t6 = t4 + t5;
1001                        final double t8 = f5 * r;
1002                        final double t9 = f8 * s;
1003                        final double t10 = t8 + t9;
1004                        final double t16 = f6 * r;
1005                        final double t17 = f9 * s;
1006                        final double t2 = t16 + t17 + t6 * x + t10 * y;
1007                        final double t3 = f6 + f4 * x + f5 * y;
1008                        final double t12 = Y * f4;
1009                        final double t13 = X * t6;
1010                        final double t7 = f7 + t12 + t13;
1011                        final double t14 = Y * f5;
1012                        final double t15 = X * t10;
1013                        final double t11 = f8 + t14 + t15;
1014                        final double t18 = f9 + Y * f6 + t7 * x + t11 * y + X * (t16 + t17);
1015                        final double t0 = (t18 * t18) / (t2 * t2 + t3 * t3 + t7 * t7 + t11 * t11);
1016
1017                        return t0;
1018                }
1019
1020                @Override
1021                double[] computeJacobian(double x, double y, double X, double Y, double f4, double f5, double f6, double f7,
1022                                double f8, double f9, double r, double s)
1023                {
1024                        final double[] A0 = new double[8];
1025
1026                        final double t2 = f4 * r;
1027                        final double t3 = f7 * s;
1028                        final double t4 = t2 + t3;
1029                        final double t5 = f6 * r;
1030                        final double t6 = f9 * s;
1031                        final double t7 = t4 * x;
1032                        final double t8 = f5 * r;
1033                        final double t9 = f8 * s;
1034                        final double t10 = t8 + t9;
1035                        final double t11 = t10 * y;
1036                        final double t12 = t5 + t6 + t7 + t11;
1037                        final double t13 = f4 * x;
1038                        final double t14 = f5 * y;
1039                        final double t15 = f6 + t13 + t14;
1040                        final double t16 = Y * f4;
1041                        final double t17 = X * t4;
1042                        final double t18 = f7 + t16 + t17;
1043                        final double t20 = Y * f5;
1044                        final double t21 = X * t10;
1045                        final double t19 = f8 + t20 + t21;
1046                        final double t30 = Y * f6;
1047                        final double t31 = t18 * x;
1048                        final double t32 = t19 * y;
1049                        final double t33 = t5 + t6;
1050                        final double t34 = X * t33;
1051                        final double t22 = f9 + t30 + t31 + t32 + t34;
1052                        final double t23 = X * r;
1053                        final double t24 = Y + t23;
1054                        final double t25 = t12 * t12;
1055                        final double t26 = t15 * t15;
1056                        final double t27 = t18 * t18;
1057                        final double t28 = t19 * t19;
1058                        final double t29 = t25 + t26 + t27 + t28;
1059                        final double t35 = 1.0 / (t29 * t29);
1060                        final double t36 = t22 * t22;
1061                        final double t37 = 1.0 / t29;
1062                        final double t38 = X * s;
1063                        final double t39 = t38 + 1.0;
1064                        A0[0] = -t35 * t36 * (t18 * t24 * 2.0 + t15 * x * 2.0 + r * t12 * x * 2.0) + t22 * t24 * t37 * x * 2.0;
1065                        A0[1] = -t35 * t36 * (t19 * t24 * 2.0 + t15 * y * 2.0 + r * t12 * y * 2.0) + t22 * t24 * t37 * y * 2.0;
1066                        A0[2] = -t35 * t36 * (f6 * 2.0 + f4 * x * 2.0 + f5 * y * 2.0 + r * t12 * 2.0) + t22 * t24 * t37 * 2.0;
1067                        A0[3] = -t35 * t36 * (t18 * t39 * 2.0 + s * t12 * x * 2.0) + t22 * t37 * t39 * x * 2.0;
1068                        A0[4] = -t35 * t36 * (t19 * t39 * 2.0 + s * t12 * y * 2.0) + t22 * t37 * t39 * y * 2.0;
1069                        A0[5] = t22 * t37 * t39 * 2.0 - s * t12 * t35 * t36 * 2.0;
1070                        A0[6] = -t35 * t36 * (t12 * t15 * 2.0 + X * f4 * t18 * 2.0 + X * f5 * t19 * 2.0) + t22 * t37
1071                                        * (X * f6 + X * f4 * x + X * f5 * y) * 2.0;
1072                        A0[7] = -t35 * t36 * (t12 * (f9 + f7 * x + f8 * y) * 2.0 + X * f7 * t18 * 2.0 + X * f8 * t19 * 2.0) + t22
1073                                        * t37 * (X * f9 + X * f7 * x + X * f8 * y) * 2.0;
1074
1075                        return A0;
1076                }
1077        }
1078}