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

Linux Cross Reference
Tina4/src/math/geom/proj2.c

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

  1 /**@(#)Image point corresponding to projective point etc
  2 **/
  3 
  4 #include <math.h>
  5 #include <stdio.h>
  6 #include <tina/sys.h>
  7 #include <tina/math.h>
  8 #include <tina/mathfuncs.h>
  9 
 10 /** image point corresponding to projective point **/
 11 Vec2    vec2_of_proj2(Vec3 v)
 12 {
 13     double  z = vec3_z(v);
 14 
 15     return (vec2(vec3_x(v) / z, vec3_y(v) / z));
 16 }
 17 
 18 /** (unit) projective point corresponding to image point **/
 19 Vec3    proj2_of_vec2(Vec2 v)
 20 {
 21     return (vec3(vec2_x(v), vec2_y(v), 1.0));
 22 }
 23 
 24 /** result of applying projective transform to projective point **/
 25 Vec3    proj2_rectify(Mat3 m, Vec3 v)
 26 {
 27     return (vec3_unit(mat3_vprod(m, v)));
 28 }
 29 
 30 /** result of applying projective transform to image point **/
 31 Vec2    vec2_rectify(Mat3 m, Vec2 v)
 32 {
 33     Vec3    w = {Vec3_id};
 34 
 35     w = mat3_vprod(m, vec3(vec2_x(v), vec2_y(v), 1.0));
 36     return (vec2(vec3_x(w) / vec3_z(w), vec3_y(w) / vec3_z(w)));
 37 }
 38 
 39 /** projective coordinates of projective line joining two points **/
 40 Vec3    proj2_join(Vec3 p, Vec3 q)
 41 {
 42     return (vec3_unitcross(p, q));
 43 }
 44 
 45 /** projective coordinates of intersection of two lines **/
 46 Vec3    proj2_intersect(Vec3 p, Vec3 q)
 47 {
 48     return (vec3_unitcross(p, q));
 49 }
 50 
 51 /** temporary (not so stable) **/
 52 static Vec3 solve(Mat3 m, Vec3 y)
 53 {
 54     return (mat3_vprod(mat3_inverse(m), y));
 55 }
 56 
 57 Mat3    proj2_to_frame(Vec3 p00, Vec3 p10, Vec3 p01, Vec3 p11)
 58 {
 59     Mat3    m = {Mat3_id};
 60     Vec3    px = {Vec3_id};
 61     Vec3    py = {Vec3_id};
 62     Vec3    pz = {Vec3_id};
 63     Vec3    v = {Vec3_id};
 64 
 65     /** for stability **/
 66     p00 = vec3_unit(p00);
 67     p01 = vec3_unit(p01);
 68     p10 = vec3_unit(p10);
 69     p11 = vec3_unit(p11);
 70 
 71     /** images of (1 0 0), (0 1 0), and (0 0 1) **/
 72     px = proj2_intersect(proj2_join(p00, p10), proj2_join(p01, p11));
 73     py = proj2_intersect(proj2_join(p00, p01), proj2_join(p10, p11));
 74     pz = vec3_unit(p00);
 75 
 76     /** found m up to scale factor on each column **/
 77     m = mat3_of_cols(px, py, pz);
 78 
 79     /** now make (1 1 1) go to p11 **/
 80     v = vec3_unit(solve(m, p11));
 81     m = mat3_of_cols(vec3_times(vec3_x(v), px),
 82                      vec3_times(vec3_y(v), py),
 83                      vec3_times(vec3_z(v), pz));
 84 
 85     return (m);
 86 }
 87 
 88 Mat3    proj2_between(Vec2 p00, Vec2 p10, Vec2 p01, Vec2 p11, Vec2 q00, Vec2 q10, Vec2 q01, Vec2 q11)
 89 {
 90     Mat3    mr = {Mat3_id};
 91     Mat3    ms = {Mat3_id};
 92     Mat3    mrs = {Mat3_id};
 93     Vec3    r00 = {Vec3_id};
 94     Vec3    r10 = {Vec3_id};
 95     Vec3    r01 = {Vec3_id};
 96     Vec3    r11 = {Vec3_id};
 97     Vec3    s00 = {Vec3_id};
 98     Vec3    s10 = {Vec3_id};
 99     Vec3    s01 = {Vec3_id};
100     Vec3    s11 = {Vec3_id};
101 
102     r00 = proj2_of_vec2(p00);
103     r10 = proj2_of_vec2(p10);
104     r01 = proj2_of_vec2(p01);
105     r11 = proj2_of_vec2(p11);
106 
107     s00 = proj2_of_vec2(q00);
108     s10 = proj2_of_vec2(q10);
109     s01 = proj2_of_vec2(q01);
110     s11 = proj2_of_vec2(q11);
111 
112     mr = proj2_to_frame(r00, r10, r01, r11);
113     ms = proj2_to_frame(s00, s10, s01, s11);
114     mrs = mat3_prod(ms, mat3_inverse(mr));
115     return (mrs);
116 }
117 
118 Mat3    proj2_between_proj2(Vec3 p00, Vec3 p10, Vec3 p01, Vec3 p11, Vec3 q00, Vec3 q10, Vec3 q01, Vec3 q11)
119 {
120     Mat3    mp = {Mat3_id};
121     Mat3    mq = {Mat3_id};
122 
123     mp = proj2_to_frame(p00, p10, p01, p11);
124     mq = proj2_to_frame(q00, q10, q01, q11);
125     return (mat3_prod(mq, mat3_inverse(mp)));
126 }
127 
128 /**
129 least squares replacement for geometrical method above
130 **/
131 
132 /**
133 proj_between_x1 is unstable if zx compoment of
134 projection is small
135 use proj_between_ls below which does all three and gets best
136 **/
137 
138 Mat3    proj_x1(int n, Vec3 * p, Vec3 * q, double *badness)
139 {
140     Mat3    m = {Mat3_id};
141     Matrix *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
142     Vector *b = vector_alloc(2 * n, double_v);
143     Vector *c;
144     int     i;
145 
146     for (i = 0; i < n; i++)
147     {
148         double  xp = vec3_x(p[i]), xq = vec3_x(q[i]);
149         double  yp = vec3_y(p[i]), yq = vec3_y(q[i]);
150         double  zp = vec3_z(p[i]), zq = vec3_z(q[i]);
151 
152         matrix_putf(zq * yp, a, 2 * i, 0);
153         matrix_putf(zq * zp, a, 2 * i, 1);
154         matrix_putf(zq * xp, a, 2 * i, 2);
155         matrix_putf(-xq * yp, a, 2 * i, 6);
156         matrix_putf(-xq * zp, a, 2 * i, 7);
157         vector_putf(xq * xp, b, 2 * i);
158 
159         matrix_putf(zq * yp, a, 2 * i + 1, 3);
160         matrix_putf(zq * zp, a, 2 * i + 1, 4);
161         matrix_putf(zq * xp, a, 2 * i + 1, 5);
162         matrix_putf(-yq * yp, a, 2 * i + 1, 6);
163         matrix_putf(-yq * zp, a, 2 * i + 1, 7);
164         vector_putf(yq * xp, b, 2 * i + 1);
165     }
166 
167     if ((c = matrix_cholesky_least_square(a, b)) == NULL)
168     {
169         *badness = 1e10;
170         return (mat3_zero());
171     }
172     mat3_xy(m) = vector_getf(c, 0);
173     mat3_xz(m) = vector_getf(c, 1);
174     mat3_xx(m) = vector_getf(c, 2);
175     mat3_yy(m) = vector_getf(c, 3);
176     mat3_yz(m) = vector_getf(c, 4);
177     mat3_yx(m) = vector_getf(c, 5);
178     mat3_zy(m) = vector_getf(c, 6);
179     mat3_zz(m) = vector_getf(c, 7);
180     mat3_zx(m) = 1.0;
181 
182     matrix_free(a);
183     vector_free(b);
184     vector_free(c);
185 
186     *badness = MAX(fabs(mat3_zy(m)), fabs(mat3_zz(m)));
187     return (m);
188 }
189 
190 Mat3    proj_y1(int n, Vec3 * p, Vec3 * q, double *badness)
191 {
192     Mat3    m = {Mat3_id};
193     Matrix *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
194     Vector *b = vector_alloc(2 * n, double_v);
195     Vector *c;
196     int     i;
197 
198     for (i = 0; i < n; i++)
199     {
200         double  xp = vec3_x(p[i]), xq = vec3_x(q[i]);
201         double  yp = vec3_y(p[i]), yq = vec3_y(q[i]);
202         double  zp = vec3_z(p[i]), zq = vec3_z(q[i]);
203 
204         matrix_putf(zq * zp, a, 2 * i, 0);
205         matrix_putf(zq * xp, a, 2 * i, 1);
206         matrix_putf(zq * yp, a, 2 * i, 2);
207         matrix_putf(-xq * zp, a, 2 * i, 6);
208         matrix_putf(-xq * xp, a, 2 * i, 7);
209         vector_putf(xq * yp, b, 2 * i);
210 
211         matrix_putf(zq * zp, a, 2 * i + 1, 3);
212         matrix_putf(zq * xp, a, 2 * i + 1, 4);
213         matrix_putf(zq * yp, a, 2 * i + 1, 5);
214         matrix_putf(-yq * zp, a, 2 * i + 1, 6);
215         matrix_putf(-yq * xp, a, 2 * i + 1, 7);
216         vector_putf(yq * yp, b, 2 * i + 1);
217     }
218 
219     if ((c = matrix_cholesky_least_square(a, b)) == NULL)
220     {
221         *badness = 1e10;
222         return (mat3_zero());
223     }
224     mat3_xz(m) = vector_getf(c, 0);
225     mat3_xx(m) = vector_getf(c, 1);
226     mat3_xy(m) = vector_getf(c, 2);
227     mat3_yz(m) = vector_getf(c, 3);
228     mat3_yx(m) = vector_getf(c, 4);
229     mat3_yy(m) = vector_getf(c, 5);
230     mat3_zz(m) = vector_getf(c, 6);
231     mat3_zx(m) = vector_getf(c, 7);
232     mat3_zy(m) = 1.0;
233 
234     matrix_free(a);
235     vector_free(b);
236     vector_free(c);
237 
238     *badness = MAX(fabs(mat3_zz(m)), fabs(mat3_zx(m)));
239     return (m);
240 }
241 
242 Mat3    proj_z1(int n, Vec3 * p, Vec3 * q, double *badness)
243 {
244     Mat3    m = {Mat3_id};
245     Matrix *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
246     Vector *b = vector_alloc(2 * n, double_v);
247     Vector *c;
248     int     i;
249 
250     for (i = 0; i < n; i++)
251     {
252         double  xp = vec3_x(p[i]), xq = vec3_x(q[i]);
253         double  yp = vec3_y(p[i]), yq = vec3_y(q[i]);
254         double  zp = vec3_z(p[i]), zq = vec3_z(q[i]);
255 
256         matrix_putf(zq * xp, a, 2 * i, 0);
257         matrix_putf(zq * yp, a, 2 * i, 1);
258         matrix_putf(zq * zp, a, 2 * i, 2);
259         matrix_putf(-xq * xp, a, 2 * i, 6);
260         matrix_putf(-xq * yp, a, 2 * i, 7);
261         vector_putf(xq * zp, b, 2 * i);
262 
263         matrix_putf(zq * xp, a, 2 * i + 1, 3);
264         matrix_putf(zq * yp, a, 2 * i + 1, 4);
265         matrix_putf(zq * zp, a, 2 * i + 1, 5);
266         matrix_putf(-yq * xp, a, 2 * i + 1, 6);
267         matrix_putf(-yq * yp, a, 2 * i + 1, 7);
268         vector_putf(yq * zp, b, 2 * i + 1);
269     }
270 
271     if ((c = matrix_cholesky_least_square(a, b)) == NULL)
272     {
273         *badness = 1e10;
274         return (mat3_zero());
275     }
276     mat3_xx(m) = vector_getf(c, 0);
277     mat3_xy(m) = vector_getf(c, 1);
278     mat3_xz(m) = vector_getf(c, 2);
279     mat3_yx(m) = vector_getf(c, 3);
280     mat3_yy(m) = vector_getf(c, 4);
281     mat3_yz(m) = vector_getf(c, 5);
282     mat3_zx(m) = vector_getf(c, 6);
283     mat3_zy(m) = vector_getf(c, 7);
284     mat3_zz(m) = 1.0;
285 
286     matrix_free(a);
287     vector_free(b);
288     vector_free(c);
289 
290     *badness = MAX(fabs(mat3_zx(m)), fabs(mat3_zy(m)));
291     return (m);
292 }
293 
294 Mat3    proj_between_ls(int n, Vec3 * p, Vec3 * q)
295 {
296     Mat3    mx = {Mat3_id};
297     Mat3    my = {Mat3_id};
298     Mat3    mz = {Mat3_id};
299     double  bx, by, bz, b;
300 
301     mx = proj_x1(n, p, q, &bx);
302     my = proj_y1(n, p, q, &by);
303     mz = proj_z1(n, p, q, &bz);
304 
305     b = MIN3(bx, by, bz);
306     if (b == bz)
307         return (mz);
308     if (b == by)
309         return (my);
310     return (mx);
311 }
312 

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