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

Linux Cross Reference
Tina4/src/vision/curve3/affine_cv.c

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

  1 /**@(#)
  2 **/
  3 /* affine_cv.c
  4  * 
  5  * fitting plane curves to strings of matched edge points by recovering
  6  * affine transform
  7  * 
  8  */
  9 
 10 #include <math.h>
 11 #include <tina/sys.h>
 12 #include <tina/sysfuncs.h>
 13 #include <tina/math.h>
 14 #include <tina/mathfuncs.h>
 15 #include <tina/vision.h>
 16 #include <tina/visionfuncs.h>
 17 
 18 #define HORIZONTAL_THRES 4
 19 
 20 static Vec2 aprox_dir2(List * pos)
 21 {
 22     Vec3    p1 = {Vec3_id};
 23     Vec3    p2 = {Vec3_id};
 24     Vec3    v3 = {Vec3_id};
 25 
 26     /* BUGFIX: was Vec3 v2 */
 27     Vec2    v2 = {Vec2_id};
 28 
 29     if (pos->next == NULL && pos->last == NULL)
 30         return (vec2_zero());
 31 
 32     if (pos->last != NULL)
 33         p1 = *(Vec3 *) pos->last->to;
 34     else
 35         p1 = *(Vec3 *) pos->to;
 36 
 37     if (pos->next != NULL)
 38         p2 = *(Vec3 *) pos->next->to;
 39     else
 40         p2 = *(Vec3 *) pos->to;
 41 
 42     v3 = vec3_diff(p2, p1);
 43     v2 = vec2_of_vec3(v3);
 44     return (vec2_unit(v2));
 45 }
 46 
 47 /* from string of disparity vectors find affine transform between
 48  * images */
 49 int     affine_curve(Tstring * string, double *a, double *b, double *c, double *resid)
 50 /* string of Vec3 (xl, y, xr-xl) */
 51 /* for affine parameters Xl = a.Xr + b.Yr + c */
 52 /* weighted sum sqr distance */
 53 {
 54     List *start;
 55     List *end;
 56     List *ptr;
 57     Matrix *A;                  /* each row of form  [wXr wYr w] */
 58     Vector *B;                  /* vector of wXl */
 59     Vector *C;
 60     Vector *X;
 61     Vector *W;
 62     int     i, n;
 63 
 64     if (string == NULL)
 65         return (0);
 66 
 67     start = string->start;
 68     end = string->end;
 69 
 70     for (n = 1, ptr = start; ptr != end; ptr = ptr->next)
 71         ++n;
 72 
 73     A = matrix_alloc(n, 3, matrix_full, double_v);
 74     B = vector_alloc(n, double_v);
 75     W = vector_alloc(n, double_v);
 76 
 77     /* BUGFIX sumw unused */
 78     /* double  sumw; sumw = 0; */
 79 
 80     for (i = 0, ptr = start; i < n; ++i, ptr = ptr->next)
 81     {
 82         Vec2    v2a = {Vec2_id};
 83         double  w = fabs(vec2_y(v2a));  /* weight by y component */
 84         Vec3    p = {Vec3_id};
 85 
 86         v2a = aprox_dir2(ptr);
 87         p = *(Vec3 *) ptr->to;
 88 
 89         VECTOR_DOUBLE(W, i) = w;
 90         VECTOR_DOUBLE(B, i) = vec3_x(p) * w;
 91         A->el.double_v[i][0] = (vec3_x(p) + vec3_z(p)) * w;
 92         A->el.double_v[i][1] = vec3_y(p) * w;
 93         A->el.double_v[i][2] = w;
 94 
 95         /* BUGFIX sumw unused */
 96         /* sumw += w; */
 97     }
 98 
 99     X = matrix_cholesky_least_square(A, B);     /* solve for affine
100                                                  * parameters */
101     if (X == NULL)
102     {
103         matrix_free(A);
104         vector_free(B);
105         vector_free(W);
106         return (0);
107     }
108     C = matrix_vprod(A, X);     /* measure weighted residual */
109     *resid = 0;
110     for (i = 0; i < n; ++i)
111         *resid += sqr(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i));
112     *resid /= n;
113 
114     matrix_free(A);
115     vector_free(B);
116     vector_free(C);
117     *a = VECTOR_DOUBLE(X, 0);
118     *b = VECTOR_DOUBLE(X, 1);
119     *c = VECTOR_DOUBLE(X, 2);
120     vector_free(X);
121 
122     return (1);
123 }
124 
125 int     affine_curve_it(Tstring * string, double thres, double *a, double *b, double *c, double *resid, Vec2 * p1, Vec2 * p2, Vec2 * pm)
126 /* string of Vec3 (xl, y, xr-xl) */
127 /* fit threshold */
128 /* for affine parameters Xl = a.Xr + b.Yr + c */
129 /* weighted sum sqr distance */
130 
131 {
132     List *start;
133     List *end;
134     List *ptr;
135     Matrix *A;                  /* each row of form  [wXr wYr w] */
136     Vector *B;                  /* vector of wXl */
137     Vector *C;
138     Vector *X;
139     Vector *W;
140     Vector *index;
141     Vector *pos;
142     int     i, j, k, mid, n, ntot, nthres;
143 
144     if (string == NULL)
145         return (0);
146 
147     start = string->start;
148     end = string->end;
149 
150     for (n = 1, ptr = start; ptr != end; ptr = ptr->next)
151         ++n;
152 
153     if (n < 10)
154         return (0);
155 
156     nthres = MAX(n / 2, 10);
157 
158     A = matrix_alloc(n, 3, matrix_full, double_v);
159     B = vector_alloc(n, double_v);
160     W = vector_alloc(n, double_v);
161     index = vector_alloc(n, int_v);
162     pos = vector_alloc(n, ptr_v);
163 
164     /* BUGFIX sumw unused */
165     /* double  sumw; sumw = 0; */
166 
167     for (i = 0, ptr = start; i < n; ++i, ptr = ptr->next)
168     {
169         double  w = 1.0;        /* fabs(vec2_y(aprox_dir2(ptr)));weight
170                                  * by y component */
171         Vec3    p = {Vec3_id};
172 
173         VECTOR_PTR(pos, i) = ptr->to;
174         p = *(Vec3 *) ptr->to;
175         VECTOR_INT(index, i) = i;
176 
177         VECTOR_DOUBLE(W, i) = w;
178         VECTOR_DOUBLE(B, i) = vec3_x(p) * w;
179         A->el.double_v[i][0] = (vec3_x(p) + vec3_z(p)) * w;
180         A->el.double_v[i][1] = vec3_y(p) * w;
181         A->el.double_v[i][2] = w;
182 
183         /* BUGFIX sumw unused */
184         /* sumw += w; */
185     }
186 
187     ntot = n;
188     do
189     {
190         int     maxi = 0;
191         double  diff, max_diff = 0;
192 
193         X = matrix_cholesky_least_square(A, B);
194         if (X == NULL)
195         {
196             A->m = B->n = W->n = ntot;
197             matrix_free(A);
198             vector_free(B);
199             vector_free(W);
200             vector_free(index); /* BUGFIX line added by JB for Takashi */
201             vector_free(pos);   /* BUGFIX line added by JB for Takashi */
202             return (0);
203         }
204         /* find outliers */
205         C = matrix_vprod(A, X);
206         for (i = 0; i < n; ++i)
207         {
208             diff = fabs(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i));
209 
210             if (diff > max_diff)
211             {
212                 max_diff = diff;
213                 maxi = i;
214             }
215         }
216         vector_free(C);
217 
218         if (max_diff < thres)   /* no outlying points */
219             break;
220 
221         vector_free(X);
222         n--;
223         if (n <= nthres)        /* failed */
224         {
225             A->m = B->n = W->n = ntot;
226             matrix_free(A);
227             vector_free(B);
228             vector_free(W);
229             vector_free(index); /* BUGFIX line added by JB for Takashi */
230             vector_free(pos);   /* BUGFIX line added by JB for Takashi */
231             return (0);
232         }
233         /* swap row maxi with row n-1 */
234         (void) matrix_swap_rows(A, maxi, n);
235         SWAP(double, VECTOR_DOUBLE(B, maxi), VECTOR_DOUBLE(B, n));
236         SWAP(double, VECTOR_DOUBLE(W, maxi), VECTOR_DOUBLE(W, n));
237         SWAP(int, VECTOR_INT(index, maxi), VECTOR_INT(index, n));
238         A->m = B->n = W->n = n;
239     } while (1);
240 
241     C = matrix_vprod(A, X);     /* measure weighted residual */
242     *resid = 0;
243     for (i = 0; i < n; ++i)
244         *resid += sqr(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i));
245     *resid /= n;
246     vector_free(C);
247     A->m = B->n = W->n = ntot;
248 
249     C = matrix_vprod(A, X);
250     for (i = n; i < ntot; ++i)  /* these have been eliminated */
251     {
252         if (fabs(VECTOR_DOUBLE(B, i) - VECTOR_DOUBLE(C, i)) < thres)
253             continue;           /* ok after all */
254         j = VECTOR_INT(index, i);
255         VECTOR_PTR(pos, j) = NULL;
256     }
257     vector_free(C);
258 
259     for (i = 0; i < ntot; ++i)
260         if (VECTOR_PTR(pos, i) != NULL)
261             break;
262     *p1 = vec2_of_vec3(*(Vec3 *) VECTOR_PTR(pos, i));
263 
264     for (j = ntot - 1; j >= 0; --j)
265         if (VECTOR_PTR(pos, j) != NULL)
266             break;
267     *p2 = vec2_of_vec3(*(Vec3 *) VECTOR_PTR(pos, j));
268 
269     mid = (i + j) / 2;
270     for (k = 0; mid + k < j || mid - k > i; ++k)
271         if (mid + k < j && VECTOR_PTR(pos, mid + k) != NULL)
272         {
273             *pm = vec2_of_vec3(*(Vec3 *) VECTOR_PTR(pos, mid + k));
274             break;
275         } else if (mid - k > i && VECTOR_PTR(pos, mid - k) != NULL)
276         {
277             *pm = vec2_of_vec3(*(Vec3 *) VECTOR_PTR(pos, mid - k));
278             break;
279         }
280     matrix_free(A);
281     vector_free(B);
282     vector_free(W);
283     vector_free(index);
284     vector_free(pos);
285     *a = VECTOR_DOUBLE(X, 0);
286     *b = VECTOR_DOUBLE(X, 1);
287     *c = VECTOR_DOUBLE(X, 2);
288     vector_free(X);
289 
290     return (1);
291 }
292 
293 Plane  *plane_from_affine(double A, double B, double C)
294 {
295     double  div;
296     Vec3    n = {Vec3_id};
297     Vec3    p = {Vec3_id};
298 
299     div = A;
300     A = (1 - A) / A;
301     B = -B / div;
302     C = -C / div;
303 
304     n = vec3_unit(vec3(A, B, -1.0));
305     p = vec3(0.0, 0.0, C);
306     return (plane_make(p, n, DISPARITY));
307 }
308 
309 static double y_range(Tstring * str3)
310 {
311     List *ptr;
312     double  ly, uy;
313 
314     ly = uy = vec3_y(*(Vec3 *) str3->start->to);
315     for (ptr = str3->start;; ptr = ptr->next)
316     {
317         double  y = vec3_y(*(Vec3 *) ptr->to);
318 
319         if (y < ly)
320             ly = y;
321         if (y > uy)
322             uy = y;
323         if (ptr == str3->end)
324             break;
325     }
326     return (uy - ly);
327 }
328 
329 Plane  *plane_curve_ls(Tstring * curv, Tstring * es, double resid_thres, Vec2 * p1, Vec2 * p2, Vec2 * pm)
330 /* approximated geometrical string */
331 /* string of matched edges */
332 /* residual threshold */
333 
334 {
335     double  A, B, C;            /* affine parameters Xl = A Xr + B Yr
336                                  * +C */
337     int     good_affine = 0;
338     double  resid, thres2 = resid_thres * 2;
339     Plane  *plane;
340     Vec3    c = {Vec3_id};
341     Vec3    v = {Vec3_id};
342     Tstring *string3d;
343 
344     if (curv == NULL || es == NULL)
345         return (NULL);
346 
347     resid_thres *= resid_thres;
348 
349     string3d = es_build_string3(curv, es);
350 
351     if (string3d == NULL || y_range(string3d) < HORIZONTAL_THRES)
352     {
353         str_rm(string3d, rfree);
354         return (NULL);
355     }
356     good_affine = affine_curve_it(string3d, thres2, &A, &B, &C, &resid, p1, p2, pm);
357     str_rm(string3d, rfree);
358 
359     if (!good_affine || resid > resid_thres)
360         return (NULL);
361 
362     plane = plane_from_affine(A, B, C);
363     if (plane == NULL)
364         return (NULL);
365 
366     if (fabs(vec3_z(plane->n)) < 0.3)   /* a disparity gradient of 1.5 */
367     {
368         plane_free(plane);
369         return (NULL);
370     }
371     plane_par_proj_3d(plane);
372     par_proj_ray(str2_centroid(curv), &c, &v);  /* find ray through curv
373                                                  * centre */
374     plane->p = vec3_inter_line_plane(c, v, plane->p, plane->n);
375 
376     return (plane);
377 }
378 

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