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

Linux Cross Reference
Tina5/tina-libs/tina/math/mathGeom_proj2.c

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

  1 /**********
  2  *
  3  * Copyright (c) 2003, Division of Imaging Science and Biomedical Engineering,
  4  * University of Manchester, UK.  All rights reserved.
  5  * 
  6  * Redistribution and use in source and binary forms, with or without modification, 
  7  * are permitted provided that the following conditions are met:
  8  * 
  9  *   . Redistributions of source code must retain the above copyright notice, 
 10  *     this list of conditions and the following disclaimer.
 11  *    
 12  *   . Redistributions in binary form must reproduce the above copyright notice,
 13  *     this list of conditions and the following disclaimer in the documentation 
 14  *     and/or other materials provided with the distribution.
 15  * 
 16  *   . Neither the name of the University of Manchester nor the names of its
 17  *     contributors may be used to endorse or promote products derived from this 
 18  *     software without specific prior written permission.
 19  * 
 20  * 
 21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 24  * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
 25  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 26  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 27  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 28  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 29  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 30  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 31  * POSSIBILITY OF SUCH DAMAGE.
 32  *
 33  **********
 34  *
 35  * Program :    TINA
 36  * File    :  $Source: /home/tina/cvs/tina-libs/tina/math/mathGeom_proj2.c,v $
 37  * Date    :  $Date: 2003/09/23 11:32:20 $
 38  * Version :  $Revision: 1.4 $
 39  * CVS Id  :  $Id: mathGeom_proj2.c,v 1.4 2003/09/23 11:32:20 matts Exp $
 40  *
 41  * Author  :  Legacy TINA
 42  *
 43  * Notes : Image point corresponding to projective point etc
 44  *
 45  *********
 46 */
 47 /** 
 48  *  @file
 49  *  @brief  Projection functions (2D) between image points and projected points.        
 50  *
 51  *  Projections of (image) points to projective points, the application of 
 52  *  projective transforms etc. 
 53  *  Makes use of Vec2, Vec3, Mat3.
 54  * 
 55 */
 56 
 57 #include "mathGeom_proj2.h"
 58 
 59 #if HAVE_CONFIG_H
 60 #include <config.h>
 61 #endif
 62 
 63 
 64 #include <math.h>
 65 #include <stdio.h>
 66 #include <tina/sys/sysDef.h>
 67 #include <tina/math/math_MatrDef.h>
 68 #include <tina/math/math_MatrPro.h>
 69 #include <tina/math/math_VecDef.h>
 70 #include <tina/math/math_VecPro.h>
 71 #include <tina/math/math_GeomDef.h>
 72 #include <tina/math/mathGeom_vec2.h>
 73 #include <tina/math/mathGeom_vec3.h>
 74 #include <tina/math/mathGeom_mat3.h>
 75 #include <tina/math/math_NumDef.h>
 76 #include <tina/math/math_NumPro.h>
 77 
 78 /** image point corresponding to projective point **/
 79 Vec2            vec2_of_proj2(Vec3 v)
 80 {
 81         double          z = vec3_z(v);
 82 
 83         return (vec2(vec3_x(v) / z, vec3_y(v) / z));
 84 }
 85 
 86 /** (unit) projective point corresponding to image point **/
 87 Vec3            proj2_of_vec2(Vec2 v)
 88 {
 89         return (vec3(vec2_x(v), vec2_y(v), 1.0));
 90 }
 91 
 92 /** result of applying projective transform to projective point **/
 93 Vec3            proj2_rectify(Mat3 m, Vec3 v)
 94 {
 95         return (vec3_unit(mat3_vprod(m, v)));
 96 }
 97 
 98 /** result of applying projective transform to image point **/
 99 Vec2            vec2_rectify(Mat3 m, Vec2 v)
100 {
101         Vec3            w = {Vec3_id};
102 
103         w = mat3_vprod(m, vec3(vec2_x(v), vec2_y(v), 1.0));
104         return (vec2(vec3_x(w) / vec3_z(w), vec3_y(w) / vec3_z(w)));
105 }
106 
107 /** projective coordinates of projective line joining two points **/
108 Vec3            proj2_join(Vec3 p, Vec3 q)
109 {
110         return (vec3_unitcross(p, q));
111 }
112 
113 /** projective coordinates of intersection of two lines **/
114 Vec3            proj2_intersect(Vec3 p, Vec3 q)
115 {
116         return (vec3_unitcross(p, q));
117 }
118 
119 /** temporary (not so stable) **/
120 static Vec3     solve(Mat3 m, Vec3 y)
121 {
122         return (mat3_vprod(mat3_inverse(m), y));
123 }
124 
125 Mat3            proj2_to_frame(Vec3 p00, Vec3 p10, Vec3 p01, Vec3 p11)
126 {
127         Mat3            m = {Mat3_id};
128         Vec3            px = {Vec3_id};
129         Vec3            py = {Vec3_id};
130         Vec3            pz = {Vec3_id};
131         Vec3            v = {Vec3_id};
132 
133         /** for stability **/
134         p00 = vec3_unit(p00);
135         p01 = vec3_unit(p01);
136         p10 = vec3_unit(p10);
137         p11 = vec3_unit(p11);
138 
139         /** images of (1 0 0), (0 1 0), and (0 0 1) **/
140         px = proj2_intersect(proj2_join(p00, p10), proj2_join(p01, p11));
141         py = proj2_intersect(proj2_join(p00, p01), proj2_join(p10, p11));
142         pz = vec3_unit(p00);
143 
144         /** found m up to scale factor on each column **/
145         m = mat3_of_cols(px, py, pz);
146 
147         /** now make (1 1 1) go to p11 **/
148         v = vec3_unit(solve(m, p11));
149         m = mat3_of_cols(vec3_times(vec3_x(v), px),
150                          vec3_times(vec3_y(v), py),
151                          vec3_times(vec3_z(v), pz));
152 
153         return (m);
154 }
155 
156 Mat3            proj2_between(Vec2 p00, Vec2 p10, Vec2 p01, Vec2 p11, Vec2 q00, Vec2 q10, Vec2 q01, Vec2 q11)
157 {
158         Mat3            mr = {Mat3_id};
159         Mat3            ms = {Mat3_id};
160         Mat3            mrs = {Mat3_id};
161         Vec3            r00 = {Vec3_id};
162         Vec3            r10 = {Vec3_id};
163         Vec3            r01 = {Vec3_id};
164         Vec3            r11 = {Vec3_id};
165         Vec3            s00 = {Vec3_id};
166         Vec3            s10 = {Vec3_id};
167         Vec3            s01 = {Vec3_id};
168         Vec3            s11 = {Vec3_id};
169 
170         r00 = proj2_of_vec2(p00);
171         r10 = proj2_of_vec2(p10);
172         r01 = proj2_of_vec2(p01);
173         r11 = proj2_of_vec2(p11);
174 
175         s00 = proj2_of_vec2(q00);
176         s10 = proj2_of_vec2(q10);
177         s01 = proj2_of_vec2(q01);
178         s11 = proj2_of_vec2(q11);
179 
180         mr = proj2_to_frame(r00, r10, r01, r11);
181         ms = proj2_to_frame(s00, s10, s01, s11);
182         mrs = mat3_prod(ms, mat3_inverse(mr));
183         return (mrs);
184 }
185 
186 Mat3            proj2_between_proj2(Vec3 p00, Vec3 p10, Vec3 p01, Vec3 p11, Vec3 q00, Vec3 q10, Vec3 q01, Vec3 q11)
187 {
188         Mat3            mp = {Mat3_id};
189         Mat3            mq = {Mat3_id};
190 
191         mp = proj2_to_frame(p00, p10, p01, p11);
192         mq = proj2_to_frame(q00, q10, q01, q11);
193         return (mat3_prod(mq, mat3_inverse(mp)));
194 }
195 
196 /**
197 least squares replacement for geometrical method above
198 **/
199 
200 /**
201 proj_between_x1 is unstable if zx compoment of
202 projection is small
203 use proj_between_ls below which does all three and gets best
204 **/
205 
206 Mat3            proj_x1(int n, Vec3 * p, Vec3 * q, double *badness)
207 {
208         Mat3            m = {Mat3_id};
209         Matrix         *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
210         Vector         *b = vector_alloc(2 * n, double_v);
211         Vector         *c;
212         int             i;
213 
214         for (i = 0; i < n; i++)
215         {
216                 double          xp = vec3_x(p[i]), xq = vec3_x(q[i]);
217                 double          yp = vec3_y(p[i]), yq = vec3_y(q[i]);
218                 double          zp = vec3_z(p[i]), zq = vec3_z(q[i]);
219 
220                 matrix_putf(zq * yp, a, 2 * i, 0);
221                 matrix_putf(zq * zp, a, 2 * i, 1);
222                 matrix_putf(zq * xp, a, 2 * i, 2);
223                 matrix_putf(-xq * yp, a, 2 * i, 6);
224                 matrix_putf(-xq * zp, a, 2 * i, 7);
225                 vector_putf(xq * xp, b, 2 * i);
226 
227                 matrix_putf(zq * yp, a, 2 * i + 1, 3);
228                 matrix_putf(zq * zp, a, 2 * i + 1, 4);
229                 matrix_putf(zq * xp, a, 2 * i + 1, 5);
230                 matrix_putf(-yq * yp, a, 2 * i + 1, 6);
231                 matrix_putf(-yq * zp, a, 2 * i + 1, 7);
232                 vector_putf(yq * xp, b, 2 * i + 1);
233         }
234 
235         if ((c = matrix_cholesky_least_square(a, b)) == NULL)
236         {
237                 *badness = 1e10;
238                 return (mat3_zero());
239         }
240         mat3_xy(m) = vector_getf(c, 0);
241         mat3_xz(m) = vector_getf(c, 1);
242         mat3_xx(m) = vector_getf(c, 2);
243         mat3_yy(m) = vector_getf(c, 3);
244         mat3_yz(m) = vector_getf(c, 4);
245         mat3_yx(m) = vector_getf(c, 5);
246         mat3_zy(m) = vector_getf(c, 6);
247         mat3_zz(m) = vector_getf(c, 7);
248         mat3_zx(m) = 1.0;
249 
250         matrix_free(a);
251         vector_free(b);
252         vector_free(c);
253 
254         *badness = MAX(fabs(mat3_zy(m)), fabs(mat3_zz(m)));
255         return (m);
256 }
257 
258 Mat3            proj_y1(int n, Vec3 * p, Vec3 * q, double *badness)
259 {
260         Mat3            m = {Mat3_id};
261         Matrix         *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
262         Vector         *b = vector_alloc(2 * n, double_v);
263         Vector         *c;
264         int             i;
265 
266         for (i = 0; i < n; i++)
267         {
268                 double          xp = vec3_x(p[i]), xq = vec3_x(q[i]);
269                 double          yp = vec3_y(p[i]), yq = vec3_y(q[i]);
270                 double          zp = vec3_z(p[i]), zq = vec3_z(q[i]);
271 
272                 matrix_putf(zq * zp, a, 2 * i, 0);
273                 matrix_putf(zq * xp, a, 2 * i, 1);
274                 matrix_putf(zq * yp, a, 2 * i, 2);
275                 matrix_putf(-xq * zp, a, 2 * i, 6);
276                 matrix_putf(-xq * xp, a, 2 * i, 7);
277                 vector_putf(xq * yp, b, 2 * i);
278 
279                 matrix_putf(zq * zp, a, 2 * i + 1, 3);
280                 matrix_putf(zq * xp, a, 2 * i + 1, 4);
281                 matrix_putf(zq * yp, a, 2 * i + 1, 5);
282                 matrix_putf(-yq * zp, a, 2 * i + 1, 6);
283                 matrix_putf(-yq * xp, a, 2 * i + 1, 7);
284                 vector_putf(yq * yp, b, 2 * i + 1);
285         }
286 
287         if ((c = matrix_cholesky_least_square(a, b)) == NULL)
288         {
289                 *badness = 1e10;
290                 return (mat3_zero());
291         }
292         mat3_xz(m) = vector_getf(c, 0);
293         mat3_xx(m) = vector_getf(c, 1);
294         mat3_xy(m) = vector_getf(c, 2);
295         mat3_yz(m) = vector_getf(c, 3);
296         mat3_yx(m) = vector_getf(c, 4);
297         mat3_yy(m) = vector_getf(c, 5);
298         mat3_zz(m) = vector_getf(c, 6);
299         mat3_zx(m) = vector_getf(c, 7);
300         mat3_zy(m) = 1.0;
301 
302         matrix_free(a);
303         vector_free(b);
304         vector_free(c);
305 
306         *badness = MAX(fabs(mat3_zz(m)), fabs(mat3_zx(m)));
307         return (m);
308 }
309 
310 Mat3            proj_z1(int n, Vec3 * p, Vec3 * q, double *badness)
311 {
312         Mat3            m = {Mat3_id};
313         Matrix         *a = matrix_alloc(2 * n, 8, matrix_full, double_v);
314         Vector         *b = vector_alloc(2 * n, double_v);
315         Vector         *c;
316         int             i;
317 
318         for (i = 0; i < n; i++)
319         {
320                 double          xp = vec3_x(p[i]), xq = vec3_x(q[i]);
321                 double          yp = vec3_y(p[i]), yq = vec3_y(q[i]);
322                 double          zp = vec3_z(p[i]), zq = vec3_z(q[i]);
323 
324                 matrix_putf(zq * xp, a, 2 * i, 0);
325                 matrix_putf(zq * yp, a, 2 * i, 1);
326                 matrix_putf(zq * zp, a, 2 * i, 2);
327                 matrix_putf(-xq * xp, a, 2 * i, 6);
328                 matrix_putf(-xq * yp, a, 2 * i, 7);
329                 vector_putf(xq * zp, b, 2 * i);
330 
331                 matrix_putf(zq * xp, a, 2 * i + 1, 3);
332                 matrix_putf(zq * yp, a, 2 * i + 1, 4);
333                 matrix_putf(zq * zp, a, 2 * i + 1, 5);
334                 matrix_putf(-yq * xp, a, 2 * i + 1, 6);
335                 matrix_putf(-yq * yp, a, 2 * i + 1, 7);
336                 vector_putf(yq * zp, b, 2 * i + 1);
337         }
338 
339         if ((c = matrix_cholesky_least_square(a, b)) == NULL)
340         {
341                 *badness = 1e10;
342                 return (mat3_zero());
343         }
344         mat3_xx(m) = vector_getf(c, 0);
345         mat3_xy(m) = vector_getf(c, 1);
346         mat3_xz(m) = vector_getf(c, 2);
347         mat3_yx(m) = vector_getf(c, 3);
348         mat3_yy(m) = vector_getf(c, 4);
349         mat3_yz(m) = vector_getf(c, 5);
350         mat3_zx(m) = vector_getf(c, 6);
351         mat3_zy(m) = vector_getf(c, 7);
352         mat3_zz(m) = 1.0;
353 
354         matrix_free(a);
355         vector_free(b);
356         vector_free(c);
357 
358         *badness = MAX(fabs(mat3_zx(m)), fabs(mat3_zy(m)));
359         return (m);
360 }
361 
362 Mat3            proj_between_ls(int n, Vec3 * p, Vec3 * q)
363 {
364         Mat3            mx = {Mat3_id};
365         Mat3            my = {Mat3_id};
366         Mat3            mz = {Mat3_id};
367         double          bx, by, bz, b;
368 
369         mx = proj_x1(n, p, q, &bx);
370         my = proj_y1(n, p, q, &by);
371         mz = proj_z1(n, p, q, &bz);
372 
373         b = MIN3(bx, by, bz);
374         if (b == bz)
375                 return (mz);
376         if (b == by)
377                 return (my);
378         return (mx);
379 }
380 

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