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

Linux Cross Reference
Tina5/tina-tools/tinatool/tlvision/tlvisCalib_model.c

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

  1 /**********
  2  *
  3  *
  4  * Copyright (c) 2003, Division of Imaging Science and Biomedical Engineering,
  5  * University of Manchester, UK.  All rights reserved.
  6  * 
  7  * Redistribution and use in source and binary forms, with or without modification, 
  8  * are permitted provided that the following conditions are met:
  9  * 
 10  *   . Redistributions of source code must retain the above copyright notice, 
 11  *     this list of conditions and the following disclaimer.
 12  *    
 13  *   . Redistributions in binary form must reproduce the above copyright notice,
 14  *     this list of conditions and the following disclaimer in the documentation 
 15  *     and/or other materials provided with the distribution.
 16  * 
 17  *   . Neither the name of the University of Manchester nor the names of its
 18  *     contributors may be used to endorse or promote products derived from this 
 19  *     software without specific prior written permission.
 20  * 
 21  * 
 22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 23  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 24  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 25  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 26  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 27  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 28  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 29  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 30  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 31  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 32  * POSSIBILITY OF SUCH DAMAGE.
 33  **********
 34  *
 35  * Program :   TINA
 36  * File    :  $Source: /home/tina/cvs/tina-tools/tinatool/tlvision/tlvisCalib_model.c,v $
 37  * Date    :  $Date: 2008/10/30 13:10:13 $
 38  * Version :  $Revision: 1.4 $
 39  * CVS Id  :  $Id: tlvisCalib_model.c,v 1.4 2008/10/30 13:10:13 paul Exp $
 40  *
 41  * Author  : Legacy TINA
 42 */
 43 
 44 
 45 
 46 #if HAVE_CONFIG_H
 47 #include <config.h>
 48 #endif
 49 
 50 #include <stdio.h>
 51 #include <memory.h>
 52 #include <math.h>
 53 
 54 #include <tina/sys/sysDef.h>
 55 #include <tina/sys/sysPro.h>
 56 #include <tina/file/fileDef.h>
 57 #include <tina/file/filePro.h>
 58 #include <tina/math/mathDef.h>
 59 #include <tina/math/mathPro.h>
 60 #include <tina/geometry/geomPro.h>
 61 #include <tina/image/imgDef.h>
 62 #include <tina/image/imgPro.h>
 63 #include <tina/vision/visDef.h>
 64 #include <tina/vision/visPro.h>
 65 #include <tlvisSmm_matcher.h>
 66 
 67 #include "tlvisCalib_model.h"
 68 
 69 #include <tinatool/draw/drawDef.h>
 70 #include <tinatool/draw/drawPro.h>
 71 
 72 #define FLAG_TYPE  11117 /* geometry integrity checking */ 
 73 
 74 static int ov_length,  hyp_count, feature_pt_tot;
 75 
 76 Imrect *imf_eprob(Imrect *rank, Imrect *grad, double noise)
 77 {
 78     double probe, probt, probe2, probe4;
 79     Imrect *im2;
 80     Imregion *roi;
 81     float  *row1, *row2, *row3;
 82     double diff;
 83     int     lx, ux, ly, uy;
 84     int     i, j;
 85 
 86     if (rank == NULL)
 87         return (NULL);
 88 
 89     roi = rank->region;
 90 
 91     if (roi == NULL)
 92         return (NULL);
 93 
 94     lx = roi->lx;
 95     ux = roi->ux;
 96     ly = roi->ly;
 97     uy = roi->uy;
 98 
 99 
100     im2 = im_alloc(rank->height, rank->width, roi, float_v);
101     row1 = fvector_alloc(lx, ux);
102     row2 = fvector_alloc(lx, ux);
103     row3 = fvector_alloc(lx, ux);
104 
105     for (i = ly; i < uy; ++i)
106     {
107        im_get_rowf(row1, rank, i, lx, ux);
108        im_get_rowf(row2, grad, i, lx, ux);
109 
110        for (j = lx + 1; j < ux - 1; ++j)
111        {
112 /* cubic approximation to a Rician integral with limited minimum value NAT  10/8/08*/
113            diff = 0.75*row2[j]/noise;
114            if (diff >2.0)
115                probt = 1.0;
116            else if (diff > 0.1)
117                probt = (3.0*diff*diff - diff*diff*diff)/4.0;
118            else
119                probt = 0.00725;
120 
121 /* normalise .. max value from rank in 9x9 region is = 80.5 */
122            probe = (row1[j])/80.0;
123 /*   theoretical integral of binomial distribtion for edge strength greater than 6 of 8
124      neighbours  NAT 9/8/08
125 */
126            probe2 = probe*probe;
127            probe4 = probe2*probe2;
128            row3[j] = log(12.0*probe4*probe2*probe - 18.0*probe4*probe4 +
129                          7.0*probe4*probe4*probe) + log(probt);
130 /*  fast approximation of binomial hypothesis  (also the likelihood) NAT see memo 2007-006
131            row3[j]  = 4.6*log(probe)+log(probt);
132 */
133        }
134        im_put_rowf(row3, im2, i, lx, ux);
135     }
136 
137     fvector_free(row1, lx);
138     fvector_free(row2, lx);
139     fvector_free(row3, lx);
140     return (im2);
141 }
142 
143 
144 /* step edge detector NAT */
145 Imrect         *make_impot(Imrect *gradx, Imrect *grady, double noise)
146 {
147         Imrect         *im1, *im2, *im3, *im4, *im5, *im6;
148 
149         if (gradx==NULL || grady == NULL) return(NULL);
150         im1 = imf_sqr(gradx);
151         im2 = imf_sqr(grady);
152         im3 = imf_sum(im1,im2);
153         im4 = imf_sqrt(im3);
154         im_free(im1);
155         im_free(im2);
156         im_free(im3);
157 /* 7x7 region (3) is minimum for a stable estimate of the rank */
158 /* and 3.0 * noise approximates erf slope NAT 8/8/8            */
159         im5 = imf_rank(im4, 4, 6.0*noise);
160         im6 = imf_eprob(im5, im4, noise);
161         im_free(im4);
162         im_free(im5);
163         return (im6);
164 }
165 
166 /* edge defned as point of maximum image variance NAT */
167 Imrect         *make_impot2(Imrect *im, double noise, Imrect **gradx, Imrect **grady)
168 {
169         Imrect *im1, *im2, *im3, *im4, *im5, *im6, *im7, *im8;
170         double  sig = 0.70, precision = 0.01;
171        
172         im1 = imf_sqr(im);
173         im2 = imf_gauss(im1, sig, precision);
174         im3 = imf_gauss(im, sig, precision);
175         im4 = imf_sqr(im3);
176         im5 = im_diff(im2,im4);
177 /*
178         im6 = imf_log(im5);
179 */
180         im6 = imf_sqrt(im5);
181         im_free(im1);
182         im_free(im2);
183         im_free(im3);
184         im_free(im4);
185         im_free(im5);
186 /* factors of 4 and 18 give exponential likelihood for true edge NAT */
187         im7 = imf_rank(im6, 4, 12.0*noise);
188         gradient_im(im6, gradx, grady); 
189         im8 = imf_eprob(im7, im6, 3.0*noise);
190         im_free(im6);
191         im_free(im7);
192         return(im8);
193 }
194 
195 List *cliche_feats_get(List *cliche_used)
196 {
197     List *feats=NULL, *ptr, *ptr2;
198     int   id, geom_num, geom_type, check1, check2;
199     void *geom, *geom_new;
200     Cliche_id *cl_id;
201 
202     for ( ptr = cliche_used; ptr != NULL; ptr = ptr->next )
203     {
204         cl_id = ((Cliche_id *)(ptr->to));
205         id = cl_id->feat1;
206 
207         if ( cl_id->type != 'O' )
208         {
209             for ( ptr2 = model_get(); ptr2 != NULL; ptr2 = ptr2->next )
210             {
211                 geom = (void *) (ptr2->to);
212                 geom_type = (int)(ptr2->type);
213                 geom_num = geom_label_get( geom, geom_type );
214                 if ( geom_num == id )
215                 {
216                     geom_new = geom_copy(geom, geom_type);
217                     feats = ref_addtostart( feats, (void *) geom_new, geom_type );
218                     break;
219                 }
220             }
221         }
222     }
223     for ( ptr = cliche_used; ptr != NULL; ptr = ptr->next )
224     {
225         cl_id = ((Cliche_id *)(ptr->to));
226         id = cl_id->feat1;
227         if ( cl_id->type == 'O' )
228         {
229             check1 = 0; check2 = 0;
230             for ( ptr2 = feats; ptr2 != NULL; ptr2 = ptr2->next )
231             {
232                 geom = (void *) (ptr2->to);
233                 geom_type = (int)(ptr2->type);
234                 geom_num = geom_label_get( geom, geom_type );
235                 if (  geom_num == cl_id->feat1 ) check1 = 1;
236                 if (  geom_num == cl_id->feat2 ) check2 = 1;
237                 if ( check1 == 1 && check2 == 1 ) break;
238             }
239             if ( check1 == 0 )
240             {
241                for ( ptr2 = model_get(); ptr2 != NULL; ptr2 = ptr2->next )
242                {
243                    geom = (void *) (ptr2->to);
244                    geom_type = (int)(ptr2->type);
245                    geom_num = geom_label_get( geom, geom_type );
246                    if ( cl_id->feat1 ==  geom_num )
247                    {
248                        geom_new = geom_copy(geom, geom_type);
249                        feats = ref_addtostart( feats, geom_new, CONIC3 );
250                        break;
251                    }
252                }
253             }
254             if ( check2 == 0 )
255             {
256                for ( ptr2 = model_get(); ptr2 != NULL; ptr2 = ptr2->next )
257                {
258                    geom = (void *) (ptr2->to);
259                    geom_type = (int)(ptr2->type);
260                    geom_num = geom_label_get( geom, geom_type );
261                    if ( cl_id->feat2 ==  geom_num )
262                    {
263                        geom_new = geom_copy(geom, geom_type);
264                        feats = ref_addtostart( feats, geom_new, CONIC3 );
265                        break;
266                    }
267                }
268             }
269         }
270      }
271      return ( feats );
272 }
273 
274 void partL_free( List *partList )
275 {
276         List *ptr, *memo, *nextptr;
277         Generic *gen;
278         int geom_type;
279 
280         for ( ptr = partList; ptr != NULL; ptr = nextptr )
281         {
282             gen = ((Generic *)ptr->to);
283 
284             geom_type = gen->type;
285 
286             if ( geom_type == CONIC3 ) gen->props = proplist_free( gen->props, ELLIPSE3D_OR );
287 
288             memo = gen->to;
289             list_rm(memo, rfree);
290             rfree( (void *) gen );
291             nextptr = ptr->next;
292             rfree(ptr);
293         }
294 }
295 
296 void     model_calib_init(Camera *left, Camera *right, List *matches, List **partsl, double pix_sep )
297 /* set up 3D positions of sample point features */
298 {   
299     Vec3            vec3_along_line(Line3 * line, double frac);
300     List           *matcher_matches_get(), *partListl;
301     Vec2            leftpos1, leftpos2, rightpos1, rightpos2;
302     Vec3            pos1, pos2, *refPt;
303     Vec3           *point; 
304     double          diff;
305     List           *list, *memo;
306     void           *geom;
307     int             geom_type, *flagnum=NULL, geomNumber;
308     Generic        *gen = NULL;                 
309     int             i, count =0;
310     
311     partListl = *partsl;
312     
313     if ( partListl != NULL ) partL_free( partListl);
314     partListl = NULL;
315 
316     for (list=matches; list != NULL; list = list->next)
317     {
318         geom = (void *) (list->to);
319         geom_type = (int)(list->type);
320         geomNumber = geom_label_get( geom, geom_type );
321         switch (geom_type)
322         {
323             Conic3         *con3;
324             Line3          *line;
325             double          t;
326 
327             case LINE3:
328 
329             if( prop_present( ((Line3 *)geom)->props, FLAG_TYPE ) == 0 )
330             {
331                 flagnum = (int *)ralloc( sizeof(int) );
332                 ((Line3 *)geom)->props = proplist_add( ((Line3 *)geom)->props, flagnum, FLAG_TYPE, rfree);
333                 gen = (Generic *)ralloc( sizeof(Generic));
334                 gen->label = geomNumber;
335                 partListl =  list_append( (List *) partListl, link_alloc((void *) gen, 0));
336                 line = (Line3 *) geom;
337                 pos1 = line->p1;
338                 leftpos1 = cam_proj(left, pos1);
339                 rightpos1 = cam_proj(right, pos1);
340                 pos2 = line->p2;
341                 leftpos2 = cam_proj(left, pos2);
342                 rightpos2 = cam_proj(right, pos2);
343                 diff = vec2_dist(leftpos1, leftpos2);
344                 diff += vec2_dist(rightpos1, rightpos2);
345                 diff = 2.0 / diff;
346                 diff *= pix_sep;
347 
348                 memo = NULL;
349                 for (t = 0.5*diff ; t < 1.0 ; t += diff)
350                 {
351                     point = vec3_alloc();
352                    *point = vec3_along_line(line, t);
353                     memo = list_append( (List *) memo, link_alloc((void *) point, 0 ));
354                     count++;
355                 }                       
356                 gen->to = memo;
357                 gen->type = geom_type;
358             }
359             else
360                 error("line already in list \n",non_fatal);
361             break;
362 
363             case CONIC3:
364 
365             if( prop_present( ((Conic3 *)geom)->conic->props, FLAG_TYPE ) == 0 )
366             {
367                 con3 = (Conic3 *) geom;
368                 pos1 = conic3_point(con3, con3->conic->t1);
369                 leftpos1 = cam_proj(left, pos1);
370                 rightpos1 = cam_proj(right, pos1);
371                 diff = 0.0;
372                 i = 0;
373 
374                 for (t = con3->conic->t1; i < 10 ; t += (con3->conic->t2 - con3->conic->t1) / 10.0  )
375                 {
376                     i++;
377                     leftpos2 = leftpos1;
378                     rightpos2 = rightpos1;
379                     pos1 = conic3_point(con3, t);
380                     leftpos1 = cam_proj(left, pos1);
381                     rightpos1 = cam_proj(right, pos1);
382                     diff += vec2_dist(leftpos1, leftpos2) + vec2_dist(rightpos1, rightpos2);
383                 }
384                 diff = 2.0 / diff;
385                 diff *= pix_sep;
386                 if (!(diff < 0.2))
387                 {
388 /*
389                      format("too few points on ellipse\n");
390 */
391 
392                      break;
393                 }
394                 else if (diff < 0.001)
395                 {
396                      format("millions of points on ellipse\n");
397                      format("check starting calibration\n");
398                 }
399      
400                 flagnum = (int *)ralloc( sizeof(int) );
401                 ((Conic3 *)geom)->conic->props = proplist_add( ((Conic3 *)geom)->conic->props, flagnum, FLAG_TYPE, rfree);
402                 gen = (Generic *)ralloc( sizeof(Generic));
403                 gen->label = geomNumber;
404                 partListl =  list_append( (List *) partListl, link_alloc((void *) gen, 0));
405 
406                 memo = NULL;
407                 diff = (con3->conic->t2 - con3->conic->t1) * diff;
408                 for (t = con3->conic->t1 + 0.5*diff; t < con3->conic->t2 ; t += diff  )
409                 {
410                     point = vec3_alloc();
411                    *point = conic3_point(con3, t);
412                     memo = list_append( (List *) memo, link_alloc((void *) point, 0 ));
413                     count++;
414                 }
415                  gen->to = memo;
416                  gen->type= geom_type;
417 /* orientation of ellipse relative to camera NAT */
418                  refPt = (Vec3 *)ralloc( sizeof(Vec3));
419                 *refPt =  vec3_sum( ((Conic3*)geom)->origin,   vec3_times(10.0,((Conic3 *) geom)->ez));
420                  gen->props =  proplist_add(gen->props,refPt, ELLIPSE3D_OR, rfree);
421             }
422             else
423                 error("ellipse already in list \n",non_fatal);
424             break;
425         }
426     }
427     for (list = matches; list != NULL; list = list->next)
428     {
429         geom = (void *) (list->to);
430         geom_type = (int)(list->type);
431 
432         switch (geom_type)
433         {
434             case LINE3:
435             ((Line3 *)geom)->props = proplist_free(((Line3 *)geom)->props, FLAG_TYPE);
436             break;
437 
438             case  CONIC3:
439             ((Conic3 *)geom)->conic->props = proplist_free(((Conic3 *)geom)->conic->props, FLAG_TYPE);
440             break;
441         }
442     }
443     *partsl = partListl;
444     if (count > 10000) format("Warning: >10000 points check calibration \n");
445 }
446 
447 void occ_count_reset(List *cliche_used)
448 {
449     List *ptr;
450     Cliche_id *cl_id;
451 
452     for ( ptr = cliche_used; ptr != NULL; ptr = ptr->next )
453     {
454         cl_id = ((Cliche_id *)(ptr->to));
455 
456         if ( cl_id->type == 'O' )
457         {
458              cl_id->n1 =0;
459              cl_id->n2 =0;
460         }
461     }
462 }
463 
464 void          plot_tvpix(Tv *tv, Vec2 pos, int col)
465 {
466         tv_save_draw(tv);
467         tv_set_color(tv, col);
468         tv_pixel2(tv, pos); 
469         tv_reset_draw(tv);
470 }
471 
472 double ang_d( double f_orient, double i_orient, double idx, double idy, double noise, double or_err)
473 {
474     double var_i_or,  var_i_or2;
475     double ang_diff, ang_diff2;
476     double grad;
477     shistogram **hists =(shistogram **)hist_vec();
478 
479 /*
480     grad =  ((idx*idx) + (idy*idy) - 0.5*noise*noise)/ noise*noise;
481 */
482     grad =  ((idx*idx) + (idy*idy))/ noise*noise;
483     if (grad<1.0) grad = 1.0;
484     var_i_or = 0.3 / grad ;           /* approx error following image smoothing */
485     var_i_or += (or_err*or_err);
486     var_i_or2 = sqrt( var_i_or );
487     ang_diff = fabs( f_orient - i_orient );
488     hfill1(hists[12],  f_orient - i_orient , 1.0);
489     if( ang_diff > ( PI / 2.0 ) ) ang_diff = PI - ang_diff;
490     ang_diff2 = ang_diff/var_i_or2;
491     return(ang_diff2);
492 }
493 
494 double ang_d2( double f_orient, double i_orient, double or_err)
495 {
496     double var_i_or;
497     double ang_diff, ang_diff2;
498 
499     var_i_or = 2.0*or_err;
500     ang_diff = fabs( f_orient - i_orient );
501     if( ang_diff > ( PI / 2 ) ) ang_diff = PI - ang_diff;
502     ang_diff2 = ang_diff/var_i_or;
503     return(ang_diff2);
504 }
505 
506 double joint_hyp(double edge, double angle)
507 {
508       double L1, L2, L3;
509 
510       L1 = exp( -edge );
511       L2 = 1.0 - erf( angle );
512 
513       if ( L1 == 0.0 || L2 == 0.0 )
514              L3 = 0.0;
515       else
516              L3 = (L1 * L2) * ( 1 - log( L1 * L2 ) );
517       return(L3);
518 }
519 
520 int disp_init( Tv * tv, List *memo, double l_p, Model_smplx *ms, double shift, Vec2 perp_vec, double f_orient, List * orients, double hyp_limit)
521 {
522     Camera *cam;
523     double  idx, idy, i_orient;
524     List   *l, *l2 = orients;
525     Vec2    pt;
526     Imrect *gradx, *grady, *impot;
527     double  ledge, langle, combined=0.0;
528     int     length=0;
529     double scale_factor, scale_factor2, noise, or_err;
530     shistogram **hists;
531 
532     cam   = ms->cam;
533     gradx = ms->gradx;
534     grady = ms->grady;
535     impot = ms->potim;
536     scale_factor = ms->scale_factor;
537     scale_factor2 = ms->scale_factor2;
538     hists =(shistogram **)hist_vec(); 
539     noise = ms->noise;
540 
541     length = list_length(memo);
542     if (orients!=NULL )
543     {
544         or_err = PI/(float)(length*ms->pix_sep);
545         or_err = sqrt((or_err*or_err)+(ms->or_err*ms->or_err));
546     }
547     else or_err = ms->or_err;
548     length = 0;
549 
550     for( l = memo; l != NULL; l = l->next )
551     {
552          pt = *(Vec2 *)l->to;
553 
554          pt = vec2_sum( pt, vec2_times( shift, perp_vec) );
555 
556          idx = im_sub_pixf( gradx , pt.el[1], pt.el[0] );
557          idy = im_sub_pixf( grady , pt.el[1], pt.el[0] );
558 
559          if ( idx == 0.0 && idy != 0.0 )      i_orient = PI / 2.0;
560          else if ( idy == 0.0 && idx != 0.0 ) i_orient = 0.0;
561          else                                 i_orient = atan( idy/idx );
562 
563          if ( orients!=NULL ) 
564          { 
565               f_orient = *(double *)l2->to;  
566               l2 = l2->next; 
567          }
568          langle =  scale_factor2*ang_d( f_orient, i_orient, idx, idy,  noise, or_err);
569 
570          ledge = -scale_factor*im_get_pixf( impot, pt.el[1], pt.el[0] );
571          if (ms->lSwitch ==3) combined = joint_hyp( ledge, langle);
572          else if (ms->lSwitch ==2) combined = 1.0 - erf( langle );
573          else if (ms->lSwitch == 1) combined = exp(-ledge);
574 
575          if (1.0-erf(langle)>=hyp_limit) hfill1(hists[0], ledge, 1.0);
576          if (exp(-ledge)>=hyp_limit)    
577          {
578              hfill1(hists[1],langle,1.0);
579          }
580          hfill1(hists[3], combined, 1.0);
581 
582          if ( combined >= hyp_limit )
583          {
584             length++;
585             plot_tvpix(tv, pt ,green);
586          }
587          else
588             plot_tvpix(tv, pt ,blue);
589 
590          langle =  scale_factor2*ang_d2( f_orient, i_orient, ms->or_err);
591          hfill1(hists[2], langle, 1.0);
592     }
593     return(length);
594 }
595 
596 double l_prop( List * memo, Model_smplx *ms, double shift, Vec2 perp_vec, double f_orient, List * orients, double hyp_limit  )
597 {
598     Camera *cam;
599     double  idx, idy, i_orient, langle, ledge;
600     int     l_p = 0, length=0;
601     List   *l, *l2 = orients;
602     Vec2    pt;
603     Imrect *gradx, *grady, *impot;
604     double  combined=0.0, noise, or_err;
605     double scale_factor, scale_factor2;
606 
607     cam =   ms->cam;
608     gradx = ms->gradx;
609     grady = ms->grady;
610     impot = ms->potim;
611     scale_factor = ms->scale_factor;
612     scale_factor2 = ms->scale_factor2;
613     noise = ms->noise;
614 
615 /* set limit on angle measurement */
616     length = list_length(memo);
617     if (orients!=NULL )
618     {
619         or_err = PI/(float)(length*ms->pix_sep);
620         or_err = sqrt((or_err*or_err)+(ms->or_err*ms->or_err));
621     }
622     else  or_err = ms->or_err;
623 
624     for( l = memo; l != NULL; l = l->next )
625     {
626         pt = *(Vec2 *)l->to;
627 
628         pt = vec2_sum(pt, vec2_times( shift, perp_vec) );
629         idx = im_sub_pixf( gradx , pt.el[1], pt.el[0] );
630         idy = im_sub_pixf( grady , pt.el[1], pt.el[0] );
631 
632         if ( idx == 0.0 && idy != 0.0 )      i_orient = PI / 2.0;
633         else if ( idy == 0.0 && idx != 0.0 ) i_orient = 0.0;
634         else                                 i_orient = atan( idy/idx );
635 
636         if ( orients!=NULL ) 
637         {   
638             f_orient = *(double *)l2->to;  
639             l2 = l2->next; 
640         }
641         langle = scale_factor2*ang_d( f_orient, i_orient, idx, idy,  noise, or_err);
642 
643 
644         ledge = -scale_factor*im_get_pixf( impot, pt.el[1], pt.el[0] );
645 
646          if (ms->lSwitch ==3) combined = joint_hyp( ledge, langle);
647          else if (ms->lSwitch ==2) combined = 1.0 - erf( langle );
648          else if (ms->lSwitch == 1) combined = exp(-ledge);
649 
650         if ( combined >= hyp_limit )  
651             hyp_count++;  // 0.05 -> 0.01
652         else 
653             l_p++;
654     }
655     ov_length+=length;
656     if (length > 0)
657         return ( (float)l_p/(float)length );
658     else
659         return (1.0);
660 }
661 
662 
663 double proj_search( Tv *tv, List *cliche_used, Model_smplx *ms, int feature2, double propThresh , double hyp_limit)
664 {
665     List *parts;
666     Camera *cam;
667     List   *memo, *ptr, *ptr2, *ptr3, *gcm_list;
668     Vec3    semi_memo;
669     int     i=0, length, geom_type, partNum;
670     int     neg_occ_b_count=0;
671     Vec2    perpVec, centre, cam_proj();
672     List   *list = NULL, *elist1, *elist2, *partList, *orients;
673     List   *orients1, *orients2, *occ_bounds = NULL, *occ_line;
674     double  fOrient, x, y, llPeak, lat_shift1, lat_shift2, lat_shift1s, lat_shift2s;
675     double  l_p, l_p1, l_p2, l_s1, l_s2;
676     double  def_shift = 5.0, theta, alpha;
677     Cliche_id   *cl_id;
678     Cliche_id  *o_s;
679     Bool    save_flag;
680     int lSwitch;
681     double e_s_p_factor;
682     double shift, shift1, shift2, pix_sep;
683     Transform3 trans;
684 
685     parts = ms->parts;
686     cam = ms->cam;
687     lSwitch = ms->lSwitch;
688     e_s_p_factor = ms->shift_factor;
689     pix_sep = ms->pix_sep;
690 
691     ov_length = 0;  hyp_count =0; feature_pt_tot = 0;
692 
693     for( partList = parts ; partList != NULL; partList = partList->next)
694     {
695         geom_type = ((Generic *) partList->to)->type;
696         if( geom_type == CONIC3 )
697             i+=2;
698         else if( geom_type == LINE3 )
699             i++;
700         memo = ((Generic *) partList->to)->to;
701         length = list_length( memo );
702     }
703     if ( ms->match == 1)
704     {
705         for ( ptr = cliche_used; ptr != NULL; ptr = ptr->next )
706         {
707              cl_id = (Cliche_id *)(ptr->to);
708              if ( cl_id->type == 'O' )
709              {
710                  occ_bounds = ref_addtostart( occ_bounds, (void *) cl_id, 0 );
711              }
712         }
713     }
714     else
715     {
716         trans = *(cam->transf);
717         printf("V (%3.2f %3.2f %3.2f) ?? ??\n",-trans.R.el[2][0],-trans.R.el[2][1],-trans.R.el[2][2]);
718         gcm_list = get_global_cliche_mem_list();
719         for ( ptr = gcm_list; ptr != NULL; ptr = ptr->next )
720         {
721             for ( ptr2 = (List *)ptr->to; ptr2 != NULL; ptr2 = ptr2->next )
722             {
723                 cl_id = (Cliche_id *)(ptr2->to);
724                 if ( cl_id->type == 'O' )
725                 {
726                     save_flag = true;
727                     for ( ptr3 = occ_bounds; ptr3 != NULL; ptr3 = ptr3->next )
728                     {
729                         o_s = (Cliche_id *)ptr3->to;
730                         if ( (o_s->feat1 == cl_id->feat1 && o_s->feat2 == cl_id->feat2)
731                           || (o_s->feat1 == cl_id->feat2 && o_s->feat2 == cl_id->feat1) )
732                         {
733                             save_flag = false;
734                             break;
735                         }
736                     }
737                     if ( save_flag == true )
738                     {
739                         o_s = (Cliche_id *)ralloc( sizeof( Cliche_id ) );
740                         o_s->feat1 = cl_id->feat1; o_s->feat2 = cl_id->feat2;
741                         o_s->display1 = 1; o_s->display2 = 1;
742                         o_s->latShift1 = def_shift;
743                         o_s->latShift2 = def_shift;
744                         o_s->latShift3 = def_shift;
745                         o_s->latShift4 = def_shift;
746                         o_s->n1 = 0;
747                         o_s->n2 = 0;
748                         occ_bounds = ref_addtostart( occ_bounds, (void *) o_s, 0 );
749                     }
750                 }
751             }
752         }
753     }
754 
755     for ( partList = parts; partList != NULL; partList = partList->next )
756     {
757         geom_type   = ((Generic *) partList->to)->type;
758         memo        =  ((Generic *) partList->to)->to;
759         partNum = ((Generic *)(partList->to))->label;
760 
761         if( ( partNum == feature2 || feature2 <= 0 )  )
762         {
763             cl_id =NULL;
764             for ( ptr = cliche_used; ptr != NULL; ptr = ptr->next )
765             {
766                cl_id = ((Cliche_id *)(ptr->to));
767                if ( cl_id->feat1 == partNum && cl_id->type != 'O' ) break;
768                else if ( ptr->next == NULL ) cl_id = NULL;
769             }
770             switch ( geom_type )
771             {
772                 case LINE3:
773 
774                 elist1 = proj_linepts(memo, cam, &fOrient, &perpVec);
775                 if ( cl_id != NULL )
776                      { lat_shift1 = cl_id->latShift1; lat_shift2 = cl_id->latShift2; }
777                 else
778                      { lat_shift1 = def_shift; lat_shift2 = def_shift; }
779 
780                 semi_memo = get_semi_memo( memo );
781 /* allow nominal shift of 1 pixel when hypothesis testing */
782                 lat_shift1s = 1.0 + e_s_p_factor*proj_shift_line( lat_shift1, cam, semi_memo );
783                 lat_shift2s = 1.0 + e_s_p_factor*proj_shift_line( lat_shift2, cam, semi_memo );
784 
785                 llPeak = lat_shift( elist1, perpVec, cam, ms, lat_shift1s, lat_shift2s, fOrient, NULL, &shift );
786                 l_p = l_prop( elist1, ms, shift, perpVec, fOrient, NULL, hyp_limit );
787 
788                 if( ( partNum == feature2 || feature2 == 0 )&& l_p <= 1.0 - propThresh/100.0  )
789                     feature_pt_tot += disp_init(tv, elist1, l_p, ms, shift, perpVec, fOrient, NULL, hyp_limit );
790                 if (ms->match == 2 && (l_p <= 1.0 - propThresh/100.0) )
791                 {
792                     shift *= lat_shift1/lat_shift1s;
793                     shift *= lat_shift2/lat_shift2s;
794                     printf("S %d (%2.1f %2.1f)[0.0]{%3.2f 0}\n",
795                             partNum,fabs(shift),fabs(shift),(float)(1.0 - l_p)); 
796                 }
797 
798                 list_rm(elist1, rfree);
799                 elist1 = NULL;
800                 break;
801 
802                 case CONIC3:
803 
804                 elist1 = NULL; elist2 = NULL; orients = NULL, orients1 = NULL; orients2 = NULL;
805                 split_ellipse( memo, partList, &elist1, &elist2, &orients1, &orients2, cam,
806                                &alpha, &theta, &centre, &perpVec );
807                 x = vec2_get_x( &centre );
808                 y = vec2_get_y( &centre );
809                 semi_memo = get_semi_memo( memo );
810 
811                 for ( ptr = occ_bounds; ptr != NULL; ptr = ptr->next )
812                 {
813                     o_s = ptr->to;
814                     if ( o_s->feat1 == partNum )
815                     {
816                         o_s->p1 = vec2( alpha * cos(theta) + x,  alpha * sin(theta) + y );
817                         o_s->p3 = vec2( -(alpha * cos(theta)) + x, -(alpha * sin(theta)) + y );
818                         o_s->l1 = semi_memo;
819                     }
820                     if ( o_s->feat2 == partNum )
821                     {
822                         o_s->p2 = vec2( alpha * cos(theta) + x,  alpha * sin(theta) + y );
823                         o_s->p4 = vec2( -(alpha * cos(theta)) + x, -(alpha * sin(theta)) + y );
824                         o_s->l2 = semi_memo;
825                     }
826                 }
827 
828                 l_p1 = 1.0;
829                 if (  ((cl_id != NULL ) && ( cl_id->display1 == 1 )) || ms->match == 2 )
830                 {
831                      if ( cl_id != NULL )
832                         { lat_shift1 = cl_id->latShift1; lat_shift2 = cl_id->latShift2; }
833                      else
834                         { lat_shift1 = def_shift; lat_shift2 = def_shift; }
835 /* allow nominal shift again */
836                     lat_shift1s = 1.0 + e_s_p_factor*proj_shift_line( lat_shift1, cam, semi_memo );
837                     lat_shift2s = 1.0 + e_s_p_factor*proj_shift_line( lat_shift2, cam, semi_memo );
838                     list = elist1; orients = orients1;
839                     llPeak = lat_shift( list, perpVec, cam, ms, lat_shift1s, lat_shift2s, 0 , orients, &shift1 );
840                     l_p1 = l_prop( list, ms, shift1, perpVec, 0, orients, hyp_limit );
841                     if( ( partNum == feature2 || feature2 == 0 ) && l_p1 <= 1.0 - propThresh/100.0  )
842                        feature_pt_tot += disp_init(tv, list, l_p1, ms, shift1, perpVec, 0, orients, hyp_limit );
843                 }
844                 l_p2 = 1.0;
845                 if (  ((cl_id != NULL ) && (cl_id->display2 == 1 )) ||  ms->match == 2 )
846                 {
847                     if ( cl_id != NULL )
848                        { lat_shift1 = cl_id->latShift3; lat_shift2 = cl_id->latShift4; }
849                     else
850                        { lat_shift1 = def_shift; lat_shift2 = def_shift; }
851                     lat_shift1s = e_s_p_factor*proj_shift_line( lat_shift1, cam, semi_memo );
852                     lat_shift2s = e_s_p_factor*proj_shift_line( lat_shift2, cam, semi_memo );
853                     list = elist2; orients = orients2;
854                     llPeak = lat_shift( list, perpVec, cam, ms, lat_shift1s, lat_shift2s, 0 , orients, &shift2 );
855                     l_p2 = l_prop( list, ms, shift2, perpVec, 0, orients, hyp_limit );
856                     if( ( partNum == feature2 || feature2 == 0 ) && l_p2 <= 1.0 - propThresh/100.0 )
857                        feature_pt_tot += disp_init(tv, list, l_p2, ms, shift2, perpVec, 0, orients, hyp_limit );
858                 }
859                
860                 if (ms->match == 2 && ((l_p2 <= 1.0 - propThresh/100.0) || (l_p1 <= 1.0 - propThresh/100.0)))
861                 {
862                     if (lat_shift1s!=0.0) shift1 *= lat_shift1/lat_shift1s;
863                     if (lat_shift2s!=0.0) shift2 *= lat_shift2/lat_shift2s;
864                     printf("E %d (%2.1f %2.1f %2.1f %2.1f)[0.0]{%3.2f %3.2f 0}\n",
865                     partNum,fabs(shift1),fabs(shift1),fabs(shift2),fabs(shift2), 
866                     (1.0 - l_p1),(1.0 - l_p2) ); 
867                 }
868 
869                 list_rm(elist1, rfree);
870                 list_rm(elist2, rfree);
871                 list_rm(orients1, rfree);
872                 list_rm(orients2, rfree);
873 
874                 break;
875             }// end switch
876         }
877     }// end for loop
878     if ( feature2 <= 0 && ms->occflag)
879     {
880          for ( ptr = occ_bounds; ptr != NULL; ptr = ptr->next )
881          {
882              neg_occ_b_count -= 1;
883              o_s = ptr->to;
884              l_p1 =1.0;
885              if ( o_s->display1 == 1 || ms->match == 2)
886              {
887                   occ_line = occluding_line( 0, o_s, &fOrient, &perpVec, pix_sep);
888                   if (o_s != NULL)
889                   {
890                       l_s1 = o_s->latShift1;
891                       l_s2 = o_s->latShift2;
892                   }
893                   else
894                   {
895                       l_s1 = def_shift;
896                       l_s2 = def_shift;
897                   }
898                   lat_shift1 = e_s_p_factor*proj_shift_line( l_s1, cam, o_s->l1 );
899                   lat_shift2 = e_s_p_factor*proj_shift_line( l_s2, cam, o_s->l2 );
900                   llPeak = lat_shift( occ_line, perpVec, cam, ms, lat_shift1, lat_shift2, fOrient, NULL , &shift1);
901                   l_p1 = l_prop( occ_line, ms, shift1, perpVec, fOrient, NULL, hyp_limit );
902                   if( (feature2 == neg_occ_b_count || feature2 == 0) && l_p1 <= 1.0 - propThresh/100.0 )
903                       feature_pt_tot += disp_init(tv, occ_line, l_p1, ms, shift1, perpVec, fOrient, NULL, hyp_limit );
904                   list_rm(occ_line, rfree);
905               }
906               l_p2 =1.0;
907               if ( o_s->display2 == 1 || ms->match == 2)
908               {
909                   occ_line = occluding_line( 1, o_s, &fOrient, &perpVec, pix_sep);
910                   if (o_s != NULL)
911                   {
912                       l_s1 = o_s->latShift1;
913                       l_s2 = o_s->latShift2;
914                   }
915                   else
916                   {
917                       l_s1 = def_shift;
918                       l_s2 = def_shift;
919                   }
920                   lat_shift1 = e_s_p_factor*proj_shift_line( l_s1, cam, o_s->l1 );
921                   lat_shift2 = e_s_p_factor*proj_shift_line( l_s2, cam, o_s->l2 );
922                   llPeak = lat_shift(  occ_line, perpVec, cam, ms, lat_shift1, lat_shift2, fOrient, NULL, &shift2 );
923                   l_p2 = l_prop( occ_line, ms, shift2, perpVec, fOrient, NULL, hyp_limit );
924                   if( (feature2 == neg_occ_b_count || feature2 == 0) && l_p2 <= 1.0 - propThresh/100.0 )
925                       feature_pt_tot += disp_init(tv, occ_line, l_p2, ms, shift2, perpVec, fOrient, NULL, hyp_limit );
926                   list_rm(occ_line, rfree);
927               }
928               if (ms->match == 2 && ((l_p2 <= 1.0 - propThresh/100.0) || (l_p1 <= 1.0 - propThresh/100.0)))
929               {
930                    if (lat_shift1!=0.0) shift1 *= l_s1/lat_shift1;
931                    if (lat_shift2!=0.0) shift2 *= l_s2/lat_shift2;
932                    printf("0 %d %d (%2.1f %2.1f %2.1f %2.1f)[0.0 0.0]{%3.2f %3.2f 0 0}\n",
933                    o_s->feat1, o_s->feat2,fabs(shift1),fabs(shift1),fabs(shift2),fabs(shift2),
934                    (1.0 - l_p1),(1.0 - l_p2));
935               }
936           }
937      }
938      list_rm_links( occ_bounds );
939 
940      printf("hyp data %d %d %f\n", hyp_count, ov_length, (float)hyp_count/ (float) ov_length  );
941      return(hyp_count / (double)ov_length);
942 }
943 

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