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

Linux Cross Reference
Tina6/tina-libs/tina/vision/visCalib_conv.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_conv.c,v $
 27  * Date    :  $Date: 2008/12/04 15:28:02 $
 28  * Version :  $Revision: 1.5 $
 29  * CVS Id  :  $Id: visCalib_conv.c,v 1.5 2008/12/04 15:28:02 paul Exp $
 30  *
 31  * Author  : Legacy TINA
 32  *
 33  * Notes :
 34  *
 35  *********
 36 */
 37 
 38 #include "visCalib_conv.h"
 39 
 40 #if HAVE_CONFIG_H
 41   #include <config.h>
 42 #endif
 43 
 44 #include <math.h>
 45 #include <stdio.h>
 46 
 47 #include <tina/sys/sysDef.h>
 48 #include <tina/math/mathDef.h>
 49 #include <tina/math/mathPro.h>
 50 #include <tina/geometry/geomDef.h>
 51 #include <tina/geometry/geomPro.h>
 52 #include <tina/vision/vis_CalibPro.h>
 53 
 54 
 55 #define Bit(mask,num)     (mask&(1<<num))
 56 
 57 int     conv_camera_int(int mask, Camera * cam, double *a)
 58 {
 59     int     i = 0;
 60     double *b;
 61 
 62     if (cam == NULL || a == NULL)
 63         return (0);
 64 
 65     if (Bit(mask, 0))
 66         a[i++] = (double) cam->f;
 67     if (Bit(mask, 1))
 68         a[i++] = (double) cam->cx;
 69     if (Bit(mask, 2))
 70         a[i++] = (double) cam->cy;
 71     if (Bit(mask, 3))
 72         a[i++] = 1.0 / (double) cam->ax;
 73     if (Bit(mask, 4))
 74         a[i++] = 1.0 / (double) cam->ay;
 75     if (Bit(mask, 5) && cam->distort_params != NULL)
 76     {
 77         b = cam->distort_params;
 78         a[i++] = b[1];
 79     }
 80     return (i);
 81 }
 82 
 83 int     conv_camera_ext(Camera * cam, double *a)
 84 {
 85     double  x[4], ex[3], ey[3], ez[3];
 86     int     i;
 87 
 88     if (cam == NULL || a == NULL)
 89         return (0);
 90 
 91 /*
 92     conv_rot_to_quat(&(cam->transf->R), x);
 93 */
 94     ex[0] = cam->transf->R.el[0][0];
 95     ex[1] = cam->transf->R.el[0][1];
 96     ex[2] = cam->transf->R.el[0][2];
 97     ey[0] = cam->transf->R.el[1][0];
 98     ey[1] = cam->transf->R.el[1][1];
 99     ey[2] = cam->transf->R.el[1][2];
100     ez[0] = cam->transf->R.el[2][0];
101     ez[1] = cam->transf->R.el[2][1];
102     ez[2] = cam->transf->R.el[2][2];
103     conv_rot_to_quat_pab(ex, ey, ez, x);
104     for (i = 0; i < 3; i++)
105         a[i] = x[i + 1];
106     a[3] = cam->transf->t.el[0];
107     a[4] = cam->transf->t.el[1];
108     a[5] = cam->transf->t.el[2];
109 
110     return (1);
111 }
112 
113 int     conv_camera_rel(Camera * cam1, Camera * cam2, double *a)
114 {
115     double  x[4], ex[3], ey[3], ez[3];
116     Transform3 inv2 = {Transform3_id};
117     Transform3 rel2to1 = {Transform3_id};
118     Transform3 trans3_inverse();
119     Transform3 trans3_prod();
120     int     i;
121 
122     if (cam1 == NULL || cam2 == NULL || a == NULL)
123         return (0);
124     inv2 = trans3_inverse(*(cam2->transf));
125     rel2to1 = trans3_prod(*(cam1->transf), inv2);
126 /*
127     conv_rot_to_quat(&(rel2to1.R), x);
128 */
129     ex[0] = rel2to1.R.el[0][0];
130     ex[1] = rel2to1.R.el[0][1];
131     ex[2] = rel2to1.R.el[0][2];
132     ey[0] = rel2to1.R.el[1][0];
133     ey[1] = rel2to1.R.el[1][1];
134     ey[2] = rel2to1.R.el[1][2];
135     ez[0] = rel2to1.R.el[2][0];
136     ez[1] = rel2to1.R.el[2][1];
137     ez[2] = rel2to1.R.el[2][2];
138     conv_rot_to_quat_pab(ex, ey, ez, x);
139     
140     for (i = 0; i < 3; i++)
141         a[i + 2] = x[i + 1];
142     a[5] = sqrt(rel2to1.t.el[0] * rel2to1.t.el[0]
143                 + rel2to1.t.el[1] * rel2to1.t.el[1]
144                 + rel2to1.t.el[2] * rel2to1.t.el[2]);
145     if (rel2to1.t.el[0] < 0.0)
146         a[5] *= -1.0;
147     a[0] = rel2to1.t.el[1] / a[5];
148     a[1] = rel2to1.t.el[2] / a[5];
149     return (1);
150 }
151 
152 int     store_camera_int(int mask, double *a, Camera * cam)
153 {
154     double *b;
155     int     i = 0;
156 
157     if (cam == NULL || a == NULL)
158         return (0);
159 
160     if (Bit(mask, 0))
161         cam->f = (float)a[i++];
162     if (Bit(mask, 1))
163         cam->cx = (float)a[i++];
164     if (Bit(mask, 2))
165         cam->cy = (float)a[i++];
166     if (Bit(mask, 3))
167         cam->ax = (float)(1.0 / a[i++]);
168     if (Bit(mask, 4))
169         cam->ay = (float)(1.0 / a[i++]);
170     if (Bit(mask, 5) && cam->distort_params != NULL)
171     {
172         b = cam->distort_params;
173         b[1] = a[i++];
174     }
175     return (i);
176 }
177 
178 int     store_camera_ext(double *a, Camera * cam)
179 {
180     void    conv_quat_to_rot();
181     double  x[4], ex[3], ey[3], ez[3];
182     double  kx, ky;
183     double norm;
184     int     i;
185 
186     if (cam == NULL || a == NULL)
187         return (0);
188 
189     for (i = 0; i < 3; i++)
190         x[i + 1] = a[i];
191     norm =  x[1]*x[1] + x[2]*x[2] + x[3]*x[3];
192     if ((x[0] = 1.0 - norm) > 0.00001)
193     {
194         x[0] = sqrt(x[0]);
195     } 
196     else
197     {
198         x[0] = sqrt(0.00001);
199         x[1] = x[1]/sqrt(norm-0.00001);    
200         x[2] = x[2]/sqrt(norm-0.00001);    
201         x[3] = x[3]/sqrt(norm-0.00001);    
202     }
203 
204 /*
205     conv_quat_to_rot(x, &(cam->transf->R));
206 */
207     conv_quat_to_rot_pab(x, ex, ey, ez);
208   
209     cam->transf->R.el[0][0] = ex[0];
210     cam->transf->R.el[0][1] = ex[1];
211     cam->transf->R.el[0][2] = ex[2];
212     cam->transf->R.el[1][0] = ey[0];
213     cam->transf->R.el[1][1] = ey[1];
214     cam->transf->R.el[1][2] = ey[2];
215     cam->transf->R.el[2][0] = ez[0];
216     cam->transf->R.el[2][1] = ez[1];
217     cam->transf->R.el[2][2] = ez[2];
218     
219     cam->transf->t.el[0] = (float)a[3];
220     cam->transf->t.el[1] = (float)a[4];
221     cam->transf->t.el[2] = (float)a[5];
222 
223     kx = cam->ax * cam->f / cam->pixel;
224     ky = cam->ay * cam->f / cam->pixel;
225     cam->cam_to_im = mat3(kx, 0.0, cam->cx,
226                           0.0, ky, cam->cy,
227                           0.0, 0.0, 1.0);
228     cam->im_to_cam = mat3(1.0 / kx, 0.0, -cam->cx / kx,
229                           0.0, 1.0 / ky, -cam->cy / ky,
230                           0.0, 0.0, 1.0);
231     return (1);
232 }
233 
234 int     store_camera_rel(double *a, Camera * cam1, Camera * cam2)
235 {
236     void    conv_quat_to_rot();
237     double  x[4], ex[3], ey[3], ez[3];
238     double  kx, ky, norm;
239     Transform3 invrel = {Transform3_id};
240     Transform3 rel2to1 = {Transform3_id};
241     Transform3 trans3_prod();
242     Transform3 trans3_inverse();
243     int     i;
244 
245     if (cam1 == NULL || cam2 == NULL || a == NULL)
246         return (0);
247 
248     if ((1.0 - a[0] * a[0] - a[1] * a[1]) < 0.0)
249         return (0);
250     for (i = 0; i < 3; i++)
251         x[i + 1] = a[i + 2];
252     if ((x[0] = 1.0 - x[1] * x[1] - x[2] * x[2] - x[3] * x[3]) > 0.00001)
253         x[0] = sqrt(x[0]);
254     else
255     {
256         norm =  x[1]*x[1] + x[2]*x[2] + x[3]*x[3];
257         x[0] = sqrt(0.00001);
258         x[1] = x[1]/sqrt(norm-0.00001);    
259         x[2] = x[2]/sqrt(norm-0.00001);    
260         x[3] = x[3]/sqrt(norm-0.00001);    
261     }
262 /*
263         return (0);
264     conv_quat_to_rot(x, &(rel2to1.R));
265 */
266 
267     conv_quat_to_rot_pab(x, ex, ey, ez);
268 
269     rel2to1.R.el[0][0] = ex[0];
270     rel2to1.R.el[0][1] = ex[1];
271     rel2to1.R.el[0][2] = ex[2];
272     rel2to1.R.el[1][0] = ey[0];
273     rel2to1.R.el[1][1] = ey[1];
274     rel2to1.R.el[1][2] = ey[2];
275     rel2to1.R.el[2][0] = ez[0];
276     rel2to1.R.el[2][1] = ez[1];
277     rel2to1.R.el[2][2] = ez[2];
278 
279     rel2to1.t.el[0] = (float)(sqrt(1.0 - a[0] * a[0] - a[1] * a[1]) * a[5]);
280     rel2to1.t.el[1] = (float)(a[0] * a[5]);
281     rel2to1.t.el[2] = (float)(a[1] * a[5]);
282 
283     invrel = trans3_inverse(rel2to1);
284     *(cam2->transf) = trans3_prod(invrel, *(cam1->transf));
285 
286     kx = cam2->ax * cam2->f / cam2->pixel;
287     ky = cam2->ay * cam2->f / cam2->pixel;
288     cam2->cam_to_im = mat3(kx, 0.0, cam2->cx,
289                            0.0, ky, cam2->cy,
290                            0.0, 0.0, 1.0);
291     cam2->im_to_cam = mat3(1.0 / kx, 0.0, -cam2->cx / kx,
292                            0.0, 1.0 / ky, -cam2->cy / ky,
293                            0.0, 0.0, 1.0);
294     return (1);
295 }
296 

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