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

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

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