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

Linux Cross Reference
Tina4/src/tools/calib/calib_tool.c

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

  1 /**@(#)
  2 **/
  3 #include <stdio.h>
  4 #include <values.h>
  5 #include <tina/vision.h>
  6 #include <tina/draw.h>
  7 #include <tina/drawfuncs.h>
  8 #include <tina/sysfuncs.h>
  9 #include <tina/mathfuncs.h>
 10 #include <tina/visionfuncs.h>
 11 #include <tina/tvfuncs.h>
 12 #include <tina/file.h>
 13 #include <tina/filefuncs.h>
 14 #include <tina/toolsfuncs.h>
 15 #include <tina/tw_Xfuncs.h>
 16 
 17 static char basename[256] = "./small_grid";
 18 static char calibname[256] = "./default";
 19 static void *graph_tool = NULL;
 20 static Tv *graph_tv = NULL;
 21 static double fit_thres = 0.5, co_thres = 5.0;
 22 static double join_thres = 2.0, grid_thres = 0.5;
 23 static double vert_thres = 5.0;
 24 static double condition = 1.0e-14, accuracy = 1.0;
 25 static double scale_init = 0.04, c_test1 = 0.00001, c_test2 = 0.1;
 26 static double leftf=1.0,leftcx=128.0,leftcy=128.0,leftax=1.0,leftay=1.0, leftp=0.01;
 27 static int leftw=256, lefth=256;
 28 static double rightf=1.0,rightcx=128.0,rightcy=128.0,rightax=1.0,rightay=1.0, rightp=0.01;
 29 static int rightw=256, righth=256;
 30 static double stereoq1 = 0.0, stereoq2 = -0.1, stereoq3 = 0.0;
 31 static double stereot1 = 0.0, stereot2 = 0.0, stereot3 = 100.0;
 32 static List *world3d = NULL;
 33 static Camera *leftcam = NULL;
 34 static Camera *rightcam = NULL;
 35 static Covar *inv_covar = NULL;
 36 static int mask = 1;
 37 
 38 static void world_free(List * world);
 39 static void geom_list_free(List * ptr);
 40 
 41 
 42 static void par_choice_notify_proc(int value)
 43 {
 44     mask = value;
 45 }
 46 
 47 static void make_filename(char *fname, char *extend)
 48 {
 49     (void) string_append(fname, basename, extend, NULL);
 50 }
 51 
 52 static void make_calibname(char *fname, char *extend)
 53 {
 54     (void) string_append(fname, calibname, extend, NULL);
 55 }
 56 
 57 static void grid_param_dialog(void)
 58 {
 59     static void *dialog = 0;
 60 
 61     if (dialog)
 62     {
 63         tw_show_dialog(dialog);
 64         return;
 65     }
 66     dialog = tw_dialog("Grid Parameters");
 67 
 68     (void) tw_fglobal("Fit Thres    :", &fit_thres, 20);
 69     tw_newrow();
 70     (void) tw_fglobal("Co Thres     :", &co_thres, 20);
 71     tw_newrow();
 72     (void) tw_fglobal("Join Thres   :", &join_thres, 20);
 73     tw_newrow();
 74     (void) tw_fglobal("Grid Thres   :", &grid_thres, 20);
 75     tw_newrow();
 76     (void) tw_fglobal("Vert Thres   :", &vert_thres, 20);
 77 
 78     tw_end_dialog();
 79 }
 80 
 81 static void calib_param_dialog(void)
 82 {
 83     static void *dialog = NULL;
 84 
 85     if (dialog)
 86     {
 87         tw_show_dialog(dialog);
 88         return;
 89     }
 90     dialog = tw_dialog("Calib Parameters");
 91 
 92     (void) tw_fglobal("Init Scale   :", &scale_init, 20);
 93     tw_newrow();
 94     (void) tw_fglobal("C_criteria 1 :", &c_test1, 20);
 95     tw_newrow();
 96     (void) tw_fglobal("C_criteria 2 :", &c_test2, 20);
 97     tw_newrow();
 98     (void) tw_fglobal("Condition    :", &condition, 20);
 99     tw_newrow();
100     (void) tw_fglobal("Accuracy    :", &accuracy, 20);
101 
102     tw_end_dialog();
103 }
104 
105 static void *lf, *lcx, *lcy, *lax, *lay; /* callback handles */
106 static void *lp, *lw, *lh;
107 static void *rf, *rcx, *rcy, *rax, *ray; /* callback handles */
108 static void *rp, *rw, *rh;
109 static void *sq1, *sq2, *sq3, *st1, *st2, *st3;
110 
111 static void cam_param_dialog(void)
112 {
113     static void *dialog = NULL;
114 
115     if (dialog)
116     {
117         tw_show_dialog(dialog);
118         return;
119     }
120     dialog = tw_dialog("Camera Parameters");
121 
122     lf =  tw_fglobal("left:  F,  Cx,  Cy,  Ax,  Ay ", &leftf, 10 );
123     tw_newrow();
124     lcx = tw_fglobal(" ", &leftcx, 10);
125     lcy = tw_fglobal(",", &leftcy, 10);
126     lax = tw_fglobal(",", &leftax, 10);
127     lay = tw_fglobal(",", &leftay, 10);
128     tw_newrow();
129     lp = tw_fglobal("pix, w, h :", &leftp, 10);
130     lw = tw_iglobal(",", &leftw, 10);
131     lh = tw_iglobal(",", &lefth, 10);
132     tw_newrow();
133     rf = tw_fglobal("right:  F,  Cx,  Cy,  Ax,  Ay ", &rightf, 10 );
134     tw_newrow();
135     rcx = tw_fglobal(" ", &rightcx, 10);
136     rcy = tw_fglobal(",", &rightcy, 10);
137     rax = tw_fglobal(",", &rightax, 10);
138     ray = tw_fglobal(",", &rightay, 10);
139     tw_newrow();
140     rp = tw_fglobal("pix, w, h :", &rightp, 10);
141     rw = tw_iglobal(",", &rightw, 10);
142     rh = tw_iglobal(",", &righth, 10);
143     tw_newrow();
144     (void) tw_label("Stereo: ");
145     tw_newrow();
146     sq1 = tw_fglobal("q ", &stereoq1, 12);
147     sq2 = tw_fglobal(",", &stereoq2, 12);
148     sq3 = tw_fglobal(",", &stereoq3, 12);
149     tw_newrow();
150     st1 = tw_fglobal("t ", &stereot1, 12);
151     st2 = tw_fglobal(",", &stereot2, 12);
152     st3 = tw_fglobal(",", &stereot3, 12);
153 
154     tw_end_dialog();
155 }
156 
157 static void reset_cam_params()
158 {
159    double a[6];
160    if (leftcam != NULL)
161    {
162       leftf = leftcam->f;
163       leftcx = leftcam->cx;
164       leftcy = leftcam->cy;
165       leftax = leftcam->ax;
166       leftay = leftcam->ay;
167       leftp = leftcam->pixel;
168       lefth = leftcam->height;
169       leftw = leftcam->width;
170       
171       (void) tw_fglobal_reset(lf);
172       (void) tw_fglobal_reset(lcx);
173       (void) tw_fglobal_reset(lcy);
174       (void) tw_fglobal_reset(lax);
175       (void) tw_fglobal_reset(lay);
176       (void) tw_fglobal_reset(lp);
177       (void) tw_iglobal_reset(lh);
178       (void) tw_iglobal_reset(lw);
179    }
180    if (rightcam!=NULL)
181    {
182       rightf = rightcam->f;
183       rightcx = rightcam->cx;
184       rightcy = rightcam->cy;
185       rightax = rightcam->ax;
186       rightay = rightcam->ay;
187       rightp = rightcam->pixel;
188       righth = rightcam->height;
189       rightw = rightcam->width;
190       
191       (void) tw_fglobal_reset(rf);
192       (void) tw_fglobal_reset(rcx);
193       (void) tw_fglobal_reset(rcy);
194       (void) tw_fglobal_reset(rax);
195       (void) tw_fglobal_reset(ray);
196       (void) tw_fglobal_reset(rp);
197       (void) tw_iglobal_reset(rh);
198       (void) tw_iglobal_reset(rw);
199    }
200    if (leftcam&&rightcam)
201    {
202       conv_camera_rel(leftcam, rightcam, a); 
203       stereoq1 = a[2];
204       stereoq2 = a[3];
205       stereoq3 = a[4];
206       stereot1 = a[0];
207       stereot2 = a[1];
208       stereot3 = a[5];
209       (void) tw_fglobal_reset(sq1);
210       (void) tw_fglobal_reset(sq2);
211       (void) tw_fglobal_reset(sq3);
212       (void) tw_fglobal_reset(st1);
213       (void) tw_fglobal_reset(st2);
214       (void) tw_fglobal_reset(st3);
215    }
216 }
217 
218 static void set_cam_params()
219 {
220     double a[6];
221     Transform3 transf = {Transform3_id};
222     Vec3    ex = {Vec3_id};
223     Vec3    ey = {Vec3_id};
224     Vec3    ez = {Vec3_id};
225     Vec3    p  = {Vec3_id};
226 
227     if (leftcam == NULL)
228     {
229        ex.el[0] = 1.0;
230        ex.el[1] = 0.0;
231        ex.el[2] = 0.0;
232        ey.el[0] = 0.0;
233        ey.el[1] = 1.0;
234        ey.el[2] = 0.0;
235        ez.el[0] = 0.0;
236        ez.el[1] = 0.0;
237        ez.el[2] = 1.0;
238        p.el[0] = 0.0;
239        p.el[0] = 0.0;
240        p.el[0] = 0.0;
241        transf = trans3_to_frame(p, ex, ey, ez);
242        leftcam = cam_make(CAMERA_PHYSICAL, &transf, (float)leftf,
243                           (float) leftp, (float)leftax, (float)leftay,
244                           (float) leftcx, (float)leftcy,
245                           (int)leftw, (int)lefth);
246     }
247     else
248     {
249        leftcam->f =  leftf ;
250        leftcam->cx = leftcx;
251        leftcam->cy = leftcy;
252        leftcam->ax = leftax;
253        leftcam->ay = leftay;
254        leftcam->pixel = leftp;
255        leftcam->height = lefth;
256        leftcam->width = leftw;
257     }
258     if (rightcam == NULL)
259     {
260               ex.el[0] = 1.0;
261        ex.el[1] = 0.0;
262        ex.el[2] = 0.0;
263        ey.el[0] = 0.0;
264        ey.el[1] = 1.0;
265        ey.el[2] = 0.0;
266        ez.el[0] = 0.0;
267        ez.el[1] = 0.0;
268        ez.el[2] = 1.0;
269        p.el[0] = 100.0;
270        p.el[0] = 0.0;
271        p.el[0] = 0.0;
272        transf = trans3_to_frame(p, ex, ey, ez);
273        rightcam = cam_make(CAMERA_PHYSICAL, &transf, (float)rightf,
274                           (float) rightp, (float)rightax, (float)rightay,
275                           (float) rightcx, (float)rightcy,
276                           (int)rightw, (int)righth);
277     }
278     else
279     {
280        rightcam->f = rightf ;
281        rightcam->cx = rightcx;
282        rightcam->cy = rightcy;
283        rightcam->ax = rightax;
284        rightcam->ay = rightay;
285        rightcam->pixel = rightp;
286        rightcam->height = righth;
287        rightcam->width = rightw;
288     }
289     if (leftcam&&rightcam)
290     {
291        a[2] = stereoq1; 
292        a[3] = stereoq2; 
293        a[4] = stereoq3; 
294        a[0] = stereot1;
295        a[1] = stereot2;
296        a[5] = stereot3;
297        store_camera_rel(a, leftcam, rightcam);
298     }
299 }
300 
301 static void draw_leftcross(Vec2 * l)
302 {
303     Tv     *tv;
304 
305     tv = left_tv();
306     tv_save_draw(tv);
307     tv_set_color(tv, red);
308     tv_cross2(tv, *l, 5);
309     tv_reset_draw(tv);
310     tv_flush(tv);
311 }
312 
313 static void draw_rightcross(Vec2 * r)
314 {
315     Tv     *tv;
316 
317     tv = right_tv();
318     tv_save_draw(tv);
319     tv_set_color(tv, red);
320     tv_cross2(tv, *r, 5);
321     tv_reset_draw(tv);
322     tv_flush(tv);
323 }
324 
325 static void restore_proc(void)
326 {
327     char    pathname[256];
328     FILE   *fpcor;
329     char    temp[1024];
330     Vec3    point = {Vec3_id};
331     Vec2    posl = {Vec2_id};
332     Vec2    posr = {Vec2_id};
333 
334     if (world3d != NULL)
335         world_free(world3d);
336     world3d = NULL;
337 
338     make_filename(pathname, ".cor");
339     if ((fpcor = fopen(pathname, "r")) == NULL)
340     {
341         (void) sprintf(temp, " no file %s \n ", pathname);
342         format(temp);
343         return;
344     }
345     while (freadline(fpcor, temp) != EOF)
346     {
347         (void) sscanf(temp, "%f %f %f %f %f %f %f",
348                       &posl.el[0], &posl.el[1],
349                       &posr.el[0], &posr.el[1],
350                       &point.el[0], &point.el[1], &point.el[2]);
351 
352         /* filter routines removed now that robust minimisation used
353          * NAT 12/5/92 if (leftcam&&rightcam) { el =
354          * cam_proj(leftcam,point); er = cam_proj(rightcam,point);
355          * diffxl = el.el[0] - posl.el[0]; diffyl = el.el[1] -
356          * posl.el[1]; diffxr = er.el[0] - posr.el[0]; diffyr =
357          * er.el[1] - posr.el[1]; if ( diffxl*diffxl + diffyl*diffyl <
358          * 4.0*accuracy*accuracy && diffxr*diffxr + diffyr*diffyr <
359          * 4.0*accuracy*accuracy) add_to_cal3d(&posl,&posr,&point);
360          * else { (void) sprintf(temp," data %3.1f %3.1f rejected with
361          * errors : %2.1f %2.1f %2.1f %2.1f \n ", posl.el[0],
362          * posl.el[1], diffxl, diffyl, diffxr, diffyr); format(temp); }
363          * } else */
364         add_to_cal3d(&posl, &posr, &point);
365     }
366     (void) fclose(fpcor);
367 }
368 
369 static void corner_calib_proc(void)
370 {
371     void    set_corner_3d(Edgel * corner, int type, void *data, int i, int j);
372 
373     set_cam_params();
374     pcam_set(parcam_make(leftcam, rightcam, CAMERA_PHYSICAL));
375 
376     if (world3d != NULL)
377         world_free(world3d);
378     world3d = NULL;
379     er_apply_to_all_edges(left_corners_get(), set_corner_3d, NULL);
380     if (left_camera() && right_camera())
381         pcam_set(parcam_make(left_camera(), right_camera(), CAMERA_PHYSICAL));
382 }
383 
384 /* ARGSUSED Quieten Lint */
385 void    set_corner_3d(Edgel * corner, int type, void *data, int i, int j)
386 {
387     Match  *cor_match;
388     Edgel  *left_cor;
389     Edgel  *right_cor;
390     Vec2    left_rect = {Vec2_id};
391     Vec2    right_rect = {Vec2_id};
392     Vec3    disp = {Vec3_id};
393     Vec3    pos = {Vec3_id};
394     Parcam *pcam;
395     Transform3 rect_unrect = {Transform3_id};
396 
397     int     matchlist = MLIST;
398 
399     if (corner == NULL)
400         return;
401     if ((cor_match = (Match *) get_fixed_match(corner, &matchlist)))
402     {
403         pcam = pcam_get();
404 
405         left_cor = cor_match->to1;
406         right_cor = cor_match->to2;
407         left_rect = rectify_pos(pcam->rect1, cam_correct(left_cor->pos, pcam->cam1));
408         right_rect = rectify_pos(pcam->rect2, cam_correct(right_cor->pos, pcam->cam2));
409         disp.el[0] = left_rect.el[0];
410         disp.el[1] = (left_rect.el[1] + right_rect.el[1]) / 2.0;
411         disp.el[2] = right_rect.el[0] - left_rect.el[0];
412         pos = vec3_par_proj_3d(disp);
413         rect_unrect =
414         /* trans3_prod(*(pcam->cam1->transf), */
415             trans3_inverse(*(pcam->rcam1->transf));
416         pos = trans3_pos(rect_unrect, pos);
417         add_to_cal3d(&left_cor->pos, &right_cor->pos, &pos);
418     }
419 }
420 
421 static void grid_proc(void)
422 {
423     Imrect *left_er;
424     Imrect *right_er;
425     List   *left_strings;
426     List   *right_strings;
427     List   *left_horiz = NULL;
428     List   *left_vert = NULL;
429     List   *right_horiz = NULL;
430     List   *right_vert = NULL;
431     List   *geom_list_make_flat();
432     List   *vec3_list_read();
433     void    store_left_vertex(List * world, Line2 * line1, Line2 * line2, int label);
434     void    store_right_vertex(List * world, Line2 * line1, Line2 * line2, int label);
435     char    pathname[256];
436 
437     if ((left_er = left_edges()) == NULL ||
438         (right_er = right_edges()) == NULL)
439         return;
440 
441     make_filename(pathname, ".dat");
442     if (world3d != NULL)
443         world_free(world3d);
444     world3d = cal_geom3_list_read(pathname);
445 
446     if (world3d == NULL)
447         return;
448 
449     left_strings = (List *) prop_get(left_er->props, STRING);
450     right_strings = (List *) prop_get(right_er->props, STRING);
451 
452     left_strings = geom_list_make_flat(linear_strings(left_strings, fit_thres));
453     right_strings = geom_list_make_flat(linear_strings(right_strings, fit_thres));
454 
455     colinear_2Dlist(left_strings, &left_horiz, &left_vert, co_thres, join_thres);
456     grid_match(world3d, left_horiz, left_vert, grid_thres, vert_thres, store_left_vertex);
457     geom_list_free(left_horiz);
458     geom_list_free(left_vert);
459 
460     colinear_2Dlist(right_strings, &right_horiz, &right_vert, co_thres, join_thres);
461     grid_match(world3d, right_horiz, right_vert, grid_thres, vert_thres, store_right_vertex);
462     geom_list_free(right_horiz);
463     geom_list_free(right_vert);
464 }
465 
466 static void geom_list_free(List * ptr)
467 {
468     List   *p;
469     List   *p_next;
470 
471     ptr = geom_list_make_flat(ptr);
472 
473     for (p = ptr; p != NULL; p = p_next)
474     {
475         p_next = p->next;
476         ptr = list_rm_el(ptr, p, geom_free);
477     }
478 }
479 
480 void    store_left_vertex(List * world, Line2 * line1, Line2 * line2, int label)
481 {
482     List   *ptr;
483     Match  *match_to_3d;
484     Match  *match_to_im;
485     Vec2   *vec2_inter_par_test();
486     Vec2   *inter;
487 
488     inter = vec2_inter_par_test(line1->p, line1->v, line2->p, line2->v, 0.97);
489     draw_leftcross(inter);
490 
491     for (ptr = world; ptr != NULL; ptr = ptr->next)
492     {
493         match_to_3d = ptr->to;
494         if (match_to_3d->label == label)
495         {
496             if ((match_to_im = match_to_3d->to2) == NULL)
497             {
498                 match_to_im = match_to_3d->to2 = match_alloc(0);
499             }
500             match_to_im->to1 = inter;
501         }
502     }
503 }
504 
505 void    store_right_vertex(List * world, Line2 * line1, Line2 * line2, int label)
506 {
507     List   *ptr;
508     Match  *match_to_3d;
509     Match  *match_to_im;
510     Vec2   *vec2_inter_par_test();
511     Vec2   *inter;
512 
513     inter = vec2_inter_par_test(line1->p, line1->v, line2->p, line2->v, 0.97);
514     draw_rightcross(inter);
515 
516     for (ptr = world; ptr != NULL; ptr = ptr->next)
517     {
518         match_to_3d = ptr->to;
519         if (match_to_3d->label == label)
520         {
521             if ((match_to_im = match_to_3d->to2) == NULL)
522             {
523                 match_to_im = match_to_3d->to2 = match_alloc(0);
524             }
525             match_to_im->to2 = inter;
526         }
527     }
528 }
529 
530 void    add_to_cal3d(Vec2 * leftpos, Vec2 * rightpos, Vec3 * pos)
531 {
532     Match  *match_to_3d;
533     Match  *match_to_im;
534     Vec2   *lpos;
535     Vec2   *rpos;
536     Vec3   *point;
537 
538     match_to_3d = match_alloc(0);
539     match_to_3d->to1 = point = vec3_alloc();
540     *point = *pos;
541     match_to_im = match_alloc(0);
542     match_to_3d->to2 = match_to_im;
543     lpos = vec2_alloc();
544     rpos = vec2_alloc();
545     *lpos = *leftpos;
546     *rpos = *rightpos;
547     match_to_im->to1 = lpos;
548     match_to_im->to2 = rpos;
549     world3d = list_append((List *) world3d, link_alloc((void *) match_to_3d, 0));
550 }
551 
552 static void world_free(List * world)
553 {
554     List   *ptr;
555     List   *ptr_next;
556     Match  *match_to_3d;
557     Match  *match_to_im;
558 
559     for (ptr = world; ptr != NULL; ptr = ptr_next)
560     {
561         match_to_3d = ptr->to;
562         ptr_next = ptr->next;
563         if ((match_to_im = match_to_3d->to2) != NULL)
564         {
565             if (match_to_im->to1)
566                 vec2_free(match_to_im->to1);
567             if (match_to_im->to2)
568                 vec2_free(match_to_im->to2);
569             match_free(match_to_im);
570         }
571         if (match_to_3d->to1)
572             vec3_free(match_to_3d->to1);
573         world = list_rm_el(world, ptr, match_free);
574     }
575 }
576 
577 static void input_proc(void)
578 {
579     Camera *cam_read();
580     Covar  *covar_read();
581     void    rdist_read_fp();
582     void    cam_write();
583     void    rdist_write_fp();
584     char    calib_file[256];
585 
586     make_calibname(calib_file, ".l.cam");
587     if (leftcam)
588         cam_free(leftcam);
589     leftcam = cam_read(calib_file, rdist_read_fp);
590     make_calibname(calib_file, ".r.cam");
591     if (rightcam)
592         cam_free(rightcam);
593     rightcam = cam_read(calib_file, rdist_read_fp);
594     if (leftcam == NULL || rightcam == NULL)
595         return;
596     make_calibname(calib_file, ".s.cam");
597     if (inv_covar != NULL)
598         covar_free(inv_covar);
599     inv_covar = covar_read(calib_file);
600 
601     make_calibname(calib_file, "_save.l.cam");
602     cam_write(calib_file, leftcam, rdist_write_fp);
603     make_calibname(calib_file, "_save.r.cam");
604     cam_write(calib_file, rightcam, rdist_write_fp);
605     make_calibname(calib_file, "_save.s.cam");
606     covar_write(calib_file, inv_covar);
607     reset_cam_params();
608 }
609 
610 static void output_proc(void)
611 {
612     void    cam_write();
613     void    rdist_write_fp();
614     char    calib_file[256];
615 
616     set_cam_params();
617     make_calibname(calib_file, ".l.cam");
618     cam_write(calib_file, leftcam, rdist_write_fp);
619     make_calibname(calib_file, ".r.cam");
620     cam_write(calib_file, rightcam, rdist_write_fp);
621     if (inv_covar == NULL)
622         return;
623     make_calibname(calib_file, ".s.cam");
624     covar_write(calib_file, inv_covar);
625 }
626 
627 static void calib_init_proc(void)
628 {
629     if (world3d != NULL)
630     {
631         format("calibration data removed\n");
632         world_free(world3d);
633         world3d = NULL;
634     }
635     set_cam_params();
636 }
637 
638 static void init_covar_proc(void)
639 {
640     Covar *init_cal_invcovar();
641 
642     if (inv_covar!=NULL)
643     {
644         format("covariances cleared\n");
645         covar_free(inv_covar);
646     }
647     if (leftcam==NULL||rightcam==NULL)
648     {
649         format("no stored calibration\n");
650         return;
651     }
652 
653     inv_covar = init_cal_invcovar(leftcam,rightcam,mask);
654 }
655 
656 static void calib_geom_proc(void)
657 {
658     List   *calib_geom = NULL;
659     List   *ptr;
660     Match  *match_to_3d;
661     Point3 *point;
662     double  point3_get_z();
663 
664     for (ptr = world3d; ptr != NULL; ptr = ptr->next)
665     {
666         match_to_3d = (Match *) ptr->to;
667         point = point3_alloc(POINT3);
668         point->p = *(Vec3 *) (match_to_3d->to1);
669         calib_geom = list_add_sorted(calib_geom, link_alloc((void *) point, POINT3), point3_get_z);
670     }
671     threed_geom_set(calib_geom);
672 }
673 
674 static void store_proc(void)
675 {
676     List   *ptr;
677     Match  *match_to_3d;
678     Match  *cormatch;
679     Vec2   *leftpos;
680     Vec2   *rightpos;
681     Vec3   *point;
682     FILE   *fpcor;
683     char    pathname[256];
684 
685 
686     make_filename(pathname, ".cor");
687     if ((fpcor = fopen(pathname, "w")) == NULL)
688         fpcor = stdout;
689 
690     for (ptr = world3d; ptr != NULL; ptr = ptr->next)
691     {
692         if ((match_to_3d = (Match *) ptr->to) &&
693             (cormatch = match_to_3d->to2))
694         {
695             leftpos = cormatch->to1;
696             rightpos = cormatch->to2;
697             point = (Vec3 *) (match_to_3d->to1);
698             if ((leftpos != NULL) && (rightpos != NULL) && (point != NULL))
699                 (void) fprintf(fpcor, "%f %f %f %f %f %f %f\n "
700                                ,leftpos->el[0], leftpos->el[1]
701                                ,rightpos->el[0], rightpos->el[1]
702                            ,point->el[0], point->el[1], point->el[2]);
703         }
704     }
705     (void) fclose(fpcor);
706 }
707 
708 static void image_plane_min_proc(void)
709 {
710     set_cam_params();
711     cam_cal_smplx_params_set(scale_init, c_test1, c_test2, accuracy);
712     (void) cam_cal_simplex(leftcam, mask, world3d, world_get, leftpix_get,
713                            (Covar *) NULL);
714     (void) cam_cal_simplex(rightcam, mask, world3d, world_get, rightpix_get,
715                            (Covar *) NULL);
716     reset_cam_params();
717 }
718 
719 static void tsai_proc(void)
720 {
721     set_cam_params();
722     (void) cam_cal_tsai(leftcam, world3d, world_get, leftpix_get);
723     (void) cam_cal_tsai(rightcam, world3d, world_get, rightpix_get);
724     reset_cam_params();
725 }
726 
727 static void stereo_image_plane_min_proc(void)
728 {
729     if (inv_covar && inv_covar->label != mask)
730     {
731         format("inconsistent covariance matrix");
732         return;
733     }
734     set_cam_params();
735     cam_cal_stereo_smplx_params_set(scale_init, c_test1, c_test2, accuracy);
736     /* rescale the covariance matrix to provide the correct constraint
737      * on the current set of parameters */
738     if (inv_covar)
739         inv_covar->mat = matrix_times(accuracy * accuracy, inv_covar->mat);
740     (void) cam_cal_stereo_simplex(leftcam, rightcam, mask, world3d, world_get, leftpix_get, rightpix_get, inv_covar);
741     if (inv_covar)
742         inv_covar->mat = matrix_times(1.0 / (accuracy * accuracy), inv_covar->mat);
743     reset_cam_params();
744 }
745 
746 static void epipolar_min_proc(void)
747 {
748     double  triv_calib();
749 
750     if (!inv_covar && mask != 0)
751     {
752         format("no covariance matrix\n");
753         return;
754     }
755     if (inv_covar && inv_covar->label != mask)
756     {
757         format("inconsistent covariance matrix");
758         return;
759     }
760     set_cam_params();
761     cam_cal_triv_smplx_params_set(scale_init, c_test1, c_test2, accuracy);
762 
763     /* rescale the covariance matrix to provide the correct constraint
764      * on the current set of parameters */
765     if (inv_covar)
766         inv_covar->mat = matrix_times(accuracy * accuracy, inv_covar->mat);
767     (void) cam_cal_triv_simplex(leftcam, rightcam, mask, world3d, leftpix_get, rightpix_get, inv_covar);
768     if (inv_covar)
769         inv_covar->mat = matrix_times(1.0 / (accuracy * accuracy), inv_covar->mat);
770     reset_cam_params();
771 }
772 
773 static void full_covar_proc(void)
774 {
775     Covar  *cov;
776     Covar  *stereo_inv;
777     Covar  *tot_inv;
778 
779     reset_cam_params();
780     cov = cal_full_stereo_covar(leftcam, rightcam, mask, world3d, world_get, leftpix_get, rightpix_get);
781     if (cov != NULL)
782         cov->mat = matrix_times(accuracy * accuracy, cov->mat);
783     stereo_inv = cal_full_stereo_invcovar(mask, cov, condition);
784 
785     if (inv_covar != NULL && inv_covar->label != mask)
786     {
787         format("Incompatable covariance matricies, matrix replaced.");
788         covar_free(inv_covar);
789         inv_covar = stereo_inv;
790     } else if (stereo_inv != NULL)
791     {
792         format("Compatable covariance matricies, matrices added.");
793         tot_inv = covar_add(stereo_inv, inv_covar);
794         if (inv_covar != NULL)
795             covar_free(inv_covar);
796         covar_free(stereo_inv);
797         inv_covar = tot_inv;
798     }
799 }
800 
801 static void epi_covar_proc(void)
802 {
803     Covar  *stereo_inv;
804     Covar  *tot_inv;
805 
806     set_cam_params();
807     stereo_inv = cal_trivedi_invcovar(leftcam, rightcam, mask, world3d, leftpix_get, rightpix_get);
808     if (stereo_inv != NULL)
809         stereo_inv->mat = matrix_times(1.0 / (accuracy * accuracy), stereo_inv->mat);
810 
811     if (inv_covar != NULL && inv_covar->label != mask)
812     {
813         format("Incompatable covariance matricies, matrix replaced.");
814         covar_free(inv_covar);
815         inv_covar = stereo_inv;
816     } else if (stereo_inv != NULL)
817     {
818         format("Compatable covariance matricies, matrices added.");
819         tot_inv = covar_add(stereo_inv, inv_covar);
820         if (inv_covar != NULL)
821             covar_free(inv_covar);
822         inv_covar = tot_inv;
823         covar_free(stereo_inv);
824     }
825 }
826 
827 static void graph_tool_proc(void)
828 {
829     extern void *display_tool();
830 
831     graph_tool = (void *) display_tool(100, 200, 400, 256);
832     graph_tv = tv_create("graph");
833     tv_install(graph_tv, graph_tool);
834 }
835 
836 static void epierr_proc(void)
837 {
838     double *epi_err;
839     float   lx[512];
840     int     i, n_data = 1024;
841     Vec2   *leftpix_get();
842     Vec2   *rightpix_get();
843 
844     set_cam_params();
845     if (graph_tool == NULL)
846     {
847         error("no graph tv_screen", warning);
848         return;
849     }
850     tv_erase(graph_tv);
851     epi_err = (double *) ralloc(sizeof(double) * (unsigned int) n_data);
852     (void) triv_camerror(&n_data, epi_err, leftcam, rightcam, world3d, leftpix_get, rightpix_get, accuracy);
853     if (n_data == 1024)
854     {
855         format("warning only 512 data points displayed\n");
856     }
857     for (i = 0; i < n_data; i += 2)
858     {
859         lx[i / 2] = (float) sqrt( epi_err[i]* epi_err[i] +  epi_err[i+1]* epi_err[i+1]);
860         if (fabs(epi_err[i])>fabs(epi_err[i+1]))
861             lx[i/2] *= epi_err[i]/fabs(epi_err[i]);
862         else
863             lx[i/2] *= epi_err[i+1]/fabs(epi_err[i+1]);
864 
865         if (lx[i / 2] >= 3.0 * accuracy)
866             lx[i / 2] = 3.0 * accuracy;
867         if (lx[i / 2] < -3.0 * accuracy)
868             lx[i / 2] = -3.0 * accuracy;
869     }
870     n_data /= 2;
871     plot(PL_INIT,
872 
873          PL_TV, graph_tv,
874          PL_AXIS_COLOR, black,
875          PL_TITLE, "epi-polar error",
876 
877          PL_HIST_RANGE, -3.0 * accuracy, 3.0 * accuracy, 3.0 * accuracy / 30.0,
878 
879          PL_COLOR, red,
880          PL_HIST_DATA, n_data, lx,
881 
882          PL_PLOT,
883          NULL);
884 
885     rfree((void *) epi_err);
886 }
887 
888 static void residual_proc(void)
889 {
890     double *left_err;
891     double *right_err;
892     float   lx[512];
893     float   rx[512];
894     int     i, left_data = 1024, right_data = 1024;
895     Vec2   *leftpix_get();
896     Vec2   *rightpix_get();
897     Vec3   *world_get();
898 
899     set_cam_params();
900     if (graph_tool == NULL)
901     {
902         error("no graph tv_screen", warning);
903         return;
904     }
905     tv_erase(graph_tv);
906     left_err = (double *) ralloc(sizeof(double) * (unsigned int) left_data);
907     right_err = (double *) ralloc(sizeof(double) * (unsigned int) right_data);
908 
909     (void) camerror(&left_data, left_err, leftcam, world3d, leftpix_get, world_get, accuracy);
910     (void) camerror(&right_data, right_err, rightcam, world3d, rightpix_get, world_get, accuracy);
911     if (left_data == 1024 || right_data == 1024)
912     {
913         format("warning only 512 data points displayed\n");
914     }
915     left_data /= 2;
916     for (i = 0; i < left_data; i++)
917     {
918         lx[i] = (float) left_err[2 * i];
919         if (lx[i] >= 3.0 * accuracy)
920             lx[i] = 3.0 * accuracy;
921         if (lx[i] < -3.0 * accuracy)
922             lx[i] = -3.0 * accuracy;
923     }
924     right_data /= 2;
925     for (i = 0; i < right_data; i++)
926     {
927         rx[i] = (float) right_err[2 * i];
928         if (rx[i] >= 3.0 * accuracy)
929             rx[i] = 3.0 * accuracy;
930         if (rx[i] < -3.0 * accuracy)
931             rx[i] = -3.0 * accuracy;
932     }
933     plot(PL_INIT,
934          PL_TV, graph_tv,
935          PL_AXIS_COLOR, black,
936          PL_TITLE, "Residuals",
937          PL_HIST_RANGE, -3.0 * accuracy, 3.0 * accuracy, 3.0 * accuracy / 30.0,
938 
939          PL_COLOR, red,
940          PL_HIST_DATA, left_data, lx,
941 
942          PL_COLOR, blue,
943          PL_HIST_DATA, right_data, rx,
944          PL_PLOT,
945          NULL);
946 
947     rfree((void *) left_err);
948     rfree((void *) right_err);
949 }
950 
951 static void rdist_proc(void)
952 {
953     double *left_rad, *left_err;
954     double *right_rad, *right_err;
955     float   lx[512];
956     float   ly[512];
957     float   rx[512];
958     float   ry[512];
959     int     i, left_data = 256, right_data = 256;
960     Vec2   *leftpix_get();
961     Vec2   *rightpix_get();
962     Vec3   *world_get();
963 
964     set_cam_params();
965     if (graph_tool == NULL)
966     {
967         error("no graph tv_screen", warning);
968         return;
969     }
970     tv_erase(graph_tv);
971     left_err = (double *) ralloc(sizeof(double) * (unsigned int) left_data);
972     left_rad = (double *) ralloc(sizeof(double) * (unsigned int) left_data);
973     right_err = (double *) ralloc(sizeof(double) * (unsigned int) right_data);
974     right_rad = (double *) ralloc(sizeof(double) * (unsigned int) right_data);
975 
976     (void) rad_camerror(&left_data, left_rad, left_err, leftcam, world3d, leftpix_get, world_get);
977     (void) rad_camerror(&right_data, right_rad, right_err, rightcam, world3d, rightpix_get, world_get);
978     if (left_data == 256 || right_data == 256)
979     {
980         format("warning only 256 data points displayed\n");
981     }
982     for (i = 0; i < left_data; i++)
983     {
984         lx[i] = (float) left_err[i];
985         if (lx[i] > 3.0 * accuracy)
986             lx[i] = 3.0 * accuracy;
987         if (lx[i] < -3.0 * accuracy)
988             lx[i] = -3.0 * accuracy;
989         ly[i] = (float) left_rad[i];;
990     }
991     for (i = 0; i < right_data; i++)
992     {
993         rx[i] = (float) right_err[i];
994         if (rx[i] > 3.0 * accuracy)
995             rx[i] = 3.0 * accuracy;
996         if (rx[i] < -3.0 * accuracy)
997             rx[i] = -3.0 * accuracy;
998         ry[i] = (float) right_rad[i];
999     }
1000     plot(PL_INIT,
1001 
1002          PL_TV, graph_tv,
1003          PL_AXIS_COLOR, black,
1004          PL_TITLE, "Radial Distortion",
1005 
1006          PL_X_RANGE, -3.0 * accuracy, 3.0 * accuracy, accuracy,
1007          PL_Y_RANGE, 0.0, 400.0, 100.0,
1008 
1009          PL_COLOR, red,
1010          PL_SCATTER_DATA, left_data, lx, ly,
1011 
1012          PL_COLOR, blue,
1013          PL_SCATTER_DATA, right_data, rx, ry,
1014 
1015          PL_PLOT,
1016          NULL);
1017 
1018     rfree((void *) left_rad);
1019     rfree((void *) left_err);
1020     rfree((void *) right_rad);
1021     rfree((void *) right_err);
1022 }
1023 
1024 static void cal_get_proc(void)
1025 {
1026     Camera *cam_copy();
1027     void   *cam_distort_copy();
1028 
1029     if (leftcam)
1030         cam_free(leftcam);
1031     if (rightcam)
1032         cam_free(rightcam);
1033     leftcam = cam_copy(left_camera());
1034     rightcam = cam_copy(right_camera());
1035     reset_cam_params();
1036 }
1037 
1038 static void cal_set_proc(void)
1039 {
1040     set_cam_params();
1041     if (leftcam && rightcam)
1042     {
1043         left_camera_set(cam_copy(leftcam));
1044         right_camera_set(cam_copy(rightcam));
1045         pcam_update(leftcam,rightcam);
1046     }
1047 }
1048 
1049 /********** Tool creation **********/
1050 
1051 void    calib_tool(int x, int y)
1052 {
1053     static void *tv_screen = NULL;
1054 
1055     if (tv_screen)
1056     {
1057         tw_show_tool(tv_screen);
1058         return;
1059     }
1060     tv_screen = tw_tool("Calib Tool", x, y);
1061 
1062     tw_check("Model", par_choice_notify_proc, mask,
1063              "F", "Cx", "Cy", "Ax", "Ay", "dsrt", NULL);
1064 
1065     tw_help_button ("calib_tool");
1066 
1067     set_cam_params();
1068     tw_newrow();
1069     (void) tw_label("Tina Interaction");
1070     tw_button("get calib", cal_get_proc, NULL);
1071     tw_button("set calib", cal_set_proc, NULL);
1072     tw_button("params", cam_param_dialog, NULL);
1073 
1074     tw_newrow();
1075     (void) tw_sglobal("Base Filename:", basename, 30);
1076     tw_newrow();
1077     (void) tw_sglobal("Calib Filename:", calibname, 30);
1078 
1079     tw_newrow();
1080     (void) tw_label("Input/Output");
1081     tw_newrow();
1082     tw_button("input cam", input_proc, NULL);
1083     tw_button("output cam", output_proc, NULL);
1084     tw_button("store geom", store_proc, NULL);
1085     tw_button("restore geom", restore_proc, NULL);
1086 
1087     tw_newrow();
1088     (void) tw_label("Processing");
1089     tw_newrow();
1090     tw_button("init", calib_init_proc, NULL);
1091     tw_button("grid", grid_proc, NULL);
1092     tw_button("geom", calib_geom_proc, NULL);
1093     tw_button("corners", corner_calib_proc, NULL);
1094 
1095     tw_newrow();
1096     (void) tw_label("Calibration Methods");
1097     tw_newrow();
1098     tw_button("Tsai", tsai_proc, NULL);
1099     tw_button("IP min", image_plane_min_proc, NULL);
1100     tw_button("IPS min", stereo_image_plane_min_proc, NULL);
1101     tw_button("Epi min", epipolar_min_proc, NULL);
1102 
1103     tw_newrow();
1104     (void) tw_label("Covarience Computation");
1105     tw_newrow();
1106     tw_button("init covar", init_covar_proc, NULL);
1107     tw_button("full covar", full_covar_proc, NULL);
1108     tw_button("epi covar", epi_covar_proc, NULL);
1109 
1110     tw_newrow();
1111     (void) tw_label("Calibration Errors");
1112     tw_newrow();
1113     tw_button("graph tv_screen", graph_tool_proc, NULL);
1114     tw_button("r-dist", rdist_proc, NULL);
1115     tw_button("epi-err", epierr_proc, NULL);
1116     tw_button("x-err", residual_proc, NULL);
1117 
1118     tw_newrow();
1119     (void) tw_label("Calibration Parameters");
1120     tw_newrow();
1121     tw_button("grid params", grid_param_dialog, NULL);
1122     tw_button("calib params", calib_param_dialog, NULL);
1123 
1124     tw_end_tool();
1125 }
1126 

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