Visual Servoing Platform  version 3.0.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
vpCameraParameters.cpp
1 /****************************************************************************
2  *
3  * This file is part of the ViSP software.
4  * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * ("GPL") version 2 as published by the Free Software Foundation.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See http://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Camera intrinsic parameters.
32  *
33  * Authors:
34  * Eric Marchand
35  * Anthony Saunier
36  *
37  *****************************************************************************/
38 
39 
47 #include <visp3/core/vpCameraParameters.h>
48 #include <visp3/core/vpDebug.h>
49 #include <visp3/core/vpException.h>
50 #include <visp3/core/vpRotationMatrix.h>
51 #include <cmath>
52 #include <limits>
53 #include <iostream>
54 #include <sstream>
55 #include <iomanip>
56 
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;
64  vpCameraParameters::DEFAULT_PROJ_TYPE =
66 
74  :
75  px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
76  u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
77  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
78  width(0), height(0),
79  isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
80  inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
81  projModel(DEFAULT_PROJ_TYPE)
82 {
83  init() ;
84 }
85 
90  :
91  px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
92  u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
93  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
94  width(0), height(0),
95  isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
96  inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
97  projModel(DEFAULT_PROJ_TYPE)
98 {
99  init(c) ;
100 }
101 
109 vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
110  const double cam_u0, const double cam_v0)
111  :
112  px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
113  u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
114  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
115  width(0), height(0),
116  isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
117  inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
118  projModel(DEFAULT_PROJ_TYPE)
119 {
120  initPersProjWithoutDistortion(cam_px,cam_py,cam_u0,cam_v0) ;
121 }
122 
132 vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
133  const double cam_u0, const double cam_v0,
134  const double cam_kud, const double cam_kdu)
135  :
136  px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
137  u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
138  kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
139  width(0), height(0),
140  isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
141  inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
142  projModel(DEFAULT_PROJ_TYPE)
143 {
144  initPersProjWithDistortion(cam_px,cam_py,cam_u0,cam_v0,cam_kud,cam_kdu) ;
145 }
146 
150 void
152 {
153  if (fabs(this->px)<1e-6)
154  {
155  vpERROR_TRACE("Camera parameter px = 0") ;
157  "Camera parameter px = 0")) ;
158  }
159  if (fabs(this->py)<1e-6)
160  {
161  vpERROR_TRACE("Camera parameter px = 0") ;
163  "Camera parameter px = 0")) ;
164  }
165  this->inv_px = 1./this->px;
166  this->inv_py = 1./this->py;
167 }
168 
205 void
206 vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, const double cam_py,
207  const double cam_u0, const double cam_v0)
208 {
210 
211  this->px = cam_px ;
212  this->py = cam_py ;
213  this->u0 = cam_u0 ;
214  this->v0 = cam_v0 ;
215  this->kud = 0 ;
216  this->kdu = 0 ;
217 
218  if (fabs(px)<1e-6)
219  {
220  vpERROR_TRACE("Camera parameter px = 0") ;
222  "Camera parameter px = 0")) ;
223  }
224  if (fabs(py)<1e-6)
225  {
226  vpERROR_TRACE("Camera parameter px = 0") ;
228  "Camera parameter px = 0")) ;
229  }
230  this->inv_px = 1./px;
231  this->inv_py = 1./py;
232 }
233 
274 void
275 vpCameraParameters::initPersProjWithDistortion(const double cam_px, const double cam_py,
276  const double cam_u0, const double cam_v0,
277  const double cam_kud, const double cam_kdu)
278 {
280 
281  this->px = cam_px ;
282  this->py = cam_py ;
283  this->u0 = cam_u0 ;
284  this->v0 = cam_v0 ;
285  this->kud = cam_kud ;
286  this->kdu = cam_kdu ;
287 
288  if (fabs(px)<1e-6)
289  {
290  vpERROR_TRACE("Camera parameter px = 0") ;
292  "Camera parameter px = 0")) ;
293  }
294  if (fabs(py)<1e-6)
295  {
296  vpERROR_TRACE("Camera parameter px = 0") ;
298  "Camera parameter px = 0")) ;
299  }
300  this->inv_px = 1./px;
301  this->inv_py = 1./py;
302 }
303 
310 {
311 }
312 
316 void
318 {
319  *this = c ;
320 }
321 
322 
337 void
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 
383 void
384 vpCameraParameters::initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov)
385 {
387  u0 = (double)w/2.;
388  v0 = (double)h/2.;
389  px = u0 / tan(hfov/2);
390  py = v0 / tan(vfov/2);
391  kud = 0;
392  kdu = 0;
393  inv_px = 1./px;
394  inv_py = 1./py;
395  computeFov(w, h);
396 }
397 
403 {
404  projModel = cam.projModel ;
405  px = cam.px ;
406  py = cam.py ;
407  u0 = cam.u0 ;
408  v0 = cam.v0 ;
409  kud = cam.kud ;
410  kdu = cam.kdu ;
411 
412  inv_px = cam.inv_px;
413  inv_py = cam.inv_py;
414 
415  isFov = cam.isFov;
416  m_hFovAngle = cam.m_hFovAngle;
417  m_vFovAngle = cam.m_vFovAngle;
418  width = cam.width;
419  height = cam.height;
420  fovNormals = cam.fovNormals;
421 
422  return *this ;
423 }
424 
431 void
432 vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
433 {
434  if( !isFov && w != width && h != height && w != 0 && h != 0){
435  fovNormals = std::vector<vpColVector>(4);
436 
437  isFov = true;
438 
439  double hFovAngle = atan(((double)w - u0) * ( 1.0 / px ));
440  double vFovAngle = atan(( v0 ) * ( 1.0 / py ));
441  double minushFovAngle = atan(( u0 ) * ( 1.0 / px ));
442  double minusvFovAngle = atan(((double)h - v0) * ( 1.0 / py ));
443 
444  width = w;
445  height = h;
446 
447  vpColVector n(3);
448  n = 0;
449  n[0] = 1.0;
450 
451  vpRotationMatrix Rleft(0,-minushFovAngle,0);
452  vpRotationMatrix Rright(0,hFovAngle,0);
453 
454  vpColVector nLeft, nRight;
455 
456  nLeft = Rleft * (-n);
457  fovNormals[0] = nLeft.normalize();
458 
459  nRight = Rright * n;
460  fovNormals[1] = nRight.normalize();
461 
462  n = 0;
463  n[1] = 1.0;
464 
465  vpRotationMatrix Rup(vFovAngle,0,0);
466  vpRotationMatrix Rdown(-minusvFovAngle,0,0);
467 
468  vpColVector nUp, nDown;
469 
470  nUp = Rup * (-n);
471  fovNormals[2] = nUp.normalize();
472 
473  nDown = Rdown * n;
474  fovNormals[3] = nDown.normalize();
475 
476  m_hFovAngle = hFovAngle + minushFovAngle;
477  m_vFovAngle = vFovAngle + minusvFovAngle;
478  }
479 }
480 
481 
493 vpMatrix
495 {
496  vpMatrix K(3, 3, 0.);
497  K[0][0] = px ;
498  K[1][1] = py ;
499  K[0][2] = u0 ;
500  K[1][2] = v0 ;
501  K[2][2] = 1.0 ;
502 
503  return K;
504 }
516 vpMatrix
518 {
519  vpMatrix K_inv(3, 3, 0.);
520  K_inv[0][0] = inv_px ;
521  K_inv[1][1] = inv_py ;
522  K_inv[0][2] = -u0*inv_px ;
523  K_inv[1][2] = -v0*inv_py ;
524  K_inv[2][2] = 1.0 ;
525 
526  return K_inv;
527 }
528 
529 
535 void
537 {
538  std::ios::fmtflags original_flags( std::cout.flags() );
539  switch(projModel){
541  std::cout.precision(10);
542  std::cout << "Camera parameters for perspective projection without distortion:"
543  << std::endl ;
544  std::cout << " px = " << px <<"\t py = "<< py << std::endl ;
545  std::cout << " u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
546  break;
548  std::cout.precision(10);
549  std::cout << "Camera parameters for perspective projection with distortion:"
550  << std::endl ;
551  std::cout << " px = " << px <<"\t py = "<< py << std::endl ;
552  std::cout << " u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
553  std::cout << " kud = " << kud << std::endl ;
554  std::cout << " kdu = " << kdu << std::endl ;
555  break;
556  }
557  // Restore ostream format
558  std::cout.flags(original_flags);
559 }
567 VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam)
568 {
569  switch(cam.get_projModel()){
571  os << "Camera parameters for perspective projection without distortion:"
572  << std::endl ;
573  os << " px = " << cam.get_px() <<"\t py = "<< cam.get_py()
574  << std::endl ;
575  os << " u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
576  << std::endl ;
577  break;
579  std::ios_base::fmtflags original_flags = os.flags();
580  os.precision(10);
581  os << "Camera parameters for perspective projection with distortion:"
582  << std::endl ;
583  os << " px = " << cam.get_px() <<"\t py = "<< cam.get_py()
584  << std::endl ;
585  os << " u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
586  << std::endl ;
587  os << " kud = " << cam.get_kud() << std::endl ;
588  os << " kdu = " << cam.get_kdu() << std::endl ;
589 
590  os.flags(original_flags); // restore os to standard state
591  break;
592  }
593  return os;
594 }
595 
596 
vpMatrix get_K_inverse() const
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void initFromCalibrationMatrix(const vpMatrix &_K)
double get_u0() const
void init()
basic initialization with the default parameters
Perspective projection without distortion model.
#define vpERROR_TRACE
Definition: vpDebug.h:391
error that can be emited by ViSP classes.
Definition: vpException.h:73
double get_py() const
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
vpColVector & normalize()
void initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov)
double get_v0() const
Generic class defining intrinsic camera parameters.
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Definition: vpArray2D.h:267
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
Perspective projection with distortion model.
double get_px() const
double get_kud() const
vpMatrix get_K() const
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
vpCameraParametersProjType get_projModel() const
double get_kdu() const
vpCameraParameters & operator=(const vpCameraParameters &c)
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)
void computeFov(const unsigned int &w, const unsigned int &h)