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

Linux Cross Reference
Tina4/src/vision/camera/par_cam.c

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

  1 /**@(#)
  2 **/
  3 #include <tina/sys.h>
  4 #include <tina/sysfuncs.h>
  5 #include <tina/math.h>
  6 #include <tina/mathfuncs.h>
  7 #include <tina/vision.h>
  8 #include <tina/visionfuncs.h>
  9 
 10 Parcam *parcam_alloc(unsigned int type)
 11 {
 12     Parcam *pcam = ts_ralloc(Parcam);
 13 
 14     pcam->label = new_label();
 15     pcam->type = type;
 16     pcam->rect1 = pcam->derect1 = pcam->rect2 = pcam->derect2 = mat3_unit();
 17     pcam->I = pcam->f = (float) 1.0;    /* default values */
 18     pcam->pixel = (float) 0.001;        /* notional value */
 19     return (pcam);
 20 }
 21 
 22 /** compute rectification matrix from original image coords to new camera
 23     coords with focal length adjustment (focal lengths in pixel units) **/
 24 /* image center of original camera */
 25 /* aspect ratios of original camera */
 26 static Vec3 rect_vector(Vec3 e, double f, double cx, double cy, double ax, double ay)
 27 {
 28     double   x = vec3_x(e) * ax, y = vec3_y(e) * ay, z = vec3_z(e);
 29 
 30     return (vec3(x, y, z * f - cx * x - cy * y));
 31 }
 32 
 33 /* original and new focal lengths */
 34 /* directions vectors of original camera */
 35 /* direction vectors of new camera */
 36 /* image center of original camera */
 37 /* aspect ratios of original camera */
 38 static Mat3 rect_image_to_cam(double f1, Vec3 ex1, Vec3 ey1, Vec3 ez1,
 39                               double f2, Vec3 ex2, Vec3 ey2, Vec3 ez2,
 40                               double cx, double cy, double ax, double ay)
 41 {
 42     Vec3    ex = {Vec3_id};
 43     Vec3    ey = {Vec3_id};
 44     Vec3    ez = {Vec3_id};
 45 
 46     ex = vec3(vec3_dot(ex2, ex1), vec3_dot(ex2, ey1), vec3_dot(ex2, ez1));
 47     ey = vec3(vec3_dot(ey2, ex1), vec3_dot(ey2, ey1), vec3_dot(ey2, ez1));
 48     ez = vec3(vec3_dot(ez2, ex1), vec3_dot(ez2, ey1), vec3_dot(ez2, ez1));
 49 
 50     return (mat3_of_rows(
 51           vec3_times(f2, rect_vector(ex, f1, cx, cy, 1 / ax, 1 / ay)),
 52           vec3_times(f2, rect_vector(ey, f1, cx, cy, 1 / ax, 1 / ay)),
 53                          rect_vector(ez, f1, cx, cy, 1 / ax, 1 / ay)));
 54 }
 55 
 56 /* Construction of a standard parallel camera geometry */
 57 Parcam *parcam_make(Camera * cam1, Camera * cam2, unsigned int type)
 58 /* a pair of cameras with respect to same origin */
 59 
 60 {
 61     Vec3    p1 = {Vec3_id};
 62     Vec3    ex1 = {Vec3_id};
 63     Vec3    ey1 = {Vec3_id};
 64     Vec3    ez1 = {Vec3_id};
 65     Vec3    p2 = {Vec3_id};
 66     Vec3    ex2 = {Vec3_id};
 67     Vec3    ey2 = {Vec3_id};
 68     Vec3    ez2 = {Vec3_id};
 69     Vec3    Px = {Vec3_id};
 70     Vec3    Py = {Vec3_id};
 71     Vec3    Pz = {Vec3_id};
 72     double   f1, f2, fp;                /* focal lengths */
 73     int     height, width;
 74     Transform3 transf = {Transform3_id};
 75     Parcam *pcam = (Parcam *) NULL;
 76 
 77     if (cam1 == NULL || cam2 == NULL)
 78         return (pcam);
 79 
 80     pcam = parcam_alloc(type);
 81     pcam->cam1 = cam_copy(cam1);
 82     pcam->cam2 = cam_copy(cam2);
 83 
 84     pcam->f = cam1->f;
 85     pcam->pixel = cam1->pixel;
 86 
 87     if (cam1->transf == NULL || cam2->transf == NULL)
 88         return (pcam);
 89 
 90     width = cam1->width;
 91     height = cam1->height;
 92 
 93     /* compute camera frames in world coords from transforms */
 94     trans3_get_frame2in1(*cam1->transf, &p1, &ex1, &ey1, &ez1);
 95     trans3_get_frame2in1(*cam2->transf, &p2, &ex2, &ey2, &ez2);
 96 
 97     /* construct parallel camera coordinate frame */
 98     Px = vec3_diff(p2, p1);     /* Interoccular axis from p1 to p2 */
 99     pcam->I = (float) vec3_mod(Px);     /* Interocular separation */
100     Px = vec3_unit(Px);         /* parallel x axis */
101     Pz = vec3_unit(vec3_cross(Px, vec3_cross(ez1, Px)));        /* parallel z axis */
102     Py = vec3_cross(Pz, Px);    /* parallel y axis */
103 
104     /* build left and right hand parallel rectified cameras left camera
105      * focal length, pixel size and dimensions origin at (0, 0) no
106      * aspect ratio */
107     transf = trans3_to_frame(p1, Px, Py, Pz);
108     pcam->rcam1 = cam_make(CAMERA_PARALLEL, &transf, pcam->f, pcam->pixel,
109                            1.0, 1.0, 0.0, 0.0, width, height);
110     pcam->rcam2 = cam_copy(pcam->rcam1);
111     *(pcam->rcam2->transf) = trans3_to_frame(p2, Px, Py, Pz);
112 
113     /* compute rectification transforms between standard and parallel
114      * coords */
115     fp = pcam->f / pcam->pixel; /* focal lengths in pixels */
116     f1 = cam1->f / cam1->pixel;
117     f2 = cam2->f / cam2->pixel;
118     pcam->rect1 = rect_image_to_cam(f1, ex1, ey1, ez1, fp, Px, Py, Pz,
119                               cam1->cx, cam1->cy, cam1->ax, cam1->ay);
120     pcam->derect1 = mat3_inverse(pcam->rect1);
121     pcam->rect2 = rect_image_to_cam(f2, ex2, ey2, ez2, fp, Px, Py, Pz,
122                               cam2->cx, cam2->cy, cam2->ax, cam2->ay);
123     pcam->derect2 = mat3_inverse(pcam->rect2);
124 
125     /** compute E-matrix **/
126     pcam->e = mat3(0.0, 0.0, 0.0,
127                    0.0, 0.0, 1.0,
128                    0.0, -1.0, 0.0);
129     pcam->e = mat3_prod(mat3_transpose(pcam->rect1), pcam->e);
130     pcam->e = mat3_prod(pcam->e, pcam->rect2);
131 
132     return (pcam);
133 }
134 
135 /* A pair of cameras with respect to same origin */
136 Parcam *parcam_scaled_make(Camera * cam1, Camera * cam2, double scale, unsigned int type)
137 
138 /* scale parallel camera image size */
139 
140 {
141     Vec3    p1 = {Vec3_id};
142     Vec3    ex1 = {Vec3_id};
143     Vec3    ey1 = {Vec3_id};
144     Vec3    ez1 = {Vec3_id};
145     Vec3    p2 = {Vec3_id};
146     Vec3    ex2 = {Vec3_id};
147     Vec3    ey2 = {Vec3_id};
148     Vec3    ez2 = {Vec3_id};
149     Vec3    Px = {Vec3_id};
150     Vec3    Py = {Vec3_id};
151     Vec3    Pz = {Vec3_id};
152     double   f1, f2, fp;                /* focal lengths */
153     int     height, width;
154     Transform3 transf = {Transform3_id};
155     Parcam *pcam = (Parcam *) NULL;
156 
157     if (cam1 == NULL || cam2 == NULL)
158         return (pcam);
159 
160     pcam = parcam_alloc(type);
161     pcam->cam1 = cam_copy(cam1);
162     pcam->cam2 = cam_copy(cam2);
163 
164     pcam->f = cam1->f;
165     pcam->pixel = (float) (cam1->pixel / scale);
166 
167     if (cam1->transf == NULL || cam2->transf == NULL)
168         return (pcam);
169 
170     width = (int)(cam1->width * scale);
171     height = (int)(cam1->height * scale);
172 
173     /* compute camera frames in world coords from transforms */
174     trans3_get_frame2in1(*cam1->transf, &p1, &ex1, &ey1, &ez1);
175     trans3_get_frame2in1(*cam2->transf, &p2, &ex2, &ey2, &ez2);
176 
177     /* construct parallel camera coordinate frame */
178     Px = vec3_diff(p2, p1);     /* Interocular axis from p1 to p2 */
179     pcam->I = (float) vec3_mod(Px);     /* Interocular separation */
180     Px = vec3_unit(Px);         /* parallel x axis */
181     Pz = vec3_unit(vec3_cross(Px, vec3_cross(ez1, Px)));        /* parallel z axis */
182     Py = vec3_cross(Pz, Px);    /* parallel y axis */
183 
184     /* build left and right hand parallel rectified cameras left camera
185      * focal length, pixel size and dimensions origin at (0, 0) no
186      * aspect ratio */
187     transf = trans3_to_frame(p1, Px, Py, Pz);
188     pcam->rcam1 = cam_make(CAMERA_PARALLEL, &transf, pcam->f, pcam->pixel,
189                            1.0, 1.0, 0.0, 0.0, width, height);
190     pcam->rcam2 = cam_copy(pcam->rcam1);
191     *(pcam->rcam2->transf) = trans3_to_frame(p2, Px, Py, Pz);
192 
193     /* compute rectification transforms between standard and parallel
194      * coords */
195     fp = pcam->f / pcam->pixel; /* focal lengths in pixels */
196     f1 = cam1->f / cam1->pixel;
197     f2 = cam2->f / cam2->pixel;
198     pcam->rect1 = rect_image_to_cam(f1, ex1, ey1, ez1, fp, Px, Py, Pz,
199                               cam1->cx, cam1->cy, cam1->ax, cam1->ay);
200     pcam->derect1 = mat3_inverse(pcam->rect1);
201     pcam->rect2 = rect_image_to_cam(f2, ex2, ey2, ez2, fp, Px, Py, Pz,
202                               cam2->cx, cam2->cy, cam2->ax, cam2->ay);
203     pcam->derect2 = mat3_inverse(pcam->rect2);
204 
205     /** compute E-matrix **/
206     pcam->e = mat3(0.0, 0.0, 0.0,
207                    0.0, 0.0, 1.0,
208                    0.0, -1.0, 0.0);
209     pcam->e = mat3_prod(mat3_transpose(pcam->rect1), pcam->e);
210     pcam->e = mat3_prod(pcam->e, pcam->rect2);
211 
212     return (pcam);
213 }
214 
215 void    pcam_free(Parcam * pcam)
216 {
217     if (pcam == NULL)
218         return;
219 
220     cam_free(pcam->cam1);
221     cam_free(pcam->rcam1);
222     cam_free(pcam->cam2);
223     cam_free(pcam->rcam2);
224 
225     rfree((void *) pcam);
226 }
227 
228 void     pcam_update(Camera *lcam, Camera *rcam)
229 {
230     Parcam         *pcam;
231 
232     if (lcam == NULL || rcam == NULL)
233     {
234         error("no camera data", non_fatal);
235         return;
236     }
237 
238     pcam = parcam_make(lcam, rcam, CAMERA_PHYSICAL);
239     pcam_set(pcam);
240     set_par_proj(pcam->f / pcam->pixel, pcam->I);
241 }
242 

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