Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
56const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
57const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
58const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
59const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
60const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
61const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
62const 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
99vpCameraParameters::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
117vpCameraParameters::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
135vpCameraParameters::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
197void 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
262void 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
291void 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
326void 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
386void 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
474bool vpCameraParameters::operator!=(const vpCameraParameters &c) const { return !(*this == c); }
475
482void 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}
616VISP_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
unsigned int getRows() const
Definition vpArray2D.h:290
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
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
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)
std::vector< double > getKannalaBrandtDistortionCoefficients() const
bool operator==(const vpCameraParameters &c) 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)
Implementation of column vector and the associated operations.
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.