1 /*******************************************************************************\
2 * *
3 * This file contains routines for calibrating Tsai's 11 parameter camera model. *
4 * The camera model is based on the pin hole model of 3D-2D perspective *
5 * projection with 1st order radial lens distortion. The model consists of *
6 * 5 internal (also called intrinsic or interior) camera parameters: *
7 * *
8 * f - effective focal length of the pin hole camera *
9 * kappa1 - 1st order radial lens distortion coefficient *
10 * Cx, Cy - coordinates of center of radial lens distortion *
11 * (also used as the piercing point of the camera coordinate *
12 * frame's Z axis with the camera's sensor plane) *
13 * sx - uncertainty factor for scale of horizontal scanline *
14 * *
15 * and 6 external (also called extrinsic or exterior) camera parameters: *
16 * *
17 * Rx, Ry, Rz, Tx, Ty, Tz - rotational and translational components of *
18 * the transform between the world's coordinate *
19 * frame and the camera's coordinate frame. *
20 * *
21 * Data for model calibration consists of the (x,y,z) world coordinates of a *
22 * feature point (in mm) and the corresponding coordinates (Xf,Yf) (in pixels) *
23 * of the feature point in the image. Two types of calibration are available: *
24 * *
25 * coplanar - all of the calibration points lie in a single plane *
26 * non-coplanar - the calibration points do not lie in a single plane *
27 * *
28 * This file contains routines for two levels of calibration. The first level *
29 * of calibration is a direct implementation of Tsai's algorithm in which only *
30 * the f, Tz and kappa1 parameters are optimized for. The routines are: *
31 * *
32 * coplanar_calibration () *
33 * noncoplanar_calibration () *
34 * *
35 * The second level of calibration optimizes for everything. This level is *
36 * very slow but provides the most accurate calibration. The routines are: *
37 * *
38 * coplanar_calibration_with_full_optimization () *
39 * noncoplanar_calibration_with_full_optimization () *
40 * *
41 * Routines are also provided for initializing camera parameter variables *
42 * for five of our camera/frame grabber systems. These routines are: *
43 * *
44 * initialize_photometrics_parms () *
45 * initialize_general_imaging_mos5300_matrox_parms () *
46 * initialize_panasonic_gp_mf702_matrox_parms () *
47 * initialize_sony_xc75_matrox_parms () *
48 * initialize_sony_xc77_matrox_parms () *
49 * initialize_sony_xc57_androx_parms () *
50 * *
51 * *
52 * External routines *
53 * ----------------- *
54 * *
55 * Nonlinear optimization for these camera calibration routines is performed *
56 * by the MINPACK lmdif subroutine. lmdif uses a modified Levenberg-Marquardt *
57 * with a jacobian calculated by a forward-difference approximation. *
58 * The MINPACK FORTRAN routines were translated into C generated using f2c. *
59 * *
60 * Matrix operations (inversions, multiplications, etc.) are also provided by *
61 * external routines. *
62 * *
63 * *
64 * Extra notes *
65 * ----------- *
66 * *
67 * An explanation of the basic algorithms and description of the variables *
68 * can be found in several publications, including: *
69 * *
70 * "An Efficient and Accurate Camera Calibration Technique for 3D Machine *
71 * Vision", Roger Y. Tsai, Proceedings of IEEE Conference on Computer Vision *
72 * and Pattern Recognition, Miami Beach, FL, 1986, pages 364-374. *
73 * *
74 * and *
75 * *
76 * "A versatile Camera Calibration Technique for High-Accuracy 3D Machine *
77 * Vision Metrology Using Off-the-Shelf TV Cameras and Lenses", Roger Y. Tsai, *
78 * IEEE Journal of Robotics and Automation, Vol. RA-3, No. 4, August 1987, *
79 * pages 323-344. *
80 * *
81 * *
82 * Notation *
83 * -------- *
84 * *
85 * The camera's X axis runs along increasing column coordinates in the *
86 * image/frame. The Y axis runs along increasing row coordinates. *
87 * *
88 * pix == image/frame grabber picture element *
89 * sel == camera sensor element *
90 * *
91 * Internal routines starting with "cc_" are for coplanar calibration. *
92 * Internal routines starting with "ncc_" are for noncoplanar calibration. *
93 * *
94 * *
95 * History *
96 * ------- *
97 * *
98 * 20-May-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN *
99 * Return the error to lmdif rather than the squared error. *
100 * lmdif calculates the squared error internally during optimization. *
101 * Before this change calibration was essentially optimizing error^4. *
102 * Put transform and evaluation routines into separate files. *
103 * *
104 * 02-Apr-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN *
105 * Rewrite memory allocation to avoid memory alignment problems *
106 * on some machines. *
107 * Strip out IMSL code. MINPACK seems to work fine. *
108 * Filename changes for DOS port. *
109 * *
110 * 04-Jun-94 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN *
111 * Replaced ncc_compute_Xdp_and_Ydp with ncc_compute_Xd_Yd_and_r_squared. *
112 * (effectively propagates the 22-Mar-94 to the non-coplanar routines) *
113 * Added alternate macro definitions for less common math functions. *
114 * *
115 * 25-Mar-94 Torfi Thorhallsson (torfit@verk.hi.is) at the University of Iceland*
116 * Added a new version of the routines: *
117 * cc_compute_exact_f_and_Tz () *
118 * cc_five_parm_optimization_with_late_distortion_removal () *
119 * cc_five_parm_optimization_with_early_distortion_removal () *
120 * cc_nic_optimization () *
121 * cc_full_optimization () *
122 * ncc_compute_exact_f_and_Tz () *
123 * ncc_nic_optimization () *
124 * ncc_full_optimization () *
125 * *
126 * The new routines use the *public domain* MINPACK library for *
127 * optimization instead of the commercial IMSL library. *
128 * To select the new routines, compile this file with the flag -DMINPACK *
129 * *
130 * 22-Mar-94 Torfi Thorhallsson (torfit@verk.hi.is) at the University of Iceland*
131 * Fixed a bug in cc_nic_optimization_error and cc_full_optimization_error.*
132 * A division by cp.sx was missing. *
133 * *
134 * 15-Feb-94 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University *
135 * Included Frederic Devernay's (<Frederic.Devernay@sophia.inria.fr>) *
136 * significantly improved routine for converting from undistorted to *
137 * distorted sensor coordinates. Rather than iteratively solving a *
138 * system of two non-linear equations to perform the conversion, the *
139 * new routine algebraically solves a cubic polynomial in Rd (using *
140 * the Cardan method). *
141 * *
142 * 14-Feb-94 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University *
143 * Fixed a coding bug in ncc_compute_R and ncc_compute_better_R. *
144 * The r4, r5, and r6 terms should not be divided by cp.sx. *
145 * Bug reported by: Volker Rodehorst <vr@cs.tu-berlin.de> *
146 * *
147 * 04-Jul-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University *
148 * Added new routines to evaluate the accuracy of camera calibration. *
149 * *
150 * Added check for coordinate handedness problem in calibration data. *
151 * *
152 * 01-May-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University *
153 * For efficiency the non-linear optimizations are now all based on *
154 * the minimization of squared error in undistorted image coordinates *
155 * instead of the squared error in distorted image coordinates. *
156 * *
157 * New routine for inverse perspective projection. *
158 * *
159 * 14-Feb-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University *
160 * Bug fixes and speed ups. *
161 * *
162 * 07-Feb-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University *
163 * Original implementation. *
164 * *
165 \*******************************************************************************/
166
167 #include <stdlib.h>
168 #include <stdio.h>
169 #include <math.h>
170 #include <errno.h>
171 #include "matrix/matrix.h"
172 #include "cal_main.h"
173 #include "minpack/f2c.h"
174
175
176 /* Variables used by the subroutines for I/O (perhaps not the best way of doing this) */
177 struct camera_parameters cp;
178 struct calibration_data cd;
179 struct calibration_constants cc;
180
181 /* Local working storage */
182 double Xd[MAX_POINTS],
183 Yd[MAX_POINTS],
184 r_squared[MAX_POINTS],
185 U[7];
186
187
188 char camera_type[256] = "unknown";
189
190
191 /************************************************************************/
initialize_photometrics_parms()192 void initialize_photometrics_parms ()
193 {
194 strcpy (camera_type, "Photometrics Star I");
195
196 cp.Ncx = 576; /* [sel] */
197 cp.Nfx = 576; /* [pix] */
198 cp.dx = 0.023; /* [mm/sel] */
199 cp.dy = 0.023; /* [mm/sel] */
200 cp.dpx = cp.dx * cp.Ncx / cp.Nfx; /* [mm/pix] */
201 cp.dpy = cp.dy; /* [mm/pix] */
202 cp.Cx = 576 / 2; /* [pix] */
203 cp.Cy = 384 / 2; /* [pix] */
204 cp.sx = 1.0; /* [] */
205
206 /* something a bit more realistic */
207 cp.Cx = 258.0;
208 cp.Cy = 204.0;
209 }
210
211
212 /************************************************************************/
initialize_general_imaging_mos5300_matrox_parms()213 void initialize_general_imaging_mos5300_matrox_parms ()
214 {
215 strcpy (camera_type, "General Imaging MOS5300 + Matrox");
216
217 cp.Ncx = 649; /* [sel] */
218 cp.Nfx = 512; /* [pix] */
219 cp.dx = 0.015; /* [mm/sel] */
220 cp.dy = 0.015; /* [mm/sel] */
221 cp.dpx = cp.dx * cp.Ncx / cp.Nfx; /* [mm/pix] */
222 cp.dpy = cp.dy; /* [mm/pix] */
223 cp.Cx = 512 / 2; /* [pix] */
224 cp.Cy = 480 / 2; /* [pix] */
225 cp.sx = 1.0; /* [] */
226 }
227
228
229 /************************************************************************/
initialize_panasonic_gp_mf702_matrox_parms()230 void initialize_panasonic_gp_mf702_matrox_parms ()
231 {
232 strcpy (camera_type, "Panasonic GP-MF702 + Matrox");
233
234 cp.Ncx = 649; /* [sel] */
235 cp.Nfx = 512; /* [pix] */
236 cp.dx = 0.015; /* [mm/sel] */
237 cp.dy = 0.015; /* [mm/sel] */
238 cp.dpx = cp.dx * cp.Ncx / cp.Nfx; /* [mm/pix] */
239 cp.dpy = cp.dy; /* [mm/pix] */
240
241 cp.Cx = 268; /* [pix] */
242 cp.Cy = 248; /* [pix] */
243
244 cp.sx = 1.078647; /* [] */
245 }
246
247
248 /************************************************************************/
initialize_sony_xc75_matrox_parms()249 void initialize_sony_xc75_matrox_parms ()
250 {
251 strcpy (camera_type, "Sony XC75 + Matrox");
252
253 cp.Ncx = 768; /* [sel] */
254 cp.Nfx = 512; /* [pix] */
255 cp.dx = 0.0084; /* [mm/sel] */
256 cp.dy = 0.0098; /* [mm/sel] */
257 cp.dpx = cp.dx * cp.Ncx / cp.Nfx; /* [mm/pix] */
258 cp.dpy = cp.dy; /* [mm/pix] */
259 cp.Cx = 512 / 2; /* [pix] */
260 cp.Cy = 480 / 2; /* [pix] */
261 cp.sx = 1.0; /* [] */
262 }
263
264
265 /************************************************************************/
initialize_sony_xc77_matrox_parms()266 void initialize_sony_xc77_matrox_parms ()
267 {
268 strcpy (camera_type, "Sony XC77 + Matrox");
269
270 cp.Ncx = 768; /* [sel] */
271 cp.Nfx = 512; /* [pix] */
272 cp.dx = 0.011; /* [mm/sel] */
273 cp.dy = 0.013; /* [mm/sel] */
274 cp.dpx = cp.dx * cp.Ncx / cp.Nfx; /* [mm/pix] */
275 cp.dpy = cp.dy; /* [mm/pix] */
276 cp.Cx = 512 / 2; /* [pix] */
277 cp.Cy = 480 / 2; /* [pix] */
278 cp.sx = 1.0; /* [] */
279 }
280
281
282 /************************************************************************/
initialize_sony_xc57_androx_parms()283 void initialize_sony_xc57_androx_parms ()
284 {
285 strcpy (camera_type, "Sony XC57 + Androx");
286
287 cp.Ncx = 510; /* [sel] */
288 cp.Nfx = 512; /* [pix] */
289 cp.dx = 0.017; /* [mm/sel] */
290 cp.dy = 0.013; /* [mm/sel] */
291 cp.dpx = cp.dx * cp.Ncx / cp.Nfx; /* [mm/pix] */
292 cp.dpy = cp.dy; /* [mm/pix] */
293 cp.Cx = 512 / 2; /* [pix] */
294 cp.Cy = 480 / 2; /* [pix] */
295 cp.sx = 1.107914; /* [] */
296 }
297
298
299 /************************************************************************/
300 /* John Krumm, 9 November 1993 */
301 /* This assumes that every other row, starting with the second row from */
302 /* the top, has been removed. The Xap Shot CCD only has 250 lines, and */
303 /* it inserts new rows in between the real rows to make a full size */
304 /* picture. Its algorithm for making these new rows isn't very good, */
305 /* so it's best just to throw them away. */
306 /* The camera's specs give the CCD size as 6.4(V)x4.8(H) mm. */
307 /* A call to the manufacturer found that the CCD has 250 rows */
308 /* and 782 columns. It is underscanned to 242 rows and 739 columns. */
309 /************************************************************************/
initialize_xapshot_matrox_parms()310 void initialize_xapshot_matrox_parms ()
311 {
312 strcpy (camera_type, "Canon Xap Shot");
313
314 cp.Ncx = 739; /* [sel] */
315 cp.Nfx = 512; /* [pix] */
316 cp.dx = 6.4 / 782.0; /* [mm/sel] */
317 cp.dy = 4.8 / 250.0; /* [mm/sel] */
318 cp.dpx = cp.dx * cp.Ncx / cp.Nfx; /* [mm/pix] */
319 cp.dpy = cp.dy; /* [mm/pix] */
320 cp.Cx = 512 / 2; /* [pix] */
321 cp.Cy = 240 / 2; /* [pix] */
322 cp.sx = 1.027753; /* from noncoplanar calibration *//* [] */
323 }
324
325
326 /***********************************************************************\
327 * This routine solves for the roll, pitch and yaw angles (in radians) *
328 * for a given orthonormal rotation matrix (from Richard P. Paul, *
329 * Robot Manipulators: Mathematics, Programming and Control, p70). *
330 * Note 1, should the rotation matrix not be orthonormal these will not *
331 * be the "best fit" roll, pitch and yaw angles. *
332 * Note 2, there are actually two possible solutions for the matrix. *
333 * The second solution can be found by adding 180 degrees to Rz before *
334 * Ry and Rx are calculated. *
335 \***********************************************************************/
solve_RPY_transform()336 void solve_RPY_transform ()
337 {
338 double sg,
339 cg;
340
341 cc.Rz = atan2 (cc.r4, cc.r1);
342
343 SINCOS (cc.Rz, sg, cg);
344
345 cc.Ry = atan2 (-cc.r7, cc.r1 * cg + cc.r4 * sg);
346
347 cc.Rx = atan2 (cc.r3 * sg - cc.r6 * cg, cc.r5 * cg - cc.r2 * sg);
348 }
349
350
351 /***********************************************************************\
352 * This routine simply takes the roll, pitch and yaw angles and fills in *
353 * the rotation matrix elements r1-r9. *
354 \***********************************************************************/
apply_RPY_transform()355 void apply_RPY_transform ()
356 {
357 double sa,
358 ca,
359 sb,
360 cb,
361 sg,
362 cg;
363
364 SINCOS (cc.Rx, sa, ca);
365 SINCOS (cc.Ry, sb, cb);
366 SINCOS (cc.Rz, sg, cg);
367
368 cc.r1 = cb * cg;
369 cc.r2 = cg * sa * sb - ca * sg;
370 cc.r3 = sa * sg + ca * cg * sb;
371 cc.r4 = cb * sg;
372 cc.r5 = sa * sb * sg + ca * cg;
373 cc.r6 = ca * sb * sg - cg * sa;
374 cc.r7 = -sb;
375 cc.r8 = cb * sa;
376 cc.r9 = ca * cb;
377 }
378
379
380 /***********************************************************************\
381 * Routines for coplanar camera calibration *
382 \***********************************************************************/
cc_compute_Xd_Yd_and_r_squared()383 void cc_compute_Xd_Yd_and_r_squared ()
384 {
385 int i;
386
387 double Xd_,
388 Yd_;
389
390 for (i = 0; i < cd.point_count; i++) {
391 Xd[i] = Xd_ = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx; /* [mm] */
392 Yd[i] = Yd_ = cp.dpy * (cd.Yf[i] - cp.Cy); /* [mm] */
393 r_squared[i] = SQR (Xd_) + SQR (Yd_); /* [mm^2] */
394 }
395 }
396
397
cc_compute_U()398 void cc_compute_U ()
399 {
400 int i;
401
402 dmat M,
403 a,
404 b;
405
406 M = newdmat (0, (cd.point_count - 1), 0, 4, &errno);
407 if (errno) {
408 fprintf (stderr, "cc compute U: unable to allocate matrix M\n");
409 exit (-1);
410 }
411
412 a = newdmat (0, 4, 0, 0, &errno);
413 if (errno) {
414 fprintf (stderr, "cc compute U: unable to allocate vector a\n");
415 exit (-1);
416 }
417
418 b = newdmat (0, (cd.point_count - 1), 0, 0, &errno);
419 if (errno) {
420 fprintf (stderr, "cc compute U: unable to allocate vector b\n");
421 exit (-1);
422 }
423
424 for (i = 0; i < cd.point_count; i++) {
425 M.el[i][0] = Yd[i] * cd.xw[i];
426 M.el[i][1] = Yd[i] * cd.yw[i];
427 M.el[i][2] = Yd[i];
428 M.el[i][3] = -Xd[i] * cd.xw[i];
429 M.el[i][4] = -Xd[i] * cd.yw[i];
430 b.el[i][0] = Xd[i];
431 }
432
433 if (solve_system (M, a, b)) {
434 fprintf (stderr, "cc compute U: unable to solve system Ma=b\n");
435 exit (-1);
436 }
437
438 U[0] = a.el[0][0];
439 U[1] = a.el[1][0];
440 U[2] = a.el[2][0];
441 U[3] = a.el[3][0];
442 U[4] = a.el[4][0];
443
444 freemat (M);
445 freemat (a);
446 freemat (b);
447 }
448
449
cc_compute_Tx_and_Ty()450 void cc_compute_Tx_and_Ty ()
451 {
452 int i,
453 far_point;
454
455 double Tx,
456 Ty,
457 Ty_squared,
458 x,
459 y,
460 Sr,
461 r1p,
462 r2p,
463 r4p,
464 r5p,
465 r1,
466 r2,
467 r4,
468 r5,
469 distance,
470 far_distance;
471
472 r1p = U[0];
473 r2p = U[1];
474 r4p = U[3];
475 r5p = U[4];
476
477 /* first find the square of the magnitude of Ty */
478 if ((fabs (r1p) < EPSILON) && (fabs (r2p) < EPSILON))
479 Ty_squared = 1 / (SQR (r4p) + SQR (r5p));
480 else if ((fabs (r4p) < EPSILON) && (fabs (r5p) < EPSILON))
481 Ty_squared = 1 / (SQR (r1p) + SQR (r2p));
482 else if ((fabs (r1p) < EPSILON) && (fabs (r4p) < EPSILON))
483 Ty_squared = 1 / (SQR (r2p) + SQR (r5p));
484 else if ((fabs (r2p) < EPSILON) && (fabs (r5p) < EPSILON))
485 Ty_squared = 1 / (SQR (r1p) + SQR (r4p));
486 else {
487 Sr = SQR (r1p) + SQR (r2p) + SQR (r4p) + SQR (r5p);
488 Ty_squared = (Sr - sqrt (SQR (Sr) - 4 * SQR (r1p * r5p - r4p * r2p))) /
489 (2 * SQR (r1p * r5p - r4p * r2p));
490 }
491
492 /* find a point that is far from the image center */
493 far_distance = 0;
494 far_point = 0;
495 for (i = 0; i < cd.point_count; i++)
496 if ((distance = r_squared[i]) > far_distance) {
497 far_point = i;
498 far_distance = distance;
499 }
500
501 /* now find the sign for Ty */
502 /* start by assuming Ty > 0 */
503 Ty = sqrt (Ty_squared);
504 r1 = U[0] * Ty;
505 r2 = U[1] * Ty;
506 Tx = U[2] * Ty;
507 r4 = U[3] * Ty;
508 r5 = U[4] * Ty;
509 x = r1 * cd.xw[far_point] + r2 * cd.yw[far_point] + Tx;
510 y = r4 * cd.xw[far_point] + r5 * cd.yw[far_point] + Ty;
511
512 /* flip Ty if we guessed wrong */
513 if ((SIGNBIT (x) != SIGNBIT (Xd[far_point])) ||
514 (SIGNBIT (y) != SIGNBIT (Yd[far_point])))
515 Ty = -Ty;
516
517 /* update the calibration constants */
518 cc.Tx = U[2] * Ty;
519 cc.Ty = Ty;
520 }
521
522
cc_compute_R()523 void cc_compute_R ()
524 {
525 double r1,
526 r2,
527 r3,
528 r4,
529 r5,
530 r6,
531 r7,
532 r8,
533 r9;
534
535 r1 = U[0] * cc.Ty;
536 r2 = U[1] * cc.Ty;
537 r3 = sqrt (1 - SQR (r1) - SQR (r2));
538
539 r4 = U[3] * cc.Ty;
540 r5 = U[4] * cc.Ty;
541 r6 = sqrt (1 - SQR (r4) - SQR (r5));
542 if (!SIGNBIT (r1 * r4 + r2 * r5))
543 r6 = -r6;
544
545 /* use the outer product of the first two rows to get the last row */
546 r7 = r2 * r6 - r3 * r5;
547 r8 = r3 * r4 - r1 * r6;
548 r9 = r1 * r5 - r2 * r4;
549
550 /* update the calibration constants */
551 cc.r1 = r1;
552 cc.r2 = r2;
553 cc.r3 = r3;
554 cc.r4 = r4;
555 cc.r5 = r5;
556 cc.r6 = r6;
557 cc.r7 = r7;
558 cc.r8 = r8;
559 cc.r9 = r9;
560
561 /* fill in cc.Rx, cc.Ry and cc.Rz */
562 solve_RPY_transform ();
563 }
564
565
cc_compute_approximate_f_and_Tz()566 void cc_compute_approximate_f_and_Tz ()
567 {
568 int i;
569
570 dmat M,
571 a,
572 b;
573
574 M = newdmat (0, (cd.point_count - 1), 0, 1, &errno);
575 if (errno) {
576 fprintf (stderr, "cc compute apx: unable to allocate matrix M\n");
577 exit (-1);
578 }
579
580 a = newdmat (0, 1, 0, 0, &errno);
581 if (errno) {
582 fprintf (stderr, "cc compute apx: unable to allocate vector a\n");
583 exit (-1);
584 }
585
586 b = newdmat (0, (cd.point_count - 1), 0, 0, &errno);
587 if (errno) {
588 fprintf (stderr, "cc compute apx: unable to allocate vector b\n");
589 exit (-1);
590 }
591
592 for (i = 0; i < cd.point_count; i++) {
593 M.el[i][0] = cc.r4 * cd.xw[i] + cc.r5 * cd.yw[i] + cc.Ty;
594 M.el[i][1] = -Yd[i];
595 b.el[i][0] = (cc.r7 * cd.xw[i] + cc.r8 * cd.yw[i]) * Yd[i];
596 }
597
598 if (solve_system (M, a, b)) {
599 fprintf (stderr, "cc compute apx: unable to solve system Ma=b\n");
600 exit (-1);
601 }
602
603 /* update the calibration constants */
604 cc.f = a.el[0][0];
605 cc.Tz = a.el[1][0];
606 cc.kappa1 = 0.0; /* this is the assumption that our calculation was made under */
607
608 freemat (M);
609 freemat (a);
610 freemat (b);
611 }
612
613
614 /************************************************************************/
cc_compute_exact_f_and_Tz_error(m_ptr,n_ptr,params,err)615 void cc_compute_exact_f_and_Tz_error (m_ptr, n_ptr, params, err)
616 integer *m_ptr; /* pointer to number of points to fit */
617 integer *n_ptr; /* pointer to number of parameters */
618 doublereal *params; /* vector of parameters */
619 doublereal *err; /* vector of error from data */
620 {
621 int i;
622
623 double f,
624 Tz,
625 kappa1,
626 xc,
627 yc,
628 zc,
629 Xu_1,
630 Yu_1,
631 Xu_2,
632 Yu_2,
633 distortion_factor;
634
635 f = params[0];
636 Tz = params[1];
637 kappa1 = params[2];
638
639 for (i = 0; i < cd.point_count; i++) {
640 /* convert from world coordinates to camera coordinates */
641 /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */
642 xc = cc.r1 * cd.xw[i] + cc.r2 * cd.yw[i] + cc.Tx;
643 yc = cc.r4 * cd.xw[i] + cc.r5 * cd.yw[i] + cc.Ty;
644 zc = cc.r7 * cd.xw[i] + cc.r8 * cd.yw[i] + Tz;
645
646 /* convert from camera coordinates to undistorted sensor coordinates */
647 Xu_1 = f * xc / zc;
648 Yu_1 = f * yc / zc;
649
650 /* convert from distorted sensor coordinates to undistorted sensor coordinates */
651 distortion_factor = 1 + kappa1 * (SQR (Xd[i]) + SQR (Yd[i]));
652 Xu_2 = Xd[i] * distortion_factor;
653 Yu_2 = Yd[i] * distortion_factor;
654
655 /* record the error in the undistorted sensor coordinates */
656 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
657 }
658 }
659
660
cc_compute_exact_f_and_Tz()661 void cc_compute_exact_f_and_Tz ()
662 {
663 #define NPARAMS 3
664
665 int i;
666
667 /* Parameters needed by MINPACK's lmdif() */
668
669 integer m = cd.point_count;
670 integer n = NPARAMS;
671 doublereal x[NPARAMS];
672 doublereal *fvec;
673 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
674 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
675 doublereal gtol = ORTHO_TOLERANCE_gtol;
676 integer maxfev = MAXFEV;
677 doublereal epsfcn = EPSFCN;
678 doublereal diag[NPARAMS];
679 integer mode = MODE;
680 doublereal factor = FACTOR;
681 integer nprint = 0;
682 integer info;
683 integer nfev;
684 doublereal *fjac;
685 integer ldfjac = m;
686 integer ipvt[NPARAMS];
687 doublereal qtf[NPARAMS];
688 doublereal wa1[NPARAMS];
689 doublereal wa2[NPARAMS];
690 doublereal wa3[NPARAMS];
691 doublereal *wa4;
692
693 /* allocate some workspace */
694 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
695 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
696 exit(-1);
697 }
698
699 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
700 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
701 exit(-1);
702 }
703
704 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
705 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
706 exit(-1);
707 }
708
709 /* use the current calibration constants as an initial guess */
710 x[0] = cc.f;
711 x[1] = cc.Tz;
712 x[2] = cc.kappa1;
713
714 /* define optional scale factors for the parameters */
715 if ( mode == 2 ) {
716 for (i = 0; i < NPARAMS; i++)
717 diag[i] = 1.0; /* some user-defined values */
718 }
719
720 /* perform the optimization */
721 lmdif_ (cc_compute_exact_f_and_Tz_error,
722 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
723 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
724 ipvt, qtf, wa1, wa2, wa3, wa4);
725
726 /* update the calibration constants */
727 cc.f = x[0];
728 cc.Tz = x[1];
729 cc.kappa1 = x[2];
730
731 /* release allocated workspace */
732 free (fvec);
733 free (fjac);
734 free (wa4);
735
736 #ifdef DEBUG
737 /* print the number of function calls during iteration */
738 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
739 #endif
740
741 #undef NPARAMS
742 }
743
744
745 /************************************************************************/
cc_three_parm_optimization()746 void cc_three_parm_optimization ()
747 {
748 int i;
749
750 for (i = 0; i < cd.point_count; i++)
751 if (cd.zw[i]) {
752 fprintf (stderr, "error - coplanar calibration tried with data outside of Z plane\n\n");
753 exit (-1);
754 }
755
756 cc_compute_Xd_Yd_and_r_squared ();
757
758 cc_compute_U ();
759
760 cc_compute_Tx_and_Ty ();
761
762 cc_compute_R ();
763
764 cc_compute_approximate_f_and_Tz ();
765
766 if (cc.f < 0) {
767 /* try the other solution for the orthonormal matrix */
768 cc.r3 = -cc.r3;
769 cc.r6 = -cc.r6;
770 cc.r7 = -cc.r7;
771 cc.r8 = -cc.r8;
772 solve_RPY_transform ();
773
774 cc_compute_approximate_f_and_Tz ();
775
776 if (cc.f < 0) {
777 fprintf (stderr, "error - possible handedness problem with data\n");
778 exit (-1);
779 }
780 }
781
782 cc_compute_exact_f_and_Tz ();
783 }
784
785
786 /************************************************************************/
cc_remove_sensor_plane_distortion_from_Xd_and_Yd()787 void cc_remove_sensor_plane_distortion_from_Xd_and_Yd ()
788 {
789 int i;
790 double Xu,
791 Yu;
792
793 for (i = 0; i < cd.point_count; i++) {
794 distorted_to_undistorted_sensor_coord (Xd[i], Yd[i], &Xu, &Yu);
795 Xd[i] = Xu;
796 Yd[i] = Yu;
797 r_squared[i] = SQR (Xu) + SQR (Yu);
798 }
799 }
800
801
802 /************************************************************************/
cc_five_parm_optimization_with_late_distortion_removal_error(m_ptr,n_ptr,params,err)803 void cc_five_parm_optimization_with_late_distortion_removal_error (m_ptr, n_ptr, params, err)
804 integer *m_ptr; /* pointer to number of points to fit */
805 integer *n_ptr; /* pointer to number of parameters */
806 doublereal *params; /* vector of parameters */
807 doublereal *err; /* vector of error from data */
808 {
809 int i;
810
811 double f,
812 Tz,
813 kappa1,
814 xc,
815 yc,
816 zc,
817 Xu_1,
818 Yu_1,
819 Xu_2,
820 Yu_2,
821 distortion_factor;
822
823 /* in this routine radial lens distortion is only taken into account */
824 /* after the rotation and translation constants have been determined */
825
826 f = params[0];
827 Tz = params[1];
828 kappa1 = params[2];
829
830 cp.Cx = params[3];
831 cp.Cy = params[4];
832
833 cc_compute_Xd_Yd_and_r_squared ();
834
835 cc_compute_U ();
836
837 cc_compute_Tx_and_Ty ();
838
839 cc_compute_R ();
840
841 cc_compute_approximate_f_and_Tz ();
842
843 if (cc.f < 0) {
844 /* try the other solution for the orthonormal matrix */
845 cc.r3 = -cc.r3;
846 cc.r6 = -cc.r6;
847 cc.r7 = -cc.r7;
848 cc.r8 = -cc.r8;
849 solve_RPY_transform ();
850
851 cc_compute_approximate_f_and_Tz ();
852
853 if (cc.f < 0) {
854 fprintf (stderr, "error - possible handedness problem with data\n");
855 exit (-1);
856 }
857 }
858
859 for (i = 0; i < cd.point_count; i++) {
860 /* convert from world coordinates to camera coordinates */
861 /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */
862 xc = cc.r1 * cd.xw[i] + cc.r2 * cd.yw[i] + cc.Tx;
863 yc = cc.r4 * cd.xw[i] + cc.r5 * cd.yw[i] + cc.Ty;
864 zc = cc.r7 * cd.xw[i] + cc.r8 * cd.yw[i] + Tz;
865
866 /* convert from camera coordinates to undistorted sensor coordinates */
867 Xu_1 = f * xc / zc;
868 Yu_1 = f * yc / zc;
869
870 /* convert from distorted sensor coordinates to undistorted sensor coordinates */
871 distortion_factor = 1 + kappa1 * r_squared[i];
872 Xu_2 = Xd[i] * distortion_factor;
873 Yu_2 = Yd[i] * distortion_factor;
874
875 /* record the error in the undistorted sensor coordinates */
876 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
877 }
878 }
879
880
cc_five_parm_optimization_with_late_distortion_removal()881 void cc_five_parm_optimization_with_late_distortion_removal ()
882 {
883 #define NPARAMS 5
884
885 int i;
886
887 /* Parameters needed by MINPACK's lmdif() */
888
889 integer m = cd.point_count;
890 integer n = NPARAMS;
891 doublereal x[NPARAMS];
892 doublereal *fvec;
893 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
894 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
895 doublereal gtol = ORTHO_TOLERANCE_gtol;
896 integer maxfev = MAXFEV;
897 doublereal epsfcn = EPSFCN;
898 doublereal diag[NPARAMS];
899 integer mode = MODE;
900 doublereal factor = FACTOR;
901 integer nprint = 0;
902 integer info;
903 integer nfev;
904 doublereal *fjac;
905 integer ldfjac = m;
906 integer ipvt[NPARAMS];
907 doublereal qtf[NPARAMS];
908 doublereal wa1[NPARAMS];
909 doublereal wa2[NPARAMS];
910 doublereal wa3[NPARAMS];
911 doublereal *wa4;
912
913 /* allocate some workspace */
914 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
915 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
916 exit(-1);
917 }
918
919 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
920 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
921 exit(-1);
922 }
923
924 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
925 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
926 exit(-1);
927 }
928
929 /* use the current calibration constants as an initial guess */
930 x[0] = cc.f;
931 x[1] = cc.Tz;
932 x[2] = cc.kappa1;
933 x[3] = cp.Cx;
934 x[4] = cp.Cy;
935
936 /* define optional scale factors for the parameters */
937 if ( mode == 2 ) {
938 for (i = 0; i < NPARAMS; i++)
939 diag[i] = 1.0; /* some user-defined values */
940 }
941
942 /* perform the optimization */
943 lmdif_ (cc_five_parm_optimization_with_late_distortion_removal_error,
944 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
945 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
946 ipvt, qtf, wa1, wa2, wa3, wa4);
947
948 /* update the calibration and camera constants */
949 cc.f = x[0];
950 cc.Tz = x[1];
951 cc.kappa1 = x[2];
952 cp.Cx = x[3];
953 cp.Cy = x[4];
954
955 /* release allocated workspace */
956 free (fvec);
957 free (fjac);
958 free (wa4);
959
960 #ifdef DEBUG
961 /* print the number of function calls during iteration */
962 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
963 #endif
964
965 #undef NPARAMS
966 }
967
968
969 /************************************************************************/
cc_five_parm_optimization_with_early_distortion_removal_error(m_ptr,n_ptr,params,err)970 void cc_five_parm_optimization_with_early_distortion_removal_error (m_ptr, n_ptr, params, err)
971 integer *m_ptr; /* pointer to number of points to fit */
972 integer *n_ptr; /* pointer to number of parameters */
973 doublereal *params; /* vector of parameters */
974 doublereal *err; /* vector of error from data */
975 {
976 int i;
977
978 double f,
979 Tz,
980 xc,
981 yc,
982 zc,
983 Xu_1,
984 Yu_1,
985 Xu_2,
986 Yu_2;
987
988 /* in this routine radial lens distortion is taken into account */
989 /* before the rotation and translation constants are determined */
990 /* (this assumes we have the distortion reasonably modelled) */
991
992 f = params[0];
993 Tz = params[1];
994
995 cc.kappa1 = params[2];
996 cp.Cx = params[3];
997 cp.Cy = params[4];
998
999 cc_compute_Xd_Yd_and_r_squared ();
1000
1001 /* remove the sensor distortion before computing the translation and rotation stuff */
1002 cc_remove_sensor_plane_distortion_from_Xd_and_Yd ();
1003
1004 cc_compute_U ();
1005
1006 cc_compute_Tx_and_Ty ();
1007
1008 cc_compute_R ();
1009
1010 /* we need to do this just to see if we have to flip the rotation matrix */
1011 cc_compute_approximate_f_and_Tz ();
1012
1013 if (cc.f < 0) {
1014 /* try the other solution for the orthonormal matrix */
1015 cc.r3 = -cc.r3;
1016 cc.r6 = -cc.r6;
1017 cc.r7 = -cc.r7;
1018 cc.r8 = -cc.r8;
1019 solve_RPY_transform ();
1020
1021 cc_compute_approximate_f_and_Tz ();
1022
1023 if (cc.f < 0) {
1024 fprintf (stderr, "error - possible handedness problem with data\n");
1025 exit (-1);
1026 }
1027 }
1028
1029 /* now calculate the squared error assuming zero distortion */
1030 for (i = 0; i < cd.point_count; i++) {
1031 /* convert from world coordinates to camera coordinates */
1032 /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */
1033 xc = cc.r1 * cd.xw[i] + cc.r2 * cd.yw[i] + cc.Tx;
1034 yc = cc.r4 * cd.xw[i] + cc.r5 * cd.yw[i] + cc.Ty;
1035 zc = cc.r7 * cd.xw[i] + cc.r8 * cd.yw[i] + Tz;
1036
1037 /* convert from camera coordinates to undistorted sensor coordinates */
1038 Xu_1 = f * xc / zc;
1039 Yu_1 = f * yc / zc;
1040
1041 /* convert from distorted sensor coordinates to undistorted sensor coordinates */
1042 /* (already done, actually) */
1043 Xu_2 = Xd[i];
1044 Yu_2 = Yd[i];
1045
1046 /* record the error in the undistorted sensor coordinates */
1047 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
1048 }
1049 }
1050
1051
cc_five_parm_optimization_with_early_distortion_removal()1052 void cc_five_parm_optimization_with_early_distortion_removal ()
1053 {
1054 #define NPARAMS 5
1055
1056 int i;
1057
1058 /* Parameters needed by MINPACK's lmdif() */
1059
1060 integer m = cd.point_count;
1061 integer n = NPARAMS;
1062 doublereal x[NPARAMS];
1063 doublereal *fvec;
1064 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
1065 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
1066 doublereal gtol = ORTHO_TOLERANCE_gtol;
1067 integer maxfev = MAXFEV;
1068 doublereal epsfcn = EPSFCN;
1069 doublereal diag[NPARAMS];
1070 integer mode = MODE;
1071 doublereal factor = FACTOR;
1072 integer nprint = 0;
1073 integer info;
1074 integer nfev;
1075 doublereal *fjac;
1076 integer ldfjac = m;
1077 integer ipvt[NPARAMS];
1078 doublereal qtf[NPARAMS];
1079 doublereal wa1[NPARAMS];
1080 doublereal wa2[NPARAMS];
1081 doublereal wa3[NPARAMS];
1082 doublereal *wa4;
1083
1084 /* allocate some workspace */
1085 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
1086 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
1087 exit(-1);
1088 }
1089
1090 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
1091 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
1092 exit(-1);
1093 }
1094
1095 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
1096 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
1097 exit(-1);
1098 }
1099
1100 /* use the current calibration and camera constants as a starting point */
1101 x[0] = cc.f;
1102 x[1] = cc.Tz;
1103 x[2] = cc.kappa1;
1104 x[3] = cp.Cx;
1105 x[4] = cp.Cy;
1106
1107 /* define optional scale factors for the parameters */
1108 if ( mode == 2 ) {
1109 for (i = 0; i < NPARAMS; i++)
1110 diag[i] = 1.0; /* some user-defined values */
1111 }
1112
1113 /* perform the optimization */
1114 lmdif_ (cc_five_parm_optimization_with_early_distortion_removal_error,
1115 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
1116 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
1117 ipvt, qtf, wa1, wa2, wa3, wa4);
1118
1119 /* update the calibration and camera constants */
1120 cc.f = x[0];
1121 cc.Tz = x[1];
1122 cc.kappa1 = x[2];
1123 cp.Cx = x[3];
1124 cp.Cy = x[4];
1125
1126 /* release allocated workspace */
1127 free (fvec);
1128 free (fjac);
1129 free (wa4);
1130
1131 #ifdef DEBUG
1132 /* print the number of function calls during iteration */
1133 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
1134 #endif
1135
1136 #undef NPARAMS
1137 }
1138
1139
1140 /************************************************************************/
cc_nic_optimization_error(m_ptr,n_ptr,params,err)1141 void cc_nic_optimization_error (m_ptr, n_ptr, params, err)
1142 integer *m_ptr; /* pointer to number of points to fit */
1143 integer *n_ptr; /* pointer to number of parameters */
1144 doublereal *params; /* vector of parameters */
1145 doublereal *err; /* vector of error from data */
1146 {
1147 int i;
1148
1149 double xc,
1150 yc,
1151 zc,
1152 Xd_,
1153 Yd_,
1154 Xu_1,
1155 Yu_1,
1156 Xu_2,
1157 Yu_2,
1158 distortion_factor,
1159 Rx,
1160 Ry,
1161 Rz,
1162 Tx,
1163 Ty,
1164 Tz,
1165 kappa1,
1166 f,
1167 r1,
1168 r2,
1169 r4,
1170 r5,
1171 r7,
1172 r8,
1173 sa,
1174 sb,
1175 sg,
1176 ca,
1177 cb,
1178 cg;
1179
1180 Rx = params[0];
1181 Ry = params[1];
1182 Rz = params[2];
1183 Tx = params[3];
1184 Ty = params[4];
1185 Tz = params[5];
1186 kappa1 = params[6];
1187 f = params[7];
1188
1189 SINCOS (Rx, sa, ca);
1190 SINCOS (Ry, sb, cb);
1191 SINCOS (Rz, sg, cg);
1192 r1 = cb * cg;
1193 r2 = cg * sa * sb - ca * sg;
1194 r4 = cb * sg;
1195 r5 = sa * sb * sg + ca * cg;
1196 r7 = -sb;
1197 r8 = cb * sa;
1198
1199 for (i = 0; i < cd.point_count; i++) {
1200 /* convert from world coordinates to camera coordinates */
1201 /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */
1202 xc = r1 * cd.xw[i] + r2 * cd.yw[i] + Tx;
1203 yc = r4 * cd.xw[i] + r5 * cd.yw[i] + Ty;
1204 zc = r7 * cd.xw[i] + r8 * cd.yw[i] + Tz;
1205
1206 /* convert from camera coordinates to undistorted sensor plane coordinates */
1207 Xu_1 = f * xc / zc;
1208 Yu_1 = f * yc / zc;
1209
1210 /* convert from 2D image coordinates to distorted sensor coordinates */
1211 Xd_ = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx;
1212 Yd_ = cp.dpy * (cd.Yf[i] - cp.Cy);
1213
1214 /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
1215 distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_));
1216 Xu_2 = Xd_ * distortion_factor;
1217 Yu_2 = Yd_ * distortion_factor;
1218
1219 /* record the error in the undistorted sensor coordinates */
1220 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
1221 }
1222 }
1223
1224
cc_nic_optimization()1225 void cc_nic_optimization ()
1226 {
1227 #define NPARAMS 8
1228
1229 int i;
1230
1231 /* Parameters needed by MINPACK's lmdif() */
1232
1233 integer m = cd.point_count;
1234 integer n = NPARAMS;
1235 doublereal x[NPARAMS];
1236 doublereal *fvec;
1237 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
1238 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
1239 doublereal gtol = ORTHO_TOLERANCE_gtol;
1240 integer maxfev = MAXFEV;
1241 doublereal epsfcn = EPSFCN;
1242 doublereal diag[NPARAMS];
1243 integer mode = MODE;
1244 doublereal factor = FACTOR;
1245 integer nprint = 0;
1246 integer info;
1247 integer nfev;
1248 doublereal *fjac;
1249 integer ldfjac = m;
1250 integer ipvt[NPARAMS];
1251 doublereal qtf[NPARAMS];
1252 doublereal wa1[NPARAMS];
1253 doublereal wa2[NPARAMS];
1254 doublereal wa3[NPARAMS];
1255 doublereal *wa4;
1256
1257 /* allocate some workspace */
1258 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
1259 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
1260 exit(-1);
1261 }
1262
1263 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
1264 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
1265 exit(-1);
1266 }
1267
1268 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
1269 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
1270 exit(-1);
1271 }
1272
1273 /* use the current calibration and camera constants as a starting point */
1274 x[0] = cc.Rx;
1275 x[1] = cc.Ry;
1276 x[2] = cc.Rz;
1277 x[3] = cc.Tx;
1278 x[4] = cc.Ty;
1279 x[5] = cc.Tz;
1280 x[6] = cc.kappa1;
1281 x[7] = cc.f;
1282
1283 /* define optional scale factors for the parameters */
1284 if ( mode == 2 ) {
1285 for (i = 0; i < NPARAMS; i++)
1286 diag[i] = 1.0; /* some user-defined values */
1287 }
1288
1289 /* perform the optimization */
1290 lmdif_ (cc_nic_optimization_error,
1291 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
1292 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
1293 ipvt, qtf, wa1, wa2, wa3, wa4);
1294
1295 /* update the calibration and camera constants */
1296 cc.Rx = x[0];
1297 cc.Ry = x[1];
1298 cc.Rz = x[2];
1299 apply_RPY_transform ();
1300
1301 cc.Tx = x[3];
1302 cc.Ty = x[4];
1303 cc.Tz = x[5];
1304 cc.kappa1 = x[6];
1305 cc.f = x[7];
1306
1307 /* release allocated workspace */
1308 free (fvec);
1309 free (fjac);
1310 free (wa4);
1311
1312 #ifdef DEBUG
1313 /* print the number of function calls during iteration */
1314 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
1315 #endif
1316
1317 #undef NPARAMS
1318 }
1319
1320
1321 /************************************************************************/
cc_full_optimization_error(m_ptr,n_ptr,params,err)1322 void cc_full_optimization_error (m_ptr, n_ptr, params, err)
1323 integer *m_ptr; /* pointer to number of points to fit */
1324 integer *n_ptr; /* pointer to number of parameters */
1325 doublereal *params; /* vector of parameters */
1326 doublereal *err; /* vector of error from data */
1327 {
1328 int i;
1329
1330 double xc,
1331 yc,
1332 zc,
1333 Xd_,
1334 Yd_,
1335 Xu_1,
1336 Yu_1,
1337 Xu_2,
1338 Yu_2,
1339 distortion_factor,
1340 Rx,
1341 Ry,
1342 Rz,
1343 Tx,
1344 Ty,
1345 Tz,
1346 kappa1,
1347 f,
1348 Cx,
1349 Cy,
1350 r1,
1351 r2,
1352 r4,
1353 r5,
1354 r7,
1355 r8,
1356 sa,
1357 sb,
1358 sg,
1359 ca,
1360 cb,
1361 cg;
1362
1363 Rx = params[0];
1364 Ry = params[1];
1365 Rz = params[2];
1366 Tx = params[3];
1367 Ty = params[4];
1368 Tz = params[5];
1369 kappa1 = params[6];
1370 f = params[7];
1371 Cx = params[8];
1372 Cy = params[9];
1373
1374 SINCOS (Rx, sa, ca);
1375 SINCOS (Ry, sb, cb);
1376 SINCOS (Rz, sg, cg);
1377 r1 = cb * cg;
1378 r2 = cg * sa * sb - ca * sg;
1379 r4 = cb * sg;
1380 r5 = sa * sb * sg + ca * cg;
1381 r7 = -sb;
1382 r8 = cb * sa;
1383
1384 for (i = 0; i < cd.point_count; i++) {
1385 /* convert from world coordinates to camera coordinates */
1386 /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */
1387 xc = r1 * cd.xw[i] + r2 * cd.yw[i] + Tx;
1388 yc = r4 * cd.xw[i] + r5 * cd.yw[i] + Ty;
1389 zc = r7 * cd.xw[i] + r8 * cd.yw[i] + Tz;
1390
1391 /* convert from camera coordinates to undistorted sensor plane coordinates */
1392 Xu_1 = f * xc / zc;
1393 Yu_1 = f * yc / zc;
1394
1395 /* convert from 2D image coordinates to distorted sensor coordinates */
1396 Xd_ = cp.dpx * (cd.Xf[i] - Cx) / cp.sx;
1397 Yd_ = cp.dpy * (cd.Yf[i] - Cy);
1398
1399 /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
1400 distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_));
1401 Xu_2 = Xd_ * distortion_factor;
1402 Yu_2 = Yd_ * distortion_factor;
1403
1404 /* record the error in the undistorted sensor coordinates */
1405 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
1406 }
1407 }
1408
1409
cc_full_optimization()1410 void cc_full_optimization ()
1411 {
1412 #define NPARAMS 10
1413
1414 int i;
1415
1416 /* Parameters needed by MINPACK's lmdif() */
1417
1418 integer m = cd.point_count;
1419 integer n = NPARAMS;
1420 doublereal x[NPARAMS];
1421 doublereal *fvec;
1422 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
1423 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
1424 doublereal gtol = ORTHO_TOLERANCE_gtol;
1425 integer maxfev = MAXFEV;
1426 doublereal epsfcn = EPSFCN;
1427 doublereal diag[NPARAMS];
1428 integer mode = MODE;
1429 doublereal factor = FACTOR;
1430 integer nprint = 0;
1431 integer info;
1432 integer nfev;
1433 doublereal *fjac;
1434 integer ldfjac = m;
1435 integer ipvt[NPARAMS];
1436 doublereal qtf[NPARAMS];
1437 doublereal wa1[NPARAMS];
1438 doublereal wa2[NPARAMS];
1439 doublereal wa3[NPARAMS];
1440 doublereal *wa4;
1441
1442 /* allocate some workspace */
1443 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
1444 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
1445 exit(-1);
1446 }
1447
1448 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
1449 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
1450 exit(-1);
1451 }
1452
1453 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
1454 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
1455 exit(-1);
1456 }
1457
1458 /* use the current calibration and camera constants as a starting point */
1459 x[0] = cc.Rx;
1460 x[1] = cc.Ry;
1461 x[2] = cc.Rz;
1462 x[3] = cc.Tx;
1463 x[4] = cc.Ty;
1464 x[5] = cc.Tz;
1465 x[6] = cc.kappa1;
1466 x[7] = cc.f;
1467 x[8] = cp.Cx;
1468 x[9] = cp.Cy;
1469
1470 /* define optional scale factors for the parameters */
1471 if ( mode == 2 ) {
1472 for (i = 0; i < NPARAMS; i++)
1473 diag[i] = 1.0; /* some user-defined values */
1474 }
1475
1476 /* perform the optimization */
1477 lmdif_ (cc_full_optimization_error,
1478 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
1479 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
1480 ipvt, qtf, wa1, wa2, wa3, wa4);
1481
1482 /* update the calibration and camera constants */
1483 cc.Rx = x[0];
1484 cc.Ry = x[1];
1485 cc.Rz = x[2];
1486 apply_RPY_transform ();
1487
1488 cc.Tx = x[3];
1489 cc.Ty = x[4];
1490 cc.Tz = x[5];
1491 cc.kappa1 = x[6];
1492 cc.f = x[7];
1493 cp.Cx = x[8];
1494 cp.Cy = x[9];
1495
1496 /* release allocated workspace */
1497 free (fvec);
1498 free (fjac);
1499 free (wa4);
1500
1501 #ifdef DEBUG
1502 /* print the number of function calls during iteration */
1503 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
1504 #endif
1505
1506 #undef NPARAMS
1507 }
1508
1509
1510 /***********************************************************************\
1511 * Routines for noncoplanar camera calibration *
1512 \***********************************************************************/
ncc_compute_Xd_Yd_and_r_squared()1513 void ncc_compute_Xd_Yd_and_r_squared ()
1514 {
1515 int i;
1516
1517 double Xd_,
1518 Yd_;
1519
1520 for (i = 0; i < cd.point_count; i++) {
1521 Xd[i] = Xd_ = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx; /* [mm] */
1522 Yd[i] = Yd_ = cp.dpy * (cd.Yf[i] - cp.Cy); /* [mm] */
1523 r_squared[i] = SQR (Xd_) + SQR (Yd_); /* [mm^2] */
1524 }
1525 }
1526
1527
ncc_compute_U()1528 void ncc_compute_U ()
1529 {
1530 int i;
1531
1532 dmat M,
1533 a,
1534 b;
1535
1536 M = newdmat (0, (cd.point_count - 1), 0, 6, &errno);
1537 if (errno) {
1538 fprintf (stderr, "ncc compute U: unable to allocate matrix M\n");
1539 exit (-1);
1540 }
1541
1542 a = newdmat (0, 6, 0, 0, &errno);
1543 if (errno) {
1544 fprintf (stderr, "ncc compute U: unable to allocate vector a\n");
1545 exit (-1);
1546 }
1547
1548 b = newdmat (0, (cd.point_count - 1), 0, 0, &errno);
1549 if (errno) {
1550 fprintf (stderr, "ncc compute U: unable to allocate vector b\n");
1551 exit (-1);
1552 }
1553
1554 for (i = 0; i < cd.point_count; i++) {
1555 M.el[i][0] = Yd[i] * cd.xw[i];
1556 M.el[i][1] = Yd[i] * cd.yw[i];
1557 M.el[i][2] = Yd[i] * cd.zw[i];
1558 M.el[i][3] = Yd[i];
1559 M.el[i][4] = -Xd[i] * cd.xw[i];
1560 M.el[i][5] = -Xd[i] * cd.yw[i];
1561 M.el[i][6] = -Xd[i] * cd.zw[i];
1562 b.el[i][0] = Xd[i];
1563 }
1564
1565 if (solve_system (M, a, b)) {
1566 fprintf (stderr, "ncc compute U: error - non-coplanar calibration tried with data which may possibly be coplanar\n\n");
1567 exit (-1);
1568 }
1569
1570 U[0] = a.el[0][0];
1571 U[1] = a.el[1][0];
1572 U[2] = a.el[2][0];
1573 U[3] = a.el[3][0];
1574 U[4] = a.el[4][0];
1575 U[5] = a.el[5][0];
1576 U[6] = a.el[6][0];
1577
1578 freemat (M);
1579 freemat (a);
1580 freemat (b);
1581 }
1582
1583
ncc_compute_Tx_and_Ty()1584 void ncc_compute_Tx_and_Ty ()
1585 {
1586 int i,
1587 far_point;
1588
1589 double Tx,
1590 Ty,
1591 Ty_squared,
1592 x,
1593 y,
1594 r1,
1595 r2,
1596 r3,
1597 r4,
1598 r5,
1599 r6,
1600 distance,
1601 far_distance;
1602
1603 /* first find the square of the magnitude of Ty */
1604 Ty_squared = 1 / (SQR (U[4]) + SQR (U[5]) + SQR (U[6]));
1605
1606 /* find a point that is far from the image center */
1607 far_distance = 0;
1608 far_point = 0;
1609 for (i = 0; i < cd.point_count; i++)
1610 if ((distance = r_squared[i]) > far_distance) {
1611 far_point = i;
1612 far_distance = distance;
1613 }
1614
1615 /* now find the sign for Ty */
1616 /* start by assuming Ty > 0 */
1617 Ty = sqrt (Ty_squared);
1618 r1 = U[0] * Ty;
1619 r2 = U[1] * Ty;
1620 r3 = U[2] * Ty;
1621 Tx = U[3] * Ty;
1622 r4 = U[4] * Ty;
1623 r5 = U[5] * Ty;
1624 r6 = U[6] * Ty;
1625 x = r1 * cd.xw[far_point] + r2 * cd.yw[far_point] + r3 * cd.zw[far_point] + Tx;
1626 y = r4 * cd.xw[far_point] + r5 * cd.yw[far_point] + r6 * cd.zw[far_point] + Ty;
1627
1628 /* flip Ty if we guessed wrong */
1629 if ((SIGNBIT (x) != SIGNBIT (Xd[far_point])) ||
1630 (SIGNBIT (y) != SIGNBIT (Yd[far_point])))
1631 Ty = -Ty;
1632
1633 /* update the calibration constants */
1634 cc.Tx = U[3] * Ty;
1635 cc.Ty = Ty;
1636 }
1637
1638
ncc_compute_sx()1639 void ncc_compute_sx ()
1640 {
1641 cp.sx = sqrt (SQR (U[0]) + SQR (U[1]) + SQR (U[2])) * fabs (cc.Ty);
1642 }
1643
1644
ncc_compute_R()1645 void ncc_compute_R ()
1646 {
1647 double r1,
1648 r2,
1649 r3,
1650 r4,
1651 r5,
1652 r6,
1653 r7,
1654 r8,
1655 r9;
1656
1657 r1 = U[0] * cc.Ty / cp.sx;
1658 r2 = U[1] * cc.Ty / cp.sx;
1659 r3 = U[2] * cc.Ty / cp.sx;
1660
1661 r4 = U[4] * cc.Ty;
1662 r5 = U[5] * cc.Ty;
1663 r6 = U[6] * cc.Ty;
1664
1665 /* use the outer product of the first two rows to get the last row */
1666 r7 = r2 * r6 - r3 * r5;
1667 r8 = r3 * r4 - r1 * r6;
1668 r9 = r1 * r5 - r2 * r4;
1669
1670 /* update the calibration constants */
1671 cc.r1 = r1;
1672 cc.r2 = r2;
1673 cc.r3 = r3;
1674 cc.r4 = r4;
1675 cc.r5 = r5;
1676 cc.r6 = r6;
1677 cc.r7 = r7;
1678 cc.r8 = r8;
1679 cc.r9 = r9;
1680
1681 /* fill in cc.Rx, cc.Ry and cc.Rz */
1682 solve_RPY_transform ();
1683 }
1684
1685
ncc_compute_better_R()1686 void ncc_compute_better_R ()
1687 {
1688 double r1,
1689 r2,
1690 r3,
1691 r4,
1692 r5,
1693 r6,
1694 r7,
1695 sa,
1696 ca,
1697 sb,
1698 cb,
1699 sg,
1700 cg;
1701
1702 r1 = U[0] * cc.Ty / cp.sx;
1703 r2 = U[1] * cc.Ty / cp.sx;
1704 r3 = U[2] * cc.Ty / cp.sx;
1705
1706 r4 = U[4] * cc.Ty;
1707 r5 = U[5] * cc.Ty;
1708 r6 = U[6] * cc.Ty;
1709
1710 /* use the outer product of the first two rows to get the last row */
1711 r7 = r2 * r6 - r3 * r5;
1712
1713 /* now find the RPY angles corresponding to the estimated rotation matrix */
1714 cc.Rz = atan2 (r4, r1);
1715
1716 SINCOS (cc.Rz, sg, cg);
1717
1718 cc.Ry = atan2 (-r7, r1 * cg + r4 * sg);
1719
1720 cc.Rx = atan2 (r3 * sg - r6 * cg, r5 * cg - r2 * sg);
1721
1722 SINCOS (cc.Rx, sa, ca);
1723
1724 SINCOS (cc.Ry, sb, cb);
1725
1726 /* now generate a more orthonormal rotation matrix from the RPY angles */
1727 cc.r1 = cb * cg;
1728 cc.r2 = cg * sa * sb - ca * sg;
1729 cc.r3 = sa * sg + ca * cg * sb;
1730 cc.r4 = cb * sg;
1731 cc.r5 = sa * sb * sg + ca * cg;
1732 cc.r6 = ca * sb * sg - cg * sa;
1733 cc.r7 = -sb;
1734 cc.r8 = cb * sa;
1735 cc.r9 = ca * cb;
1736 }
1737
1738
ncc_compute_approximate_f_and_Tz()1739 void ncc_compute_approximate_f_and_Tz ()
1740 {
1741 int i;
1742
1743 dmat M,
1744 a,
1745 b;
1746
1747 M = newdmat (0, (cd.point_count - 1), 0, 1, &errno);
1748 if (errno) {
1749 fprintf (stderr, "ncc compute apx: unable to allocate matrix M\n");
1750 exit (-1);
1751 }
1752
1753 a = newdmat (0, 1, 0, 0, &errno);
1754 if (errno) {
1755 fprintf (stderr, "ncc compute apx: unable to allocate vector a\n");
1756 exit (-1);
1757 }
1758
1759 b = newdmat (0, (cd.point_count - 1), 0, 0, &errno);
1760 if (errno) {
1761 fprintf (stderr, "ncc compute apx: unable to allocate vector b\n");
1762 exit (-1);
1763 }
1764
1765 for (i = 0; i < cd.point_count; i++) {
1766 M.el[i][0] = cc.r4 * cd.xw[i] + cc.r5 * cd.yw[i] + cc.r6 * cd.zw[i] + cc.Ty;
1767 M.el[i][1] = -Yd[i];
1768 b.el[i][0] = (cc.r7 * cd.xw[i] + cc.r8 * cd.yw[i] + cc.r9 * cd.zw[i]) * Yd[i];
1769 }
1770
1771 if (solve_system (M, a, b)) {
1772 fprintf (stderr, "ncc compute apx: unable to solve system Ma=b\n");
1773 exit (-1);
1774 }
1775
1776 /* update the calibration constants */
1777 cc.f = a.el[0][0];
1778 cc.Tz = a.el[1][0];
1779 cc.kappa1 = 0.0; /* this is the assumption that our calculation was made under */
1780
1781 freemat (M);
1782 freemat (a);
1783 freemat (b);
1784 }
1785
1786
1787 /************************************************************************/
ncc_compute_exact_f_and_Tz_error(m_ptr,n_ptr,params,err)1788 void ncc_compute_exact_f_and_Tz_error (m_ptr, n_ptr, params, err)
1789 integer *m_ptr; /* pointer to number of points to fit */
1790 integer *n_ptr; /* pointer to number of parameters */
1791 doublereal *params; /* vector of parameters */
1792 doublereal *err; /* vector of error from data */
1793 {
1794 int i;
1795
1796 double xc,
1797 yc,
1798 zc,
1799 Xu_1,
1800 Yu_1,
1801 Xu_2,
1802 Yu_2,
1803 distortion_factor,
1804 f,
1805 Tz,
1806 kappa1;
1807
1808 f = params[0];
1809 Tz = params[1];
1810 kappa1 = params[2];
1811
1812 for (i = 0; i < cd.point_count; i++) {
1813 /* convert from world coordinates to camera coordinates */
1814 xc = cc.r1 * cd.xw[i] + cc.r2 * cd.yw[i] + cc.r3 * cd.zw[i] + cc.Tx;
1815 yc = cc.r4 * cd.xw[i] + cc.r5 * cd.yw[i] + cc.r6 * cd.zw[i] + cc.Ty;
1816 zc = cc.r7 * cd.xw[i] + cc.r8 * cd.yw[i] + cc.r9 * cd.zw[i] + Tz;
1817
1818 /* convert from camera coordinates to undistorted sensor coordinates */
1819 Xu_1 = f * xc / zc;
1820 Yu_1 = f * yc / zc;
1821
1822 /* convert from distorted sensor coordinates to undistorted sensor coordinates */
1823 distortion_factor = 1 + kappa1 * r_squared[i];
1824 Xu_2 = Xd[i] * distortion_factor;
1825 Yu_2 = Yd[i] * distortion_factor;
1826
1827 /* record the error in the undistorted sensor coordinates */
1828 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
1829 }
1830 }
1831
1832
ncc_compute_exact_f_and_Tz()1833 void ncc_compute_exact_f_and_Tz ()
1834 {
1835 #define NPARAMS 3
1836
1837 int i;
1838
1839 /* Parameters needed by MINPACK's lmdif() */
1840
1841 integer m = cd.point_count;
1842 integer n = NPARAMS;
1843 doublereal x[NPARAMS];
1844 doublereal *fvec;
1845 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
1846 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
1847 doublereal gtol = ORTHO_TOLERANCE_gtol;
1848 integer maxfev = MAXFEV;
1849 doublereal epsfcn = EPSFCN;
1850 doublereal diag[NPARAMS];
1851 integer mode = MODE;
1852 doublereal factor = FACTOR;
1853 integer nprint = 0;
1854 integer info;
1855 integer nfev;
1856 doublereal *fjac;
1857 integer ldfjac = m;
1858 integer ipvt[NPARAMS];
1859 doublereal qtf[NPARAMS];
1860 doublereal wa1[NPARAMS];
1861 doublereal wa2[NPARAMS];
1862 doublereal wa3[NPARAMS];
1863 doublereal *wa4;
1864
1865 /* allocate some workspace */
1866 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
1867 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
1868 exit(-1);
1869 }
1870
1871 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
1872 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
1873 exit(-1);
1874 }
1875
1876 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
1877 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
1878 exit(-1);
1879 }
1880
1881 /* use the current calibration constants as an initial guess */
1882 x[0] = cc.f;
1883 x[1] = cc.Tz;
1884 x[2] = cc.kappa1;
1885
1886 /* define optional scale factors for the parameters */
1887 if ( mode == 2 ) {
1888 for (i = 0; i < NPARAMS; i++)
1889 diag[i] = 1.0; /* some user-defined values */
1890 }
1891
1892 /* perform the optimization */
1893 lmdif_ (ncc_compute_exact_f_and_Tz_error,
1894 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
1895 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
1896 ipvt, qtf, wa1, wa2, wa3, wa4);
1897
1898 /* update the calibration constants */
1899 cc.f = x[0];
1900 cc.Tz = x[1];
1901 cc.kappa1 = x[2];
1902
1903 /* release allocated workspace */
1904 free (fvec);
1905 free (fjac);
1906 free (wa4);
1907
1908 #ifdef DEBUG
1909 /* print the number of function calls during iteration */
1910 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
1911 #endif
1912
1913 #undef NPARAMS
1914 }
1915
1916
1917 /************************************************************************/
ncc_three_parm_optimization()1918 void ncc_three_parm_optimization ()
1919 {
1920 ncc_compute_Xd_Yd_and_r_squared ();
1921
1922 ncc_compute_U ();
1923
1924 ncc_compute_Tx_and_Ty ();
1925
1926 ncc_compute_sx ();
1927
1928 ncc_compute_Xd_Yd_and_r_squared ();
1929
1930 ncc_compute_better_R ();
1931
1932 ncc_compute_approximate_f_and_Tz ();
1933
1934 if (cc.f < 0) {
1935 /* try the other solution for the orthonormal matrix */
1936 cc.r3 = -cc.r3;
1937 cc.r6 = -cc.r6;
1938 cc.r7 = -cc.r7;
1939 cc.r8 = -cc.r8;
1940 solve_RPY_transform ();
1941
1942 ncc_compute_approximate_f_and_Tz ();
1943
1944 if (cc.f < 0) {
1945 fprintf (stderr, "error - possible handedness problem with data\n");
1946 exit (-1);
1947 }
1948 }
1949
1950 ncc_compute_exact_f_and_Tz ();
1951 }
1952
1953
1954 /************************************************************************/
ncc_nic_optimization_error(m_ptr,n_ptr,params,err)1955 void ncc_nic_optimization_error (m_ptr, n_ptr, params, err)
1956 integer *m_ptr; /* pointer to number of points to fit */
1957 integer *n_ptr; /* pointer to number of parameters */
1958 doublereal *params; /* vector of parameters */
1959 doublereal *err; /* vector of error from data */
1960 {
1961 int i;
1962
1963 double xc,
1964 yc,
1965 zc,
1966 Xd_,
1967 Yd_,
1968 Xu_1,
1969 Yu_1,
1970 Xu_2,
1971 Yu_2,
1972 distortion_factor,
1973 Rx,
1974 Ry,
1975 Rz,
1976 Tx,
1977 Ty,
1978 Tz,
1979 kappa1,
1980 sx,
1981 f,
1982 r1,
1983 r2,
1984 r3,
1985 r4,
1986 r5,
1987 r6,
1988 r7,
1989 r8,
1990 r9,
1991 sa,
1992 sb,
1993 sg,
1994 ca,
1995 cb,
1996 cg;
1997
1998 Rx = params[0];
1999 Ry = params[1];
2000 Rz = params[2];
2001 Tx = params[3];
2002 Ty = params[4];
2003 Tz = params[5];
2004 kappa1 = params[6];
2005 f = params[7];
2006 sx = params[8];
2007
2008 SINCOS (Rx, sa, ca);
2009 SINCOS (Ry, sb, cb);
2010 SINCOS (Rz, sg, cg);
2011 r1 = cb * cg;
2012 r2 = cg * sa * sb - ca * sg;
2013 r3 = sa * sg + ca * cg * sb;
2014 r4 = cb * sg;
2015 r5 = sa * sb * sg + ca * cg;
2016 r6 = ca * sb * sg - cg * sa;
2017 r7 = -sb;
2018 r8 = cb * sa;
2019 r9 = ca * cb;
2020
2021 for (i = 0; i < cd.point_count; i++) {
2022 /* convert from world coordinates to camera coordinates */
2023 xc = r1 * cd.xw[i] + r2 * cd.yw[i] + r3 * cd.zw[i] + Tx;
2024 yc = r4 * cd.xw[i] + r5 * cd.yw[i] + r6 * cd.zw[i] + Ty;
2025 zc = r7 * cd.xw[i] + r8 * cd.yw[i] + r9 * cd.zw[i] + Tz;
2026
2027 /* convert from camera coordinates to undistorted sensor plane coordinates */
2028 Xu_1 = f * xc / zc;
2029 Yu_1 = f * yc / zc;
2030
2031 /* convert from 2D image coordinates to distorted sensor coordinates */
2032 Xd_ = cp.dpx * (cd.Xf[i] - cp.Cx) / sx;
2033 Yd_ = cp.dpy * (cd.Yf[i] - cp.Cy);
2034
2035 /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
2036 distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_));
2037 Xu_2 = Xd_ * distortion_factor;
2038 Yu_2 = Yd_ * distortion_factor;
2039
2040 /* record the error in the undistorted sensor coordinates */
2041 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
2042 }
2043 }
2044
2045
ncc_nic_optimization()2046 void ncc_nic_optimization ()
2047 {
2048 #define NPARAMS 9
2049
2050 int i;
2051
2052 /* Parameters needed by MINPACK's lmdif() */
2053
2054 integer m = cd.point_count;
2055 integer n = NPARAMS;
2056 doublereal x[NPARAMS];
2057 doublereal *fvec;
2058 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
2059 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
2060 doublereal gtol = ORTHO_TOLERANCE_gtol;
2061 integer maxfev = MAXFEV;
2062 doublereal epsfcn = EPSFCN;
2063 doublereal diag[NPARAMS];
2064 integer mode = MODE;
2065 doublereal factor = FACTOR;
2066 integer nprint = 0;
2067 integer info;
2068 integer nfev;
2069 doublereal *fjac;
2070 integer ldfjac = m;
2071 integer ipvt[NPARAMS];
2072 doublereal qtf[NPARAMS];
2073 doublereal wa1[NPARAMS];
2074 doublereal wa2[NPARAMS];
2075 doublereal wa3[NPARAMS];
2076 doublereal *wa4;
2077
2078 /* allocate some workspace */
2079 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
2080 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
2081 exit(-1);
2082 }
2083
2084 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
2085 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
2086 exit(-1);
2087 }
2088
2089 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
2090 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
2091 exit(-1);
2092 }
2093
2094 /* use the current calibration and camera constants as a starting point */
2095 x[0] = cc.Rx;
2096 x[1] = cc.Ry;
2097 x[2] = cc.Rz;
2098 x[3] = cc.Tx;
2099 x[4] = cc.Ty;
2100 x[5] = cc.Tz;
2101 x[6] = cc.kappa1;
2102 x[7] = cc.f;
2103 x[8] = cp.sx;
2104
2105 /* define optional scale factors for the parameters */
2106 if ( mode == 2 ) {
2107 for (i = 0; i < NPARAMS; i++)
2108 diag[i] = 1.0; /* some user-defined values */
2109 }
2110
2111 /* perform the optimization */
2112 lmdif_ (ncc_nic_optimization_error,
2113 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
2114 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
2115 ipvt, qtf, wa1, wa2, wa3, wa4);
2116
2117 /* update the calibration and camera constants */
2118 cc.Rx = x[0];
2119 cc.Ry = x[1];
2120 cc.Rz = x[2];
2121 apply_RPY_transform ();
2122
2123 cc.Tx = x[3];
2124 cc.Ty = x[4];
2125 cc.Tz = x[5];
2126 cc.kappa1 = x[6];
2127 cc.f = x[7];
2128 cp.sx = x[8];
2129
2130 /* release allocated workspace */
2131 free (fvec);
2132 free (fjac);
2133 free (wa4);
2134
2135 #ifdef DEBUG
2136 /* print the number of function calls during iteration */
2137 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
2138 #endif
2139
2140 #undef NPARAMS
2141 }
2142
2143
2144 /************************************************************************/
ncc_full_optimization_error(m_ptr,n_ptr,params,err)2145 void ncc_full_optimization_error (m_ptr, n_ptr, params, err)
2146 integer *m_ptr; /* pointer to number of points to fit */
2147 integer *n_ptr; /* pointer to number of parameters */
2148 doublereal *params; /* vector of parameters */
2149 doublereal *err; /* vector of error from data */
2150 {
2151 int i;
2152
2153 double xc,
2154 yc,
2155 zc,
2156 Xd_,
2157 Yd_,
2158 Xu_1,
2159 Yu_1,
2160 Xu_2,
2161 Yu_2,
2162 distortion_factor,
2163 Rx,
2164 Ry,
2165 Rz,
2166 Tx,
2167 Ty,
2168 Tz,
2169 kappa1,
2170 sx,
2171 f,
2172 Cx,
2173 Cy,
2174 r1,
2175 r2,
2176 r3,
2177 r4,
2178 r5,
2179 r6,
2180 r7,
2181 r8,
2182 r9,
2183 sa,
2184 sb,
2185 sg,
2186 ca,
2187 cb,
2188 cg;
2189
2190 Rx = params[0];
2191 Ry = params[1];
2192 Rz = params[2];
2193 Tx = params[3];
2194 Ty = params[4];
2195 Tz = params[5];
2196 kappa1 = params[6];
2197 f = params[7];
2198 sx = params[8];
2199 Cx = params[9];
2200 Cy = params[10];
2201
2202 SINCOS (Rx, sa, ca);
2203 SINCOS (Ry, sb, cb);
2204 SINCOS (Rz, sg, cg);
2205 r1 = cb * cg;
2206 r2 = cg * sa * sb - ca * sg;
2207 r3 = sa * sg + ca * cg * sb;
2208 r4 = cb * sg;
2209 r5 = sa * sb * sg + ca * cg;
2210 r6 = ca * sb * sg - cg * sa;
2211 r7 = -sb;
2212 r8 = cb * sa;
2213 r9 = ca * cb;
2214
2215 for (i = 0; i < cd.point_count; i++) {
2216 /* convert from world coordinates to camera coordinates */
2217 xc = r1 * cd.xw[i] + r2 * cd.yw[i] + r3 * cd.zw[i] + Tx;
2218 yc = r4 * cd.xw[i] + r5 * cd.yw[i] + r6 * cd.zw[i] + Ty;
2219 zc = r7 * cd.xw[i] + r8 * cd.yw[i] + r9 * cd.zw[i] + Tz;
2220
2221 /* convert from camera coordinates to undistorted sensor plane coordinates */
2222 Xu_1 = f * xc / zc;
2223 Yu_1 = f * yc / zc;
2224
2225 /* convert from 2D image coordinates to distorted sensor coordinates */
2226 Xd_ = cp.dpx * (cd.Xf[i] - Cx) / sx;
2227 Yd_ = cp.dpy * (cd.Yf[i] - Cy);
2228
2229 /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
2230 distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_));
2231 Xu_2 = Xd_ * distortion_factor;
2232 Yu_2 = Yd_ * distortion_factor;
2233
2234 /* record the error in the undistorted sensor coordinates */
2235 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
2236 }
2237 }
2238
2239
ncc_full_optimization()2240 void ncc_full_optimization ()
2241 {
2242 #define NPARAMS 11
2243
2244 int i;
2245
2246 /* Parameters needed by MINPACK's lmdif() */
2247
2248 integer m = cd.point_count;
2249 integer n = NPARAMS;
2250 doublereal x[NPARAMS];
2251 doublereal *fvec;
2252 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
2253 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
2254 doublereal gtol = ORTHO_TOLERANCE_gtol;
2255 integer maxfev = MAXFEV;
2256 doublereal epsfcn = EPSFCN;
2257 doublereal diag[NPARAMS];
2258 integer mode = MODE;
2259 doublereal factor = FACTOR;
2260 integer nprint = 0;
2261 integer info;
2262 integer nfev;
2263 doublereal *fjac;
2264 integer ldfjac = m;
2265 integer ipvt[NPARAMS];
2266 doublereal qtf[NPARAMS];
2267 doublereal wa1[NPARAMS];
2268 doublereal wa2[NPARAMS];
2269 doublereal wa3[NPARAMS];
2270 doublereal *wa4;
2271
2272 /* allocate some workspace */
2273 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
2274 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
2275 exit(-1);
2276 }
2277
2278 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
2279 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
2280 exit(-1);
2281 }
2282
2283 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
2284 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
2285 exit(-1);
2286 }
2287
2288 /* use the current calibration and camera constants as a starting point */
2289 x[0] = cc.Rx;
2290 x[1] = cc.Ry;
2291 x[2] = cc.Rz;
2292 x[3] = cc.Tx;
2293 x[4] = cc.Ty;
2294 x[5] = cc.Tz;
2295 x[6] = cc.kappa1;
2296 x[7] = cc.f;
2297 x[8] = cp.sx;
2298 x[9] = cp.Cx;
2299 x[10] = cp.Cy;
2300
2301 /* define optional scale factors for the parameters */
2302 if ( mode == 2 ) {
2303 for (i = 0; i < NPARAMS; i++)
2304 diag[i] = 1.0; /* some user-defined values */
2305 }
2306
2307 /* perform the optimization */
2308 lmdif_ (ncc_full_optimization_error,
2309 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
2310 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
2311 ipvt, qtf, wa1, wa2, wa3, wa4);
2312
2313 /* update the calibration and camera constants */
2314 cc.Rx = x[0];
2315 cc.Ry = x[1];
2316 cc.Rz = x[2];
2317 apply_RPY_transform ();
2318
2319 cc.Tx = x[3];
2320 cc.Ty = x[4];
2321 cc.Tz = x[5];
2322 cc.kappa1 = x[6];
2323 cc.f = x[7];
2324 cp.sx = x[8];
2325 cp.Cx = x[9];
2326 cp.Cy = x[10];
2327
2328 /* release allocated workspace */
2329 free (fvec);
2330 free (fjac);
2331 free (wa4);
2332
2333 #ifdef DEBUG
2334 /* print the number of function calls during iteration */
2335 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
2336 #endif
2337
2338 #undef NPARAMS
2339 }
2340
2341
2342 /************************************************************************/
2343
coplanar_calibration()2344 void coplanar_calibration ()
2345 {
2346 /* just do the basic 3 parameter (Tz, f, kappa1) optimization */
2347 cc_three_parm_optimization ();
2348 }
2349
2350
coplanar_calibration_with_full_optimization()2351 void coplanar_calibration_with_full_optimization ()
2352 {
2353 /* start with a 3 parameter (Tz, f, kappa1) optimization */
2354 cc_three_parm_optimization ();
2355
2356 /* do a 5 parameter (Tz, f, kappa1, Cx, Cy) optimization */
2357 cc_five_parm_optimization_with_late_distortion_removal ();
2358
2359 /* do a better 5 parameter (Tz, f, kappa1, Cx, Cy) optimization */
2360 cc_five_parm_optimization_with_early_distortion_removal ();
2361
2362 /* do a full optimization minus the image center */
2363 cc_nic_optimization ();
2364
2365 /* do a full optimization including the image center */
2366 cc_full_optimization ();
2367 }
2368
2369
noncoplanar_calibration()2370 void noncoplanar_calibration ()
2371 {
2372 /* just do the basic 3 parameter (Tz, f, kappa1) optimization */
2373 ncc_three_parm_optimization ();
2374 }
2375
2376
noncoplanar_calibration_with_full_optimization()2377 void noncoplanar_calibration_with_full_optimization ()
2378 {
2379 /* start with a 3 parameter (Tz, f, kappa1) optimization */
2380 ncc_three_parm_optimization ();
2381
2382 /* do a full optimization minus the image center */
2383 ncc_nic_optimization ();
2384
2385 /* do a full optimization including the image center */
2386 ncc_full_optimization ();
2387 }
2388