~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

Linux Cross Reference
Tina4/src/vision/calib/scal_smplx.c

Version: ~
Architecture: ~ [ i386 ] ~ [ alpha ] ~ [ m68k ] ~ [ mips ] ~ [ ppc ] ~ [ sparc ] ~ [ sparc64 ] ~

  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_stereo_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_caml;
 38 static Camera *cal_camr;
 39 static int cal_mask;
 40 static List *cal_data;
 41 static Vec3 *(*cal_get_3d) ();
 42 static Vec2 *(*cal_get_pixl) ();
 43 static Vec2 *(*cal_get_pixr) ();
 44 static Covar *cal_in_cov;
 45 
 46 static double pixchisq(int n_par, double *a)
 47 {
 48     double  chisq = MAXDOUBLE;
 49     double *f = NULL;
 50     int     n;
 51 
 52     (void) store_camera_int(cal_mask, a + 6, cal_caml);
 53     (void) store_camera_int(cal_mask, a + n_par / 2 + 6, cal_camr);
 54     if (store_camera_ext(a, cal_caml) &&
 55         store_camera_rel(a + n_par / 2, cal_caml, cal_camr))
 56     {
 57         n = MAXINT;
 58         chisq = camerror(&n, f, cal_caml, cal_data, cal_get_pixl, cal_get_3d, accuracy);
 59         n = MAXINT;
 60         chisq += camerror(&n, f, cal_camr, cal_data, cal_get_pixr, cal_get_3d, accuracy);
 61         chisq += stereo_reg(cal_in_cov, cal_mask, a + 6);
 62     }
 63     return (chisq);
 64 }
 65 
 66 /* inverse covarience */
 67 double  cam_cal_stereo_simplex(Camera * caml, Camera * camr, int mask, List * data, Vec3 * (*get3d) ( /* ??? */ ), Vec2 * (*getpixl) ( /* ??? */ ), Vec2 * (*getpixr) ( /* ??? */ ), Covar * in_cov)
 68 {
 69     double  chisq, chisq_old;
 70     double *a;
 71     int     n_par, n_parm2p12, i;
 72 
 73     if (data == NULL || caml == NULL || camr == NULL)
 74         return (0.0);
 75 
 76     cal_mask = mask;
 77     cal_data = data;
 78     cal_caml = caml;
 79     cal_camr = camr;
 80     cal_get_3d = get3d;
 81     cal_get_pixl = getpixl;
 82     cal_get_pixr = getpixr;
 83     cal_in_cov = in_cov;
 84 
 85     for (i = 0, n_par = 0; i < 16; i++)
 86         if (mask & (1 << i))
 87             n_par++;
 88     n_parm2p12 = 2 * n_par + 12;
 89     a = (double *) ralloc((unsigned) n_parm2p12 * sizeof(double));
 90 
 91     (void) conv_camera_int(mask, caml, a + 6);
 92     (void) conv_camera_ext(caml, a);
 93     (void) conv_camera_int(mask, camr, a + n_par + 12);
 94     (void) conv_camera_rel(caml, camr, a + n_par + 6);
 95 
 96     format_params("Initial camera parameters = ", a, n_parm2p12);
 97 
 98     chisq_old = pixchisq(n_parm2p12, a);
 99     for (i = 0; i < 5; ++i)
100     {
101         chisq = simplexmin(n_parm2p12, a, scale_init, pixchisq, c_test1,
102                            (void (*) ()) format);
103         if (chisq_old - chisq < c_test2)
104             break;
105         chisq_old = chisq;
106     }
107 
108     format_params("Final camera parameters = ", a, n_parm2p12);
109 
110     (void) store_camera_int(mask, a + 6, caml);
111     (void) store_camera_ext(a, caml);
112     (void) store_camera_int(mask, a + n_par + 12, camr);
113     (void) store_camera_rel(a + n_par + 6, caml, camr);
114     rfree((void *) a);
115     return (chisq);
116 }
117 
118 static double stereo_chisq(double *f, int n_data, double *a, int n_par)
119 {
120     double  chisq = MAXDOUBLE;
121     double *g = NULL;
122     int     n = n_data;
123 
124     (void) store_camera_int(cal_mask, a + 6, cal_caml);
125     (void) store_camera_int(cal_mask, a + n_par / 2 + 6, cal_camr);
126     if (store_camera_ext(a, cal_caml) &&
127         store_camera_rel(a + n_par / 2, cal_caml, cal_camr))
128     {
129         chisq = camerror(&n, f, cal_caml, cal_data, cal_get_pixl, cal_get_3d, accuracy);
130         if (f)
131             g = &f[n];
132         chisq += camerror(&n_data, g, cal_camr, cal_data, cal_get_pixr, cal_get_3d, accuracy);
133     }
134     return (chisq);
135 }
136 
137 Covar  *cal_full_stereo_covar(Camera * caml, Camera * camr, int mask, List * data, Vec3 * (*get3d) ( /* ??? */ ), Vec2 * (*getpixl) ( /* ??? */ ), Vec2 * (*getpixr) ( /* ??? */ ))
138 {
139     Covar  *cov;
140     Covar  *incov;
141     double *a;
142     int     n_data1 = MAXINT, n_data2 = MAXINT, n_data;
143     int     n_par, i;
144     double  condition = 1.0e-9;
145 
146     if (data == NULL || caml == NULL || camr == NULL)
147         return (NULL);
148 
149     cal_mask = mask;
150     cal_data = data;
151     cal_caml = caml;
152     cal_camr = camr;
153     cal_get_3d = get3d;
154     cal_get_pixl = getpixl;
155     cal_get_pixr = getpixr;
156 
157     for (i = 0, n_par = 6; i < 16; i++)
158         if (mask & (1 << i))
159             n_par++;
160     a = (double *) ralloc((unsigned) 2 * n_par * sizeof(double));
161 
162     (void) conv_camera_int(mask, caml, a + 6);
163     (void) conv_camera_int(mask, camr, a + n_par + 6);
164     (void) conv_camera_ext(caml, a);
165     (void) conv_camera_rel(caml, camr, a + n_par);
166 
167     /* .  BUGFIX 1 camerror takes 7 args,  added 1.0 BUGFIX 2 chisq
168      * unused */
169     /* BUGFIX 3 --- BUGFIX 2 is incorrect. 
170      *   Yes, chisq is unused.
171      *   However, camerror updates n_data1 & n_data2
172      *   The code has been uncommented and enclosed in a local group
173      *   SDHC 08/12/93
174      */
175     {
176             double  *f = NULL; double  chisq; chisq = camerror(&n_data1, f,
177               caml, data, getpixl, get3d, 1.0);
178             chisq += camerror(&n_data2, f, camr, data, getpixr, get3d, 1.0);
179     }
180     n_data = n_data1 + n_data2;
181 
182     cov = covar(2 * n_par, a, stereo_chisq, n_data, condition);
183     cov->label = mask;
184     incov = invcov(2 * n_par, a, stereo_chisq, n_data);
185     dmatrix_checkinv((void (*) ()) format, cov->mat, incov->mat, "covariance matrix");
186 
187     rfree((void *) a);
188     return (cov);
189 }
190 
191 Covar  *cal_full_stereo_invcovar(int mask, Covar * cov, double condition)
192 {
193     int     n_par, i, j, n;
194     Covar  *stereo_incov;
195     Matrix *subcov;
196 
197     if (cov == NULL)
198         return (NULL);
199 
200     for (i = 0, n_par = 6; i < 16; i++)
201         if (mask & (1 << i))
202             n_par++;
203     n = n_par * 2 - 6;
204 
205     stereo_incov = covar_alloc(n);
206     subcov = matrix_alloc(n, n, matrix_full, double_v);
207     stereo_incov->n = n;
208     stereo_incov->label = mask;
209     stereo_incov->vec = vector_alloc(n, double_v);
210     for (i = 0; i < n; i++)
211     {
212         VECTOR_SET(stereo_incov->vec, i, VECTOR_DOUBLE(cov->vec, i + 6));
213         for (j = 0; j < n; j++)
214             subcov->el.double_v[i][j] = cov->mat->el.double_v[i + 6][j + 6];
215     }
216 
217     stereo_incov->mat = matrix_invsvd(subcov, condition);
218     dmatrix_checkinv((void (*) ()) format, stereo_incov->mat, subcov, "sub covariance inverse");
219     matrix_free(subcov);
220 
221     return (stereo_incov);
222 }
223 

~ [ source navigation ] ~ [ diff markup ] ~ [ identifier search ] ~ [ freetext search ] ~ [ file search ] ~

This page was automatically generated by the LXR engine.
Visit the LXR main site for more information.