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

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

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