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

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

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