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

Linux Cross Reference
Tina6/tina-libs/tina/geometry/geomCam_par.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/geometry/geomCam_par.c,v $
 27  * Date    :  $Date: 2005/01/09 17:49:25 $
 28  * Version :  $Revision: 1.2 $
 29  * CVS Id  :  $Id: geomCam_par.c,v 1.2 2005/01/09 17:49:25 paul Exp $
 30  *
 31  * Author  : Legacy TINA
 32  *
 33  * Notes :
 34  *
 35  *********
 36 */
 37 
 38 #include "geomCam_par.h"
 39 
 40 #if HAVE_CONFIG_H
 41   #include <config.h>
 42 #endif
 43 
 44 #include <math.h>
 45 #include <tina/sys/sysDef.h>
 46 #include <tina/sys/sysPro.h>
 47 #include <tina/math/mathDef.h>
 48 #include <tina/math/mathPro.h>
 49 #include <tina/geometry/geomDef.h>
 50 #include <tina/geometry/geom_CamDef.h>
 51 #include <tina/geometry/geomCam_gen.h>
 52 #include <tina/geometry/geomCam_par_getset.h>
 53 #include <tina/geometry/geomCam_par_proj.h>
 54 
 55 
 56 Parcam *parcam_alloc(unsigned int type)
 57 {
 58     Parcam *pcam = ts_ralloc(Parcam);
 59 
 60     pcam->label = new_label();
 61     pcam->type = type;
 62     pcam->rect1 = pcam->derect1 = pcam->rect2 = pcam->derect2 = mat3_unit();
 63     pcam->I = pcam->f = (float) 1.0;    /* default values */
 64     pcam->pixel = (float) 0.001;        /* notional value */
 65     return (pcam);
 66 }
 67 
 68 /** compute rectification matrix from original image coords to new camera
 69     coords with focal length adjustment (focal lengths in pixel units) **/
 70 /* image center of original camera */
 71 /* aspect ratios of original camera */
 72 static Vec3 rect_vector(Vec3 e, double f, double cx, double cy, double ax, double ay)
 73 {
 74     double   x = vec3_x(e) * ax, y = vec3_y(e) * ay, z = vec3_z(e);
 75 
 76     return (vec3(x, y, z * f - cx * x - cy * y));
 77 }
 78 
 79 /* original and new focal lengths */
 80 /* directions vectors of original camera */
 81 /* direction vectors of new camera */
 82 /* image center of original camera */
 83 /* aspect ratios of original camera */
 84 static Mat3 rect_image_to_cam(double f1, Vec3 ex1, Vec3 ey1, Vec3 ez1,
 85                               double f2, Vec3 ex2, Vec3 ey2, Vec3 ez2,
 86                               double cx, double cy, double ax, double ay)
 87 {
 88     Vec3    ex = {Vec3_id};
 89     Vec3    ey = {Vec3_id};
 90     Vec3    ez = {Vec3_id};
 91 
 92     ex = vec3(vec3_dot(ex2, ex1), vec3_dot(ex2, ey1), vec3_dot(ex2, ez1));
 93     ey = vec3(vec3_dot(ey2, ex1), vec3_dot(ey2, ey1), vec3_dot(ey2, ez1));
 94     ez = vec3(vec3_dot(ez2, ex1), vec3_dot(ez2, ey1), vec3_dot(ez2, ez1));
 95 
 96     return (mat3_of_rows(
 97           vec3_times(f2, rect_vector(ex, f1, cx, cy, 1 / ax, 1 / ay)),
 98           vec3_times(f2, rect_vector(ey, f1, cx, cy, 1 / ax, 1 / ay)),
 99                          rect_vector(ez, f1, cx, cy, 1 / ax, 1 / ay)));
100 }
101 
102 /* Construction of a standard parallel camera geometry */
103 Parcam *parcam_make(Camera * cam1, Camera * cam2, unsigned int type)
104 /* a pair of cameras with respect to same origin */
105 
106 {
107     Vec3    p1 = {Vec3_id};
108     Vec3    ex1 = {Vec3_id};
109     Vec3    ey1 = {Vec3_id};
110     Vec3    ez1 = {Vec3_id};
111     Vec3    p2 = {Vec3_id};
112     Vec3    ex2 = {Vec3_id};
113     Vec3    ey2 = {Vec3_id};
114     Vec3    ez2 = {Vec3_id};
115     Vec3    Px = {Vec3_id};
116     Vec3    Py = {Vec3_id};
117     Vec3    Pz = {Vec3_id};
118     double   f1, f2, fp;                /* focal lengths */
119     int     height, width;
120     Transform3 transf = {Transform3_id};
121     Parcam *pcam = (Parcam *) NULL;
122 
123     if (cam1 == NULL || cam2 == NULL)
124         return (pcam);
125 
126     pcam = parcam_alloc(type);
127     pcam->cam1 = cam_copy(cam1);
128     pcam->cam2 = cam_copy(cam2);
129 
130     pcam->f = cam1->f;
131     pcam->pixel = cam1->pixel;
132 
133     if (cam1->transf == NULL || cam2->transf == NULL)
134         return (pcam);
135 
136     width = cam1->width;
137     height = cam1->height;
138 
139     /* compute camera frames in world coords from transforms */
140     trans3_get_frame2in1(*cam1->transf, &p1, &ex1, &ey1, &ez1);
141     trans3_get_frame2in1(*cam2->transf, &p2, &ex2, &ey2, &ez2);
142 
143     /* construct parallel camera coordinate frame */
144     Px = vec3_diff(p2, p1);     /* Interoccular axis from p1 to p2 */
145     pcam->I = (float) vec3_mod(Px);     /* Interocular separation */
146     Px = vec3_unit(Px);         /* parallel x axis */
147     Pz = vec3_unit(vec3_cross(Px, vec3_cross(ez1, Px)));        /* parallel z axis */
148     Py = vec3_cross(Pz, Px);    /* parallel y axis */
149 
150     /* build left and right hand parallel rectified cameras left camera
151      * focal length, pixel size and dimensions origin at (0, 0) no
152      * aspect ratio */
153     transf = trans3_to_frame(p1, Px, Py, Pz);
154     pcam->rcam1 = cam_make(CAMERA_PARALLEL, &transf, pcam->f, pcam->pixel,
155                            1.0, 1.0, 0.0, 0.0, width, height);
156     pcam->rcam2 = cam_copy(pcam->rcam1);
157     *(pcam->rcam2->transf) = trans3_to_frame(p2, Px, Py, Pz);
158 
159     /* compute rectification transforms between standard and parallel
160      * coords */
161     fp = pcam->f / pcam->pixel; /* focal lengths in pixels */
162     f1 = cam1->f / cam1->pixel;
163     f2 = cam2->f / cam2->pixel;
164     pcam->rect1 = rect_image_to_cam(f1, ex1, ey1, ez1, fp, Px, Py, Pz,
165                               cam1->cx, cam1->cy, cam1->ax, cam1->ay);
166     pcam->derect1 = mat3_inverse(pcam->rect1);
167     pcam->rect2 = rect_image_to_cam(f2, ex2, ey2, ez2, fp, Px, Py, Pz,
168                               cam2->cx, cam2->cy, cam2->ax, cam2->ay);
169     pcam->derect2 = mat3_inverse(pcam->rect2);
170 
171     /** compute E-matrix **/
172     pcam->e = mat3(0.0, 0.0, 0.0,
173                    0.0, 0.0, 1.0,
174                    0.0, -1.0, 0.0);
175     pcam->e = mat3_prod(mat3_transpose(pcam->rect1), pcam->e);
176     pcam->e = mat3_prod(pcam->e, pcam->rect2);
177 
178     return (pcam);
179 }
180 
181 /* A pair of cameras with respect to same origin */
182 Parcam *parcam_scaled_make(Camera * cam1, Camera * cam2, double scale, unsigned int type)
183 
184 /* scale parallel camera image size */
185 
186 {
187     Vec3    p1 = {Vec3_id};
188     Vec3    ex1 = {Vec3_id};
189     Vec3    ey1 = {Vec3_id};
190     Vec3    ez1 = {Vec3_id};
191     Vec3    p2 = {Vec3_id};
192     Vec3    ex2 = {Vec3_id};
193     Vec3    ey2 = {Vec3_id};
194     Vec3    ez2 = {Vec3_id};
195     Vec3    Px = {Vec3_id};
196     Vec3    Py = {Vec3_id};
197     Vec3    Pz = {Vec3_id};
198     double   f1, f2, fp;                /* focal lengths */
199     int     height, width;
200     Transform3 transf = {Transform3_id};
201     Parcam *pcam = (Parcam *) NULL;
202 
203     if (cam1 == NULL || cam2 == NULL)
204         return (pcam);
205 
206     pcam = parcam_alloc(type);
207     pcam->cam1 = cam_copy(cam1);
208     pcam->cam2 = cam_copy(cam2);
209 
210     pcam->f = cam1->f;
211     pcam->pixel = (float) (cam1->pixel / scale);
212 
213     if (cam1->transf == NULL || cam2->transf == NULL)
214         return (pcam);
215 
216     width = (int)(cam1->width * scale);
217     height = (int)(cam1->height * scale);
218 
219     /* compute camera frames in world coords from transforms */
220     trans3_get_frame2in1(*cam1->transf, &p1, &ex1, &ey1, &ez1);
221     trans3_get_frame2in1(*cam2->transf, &p2, &ex2, &ey2, &ez2);
222 
223     /* construct parallel camera coordinate frame */
224     Px = vec3_diff(p2, p1);     /* Interocular axis from p1 to p2 */
225     pcam->I = (float) vec3_mod(Px);     /* Interocular separation */
226     Px = vec3_unit(Px);         /* parallel x axis */
227     Pz = vec3_unit(vec3_cross(Px, vec3_cross(ez1, Px)));        /* parallel z axis */
228     Py = vec3_cross(Pz, Px);    /* parallel y axis */
229 
230     /* build left and right hand parallel rectified cameras left camera
231      * focal length, pixel size and dimensions origin at (0, 0) no
232      * aspect ratio */
233     transf = trans3_to_frame(p1, Px, Py, Pz);
234     pcam->rcam1 = cam_make(CAMERA_PARALLEL, &transf, pcam->f, pcam->pixel,
235                            1.0, 1.0, 0.0, 0.0, width, height);
236     pcam->rcam2 = cam_copy(pcam->rcam1);
237     *(pcam->rcam2->transf) = trans3_to_frame(p2, Px, Py, Pz);
238 
239     /* compute rectification transforms between standard and parallel
240      * coords */
241     fp = pcam->f / pcam->pixel; /* focal lengths in pixels */
242     f1 = cam1->f / cam1->pixel;
243     f2 = cam2->f / cam2->pixel;
244     pcam->rect1 = rect_image_to_cam(f1, ex1, ey1, ez1, fp, Px, Py, Pz,
245                               cam1->cx, cam1->cy, cam1->ax, cam1->ay);
246     pcam->derect1 = mat3_inverse(pcam->rect1);
247     pcam->rect2 = rect_image_to_cam(f2, ex2, ey2, ez2, fp, Px, Py, Pz,
248                               cam2->cx, cam2->cy, cam2->ax, cam2->ay);
249     pcam->derect2 = mat3_inverse(pcam->rect2);
250 
251     /** compute E-matrix **/
252     pcam->e = mat3(0.0, 0.0, 0.0,
253                    0.0, 0.0, 1.0,
254                    0.0, -1.0, 0.0);
255     pcam->e = mat3_prod(mat3_transpose(pcam->rect1), pcam->e);
256     pcam->e = mat3_prod(pcam->e, pcam->rect2);
257 
258     return (pcam);
259 }
260 
261 void    pcam_free(Parcam * pcam)
262 {
263     if (pcam == NULL)
264         return;
265 
266     cam_free(pcam->cam1);
267     cam_free(pcam->rcam1);
268     cam_free(pcam->cam2);
269     cam_free(pcam->rcam2);
270 
271     rfree((void *) pcam);
272 }
273 
274 void     pcam_update(Camera *lcam, Camera *rcam)
275 {
276     Parcam         *pcam;
277 
278     if (lcam == NULL || rcam == NULL)
279     {
280         error("no camera data", non_fatal);
281         return;
282     }
283 
284     pcam = parcam_make(lcam, rcam, CAMERA_PHYSICAL);
285     pcam_set(pcam);
286     set_par_proj(pcam->f / pcam->pixel, pcam->I);
287 }
288 

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