Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
vpCameraParameters.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Camera intrinsic parameters.
33  *
34  * Authors:
35  * Eric Marchand
36  * Anthony Saunier
37  *
38  *****************************************************************************/
39 
47 #include <cmath>
48 #include <iomanip>
49 #include <iostream>
50 #include <limits>
51 #include <sstream>
52 #include <visp3/core/vpCameraParameters.h>
53 #include <visp3/core/vpDebug.h>
54 #include <visp3/core/vpException.h>
55 #include <visp3/core/vpRotationMatrix.h>
56 
57 const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
58 const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
59 const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
60 const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
61 const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
62 const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
63 const vpCameraParameters::vpCameraParametersProjType vpCameraParameters::DEFAULT_PROJ_TYPE =
65 
73  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
74  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), width(0), height(0), isFov(false), m_hFovAngle(0),
75  m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
76  projModel(DEFAULT_PROJ_TYPE)
77 {
78  init();
79 }
80 
85  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
86  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), width(0), height(0), isFov(false), m_hFovAngle(0),
87  m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
88  projModel(DEFAULT_PROJ_TYPE)
89 {
90  init(c);
91 }
92 
100 vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py, const double cam_u0,
101  const double cam_v0)
102  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
103  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), width(0), height(0), isFov(false), m_hFovAngle(0),
104  m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
105  projModel(DEFAULT_PROJ_TYPE)
106 {
107  initPersProjWithoutDistortion(cam_px, cam_py, cam_u0, cam_v0);
108 }
109 
119 vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py, const double cam_u0,
120  const double cam_v0, const double cam_kud, const double cam_kdu)
121  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
122  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), width(0), height(0), isFov(false), m_hFovAngle(0),
123  m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
124  projModel(DEFAULT_PROJ_TYPE)
125 {
126  initPersProjWithDistortion(cam_px, cam_py, cam_u0, cam_v0, cam_kud, cam_kdu);
127 }
128 
133 {
134  if (fabs(this->px) < 1e-6) {
135  vpERROR_TRACE("Camera parameter px = 0");
136  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
137  }
138  if (fabs(this->py) < 1e-6) {
139  vpERROR_TRACE("Camera parameter px = 0");
140  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
141  }
142  this->inv_px = 1. / this->px;
143  this->inv_py = 1. / this->py;
144 }
145 
183 void vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, const double cam_py, const double cam_u0,
184  const double cam_v0)
185 {
187 
188  this->px = cam_px;
189  this->py = cam_py;
190  this->u0 = cam_u0;
191  this->v0 = cam_v0;
192  this->kud = 0;
193  this->kdu = 0;
194 
195  if (fabs(px) < 1e-6) {
196  vpERROR_TRACE("Camera parameter px = 0");
197  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
198  }
199  if (fabs(py) < 1e-6) {
200  vpERROR_TRACE("Camera parameter px = 0");
201  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
202  }
203  this->inv_px = 1. / px;
204  this->inv_py = 1. / py;
205 }
206 
249 void vpCameraParameters::initPersProjWithDistortion(const double cam_px, const double cam_py, const double cam_u0,
250  const double cam_v0, const double cam_kud, const double cam_kdu)
251 {
253 
254  this->px = cam_px;
255  this->py = cam_py;
256  this->u0 = cam_u0;
257  this->v0 = cam_v0;
258  this->kud = cam_kud;
259  this->kdu = cam_kdu;
260 
261  if (fabs(px) < 1e-6) {
262  vpERROR_TRACE("Camera parameter px = 0");
263  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
264  }
265  if (fabs(py) < 1e-6) {
266  vpERROR_TRACE("Camera parameter px = 0");
267  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
268  }
269  this->inv_px = 1. / px;
270  this->inv_py = 1. / py;
271 }
272 
279 
283 void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; }
284 
300 {
301  if (_K.getRows() != 3 || _K.getCols() != 3) {
302  throw vpException(vpException::dimensionError, "bad size for calibration matrix");
303  }
304  if (std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()) {
305  throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
306  }
307  initPersProjWithoutDistortion(_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
308 }
309 
343 void vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov,
344  const double &vfov)
345 {
347  u0 = (double)w / 2.;
348  v0 = (double)h / 2.;
349  px = u0 / tan(hfov / 2);
350  py = v0 / tan(vfov / 2);
351  kud = 0;
352  kdu = 0;
353  inv_px = 1. / px;
354  inv_py = 1. / py;
355  computeFov(w, h);
356 }
357 
362 {
363  projModel = cam.projModel;
364  px = cam.px;
365  py = cam.py;
366  u0 = cam.u0;
367  v0 = cam.v0;
368  kud = cam.kud;
369  kdu = cam.kdu;
370 
371  inv_px = cam.inv_px;
372  inv_py = cam.inv_py;
373 
374  isFov = cam.isFov;
375  m_hFovAngle = cam.m_hFovAngle;
376  m_vFovAngle = cam.m_vFovAngle;
377  width = cam.width;
378  height = cam.height;
379  fovNormals = cam.fovNormals;
380 
381  return *this;
382 }
383 
388  if (projModel != c.projModel)
389  return false;
390 
391  if (!vpMath::equal(px, c.px, std::numeric_limits<double>::epsilon()) ||
392  !vpMath::equal(py, c.py, std::numeric_limits<double>::epsilon()) ||
393  !vpMath::equal(u0, c.u0, std::numeric_limits<double>::epsilon()) ||
394  !vpMath::equal(v0, c.v0, std::numeric_limits<double>::epsilon()) ||
395  !vpMath::equal(kud, c.kud, std::numeric_limits<double>::epsilon()) ||
396  !vpMath::equal(kdu, c.kdu, std::numeric_limits<double>::epsilon()) ||
397  !vpMath::equal(inv_px, c.inv_px, std::numeric_limits<double>::epsilon()) ||
398  !vpMath::equal(inv_py, c.inv_py, std::numeric_limits<double>::epsilon()))
399  return false;
400 
401  if (isFov != c.isFov ||
402  !vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits<double>::epsilon()) ||
403  !vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits<double>::epsilon()) ||
404  width != c.width || height != c.height)
405  return false;
406 
407  if (fovNormals.size() != c.fovNormals.size())
408  return false;
409 
410  std::vector<vpColVector>::const_iterator it1 = fovNormals.begin();
411  std::vector<vpColVector>::const_iterator it2 = c.fovNormals.begin();
412  for (; it1 != fovNormals.end() && it2 != c.fovNormals.end(); ++it1, ++it2) {
413  if (*it1 != *it2)
414  return false;
415  }
416 
417  return true;
418 }
419 
424  return !(*this == c);
425 }
426 
433 void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
434 {
435  if ((!isFov || w != width || h != height) && w != 0 && h != 0) {
436  fovNormals = std::vector<vpColVector>(4);
437 
438  isFov = true;
439 
440  double hFovAngle = atan(((double)w - u0) * (1.0 / px));
441  double vFovAngle = atan((v0) * (1.0 / py));
442  double minushFovAngle = atan((u0) * (1.0 / px));
443  double minusvFovAngle = atan(((double)h - v0) * (1.0 / py));
444 
445  width = w;
446  height = h;
447 
448  vpColVector n(3);
449  n = 0;
450  n[0] = 1.0;
451 
452  vpRotationMatrix Rleft(0, -minushFovAngle, 0);
453  vpRotationMatrix Rright(0, hFovAngle, 0);
454 
455  vpColVector nLeft, nRight;
456 
457  nLeft = Rleft * (-n);
458  fovNormals[0] = nLeft.normalize();
459 
460  nRight = Rright * n;
461  fovNormals[1] = nRight.normalize();
462 
463  n = 0;
464  n[1] = 1.0;
465 
466  vpRotationMatrix Rup(vFovAngle, 0, 0);
467  vpRotationMatrix Rdown(-minusvFovAngle, 0, 0);
468 
469  vpColVector nUp, nDown;
470 
471  nUp = Rup * (-n);
472  fovNormals[2] = nUp.normalize();
473 
474  nDown = Rdown * n;
475  fovNormals[3] = nDown.normalize();
476 
477  m_hFovAngle = hFovAngle + minushFovAngle;
478  m_vFovAngle = vFovAngle + minusvFovAngle;
479  }
480 }
481 
494 {
495  vpMatrix K(3, 3, 0.);
496  K[0][0] = px;
497  K[1][1] = py;
498  K[0][2] = u0;
499  K[1][2] = v0;
500  K[2][2] = 1.0;
501 
502  return K;
503 }
516 {
517  vpMatrix K_inv(3, 3, 0.);
518  K_inv[0][0] = inv_px;
519  K_inv[1][1] = inv_py;
520  K_inv[0][2] = -u0 * inv_px;
521  K_inv[1][2] = -v0 * inv_py;
522  K_inv[2][2] = 1.0;
523 
524  return K_inv;
525 }
526 
533 {
534  std::ios::fmtflags original_flags(std::cout.flags());
535  switch (projModel) {
537  std::cout.precision(10);
538  std::cout << "Camera parameters for perspective projection without distortion:" << std::endl;
539  std::cout << " px = " << px << "\t py = " << py << std::endl;
540  std::cout << " u0 = " << u0 << "\t v0 = " << v0 << std::endl;
541  break;
543  std::cout.precision(10);
544  std::cout << "Camera parameters for perspective projection with distortion:" << std::endl;
545  std::cout << " px = " << px << "\t py = " << py << std::endl;
546  std::cout << " u0 = " << u0 << "\t v0 = " << v0 << std::endl;
547  std::cout << " kud = " << kud << std::endl;
548  std::cout << " kdu = " << kdu << std::endl;
549  break;
550  }
551  // Restore ostream format
552  std::cout.flags(original_flags);
553 }
561 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam)
562 {
563  switch (cam.get_projModel()) {
565  os << "Camera parameters for perspective projection without distortion:" << std::endl;
566  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
567  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
568  break;
570  std::ios_base::fmtflags original_flags = os.flags();
571  os.precision(10);
572  os << "Camera parameters for perspective projection with distortion:" << std::endl;
573  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
574  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
575  os << " kud = " << cam.get_kud() << std::endl;
576  os << " kdu = " << cam.get_kdu() << std::endl;
577 
578  os.flags(original_flags); // restore os to standard state
579  break;
580  }
581  return os;
582 }
bool operator!=(const vpCameraParameters &c) const
vpMatrix get_K_inverse() const
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:104
void initFromCalibrationMatrix(const vpMatrix &_K)
double get_u0() const
void init()
basic initialization with the default parameters
#define vpERROR_TRACE
Definition: vpDebug.h:393
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:290
error that can be emited by ViSP classes.
Definition: vpException.h:71
double get_py() const
unsigned int getCols() const
Definition: vpArray2D.h:146
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
vpColVector & normalize()
void initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov)
double get_v0() const
Generic class defining intrinsic camera parameters.
unsigned int getRows() const
Definition: vpArray2D.h:156
double get_px() const
double get_kud() const
vpMatrix get_K() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpCameraParametersProjType get_projModel() const
double get_kdu() const
bool operator==(const vpCameraParameters &c) const
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpCameraParameters &cam)
vpCameraParameters & operator=(const vpCameraParameters &c)
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
void computeFov(const unsigned int &w, const unsigned int &h)