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

Linux Cross Reference
Tina5/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 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-libs/tina/vision/visPgh_HT.c,v $
 23  * Date    :  $Date: 2008/12/04 15:28:02 $
 24  * Version :  $Revision: 1.7 $
 25  * CVS Id  :  $Id: visPgh_HT.c,v 1.7 2008/12/04 15:28:02 paul Exp $
 26  *
 27  * Author  : NAT, C.Brown, A.Ashbrook.
 28  *
 29  */
 30 /**
 31  * @file Alternative functions for the location of a scaled rigid edge based object.
 32  * @brief A hough transform approach is used in several variants as an approximation
 33  *        to a likelihood estimation process for position and orientation estimation.
 34  *
 35  */
 36 
 37 #include "visPgh_HT.h"
 38 #include <stdio.h>
 39 #include <math.h>
 40 #include <tina/math/mathDef.h>
 41 #include <tina/math/mathPro.h>
 42 #include <tina/geometry/geomPro.h>
 43 #include <tina/vision/vis_ShapeDef.h>
 44 #include <tina/vision/vis_ShapePro.h>
 45 #include <tina/vision/visShape_hough1.h>
 46 #include <tina/vision/visPgh_histfuncs.h>
 47 #include <tina/vision/visPgh_HTcovar.h>
 48 #include <tina/vision/visPgh_match.h>
 49 #include <tina/vision/visPgh_var.h>
 50 
 51 
 52 
 53 /* ---------- */
 54 
 55 int locate_using_long_lines(Model_poly_header *current_model, Imrect *location_hough_space, List *scene_pairs_list, Match_cut_def
 56 *match_cut_def)
 57 {
 58     int         num_lines_plotted=0;
 59     List       *scene_ptr;
 60     List       *match_ptr;
 61     Vec2        model_perp_vec;
 62     Vec2        scene_perp_vec;
 63     Mat2        rotation_mat;
 64     Line2      *scene_line;
 65     Line2      *match_line;
 66     Line2       line;
 67     Imrect     *scene_hist;
 68     Imrect     *match_hist;    
 69     double      rotation;
 70     Hist_ref   *scene_hist_ref;
 71     Hist_ref   *match_hist_ref;
 72     Transform2  trans;
 73     Match_ref  *match_ref;
 74     List       *cut_match_list;
 75     Transform2  transland;
 76 
 77     int         orientation; /* Added apa 10/1/96 */
 78 
 79 
 80         /* (1) For each scene histogram...
 81            (2)     For each matched histogram in the list...
 82            (3)         If the matched histogram comes from the
 83                        current model plot 2 lines parallel to the
 84                        scene line */
 85 /* define the landmark for location */
 86     rotation_mat = rot2(0.0);
 87     transland.R = rotation_mat;
 88     transland.t = current_model->landmark;
 89 
 90     for(scene_ptr=scene_pairs_list;scene_ptr!=NULL;scene_ptr=scene_ptr->next)
 91     {
 92         scene_hist = scene_ptr->to;
 93         scene_hist_ref = hist_ref_get(scene_hist);
 94         scene_line = scene_hist_ref->line_seg;
 95 
 96         cut_match_list = 
 97                      copy_and_cut_matches_list(scene_hist_ref, match_cut_def);
 98 
 99         for(match_ptr=cut_match_list;match_ptr!=NULL;
100                                       match_ptr=match_ptr->next)
101         {
102             match_ref = match_ptr->to;
103             match_hist = match_ref->hist;
104             match_hist_ref = hist_ref_get(match_hist);
105 /*
106             match_line = match_hist_ref->line_seg;
107 */
108             match_line = line2_copy(match_hist_ref->line_seg);
109             line2_transform(match_line, transland);
110 
111             if (match_hist_ref->type == DIRECTED)
112                orientation = match_ref->orientation;
113             else
114                orientation = 0;
115 
116             if (match_hist_ref->model==current_model)
117             {
118 
119                 model_perp_vec = vec2_projperp(match_line->p, match_line->v);
120                 model_perp_vec = vec2_times(match_ref->scale_factor, 
121                                                                 model_perp_vec);
122                 rotation = vec2_angle(scene_line->v, match_line->v);
123                 rotation_mat = rot2(rotation);
124                 scene_perp_vec = mat2_vprod(rotation_mat, model_perp_vec);
125 
126                 if (orientation == 0 || orientation == -1 )
127                 {
128                     line = *scene_line;         
129                     rotation_mat = rot2(0.0);
130                     trans.R = rotation_mat;
131                     trans.t = scene_perp_vec;
132                     line2_transform(&line, trans);
133                     if (hough2_extend_line(&line, location_hough_space)==TRUE)
134                     {
135                         num_lines_plotted++;
136                         hough2_plot_line(&line, location_hough_space, 
137                                                           scene_line->length);
138                     }
139                 }
140 
141 
142                 if (orientation == 0 || orientation == 1 )
143                 {
144                     line = *scene_line;       
145                     rotation_mat = rot2(0.0);
146                     trans.R = rotation_mat;
147                     trans.t = vec2_minus(scene_perp_vec);
148                     line2_transform(&line, trans);
149                     if (hough2_extend_line(&line, location_hough_space)==TRUE)
150                     {
151                         num_lines_plotted++;
152                         hough2_plot_line(&line, location_hough_space, 
153                                                           scene_line->length);
154                     }
155                 }
156 
157             }/*endif*/
158             line2_free(match_line);
159         }/*endfor(match_ptr)*/
160 
161       /* Free the generated match list */
162 
163       list_rm(cut_match_list, rfree);
164 
165     }/*endfor(scene_ptr)*/
166 
167     return (num_lines_plotted);
168 
169 }
170 
171 /* ---------- */
172 
173 int locate_using_short_lines(Model_poly_header *current_model, Imrect *location_hough_space, 
174                              List *scene_pairs_list, Match_cut_def *match_cut_def)
175 {
176     int         num_lines_plotted=0;
177     List       *scene_ptr;
178     List       *match_ptr;
179     Vec2        model_vec;
180     Vec2        scene_vec;
181     Mat2        rotation_mat;
182     Line2      *scene_line;
183     Line2      *match_line;
184     Line2       line;
185     Imrect     *scene_hist;
186     Imrect     *match_hist;    
187     double      rotation;
188     Hist_ref   *scene_hist_ref;
189     Hist_ref   *match_hist_ref;
190     Match_ref  *match_ref;
191     List       *cut_match_list;
192     double      model_l0;
193     Transform2  trans;
194     
195 
196         /* (1) For each scene histogram...
197            (2)     For each matched histogram in the list...
198            (3)         If the matched histogram comes from the
199                        current model plot 2 lines parallel to the
200                        scene line of length 2*model lines length */
201 /* define the landmark for location */
202     rotation_mat = rot2(0.0);
203     trans.R = rotation_mat;
204     trans.t = current_model->landmark;
205 
206     for(scene_ptr=scene_pairs_list;scene_ptr!=NULL;scene_ptr=scene_ptr->next)
207     {
208         scene_hist = scene_ptr->to;
209         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
210         scene_line = scene_hist_ref->line_seg;
211 
212         cut_match_list = 
213                      copy_and_cut_matches_list(scene_hist_ref, match_cut_def);
214 
215         for(match_ptr=cut_match_list;match_ptr!=NULL;
216                                       match_ptr=match_ptr->next)
217         {
218             match_ref = match_ptr->to;
219             match_hist = match_ref->hist;
220             match_hist_ref = prop_get(match_hist->props, HIST_REF_TYPE);
221             model_l0 = match_hist_ref->l0;
222 /*
223             match_line = match_hist_ref->line_seg;
224 */
225             match_line = line2_copy(match_hist_ref->line_seg);
226             line2_transform(match_line, trans);
227 
228             if (match_hist_ref->model==current_model)
229             {
230 
231     /* Perhaps there should be 4 cases, rather than just 2 ???? */
232 
233                 model_vec = match_line->p;
234                 rotation = vec2_angle(scene_line->v, match_line->v);
235                 rotation_mat = rot2(rotation);
236                 scene_vec = mat2_vprod(rotation_mat, model_vec);
237 
238                 /* Now scale the scene vector by the scale factor */
239                 scene_vec = vec2_times(match_ref->scale_factor, scene_vec);
240 
241 
242                 line.p =  vec2_diff(scene_line->p, scene_vec);
243                 line.p1 = vec2_diff(line.p, vec2_times(
244                     (match_ref->scale_factor*model_l0/2.0), scene_line->v));
245                 line.p2 = vec2_sum(line.p, vec2_times(
246                     (match_ref->scale_factor*model_l0/2.0), scene_line->v));
247                 line.v = scene_line->v;
248                 line.length = (float)model_l0;
249 
250                 num_lines_plotted++;
251                     
252                 hough2_plot_line(&line, location_hough_space, 
253                                 match_ref->scale_factor*scene_line->length);
254                 line.p1 = vec2_diff(line.p, vec2_times(
255                     (match_ref->scale_factor*model_l0/2.0), scene_line->v));
256                 line.p2 = vec2_sum(line.p, vec2_times(
257                     (match_ref->scale_factor*model_l0/2.0), scene_line->v));
258 
259                 line.p =  vec2_sum(scene_line->p, scene_vec);
260 
261                 line.v = scene_line->v;
262                 line.length = (float)(2.0*model_l0);
263 
264                 num_lines_plotted++;
265                     
266                 hough2_plot_line(&line, location_hough_space, 
267                                 match_ref->scale_factor*scene_line->length);
268 
269 
270             }/*endif*/
271             line2_free(match_line);
272 
273         }/*endfor(match_ptr)*/
274 
275       /* Free the generated match list */
276 
277       list_rm(cut_match_list, rfree);
278 
279     }/*endfor(scene_ptr)*/
280 
281     return (num_lines_plotted);
282 }
283 
284 /* ---------- */
285 
286 void locate_using_points(Model_poly_header *current_model, Imrect *location_hough_space, 
287                          List *scene_pairs_list, Match_cut_def *match_cut_def)
288 {
289         /* (1) For each pair of scene histograms which have been 
290                matched to lines from this model place votes in 
291                the hough space at the respective location - ie,  4
292                points which are the intersection of parallel lines 
293                to the scene lines. */
294 
295     List       *scene_ptr;
296     List       *match_ptr;
297     Vec2        model_perp_vec;
298     Vec2        scene_perp_vec;
299     Mat2        rotation_mat;
300     Line2      *scene_line;
301     Line2      *match_line;
302     Imrect     *scene_hist;
303     Imrect     *match_hist;    
304     double      rotation;
305     Hist_ref   *scene_hist_ref;
306     Hist_ref   *match_hist_ref;
307     Match_ref  *match_ref;
308     List       *cut_match_list;
309     List       *entry_list = NULL;
310     Pairs_HT_entry *entry;
311     int         orientation;
312     Pgh_serr *serr;
313     Transform2  trans;
314     
315 /* define the landmark for location */
316     rotation_mat = rot2(0.0);
317     trans.R = rotation_mat;
318     trans.t = current_model->landmark;
319 
320 /* Get the data from the scene and the match list */
321 
322   for(scene_ptr=scene_pairs_list;scene_ptr!=NULL;scene_ptr=scene_ptr->next)
323     {
324         scene_hist = scene_ptr->to;
325         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
326 /* selected scene line */
327         scene_line = scene_hist_ref->line_seg;
328 
329         cut_match_list = copy_and_cut_matches_list(scene_hist_ref, match_cut_def);
330 
331         for(match_ptr=cut_match_list;match_ptr!=NULL;
332                                       match_ptr=match_ptr->next)
333         {
334             match_ref = match_ptr->to;
335             match_hist = match_ref->hist;
336             match_hist_ref = prop_get(match_hist->props, HIST_REF_TYPE);
337             if (match_hist_ref->type == DIRECTED)
338                orientation = match_ref->orientation;
339             else
340                orientation = 0;
341 
342 /* hypothesised matching model line */
343             match_line = line2_copy(match_hist_ref->line_seg);
344             line2_transform(match_line, trans);
345 
346             if (match_hist_ref->model==current_model)
347             {
348                 /* Create a list of HT entry data */
349 
350                 model_perp_vec = vec2_projperp(match_line->p, match_line->v);
351                 rotation = vec2_angle(scene_line->v, match_line->v);
352                 rotation_mat = rot2(rotation);
353                 scene_perp_vec = mat2_vprod(rotation_mat, model_perp_vec);
354 
355                 if (orientation == 0 || orientation == -1 )
356                 {
357                     entry = ralloc(sizeof(Pairs_HT_entry));
358                     entry->scene_line = scene_line;
359                     entry->perp_vec = scene_perp_vec;
360                     entry->scale = match_ref->scale_factor;
361 
362                     if ((serr = prop_get(match_hist->props, PGH_SERR_TYPE))==NULL)
363                     {
364                         entry->smin = match_ref->scale_factor;
365                         entry->smax = match_ref->scale_factor;
366                     }
367                     else
368                     {
369                         entry->smin = serr->smin;
370                         entry->smax = serr->smax;
371                     }
372 
373                     entry_list = ref_addtostart(entry_list, entry, 
374                                                           PAIRS_HT_ENTRY);
375                         }
376 
377                 if (orientation == 0 || orientation == 1 )
378                 {
379                     entry = ralloc(sizeof(Pairs_HT_entry));
380 
381                     entry->scene_line = scene_line;
382                     entry->perp_vec = vec2_minus(scene_perp_vec);
383                     entry->scale = match_ref->scale_factor;
384 
385                     if ((serr = prop_get(match_hist->props, PGH_SERR_TYPE))==NULL)
386                     {
387                         entry->smin = match_ref->scale_factor;
388                         entry->smax = match_ref->scale_factor;
389                     }
390                     else
391                     {
392                         entry->smin = serr->smin;
393                         entry->smax = serr->smax;
394                     }
395 
396                     entry_list = ref_addtostart(entry_list, entry, PAIRS_HT_ENTRY);
397                 } 
398             } /*end if match_hist_ref*/
399             line2_free(match_line);
400         } /*end for match_ptr*/
401         
402         list_rm(cut_match_list, rfree);
403 
404      } /*end for scene_ptr*/
405 
406 
407     /* Call the plotting procedure then on return delete the parallel lines list */
408 
409         hough2_fill(location_hough_space, 0.0);
410     hough2_plot_points(entry_list, location_hough_space);
411     list_rm(entry_list, rfree);
412 }
413 
414 /* ---------- */
415 
416 void angle_from_loc(Imrect *hough1, Point2 *loc, Model_poly_header *current_model)
417 {
418     Vec2       scene_ref_vec;
419     List      *scene_ptr, *match_ptr;
420     Line2     *scene_line, *match_line;
421     double     angle1,angle2,sigma;
422     Imrect    *scene_hist, *match_hist;
423     Hist_ref  *scene_hist_ref, *match_hist_ref;
424     Match_ref *match_ref;
425     List      *cut_match_list;
426     List          *scene_pairs_list;
427     Match_cut_def *match_cut_def;
428     Pairs_hist_def *hist_def;
429 
430         /* (1) For each scene line which has matched to
431                the current model...
432            (2) Plot a point in hough space for the angle 
433                between the scene reference vector and the 
434                matched model reference vector. */
435     scene_pairs_list = pairs_scene_pairs_list_get();
436     match_cut_def = pairs_match_cut_def_get();
437     hist_def = pairs_hist_def_get();
438 
439     for(scene_ptr=scene_pairs_list; scene_ptr!=NULL;
440                                        scene_ptr=scene_ptr->next)
441     {
442         scene_hist = scene_ptr->to;
443         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
444         scene_line = scene_hist_ref->line_seg;
445 
446         cut_match_list = copy_and_cut_matches_list(scene_hist_ref, 
447                                                             match_cut_def);
448 
449         for(match_ptr=cut_match_list;match_ptr!=NULL;
450                                               match_ptr=match_ptr->next)
451         {
452             match_ref = match_ptr->to;
453             match_hist = match_ref->hist;
454             match_hist_ref = prop_get(match_hist->props, HIST_REF_TYPE);
455             match_line = match_hist_ref->line_seg;
456 
457             if ( (match_hist_ref->model==current_model) &&
458                  (scene_line->length>hist_def->min_length) )
459             {
460                 sigma = (float)(sqrt(pow(atan((double)(2.0
461                                /(scene_line->length))),2.0)
462                                + pow(atan((double)(2.0
463                                /(match_line->length))),2.0)));
464 
465                 scene_ref_vec = vec2_sum(scene_line->p, vec2_minus(loc->p));
466 /* nat nodified 3/3/06 */
467                 angle1 = vec2_angle(scene_ref_vec, vec2_sum(match_line->p,current_model->landmark));
468                 angle2 = vec2_angle(scene_line->v, match_line->v);
469                 if (fabs(angle1-angle2)<3.0*sigma)
470                 {
471                     hough1_plot_log_gauss(hough1, angle2-2.0*PI, sigma);
472                     hough1_plot_log_gauss(hough1, angle2, sigma);
473                     hough1_plot_log_gauss(hough1, angle2+2.0*PI, sigma);
474                 }
475                 else if (fabs(angle1-angle2+PI)<3.0*sigma)
476                 {
477                     hough1_plot_log_gauss(hough1, angle2-PI, sigma);
478                     hough1_plot_log_gauss(hough1, angle2+PI, sigma);
479                     hough1_plot_log_gauss(hough1, angle2+3.0*PI, sigma);
480                 }
481                 else if (fabs(angle1-angle2-PI)<3.0*sigma)
482                 {
483                     hough1_plot_log_gauss(hough1, angle2-3.0*PI, sigma);
484                     hough1_plot_log_gauss(hough1, angle2-PI, sigma);
485                     hough1_plot_log_gauss(hough1, angle2+PI, sigma);
486                 }
487             }
488         }
489 
490       /* Free the generated match list */
491 
492       list_rm(cut_match_list, rfree);
493     }
494 
495 }
496 
497 /* ---------- */
498 
499 double scale_from_loc(Point2 *loc, Model_poly_header *model)
500 {
501     Imrect *scale_hough;
502     Pairs_scale_def *scale_def;
503     List *scene_pairs_list, *scene_ptr;
504     Imrect *scene_hist;
505     Hist_ref *scene_hist_ref;
506     Line2 *scene_line;
507     List *cut_match_list, *match_ptr;
508     Match_cut_def *match_cut_def;
509     Match_ref *match_ref;
510     Imrect *match_hist;
511     Hist_ref *match_hist_ref;
512     Line2 *match_line;
513     double scene_dist, model_dist, scale=0;
514     double model_perp_dist, scene_perp_dist, dist_error;
515     Hough1_peak peak;
516     Mat2 *C;
517     Mat2 R, Rt, C_rot;
518     double scene_line_angle;
519     double scale_error;
520 
521     scale_def = pairs_scale_def_get();
522     scene_pairs_list = pairs_scene_pairs_list_get();
523     match_cut_def = pairs_match_cut_def_get();
524 
525     if ( (scale_def->scale_max==1.0) &&
526          (scale_def->scale_min==1.0) )
527         return 1.0;
528 
529     scale_hough = hough1_alloc(scale_def->scale_min, scale_def->scale_max,
530                                scale_def->HTbinsize, float_v);
531 
532     C = prop_get(loc->props, COVAR2);
533 
534 /* Make scale entries for matches which may have been responsible for this
535    peak in the location HT */
536 
537     for(scene_ptr=scene_pairs_list;scene_ptr!=NULL;scene_ptr=scene_ptr->next)
538     {
539         scene_hist = scene_ptr->to;
540         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
541         scene_line = scene_hist_ref->line_seg;
542 
543         scene_line_angle = acos(vec2_dot(scene_line->v, vec2_ex()));
544         R = rot2(-scene_line_angle);
545         Rt = mat2_transpose(R);
546         C_rot = mat2_prod(mat2_prod(Rt, *C), R);
547         scale_error = sqrt(mat2_yy(C_rot));
548 
549         cut_match_list = copy_and_cut_matches_list(scene_hist_ref, match_cut_def);
550 
551         for(match_ptr=cut_match_list;match_ptr!=NULL; match_ptr=match_ptr->next)
552         {
553             match_ref = match_ptr->to;
554             match_hist = match_ref->hist;
555             match_hist_ref = hist_ref_get(match_hist);
556             match_line = match_hist_ref->line_seg;
557 
558             if (match_hist_ref->model==model)
559             {
560 
561                 /* Could this match have contributed to the peak */
562                 model_perp_dist = 
563                         vec2_mod(vec2_projperp(match_line->p, match_line->v));
564                 scene_perp_dist = vec2_dist_point_line(loc->p, scene_line->p, 
565                                                                scene_line->v);
566                 dist_error = scene_perp_dist-model_perp_dist*
567                                                 match_ref->scale_factor;
568 
569                 if ((dist_error>-scale_def->HTd_error) &&
570                     (dist_error<scale_def->HTd_error))
571                 {
572 
573                     scene_dist = vec2_dist(scene_line->p, loc->p);
574                     model_dist = vec2_mod(match_line->p);
575 
576                     switch(scale_def->HTmethod)
577                     {
578                         case SCALE_HT_REF:
579                             scale = match_ref->scale_factor;
580                             break;
581                         case SCALE_HT_PERPDIST:
582                             scale = scene_perp_dist/model_perp_dist;
583                             break;
584                         case SCALE_HT_DIST:
585                             scale = scene_dist/model_dist;
586                     }
587 
588                     hough1_plot_log_gauss(scale_hough, scale, scale_error*0.2/*scale_def->HTsigma*/);
589                 }
590             }
591         }
592     }
593 
594     /* Output the scale data to the stdio */
595     /*printf("Scale Space Plot\n----------------\n");
596     hough1_output(scale_hough);*/
597     
598     /*stack_push(scale_hough, IMRECT, im_free);*/
599 
600     peak = hough1_locate_max_peak(scale_hough, 0.0, float_v);
601 
602     return peak.x;
603 }
604 
605 /* ---------- */
606 
607 /* ---------- Code from hough2_points.c APA 4/2/94 ---------- */
608 
609 
610 void hough2_plot_points(List *entry_list, Imrect *hough2)
611 {
612     double          min_angle;
613     Line2          *line1, *line2;
614     List           *ptr1;
615     List           *ptr2;
616     double          weight, angle;
617     Bool            t1, t2;
618     Pairs_hough_def *hough_def;
619 
620     Pairs_HT_entry *entry1, *entry2;
621     Mat2 R;
622     Transform2 T;
623     Vec2 OS0, OS1, S0S1;
624     double smin, smax;
625     double smin1, smax1;
626     double smin2, smax2;
627     Vec2 p0, p1, p;
628 
629     double a, b, c, d, det;    /* APA 31.1.96 */
630     Mat2 C;
631     double theta, l1, l2, eig_lim;
632     double sigma2;
633 
634     double sdx, sdy, s_angle;
635     Mat2 SCov, U, U_trans, SCov_rot;
636     Mat2 TotCov;
637 
638 /* Get the values which define the extended parallel lines from the list then find the */
639 /* intersection between a pair.  Plot a point in the hough space to correspond to the */
640                     /* intersection after smoothing using a gaussian */
641 
642     hough_def = pairs_hough_def_get();
643     min_angle = hough_def->min_angle;
644     sigma2 = hough_def->gauss_sigma*hough_def->gauss_sigma;
645     eig_lim = hough_def->eig_lim;
646 
647     R = rot2(0.0);
648     T.R = R;
649 
650     for(ptr1=entry_list;ptr1!=NULL;ptr1=ptr1->next)
651     {
652         entry1 = ptr1->to;
653 
654 
655         for(ptr2=ptr1->next;ptr2!=NULL;ptr2=ptr2->next)
656         {
657             entry2 = ptr2->to;
658 
659             /* Determine the start of the line of equal scale 
660                (where the scene lines intersect) */
661 
662             OS0 = get_intersection(entry1->scene_line, entry2->scene_line, &t1, &t2);
663 
664             /* Determine the point at scale=1 */
665 
666             line1 = line2_copy(entry1->scene_line);
667             T.t = entry1->perp_vec;
668             line2_transform(line1, T);
669 
670             line2 = line2_copy(entry2->scene_line);
671             T.t = entry2->perp_vec;
672             line2_transform(line2, T);
673 
674             OS1 = get_intersection(line1, line2, &t1, &t2);
675 
676             S0S1 = vec2_sum(OS1, vec2_minus(OS0));
677 
678             /* Now: p(s) = OS0 + s x S0S1 */
679 
680             smin1 = entry1->smin;
681             smax1 = entry1->smax;
682             smin2 = entry2->smin;
683             smax2 = entry2->smax;
684 
685             if (smin1<smin2)
686                 smin = smin2;
687             else
688                 smin = smin1;
689 
690             if (smax1>smax2)
691                 smax = smax2;
692             else
693                 smax = smax1;
694 
695             p0 = vec2_sum(OS0, vec2_times(smin, S0S1));
696             p1 = vec2_sum(OS0, vec2_times(smax, S0S1));
697 
698             /* Construct a covariance matrix to represent the scale 
699                error if dealing with variable scale data. This is 
700                an approximation to the correct error */
701 
702             sdx = vec2_mod(vec2_diff(p1, p0))*2.0;
703             sdy = sdx/1000.0;
704 
705             if (smin!=smax)
706                 s_angle = vec2_angle(vec2_ex(), vec2_diff(p0, p1));
707             else
708                 s_angle = 0.0;            
709 
710             SCov = mat2(sdx*sdx, 0.0, 0.0, sdy*sdy);
711             U = rot2(s_angle);
712             U_trans = mat2_transpose(U);
713             SCov_rot = mat2_prod(mat2_prod(U_trans, SCov), U);
714 
715             p = vec2_midpoint(p0, p1);
716 
717             angle = fabs(vec2_angle(line2->v , line1->v));
718             if(angle > min_angle)
719             {
720                 weight = entry1->scene_line->length + 
721                                      entry2->scene_line->length;
722 
723 
724                 /* Remove short lines */
725                 if( (entry1->scene_line->length >= 4.0) && 
726                     (entry2->scene_line->length >= 4.0) )
727                 {
728                     get_covariance(line1, line2, &a, &b, &c, &d);
729                     det = a*d - c*b;
730                     conic_eigen(d/det, -c/det, a/det, &theta, &l1, &l2);
731 
732                     /* Don't bother with v.large ellipses */
733                     if ( (l1 <= eig_lim*eig_lim) &&
734                          (l2 <= eig_lim*eig_lim) )
735                     {
736                         C = mat2(d/det, -c/det, -b/det, a/det); 
737 
738                         /* Scale the covariance matrix according to
739                            line endpoint error*/
740                         mat2_xx(C) *= (float)sigma2;
741                         mat2_xy(C) *= (float)sigma2;
742                         mat2_yx(C) *= (float)sigma2;
743                         mat2_yy(C) *= (float)sigma2;
744 
745                         TotCov = mat2_sum(C, SCov_rot);
746 
747                         hough2_plot_ellipse(hough2, &p, &TotCov, weight);
748                     }
749                 }
750             }
751         }
752     }
753 }
754 
755 
756 /* ---------- */
757 
758 
759 

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