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

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

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

  1 /**@(#)
  2 **/
  3 #include <math.h>
  4 #include <stdio.h>
  5 #include <tina/sys.h>
  6 #include <tina/math.h>
  7 #include <tina/mathfuncs.h>
  8 #include <tina/vision.h>
  9 #include <tina/visionfuncs.h>
 10 
 11 #define Bit(mask,num)     (mask&(1<<num))
 12 
 13 int     conv_camera_int(int mask, Camera * cam, double *a)
 14 {
 15     int     i = 0;
 16     double *b;
 17 
 18     if (cam == NULL || a == NULL)
 19         return (0);
 20 
 21     if (Bit(mask, 0))
 22         a[i++] = (double) cam->f;
 23     if (Bit(mask, 1))
 24         a[i++] = (double) cam->cx;
 25     if (Bit(mask, 2))
 26         a[i++] = (double) cam->cy;
 27     if (Bit(mask, 3))
 28         a[i++] = 1.0 / (double) cam->ax;
 29     if (Bit(mask, 4))
 30         a[i++] = 1.0 / (double) cam->ay;
 31     if (Bit(mask, 5) && cam->distort_params != NULL)
 32     {
 33         b = cam->distort_params;
 34         a[i++] = b[1];
 35     }
 36     return (i);
 37 }
 38 
 39 int     conv_camera_ext(Camera * cam, double *a)
 40 {
 41     double  x[4];
 42     int     i;
 43 
 44     if (cam == NULL || a == NULL)
 45         return (0);
 46 
 47     conv_rot_to_quat(&(cam->transf->R), x);
 48     for (i = 0; i < 3; i++)
 49         a[i] = x[i + 1];
 50     a[3] = cam->transf->t.el[0];
 51     a[4] = cam->transf->t.el[1];
 52     a[5] = cam->transf->t.el[2];
 53 
 54     return (1);
 55 }
 56 
 57 int     conv_camera_rel(Camera * cam1, Camera * cam2, double *a)
 58 {
 59     double  x[4];
 60     Transform3 inv2 = {Transform3_id};
 61     Transform3 rel2to1 = {Transform3_id};
 62     Transform3 trans3_inverse();
 63     Transform3 trans3_prod();
 64     int     i;
 65 
 66     if (cam1 == NULL || cam2 == NULL || a == NULL)
 67         return (0);
 68     inv2 = trans3_inverse(*(cam2->transf));
 69     rel2to1 = trans3_prod(*(cam1->transf), inv2);
 70     conv_rot_to_quat(&(rel2to1.R), x);
 71     for (i = 0; i < 3; i++)
 72         a[i + 2] = x[i + 1];
 73     a[5] = sqrt(rel2to1.t.el[0] * rel2to1.t.el[0]
 74                 + rel2to1.t.el[1] * rel2to1.t.el[1]
 75                 + rel2to1.t.el[2] * rel2to1.t.el[2]);
 76     if (rel2to1.t.el[0] < 0.0)
 77         a[5] *= -1.0;
 78     a[0] = rel2to1.t.el[1] / a[5];
 79     a[1] = rel2to1.t.el[2] / a[5];
 80     return (1);
 81 }
 82 
 83 int     store_camera_int(int mask, double *a, Camera * cam)
 84 {
 85     double *b;
 86     int     i = 0;
 87 
 88     if (cam == NULL || a == NULL)
 89         return (0);
 90 
 91     if (Bit(mask, 0))
 92         cam->f = (float)a[i++];
 93     if (Bit(mask, 1))
 94         cam->cx = (float)a[i++];
 95     if (Bit(mask, 2))
 96         cam->cy = (float)a[i++];
 97     if (Bit(mask, 3))
 98         cam->ax = (float)(1.0 / a[i++]);
 99     if (Bit(mask, 4))
100         cam->ay = (float)(1.0 / a[i++]);
101     if (Bit(mask, 5) && cam->distort_params != NULL)
102     {
103         b = cam->distort_params;
104         b[1] = a[i++];
105     }
106     return (i);
107 }
108 
109 int     store_camera_ext(double *a, Camera * cam)
110 {
111     void    conv_quat_to_rot();
112     double  x[4];
113     double  kx, ky;
114     int     i;
115 
116     if (cam == NULL || a == NULL)
117         return (0);
118 
119     for (i = 0; i < 3; i++)
120         x[i + 1] = a[i];
121     if ((x[0] = 1.0 - x[1] * x[1] - x[2] * x[2] - x[3] * x[3]) > 0.0)
122         x[0] = sqrt(x[0]);
123     else
124         return (0);
125 
126     conv_quat_to_rot(x, &(cam->transf->R));
127     cam->transf->t.el[0] = (float)a[3];
128     cam->transf->t.el[1] = (float)a[4];
129     cam->transf->t.el[2] = (float)a[5];
130 
131     kx = cam->ax * cam->f / cam->pixel;
132     ky = cam->ay * cam->f / cam->pixel;
133     cam->cam_to_im = mat3(kx, 0.0, cam->cx,
134                           0.0, ky, cam->cy,
135                           0.0, 0.0, 1.0);
136     cam->im_to_cam = mat3(1.0 / kx, 0.0, -cam->cx / kx,
137                           0.0, 1.0 / ky, -cam->cy / ky,
138                           0.0, 0.0, 1.0);
139     return (1);
140 }
141 
142 int     store_camera_rel(double *a, Camera * cam1, Camera * cam2)
143 {
144     void    conv_quat_to_rot();
145     double  x[4];
146     double  kx, ky;
147     Transform3 invrel = {Transform3_id};
148     Transform3 rel2to1 = {Transform3_id};
149     Transform3 trans3_prod();
150     Transform3 trans3_inverse();
151     int     i;
152 
153     if (cam1 == NULL || cam2 == NULL || a == NULL)
154         return (0);
155 
156     if ((1.0 - a[0] * a[0] - a[1] * a[1]) < 0.0)
157         return (0);
158     for (i = 0; i < 3; i++)
159         x[i + 1] = a[i + 2];
160     if ((x[0] = 1.0 - x[1] * x[1] - x[2] * x[2] - x[3] * x[3]) > 0.0)
161         x[0] = sqrt(x[0]);
162     else
163         return (0);
164     conv_quat_to_rot(x, &(rel2to1.R));
165     rel2to1.t.el[0] = (float)(sqrt(1.0 - a[0] * a[0] - a[1] * a[1]) * a[5]);
166     rel2to1.t.el[1] = (float)(a[0] * a[5]);
167     rel2to1.t.el[2] = (float)(a[1] * a[5]);
168 
169     invrel = trans3_inverse(rel2to1);
170     *(cam2->transf) = trans3_prod(invrel, *(cam1->transf));
171 
172     kx = cam2->ax * cam2->f / cam2->pixel;
173     ky = cam2->ay * cam2->f / cam2->pixel;
174     cam2->cam_to_im = mat3(kx, 0.0, cam2->cx,
175                            0.0, ky, cam2->cy,
176                            0.0, 0.0, 1.0);
177     cam2->im_to_cam = mat3(1.0 / kx, 0.0, -cam2->cx / kx,
178                            0.0, 1.0 / ky, -cam2->cy / ky,
179                            0.0, 0.0, 1.0);
180     return (1);
181 }
182 

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