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

Linux Cross Reference
Tina4/src/vision/transform/transform.c

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

  1 /** @(#)Least squares transform functions for matched 3D geometry
  2  * position (pos) and vector (vec) treated seperately
  3  */
  4 
  5 #include <tina/sys.h>
  6 #include <tina/sysfuncs.h>
  7 #include <tina/math.h>
  8 #include <tina/mathfuncs.h>
  9 #include <tina/vision.h>
 10 #include <tina/visionfuncs.h>
 11 
 12 /* Compute approx (rot first) least square transformation on the basis
 13  * of vector data obtained from appropriate geometrical primitives
 14  * transform from match->t1 to match->t2 for each member of the list */
 15 Transform3 mlist_comp_transform_vec3(List * match_list, int *terror)
 16 {
 17     Transform3 trans = {Transform3_id};
 18 
 19     trans.type = 0;
 20     trans.R = mlist_comp_rot_all(match_list, terror);   /* uses all */
 21     if (*terror)
 22         trans.t = vec3_zero();
 23     else
 24         trans.t = mlist_comp_translate_all(match_list, trans.R);
 25 
 26     if (*terror)
 27         error("there is a transform error", warning);
 28 
 29     return (trans);
 30 }
 31 
 32 /* Compute approx (rot first) least square transformation on the basis
 33  * of position data obtained from appropriate geometrical primitives
 34  * centroid can be weighted (on basis of match strength) if required
 35  * transform from match->t1 to match->t2 for each member of the list */
 36 Transform3 mlist_comp_transform_pos3(List * match_list, Bool weighted, int *terror)
 37 
 38 /* if true use a weighted centroid */
 39 
 40 {
 41     Transform3 trans = {Transform3_id};
 42     float   tot_weight = (float)0.0;
 43     List   *lptr;
 44     Vec3    c1 = {Vec3_id};
 45     Vec3    c2 = {Vec3_id};
 46 
 47     c1 = c2 = vec3_zero();
 48     trans.type = 0;
 49 
 50     for (lptr = match_list; lptr != NULL; lptr = lptr->next)
 51     {
 52         Match  *mptr = (Match *) lptr->to;
 53         Point3 *p1;
 54         Point3 *p2;
 55         float   weight;
 56 
 57         if (mptr->type != POINT3)
 58             continue;
 59 
 60         p1 = (Point3 *) mptr->to1;
 61         p2 = (Point3 *) mptr->to2;
 62         weight = (float)((weighted == true) ? mptr->weight : 1.0);
 63 
 64         c1 = vec3_times(weight, vec3_sum(c1, p1->p));
 65         c2 = vec3_sum(c2, p2->p);
 66         tot_weight += weight;
 67     }
 68 
 69     c1 = vec3_times(1 / tot_weight, c1);
 70     c2 = vec3_times(1 / tot_weight, c2);
 71 
 72     trans.R = mlist_comp_rot_pos3(match_list, c1, c2, terror);
 73     trans.t = (terror) ? vec3_zero() : vec3_diff(c2, mat3_vprod(trans.R, c1));
 74 
 75     if (*terror)
 76         error("there is a transform error", warning);
 77 
 78     return (trans);
 79 }
 80 
 81 /* Test if particular match is consitent with transformation trans
 82  * should take geometry in match->t1 to match->t2 */
 83 Bool    transform_match_ok(Match * match, Transform3 trans)
 84 {
 85     Bool    within_error;
 86     void   *geom;
 87 
 88     if (match == NULL)
 89         return (false);
 90 
 91     geom = geom_copy(match->to1, match->type);
 92     geom_transform(geom, match->type, &trans);
 93     within_error = geom_within_error(geom, match->to2, match->type);
 94     geom_free(geom, match->type);
 95     return (within_error);
 96 }
 97 
 98 /* Remove elements of match list (list) that are not consistent with
 99  * transformation (trans) use freematch to determine whether to free up
100  * Match structure or only the List element */
101 List   *transform_prune_mlist(List * list, Transform3 trans, Bool freematch)
102 {
103     List   *pruned = NULL;
104     List   *lptr;
105     List   *next;
106 
107     for (lptr = list; lptr != NULL; lptr = next)
108     {
109         next = lptr->next;
110         if (transform_match_ok((Match *) lptr->to, trans) == false)
111         {
112             if (freematch == true)
113                 (void) link_rm(lptr, rfree);    /* delete this match */
114             else
115                 (void) link_rm_el(lptr);
116             continue;
117         }
118         pruned = link_addtostart((List *) pruned, lptr);
119     }
120     return (list_reverse(pruned));
121 }
122 
123 /* Starting from the pair of matches {m1, m2} compute transformation
124  * recruit additional matches from match list (list) if possible */
125 Transform3 match_pair_transform_vec3(Match * m1, Match * m2, List * list, int *match_count)
126 
127 /* full list of matches */
128 
129 {
130     List   *tlist = NULL;
131     Transform3 trans = {Transform3_id};
132     Transform3 oldtrans = {Transform3_id};
133     int     old_count, count = 0;
134     int     terror;
135 
136     *match_count = 0;
137 
138     trans.R = mat3_unit();
139     trans.t = vec3_zero();
140 
141     if (m1 == NULL || m2 == NULL)
142         return (trans);
143     tlist = ref_addtostart(tlist, (void *) m1, MATCH);
144     tlist = ref_addtostart(tlist, (void *) m2, MATCH);
145 
146     do
147     {
148         old_count = count;
149         oldtrans = trans;
150         trans = mlist_comp_transform_vec3(tlist, &terror);
151         list_rm(tlist, (void (*) ()) NULL);
152         if (!terror)
153         {
154             tlist = transform_prune_mlist(
155             list_copy(list, (void *(*) ()) NULL, NULL), trans, false);
156             count = list_length(tlist);
157         }
158     }
159     while (count >= 2 && count > old_count);
160 
161     list_rm(tlist, (void (*) ()) NULL);
162     *match_count = old_count;
163 
164     return (oldtrans);
165 }
166 
167 /* Find the best (highest cardinality) transformation from the match
168  * list try each pair of matches in turn as the basis of the
169  * transformation */
170 Transform3 mlist_best_transform_vec3(List * list, int *match_count)
171 {
172     Transform3 trans = {Transform3_id};
173     Transform3 best_trans = {Transform3_id};
174     int     count, maxcount = -1;
175     List   *l1;
176     List   *l2;
177 
178     best_trans.R = mat3_unit();
179     best_trans.t = vec3_zero();
180 
181     for (l1 = list; l1 != NULL; l1 = l1->next)
182         for (l2 = l1->next; l2 != NULL; l2 = l2->next)
183         {
184             Match  *m1 = (Match *) l1->to;
185             Match  *m2 = (Match *) l2->to;
186 
187             trans = match_pair_transform_vec3(m1, m2, list, &count);
188             if (count > maxcount)
189             {
190                 maxcount = count;
191                 best_trans = trans;
192             }
193         }
194 
195     *match_count = maxcount;
196     return (best_trans);
197 }
198 
199 /* Compute transromation from match list (list) iteratively prune and
200  * grow transformation until stable */
201 Transform3 mlist_transform_vec3(List * list, int *match_count)
202 {
203     List   *tlist;
204     Transform3 trans = {Transform3_id};
205     Transform3 oldtrans = {Transform3_id};
206     int     old_count, count = 0;
207     int     terror;
208 
209     trans.R = mat3_unit();
210     trans.t = vec3_zero();
211     tlist = list_copy(list, (void *(*) ()) NULL, NULL);
212 
213     do
214     {
215         old_count = count;
216         oldtrans = trans;
217         trans = mlist_comp_transform_vec3(tlist, &terror);
218         list_rm(tlist, (void (*) ()) NULL);
219         if (!terror)
220         {
221             tlist = transform_prune_mlist(list_copy(list,
222                             (void *(*) ()) NULL, NULL), trans, false);
223             count = list_length(tlist);
224         }
225     }
226     while (count >= 2 && count > old_count);
227 
228     list_rm(tlist, (void (*) ()) NULL);
229     *match_count = old_count;
230 
231     return (oldtrans);
232 }
233 

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