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/geometry/geomCurve_affine_cv.c,v $
23 * Date : $Date: 2008/10/30 09:43:59 $
24 * Version : $Revision: 1.3 $
25 * CVS Id : $Id: geomCurve_affine_cv.c,v 1.3 2008/10/30 09:43:59 neil Exp $
26 *
27 * Author : Legacy TINA
28 *
29 * Notes :
30 *
31 * fitting plane curves to strings of matched edge points by recovering
32 * affine transform
33 *
34 *********
35 */
36
37 #include "geomCurve_affine_cv.h"
38
39 #if HAVE_CONFIG_H
40 #include <config.h>
41 #endif
42
43 #include <math.h>
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/geom_CamPro.h>
50 #include <tina/geometry/geom_PlaneDef.h>
51 #include <tina/geometry/geom_PlanePro.h>
52 #include <tina/geometry/geom_EdgePro.h>
53 #include <tina/geometry/geomCurve_es_string3.h>
54
55
56
57 #define HORIZONTAL_THRES 2
58
59 static Vec2 aprox_dir2(List * pos)
60 {
61 Vec3 p1 = {Vec3_id};
62 Vec3 p2 = {Vec3_id};
63 Vec3 v3 = {Vec3_id};
64
65 /* BUGFIX: was Vec3 v2 */
66 Vec2 v2 = {Vec2_id};
67
68 if (pos->next == NULL && pos->last == NULL)
69 return (vec2_zero());
70
71 if (pos->last != NULL)
72 p1 = *(Vec3 *) pos->last->to;
73 else
74 p1 = *(Vec3 *) pos->to;
75
76 if (pos->next != NULL)
77 p2 = *(Vec3 *) pos->next->to;
78 else
79 p2 = *(Vec3 *) pos->to;
80
81 v3 = vec3_diff(p2, p1);
82 v2 = vec2_of_vec3(v3);
83 return (vec2_unit(v2));
84 }
85
86 /* from string of disparity vectors find affine transform between
87 * images */
88 int affine_curve(Tstring * string, double *a, double *b, double *c, double *resid)
89 /* string of Vec3 (xl, y, xr-xl) */
90 /* for affine parameters Xl = a.Xr + b.Yr + c */
91 /* weighted sum sqr distance */
92 {
93 List *start;
94 List *end;
95 List *ptr;
96 Matrix *A; /* each row of form [wXr wYr w] */
97 Vector *B; /* vector of wXl */
98 Vector *C;
99 Vector *X;
100 Vector *W;
101 int i, n;
102
103 if (string == NULL)
104 return (0);
105
106 start = string->start;
107 end = string->end;
108
109 for (n = 1, ptr = start; ptr != end; ptr = ptr->next)
110 ++n;
111
112 A = matrix_alloc(n, 3, matrix_full, double_v);
113 B = vector_alloc(n, double_v);
114 W = vector_alloc(n, double_v);
115
116 /* BUGFIX sumw unused */
117 /* double sumw; sumw = 0; */
118
119 for (i = 0, ptr = start; i < n; ++i, ptr = ptr->next)
120 {
121 Vec2 v2a = {Vec2_id};
122 double w = fabs(vec2_y(v2a)); /* weight by y component */
123 Vec3 p = {Vec3_id};
124
125 v2a = aprox_dir2(ptr);
126 p = *(Vec3 *) ptr->to;
127
128 VECTOR_DOUBLE(W, i) = w;
129 VECTOR_DOUBLE(B, i) = vec3_x(p) * w;
130 A->el.double_v[i][0] = (vec3_x(p) + vec3_z(p)) * w;
131 A->el.double_v[i][1] = vec3_y(p) * w;
132 A->el.double_v[i][2] = w;
133
134 /* BUGFIX sumw unused */
135 /* sumw += w; */
136 }
137
138 X = matrix_cholesky_least_square(A, B); /* solve for affine
139 * parameters */
140 if (X == NULL)
141 {
142 matrix_free(A);
143 vector_free(B);
144 vector_free(W);
145 return (0);
146 }
147 C = matrix_vprod(A, X); /* measure weighted residual */
148 *resid = 0;
149 for (i = 0; i < n; ++i)
150 *resid += sqr(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i));
151 *resid /= n;
152
153 matrix_free(A);
154 vector_free(B);
155 vector_free(C);
156 *a = VECTOR_DOUBLE(X, 0);
157 *b = VECTOR_DOUBLE(X, 1);
158 *c = VECTOR_DOUBLE(X, 2);
159 vector_free(X);
160
161 return (1);
162 }
163
164 int affine_curve_it(Tstring * string, double thres, double *a, double *b, double *c, double *resid, Vec2 * p1, Vec2 * p2, Vec2 * pm)
165 /* string of Vec3 (xl, y, xr-xl) */
166 /* fit threshold */
167 /* for affine parameters Xl = a.Xr + b.Yr + c */
168 /* weighted sum sqr distance */
169
170 {
171 List *start;
172 List *end;
173 List *ptr;
174 Matrix *A; /* each row of form [wXr wYr w] */
175 Vector *B; /* vector of wXl */
176 Vector *C;
177 Vector *X;
178 Vector *W;
179 Vector *index;
180 Vector *pos;
181 int i, j, k, mid, n, ntot, nthres;
182
183 if (string == NULL)
184 return (0);
185
186 start = string->start;
187 end = string->end;
188
189 for (n = 1, ptr = start; ptr != end; ptr = ptr->next)
190 ++n;
191
192 if (n < 6)
193 return (0);
194
195 nthres = MAX(n / 2, 6);
196
197 A = matrix_alloc(n, 3, matrix_full, double_v);
198 B = vector_alloc(n, double_v);
199 W = vector_alloc(n, double_v);
200 index = vector_alloc(n, int_v);
201 pos = vector_alloc(n, ptr_v);
202
203 /* BUGFIX sumw unused */
204 /* double sumw; sumw = 0; */
205
206 for (i = 0, ptr = start; i < n; ++i, ptr = ptr->next)
207 {
208 double w = 1.0; /* fabs(vec2_y(aprox_dir2(ptr)));weight
209 * by y component */
210 Vec3 p = {Vec3_id};
211
212 VECTOR_PTR(pos, i) = ptr->to;
213 p = *(Vec3 *) ptr->to;
214 VECTOR_INT(index, i) = i;
215
216 VECTOR_DOUBLE(W, i) = w;
217 VECTOR_DOUBLE(B, i) = vec3_x(p) * w;
218 A->el.double_v[i][0] = (vec3_x(p) + vec3_z(p)) * w;
219 A->el.double_v[i][1] = vec3_y(p) * w;
220 A->el.double_v[i][2] = w;
221
222 /* BUGFIX sumw unused */
223 /* sumw += w; */
224 }
225
226 ntot = n;
227 do
228 {
229 int maxi = 0;
230 double diff, max_diff = 0;
231 double mean1=0.0, mean2=0.0, mean3=0.0, slope1=0.0, slope2=0.0;
232
233 /* test for horizontal or vertical plane rotation before atempting full 2D fit
234 to remove later numerical instabilities for lines during 3D projection NAT 20/10/08 */
235 X = vector_alloc(3,double_v);
236 for (i = 0; i < n; ++i)
237 {
238 mean1 += VECTOR_DOUBLE(B, i);
239 mean2 += A->el.double_v[i][0];
240 }
241 mean1 /= n;
242 mean2 /= n;
243 for (i = 0; i < n; ++i)
244 {
245 slope1 += (VECTOR_DOUBLE(B, i)-mean1) * (A->el.double_v[i][0]-mean2);
246 slope2 += (A->el.double_v[i][0]-mean2)*(A->el.double_v[i][0]-mean2);
247 }
248 if (slope1/slope2 < 0.2)
249 {
250 for (i = 0; i < n; ++i)
251 {
252 mean3 += A->el.double_v[i][1];
253 }
254 mean3 /= n;
255 slope1 = 0,0;
256 slope2 = 0.0;
257 for (i = 0; i < n; ++i)
258 {
259 slope1 += (VECTOR_DOUBLE(B, i)-mean1 - (A->el.double_v[i][0]-mean2))*(A->el.double_v[i][1]-mean3);
260 slope2 += (A->el.double_v[i][1]-mean3)*(A->el.double_v[i][1]-mean3);
261 }
262 VECTOR_DOUBLE(X, 0) = 1.0;
263 VECTOR_DOUBLE(X, 1) = slope1/slope2;
264 VECTOR_DOUBLE(X, 2) = mean1 - mean2 - mean3*slope1/slope2 ;
265 }
266 else
267 {
268 VECTOR_DOUBLE(X, 0) = slope1/slope2;
269 VECTOR_DOUBLE(X, 1) = 0.0;
270 VECTOR_DOUBLE(X, 2) = mean1 - mean2*slope1/slope2;
271 }
272
273 C = matrix_vprod(A, X);
274 for (i = 0; i < n; ++i)
275 {
276 diff = fabs(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i));
277
278 if (diff > max_diff)
279 {
280 max_diff = diff;
281 maxi = i;
282 }
283 }
284 vector_free(C);
285
286 /* no outlying points*/
287 if (max_diff < thres)
288 break;
289
290 vector_free(X);
291 X = matrix_cholesky_least_square(A, B);
292 if (X == NULL)
293 {
294 A->m = B->n = W->n = ntot;
295 matrix_free(A);
296 vector_free(B);
297 vector_free(W);
298 vector_free(index); /* BUGFIX line added by JB for Takashi */
299 vector_free(pos); /* BUGFIX line added by JB for Takashi */
300 return (0);
301 }
302 /* find outliers */
303 C = matrix_vprod(A, X);
304 maxi=0;
305 max_diff=0;
306 for (i = 0; i < n; ++i)
307 {
308 diff = fabs(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i));
309
310 if (diff > max_diff)
311 {
312 max_diff = diff;
313 maxi = i;
314 }
315 }
316 vector_free(C);
317
318 /* no outlying points and stable plane */
319 if (max_diff < thres && fabs(VECTOR_DOUBLE(X, 0))> 0.2 )
320 break;
321
322 vector_free(X);
323 n--;
324 if (n <= nthres) /* failed */
325 {
326 A->m = B->n = W->n = ntot;
327 matrix_free(A);
328 vector_free(B);
329 vector_free(W);
330 vector_free(index); /* BUGFIX line added by JB for Takashi */
331 vector_free(pos); /* BUGFIX line added by JB for Takashi */
332 return (0);
333 }
334 /* swap row maxi with row n-1 */
335 (void) matrix_swap_rows(A, maxi, n);
336 SWAP(double, VECTOR_DOUBLE(B, maxi), VECTOR_DOUBLE(B, n));
337 SWAP(double, VECTOR_DOUBLE(W, maxi), VECTOR_DOUBLE(W, n));
338 SWAP(int, VECTOR_INT(index, maxi), VECTOR_INT(index, n));
339 A->m = B->n = W->n = n;
340 } while (1);
341
342 C = matrix_vprod(A, X); /* measure weighted residual */
343 *resid = 0;
344 for (i = 0; i < n; ++i)
345 *resid += sqr(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i));
346 *resid /= n;
347 vector_free(C);
348 A->m = B->n = W->n = ntot;
349
350 C = matrix_vprod(A, X);
351 for (i = n; i < ntot; ++i) /* these have been eliminated */
352 {
353 if (fabs(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i)) < thres)
354 continue; /* ok after all */
355 j = VECTOR_INT(index, i);
356 VECTOR_PTR(pos, j) = NULL;
357 }
358 vector_free(C);
359
360 for (i = 0; i < ntot; ++i)
361 if (VECTOR_PTR(pos, i) != NULL)
362 break;
363 *p1 = vec2_of_vec3(*(Vec3 *) VECTOR_PTR(pos, i));
364
365 for (j = ntot - 1; j >= 0; --j)
366 if (VECTOR_PTR(pos, j) != NULL)
367 break;
368 *p2 = vec2_of_vec3(*(Vec3 *) VECTOR_PTR(pos, j));
369
370 mid = (i + j) / 2;
371 for (k = 0; mid + k < j || mid - k > i; ++k)
372 if (mid + k < j && VECTOR_PTR(pos, mid + k) != NULL)
373 {
374 *pm = vec2_of_vec3(*(Vec3 *) VECTOR_PTR(pos, mid + k));
375 break;
376 } else if (mid - k > i && VECTOR_PTR(pos, mid - k) != NULL)
377 {
378 *pm = vec2_of_vec3(*(Vec3 *) VECTOR_PTR(pos, mid - k));
379 break;
380 }
381 matrix_free(A);
382 vector_free(B);
383 vector_free(W);
384 vector_free(index);
385 vector_free(pos);
386 *a = VECTOR_DOUBLE(X, 0);
387 *b = VECTOR_DOUBLE(X, 1);
388 *c = VECTOR_DOUBLE(X, 2);
389 vector_free(X);
390
391 return (1);
392 }
393
394 Plane *plane_from_affine(double A, double B, double C)
395 {
396 double div;
397 Vec3 n = {Vec3_id};
398 Vec3 p = {Vec3_id};
399
400 div = A;
401 A = (1 - A) / A;
402 B = -B / div;
403 C = -C / div;
404
405 n = vec3_unit(vec3(A, B, -1.0));
406 p = vec3(0.0, 0.0, C);
407 return (plane_make(p, n, DISPARITY));
408 }
409
410 Plane *plane_curve_ls(Tstring * curv, Tstring *string3d, double resid_thres, Vec2 * p1, Vec2 * p2, Vec2 * pm)
411 /* approximated geometrical string */
412 /* string of matched edges */
413 /* residual threshold */
414
415 {
416 double A, B, C; /* affine parameters Xl = A Xr + B Yr
417 * +C */
418 int good_affine = 0;
419 double resid, thres2 = resid_thres * 2;
420 Plane *plane;
421 Vec3 c = {Vec3_id};
422 Vec3 v = {Vec3_id};
423
424 if (curv == NULL || string3d == NULL)
425 return (NULL);
426
427 resid_thres *= resid_thres;
428
429 good_affine = affine_curve_it(string3d, thres2, &A, &B, &C, &resid, p1, p2, pm);
430
431 if (!good_affine || resid > resid_thres)
432 return (NULL);
433
434 plane = plane_from_affine(A, B, C);
435 if (plane == NULL)
436 return (NULL);
437
438 plane_par_proj_3d(plane);
439 par_proj_ray(str2_centroid(curv), &c, &v); /* find ray through curv
440 * centre */
441 plane->p = vec3_inter_line_plane(c, v, plane->p, plane->n);
442
443 return (plane);
444 }
445
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.