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.citation.annotation.Reference; 049import org.openimaj.citation.annotation.ReferenceType; 050import org.openimaj.image.camera.Camera; 051import org.openimaj.image.camera.CameraIntrinsics; 052import org.openimaj.math.geometry.point.Point2d; 053import org.openimaj.math.geometry.point.Point3dImpl; 054import org.openimaj.math.geometry.transforms.HomographyRefinement; 055import org.openimaj.math.geometry.transforms.TransformUtilities; 056import org.openimaj.math.matrix.MatrixUtils; 057import org.openimaj.util.array.ArrayUtils; 058import org.openimaj.util.pair.IndependentPair; 059 060import Jama.Matrix; 061 062/** 063 * Implementation of Zhengyou Zhang's camera calibration routine using a planar 064 * calibration pattern. This calibration routine assumes a camera with a 2-term 065 * radial distortion; the third radial distortion term (k3) and tangential terms 066 * (p1, p1) of the {@link CameraIntrinsics} will be set to zero. 067 * 068 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 069 */ 070@Reference( 071 type = ReferenceType.Article, 072 author = { "Zhengyou Zhang" }, 073 title = "A flexible new technique for camera calibration", 074 year = "2000", 075 journal = "Pattern Analysis and Machine Intelligence, IEEE Transactions on", 076 pages = { "1330", "1334" }, 077 month = "Nov", 078 number = "11", 079 volume = "22", 080 customData = { 081 "keywords", 082 "calibration;computer vision;geometry;image sensors;matrix algebra;maximum likelihood estimation;optimisation;3D computer vision;camera calibration;flexible technique;maximum likelihood criterion;planar pattern;radial lens distortion;Calibration;Cameras;Closed-form solution;Computer simulation;Computer vision;Layout;Lenses;Maximum likelihood estimation;Nonlinear distortion;Testing", 083 "doi", "10.1109/34.888718", 084 "ISSN", "0162-8828" 085 }) 086public class CameraCalibrationZhang { 087 protected List<List<? extends IndependentPair<? extends Point2d, ? extends Point2d>>> points; 088 protected List<Camera> cameras; 089 090 /** 091 * Calibrate a camera using Zhang's method based on the given model-image 092 * point pairs across a number of images. The model points are in the world 093 * coordinate system and assumed to be on the Z=0 plane. 094 * 095 * @param points 096 * the pairs of model-image points to calibrate the camera with 097 * @param width 098 * the image width of the camera in pixels 099 * @param height 100 * the image height of the camera in pixels 101 */ 102 public CameraCalibrationZhang(List<List<? extends IndependentPair<? extends Point2d, ? extends Point2d>>> points, 103 int width, int height) 104 { 105 this.points = points; 106 107 performCalibration(width, height); 108 } 109 110 protected void performCalibration(int width, int height) { 111 // compute the homographies 112 final List<Matrix> homographies = new ArrayList<Matrix>(); 113 for (int i = 0; i < points.size(); i++) { 114 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data = points.get(i); 115 final Matrix h = HomographyRefinement.SINGLE_IMAGE_TRANSFER.refine( 116 TransformUtilities.homographyMatrixNorm(data), data); 117 118 homographies.add(h); 119 } 120 121 // intial estimate of intrisics and extrinsics 122 estimateIntrisicAndExtrinsics(homographies, width, height); 123 124 // initial estimate of radial distortion 125 estimateRadialDistortion(); 126 127 // non-linear optimisation using analytic jacobian 128 refine(); 129 } 130 131 /** 132 * Get the computed (extrinsic and intrinsic) camera parameters for all 133 * images used at construction time (in the same order). 134 * 135 * @return the camera parameters for each image 136 */ 137 public List<Camera> getCameras() { 138 return cameras; 139 } 140 141 /** 142 * Get the computed intrinsic parameters calculated during construction. 143 * 144 * @return the intrinsic parameters of the calibrated camera 145 */ 146 public CameraIntrinsics getIntrisics() { 147 return cameras.get(0).intrinsicParameters; 148 } 149 150 private double[] vij(Matrix h, int i, int j) { 151 h = h.transpose(); 152 153 final double[] vij = new double[] { 154 h.get(i, 0) * h.get(j, 0), 155 h.get(i, 0) * h.get(j, 1) + h.get(i, 1) * h.get(j, 0), 156 h.get(i, 1) * h.get(j, 1), 157 h.get(i, 2) * h.get(j, 0) + h.get(i, 0) * h.get(j, 2), 158 h.get(i, 2) * h.get(j, 1) + h.get(i, 1) * h.get(j, 2), 159 h.get(i, 2) * h.get(j, 2) 160 }; 161 162 return vij; 163 } 164 165 /** 166 * Compute the initial estimate of the intrinsics 167 * 168 * @param homographies 169 * the homographies 170 * @param height 171 * @param width 172 * @return the intrinsics 173 */ 174 private CameraIntrinsics estimateIntrinsics(List<Matrix> homographies, int width, int height) { 175 final double[][] V = new double[homographies.size() == 2 ? 5 : 2 * homographies.size()][]; 176 177 for (int i = 0, j = 0; i < homographies.size(); i++, j += 2) { 178 final Matrix h = homographies.get(i); 179 180 V[j] = vij(h, 0, 1); // v12 181 V[j + 1] = ArrayUtils.subtract(vij(h, 0, 0), vij(h, 1, 1)); // v11-v22 182 } 183 184 if (homographies.size() == 2) { 185 V[V.length - 1] = new double[] { 0, 1, 0, 0, 0, 0 }; 186 } 187 188 final double[] b = MatrixUtils.solveHomogeneousSystem(V); 189 final double v0 = (b[1] * b[3] - b[0] * b[4]) / (b[0] * b[2] - b[1] * b[1]); 190 final double lamda = b[5] - (b[3] * b[3] + v0 * (b[1] * b[3] - b[0] * b[4])) / b[0]; 191 final double alpha = Math.sqrt(lamda / b[0]); 192 final double beta = Math.sqrt(lamda * b[0] / (b[0] * b[2] - b[1] * b[1])); 193 final double gamma = -b[1] * alpha * alpha * beta / lamda; 194 final double u0 = gamma * v0 / beta - b[3] * alpha * alpha / lamda; 195 196 final Matrix A = new Matrix(new double[][] { 197 { alpha, gamma, u0 }, 198 { 0, beta, v0 }, 199 { 0, 0, 1 } 200 }); 201 202 return new CameraIntrinsics(A, width, height); 203 } 204 205 /** 206 * Produce an initial estimate of the radial distortion parameters, and 207 * update the intrinsics 208 */ 209 protected void estimateRadialDistortion() 210 { 211 final CameraIntrinsics ci = cameras.get(0).intrinsicParameters; 212 213 int totalPoints = 0; 214 for (int i = 0; i < points.size(); i++) 215 totalPoints += points.get(i).size(); 216 217 final Matrix D = new Matrix(2 * totalPoints, 2); 218 final Matrix d = new Matrix(2 * totalPoints, 1); 219 220 for (int i = 0, k = 0; i < points.size(); i++) { 221 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> pointPairs = points.get(i); 222 final Matrix idealH = cameras.get(i).computeHomography(); 223 224 for (int j = 0; j < pointPairs.size(); j++, k++) { 225 // model point 226 final Point2d XY = pointPairs.get(j).firstObject(); 227 // transformed ideal point 228 final Point2d uv = XY.transform(idealH); 229 // observed point 230 final Point2d ipt = pointPairs.get(j).secondObject(); 231 232 d.set(k * 2 + 0, 0, ipt.getX() - uv.getX()); 233 d.set(k * 2 + 1, 0, ipt.getY() - uv.getY()); 234 235 final double tmp1 = uv.getX() - ci.getPrincipalPointX(); // u-u0 236 final double tmp2 = uv.getY() - ci.getPrincipalPointY(); // v-v0 237 final double x = tmp1 / ci.getFocalLengthX(); // (u-u0)/fx 238 final double y = tmp2 / ci.getFocalLengthY(); // (v-v0)/fy 239 final double r2 = x * x + y * y; 240 final double r4 = r2 * r2; 241 242 D.set(k * 2 + 0, 0, tmp1 * r2); 243 D.set(k * 2 + 0, 1, tmp1 * r4); 244 D.set(k * 2 + 1, 0, tmp2 * r2); 245 D.set(k * 2 + 1, 1, tmp2 * r4); 246 } 247 } 248 249 final Matrix result = D.solve(d); 250 ci.k1 = result.get(0, 0); 251 ci.k2 = result.get(1, 0); 252 ci.k3 = 0; 253 } 254 255 /** 256 * Compute the initial estimate of the intrinsic parameters and then the 257 * extrinsic parameters assuming zero distortion. 258 * 259 * @param homographies 260 * the homographies 261 * @param height 262 * @param width 263 */ 264 protected void estimateIntrisicAndExtrinsics(List<Matrix> homographies, int width, int height) { 265 cameras = new ArrayList<Camera>(homographies.size()); 266 final CameraIntrinsics intrinsic = estimateIntrinsics(homographies, width, height); 267 268 for (int i = 0; i < homographies.size(); i++) { 269 cameras.add(estimateExtrinsics(homographies.get(i), intrinsic)); 270 } 271 } 272 273 /** 274 * Estimate the extrinsic parameters for a single camera given its 275 * homography and intrinsic parameters. 276 * 277 * @param h 278 * the homography 279 * @param intrinsic 280 * the intrinsic parameters 281 * @return the extrinsic parameters 282 */ 283 private Camera estimateExtrinsics(Matrix h, CameraIntrinsics intrinsic) { 284 final Matrix Ainv = intrinsic.calibrationMatrix.inverse(); 285 final Matrix h1 = h.getMatrix(0, 2, 0, 0); 286 final Matrix h2 = h.getMatrix(0, 2, 1, 1); 287 final Matrix h3 = h.getMatrix(0, 2, 2, 2); 288 289 final Matrix r1 = Ainv.times(h1); 290 final double lamda = 1 / r1.norm2(); 291 MatrixUtils.times(r1, lamda); 292 293 final Matrix r2 = Ainv.times(h2); 294 MatrixUtils.times(r2, lamda); 295 296 final Matrix r3 = new Matrix(new double[][] { 297 { r1.get(1, 0) * r2.get(2, 0) - r1.get(2, 0) * r2.get(1, 0) }, 298 { r1.get(2, 0) * r2.get(0, 0) - r1.get(0, 0) * r2.get(2, 0) }, 299 { r1.get(0, 0) * r2.get(1, 0) - r1.get(1, 0) * r2.get(0, 0) } 300 }); 301 302 final Matrix R = TransformUtilities.approximateRotationMatrix(MatrixUtils.hstack(r1, r2, r3)); 303 304 final Matrix t = Ainv.times(h3); 305 MatrixUtils.times(t, lamda); 306 307 final Camera ce = new Camera(); 308 ce.intrinsicParameters = intrinsic; 309 ce.rotation = R; 310 ce.translation = new Point3dImpl(t.getColumnPackedCopy()); 311 312 return ce; 313 } 314 315 /** 316 * This is the implementation of the value function for the optimiser. It 317 * computes the predicted location of an image point by projecting a model 318 * point through the camera homography and then applying the distortion. The 319 * implementation is converted from the C code produced by the following 320 * matlab symbolic code: 321 * 322 * <pre> 323 * <code> 324 * syms u0 v0 fx fy sk real 325 * syms tx ty tz wx wy wz real 326 * syms k1 k2 real 327 * syms X Y real 328 * 329 * % the intrinsic parameter matrix 330 * K=[fx sk u0; 0 fy v0; 0 0 1]; 331 * 332 * % Expression for the rotation matrix based on the Rodrigues formula 333 * theta=sqrt(wx^2+wy^2+wz^2); 334 * omega=[0 -wz wy; wz 0 -wx; -wy wx 0]; 335 * R = eye(3) + (sin(theta)/theta)*omega + ((1-cos(theta))/theta^2)*(omega*omega); 336 * 337 * % Expression for the translation vector 338 * t=[tx;ty;tz]; 339 * 340 * % perspective projection of the model point (X,Y) 341 * uvs=K*[R(:,1) R(:,2) t]*[X; Y; 1]; 342 * u=uvs(1)/uvs(3); 343 * v=uvs(2)/uvs(3); 344 * 345 * % application of 2-term radial distortion 346 * uu0 = u - u0; 347 * vv0 = v - v0; 348 * x = uu0/fx; 349 * y = vv0/fy; 350 * r2 = x*x + y*y; 351 * r4 = r2*r2; 352 * uv = [u + uu0*(k1*r2 + k2*r4); v + vv0*(k1*r2 + k2*r4)]; 353 * ccode(uv, 'file', 'zhang-value.c') 354 * </code> 355 * </pre> 356 * 357 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 358 * 359 */ 360 private class Value implements MultivariateVectorFunction { 361 @Override 362 public double[] value(double[] params) throws IllegalArgumentException { 363 int totalPoints = 0; 364 for (int i = 0; i < points.size(); i++) 365 totalPoints += points.get(i).size(); 366 367 final double[] result = new double[2 * totalPoints]; 368 369 for (int i = 0, k = 0; i < points.size(); i++) { 370 for (int j = 0; j < points.get(i).size(); j++, k++) { 371 final double[] tmp = computeValue(i, j, params); 372 result[k * 2 + 0] = tmp[0]; 373 result[k * 2 + 1] = tmp[1]; 374 } 375 } 376 377 return result; 378 } 379 380 private double[] computeValue(int img, int point, double[] params) { 381 final double[][] A0 = new double[2][1]; 382 383 final double X = points.get(img).get(point).firstObject().getX(); 384 final double Y = points.get(img).get(point).firstObject().getY(); 385 386 final double fx = params[0]; 387 final double fy = params[1]; 388 final double u0 = params[2]; 389 final double v0 = params[3]; 390 final double sk = params[4]; 391 final double k1 = params[5]; 392 final double k2 = params[6]; 393 394 final double wx = params[img * 6 + 7]; 395 final double wy = params[img * 6 + 8]; 396 final double wz = params[img * 6 + 9]; 397 final double tx = params[img * 6 + 10]; 398 final double ty = params[img * 6 + 11]; 399 final double tz = params[img * 6 + 12]; 400 401 // begin matlab code 402 final double t2 = wx * wx; 403 final double t3 = wy * wy; 404 final double t4 = wz * wz; 405 final double t5 = t2 + t3 + t4; 406 final double t6 = sqrt(t5); 407 final double t7 = sin(t6); 408 final double t8 = 1.0 / sqrt(t5); 409 final double t9 = cos(t6); 410 final double t10 = t9 - 1.0; 411 final double t11 = 1.0 / t5; 412 final double t12 = t7 * t8 * wy; 413 final double t13 = t10 * t11 * wx * wz; 414 final double t14 = t12 + t13; 415 final double t15 = t7 * t8 * wz; 416 final double t16 = t7 * t8 * wx; 417 final double t18 = t10 * t11 * wy * wz; 418 final double t17 = t16 - t18; 419 final double t19 = Y * t17; 420 final double t39 = X * t14; 421 final double t20 = t19 - t39 + tz; 422 final double t21 = 1.0 / t20; 423 final double t22 = t10 * t11 * wx * wy; 424 final double t23 = t3 + t4; 425 final double t24 = t10 * t11 * t23; 426 final double t25 = t24 + 1.0; 427 final double t26 = fx * t25; 428 final double t27 = t15 + t22; 429 final double t28 = t17 * u0; 430 final double t29 = t2 + t4; 431 final double t30 = t10 * t11 * t29; 432 final double t31 = t30 + 1.0; 433 final double t32 = sk * t31; 434 final double t47 = fx * t27; 435 final double t33 = t28 + t32 - t47; 436 final double t34 = Y * t33; 437 final double t35 = fx * tx; 438 final double t36 = sk * ty; 439 final double t37 = tz * u0; 440 final double t40 = t15 - t22; 441 final double t43 = sk * t40; 442 final double t44 = t14 * u0; 443 final double t45 = t26 + t43 - t44; 444 final double t46 = X * t45; 445 final double t48 = t34 + t35 + t36 + t37 + t46; 446 final double t49 = t21 * t48; 447 final double t38 = -t49 + u0; 448 final double t53 = fy * ty; 449 final double t54 = fy * t40; 450 final double t55 = t14 * v0; 451 final double t56 = t54 - t55; 452 final double t57 = X * t56; 453 final double t58 = tz * v0; 454 final double t59 = t17 * v0; 455 final double t60 = fy * t31; 456 final double t61 = t59 + t60; 457 final double t62 = Y * t61; 458 final double t63 = t53 + t57 + t58 + t62; 459 final double t64 = t21 * t63; 460 final double t41 = -t64 + v0; 461 final double t42 = 1.0 / (fx * fx); 462 final double t50 = t38 * t38; 463 final double t51 = t42 * t50; 464 final double t52 = 1.0 / (fy * fy); 465 final double t65 = t41 * t41; 466 final double t66 = t52 * t65; 467 final double t67 = t51 + t66; 468 final double t68 = k1 * t67; 469 final double t69 = t67 * t67; 470 final double t70 = k2 * t69; 471 final double t71 = t68 + t70; 472 A0[0][0] = -t38 * t71 + t21 473 * (t34 + t35 + t36 + t37 + X * (t26 - t14 * u0 + sk * (t15 - t10 * t11 * wx * wy))); 474 A0[1][0] = t64 - t41 * t71; 475 // end matlab code 476 477 return new double[] { A0[0][0], A0[1][0] }; 478 } 479 } 480 481 /** 482 * This is the implementation of the Jacobian function for the optimiser; it 483 * is the partial derivative of the value function with respect to the 484 * parameters. The implementation is based on the matlab symbolic code: 485 * 486 * <pre> 487 * <code> 488 * syms u0 v0 fx fy sk real 489 * syms tx ty tz wx wy wz real 490 * syms k1 k2 real 491 * syms X Y real 492 * 493 * % the intrinsic parameter matrix 494 * K=[fx sk u0; 0 fy v0; 0 0 1]; 495 * 496 * % Expression for the rotation matrix based on the Rodrigues formula 497 * theta=sqrt(wx^2+wy^2+wz^2); 498 * omega=[0 -wz wy; wz 0 -wx; -wy wx 0]; 499 * R = eye(3) + (sin(theta)/theta)*omega + ((1-cos(theta))/theta^2)*(omega*omega); 500 * 501 * % Expression for the translation vector 502 * t=[tx;ty;tz]; 503 * 504 * % perspective projection of the model point (X,Y) 505 * uvs=K*[R(:,1) R(:,2) t]*[X; Y; 1]; 506 * u=uvs(1)/uvs(3); 507 * v=uvs(2)/uvs(3); 508 * 509 * % application of 2-term radial distortion 510 * uu0 = u - u0; 511 * vv0 = v - v0; 512 * x = uu0/fx; 513 * y = vv0/fy; 514 * r2 = x*x + y*y; 515 * r4 = r2*r2; 516 * uv = [u + uu0*(k1*r2 + k2*r4); v + vv0*(k1*r2 + k2*r4)]; 517 * J=jacobian(uv,[fx,fy,u0,v0,sk,k1,k2, wx wy wz tx ty tz]); 518 * ccode(J, 'file', 'zhang-jacobian.c') 519 * </code> 520 * </pre> 521 * 522 * @author Jonathon Hare (jsh2@ecs.soton.ac.uk) 523 * 524 */ 525 private class Jacobian implements MultivariateMatrixFunction { 526 @Override 527 public double[][] value(double[] params) { 528 // Note that we're building the jacobian for all cameras/images and 529 // points. The params vector is 7 + 6*numCameras elements long (7 530 // intrinsic params and 6 extrinsic per camera) 531 int totalPoints = 0; 532 for (int i = 0; i < points.size(); i++) 533 totalPoints += points.get(i).size(); 534 535 final double[][] result = new double[2 * totalPoints][]; 536 537 for (int i = 0, k = 0; i < points.size(); i++) { 538 for (int j = 0; j < points.get(i).size(); j++, k++) { 539 final double[][] tmp = computeJacobian(i, j, params); 540 541 result[k * 2 + 0] = tmp[0]; 542 result[k * 2 + 1] = tmp[1]; 543 } 544 } 545 546 return result; 547 } 548 549 private double[][] computeJacobian(int img, int point, double[] params) { 550 final double[][] A0 = new double[2][13]; 551 552 final double X = points.get(img).get(point).firstObject().getX(); 553 final double Y = points.get(img).get(point).firstObject().getY(); 554 555 final double fx = params[0]; 556 final double fy = params[1]; 557 final double u0 = params[2]; 558 final double v0 = params[3]; 559 final double sk = params[4]; 560 final double k1 = params[5]; 561 final double k2 = params[6]; 562 563 final double wx = params[img * 6 + 7]; 564 final double wy = params[img * 6 + 8]; 565 final double wz = params[img * 6 + 9]; 566 final double tx = params[img * 6 + 10]; 567 final double ty = params[img * 6 + 11]; 568 final double tz = params[img * 6 + 12]; 569 570 // begin matlab code 571 final double t2 = wx * wx; 572 final double t3 = wy * wy; 573 final double t4 = wz * wz; 574 final double t5 = t2 + t3 + t4; 575 final double t6 = sqrt(t5); 576 final double t7 = sin(t6); 577 final double t8 = 1.0 / sqrt(t5); 578 final double t9 = cos(t6); 579 final double t10 = t9 - 1.0; 580 final double t11 = 1.0 / t5; 581 final double t12 = t7 * t8 * wy; 582 final double t13 = t10 * t11 * wx * wz; 583 final double t14 = t12 + t13; 584 final double t15 = t7 * t8 * wz; 585 final double t16 = t7 * t8 * wx; 586 final double t18 = t10 * t11 * wy * wz; 587 final double t17 = t16 - t18; 588 final double t19 = Y * t17; 589 final double t39 = X * t14; 590 final double t20 = t19 - t39 + tz; 591 final double t21 = 1.0 / t20; 592 final double t22 = t10 * t11 * wx * wy; 593 final double t23 = t3 + t4; 594 final double t24 = t10 * t11 * t23; 595 final double t25 = t24 + 1.0; 596 final double t26 = fx * t25; 597 final double t27 = t15 + t22; 598 final double t28 = t17 * u0; 599 final double t29 = t2 + t4; 600 final double t30 = t10 * t11 * t29; 601 final double t31 = t30 + 1.0; 602 final double t32 = sk * t31; 603 final double t45 = fx * t27; 604 final double t33 = t28 + t32 - t45; 605 final double t34 = Y * t33; 606 final double t35 = fx * tx; 607 final double t36 = sk * ty; 608 final double t37 = tz * u0; 609 final double t40 = t15 - t22; 610 final double t41 = sk * t40; 611 final double t42 = t14 * u0; 612 final double t43 = t26 + t41 - t42; 613 final double t44 = X * t43; 614 final double t46 = t34 + t35 + t36 + t37 + t44; 615 final double t47 = t21 * t46; 616 final double t38 = -t47 + u0; 617 final double t48 = 1.0 / (fx * fx * fx); 618 final double t49 = t38 * t38; 619 final double t50 = t48 * t49 * 2.0; 620 final double t51 = 1.0 / (fx * fx); 621 final double t52 = X * t25; 622 final double t57 = Y * t27; 623 final double t53 = t52 - t57 + tx; 624 final double t54 = t21 * t38 * t51 * t53 * 2.0; 625 final double t55 = t50 + t54; 626 final double t60 = fy * ty; 627 final double t61 = fy * t40; 628 final double t62 = t14 * v0; 629 final double t63 = t61 - t62; 630 final double t64 = X * t63; 631 final double t65 = tz * v0; 632 final double t66 = t17 * v0; 633 final double t67 = fy * t31; 634 final double t68 = t66 + t67; 635 final double t69 = Y * t68; 636 final double t70 = t60 + t64 + t65 + t69; 637 final double t71 = t21 * t70; 638 final double t56 = -t71 + v0; 639 final double t58 = t49 * t51; 640 final double t59 = 1.0 / (fy * fy); 641 final double t72 = t56 * t56; 642 final double t73 = t59 * t72; 643 final double t74 = t58 + t73; 644 final double t75 = 1.0 / (fy * fy * fy); 645 final double t76 = t72 * t75 * 2.0; 646 final double t77 = X * t40; 647 final double t78 = Y * t31; 648 final double t79 = t77 + t78 + ty; 649 final double t80 = t21 * t56 * t59 * t79 * 2.0; 650 final double t81 = t76 + t80; 651 final double t82 = k1 * t74; 652 final double t83 = t74 * t74; 653 final double t84 = k2 * t83; 654 final double t85 = t82 + t84; 655 final double t86 = 1.0 / pow(t5, 3.0 / 2.0); 656 final double t87 = 1.0 / (t5 * t5); 657 final double t88 = t9 * t11 * wx * wz; 658 final double t89 = t2 * t7 * t86 * wy; 659 final double t90 = t2 * t10 * t87 * wy * 2.0; 660 final double t91 = t7 * t86 * wx * wy; 661 final double t92 = t2 * t7 * t86 * wz; 662 final double t93 = t2 * t10 * t87 * wz * 2.0; 663 final double t105 = t10 * t11 * wz; 664 final double t106 = t9 * t11 * wx * wy; 665 final double t94 = t91 + t92 + t93 - t105 - t106; 666 final double t95 = t7 * t8; 667 final double t96 = t2 * t9 * t11; 668 final double t97 = t10 * t87 * wx * wy * wz * 2.0; 669 final double t98 = t7 * t86 * wx * wy * wz; 670 final double t103 = t2 * t7 * t86; 671 final double t99 = t95 + t96 + t97 + t98 - t103; 672 final double t100 = t10 * t29 * t87 * wx * 2.0; 673 final double t101 = t7 * t29 * t86 * wx; 674 final double t116 = t10 * t11 * wx * 2.0; 675 final double t102 = t100 + t101 - t116; 676 final double t104 = t7 * t86 * wx * wz; 677 final double t107 = X * t94; 678 final double t108 = Y * t99; 679 final double t109 = t107 + t108; 680 final double t110 = 1.0 / (t20 * t20); 681 final double t111 = t10 * t23 * t87 * wx * 2.0; 682 final double t112 = t7 * t23 * t86 * wx; 683 final double t113 = t111 + t112; 684 final double t117 = t10 * t11 * wy; 685 final double t114 = t88 + t89 + t90 - t104 - t117; 686 final double t115 = t94 * u0; 687 final double t118 = t99 * u0; 688 final double t119 = fy * t102; 689 final double t262 = t99 * v0; 690 final double t120 = t119 - t262; 691 final double t121 = Y * t120; 692 final double t122 = fy * t114; 693 final double t123 = t94 * v0; 694 final double t124 = t122 + t123; 695 final double t263 = X * t124; 696 final double t125 = t121 - t263; 697 final double t126 = t21 * t125; 698 final double t127 = t70 * t109 * t110; 699 final double t128 = t126 + t127; 700 final double t129 = sk * t114; 701 final double t141 = fx * t113; 702 final double t130 = t115 + t129 - t141; 703 final double t131 = X * t130; 704 final double t132 = -t88 + t89 + t90 + t104 - t117; 705 final double t133 = fx * t132; 706 final double t142 = sk * t102; 707 final double t134 = t118 + t133 - t142; 708 final double t135 = Y * t134; 709 final double t136 = t131 + t135; 710 final double t137 = t21 * t136; 711 final double t143 = t46 * t109 * t110; 712 final double t138 = t137 - t143; 713 final double t139 = t38 * t51 * t138 * 2.0; 714 final double t264 = t56 * t59 * t128 * 2.0; 715 final double t140 = t139 - t264; 716 final double t144 = t3 * t7 * t86 * wz; 717 final double t145 = t3 * t10 * t87 * wz * 2.0; 718 final double t146 = -t91 - t105 + t106 + t144 + t145; 719 final double t147 = t3 * t7 * t86; 720 final double t156 = t3 * t9 * t11; 721 final double t148 = -t95 + t97 + t98 + t147 - t156; 722 final double t149 = t10 * t29 * t87 * wy * 2.0; 723 final double t150 = t7 * t29 * t86 * wy; 724 final double t151 = t149 + t150; 725 final double t152 = t9 * t11 * wy * wz; 726 final double t153 = t3 * t7 * t86 * wx; 727 final double t154 = t3 * t10 * t87 * wx * 2.0; 728 final double t155 = t7 * t86 * wy * wz; 729 final double t157 = Y * t146; 730 final double t158 = X * t148; 731 final double t159 = t157 + t158; 732 final double t161 = t10 * t11 * wx; 733 final double t160 = t152 + t153 + t154 - t155 - t161; 734 final double t162 = fy * t160; 735 final double t163 = t148 * v0; 736 final double t164 = t162 + t163; 737 final double t165 = X * t164; 738 final double t166 = fy * t151; 739 final double t267 = t146 * v0; 740 final double t167 = t166 - t267; 741 final double t268 = Y * t167; 742 final double t168 = t165 - t268; 743 final double t169 = t21 * t168; 744 final double t269 = t70 * t110 * t159; 745 final double t170 = t169 - t269; 746 final double t171 = t56 * t59 * t170 * 2.0; 747 final double t172 = -t152 + t153 + t154 + t155 - t161; 748 final double t173 = fx * t172; 749 final double t174 = t146 * u0; 750 final double t189 = sk * t151; 751 final double t175 = t173 + t174 - t189; 752 final double t176 = Y * t175; 753 final double t177 = t10 * t23 * t87 * wy * 2.0; 754 final double t178 = t7 * t23 * t86 * wy; 755 final double t190 = t10 * t11 * wy * 2.0; 756 final double t179 = t177 + t178 - t190; 757 final double t180 = sk * t160; 758 final double t181 = t148 * u0; 759 final double t191 = fx * t179; 760 final double t182 = t180 + t181 - t191; 761 final double t183 = X * t182; 762 final double t184 = t176 + t183; 763 final double t185 = t21 * t184; 764 final double t192 = t46 * t110 * t159; 765 final double t186 = t185 - t192; 766 final double t187 = t38 * t51 * t186 * 2.0; 767 final double t188 = t171 + t187; 768 final double t193 = t4 * t9 * t11; 769 final double t194 = t4 * t7 * t86 * wx; 770 final double t195 = t4 * t10 * t87 * wx * 2.0; 771 final double t196 = -t152 + t155 - t161 + t194 + t195; 772 final double t197 = t4 * t7 * t86; 773 final double t198 = t10 * t29 * t87 * wz * 2.0; 774 final double t199 = t7 * t29 * t86 * wz; 775 final double t204 = t10 * t11 * wz * 2.0; 776 final double t200 = t198 + t199 - t204; 777 final double t201 = t4 * t7 * t86 * wy; 778 final double t202 = t4 * t10 * t87 * wy * 2.0; 779 final double t203 = t88 - t104 - t117 + t201 + t202; 780 final double t205 = t10 * t23 * t87 * wz * 2.0; 781 final double t206 = t7 * t23 * t86 * wz; 782 final double t207 = t196 * u0; 783 final double t208 = t95 + t97 + t98 + t193 - t197; 784 final double t209 = t203 * u0; 785 final double t210 = -t95 + t97 + t98 - t193 + t197; 786 final double t211 = fx * t210; 787 final double t231 = sk * t200; 788 final double t212 = t209 + t211 - t231; 789 final double t213 = Y * t212; 790 final double t214 = X * t196; 791 final double t215 = Y * t203; 792 final double t216 = t214 + t215; 793 final double t217 = t196 * v0; 794 final double t218 = fy * t208; 795 final double t219 = t217 + t218; 796 final double t220 = X * t219; 797 final double t221 = fy * t200; 798 final double t273 = t203 * v0; 799 final double t222 = t221 - t273; 800 final double t274 = Y * t222; 801 final double t223 = t220 - t274; 802 final double t224 = t21 * t223; 803 final double t275 = t70 * t110 * t216; 804 final double t225 = t224 - t275; 805 final double t226 = t56 * t59 * t225 * 2.0; 806 final double t227 = -t204 + t205 + t206; 807 final double t228 = sk * t208; 808 final double t237 = fx * t227; 809 final double t229 = t207 + t228 - t237; 810 final double t230 = X * t229; 811 final double t232 = t213 + t230; 812 final double t233 = t21 * t232; 813 final double t238 = t46 * t110 * t216; 814 final double t234 = t233 - t238; 815 final double t235 = t38 * t51 * t234 * 2.0; 816 final double t236 = t226 + t235; 817 final double t239 = 1.0 / fx; 818 final double t240 = 1.0 / fy; 819 final double t241 = t21 * t56 * t240 * 2.0; 820 final double t242 = sk * t21 * t38 * t51 * 2.0; 821 final double t243 = t241 + t242; 822 final double t244 = t21 * u0; 823 final double t248 = t46 * t110; 824 final double t245 = t244 - t248; 825 final double t246 = t70 * t110; 826 final double t285 = t21 * v0; 827 final double t247 = t246 - t285; 828 final double t249 = t38 * t51 * t245 * 2.0; 829 final double t286 = t56 * t59 * t247 * 2.0; 830 final double t250 = t249 - t286; 831 final double t251 = k1 * t55; 832 final double t252 = k2 * t55 * t74 * 2.0; 833 final double t253 = t251 + t252; 834 final double t254 = k1 * t81; 835 final double t255 = k2 * t74 * t81 * 2.0; 836 final double t256 = t254 + t255; 837 final double t257 = t21 * t79; 838 final double t258 = t21 * t79 * t85; 839 final double t259 = k1 * t21 * t38 * t51 * t79 * 2.0; 840 final double t260 = k2 * t21 * t38 * t51 * t74 * t79 * 4.0; 841 final double t261 = t259 + t260; 842 final double t265 = k1 * t140; 843 final double t266 = k2 * t74 * t140 * 2.0; 844 final double t270 = k1 * t188; 845 final double t271 = k2 * t74 * t188 * 2.0; 846 final double t272 = t270 + t271; 847 final double t276 = k1 * t236; 848 final double t277 = k2 * t74 * t236 * 2.0; 849 final double t278 = t276 + t277; 850 final double t279 = k1 * t21 * t38 * t239 * 2.0; 851 final double t280 = k2 * t21 * t38 * t74 * t239 * 4.0; 852 final double t281 = t279 + t280; 853 final double t282 = k1 * t243; 854 final double t283 = k2 * t74 * t243 * 2.0; 855 final double t284 = t282 + t283; 856 final double t287 = k1 * t250; 857 final double t288 = k2 * t74 * t250 * 2.0; 858 final double t289 = t287 + t288; 859 A0[0][0] = t21 * t53 + t253 860 * (u0 - t21 * (t34 + t35 + t36 + t37 + X * (t26 - t14 * u0 + sk * (t15 - t10 * t11 * wx * wy)))) 861 + t21 * t53 * t85; 862 A0[0][1] = t38 * t256; 863 A0[0][2] = 1.0; 864 A0[0][4] = t257 + t258 + t38 * t261; 865 A0[0][5] = -t38 * t74; 866 A0[0][6] = -t38 * t83; 867 A0[0][7] = t137 868 - t143 869 + t38 870 * (t265 + t266) 871 + t85 872 * (t21 873 * (X * (t115 - fx * t113 + sk * (t88 + t89 + t90 - t10 * t11 * wy - t7 * t86 * wx * wz)) + Y 874 * (t118 + fx * (-t88 + t89 + t90 + t104 - t10 * t11 * wy) - sk * t102)) - t46 * t109 875 * t110); 876 A0[0][8] = t185 - t192 + t85 * t186 + t38 * t272; 877 A0[0][9] = -t238 878 + t38 879 * t278 880 + t85 881 * t234 882 + t21 883 * (t213 + X 884 * (t207 + sk * (t95 + t97 + t98 + t193 - t4 * t7 * t86) - fx 885 * (t205 + t206 - t10 * t11 * wz * 2.0))); 886 A0[0][10] = fx * t21 + t38 * t281 + fx * t21 * t85; 887 A0[0][11] = sk * t21 + t38 * t284 + sk * t21 * t85; 888 A0[0][12] = t244 - t46 * t110 + t38 * t289 + t85 * t245; 889 A0[1][0] = t56 * t253; 890 A0[1][1] = t257 + t258 + t56 * t256; 891 A0[1][3] = 1.0; 892 A0[1][4] = t56 * t261; 893 A0[1][5] = -t56 * t74; 894 A0[1][6] = -t56 * t83; 895 A0[1][7] = -t126 - t127 + t56 * (t265 + t266) - t85 * t128; 896 A0[1][8] = t169 - t269 + t85 * t170 + t56 * t272; 897 A0[1][9] = t224 - t275 + t85 * t225 + t56 * t278; 898 A0[1][10] = t56 * t281; 899 A0[1][11] = fy * t21 + t56 * t284 + fy * t21 * t85; 900 A0[1][12] = -t246 + t285 - t85 * t247 + t56 * t289; 901 // end matlab code 902 903 final double[][] result = new double[2][7 + 6 * points.size()]; 904 System.arraycopy(A0[0], 0, result[0], 0, 7); 905 System.arraycopy(A0[1], 0, result[1], 0, 7); 906 System.arraycopy(A0[0], 7, result[0], 7 + img * 6, 6); 907 System.arraycopy(A0[1], 7, result[1], 7 + img * 6, 6); 908 909 return result; 910 } 911 } 912 913 /** 914 * Stack the observed image locations of the calibration pattern points into 915 * a vector 916 * 917 * @return the observed vector 918 */ 919 protected RealVector buildObservedVector() 920 { 921 int totalPoints = 0; 922 for (int i = 0; i < points.size(); i++) 923 totalPoints += points.get(i).size(); 924 925 final double[] vec = new double[totalPoints * 2]; 926 927 for (int i = 0, k = 0; i < points.size(); i++) { 928 for (int j = 0; j < points.get(i).size(); j++, k++) { 929 vec[k * 2 + 0] = points.get(i).get(j).secondObject().getX(); 930 vec[k * 2 + 1] = points.get(i).get(j).secondObject().getY(); 931 } 932 } 933 934 return new ArrayRealVector(vec, false); 935 } 936 937 /** 938 * Perform Levenburg-Marquardt non-linear optimisation to get better 939 * estimates of the parameters 940 */ 941 private void refine() 942 { 943 final LevenbergMarquardtOptimizer lm = new LevenbergMarquardtOptimizer(); 944 final RealVector start = buildInitialVector(); 945 final RealVector observed = buildObservedVector(); 946 final int maxEvaluations = 1000; 947 final int maxIterations = 1000; 948 949 final MultivariateVectorFunction value = new Value(); 950 final MultivariateMatrixFunction jacobian = new Jacobian(); 951 final MultivariateJacobianFunction model = LeastSquaresFactory.model(value, jacobian); 952 953 final Optimum result = lm.optimize(LeastSquaresFactory.create(model, 954 observed, start, null, maxEvaluations, maxIterations)); 955 956 updateEstimates(result.getPoint()); 957 } 958 959 /** 960 * Extract the data from the optimised parameter vector and put it back into 961 * our camera model 962 * 963 * @param point 964 * the optimised parameter vector 965 */ 966 private void updateEstimates(RealVector point) { 967 final CameraIntrinsics intrinsic = cameras.get(0).intrinsicParameters; 968 969 intrinsic.setFocalLengthX(point.getEntry(0)); 970 intrinsic.setFocalLengthY(point.getEntry(1)); 971 intrinsic.setPrincipalPointX(point.getEntry(2)); 972 intrinsic.setPrincipalPointY(point.getEntry(3)); 973 intrinsic.setSkewFactor(point.getEntry(4)); 974 intrinsic.k1 = point.getEntry(5); 975 intrinsic.k2 = point.getEntry(6); 976 977 for (int i = 0; i < cameras.size(); i++) { 978 final Camera e = cameras.get(i); 979 final double[] rv = new double[] { point.getEntry(i * 6 + 7), point.getEntry(i * 6 + 8), 980 point.getEntry(i * 6 + 9) }; 981 e.rotation = TransformUtilities.rodrigues(rv); 982 983 e.translation.setX(point.getEntry(i * 6 + 10)); 984 e.translation.setY(point.getEntry(i * 6 + 11)); 985 e.translation.setZ(point.getEntry(i * 6 + 12)); 986 } 987 } 988 989 private RealVector buildInitialVector() { 990 final CameraIntrinsics intrinsic = cameras.get(0).intrinsicParameters; 991 992 final double[] vector = new double[7 + cameras.size() * 6]; 993 994 vector[0] = intrinsic.getFocalLengthX(); 995 vector[1] = intrinsic.getFocalLengthY(); 996 vector[2] = intrinsic.getPrincipalPointX(); 997 vector[3] = intrinsic.getPrincipalPointY(); 998 vector[4] = intrinsic.getSkewFactor(); 999 vector[5] = intrinsic.k1; 1000 vector[6] = intrinsic.k2; 1001 1002 for (int i = 0; i < cameras.size(); i++) { 1003 final Camera e = cameras.get(i); 1004 final double[] rv = TransformUtilities.rodrigues(e.rotation); 1005 1006 vector[i * 6 + 7] = rv[0]; 1007 vector[i * 6 + 8] = rv[1]; 1008 vector[i * 6 + 9] = rv[2]; 1009 vector[i * 6 + 10] = e.translation.getX(); 1010 vector[i * 6 + 11] = e.translation.getY(); 1011 vector[i * 6 + 12] = e.translation.getZ(); 1012 } 1013 1014 return new ArrayRealVector(vector, false); 1015 } 1016 1017 /** 1018 * Compute the average per-pixel error (in pixels) 1019 * 1020 * @return the average per-pixel error 1021 */ 1022 public double calculateError() { 1023 double error = 0; 1024 int nPoints = 0; 1025 1026 for (int i = 0; i < points.size(); i++) { 1027 for (int j = 0; j < points.get(i).size(); j++) { 1028 nPoints++; 1029 final Point2d model = points.get(i).get(j).firstObject(); 1030 final Point2d observed = points.get(i).get(j).secondObject(); 1031 final Point2d predicted = cameras.get(i).project(model); 1032 1033 final float dx = observed.getX() - predicted.getX(); 1034 final float dy = observed.getY() - predicted.getY(); 1035 error += Math.sqrt(dx * dx + dy * dy); 1036 } 1037 } 1038 1039 return error / nPoints; 1040 } 1041}