52 #include <visp3/core/vpCameraParameters.h>
53 #include <visp3/core/vpDebug.h>
54 #include <visp3/core/vpException.h>
55 #include <visp3/core/vpRotationMatrix.h>
57 const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
58 const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
59 const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
60 const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
61 const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
62 const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
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), isFov(false),
75 m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
76 projModel(DEFAULT_PROJ_TYPE)
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), isFov(false),
87 m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
88 projModel(DEFAULT_PROJ_TYPE)
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), isFov(false),
103 m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
104 projModel(DEFAULT_PROJ_TYPE)
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), isFov(false),
122 m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
123 projModel(DEFAULT_PROJ_TYPE)
137 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), isFov(false),
140 m_hFovAngle(0), m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
141 projModel(DEFAULT_PROJ_TYPE)
151 if (fabs(this->px) < 1e-6) {
154 if (fabs(this->py) < 1e-6) {
157 this->inv_px = 1. / this->px;
158 this->inv_py = 1. / this->py;
209 if (fabs(px) < 1e-6) {
212 if (fabs(py) < 1e-6) {
215 this->inv_px = 1. / px;
216 this->inv_py = 1. / py;
262 double cam_kud,
double cam_kdu)
273 if (fabs(px) < 1e-6) {
276 if (fabs(py) < 1e-6) {
279 this->inv_px = 1. / px;
280 this->inv_py = 1. / py;
290 const std::vector<double> &coefficients)
299 if (fabs(px) < 1e-6) {
302 if (fabs(py) < 1e-6) {
305 this->inv_px = 1. / px;
306 this->inv_py = 1. / py;
308 this->m_dist_coefs = coefficients;
342 if (std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()) {
387 px = u0 / tan(hfov / 2);
388 py = v0 / tan(vfov / 2);
401 projModel = cam.projModel;
408 m_dist_coefs = cam.m_dist_coefs;
414 m_hFovAngle = cam.m_hFovAngle;
415 m_vFovAngle = cam.m_vFovAngle;
418 fovNormals = cam.fovNormals;
428 if (projModel != c.projModel)
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()))
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()))
445 if (isFov != c.isFov || !
vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits<double>::epsilon()) ||
446 !
vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits<double>::epsilon()) || width != c.width ||
450 if (fovNormals.size() != c.fovNormals.size())
453 std::vector<vpColVector>::const_iterator it1 = fovNormals.begin();
454 std::vector<vpColVector>::const_iterator it2 = c.fovNormals.begin();
455 for (; it1 != fovNormals.end() && it2 != c.fovNormals.end(); ++it1, ++it2) {
476 if ((!isFov || w != width || h != height) && w != 0 && h != 0) {
477 fovNormals = std::vector<vpColVector>(4);
481 double hFovAngle = atan(((
double)w - u0) * (1.0 / px));
482 double vFovAngle = atan((v0) * (1.0 / py));
483 double minushFovAngle = atan((u0) * (1.0 / px));
484 double minusvFovAngle = atan(((
double)h - v0) * (1.0 / py));
498 nLeft = Rleft * (-n);
518 m_hFovAngle = hFovAngle + minushFovAngle;
519 m_vFovAngle = vFovAngle + minusvFovAngle;
559 K_inv[0][0] = inv_px;
560 K_inv[1][1] = inv_py;
561 K_inv[0][2] = -u0 * inv_px;
562 K_inv[1][2] = -v0 * inv_py;
575 std::ios::fmtflags original_flags(std::cout.flags());
578 std::cout.precision(10);
579 std::cout <<
"Camera parameters for perspective projection without distortion:" << std::endl;
580 std::cout <<
" px = " << px <<
"\t py = " << py << std::endl;
581 std::cout <<
" u0 = " << u0 <<
"\t v0 = " << v0 << std::endl;
584 std::cout.precision(10);
585 std::cout <<
"Camera parameters for perspective projection with distortion:" << std::endl;
586 std::cout <<
" px = " << px <<
"\t py = " << py << std::endl;
587 std::cout <<
" u0 = " << u0 <<
"\t v0 = " << v0 << std::endl;
588 std::cout <<
" kud = " << kud << std::endl;
589 std::cout <<
" kdu = " << kdu << std::endl;
592 std::cout <<
" Coefficients: ";
593 for (
unsigned int i = 0; i < m_dist_coefs.size(); i++)
594 std::cout <<
" " << m_dist_coefs[i];
595 std::cout << std::endl;
599 std::cout.flags(original_flags);
612 os <<
"Camera parameters for perspective projection without distortion:" << std::endl;
613 os <<
" px = " << cam.
get_px() <<
"\t py = " << cam.
get_py() << std::endl;
614 os <<
" u0 = " << cam.
get_u0() <<
"\t v0 = " << cam.
get_v0() << std::endl;
617 std::ios_base::fmtflags original_flags = os.flags();
619 os <<
"Camera parameters for perspective projection with distortion:" << std::endl;
620 os <<
" px = " << cam.
get_px() <<
"\t py = " << cam.
get_py() << std::endl;
621 os <<
" u0 = " << cam.
get_u0() <<
"\t v0 = " << cam.
get_v0() << std::endl;
622 os <<
" kud = " << cam.
get_kud() << std::endl;
623 os <<
" kdu = " << cam.
get_kdu() << std::endl;
624 os.flags(original_flags);
627 os <<
"Camera parameters for projection with Kannala-Brandt 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 <<
" Coefficients: ";
632 for (
unsigned int i = 0; i < tmp_coefs.size(); i++)
633 os <<
" " << tmp_coefs[i];
unsigned int getCols() const
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
unsigned int getRows() const
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)
vpCameraParametersProjType
@ perspectiveProjWithDistortion
@ ProjWithKannalaBrandtDistortion
@ perspectiveProjWithoutDistortion
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)
virtual ~vpCameraParameters()
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 emited by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ dimensionError
Bad dimension.
@ divideByZeroError
Division by zero.
static bool equal(double x, double y, double s=0.001)
Implementation of a matrix and operations on matrices.
Implementation of a rotation matrix and operations on such kind of matrices.