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

Linux Cross Reference
Tina5/tina-libs/tina/vision/visCalib_tsai.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 Lesser 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 Lesser General Public License for more details.
 14  *
 15  * You should have received a copy of the GNU Lesser 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  **********
 20  *
 21  * Program :    TINA
 22  * File    :  $Source: /home/tina/cvs/tina-libs/tina/vision/visCalib_tsai.c,v $
 23  * Date    :  $Date: 2003/10/06 12:29:47 $
 24  * Version :  $Revision: 1.3 $
 25  * CVS Id  :  $Id: visCalib_tsai.c,v 1.3 2003/10/06 12:29:47 neil Exp $
 26  *
 27  * Author  : Legacy TINA
 28  *
 29  * Notes :
 30  *
 31  *********
 32 */
 33 
 34 #include "visCalib_tsai.h"
 35 
 36 #if HAVE_CONFIG_H
 37   #include <config.h>
 38 #endif
 39 
 40 #include <math.h>
 41 #include <tina/sys/sysDef.h>
 42 #include <tina/sys/sysPro.h>
 43 #include <tina/math/mathDef.h>
 44 #include <tina/math/mathPro.h>
 45 #include <tina/geometry/geomDef.h>
 46 #include <tina/geometry/geomPro.h>
 47 #include <tina/vision/vis_CalibPro.h>
 48 
 49 
 50 static void stage1ii(void);
 51 static void stage1iii(void);
 52 static int stage2(void);
 53 
 54 static int n_points;
 55 static List *tsai_data;
 56 static Vec3 *(*tsai_get_3d) ();
 57 static Vec2 *(*tsai_get_pix) ();
 58 
 59 static Mat3 R = {Mat3_id};      /* rotation */
 60 static Vec3 t = {Vec3_id};      /* translation */
 61 static double f;                /* focal length */
 62 static Vector *Tr;              /* some unknowns */
 63 static Mat3 im_to_cam = {Mat3_id};      /* image to camera coordinates */
 64 static Vec2 m_e_pix = {Vec2_id};/* most eccentric point in image */
 65 static Vec3 m_e3 = {Vec3_id};   /* most eccentric world in image */
 66 
 67 static int valid_data_count(List * data, Vec3 * (*get_3d) ( /* ??? */ ), Vec2 * (*get_pix) ( /* ??? */ ))
 68 {
 69     int     i;
 70 
 71     for (i = 0; data != NULL; data = data->next)
 72     {
 73         if (get_3d(data) == NULL || get_pix(data) == NULL)
 74             continue;
 75         ++i;
 76     }
 77     return (i);
 78 }
 79 
 80 int     cam_cal_tsai(Camera * cam, List * data, Vec3 * (*get_3d) ( /* ??? */ ), Vec2 * (*get_pix) ( /* ??? */ ))
 81 {
 82     double  kx, ky;
 83     double  cx, cy;
 84 
 85     if (data == NULL || cam == NULL)
 86         return (0);
 87 
 88     n_points = valid_data_count(data, get_3d, get_pix);
 89     tsai_data = data;
 90     tsai_get_3d = get_3d;
 91     tsai_get_pix = get_pix;
 92 
 93     /* ret up image projection */
 94     kx = cam->ax / cam->pixel;
 95     ky = cam->ay / cam->pixel;
 96     cx = cam->cx;
 97     cy = cam->cy;
 98     im_to_cam = mat3(1 / kx, 0.0, -cx / kx,
 99                      0.0, 1 / ky, -cy / ky,
100                      0.0, 0.0, 1.0);
101 
102     stage1ii();                 /* sets Tr */
103     if (Tr == NULL)
104         return (0);
105 
106     stage1iii();
107     vector_free(Tr);            /* finished with */
108 
109     if (!stage2())
110         return (0);
111 
112     if (f < 0)                  /* change sign and repete stage 2 */
113     {
114         mat3_xz(R) *= -1;
115         mat3_yz(R) *= -1;
116         mat3_zx(R) *= -1;
117         mat3_zy(R) *= -1;
118         if (!stage2())
119             return (0);
120     }
121     cam->f = (float)f;
122     cam->transf = trans3_make(R, t);
123     cam_comp_default_rects(cam);/* update camera rectification mats */
124     return (1);
125 }
126 
127 static void stage1ii(void)
128 {
129     Matrix *M;
130     Matrix *W;
131     Vector *b;
132     List   *ptr;
133     double  max_r2 = -1;
134     int     i;
135 
136     M = matrix_alloc(n_points, 5, matrix_full, double_v);
137     W = matrix_alloc(n_points, n_points, matrix_full, double_v);        /* diag */
138     b = vector_alloc(n_points, double_v);
139 
140     for (i = 0, ptr = tsai_data; ptr != NULL; ptr = ptr->next)
141     {
142         Vec3    p3 = {Vec3_id};
143         Vec3   *pp3;
144         Vec2    p2 = {Vec2_id};
145         Vec2   *pp2;
146         double  r2;
147 
148         if ((pp3 = tsai_get_3d(ptr)) == NULL || (pp2 = tsai_get_pix(ptr)) == NULL)
149             continue;
150         p3 = *pp3;
151         p2 = *pp2;
152         p2 = rectify_pos(im_to_cam, p2);
153 
154         r2 = SQR(vec2_x(p2)) + SQR(vec2_y(p2));
155 
156         matrix_putf(vec2_y(p2) * vec3_x(p3), M, i, 0);
157         matrix_putf(vec2_y(p2) * vec3_y(p3), M, i, 1);
158         matrix_putf(vec2_y(p2), M, i, 2);
159         matrix_putf(-vec2_x(p2) * vec3_x(p3), M, i, 3);
160         matrix_putf(-vec2_x(p2) * vec3_y(p3), M, i, 4);
161         vector_putf(vec2_x(p2), b, i);
162         matrix_putf(r2, W, i, i);
163         ++i;
164 
165         if (r2 > max_r2)
166         {
167             max_r2 = r2;
168             m_e_pix = p2;       /* remember for later */
169             m_e3 = p3;
170         }
171     }
172 
173     Tr = matrix_cholesky_least_square(M, b);
174 
175     matrix_free(M);
176     matrix_free(W);
177     vector_free(b);
178 }
179 
180 static void stage1iii(void)
181 {
182     double *T = (double *) Tr->data;
183     double  Sr;
184     double  x, y;
185     Vec3    v3a = {Vec3_id};
186     Vec3    v3b = {Vec3_id};
187     Vec3    v3c = {Vec3_id};
188 
189 
190     Sr = T[0] * T[0] + T[1] * T[1] + T[3] * T[3] + T[4] * T[4];
191 
192     if ((fabs(T[0]) < 0.000001 || fabs(T[4]) < 0.000001) &&
193         (fabs(T[1]) < 0.000001 || fabs(T[3]) < 0.000001))
194         vec3_y(t) = (float)sqrt(1 / Sr);
195     else
196     {
197         double  a = T[0] * T[4] - T[1] * T[3];
198 
199         a *= 2 * a;
200         vec3_y(t) = (float)sqrt((Sr - sqrt(Sr * Sr - 2 * a)) / a);
201     }
202 
203     mat3_xx(R) = (float)(T[0] * vec3_y(t));
204     mat3_xy(R) = (float)(T[1] * vec3_y(t));
205     mat3_yx(R) = (float)(T[3] * vec3_y(t));
206     mat3_yy(R) = (float)(T[4] * vec3_y(t));
207     vec3_x(t) = (float)(T[2] * vec3_y(t));
208 
209     /* set the ambiguous sign */
210 
211     x = mat3_xx(R) * vec3_x(m_e3) + mat3_xy(R) * vec3_y(m_e3) + vec3_x(t);
212     y = mat3_yx(R) * vec3_x(m_e3) + mat3_yy(R) * vec3_y(m_e3) + vec3_y(t);
213 
214     if (!SAME_SIGN(vec2_x(m_e_pix), x) || !SAME_SIGN(vec2_y(m_e_pix), y))
215     {
216         mat3_xx(R) *= -1;
217         mat3_xy(R) *= -1;
218         mat3_yx(R) *= -1;
219         mat3_yy(R) *= -1;
220         vec3_x(t) *= -1;
221         vec3_y(t) *= -1;
222     }
223     /* solve for rest of R */
224 
225     mat3_xz(R) = (float)sqrt(1 - SQR(mat3_xx(R)) - SQR(mat3_xy(R)));
226 
227     if ((mat3_xx(R) * mat3_yx(R) + mat3_xy(R) * mat3_yy(R)) > 0)
228         mat3_yz(R) = (float)-sqrt(1 - SQR(mat3_yx(R)) - SQR(mat3_yy(R)));
229     else
230         mat3_yz(R) = (float)sqrt(1 - SQR(mat3_yx(R)) - SQR(mat3_yy(R)));
231     v3a = mat3_rowx(R);
232     v3b = mat3_rowy(R);
233     v3c = vec3_cross(v3a, v3b);
234     mat3_rowz_set(R, v3c);
235 }
236 
237 static int stage2(void)
238 {
239     Matrix *M;
240     Vector *b;
241     Vector *a;
242     List   *ptr;
243     double  t1;
244     int     i;
245 
246     M = matrix_alloc(n_points, 2, matrix_full, double_v);
247     b = vector_alloc(n_points, double_v);
248 
249     t1 = vec3_y(t);
250 
251     for (i = 0, ptr = tsai_data; ptr != NULL; ptr = ptr->next)
252     {
253         Vec3    p3 = {Vec3_id};
254         Vec3   *pp3;
255         Vec2    p2 = {Vec2_id};
256         Vec2   *pp2;
257         double  x, y;
258 
259         if ((pp3 = tsai_get_3d(ptr)) == NULL || (pp2 = tsai_get_pix(ptr)) == NULL)
260             continue;
261         p3 = *pp3;
262         p2 = *pp2;
263         p2 = rectify_pos(im_to_cam, p2);
264 
265         x = vec3_x(p3);
266         y = vec3_y(p3);
267 
268         matrix_putf(mat3_yx(R) * x + mat3_yy(R) * y + t1, M, i, 0);
269         matrix_putf(-vec2_y(p2), M, i, 1);
270         vector_putf(vec2_y(p2) * (mat3_zx(R) * x + mat3_zy(R) * y), b, i);
271         ++i;
272     }
273 
274     a = matrix_cholesky_least_square(M, b);
275     if (a == NULL)
276         return (0);
277 
278     f = vector_getf(a, 0);
279     vec3_z(t) = (float)vector_getf(a, 1);
280 
281     matrix_free(M);
282     vector_free(a);
283     vector_free(b);
284 
285     return (1);
286 }
287 

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