Visual Servoing Platform  version 3.6.1 under development (2024-10-10)
vpCameraParameters.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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/vpException.h>
47 #include <visp3/core/vpRotationMatrix.h>
48 #include <visp3/core/vpMath.h>
49 
50 BEGIN_VISP_NAMESPACE
51 
52 const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
53 const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
54 const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
55 const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
56 const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
57 const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
58 const vpCameraParameters::vpCameraParametersProjType vpCameraParameters::DEFAULT_PROJ_TYPE =
60 
68  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
69  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
70  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
71  m_projModel(DEFAULT_PROJ_TYPE)
72 {
73  init();
74 }
75 
80  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
81  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
82  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
83  m_projModel(DEFAULT_PROJ_TYPE)
84 {
85  init(c);
86 }
87 
96 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0)
97  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
98  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
99  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
100  m_projModel(DEFAULT_PROJ_TYPE)
101 {
102  initPersProjWithoutDistortion(cam_px, cam_py, cam_u0, cam_v0);
103 }
104 
115 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0, double cam_kud,
116  double cam_kdu)
117  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
118  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
119  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
120  m_projModel(DEFAULT_PROJ_TYPE)
121 {
122  initPersProjWithDistortion(cam_px, cam_py, cam_u0, cam_v0, cam_kud, cam_kdu);
123 }
124 
134 vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0,
135  const std::vector<double> &coefficients)
136  : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
137  m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
138  m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
139  m_projModel(DEFAULT_PROJ_TYPE)
140 {
141  initProjWithKannalaBrandtDistortion(cam_px, cam_py, cam_u0, cam_v0, coefficients);
142 }
143 
148 {
149  if (fabs(this->m_px) < 1e-6) {
150  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
151  }
152  if (fabs(this->m_py) < 1e-6) {
153  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
154  }
155  this->m_inv_px = 1. / this->m_px;
156  this->m_inv_py = 1. / this->m_py;
157 }
158 
202 void vpCameraParameters::initPersProjWithoutDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0)
203 {
205 
206  this->m_px = cam_px;
207  this->m_py = cam_py;
208  this->m_u0 = cam_u0;
209  this->m_v0 = cam_v0;
210  this->m_kud = 0;
211  this->m_kdu = 0;
212 
213  this->m_dist_coefs.clear();
214 
215  if (fabs(m_px) < 1e-6) {
216  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
217  }
218  if (fabs(m_py) < 1e-6) {
219  throw(vpException(vpException::divideByZeroError, "Camera parameter py = 0"));
220  }
221  this->m_inv_px = 1. / m_px;
222  this->m_inv_py = 1. / m_py;
223 }
224 
274 void vpCameraParameters::initPersProjWithDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0,
275  double cam_kud, double cam_kdu)
276 {
278 
279  this->m_px = cam_px;
280  this->m_py = cam_py;
281  this->m_u0 = cam_u0;
282  this->m_v0 = cam_v0;
283  this->m_kud = cam_kud;
284  this->m_kdu = cam_kdu;
285  this->m_dist_coefs.clear();
286 
287  if (fabs(m_px) < 1e-6) {
288  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
289  }
290  if (fabs(m_py) < 1e-6) {
291  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
292  }
293  this->m_inv_px = 1. / m_px;
294  this->m_inv_py = 1. / m_py;
295 }
296 
306 void vpCameraParameters::initProjWithKannalaBrandtDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0,
307  const std::vector<double> &coefficients)
308 {
310 
311  this->m_px = cam_px;
312  this->m_py = cam_py;
313  this->m_u0 = cam_u0;
314  this->m_v0 = cam_v0;
315 
316  this->m_kud = 0.0;
317  this->m_kdu = 0.0;
318 
319  if (fabs(m_px) < 1e-6) {
320  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
321  }
322  if (fabs(m_py) < 1e-6) {
323  throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
324  }
325  this->m_inv_px = 1. / m_px;
326  this->m_inv_py = 1. / m_py;
327 
328  this->m_dist_coefs = coefficients;
329 }
330 
335 
339 void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; }
340 
356 {
357  const unsigned int nparam = 3;
358  const unsigned int index_0 = 0;
359  const unsigned int index_1 = 1;
360  const unsigned int index_2 = 2;
361  if ((K.getRows() != nparam) || (K.getCols() != nparam)) {
362  throw vpException(vpException::dimensionError, "bad size for calibration matrix");
363  }
364  if (std::fabs(K[index_2][index_2] - 1.0) > std::numeric_limits<double>::epsilon()) {
365  throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
366  }
367  initPersProjWithoutDistortion(K[index_0][index_0], K[index_1][index_1], K[index_0][index_2], K[index_1][index_2]);
368 }
369 
409 void vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov,
410  const double &vfov)
411 {
413  m_u0 = static_cast<double>(w) / 2.;
414  m_v0 = static_cast<double>(h) / 2.;
415  m_px = m_u0 / tan(hfov / 2.);
416  m_py = m_v0 / tan(vfov / 2.);
417  m_kud = 0;
418  m_kdu = 0;
419  m_inv_px = 1. / m_px;
420  m_inv_py = 1. / m_py;
421  computeFov(w, h);
422 }
423 
428 {
429  m_projModel = cam.m_projModel;
430  m_px = cam.m_px;
431  m_py = cam.m_py;
432  m_u0 = cam.m_u0;
433  m_v0 = cam.m_v0;
434  m_kud = cam.m_kud;
435  m_kdu = cam.m_kdu;
436  m_dist_coefs = cam.m_dist_coefs;
437 
438  m_inv_px = cam.m_inv_px;
439  m_inv_py = cam.m_inv_py;
440 
441  m_isFov = cam.m_isFov;
442  m_hFovAngle = cam.m_hFovAngle;
443  m_vFovAngle = cam.m_vFovAngle;
444  m_width = cam.m_width;
445  m_height = cam.m_height;
446  m_fovNormals = cam.m_fovNormals;
447 
448  return *this;
449 }
450 
455 {
456  if (m_projModel != c.m_projModel) {
457  return false;
458  }
459 
460  // maximum allowed conditional operators shall be maximum 3
461  if ((!vpMath::equal(m_px, c.m_px, std::numeric_limits<double>::epsilon())) ||
462  (!vpMath::equal(m_py, c.m_py, std::numeric_limits<double>::epsilon())) ||
463  (!vpMath::equal(m_u0, c.m_u0, std::numeric_limits<double>::epsilon()))) {
464  return false;
465  }
466  if ((!vpMath::equal(m_v0, c.m_v0, std::numeric_limits<double>::epsilon())) ||
467  (!vpMath::equal(m_kud, c.m_kud, std::numeric_limits<double>::epsilon())) ||
468  (!vpMath::equal(m_kdu, c.m_kdu, std::numeric_limits<double>::epsilon()))) {
469  return false;
470  }
471  if ((!vpMath::equal(m_inv_px, c.m_inv_px, std::numeric_limits<double>::epsilon())) ||
472  (!vpMath::equal(m_inv_py, c.m_inv_py, std::numeric_limits<double>::epsilon()))) {
473  return false;
474  }
475 
476  if (m_dist_coefs.size() != c.m_dist_coefs.size()) {
477  return false;
478  }
479 
480  size_t m_dist_coefs_size = m_dist_coefs.size();
481  for (size_t i = 0; i < m_dist_coefs_size; ++i) {
482  if (!vpMath::equal(m_dist_coefs[i], c.m_dist_coefs[i], std::numeric_limits<double>::epsilon())) {
483  return false;
484  }
485  }
486 
487  if ((m_isFov != c.m_isFov) || (!vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits<double>::epsilon())) ||
488  (!vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits<double>::epsilon()))) {
489  return false;
490  }
491  if ((m_width != c.m_width) || (m_height != c.m_height)) {
492  return false;
493  }
494 
495  if (m_fovNormals.size() != c.m_fovNormals.size()) {
496  return false;
497  }
498 
499  std::vector<vpColVector>::const_iterator it1 = m_fovNormals.begin();
500  std::vector<vpColVector>::const_iterator it1_end = m_fovNormals.end();
501  std::vector<vpColVector>::const_iterator it2 = c.m_fovNormals.begin();
502  std::vector<vpColVector>::const_iterator it2_end = c.m_fovNormals.end();
503 
504  for (; (it1 != it1_end) && (it2 != it2_end); ++it1, ++it2) {
505  if (*it1 != *it2) {
506  return false;
507  }
508  }
509 
510  return true;
511 }
512 
516 bool vpCameraParameters::operator!=(const vpCameraParameters &c) const { return !(*this == c); }
517 
524 void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
525 {
526  bool cond1 = (!m_isFov) || (w != m_width) || (h != m_height);
527  if (cond1 && (w != 0) && (h != 0)) {
528  const unsigned int nparam_3 = 3;
529  const unsigned int nparam_4 = 4;
530  const unsigned int index_0 = 0;
531  const unsigned int index_1 = 1;
532  const unsigned int index_2 = 2;
533  const unsigned int index_3 = 3;
534  m_fovNormals = std::vector<vpColVector>(nparam_4);
535 
536  m_isFov = true;
537 
538  double hFovAngle = atan((static_cast<double>(w) - m_u0) * (1.0 / m_px));
539  double vFovAngle = atan(m_v0 * (1.0 / m_py));
540  double minushFovAngle = atan(m_u0 * (1.0 / m_px));
541  double minusvFovAngle = atan((static_cast<double>(h) - m_v0) * (1.0 / m_py));
542 
543  m_width = w;
544  m_height = h;
545 
546  vpColVector n(nparam_3);
547  n = 0;
548  n[0] = 1.0;
549 
550  vpRotationMatrix Rleft(0, -minushFovAngle, 0);
551  vpRotationMatrix Rright(0, hFovAngle, 0);
552 
553  vpColVector nLeft, nRight;
554 
555  nLeft = Rleft * (-n);
556  m_fovNormals[index_0] = nLeft.normalize();
557 
558  nRight = Rright * n;
559  m_fovNormals[index_1] = nRight.normalize();
560 
561  n = 0;
562  n[1] = 1.0;
563 
564  vpRotationMatrix Rup(vFovAngle, 0, 0);
565  vpRotationMatrix Rdown(-minusvFovAngle, 0, 0);
566 
567  vpColVector nUp, nDown;
568 
569  nUp = Rup * (-n);
570  m_fovNormals[index_2] = nUp.normalize();
571 
572  nDown = Rdown * n;
573  m_fovNormals[index_3] = nDown.normalize();
574 
575  m_hFovAngle = hFovAngle + minushFovAngle;
576  m_vFovAngle = vFovAngle + minusvFovAngle;
577  }
578 }
579 
592 {
593  const unsigned int index_0 = 0;
594  const unsigned int index_1 = 1;
595  const unsigned int index_2 = 2;
596  vpMatrix K(3, 3, 0.);
597  K[index_0][index_0] = m_px;
598  K[index_1][index_1] = m_py;
599  K[index_0][index_2] = m_u0;
600  K[index_1][index_2] = m_v0;
601  K[index_2][index_2] = 1.0;
602 
603  return K;
604 }
617 {
618  const unsigned int index_0 = 0;
619  const unsigned int index_1 = 1;
620  const unsigned int index_2 = 2;
621 
622  vpMatrix K_inv(3, 3, 0.);
623  K_inv[index_0][index_0] = m_inv_px;
624  K_inv[index_1][index_1] = m_inv_py;
625  K_inv[index_0][index_2] = -m_u0 * m_inv_px;
626  K_inv[index_1][index_2] = -m_v0 * m_inv_py;
627  K_inv[index_2][index_2] = 1.0;
628 
629  return K_inv;
630 }
631 
638 {
639  size_t m_dist_coefs_size = m_dist_coefs.size();
640  std::ios::fmtflags original_flags(std::cout.flags());
641  switch (m_projModel) {
643  std::cout.precision(10);
644  std::cout << "Camera parameters for perspective projection without distortion:" << std::endl;
645  std::cout << " px = " << m_px << "\t py = " << m_py << std::endl;
646  std::cout << " u0 = " << m_u0 << "\t v0 = " << m_v0 << std::endl;
647  break;
648  }
650  std::cout.precision(10);
651  std::cout << "Camera parameters for perspective projection with distortion:" << std::endl;
652  std::cout << " px = " << m_px << "\t py = " << m_py << std::endl;
653  std::cout << " u0 = " << m_u0 << "\t v0 = " << m_v0 << std::endl;
654  std::cout << " kud = " << m_kud << std::endl;
655  std::cout << " kdu = " << m_kdu << std::endl;
656  break;
657  }
659  std::cout << " Coefficients: ";
660  for (size_t i = 0; i < m_dist_coefs_size; ++i) {
661  std::cout << " " << m_dist_coefs[i];
662  }
663  std::cout << std::endl;
664  break;
665  }
666  default: {
667  std::cout << "projection model not identified" << std::endl;
668  }
669  }
670  // Restore ostream format
671  std::cout.flags(original_flags);
672 }
673 
680 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam)
681 {
682  switch (cam.get_projModel()) {
684  os << "Camera parameters for perspective projection without distortion:" << std::endl;
685  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
686  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
687  break;
688  }
690  std::ios_base::fmtflags original_flags = os.flags();
691  const unsigned int precision = 10;
692  os.precision(precision);
693  os << "Camera parameters for perspective projection with distortion:" << std::endl;
694  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
695  os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
696  os << " kud = " << cam.get_kud() << std::endl;
697  os << " kdu = " << cam.get_kdu() << std::endl;
698  os.flags(original_flags); // restore os to standard state
699  break;
700  }
702  os << "Camera parameters for projection with Kannala-Brandt distortion:" << std::endl;
703  os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
704  os << " Coefficients: ";
705  std::vector<double> tmp_coefs = cam.getKannalaBrandtDistortionCoefficients();
706  size_t tmp_coefs_size = tmp_coefs.size();
707  for (size_t i = 0; i < tmp_coefs_size; ++i) {
708  os << " " << tmp_coefs[i];
709  }
710  os << std::endl;
711  break;
712  }
713  default: {
714  std::cout << "Unidentified camera parameters model" << std::endl;
715  }
716  }
717  return os;
718 }
719 END_VISP_NAMESPACE
unsigned int getCols() const
Definition: vpArray2D.h:337
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:611
unsigned int getRows() const
Definition: vpArray2D.h:347
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:191
vpColVector & normalize()
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:73
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ divideByZeroError
Division by zero.
Definition: vpException.h:70
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:169
Implementation of a rotation matrix and operations on such kind of matrices.