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
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.