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

Linux Cross Reference
Tina6/tina-libs/tina/vision/visPgh_HT.c

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

  1 /**********
  2  *
  3  * This file is part of the TINA Open Source Image Analysis Environment
  4  * henceforth known as TINA
  5  *
  6  * TINA is free software; you can redistribute it and/or modify
  7  * it under the terms of the GNU General Public License as
  8  * published by the Free Software Foundation.
  9  *
 10  * TINA is distributed in the hope that it will be useful,
 11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
 12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13  * GNU General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU General Public License
 16  * along with TINA; if not, write to the Free Software Foundation, Inc.,
 17  * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 18  *
 19  * ANY users of TINA who require exemption from the existing licence must
 20  * negotiate a new licence with Dr. Neil.A.Thacker, the sole agent for
 21  * the University of Manchester.
 22  *
 23  **********
 24  *
 25  * Program :    TINA
 26  * File    :  $Source: visPgh_HT.c $
 27  * Date    :  $Date: 2013/05/13 13:50 $
 28  * Version :  $Revision: 1.9 $
 29  *
 30  * Author  : Legacy TINA modified NAT
 31  *
 32  */
 33 /**
 34  * @file Alternative functions for the location of a scaled rigid edge based object.
 35  * @brief A hough transform approach is used in several variants as an approximation
 36  *        to a likelihood estimation process for position and orientation estimation.
 37  *
 38  */
 39 
 40 
 41 #include "visPgh_HT.h"
 42 
 43 #include <stdio.h>
 44 #include <math.h>
 45 #include <tina/image/img_GenDef.h>
 46 #include <tina/image/img_GenPro.h>
 47 #include <tina/math/mathDef.h>
 48 #include <tina/math/mathPro.h>
 49 #include <tina/geometry/geomPro.h>
 50 #include <tina/vision/vis_ShapeDef.h>
 51 #include <tina/vision/vis_ShapePro.h>
 52 #include <tina/vision/visShape_hough1.h>
 53 #include <tina/vision/visPgh_histfuncs.h>
 54 #include <tina/vision/visPgh_HTcovar.h>
 55 #include <tina/vision/visPgh_match.h>
 56 #include <tina/vision/visPgh_var.h>
 57 
 58 /*
 59 static int TYPE1001 = 1001;   
 60 static  List *hist_list=NULL;
 61 */
 62 
 63 /* HR: 10/05/2013 ((*/
 64 
 65 #ifdef _PGHDEBUG
 66 
 67 static double peak_posx=0.0,  peak_posy=0.0;
 68 
 69 void set_peak_pos(Point2 *peak_loc)
 70 {
 71    peak_posx = vec2_x(peak_loc->p);
 72    peak_posy = vec2_y(peak_loc->p);
 73 }
 74 
 75 #endif
 76 
 77 /* HR: 10/05/2013 ))*/
 78 
 79 
 80 
 81 
 82 /* ---------- */
 83 
 84 int locate_using_long_lines(Model_poly_header *current_model, Imrect *location_hough_space, List *scene_pairs_list, Match_cut_def
 85 *match_cut_def)
 86 {
 87     int         num_lines_plotted=0;
 88     List       *scene_ptr;
 89     List       *match_ptr;
 90     Vec2        model_perp_vec;
 91     Vec2        scene_perp_vec;
 92     Mat2        rotation_mat;
 93     Line2      *scene_line;
 94     Line2      *match_line;
 95     Line2       line;
 96     Imrect     *scene_hist;
 97     Imrect     *match_hist;    
 98     double      rotation;
 99     Hist_ref   *scene_hist_ref;
100     Hist_ref   *match_hist_ref;
101     Transform2  trans;
102     Match_ref  *match_ref;
103     List       *cut_match_list;
104     Transform2  transland;
105 
106     int         orientation; /* Added apa 10/1/96 */
107 
108 
109         /* (1) For each scene histogram...
110            (2)     For each matched histogram in the list...
111            (3)         If the matched histogram comes from the
112                        current model plot 2 lines parallel to the
113                        scene line */
114 /* define the landmark for location */
115     rotation_mat = rot2(0.0);
116     transland.R = rotation_mat;
117     transland.t = current_model->landmark;
118 
119     for(scene_ptr=scene_pairs_list;scene_ptr!=NULL;scene_ptr=scene_ptr->next)
120     {
121         scene_hist = scene_ptr->to;
122         scene_hist_ref = hist_ref_get(scene_hist);
123         scene_line = scene_hist_ref->line_seg;
124 
125         cut_match_list = 
126                      copy_and_cut_matches_list(scene_hist_ref, match_cut_def);
127 
128         for(match_ptr=cut_match_list;match_ptr!=NULL;
129                                       match_ptr=match_ptr->next)
130         {
131             match_ref = match_ptr->to;
132             match_hist = match_ref->hist;
133             match_hist_ref = hist_ref_get(match_hist);
134 /*
135             match_line = match_hist_ref->line_seg;
136 */
137             match_line = line2_copy(match_hist_ref->line_seg);
138             line2_transform(match_line, transland);
139 
140             if (match_hist_ref->type == DIRECTED)
141                orientation = match_ref->orientation;
142             else
143                orientation = 0;
144 
145             if (match_hist_ref->model==current_model)
146             {
147 
148                 model_perp_vec = vec2_projperp(match_line->p, match_line->v);
149                 model_perp_vec = vec2_times(match_ref->scale_factor, 
150                                                                 model_perp_vec);
151                 rotation = vec2_angle(scene_line->v, match_line->v);
152                 rotation_mat = rot2(rotation);
153                 scene_perp_vec = mat2_vprod(rotation_mat, model_perp_vec);
154 
155                 if (orientation == 0 || orientation == -1 )
156                 {
157                     line = *scene_line;         
158                     rotation_mat = rot2(0.0);
159                     trans.R = rotation_mat;
160                     trans.t = scene_perp_vec;
161                     line2_transform(&line, trans);
162                     if (hough2_extend_line(&line, location_hough_space)==TRUE)
163                     {
164                         num_lines_plotted++;
165                         hough2_plot_line(&line, location_hough_space, 
166                                                           scene_line->length);
167                     }
168                 }
169 
170 
171                 if (orientation == 0 || orientation == 1 )
172                 {
173                     line = *scene_line;       
174                     rotation_mat = rot2(0.0);
175                     trans.R = rotation_mat;
176                     trans.t = vec2_minus(scene_perp_vec);
177                     line2_transform(&line, trans);
178                     if (hough2_extend_line(&line, location_hough_space)==TRUE)
179                     {
180                         num_lines_plotted++;
181                         hough2_plot_line(&line, location_hough_space, 
182                                                           scene_line->length);
183                     }
184                 }
185 
186             }/*endif*/
187             line2_free(match_line);
188         }/*endfor(match_ptr)*/
189 
190       /* Free the generated match list */
191 
192       list_rm(cut_match_list, rfree);
193 
194     }/*endfor(scene_ptr)*/
195 
196     return (num_lines_plotted);
197 
198 }
199 
200 /* ---------- */
201 
202 int locate_using_short_lines(Model_poly_header *current_model, Imrect *location_hough_space, 
203                              List *scene_pairs_list, Match_cut_def *match_cut_def)
204 {
205     int         num_lines_plotted=0;
206     List       *scene_ptr;
207     List       *match_ptr;
208     Vec2        model_vec;
209     Vec2        scene_vec;
210     Mat2        rotation_mat;
211     Line2      *scene_line;
212     Line2      *match_line;
213     Line2       line;
214     Imrect     *scene_hist;
215     Imrect     *match_hist;    
216     double      rotation;
217     Hist_ref   *scene_hist_ref;
218     Hist_ref   *match_hist_ref;
219     Match_ref  *match_ref;
220     List       *cut_match_list;
221     double      model_l0;
222     Transform2  trans;
223     
224 
225         /* (1) For each scene histogram...
226            (2)     For each matched histogram in the list...
227            (3)         If the matched histogram comes from the
228                        current model plot 2 lines parallel to the
229                        scene line of length 2*model lines length */
230 /* define the landmark for location */
231     rotation_mat = rot2(0.0);
232     trans.R = rotation_mat;
233     trans.t = current_model->landmark;
234 
235     for(scene_ptr=scene_pairs_list;scene_ptr!=NULL;scene_ptr=scene_ptr->next)
236     {
237         scene_hist = scene_ptr->to;
238         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
239         scene_line = scene_hist_ref->line_seg;
240 
241         cut_match_list = 
242                      copy_and_cut_matches_list(scene_hist_ref, match_cut_def);
243 
244         for(match_ptr=cut_match_list;match_ptr!=NULL;
245                                       match_ptr=match_ptr->next)
246         {
247             match_ref = match_ptr->to;
248             match_hist = match_ref->hist;
249             match_hist_ref = prop_get(match_hist->props, HIST_REF_TYPE);
250             model_l0 = match_hist_ref->l0;
251 /*
252             match_line = match_hist_ref->line_seg;
253 */
254             match_line = line2_copy(match_hist_ref->line_seg);
255             line2_transform(match_line, trans);
256 
257             if (match_hist_ref->model==current_model)
258             {
259 
260     /* Perhaps there should be 4 cases, rather than just 2 ???? */
261 
262                 model_vec = match_line->p;
263                 rotation = vec2_angle(scene_line->v, match_line->v);
264                 rotation_mat = rot2(rotation);
265                 scene_vec = mat2_vprod(rotation_mat, model_vec);
266 
267                 /* Now scale the scene vector by the scale factor */
268                 scene_vec = vec2_times(match_ref->scale_factor, scene_vec);
269 
270 
271                 line.p =  vec2_diff(scene_line->p, scene_vec);
272                 line.p1 = vec2_diff(line.p, vec2_times(
273                     (match_ref->scale_factor*model_l0/2.0), scene_line->v));
274                 line.p2 = vec2_sum(line.p, vec2_times(
275                     (match_ref->scale_factor*model_l0/2.0), scene_line->v));
276                 line.v = scene_line->v;
277                 line.length = (float)model_l0;
278 
279                 num_lines_plotted++;
280                     
281                 hough2_plot_line(&line, location_hough_space, 
282                                 match_ref->scale_factor*scene_line->length);
283                 line.p1 = vec2_diff(line.p, vec2_times(
284                     (match_ref->scale_factor*model_l0/2.0), scene_line->v));
285                 line.p2 = vec2_sum(line.p, vec2_times(
286                     (match_ref->scale_factor*model_l0/2.0), scene_line->v));
287 
288                 line.p =  vec2_sum(scene_line->p, scene_vec);
289 
290                 line.v = scene_line->v;
291                 line.length = (float)(2.0*model_l0);
292 
293                 num_lines_plotted++;
294                     
295                 hough2_plot_line(&line, location_hough_space, 
296                                 match_ref->scale_factor*scene_line->length);
297 
298 
299             }/*endif*/
300             line2_free(match_line);
301 
302         }/*endfor(match_ptr)*/
303 
304       /* Free the generated match list */
305 
306       list_rm(cut_match_list, rfree);
307 
308     }/*endfor(scene_ptr)*/
309 
310     return (num_lines_plotted);
311 }
312 
313 /* ---------- */
314 
315 void locate_using_points(Model_poly_header *current_model, Imrect *location_hough_space, 
316                          List *scene_pairs_list, Match_cut_def *match_cut_def)
317 {
318         /* (1) For each pair of scene histograms which have been 
319                matched to lines from this model place votes in 
320                the hough space at the respective location - ie,  4
321                points which are the intersection of parallel lines 
322                to the scene lines. */
323 
324     List       *scene_ptr;
325     List       *match_ptr;
326     Vec2        model_perp_vec;
327     Vec2        scene_perp_vec;
328     Mat2        rotation_mat;
329     Line2      *scene_line;
330     Line2      *match_line;
331     Imrect     *scene_hist;
332     Imrect     *match_hist;    
333     double      rotation;
334     Hist_ref   *scene_hist_ref;
335     Hist_ref   *match_hist_ref;
336     Match_ref  *match_ref;
337     List       *cut_match_list;
338     List       *entry_list = NULL;
339     Pairs_HT_entry *entry;
340     int         orientation;
341     Pgh_serr *serr;
342     Transform2  trans;
343     
344 /* define the landmark for location */
345     rotation_mat = rot2(0.0);
346     trans.R = rotation_mat;
347     trans.t = current_model->landmark;
348 
349 /* Get the data from the scene and the match list */
350 
351   for(scene_ptr=scene_pairs_list;scene_ptr!=NULL;scene_ptr=scene_ptr->next)
352     {
353         scene_hist = scene_ptr->to;
354         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
355 /* selected scene line */
356         scene_line = scene_hist_ref->line_seg;
357 
358         cut_match_list = copy_and_cut_matches_list(scene_hist_ref, match_cut_def);
359 
360         for(match_ptr=cut_match_list;match_ptr!=NULL;
361                                       match_ptr=match_ptr->next)
362         {
363             match_ref = match_ptr->to;
364             match_hist = match_ref->hist;
365             match_hist_ref = prop_get(match_hist->props, HIST_REF_TYPE);
366             if (match_hist_ref->type == DIRECTED)
367                orientation = match_ref->orientation;
368             else
369                orientation = 0;
370 
371 /* hypothesised matching model line */
372             match_line = line2_copy(match_hist_ref->line_seg);
373             line2_transform(match_line, trans);
374 
375             if (match_hist_ref->model==current_model)
376             {
377                 /* Create a list of HT entry data */
378 
379                 model_perp_vec = vec2_projperp(match_line->p, match_line->v);
380                 rotation = vec2_angle(scene_line->v, match_line->v);
381                 rotation_mat = rot2(rotation);
382                 scene_perp_vec = mat2_vprod(rotation_mat, model_perp_vec);
383 
384                 if (orientation == 0 || orientation == -1 )
385                 {
386                     entry = ralloc(sizeof(Pairs_HT_entry));
387                     entry->scene_line = scene_line;
388                     entry->perp_vec = scene_perp_vec;
389                     entry->scale = match_ref->scale_factor;
390 
391                     if ((serr = prop_get(match_hist->props, PGH_SERR_TYPE))==NULL)
392                     {
393                         entry->smin = match_ref->scale_factor;
394                         entry->smax = match_ref->scale_factor;
395                     }
396                     else
397                     {
398                         entry->smin = serr->smin;
399                         entry->smax = serr->smax;
400                     }
401 
402                     entry_list = ref_addtostart(entry_list, entry, 
403                                                           PAIRS_HT_ENTRY);
404                         }
405 
406                 if (orientation == 0 || orientation == 1 )
407                 {
408                     entry = ralloc(sizeof(Pairs_HT_entry));
409 
410                     entry->scene_line = scene_line;
411                     entry->perp_vec = vec2_minus(scene_perp_vec);
412                     entry->scale = match_ref->scale_factor;
413 
414                     if ((serr = prop_get(match_hist->props, PGH_SERR_TYPE))==NULL)
415                     {
416                         entry->smin = match_ref->scale_factor;
417                         entry->smax = match_ref->scale_factor;
418                     }
419                     else
420                     {
421                         entry->smin = serr->smin;
422                         entry->smax = serr->smax;
423                     }
424 
425                     entry_list = ref_addtostart(entry_list, entry, PAIRS_HT_ENTRY);
426                 } 
427             } /*end if match_hist_ref*/
428             line2_free(match_line);
429         } /*end for match_ptr*/
430         
431         list_rm(cut_match_list, rfree);
432 
433      } /*end for scene_ptr*/
434 
435 
436     /* Call the plotting procedure then on return delete the parallel lines list */
437 
438         hough2_fill(location_hough_space, 0.0);
439     hough2_plot_points(entry_list, location_hough_space);
440     list_rm(entry_list, rfree);
441 }
442 
443 /* ---------- */
444 
445 void angle_from_loc(Imrect *hough1, Point2 *loc, Model_poly_header *current_model)
446 {
447     Vec2       scene_ref_vec;
448     List      *scene_ptr, *match_ptr;
449     Line2     *scene_line, *match_line;
450     double     angle1,angle2,sigma;
451     Imrect    *scene_hist, *match_hist;
452     Hist_ref  *scene_hist_ref, *match_hist_ref;
453     Match_ref *match_ref;
454     List      *cut_match_list;
455     List          *scene_pairs_list;
456     Match_cut_def *match_cut_def;
457     Pairs_hist_def *hist_def;
458     Pairs_scale_def *scale_def;
459 
460 #ifdef _PGHDEBUG
461 shistogram **hists = (shistogram **)hist_vec();  
462 int  no_bins=100;
463 float  hist_range= 2*PI;
464 
465 if( hists[130] !=NULL) hfree(hists[130]);
466 if( hists[131] !=NULL) hfree(hists[131]);
467 if( hists[132] !=NULL) hfree(hists[132]);
468 hists[130] = hbook1(130,"angle2 \0",(float)(-hist_range),(float)(+hist_range), no_bins);
469 hists[131] = hbook1(131,"angle1-angle2/sigma \0",(float)-10.0,(float)10.0, no_bins);
470 hists[132] = hbook1(132,"angle2 wrapped \0",(float)(-hist_range),(float)(+hist_range), no_bins);
471 #endif
472 
473         /* (1) For each scene line which has matched to
474                the current model...
475            (2) Plot a point in hough space for the angle 
476                between the scene reference vector and the 
477                matched model reference vector. */
478     scene_pairs_list = pairs_scene_pairs_list_get();
479     match_cut_def = pairs_match_cut_def_get();
480     hist_def = pairs_hist_def_get();
481     scale_def = pairs_scale_def_get();
482 
483 
484     for(scene_ptr=scene_pairs_list; scene_ptr!=NULL;
485                                        scene_ptr=scene_ptr->next)
486     {
487         scene_hist = scene_ptr->to;
488         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
489         scene_line = scene_hist_ref->line_seg;
490 
491         cut_match_list = copy_and_cut_matches_list(scene_hist_ref, 
492                                                             match_cut_def);
493 
494         for(match_ptr=cut_match_list;match_ptr!=NULL;
495                                               match_ptr=match_ptr->next)
496         {
497             match_ref = match_ptr->to;
498             match_hist = match_ref->hist;
499             match_hist_ref = prop_get(match_hist->props, HIST_REF_TYPE);
500             match_line = match_hist_ref->line_seg;
501 
502             if ( (match_hist_ref->model==current_model) &&
503                  (scene_line->length>hist_def->min_length) )
504             {
505                 sigma = (float)(sqrt(pow(atan((double)(2.0/(scene_line->length))),2.0)
506                                + pow(atan((double)(2.0/(match_line->length))),2.0)));
507 
508                 sigma *= scale_def->HTsigma;
509                 scene_ref_vec = vec2_sum(scene_line->p, vec2_minus(loc->p));
510 /* nat modified 3/3/06 */
511                 angle1 = vec2_angle(scene_ref_vec, vec2_sum(match_line->p,current_model->landmark));
512                 angle2 = vec2_angle(scene_line->v, match_line->v);
513 
514 /* basic debug for checking orinetation data NAT 14/5/12 */
515 #ifdef _PGHDEBUG
516 hfill1(hists[130], angle2 , 1.0);
517 hfill1(hists[131], (angle1-angle2)/sigma , 1.0);
518 #endif
519 
520                 if (fabs(angle1-angle2)<3.0*sigma)
521                 {
522                     hough1_plot_log_gauss(hough1, angle2-2.0*PI, sigma);
523                     hough1_plot_log_gauss(hough1, angle2, sigma);
524                     hough1_plot_log_gauss(hough1, angle2+2.0*PI, sigma);
525 #ifdef _PGHDEBUG
526 hfill1(hists[132], angle2-2.0*PI , 1.0);
527 hfill1(hists[132], angle2 , 1.0);
528 hfill1(hists[132], angle2+2.0*PI , 1.0);
529 #endif
530                 }
531                 else if (fabs(angle1-angle2+PI)<3.0*sigma)
532                 {
533                     hough1_plot_log_gauss(hough1, angle2-PI, sigma);
534                     hough1_plot_log_gauss(hough1, angle2+PI, sigma);
535                     hough1_plot_log_gauss(hough1, angle2-3.0*PI, sigma);  /* fixed */
536 #ifdef _PGHDEBUG
537 hfill1(hists[132], angle2-PI, 1.0);
538 hfill1(hists[132], angle2+PI , 1.0);
539 hfill1(hists[132], angle2-3.0*PI , 1.0);
540 #endif
541                 }
542                 else if (fabs(angle1-angle2-PI)<3.0*sigma)
543                 {
544                     hough1_plot_log_gauss(hough1, angle2+3.0*PI, sigma); /* fixed */
545                     hough1_plot_log_gauss(hough1, angle2-PI, sigma);
546                     hough1_plot_log_gauss(hough1, angle2+PI, sigma);
547 #ifdef _PGHDEBUG
548 hfill1(hists[132], angle2+3.0*PI , 1.0);
549 hfill1(hists[132], angle2-PI , 1.0);
550 hfill1(hists[132], angle2+PI , 1.0);
551 #endif
552                 }
553             }
554         }
555 
556       /* Free the generated match list */
557 
558       list_rm(cut_match_list, rfree);
559     }
560 
561 }
562 
563 /* ---------- */
564 
565 double scale_from_loc(Point2 *loc, Model_poly_header *model)
566 {
567     Imrect *scale_hough;
568     Pairs_scale_def *scale_def;
569     List *scene_pairs_list, *scene_ptr;
570     Imrect *scene_hist;
571     Hist_ref *scene_hist_ref;
572     Line2 *scene_line;
573     List *cut_match_list, *match_ptr;
574     Match_cut_def *match_cut_def;
575     Match_ref *match_ref;
576     Imrect *match_hist;
577     Hist_ref *match_hist_ref;
578     Line2 *match_line;
579     double scene_dist, model_dist, scale;
580     double model_perp_dist, scene_perp_dist, dist_error;
581     Hough1_peak peak;
582     Mat2 *C;
583     Mat2 R, Rt, C_rot;
584     double scene_line_angle;
585     double scale_error, perp_error, tang_error, proj_error, scene_tang_dist;
586 
587 #ifdef _PGHDEBUG
588 shistogram **hists = (shistogram **)hist_vec();  
589 int  no_bins=100;
590 float  hist_range= 50.0;
591 
592 if( hists[133] !=NULL) hfree(hists[133]);
593 if( hists[134] !=NULL) hfree(hists[134]);
594 if( hists[135] !=NULL) hfree(hists[135]);
595 hists[133] = hbook1(133,"perp dist error \0",(float)(-hist_range),(float)(+hist_range), no_bins);
596 hists[134] = hbook1(134,"scaled perp dist / error  \0",(float)(-10.0),(float)(+10.0), no_bins);
597 hists[135] = hbook1(135,"scale  \0",(float)0.0,(float)2.0, no_bins);
598 #endif
599 
600     scale_def = pairs_scale_def_get();
601     scene_pairs_list = pairs_scene_pairs_list_get();
602     match_cut_def = pairs_match_cut_def_get();
603 
604     if ( (scale_def->scale_max==1.0) &&
605          (scale_def->scale_min==1.0) )
606         return 1.0;
607 
608     scale_hough = hough1_alloc(scale_def->scale_min, scale_def->scale_max,
609                                scale_def->HTbinsize, float_v);
610 
611     C = prop_get(loc->props, COVAR2);
612 
613 /* Make scale entries for matches which may have been responsible for this
614    peak in the location HT */
615 
616     for(scene_ptr=scene_pairs_list;scene_ptr!=NULL;scene_ptr=scene_ptr->next)
617     {
618         scene_hist = scene_ptr->to;
619         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
620         scene_line = scene_hist_ref->line_seg;
621 
622 /* propegate error for scale from error on location */
623         scene_line_angle = acos(vec2_dot(scene_line->v, vec2_ex()));
624         R = rot2(-scene_line_angle);
625         Rt = mat2_transpose(R);
626         C_rot = mat2_prod(mat2_prod(Rt, *C), R);
627 /* error on perpendicular distance is now y ordinate */
628         perp_error = sqrt(mat2_yy(C_rot));
629 
630         cut_match_list = copy_and_cut_matches_list(scene_hist_ref, 
631                                                             match_cut_def);
632 
633         for(match_ptr=cut_match_list;match_ptr!=NULL;
634                                               match_ptr=match_ptr->next)
635         {
636             match_ref = match_ptr->to;
637             match_hist = match_ref->hist;
638             match_hist_ref = hist_ref_get(match_hist);
639             match_line = match_hist_ref->line_seg;
640 
641             if (match_hist_ref->model==model)
642             {
643 
644                 /* Could this match have contributed to the location peak? */
645                 model_perp_dist = 
646                     vec2_mod(vec2_projperp(vec2_sum(match_line->p,model->landmark), match_line->v));
647                 scene_perp_dist = vec2_dist_point_line(loc->p, scene_line->p, scene_line->v);
648                 /* estimate difference  between perpendicular distance for model and scene */
649                 dist_error = scene_perp_dist-model_perp_dist*match_ref->scale_factor;
650                 scene_dist = vec2_dist(scene_line->p, loc->p);
651                 model_dist = vec2_mod(vec2_sum(match_line->p,model->landmark));
652                 /* propegate error from orientation of scene line */
653                 scene_tang_dist = scene_dist*cos(asin(scene_perp_dist/scene_dist));
654                 tang_error = scale_def->HTsigma*scene_tang_dist*2.0/scene_line->length;
655                 /* total error on perpendicular distance*/
656                 proj_error = sqrt(perp_error*perp_error+tang_error*tang_error);
657 #ifdef _PGHDEBUG
658 /*
659 printf("model_dist %f scene_dist %f \n", model_dist, scene_dist);
660 printf("model_perp %f scene_perp %f \n", model_perp_dist, scene_perp_dist);
661 printf("perp_error %f, tang_error %f \n", perp_error, tang_error);
662 */
663 hfill1(hists[133], dist_error , 1.0);
664 hfill1(hists[134], dist_error/proj_error , 1.0);
665 #endif
666 
667                 if ((dist_error/proj_error>-scale_def->HTd_error) &&
668                     (dist_error/proj_error<scale_def->HTd_error))
669                 {
670                     scale_error = proj_error/model_perp_dist;
671                     switch(scale_def->HTmethod)
672                     {
673                         case SCALE_HT_REF:
674                             scale = match_ref->scale_factor;
675                             break;
676                         case SCALE_HT_PERPDIST:
677                             scale = scene_perp_dist/model_perp_dist;
678                             break;
679                         case SCALE_HT_DIST:
680                             scale = scene_dist/model_dist;
681                     }
682                     hough1_plot_log_gauss(scale_hough, scale, scale_error);
683 #ifdef _PGHDEBUG
684 /*
685 printf("scale, error and residual %f %f %f \n", scale, scale_error, dist_error/proj_error);
686 */
687 hfill1(hists[135], scale , 1.0);
688 #endif
689                 }
690             }
691         }
692     }
693 
694     peak = hough1_locate_max_peak(scale_hough, 0.0, float_v);
695 #ifdef _PGHDEBUG    
696     stack_push(scale_hough, IMRECT, im_free);
697 #else
698     im_free(scale_hough);
699 #endif
700 
701     return peak.x;
702 }
703 
704 /* ---------- */
705 
706 /* ---------- Code from hough2_points.c APA 4/2/94 ---------- */
707 
708 
709 void hough2_plot_points(List *entry_list, Imrect *hough2)
710 {
711     double          min_angle;
712     Line2          *line1, *line2;
713     List           *ptr1;
714     List           *ptr2;
715     double          weight, angle;
716     Bool            t1, t2;
717     Pairs_hough_def *hough_def;
718 
719     Pairs_HT_entry *entry1, *entry2;
720     Mat2 R;
721     Transform2 T;
722     Vec2 OS0, OS1, S0S1;
723     double smin, smax;
724     double smin1, smax1;
725     double smin2, smax2;
726     Vec2 p0, p1, p;
727 
728     double a, b, c, d, det;    /* APA 31.1.96 */
729     Mat2 C;
730     double theta, l1, l2, eig_lim;
731     double sigma2;
732 
733     double sdx, sdy, s_angle;
734     Mat2 SCov, U, U_trans, SCov_rot;
735     Mat2 TotCov;
736 
737 /* HR: 13/05/2013 ((*/
738 
739 #ifdef _PGHDEBUG
740     Mat2 invC;
741     double dx, dy, detc, chi2;
742     shistogram **hists;
743 
744     hists = (shistogram **)hist_vec();
745     if( hists[195] !=NULL) hfree(hists[195]);
746     hists[195] = hbook1(195, "Chi squared distances\0", 0.0, 10.0, 50);
747 #endif
748 
749 /* HR: 13/05/2013 ))*/
750 
751 
752 /* Get the values which define the extended parallel lines from the list then find the */
753 /* intersection between a pair.  Plot a point in the hough space to correspond to the */
754                     /* intersection after smoothing using a gaussian */
755 
756     hough_def = pairs_hough_def_get();
757     min_angle = hough_def->min_angle;
758     sigma2 = hough_def->gauss_sigma*hough_def->gauss_sigma;
759     eig_lim = hough_def->eig_lim;
760 
761     R = rot2(0.0);
762     T.R = R;
763 
764     for(ptr1=entry_list;ptr1!=NULL;ptr1=ptr1->next)
765     {
766         entry1 = ptr1->to;
767 
768 
769         for(ptr2=ptr1->next;ptr2!=NULL;ptr2=ptr2->next)
770         {
771             entry2 = ptr2->to;
772 
773             /* Determine the start of the line of equal scale 
774                (where the scene lines intersect) */
775 
776             OS0 = get_intersection(entry1->scene_line, entry2->scene_line, &t1, &t2);
777 
778             /* Determine the point at scale=1 */
779 
780             line1 = line2_copy(entry1->scene_line);
781             T.t = entry1->perp_vec;
782             line2_transform(line1, T);
783 
784             line2 = line2_copy(entry2->scene_line);
785             T.t = entry2->perp_vec;
786             line2_transform(line2, T);
787 
788             OS1 = get_intersection(line1, line2, &t1, &t2);
789 
790             S0S1 = vec2_sum(OS1, vec2_minus(OS0));
791 
792             /* Now: p(s) = OS0 + s x S0S1 */
793 
794             smin1 = entry1->smin;
795             smax1 = entry1->smax;
796             smin2 = entry2->smin;
797             smax2 = entry2->smax;
798 
799             if (smin1<smin2)
800                 smin = smin2;
801             else
802                 smin = smin1;
803 
804             if (smax1>smax2)
805                 smax = smax2;
806             else
807                 smax = smax1;
808 
809             p0 = vec2_sum(OS0, vec2_times(smin, S0S1));
810             p1 = vec2_sum(OS0, vec2_times(smax, S0S1));
811 
812             /* Construct a covariance matrix to represent the scale 
813                error if dealing with variable scale data. This is 
814                an approximation to the correct error */
815 
816             sdx = vec2_mod(vec2_diff(p1, p0))/sqrt(12.0); /* NAT: 13/05/2013: amended */
817             sdy = sdx/1000.0;
818 
819             if (smin!=smax)
820                 s_angle = vec2_angle(vec2_ex(), vec2_diff(p0, p1));
821             else
822                 s_angle = 0.0;            
823 
824             SCov = mat2(sdx*sdx, 0.0, 0.0, sdy*sdy);
825             U = rot2(s_angle);
826             U_trans = mat2_transpose(U);
827             SCov_rot = mat2_prod(mat2_prod(U_trans, SCov), U);
828 
829             p = vec2_midpoint(p0, p1);
830 
831             angle = fabs(vec2_angle(line2->v , line1->v));
832             if(angle > min_angle)
833             {
834                 weight = entry1->scene_line->length + 
835                                      entry2->scene_line->length;
836 
837 
838                 /* Remove short lines */
839                 if( (entry1->scene_line->length >= 4.0) && 
840                     (entry2->scene_line->length >= 4.0) )
841                 {
842                     get_covariance(line1, line2, &a, &b, &c, &d);
843                     det = a*d - c*b;
844                     conic_eigen(d/det, -c/det, a/det, &theta, &l1, &l2);
845 
846                     /* Don't bother with v.large ellipses */
847                     if ( (l1 <= eig_lim*eig_lim) &&
848                          (l2 <= eig_lim*eig_lim) )
849                     {
850                         C = mat2(d/det, -c/det, -b/det, a/det); 
851 
852                         /* Scale the covariance matrix according to
853                            line endpoint error*/
854                         mat2_xx(C) *= (float)sigma2;
855                         mat2_xy(C) *= (float)sigma2;
856                         mat2_yx(C) *= (float)sigma2;
857                         mat2_yy(C) *= (float)sigma2;
858 
859                         TotCov = mat2_sum(C, SCov_rot);
860 
861                         hough2_plot_ellipse(hough2, &p, &TotCov, weight);
862 
863 /* HR: 13/05/2013: used some code from hough2_plot_ellipse in visShape_hough2.c ((*/
864 
865 #ifdef _PGHDEBUG
866                         detc = mat2_xx(TotCov)*mat2_yy(TotCov) - mat2_xy(TotCov)*mat2_yx(TotCov);
867 
868                         mat2_xx(invC) = (float)(mat2_yy(TotCov)/detc);
869                         mat2_xy(invC) = (float)(-mat2_xy(TotCov)/detc);
870                         mat2_yx(invC) = (float)(-mat2_yx(TotCov)/detc);
871                         mat2_yy(invC) = (float)(mat2_xx(TotCov)/detc);
872 
873                         dx = peak_posx - vec2_x(p);
874                         dy = peak_posy - vec2_y(p);
875 
876                         chi2 = dx*dx*mat2_xx(invC) + dx*dy*mat2_xy(invC) + 
877                         dx*dy*mat2_yx(invC) + dy*dy*mat2_yy(invC);
878 
879                         hfill1(hists[195], chi2, 1.0);       
880 #endif
881 
882 /* HR: 13/05/2013 ))*/
883 
884                     }
885                 }
886             }
887         }
888     }
889 }
890 
891 
892 /* ---------- */
893 
894 
895 

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