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

Linux Cross Reference
Tina4/src/vision/conic/conic_5pt.c

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

  1 /**@(#)
  2 **/
  3 /**
  4 * conic_5pt.c:
  5 * recover coeffs of conic through 5pts by
  6 * four line method
  7 **/
  8 
  9 #include <math.h>
 10 #include <tina/sys.h>
 11 #include <tina/sysfuncs.h>
 12 #include <tina/math.h>
 13 #include <tina/mathfuncs.h>
 14 #include <tina/vision.h>
 15 #include <tina/visionfuncs.h>
 16 
 17 /**
 18 coefficients of line  L = l0*x+l0*y+l2 = 0
 19 
 20 through  (x0 y0)  and  (x1 y1)
 21 **/
 22 static void line_coeffs(double *line, Vec2 p1, Vec2 p2)
 23 {
 24     line[0] = vec2_y(p2) - vec2_y(p1);
 25     line[1] = vec2_x(p1) - vec2_x(p2);
 26     line[2] = vec2_x(p2) * vec2_y(p1) - vec2_x(p1) * vec2_y(p2);
 27 }
 28 
 29 /**
 30 
 31 coefficients of degenerate conic (pair of lines)
 32 C = L0*L1
 33 **/
 34 static Conic *conic_from_2_lines(double *l0, double *l1)
 35 {
 36     Conic  *conic = conic_alloc((int) NULL);
 37 
 38     conic->a = l0[0] * l1[0];
 39     conic->b = 0.5 * (l0[0] * l1[1] + l0[1] * l1[0]);
 40     conic->c = l0[1] * l1[1];
 41     conic->d = 0.5 * (l0[0] * l1[2] + l0[2] * l1[0]);
 42     conic->e = 0.5 * (l0[1] * l1[2] + l0[2] * l1[1]);
 43     conic->f = l0[2] * l1[2];
 44     return (conic);
 45 }
 46 
 47 /**
 48 linear combination  C = t*C0+(1-t)*C1  of two conics
 49 **/
 50 static Conic *conic_combine(Conic * conic0, Conic * conic1, double t)
 51 {
 52     Conic  *conic = conic_alloc((int) NULL);
 53     double  t1 = 1.0 - t;
 54 
 55     conic->a = t * conic0->a + t1 * conic1->a;
 56     conic->b = t * conic0->b + t1 * conic1->b;
 57     conic->c = t * conic0->c + t1 * conic1->c;
 58     conic->d = t * conic0->d + t1 * conic1->d;
 59     conic->e = t * conic0->e + t1 * conic1->e;
 60     conic->f = t * conic0->f + t1 * conic1->f;
 61     return (conic);
 62 }
 63 
 64 /**
 65 returns circular conic through three points
 66 **/
 67 Conic  *conic_circ_3pt(Vec2 p1, Vec2 p2, Vec2 p3)
 68 {
 69     double  r;
 70     Vec2    c = {Vec2_id};
 71     Conic  *conic;
 72 
 73     vec2_circ_3_points(p1, p2, p3, &c, &r);
 74     conic = conic_make(ELLIPSE, c, 0.0, r, r, 0.0, TWOPI, 0);
 75     conic_set_ends(conic, p1, p3, p2);
 76     return (conic);
 77 }
 78 
 79 /**
 80 conic passing through  5  given points
 81 calculated by the 4-line method
 82 **/
 83 Conic  *conic_5pt(Vec2 p1, Vec2 p2, Vec2 p3, Vec2 p4, Vec2 p5)
 84 {
 85     Conic  *conic;
 86     Conic  *conic0;
 87     Conic  *conic1;
 88     double  l0[3], l1[3], l2[3], l3[3];
 89 
 90     line_coeffs(l0, p1, p2);
 91     line_coeffs(l1, p2, p3);
 92     line_coeffs(l2, p3, p4);
 93     line_coeffs(l3, p4, p1);
 94     conic0 = conic_from_2_lines(l0, l2);
 95     conic1 = conic_from_2_lines(l1, l3);
 96     conic = conic_combine(conic0, conic1, conic_lambda(conic0, conic1, p5));
 97     conic_free(conic0);
 98     conic_free(conic1);
 99     conic_setup(conic);
100     return (conic);
101 }
102 
103 /* Ellipse passing through  5  given points if it exists aspect ratio
104  * is too small or 5pt fit is ellipse returns 3pt circle */
105 Conic  *conic_ellipse_5pt(Vec2 p1, Vec2 p2, Vec2 p3, Vec2 p4, Vec2 p5, double min_aratio)
106 {
107     Conic  *conic = conic_5pt(p1, p2, p3, p4, p5);
108 
109     if (conic->type == HYPERBOLA || conic_aratio(conic) < min_aratio)
110     {
111         conic_free(conic);
112         return (conic_circ_3pt(p1, p3, p5));
113     } else
114         return (conic);
115 }
116 
117 /**
118 conic given 2  tangencies and one other point
119 **/
120 Conic  *conic_3pt(Vec2 p1, Vec2 v1, Vec2 p2, Vec2 v2, Vec2 p3)
121 {
122     Conic  *conic;
123     Conic  *conic0;
124     Conic  *conic1;
125     double  l0[3], l1[3], l2[3];
126 
127     line_coeffs(l0, p1, vec2_sum(p1, v1));
128     line_coeffs(l1, p2, vec2_sum(p2, v2));
129     line_coeffs(l2, p1, p2);
130     conic0 = conic_from_2_lines(l0, l1);
131     conic1 = conic_from_2_lines(l2, l2);
132     conic = conic_combine(conic0, conic1, conic_lambda(conic0, conic1, p3));
133     conic_free(conic0);
134     conic_free(conic1);
135     conic_setup(conic);
136     return (conic);
137 }
138 
139 /**
140 ellipse given 2  tangencies and one other point if it exists
141 aspect ratio is too small or 5pt fit is ellipse returns
142 3pt circle
143 **/
144 Conic  *conic_ellipse_3pt(Vec2 p1, Vec2 v1, Vec2 p2, Vec2 v2, Vec2 p3, double min_aratio)
145 {
146     Conic  *conic = conic_3pt(p1, v1, p2, v2, p3);
147 
148     /** temporary change **/
149     if (conic->type == HYPERBOLA || conic_aratio(conic) < min_aratio)
150     {
151         conic_free(conic);
152         return (conic_circ_3pt(p1, p2, p3));
153     } else
154         return (conic);
155 }
156 
157 /**
158 5 point  conic fit to dd string -
159 looks for well separated points
160 **/
161 Conic  *ddstr_conic_5pt(List * start, List * end)
162 {
163     List *m1;
164     List *m2;
165     List *m3;
166     Vec2    p0 = {Vec2_id};
167     Vec2    p1 = {Vec2_id};
168     Vec2    p2 = {Vec2_id};
169     Vec2    p3 = {Vec2_id};
170     Vec2    p4 = {Vec2_id};
171     Conic  *conic;
172 
173     if (start == end)
174         return (NULL);
175 
176     m2 = ddstr_mid_point(start, end);
177     m1 = ddstr_mid_point(start, m2);
178     m3 = ddstr_mid_point(m2, end);
179 
180     if (start == m1 || m1 == m2 || m2 == m3 || m3 == end)       /* not distinct points */
181         return (NULL);
182 
183     DD_GET_POS2(start, p0);
184     DD_GET_POS2(m1, p1);
185     DD_GET_POS2(m2, p2);
186     DD_GET_POS2(m3, p3);
187     DD_GET_POS2(end, p4);
188 
189     if (vec2_mod(vec2_diff(p0, p4)) < vec2_mod(vec2_diff(p0, p1)))
190         conic = ddstr_conic_5pt(m1, end);       /* end points too close */
191     else
192         conic = conic_5pt(p0, p1, p2, p3, p4);
193     conic_set_ends(conic, p0, p4, p2);
194     return (conic);
195 }
196 
197 /* 5 point ellipse fit to point string - looks for well separated
198  * points */
199 Conic  *ddstr_conic_ellipse_5pt(List * start, List * end, double min_aratio)
200 {
201     List *m1;
202     List *m2;
203     List *m3;
204     Vec2    p0 = {Vec2_id};
205     Vec2    p1 = {Vec2_id};
206     Vec2    p2 = {Vec2_id};
207     Vec2    p3 = {Vec2_id};
208     Vec2    p4 = {Vec2_id};
209     Conic  *conic;
210 
211     if (start == end)
212         return (NULL);
213 
214     m2 = ddstr_mid_point(start, end);
215     m1 = ddstr_mid_point(start, m2);
216     m3 = ddstr_mid_point(m2, end);
217 
218     if (start == m1 || m1 == m2 || m2 == m3 || m3 == end)       /* not distinct points */
219         return (NULL);
220 
221     DD_GET_POS2(start, p0);
222     DD_GET_POS2(m1, p1);
223     DD_GET_POS2(m2, p2);
224     DD_GET_POS2(m3, p3);
225     DD_GET_POS2(end, p4);
226 
227     if (vec2_mod(vec2_diff(p0, p4)) < vec2_mod(vec2_diff(p0, p1)))
228         conic = ddstr_conic_ellipse_5pt(m1, end, min_aratio);
229     else
230         conic = conic_ellipse_5pt(p0, p1, p2, p3, p4, min_aratio);
231     conic_set_ends(conic, p0, p4, p2);
232     return (conic);
233 }
234 
235 /**
236 3 point circle fit to point string -
237 looks for well separated points
238 **/
239 Conic  *ddstr_conic_circ_3pt(List * start, List * end)
240 {
241     List *mid;
242     Vec2    p0 = {Vec2_id};
243     Vec2    p1 = {Vec2_id};
244     Vec2    p2 = {Vec2_id};
245     Conic  *conic;
246 
247     if (start == end)
248         return (NULL);
249 
250     mid = ddstr_mid_point(start, end);
251 
252     if (start == mid || mid == end)     /* not distinct points */
253         return (NULL);
254 
255     DD_GET_POS2(start, p0);
256     DD_GET_POS2(mid, p1);
257     DD_GET_POS2(end, p2);
258 
259     if (vec2_mod(vec2_diff(p0, p2)) < vec2_mod(vec2_diff(p0, p1)) / 2)
260         conic = ddstr_conic_circ_3pt(start, ddstr_mid_point(mid, end));
261     else
262         conic = conic_circ_3pt(p0, p1, p2);
263     conic_set_ends(conic, p0, p2, p1);
264     return (conic);
265 }
266 
267 /** chooses same points on string as conic fitting **/
268 
269 Bool    ddstr_5pts_choose(List * start, List * end, Vec2 * p)
270 {
271     List *m1;
272     List *m2;
273     List *m3;
274 
275     if (start == end)
276         return (false);
277 
278     m2 = ddstr_mid_point(start, end);
279     m1 = ddstr_mid_point(start, m2);
280     m3 = ddstr_mid_point(m2, end);
281 
282     if (start == m1 || m1 == m2 || m2 == m3 || m3 == end)
283         /* not distinct points */
284         return (false);
285 
286     DD_GET_POS2(start, p[0]);
287     DD_GET_POS2(m1, p[1]);
288     DD_GET_POS2(m2, p[2]);
289     DD_GET_POS2(m3, p[3]);
290     DD_GET_POS2(end, p[4]);
291 
292     if (vec2_mod(vec2_diff(p[0], p[4])) < vec2_mod(vec2_diff(p[0], p[1])))
293         /* end points too close */
294         return (ddstr_5pts_choose(m1, end, p));
295     return (true);
296 }
297 
298 double  line_error(Line2 * line, Vec2 p)
299 {
300     return (vec2_cross(line->v, vec2_diff(p, line->p)));
301 }
302 
303 void    line_errors_check(Line2 * line, double *sum1, double *sum2)
304 {
305     Tstring *str;
306     List *ptr;
307 
308     if (line == NULL)
309         return;
310     str = prop_get(line->props, STRING);
311     if (str == NULL)
312         return;
313 
314     for (ptr = str->start;; ptr = ptr->next)
315     {
316         double  e;
317         Vec2    p = {Vec2_id};
318 
319         DD_GET_POS2(ptr, p);
320         e = line_error(line, p);
321         *sum1 += e;
322         *sum2 += e * e;
323         if (ptr == str->end)
324             break;
325     }
326     *sum1 /= str->count;
327     *sum2 /= str->count;
328 }
329 

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