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