Visual Servoing Platform  version 3.6.0 under development (2023-09-29)
vpCameraParameters.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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  * Anthony Saunier
36  *
37 *****************************************************************************/
38 
46 #include <cmath>
47 #include <iomanip>
48 #include <iostream>
49 #include <limits>
50 #include <sstream>
51 #include <visp3/core/vpCameraParameters.h>
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpException.h>
54 #include <visp3/core/vpRotationMatrix.h>
55 
56 const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
57 const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
58 const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
59 const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
60 const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
61 const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
62 const vpCameraParameters::vpCameraParametersProjType vpCameraParameters::DEFAULT_PROJ_TYPE =
64 
72  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
73  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), width(0), height(0), isFov(false),
74  m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
75  projModel(DEFAULT_PROJ_TYPE)
76 {
77  init();
78 }
79 
84  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
85  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), width(0), height(0), isFov(false),
86  m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
87  projModel(DEFAULT_PROJ_TYPE)
88 {
89  init(c);
90 }
91 
99 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0)
100  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
101  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), width(0), height(0), isFov(false),
102  m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
103  projModel(DEFAULT_PROJ_TYPE)
104 {
105  initPersProjWithoutDistortion(cam_px, cam_py, cam_u0, cam_v0);
106 }
107 
117 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0, double cam_kud,
118  double cam_kdu)
119  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
120  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), width(0), height(0), isFov(false),
121  m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
122  projModel(DEFAULT_PROJ_TYPE)
123 {
124  initPersProjWithDistortion(cam_px, cam_py, cam_u0, cam_v0, cam_kud, cam_kdu);
125 }
126 
135 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0,
136  const std::vector<double> &coefficients)
137  : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
138  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), width(0), height(0), isFov(false),
139  m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
140  projModel(DEFAULT_PROJ_TYPE)
141 {
142  initProjWithKannalaBrandtDistortion(cam_px, cam_py, cam_u0, cam_v0, coefficients);
143 }
144 
149 {
150  if (fabs(this->px) < 1e-6) {
151  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
152  }
153  if (fabs(this->py) < 1e-6) {
154  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
155  }
156  this->inv_px = 1. / this->px;
157  this->inv_py = 1. / this->py;
158 }
159 
197 void vpCameraParameters::initPersProjWithoutDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0)
198 {
200 
201  this->px = cam_px;
202  this->py = cam_py;
203  this->u0 = cam_u0;
204  this->v0 = cam_v0;
205  this->kud = 0;
206  this->kdu = 0;
207 
208  this->m_dist_coefs.clear();
209 
210  if (fabs(px) < 1e-6) {
211  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
212  }
213  if (fabs(py) < 1e-6) {
214  throw(vpException(vpException::divideByZeroError, "Camera parameter py = 0"));
215  }
216  this->inv_px = 1. / px;
217  this->inv_py = 1. / py;
218 }
219 
262 void vpCameraParameters::initPersProjWithDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0,
263  double cam_kud, double cam_kdu)
264 {
266 
267  this->px = cam_px;
268  this->py = cam_py;
269  this->u0 = cam_u0;
270  this->v0 = cam_v0;
271  this->kud = cam_kud;
272  this->kdu = cam_kdu;
273  this->m_dist_coefs.clear();
274 
275  if (fabs(px) < 1e-6) {
276  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
277  }
278  if (fabs(py) < 1e-6) {
279  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
280  }
281  this->inv_px = 1. / px;
282  this->inv_py = 1. / py;
283 }
284 
291 void vpCameraParameters::initProjWithKannalaBrandtDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0,
292  const std::vector<double> &coefficients)
293 {
295 
296  this->px = cam_px;
297  this->py = cam_py;
298  this->u0 = cam_u0;
299  this->v0 = cam_v0;
300 
301  this->kud = 0.0;
302  this->kdu = 0.0;
303 
304  if (fabs(px) < 1e-6) {
305  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
306  }
307  if (fabs(py) < 1e-6) {
308  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
309  }
310  this->inv_px = 1. / px;
311  this->inv_py = 1. / py;
312 
313  this->m_dist_coefs = coefficients;
314 }
315 
322 
326 void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; }
327 
343 {
344  if (_K.getRows() != 3 || _K.getCols() != 3) {
345  throw vpException(vpException::dimensionError, "bad size for calibration matrix");
346  }
347  if (std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()) {
348  throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
349  }
350  initPersProjWithoutDistortion(_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
351 }
352 
386 void vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov,
387  const double &vfov)
388 {
390  u0 = (double)w / 2.;
391  v0 = (double)h / 2.;
392  px = u0 / tan(hfov / 2);
393  py = v0 / tan(vfov / 2);
394  kud = 0;
395  kdu = 0;
396  inv_px = 1. / px;
397  inv_py = 1. / py;
398  computeFov(w, h);
399 }
400 
405 {
406  projModel = cam.projModel;
407  px = cam.px;
408  py = cam.py;
409  u0 = cam.u0;
410  v0 = cam.v0;
411  kud = cam.kud;
412  kdu = cam.kdu;
413  m_dist_coefs = cam.m_dist_coefs;
414 
415  inv_px = cam.inv_px;
416  inv_py = cam.inv_py;
417 
418  isFov = cam.isFov;
419  m_hFovAngle = cam.m_hFovAngle;
420  m_vFovAngle = cam.m_vFovAngle;
421  width = cam.width;
422  height = cam.height;
423  fovNormals = cam.fovNormals;
424 
425  return *this;
426 }
427 
432 {
433  if (projModel != c.projModel)
434  return false;
435 
436  if (!vpMath::equal(px, c.px, std::numeric_limits<double>::epsilon()) ||
437  !vpMath::equal(py, c.py, std::numeric_limits<double>::epsilon()) ||
438  !vpMath::equal(u0, c.u0, std::numeric_limits<double>::epsilon()) ||
439  !vpMath::equal(v0, c.v0, std::numeric_limits<double>::epsilon()) ||
440  !vpMath::equal(kud, c.kud, std::numeric_limits<double>::epsilon()) ||
441  !vpMath::equal(kdu, c.kdu, std::numeric_limits<double>::epsilon()) ||
442  !vpMath::equal(inv_px, c.inv_px, std::numeric_limits<double>::epsilon()) ||
443  !vpMath::equal(inv_py, c.inv_py, std::numeric_limits<double>::epsilon()))
444  return false;
445 
446  if(m_dist_coefs.size() != c.m_dist_coefs.size())
447  return false;
448 
449  for (unsigned int i = 0; i < m_dist_coefs.size(); i++)
450  if (!vpMath::equal(m_dist_coefs[i], c.m_dist_coefs[i], std::numeric_limits<double>::epsilon()))
451  return false;
452 
453  if (isFov != c.isFov || !vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits<double>::epsilon()) ||
454  !vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits<double>::epsilon()) || width != c.width ||
455  height != c.height)
456  return false;
457 
458  if (fovNormals.size() != c.fovNormals.size())
459  return false;
460 
461  std::vector<vpColVector>::const_iterator it1 = fovNormals.begin();
462  std::vector<vpColVector>::const_iterator it2 = c.fovNormals.begin();
463  for (; it1 != fovNormals.end() && it2 != c.fovNormals.end(); ++it1, ++it2) {
464  if (*it1 != *it2)
465  return false;
466  }
467 
468  return true;
469 }
470 
474 bool vpCameraParameters::operator!=(const vpCameraParameters &c) const { return !(*this == c); }
475 
482 void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
483 {
484  if ((!isFov || w != width || h != height) && w != 0 && h != 0) {
485  fovNormals = std::vector<vpColVector>(4);
486 
487  isFov = true;
488 
489  double hFovAngle = atan(((double)w - u0) * (1.0 / px));
490  double vFovAngle = atan((v0) * (1.0 / py));
491  double minushFovAngle = atan((u0) * (1.0 / px));
492  double minusvFovAngle = atan(((double)h - v0) * (1.0 / py));
493 
494  width = w;
495  height = h;
496 
497  vpColVector n(3);
498  n = 0;
499  n[0] = 1.0;
500 
501  vpRotationMatrix Rleft(0, -minushFovAngle, 0);
502  vpRotationMatrix Rright(0, hFovAngle, 0);
503 
504  vpColVector nLeft, nRight;
505 
506  nLeft = Rleft * (-n);
507  fovNormals[0] = nLeft.normalize();
508 
509  nRight = Rright * n;
510  fovNormals[1] = nRight.normalize();
511 
512  n = 0;
513  n[1] = 1.0;
514 
515  vpRotationMatrix Rup(vFovAngle, 0, 0);
516  vpRotationMatrix Rdown(-minusvFovAngle, 0, 0);
517 
518  vpColVector nUp, nDown;
519 
520  nUp = Rup * (-n);
521  fovNormals[2] = nUp.normalize();
522 
523  nDown = Rdown * n;
524  fovNormals[3] = nDown.normalize();
525 
526  m_hFovAngle = hFovAngle + minushFovAngle;
527  m_vFovAngle = vFovAngle + minusvFovAngle;
528  }
529 }
530 
543 {
544  vpMatrix K(3, 3, 0.);
545  K[0][0] = px;
546  K[1][1] = py;
547  K[0][2] = u0;
548  K[1][2] = v0;
549  K[2][2] = 1.0;
550 
551  return K;
552 }
565 {
566  vpMatrix K_inv(3, 3, 0.);
567  K_inv[0][0] = inv_px;
568  K_inv[1][1] = inv_py;
569  K_inv[0][2] = -u0 * inv_px;
570  K_inv[1][2] = -v0 * inv_py;
571  K_inv[2][2] = 1.0;
572 
573  return K_inv;
574 }
575 
582 {
583  std::ios::fmtflags original_flags(std::cout.flags());
584  switch (projModel) {
586  std::cout.precision(10);
587  std::cout << "Camera parameters for perspective projection without distortion:" << std::endl;
588  std::cout << " px = " << px << "\t py = " << py << std::endl;
589  std::cout << " u0 = " << u0 << "\t v0 = " << v0 << std::endl;
590  break;
592  std::cout.precision(10);
593  std::cout << "Camera parameters for perspective projection with distortion:" << std::endl;
594  std::cout << " px = " << px << "\t py = " << py << std::endl;
595  std::cout << " u0 = " << u0 << "\t v0 = " << v0 << std::endl;
596  std::cout << " kud = " << kud << std::endl;
597  std::cout << " kdu = " << kdu << std::endl;
598  break;
600  std::cout << " Coefficients: ";
601  for (unsigned int i = 0; i < m_dist_coefs.size(); i++)
602  std::cout << " " << m_dist_coefs[i];
603  std::cout << std::endl;
604  break;
605  }
606  // Restore ostream format
607  std::cout.flags(original_flags);
608 }
616 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam)
617 {
618  switch (cam.get_projModel()) {
620  os << "Camera parameters for perspective projection without distortion:" << std::endl;
621  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
622  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
623  break;
625  std::ios_base::fmtflags original_flags = os.flags();
626  os.precision(10);
627  os << "Camera parameters for perspective projection with distortion:" << std::endl;
628  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
629  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
630  os << " kud = " << cam.get_kud() << std::endl;
631  os << " kdu = " << cam.get_kdu() << std::endl;
632  os.flags(original_flags); // restore os to standard state
633  } break;
635  os << "Camera parameters for projection with Kannala-Brandt distortion:" << std::endl;
636  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
637  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
638  os << " Coefficients: ";
639  std::vector<double> tmp_coefs = cam.getKannalaBrandtDistortionCoefficients();
640  for (unsigned int i = 0; i < tmp_coefs.size(); i++)
641  os << " " << tmp_coefs[i];
642  os << std::endl;
643  } break;
644  }
645  return os;
646 }
unsigned int getCols() const
Definition: vpArray2D.h:280
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:529
unsigned int getRows() const
Definition: vpArray2D.h:290
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
std::vector< double > getKannalaBrandtDistortionCoefficients() const
void initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov)
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
vpMatrix get_K_inverse() const
vpMatrix get_K() const
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
vpCameraParameters & operator=(const vpCameraParameters &c)
void computeFov(const unsigned int &w, const unsigned int &h)
bool operator==(const vpCameraParameters &c) const
double get_kdu() const
void initFromCalibrationMatrix(const vpMatrix &_K)
bool operator!=(const vpCameraParameters &c) const
vpCameraParametersProjType get_projModel() const
void init()
basic initialization with the default parameters
void initProjWithKannalaBrandtDistortion(double px, double py, double u0, double v0, const std::vector< double > &distortion_coefficients)
double get_kud() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:167
vpColVector & normalize()
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:85
@ dimensionError
Bad dimension.
Definition: vpException.h:83
@ divideByZeroError
Division by zero.
Definition: vpException.h:82
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:369
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:152
Implementation of a rotation matrix and operations on such kind of matrices.