45 #include <visp3/core/vpCameraParameters.h>
46 #include <visp3/core/vpException.h>
47 #include <visp3/core/vpRotationMatrix.h>
48 #include <visp3/core/vpMath.h>
52 const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
53 const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
54 const double vpCameraParameters::DEFAULT_U0_PARAMETER = 192.0;
55 const double vpCameraParameters::DEFAULT_V0_PARAMETER = 144.0;
56 const double vpCameraParameters::DEFAULT_KUD_PARAMETER = 0.0;
57 const double vpCameraParameters::DEFAULT_KDU_PARAMETER = 0.0;
68 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
69 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
70 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
71 m_projModel(DEFAULT_PROJ_TYPE)
80 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
81 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
82 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
83 m_projModel(DEFAULT_PROJ_TYPE)
97 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
98 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
99 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
100 m_projModel(DEFAULT_PROJ_TYPE)
117 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
118 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
119 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
120 m_projModel(DEFAULT_PROJ_TYPE)
135 const std::vector<double> &coefficients)
136 : m_px(DEFAULT_PX_PARAMETER), m_py(DEFAULT_PY_PARAMETER), m_u0(DEFAULT_U0_PARAMETER), m_v0(DEFAULT_V0_PARAMETER),
137 m_kud(DEFAULT_KUD_PARAMETER), m_kdu(DEFAULT_KDU_PARAMETER), m_dist_coefs(), m_width(0), m_height(0), m_isFov(false),
138 m_hFovAngle(0), m_vFovAngle(0), m_fovNormals(), m_inv_px(1. / DEFAULT_PX_PARAMETER), m_inv_py(1. / DEFAULT_PY_PARAMETER),
139 m_projModel(DEFAULT_PROJ_TYPE)
149 if (fabs(this->m_px) < 1e-6) {
152 if (fabs(this->m_py) < 1e-6) {
155 this->m_inv_px = 1. / this->m_px;
156 this->m_inv_py = 1. / this->m_py;
213 this->m_dist_coefs.clear();
215 if (fabs(m_px) < 1e-6) {
218 if (fabs(m_py) < 1e-6) {
221 this->m_inv_px = 1. / m_px;
222 this->m_inv_py = 1. / m_py;
275 double cam_kud,
double cam_kdu)
283 this->m_kud = cam_kud;
284 this->m_kdu = cam_kdu;
285 this->m_dist_coefs.clear();
287 if (fabs(m_px) < 1e-6) {
290 if (fabs(m_py) < 1e-6) {
293 this->m_inv_px = 1. / m_px;
294 this->m_inv_py = 1. / m_py;
307 const std::vector<double> &coefficients)
319 if (fabs(m_px) < 1e-6) {
322 if (fabs(m_py) < 1e-6) {
325 this->m_inv_px = 1. / m_px;
326 this->m_inv_py = 1. / m_py;
328 this->m_dist_coefs = coefficients;
357 const unsigned int nparam = 3;
358 const unsigned int index_0 = 0;
359 const unsigned int index_1 = 1;
360 const unsigned int index_2 = 2;
364 if (std::fabs(K[index_2][index_2] - 1.0) > std::numeric_limits<double>::epsilon()) {
413 m_u0 =
static_cast<double>(w) / 2.;
414 m_v0 =
static_cast<double>(h) / 2.;
415 m_px = m_u0 / tan(hfov / 2.);
416 m_py = m_v0 / tan(vfov / 2.);
419 m_inv_px = 1. / m_px;
420 m_inv_py = 1. / m_py;
429 m_projModel = cam.m_projModel;
436 m_dist_coefs = cam.m_dist_coefs;
438 m_inv_px = cam.m_inv_px;
439 m_inv_py = cam.m_inv_py;
441 m_isFov = cam.m_isFov;
442 m_hFovAngle = cam.m_hFovAngle;
443 m_vFovAngle = cam.m_vFovAngle;
444 m_width = cam.m_width;
445 m_height = cam.m_height;
446 m_fovNormals = cam.m_fovNormals;
456 if (m_projModel != c.m_projModel) {
461 if ((!
vpMath::equal(m_px, c.m_px, std::numeric_limits<double>::epsilon())) ||
462 (!
vpMath::equal(m_py, c.m_py, std::numeric_limits<double>::epsilon())) ||
463 (!
vpMath::equal(m_u0, c.m_u0, std::numeric_limits<double>::epsilon()))) {
466 if ((!
vpMath::equal(m_v0, c.m_v0, std::numeric_limits<double>::epsilon())) ||
467 (!
vpMath::equal(m_kud, c.m_kud, std::numeric_limits<double>::epsilon())) ||
468 (!
vpMath::equal(m_kdu, c.m_kdu, std::numeric_limits<double>::epsilon()))) {
471 if ((!
vpMath::equal(m_inv_px, c.m_inv_px, std::numeric_limits<double>::epsilon())) ||
472 (!
vpMath::equal(m_inv_py, c.m_inv_py, std::numeric_limits<double>::epsilon()))) {
476 if (m_dist_coefs.size() != c.m_dist_coefs.size()) {
480 size_t m_dist_coefs_size = m_dist_coefs.size();
481 for (
size_t i = 0; i < m_dist_coefs_size; ++i) {
482 if (!
vpMath::equal(m_dist_coefs[i], c.m_dist_coefs[i], std::numeric_limits<double>::epsilon())) {
487 if ((m_isFov != c.m_isFov) || (!
vpMath::equal(m_hFovAngle, c.m_hFovAngle, std::numeric_limits<double>::epsilon())) ||
488 (!
vpMath::equal(m_vFovAngle, c.m_vFovAngle, std::numeric_limits<double>::epsilon()))) {
491 if ((m_width != c.m_width) || (m_height != c.m_height)) {
495 if (m_fovNormals.size() != c.m_fovNormals.size()) {
499 std::vector<vpColVector>::const_iterator it1 = m_fovNormals.begin();
500 std::vector<vpColVector>::const_iterator it1_end = m_fovNormals.end();
501 std::vector<vpColVector>::const_iterator it2 = c.m_fovNormals.begin();
502 std::vector<vpColVector>::const_iterator it2_end = c.m_fovNormals.end();
504 for (; (it1 != it1_end) && (it2 != it2_end); ++it1, ++it2) {
526 bool cond1 = (!m_isFov) || (w != m_width) || (h != m_height);
527 if (cond1 && (w != 0) && (h != 0)) {
528 const unsigned int nparam_3 = 3;
529 const unsigned int nparam_4 = 4;
530 const unsigned int index_0 = 0;
531 const unsigned int index_1 = 1;
532 const unsigned int index_2 = 2;
533 const unsigned int index_3 = 3;
534 m_fovNormals = std::vector<vpColVector>(nparam_4);
538 double hFovAngle = atan((
static_cast<double>(w) - m_u0) * (1.0 / m_px));
539 double vFovAngle = atan(m_v0 * (1.0 / m_py));
540 double minushFovAngle = atan(m_u0 * (1.0 / m_px));
541 double minusvFovAngle = atan((
static_cast<double>(h) - m_v0) * (1.0 / m_py));
555 nLeft = Rleft * (-n);
556 m_fovNormals[index_0] = nLeft.
normalize();
559 m_fovNormals[index_1] = nRight.
normalize();
573 m_fovNormals[index_3] = nDown.
normalize();
575 m_hFovAngle = hFovAngle + minushFovAngle;
576 m_vFovAngle = vFovAngle + minusvFovAngle;
593 const unsigned int index_0 = 0;
594 const unsigned int index_1 = 1;
595 const unsigned int index_2 = 2;
597 K[index_0][index_0] = m_px;
598 K[index_1][index_1] = m_py;
599 K[index_0][index_2] = m_u0;
600 K[index_1][index_2] = m_v0;
601 K[index_2][index_2] = 1.0;
618 const unsigned int index_0 = 0;
619 const unsigned int index_1 = 1;
620 const unsigned int index_2 = 2;
623 K_inv[index_0][index_0] = m_inv_px;
624 K_inv[index_1][index_1] = m_inv_py;
625 K_inv[index_0][index_2] = -m_u0 * m_inv_px;
626 K_inv[index_1][index_2] = -m_v0 * m_inv_py;
627 K_inv[index_2][index_2] = 1.0;
639 size_t m_dist_coefs_size = m_dist_coefs.size();
640 std::ios::fmtflags original_flags(std::cout.flags());
641 switch (m_projModel) {
643 std::cout.precision(10);
644 std::cout <<
"Camera parameters for perspective projection without distortion:" << std::endl;
645 std::cout <<
" px = " << m_px <<
"\t py = " << m_py << std::endl;
646 std::cout <<
" u0 = " << m_u0 <<
"\t v0 = " << m_v0 << std::endl;
650 std::cout.precision(10);
651 std::cout <<
"Camera parameters for perspective projection with distortion:" << std::endl;
652 std::cout <<
" px = " << m_px <<
"\t py = " << m_py << std::endl;
653 std::cout <<
" u0 = " << m_u0 <<
"\t v0 = " << m_v0 << std::endl;
654 std::cout <<
" kud = " << m_kud << std::endl;
655 std::cout <<
" kdu = " << m_kdu << std::endl;
659 std::cout <<
" Coefficients: ";
660 for (
size_t i = 0; i < m_dist_coefs_size; ++i) {
661 std::cout <<
" " << m_dist_coefs[i];
663 std::cout << std::endl;
667 std::cout <<
"projection model not identified" << std::endl;
671 std::cout.flags(original_flags);
684 os <<
"Camera parameters for perspective projection without distortion:" << std::endl;
685 os <<
" px = " << cam.
get_px() <<
"\t py = " << cam.
get_py() << std::endl;
686 os <<
" u0 = " << cam.
get_u0() <<
"\t v0 = " << cam.
get_v0() << std::endl;
690 std::ios_base::fmtflags original_flags = os.flags();
691 const unsigned int precision = 10;
692 os.precision(precision);
693 os <<
"Camera parameters for perspective projection with distortion:" << std::endl;
694 os <<
" px = " << cam.
get_px() <<
"\t py = " << cam.
get_py() << std::endl;
695 os <<
" u0 = " << cam.
get_u0() <<
"\t v0 = " << cam.
get_v0() << std::endl;
696 os <<
" kud = " << cam.
get_kud() << std::endl;
697 os <<
" kdu = " << cam.
get_kdu() << std::endl;
698 os.flags(original_flags);
702 os <<
"Camera parameters for projection with Kannala-Brandt distortion:" << std::endl;
703 os <<
" px = " << cam.
get_px() <<
"\t py = " << cam.
get_py() << std::endl <<
" u0 = " << cam.
get_u0() <<
"\t v0 = " << cam.
get_v0() << std::endl;
704 os <<
" Coefficients: ";
706 size_t tmp_coefs_size = tmp_coefs.size();
707 for (
size_t i = 0; i < tmp_coefs_size; ++i) {
708 os <<
" " << tmp_coefs[i];
714 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.