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}