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

Linux Cross Reference
Tina4/src/vision/calib/grid_match.c

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

  1 /**@(#)
  2 **/
  3 #include <stdio.h>
  4 #include <math.h>
  5 #ifdef TRANSPUTER
  6 #include <valuesdual.h>
  7 #else
  8 #include <values.h>
  9 #endif
 10 #include <tina/sys.h>
 11 #include <tina/sysfuncs.h>
 12 #include <tina/math.h>
 13 #include <tina/mathfuncs.h>
 14 #include <tina/vision.h>
 15 #include <tina/visionfuncs.h>
 16 
 17 /* FORWARD REFS  */
 18 static void label_grid_lines(List * lines, List ** inter_lines, int num, double grid_thres, double vert_thres, int *label);
 19 
 20 static float invariant[9] =
 21 {(float)0.416, (float)0.400, (float)0.375, (float)0.333, (float)0.250,
 22 (float)0.750, (float)0.666, (float)0.625, (float)0.600};
 23 
 24 void    grid_match(List * world, List * horiz_lines, List * vert_lines, double grid_thres, double vert_thres, void (*store_vertex) ( /* ??? */ ))
 25 {
 26     List   *co_lines;
 27     List   *co_list;
 28     List  **horiz_grid;
 29     List  **vert_grid;
 30     Line2  *line1;
 31     Line2  *line2;
 32     Vec2   *vec2_inter_par_test();
 33     double  min_interx(List * ptr), min_intery(List * ptr);
 34     int    *horiz_label, *vert_label;
 35     int     horiz_num, vert_num;
 36     int     i, j, k, r, c, label;
 37     int     max_rec, max_num, repeat, num_rec;
 38 
 39     for (co_list = horiz_lines, horiz_num = 0; co_list != NULL; co_list = co_list->next)
 40         horiz_num++;
 41     for (co_list = vert_lines, vert_num = 0; co_list != NULL; co_list = co_list->next)
 42         vert_num++;
 43     if (horiz_num < 4 || vert_num < 4)
 44     {
 45         format("grid matcher failed:  insufficient co_linear lines\n");
 46         return;
 47     }
 48     horiz_grid = (List **) ralloc((unsigned) horiz_num * sizeof(List *));
 49     vert_grid = (List **) ralloc((unsigned) vert_num * sizeof(List *));
 50     horiz_label = (int *) ralloc((unsigned) horiz_num * sizeof(int));
 51     vert_label = (int *) ralloc((unsigned) vert_num * sizeof(int));
 52 
 53     for (i = 0; i < horiz_num; i++)
 54         horiz_grid[i] = NULL;
 55     for (i = 0; i < vert_num; i++)
 56         vert_grid[i] = NULL;
 57 
 58     for (co_list = horiz_lines, i = 0, j = 0; co_list != NULL; co_list = co_list->next)
 59     {
 60         co_lines = co_list->to;
 61         horiz_label[i] = -1;
 62         horiz_grid[i++] = co_lines;
 63     }
 64 
 65     for (co_list = vert_lines, i = 0, j = 0; co_list != NULL; co_list = co_list->next)
 66     {
 67         co_lines = co_list->to;
 68         vert_label[i] = -1;
 69         vert_grid[i++] = co_lines;
 70     }
 71 
 72     compute_horiz_invariants(world);
 73     for (i = 0, max_rec = 0, max_num = -1; i < horiz_num; i++)
 74     {
 75         label_grid_lines(horiz_grid[i], vert_grid, vert_num, grid_thres, vert_thres, vert_label);
 76         for (j = 0, repeat = 1, num_rec = 0; j < vert_num; j++)
 77         {
 78             for (k = 0; k < vert_num; k++)
 79                 if (j != k && vert_label[j] >= 0 && vert_label[j] == vert_label[k])
 80                     repeat = 0;
 81             if (vert_label[j] >= 0)
 82                 num_rec++;
 83         }
 84         if (num_rec > max_rec && repeat)
 85         {
 86             max_rec = num_rec;
 87             max_num = i;
 88         }
 89         if (num_rec == 8 && repeat)
 90             break;
 91     }
 92 
 93     if (max_num >= 0 && i != max_num)
 94         label_grid_lines(horiz_grid[max_num], vert_grid, vert_num, grid_thres, vert_thres, vert_label);
 95     if (max_num < 0)
 96     {
 97         format("grid matcher failed to label vertical lines\n");
 98         rfree((void *) horiz_label);
 99         rfree((void *) horiz_grid);
100         rfree((void *) vert_label);
101         rfree((void *) vert_grid);
102         return;
103     }
104     compute_vert_invariants(world);
105     for (i = 0, max_rec = 0, max_num = -1; i < vert_num; i++)
106     {
107         label_grid_lines(vert_grid[i], horiz_grid, horiz_num, grid_thres, vert_thres, horiz_label);
108         for (j = 0, repeat = 1, num_rec = 0; j < horiz_num; j++)
109         {
110             for (k = 0; k < horiz_num; k++)
111                 if (j != k && horiz_label[j] >= 0 && horiz_label[j] == horiz_label[k])
112                     repeat = 0;
113             if (horiz_label[j] >= 0)
114                 num_rec++;
115         }
116         if (num_rec > max_rec && repeat)
117         {
118             max_rec = num_rec;
119             max_num = i;
120         }
121         if (num_rec == 8 && repeat)
122             break;;
123     }
124     if (max_num >= 0 && i != max_num)
125         label_grid_lines(vert_grid[max_num], horiz_grid, horiz_num, grid_thres, vert_thres, horiz_label);
126     if (max_num < 0)
127     {
128         format("grid matcher failed to label horizontal lines\n");
129         rfree((void *) horiz_label);
130         rfree((void *) horiz_grid);
131         rfree((void *) vert_label);
132         rfree((void *) vert_grid);
133         return;
134     }
135     for (i = 0, max_rec = 0, max_num = -1; i < horiz_num; i++)
136     {
137         if ((r = horiz_label[i]) >= 0)
138         {
139             for (j = 0; j < vert_num; j++)
140             {
141                 if ((c = vert_label[j]) >= 0)
142                 {
143                     get_inter_lines(horiz_grid[i], vert_grid[j], vert_thres, &line1, &line2);
144                     if (line1 != NULL && line2 != NULL)
145                     {
146                         label = 8 * (7 - r) + c;
147                         store_vertex(world, line1, line2, label);
148                     }
149                 }
150             }
151         }
152     }
153     rfree((void *) horiz_label);
154     rfree((void *) horiz_grid);
155     rfree((void *) vert_label);
156     rfree((void *) vert_grid);
157 }
158 
159 static void label_grid_lines(List * lines, List ** inter_lines, int num, double grid_thres, double vert_thres, int *label)
160 {
161     int     j, k, n, min_label;
162     double  da, db, dc = 0.0, dd = 0.0;
163     Vec2   *inter[64];
164     Line2  *line1;
165     Line2  *line2;
166 
167     for (j = 0; j < num; j++)
168         label[j] = -1;
169     for (j = 0; j < 64; j++)
170         inter[j] = NULL;
171     for (j = 0, n = 0; j < num; j++)
172     {
173         get_inter_lines(lines, inter_lines[j], vert_thres, &line1, &line2);
174         if (line1 != NULL && line2 != NULL && n < 62)
175         {
176             if ((inter[n] = vec2_inter_par_test(line1->p, line1->v, line2->p, line2->v, 0.97)))
177             {
178                 if (n >= 3)
179                 {
180                     da = vec2_mod(vec2_diff(*(inter[n - 3]), *(inter[n - 2])));
181                     db = vec2_mod(vec2_diff(*(inter[n - 1]), *(inter[n])));
182                     dc = vec2_mod(vec2_diff(*(inter[n - 2]), *(inter[n])));
183                     dd = vec2_mod(vec2_diff(*(inter[n - 3]), *(inter[n - 1])));
184                     if (fabs(da * db - invariant[4] * dc * dd) < grid_thres * (da + dc))
185                         break;
186                 }
187                 n++;
188             }
189         }
190     }
191     if ((fabs(da * db - invariant[4] * dc * dd) >= grid_thres * (da + dc))
192         || n < 3)
193     {
194         for (j = 0; j < 64; j++)
195             if (inter[j] != NULL)
196                 vec2_free((void *) inter[j]);
197         return;
198     }
199     for (k = 0; k < num; k++)
200     {
201         get_inter_lines(lines, inter_lines[k], vert_thres, &line1, &line2);
202         if (line1 != NULL && line2 != NULL)
203         {
204             if ((inter[n + 1] = vec2_inter_par_test(line1->p, line1->v, line2->p, line2->v, 0.97)))
205             {
206                 if (vec2_mod(vec2_diff(*(inter[n - 3]), *(inter[n + 1]))) < 0.001)
207                     label[k] = 6;
208                 else if (vec2_mod(vec2_diff(*(inter[n - 2]), *(inter[n + 1]))) < 0.001)
209                     label[k] = 7;
210                 else if (vec2_mod(vec2_diff(*(inter[n - 1]), *(inter[n + 1]))) < 0.001)
211                     label[k] = 8;
212                 else if (vec2_mod(vec2_diff(*(inter[n]), *(inter[n + 1]))) < 0.001)
213                     label[k] = 9;
214                 else
215                 {
216                     da = vec2_mod(vec2_diff(*(inter[n + 1]), *(inter[n - 2])));
217                     db = vec2_mod(vec2_diff(*(inter[n - 1]), *(inter[n])));
218                     dc = vec2_mod(vec2_diff(*(inter[n - 2]), *(inter[n])));
219                     dd = vec2_mod(vec2_diff(*(inter[n + 1]), *(inter[n - 1])));
220                     if (fabs(da * db - invariant[0] * dc * dd) < grid_thres * (da + db))
221                         label[k] = 2;
222                     else if (fabs(da * db - invariant[1] * dc * dd) < grid_thres * (da + db))
223                         label[k] = 3;
224                     else if (fabs(da * db - invariant[2] * dc * dd) < grid_thres * (da + db))
225                         label[k] = 4;
226                     else if (fabs(da * db - invariant[3] * dc * dd) < grid_thres * (da + db))
227                         label[k] = 5;
228                     else if (fabs(da * db - invariant[5] * dc * dd) < grid_thres * (da + db))
229                         label[k] = 10;
230                     else if (fabs(da * db - invariant[6] * dc * dd) < grid_thres * (da + db))
231                         label[k] = 11;
232                     else if (fabs(da * db - invariant[7] * dc * dd) < grid_thres * (da + db))
233                         label[k] = 12;
234                     else if (fabs(da * db - invariant[8] * dc * dd) < grid_thres * (da + db))
235                         label[k] = 13;
236                 }
237             }
238         }
239     }
240     for (k = 0, min_label = 14; k < num; k++)
241     {
242         if (label[k] > 0 && label[k] < min_label)
243             min_label = label[k];
244     }
245     for (k = 0; k < num; k++)
246         if (label[k] > 0)
247             label[k] -= min_label;
248 
249     for (j = 0; j < 64; j++)
250         if (inter[j] != NULL)
251             vec2_free((void *) inter[j]);
252 }
253 
254 void    get_inter_lines(List * horiz, List * vert, double thres, Line2 ** hline, Line2 ** vline)
255 {
256     List   *hptr;
257     List   *vptr;
258 
259     for (hptr = horiz; hptr != NULL; hptr = hptr->next)
260     {
261         for (vptr = vert; vptr != NULL; vptr = vptr->next)
262         {
263             *hline = hptr->to;
264             *vline = vptr->to;
265             if ((*hline)->length > 10.0 && (*vline)->length > 10.0)
266                 if (vec2_mod(vec2_diff((*hline)->p1, (*vline)->p1)) < thres
267                     || vec2_mod(vec2_diff((*hline)->p1, (*vline)->p2)) < thres
268                     || vec2_mod(vec2_diff((*hline)->p2, (*vline)->p1)) < thres
269                     || vec2_mod(vec2_diff((*hline)->p2, (*vline)->p2)) < thres)
270                 {
271                     return;
272                 }
273         }
274     }
275     *hline = NULL;
276     *vline = NULL;
277 }
278 
279 double  min_interx(List * ptr)
280 {
281     Line2  *line;
282     Vec2    xaxis = {Vec2_id};
283     Vec2    origin = {Vec2_id};
284     Vec2   *vec2_inter_par_test();
285     Vec2   *inters;
286     double  inter = MAXDOUBLE;
287 
288     xaxis.el[0] = (float)1.0;
289     xaxis.el[1] = (float)0.0;
290     origin.el[0] = (float)0.0;
291     origin.el[1] = (float)0.0;
292 
293     line = ptr->to;
294     inters = vec2_inter_par_test(line->p, line->v, origin, xaxis, 0.97);
295     if (inters != NULL)
296     {
297         inter = inters->el[0];
298         rfree((void *) inters);
299     }
300     return (inter);
301 }
302 
303 double  min_intery(List * ptr)
304 {
305     Line2  *line;
306     Vec2    yaxis = {Vec2_id};
307     Vec2    origin = {Vec2_id};
308     Vec2   *inters;
309     Vec2   *vec2_inter_par_test();
310     double  inter = MAXDOUBLE;
311 
312     yaxis.el[0] = (float)0.0;
313     yaxis.el[1] = (float)1.0;
314     origin.el[0] = (float)0.0;
315     origin.el[1] = (float)0.0;
316 
317     line = ptr->to;
318     inters = vec2_inter_par_test(line->p, line->v, origin, yaxis, 0.97);
319     if (inters != NULL)
320     {
321         inter = inters->el[1];
322         rfree((void *) inters);
323     }
324     return (inter);
325 }
326 
327 void    compute_vert_invariants(List * world)
328 {
329     List   *ptr;
330     int     i;
331     Match  *match_to_3d;
332     Vec3   *vertex[8];
333     float   da, db, dc, dd;
334 
335     for (i = 0; i < 64; i += 8)
336     {
337         for (ptr = world; ptr != NULL; ptr = ptr->next)
338         {
339             match_to_3d = ptr->to;
340             if (match_to_3d->label == i)
341             {
342                 vertex[i / 8] = match_to_3d->to1;
343                 break;
344             }
345         }
346     }
347     for (i = 4; i < 8; i++)
348     {
349         da = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[1])));
350         db = (float)vec3_mod(vec3_diff(*(vertex[2]), *(vertex[3])));
351         dc = (float)vec3_mod(vec3_diff(*(vertex[1]), *(vertex[3])));
352         dd = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[2])));
353         invariant[i + 1] = da * db / (dc * dd);
354     }
355     for (i = 0; i < 5; i++)
356     {
357         da = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[5])));
358         db = (float)vec3_mod(vec3_diff(*(vertex[6]), *(vertex[7])));
359         dc = (float)vec3_mod(vec3_diff(*(vertex[5]), *(vertex[7])));
360         dd = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[6])));
361         invariant[i] = da * db / (dc * dd);
362     }
363 }
364 
365 void    compute_horiz_invariants(List * world)
366 {
367     List   *ptr;
368     int     i;
369     Match  *match_to_3d;
370     Vec3   *vertex[8];
371     float   da, db, dc, dd;
372 
373     for (i = 0; i < 8; i++)
374     {
375         for (ptr = world; ptr != NULL; ptr = ptr->next)
376         {
377             match_to_3d = ptr->to;
378             if (match_to_3d->label == i)
379             {
380                 vertex[i] = match_to_3d->to1;
381                 break;
382             }
383         }
384     }
385     for (i = 4; i < 8; i++)
386     {
387         da = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[1])));
388         db = (float)vec3_mod(vec3_diff(*(vertex[2]), *(vertex[3])));
389         dc = (float)vec3_mod(vec3_diff(*(vertex[1]), *(vertex[3])));
390         dd = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[2])));
391         invariant[i + 1] = da * db / (dc * dd);
392     }
393     for (i = 0; i < 5; i++)
394     {
395         da = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[5])));
396         db = (float)vec3_mod(vec3_diff(*(vertex[6]), *(vertex[7])));
397         dc = (float)vec3_mod(vec3_diff(*(vertex[5]), *(vertex[7])));
398         dd = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[6])));
399         invariant[i] = da * db / (dc * dd);
400     }
401 }
402 

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