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

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

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