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

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

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