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

Linux Cross Reference
Tina6/tina-tools/tinatool/tlvision/tlvisSmm_matcher.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/tlvisSmm_matcher.c,v $
 37  * Date    :  $Date: 2009/02/03 15:37:46 $
 38  * Version :  $Revision: 1.5 $
 39  * CVS Id  :  $Id: tlvisSmm_matcher.c,v 1.5 2009/02/03 15:37:46 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 
 53 #include <tina/sys/sysDef.h>
 54 #include <tina/sys/sysPro.h>
 55 #include <tina/math/mathDef.h>
 56 #include <tina/math/mathPro.h>
 57 #include <tina/geometry/geomPro.h>
 58 #include <tina/file/filePro.h>
 59 #include <tina/vision/visDef.h>
 60 #include <tina/vision/visPro.h>
 61 #include <tina/sys/sys_LstDef.h>
 62 #include <tina/sys/sysMem_ralloc.h>
 63 #include <tina/sys/sysLst_list.h>
 64 
 65 #include <tinatool/draw/drawDef.h>
 66 #include <tinatool/draw/drawPro.h>
 67 
 68 #include "tlvisSmm_matcher.h"
 69 
 70 #define RM_ENTRY 749865
 71 
 72 
 73 static List    *model;
 74 static Matrix  *model_tbl;
 75 static List    *scene;
 76 static List    *scene_ml;
 77 static Matrix  *scene_tbl;
 78 static List    *matches=NULL;
 79 static Transform3 trans = {Transform3_id};
 80 
 81 static List    *mlists = NULL;  /* pointer to geometry for current
 82                                  * match-clique */
 83 
 84 static List    *focus = NULL;
 85 static List    *group = NULL;
 86 static List    *cliches = NULL;
 87 static float    pos_error = 6.0;
 88 static float    rot_error = 0.1;
 89 static int      clique_size = 5, transform_error = 0;
 90 
 91 
 92 static Tv      *tv_model;
 93 static Tv      *tv_scene;
 94 
 95 static int  match_num = 1;
 96 static int mlist_size = 30;
 97 
 98 int matches_found = 0;
 99 
100 Vec3 vp;
101 
102 Tv             *model_tv(void)
103 {
104         return (tv_model);
105 }
106 
107 Tv             *model_tv_get(void)
108 {
109         return (tv_model);
110 }
111 
112 void            model_tv_set(Tv * tv)
113 {
114         tv_model = tv;
115 }
116 
117 Tv             *scene_tv(void)
118 {
119         return (tv_scene);
120 }
121 
122 Tv             *scene_tv_get(void)
123 {
124         return (tv_scene);
125 }
126 
127 void            scene_tv_set(Tv * tv)
128 {
129         tv_scene = tv;
130 }
131 
132 void            matcher_set_pos_error(double error)
133 {
134         pos_error = error;
135 }
136 
137 double          matcher_get_pos_error(void)
138 {
139         return (pos_error);
140 }
141 
142 void            matcher_set_rot_error(double error)
143 {
144         rot_error = error;
145 }
146 
147 double          matcher_get_rot_error(void)
148 {
149         return (rot_error);
150 }
151 
152 void            matcher_set_csize(int csize)
153 {
154         clique_size = csize;
155 }
156 
157 int             matcher_get_csize(void)
158 {
159         return (clique_size);
160 }
161 
162 void            focus_set(List * f)
163 {
164         focus = f;
165 }
166 
167 List           *focus_get(void)
168 {
169         return (focus);
170 }
171 
172 void            group_set(List * g)
173 {
174         list_rm_links(group);   /* eventually does the same as list_rm with a
175                                  * NULL freefunc NAT 29/9/95 */
176         group = g;
177 }
178 
179 List           *group_get(void)
180 {
181         return (group);
182 }
183 
184 List           *scene_get(void)
185 {
186         return (scene);
187 }
188 
189 List           *scene_ml_get(void)
190 {
191         return (scene_ml);
192 }
193 
194 void            scene_set(List * newscene)
195 {
196         list_rm(scene, geom_free);
197         scene = newscene;
198         list_rm(scene_ml, geom_free);
199         scene_ml = NULL;
200         list_rm(matches, match_free);
201         matches = NULL;
202         iso_set_defaults(pos_error, rot_error); /* for hand picking */
203         list_apply_func(scene, geom_set_iso_default_error, NULL);
204 }
205 
206 void            scene_reset(List * newscene)
207 {
208         scene = newscene;
209         list_rm(scene_ml, geom_free);
210         scene_ml = NULL;
211         list_rm(matches, match_free);
212         matches = NULL;
213         iso_set_defaults(pos_error, rot_error); /* for hand picking */
214         list_apply_func(scene, geom_set_iso_default_error, NULL);
215 }
216 
217 List           *model_get(void)
218 {
219         return (model);
220 }
221 
222 void            model_set(List * newmodel)
223 {
224         list_rm(model, geom_free);
225         model = newmodel;
226         list_rm(matches, match_free);
227         matches = NULL;
228         list_apply_func(model, geom_set_iso_zero_error, NULL);
229         list_rm(cliches, match_cliche_free);
230         cliches = NULL;
231         list_rm(focus, (void (*) ()) NULL);
232         focus = NULL;
233         list_rm(group, (void (*) ()) NULL);
234         group = NULL;
235 }
236 
237 void            model_reset(List * newmodel)
238 {
239         model = newmodel;
240         list_apply_func(model, geom_set_iso_zero_error, NULL);
241 }
242 
243 static  void        build_scene_ml(void)
244 {
245         void           *smm_filter();
246 
247         list_rm(scene_ml, geom_free);
248         scene_ml = list_copy(scene, (void *(*) ()) smm_filter, NULL);
249         scene_ml = list_append(scene_ml, list_copy(scene_ml, geom_negative,
250                                                    NULL));
251 }
252 
253 void            matcher_build_tables(void)
254 {
255         pwtre_table_free(scene_tbl);
256         pwtre_table_free(model_tbl);
257         list_rm(matches, match_free);
258         matches = NULL;
259         iso_set_defaults(pos_error, rot_error);
260         list_apply_func(scene, geom_set_iso_default_error, NULL);       /* for hand picking */
261         build_scene_ml();
262         list_apply_func(scene_ml, geom_set_iso_default_error, NULL);
263         scene_tbl = pwrte_build_full_table(scene_ml);
264         model_tbl = pwrte_build_full_table(model);
265 }
266 
267 void            matcher_add_cliche(void)
268 {
269         Match_cliche   *cliche;
270         List           *f = list_copy(focus, (void *(*) ()) NULL, NULL);
271         List           *g = list_copy(group, (void *(*) ()) NULL, NULL);
272 
273         cliche = match_cliche_make(f, list_length(f), g, clique_size);
274         cliches = ref_addtostart(cliches, (void *) cliche, MATCH_CLICHE);
275 }
276 
277 void            matcher_set_cliches(List * clist)
278 {
279         list_rm(cliches, match_cliche_free);
280         cliches = clist;
281 }
282 
283 List           *matcher_get_cliches(void)
284 {
285         return (cliches);
286 }
287 
288 Bool            matcher_compute_transform(void)
289 {
290         int             tot, count;
291         Vec3            vec3_zero();
292 
293         tot = list_length(matches);
294         trans = mlist_transform_vec3(matches, &count);
295 
296         if (count < 2 || count < tot / 3 || count < clique_size)
297         {
298                 trans = mlist_best_transform_vec3(matches, &count);
299         }
300         if (count < 2 || count < tot / 3 || count < clique_size)
301         {
302                 format("count %d < 2, and < %f and < clique size \n",count,tot/3.0);
303                 error("poor transformation", warning);   transform_error = 1;
304                 trans.R = mat3_unit();
305                 trans.t = vec3_zero();
306                 return (false);
307         }
308         if (scene_ml == NULL)
309                 build_scene_ml();
310         list_rm(matches, match_free);
311         matches = geom3_all_matches_under_trans(model, scene_ml, trans);
312         format("computed tranform is\n");
313         trans3_format(trans);
314         return (true);
315 }
316 
317 static double   neg_mlist_good(List * mlist)
318 {
319         List           *ptr;
320         double          sum = 0;
321 
322         for (ptr = mlist; ptr != NULL; ptr = ptr->next)
323                 sum += ((Match *) ptr->to)->weight;
324 
325         return (-sum);
326 }
327 
328 int *matcher_get_mlsize( void )
329 {
330     return (&mlist_size);
331 }
332 
333 void    matcher_run_smm( int cliche_size )
334 {
335         List           *ptr;
336         void            match_copy_free();
337 
338 /*
339         init_cliches( cliche_size );
340 */
341 
342         list_rm_links(mlists);
343 
344         list_rm(matches, match_copy_free);
345         matches = NULL;
346 
347         smm_set(model, scene_ml, model_tbl, scene_tbl);
348 
349         mlists = smm_run(cliches);
350 
351         if (mlists == NULL)
352         {
353                 printf("No Matches Found\n");
354                 return;
355         }
356 
357         for (ptr = mlists; ptr != NULL; ptr = ptr->next)
358                 ptr->to = smm_mats_con_with_mlist_2(model, scene_ml, (List *) ptr->to);
359 
360         mlists = sort_list(mlists, neg_mlist_good, NULL);
361         mlists = new_reduce_lists( mlists );
362 
363 
364         if (mlists == NULL)
365         {
366             format("No matches found, try increasing cliche size\n");
367             return;
368         }
369         else
370         {
371             format("Matcher found %d unique transformations \n",list_length(mlists));
372         }
373 
374         update_mm(mlists);
375 }
376 
377 void update_mm( List *ptr )
378 {
379     list_rm(matches, match_copy_free);
380     matches = NULL;
381     matches = list_copy((List *) ptr->to, (void *(*) ()) match_copy, NULL);
382 }
383 
384 void smm_go_to( int count )
385 {
386     List *mlist_ptr =NULL;
387     int i;
388 
389     mlist_ptr = mlists;
390     match_num = 1;
391     for ( i = 1; i < count; i++ )
392     {
393         mlist_ptr = mlist_ptr->next;
394         match_num++;
395     }
396     if (mlist_ptr == NULL)
397               return;
398     update_mm(mlist_ptr);
399 }
400 
401 void            matcher_matches_init(void)
402 {
403         list_rm(matches, match_free);
404         matches = NULL;
405 }
406 
407 List           *matcher_matches_get(void)
408 {
409         return (matches);
410 }
411 
412 void    matcher_add_match(void *p1, void *p2, int type)
413 {
414         matches = ref_addtostart(matches, (void *) match_make(p1, p2, type), MATCH);
415 }
416 
417 Transform3  matcher_get_trans(void)
418 {
419         return (trans);
420 }
421 
422 void  matcher_set_trans(Transform3 newtrans)
423 {
424         trans = newtrans;
425 }
426 
427 List *get_match_mlists()
428 {
429     return(mlists);
430 }
431 
432 double get_dp( Generic *gen )
433 {
434     double dp;
435 
436     dp = *(double *)(prop_get( gen->props, 44444 ));
437 
438     return ( -dp );
439 }
440 
441 double rot_similarity( Mat3 r1, Mat3 r2 )
442 {
443     Mat3 r3, r4;
444     double sum;
445 
446     r3 = mat3_inverse( r2 );
447     r4 = mat3_prod( r1, r3 );
448     sum = ( r4.el[0][0] + r4.el[1][1] + r4.el[2][2] ) / 3.0;
449 
450     return ( sum );
451 }
452 
453 Bool valid_cliche_views( List *ptr )
454 {
455     Vec3     cliche_view, view_obj_vec, cv;
456     int      i,   cliche_num, gcm_l;
457     List    *cliche_used, *dpl = NULL, *ptr2, *cl_list;
458     Generic *gen;
459     Match   *m;
460     Cliche_id *cl_id;
461     double *dp;
462     Transform3  trans;
463 
464     trans = matcher_get_trans();
465 
466     ptr2 = (List *)(ptr->to);
467     m = ptr2->to;
468     cliche_used = prop_get( m->props, 11111 );
469 
470     cliche_num = *(int *)(cliche_used->to);
471 
472     gcm_l = list_length(get_global_cliche_mem_list()) - 1;
473 
474     for ( ptr2 = get_global_cliche_mem_list(), i = 0; ptr2 != NULL;  ptr2 = ptr2->next, i++ )
475     {
476         cl_list = (List *)ptr2->to;
477 
478         cl_id = (Cliche_id *)cl_list->to;
479         cv = vec3( cl_id->vx, cl_id->vy, cl_id->vz );
480 
481         view_obj_vec = vec3_unit(cv);
482         cliche_view = mat3_vprod(trans.R,view_obj_vec);
483 
484         gen =  (Generic *)ralloc( sizeof(Generic));
485 
486         dp =  (double *)ralloc( sizeof(double));
487        *dp = -vec3_z(cliche_view);
488         gen->props = proplist_add( gen->props, dp, 44444, rfree );
489 
490         gen->label = gcm_l - i;
491         dpl = ref_addtostart( dpl, (void *)gen, 0 );
492     }
493 
494     dpl = sort_list( dpl, get_dp, NULL );
495 
496     if ( cliche_num == ((Generic *)(dpl->to))->label ||
497         cliche_num == ((Generic *)(dpl->next->to))->label ||
498         cliche_num == ((Generic *)(dpl->next->next->to))->label )
499     {
500         list_free_refs( dpl, rfree );
501         return ( true );
502     }
503     else
504     {
505         list_free_refs( dpl, rfree );
506         return (false );
507     }
508 }
509 
510 Bool seen_before( List *ptr, List *mlists )
511 {
512     List *ptr2 = NULL, *ptr3 = NULL;
513     Mat3  rot_new, *rot_old;
514     double sim, diff;
515     Match *m;
516     Vec3   trans_new, *trans_old;
517 
518     ptr2 = ptr->to;
519     m = ptr2->to;
520 
521     rot_new = ((Transform3)matcher_get_trans()).R;
522     trans_new = ((Transform3)matcher_get_trans()).t;
523 
524     for ( ptr3 = mlists; ptr3 != NULL; ptr3 = ptr3->next )
525     {
526         if ( (ptr2 = ptr3->to) == NULL ) printf("error.....!\n");
527         if (ptr3 == ptr) continue;
528         m = ptr2->to;
529 
530         rot_old = (Mat3 *)prop_get( m->props, 22222 );
531         if ( rot_old == NULL ) break; 
532         sim = rot_similarity( *rot_old, rot_new );
533 
534         trans_old = (Vec3 *)prop_get( m->props, 33333 );
535         if ( trans_old == NULL ) break; 
536         diff = vec3_sqrdist( *trans_old, trans_new );
537 
538 
539 /* set these params as defines NAT 6/8/08 */
540         if ( ( (sim > 0.98)  && (sim < 1.02) )   &&  ( diff < 10.0 )  ) return ( true ); 
541 
542     }
543     return ( false );
544 }
545 
546 List *new_reduce_lists( List *mlists )
547 {
548     List   *ptr2, *m2 = NULL, *m3;
549     Mat3   *rot;
550     Match  *m;
551     Vec3   *trans;
552 
553     m2 = mlists;
554     while ( m2 != NULL )
555     {
556         update_mm( m2 );
557         transform_proc();
558         if ( transform_error == 1 ) 
559         { 
560             printf("\n\n Warning: Transformation error!\n\n\n"); 
561 
562             m3 = m2->next;
563             mlists = list_rm_el( mlists, m2, NULL ); 
564             m2 = m3;
565             transform_error = 0; 
566         }
567         else
568         {
569             trans_model_proc();
570             if ( seen_before( m2, mlists  ) )
571             {
572                 m3 = m2->next;
573                 mlists = list_rm_el( mlists, m2, NULL ); 
574                 m2 = m3;
575             }
576             else
577             {                                                           
578                 if ( valid_cliche_views( m2 )  ) 
579                 {
580                     ptr2 = m2->to;
581                     m = ptr2->to;
582 
583                     rot = (Mat3 *)ralloc( sizeof(Mat3) ); 
584                    *rot = ((Transform3)matcher_get_trans()).R;
585                     m->props = proplist_addifnp( m->props, rot, 22222, rfree, false ); 
586 
587                     trans = (Vec3 *)ralloc( sizeof(Vec3) ); 
588                    *trans = ((Transform3)matcher_get_trans()).t;
589                     m->props = proplist_addifnp( m->props, trans, 33333, rfree, false ); 
590 
591                     m2 = m2->next;
592                 }
593                 else  
594                 {
595                       m3 = m2->next;
596                       mlists = list_rm_el( mlists, m2, NULL ); 
597                       m2 = m3;
598                 }
599             }
600         }
601     }    
602     return ( dd_get_start( mlists ) );
603 }
604 

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