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

Linux Cross Reference
Tina4/src/pgh/pgh_locate.c

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

  1 #include <stdio.h>
  2 #include <sys/param.h>
  3 #include <string.h>
  4 #include <tina/sys.h>
  5 #include <tina/sysfuncs.h>
  6 #include <tina/math.h>
  7 #include <tina/mathfuncs.h>
  8 #include <tina/vision.h>
  9 #include <tina/visionfuncs.h>
 10 #include <tina/file.h>
 11 #include <tina/filefuncs.h>
 12 #include <tina/file_gen.h>
 13 #include <tina/draw.h>
 14 #include <tina/drawfuncs.h>
 15 #include <tina/tv.h>
 16 #include <tina/tvfuncs.h>
 17 #include <tina/toolsfuncs.h>
 18 #include <tina/tw_Xfuncs.h>
 19 #include <pgh/pgh_defs.h>
 20 #include <pgh/pgh_types.h>
 21 #include <pgh/pgh_funcs.h>
 22 
 23 #include <pgh/extra.h>
 24 
 25 double pgh_scale_estimate;
 26 
 27 /* ---------- */
 28 
 29 Point2 *line_to_model(void)
 30 {
 31     Vec2             translation; 
 32     Mat2             rotation_matrix;
 33     List            *ptr;
 34     List            *geom=NULL;
 35     List            *match_list;
 36     Line2           *lptr;
 37     Imrect          *im, *im_pairs_alloc(), *match_hist;
 38     double           rotation;
 39     Hist_ref        *scene_hist_ref, *match_hist_ref; 
 40     Transform2       model_to_scene_trans;
 41     Match_ref       *match_ref;
 42     List            *model_pairs_list;
 43     List            *matched_models_list;
 44     Line2           *matched_line;
 45     List            *picked_geom;
 46     Pairs_hist_def  *hist_def;
 47     Pairs_scale_def *scale_def;
 48 
 49 
 50        /* (1) Generate a pairwise histogram for the currently 
 51               selected line. (Or the first line if a number 
 52               of lines have been selected).
 53           (2) Find the best match between this histogram and
 54               all of the model histograms.
 55           (3) Report this match to the main Tina window.
 56           (4) Overlay the model corresponding to the best match
 57               onto the scene. Allow the user to transform 
 58               between the 4 possible locations. */
 59 
 60     model_pairs_list = pairs_model_pairs_list_get();
 61     matched_models_list = pairs_matched_models_list_get();
 62     matched_line = pairs_matched_line_get();
 63     picked_geom = pairs_picked_geom_get();
 64     hist_def = pairs_hist_def_get();
 65     scale_def = pairs_scale_def_get();
 66 
 67     init_pairs_entry(hist_def->pairs_type,
 68                      hist_def->dbin_max,
 69                      hist_def->dbin_size,
 70                      hist_def->num_abin,
 71                      hist_def->angle_sigma,
 72                      hist_def->dist_ramp);
 73 
 74     geom = mono_geom_get();
 75     if (geom==NULL||picked_geom==NULL||model_pairs_list==NULL) return(NULL); 
 76 
 77     lptr = picked_geom->to;
 78    
 79 /* Build the pairwise histogram */
 80 
 81     im = build_normalized_pairwise(lptr, NULL, geom, hist_def->dbin_max);
 82 
 83 /* Now search the model database for the best match */
 84 
 85     match_scene_pairs_to_model_list(im, model_pairs_list, scale_def);
 86 
 87 /* Fetch the best (first) match from the histogram matches list */
 88 
 89     scene_hist_ref = hist_ref_get(im);
 90     match_list = scene_hist_ref->matches;
 91     if (match_list==NULL) return(NULL);
 92         match_ref = match_list->to;
 93     match_hist = match_ref->hist;
 94     match_hist_ref = hist_ref_get(match_hist);
 95 
 96 /* Report the best match */
 97 
 98         format("Scale:%f Score:%f\n", match_ref->scale_factor, match_ref->dp);
 99 
100 /* If requested output the scores for matches to this line at different scales */
101 
102 /* Now overlay the model over the scene */
103 /* set the p-field of the scene line to its mid-point */
104 
105     lptr->p = vec2_midpoint(lptr->p1, lptr->p2);
106 
107 /* First transform the model line to fit the scene line */
108 
109 /* Initially determine the rotation required to map the model line to the 
110    same orientation as the scene line */
111 
112     rotation = vec2_angle( lptr->v, match_hist_ref->line_seg->v);
113 
114 /* Construct the rotation matrix with the appropiate scale factor */
115 
116     rotation_matrix = rot2_with_scale(rotation, match_ref->scale_factor);
117 
118 /* Now determine the translation required to map the midpoint of the model 
119    line onto the midpoint of the scene line. This mapping has 2 components:
120 
121    (1) Mapping the centre point of the model line back to its position before
122        the rotation.
123    (2) Mapping the centre point of the models line from its original position
124        to the midpoint of the scene line. */
125 
126     translation = vec2_diff(match_hist_ref->line_seg->p,
127                    mat2_vprod(rotation_matrix, match_hist_ref->line_seg->p));
128 
129     translation = vec2_sum(translation, 
130                     vec2_diff(lptr->p, match_hist_ref->line_seg->p));
131 
132 
133     model_to_scene_trans.R = rotation_matrix;
134     model_to_scene_trans.t = translation;
135 
136 /* Now copy the model to the matched model data */
137 
138     list_rm(matched_models_list, geom_free);
139     matched_models_list=NULL;
140     
141     for(ptr=match_hist_ref->model->model_poly_data;ptr!=NULL;ptr=ptr->next)
142     {
143         lptr = line2_copy(ptr->to);
144         matched_models_list = 
145            list_addtoend(matched_models_list, link_alloc(lptr, (int)NULL/*type*/));
146         if (ptr->to==match_hist_ref->line_seg)
147             matched_line = lptr;
148     }   
149 
150     pairs_matched_models_list_set(matched_models_list); 
151     pairs_matched_line_set(matched_line);
152 
153 /* Now transform the copied model data */
154 
155     for(ptr=matched_models_list;ptr!=NULL;ptr=ptr->next)
156         line2_transform(ptr->to, model_to_scene_trans);      
157 
158     if(match_ref->orientation == -1)
159                 rotate_matched_model();
160         else
161                 draw_matched_models();
162 
163     return(point2_make(translation, double_v));
164 }
165 
166 /* ---------- */
167 
168 List * segment_model(char *model_name)
169 {
170     List          *ptr, *match_list, *cut_match_list;
171     List          *segmented_lines_list = NULL;
172     Imrect        *scene_hist, *match_hist;
173     Hist_ref      *scene_hist_ref, *match_hist_ref; 
174     int            found_match;   
175     Match_ref     *match_ref;
176     List          *scene_pairs_list;
177     Match_cut_def *match_cut_def;
178 
179         /* (1) Descend the scene pairwise list adding each
180                scene line to the segmented_lines_list if it
181                has a match corresponding to the specified model.
182            (2) Display the segmented_lines_list. */
183 
184     scene_pairs_list = pairs_scene_pairs_list_get();
185     match_cut_def = pairs_match_cut_def_get();
186 
187     if (scene_pairs_list==NULL) 
188         return(NULL);
189 
190     strip_spaces(model_name); /* prepare the model name for comparison later */
191 
192     for(ptr=scene_pairs_list;ptr!=NULL;ptr=ptr->next)
193     {
194         scene_hist = ptr->to;
195         scene_hist_ref = prop_get(scene_hist->props, HIST_REF_TYPE);
196         found_match=FALSE; 
197 
198         if (scene_hist_ref!=NULL)
199         {
200           /* Make a cut copy of the matches list */
201   
202           cut_match_list = copy_and_cut_matches_list(scene_hist_ref,  
203                                                               match_cut_def); 
204 
205           for(match_list=cut_match_list;match_list!=NULL;
206                                                 match_list=match_list->next)
207             {
208                 match_ref = match_list->to;
209                 match_hist = match_ref->hist;
210                 if (match_hist!=NULL)
211                 {
212                     match_hist_ref = prop_get(match_hist->props,
213                                          HIST_REF_TYPE);
214                     if (match_hist_ref!=NULL &&
215                         strcmp(model_name, match_hist_ref->model->name)==0)
216                         found_match=TRUE;
217                 }
218             }
219 
220         if (found_match)
221             segmented_lines_list = ref_addtostart(segmented_lines_list, 
222                                            scene_hist_ref->line_seg, LINE2);
223 
224         /* Free the generated match list */
225 
226         list_rm(cut_match_list, rfree);
227         }
228 
229     }/*endfor(ptr)*/
230     return(segmented_lines_list);
231 }
232 
233 /* ---------- */
234 /* This function identifies any known models in the mono scene - the hough 
235    transforms constructed for 'model_name' are placed onto the image stack */
236 
237 Point2 *locate_models(char *model_name, List **new_peak_list)
238 {
239     List              *model_ptr;
240     List              *peak_list, *peak_ptr;
241     double             thres;
242     Imrect            *location_hough_space;
243     Imrect            *orient_hough;
244     Model_poly_header *polyh;
245     int                peak_count;
246     List              *scene_pairs_list;
247     List              *matched_models_list;
248     List              *model_poly_list;
249     Pairs_hough_def   *hough_def;
250     Match_cut_def     *match_cut_def;
251     int                max_x,max_y;
252     Hough1_peak        orient_peak;
253     List              *geom;
254     Vec2               origin;
255     Point2            *peak_pos;
256     double             peak_func();
257     char               mess[64];
258     double scale_factor;
259     Mat2 *C;
260     double theta, lam1, lam2, major, minor;
261     double scale_from_loc();
262     int    locate_using_long_lines();
263     int    locate_using_short_lines();
264     int    locate_using_points();
265     void   angle_from_loc();
266 
267     vec2_x(origin)=(float)0.0;
268     vec2_y(origin)=(float)0.0;
269 
270     
271         /* (1) For the chosen model in the database, construct a Hough space, 
272                and vote for the position using the matched scene histograms.
273            (2) Search for significant peaks which
274                hypothesize the presence of a model in the scene.ref
275            (3) For the chosen peak peak determine the orientation of the model 
276                at that location by voting for the angle between each
277                scene line and the model line it has matched to.
278         */
279 
280     scene_pairs_list = pairs_scene_pairs_list_get();
281     matched_models_list = pairs_matched_models_list_get();
282     model_poly_list = pairs_model_poly_list_get();
283     hough_def = pairs_hough_def_get();
284     match_cut_def = pairs_match_cut_def_get();
285 
286     if (scene_pairs_list==NULL) return(NULL);
287 
288 /* Free the matched_models list */
289 
290     if(matched_models_list!=NULL) list_rm(matched_models_list, geom_free);
291     matched_models_list=NULL;
292     pairs_matched_models_list_set(matched_models_list);
293 
294     for(model_ptr=model_poly_list;model_ptr!=NULL;model_ptr=model_ptr->next)
295     {
296         polyh = model_ptr->to;
297         if (strcmp(model_name, polyh->name)==0) break;
298     }
299     if (model_ptr==NULL) return(NULL);
300 
301 
302     location_hough_space = hough2_alloc(hough_def->loc_hough2_min_x, 
303                                         hough_def->loc_hough2_min_y,
304                                         hough_def->loc_hough2_max_x, 
305                                         hough_def->loc_hough2_max_y,
306                                         hough_def->loc_hough2_binsize_x, 
307                                         hough_def->loc_hough2_binsize_y, 
308                                         double_v);
309 
310         /*TEMP*/
311         hough2_fill(location_hough_space, 0.0);
312 
313     if (hough_def->plot_type==LONG_LINES) 
314     {
315         locate_using_long_lines(polyh, location_hough_space,
316                                scene_pairs_list, match_cut_def);
317     }
318 
319     if (hough_def->plot_type==SHORT_LINES) 
320     {
321         locate_using_short_lines(polyh, location_hough_space,
322                                  scene_pairs_list, match_cut_def);
323     }
324          
325     if (hough_def->plot_type==POINTS)
326     {
327         locate_using_points(polyh,
328                      location_hough_space, scene_pairs_list, match_cut_def);
329     }
330 
331 /* Sqrt the hough transform to approximate a chi_2 */
332 
333     sqrt_hough2(location_hough_space);
334 
335 /* Search the loc hough2 for peaks */
336 
337     thres = imf_max(location_hough_space, &max_y, &max_x)*
338                      (hough_def->peak_percent/100.0);
339     peak_list = hough2_locate_peaks(location_hough_space, thres);
340 
341 #ifndef _PCC
342     stack_push(location_hough_space, IMRECT, im_free);
343 #endif
344 
345 /* Determine model orientation at hough2 peak_no */
346 
347     peak_list = sort_list(peak_list, peak_func, NULL);
348 
349     for(peak_ptr=peak_list,peak_count=0;peak_ptr!=NULL;peak_count++,peak_ptr=peak_ptr->next)
350     {
351         if(peak_count==hough_def->peak_no) break;
352     }
353 
354     if (peak_ptr==NULL) return(NULL);
355 
356     sprintf(mess,"hough peak = %4.2f / %4.2f \n",-peak_func(peak_ptr->to),thres);
357     format(mess);
358 
359 
360     /* Display the error of this peak */
361     C = prop_get(((Point2*)peak_ptr->to)->props, COVAR2);
362     conic_eigen(mat2_xx(*C), mat2_xy(*C), mat2_yy(*C),
363                 &theta, &lam1, &lam2);
364     theta = 180.0*theta/PI;
365     /* Assumes binsizex = binsizey */
366     major = sqrt(lam1)*hough_def->loc_hough2_binsize_x;
367     minor = sqrt(lam2)*hough_def->loc_hough2_binsize_x;
368     format("major: %f\nminor: = %f\nangle = %f\n\n", major, minor, theta);
369 
370     /* Determine the scale of the model responsible for this peak */
371 
372         scale_factor = scale_from_loc(peak_ptr->to, model_ptr->to);
373         format("Scale:%f\n", scale_factor);
374 
375     pgh_scale_estimate = scale_factor; /* Stored for global access */
376 
377     orient_hough = hough1_alloc(-PI-(PI/hough_def->orient_binsize),
378                                  PI+(PI/hough_def->orient_binsize),
379                                 (PI/hough_def->orient_binsize), float_v);
380 
381     angle_from_loc(orient_hough, peak_ptr->to, model_ptr->to);
382     thres = imf_max(orient_hough, &max_y, &max_x)*
383                      (hough_def->peak_percent/100.0);
384     orient_peak = hough1_locate_max_peak(orient_hough, thres ,float_v);
385                
386 /* Make a copy of the model data */
387 
388     polyh=model_ptr->to;
389     geom = poly_copy(polyh->model_poly_data);
390 
391 /* Transform the copied data to the determined angle, position and scale*/
392 
393     poly_rotate_and_scale(geom, orient_peak.x, scale_factor, origin);
394     peak_pos = peak_ptr->to;
395     poly_translate(geom, peak_pos->p );
396 
397 /* Add the transformed copy to the matched_models list */
398 
399     matched_models_list = list_append(matched_models_list, geom);
400 
401 #ifndef _PCC
402     stack_push(orient_hough, IMRECT, im_free);
403 #endif
404 
405     pairs_matched_models_list_set(matched_models_list);
406 
407     if(new_peak_list != NULL) *new_peak_list = peak_list;
408     return(peak_pos);
409 }
410 
411 /* ---------- */
412 
413 

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