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

Linux Cross Reference
Tina5/tina-libs/tina/geometry/geomLine_2fit.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/geomLine_2fit.c,v $
 23  * Date    :  $Date: 2009/03/19 18:36:26 $
 24  * Version :  $Revision: 1.2 $
 25  * CVS Id  :  $Id: geomLine_2fit.c,v 1.2 2009/03/19 18:36:26 paul Exp $
 26  *
 27  * Notes :
 28  *
 29  *********
 30 */
 31 
 32 
 33 #include "geomLine_2fit.h"
 34 
 35 #if HAVE_CONFIG_H
 36   #include <config.h>
 37 #endif
 38 
 39 #include <stdio.h>
 40 #include <math.h>
 41 #include <tina/sys/sysDef.h>
 42 #include <tina/sys/sysPro.h>
 43 #include <tina/math/mathDef.h>
 44 #include <tina/math/mathPro.h>
 45 #include <tina/geometry/geomDef.h>
 46 #include <tina/geometry/geom_LineDef.h>
 47 #include <tina/geometry/geomLine_2line.h>
 48 
 49 
 50 
 51 #define FIT_GAP_THRESHOLD 10
 52 
 53 Bool    fit2_point_on_line(Vec2 p, Vec2 v, Vec2 c, double thres)
 54 {
 55     Vec2    d = {Vec2_id};
 56     double  dp;
 57 
 58     d = vec2_diff(p, c);
 59     dp = vec2_dot(v, d);
 60     d = vec2_diff(d, vec2_times(dp, v));
 61     return ((vec2_sqrmod(d) < SQR(thres)) ? true : false);
 62 }
 63 
 64 Bool    fit2_all_points_on_line(List * start, List * end, Vec2 v, Vec2 c, double thres)
 65 {
 66     List *dptr;
 67     Vec2    p = {Vec2_id};
 68 
 69     for (dptr = start;; dptr = dptr->next)
 70     {
 71         DD_GET_POS2(dptr, p);
 72         if (fit2_point_on_line(p, v, c, thres) == false)
 73             return (false);
 74         if (dptr == end)
 75             break;
 76     }
 77 
 78     return (true);
 79 }
 80 
 81 int     fit2_points_off_line(List * start, List * end, Vec2 v, Vec2 c, double thres)
 82 {
 83     List *dptr;
 84     int     n = 0;
 85     Vec2    p = {Vec2_id};
 86 
 87     for (dptr = start;; dptr = dptr->next)
 88     {
 89         DD_GET_POS2(dptr, p);
 90         if (fit2_point_on_line(p, v, c, thres) == false)
 91             ++n;
 92         if (dptr == end)
 93             break;
 94     }
 95 
 96     return (n);
 97 }
 98 
 99 List *fit2_findstart(List * pos, List * start, Vec2 v, Vec2 c, double thres)
100 {
101     List *last;
102     List *dptr;
103     int     gap = 0;
104 
105     if (start == NULL || pos == NULL)
106         return (pos);
107 
108     for (last = dptr = pos;; dptr = dptr->last)
109     {
110         Vec2    p = {Vec2_id};
111 
112         DD_GET_POS2(dptr, p);
113         if (!fit2_point_on_line(p, v, c, thres * 2))
114             break;
115         if (fit2_point_on_line(p, v, c, thres))
116         {
117             gap = 0;
118             last = dptr;
119         } else
120             ++gap;
121 
122         if (dptr == start || gap > FIT_GAP_THRESHOLD)
123             break;
124     }
125     return (last);
126 }
127 
128 List *fit2_findend(List * pos, List * end, Vec2 v, Vec2 c, double thres)
129 {
130     List *last;
131     List *dptr;
132     int     gap = 0;
133 
134     if (end == NULL || pos == NULL)
135         return (pos);
136 
137     for (last = dptr = pos;; dptr = dptr->next)
138     {
139         Vec2    p = {Vec2_id};
140 
141         DD_GET_POS2(dptr, p);
142         if (!fit2_point_on_line(p, v, c, thres * 2))
143             break;
144         if (fit2_point_on_line(p, v, c, thres))
145         {
146             gap = 0;
147             last = dptr;
148         } else
149             ++gap;
150 
151         if (dptr == end || gap > FIT_GAP_THRESHOLD)
152             break;
153     }
154     return (last);
155 }
156 
157 Bool    fit2_regres(List * start, List * end, Vec2 * v, Vec2 * c, double *residual)
158 {
159     List *dptr;
160     int     n;
161     double  cx, cy, x2, y2, xy;
162 
163     cx = 0;
164     cy = 0;
165     x2 = 0;
166     y2 = 0;
167     xy = 0;
168     for (n = 0, dptr = start;; dptr = dptr->next)
169     {
170         Vec2    p = {Vec2_id};
171         float   x, y;
172 
173         DD_GET_POS2(dptr, p);
174         x = vec2_x(p);
175         y = vec2_y(p);
176 
177         cx += x;
178         cy += y;
179         x2 += x * x;
180         y2 += y * y;
181         xy += x * y;
182         ++n;
183         if (dptr == end)
184             break;
185     }
186 
187     if (n < 3)
188         return (false);
189 
190     cx /= n;
191     cy /= n;
192     x2 /= n;
193     x2 -= cx * cx;
194     y2 /= n;
195     y2 -= cy * cy;
196     xy /= n;
197     xy -= cx * cy;
198 
199     if (fabs(xy) < 0.0001)
200         *v = (x2 > y2) ? vec2(1.0, 0.0) : vec2(0.0, 1.0);
201     else
202     {
203         double  lam1, lam2;
204         double  flam1, flam2;
205         double  b, root;
206 
207         b = x2 + y2;
208         root = sqrt(b * b - 4 * (x2 * y2 - xy * xy));
209         flam1 = fabs(lam1 = (b + root) / 2);
210         flam2 = fabs(lam2 = (b - root) / 2);
211         if (flam1 < flam2)
212         {
213             *residual = flam1;
214             *v = vec2_unit(vec2(1.0, -(x2 - lam2) / xy));
215         } else
216         {
217             *residual = flam2;
218             *v = vec2_unit(vec2(1.0, -(x2 - lam1) / xy));
219         }
220     }
221     *c = vec2(cx, cy);
222 
223     return (true);
224 }
225 
226 Bool    fit2_regres_it(List * start, List * end, Vec2 * v, Vec2 * c, double *residual, double thres)
227 {
228     List *dptr;
229     int     n, nthres;
230     Bool    outliers;
231     double  cx, cy, x2, y2, xy;
232     double  CX, CY, X2, Y2, XY;
233 
234     thres *= thres;             /* save computation: check against perp
235                                  * squared */
236     CX = 0;
237     CY = 0;
238     X2 = 0;
239     Y2 = 0;
240     XY = 0;
241     for (n = 0, dptr = start;; dptr = dptr->next)
242     {
243         Vec2    p = {Vec2_id};
244         float   x, y;
245 
246         DD_GET_POS2(dptr, p);
247         x = vec2_x(p);
248         y = vec2_y(p);
249 
250         CX += x;
251         CY += y;
252         X2 += x * x;
253         Y2 += y * y;
254         XY += x * y;
255         ++n;
256         if (dptr == end)
257             break;
258     }
259 
260     if (n < 5)
261         return (false);
262     nthres = MAX(n / 2, 5);
263 
264     do
265     {
266         List *maxptr = NULL;
267         double  maxperpsqr = 0;
268 
269         outliers = false;
270         cx = CX / n;
271         cy = CY / n;
272         x2 = X2 / n;
273         x2 -= cx * cx;
274         y2 = Y2 / n;
275         y2 -= cy * cy;
276         xy = XY / n;
277         xy -= cx * cy;
278         if (fabs(xy) < 0.0001)
279             *v = (x2 > y2) ? vec2(1.0, 0.0) : vec2(0.0, 1.0);
280         else
281         {
282             double  lam1, lam2;
283             double  flam1, flam2;
284             double  b, root;
285 
286             b = x2 + y2;
287             root = sqrt(b * b - 4 * (x2 * y2 - xy * xy));
288             flam1 = fabs(lam1 = (b + root) / 2);
289             flam2 = fabs(lam2 = (b - root) / 2);
290             if (flam1 < flam2)
291             {
292                 *residual = flam1;
293                 *v = vec2_unit(vec2(1.0, -(x2 - lam2) / xy));
294             } else
295             {
296                 *residual = flam2;
297                 *v = vec2_unit(vec2(1.0, -(x2 - lam1) / xy));
298             }
299         }
300         *c = vec2(cx, cy);
301 
302         for (dptr = start;; dptr = dptr->next)
303         {
304             Vec2    p = {Vec2_id};
305             Vec2    d = {Vec2_id};
306             float   dp, perpsqr;
307 
308             if (dptr->type < IGNORE_ME)
309             {
310                 DD_GET_POS2(dptr, p);
311                 d = vec2_diff(p, *c);
312                 dp = (float) vec2_dot(*v, d);
313                 d = vec2_diff(d, vec2_times(dp, *v));
314                 perpsqr = (float) vec2_sqrmod(d);
315                 if (maxptr == NULL || perpsqr > maxperpsqr)
316                 {
317                     maxptr = dptr;
318                     maxperpsqr = perpsqr;
319                 }
320             }
321             if (dptr == end)
322                 break;
323         }
324 
325         if (maxperpsqr > thres)
326         {
327             Vec2    p = {Vec2_id};
328             float   x, y;
329 
330             outliers = true;
331             DD_GET_POS2(maxptr, p);
332             x = vec2_x(p);
333             y = vec2_y(p);
334             CX -= x;
335             CY -= y;
336             X2 -= x * x;
337             Y2 -= y * y;
338             XY -= x * y;
339             --n;
340             maxptr->type += IGNORE_ME;
341         }
342     }
343     while (outliers == true && n >= nthres);
344 
345     for (dptr = start;; dptr = dptr->next)
346     {
347         if (dptr->type >= IGNORE_ME)
348             dptr->type -= IGNORE_ME;
349         if (dptr == end)
350             break;
351     }
352 
353     return ((n < nthres) ? false : true);
354 }
355 
356 Line2  *line2_fit(List * start, List * end)     /* permits POS2 strings */
357 
358 {
359     Vec2    p1 = {Vec2_id};
360     Vec2    p2 = {Vec2_id};
361     Vec2    v = {Vec2_id};
362     Vec2    c = {Vec2_id};
363     Line2  *line;
364     Tstring *string;
365     double  residual;
366 
367     if (start == NULL || end == NULL || start == end)
368         return (NULL);
369 
370     DD_GET_POS2(start, p1);
371     DD_GET_POS2(end, p2);
372 
373     if (fit2_regres(start, end, &v, &c, &residual) == false)
374         return (NULL);
375 
376     p1 = vec2_sum(c, vec2_times(vec2_dot(vec2_diff(p1, c), v), v));
377     p2 = vec2_sum(c, vec2_times(vec2_dot(vec2_diff(p2, c), v), v));
378 
379     line = line2_make(p1, p2, LINE_REG_FIT);
380     string = str_make(STRING, start, end);
381     line->props = proplist_add(line->props, (void *) string, STRING, str_rm_only_str);
382     return (line);
383 }
384 
385 Line2  *line2_best_fit(List * start, List * end, double thres)  /* permits Edgel and
386                                                                          * Vec2 strings */
387 
388 /* orthogonal fit threshold */
389 {
390     Vec2    p1 = {Vec2_id};
391     Vec2    p2 = {Vec2_id};
392     Vec2    v = {Vec2_id};
393     Vec2    c = {Vec2_id};
394     Line2  *line;
395     List *dptr;
396     Tstring *string;
397     double  residual;
398 
399     if (start == NULL || end == NULL || start == end)
400         return (NULL);
401 
402     if (fit2_regres(start, end, &v, &c, &residual) == false)
403         return (NULL);
404 
405     if (fit2_points_off_line(start, end, v, c, thres))
406         if (fit2_regres_it(start, end, &v, &c, &residual, thres) == false)
407             return (NULL);
408 
409     for (dptr = start; dptr != end; dptr = dptr->next)
410     {
411         DD_GET_POS2(dptr, p1);
412         if (fit2_point_on_line(p1, v, c, thres) == true)
413         {
414             p1 = vec2_sum(c, vec2_times(vec2_dot(vec2_diff(p1, c), v), v));
415             break;
416         }
417     }
418     start = dptr;               /* new start position */
419 
420     for (dptr = end; dptr != start; dptr = dptr->last)
421     {
422         DD_GET_POS2(dptr, p2);
423         if (fit2_point_on_line(p2, v, c, thres) == true)
424         {
425             p2 = vec2_sum(c, vec2_times(vec2_dot(vec2_diff(p2, c), v), v));
426             break;
427         }
428     }
429     end = dptr;
430 
431     if (start == end || vec2_mod(vec2_diff(p2, p1)) == 0.0)
432         return (NULL);
433 
434     line = line2_make(p1, p2, LINE_REG_FIT);
435     string = str_make(STRING, start, end);
436     line->props = proplist_add(line->props, (void *) string, STRING, str_rm_only_str);
437     return (line);
438 }
439 
440 Line2  *line2_fit_and_grow(List ** pos1ptr, List ** pos2ptr, List * start, List * end, double thres)
441 /* initial region for fitting */
442 {
443     List *pos1_last;
444     List *pos2_last;
445     List *pos1;
446     List *pos2;
447     Line2  *line;
448     Tstring *string;
449     double  residual;
450     Vec2    p1 = {Vec2_id};
451     Vec2    p2 = {Vec2_id};
452     Vec2    v = {Vec2_id};
453     Vec2    c = {Vec2_id};
454 
455     if (start == NULL || end == NULL || start == end)
456         return (NULL);
457 
458     if (pos1ptr == NULL || pos2ptr == NULL || *pos1ptr == NULL || *pos2ptr == NULL)
459         return (NULL);
460 
461     pos1 = *pos1ptr;
462     pos2 = *pos2ptr;
463 
464     do
465     {
466         pos1_last = pos1;
467         pos2_last = pos2;
468 
469         if (fit2_regres(pos1, pos2, &v, &c, &residual) == false)
470             return (NULL);
471 
472         pos1 = fit2_findstart(pos1, start, v, c, thres);
473         pos2 = fit2_findend(pos2, end, v, c, thres);
474     }
475     while (pos1 != pos1_last || pos2 != pos2_last);
476 
477     *pos1ptr = pos1;
478     *pos2ptr = pos2;
479     DD_GET_POS2(pos1, p1);
480     DD_GET_POS2(pos2, p2);
481     p1 = vec2_sum(c, vec2_times(vec2_dot(vec2_diff(p1, c), v), v));
482     p2 = vec2_sum(c, vec2_times(vec2_dot(vec2_diff(p2, c), v), v));
483     line = line2_make(p1, p2, LINE_REG_FIT);
484     string = str_make(STRING, pos1, pos2);
485     line->props = proplist_add(line->props, (void *) string, STRING, str_rm_only_str);
486     return (line);
487 }
488 
489 Line2  *line2_between(List * start, List * end)
490 {
491     Vec2    p1 = {Vec2_id};
492     Vec2    p2 = {Vec2_id};
493     Line2  *line;
494     Tstring *string;
495 
496     if (start == NULL || end == NULL || start == end)
497         return (NULL);
498 
499     DD_GET_POS2(start, p1);
500     DD_GET_POS2(end, p2);
501     line = line2_make(p1, p2, LINE_NO_FIT);
502     string = str_make(STRING, start, end);
503     line->props = proplist_add(line->props, (void *) string, STRING, str_rm_only_str);
504     return (line);
505 }
506 

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