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
This page was automatically generated by the
LXR engine.
Visit the LXR main site for more
information.