Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
vpCameraParameters.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Camera intrinsic parameters.
32  */
33 
40 #include <cmath>
41 #include <iomanip>
42 #include <iostream>
43 #include <limits>
44 #include <sstream>
45 #include <visp3/core/vpCameraParameters.h>
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpRotationMatrix.h>
49 #include <visp3/core/vpMath.h>
50 
51 const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
52 const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
53 const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
54 const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
55 const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
56 const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
57 const vpCameraParameters::vpCameraParametersProjType vpCameraParameters::DEFAULT_PROJ_TYPE =
59 
67  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
68  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
69  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
70  m_projModel(DEFAULT_PROJ_TYPE)
71 {
72  init();
73 }
74 
79  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
80  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
81  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
82  m_projModel(DEFAULT_PROJ_TYPE)
83 {
84  init(c);
85 }
86 
95 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0)
96  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
97  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
98  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
99  m_projModel(DEFAULT_PROJ_TYPE)
100 {
101  initPersProjWithoutDistortion(cam_px, cam_py, cam_u0, cam_v0);
102 }
103 
114 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0, double cam_kud,
115  double cam_kdu)
116  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
117  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
118  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
119  m_projModel(DEFAULT_PROJ_TYPE)
120 {
121  initPersProjWithDistortion(cam_px, cam_py, cam_u0, cam_v0, cam_kud, cam_kdu);
122 }
123 
133 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0,
134  const std::vector<double> &coefficients)
135  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
136  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
137  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
138  m_projModel(DEFAULT_PROJ_TYPE)
139 {
140  initProjWithKannalaBrandtDistortion(cam_px, cam_py, cam_u0, cam_v0, coefficients);
141 }
142 
147 {
148  if (fabs(this->m_px) < 1e-6) {
149  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
150  }
151  if (fabs(this->m_py) < 1e-6) {
152  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
153  }
154  this->m_inv_px = 1. / this->m_px;
155  this->m_inv_py = 1. / this->m_py;
156 }
157 
197 void vpCameraParameters::initPersProjWithoutDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0)
198 {
200 
201  this->m_px = cam_px;
202  this->m_py = cam_py;
203  this->m_u0 = cam_u0;
204  this->m_v0 = cam_v0;
205  this->m_kud = 0;
206  this->m_kdu = 0;
207 
208  this->m_dist_coefs.clear();
209 
210  if (fabs(m_px) < 1e-6) {
211  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
212  }
213  if (fabs(m_py) < 1e-6) {
214  throw(vpException(vpException::divideByZeroError, "Camera parameter py = 0"));
215  }
216  this->m_inv_px = 1. / m_px;
217  this->m_inv_py = 1. / m_py;
218 }
219 
265 void vpCameraParameters::initPersProjWithDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0,
266  double cam_kud, double cam_kdu)
267 {
269 
270  this->m_px = cam_px;
271  this->m_py = cam_py;
272  this->m_u0 = cam_u0;
273  this->m_v0 = cam_v0;
274  this->m_kud = cam_kud;
275  this->m_kdu = cam_kdu;
276  this->m_dist_coefs.clear();
277 
278  if (fabs(m_px) < 1e-6) {
279  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
280  }
281  if (fabs(m_py) < 1e-6) {
282  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
283  }
284  this->m_inv_px = 1. / m_px;
285  this->m_inv_py = 1. / m_py;
286 }
287 
297 void vpCameraParameters::initProjWithKannalaBrandtDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0,
298  const std::vector<double> &coefficients)
299 {
301 
302  this->m_px = cam_px;
303  this->m_py = cam_py;
304  this->m_u0 = cam_u0;
305  this->m_v0 = cam_v0;
306 
307  this->m_kud = 0.0;
308  this->m_kdu = 0.0;
309 
310  if (fabs(m_px) < 1e-6) {
311  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
312  }
313  if (fabs(m_py) < 1e-6) {
314  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
315  }
316  this->m_inv_px = 1. / m_px;
317  this->m_inv_py = 1. / m_py;
318 
319  this->m_dist_coefs = coefficients;
320 }
321 
326 
330 void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; }
331 
347 {
348  if (K.getRows() != 3 || K.getCols() != 3) {
349  throw vpException(vpException::dimensionError, "bad size for calibration matrix");
350  }
351  if (std::fabs(K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()) {
352  throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
353  }
354  initPersProjWithoutDistortion(K[0][0], K[1][1], K[0][2], K[1][2]);
355 }
356 
392 void vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov,
393  const double &vfov)
394 {
396  m_u0 = (double)w / 2.;
397  m_v0 = (double)h / 2.;
398  m_px = m_u0 / tan(hfov / 2);
399  m_py = m_v0 / tan(vfov / 2);
400  m_kud = 0;
401  m_kdu = 0;
402  m_inv_px = 1. / m_px;
403  m_inv_py = 1. / m_py;
404  computeFov(w, h);
405 }
406 
411 {
412  m_projModel = cam.m_projModel;
413  m_px = cam.m_px;
414  m_py = cam.m_py;
415  m_u0 = cam.m_u0;
416  m_v0 = cam.m_v0;
417  m_kud = cam.m_kud;
418  m_kdu = cam.m_kdu;
419  m_dist_coefs = cam.m_dist_coefs;
420 
421  m_inv_px = cam.m_inv_px;
422  m_inv_py = cam.m_inv_py;
423 
424  m_isFov = cam.m_isFov;
425  m_hFovAngle = cam.m_hFovAngle;
426  m_vFovAngle = cam.m_vFovAngle;
427  m_width = cam.m_width;
428  m_height = cam.m_height;
429  m_fovNormals = cam.m_fovNormals;
430 
431  return *this;
432 }
433 
438 {
439  if (m_projModel != c.m_projModel)
440  return false;
441 
442  if (!vpMath::equal(m_px, c.m_px, std::numeric_limits<double>::epsilon()) ||
443  !vpMath::equal(m_py, c.m_py, std::numeric_limits<double>::epsilon()) ||
444  !vpMath::equal(m_u0, c.m_u0, std::numeric_limits<double>::epsilon()) ||
445  !vpMath::equal(m_v0, c.m_v0, std::numeric_limits<double>::epsilon()) ||
446  !vpMath::equal(m_kud, c.m_kud, std::numeric_limits<double>::epsilon()) ||
447  !vpMath::equal(m_kdu, c.m_kdu, std::numeric_limits<double>::epsilon()) ||
448  !vpMath::equal(m_inv_px, c.m_inv_px, std::numeric_limits<double>::epsilon()) ||
449  !vpMath::equal(m_inv_py, c.m_inv_py, std::numeric_limits<double>::epsilon()))
450  return false;
451 
452  if (m_dist_coefs.size() != c.m_dist_coefs.size())
453  return false;
454 
455  for (unsigned int i = 0; i < m_dist_coefs.size(); i++)
456  if (!vpMath::equal(m_dist_coefs[i], c.m_dist_coefs[i], std::numeric_limits<double>::epsilon()))
457  return false;
458 
459  if (m_isFov != c.m_isFov || !vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits<double>::epsilon()) ||
460  !vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits<double>::epsilon()) || m_width != c.m_width ||
461  m_height != c.m_height)
462  return false;
463 
464  if (m_fovNormals.size() != c.m_fovNormals.size())
465  return false;
466 
467  std::vector<vpColVector>::const_iterator it1 = m_fovNormals.begin();
468  std::vector<vpColVector>::const_iterator it2 = c.m_fovNormals.begin();
469  for (; it1 != m_fovNormals.end() && it2 != c.m_fovNormals.end(); ++it1, ++it2) {
470  if (*it1 != *it2)
471  return false;
472  }
473 
474  return true;
475 }
476 
480 bool vpCameraParameters::operator!=(const vpCameraParameters &c) const { return !(*this == c); }
481 
488 void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
489 {
490  if ((!m_isFov || w != m_width || h != m_height) && w != 0 && h != 0) {
491  m_fovNormals = std::vector<vpColVector>(4);
492 
493  m_isFov = true;
494 
495  double hFovAngle = atan(((double)w - m_u0) * (1.0 / m_px));
496  double vFovAngle = atan((m_v0) * (1.0 / m_py));
497  double minushFovAngle = atan((m_u0) * (1.0 / m_px));
498  double minusvFovAngle = atan(((double)h - m_v0) * (1.0 / m_py));
499 
500  m_width = w;
501  m_height = h;
502 
503  vpColVector n(3);
504  n = 0;
505  n[0] = 1.0;
506 
507  vpRotationMatrix Rleft(0, -minushFovAngle, 0);
508  vpRotationMatrix Rright(0, hFovAngle, 0);
509 
510  vpColVector nLeft, nRight;
511 
512  nLeft = Rleft * (-n);
513  m_fovNormals[0] = nLeft.normalize();
514 
515  nRight = Rright * n;
516  m_fovNormals[1] = nRight.normalize();
517 
518  n = 0;
519  n[1] = 1.0;
520 
521  vpRotationMatrix Rup(vFovAngle, 0, 0);
522  vpRotationMatrix Rdown(-minusvFovAngle, 0, 0);
523 
524  vpColVector nUp, nDown;
525 
526  nUp = Rup * (-n);
527  m_fovNormals[2] = nUp.normalize();
528 
529  nDown = Rdown * n;
530  m_fovNormals[3] = nDown.normalize();
531 
532  m_hFovAngle = hFovAngle + minushFovAngle;
533  m_vFovAngle = vFovAngle + minusvFovAngle;
534  }
535 }
536 
549 {
550  vpMatrix K(3, 3, 0.);
551  K[0][0] = m_px;
552  K[1][1] = m_py;
553  K[0][2] = m_u0;
554  K[1][2] = m_v0;
555  K[2][2] = 1.0;
556 
557  return K;
558 }
571 {
572  vpMatrix K_inv(3, 3, 0.);
573  K_inv[0][0] = m_inv_px;
574  K_inv[1][1] = m_inv_py;
575  K_inv[0][2] = -m_u0 * m_inv_px;
576  K_inv[1][2] = -m_v0 * m_inv_py;
577  K_inv[2][2] = 1.0;
578 
579  return K_inv;
580 }
581 
588 {
589  std::ios::fmtflags original_flags(std::cout.flags());
590  switch (m_projModel) {
592  std::cout.precision(10);
593  std::cout << "Camera parameters for perspective projection without distortion:" << std::endl;
594  std::cout << " px = " << m_px << "\t py = " << m_py << std::endl;
595  std::cout << " u0 = " << m_u0 << "\t v0 = " << m_v0 << std::endl;
596  break;
598  std::cout.precision(10);
599  std::cout << "Camera parameters for perspective projection with distortion:" << std::endl;
600  std::cout << " px = " << m_px << "\t py = " << m_py << std::endl;
601  std::cout << " u0 = " << m_u0 << "\t v0 = " << m_v0 << std::endl;
602  std::cout << " kud = " << m_kud << std::endl;
603  std::cout << " kdu = " << m_kdu << std::endl;
604  break;
606  std::cout << " Coefficients: ";
607  for (unsigned int i = 0; i < m_dist_coefs.size(); i++)
608  std::cout << " " << m_dist_coefs[i];
609  std::cout << std::endl;
610  break;
611  }
612  // Restore ostream format
613  std::cout.flags(original_flags);
614 }
615 
622 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam)
623 {
624  switch (cam.get_projModel()) {
626  os << "Camera parameters for perspective projection without distortion:" << std::endl;
627  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
628  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
629  break;
631  std::ios_base::fmtflags original_flags = os.flags();
632  os.precision(10);
633  os << "Camera parameters for perspective projection with distortion:" << std::endl;
634  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
635  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
636  os << " kud = " << cam.get_kud() << std::endl;
637  os << " kdu = " << cam.get_kdu() << std::endl;
638  os.flags(original_flags); // restore os to standard state
639  } break;
641  os << "Camera parameters for projection with Kannala-Brandt distortion:" << std::endl;
642  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
643  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
644  os << " Coefficients: ";
645  std::vector<double> tmp_coefs = cam.getKannalaBrandtDistortionCoefficients();
646  for (unsigned int i = 0; i < tmp_coefs.size(); i++)
647  os << " " << tmp_coefs[i];
648  os << std::endl;
649  } break;
650  }
651  return os;
652 }
unsigned int getCols() const
Definition: vpArray2D.h:274
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:523
unsigned int getRows() const
Definition: vpArray2D.h:284
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:163
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:449
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:146
Implementation of a rotation matrix and operations on such kind of matrices.