Visual Servoing Platform version 3.5.0
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
57const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
58const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
59const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
60const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
61const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
62const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
63const 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), m_dist_coefs(), width(0), height(0),
75 isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER),
76 inv_py(1. / DEFAULT_PY_PARAMETER), 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), m_dist_coefs(), width(0), height(0),
87 isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER),
88 inv_py(1. / DEFAULT_PY_PARAMETER), projModel(DEFAULT_PROJ_TYPE)
89{
90 init(c);
91}
92
100vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0, double cam_v0)
101 : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
102 kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), width(0), height(0),
103 isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER),
104 inv_py(1. / DEFAULT_PY_PARAMETER), projModel(DEFAULT_PROJ_TYPE)
105{
106 initPersProjWithoutDistortion(cam_px, cam_py, cam_u0, cam_v0);
107}
108
118vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0,
119 double cam_v0, double cam_kud, double cam_kdu)
120 : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
121 kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), width(0), height(0),
122 isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER),
123 inv_py(1. / DEFAULT_PY_PARAMETER), projModel(DEFAULT_PROJ_TYPE)
124{
125 initPersProjWithDistortion(cam_px, cam_py, cam_u0, cam_v0, cam_kud, cam_kdu);
126}
127
136vpCameraParameters::vpCameraParameters(double cam_px, double cam_py, double cam_u0,
137 double cam_v0, const std::vector<double> &coefficients)
138 : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
139 kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), width(0), height(0),
140 isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER),
141 inv_py(1. / DEFAULT_PY_PARAMETER), projModel(DEFAULT_PROJ_TYPE)
142{
143 initProjWithKannalaBrandtDistortion(cam_px, cam_py, cam_u0, cam_v0, coefficients);
144}
145
150{
151 if (fabs(this->px) < 1e-6) {
152 throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
153 }
154 if (fabs(this->py) < 1e-6) {
155 throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
156 }
157 this->inv_px = 1. / this->px;
158 this->inv_py = 1. / this->py;
159}
160
198void vpCameraParameters::initPersProjWithoutDistortion(double cam_px, double cam_py, double cam_u0,
199 double cam_v0)
200{
202
203 this->px = cam_px;
204 this->py = cam_py;
205 this->u0 = cam_u0;
206 this->v0 = cam_v0;
207 this->kud = 0;
208 this->kdu = 0;
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 px = 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,
263 double cam_v0, 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
274 if (fabs(px) < 1e-6) {
275 throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
276 }
277 if (fabs(py) < 1e-6) {
278 throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
279 }
280 this->inv_px = 1. / px;
281 this->inv_py = 1. / py;
282}
283
290void vpCameraParameters::initProjWithKannalaBrandtDistortion(double cam_px, double cam_py, double cam_u0, double cam_v0,
291 const std::vector<double> &coefficients)
292{
294
295 this->px = cam_px;
296 this->py = cam_py;
297 this->u0 = cam_u0;
298 this->v0 = cam_v0;
299
300 if (fabs(px) < 1e-6) {
301 throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
302 }
303 if (fabs(py) < 1e-6) {
304 throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
305 }
306 this->inv_px = 1. / px;
307 this->inv_py = 1. / py;
308
309 this->m_dist_coefs = coefficients;
310}
311
318
322void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; }
323
339{
340 if (_K.getRows() != 3 || _K.getCols() != 3) {
341 throw vpException(vpException::dimensionError, "bad size for calibration matrix");
342 }
343 if (std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()) {
344 throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
345 }
346 initPersProjWithoutDistortion(_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
347}
348
382void vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov,
383 const double &vfov)
384{
386 u0 = (double)w / 2.;
387 v0 = (double)h / 2.;
388 px = u0 / tan(hfov / 2);
389 py = v0 / tan(vfov / 2);
390 kud = 0;
391 kdu = 0;
392 inv_px = 1. / px;
393 inv_py = 1. / py;
394 computeFov(w, h);
395}
396
401{
402 projModel = cam.projModel;
403 px = cam.px;
404 py = cam.py;
405 u0 = cam.u0;
406 v0 = cam.v0;
407 kud = cam.kud;
408 kdu = cam.kdu;
409 m_dist_coefs = cam.m_dist_coefs;
410
411 inv_px = cam.inv_px;
412 inv_py = cam.inv_py;
413
414 isFov = cam.isFov;
415 m_hFovAngle = cam.m_hFovAngle;
416 m_vFovAngle = cam.m_vFovAngle;
417 width = cam.width;
418 height = cam.height;
419 fovNormals = cam.fovNormals;
420
421 return *this;
422}
423
428 if (projModel != c.projModel)
429 return false;
430
431 if (!vpMath::equal(px, c.px, std::numeric_limits<double>::epsilon()) ||
432 !vpMath::equal(py, c.py, std::numeric_limits<double>::epsilon()) ||
433 !vpMath::equal(u0, c.u0, std::numeric_limits<double>::epsilon()) ||
434 !vpMath::equal(v0, c.v0, std::numeric_limits<double>::epsilon()) ||
435 !vpMath::equal(kud, c.kud, std::numeric_limits<double>::epsilon()) ||
436 !vpMath::equal(kdu, c.kdu, std::numeric_limits<double>::epsilon()) ||
437 !vpMath::equal(inv_px, c.inv_px, std::numeric_limits<double>::epsilon()) ||
438 !vpMath::equal(inv_py, c.inv_py, std::numeric_limits<double>::epsilon()))
439 return false;
440
441 for(unsigned int i = 0; i < m_dist_coefs.size(); i++)
442 if(!vpMath::equal(m_dist_coefs[i], c.m_dist_coefs[i], std::numeric_limits<double>::epsilon()))
443 return false;
444
445 if (isFov != c.isFov ||
446 !vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits<double>::epsilon()) ||
447 !vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits<double>::epsilon()) ||
448 width != c.width || height != c.height)
449 return false;
450
451 if (fovNormals.size() != c.fovNormals.size())
452 return false;
453
454 std::vector<vpColVector>::const_iterator it1 = fovNormals.begin();
455 std::vector<vpColVector>::const_iterator it2 = c.fovNormals.begin();
456 for (; it1 != fovNormals.end() && it2 != c.fovNormals.end(); ++it1, ++it2) {
457 if (*it1 != *it2)
458 return false;
459 }
460
461 return true;
462}
463
468 return !(*this == c);
469}
470
477void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
478{
479 if ((!isFov || w != width || h != height) && w != 0 && h != 0) {
480 fovNormals = std::vector<vpColVector>(4);
481
482 isFov = true;
483
484 double hFovAngle = atan(((double)w - u0) * (1.0 / px));
485 double vFovAngle = atan((v0) * (1.0 / py));
486 double minushFovAngle = atan((u0) * (1.0 / px));
487 double minusvFovAngle = atan(((double)h - v0) * (1.0 / py));
488
489 width = w;
490 height = h;
491
492 vpColVector n(3);
493 n = 0;
494 n[0] = 1.0;
495
496 vpRotationMatrix Rleft(0, -minushFovAngle, 0);
497 vpRotationMatrix Rright(0, hFovAngle, 0);
498
499 vpColVector nLeft, nRight;
500
501 nLeft = Rleft * (-n);
502 fovNormals[0] = nLeft.normalize();
503
504 nRight = Rright * n;
505 fovNormals[1] = nRight.normalize();
506
507 n = 0;
508 n[1] = 1.0;
509
510 vpRotationMatrix Rup(vFovAngle, 0, 0);
511 vpRotationMatrix Rdown(-minusvFovAngle, 0, 0);
512
513 vpColVector nUp, nDown;
514
515 nUp = Rup * (-n);
516 fovNormals[2] = nUp.normalize();
517
518 nDown = Rdown * n;
519 fovNormals[3] = nDown.normalize();
520
521 m_hFovAngle = hFovAngle + minushFovAngle;
522 m_vFovAngle = vFovAngle + minusvFovAngle;
523 }
524}
525
538{
539 vpMatrix K(3, 3, 0.);
540 K[0][0] = px;
541 K[1][1] = py;
542 K[0][2] = u0;
543 K[1][2] = v0;
544 K[2][2] = 1.0;
545
546 return K;
547}
560{
561 vpMatrix K_inv(3, 3, 0.);
562 K_inv[0][0] = inv_px;
563 K_inv[1][1] = inv_py;
564 K_inv[0][2] = -u0 * inv_px;
565 K_inv[1][2] = -v0 * inv_py;
566 K_inv[2][2] = 1.0;
567
568 return K_inv;
569}
570
577{
578 std::ios::fmtflags original_flags(std::cout.flags());
579 switch (projModel) {
581 std::cout.precision(10);
582 std::cout << "Camera parameters for perspective projection without distortion:" << std::endl;
583 std::cout << " px = " << px << "\t py = " << py << std::endl;
584 std::cout << " u0 = " << u0 << "\t v0 = " << v0 << std::endl;
585 break;
587 std::cout.precision(10);
588 std::cout << "Camera parameters for perspective projection with distortion:" << std::endl;
589 std::cout << " px = " << px << "\t py = " << py << std::endl;
590 std::cout << " u0 = " << u0 << "\t v0 = " << v0 << std::endl;
591 std::cout << " kud = " << kud << std::endl;
592 std::cout << " kdu = " << kdu << std::endl;
593 break;
595 std::cout << " Coefficients: ";
596 for (unsigned int i = 0; i < m_dist_coefs.size(); i++)
597 std::cout << " " << m_dist_coefs[i];
598 std::cout << std::endl;
599 break;
600 }
601 // Restore ostream format
602 std::cout.flags(original_flags);
603}
611VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam)
612{
613 switch (cam.get_projModel()) {
615 os << "Camera parameters for perspective projection without distortion:" << std::endl;
616 os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
617 os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
618 break;
620 {
621 std::ios_base::fmtflags original_flags = os.flags();
622 os.precision(10);
623 os << "Camera parameters for perspective projection with distortion:" << std::endl;
624 os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
625 os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
626 os << " kud = " << cam.get_kud() << std::endl;
627 os << " kdu = " << cam.get_kdu() << std::endl;
628 os.flags(original_flags); // restore os to standard state
629 }
630 break;
632 {
633 os << "Camera parameters for projection with Kannala-Brandt 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 << " Coefficients: ";
637 std::vector<double> tmp_coefs = cam.getKannalaBrandtDistortionCoefficients();
638 for(unsigned int i = 0; i < tmp_coefs.size(); i++)
639 os << " " << tmp_coefs[i];
640 os << std::endl;
641 }
642 break;
643 }
644 return os;
645}
unsigned int getCols() const
Definition: vpArray2D.h:279
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:493
unsigned int getRows() const
Definition: vpArray2D.h:289
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)
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)
std::vector< double > getKannalaBrandtDistortionCoefficients() const
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:131
vpColVector & normalize()
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ divideByZeroError
Division by zero.
Definition: vpException.h:94
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:154
Implementation of a rotation matrix and operations on such kind of matrices.