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

Linux Cross Reference
Tina4/src/vision/calib/cal_tsai.c

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

  1 /**@(#)
  2 **/
  3 #include <math.h>
  4 #include <tina/sys.h>
  5 #include <tina/sysfuncs.h>
  6 #include <tina/math.h>
  7 #include <tina/mathfuncs.h>
  8 #include <tina/vision.h>
  9 #include <tina/visionfuncs.h>
 10 
 11 static void stage1ii(void);
 12 static void stage1iii(void);
 13 static int stage2(void);
 14 
 15 static int n_points;
 16 static List *tsai_data;
 17 static Vec3 *(*tsai_get_3d) ();
 18 static Vec2 *(*tsai_get_pix) ();
 19 
 20 static Mat3 R = {Mat3_id};      /* rotation */
 21 static Vec3 t = {Vec3_id};      /* translation */
 22 static double f;                /* focal length */
 23 static Vector *Tr;              /* some unknowns */
 24 static Mat3 im_to_cam = {Mat3_id};      /* image to camera coordinates */
 25 static Vec2 m_e_pix = {Vec2_id};/* most eccentric point in image */
 26 static Vec3 m_e3 = {Vec3_id};   /* most eccentric world in image */
 27 
 28 static int valid_data_count(List * data, Vec3 * (*get_3d) ( /* ??? */ ), Vec2 * (*get_pix) ( /* ??? */ ))
 29 {
 30     int     i;
 31 
 32     for (i = 0; data != NULL; data = data->next)
 33     {
 34         if (get_3d(data) == NULL || get_pix(data) == NULL)
 35             continue;
 36         ++i;
 37     }
 38     return (i);
 39 }
 40 
 41 int     cam_cal_tsai(Camera * cam, List * data, Vec3 * (*get_3d) ( /* ??? */ ), Vec2 * (*get_pix) ( /* ??? */ ))
 42 {
 43     double  kx, ky;
 44     double  cx, cy;
 45 
 46     if (data == NULL || cam == NULL)
 47         return (0);
 48 
 49     n_points = valid_data_count(data, get_3d, get_pix);
 50     tsai_data = data;
 51     tsai_get_3d = get_3d;
 52     tsai_get_pix = get_pix;
 53 
 54     /* ret up image projection */
 55     kx = cam->ax / cam->pixel;
 56     ky = cam->ay / cam->pixel;
 57     cx = cam->cx;
 58     cy = cam->cy;
 59     im_to_cam = mat3(1 / kx, 0.0, -cx / kx,
 60                      0.0, 1 / ky, -cy / ky,
 61                      0.0, 0.0, 1.0);
 62 
 63     stage1ii();                 /* sets Tr */
 64     if (Tr == NULL)
 65         return (0);
 66 
 67     stage1iii();
 68     vector_free(Tr);            /* finished with */
 69 
 70     if (!stage2())
 71         return (0);
 72 
 73     if (f < 0)                  /* change sign and repete stage 2 */
 74     {
 75         mat3_xz(R) *= -1;
 76         mat3_yz(R) *= -1;
 77         mat3_zx(R) *= -1;
 78         mat3_zy(R) *= -1;
 79         if (!stage2())
 80             return (0);
 81     }
 82     cam->f = (float)f;
 83     cam->transf = trans3_make(R, t);
 84     cam_comp_default_rects(cam);/* update camera rectification mats */
 85     return (1);
 86 }
 87 
 88 static void stage1ii(void)
 89 {
 90     Matrix *M;
 91     Matrix *W;
 92     Vector *b;
 93     List   *ptr;
 94     double  max_r2 = -1;
 95     int     i;
 96 
 97     M = matrix_alloc(n_points, 5, matrix_full, double_v);
 98     W = matrix_alloc(n_points, n_points, matrix_full, double_v);        /* diag */
 99     b = vector_alloc(n_points, double_v);
100 
101     for (i = 0, ptr = tsai_data; ptr != NULL; ptr = ptr->next)
102     {
103         Vec3    p3 = {Vec3_id};
104         Vec3   *pp3;
105         Vec2    p2 = {Vec2_id};
106         Vec2   *pp2;
107         double  r2;
108 
109         if ((pp3 = tsai_get_3d(ptr)) == NULL || (pp2 = tsai_get_pix(ptr)) == NULL)
110             continue;
111         p3 = *pp3;
112         p2 = *pp2;
113         p2 = rectify_pos(im_to_cam, p2);
114 
115         r2 = SQR(vec2_x(p2)) + SQR(vec2_y(p2));
116 
117         matrix_putf(vec2_y(p2) * vec3_x(p3), M, i, 0);
118         matrix_putf(vec2_y(p2) * vec3_y(p3), M, i, 1);
119         matrix_putf(vec2_y(p2), M, i, 2);
120         matrix_putf(-vec2_x(p2) * vec3_x(p3), M, i, 3);
121         matrix_putf(-vec2_x(p2) * vec3_y(p3), M, i, 4);
122         vector_putf(vec2_x(p2), b, i);
123         matrix_putf(r2, W, i, i);
124         ++i;
125 
126         if (r2 > max_r2)
127         {
128             max_r2 = r2;
129             m_e_pix = p2;       /* remember for later */
130             m_e3 = p3;
131         }
132     }
133 
134     Tr = matrix_cholesky_least_square(M, b);
135 
136     matrix_free(M);
137     matrix_free(W);
138     vector_free(b);
139 }
140 
141 static void stage1iii(void)
142 {
143     double *T = (double *) Tr->data;
144     double  Sr;
145     double  x, y;
146     Vec3    v3a = {Vec3_id};
147     Vec3    v3b = {Vec3_id};
148     Vec3    v3c = {Vec3_id};
149 
150 
151     Sr = T[0] * T[0] + T[1] * T[1] + T[3] * T[3] + T[4] * T[4];
152 
153     if ((fabs(T[0]) < 0.000001 || fabs(T[4]) < 0.000001) &&
154         (fabs(T[1]) < 0.000001 || fabs(T[3]) < 0.000001))
155         vec3_y(t) = (float)sqrt(1 / Sr);
156     else
157     {
158         double  a = T[0] * T[4] - T[1] * T[3];
159 
160         a *= 2 * a;
161         vec3_y(t) = (float)sqrt((Sr - sqrt(Sr * Sr - 2 * a)) / a);
162     }
163 
164     mat3_xx(R) = (float)(T[0] * vec3_y(t));
165     mat3_xy(R) = (float)(T[1] * vec3_y(t));
166     mat3_yx(R) = (float)(T[3] * vec3_y(t));
167     mat3_yy(R) = (float)(T[4] * vec3_y(t));
168     vec3_x(t) = (float)(T[2] * vec3_y(t));
169 
170     /* set the ambiguous sign */
171 
172     x = mat3_xx(R) * vec3_x(m_e3) + mat3_xy(R) * vec3_y(m_e3) + vec3_x(t);
173     y = mat3_yx(R) * vec3_x(m_e3) + mat3_yy(R) * vec3_y(m_e3) + vec3_y(t);
174 
175     if (!SAME_SIGN(vec2_x(m_e_pix), x) || !SAME_SIGN(vec2_y(m_e_pix), y))
176     {
177         mat3_xx(R) *= -1;
178         mat3_xy(R) *= -1;
179         mat3_yx(R) *= -1;
180         mat3_yy(R) *= -1;
181         vec3_x(t) *= -1;
182         vec3_y(t) *= -1;
183     }
184     /* solve for rest of R */
185 
186     mat3_xz(R) = (float)sqrt(1 - SQR(mat3_xx(R)) - SQR(mat3_xy(R)));
187 
188     if ((mat3_xx(R) * mat3_yx(R) + mat3_xy(R) * mat3_yy(R)) > 0)
189         mat3_yz(R) = (float)-sqrt(1 - SQR(mat3_yx(R)) - SQR(mat3_yy(R)));
190     else
191         mat3_yz(R) = (float)sqrt(1 - SQR(mat3_yx(R)) - SQR(mat3_yy(R)));
192     v3a = mat3_rowx(R);
193     v3b = mat3_rowy(R);
194     v3c = vec3_cross(v3a, v3b);
195     mat3_rowz_set(R, v3c);
196 }
197 
198 static int stage2(void)
199 {
200     Matrix *M;
201     Vector *b;
202     Vector *a;
203     List   *ptr;
204     double  t1;
205     int     i;
206 
207     M = matrix_alloc(n_points, 2, matrix_full, double_v);
208     b = vector_alloc(n_points, double_v);
209 
210     t1 = vec3_y(t);
211 
212     for (i = 0, ptr = tsai_data; ptr != NULL; ptr = ptr->next)
213     {
214         Vec3    p3 = {Vec3_id};
215         Vec3   *pp3;
216         Vec2    p2 = {Vec2_id};
217         Vec2   *pp2;
218         double  x, y;
219 
220         if ((pp3 = tsai_get_3d(ptr)) == NULL || (pp2 = tsai_get_pix(ptr)) == NULL)
221             continue;
222         p3 = *pp3;
223         p2 = *pp2;
224         p2 = rectify_pos(im_to_cam, p2);
225 
226         x = vec3_x(p3);
227         y = vec3_y(p3);
228 
229         matrix_putf(mat3_yx(R) * x + mat3_yy(R) * y + t1, M, i, 0);
230         matrix_putf(-vec2_y(p2), M, i, 1);
231         vector_putf(vec2_y(p2) * (mat3_zx(R) * x + mat3_zy(R) * y), b, i);
232         ++i;
233     }
234 
235     a = matrix_cholesky_least_square(M, b);
236     if (a == NULL)
237         return (0);
238 
239     f = vector_getf(a, 0);
240     vec3_z(t) = (float)vector_getf(a, 1);
241 
242     matrix_free(M);
243     vector_free(a);
244     vector_free(b);
245 
246     return (1);
247 }
248 

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