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

Linux Cross Reference
Tina4/src/vision/line2/fitline2.c

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

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

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