1 /**********
2 *
3 * This file is part of the TINA Open Source Image Analysis Environment
4 * henceforth known as TINA
5 *
6 * TINA is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU Lesser General Public License as
8 * published by the Free Software Foundation.
9 *
10 * TINA is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU Lesser General Public License for more details.
14 *
15 * You should have received a copy of the GNU Lesser General Public License
16 * along with TINA; if not, write to the Free Software Foundation, Inc.,
17 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 *
19 **********
20 *
21 * Program : TINA
22 * File : $Source: /home/tina/cvs/tina-libs/tina/vision/visCalib_grid.c,v $
23 * Date : $Date: 2005/01/09 17:49:25 $
24 * Version : $Revision: 1.4 $
25 * CVS Id : $Id: visCalib_grid.c,v 1.4 2005/01/09 17:49:25 paul Exp $
26 *
27 * Author : Legacy TINA
28 *
29 * Notes :
30 *
31 *********
32 */
33
34 #include "visCalib_grid.h"
35
36 #if HAVE_CONFIG_H
37 #include <config.h>
38 #endif
39
40 #include <stdio.h>
41 #include <math.h>
42 #include <float.h>
43
44 #include <tina/sys/sysDef.h>
45 #include <tina/sys/sysPro.h>
46 #include <tina/math/mathDef.h>
47 #include <tina/math/mathPro.h>
48 #include <tina/geometry/geomDef.h>
49 #include <tina/geometry/geomPro.h>
50
51 /* FORWARD REFS */
52 static void label_grid_lines(List * lines, List ** inter_lines, int num, double grid_thres, double vert_thres, int *label);
53
54 static float invariant[9] =
55 {(float)0.416, (float)0.400, (float)0.375, (float)0.333, (float)0.250,
56 (float)0.750, (float)0.666, (float)0.625, (float)0.600};
57
58 void grid_match(List * world, List * horiz_lines, List * vert_lines, double grid_thres, double vert_thres, void (*store_vertex) ( /* ??? */
59 ))
60 {
61 List *co_lines;
62 List *co_list;
63 List **horiz_grid;
64 List **vert_grid;
65 Line2 *line1;
66 Line2 *line2;
67 Vec2 *vec2_inter_par_test();
68 double min_interx(List * ptr), min_intery(List * ptr);
69 int *horiz_label, *vert_label;
70 int horiz_num, vert_num;
71 int i, j, k, r, c, label;
72 int max_rec, max_num, repeat, num_rec;
73
74 for (co_list = horiz_lines, horiz_num = 0; co_list != NULL; co_list = co_list->next)
75 horiz_num++;
76 for (co_list = vert_lines, vert_num = 0; co_list != NULL; co_list = co_list->next)
77 vert_num++;
78 if (horiz_num < 4 || vert_num < 4)
79 {
80 format("grid matcher failed: insufficient co_linear lines\n");
81 return;
82 }
83 horiz_grid = (List **) ralloc((unsigned) horiz_num * sizeof(List *));
84 vert_grid = (List **) ralloc((unsigned) vert_num * sizeof(List *));
85 horiz_label = (int *) ralloc((unsigned) horiz_num * sizeof(int));
86 vert_label = (int *) ralloc((unsigned) vert_num * sizeof(int));
87
88 for (i = 0; i < horiz_num; i++)
89 horiz_grid[i] = NULL;
90 for (i = 0; i < vert_num; i++)
91 vert_grid[i] = NULL;
92
93 for (co_list = horiz_lines, i = 0, j = 0; co_list != NULL; co_list = co_list->next)
94 {
95 co_lines = co_list->to;
96 horiz_label[i] = -1;
97 horiz_grid[i++] = co_lines;
98 }
99
100 for (co_list = vert_lines, i = 0, j = 0; co_list != NULL; co_list = co_list->next)
101 {
102 co_lines = co_list->to;
103 vert_label[i] = -1;
104 vert_grid[i++] = co_lines;
105 }
106
107 compute_horiz_invariants(world);
108 for (i = 0, max_rec = 0, max_num = -1; i < horiz_num; i++)
109 {
110 label_grid_lines(horiz_grid[i], vert_grid, vert_num, grid_thres, vert_thres, vert_label);
111 for (j = 0, repeat = 1, num_rec = 0; j < vert_num; j++)
112 {
113 for (k = 0; k < vert_num; k++)
114 if (j != k && vert_label[j] >= 0 && vert_label[j] == vert_label[k])
115 repeat = 0;
116 if (vert_label[j] >= 0)
117 num_rec++;
118 }
119 if (num_rec > max_rec && repeat)
120 {
121 max_rec = num_rec;
122 max_num = i;
123 }
124 if (num_rec == 8 && repeat)
125 break;
126 }
127
128 if (max_num >= 0 && i != max_num)
129 label_grid_lines(horiz_grid[max_num], vert_grid, vert_num, grid_thres, vert_thres, vert_label);
130 if (max_num < 0)
131 {
132 format("grid matcher failed to label vertical lines\n");
133 rfree((void *) horiz_label);
134 rfree((void *) horiz_grid);
135 rfree((void *) vert_label);
136 rfree((void *) vert_grid);
137 return;
138 }
139 compute_vert_invariants(world);
140 for (i = 0, max_rec = 0, max_num = -1; i < vert_num; i++)
141 {
142 label_grid_lines(vert_grid[i], horiz_grid, horiz_num, grid_thres, vert_thres, horiz_label);
143 for (j = 0, repeat = 1, num_rec = 0; j < horiz_num; j++)
144 {
145 for (k = 0; k < horiz_num; k++)
146 if (j != k && horiz_label[j] >= 0 && horiz_label[j] == horiz_label[k])
147 repeat = 0;
148 if (horiz_label[j] >= 0)
149 num_rec++;
150 }
151 if (num_rec > max_rec && repeat)
152 {
153 max_rec = num_rec;
154 max_num = i;
155 }
156 if (num_rec == 8 && repeat)
157 break;;
158 }
159 if (max_num >= 0 && i != max_num)
160 label_grid_lines(vert_grid[max_num], horiz_grid, horiz_num, grid_thres, vert_thres, horiz_label);
161 if (max_num < 0)
162 {
163 format("grid matcher failed to label horizontal lines\n");
164 rfree((void *) horiz_label);
165 rfree((void *) horiz_grid);
166 rfree((void *) vert_label);
167 rfree((void *) vert_grid);
168 return;
169 }
170 for (i = 0, max_rec = 0, max_num = -1; i < horiz_num; i++)
171 {
172 if ((r = horiz_label[i]) >= 0)
173 {
174 for (j = 0; j < vert_num; j++)
175 {
176 if ((c = vert_label[j]) >= 0)
177 {
178 get_inter_lines(horiz_grid[i], vert_grid[j], vert_thres, &line1, &line2);
179 if (line1 != NULL && line2 != NULL)
180 {
181 label = 8 * (7 - r) + c;
182 store_vertex(world, line1, line2, label);
183 }
184 }
185 }
186 }
187 }
188 rfree((void *) horiz_label);
189 rfree((void *) horiz_grid);
190 rfree((void *) vert_label);
191 rfree((void *) vert_grid);
192 }
193
194 static void label_grid_lines(List * lines, List ** inter_lines, int num, double grid_thres, double vert_thres, int *label)
195 {
196 int j, k, n, min_label;
197 double da=0.0, db=0.0, dc = 0.0, dd = 0.0;
198 Vec2 *inter[64];
199 Line2 *line1;
200 Line2 *line2;
201
202 for (j = 0; j < num; j++)
203 label[j] = -1;
204 for (j = 0; j < 64; j++)
205 inter[j] = NULL;
206 for (j = 0, n = 0; j < num; j++)
207 {
208 get_inter_lines(lines, inter_lines[j], vert_thres, &line1, &line2);
209 if (line1 != NULL && line2 != NULL && n < 62)
210 {
211 if ((inter[n] = vec2_inter_par_test(line1->p, line1->v, line2->p, line2->v, 0.97)))
212 {
213 if (n >= 3)
214 {
215 da = vec2_mod(vec2_diff(*(inter[n - 3]), *(inter[n - 2])));
216 db = vec2_mod(vec2_diff(*(inter[n - 1]), *(inter[n])));
217 dc = vec2_mod(vec2_diff(*(inter[n - 2]), *(inter[n])));
218 dd = vec2_mod(vec2_diff(*(inter[n - 3]), *(inter[n - 1])));
219 if (fabs(da * db - invariant[4] * dc * dd) < grid_thres * (da + dc))
220 break;
221 }
222 n++;
223 }
224 }
225 }
226 if ((fabs(da * db - invariant[4] * dc * dd) >= grid_thres * (da + dc))
227 || n < 3)
228 {
229 for (j = 0; j < 64; j++)
230 if (inter[j] != NULL)
231 vec2_free((void *) inter[j]);
232 return;
233 }
234 for (k = 0; k < num; k++)
235 {
236 get_inter_lines(lines, inter_lines[k], vert_thres, &line1, &line2);
237 if (line1 != NULL && line2 != NULL)
238 {
239 if ((inter[n + 1] = vec2_inter_par_test(line1->p, line1->v, line2->p, line2->v, 0.97)))
240 {
241 if (vec2_mod(vec2_diff(*(inter[n - 3]), *(inter[n + 1]))) < 0.001)
242 label[k] = 6;
243 else if (vec2_mod(vec2_diff(*(inter[n - 2]), *(inter[n + 1]))) < 0.001)
244 label[k] = 7;
245 else if (vec2_mod(vec2_diff(*(inter[n - 1]), *(inter[n + 1]))) < 0.001)
246 label[k] = 8;
247 else if (vec2_mod(vec2_diff(*(inter[n]), *(inter[n + 1]))) < 0.001)
248 label[k] = 9;
249 else
250 {
251 da = vec2_mod(vec2_diff(*(inter[n + 1]), *(inter[n - 2])));
252 db = vec2_mod(vec2_diff(*(inter[n - 1]), *(inter[n])));
253 dc = vec2_mod(vec2_diff(*(inter[n - 2]), *(inter[n])));
254 dd = vec2_mod(vec2_diff(*(inter[n + 1]), *(inter[n - 1])));
255 if (fabs(da * db - invariant[0] * dc * dd) < grid_thres * (da + db))
256 label[k] = 2;
257 else if (fabs(da * db - invariant[1] * dc * dd) < grid_thres * (da + db))
258 label[k] = 3;
259 else if (fabs(da * db - invariant[2] * dc * dd) < grid_thres * (da + db))
260 label[k] = 4;
261 else if (fabs(da * db - invariant[3] * dc * dd) < grid_thres * (da + db))
262 label[k] = 5;
263 else if (fabs(da * db - invariant[5] * dc * dd) < grid_thres * (da + db))
264 label[k] = 10;
265 else if (fabs(da * db - invariant[6] * dc * dd) < grid_thres * (da + db))
266 label[k] = 11;
267 else if (fabs(da * db - invariant[7] * dc * dd) < grid_thres * (da + db))
268 label[k] = 12;
269 else if (fabs(da * db - invariant[8] * dc * dd) < grid_thres * (da + db))
270 label[k] = 13;
271 }
272 }
273 }
274 }
275 for (k = 0, min_label = 14; k < num; k++)
276 {
277 if (label[k] > 0 && label[k] < min_label)
278 min_label = label[k];
279 }
280 for (k = 0; k < num; k++)
281 if (label[k] > 0)
282 label[k] -= min_label;
283
284 for (j = 0; j < 64; j++)
285 if (inter[j] != NULL)
286 vec2_free((void *) inter[j]);
287 }
288
289 void get_inter_lines(List * horiz, List * vert, double thres, Line2 ** hline, Line2 ** vline)
290 {
291 List *hptr;
292 List *vptr;
293
294 for (hptr = horiz; hptr != NULL; hptr = hptr->next)
295 {
296 for (vptr = vert; vptr != NULL; vptr = vptr->next)
297 {
298 *hline = hptr->to;
299 *vline = vptr->to;
300 if ((*hline)->length > 10.0 && (*vline)->length > 10.0)
301 if (vec2_mod(vec2_diff((*hline)->p1, (*vline)->p1)) < thres
302 || vec2_mod(vec2_diff((*hline)->p1, (*vline)->p2)) < thres
303 || vec2_mod(vec2_diff((*hline)->p2, (*vline)->p1)) < thres
304 || vec2_mod(vec2_diff((*hline)->p2, (*vline)->p2)) < thres)
305 {
306 return;
307 }
308 }
309 }
310 *hline = NULL;
311 *vline = NULL;
312 }
313
314 double min_interx(List * ptr)
315 {
316 Line2 *line;
317 Vec2 xaxis = {Vec2_id};
318 Vec2 origin = {Vec2_id};
319 Vec2 *vec2_inter_par_test();
320 Vec2 *inters;
321 double inter = DBL_MAX;
322
323 xaxis.el[0] = (float)1.0;
324 xaxis.el[1] = (float)0.0;
325 origin.el[0] = (float)0.0;
326 origin.el[1] = (float)0.0;
327
328 line = ptr->to;
329 inters = vec2_inter_par_test(line->p, line->v, origin, xaxis, 0.97);
330 if (inters != NULL)
331 {
332 inter = inters->el[0];
333 rfree((void *) inters);
334 }
335 return (inter);
336 }
337
338 double min_intery(List * ptr)
339 {
340 Line2 *line;
341 Vec2 yaxis = {Vec2_id};
342 Vec2 origin = {Vec2_id};
343 Vec2 *inters;
344 Vec2 *vec2_inter_par_test();
345 double inter = DBL_MAX;
346
347 yaxis.el[0] = (float)0.0;
348 yaxis.el[1] = (float)1.0;
349 origin.el[0] = (float)0.0;
350 origin.el[1] = (float)0.0;
351
352 line = ptr->to;
353 inters = vec2_inter_par_test(line->p, line->v, origin, yaxis, 0.97);
354 if (inters != NULL)
355 {
356 inter = inters->el[1];
357 rfree((void *) inters);
358 }
359 return (inter);
360 }
361
362 void compute_vert_invariants(List * world)
363 {
364 List *ptr;
365 int i;
366 Match *match_to_3d;
367 Vec3 *vertex[8];
368 float da, db, dc, dd;
369
370 for (i = 0; i < 64; i += 8)
371 {
372 for (ptr = world; ptr != NULL; ptr = ptr->next)
373 {
374 match_to_3d = ptr->to;
375 if (match_to_3d->label == i)
376 {
377 vertex[i / 8] = match_to_3d->to1;
378 break;
379 }
380 }
381 }
382 for (i = 4; i < 8; i++)
383 {
384 da = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[1])));
385 db = (float)vec3_mod(vec3_diff(*(vertex[2]), *(vertex[3])));
386 dc = (float)vec3_mod(vec3_diff(*(vertex[1]), *(vertex[3])));
387 dd = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[2])));
388 invariant[i + 1] = da * db / (dc * dd);
389 }
390 for (i = 0; i < 5; i++)
391 {
392 da = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[5])));
393 db = (float)vec3_mod(vec3_diff(*(vertex[6]), *(vertex[7])));
394 dc = (float)vec3_mod(vec3_diff(*(vertex[5]), *(vertex[7])));
395 dd = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[6])));
396 invariant[i] = da * db / (dc * dd);
397 }
398 }
399
400 void compute_horiz_invariants(List * world)
401 {
402 List *ptr;
403 int i;
404 Match *match_to_3d;
405 Vec3 *vertex[8];
406 float da, db, dc, dd;
407
408 for (i = 0; i < 8; i++)
409 {
410 for (ptr = world; ptr != NULL; ptr = ptr->next)
411 {
412 match_to_3d = ptr->to;
413 if (match_to_3d->label == i)
414 {
415 vertex[i] = match_to_3d->to1;
416 break;
417 }
418 }
419 }
420 for (i = 4; i < 8; i++)
421 {
422 da = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[1])));
423 db = (float)vec3_mod(vec3_diff(*(vertex[2]), *(vertex[3])));
424 dc = (float)vec3_mod(vec3_diff(*(vertex[1]), *(vertex[3])));
425 dd = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[2])));
426 invariant[i + 1] = da * db / (dc * dd);
427 }
428 for (i = 0; i < 5; i++)
429 {
430 da = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[5])));
431 db = (float)vec3_mod(vec3_diff(*(vertex[6]), *(vertex[7])));
432 dc = (float)vec3_mod(vec3_diff(*(vertex[5]), *(vertex[7])));
433 dd = (float)vec3_mod(vec3_diff(*(vertex[i]), *(vertex[6])));
434 invariant[i] = da * db / (dc * dd);
435 }
436 }
437
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.