bazar  1.3.1
camera.cpp
Go to the documentation of this file.
1 #include <fstream>
2 #include <iostream>
3 #include <string.h> // for memset
4 #include "camera.h"
5 #include "matvec.h"
6 
7 #ifdef HAVE_CONFIG_H
8 #include <config.h>
9 #endif
10 
11 #ifdef HAVE_GL
12 
13 #ifdef HAVE_APPLE_OPENGL_FRAMEWORK
14 #include <OpenGL/gl.h>
15 #else
16 #include <GL/gl.h>
17 #endif
18 
19 #else
20 
21 #ifdef WIN32
22 #define WIN32_LEAN_AND_MEAN
23 #include <windows.h>
24 #include <GL/gl.h>
25 #define HAVE_GL
26 #endif
27 
28 #endif
29 
30 using namespace std;
31 
32 PerspectiveProjection::PerspectiveProjection()
33 {
34  set(640,480,1000,1000,320,240,0);
35 }
36 
37 PerspectiveProjection::PerspectiveProjection(int w, int h, double f, double g, double cx, double cy, double s)
38 {
39  set(w,h,f,g,cx,cy,s);
40 }
41 
42 void PerspectiveProjection::set(int w, int h, double f, double g, double cx, double cy, double s)
43 {
44  this->width = w;
45  this->height = h;
46  this->f = f;
47  this->g = g;
48  this->cx = cx;
49  this->cy = cy;
50  this->s = s;
51  distortion = 0;
52 
53  nearPlane = f/16;
54  farPlane = f*16;
55  cmpEyeToImageMat();
56 }
57 
63 bool PerspectiveProjection::setPlanes(double nearPlane, double farPlane)
64 {
65  if ((nearPlane > 0) && (nearPlane < farPlane)) {
66  this->nearPlane = nearPlane;
67  this->farPlane = farPlane;
68  return true;
69  }
70  return false;
71 }
72 
73 
78 void PerspectiveProjection::eyeToImage(const double eye[3], double uv[2]) const
79 {
80  uv[0] = (f * eye[0] + s * eye[1]) / eye[2] + cx;
81  uv[1] = (g * eye[1]) / eye[2] + cy;
82 }
83 
88 void PerspectiveProjection::imageToEye(double u, double v, double eye[3], double w) const
89 {
90  if (w==0) w = nearPlane;
91 
92  eye[1] = (v - cy)*(f/g);
93  eye[0] = (u - cx - s*eye[1]);
94  eye[2] = f;
95 
96  //double l = this->nearPlane / sqrt(eye[0]*eye[0] + eye[1]*eye[1] + eye[2]*eye[2]);
97  double l = w / f;
98  eye[0] *= l;
99  eye[1] *= l;
100  eye[2] *= l;
101 }
102 
103 void PerspectiveProjection::imageToEye(const double uv[2], double eye[3], double w) const
104 {
105  imageToEye(uv[0], uv[1], eye, w);
106 }
107 
118 void PerspectiveProjection::setGlProjection() const
119 {
120 #ifdef HAVE_GL
121  glMatrixMode(GL_PROJECTION);
122  glLoadIdentity();
123 
124  GLdouble m[16];
125  for (int i=0; i<16; ++i) m[i]=0;
126  double w = (double) width;
127  double h = (double) height;
128 
129  double _near = this->nearPlane;
130  double _far = this->farPlane;
131 
132  m[0 + 0*4] = 2*f/w;
133  m[0 + 1*4] = 2*s/w;
134  m[0 + 2*4] = (-2*cx/w +1);
135  m[1 + 1*4] = 2*g/h;
136  m[1 + 2*4] = (-2*cy/h +1);
137  m[2 + 2*4] = -(_far+_near)/(_far-_near);
138  m[2 + 3*4] = -2*_far*_near/(_far-_near);
139  m[3 + 2*4] = -1;
140 
141  glLoadMatrixd(m);
142 #else
143  cerr << "camera.cpp was not compiled with OpenGL support.\n";
144 #endif
145 }
146 
147 void PerspectiveProjection::cmpEyeToImageMat()
148 {
149  eyeToImageMat.m[0][0] = f;
150  eyeToImageMat.m[1][0] = 0;
151  eyeToImageMat.m[2][0] = 0;
152  eyeToImageMat.m[0][1] = s;
153  eyeToImageMat.m[1][1] = g;
154  eyeToImageMat.m[2][1] = 0;
155  eyeToImageMat.m[0][2] = cx;
156  eyeToImageMat.m[1][2] = cy;
157  eyeToImageMat.m[2][2] = 1;
158 }
159 
162 void PerspectiveProjection::flip()
163 {
164  g= -g;
165  cy = height -1 -cy;
166  cmpEyeToImageMat();
167 }
168 
169 bool PerspectiveProjection::getUndistortMap(CvMat *xmap, CvMat *ymap)
170 {
171  if (distortion == 0) return false;
172 
173  for (int y=0; y<xmap->height; y++) {
174  for (int x=0; x<ymap->height; y++) {
175  float image[2], distor[2];
176 
177  image[0] = (x - (cx))/f;
178  image[1] = (y - (cy))/g;
179 
180  distor[0] = image[0] - distortion * image[0] * (image[0]*image[0]+image[1]*image[1]);
181  distor[1] = image[1] - distortion * image[1] * (image[0]*image[0]+image[1]*image[1]);
182 
183 
184  cvSetReal2D(xmap, y, x, distor[0]*f + cx);
185  cvSetReal2D(ymap, y, x, distor[1]*g + cy);
186  }
187  }
188  return true;
189 }
190 
191 PerspectiveCamera::PerspectiveCamera()
192 {
193  clearExternalParams();
194  translate(0,0,-f);
195 }
196 
197 void PerspectiveCamera::worldToImage(const Vec3 &p, Vec3 &uvw) const
198 {
199  uvw.setMul(worldToImageMat, p);
200  uvw.v[0] /= uvw.v[2];
201  uvw.v[1] /= uvw.v[2];
202 }
203 
204 void PerspectiveCamera::worldToImage(const double p[3], double uvw[3]) const
205 {
206  worldToImage(*((Vec3 *)p), *((Vec3 *)uvw));
207 }
208 
209 void PerspectiveCamera::worldToEye(const Vec3 &src, Vec3 &dst) const
210 {
211  dst.setMul(worldToEyeMat, src);
212 }
213 
214 void PerspectiveCamera::worldToEye(const double src[3], double dst[3]) const
215 {
216  ((Vec3 *)dst)->setMul(worldToEyeMat, *((Vec3 *)src));
217 }
218 
219 void PerspectiveCamera::setGlModelView() const
220 {
221 #ifdef HAVE_GL
222  GLdouble m[16];
223 
224  for (int i=0; i<3; ++i)
225  for (int j=0;j<4; ++j)
226  m[i+j*4] = worldToEyeMat.m[i][j];
227  m[3+0*4] = m[3+1*4] = m[3+2*4] = 0;
228  m[3+3*4]=1;
229  glMatrixMode(GL_MODELVIEW);
230  glLoadIdentity();
231  glScalef(1,1,-1);
232  glMultMatrixd(m);
233 #else
234  cerr << "camera.cpp was not compiled with OpenGL support.\n";
235 #endif
236 }
237 
238 void PerspectiveCamera::setByTarget(const Vec3 pos, const Vec3 target, double roll)
239 {
240  Vec3 x,y,z;
241 
242  // TODO: is this correct ?
243  Vec3 up(sin(roll), cos(roll), 0);
244 
245  z.setSub(target, pos);
246  z.normalize();
247 
248  x.setCross(z,up);
249  y.setCross(x,z);
250 
251  for (int i=0; i<3; i++) {
252  worldToEyeMat.m[0][i] = x[i];
253  worldToEyeMat.m[1][i] = y[i];
254  worldToEyeMat.m[2][i] = z[i];
255  }
256 
257  for (int i=0; i<3; ++i) {
258  worldToEyeMat.m[i][3] = -worldToEyeMat.m[i][0]*pos[0]
259  - worldToEyeMat.m[i][1]*pos[1]
260  - worldToEyeMat.m[i][2]*pos[2];
261  }
262  cmpWorldToImageMat();
263 }
264 
265 static void get3x3MulWithTransposed(const double m[3][4], double mmt[3][3])
266 {
267  double t7 = m[0][0]*m[0][0];
268  double t8 = m[0][1]*m[0][1];
269  double t9 = m[0][2]*m[0][2];
270  double t14 = m[0][0]*m[1][0]+m[0][1]*m[1][1]+m[0][2]*m[1][2];
271  double t18 = m[0][0]*m[2][0]+m[0][1]*m[2][1]+m[0][2]*m[2][2];
272  double t19 = m[1][0]*m[1][0];
273  double t20 = m[1][1]*m[1][1];
274  double t21 = m[1][2]*m[1][2];
275  double t26 = m[1][0]*m[2][0]+m[1][1]*m[2][1]+m[1][2]*m[2][2];
276  double t27 = m[2][0]*m[2][0];
277  double t28 = m[2][1]*m[2][1];
278  double t29 = m[2][2]*m[2][2];
279  mmt[0][0] = t7+t8+t9;
280  mmt[0][1] = t14;
281  mmt[0][2] = t18;
282  mmt[1][0] = t14;
283  mmt[1][1] = t19+t20+t21;
284  mmt[1][2] = t26;
285  mmt[2][0] = t18;
286  mmt[2][1] = t26;
287  mmt[2][2] = t27+t28+t29;
288 }
289 
295 void PerspectiveCamera::loadTdir(const double _tdir[3][4], int w, int h)
296 {
297  this->width=w;
298  this->height=h;
299  double tdir[3][4];
300 
301  double ppt[3][3];
302 
303  double l = sqrt(_tdir[2][0]*_tdir[2][0]
304  + _tdir[2][1]*_tdir[2][1]
305  + _tdir[2][2]*_tdir[2][2]);
306  for (int i=0; i<3; ++i)
307  for (int j=0;j<4; ++j)
308  tdir[i][j] = _tdir[i][j]/l;
309 
310  get3x3MulWithTransposed(tdir, ppt);
311 
312  worldToEyeMat.m[2][0] = tdir[2][0];
313  worldToEyeMat.m[2][1] = tdir[2][1];
314  worldToEyeMat.m[2][2] = tdir[2][2];
315  worldToEyeMat.m[2][3] = tdir[2][3];
316 
317  cx = ppt[2][0];
318  cy = ppt[2][1];
319 
320  // sign is unkown!
321  g = sqrt(ppt[1][1]-cy*cy);
322 
323  s = (ppt[1][0]-cx*cy)/g;
324 
325  // sign is also unknown!
326  f = sqrt(ppt[0][0]-cx*cx-s*s);
327 
328  worldToEyeMat.m[1][0] = (tdir[1][0] - cy*worldToEyeMat.m[2][0])/g;
329  worldToEyeMat.m[0][0] = (tdir[0][0] - s*worldToEyeMat.m[1][0] - cx*worldToEyeMat.m[2][0])/f;
330 
331  worldToEyeMat.m[1][1] = (tdir[1][1] - cy*worldToEyeMat.m[2][1])/g;
332  worldToEyeMat.m[0][1] = (tdir[0][1] - s*worldToEyeMat.m[1][1] - cx*worldToEyeMat.m[2][1])/f;
333 
334  worldToEyeMat.m[1][2] = (tdir[1][2] - cy*worldToEyeMat.m[2][2])/g;
335  worldToEyeMat.m[0][2] = (tdir[0][2] - s*worldToEyeMat.m[1][2] - cx*worldToEyeMat.m[2][2])/f;
336 
337  worldToEyeMat.m[1][3] = (tdir[1][3] - cy*worldToEyeMat.m[2][3])/g;
338  worldToEyeMat.m[0][3] = (tdir[0][3] - s*worldToEyeMat.m[1][3] - cx*worldToEyeMat.m[2][3])/f;
339 
340  if (worldToEyeMat.det() < 0) {
341  g=-g;
342  for (int i=0;i<4;++i) worldToEyeMat.m[1][i] = -worldToEyeMat.m[1][i];
343  }
344 
345  cmpEyeToImageMat();
346  eyeToWorldMat.setInverseByTranspose(worldToEyeMat);
347  memcpy(&worldToImageMat, tdir, sizeof(worldToImageMat));
348 
349 }
350 
356 bool PerspectiveCamera::loadTdir(const char *tdirFile, int w, int h)
357 {
358  ifstream file;
359  file.open(tdirFile);
360 
361  if (!file.good()) return false;
362  double tdir[3][4];
363 
364  for (int j=0; j<3; ++j)
365  for (int i=0; i<4; ++i)
366  file >> tdir[j][i];
367  char line[256];
368  while (file.getline(line,256)) {
369  if (memcmp(line, "distortion:", 11)==0) {
370  double d;
371  if (sscanf(line+11, "%lf", &d)==1) {
372  distortion = d;
373  printf("Camera distortion: %f\n", d);
374  }
375  } else if (memcmp(line, "width:", 6)==0) {
376  int nw;
377  if (sscanf(line+6, "%d", &nw)==1) {
378  w = nw;
379  }
380  } else if (memcmp(line, "height:", 7)==0) {
381  int nh;
382  if (sscanf(line+7, "%d", &nh)==1) {
383  h = nh;
384  }
385  } else if (memcmp(line, "near:", 5)==0) {
386  double d;
387  if (sscanf(line+5, "%lf", &d)==1) {
388  nearPlane = d;
389  printf("Near clipping plane: %f\n", d);
390  }
391  } else if (memcmp(line, "far:", 4)==0) {
392  double d;
393  if (sscanf(line+4, "%lf", &d)==1) {
394  farPlane = d;
395  printf("Near clipping plane: %f\n", d);
396  }
397  }
398  }
399  file.close();
400  loadTdir(tdir, w,h);
401 
402  // look for a .plane file.
403  if (strlen(tdirFile) > 5) {
404  char fn[512];
405  strncpy(fn, tdirFile, 511);
406  strcpy(fn + strlen(fn) - strlen(".tdir"), ".plane");
407  file.open(fn);
408 
409  if (file.good()) {
410  double d,n=nearPlane,f=farPlane;
411  for (int i=0; i<4; ++i)
412  file >> d;
413  if (file.good()) file >> n;
414  if (file.good()) file >> f;
415 
416  file.close();
417  if (setPlanes(n,f))
418  std::cout << "Reading clipping planes from " << fn
419  << ": near=" << n
420  << " far=" << f << endl;
421  else
422  std::cerr << "Troubles loading " << fn << "!\n";
423  }
424  }
425  return true;
426 }
427 
428 struct GuessMode {
429  int w, h;
430 };
431 static double diag(double x, double y)
432 {
433  return sqrt(x*x+y*y);
434 }
435 
436 static double diagDiff(const GuessMode &m, double dx, double dy)
437 {
438  double mdiag = diag(double(m.w), double(m.h));
439  return fabs(double(mdiag - diag(2*dx,2*dy)));
440 }
441 
442 
450 bool PerspectiveCamera::loadTdir(const char *tdirFile)
451 {
452  if (!loadTdir(tdirFile,0,0)) return false;
453 
454  if ((width !=0) && (height !=0)) return true;
455 
456  static const GuessMode mode[] = {
457  { 640, 480 },
458  { 320, 240 },
459  { 720, 576 },
460  { 360, 288 },
461  { 800, 600 },
462  { 1024, 768},
463  { 512, 384},
464  { 1280, 1024},
465  { -1, -1}
466  };
467 
468  int best=0;
469  double bestDelta = diagDiff(mode[0], cx, cy);
470  for (int i=1; mode[i].w != -1; ++i) {
471  double delta = diagDiff(mode[i], cx, cy);
472  if (delta < bestDelta) {
473  best = i;
474  bestDelta = delta;
475  }
476  }
477  width = mode[best].w;
478  height = mode[best].h;
479 
480  return true;
481 }
482 
483 
484 void PerspectiveCamera::clearExternalParams()
485 {
486  worldToEyeMat.setIdentity();
487  cmpWorldToImageMat();
488 }
489 
490 void PerspectiveCamera::cmpWorldToImageMat()
491 {
492  worldToImageMat.setMul(eyeToImageMat, worldToEyeMat);
493  eyeToWorldMat.setInverseByTranspose(worldToEyeMat);
494 }
495 
496 void PerspectiveCamera::eyeToWorld(const Vec3 &uvw, Vec3 &w) const
497 {
498  w.setMul(eyeToWorldMat, uvw);
499 }
500 
501 void PerspectiveCamera::imageToWorld(double u, double v, Vec3 &w, double depth) const
502 {
503  Vec3 uvw;
504  imageToEye(u,v, uvw.v, depth);
505  eyeToWorld(uvw, w);
506 }
507 
508 void PerspectiveCamera::translate(double dx, double dy, double dz)
509 {
510  worldToEyeMat.m[0][3] -= dx;
511  worldToEyeMat.m[1][3] -= dy;
512  worldToEyeMat.m[2][3] -= dz;
513  cmpWorldToImageMat();
514 }
515 
516 void PerspectiveCamera::setWorldToEyeMat(const Mat3x4 &m)
517 {
518  worldToEyeMat = m;
519  cmpWorldToImageMat();
520 }
521 
522 bool PerspectiveCamera::saveTdir(const char *file)
523 {
524  ofstream f(file);
525 
526  if (!f.good()) return false;
527 
528  for (int l=0; l<3; l++) {
529  for (int c=0; c<4; c++) {
530  f << worldToImageMat.m[l][c] << " ";
531  }
532  f << "\n";
533  }
534  f << "width: " << width << endl;
535  f << "height: " << height << endl;
536  f << "distortion: " << distortion << endl;
537  f << "near: " << nearPlane << endl;
538  f << "far: " << farPlane << endl;
539  return true;
540 }
541 
542 ostream& operator << (ostream& os, const PerspectiveProjection &cam)
543 {
544  os << "fx=" << cam.f
545  << ", fy="<< cam.g
546  << ", cx="<< cam.cx
547  << ", cy="<< cam.cy
548  << ", s="<< cam.s
549  << ", near="<< cam.nearPlane
550  << ", far="<< cam.farPlane
551  << ", width="<<cam.width
552  << ", height="<<cam.height;
553  return os;
554 }
555 
556 ostream& operator << (ostream& os, const PerspectiveCamera &cam)
557 {
558  os << ((PerspectiveProjection) cam) << endl;
559  os << cam.getWorldToEyeMat();
560  return os;
561 }
562 
563 
Generated on Tue Jan 15 2013 12:50:21 for bazar by   doxygen 1.8.1.2