001/******************************************************************************* 002 * Permission to use and distribute this software is granted under the BSD 2-Clause 003 * "Simplified" License (see http://opensource.org/licenses/BSD-2-Clause). 004 * Copyright (c) 2016-2023 Wilhelm Burger. All rights reserved. 005 * Visit https://imagingbook.com for additional details. 006 ******************************************************************************/ 007package imagingbook.calibration.zhang; 008 009import imagingbook.common.geometry.basic.Pnt2d; 010import org.apache.commons.math3.analysis.MultivariateMatrixFunction; 011import org.apache.commons.math3.analysis.MultivariateVectorFunction; 012 013import static java.lang.Math.cos; 014import static java.lang.Math.pow; 015import static java.lang.Math.sin; 016import static java.lang.Math.sqrt; 017 018 019public class NonlinearOptimizerAnalytic extends NonlinearOptimizer { 020 021 NonlinearOptimizerAnalytic(Pnt2d[] modelPts, Pnt2d[][] obsPts) { 022 super(modelPts, obsPts); 023 } 024 025 @Override 026 MultivariateVectorFunction makeValueFun() { 027 return new ValueFun(); 028 } 029 030 @Override 031 MultivariateMatrixFunction makeJacobianFun() { 032 return new JacobianFun(); 033 } 034 035 private class JacobianFun implements MultivariateMatrixFunction { 036 /** 037 * Calculates a "stacked" Jacobian matrix with 2MN rows and K = 7 + 6M columns (for M views with N points each, 038 * K parameters). For example, with M = 5 views and N = 256 points each, J is of size 2560 × 37. Each pair of 039 * rows in the Jacobian corresponds to one point. 040 */ 041 @Override 042 public double[][] value(double[] params) { 043 final double[][] J = new double[2 * M * N][]; 044 int r = 0; // row 045 for (int i = 0; i < M; i++) { // for all views 046 for (int j = 0; j < N; j++) { // for all points 047 final double[][] Jij = subJacobian(i, j, params); 048 J[r + 0] = Jij[0]; // row 0 (x-coordinate) 049 J[r + 1] = Jij[1]; // row 1 (y-coordinate) 050 r = r + 2; 051 } 052 } 053// System.out.format("**** J = %d / %d\n", J.length, J[0].length); 054// System.out.println(NonlinearOptimizerAnalytic.class.getSimpleName() + 055// ": Jacobian inverse condition number = " + MathUtil.inverseConditionNumber(J)); 056 return J; 057 } 058 059 /** 060 * Calculates the sub-Jacobian for view 'i' / model point 'j' with the current parameter vector 'params' . 061 * 062 * @param i the point index (= 0,...,M) 063 * @param j the view index (= 0,...,N) 064 * @param params the current parameters (of length K) 065 * @return the two rows (2 x K sub-matrix) of the Jacobian for the given point 066 */ 067 private double[][] subJacobian(int i, int j, double[] params) { 068 final double[][] A0 = new double[2][camParLength + viewParLength]; 069 070 final double X = modelPts[j].getX(); 071 final double Y = modelPts[j].getY(); 072 073 final double alpha = params[0]; 074 final double beta = params[1]; 075 final double gamma = params[2]; 076 final double uc = params[3]; 077 final double vc = params[4]; 078 final double k0 = params[5]; 079 final double k1 = params[6]; 080 081 final double wx = params[i * viewParLength + camParLength + 0]; 082 final double wy = params[i * viewParLength + camParLength + 1]; 083 final double wz = params[i * viewParLength + camParLength + 2]; 084 final double tx = params[i * viewParLength + camParLength + 3]; 085 final double ty = params[i * viewParLength + camParLength + 4]; 086 final double tz = params[i * viewParLength + camParLength + 5]; 087 088 // begin matlab code 089 final double t2 = wx * wx; 090 final double t3 = wy * wy; 091 final double t4 = wz * wz; 092 final double t5 = t2 + t3 + t4; 093 final double t6 = sqrt(t5); 094 final double t7 = sin(t6); 095 final double t8 = 1.0 / sqrt(t5); 096 final double t9 = cos(t6); 097 final double t10 = t9 - 1.0; 098 final double t11 = 1.0 / t5; 099 final double t12 = t7 * t8 * wy; 100 final double t13 = t10 * t11 * wx * wz; 101 final double t14 = t12 + t13; 102 final double t15 = t7 * t8 * wz; 103 final double t16 = t7 * t8 * wx; 104 final double t18 = t10 * t11 * wy * wz; 105 final double t17 = t16 - t18; 106 final double t19 = Y * t17; 107 final double t39 = X * t14; 108 final double t20 = t19 - t39 + tz; 109 final double t21 = 1.0 / t20; 110 final double t22 = t10 * t11 * wx * wy; 111 final double t23 = t3 + t4; 112 final double t24 = t10 * t11 * t23; 113 final double t25 = t24 + 1.0; 114 final double t26 = alpha * t25; 115 final double t27 = t15 + t22; 116 final double t28 = t17 * uc; 117 final double t29 = t2 + t4; 118 final double t30 = t10 * t11 * t29; 119 final double t31 = t30 + 1.0; 120 final double t32 = gamma * t31; 121 final double t45 = alpha * t27; 122 final double t33 = t28 + t32 - t45; 123 final double t34 = Y * t33; 124 final double t35 = alpha * tx; 125 final double t36 = gamma * ty; 126 final double t37 = tz * uc; 127 final double t40 = t15 - t22; 128 final double t41 = gamma * t40; 129 final double t42 = t14 * uc; 130 final double t43 = t26 + t41 - t42; 131 final double t44 = X * t43; 132 final double t46 = t34 + t35 + t36 + t37 + t44; 133 final double t47 = t21 * t46; 134 final double t38 = -t47 + uc; 135 final double t48 = 1.0 / (alpha * alpha * alpha); 136 final double t49 = t38 * t38; 137 final double t50 = t48 * t49 * 2.0; 138 final double t51 = 1.0 / (alpha * alpha); 139 final double t52 = X * t25; 140 final double t57 = Y * t27; 141 final double t53 = t52 - t57 + tx; 142 final double t54 = t21 * t38 * t51 * t53 * 2.0; 143 final double t55 = t50 + t54; 144 final double t60 = beta * ty; 145 final double t61 = beta * t40; 146 final double t62 = t14 * vc; 147 final double t63 = t61 - t62; 148 final double t64 = X * t63; 149 final double t65 = tz * vc; 150 final double t66 = t17 * vc; 151 final double t67 = beta * t31; 152 final double t68 = t66 + t67; 153 final double t69 = Y * t68; 154 final double t70 = t60 + t64 + t65 + t69; 155 final double t71 = t21 * t70; 156 final double t56 = -t71 + vc; 157 final double t58 = t49 * t51; 158 final double t59 = 1.0 / (beta * beta); 159 final double t72 = t56 * t56; 160 final double t73 = t59 * t72; 161 final double t74 = t58 + t73; 162 final double t75 = 1.0 / (beta * beta * beta); 163 final double t76 = t72 * t75 * 2.0; 164 final double t77 = X * t40; 165 final double t78 = Y * t31; 166 final double t79 = t77 + t78 + ty; 167 final double t80 = t21 * t56 * t59 * t79 * 2.0; 168 final double t81 = t76 + t80; 169 final double t82 = k0 * t74; 170 final double t83 = t74 * t74; 171 final double t84 = k1 * t83; 172 final double t85 = t82 + t84; 173 final double t86 = 1.0 / pow(t5, 3.0 / 2.0); 174 final double t87 = 1.0 / (t5 * t5); 175 final double t88 = t9 * t11 * wx * wz; 176 final double t89 = t2 * t7 * t86 * wy; 177 final double t90 = t2 * t10 * t87 * wy * 2.0; 178 final double t91 = t7 * t86 * wx * wy; 179 final double t92 = t2 * t7 * t86 * wz; 180 final double t93 = t2 * t10 * t87 * wz * 2.0; 181 final double t105 = t10 * t11 * wz; 182 final double t106 = t9 * t11 * wx * wy; 183 final double t94 = t91 + t92 + t93 - t105 - t106; 184 final double t95 = t7 * t8; 185 final double t96 = t2 * t9 * t11; 186 final double t97 = t10 * t87 * wx * wy * wz * 2.0; 187 final double t98 = t7 * t86 * wx * wy * wz; 188 final double t103 = t2 * t7 * t86; 189 final double t99 = t95 + t96 + t97 + t98 - t103; 190 final double t100 = t10 * t29 * t87 * wx * 2.0; 191 final double t101 = t7 * t29 * t86 * wx; 192 final double t116 = t10 * t11 * wx * 2.0; 193 final double t102 = t100 + t101 - t116; 194 final double t104 = t7 * t86 * wx * wz; 195 final double t107 = X * t94; 196 final double t108 = Y * t99; 197 final double t109 = t107 + t108; 198 final double t110 = 1.0 / (t20 * t20); 199 final double t111 = t10 * t23 * t87 * wx * 2.0; 200 final double t112 = t7 * t23 * t86 * wx; 201 final double t113 = t111 + t112; 202 final double t117 = t10 * t11 * wy; 203 final double t114 = t88 + t89 + t90 - t104 - t117; 204 final double t115 = t94 * uc; 205 final double t118 = t99 * uc; 206 final double t119 = beta * t102; 207 final double t262 = t99 * vc; 208 final double t120 = t119 - t262; 209 final double t121 = Y * t120; 210 final double t122 = beta * t114; 211 final double t123 = t94 * vc; 212 final double t124 = t122 + t123; 213 final double t263 = X * t124; 214 final double t125 = t121 - t263; 215 final double t126 = t21 * t125; 216 final double t127 = t70 * t109 * t110; 217 final double t128 = t126 + t127; 218 final double t129 = gamma * t114; 219 final double t141 = alpha * t113; 220 final double t130 = t115 + t129 - t141; 221 final double t131 = X * t130; 222 final double t132 = -t88 + t89 + t90 + t104 - t117; 223 final double t133 = alpha * t132; 224 final double t142 = gamma * t102; 225 final double t134 = t118 + t133 - t142; 226 final double t135 = Y * t134; 227 final double t136 = t131 + t135; 228 final double t137 = t21 * t136; 229 final double t143 = t46 * t109 * t110; 230 final double t138 = t137 - t143; 231 final double t139 = t38 * t51 * t138 * 2.0; 232 final double t264 = t56 * t59 * t128 * 2.0; 233 final double t140 = t139 - t264; 234 final double t144 = t3 * t7 * t86 * wz; 235 final double t145 = t3 * t10 * t87 * wz * 2.0; 236 final double t146 = -t91 - t105 + t106 + t144 + t145; 237 final double t147 = t3 * t7 * t86; 238 final double t156 = t3 * t9 * t11; 239 final double t148 = -t95 + t97 + t98 + t147 - t156; 240 final double t149 = t10 * t29 * t87 * wy * 2.0; 241 final double t150 = t7 * t29 * t86 * wy; 242 final double t151 = t149 + t150; 243 final double t152 = t9 * t11 * wy * wz; 244 final double t153 = t3 * t7 * t86 * wx; 245 final double t154 = t3 * t10 * t87 * wx * 2.0; 246 final double t155 = t7 * t86 * wy * wz; 247 final double t157 = Y * t146; 248 final double t158 = X * t148; 249 final double t159 = t157 + t158; 250 final double t161 = t10 * t11 * wx; 251 final double t160 = t152 + t153 + t154 - t155 - t161; 252 final double t162 = beta * t160; 253 final double t163 = t148 * vc; 254 final double t164 = t162 + t163; 255 final double t165 = X * t164; 256 final double t166 = beta * t151; 257 final double t267 = t146 * vc; 258 final double t167 = t166 - t267; 259 final double t268 = Y * t167; 260 final double t168 = t165 - t268; 261 final double t169 = t21 * t168; 262 final double t269 = t70 * t110 * t159; 263 final double t170 = t169 - t269; 264 final double t171 = t56 * t59 * t170 * 2.0; 265 final double t172 = -t152 + t153 + t154 + t155 - t161; 266 final double t173 = alpha * t172; 267 final double t174 = t146 * uc; 268 final double t189 = gamma * t151; 269 final double t175 = t173 + t174 - t189; 270 final double t176 = Y * t175; 271 final double t177 = t10 * t23 * t87 * wy * 2.0; 272 final double t178 = t7 * t23 * t86 * wy; 273 final double t190 = t10 * t11 * wy * 2.0; 274 final double t179 = t177 + t178 - t190; 275 final double t180 = gamma * t160; 276 final double t181 = t148 * uc; 277 final double t191 = alpha * t179; 278 final double t182 = t180 + t181 - t191; 279 final double t183 = X * t182; 280 final double t184 = t176 + t183; 281 final double t185 = t21 * t184; 282 final double t192 = t46 * t110 * t159; 283 final double t186 = t185 - t192; 284 final double t187 = t38 * t51 * t186 * 2.0; 285 final double t188 = t171 + t187; 286 final double t193 = t4 * t9 * t11; 287 final double t194 = t4 * t7 * t86 * wx; 288 final double t195 = t4 * t10 * t87 * wx * 2.0; 289 final double t196 = -t152 + t155 - t161 + t194 + t195; 290 final double t197 = t4 * t7 * t86; 291 final double t198 = t10 * t29 * t87 * wz * 2.0; 292 final double t199 = t7 * t29 * t86 * wz; 293 final double t204 = t10 * t11 * wz * 2.0; 294 final double t200 = t198 + t199 - t204; 295 final double t201 = t4 * t7 * t86 * wy; 296 final double t202 = t4 * t10 * t87 * wy * 2.0; 297 final double t203 = t88 - t104 - t117 + t201 + t202; 298 final double t205 = t10 * t23 * t87 * wz * 2.0; 299 final double t206 = t7 * t23 * t86 * wz; 300 final double t207 = t196 * uc; 301 final double t208 = t95 + t97 + t98 + t193 - t197; 302 final double t209 = t203 * uc; 303 final double t210 = -t95 + t97 + t98 - t193 + t197; 304 final double t211 = alpha * t210; 305 final double t231 = gamma * t200; 306 final double t212 = t209 + t211 - t231; 307 final double t213 = Y * t212; 308 final double t214 = X * t196; 309 final double t215 = Y * t203; 310 final double t216 = t214 + t215; 311 final double t217 = t196 * vc; 312 final double t218 = beta * t208; 313 final double t219 = t217 + t218; 314 final double t220 = X * t219; 315 final double t221 = beta * t200; 316 final double t273 = t203 * vc; 317 final double t222 = t221 - t273; 318 final double t274 = Y * t222; 319 final double t223 = t220 - t274; 320 final double t224 = t21 * t223; 321 final double t275 = t70 * t110 * t216; 322 final double t225 = t224 - t275; 323 final double t226 = t56 * t59 * t225 * 2.0; 324 final double t227 = -t204 + t205 + t206; 325 final double t228 = gamma * t208; 326 final double t237 = alpha * t227; 327 final double t229 = t207 + t228 - t237; 328 final double t230 = X * t229; 329 final double t232 = t213 + t230; 330 final double t233 = t21 * t232; 331 final double t238 = t46 * t110 * t216; 332 final double t234 = t233 - t238; 333 final double t235 = t38 * t51 * t234 * 2.0; 334 final double t236 = t226 + t235; 335 final double t239 = 1.0 / alpha; 336 final double t240 = 1.0 / beta; 337 final double t241 = t21 * t56 * t240 * 2.0; 338 final double t242 = gamma * t21 * t38 * t51 * 2.0; 339 final double t243 = t241 + t242; 340 final double t244 = t21 * uc; 341 final double t248 = t46 * t110; 342 final double t245 = t244 - t248; 343 final double t246 = t70 * t110; 344 final double t285 = t21 * vc; 345 final double t247 = t246 - t285; 346 final double t249 = t38 * t51 * t245 * 2.0; 347 final double t286 = t56 * t59 * t247 * 2.0; 348 final double t250 = t249 - t286; 349 final double t251 = k0 * t55; 350 final double t252 = k1 * t55 * t74 * 2.0; 351 final double t253 = t251 + t252; 352 final double t254 = k0 * t81; 353 final double t255 = k1 * t74 * t81 * 2.0; 354 final double t256 = t254 + t255; 355 final double t257 = t21 * t79; 356 final double t258 = t21 * t79 * t85; 357 final double t259 = k0 * t21 * t38 * t51 * t79 * 2.0; 358 final double t260 = k1 * t21 * t38 * t51 * t74 * t79 * 4.0; 359 final double t261 = t259 + t260; 360 final double t265 = k0 * t140; 361 final double t266 = k1 * t74 * t140 * 2.0; 362 final double t270 = k0 * t188; 363 final double t271 = k1 * t74 * t188 * 2.0; 364 final double t272 = t270 + t271; 365 final double t276 = k0 * t236; 366 final double t277 = k1 * t74 * t236 * 2.0; 367 final double t278 = t276 + t277; 368 final double t279 = k0 * t21 * t38 * t239 * 2.0; 369 final double t280 = k1 * t21 * t38 * t74 * t239 * 4.0; 370 final double t281 = t279 + t280; 371 final double t282 = k0 * t243; 372 final double t283 = k1 * t74 * t243 * 2.0; 373 final double t284 = t282 + t283; 374 final double t287 = k0 * t250; 375 final double t288 = k1 * t74 * t250 * 2.0; 376 final double t289 = t287 + t288; 377 // alpha 378 A0[0][0] = t21 * t53 + t253 379 * (uc - t21 * (t34 + t35 + t36 + t37 + X * (t26 - t14 * uc + gamma * (t15 - t10 * t11 * wx * wy)))) 380 + t21 * t53 * t85; 381 // beta 382 A0[0][1] = t38 * t256; 383 // uc 384 A0[0][3] = 1.0; 385 // vc 386 A0[0][4] = 0.0; 387 // gamma 388 A0[0][2] = t257 + t258 + t38 * t261; 389 // k1 390 A0[0][5] = -t38 * t74; 391 // k2 392 A0[0][6] = -t38 * t83; 393 A0[0][7] = t137 394 - t143 395 + t38 396 * (t265 + t266) 397 + t85 398 * (t21 399 * (X * (t115 - alpha * t113 + gamma * (t88 + t89 + t90 - t10 * t11 * wy - t7 * t86 * wx * wz)) + Y 400 * (t118 + alpha * (-t88 + t89 + t90 + t104 - t10 * t11 * wy) - gamma * t102)) - t46 * t109 401 * t110); 402 A0[0][8] = t185 - t192 + t85 * t186 + t38 * t272; 403 A0[0][9] = -t238 404 + t38 405 * t278 406 + t85 407 * t234 408 + t21 409 * (t213 + X 410 * (t207 + gamma * (t95 + t97 + t98 + t193 - t4 * t7 * t86) - alpha 411 * (t205 + t206 - t10 * t11 * wz * 2.0))); 412 A0[0][10] = alpha * t21 + t38 * t281 + alpha * t21 * t85; 413 A0[0][11] = gamma * t21 + t38 * t284 + gamma * t21 * t85; 414 A0[0][12] = t244 - t46 * t110 + t38 * t289 + t85 * t245; 415 416 // alpha 417 A0[1][0] = t56 * t253; 418 // beta 419 A0[1][1] = t257 + t258 + t56 * t256; 420 // uc 421 A0[1][3] = 0.0; 422 // vc 423 A0[1][4] = 1.0; 424 // gamma 425 A0[1][2] = t56 * t261; 426 // k1 427 A0[1][5] = -t56 * t74; 428 // k2 429 A0[1][6] = -t56 * t83; 430 431 A0[1][7] = -t126 - t127 + t56 * (t265 + t266) - t85 * t128; 432 A0[1][8] = t169 - t269 + t85 * t170 + t56 * t272; 433 A0[1][9] = t224 - t275 + t85 * t225 + t56 * t278; 434 A0[1][10] = t56 * t281; 435 A0[1][11] = beta * t21 + t56 * t284 + beta * t21 * t85; 436 A0[1][12] = -t246 + t285 - t85 * t247 + t56 * t289; 437 // end of matlab code 438 439 final double[][] Jij = new double[2][camParLength + viewParLength * M]; 440 System.arraycopy(A0[0], 0, Jij[0], 0, camParLength); 441 System.arraycopy(A0[1], 0, Jij[1], 0, camParLength); 442 System.arraycopy(A0[0], 7, Jij[0], camParLength + i * viewParLength, viewParLength); 443 System.arraycopy(A0[1], 7, Jij[1], camParLength + i * viewParLength, viewParLength); 444 //System.out.format("**** Jij = %d / %d\n", Jij.length, Jij[0].length); 445 return Jij; 446 } 447 } 448 449}