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

Linux Cross Reference
Tina6/tina-tools/tinatool/tlvision/tlvisCalib_tool.c

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

  1 /**********
  2  *
  3  * This file is part of the TINA Open Source Image Analysis Environment
  4  * henceforth known as TINA
  5  *
  6  * TINA is free software; you can redistribute it and/or modify
  7  * it under the terms of the GNU General Public License as
  8  * published by the Free Software Foundation.
  9  *
 10  * TINA is distributed in the hope that it will be useful,
 11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13  * GNU General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU General Public License
 16  * along with TINA; if not, write to the Free Software Foundation, Inc.,
 17  * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 18  *
 19  * ANY users of TINA who require exemption from the existing licence must
 20  * negotiate a new licence with Dr. Neil.A.Thacker, the sole agent for
 21  * the University of Manchester.
 22  *
 23  **********
 24  *
 25  * Program :   TINA
 26  * File    :  $Source: /home/tina/cvs/tina-tools/tinatool/tlvision/tlvisCalib_tool.c,v $
 27  * Date    :  $Date: 2009/02/03 15:37:46 $
 28  * Version :  $Revision: 1.10 $
 29  * CVS Id  :  $Id: tlvisCalib_tool.c,v 1.10 2009/02/03 15:37:46 paul Exp $
 30  *
 31  * Notes :
 32  *
 33  *
 34  *
 35  *********
 36 */
 37 
 38 
 39 /*
 40 #include "visModel_smplx.h"
 41 #include "fileGeom_ffg_read.h"
 42 */
 43 
 44 #if HAVE_CONFIG_H
 45 #include <config.h>
 46 #endif
 47 
 48 #include <math.h>
 49 #include <stdio.h>
 50 #include <limits.h>
 51 #include <tina/sys/sysDef.h>
 52 #include <tina/sys/sysPro.h>
 53 #include <tina/math/mathDef.h>
 54 #include <tina/math/mathPro.h>
 55 #include <tina/file/fileDef.h>
 56 #include <tina/file/filePro.h>
 57 #include <tina/geometry/geomDef.h>
 58 #include <tina/geometry/geomPro.h>
 59 #include <tina/image/imgDef.h>
 60 #include <tina/image/imgPro.h>
 61 #include <tina/vision/visDef.h>
 62 #include <tina/vision/visPro.h>
 63 
 64 
 65 #include <tinatool/wdgts/wdgtsDef.h>
 66 #include <tinatool/wdgts/wdgtsPro.h>
 67 #include <tinatool/draw/drawDef.h>
 68 #include <tinatool/draw/drawPro.h>
 69 #include <tinatool/tlbase/tlbaseDef.h>
 70 #include <tinatool/tlbase/tlbasePro.h>
 71 #include <tinatool/tlvision/tlvisSmm_matcher.h>
 72 #include <tinatool/tlvision/tlvisCalib_model.h>
 73 
 74 #include "tlvisCalib_tool.h"
 75 
 76 
 77 static int      feature2=0, feature3 = 0;
 78 static char     lbasename[256] = "./small_grid";
 79 static char     calibname[256] = "./default";
 80 static void    *graph_tool = NULL;
 81 static Tv      *graph_tv = NULL;
 82 static double   fit_thres = 0.5, co_thres = 5.0;
 83 static double   join_thres = 2.0, grid_thres = 0.5;
 84 static double   vert_thres = 5.0;
 85 static double   condition = 1.0e-14, accuracy = 1.0;
 86 static double   scale_init = 0.04, c_test1 = 0.00001, c_test2 = 0.1;
 87 static double   leftf = 1.0, leftcx = 128.0, leftcy = 128.0, leftax = 1.0, leftay = 1.0,
 88                 leftp = 0.01;
 89 static int      leftw = 256, lefth = 256;
 90 static double   rightf = 1.0, rightcx = 128.0, rightcy = 128.0, rightax = 1.0, rightay = 1.0,
 91                 rightp = 0.01;
 92 static int      rightw = 256, righth = 256;
 93 static double   stereoq1 = 0.0, stereoq2 = -0.1, stereoq3 = 0.0;
 94 static double   stereot1 = 0.0, stereot2 = 0.0, stereot3 = 100.0;
 95 static int      mask = 1;
 96 static List    *world3d = NULL;
 97 static Camera  *leftcam = NULL, *rightcam = NULL;
 98 static Covar   *inv_covar = NULL;
 99 static double   noise;
100 static Imrect  *gradx_l=NULL, *gradx_r=NULL, *grady_l=NULL, *grady_r=NULL;
101 static Imrect  *left_pot = NULL, *right_pot = NULL;
102 static List    *partListl = NULL;  
103 static List    *cliche_used = NULL;
104 static int      match_type = 1, modelno =0;
105 static int      eSwitch = 1;
106 static List    *matches=NULL;
107 static double   hyp_limit = 0.01;
108 static double   propThresh = 1.0;
109 static Model_smplx model_smplx;
110 static void *ps1,*ps2;
111 
112 /* limits on plots */
113 
114 static void     world_free(List * world);
115 static void     geom_list_free(List * ptr);
116 
117 
118 Camera *get_leftcam()
119 {
120     return( leftcam );
121 }
122 
123 void     update_mask( int val )
124 {
125         mask = val;
126 }
127 
128 static void     par_choice_notify_proc(int value)
129 {
130         mask = value;
131 }
132 
133 static void edge_choice_proc(int value)
134 {
135      eSwitch = value;
136 }
137 
138 static void hype_choice_notify_proc(int value)
139 {
140      model_smplx.lSwitch = value;
141 }
142 
143 /* get the 3D model geometry for the cliche or whole model */
144 List *set_matches( int match_type )
145 {
146     List *matches_new = NULL;
147 
148     if ( match_type == 1 ) 
149     {
150         matches_new = cliche_feats_get(cliche_used); 
151     }
152     else if ( match_type == 2 )
153     {
154         matches_new = list_copy(model_get(), (void *(*) ()) geom_copy, NULL);
155     }
156     return ( matches_new );
157 }
158 
159 static void proj_choice_notify_proc(int value)
160 {
161     match_type = value+1;
162     list_rm(matches,geom_free); 
163     matches = NULL;
164     matches = set_matches(match_type);
165     model_calib_init(left_camera(), right_camera(), matches, &partListl, model_smplx.pix_sep);
166 }
167 
168 static void     make_filename(char *fname, char *extend)
169 {
170         (void) string_append(fname, lbasename, extend, NULL);
171 }
172 
173 static void     make_calibname(char *fname, char *extend)
174 {
175         (void) string_append(fname, calibname, extend, NULL);
176 }
177 
178 static void     shift_choice_proc(int value)
179 {
180         model_smplx.occflag = value;
181 }
182 
183 static void     model_param_dialog(void)
184 {
185         static void    *dialog = 0;
186 
187         if (dialog)
188         {
189                 tw_show_dialog(dialog);
190                 return;
191         }
192         dialog = tw_dialog("Model Parameters");
193 
194         tw_choice("Edges:", edge_choice_proc, 1,
195                   "gradient", "variance", NULL);
196         tw_newrow();
197         tw_choice("Boundary:", shift_choice_proc, 1,
198                   "off", "on", NULL);
199         tw_newrow();
200         tw_check("Model", hype_choice_notify_proc, 1,
201                  "edge", "orient", NULL);
202         tw_newrow();
203         tw_choice("Project", proj_choice_notify_proc, 0,
204                  "cliche", "all/print", NULL);
205         tw_newrow();
206         (void) tw_fglobal("shift factor", &model_smplx.shift_factor, 3 );
207         tw_newrow();
208         (void) tw_fglobal("pixSep", &model_smplx.pix_sep, 3 );
209         tw_newrow();
210         (void) tw_iglobal("pixDiv", &model_smplx.pix_div, 3 );
211         tw_newrow();
212         (void) tw_fglobal("OrErr", &model_smplx.or_err, 3 );
213         tw_newrow();
214         ps1 = tw_fglobal("scale edge", &model_smplx.scale_factor, 8 );
215         tw_newrow();
216         ps2 = tw_fglobal("scale angle", &model_smplx.scale_factor2, 8 );
217         tw_newrow();
218         (void) tw_fglobal("scale shift", &model_smplx.scale_factor3, 8 );
219         tw_newrow();
220         (void ) tw_fglobal("hyp_limit ", &hyp_limit , 3 );
221         tw_newrow();
222         (void) tw_fglobal("% thresh", &propThresh, 3);
223         tw_newrow();
224         (void) tw_iglobal("Feature No.", &feature2,  3);
225         tw_newrow();
226         (void) tw_iglobal("Model No.", &modelno,  3);
227         tw_end_dialog();
228 }
229 
230 static void     grid_param_dialog(void)
231 {
232         static void    *dialog = 0;
233 
234         if (dialog)
235         {
236                 tw_show_dialog(dialog);
237                 return;
238         }
239         dialog = tw_dialog("Grid Parameters");
240 
241         (void) tw_fglobal("Fit Thres    :", &fit_thres, 20);
242         tw_newrow();
243         (void) tw_fglobal("Co Thres     :", &co_thres, 20);
244         tw_newrow();
245         (void) tw_fglobal("Join Thres   :", &join_thres, 20);
246         tw_newrow();
247         (void) tw_fglobal("Grid Thres   :", &grid_thres, 20);
248         tw_newrow();
249         (void) tw_fglobal("Vert Thres   :", &vert_thres, 20);
250         tw_newrow();
251         (void) tw_iglobal("tileOrient(-1,0,1)", &feature3,  3);        
252 
253         tw_end_dialog();
254 }
255 
256 static void     calib_param_dialog(void)
257 {
258         static void    *dialog = NULL;
259 
260         if (dialog)
261         {
262                 tw_show_dialog(dialog);
263                 return;
264         }
265         dialog = tw_dialog("Calib Parameters");
266 
267         (void) tw_fglobal("Init Scale   :", &scale_init, 20);
268         tw_newrow();
269         (void) tw_fglobal("C_criteria 1 :", &c_test1, 20);
270         tw_newrow();
271         (void) tw_fglobal("C_criteria 2 :", &c_test2, 20);
272         tw_newrow();
273         (void) tw_fglobal("Condition    :", &condition, 20);
274         tw_newrow();
275         (void) tw_fglobal("Accuracy    :", &accuracy, 20);
276 
277         tw_end_dialog();
278 }
279 
280 static void    *lf, *lcx, *lcy, *lax, *lay;     /* callback handles */
281 static void    *lp, *lw, *lh;
282 static void    *rf, *rcx, *rcy, *rax, *ray;     /* callback handles */
283 static void    *rp, *rw, *rh;
284 static void    *sq1, *sq2, *sq3, *st1, *st2, *st3;
285 
286 static void     cam_param_dialog(void)
287 {
288         static void    *dialog = NULL;
289 
290         if (dialog)
291         {
292                 tw_show_dialog(dialog);
293                 return;
294         }
295         dialog = tw_dialog("Camera Parameters");
296 
297         lf = tw_fglobal("left:  F,  Cx,  Cy,  Ax,  Ay ", &leftf, 10);
298         tw_newrow();
299         lcx = tw_fglobal(" ", &leftcx, 10);
300         lcy = tw_fglobal(",", &leftcy, 10);
301         lax = tw_fglobal(",", &leftax, 10);
302         lay = tw_fglobal(",", &leftay, 10);
303         tw_newrow();
304         lp = tw_fglobal("pix, w, h :", &leftp, 10);
305         lw = tw_iglobal(",", &leftw, 10);
306         lh = tw_iglobal(",", &lefth, 10);
307         tw_newrow();
308         rf = tw_fglobal("right:  F,  Cx,  Cy,  Ax,  Ay ", &rightf, 10);
309         tw_newrow();
310         rcx = tw_fglobal(" ", &rightcx, 10);
311         rcy = tw_fglobal(",", &rightcy, 10);
312         rax = tw_fglobal(",", &rightax, 10);
313         ray = tw_fglobal(",", &rightay, 10);
314         tw_newrow();
315         rp = tw_fglobal("pix, w, h :", &rightp, 10);
316         rw = tw_iglobal(",", &rightw, 10);
317         rh = tw_iglobal(",", &righth, 10);
318         tw_newrow();
319         (void) tw_label("Stereo: ");
320         tw_newrow();
321         sq1 = tw_fglobal("q ", &stereoq1, 12);
322         sq2 = tw_fglobal(",", &stereoq2, 12);
323         sq3 = tw_fglobal(",", &stereoq3, 12);
324         tw_newrow();
325         st1 = tw_fglobal("t ", &stereot1, 12);
326         st2 = tw_fglobal(",", &stereot2, 12);
327         st3 = tw_fglobal(",", &stereot3, 12);
328 
329         tw_end_dialog();
330 }
331 
332 static void     reset_cam_params()
333 {
334         double          a[6];
335         if (leftcam != NULL)
336         {
337                 leftf = leftcam->f;
338                 leftcx = leftcam->cx;
339                 leftcy = leftcam->cy;
340                 leftax = leftcam->ax;
341                 leftay = leftcam->ay;
342                 leftp = leftcam->pixel;
343                 lefth = leftcam->height;
344                 leftw = leftcam->width;
345 
346                 (void) tw_fglobal_reset(lf);
347                 (void) tw_fglobal_reset(lcx);
348                 (void) tw_fglobal_reset(lcy);
349                 (void) tw_fglobal_reset(lax);
350                 (void) tw_fglobal_reset(lay);
351                 (void) tw_fglobal_reset(lp);
352                 (void) tw_iglobal_reset(lh);
353                 (void) tw_iglobal_reset(lw);
354         }
355         if (rightcam != NULL)
356         {
357                 rightf = rightcam->f;
358                 rightcx = rightcam->cx;
359                 rightcy = rightcam->cy;
360                 rightax = rightcam->ax;
361                 rightay = rightcam->ay;
362                 rightp = rightcam->pixel;
363                 righth = rightcam->height;
364                 rightw = rightcam->width;
365 
366                 (void) tw_fglobal_reset(rf);
367                 (void) tw_fglobal_reset(rcx);
368                 (void) tw_fglobal_reset(rcy);
369                 (void) tw_fglobal_reset(rax);
370                 (void) tw_fglobal_reset(ray);
371                 (void) tw_fglobal_reset(rp);
372                 (void) tw_iglobal_reset(rh);
373                 (void) tw_iglobal_reset(rw);
374         }
375         if (leftcam && rightcam)
376         {
377                 conv_camera_rel(leftcam, rightcam, a);
378                 stereoq1 = a[2];
379                 stereoq2 = a[3];
380                 stereoq3 = a[4];
381                 stereot1 = a[0];
382                 stereot2 = a[1];
383                 stereot3 = a[5];
384                 (void) tw_fglobal_reset(sq1);
385                 (void) tw_fglobal_reset(sq2);
386                 (void) tw_fglobal_reset(sq3);
387                 (void) tw_fglobal_reset(st1);
388                 (void) tw_fglobal_reset(st2);
389                 (void) tw_fglobal_reset(st3);
390         }
391 }
392 
393 static void     set_cam_params()
394 {
395         double          a[6];
396         Transform3      transf = {Transform3_id};
397         Vec3            ex = {Vec3_id};
398         Vec3            ey = {Vec3_id};
399         Vec3            ez = {Vec3_id};
400         Vec3            p = {Vec3_id};
401 
402         if (leftcam == NULL)
403         {
404                 ex.el[0] = 1.0;
405                 ex.el[1] = 0.0;
406                 ex.el[2] = 0.0;
407                 ey.el[0] = 0.0;
408                 ey.el[1] = 1.0;
409                 ey.el[2] = 0.0;
410                 ez.el[0] = 0.0;
411                 ez.el[1] = 0.0;
412                 ez.el[2] = 1.0;
413                 p.el[0] = 0.0;
414                 p.el[0] = 0.0;
415                 p.el[0] = 0.0;
416                 transf = trans3_to_frame(p, ex, ey, ez);
417                 leftcam = cam_make(CAMERA_PHYSICAL, &transf, leftf, leftp, leftax, leftay, leftcx, leftcy, leftw, lefth);
418         } else
419         {
420                 leftcam->f = leftf;
421                 leftcam->cx = leftcx;
422                 leftcam->cy = leftcy;
423                 leftcam->ax = leftax;
424                 leftcam->ay = leftay;
425                 leftcam->pixel = leftp;
426                 leftcam->height = lefth;
427                 leftcam->width = leftw;
428                 cam_comp_default_rects(leftcam);
429         }
430         if (rightcam == NULL)
431         {
432                 ex.el[0] = 1.0;
433                 ex.el[1] = 0.0;
434                 ex.el[2] = 0.0;
435                 ey.el[0] = 0.0;
436                 ey.el[1] = 1.0;
437                 ey.el[2] = 0.0;
438                 ez.el[0] = 0.0;
439                 ez.el[1] = 0.0;
440                 ez.el[2] = 1.0;
441                 p.el[0] = 100.0;
442                 p.el[0] = 0.0;
443                 p.el[0] = 0.0;
444                 transf = trans3_to_frame(p, ex, ey, ez);
445                 rightcam = cam_make(CAMERA_PHYSICAL, &transf, (float) rightf,
446                            (float) rightp, (float) rightax, (float) rightay,
447                                     (float) rightcx, (float) rightcy,
448                                     (int) rightw, (int) righth);
449         } else
450         {
451                 rightcam->f = rightf;
452                 rightcam->cx = rightcx;
453                 rightcam->cy = rightcy;
454                 rightcam->ax = rightax;
455                 rightcam->ay = rightay;
456                 rightcam->pixel = rightp;
457                 rightcam->height = righth;
458                 rightcam->width = rightw;
459                 cam_comp_default_rects(rightcam);
460         }
461         if (leftcam && rightcam)
462         {
463                 a[2] = stereoq1;
464                 a[3] = stereoq2;
465                 a[4] = stereoq3;
466                 a[0] = stereot1;
467                 a[1] = stereot2;
468                 a[5] = stereot3;
469                 store_camera_rel(a, leftcam, rightcam);
470         }
471 }
472 
473 static void     draw_leftcross(Vec2 * l)
474 {
475         Tv             *tv;
476 
477         tv = left_tv();
478         tv_save_draw(tv);
479         tv_set_color(tv, red);
480         tv_cross2(tv, *l, 5);
481         tv_reset_draw(tv);
482         tv_flush(tv);
483 }
484 
485 static void     draw_rightcross(Vec2 * r)
486 {
487         Tv             *tv;
488 
489         tv = right_tv();
490         tv_save_draw(tv);
491         tv_set_color(tv, red);
492         tv_cross2(tv, *r, 5);
493         tv_reset_draw(tv);
494         tv_flush(tv);
495 }
496 
497 static void     restore_proc(void)
498 {
499         Tv    *tvl = left_tv();
500         Tv    *tvr = right_tv();
501         char   pathname[256];
502         FILE  *fpcor;
503         char   temp[1024];
504         Vec3   point = {Vec3_id};
505         Vec2   posl = {Vec2_id};
506         Vec2   posr = {Vec2_id};
507         Vec2   el, er;
508         double diffxl, diffyl, diffxr, diffyr;
509 
510         if (world3d != NULL)
511                 world_free(world3d);
512         world3d = NULL;
513 
514         make_filename(pathname, ".cor");
515         if ((fpcor = fopen(pathname, "r")) == NULL)
516         {
517                 (void) sprintf(temp, " no file %s \n ", pathname);
518                 format(temp);
519                 return;
520         }
521         while (freadline(fpcor, temp) != EOF)
522         {
523                 (void) sscanf(temp, "%f %f %f %f %f %f %f",
524                               &posl.el[0], &posl.el[1],
525                               &posr.el[0], &posr.el[1],
526                               &point.el[0], &point.el[1], &point.el[2]);
527 
528         /*
529           filter routines removed now that robust minimisation used
530           NAT 12/5/92 
531         */
532           tv_set_color(tvl, green);
533           tv_set_color(tvr, green);
534           tv_cross2(tvl, posl, 7);
535           tv_cross2(tvr, posr, 7);
536           if (leftcam&&rightcam) 
537           { 
538               el = cam_proj(leftcam,point); 
539               er = cam_proj(rightcam,point);
540               tv_set_color(tvl, red);
541               tv_set_color(tvr, red);
542               tv_cross2(tvl, el, 7);
543               tv_cross2(tvr, er, 7);
544               diffxl = el.el[0] - posl.el[0]; 
545               diffyl = el.el[1] - posl.el[1]; 
546               diffxr = er.el[0] - posr.el[0]; 
547               diffyr = er.el[1] - posr.el[1]; 
548               if ( diffxl*diffxl + diffyl*diffyl < 4.0*accuracy*accuracy 
549                 && diffxr*diffxr + diffyr*diffyr < 4.0*accuracy*accuracy) 
550                 add_to_cal3d(&posl,&posr,&point);
551               else 
552               { 
553                  (void) sprintf(temp," data %3.1f %3.1f rejected with errors : %2.1f %2.1f %2.1f %2.1f \n ", 
554                                  posl.el[0], posl.el[1], diffxl, diffyl, diffxr, diffyr); 
555                   format(temp);
556               } 
557           } 
558           else
559              add_to_cal3d(&posl, &posr, &point);
560         }
561         (void) fclose(fpcor);
562 }
563 
564 /* ARGSUSED Quieten Lint */
565 void            set_corner_3d(Edgel * corner, int type, void *data, int i, int j)
566 {
567         Match          *cor_match;
568         Edgel          *left_cor;
569         Edgel          *right_cor;
570         Vec2            left_rect = {Vec2_id};
571         Vec2            right_rect = {Vec2_id};
572         Vec3            disp = {Vec3_id};
573         Vec3            pos = {Vec3_id};
574         Parcam         *pcam;
575         Transform3      rect_unrect = {Transform3_id};
576 
577         int             matchlist = MLIST;
578 
579         if (corner == NULL)
580                 return;
581         if ((cor_match = (Match *) get_fixed_match(corner, &matchlist)))
582         {
583                 pcam = pcam_get();
584 
585                 left_cor = cor_match->to1;
586                 right_cor = cor_match->to2;
587                 left_rect = rectify_pos(pcam->rect1, cam_correct(left_cor->pos, pcam->cam1));
588                 right_rect = rectify_pos(pcam->rect2, cam_correct(right_cor->pos, pcam->cam2));
589                 disp.el[0] = left_rect.el[0];
590                 disp.el[1] = (left_rect.el[1] + right_rect.el[1]) / 2.0;
591                 disp.el[2] = right_rect.el[0] - left_rect.el[0];
592                 pos = vec3_par_proj_3d(disp);
593                 rect_unrect =
594                 /* trans3_prod(*(pcam->cam1->transf), */
595                         trans3_inverse(*(pcam->rcam1->transf));
596                 pos = trans3_pos(rect_unrect, pos);
597                 add_to_cal3d(&left_cor->pos, &right_cor->pos, &pos);
598         }
599 }
600 
601 static void     corner_calib_proc(void)
602 {
603         set_cam_params();
604         pcam_set(parcam_make(leftcam, rightcam, CAMERA_PHYSICAL));
605 
606         if (world3d != NULL)
607                 world_free(world3d);
608         world3d = NULL;
609         er_apply_to_all_edges(left_corners_get(), set_corner_3d, NULL);
610         if (left_camera() && right_camera())
611                 pcam_set(parcam_make(left_camera(), right_camera(), CAMERA_PHYSICAL));
612 }
613 
614 
615 /* change this NAT 8/8/08 */
616 /*
617 void    cliche_id_update( List *cl_id )
618 {
619         cliche_used = cl_id->next;      
620 }
621 */
622 
623 List*  closest_cliche()
624 {
625     List *ptr2, *cl_list, *cl_max;
626     int i,imax;
627     Cliche_id *cl_id;
628     Vec3     cliche_view, view_obj_vec, cv;
629     Transform3  trans;
630     double dp, dpmax = -1.0;
631 
632     if (leftcam == NULL) return(NULL);
633     else
634         trans = *leftcam->transf;
635 
636     for ( ptr2 = get_global_cliche_mem_list(), i = 0; ptr2 != NULL;  ptr2 = ptr2->next, i++ )
637     {
638         cl_list = (List *)ptr2->to;
639         cl_id = (Cliche_id *)cl_list->to;
640         cv = vec3( cl_id->vx, cl_id->vy, cl_id->vz );
641         view_obj_vec = vec3_unit(cv);
642         cliche_view = mat3_vprod(trans.R,view_obj_vec);
643         dp = -vec3_z(cliche_view);
644         if (dp > dpmax) 
645         {
646             dpmax = dp;
647             cl_max = cl_list;
648             imax =i;
649         }
650     }
651     return(cl_max);
652 }
653 
654 void    model_grad_init() 
655 {
656     Imrect  *smooth, *make_impot2(Imrect *im, double noise, Imrect **gradx, Imrect **grady);
657     double  sig = 1.0, precision = 0.01;
658 
659     noise =  ( imf_diffx_noise( left_image_get(), tv_get_im_roi( left_tv_get() ) )  
660               + imf_diffy_noise( left_image_get(), tv_get_im_roi( left_tv_get() ) ) 
661               + imf_diffx_noise( right_image_get(), tv_get_im_roi( right_tv_get() ) )
662               + imf_diffy_noise( right_image_get(), tv_get_im_roi( right_tv_get() ) )
663               )/ 4.0;
664     format("image noise level %f \n",noise);
665 
666     if (gradx_l != NULL) im_free(gradx_l);
667     if (grady_l != NULL) im_free(grady_l);
668     if (gradx_r != NULL) im_free(gradx_r);
669     if (grady_r != NULL) im_free(grady_r);
670 
671 
672     if (eSwitch == 0)
673     {
674         smooth =  imf_gauss( left_image_get(), sig, precision );
675         gradx_l = imf_grad_h( smooth );
676         grady_l = imf_grad_v( smooth );
677         im_free(smooth);
678 
679         smooth =  imf_gauss( right_image_get(), sig, precision );
680         gradx_r = imf_grad_h( smooth );
681         grady_r = imf_grad_v( smooth );
682         im_free(smooth);
683         if (left_pot != NULL) im_free(left_pot);
684         left_pot = make_impot(gradx_l, grady_l, noise);
685 
686         if (right_pot != NULL) im_free(right_pot);
687         right_pot = make_impot(gradx_r, grady_r, noise);
688     }
689     else
690     {
691         if (left_pot != NULL) im_free(left_pot);
692         left_pot = make_impot2(left_image_get(), noise, &gradx_l, &grady_l);
693 
694         if (right_pot != NULL) im_free(right_pot);
695         right_pot = make_impot2(right_image_get(), noise, &gradx_r, &grady_r);
696     }
697     stack_push(im_copy(left_pot), IMRECT, im_free); 
698 
699 
700 }
701 
702 /* reset camera paramters to those used in TINA with transformation from matcher */
703 void  model_trans_init(void)
704 {
705     Camera         *left, *right;
706     Transform3      matcher_get_trans(), mat_tran, trans1, trans2, transr1, transr2;
707     Parcam         *pcam;
708     List *mlists = get_match_mlists();
709     Tv *model_tv;
710     double x,y,z;
711 
712     cal_get_proc();
713     left = left_camera();
714     right = right_camera();
715     
716     if (left && right) pcam_set(parcam_make(left, right, CAMERA_PHYSICAL));
717     else 
718     {
719         format("Failed to initialise: load calibration into Tina\n");
720         return;
721     }
722     pcam = pcam_get();
723 
724     if (mlists!=NULL) 
725     {
726        mat_tran = matcher_get_trans();
727        transr1 = trans3_inverse(*(pcam->rcam1->transf));
728        trans1 = trans3_prod(*(pcam->cam1->transf), transr1);
729       *(leftcam->transf) = trans3_prod(trans1, mat_tran);
730 
731        transr2 = trans3_inverse(*(pcam->rcam1->transf));
732        trans2 = trans3_prod(*(pcam->cam2->transf), transr2);
733       *(rightcam->transf) = trans3_prod(trans2, mat_tran);
734     }
735     else  /* pick up alignment from model Tv */
736     {
737         model_tv = model_tv_get();
738         mat_tran.R.el[0][0] = model_tv->ex3.el[0];
739         mat_tran.R.el[0][1] = model_tv->ex3.el[1];
740         mat_tran.R.el[0][2] = model_tv->ex3.el[2];
741         mat_tran.R.el[1][0] = model_tv->ey3.el[0];
742         mat_tran.R.el[1][1] = model_tv->ey3.el[1];
743         mat_tran.R.el[1][2] = model_tv->ey3.el[2];
744         mat_tran.R.el[2][0] = model_tv->ez3.el[0];
745         mat_tran.R.el[2][1] = model_tv->ez3.el[1];
746         mat_tran.R.el[2][2] = model_tv->ez3.el[2];
747         x =  (model_tv->cx - (model_tv->width/2.0)) / (model_tv->scalex*model_tv->pscale);
748         y =  (model_tv->cy - (model_tv->height/2.0)) / (model_tv->scaley*model_tv->pscale);
749         z = leftcam->f/(leftcam->pixel*model_tv->scalex*model_tv->pscale);
750         mat_tran.t.el[0] = x-model_tv->centre3.el[0];
751         mat_tran.t.el[1] = y-model_tv->centre3.el[1];
752         mat_tran.t.el[2] = z-model_tv->centre3.el[2];
753         *(leftcam->transf) = mat_tran;
754         *(rightcam->transf) = mat_tran; /* relative transformation fixed later by set_cam_params */
755     }
756 
757 }
758 
759 /*
760 void     init_model_proc(void)
761 {
762      model_grad_init();
763      model_trans_init();
764      list_rm(matches,geom_free); 
765      matches = NULL;
766      matches = set_matches(match_type);
767      model_calib_init(left_camera(), right_camera(),matches, &partListl, model_smplx.pix_sep);
768      model_smplx.parts = partListl;
769      occ_count_reset(cliche_used);
770 }
771 */
772 
773 void     model_min_proc(void)
774 {
775      set_cam_params();
776      list_rm(matches,geom_free);
777      matches = NULL;
778      cliche_used = closest_cliche();
779      matches = set_matches(match_type);
780      if (matches ==NULL)
781      {
782          format("No model data available from Tina \n");
783          return;
784      }
785      if (gradx_l==NULL || grady_l ==NULL || left_pot == NULL || 
786          gradx_r==NULL || grady_r ==NULL || right_pot == NULL)
787      {
788          format("Calibration Failed: run `model' inititalisation first \n");
789          return;
790      }
791      model_calib_init(leftcam, rightcam, matches, &partListl, model_smplx.pix_sep);
792      model_smplx.parts = partListl;
793      occ_count_reset(cliche_used);
794 
795      model_smplx.gradx = gradx_l;
796      model_smplx.grady = grady_l;
797      model_smplx.potim = imf_lsf_smooth(left_pot,2.0);
798      model_smplx.noise = noise;
799      model_smplx.cam = leftcam;
800      model_smplx.parts = partListl;
801      model_smplx.match = match_type;
802      model_cal_smplx_params_set(scale_init, c_test1, c_test2, accuracy);
803      occ_count_reset(cliche_used);
804      (void) model_cal_simplex(&model_smplx, mask,   partListl,  (Covar *) NULL, cliche_used);
805      im_free(model_smplx.potim);
806 
807      model_smplx.gradx = gradx_r;
808      model_smplx.grady = grady_r;
809      model_smplx.potim = imf_lsf_smooth(right_pot,2.0);
810      model_smplx.cam = rightcam;
811      model_smplx.parts = partListl;
812      occ_count_reset(cliche_used);
813      (void) model_cal_simplex(&model_smplx, mask,   partListl,  (Covar *) NULL, cliche_used);
814      im_free(model_smplx.potim);
815 
816      reset_cam_params();
817 }
818 
819 void proj_search_proc(void)
820 {
821      shistogram **hists;
822      double edge_pot_cut = 5.0, olThresh = 3.0;
823      Parcam *pcam = pcam_get();
824 
825      if (gradx_l==NULL || grady_l ==NULL || left_pot == NULL || 
826          gradx_r==NULL || grady_r ==NULL || right_pot == NULL)
827      {
828          format("Projection Failed: run `model' inititalisation first \n");
829          return;
830      }
831 
832      list_rm(matches,geom_free);
833      matches = NULL;
834      cliche_used = closest_cliche();
835      matches = set_matches(match_type);
836 /*
837      model_calib_init(left_camera(), right_camera(),matches, &partListl, model_smplx.pix_sep);
838 */
839      model_calib_init(leftcam, rightcam, matches, &partListl, model_smplx.pix_sep);
840      model_smplx.parts = partListl;
841      occ_count_reset(cliche_used);
842 
843      hists = (shistogram **)hist_vec();
844 
845      if( hists[20] !=NULL) hfree(hists[20]);
846      hists[20] = hbook2(20,"orientations \0", -6.0, 6.0, 100, -6.0, 6.0, 100);
847      if (hists[10] !=NULL) hfree(hists[10]);
848      hists[10] = hbook1(10,"Orientation Likelihood S\0",0.0, 5.0, 50);
849      if (hists[11] !=NULL) hfree(hists[11]);
850      hists[11] = hbook1(11,"Orientation Likelihood E\0",0.0, 5.0, 50);
851      if (hists[12] !=NULL) hfree(hists[12]);
852      hists[12] = hbook1(12,"Orientation differences >40\0",-0.6, 0.6, 50);
853 
854      if (hists[0] !=NULL) hfree(hists[0]);
855      hists[0] = hbook1(0,"L Edge Likelihood\0",0.0, edge_pot_cut, 50);
856      if (hists[1] !=NULL) hfree(hists[1]);
857      hists[1] = hbook1(1,"L Angle Likelihood\0",0.0, olThresh, 50);
858      if (hists[2] !=NULL) hfree(hists[2]);
859      hists[2] = hbook1(2,"L Angle Likelihood2\0",0.0, olThresh, 50);
860      if (hists[3] !=NULL) hfree(hists[3]);
861      hists[3] = hbook1(3,"L Hypothesis> thresh\0", hyp_limit, 1.0, 50);
862      if (hists[4] !=NULL) hfree(hists[4]);
863      hists[4] = hbook1(4,"L Shift Likelihood\0",0.0, 50.0, 50);
864 
865      set_cam_params();
866      tv_repaint((Tv *)left_tv_get());
867      model_smplx.gradx = gradx_l;
868      model_smplx.grady = grady_l;
869      model_smplx.potim = left_pot;
870      model_smplx.noise = noise;
871      model_smplx.cam = leftcam;
872      model_smplx.parts = partListl;
873      model_smplx.match = match_type;
874      model_smplx.trans = (Transform3 *)pcam->rcam1->transf;
875      model_smplx.rect = pcam->rect1;
876      occ_count_reset(cliche_used);
877      proj_search(left_tv_get(), cliche_used, &model_smplx, feature2, propThresh, hyp_limit);
878      hists[21] = hists[20];
879      hists[20] = hbook2(20,"orientations \0", -6.0, 6.0, 100, -6.0, 6.0, 100);
880 
881 
882      if (hists[5] !=NULL) hfree(hists[5]);
883      hists[5] = hists[0];
884      hists[0] = hbook1(0,"R Edge Likelihood\0",0.0, edge_pot_cut, 50);
885      if (hists[6] !=NULL) hfree(hists[6]);
886      hists[6] = hists[1];
887      hists[1] = hbook1(1,"R Angle Likelihood\0",0.0, olThresh, 50);
888      if (hists[7] !=NULL) hfree(hists[7]);
889      hists[7] = hists[2];
890      hists[2] = hbook1(2,"R Angle Likelihood2\0",0.0, olThresh, 50);
891      if (hists[8] !=NULL) hfree(hists[8]);
892      hists[8] = hists[3];
893      hists[3] = hbook1(3,"R Hypothesis> thresh\0", hyp_limit, 1.0, 50);
894      if (hists[9] !=NULL) hfree(hists[9]);
895      hists[9] = hists[4];
896      hists[4] = hbook1(4,"R Shift Likelihood\0",0.0, 50.0, 50);
897 
898      tv_repaint((Tv *)right_tv_get());
899      model_smplx.gradx = gradx_r;
900      model_smplx.grady = grady_r;
901      model_smplx.potim = right_pot;
902      model_smplx.cam = rightcam;
903      model_smplx.parts = partListl;
904      model_smplx.trans = NULL;
905      occ_count_reset(cliche_used);
906      proj_search(right_tv_get(), cliche_used, &model_smplx, feature2, propThresh, hyp_limit);
907 }
908 
909 void process_mm( List *ptr )
910 {
911      if (ptr!=NULL)
912      {
913          update_mm( ptr );
914          transform_proc();
915          trans_model_proc();
916          cliche_used = closest_cliche();
917      }
918      else
919          cliche_used = NULL;
920      if (left_camera()!=NULL && right_camera() !=NULL)
921          model_trans_init();
922      else
923      {
924          format("No camera calibration loaded in TINA \n");
925          return;
926      }
927 
928      list_rm(matches,geom_free);
929      matches = NULL;
930      matches = set_matches(match_type);
931 /*
932      model_calib_init(leftcam, rightcam,matches, &partListl, model_smplx.pix_sep);
933 */
934         model_calib_init(left_camera(), right_camera(),matches, &partListl, model_smplx.pix_sep);
935      model_smplx.parts = partListl;
936      occ_count_reset(cliche_used);
937 }
938 
939 static void gbm_proc(void)
940 {
941      List   *ptr, *bestlist=NULL;
942      double  bm, bestmatch=0.0;
943      List *mlists = get_match_mlists();
944      int match_num=1, best_pos=0;
945      double temp;
946 
947      model_grad_init();
948      temp = model_smplx.pix_sep;
949      model_smplx.pix_sep = 10.0;
950      model_smplx.gradx = gradx_l;
951      model_smplx.grady = grady_l;
952      model_smplx.potim = left_pot;
953      model_smplx.noise = noise;
954      model_smplx.match = match_type;
955 
956      for ( ptr = mlists; ptr != NULL; ptr = ptr->next )
957      {
958           if (modelno==0)
959           {
960               process_mm( ptr );
961               model_smplx.cam = leftcam;
962               (void) model_cal_simplex(&model_smplx, mask,   partListl,  (Covar *) NULL, cliche_used);
963               tv_repaint((Tv *)left_tv_get());
964               bm = proj_search(left_tv_get(), cliche_used, &model_smplx, feature2, propThresh, hyp_limit);
965               if ( bm > bestmatch )
966               {
967                    bestmatch = bm;
968                    bestlist = ptr;
969                    best_pos = match_num;
970               }
971           }
972           else
973           {
974                    bestlist = ptr;
975                    best_pos = match_num;
976                    if (match_num == modelno) break;
977           }
978           match_num++;
979      }
980      model_smplx.pix_sep = temp;
981 
982      if ( bestlist == NULL )
983      {
984           format("no match found... using transformation from Model Tv\n");
985           format("run model matcher for automatic estimate or load calibration. \n");
986           cliche_used = NULL;
987           cal_get_proc();
988           model_trans_init();
989      }
990      else
991      {
992           update_mm( bestlist );
993           transform_proc();
994           trans_model_proc();
995           cliche_used = closest_cliche();
996           cal_get_proc();
997           model_trans_init();
998           format("Match num = %d (%f)\n", best_pos, bestmatch );
999           reset_cam_params();
1000      }
1001 }
1002 
1003 static void     grid_proc(void)
1004 {
1005         Imrect         *left_er;
1006         Imrect         *right_er;
1007         List           *left_strings;
1008         List           *right_strings;
1009         List           *left_horiz = NULL;
1010         List           *left_vert = NULL;
1011         List           *right_horiz = NULL;
1012         List           *right_vert = NULL;
1013         List           *geom_list_make_flat();
1014         List           *vec3_list_read();
1015         void            store_left_vertex(List * world, Line2 * line1, Line2 * line2, int label);
1016         void            store_right_vertex(List * world, Line2 * line1, Line2 * line2, int label);
1017         char            pathname[256];
1018 
1019         if ((left_er = left_edges()) == NULL ||
1020             (right_er = right_edges()) == NULL)
1021                 return;
1022 
1023         make_filename(pathname, ".dat");
1024         if (world3d != NULL)
1025                 world_free(world3d);
1026         world3d = cal_geom3_list_read(pathname);
1027 
1028         if (world3d == NULL)
1029                 return;
1030 
1031         left_strings = (List *) prop_get(left_er->props, STRING);
1032         right_strings = (List *) prop_get(right_er->props, STRING);
1033 
1034         left_strings = geom_list_make_flat(linear_strings(left_strings, fit_thres));
1035         right_strings = geom_list_make_flat(linear_strings(right_strings, fit_thres));
1036 
1037         colinear_2Dlist(left_strings, &left_horiz, &left_vert, co_thres, join_thres, feature3);
1038         grid_match(world3d, left_horiz, left_vert, grid_thres, vert_thres, store_left_vertex);
1039         geom_list_free(left_horiz);
1040         geom_list_free(left_vert);
1041 
1042         colinear_2Dlist(right_strings, &right_horiz, &right_vert, co_thres, join_thres, feature3);
1043         grid_match(world3d, right_horiz, right_vert, grid_thres, vert_thres, store_right_vertex);
1044         geom_list_free(right_horiz);
1045         geom_list_free(right_vert);
1046 }
1047 
1048 static void     geom_list_free(List * ptr)
1049 {
1050         List           *p;
1051         List           *p_next;
1052 
1053         ptr = geom_list_make_flat(ptr);
1054 
1055         for (p = ptr; p != NULL; p = p_next)
1056         {
1057                 p_next = p->next;
1058                 ptr = list_rm_el(ptr, p, geom_free);
1059         }
1060 }
1061 
1062 void            store_left_vertex(List * world, Line2 * line1, Line2 * line2, int label)
1063 {
1064         List           *ptr;
1065         Match          *match_to_3d;
1066         Match          *match_to_im;
1067         Vec2           *vec2_inter_par_test();
1068         Vec2           *inter;
1069 
1070         inter = vec2_inter_par_test(line1->p, line1->v, line2->p, line2->v, 0.97);
1071         draw_leftcross(inter);
1072 
1073         for (ptr = world; ptr != NULL; ptr = ptr->next)
1074         {
1075                 match_to_3d = ptr->to;
1076                 if (match_to_3d->label == label)
1077                 {
1078                         if ((match_to_im = match_to_3d->to2) == NULL)
1079                         {
1080                                 match_to_im = match_to_3d->to2 = match_alloc(0);
1081                         }
1082                         match_to_im->to1 = inter;
1083                 }
1084         }
1085 }
1086 
1087 void            store_right_vertex(List * world, Line2 * line1, Line2 * line2, int label)
1088 {
1089         List           *ptr;
1090         Match          *match_to_3d;
1091         Match          *match_to_im;
1092         Vec2           *vec2_inter_par_test();
1093         Vec2           *inter;
1094 
1095         inter = vec2_inter_par_test(line1->p, line1->v, line2->p, line2->v, 0.97);
1096         draw_rightcross(inter);
1097 
1098         for (ptr = world; ptr != NULL; ptr = ptr->next)
1099         {
1100                 match_to_3d = ptr->to;
1101                 if (match_to_3d->label == label)
1102                 {
1103                         if ((match_to_im = match_to_3d->to2) == NULL)
1104                         {
1105                                 match_to_im = match_to_3d->to2 = match_alloc(0);
1106                         }
1107                         match_to_im->to2 = inter;
1108                 }
1109         }
1110 }
1111 
1112 void            add_to_cal3d(Vec2 * leftpos, Vec2 * rightpos, Vec3 * pos)
1113 {
1114         Match          *match_to_3d;
1115         Match          *match_to_im;
1116         Vec2           *lpos;
1117         Vec2           *rpos;
1118         Vec3           *point;
1119 
1120         match_to_3d = match_alloc(0);
1121         match_to_3d->to1 = point = vec3_alloc();
1122         *point = *pos;
1123         match_to_im = match_alloc(0);
1124         match_to_3d->to2 = match_to_im;
1125         lpos = vec2_alloc();
1126         rpos = vec2_alloc();
1127         *lpos = *leftpos;
1128         *rpos = *rightpos;
1129         match_to_im->to1 = lpos;
1130         match_to_im->to2 = rpos;
1131         world3d = list_append((List *) world3d, link_alloc((void *) match_to_3d, 0));
1132 }
1133 
1134 static void     world_free(List * world)
1135 {
1136         List           *ptr;
1137         List           *ptr_next;
1138         Match          *match_to_3d;
1139         Match          *match_to_im;
1140 
1141         for (ptr = world; ptr != NULL; ptr = ptr_next)
1142         {
1143                 match_to_3d = ptr->to;
1144                 ptr_next = ptr->next;
1145                 if ((match_to_im = match_to_3d->to2) != NULL)
1146                 {
1147                         if (match_to_im->to1)
1148                                 vec2_free(match_to_im->to1);
1149                         if (match_to_im->to2)
1150                                 vec2_free(match_to_im->to2);
1151                         match_free(match_to_im);
1152                 }
1153                 if (match_to_3d->to1)
1154                         vec3_free(match_to_3d->to1);
1155                 world = list_rm_el(world, ptr, match_free);
1156         }
1157 }
1158 
1159 static void     input_proc(void)
1160 {
1161         Camera         *cam_read();
1162         Covar          *covar_read();
1163         void            rdist_read_fp();
1164         void            cam_write();
1165         void            rdist_write_fp();
1166         char            calib_file[256];
1167 
1168         make_calibname(calib_file, ".l.cam");
1169         if (leftcam)
1170                 cam_free(leftcam);
1171         leftcam = cam_read(calib_file, rdist_read_fp);
1172         make_calibname(calib_file, ".r.cam");
1173         if (rightcam)
1174                 cam_free(rightcam);
1175         rightcam = cam_read(calib_file, rdist_read_fp);
1176         if (leftcam == NULL || rightcam == NULL)
1177                 return;
1178         make_calibname(calib_file, ".s.cam");
1179         if (inv_covar != NULL)
1180                 covar_free(inv_covar);
1181         inv_covar = covar_read(calib_file);
1182 
1183         make_calibname(calib_file, "_save.l.cam");
1184         cam_write(calib_file, leftcam, rdist_write_fp);
1185         make_calibname(calib_file, "_save.r.cam");
1186         cam_write(calib_file, rightcam, rdist_write_fp);
1187         make_calibname(calib_file, "_save.s.cam");
1188         covar_write(calib_file, inv_covar);
1189         reset_cam_params();
1190 }
1191 
1192 static void     output_proc(void)
1193 {
1194         void            cam_write();
1195         void            rdist_write_fp();
1196         char            calib_file[256];
1197 
1198         set_cam_params();
1199         make_calibname(calib_file, ".l.cam");
1200         cam_write(calib_file, leftcam, rdist_write_fp);
1201         make_calibname(calib_file, ".r.cam");
1202         cam_write(calib_file, rightcam, rdist_write_fp);
1203         if (inv_covar == NULL)
1204                 return;
1205         make_calibname(calib_file, ".s.cam");
1206         covar_write(calib_file, inv_covar);
1207 }
1208 
1209 static void     calib_init_proc(void)
1210 {
1211         if (world3d != NULL)
1212         {
1213                 format("calibration data removed\n");
1214                 world_free(world3d);
1215                 world3d = NULL;
1216         }
1217         set_cam_params();
1218 }
1219 
1220 static void     init_covar_proc(void)
1221 {
1222         Covar          *init_cal_invcovar();
1223 
1224         if (inv_covar != NULL)
1225         {
1226                 format("covariances cleared\n");
1227                 covar_free(inv_covar);
1228         }
1229         if (leftcam == NULL || rightcam == NULL)
1230         {
1231                 format("no stored calibration\n");
1232                 return;
1233         }
1234         inv_covar = init_cal_invcovar(leftcam, rightcam, mask);
1235 }
1236 
1237 static void     calib_geom_proc(void)
1238 {
1239     List      *calib_geom = NULL;
1240     List      *ptr;
1241     Match     *match_to_3d;
1242     Point3    *point;
1243     double     point3_get_z();
1244 
1245     for (ptr = world3d; ptr != NULL; ptr = ptr->next)
1246     {
1247         match_to_3d = (Match *) ptr->to;
1248         point = point3_alloc(POINT3);
1249         point->p = *(Vec3 *) (match_to_3d->to1);
1250         calib_geom = list_add_sorted(calib_geom, link_alloc((void *) point, POINT3), point3_get_z);
1251     }
1252     threed_geom_set(calib_geom);
1253 }
1254 
1255 static void     store_proc(void)
1256 {
1257         List           *ptr;
1258         Match          *match_to_3d;
1259         Match          *cormatch;
1260         Vec2           *leftpos;
1261         Vec2           *rightpos;
1262         Vec3           *point;
1263         FILE           *fpcor;
1264         char            pathname[256];
1265 
1266 
1267         make_filename(pathname, ".cor");
1268         if ((fpcor = fopen(pathname, "w")) == NULL)
1269                 fpcor = stdout;
1270 
1271         for (ptr = world3d; ptr != NULL; ptr = ptr->next)
1272         {
1273                 if ((match_to_3d = (Match *) ptr->to) &&
1274                     (cormatch = match_to_3d->to2))
1275                 {
1276                         leftpos = cormatch->to1;
1277                         rightpos = cormatch->to2;
1278                         point = (Vec3 *) (match_to_3d->to1);
1279                         if ((leftpos != NULL) && (rightpos != NULL) && (point != NULL))
1280                                 (void) fprintf(fpcor, "%f %f %f %f %f %f %f\n "
1281                                              ,leftpos->el[0], leftpos->el[1]
1282                                            ,rightpos->el[0], rightpos->el[1]
1283                                  ,point->el[0], point->el[1], point->el[2]);
1284                 }
1285         }
1286         (void) fclose(fpcor);
1287 }
1288 
1289 static void     image_plane_min_proc(void)
1290 {
1291         set_cam_params();
1292         cam_cal_smplx_params_set(scale_init, c_test1, c_test2, accuracy);
1293         (void) cam_cal_simplex(leftcam, mask, world3d, world_get, leftpix_get,
1294                                (Covar *) NULL);
1295         (void) cam_cal_simplex(rightcam, mask, world3d, world_get, rightpix_get,
1296                                (Covar *) NULL);
1297         reset_cam_params();
1298 }
1299 
1300 static void     tsai_proc(void)
1301 {
1302         set_cam_params();
1303         (void) cam_cal_tsai(leftcam, world3d, world_get, leftpix_get);
1304         (void) cam_cal_tsai(rightcam, world3d, world_get, rightpix_get);
1305         reset_cam_params();
1306 }
1307 
1308 static void     stereo_image_plane_min_proc(void)
1309 {
1310         if (inv_covar && inv_covar->label != mask)
1311         {
1312                 format("inconsistent covariance matrix");
1313                 return;
1314         }
1315         set_cam_params();
1316         cam_cal_stereo_smplx_params_set(scale_init, c_test1, c_test2, accuracy);
1317         /*
1318          * rescale the covariance matrix to provide the correct constraint on
1319          * the current set of parameters
1320          */
1321         if (inv_covar)
1322                 inv_covar->mat = matrix_times(accuracy * accuracy, inv_covar->mat);
1323         (void) cam_cal_stereo_simplex(leftcam, rightcam, mask, world3d, world_get, leftpix_get, rightpix_get, inv_covar);
1324         if (inv_covar)
1325                 inv_covar->mat = matrix_times(1.0 / (accuracy * accuracy), inv_covar->mat);
1326         reset_cam_params();
1327 }
1328 
1329 static void     epipolar_min_proc(void)
1330 {
1331         double          triv_calib();
1332 
1333         if (!inv_covar && mask != 0)
1334         {
1335                 format("no covariance matrix\n");
1336                 return;
1337         }
1338         if (inv_covar && inv_covar->label != mask)
1339         {
1340                 format("inconsistent covariance matrix");
1341                 return;
1342         }
1343         set_cam_params();
1344         cam_cal_triv_smplx_params_set(scale_init, c_test1, c_test2, accuracy);
1345 
1346         /*
1347          * rescale the covariance matrix to provide the correct constraint on
1348          * the current set of parameters
1349          */
1350         if (inv_covar)
1351                 inv_covar->mat = matrix_times(accuracy * accuracy, inv_covar->mat);
1352         (void) cam_cal_triv_simplex(leftcam, rightcam, mask, world3d, leftpix_get, rightpix_get, inv_covar);
1353         if (inv_covar)
1354                 inv_covar->mat = matrix_times(1.0 / (accuracy * accuracy), inv_covar->mat);
1355         reset_cam_params();
1356 }
1357 
1358 double          plot_rightpix(Vec2 rightpos)
1359 {
1360         tv_save_draw(right_tv_get());
1361         tv_set_color(right_tv_get(), green);
1362         tv_pixel2(right_tv_get(), rightpos);
1363         tv_reset_draw(right_tv_get());
1364 
1365         return (1.0);
1366 }
1367 
1368 static void     full_covar_proc(void)
1369 {
1370         Covar          *cov;
1371         Covar          *stereo_inv;
1372         Covar          *tot_inv;
1373 
1374         reset_cam_params();
1375         cov = cal_full_stereo_covar(leftcam, rightcam, mask, world3d, world_get, leftpix_get, rightpix_get);
1376         if (cov != NULL)
1377                 cov->mat = matrix_times(accuracy * accuracy, cov->mat);
1378         stereo_inv = cal_full_stereo_invcovar(mask, cov, condition);
1379 
1380         if (inv_covar != NULL && inv_covar->label != mask)
1381         {
1382                 format("Incompatable covariance matricies, matrix replaced.");
1383                 covar_free(inv_covar);
1384                 inv_covar = stereo_inv;
1385         } else if (stereo_inv != NULL)
1386         {
1387                 format("Compatable covariance matricies, matrices added.");
1388                 tot_inv = covar_add(stereo_inv, inv_covar);
1389                 if (inv_covar != NULL)
1390                         covar_free(inv_covar);
1391                 covar_free(stereo_inv);
1392                 inv_covar = tot_inv;
1393         }
1394 }
1395 
1396 
1397 static void     epi_covar_proc(void)
1398 {
1399         Covar          *stereo_inv;
1400         Covar          *tot_inv;
1401 
1402         set_cam_params();
1403         stereo_inv = cal_trivedi_invcovar(leftcam, rightcam, mask, world3d, leftpix_get, rightpix_get);
1404         if (stereo_inv != NULL)
1405                 stereo_inv->mat = matrix_times(1.0 / (accuracy * accuracy), stereo_inv->mat);
1406 
1407         if (inv_covar != NULL && inv_covar->label != mask)
1408         {
1409                 format("Incompatable covariance matricies, matrix replaced.");
1410                 covar_free(inv_covar);
1411                 inv_covar = stereo_inv;
1412         } else if (stereo_inv != NULL)
1413         {
1414                 format("Compatable covariance matricies, matrices added.");
1415                 tot_inv = covar_add(stereo_inv, inv_covar);
1416                 if (inv_covar != NULL)
1417                         covar_free(inv_covar);
1418                 inv_covar = tot_inv;
1419                 covar_free(stereo_inv);
1420         }
1421 }
1422 
1423 static void     graph_tool_proc(void)
1424 {
1425         extern void    *display_tool();
1426 
1427         graph_tool = (void *) display_tool(100, 200, 400, 256);
1428         graph_tv = tv_create("graph");
1429         tv_install(graph_tv, graph_tool);
1430 }
1431 
1432 static void     epierr_proc(void)
1433 {
1434         double         *epi_err;
1435         float           lx[512];
1436         int             i, n_data = 1024;
1437         Vec2           *leftpix_get();
1438         Vec2           *rightpix_get();
1439 
1440         set_cam_params();
1441         if (graph_tool == NULL)
1442         {
1443                 error("no graph tv_screen", warning);
1444                 return;
1445         }
1446         tv_erase(graph_tv);
1447         epi_err = (double *) ralloc(sizeof(double) * (unsigned int) n_data);
1448         (void) triv_camerror(&n_data, epi_err, leftcam, rightcam, world3d, leftpix_get, rightpix_get, accuracy);
1449         if (n_data == 1024)
1450         {
1451                 format("warning only 512 data points displayed\n");
1452         }
1453         for (i = 0; i < n_data; i += 2)
1454         {
1455                 lx[i / 2] = (float) sqrt(epi_err[i] * epi_err[i] + epi_err[i + 1] * epi_err[i + 1]);
1456                 if (fabs(epi_err[i]) > fabs(epi_err[i + 1]))
1457                         lx[i / 2] *= epi_err[i] / fabs(epi_err[i]);
1458                 else
1459                         lx[i / 2] *= epi_err[i + 1] / fabs(epi_err[i + 1]);
1460 
1461                 if (lx[i / 2] >= 3.0 * accuracy)
1462                         lx[i / 2] = 3.0 * accuracy;
1463                 if (lx[i / 2] < -3.0 * accuracy)
1464                         lx[i / 2] = -3.0 * accuracy;
1465         }
1466         n_data /= 2;
1467         plot(PL_INIT,
1468 
1469              PL_TV, graph_tv,
1470              PL_AXIS_COLOR, black,
1471              PL_TITLE, "epi-polar error",
1472 
1473              PL_HIST_RANGE, -3.0 * accuracy, 3.0 * accuracy, 3.0 * accuracy / 30.0,
1474 
1475              PL_COLOR, red,
1476              PL_HIST_DATA, n_data, lx,
1477 
1478              PL_PLOT,
1479              NULL);
1480 
1481         rfree((void *) epi_err);
1482 }
1483 
1484 static void     residual_proc(void)
1485 {
1486         double         *left_err;
1487         double         *right_err;
1488         float           lx[512];
1489         float           rx[512];
1490         int             i, left_data = 1024, right_data = 1024;
1491         Vec2           *leftpix_get();
1492         Vec2           *rightpix_get();
1493         Vec3           *world_get();
1494 
1495         set_cam_params();
1496         if (graph_tool == NULL)
1497         {
1498                 error("no graph tv_screen", warning);
1499                 return;
1500         }
1501         tv_erase(graph_tv);
1502         left_err = (double *) ralloc(sizeof(double) * (unsigned int) left_data);
1503         right_err = (double *) ralloc(sizeof(double) * (unsigned int) right_data);
1504 
1505         (void) camerror(&left_data, left_err, leftcam, world3d, leftpix_get, world_get, accuracy);
1506         (void) camerror(&right_data, right_err, rightcam, world3d, rightpix_get, world_get, accuracy);
1507         if (left_data == 1024 || right_data == 1024)
1508         {
1509                 format("warning only 512 data points displayed\n");
1510         }
1511         left_data /= 2;
1512         for (i = 0; i < left_data; i++)
1513         {
1514                 lx[i] = (float) left_err[2 * i];
1515                 if (lx[i] >= 3.0 * accuracy)
1516                         lx[i] = 3.0 * accuracy;
1517                 if (lx[i] < -3.0 * accuracy)
1518                         lx[i] = -3.0 * accuracy;
1519         }
1520         right_data /= 2;
1521         for (i = 0; i < right_data; i++)
1522         {
1523                 rx[i] = (float) right_err[2 * i];
1524                 if (rx[i] >= 3.0 * accuracy)
1525                         rx[i] = 3.0 * accuracy;
1526                 if (rx[i] < -3.0 * accuracy)
1527                         rx[i] = -3.0 * accuracy;
1528         }
1529         plot(PL_INIT,
1530              PL_TV, graph_tv,
1531              PL_AXIS_COLOR, black,
1532              PL_TITLE, "Residuals",
1533              PL_HIST_RANGE, -3.0 * accuracy, 3.0 * accuracy, 3.0 * accuracy / 30.0,
1534 
1535              PL_COLOR, red,
1536              PL_HIST_DATA, left_data, lx,
1537 
1538              PL_COLOR, blue,
1539              PL_HIST_DATA, right_data, rx,
1540              PL_PLOT,
1541              NULL);
1542 
1543         rfree((void *) left_err);
1544         rfree((void *) right_err);
1545 }
1546 
1547 static void     rdist_proc(void)
1548 {
1549         double         *left_rad, *left_err;
1550         double         *right_rad, *right_err;
1551         float           lx[512];
1552         float           ly[512];
1553         float           rx[512];
1554         float           ry[512];
1555         int             i, left_data = 256, right_data = 256;
1556         Vec2           *leftpix_get();
1557         Vec2           *rightpix_get();
1558         Vec3           *world_get();
1559 
1560         set_cam_params();
1561         if (graph_tool == NULL)
1562         {
1563                 error("no graph tv_screen", warning);
1564                 return;
1565         }
1566         tv_erase(graph_tv);
1567         left_err = (double *) ralloc(sizeof(double) * (unsigned int) left_data);
1568         left_rad = (double *) ralloc(sizeof(double) * (unsigned int) left_data);
1569         right_err = (double *) ralloc(sizeof(double) * (unsigned int) right_data);
1570         right_rad = (double *) ralloc(sizeof(double) * (unsigned int) right_data);
1571 
1572         (void) rad_camerror(&left_data, left_rad, left_err, leftcam, world3d, leftpix_get, world_get);
1573         (void) rad_camerror(&right_data, right_rad, right_err, rightcam, world3d, rightpix_get, world_get);
1574         if (left_data == 256 || right_data == 256)
1575         {
1576                 format("warning only 256 data points displayed\n");
1577         }
1578         for (i = 0; i < left_data; i++)
1579         {
1580                 lx[i] = (float) left_err[i];
1581                 if (lx[i] > 3.0 * accuracy)
1582                         lx[i] = 3.0 * accuracy;
1583                 if (lx[i] < -3.0 * accuracy)
1584                         lx[i] = -3.0 * accuracy;
1585                 ly[i] = (float) left_rad[i];;
1586         }
1587         for (i = 0; i < right_data; i++)
1588         {
1589                 rx[i] = (float) right_err[i];
1590                 if (rx[i] > 3.0 * accuracy)
1591                         rx[i] = 3.0 * accuracy;
1592                 if (rx[i] < -3.0 * accuracy)
1593                         rx[i] = -3.0 * accuracy;
1594                 ry[i] = (float) right_rad[i];
1595         }
1596         plot(PL_INIT,
1597 
1598              PL_TV, graph_tv,
1599              PL_AXIS_COLOR, black,
1600              PL_TITLE, "Radial Distortion",
1601 
1602              PL_X_RANGE, -3.0 * accuracy, 3.0 * accuracy, accuracy,
1603              PL_Y_RANGE, 0.0, 400.0, 100.0,
1604 
1605              PL_COLOR, red,
1606              PL_SCATTER_DATA, left_data, lx, ly,
1607 
1608              PL_COLOR, blue,
1609              PL_SCATTER_DATA, right_data, rx, ry,
1610 
1611              PL_PLOT,
1612              NULL);
1613 
1614         rfree((void *) left_rad);
1615         rfree((void *) left_err);
1616         rfree((void *) right_rad);
1617         rfree((void *) right_err);
1618 }
1619 
1620  void     cal_get_proc(void)
1621 {
1622         Camera         *cam_copy();
1623         void           *cam_distort_copy();
1624 
1625         if (leftcam)
1626                 cam_free(leftcam);
1627         if (rightcam)
1628                 cam_free(rightcam);
1629         leftcam = cam_copy(left_camera());
1630         rightcam = cam_copy(right_camera());
1631         reset_cam_params();
1632 }
1633 
1634  void     cal_set_proc(void)
1635 {
1636         set_cam_params();
1637         if (leftcam && rightcam)
1638         {
1639                 left_camera_set(cam_copy(leftcam));
1640                 right_camera_set(cam_copy(rightcam));
1641                 pcam_update(leftcam, rightcam);
1642         }
1643 }
1644 
1645 void            calib_plots_tool(int x, int y)
1646 {
1647         static void    *tv_screen = NULL;
1648 
1649         if (tv_screen)
1650         {
1651                 tw_show_tool(tv_screen);
1652                 return;
1653         }
1654         tv_screen = tw_tool("Calib Plots", x, y);
1655 
1656         (void) tw_label("Calibration Plots");
1657         tw_newrow();
1658         tw_button("graph tv_screen", graph_tool_proc, NULL);
1659         tw_button("r-dist", rdist_proc, NULL);
1660         tw_button("epi-err", epierr_proc, NULL);
1661         tw_button("x-err", residual_proc, NULL);
1662         tw_end_tool();
1663 }
1664 
1665 /********** Tool creation **********/
1666 
1667 void            calib_tool(int x, int y)
1668 {
1669         static void    *tv_screen = NULL;
1670 
1671         if (tv_screen)
1672         {
1673                 tw_show_tool(tv_screen);
1674                 return;
1675         }
1676         tv_screen = tw_tool("Calib Tool", x, y);
1677 
1678         tw_check("Model", par_choice_notify_proc, mask,
1679                  "F", "Cx", "Cy", "Ax", "Ay", "dsrt", NULL);
1680 
1681         tw_help_button("calib_tool");
1682 
1683         set_cam_params();
1684         tw_newrow();
1685         (void) tw_label("Tina Interaction");
1686         tw_button("get calib", cal_get_proc, NULL);
1687         tw_button("set calib", cal_set_proc, NULL);
1688         tw_button("params", cam_param_dialog, NULL);
1689 
1690         tw_newrow();
1691         (void) tw_sglobal("Base Filename:", lbasename, 30);
1692         tw_newrow();
1693         (void) tw_sglobal("Calib Filename:", calibname, 30);
1694 
1695         tw_newrow();
1696         (void) tw_label("Input/Output");
1697         tw_newrow();
1698         tw_button("input cam", input_proc, NULL);
1699         tw_button("output cam", output_proc, NULL);
1700         tw_button("store geom", store_proc, NULL);
1701         tw_button("restore geom", restore_proc, NULL);
1702         
1703         tw_newrow();
1704         (void) tw_label("Processing");
1705         tw_newrow();
1706         tw_button("init", calib_init_proc, NULL);
1707         tw_button("grid", grid_proc, NULL);
1708         tw_button("corners", corner_calib_proc, NULL);
1709 /*
1710         tw_button("model", init_model_proc, NULL);
1711 */
1712         tw_button("model", gbm_proc, NULL);
1713 
1714         tw_newrow();
1715         (void) tw_label("Calibration Methods");
1716         tw_newrow();
1717         tw_button("Tsai", tsai_proc, NULL);
1718         tw_button("IP min", image_plane_min_proc, NULL);
1719         tw_button("IPS min", stereo_image_plane_min_proc, NULL);
1720         tw_button("Epi min", epipolar_min_proc, NULL);
1721         tw_button("Model min", model_min_proc, NULL);
1722         
1723         tw_newrow();
1724         (void) tw_label("Covariance Computation");
1725         tw_newrow();
1726         tw_button("init covar", init_covar_proc, NULL);
1727         tw_button("full covar", full_covar_proc, NULL);
1728         tw_button("epi covar", epi_covar_proc, NULL);
1729 
1730         tw_newrow();
1731         (void) tw_label("Graphics");
1732         tw_newrow();
1733         tw_button("calib plots", calib_plots_tool, NULL);
1734         tw_button("3D geom", calib_geom_proc, NULL);
1735         tw_button("model proj", proj_search_proc, NULL);
1736 
1737         tw_newrow();
1738         (void) tw_label("Calibration Parameters");
1739         tw_newrow();
1740         tw_button("grid params", grid_param_dialog, NULL);
1741         tw_button("calib params", calib_param_dialog, NULL);
1742         tw_button("model params", model_param_dialog, NULL);
1743 
1744 
1745         model_smplx.occflag = 1;
1746         model_smplx.lSwitch=1;
1747         model_smplx.pix_div = 1;
1748         model_smplx.or_err = 0.02;
1749         model_smplx.pix_sep = 4.0;
1750         model_smplx.shift_factor = 1.0;
1751         model_smplx.scale_factor =1.0;
1752         model_smplx.scale_factor2 =1.0;
1753         model_smplx.scale_factor3 =1.0;
1754         model_smplx.olThresh = 3.0;
1755 
1756         tw_end_tool();
1757 }
1758 

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