45 #include <visp3/core/vpCameraParameters.h>
46 #include <visp3/core/vpDebug.h>
47 #include <visp3/core/vpException.h>
48 #include <visp3/core/vpRotationMatrix.h>
49 #include <visp3/core/vpMath.h>
51 const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
52 const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
53 const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
54 const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
55 const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
56 const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
67 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
68 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
69 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
70 m_projModel(DEFAULT_PROJ_TYPE)
79 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
80 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
81 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
82 m_projModel(DEFAULT_PROJ_TYPE)
96 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
97 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
98 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
99 m_projModel(DEFAULT_PROJ_TYPE)
116 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
117 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
118 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
119 m_projModel(DEFAULT_PROJ_TYPE)
134 const std::vector<double> &coefficients)
135 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
136 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
137 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
138 m_projModel(DEFAULT_PROJ_TYPE)
148 if (fabs(this->m_px) < 1e-6) {
151 if (fabs(this->m_py) < 1e-6) {
154 this->m_inv_px = 1. / this->m_px;
155 this->m_inv_py = 1. / this->m_py;
208 this->m_dist_coefs.clear();
210 if (fabs(m_px) < 1e-6) {
213 if (fabs(m_py) < 1e-6) {
216 this->m_inv_px = 1. / m_px;
217 this->m_inv_py = 1. / m_py;
266 double cam_kud,
double cam_kdu)
274 this->m_kud = cam_kud;
275 this->m_kdu = cam_kdu;
276 this->m_dist_coefs.clear();
278 if (fabs(m_px) < 1e-6) {
281 if (fabs(m_py) < 1e-6) {
284 this->m_inv_px = 1. / m_px;
285 this->m_inv_py = 1. / m_py;
298 const std::vector<double> &coefficients)
310 if (fabs(m_px) < 1e-6) {
313 if (fabs(m_py) < 1e-6) {
316 this->m_inv_px = 1. / m_px;
317 this->m_inv_py = 1. / m_py;
319 this->m_dist_coefs = coefficients;
351 if (std::fabs(K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()) {
396 m_u0 =
static_cast<double>(w) / 2.;
397 m_v0 =
static_cast<double>(h) / 2.;
398 m_px = m_u0 / tan(hfov / 2);
399 m_py = m_v0 / tan(vfov / 2);
402 m_inv_px = 1. / m_px;
403 m_inv_py = 1. / m_py;
412 m_projModel = cam.m_projModel;
419 m_dist_coefs = cam.m_dist_coefs;
421 m_inv_px = cam.m_inv_px;
422 m_inv_py = cam.m_inv_py;
424 m_isFov = cam.m_isFov;
425 m_hFovAngle = cam.m_hFovAngle;
426 m_vFovAngle = cam.m_vFovAngle;
427 m_width = cam.m_width;
428 m_height = cam.m_height;
429 m_fovNormals = cam.m_fovNormals;
439 if (m_projModel != c.m_projModel) {
444 if ((!
vpMath::equal(m_px, c.m_px, std::numeric_limits<double>::epsilon())) ||
445 (!
vpMath::equal(m_py, c.m_py, std::numeric_limits<double>::epsilon())) ||
446 (!
vpMath::equal(m_u0, c.m_u0, std::numeric_limits<double>::epsilon()))) {
449 if ((!
vpMath::equal(m_v0, c.m_v0, std::numeric_limits<double>::epsilon())) ||
450 (!
vpMath::equal(m_kud, c.m_kud, std::numeric_limits<double>::epsilon())) ||
451 (!
vpMath::equal(m_kdu, c.m_kdu, std::numeric_limits<double>::epsilon()))) {
454 if ((!
vpMath::equal(m_inv_px, c.m_inv_px, std::numeric_limits<double>::epsilon())) ||
455 (!
vpMath::equal(m_inv_py, c.m_inv_py, std::numeric_limits<double>::epsilon()))) {
459 if (m_dist_coefs.size() != c.m_dist_coefs.size()) {
463 unsigned int m_dist_coefs_size = m_dist_coefs.size();
464 for (
unsigned int i = 0; i < m_dist_coefs_size; ++i) {
465 if (!
vpMath::equal(m_dist_coefs[i], c.m_dist_coefs[i], std::numeric_limits<double>::epsilon())) {
470 if ((m_isFov != c.m_isFov) || (!
vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits<double>::epsilon())) ||
471 (!
vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits<double>::epsilon()))) {
474 if ((m_width != c.m_width) || (m_height != c.m_height)) {
478 if (m_fovNormals.size() != c.m_fovNormals.size()) {
482 std::vector<vpColVector>::const_iterator it1 = m_fovNormals.begin();
483 std::vector<vpColVector>::const_iterator it2 = c.m_fovNormals.begin();
484 for (; (it1 != m_fovNormals.end()) && (it2 != c.m_fovNormals.end()); ++it1, ++it2) {
506 bool cond1 = (!m_isFov) || (w != m_width) || (h != m_height);
507 if (cond1 && (w != 0) && (h != 0)) {
508 m_fovNormals = std::vector<vpColVector>(4);
512 double hFovAngle = atan((
static_cast<double>(w) - m_u0) * (1.0 / m_px));
513 double vFovAngle = atan(m_v0 * (1.0 / m_py));
514 double minushFovAngle = atan(m_u0 * (1.0 / m_px));
515 double minusvFovAngle = atan((
static_cast<double>(h) - m_v0) * (1.0 / m_py));
529 nLeft = Rleft * (-n);
549 m_hFovAngle = hFovAngle + minushFovAngle;
550 m_vFovAngle = vFovAngle + minusvFovAngle;
590 K_inv[0][0] = m_inv_px;
591 K_inv[1][1] = m_inv_py;
592 K_inv[0][2] = -m_u0 * m_inv_px;
593 K_inv[1][2] = -m_v0 * m_inv_py;
606 unsigned int m_dist_coefs_size = m_dist_coefs.size();
607 std::ios::fmtflags original_flags(std::cout.flags());
608 switch (m_projModel) {
610 std::cout.precision(10);
611 std::cout <<
"Camera parameters for perspective projection without distortion:" << std::endl;
612 std::cout <<
" px = " << m_px <<
"\t py = " << m_py << std::endl;
613 std::cout <<
" u0 = " << m_u0 <<
"\t v0 = " << m_v0 << std::endl;
617 std::cout.precision(10);
618 std::cout <<
"Camera parameters for perspective projection with distortion:" << std::endl;
619 std::cout <<
" px = " << m_px <<
"\t py = " << m_py << std::endl;
620 std::cout <<
" u0 = " << m_u0 <<
"\t v0 = " << m_v0 << std::endl;
621 std::cout <<
" kud = " << m_kud << std::endl;
622 std::cout <<
" kdu = " << m_kdu << std::endl;
626 std::cout <<
" Coefficients: ";
627 for (
unsigned int i = 0; i < m_dist_coefs_size; ++i) {
628 std::cout <<
" " << m_dist_coefs[i];
630 std::cout << std::endl;
634 std::cout <<
"projection model not identified" << std::endl;
638 std::cout.flags(original_flags);
651 os <<
"Camera parameters for perspective projection without distortion:" << std::endl;
652 os <<
" px = " << cam.
get_px() <<
"\t py = " << cam.
get_py() << std::endl;
653 os <<
" u0 = " << cam.
get_u0() <<
"\t v0 = " << cam.
get_v0() << std::endl;
657 std::ios_base::fmtflags original_flags = os.flags();
659 os <<
"Camera parameters for perspective projection with distortion:" << std::endl;
660 os <<
" px = " << cam.
get_px() <<
"\t py = " << cam.
get_py() << std::endl;
661 os <<
" u0 = " << cam.
get_u0() <<
"\t v0 = " << cam.
get_v0() << std::endl;
662 os <<
" kud = " << cam.
get_kud() << std::endl;
663 os <<
" kdu = " << cam.
get_kdu() << std::endl;
664 os.flags(original_flags);
668 os <<
"Camera parameters for projection with Kannala-Brandt distortion:" << std::endl;
669 os <<
" px = " << cam.
get_px() <<
"\t py = " << cam.
get_py() << std::endl;
670 os <<
" u0 = " << cam.
get_u0() <<
"\t v0 = " << cam.
get_v0() << std::endl;
671 os <<
" Coefficients: ";
673 unsigned int tmp_coefs_size = tmp_coefs.size();
674 for (
unsigned int i = 0; i < tmp_coefs_size; ++i) {
675 os <<
" " << tmp_coefs[i];
681 std::cout <<
"Unidentified camera parameters model" << std::endl;
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.