1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30 package org.openimaj.image.camera.calibration;
31
32 import static java.lang.Math.cos;
33 import static java.lang.Math.pow;
34 import static java.lang.Math.sin;
35 import static java.lang.Math.sqrt;
36
37 import java.util.ArrayList;
38 import java.util.List;
39
40 import org.apache.commons.math3.analysis.MultivariateMatrixFunction;
41 import org.apache.commons.math3.analysis.MultivariateVectorFunction;
42 import org.apache.commons.math3.fitting.leastsquares.LeastSquaresFactory;
43 import org.apache.commons.math3.fitting.leastsquares.LeastSquaresOptimizer.Optimum;
44 import org.apache.commons.math3.fitting.leastsquares.LevenbergMarquardtOptimizer;
45 import org.apache.commons.math3.fitting.leastsquares.MultivariateJacobianFunction;
46 import org.apache.commons.math3.linear.ArrayRealVector;
47 import org.apache.commons.math3.linear.RealVector;
48 import org.openimaj.image.camera.Camera;
49 import org.openimaj.image.camera.CameraIntrinsics;
50 import org.openimaj.math.geometry.point.Point2d;
51 import org.openimaj.math.geometry.transforms.HomographyRefinement;
52 import org.openimaj.math.geometry.transforms.TransformUtilities;
53 import org.openimaj.util.pair.IndependentPair;
54
55 import Jama.Matrix;
56
57 public class CameraCalibration extends CameraCalibrationZhang {
58
59 public CameraCalibration(List<List<? extends IndependentPair<? extends Point2d, ? extends Point2d>>> points,
60 int width, int height)
61 {
62 super(points, width, height);
63 }
64
65 @Override
66 protected void performCalibration(int width, int height) {
67
68 final List<Matrix> homographies = new ArrayList<Matrix>();
69 for (int i = 0; i < points.size(); i++) {
70 final List<? extends IndependentPair<? extends Point2d, ? extends Point2d>> data = points.get(i);
71 final Matrix h = HomographyRefinement.SINGLE_IMAGE_TRANSFER.refine(
72 TransformUtilities.homographyMatrixNorm(data), data);
73
74 homographies.add(h);
75 }
76
77
78 estimateIntrisicAndExtrinsics(homographies, width, height);
79
80
81 refine();
82 }
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
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
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
253
254 return new double[] { A0[0][0], A0[1][0] };
255 }
256 }
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304 private class Jacobian implements MultivariateMatrixFunction {
305 @Override
306 public double[][] value(double[] params) {
307
308
309
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
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
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
716
717
718
719
720
721
722 return result;
723 }
724 }
725
726
727
728
729
730
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
753
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
775
776
777
778
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 }