1 /*******************************************************************************\
2 * *
3 * This file contains routines for calibrating the extrinsic parameters of *
4 * Tsai's 11 parameter camera model. The inputs to the routines are a set of *
5 * precalibrated intrinsic camera parameters: *
6 * *
7 * f - effective focal length of the pin hole camera *
8 * kappa1 - 1st order radial lens distortion coefficient *
9 * Cx, Cy - coordinates of center of radial lens distortion *
10 * (also used as the piercing point of the camera coordinate *
11 * frame's Z axis with the camera's sensor plane) *
12 * sx - uncertainty factor for scale of horizontal scanline *
13 * *
14 * and a set of calibration data consisting of the (x,y,z) world coordinates of *
15 * a feature point (in mm) and the corresponding coordinates (Xf,Yf) (in pixels) *
16 * of the feature point in the image. The outputs of the routines are the 6 *
17 * external (also called extrinsic or exterior) camera parameters: *
18 * *
19 * Rx, Ry, Rz, Tx, Ty, Tz - rotational and translational components of *
20 * the transform between the world's coordinate *
21 * frame and the camera's coordinate frame. *
22 * *
23 * describing the camera's pose. *
24 * *
25 * This file provides two routines: *
26 * *
27 * coplanar_extrinsic_parameter_estimation () *
28 * and *
29 * noncoplanar_extrinsic_parameter_estimation () *
30 * *
31 * which are used respectively for coplanar and non-coplanar calibration data. *
32 * *
33 * Initial estimates for the extrinsic camera parameters are determined using a *
34 * modification of the first stage of Tsai's algorithm. These estimates are *
35 * then refined using iterative non-linear optimization. *
36 * *
37 * *
38 * History *
39 * ------- *
40 * *
41 * 15-Oct-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN *
42 * Added routines to coplanar_extrinsic_parameter_estimation to pick *
43 * the correct rotation matrix solution from the two possible solutions. *
44 * Bug tracked down by Pete Rander <Peter.Rander@IUS4.IUS.CS.CMU.EDU>. *
45 * *
46 * 20-May-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN *
47 * Return the error to lmdif rather than the squared error. *
48 * lmdif calculates the squared error internally during optimization. *
49 * Before this change calibration was essentially optimizing error^4. *
50 * *
51 * 02-Apr-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN *
52 * Rewrite memory allocation to avoid memory alignment problems *
53 * on some machines. *
54 * Strip out IMSL code. MINPACK seems to work fine. *
55 * Filename changes for DOS port. *
56 * *
57 * 04-Jun-94 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN *
58 * Added alternate macro definitions for less common math functions. *
59 * *
60 * 25-Mar-94 Torfi Thorhallsson (torfit@verk.hi.is) at the University of Iceland*
61 * Added a new version of the routine epe_optimize() which uses the *
62 * *public domain* MINPACK optimization library instead of IMSL. *
63 * To select the new routine, compile this file with the flag -DMINPACK *
64 * *
65 * 11-Nov-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University *
66 * Original implementation. *
67 * *
68 \*******************************************************************************/
69
70 #include <stdlib.h>
71 #include <stdio.h>
72 #include <math.h>
73 #include <errno.h>
74 #include "matrix/matrix.h"
75 #include "cal_main.h"
76 #include "minpack/f2c.h"
77
78
79 /***********************************************************************\
80 * Routines for coplanar extrinsic parameter estimation *
81 \***********************************************************************/
cepe_compute_U(U)82 void cepe_compute_U (U)
83 double U[];
84 {
85 dmat M,
86 a,
87 b;
88
89 double Xd,
90 Yd,
91 Xu,
92 Yu,
93 distortion_factor;
94
95 int i;
96
97 M = newdmat (0, (cd.point_count - 1), 0, 4, &errno);
98 if (errno) {
99 fprintf (stderr, "cepe compute U: unable to allocate matrix M\n");
100 exit (-1);
101 }
102
103 a = newdmat (0, 4, 0, 0, &errno);
104 if (errno) {
105 fprintf (stderr, "cepe compute U: unable to allocate vector a\n");
106 exit (-1);
107 }
108
109 b = newdmat (0, (cd.point_count - 1), 0, 0, &errno);
110 if (errno) {
111 fprintf (stderr, "cepe compute U: unable to allocate vector b\n");
112 exit (-1);
113 }
114
115 for (i = 0; i < cd.point_count; i++) {
116 /* convert from image coordinates to distorted sensor coordinates */
117 Xd = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx;
118 Yd = cp.dpy * (cd.Yf[i] - cp.Cy);
119
120 /* convert from distorted sensor coordinates to undistorted sensor coordinates */
121 distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd));
122 Xu = Xd * distortion_factor;
123 Yu = Yd * distortion_factor;
124
125 M.el[i][0] = Yu * cd.xw[i];
126 M.el[i][1] = Yu * cd.yw[i];
127 M.el[i][2] = Yu;
128 M.el[i][3] = -Xu * cd.xw[i];
129 M.el[i][4] = -Xu * cd.yw[i];
130 b.el[i][0] = Xu;
131 }
132
133 if (solve_system (M, a, b)) {
134 fprintf (stderr, "cepe compute U: unable to solve system Ma=b\n");
135 exit (-1);
136 }
137
138 U[0] = a.el[0][0];
139 U[1] = a.el[1][0];
140 U[2] = a.el[2][0];
141 U[3] = a.el[3][0];
142 U[4] = a.el[4][0];
143
144 freemat (M);
145 freemat (a);
146 freemat (b);
147 }
148
149
cepe_compute_Tx_and_Ty(U)150 void cepe_compute_Tx_and_Ty (U)
151 double U[];
152 {
153 double Tx,
154 Ty,
155 Ty_squared,
156 x,
157 y,
158 Sr,
159 r1p,
160 r2p,
161 r4p,
162 r5p,
163 r1,
164 r2,
165 r4,
166 r5,
167 distance,
168 far_distance;
169
170 int i,
171 far_point;
172
173 r1p = U[0];
174 r2p = U[1];
175 r4p = U[3];
176 r5p = U[4];
177
178 /* first find the square of the magnitude of Ty */
179 if ((fabs (r1p) < EPSILON) && (fabs (r2p) < EPSILON))
180 Ty_squared = 1 / (SQR (r4p) + SQR (r5p));
181 else if ((fabs (r4p) < EPSILON) && (fabs (r5p) < EPSILON))
182 Ty_squared = 1 / (SQR (r1p) + SQR (r2p));
183 else if ((fabs (r1p) < EPSILON) && (fabs (r4p) < EPSILON))
184 Ty_squared = 1 / (SQR (r2p) + SQR (r5p));
185 else if ((fabs (r2p) < EPSILON) && (fabs (r5p) < EPSILON))
186 Ty_squared = 1 / (SQR (r1p) + SQR (r4p));
187 else {
188 Sr = SQR (r1p) + SQR (r2p) + SQR (r4p) + SQR (r5p);
189 Ty_squared = (Sr - sqrt (SQR (Sr) - 4 * SQR (r1p * r5p - r4p * r2p))) /
190 (2 * SQR (r1p * r5p - r4p * r2p));
191 }
192
193 /* find a point that is far from the image center */
194 far_distance = 0;
195 far_point = 0;
196 for (i = 0; i < cd.point_count; i++)
197 if ((distance = SQR (cd.Xf[i] - cp.Cx) + SQR (cd.Yf[i] - cp.Cy)) > far_distance) {
198 far_point = i;
199 far_distance = distance;
200 }
201
202 /* now find the sign for Ty */
203 /* start by assuming Ty > 0 */
204 Ty = sqrt (Ty_squared);
205 r1 = U[0] * Ty;
206 r2 = U[1] * Ty;
207 Tx = U[2] * Ty;
208 r4 = U[3] * Ty;
209 r5 = U[4] * Ty;
210 x = r1 * cd.xw[far_point] + r2 * cd.yw[far_point] + Tx;
211 y = r4 * cd.xw[far_point] + r5 * cd.yw[far_point] + Ty;
212
213 /* flip Ty if we guessed wrong */
214 if ((SIGNBIT (x) != SIGNBIT (cd.Xf[far_point] - cp.Cx)) ||
215 (SIGNBIT (y) != SIGNBIT (cd.Yf[far_point] - cp.Cy)))
216 Ty = -Ty;
217
218 /* update the calibration constants */
219 cc.Tx = U[2] * Ty;
220 cc.Ty = Ty;
221 }
222
223
cepe_compute_R(U)224 void cepe_compute_R (U)
225 double U[];
226 {
227 double r1,
228 r2,
229 r3,
230 r4,
231 r5,
232 r6,
233 r7,
234 r8,
235 r9;
236
237 r1 = U[0] * cc.Ty;
238 r2 = U[1] * cc.Ty;
239 r3 = sqrt (1 - SQR (r1) - SQR (r2));
240
241 r4 = U[3] * cc.Ty;
242 r5 = U[4] * cc.Ty;
243 r6 = sqrt (1 - SQR (r4) - SQR (r5));
244 if (!SIGNBIT (r1 * r4 + r2 * r5))
245 r6 = -r6;
246
247 /* use the outer product of the first two rows to get the last row */
248 r7 = r2 * r6 - r3 * r5;
249 r8 = r3 * r4 - r1 * r6;
250 r9 = r1 * r5 - r2 * r4;
251
252 /* update the calibration constants */
253 cc.r1 = r1;
254 cc.r2 = r2;
255 cc.r3 = r3;
256 cc.r4 = r4;
257 cc.r5 = r5;
258 cc.r6 = r6;
259 cc.r7 = r7;
260 cc.r8 = r8;
261 cc.r9 = r9;
262
263 /* fill in cc.Rx, cc.Ry and cc.Rz */
264 solve_RPY_transform ();
265 }
266
267
cepe_compute_approximate_f(f)268 void cepe_compute_approximate_f (f)
269 double *f;
270 {
271 dmat M,
272 a,
273 b;
274
275 double Yd;
276
277 int i;
278
279 M = newdmat (0, (cd.point_count - 1), 0, 1, &errno);
280 if (errno) {
281 fprintf (stderr, "cepe compute apx: unable to allocate matrix M\n");
282 exit (-1);
283 }
284
285 a = newdmat (0, 1, 0, 0, &errno);
286 if (errno) {
287 fprintf (stderr, "cepe compute apx: unable to allocate vector a\n");
288 exit (-1);
289 }
290
291 b = newdmat (0, (cd.point_count - 1), 0, 0, &errno);
292 if (errno) {
293 fprintf (stderr, "cepe compute apx: unable to allocate vector b\n");
294 exit (-1);
295 }
296
297 for (i = 0; i < cd.point_count; i++) {
298 Yd = cp.dpy * (cd.Yf[i] - cp.Cy);
299
300 M.el[i][0] = cc.r4 * cd.xw[i] + cc.r5 * cd.yw[i] + cc.Ty;
301 M.el[i][1] = -Yd;
302 b.el[i][0] = (cc.r7 * cd.xw[i] + cc.r8 * cd.yw[i]) * Yd;
303 }
304
305 if (solve_system (M, a, b)) {
306 fprintf (stderr, "cepe compute apx: unable to solve system Ma=b\n");
307 exit (-1);
308 }
309
310 /* return the approximate effective focal length */
311 *f = a.el[0][0];
312
313 freemat (M);
314 freemat (a);
315 freemat (b);
316 }
317
318
319 /***********************************************************************\
320 * Routines for noncoplanar extrinsic parameter estimation *
321 \***********************************************************************/
ncepe_compute_U(U)322 void ncepe_compute_U (U)
323 double U[];
324 {
325 dmat M,
326 a,
327 b;
328
329 double Xu,
330 Yu,
331 Xd,
332 Yd,
333 distortion_factor;
334
335 int i;
336
337 M = newdmat (0, (cd.point_count - 1), 0, 6, &errno);
338 if (errno) {
339 fprintf (stderr, "ncepe compute U: unable to allocate matrix M\n");
340 exit (-1);
341 }
342
343 a = newdmat (0, 6, 0, 0, &errno);
344 if (errno) {
345 fprintf (stderr, "ncepe compute U: unable to allocate vector a\n");
346 exit (-1);
347 }
348
349 b = newdmat (0, (cd.point_count - 1), 0, 0, &errno);
350 if (errno) {
351 fprintf (stderr, "ncepe compute U: unable to allocate vector b\n");
352 exit (-1);
353 }
354
355 for (i = 0; i < cd.point_count; i++) {
356 /* convert from image coordinates to distorted sensor coordinates */
357 Xd = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx;
358 Yd = cp.dpy * (cd.Yf[i] - cp.Cy);
359
360 /* convert from distorted sensor coordinates to undistorted sensor coordinates */
361 distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd));
362 Xu = Xd * distortion_factor;
363 Yu = Yd * distortion_factor;
364
365 M.el[i][0] = Yu * cd.xw[i];
366 M.el[i][1] = Yu * cd.yw[i];
367 M.el[i][2] = Yu * cd.zw[i];
368 M.el[i][3] = Yu;
369 M.el[i][4] = -Xu * cd.xw[i];
370 M.el[i][5] = -Xu * cd.yw[i];
371 M.el[i][6] = -Xu * cd.zw[i];
372 b.el[i][0] = Xu;
373 }
374
375 if (solve_system (M, a, b)) {
376 fprintf (stderr, "ncepe compute U: unable to solve system Ma=b\n");
377 exit (-1);
378 }
379
380 U[0] = a.el[0][0];
381 U[1] = a.el[1][0];
382 U[2] = a.el[2][0];
383 U[3] = a.el[3][0];
384 U[4] = a.el[4][0];
385 U[5] = a.el[5][0];
386 U[6] = a.el[6][0];
387
388 freemat (M);
389 freemat (a);
390 freemat (b);
391 }
392
393
ncepe_compute_Tx_and_Ty(U)394 void ncepe_compute_Tx_and_Ty (U)
395 double U[];
396 {
397 double Tx,
398 Ty,
399 Ty_squared,
400 x,
401 y,
402 r1,
403 r2,
404 r3,
405 r4,
406 r5,
407 r6,
408 distance,
409 far_distance;
410
411 int i,
412 far_point;
413
414 /* first find the square of the magnitude of Ty */
415 Ty_squared = 1 / (SQR (U[4]) + SQR (U[5]) + SQR (U[6]));
416
417 /* find a point that is far from the image center */
418 far_distance = 0;
419 far_point = 0;
420 for (i = 0; i < cd.point_count; i++)
421 if ((distance = SQR (cd.Xf[i] - cp.Cx) + SQR (cd.Yf[i] - cp.Cy)) > far_distance) {
422 far_point = i;
423 far_distance = distance;
424 }
425
426 /* now find the sign for Ty */
427 /* start by assuming Ty > 0 */
428 Ty = sqrt (Ty_squared);
429 r1 = U[0] * Ty;
430 r2 = U[1] * Ty;
431 r3 = U[2] * Ty;
432 Tx = U[3] * Ty;
433 r4 = U[4] * Ty;
434 r5 = U[5] * Ty;
435 r6 = U[6] * Ty;
436 x = r1 * cd.xw[far_point] + r2 * cd.yw[far_point] + r3 * cd.zw[far_point] + Tx;
437 y = r4 * cd.xw[far_point] + r5 * cd.yw[far_point] + r6 * cd.zw[far_point] + Ty;
438
439 /* flip Ty if we guessed wrong */
440 if ((SIGNBIT (x) != SIGNBIT (cd.Xf[far_point] - cp.Cx)) ||
441 (SIGNBIT (y) != SIGNBIT (cd.Yf[far_point] - cp.Cy)))
442 Ty = -Ty;
443
444 /* update the calibration constants */
445 cc.Tx = U[3] * Ty;
446 cc.Ty = Ty;
447 }
448
449
ncepe_compute_R(U)450 void ncepe_compute_R (U)
451 double U[];
452 {
453 double r1,
454 r2,
455 r3,
456 r4,
457 r5,
458 r6,
459 r7,
460 r8,
461 r9;
462
463 r1 = U[0] * cc.Ty;
464 r2 = U[1] * cc.Ty;
465 r3 = U[2] * cc.Ty;
466
467 r4 = U[4] * cc.Ty;
468 r5 = U[5] * cc.Ty;
469 r6 = U[6] * cc.Ty;
470
471 /* use the outer product of the first two rows to get the last row */
472 r7 = r2 * r6 - r3 * r5;
473 r8 = r3 * r4 - r1 * r6;
474 r9 = r1 * r5 - r2 * r4;
475
476 /* update the calibration constants */
477 cc.r1 = r1;
478 cc.r2 = r2;
479 cc.r3 = r3;
480 cc.r4 = r4;
481 cc.r5 = r5;
482 cc.r6 = r6;
483 cc.r7 = r7;
484 cc.r8 = r8;
485 cc.r9 = r9;
486
487 /* fill in cc.Rx, cc.Ry and cc.Rz */
488 solve_RPY_transform ();
489 }
490
491
492 /************************************************************************/
epe_compute_Tx_Ty_Tz()493 void epe_compute_Tx_Ty_Tz ()
494 {
495 dmat M,
496 a,
497 b;
498
499 double xk,
500 yk,
501 zk,
502 Xu,
503 Yu,
504 Xd,
505 Yd,
506 distortion_factor;
507
508 int i,
509 j;
510
511 M = newdmat (0, (2 * cd.point_count - 1), 0, 2, &errno);
512 if (errno) {
513 fprintf (stderr, "epe compute Tx Ty Tz: unable to allocate matrix M\n");
514 exit (-1);
515 }
516
517 a = newdmat (0, 2, 0, 0, &errno);
518 if (errno) {
519 fprintf (stderr, "epe compute Tx Ty Tz: unable to allocate vector a\n");
520 exit (-1);
521 }
522
523 b = newdmat (0, (2 * cd.point_count - 1), 0, 0, &errno);
524 if (errno) {
525 fprintf (stderr, "epe compute Tx Ty Tz: unable to allocate vector b\n");
526 exit (-1);
527 }
528
529 for (i = 0, j = cd.point_count; i < cd.point_count; i++, j++) {
530 /* convert from world coordinates to untranslated camera coordinates */
531 xk = cc.r1 * cd.xw[i] + cc.r2 * cd.yw[i] + cc.r3 * cd.zw[i];
532 yk = cc.r4 * cd.xw[i] + cc.r5 * cd.yw[i] + cc.r6 * cd.zw[i];
533 zk = cc.r7 * cd.xw[i] + cc.r8 * cd.yw[i] + cc.r9 * cd.zw[i];
534
535 /* convert from image coordinates to distorted sensor coordinates */
536 Xd = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx;
537 Yd = cp.dpy * (cd.Yf[i] - cp.Cy);
538
539 /* convert from distorted sensor coordinates to undistorted sensor coordinates */
540 distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd));
541 Xu = Xd * distortion_factor;
542 Yu = Yd * distortion_factor;
543
544 M.el[i][0] = cc.f;
545 M.el[i][1] = 0;
546 M.el[i][2] = -Xu;
547 b.el[i][0] = Xu * zk - cc.f * xk;
548
549 M.el[j][0] = 0;
550 M.el[j][1] = cc.f;
551 M.el[j][2] = -Yu;
552 b.el[j][0] = Yu * zk - cc.f * yk;
553 }
554
555 if (solve_system (M, a, b)) {
556 fprintf (stderr, "epe compute Tx Ty Tz: unable to solve system Ma=b\n");
557 exit (-1);
558 }
559
560 cc.Tx = a.el[0][0];
561 cc.Ty = a.el[1][0];
562 cc.Tz = a.el[2][0];
563
564 freemat (M);
565 freemat (a);
566 freemat (b);
567 }
568
569
570 /************************************************************************/
epe_optimize_error(m_ptr,n_ptr,params,err)571 void epe_optimize_error (m_ptr, n_ptr, params, err)
572 integer *m_ptr; /* pointer to number of points to fit */
573 integer *n_ptr; /* pointer to number of parameters */
574 doublereal *params; /* vector of parameters */
575 doublereal *err; /* vector of error from data */
576 {
577 int i;
578
579 double xc,
580 yc,
581 zc,
582 Xd,
583 Yd,
584 Xu_1,
585 Yu_1,
586 Xu_2,
587 Yu_2,
588 distortion_factor,
589 Rx,
590 Ry,
591 Rz,
592 Tx,
593 Ty,
594 Tz,
595 r1,
596 r2,
597 r3,
598 r4,
599 r5,
600 r6,
601 r7,
602 r8,
603 r9,
604 sa,
605 sb,
606 sg,
607 ca,
608 cb,
609 cg;
610
611 Rx = params[0];
612 Ry = params[1];
613 Rz = params[2];
614 Tx = params[3];
615 Ty = params[4];
616 Tz = params[5];
617
618 SINCOS (Rx, sa, ca);
619 SINCOS (Ry, sb, cb);
620 SINCOS (Rz, sg, cg);
621
622 r1 = cb * cg;
623 r2 = cg * sa * sb - ca * sg;
624 r3 = sa * sg + ca * cg * sb;
625 r4 = cb * sg;
626 r5 = sa * sb * sg + ca * cg;
627 r6 = ca * sb * sg - cg * sa;
628 r7 = -sb;
629 r8 = cb * sa;
630 r9 = ca * cb;
631
632 for (i = 0; i < cd.point_count; i++) {
633 /* convert from world coordinates to camera coordinates */
634 xc = r1 * cd.xw[i] + r2 * cd.yw[i] + r3 * cd.zw[i] + Tx;
635 yc = r4 * cd.xw[i] + r5 * cd.yw[i] + r6 * cd.zw[i] + Ty;
636 zc = r7 * cd.xw[i] + r8 * cd.yw[i] + r9 * cd.zw[i] + Tz;
637
638 /* convert from camera coordinates to undistorted sensor plane coordinates */
639 Xu_1 = cc.f * xc / zc;
640 Yu_1 = cc.f * yc / zc;
641
642 /* convert from 2D image coordinates to distorted sensor coordinates */
643 Xd = cp.dpx * (cd.Xf[i] - cp.Cx) / cp.sx;
644 Yd = cp.dpy * (cd.Yf[i] - cp.Cy);
645
646 /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */
647 distortion_factor = 1 + cc.kappa1 * (SQR (Xd) + SQR (Yd));
648 Xu_2 = Xd * distortion_factor;
649 Yu_2 = Yd * distortion_factor;
650
651 /* record the error in the undistorted sensor coordinates */
652 err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2);
653 }
654 }
655
656
epe_optimize()657 void epe_optimize ()
658 {
659 #define NPARAMS 6
660
661 int i;
662
663 /* Parameters needed by MINPACK's lmdif() */
664
665 integer m = cd.point_count;
666 integer n = NPARAMS;
667 doublereal x[NPARAMS];
668 doublereal *fvec;
669 doublereal ftol = REL_SENSOR_TOLERANCE_ftol;
670 doublereal xtol = REL_PARAM_TOLERANCE_xtol;
671 doublereal gtol = ORTHO_TOLERANCE_gtol;
672 integer maxfev = MAXFEV;
673 doublereal epsfcn = EPSFCN;
674 doublereal diag[NPARAMS];
675 integer mode = MODE;
676 doublereal factor = FACTOR;
677 integer nprint = 0;
678 integer info;
679 integer nfev;
680 doublereal *fjac;
681 integer ldfjac = m;
682 integer ipvt[NPARAMS];
683 doublereal qtf[NPARAMS];
684 doublereal wa1[NPARAMS];
685 doublereal wa2[NPARAMS];
686 doublereal wa3[NPARAMS];
687 doublereal *wa4;
688
689 /* allocate some workspace */
690 if (( fvec = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
691 fprintf(stderr,"calloc: Cannot allocate workspace fvec\n");
692 exit(-1);
693 }
694
695 if (( fjac = (doublereal *) calloc ((unsigned int) m*n, (unsigned int) sizeof(doublereal))) == NULL ) {
696 fprintf(stderr,"calloc: Cannot allocate workspace fjac\n");
697 exit(-1);
698 }
699
700 if (( wa4 = (doublereal *) calloc ((unsigned int) m, (unsigned int) sizeof(doublereal))) == NULL ) {
701 fprintf(stderr,"calloc: Cannot allocate workspace wa4\n");
702 exit(-1);
703 }
704
705 /* use the current calibration and camera constants as a starting point */
706 x[0] = cc.Rx;
707 x[1] = cc.Ry;
708 x[2] = cc.Rz;
709 x[3] = cc.Tx;
710 x[4] = cc.Ty;
711 x[5] = cc.Tz;
712
713 /* define optional scale factors for the parameters */
714 if ( mode == 2 ) {
715 for (i = 0; i < NPARAMS; i++)
716 diag[i] = 1.0; /* some user-defined values */
717 }
718
719 /* perform the optimization */
720 lmdif_ (epe_optimize_error,
721 &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn,
722 diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac,
723 ipvt, qtf, wa1, wa2, wa3, wa4);
724
725 /* update the calibration and camera constants */
726 cc.Rx = x[0];
727 cc.Ry = x[1];
728 cc.Rz = x[2];
729 apply_RPY_transform ();
730
731 cc.Tx = x[3];
732 cc.Ty = x[4];
733 cc.Tz = x[5];
734
735 /* release allocated workspace */
736 free (fvec);
737 free (fjac);
738 free (wa4);
739
740 #ifdef DEBUG
741 /* print the number of function calls during iteration */
742 fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev);
743 #endif
744
745 #undef NPARAMS
746 }
747
748
749 /************************************************************************/
coplanar_extrinsic_parameter_estimation()750 void coplanar_extrinsic_parameter_estimation ()
751 {
752 double trial_f,
753 U[5];
754
755 cepe_compute_U (U);
756
757 cepe_compute_Tx_and_Ty (U);
758
759 cepe_compute_R (U);
760
761 cepe_compute_approximate_f (&trial_f);
762
763 if (trial_f < 0) {
764 /* try the other rotation matrix solution */
765 cc.r3 = -cc.r3;
766 cc.r6 = -cc.r6;
767 cc.r7 = -cc.r7;
768 cc.r8 = -cc.r8;
769 solve_RPY_transform ();
770
771 cepe_compute_approximate_f (&trial_f);
772
773 if (trial_f < 0) {
774 fprintf (stderr, "error - possible handedness problem with data\n");
775 exit (-1);
776 }
777 }
778
779 epe_compute_Tx_Ty_Tz ();
780
781 epe_optimize ();
782 }
783
784
785 /************************************************************************/
noncoplanar_extrinsic_parameter_estimation()786 void noncoplanar_extrinsic_parameter_estimation ()
787 {
788 double U[7];
789
790 ncepe_compute_U (U);
791
792 ncepe_compute_Tx_and_Ty (U);
793
794 ncepe_compute_R (U);
795
796 epe_compute_Tx_Ty_Tz ();
797
798 epe_optimize ();
799 }
800