Visual Servoing Platform  version 3.0.1
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 
Implementation of a matrix and operations on matrices.
Definition: vpMatrix.h:97
void initFromCalibrationMatrix(const vpMatrix &_K)
void init()
basic initialization with the default parameters
double get_kdu() const
Perspective projection without distortion model.
vpCameraParametersProjType get_projModel() const
#define vpERROR_TRACE
Definition: vpDebug.h:391
error that can be emited by ViSP classes.
Definition: vpException.h:73
unsigned int getRows() const
Return the number of rows of the 2D array.
Definition: vpArray2D.h:152
vpMatrix get_K() const
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)
unsigned int getCols() const
Return the number of columns of the 2D array.
Definition: vpArray2D.h:154
vpColVector & normalize()
void initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov)
Generic class defining intrinsic camera parameters.
Perspective projection with distortion model.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpCameraParameters &cam)
double get_kud() 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)
vpMatrix get_K_inverse() const
void computeFov(const unsigned int &w, const unsigned int &h)