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

Linux Cross Reference
Tina5/tina-libs/tina/geometry/geomCurve_affine_cv.c

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

  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 

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