bazar  1.3.1
projection_matrix.cpp
Go to the documentation of this file.
1 /*
2 Copyright 2005, 2006 Computer Vision Lab,
3 Ecole Polytechnique Federale de Lausanne (EPFL), Switzerland.
4 All rights reserved.
5 
6 This file is part of BazAR.
7 
8 BazAR is free software; you can redistribute it and/or modify it under the
9 terms of the GNU General Public License as published by the Free Software
10 Foundation; either version 2 of the License, or (at your option) any later
11 version.
12 
13 BazAR is distributed in the hope that it will be useful, but WITHOUT ANY
14 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
15 PARTICULAR PURPOSE. See the GNU General Public License for more details.
16 
17 You should have received a copy of the GNU General Public License along with
18 BazAR; if not, write to the Free Software Foundation, Inc., 51 Franklin
19 Street, Fifth Floor, Boston, MA 02110-1301, USA
20 */
21 #include <cmath>
22 #include <iostream>
23 #include <fstream>
24 #include <float.h>
25 
26 #include <math/linear_algebra.h>
27 #include "projection_matrix.h"
28 
29 using namespace std;
30 
31 projection_matrix::projection_matrix(void)
32 {
33  planes[0] = 0;
34  planes[1] = 0;
35  planes[2] = 100;
36  planes[3] = 0;
37  planes[4] = -100;
38  planes[5] = 10000;
39 }
40 
41 projection_matrix::~projection_matrix()
42 {
43 }
44 
45 projection_matrix &projection_matrix::operator=(const projection_matrix &copied)
46 {
47  cx = copied.cx;
48  cy = copied.cy;
49  fx = copied.fx;
50  fy = copied.fy;
51 
52  image_width = copied.image_width;
53  image_height = copied.image_height;
54 
55  original_cx = copied.original_cx;
56  original_cy = copied.original_cy;
57  original_fx = copied.original_fx;
58  original_fy = copied.original_fy;
59 
60  original_image_width = copied.original_image_width;
61  original_image_height = copied.original_image_height;
62 
63  gfla_copy_3x3(copied.R, R);
64  gfla_copy_3(copied.T, T);
65 
66  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
67 
68  return *this;
69 }
70 
71 void projection_matrix::getAAt(const double P[3][4], double AAt[3][3])
72 {
73  double t7 = P[0][0]*P[0][0];
74  double t8 = P[0][1]*P[0][1];
75  double t9 = P[0][2]*P[0][2];
76  double t14 = P[0][0]*P[1][0]+P[0][1]*P[1][1]+P[0][2]*P[1][2];
77  double t18 = P[0][0]*P[2][0]+P[0][1]*P[2][1]+P[0][2]*P[2][2];
78  double t19 = P[1][0]*P[1][0];
79  double t20 = P[1][1]*P[1][1];
80  double t21 = P[1][2]*P[1][2];
81  double t26 = P[1][0]*P[2][0]+P[1][1]*P[2][1]+P[1][2]*P[2][2];
82  double t27 = P[2][0]*P[2][0];
83  double t28 = P[2][1]*P[2][1];
84  double t29 = P[2][2]*P[2][2];
85  AAt[0][0] = t7+t8+t9;
86  AAt[0][1] = t14;
87  AAt[0][2] = t18;
88  AAt[1][0] = t14;
89  AAt[1][1] = t19+t20+t21;
90  AAt[1][2] = t26;
91  AAt[2][0] = t18;
92  AAt[2][1] = t26;
93  AAt[2][2] = t27+t28+t29;
94 }
95 
96 void projection_matrix::set_3x4_matrix(double P[3][4], int w, int h)
97 {
98  image_width = w;
99  image_height = h;
100 
101  double inv_l = sqrt(P[2][0] * P[2][0] + P[2][1] * P[2][1] + P[2][2] * P[2][2]);
102  for(int i = 0; i < 3; i++)
103  for(int j = 0; j < 4; j++)
104  P[i][j] = inv_l * P[i][j];
105 
106  R[2][0] = P[2][0];
107  R[2][1] = P[2][1];
108  R[2][2] = P[2][2];
109  T[2] = P[2][3];
110 
111  double AAt[3][3];
112  getAAt(P, AAt);
113  cx = AAt[2][0];
114  cy = AAt[2][1];
115  fy = sqrt(AAt[1][1] - cy * cy);
116  double s = (AAt[1][0] - cx * cy) / fy;
117  fx = sqrt(AAt[0][0] - cx * cx - s * s);
118 
119  R[1][0] = (P[1][0] - cy * R[2][0]) / fy;
120  R[0][0] = (P[0][0] - s * R[1][0] - cx * R[2][0]) / fx;
121 
122  R[1][1] = (P[1][1] - cy * R[2][1]) / fy;
123  R[0][1] = (P[0][1] - s * R[1][1] - cx * R[2][1]) / fx;
124 
125  R[1][2] = (P[1][2] - cy * R[2][2]) / fy;
126  R[0][2] = (P[0][2] - s * R[1][2] - cx * R[2][2]) / fx;
127 
128  T[1] = (P[1][3] - cy * T[2]) / fy;
129  T[0] = (P[0][3] - s * T[1] - cx * T[2]) / fx;
130 
131  if (gfla_det(R) < 0) {
132  fy = -fy; // ??
133  for(int i = 0; i < 3; i++)
134  R[1][i] = -R[1][i];
135  T[1] = -T[1];
136  }
137 
138  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
139 }
140 
146 bool projection_matrix::load_tdir(const char * tdir_filename, int w, int h)
147 {
148  ifstream file;
149  file.open(tdir_filename);
150 
151  if (!file.good()) return false;
152  double P[3][4];
153 
154  for (int j=0; j<3; ++j)
155  for (int i=0; i<4; ++i)
156  file >> P[j][i];
157  file.close();
158  set_3x4_matrix(P, w, h);
159 
160  return true;
161 }
162 
163 static double diag(double x, double y)
164 {
165  return sqrt(x*x+y*y);
166 }
167 
168 static double diagDiff(const int W, const int H, double dx, double dy)
169 {
170  double mdiag = diag(double(W), double(H));
171  return fabs(double(mdiag - diag(2 * dx, 2 * dy)));
172 }
173 
181 bool projection_matrix::load_tdir(const char * tdir_filename)
182 {
183  if (!load_tdir(tdir_filename, 0, 0)) return false;
184 
185  static const int modes[] = {
186  640, 480,
187  320, 240,
188  720, 576,
189  360, 288,
190  800, 600,
191  1024, 768,
192  512, 384,
193  1280, 1024,
194  -1, -1
195  };
196 
197  int best = 0;
198  double bestDelta = diagDiff(modes[0], modes[1], cx, cy);
199  for(int i = 1; modes[2 * i] != -1; i++) {
200  double delta = diagDiff(modes[2 * i], modes[2 * i + 1], cx, cy);
201  if (delta < bestDelta) {
202  best = i;
203  bestDelta = delta;
204  }
205  }
206 
207  image_width = modes[2 * best];
208  image_height = modes[best + 1];
209 
210  return true;
211 }
212 
213 void projection_matrix::load(FILE * f)
214 {
215  char calibration_name[500];
216  int dummy;
217 
218  fscanf(f,"%s", calibration_name);
219 
220  fscanf(f, "%d", &dummy);
221 
222  if (dummy != -1) // To correct incompatibility with older versions of keyframes
223  fscanf(f, "%d", &dummy);
224 
225  read_internal_parameters_from_tdir_file(calibration_name);
226 
227  fscanf(f, "%lf %lf %lf", &(optical_centre[0]), &(optical_centre[1]), &(optical_centre[2]));
228  fscanf(f, "%lf %lf %lf", &(R[0][0]), &(R[0][1]), &(R[0][2]));
229  fscanf(f, "%lf %lf %lf", &(R[1][0]), &(R[1][1]), &(R[1][2]));
230  fscanf(f, "%lf %lf %lf", &(R[2][0]), &(R[2][1]), &(R[2][2]));
231  fscanf(f, "%lf %lf %lf", &(T[0]), &(T[1]), &(T[2]));
232 
233  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
234 }
235 
236 void projection_matrix::read_one_line(FILE * f, char * line)
237 {
238  int n = 0;
239  char c;
240 
241  do
242  {
243  fscanf(f, "%c", &c);
244  line[n] = c;
245  n++;
246  } while (c != '\n');
247  n = n - 2; // Skip the (char)13 and the (char)10:
248  line[n] = '\0';
249 }
250 
251 FILE * projection_matrix::open_matchmover_output(char * filename)
252 {
253  FILE * f = fopen(filename, "r");
254  char line[1000];
255 
256  if (f == NULL)
257  return NULL;
258 
259  do {
260  read_one_line(f, line);
261  } while (strcmp(line, "Camera") != 0);
262 
263  read_one_line(f, line);
264 
265  return f;
266 }
267 
268 void projection_matrix::close_matchmover_output(FILE * f)
269 {
270  fclose(f);
271 }
272 
273 bool projection_matrix::read_from_matchmover_output(FILE * f)
274 {
275  int camera_index;
276  double focale_length, pixel_ratio, principal_point_u, principal_point_v, K;
277  double Oc[3], local_R[3][3], local_t[3];
278 
279  if (fscanf(f, "\t%d", &camera_index) != 1)
280  {
281  cerr << "Error while reading matchmover output (1)." << endl;
282  return false;
283  }
284 
285  if (fscanf(f, "\t F ( %lf ) Pr ( %lf ) Pp ( %lf %lf ) K ( %lf )",
286  &focale_length, &pixel_ratio, &principal_point_u, &principal_point_v, &K) != 5)
287  {
288  cerr << "Error while reading matchmover output (2)." << endl;
289  return false;
290  }
291 
292  if (fscanf(f, "\tOc ( %lf %lf %lf )", &(Oc[0]), &(Oc[1]), &(Oc[2])) != 3)
293  {
294  cerr << "Error while reading matchmover output (3)." << endl;
295  return false;
296  }
297 
298  if (fscanf(f, "\tRot ( %lf %lf %lf", &(local_R[0][0]), &(local_R[0][1]), &(local_R[0][2])) != 3)
299  {
300  cerr << "Error while reading matchmover output (4)." << endl;
301  return false;
302  }
303 
304  if (fscanf(f, "%lf %lf %lf", &(local_R[1][0]), &(local_R[1][1]), &(local_R[1][2])) != 3)
305  {
306  cerr << "Error while reading matchmover output (5)." << endl;
307  return false;
308  }
309 
310  if (fscanf(f, " %lf %lf %lf )", &(local_R[2][0]), &(local_R[2][1]), &(local_R[2][2])) != 3)
311  {
312  cerr << "Error while reading matchmover output (6)." << endl;
313  return false;
314  }
315 
316  set_original_internal_parameters(int(2 * principal_point_u), int(2 * principal_point_v), // image size
317  focale_length, pixel_ratio * focale_length,
318  principal_point_u, principal_point_v);
319 
320  gfla_mul_mat_vect_3x3(local_R, Oc, local_t);
321  gfla_opp_3(local_t);
322 
323  set_external_parameters(local_R, local_t);
324 
325  return true;
326 }
327 
328 ostream& operator<< (ostream& o, const projection_matrix& P)
329 {
330  o << "[" << P.fx << "\t" << 0 << "\t" << P.cx << "\t][" << P.R[0][0] << "\t" << P.R[0][1] << "\t" << P.R[0][2] << " | " << P.T[0] << " ]" << endl;
331  o << "[" << 0 << "\t" << P.fy << "\t" << P.cy << "\t][" << P.R[1][0] << "\t" << P.R[1][1] << "\t" << P.R[1][2] << " | " << P.T[1] << " ]" << endl;
332  o << "[" << 0 << "\t" << 0 << "\t" << 1 << "\t][" << P.R[2][0] << "\t" << P.R[2][1] << "\t" << P.R[2][2] << " | " << P.T[2] << " ]" << endl;
333 
334  return o;
335 }
336 
337 void projection_matrix::print(FILE * file, char * calibration_filename)
338 {
339  fprintf(file, "%s\n", calibration_filename);
340  fprintf(file, "0\n"); // Compatibility reasons
341  fprintf(file, "-1\n"); // Idem
342 
343  if (have_to_recompute_optical_centre)
344  compute_optical_centre();
345 
346  fprintf(file, "%lf %lf %lf ", optical_centre[0], optical_centre[1], optical_centre[2]);
347  fprintf(file, "%lf %lf %lf ", R[0][0], R[0][1], R[0][2]);
348  fprintf(file, "%lf %lf %lf ", R[1][0], R[1][1], R[1][2]);
349  fprintf(file, "%lf %lf %lf ", R[2][0], R[2][1], R[2][2]);
350  fprintf(file, "%lf %lf %lf ", T[0], T[1], T[2]);
351 }
352 
354 // Internal parameters:
355 
356 void projection_matrix::set_original_internal_parameters(int _image_width, int _image_height,
357  double _fx, double _fy, double _cx, double _cy)
358 {
359  image_width = original_image_width = _image_width;
360  image_height = original_image_height = _image_height;
361 
362  fx = original_fx = _fx;
363  fy = original_fy = _fy;
364  cx = original_cx = _cx;
365  cy = original_cy = _cy;
366 
367  have_to_recompute_invAR = have_to_recompute_P = true;
368 }
369 
370 void projection_matrix::set_original_internal_parameters(double _fx, double _fy, double _cx, double _cy)
371 {
372  fx = original_fx = _fx;
373  fy = original_fy = _fy;
374  cx = original_cx = _cx;
375  cy = original_cy = _cy;
376 
377  have_to_recompute_invAR = have_to_recompute_P = true;
378 }
379 
380 void projection_matrix::change_image_size(int new_width, int new_height)
381 {
382  image_width = new_width;
383  image_height = new_height;
384 
385  double sx = (double)new_width / original_image_width;
386  double sy = (double)new_height / original_image_height;
387 
388  fx = sx * original_fx;
389  cx = sx * original_cx;
390  fy = sy * original_fy;
391  cy = sy * original_cy;
392 
393  have_to_recompute_invAR = have_to_recompute_P = true;
394 }
395 
396 bool projection_matrix::read_internal_parameters_from_tdir_file(const char * tdir_filename)
397 {
398  double tdir_mat[12];
399 
400  FILE * tdir_file = fopen(tdir_filename, "r");
401  if (tdir_file == 0)
402  {
403  perror(tdir_filename);
404  return false;
405  }
406 
407  for(int i = 0; i < 12 ; i++)
408  {
409  if (fscanf(tdir_file, "%lf", &tdir_mat[i]) == EOF)
410  {
411  cout<<"Error loading file (corrupt or wrong format)"<<endl;
412  fclose(tdir_file);
413  return false;
414  }
415  }
416  fclose(tdir_file);
417 
418  original_fx = fx = tdir_mat[0];
419  original_fy = fy = tdir_mat[5];
420  original_cx = cx = tdir_mat[2];
421  original_cy = cy = tdir_mat[6];
422 
423  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
424 
425  return true;
426 }
427 
429 // External parameters:
430 
431 void projection_matrix::set_camera_centre_and_lookat_point(double Cx, double Cy, double Cz,
432  double Lx, double Ly, double Lz,
433  double angle_vertical)
434 {
435  double rli1[3], rli2[3], rli3[3];
436  double k[3];
437 
438  rli3[0] = Lx - Cx;
439  rli3[1] = Ly - Cy;
440  rli3[2] = Lz - Cz;
441  gfla_normalize_3(rli3);
442 
443  k[0] = sin(angle_vertical) * rli3[1];
444  k[1] = sin(angle_vertical) * rli3[0];
445  k[2] = cos(angle_vertical);
446  gfla_cross_product(rli3, k, rli1);
447 
448  gfla_cross_product(rli3, rli1, rli2);
449 
450  for(int i = 0; i < 3; i++)
451  {
452  R[0][i] = rli1[i];
453  R[1][i] = rli2[i];
454  R[2][i] = rli3[i];
455  }
456 
457  for(int i = 0; i < 3; i++)
458  T[i] = -(R[i][0] * Cx + R[i][1] * Cy + R[i][2] * Cz);
459 
460  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
461 }
462 
463 void projection_matrix::set_external_parameters(double omega, double phi, double kappa,
464  double Tx, double Ty, double Tz)
465 {
466  gfla_get_rotation_from_euler_angles(R, omega, phi, kappa);
467 
468  T[0] = Tx;
469  T[1] = Ty;
470  T[2] = Tz;
471 
472  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
473 }
474 
475 void projection_matrix::set_external_parameters(double * state)
476 {
477  set_external_parameters(state[0], state[1], state[2], state[3], state[4], state[5]);
478 }
479 
480 void projection_matrix::set_external_parameters_exp_map(double * state, CvMat * JR)
481 {
482  double * om = state;
483  double * t = state + 3;
484 
485  double length = sqrt(om[0] * om[0] + om[1] * om[1] + om[2] * om[2]);
486 
487  if (length > (2 * 3.14159 - 1))
488  {
489  double new_length = fmod(length, 2 * 3.14159);
490  double r = new_length / length;
491  om[0] *= r;
492  om[1] *= r;
493  om[2] *= r;
494  }
495 
496  // make the rotation matrix
497  double rm[9], rv[3];
498  CvMat rotMat = cvMat(3, 3, CV_64FC1, rm);
499  CvMat rotVec = cvMat(1, 3, CV_64FC1, rv);
500 
501  cvSetReal1D(&rotVec, 0, om[0]);
502  cvSetReal1D(&rotVec, 1, om[1]);
503  cvSetReal1D(&rotVec, 2, om[2]);
504 
505  // Compute the rotation matrix
506  cvRodrigues(&rotMat, &rotVec, JR, CV_RODRIGUES_V2M);
507 
508  // complete the rotation matrix with translation
509  for(int i = 0; i < 3; i++)
510  {
511  for(int j = 0; j < 3; j++)
512  R[i][j] = cvmGet(&rotMat, i, j);
513  T[i] = t[i];
514  }
515 
516  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
517 }
518 
519 void projection_matrix::get_external_parameters_exp_map(double * state)
520 {
521  // make the rotation matrix
522  double rm[9], rv[3];
523  CvMat rotMat = cvMat(3, 3, CV_64FC1, rm);
524  CvMat rotVec = cvMat(1, 3, CV_64FC1, rv);
525 
526  // Compute the rotation matrix
527  for(int i = 0; i < 3; i++)
528  for(int j = 0; j < 3; j++)
529  cvmSet(&rotMat, i, j, R[i][j]);
530 
531  cvRodrigues(&rotMat, &rotVec, 0, CV_RODRIGUES_M2V);
532 
533  state[0] = rv[0];
534  state[1] = rv[1];
535  state[2] = rv[2];
536 
537  state[3] = T[0];
538  state[4] = T[1];
539  state[5] = T[2];
540 }
541 
542 void projection_matrix::set_external_parameters(double wc2vc[3][4])
543 {
544  for(int i = 0; i < 3; i++)
545  {
546  for(int j = 0; j < 3; j++)
547  R[i][j] = wc2vc[i][j];
548  T[i] = wc2vc[i][3];
549  }
550 
551  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
552 }
553 
554 void projection_matrix::set_external_parameters(double ** wc2vc)
555 {
556  for(int i = 0; i < 3; i++)
557  {
558  for(int j = 0; j < 3; j++)
559  R[i][j] = wc2vc[i][j];
560  T[i] = wc2vc[i][3];
561  }
562 
563  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
564 }
565 
566 void projection_matrix::set_external_parameters(double rot[3][3], double transl[3])
567 {
568  gfla_copy_3x3(rot, R);
569  gfla_copy_3(transl, T);
570 
571  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
572 }
573 
574 void projection_matrix::set_rotation_to_Id(void)
575 {
576  R[0][0] = 1; R[0][1] = 0; R[0][2] = 0;
577  R[1][0] = 0; R[1][1] = 1; R[1][2] = 0;
578  R[2][0] = 0; R[2][1] = 0; R[2][2] = 1;
579 
580  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
581 }
582 
583 void projection_matrix::set_translation_to_0(void)
584 {
585  T[0] = T[1] = T[2] = 0.;
586 
587  have_to_recompute_optical_centre = have_to_recompute_P = true;
588 }
589 
590 void projection_matrix::set_external_parameters(CvMat * H)
591 {
592  double AH1[3], AH2[3];
593  double inv_fx = 1. / fx, inv_fy = 1. / fy;
594 
595  AH1[0] = inv_fx * cvmGet(H, 0, 0) - cx * inv_fx * cvmGet(H, 2, 0);
596  AH1[1] = inv_fy * cvmGet(H, 1, 0) - cy * inv_fy * cvmGet(H, 2, 0);
597  AH1[2] = cvmGet(H, 2, 0);
598 
599  AH2[0] = inv_fx * cvmGet(H, 0, 1) - cx * inv_fx * cvmGet(H, 2, 1);
600  AH2[1] = inv_fy * cvmGet(H, 1, 1) - cy * inv_fy * cvmGet(H, 2, 1);
601  AH2[2] = cvmGet(H, 2, 1);
602 
603  T[0] = inv_fx * cvmGet(H, 0, 2) - cx * inv_fx * cvmGet(H, 2, 2);
604  T[1] = inv_fy * cvmGet(H, 1, 2) - cy * inv_fy * cvmGet(H, 2, 2);
605  T[2] = cvmGet(H, 2, 2);
606 
607  double norm2_AH1 = AH1[0] * AH1[0] + AH1[1] * AH1[1] + AH1[2] * AH1[2];
608  double norm2_AH2 = AH2[0] * AH2[0] + AH2[1] * AH2[1] + AH2[2] * AH2[2];
609  double inv_l = 1. / sqrt( sqrt(norm2_AH1 * norm2_AH2) );
610 
611  double R1[3], R2[3];
612  gfla_scale_3(inv_l, AH1, R1);
613  gfla_scale_3(inv_l, AH2, R2);
614  gfla_scale_3(inv_l, T);
615 
616  double c[3], p[3], d[3];
617  gfla_add_3(R1, R2, c);
618  gfla_cross_product(R1, R2, p);
619  gfla_cross_product(c, p, d);
620 
621  gfla_normalize_3(c);
622  gfla_normalize_3(d);
623 
624  double R1p[3], R2p[3], R3p[3];
625  gfla_add_3(c, d, R1p);
626  gfla_scale_3(1. / sqrt(2.), R1p);
627 
628  gfla_sub_3(c, d, R2p);
629  gfla_scale_3(1. / sqrt(2.), R2p);
630 
631  gfla_cross_product(R1p, R2p, R3p);
632 
633  for(int i = 0; i < 3; i++)
634  {
635  R[i][0] = R1p[i];
636  R[i][1] = R2p[i];
637  R[i][2] = R3p[i];
638  }
639 
640  cout << *this << endl;
641 
642  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
643 }
644 
645 void projection_matrix::translate_x(double delta)
646 {
647  T[0] += delta;
648  have_to_recompute_optical_centre = have_to_recompute_P = true;
649 }
650 
651 
652 void projection_matrix::translate_y(double delta)
653 {
654  T[1] += delta;
655  have_to_recompute_optical_centre = have_to_recompute_P = true;
656 }
657 
658 
659 void projection_matrix::translate_z(double delta)
660 {
661  T[2] += delta;
662  have_to_recompute_optical_centre = have_to_recompute_P = true;
663 }
664 
665 
666 void projection_matrix::rotate_x(double delta)
667 {
668  double par[6];
669 
670  get_external_parameters(&par[0], &par[1], &par[2], &par[3], &par[4], &par[5]);
671  par[0] += delta;
672  set_external_parameters(par[0], par[1], par[2], par[3], par[4], par[5]);
673 
674  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
675 }
676 
677 void projection_matrix::rotate_y(double delta)
678 {
679  double par[6];
680 
681  get_external_parameters(&par[0], &par[1], &par[2], &par[3], &par[4], &par[5]);
682  par[1] += delta;
683  set_external_parameters(par[0], par[1], par[2], par[3], par[4], par[5]);
684 
685  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
686 }
687 
688 void projection_matrix::rotate_z(double delta)
689 {
690  double par[6];
691 
692  get_external_parameters(&par[0], &par[1], &par[2], &par[3], &par[4], &par[5]);
693  par[2] += delta;
694  set_external_parameters(par[0], par[1], par[2], par[3], par[4], par[5]);
695 
696  have_to_recompute_invAR = have_to_recompute_optical_centre = have_to_recompute_P = true;
697 }
698 
699 void projection_matrix::get_external_parameters(double * state)
700 {
701  get_external_parameters(&state[0], &state[1], &state[2], &state[3], &state[4], &state[5]);
702 }
703 
704 void projection_matrix::get_external_parameters(double * omega, double * phi, double * kappa,
705  double * Tx, double * Ty, double * Tz)
706 {
707  gfla_get_euler_angles_from_rotation(R, omega, phi, kappa);
708 
709  *Tx = T[0];
710  *Ty = T[1];
711  *Tz = T[2];
712 }
713 
714 void projection_matrix::world_to_cam(double * M, double * Mc)
715 {
716  if (M != Mc)
717  {
718  Mc[0] = R[0][0] * M[0] + R[0][1] * M[1] + R[0][2] * M[2] + T[0];
719  Mc[1] = R[1][0] * M[0] + R[1][1] * M[1] + R[1][2] * M[2] + T[1];
720  Mc[2] = R[2][0] * M[0] + R[2][1] * M[1] + R[2][2] * M[2] + T[2];
721  }
722  else
723  {
724  double M2[3];
725  M2[0] = R[0][0] * M[0] + R[0][1] * M[1] + R[0][2] * M[2] + T[0];
726  M2[1] = R[1][0] * M[0] + R[1][1] * M[1] + R[1][2] * M[2] + T[1];
727  M2[2] = R[2][0] * M[0] + R[2][1] * M[1] + R[2][2] * M[2] + T[2];
728  Mc[0] = M2[0];
729  Mc[1] = M2[1];
730  Mc[2] = M2[2];
731  }
732 }
733 
735 // Projection matrix:
736 
737 void projection_matrix::get_3x4_matrix(double pP[3][4])
738 {
739  if (have_to_recompute_P)
740  compute_P();
741 
742  gfla_copy_3x4(P, pP);
743 }
744 
746 // Optical centre:
747 
748 void projection_matrix::get_optical_centre(double * Cx, double * Cy, double * Cz)
749 {
750  if (have_to_recompute_optical_centre)
751  compute_optical_centre();
752 
753  *Cx = optical_centre[0];
754  *Cy = optical_centre[1];
755  *Cz = optical_centre[2];
756 }
757 
758 double * projection_matrix::get_optical_centre(void)
759 {
760  if (have_to_recompute_optical_centre)
761  compute_optical_centre();
762 
763  return optical_centre;
764 }
765 
767 // Ray casting:
768 
769 double * projection_matrix::get_V(double u, double v)
770 {
771  double * V = new double[3];
772 
773  get_V(u, v, V);
774 
775  return V;
776 }
777 
778 void projection_matrix::get_V(double u, double v, double * V)
779 {
780  if (have_to_recompute_invAR)
781  compute_invAR();
782 
783  V[0] = invAR[0][0] * u + invAR[0][1] * v + invAR[0][2];
784  V[1] = invAR[1][0] * u + invAR[1][1] * v + invAR[1][2];
785  V[2] = invAR[2][0] * u + invAR[2][1] * v + invAR[2][2];
786 
787  gfla_normalize_3(V);
788 }
789 
790 void projection_matrix::get_V(double u, double v, double * Vx, double * Vy, double * Vz)
791 {
792  if (have_to_recompute_invAR)
793  compute_invAR();
794 
795  *Vx = invAR[0][0] * u + invAR[0][1] * v + invAR[0][2];
796  *Vy = invAR[1][0] * u + invAR[1][1] * v + invAR[1][2];
797  *Vz = invAR[2][0] * u + invAR[2][1] * v + invAR[2][2];
798 
799  double inv_norme = 1. / gfla_norme(*Vx, *Vy, *Vz);
800 
801  *Vx *= inv_norme;
802  *Vy *= inv_norme;
803  *Vz *= inv_norme;
804 }
805 
807 // OpenGL:
808 
809 void projection_matrix::set_GL_zmin_zmax(double zmin, double zmax)
810 {
811  planes[4] = zmin;
812  planes[5] = zmax;
813 }
814 
815 void projection_matrix::set_GL_PROJECTION(int xdim, int ydim, float * gl_vector)
816 {
817  double tdir[3][4], S[3][3], Stdir[3][4];
818  float gl_mat[4][4];
819  double zmin = planes[4], zmax = planes[5], zratio = 2.0 / (zmax - zmin);
820 
821  for(int i = 0; i < 3; i++)
822  for(int j = 0; j < 4; j++)
823  tdir[i][j] = 0.;
824 
825  tdir[0][0] = fx;
826  tdir[1][1] = fy;
827  tdir[0][2] = cx;
828  tdir[1][2] = cy;
829  tdir[2][2] = 1.;
830 
831  S[0][0] = 2. / xdim; S[0][1] = 0.; S[0][2] = -1.;
832  S[1][0] = 0.; S[1][1] = 2. / ydim; S[1][2] = -1.;
833  S[2][0] = 0.; S[2][1] = 0.; S[2][2] = 1.;
834 
835  gfla_mul_mat_3x3x4(S, tdir, Stdir);
836 
837  for(int j = 0; j < 4; j++)
838  {
839  gl_mat[j][0] = float(Stdir[0][j]);
840  gl_mat[j][1] = float(Stdir[1][j]);
841  gl_mat[j][3] = float(Stdir[2][j]);
842  }
843 
844  // Plane distance computation in 3rd column:
845  for(int j = 0; j < 4; j++)
846  gl_mat[j][2] = float( planes[j] * zratio );
847  gl_mat[3][2]= float( -(1.0 + gl_mat[3][2] + zmin * zratio) );
848 
849  for(int i = 0; i < 4; i++)
850  gl_mat[i][1] = -gl_mat[i][1];
851 
852  for(int i = 0; i < 4; i++)
853  for(int j = 0; j < 4; j++)
854  gl_vector[j + 4 * i] = gl_mat[i][j];
855 }
856 
857 void projection_matrix::set_GL_MODELVIEW(double * gl_vector)
858 {
859  gl_vector[0*4+0] = R[0][0]; gl_vector[0*4+1] = R[1][0]; gl_vector[0*4+2] = R[2][0]; gl_vector[0*4+3] = 0.;
860  gl_vector[1*4+0] = R[0][1]; gl_vector[1*4+1] = R[1][1]; gl_vector[1*4+2] = R[2][1]; gl_vector[1*4+3] = 0.;
861  gl_vector[2*4+0] = R[0][2]; gl_vector[2*4+1] = R[1][2]; gl_vector[2*4+2] = R[2][2]; gl_vector[2*4+3] = 0.;
862  gl_vector[3*4+0] = T[0]; gl_vector[3*4+1] = T[1]; gl_vector[3*4+2] = T[2]; gl_vector[3*4+3] = 1.;
863 }
864 
865 bool projection_matrix::visible_triangle(double X1, double Y1, double Z1,
866  double X2, double Y2, double Z2,
867  double X3, double Y3, double Z3)
868 {
869  double V[3], n[3];
870  get_V(cx, cy, V);
871  double M1M2[3], M1M3[3];
872  M1M2[0] = X2 - X1; M1M2[1] = Y2 - Y1; M1M2[2] = Z2 - Z1;
873  M1M3[0] = X3 - X1; M1M3[1] = Y3 - Y1; M1M3[2] = Z3 - Z1;
874  gfla_cross_product(M1M2, M1M3, n);
875  return gfla_dot_product(n, V) < 0;
876 }
877 
879 // Internal functions:
880 
881 void projection_matrix::compute_invAR(void)
882 {
883  double t4, t6, t8, t10, t12, t14, t16, t19, t24, t27, t28, t30, t32, t34, t35, t37, t39, t43, t52, t55, t57, t59;
884 
885  t4 = 1/fx;
886  t6 = R[0][0]*R[2][2];
887  t8 = R[0][0]*R[2][1];
888  t10 = R[1][0]*R[2][2];
889  t12 = R[2][1]*R[1][0];
890  t14 = R[2][0]*R[0][1];
891  t16 = R[2][0]*R[0][2];
892  t19 = 1/(t6*R[1][1]-t8*R[1][2]-t10*R[0][1]+t12*R[0][2]+t14*R[1][2]-t16*R[1][1]);
893  t24 = 1/fy;
894  t27 = fx*R[0][1];
895  t28 = R[1][2]*fy;
896  t30 = cy*R[2][2];
897  t32 = cx*R[2][1];
898  t34 = fx*R[0][2];
899  t35 = fy*R[1][1];
900  t37 = cy*R[2][1];
901  t39 = cx*R[2][2];
902  t43 = t24*t19;
903  t52 = fx*R[0][0];
904  t55 = cx*R[2][0];
905  t57 = fy*R[1][0];
906  t59 = cy*R[2][0];
907 
908  invAR[0][0] = (R[1][1]*R[2][2]-R[2][1]*R[1][2])*t4*t19;
909  invAR[0][1] = -(R[2][2]*R[0][1]-R[2][1]*R[0][2])*t24*t19;
910  invAR[0][2] = (t27*t28+t27*t30+t32*t28-t34*t35-t34*t37-t39*t35)*t4*t43;
911  invAR[1][0] = -(t10-R[2][0]*R[1][2])*t4*t19;
912  invAR[1][1] = (t6-t16)*t24*t19;
913  invAR[1][2] = -(t52*t28+t52*t30+t55*t28-t34*t57-t34*t59-t39*t57)*t4*t43;
914  invAR[2][0] = (t12-R[2][0]*R[1][1])*t4*t19;
915  invAR[2][1] = -(t8-t14)*t24*t19;
916  invAR[2][2] = (t52*t35+t52*t37+t55*t35-t27*t57-t27*t59-t32*t57)*t4*t43;
917 
918  have_to_recompute_invAR = false;
919 }
920 
921 void projection_matrix::compute_optical_centre(void)
922 {
925  optical_centre[0] = -(R[0][0] * T[0] + R[1][0] * T[1] + R[2][0] * T[2]);
926  optical_centre[1] = -(R[0][1] * T[0] + R[1][1] * T[1] + R[2][1] * T[2]);
927  optical_centre[2] = -(R[0][2] * T[0] + R[1][2] * T[1] + R[2][2] * T[2]);
928 
929  have_to_recompute_optical_centre = false;
930 }
931 
932 void projection_matrix::compute_P(void)
933 {
934  double A[3][3];
935 
936  A[0][0] = fx; A[0][1] = 0.; A[0][2] = cx;
937  A[1][0] = 0.; A[1][1] = fy; A[1][2] = cy;
938  A[2][0] = 0.; A[2][1] = 0.; A[2][2] = 1.;
939 
940  for(int i = 0; i < 3; i++)
941  for(int j = 0; j < 4; j++)
942  {
943  P[i][j] = 0;
944  for(int k = 0; k < 3; k++)
945  if (j < 3)
946  P[i][j] += A[i][k] * R[k][j];
947  else
948  P[i][j] += A[i][k] * T[k];
949  }
950 
951  have_to_recompute_P = false;
952 }
953 
Generated on Tue Jan 15 2013 12:50:21 for bazar by   doxygen 1.8.1.2