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

Linux Cross Reference
Tina6/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 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 

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