1 /**@(#)
2 **/
3 #ifdef TRANSPUTER
4 #include <valuesdual.h>
5 #else
6 #include <values.h>
7 #endif
8 #include <math.h>
9 #include <tina/sys.h>
10 #include <tina/sysfuncs.h>
11 #include <tina/math.h>
12 #include <tina/mathfuncs.h>
13 #include <tina/vision.h>
14 #include <tina/visionfuncs.h>
15
16 static double scale_init = 0.04, c_test1 = 0.00001, c_test2 = 0.1;
17 static double accuracy = 1.0;
18
19 void cam_cal_smplx_params_set(double s_init, double c1, double c2, double a)
20 {
21 scale_init = s_init;
22 c_test1 = c1;
23 c_test2 = c2;
24 accuracy = a;
25 }
26
27 static void format_params(char *string, double *a, int n)
28 {
29 int i;
30
31 format(string);
32 for (i = 0; i < n; i++)
33 format("%f ", a[i]);
34 format("\n");
35 }
36
37 static Camera *cal_cam;
38 static int cal_mask;
39 static List *cal_data;
40 static Vec3 *(*cal_get_3d) ();
41 static Vec2 *(*cal_get_pix) ();
42 static Covar *cal_in_cov;
43
44 static double pixchisq(int n_par, double *a)
45 /* BUG n_par unused */
46
47
48 {
49 double chisq = MAXDOUBLE;
50 double *f = NULL;
51 int n_data = MAXINT;
52
53 (void) store_camera_int(cal_mask, a + 6, cal_cam);
54 if (store_camera_ext(a, cal_cam))
55 {
56 chisq = camerror(&n_data, f, cal_cam, cal_data, cal_get_pix, cal_get_3d, accuracy);
57 chisq += cam_reg(cal_in_cov, cal_mask, a);
58 }
59 return (chisq);
60 }
61
62 double cam_cal_simplex(Camera * cam, int mask, List * data, Vec3 * (*get_3d) ( /* ??? */ ), Vec2 * (*get_pix) ( /* ??? */ ), Covar * in_cov)
63
64
65
66
67
68 /* inverse covarience */
69 {
70 double chisq, chisq_old;
71 double *a;
72 int n_par, i;
73
74 if (data == NULL || cam == NULL)
75 return (0.0);
76
77 cal_mask = mask;
78 cal_data = data;
79 cal_cam = cam;
80 cal_get_3d = get_3d;
81 cal_get_pix = get_pix;
82 cal_in_cov = in_cov;
83
84 for (i = 0, n_par = 6; i < 16; i++)
85 if (mask & (1 << i))
86 n_par++;
87 a = (double *) ralloc((unsigned) n_par * sizeof(double));
88
89 (void) conv_camera_int(mask, cam, a + 6);
90 (void) conv_camera_ext(cam, a);
91
92 format_params("Initial camera parameters = ", a, n_par);
93
94 chisq_old = pixchisq(n_par, a);
95 for (i = 0; i < 5; ++i)
96 {
97 chisq = simplexmin(n_par, a, scale_init, pixchisq, c_test1,
98 (void (*) ()) format);
99 if (chisq_old - chisq < c_test2)
100 break;
101 chisq_old = chisq;
102 }
103
104 format_params("Final camera parameters = ", a, n_par);
105
106 (void) store_camera_int(mask, a + 6, cam);
107 (void) store_camera_ext(a, cam);
108 rfree((void *) a);
109 return (chisq);
110 }
111
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.