51 #include <visp3/core/vpCameraParameters.h>
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpException.h>
54 #include <visp3/core/vpRotationMatrix.h>
56 const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
57 const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
58 const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
59 const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
60 const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
61 const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
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)
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)
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)
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)
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)
150 if (fabs(this->px) < 1e-6) {
153 if (fabs(this->py) < 1e-6) {
156 this->inv_px = 1. / this->px;
157 this->inv_py = 1. / this->py;
208 this->m_dist_coefs.clear();
210 if (fabs(px) < 1e-6) {
213 if (fabs(py) < 1e-6) {
216 this->inv_px = 1. / px;
217 this->inv_py = 1. / py;
263 double cam_kud,
double cam_kdu)
273 this->m_dist_coefs.clear();
275 if (fabs(px) < 1e-6) {
278 if (fabs(py) < 1e-6) {
281 this->inv_px = 1. / px;
282 this->inv_py = 1. / py;
292 const std::vector<double> &coefficients)
304 if (fabs(px) < 1e-6) {
307 if (fabs(py) < 1e-6) {
310 this->inv_px = 1. / px;
311 this->inv_py = 1. / py;
313 this->m_dist_coefs = coefficients;
347 if (std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()) {
392 px = u0 / tan(hfov / 2);
393 py = v0 / tan(vfov / 2);
406 projModel = cam.projModel;
413 m_dist_coefs = cam.m_dist_coefs;
419 m_hFovAngle = cam.m_hFovAngle;
420 m_vFovAngle = cam.m_vFovAngle;
423 fovNormals = cam.fovNormals;
433 if (projModel != c.projModel)
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()))
446 if(m_dist_coefs.size() != c.m_dist_coefs.size())
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()))
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 ||
458 if (fovNormals.size() != c.fovNormals.size())
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) {
484 if ((!isFov || w != width || h != height) && w != 0 && h != 0) {
485 fovNormals = std::vector<vpColVector>(4);
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));
506 nLeft = Rleft * (-n);
526 m_hFovAngle = hFovAngle + minushFovAngle;
527 m_vFovAngle = vFovAngle + minusvFovAngle;
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;
583 std::ios::fmtflags original_flags(std::cout.flags());
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;
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;
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;
607 std::cout.flags(original_flags);
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;
625 std::ios_base::fmtflags original_flags = os.flags();
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);
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: ";
640 for (
unsigned int i = 0; i < tmp_coefs.size(); i++)
641 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
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)
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 emitted 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 threshold=0.001)
Implementation of a matrix and operations on matrices.
Implementation of a rotation matrix and operations on such kind of matrices.