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.image.camera.calibration; 031 032import static java.lang.Math.cos; 033import static java.lang.Math.pow; 034import static java.lang.Math.sin; 035import static java.lang.Math.sqrt; 036 037import java.util.ArrayList; 038import java.util.List; 039 040import org.apache.commons.math3.analysis.MultivariateMatrixFunction; 041import org.apache.commons.math3.analysis.MultivariateVectorFunction; 042import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory; 043import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum; 044import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer; 045import org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction; 046import org.apache.commons.math3.linear.ArrayRealVector; 047import org.apache.commons.math3.linear.RealVector; 048import org.openimaj.image.camera.Camera; 049import org.openimaj.image.camera.CameraIntrinsics; 050import org.openimaj.math.geometry.point.Point2d; 051import org.openimaj.math.geometry.transforms.HomographyRefinement; 052import org.openimaj.math.geometry.transforms.TransformUtilities; 053import org.openimaj.util.pair.IndependentPair; 054 055import Jama.Matrix; 056 057public class CameraCalibration extends CameraCalibrationZhang { 058 059 public CameraCalibration(List<List<? extends IndependentPair<? extends Point2d, ? extends Point2d>>> points, 060 int width, int height) 061 { 062 super(points, width, height); 063 } 064 065 @Override 066 protected void performCalibration(int width, int height) { 067 // compute the homographies 068 final List<Matrix> homographies = new ArrayList<Matrix>(); 069 for (int i = 0; i < points.size(); i++) { 070 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data = points.get(i); 071 final Matrix h = HomographyRefinement.SINGLE_IMAGE_TRANSFER.refine( 072 TransformUtilities.homographyMatrixNorm(data), data); 073 074 homographies.add(h); 075 } 076 077 // intial estimate of intrisics and extrinsics 078 estimateIntrisicAndExtrinsics(homographies, width, height); 079 080 // non-linear optimisation using analytic jacobian 081 refine(); 082 } 083 084 /** 085 * This is the implementation of the value function for the optimiser. It 086 * computes the predicted location of an image point by projecting a model 087 * point through the camera homography and then applying the distortion. The 088 * implementation is converted from the C code produced by the following 089 * matlab symbolic code: 090 * 091 * <pre> 092 * <code> 093 * syms u0 v0 fx fy sk real 094 * syms tx ty tz wx wy wz real 095 * syms k1 k2 k3 p1 p2 real 096 * syms X Y real 097 * 098 * % the intrinsic parameter matrix 099 * K=[fx sk u0; 0 fy v0; 0 0 1]; 100 * 101 * % Expression for the rotation matrix based on the Rodrigues formula 102 * theta=sqrt(wx^2+wy^2+wz^2); 103 * omega=[0 -wz wy; wz 0 -wx; -wy wx 0]; 104 * R = eye(3) + (sin(theta)/theta)*omega + ((1-cos(theta))/theta^2)*(omega*omega); 105 * 106 * % Expression for the translation vector 107 * t=[tx;ty;tz]; 108 * 109 * % perspective projection of the model point (X,Y) 110 * uvs=K*[R(:,1) R(:,2) t]*[X; Y; 1]; 111 * u=uvs(1)/uvs(3); 112 * v=uvs(2)/uvs(3); 113 * 114 * % application of 2-term radial distortion 115 * uu0 = u - u0; 116 * vv0 = v - v0; 117 * x = uu0/fx; 118 * y = vv0/fy; 119 * r2 = x*x + y*y; 120 * r4 = r2*r2; 121 * r6 = r2*r2*r2; 122 * uv = [u + uu0*(k1*r2 + k2*r4 + k3*r6) + 2*p1*vv0 + p2*(r2 + 2*uu0^2); 123 * v + vv0*(k1*r2 + k2*r4 + k3*r6) + p1*(r2 + 2*vv0^2) + 2*p2*uu0]; 124 * ccode(uv, 'file', 'calibrate-value.c') 125 * </code> 126 * </pre> 127 * 128 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 129 * 130 */ 131 private class Value implements MultivariateVectorFunction { 132 @Override 133 public double[] value(double[] params) throws IllegalArgumentException { 134 int totalPoints = 0; 135 for (int i = 0; i < points.size(); i++) 136 totalPoints += points.get(i).size(); 137 138 final double[] result = new double[2 * totalPoints]; 139 140 for (int i = 0, k = 0; i < points.size(); i++) { 141 for (int j = 0; j < points.get(i).size(); j++, k++) { 142 final double[] tmp = computeValue(i, j, params); 143 result[k * 2 + 0] = tmp[0]; 144 result[k * 2 + 1] = tmp[1]; 145 } 146 } 147 148 return result; 149 } 150 151 private double[] computeValue(int img, int point, double[] params) { 152 final double[][] A0 = new double[2][1]; 153 154 final double X = points.get(img).get(point).firstObject().getX(); 155 final double Y = points.get(img).get(point).firstObject().getY(); 156 157 final double fx = params[0]; 158 final double fy = params[1]; 159 final double u0 = params[2]; 160 final double v0 = params[3]; 161 final double sk = params[4]; 162 final double k1 = params[5]; 163 final double k2 = params[6]; 164 final double k3 = params[7]; 165 final double p1 = params[8]; 166 final double p2 = params[9]; 167 168 final double wx = params[img * 6 + 10]; 169 final double wy = params[img * 6 + 11]; 170 final double wz = params[img * 6 + 12]; 171 final double tx = params[img * 6 + 13]; 172 final double ty = params[img * 6 + 14]; 173 final double tz = params[img * 6 + 15]; 174 175 // begin matlab code 176 final double t2 = wx * wx; 177 final double t3 = wy * wy; 178 final double t4 = wz * wz; 179 final double t5 = t2 + t3 + t4; 180 final double t6 = sqrt(t5); 181 final double t7 = sin(t6); 182 final double t8 = 1.0 / sqrt(t5); 183 final double t9 = cos(t6); 184 final double t10 = t9 - 1.0; 185 final double t11 = 1.0 / t5; 186 final double t12 = t7 * t8 * wy; 187 final double t13 = t10 * t11 * wx * wz; 188 final double t14 = t12 + t13; 189 final double t15 = t7 * t8 * wz; 190 final double t16 = t7 * t8 * wx; 191 final double t18 = t10 * t11 * wy * wz; 192 final double t17 = t16 - t18; 193 final double t19 = Y * t17; 194 final double t38 = X * t14; 195 final double t20 = t19 - t38 + tz; 196 final double t21 = 1.0 / t20; 197 final double t22 = t10 * t11 * wx * wy; 198 final double t23 = t3 + t4; 199 final double t24 = t10 * t11 * t23; 200 final double t25 = t24 + 1.0; 201 final double t26 = fx * t25; 202 final double t27 = t15 + t22; 203 final double t28 = t17 * u0; 204 final double t29 = t2 + t4; 205 final double t30 = t10 * t11 * t29; 206 final double t31 = t30 + 1.0; 207 final double t32 = sk * t31; 208 final double t43 = fx * t27; 209 final double t33 = t28 + t32 - t43; 210 final double t34 = Y * t33; 211 final double t35 = fx * tx; 212 final double t36 = sk * ty; 213 final double t37 = tz * u0; 214 final double t39 = t15 - t22; 215 final double t40 = sk * t39; 216 final double t48 = t14 * u0; 217 final double t41 = t26 + t40 - t48; 218 final double t42 = X * t41; 219 final double t44 = t34 + t35 + t36 + t37 + t42; 220 final double t49 = t21 * t44; 221 final double t45 = -t49 + u0; 222 final double t53 = fy * ty; 223 final double t54 = fy * t39; 224 final double t55 = t14 * v0; 225 final double t56 = t54 - t55; 226 final double t57 = X * t56; 227 final double t58 = tz * v0; 228 final double t59 = t17 * v0; 229 final double t60 = fy * t31; 230 final double t61 = t59 + t60; 231 final double t62 = Y * t61; 232 final double t63 = t53 + t57 + t58 + t62; 233 final double t64 = t21 * t63; 234 final double t46 = -t64 + v0; 235 final double t47 = 1.0 / (fx * fx); 236 final double t50 = t45 * t45; 237 final double t51 = t47 * t50; 238 final double t52 = 1.0 / (fy * fy); 239 final double t65 = t46 * t46; 240 final double t66 = t52 * t65; 241 final double t67 = t51 + t66; 242 final double t68 = t67 * t67; 243 final double t69 = k1 * t67; 244 final double t70 = k2 * t68; 245 final double t71 = k3 * t67 * t68; 246 final double t72 = t69 + t70 + t71; 247 A0[0][0] = -t45 * t72 + t21 248 * (t34 + t35 + t36 + t37 + X * (t26 - t14 * u0 + sk * (t15 - t10 * t11 * wx * wy))) + p2 249 * (t50 * 2.0 + t51 + t66) + p1 * t45 * t46 * 2.0; 250 A0[1][0] = t64 - t46 * t72 + p1 * (t51 + t65 * 2.0 + t66) + p2 * t45 * t46 * 2.0; 251 252 // end matlab code 253 254 return new double[] { A0[0][0], A0[1][0] }; 255 } 256 } 257 258 /** 259 * This is the implementation of the Jacobian function for the optimiser; it 260 * is the partial derivative of the value function with respect to the 261 * parameters. The implementation is based on the matlab symbolic code: 262 * 263 * <pre> 264 * <code> 265 * syms u0 v0 fx fy sk real 266 * syms tx ty tz wx wy wz real 267 * syms k1 k2 k3 p1 p2 real 268 * syms X Y real 269 * 270 * % the intrinsic parameter matrix 271 * K=[fx sk u0; 0 fy v0; 0 0 1]; 272 * 273 * % Expression for the rotation matrix based on the Rodrigues formula 274 * theta=sqrt(wx^2+wy^2+wz^2); 275 * omega=[0 -wz wy; wz 0 -wx; -wy wx 0]; 276 * R = eye(3) + (sin(theta)/theta)*omega + ((1-cos(theta))/theta^2)*(omega*omega); 277 * 278 * % Expression for the translation vector 279 * t=[tx;ty;tz]; 280 * 281 * % perspective projection of the model point (X,Y) 282 * uvs=K*[R(:,1) R(:,2) t]*[X; Y; 1]; 283 * u=uvs(1)/uvs(3); 284 * v=uvs(2)/uvs(3); 285 * 286 * % application of 2-term radial distortion 287 * uu0 = u - u0; 288 * vv0 = v - v0; 289 * x = uu0/fx; 290 * y = vv0/fy; 291 * r2 = x*x + y*y; 292 * r4 = r2*r2; 293 * r6 = r2*r2*r2; 294 * uv = [u + uu0*(k1*r2 + k2*r4 + k3*r6) + 2*p1*vv0 + p2*(r2 + 2*uu0^2); 295 * v + vv0*(k1*r2 + k2*r4 + k3*r6) + p1*(r2 + 2*vv0^2) + 2*p2*uu0]; 296 * J=jacobian(uv,[fx,fy,u0,v0,sk,k1,k2,k3,p1,p2 wx wy wz tx ty tz]); 297 * ccode(J, 'file', 'calibrate-jacobian.c') 298 * </code> 299 * </pre> 300 * 301 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 302 * 303 */ 304 private class Jacobian implements MultivariateMatrixFunction { 305 @Override 306 public double[][] value(double[] params) { 307 // Note that we're building the jacobian for all cameras/images and 308 // points. The params vector is 10 + 6*numCameras elements long (10 309 // intrinsic params and 6 extrinsic per camera) 310 int totalPoints = 0; 311 for (int i = 0; i < points.size(); i++) 312 totalPoints += points.get(i).size(); 313 314 final double[][] result = new double[2 * totalPoints][]; 315 316 for (int i = 0, k = 0; i < points.size(); i++) { 317 for (int j = 0; j < points.get(i).size(); j++, k++) { 318 final double[][] tmp = computeJacobian(i, j, params); 319 320 result[k * 2 + 0] = tmp[0]; 321 result[k * 2 + 1] = tmp[1]; 322 } 323 } 324 325 return result; 326 } 327 328 private double[][] computeJacobian(int img, int point, double[] params) { 329 final double[][] A0 = new double[2][16]; 330 331 final double X = points.get(img).get(point).firstObject().getX(); 332 final double Y = points.get(img).get(point).firstObject().getY(); 333 334 final double fx = params[0]; 335 final double fy = params[1]; 336 final double u0 = params[2]; 337 final double v0 = params[3]; 338 final double sk = params[4]; 339 final double k1 = params[5]; 340 final double k2 = params[6]; 341 final double k3 = params[7]; 342 final double p1 = params[8]; 343 final double p2 = params[9]; 344 345 final double wx = params[img * 6 + 10]; 346 final double wy = params[img * 6 + 11]; 347 final double wz = params[img * 6 + 12]; 348 final double tx = params[img * 6 + 13]; 349 final double ty = params[img * 6 + 14]; 350 final double tz = params[img * 6 + 15]; 351 352 // begin matlab code 353 final double t2 = wx * wx; 354 final double t3 = wy * wy; 355 final double t4 = wz * wz; 356 final double t5 = t2 + t3 + t4; 357 final double t6 = sqrt(t5); 358 final double t7 = sin(t6); 359 final double t8 = 1.0 / sqrt(t5); 360 final double t9 = cos(t6); 361 final double t10 = t9 - 1.0; 362 final double t11 = 1.0 / t5; 363 final double t12 = t7 * t8 * wy; 364 final double t13 = t10 * t11 * wx * wz; 365 final double t14 = t12 + t13; 366 final double t15 = t7 * t8 * wz; 367 final double t16 = t7 * t8 * wx; 368 final double t18 = t10 * t11 * wy * wz; 369 final double t17 = t16 - t18; 370 final double t19 = Y * t17; 371 final double t39 = X * t14; 372 final double t20 = t19 - t39 + tz; 373 final double t21 = 1.0 / t20; 374 final double t22 = t10 * t11 * wx * wy; 375 final double t23 = t3 + t4; 376 final double t24 = t10 * t11 * t23; 377 final double t25 = t24 + 1.0; 378 final double t26 = fx * t25; 379 final double t27 = t15 + t22; 380 final double t28 = t17 * u0; 381 final double t29 = t2 + t4; 382 final double t30 = t10 * t11 * t29; 383 final double t31 = t30 + 1.0; 384 final double t32 = sk * t31; 385 final double t45 = fx * t27; 386 final double t33 = t28 + t32 - t45; 387 final double t34 = Y * t33; 388 final double t35 = fx * tx; 389 final double t36 = sk * ty; 390 final double t37 = tz * u0; 391 final double t40 = t15 - t22; 392 final double t41 = sk * t40; 393 final double t42 = t14 * u0; 394 final double t43 = t26 + t41 - t42; 395 final double t44 = X * t43; 396 final double t46 = t34 + t35 + t36 + t37 + t44; 397 final double t47 = t21 * t46; 398 final double t38 = -t47 + u0; 399 final double t48 = 1.0 / (fx * fx * fx); 400 final double t49 = t38 * t38; 401 final double t50 = t48 * t49 * 2.0; 402 final double t51 = 1.0 / (fx * fx); 403 final double t52 = X * t25; 404 final double t57 = Y * t27; 405 final double t53 = t52 - t57 + tx; 406 final double t54 = t21 * t38 * t51 * t53 * 2.0; 407 final double t55 = t50 + t54; 408 final double t60 = fy * ty; 409 final double t61 = fy * t40; 410 final double t62 = t14 * v0; 411 final double t63 = t61 - t62; 412 final double t64 = X * t63; 413 final double t65 = tz * v0; 414 final double t66 = t17 * v0; 415 final double t67 = fy * t31; 416 final double t68 = t66 + t67; 417 final double t69 = Y * t68; 418 final double t70 = t60 + t64 + t65 + t69; 419 final double t71 = t21 * t70; 420 final double t56 = -t71 + v0; 421 final double t58 = t49 * t51; 422 final double t59 = 1.0 / (fy * fy); 423 final double t72 = t56 * t56; 424 final double t73 = t59 * t72; 425 final double t74 = t58 + t73; 426 final double t75 = t74 * t74; 427 final double t76 = 1.0 / (fy * fy * fy); 428 final double t77 = t72 * t76 * 2.0; 429 final double t78 = X * t40; 430 final double t79 = Y * t31; 431 final double t80 = t78 + t79 + ty; 432 final double t81 = t21 * t56 * t59 * t80 * 2.0; 433 final double t82 = t77 + t81; 434 final double t83 = k1 * t74; 435 final double t84 = k2 * t75; 436 final double t85 = k3 * t74 * t75; 437 final double t86 = t83 + t84 + t85; 438 final double t87 = 1.0 / pow(t5, 3.0 / 2.0); 439 final double t88 = 1.0 / (t5 * t5); 440 final double t89 = t7 * t87 * wx * wy; 441 final double t90 = t2 * t7 * t87 * wz; 442 final double t91 = t2 * t10 * t88 * wz * 2.0; 443 final double t102 = t10 * t11 * wz; 444 final double t103 = t9 * t11 * wx * wy; 445 final double t92 = t89 + t90 + t91 - t102 - t103; 446 final double t93 = t7 * t8; 447 final double t94 = t2 * t9 * t11; 448 final double t95 = t10 * t88 * wx * wy * wz * 2.0; 449 final double t96 = t7 * t87 * wx * wy * wz; 450 final double t109 = t2 * t7 * t87; 451 final double t97 = t93 + t94 + t95 + t96 - t109; 452 final double t98 = t9 * t11 * wx * wz; 453 final double t99 = t2 * t7 * t87 * wy; 454 final double t100 = t2 * t10 * t88 * wy * 2.0; 455 final double t107 = t10 * t11 * wy; 456 final double t108 = t7 * t87 * wx * wz; 457 final double t101 = t98 + t99 + t100 - t107 - t108; 458 final double t104 = t10 * t29 * t88 * wx * 2.0; 459 final double t105 = t7 * t29 * t87 * wx; 460 final double t114 = t10 * t11 * wx * 2.0; 461 final double t106 = t104 + t105 - t114; 462 final double t110 = X * t92; 463 final double t111 = Y * t97; 464 final double t112 = t110 + t111; 465 final double t113 = 1.0 / (t20 * t20); 466 final double t115 = fy * t106; 467 final double t142 = t97 * v0; 468 final double t116 = t115 - t142; 469 final double t117 = Y * t116; 470 final double t118 = fy * t101; 471 final double t119 = t92 * v0; 472 final double t120 = t118 + t119; 473 final double t143 = X * t120; 474 final double t121 = t117 - t143; 475 final double t122 = t21 * t121; 476 final double t123 = t70 * t112 * t113; 477 final double t124 = t122 + t123; 478 final double t125 = t10 * t23 * t88 * wx * 2.0; 479 final double t126 = t7 * t23 * t87 * wx; 480 final double t127 = t125 + t126; 481 final double t128 = sk * t101; 482 final double t129 = t92 * u0; 483 final double t145 = fx * t127; 484 final double t130 = t128 + t129 - t145; 485 final double t131 = X * t130; 486 final double t132 = -t98 + t99 + t100 - t107 + t108; 487 final double t133 = fx * t132; 488 final double t134 = t97 * u0; 489 final double t146 = sk * t106; 490 final double t135 = t133 + t134 - t146; 491 final double t136 = Y * t135; 492 final double t137 = t131 + t136; 493 final double t138 = t21 * t137; 494 final double t147 = t46 * t112 * t113; 495 final double t139 = t138 - t147; 496 final double t140 = t38 * t51 * t139 * 2.0; 497 final double t144 = t56 * t59 * t124 * 2.0; 498 final double t141 = t140 - t144; 499 final double t148 = t7 * t87 * wy * wz; 500 final double t149 = t3 * t7 * t87 * wx; 501 final double t150 = t3 * t10 * t88 * wx * 2.0; 502 final double t151 = t10 * t29 * t88 * wy * 2.0; 503 final double t152 = t7 * t29 * t87 * wy; 504 final double t153 = t151 + t152; 505 final double t154 = t9 * t11 * wy * wz; 506 final double t155 = t3 * t7 * t87 * wz; 507 final double t156 = t3 * t10 * t88 * wz * 2.0; 508 final double t157 = -t89 - t102 + t103 + t155 + t156; 509 final double t158 = t157 * u0; 510 final double t159 = t10 * t23 * t88 * wy * 2.0; 511 final double t160 = t7 * t23 * t87 * wy; 512 final double t174 = t10 * t11 * wy * 2.0; 513 final double t161 = t159 + t160 - t174; 514 final double t170 = t10 * t11 * wx; 515 final double t162 = -t148 + t149 + t150 + t154 - t170; 516 final double t163 = sk * t162; 517 final double t164 = t3 * t7 * t87; 518 final double t169 = t3 * t9 * t11; 519 final double t165 = -t93 + t95 + t96 + t164 - t169; 520 final double t166 = t165 * u0; 521 final double t175 = fx * t161; 522 final double t167 = t163 + t166 - t175; 523 final double t168 = X * t167; 524 final double t171 = Y * t157; 525 final double t172 = X * t165; 526 final double t173 = t171 + t172; 527 final double t176 = t148 + t149 + t150 - t154 - t170; 528 final double t177 = fx * t176; 529 final double t183 = sk * t153; 530 final double t178 = t158 + t177 - t183; 531 final double t179 = Y * t178; 532 final double t180 = t168 + t179; 533 final double t181 = t21 * t180; 534 final double t184 = t46 * t113 * t173; 535 final double t182 = t181 - t184; 536 final double t185 = fy * t162; 537 final double t186 = t165 * v0; 538 final double t187 = t185 + t186; 539 final double t188 = X * t187; 540 final double t189 = fy * t153; 541 final double t196 = t157 * v0; 542 final double t190 = t189 - t196; 543 final double t197 = Y * t190; 544 final double t191 = t188 - t197; 545 final double t192 = t21 * t191; 546 final double t198 = t70 * t113 * t173; 547 final double t193 = t192 - t198; 548 final double t194 = t56 * t59 * t193 * 2.0; 549 final double t195 = t38 * t51 * t182 * 2.0; 550 final double t199 = t194 + t195; 551 final double t200 = t4 * t9 * t11; 552 final double t201 = t4 * t7 * t87 * wx; 553 final double t202 = t4 * t10 * t88 * wx * 2.0; 554 final double t203 = t148 - t154 - t170 + t201 + t202; 555 final double t204 = t4 * t7 * t87 * wy; 556 final double t205 = t4 * t10 * t88 * wy * 2.0; 557 final double t206 = t98 - t107 - t108 + t204 + t205; 558 final double t207 = t4 * t7 * t87; 559 final double t208 = t10 * t29 * t88 * wz * 2.0; 560 final double t209 = t7 * t29 * t87 * wz; 561 final double t214 = t10 * t11 * wz * 2.0; 562 final double t210 = t208 + t209 - t214; 563 final double t211 = X * t203; 564 final double t212 = Y * t206; 565 final double t213 = t211 + t212; 566 final double t215 = t10 * t23 * t88 * wz * 2.0; 567 final double t216 = t7 * t23 * t87 * wz; 568 final double t217 = t203 * u0; 569 final double t218 = t93 + t95 + t96 + t200 - t207; 570 final double t219 = t206 * u0; 571 final double t220 = -t93 + t95 + t96 - t200 + t207; 572 final double t221 = fx * t220; 573 final double t238 = sk * t210; 574 final double t222 = t219 + t221 - t238; 575 final double t223 = Y * t222; 576 final double t224 = t203 * v0; 577 final double t225 = fy * t218; 578 final double t226 = t224 + t225; 579 final double t227 = X * t226; 580 final double t228 = fy * t210; 581 final double t244 = t206 * v0; 582 final double t229 = t228 - t244; 583 final double t245 = Y * t229; 584 final double t230 = t227 - t245; 585 final double t231 = t21 * t230; 586 final double t246 = t70 * t113 * t213; 587 final double t232 = t231 - t246; 588 final double t233 = t56 * t59 * t232 * 2.0; 589 final double t234 = -t214 + t215 + t216; 590 final double t235 = sk * t218; 591 final double t247 = fx * t234; 592 final double t236 = t217 + t235 - t247; 593 final double t237 = X * t236; 594 final double t239 = t223 + t237; 595 final double t240 = t21 * t239; 596 final double t248 = t46 * t113 * t213; 597 final double t241 = t240 - t248; 598 final double t242 = t38 * t51 * t241 * 2.0; 599 final double t243 = t233 + t242; 600 final double t249 = 1.0 / fx; 601 final double t250 = 1.0 / fy; 602 final double t251 = t21 * t56 * t250 * 2.0; 603 final double t252 = sk * t21 * t38 * t51 * 2.0; 604 final double t253 = t251 + t252; 605 final double t254 = t21 * u0; 606 final double t255 = t70 * t113; 607 final double t260 = t21 * v0; 608 final double t256 = t255 - t260; 609 final double t262 = t46 * t113; 610 final double t257 = t254 - t262; 611 final double t258 = t38 * t51 * t257 * 2.0; 612 final double t261 = t56 * t59 * t256 * 2.0; 613 final double t259 = t258 - t261; 614 final double t263 = k1 * t55; 615 final double t264 = k2 * t55 * t74 * 2.0; 616 final double t265 = k3 * t55 * t75 * 3.0; 617 final double t266 = t263 + t264 + t265; 618 final double t267 = k1 * t82; 619 final double t268 = k3 * t75 * t82 * 3.0; 620 final double t269 = k2 * t74 * t82 * 2.0; 621 final double t270 = t267 + t268 + t269; 622 final double t271 = t21 * t80; 623 final double t272 = t21 * t80 * t86; 624 final double t273 = k1 * t21 * t38 * t51 * t80 * 2.0; 625 final double t274 = k2 * t21 * t38 * t51 * t74 * t80 * 4.0; 626 final double t275 = k3 * t21 * t38 * t51 * t75 * t80 * 6.0; 627 final double t276 = t273 + t274 + t275; 628 final double t277 = t38 * t56 * 2.0; 629 final double t278 = k1 * t141; 630 final double t279 = k2 * t74 * t141 * 2.0; 631 final double t280 = k3 * t75 * t141 * 3.0; 632 final double t281 = k1 * t199; 633 final double t282 = k2 * t74 * t199 * 2.0; 634 final double t283 = k3 * t75 * t199 * 3.0; 635 final double t284 = t281 + t282 + t283; 636 final double t285 = k1 * t243; 637 final double t286 = k2 * t74 * t243 * 2.0; 638 final double t287 = k3 * t75 * t243 * 3.0; 639 final double t288 = t285 + t286 + t287; 640 final double t289 = k1 * t21 * t38 * t249 * 2.0; 641 final double t290 = k2 * t21 * t38 * t74 * t249 * 4.0; 642 final double t291 = k3 * t21 * t38 * t75 * t249 * 6.0; 643 final double t292 = t289 + t290 + t291; 644 final double t293 = k1 * t253; 645 final double t294 = k3 * t75 * t253 * 3.0; 646 final double t295 = k2 * t74 * t253 * 2.0; 647 final double t296 = t293 + t294 + t295; 648 final double t297 = k1 * t259; 649 final double t298 = k2 * t74 * t259 * 2.0; 650 final double t299 = k3 * t75 * t259 * 3.0; 651 final double t300 = t297 + t298 + t299; 652 A0[0][0] = t21 * t53 + t266 653 * (u0 - t21 * (t34 + t35 + t36 + t37 + X * (t26 - t14 * u0 + sk * (t15 - t10 * t11 * wx * wy)))) - p2 654 * (t50 + t54 + t21 * t38 * t53 * 4.0) + t21 * t53 * t86 - p1 * t21 * t53 * t56 * 2.0; 655 A0[0][1] = -p2 * t82 + t38 * t270 - p1 * t21 * t38 * t80 * 2.0; 656 A0[0][2] = 1.0; 657 A0[0][4] = t271 + t272 - p2 * (t21 * t38 * t80 * 4.0 + t21 * t38 * t51 * t80 * 2.0) + t38 * t276 - p1 * t21 658 * t56 * t80 * 2.0; 659 A0[0][5] = -t38 * t74; 660 A0[0][6] = -t38 * t75; 661 A0[0][7] = -t38 * t74 * t75; 662 A0[0][8] = t277; 663 A0[0][9] = t49 * 2.0 + t58 + t73; 664 A0[0][10] = t138 - t147 + t86 * t139 + t38 * (t278 + t279 + t280) - p2 * (t140 - t144 + t38 * t139 * 4.0) 665 + p1 * t38 * t124 * 2.0 - p1 * t56 * t139 * 2.0; 666 A0[0][11] = -t184 + t86 * t182 + t38 * t284 - p2 * (t194 + t195 + t38 * t182 * 4.0) + t21 667 * (t168 + Y * (t158 - sk * t153 + fx * (t148 + t149 + t150 - t10 * t11 * wx - t9 * t11 * wy * wz))) 668 - p1 * t38 * t193 * 2.0 - p1 * t56 * t182 * 2.0; 669 A0[0][12] = t240 670 - t248 671 + t38 672 * t288 673 - p2 674 * (t233 + t242 + t38 * t241 * 4.0) 675 + t86 676 * (t21 677 * (t223 + X 678 * (t217 + sk * (t93 + t95 + t96 + t200 - t4 * t7 * t87) - fx 679 * (t215 + t216 - t10 * t11 * wz * 2.0))) - t46 * t113 * t213) - p1 * t38 680 * t232 * 2.0 - p1 * t56 * t241 * 2.0; 681 A0[0][13] = fx * t21 + t38 * t292 - p2 * (fx * t21 * t38 * 4.0 + t21 * t38 * t249 * 2.0) + fx * t21 * t86 682 - fx * p1 * t21 * t56 * 2.0; 683 A0[0][14] = sk * t21 + t38 * t296 - p2 * (t251 + t252 + sk * t21 * t38 * 4.0) + sk * t21 * t86 - fy * p1 684 * t21 * t38 * 2.0 - p1 * sk * t21 * t56 * 2.0; 685 A0[0][15] = t254 - t46 * t113 + t38 * t300 + t86 * t257 - p2 * (t258 - t261 + t38 * t257 * 4.0) - p1 * t56 686 * t257 * 2.0 + p1 * t38 * (t255 - t260) * 2.0; 687 A0[1][0] = -p1 * t55 + t56 * t266 - p2 * t21 * t53 * t56 * 2.0; 688 A0[1][1] = t271 + t272 + t56 * t270 - p1 * (t77 + t81 + t21 * t56 * t80 * 4.0) - p2 * t21 * t38 * t80 * 2.0; 689 A0[1][3] = 1.0; 690 A0[1][4] = t56 * t276 - p2 * t21 * t56 * t80 * 2.0 - p1 * t21 * t38 * t51 * t80 * 2.0; 691 A0[1][5] = -t56 * t74; 692 A0[1][6] = -t56 * t75; 693 A0[1][7] = -t56 * t74 * t75; 694 A0[1][8] = t58 + t72 * 2.0 + t73; 695 A0[1][9] = t277; 696 A0[1][10] = -t122 - t123 - t86 * t124 + t56 * (t278 + t279 + t280) + p1 * (-t140 + t144 + t56 * t124 * 4.0) 697 + p2 * t38 * t124 * 2.0 - p2 * t56 * t139 * 2.0; 698 A0[1][11] = t192 - t198 + t86 * t193 + t56 * t284 - p1 * (t194 + t195 + t56 * t193 * 4.0) - p2 * t38 * t193 699 * 2.0 - p2 * t56 * t182 * 2.0; 700 A0[1][12] = t231 - t246 + t86 * t232 + t56 * t288 - p1 * (t233 + t242 + t56 * t232 * 4.0) - p2 * t38 * t232 701 * 2.0 - p2 * t56 * t241 * 2.0; 702 A0[1][13] = t56 * t292 - fx * p2 * t21 * t56 * 2.0 - p1 * t21 * t38 * t249 * 2.0; 703 A0[1][14] = fy * t21 + t56 * t296 - p1 * (t251 + t252 + fy * t21 * t56 * 4.0) + fy * t21 * t86 - fy * p2 704 * t21 * t38 * 2.0 - p2 * sk * t21 * t56 * 2.0; 705 A0[1][15] = -t255 + t260 - t86 * t256 + t56 * t300 + p1 * (-t258 + t261 + t56 * t256 * 4.0) - p2 * t56 * t257 706 * 2.0 + p2 * t38 * (t255 - t260) * 2.0; 707 // end matlab code 708 709 final double[][] result = new double[2][10 + 6 * points.size()]; 710 System.arraycopy(A0[0], 0, result[0], 0, 10); 711 System.arraycopy(A0[1], 0, result[1], 0, 10); 712 System.arraycopy(A0[0], 10, result[0], 10 + img * 6, 6); 713 System.arraycopy(A0[1], 10, result[1], 10 + img * 6, 6); 714 715 // result[0][7] = 0; 716 // result[1][7] = 0; 717 // result[0][8] = 0; 718 // result[1][8] = 0; 719 // result[0][9] = 0; 720 // result[1][9] = 0; 721 722 return result; 723 } 724 } 725 726 /** 727 * Stack the observed image locations of the calibration pattern points into 728 * a vector 729 * 730 * @return the observed vector 731 */ 732 @Override 733 protected RealVector buildObservedVector() 734 { 735 int totalPoints = 0; 736 for (int i = 0; i < points.size(); i++) 737 totalPoints += points.get(i).size(); 738 739 final double[] vec = new double[totalPoints * 2]; 740 741 for (int i = 0, k = 0; i < points.size(); i++) { 742 for (int j = 0; j < points.get(i).size(); j++, k++) { 743 vec[k * 2 + 0] = points.get(i).get(j).secondObject().getX(); 744 vec[k * 2 + 1] = points.get(i).get(j).secondObject().getY(); 745 } 746 } 747 748 return new ArrayRealVector(vec, false); 749 } 750 751 /** 752 * Perform Levenburg-Marquardt non-linear optimisation to get better 753 * estimates of the parameters 754 */ 755 private void refine() 756 { 757 final LevenbergMarquardtOptimizer lm = new LevenbergMarquardtOptimizer(); 758 final RealVector start = buildInitialVector(); 759 final RealVector observed = buildObservedVector(); 760 final int maxEvaluations = 1000; 761 final int maxIterations = 1000; 762 763 final MultivariateVectorFunction value = new Value(); 764 final MultivariateMatrixFunction jacobian = new Jacobian(); 765 final MultivariateJacobianFunction model = LeastSquaresFactory.model(value, jacobian); 766 767 final Optimum result = lm.optimize(LeastSquaresFactory.create(model, 768 observed, start, null, maxEvaluations, maxIterations)); 769 770 updateEstimates(result.getPoint()); 771 } 772 773 /** 774 * Extract the data from the optimised parameter vector and put it back into 775 * our camera model 776 * 777 * @param point 778 * the optimised parameter vector 779 */ 780 private void updateEstimates(RealVector point) { 781 final CameraIntrinsics intrinsic = cameras.get(0).intrinsicParameters; 782 783 intrinsic.setFocalLengthX(point.getEntry(0)); 784 intrinsic.setFocalLengthY(point.getEntry(1)); 785 intrinsic.setPrincipalPointX(point.getEntry(2)); 786 intrinsic.setPrincipalPointY(point.getEntry(3)); 787 intrinsic.setSkewFactor(point.getEntry(4)); 788 intrinsic.k1 = point.getEntry(5); 789 intrinsic.k2 = point.getEntry(6); 790 intrinsic.k3 = point.getEntry(7); 791 intrinsic.p1 = point.getEntry(8); 792 intrinsic.p2 = point.getEntry(9); 793 794 for (int i = 0; i < cameras.size(); i++) { 795 final Camera e = cameras.get(i); 796 final double[] rv = new double[] { point.getEntry(i * 6 + 10), point.getEntry(i * 6 + 11), 797 point.getEntry(i * 6 + 12) }; 798 e.rotation = TransformUtilities.rodrigues(rv); 799 800 e.translation.setX(point.getEntry(i * 6 + 13)); 801 e.translation.setY(point.getEntry(i * 6 + 14)); 802 e.translation.setZ(point.getEntry(i * 6 + 15)); 803 } 804 } 805 806 private RealVector buildInitialVector() { 807 final CameraIntrinsics intrinsic = cameras.get(0).intrinsicParameters; 808 809 final double[] vector = new double[10 + cameras.size() * 6]; 810 811 vector[0] = intrinsic.getFocalLengthX(); 812 vector[1] = intrinsic.getFocalLengthY(); 813 vector[2] = intrinsic.getPrincipalPointX(); 814 vector[3] = intrinsic.getPrincipalPointY(); 815 vector[4] = intrinsic.getSkewFactor(); 816 vector[5] = intrinsic.k1; 817 vector[6] = intrinsic.k2; 818 vector[7] = intrinsic.k3; 819 vector[8] = intrinsic.p1; 820 vector[9] = intrinsic.p2; 821 822 for (int i = 0; i < cameras.size(); i++) { 823 final Camera e = cameras.get(i); 824 final double[] rv = TransformUtilities.rodrigues(e.rotation); 825 826 vector[i * 6 + 10] = rv[0]; 827 vector[i * 6 + 11] = rv[1]; 828 vector[i * 6 + 12] = rv[2]; 829 vector[i * 6 + 13] = e.translation.getX(); 830 vector[i * 6 + 14] = e.translation.getY(); 831 vector[i * 6 + 15] = e.translation.getZ(); 832 } 833 834 return new ArrayRealVector(vector, false); 835 } 836}