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

Linux Cross Reference
Tina4/src/vision/calib/cam_error.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 /* routine for calculating summed squared error in the image plane for
 17  * the first n_data points stored in the world3d matched structure NAT
 18  * 6/2/91     */
 19 double  camerror(int *n_data, double *f, Camera * cam, List * world3d, Vec2 * (*pix_get) ( /* ??? */ ), Vec3 * (*world_get) ( /* ??? */ ), double accuracy)
 20 {
 21     List   *l;
 22     Vec2    e = {Vec2_id};
 23     Vec2    cam_proj();
 24     Vec2   *c;
 25     Vec3   *v;
 26     double  diffx, diffy;
 27     double  errorsq = 0.0;
 28     int     i = 0;
 29 
 30     for (l = world3d; l != NULL; l = l->next)
 31     {
 32         if ((c = pix_get(l)) && (v = world_get(l)))
 33         {
 34             e = cam_proj(cam, *v);
 35             diffx = e.el[0] - c->el[0];
 36             diffy = e.el[1] - c->el[1];
 37             if (diffx > 3.0 * accuracy)
 38                 diffx = 3.0 * accuracy;
 39             if (diffx < -3.0 * accuracy)
 40                 diffx = -3.0 * accuracy;
 41             if (diffy > 3.0 * accuracy)
 42                 diffy = 3.0 * accuracy;
 43             if (diffy < -3.0 * accuracy)
 44                 diffy = -3.0 * accuracy;
 45             errorsq += diffx * diffx + diffy * diffy;
 46             if (f != NULL)
 47             {
 48                 f[i++] = diffx;
 49                 f[i++] = diffy;
 50             } else
 51                 i += 2;
 52             if (i == *n_data)
 53                 break;
 54         }
 55     }
 56     *n_data = i;
 57     if (errorsq > 0.0 || errorsq >= 0.0)
 58         return (errorsq);
 59     else
 60         return (MAXDOUBLE);
 61 }
 62 
 63 /* routine for calculating summed squared error from a fixed set of 3D
 64    distributed across a feature potential image           NAT 20/8/2000 */
 65 double pot_camerror(int *n_data, double *f, Camera *cam, List *world3d,
 66                     double (*pix_get) ( /* ??? */ ),
 67                     Vec3 * (*world_get) ( /* ??? */ ), double accuracy)
 68 {
 69     List   *l;
 70     Vec2    e = {Vec2_id};
 71     Vec3   *v;
 72     Vec2    cam_proj();
 73     double  diff, errorsq = 0.0;
 74     int     i = 0;
 75 
 76     for (l = world3d; l != NULL; l = l->next)
 77     {
 78        if( v = world_get(l))
 79        {
 80            e = cam_proj(cam, *v);
 81            diff = pix_get(e);
 82            errorsq += diff*diff;
 83            if (f!= NULL)
 84               f[i] = diff;   
 85            i++;
 86            if (i == *n_data)
 87                break; 
 88        } 
 89     } 
 90     *n_data = i;
 91     if (errorsq > 0.0 || errorsq >= 0.0)
 92         return (errorsq);
 93     else
 94         return (MAXDOUBLE);
 95 }
 96 
 97 /* routine for calculating summed squared error in the image plane for
 98  * the first n_data points stored in the world3d matched structure NAT
 99  * 6/2/91 */
100 double  rad_camerror(int *n_data, double *rad, double *err, Camera * cam, List * world3d, Vec2 * (*pix_get) ( /* ??? */ ), Vec3 * (*world_get) ( /* ??? */ ))
101 {
102     List   *l;
103     Vec2    e = {Vec2_id};
104     Vec2    cam_proj();
105     Vec2   *c;
106     Vec3   *v;
107     double  diff;
108     double  errorsq = 0.0;
109     double  rad1, rad2;
110     int     i = 0;
111 
112     for (l = world3d; l != NULL; l = l->next)
113     {
114         if ((c = pix_get(l)) && (v = world_get(l)))
115         {
116             e = cam_proj(cam, *v);
117             e.el[0] -= cam->cx;
118             e.el[1] -= cam->cy;
119             rad1 = e.el[0] * e.el[0] / (cam->ax * cam->ax)
120                 + e.el[1] * e.el[1] / (cam->ay * cam->ay);
121             rad1 = sqrt(rad1);
122             rad2 = (c->el[0] - cam->cx) * (c->el[0] - cam->cx) / (cam->ax * cam->ax)
123                 + (c->el[1] - cam->cy) * (c->el[1] - cam->cy) / (cam->ay * cam->ay);
124             rad2 = sqrt(rad2);
125             diff = rad1 - rad2;
126             errorsq += diff * diff;
127             if (rad != NULL && err != NULL)
128             {
129                 rad[i] = rad1;
130                 err[i++] = diff;
131             } else
132                 i++;
133             if (i == *n_data)
134                 break;
135         }
136     }
137     *n_data = i;
138     if (errorsq > 0.0 || errorsq >= 0.0)
139         return (errorsq);
140     else
141         return (MAXDOUBLE);
142 }
143 
144 
145 /* calculates the total off-epipolar error for a set of matched stereo
146  * data points using the Trivedi formulation for the between camera
147  * transformation NAT 6/6/91 */
148 double  triv_camerror(int *n_data, double *x, Camera * cam1, Camera * cam2, List * world3d, Vec2 * (*pix_get1) ( /* ??? */ ), Vec2 * (*pix_get2) ( /* ??? */ ), double accuracy)
149 {
150     Transform3 rel2to1 = {Transform3_id};
151     Transform3 inv2 = {Transform3_id};
152     List   *plist;
153     Vec2    tl = {Vec2_id};
154     Vec2    tr = {Vec2_id};
155     Vec2   *l;
156     Vec2   *r;
157     Vec3    lp = {Vec3_id};
158     Vec3    rp = {Vec3_id};
159     Vec3    gradfl = {Vec3_id};
160     Vec3    gradfr = {Vec3_id};
161     Vec3    mat3_vprod();
162     Vec3    mat3_transpose_vprod();
163     int     i = 0;
164     Mat3    R = {Mat3_id};
165     Mat3    S = {Mat3_id};
166     Mat3    T = {Mat3_id};
167     Mat3    S_array(double T1, double T2, double T3);
168     double  f, gradsq, lam;
169     double  m, toterr = 0.0, errorsq;
170     double  elx, ely, erx, ery;
171 
172     if (cam1 == NULL || cam2 == NULL || world3d == NULL)
173     {
174         *n_data = 0;
175         return (0.0);
176     }
177 
178     inv2 = trans3_inverse(*(cam2->transf));
179     rel2to1 = trans3_prod(*(cam1->transf), inv2);
180     m = sqrt(rel2to1.t.el[0] * rel2to1.t.el[0]
181              + rel2to1.t.el[1] * rel2to1.t.el[1]
182              + rel2to1.t.el[2] * rel2to1.t.el[2]);
183     R = rel2to1.R;
184     S = S_array(rel2to1.t.el[0] / m, rel2to1.t.el[1] / m, rel2to1.t.el[2] / m);
185     T = mat3_prod(S, R);
186 
187     for (plist = world3d; plist != NULL; plist = plist->next)
188     {
189         if ((l = pix_get1(plist)) && (r = pix_get2(plist)))
190         {
191             tl = cam_correct(*l, cam1);
192             tr = cam_correct(*r, cam2);
193             lp.el[0] = (tl.el[0] - cam1->cx) / cam1->ax;
194             lp.el[1] = (tl.el[1] - cam1->cy) / cam1->ay;
195             lp.el[2] = cam1->f / cam1->pixel;
196             rp.el[0] = (tr.el[0] - cam2->cx) / cam2->ax;
197             rp.el[1] = (tr.el[1] - cam2->cy) / cam2->ay;
198             rp.el[2] = cam2->f / cam2->pixel;
199             gradfl = mat3_vprod(T, rp);
200             gradfr = mat3_transpose_vprod(T, lp);
201 
202             f = gradfl.el[0] * lp.el[0]
203                 + gradfl.el[1] * lp.el[1]
204                 + gradfl.el[2] * lp.el[2];
205 
206             gradsq = gradfl.el[0] * gradfl.el[0] / (cam1->ax * cam1->ax)
207                 + gradfl.el[1] * gradfl.el[1] / (cam1->ay * cam1->ay)
208                 + gradfr.el[0] * gradfr.el[0] / (cam2->ax * cam2->ax)
209                 + gradfr.el[1] * gradfr.el[1] / (cam2->ay * cam2->ay);
210 
211             lam = f / gradsq;
212 
213             elx = -gradfl.el[0] * lam / (cam1->ax * cam1->ax);
214             ely = -gradfl.el[1] * lam / (cam1->ay * cam1->ay);
215             erx = -gradfr.el[0] * lam / (cam2->ax * cam2->ax);
216             ery = -gradfr.el[1] * lam / (cam2->ay * cam2->ay);
217 
218             if (elx > 3.0 * accuracy)
219                 elx = 3.0 * accuracy;
220             if (ely > 3.0 * accuracy)
221                 ely = 3.0 * accuracy;
222             if (erx > 3.0 * accuracy)
223                 erx = 3.0 * accuracy;
224             if (ery > 3.0 * accuracy)
225                 ery = 3.0 * accuracy;
226             if (elx < -3.0 * accuracy)
227                 elx = -3.0 * accuracy;
228             if (ely < -3.0 * accuracy)
229                 ely = -3.0 * accuracy;
230             if (erx < -3.0 * accuracy)
231                 erx = -3.0 * accuracy;
232             if (ery < -3.0 * accuracy)
233                 ery = -3.0 * accuracy;
234 
235             /* errorsq = lam*f; */
236             errorsq = elx * elx + ely * ely + erx * erx + ery * ery;
237             if (x != NULL)
238             {
239                 x[i++] = elx;
240                 x[i++] = ely;
241                 x[i++] = erx;
242                 x[i++] = ery;
243             } else
244                 i += 4;
245             toterr += errorsq;
246             if (i == *n_data)
247                 break;
248         }
249     }
250     *n_data = i;
251     if (toterr >= 0.0)
252         return (toterr);
253     else
254         return (MAXDOUBLE);
255 }
256 
257 double  cam_reg(Covar * incov, int mask, double *a)
258 {
259     Matrix *matrix_alloc();
260     Matrix *matrix_prod();
261     Matrix *delta;
262     Matrix *dprod;
263     double  chisq = 0.0;
264     int     i, n_par;
265 
266     if (incov == NULL)
267         return (0.0);
268 
269     for (i = 0, n_par = 6; i < 16; i++)
270         if (mask & (1 << i))
271             n_par++;
272     delta = matrix_alloc(1, n_par, matrix_full, double_v);
273     for (i = 0; i < n_par; i++)
274     {
275         delta->el.double_v[0][i] = a[i + 6] - VECTOR_DOUBLE(incov->vec, i);
276     }
277     dprod = matrix_prod(delta, incov->mat);
278     for (i = 0; i < n_par; i++)
279     {
280         chisq += dprod->el.double_v[0][i] * delta->el.double_v[0][i];
281     }
282     matrix_free(delta);
283     matrix_free(dprod);
284     return (chisq);
285 }
286 
287 double  stereo_reg(Covar * incov, int mask, double *a)
288 {
289     Matrix *matrix_alloc();
290     Matrix *matrix_prod();
291     Matrix *delta;
292     Matrix *dprod;
293     double  chisq = 0.0;
294     int     i, n_par = 0;
295 
296     if (incov == NULL)
297         return (0.0);
298 
299     for (i = 0; i < 16; i++)
300         if (mask & (1 << i))
301             n_par++;
302     delta = matrix_alloc(1, 2 * n_par + 6, matrix_full, double_v);
303     for (i = 0; i < 2 * n_par + 6; i++)
304     {
305         delta->el.double_v[0][i] = a[i] - VECTOR_DOUBLE(incov->vec, i);
306     }
307     dprod = matrix_prod(delta, incov->mat);
308     for (i = 0; i < 2 * n_par + 6; i++)
309     {
310         chisq += dprod->el.double_v[0][i] * delta->el.double_v[0][i];
311     }
312     matrix_free(delta);
313     matrix_free(dprod);
314 
315     return (chisq);
316 }
317 
318 Mat3    S_array(double T1, double T2, double T3)
319 {
320     Mat3    S = {Mat3_id};
321 
322     S.el[0][0] = (float)0.0;
323     S.el[1][0] = (float)T3;
324     S.el[2][0] = (float)-T2;
325     S.el[0][1] = (float)-T3;
326     S.el[1][1] = (float)0.0;
327     S.el[2][1] = (float)T1;
328     S.el[0][2] = (float)T2;
329     S.el[1][2] = (float)-T1;
330     S.el[2][2] = (float)0.0;
331     return (S);
332 }
333 

~ [ 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.