1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Camera intrinsic parameters.
33  *
34  * Authors:
35  * Eric Marchand
36  * Anthony Saunier
37  *
38  *****************************************************************************/
39 
40 /*!
41   \file vpCameraParameters.h
42   \brief Declaration of the vpCameraParameters class.
43   Class vpCameraParameters define the camera intrinsic parameters
44 
45 */
46 
47 #ifndef vpCameraParameters_H
48 #define vpCameraParameters_H
49 
50 #include <vector>
51 
52 #include <visp3/core/vpColVector.h>
53 #include <visp3/core/vpConfig.h>
54 #include <visp3/core/vpDebug.h>
55 #include <visp3/core/vpMatrix.h>
56 
57 /*!
58   \class vpCameraParameters
59 
60   \ingroup group_core_camera
61 
62   \brief Generic class defining intrinsic camera parameters.
63 
64   Two camera models are implemented in ViSP.
65 
66   <b>I. Pinhole camera model</b>
67 
68   In this model \cite Marchand16a, a scene view is formed by projecting 3D points
69   into the image plane using a perspective transformation.
70 
71   \f[
72   \left[ \begin{array}{c}
73   u \\
74   v \\
75   1
76   \end{array}\right] =
77   \left[ \begin{array}{ccc}
78   p_x & 0   & u_0  \\
79   0   & p_y & v_0 \\
80   0   & 0   & 1
81   \end{array}\right]
82   \left[ \begin{array}{c}
83   x  \\
84   y   \\
85   1
86   \end{array}\right]
87   \f]
88 
89   where:
90 
91   - \f$(X_c,Y_c,Z_c)\f$ are the coordinates of a 3D point in the camera frame
92   - \f$(x,y)\f$ are the coordinates of the projection of the 3D point in the image plane
93   - \f$(u,v)\f$ are the coordinates in pixels of the projected 3D point
94   - \f$(u_0,v_0)\f$ are the coordinates of the principal point (the
95   intersection of the optical axes with the image plane) that is usually near
96   the image center
97   - \f$p_x\f$ (resp \f$p_y\f$) is the ratio between the focal length of the
98   lens \f$f\f$ in meters and the size of the pixel \f$l_x\f$ in meters:
99   \f$p_x=f/l_x\f$ (resp, \f$l_y\f$ being the height of a pixel,
100   \f$p_y=f/l_y\f$).
101 
102   When \f$Z_c \neq 0\f$, the previous equation is equivalent to the following:
103   \f[
104   \begin{array}{lcl}
105   x &=& X_c / Z_c \\
106   y &=& Y_c / Z_c \\
107   u &=& u_0 + x \; p_x \\
108   v &=& v_0 + y \; p_y
109   \end{array}
110   \f]
111 
112   Real lenses usually have some radial distortion. So, the above model is
113   extended as:
114 
115   \f[
116   \begin{array}{lcl}
117   x &=& X_c / Z_c \\
118   y &=& Y_c / Z_c \\
119   x^{'} &=& x (1 + k_{ud} r^2) \\
120   y^{'} &=& y (1 + k_{ud} r^2) \\
121   r^2 &=& x^2 + y^2 \\
122   u &=& u_0 + x^{'} \; p_x \\
123   v &=& v_0 + y^{'} \; p_y
124   \end{array}
125   \f]
126 
127   where \f$k_{ud}\f$ is the first order radial distorsion. Higher order
128   distorsion coefficients are not considered in ViSP.
129 
130   Now in ViSP we consider also the inverse transformation, where from pixel
131   coordinates we want to compute their normalized coordinates in the image
132   plane. Previous equations could be written like:
133 
134   \f[
135   \begin{array}{lcl}
136   x &=& (u - u_0) / p_x \\
137   y &=& (v - v_0) / p_y
138   \end{array}
139   \f]
140 
141   Considering radial distortion, the above model is extended as:
142   \f[
143   \begin{array}{lcl}
144   (u-u_0)^{'} &=& (u-u_0) (1 + k_{du} r^2) \\
145   (v-v_0)^{'} &=& (v-v_0) (1 + k_{du} r^2) \\
146   r^2 &=& ((u-u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \\
147   x &=& (u - u_0)^{'} / p_x \\
148   y &=& (v - v_0)^{'} / p_y
149   \end{array}
150   \f]
151 
152   Finally, in ViSP the main intrinsic camera parameters are \f$(p_x, p_y)\f$
153   the ratio between the focal length and the size of a pixel, and \f$(u_0,
154   v_0)\f$ the coordinates of the principal point in pixel. The lens
155   distortion can also be considered by two additional parameters
156   \f$(k_{ud}, k_{du})\f$.
157 
158   \note The \ref tutorial-calibration-intrinsic shows how to calibrate a camera
159   to estimate the parameters corresponding to the model implemented in this
160   class.
161 
162   \note Note also that \ref tutorial-bridge-opencv gives the correspondance
163   between ViSP and OpenCV camera modelization.
164 
165   \note The conversion from pixel coordinates \f$(u,v)\f$ in the normalized
166   space \f$(x,y)\f$ is implemented in vpPixelMeterConversion, whereas
167   the conversion from normalized coordinates into pixel is implemented
168   in vpMeterPixelConversion.
169 
170   From a practical point of view, two kinds of camera modelisation are
171   implemented in this class:
172 
173   <b>1. Camera parameters for a perspective projection without distortion
174   model</b>
175 
176   In this modelisation, only \f$u_0,v_0,p_x,p_y\f$ parameters are considered.
177 
178   Initialization of such a model can be done using:
179   - initPersProjWithoutDistortion() that allows to set \f$u_0,v_0,p_x,p_y\f$
180   parameters;
181   - initFromFov() that computes the parameters from an image size and a camera
182   field of view.
183 
184   <b>2. Camera parameters for a perspective projection with distortion
185   model</b>
186 
187   In this modelisation, all the parameters \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$
188   are considered. Initialization of such a model can be done using:
189   - initPersProjWithDistortion() that allows to set
190   \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$ parameters;
191 
192   The selection of the camera model (without or with distorsion) is done
193   during vpCameraParameters initialisation.
194 
195   Here an example of camera initialisation, for a model without distortion. A
196   complete example is given in initPersProjWithoutDistortion().
197 
198 \code
199   double px = 600; double py = 600; double u0 = 320; double v0 = 240;
200 
201   // Create a camera parameter container
202   vpCameraParameters cam;
203   // Camera initialization with a perspective projection without distortion
204   // model
205   cam.initPersProjWithoutDistortion(px,py,u0,v0);
206   // It is also possible to print the current camera parameters
207   std::cout << cam << std::endl;
208 \endcode
209 
210   Here an example of camera initialisation, for a model with distortion. A
211   complete example is given in initPersProjWithDistortion().
212 
213 \code
214   double px = 600; double py = 600;
215   double u0 = 320; double v0 = 240;
216   double kud = -0.19; double kdu = 0.20;
217 
218   // Create a camera parameter container
219   vpCameraParameters cam;
220 
221   // Camera initialization with a perspective projection without distortion
222   model cam.initPersProjWithDistortion(px,py,u0,v0,kud,kdu);
223 \endcode
224 
225   The code below shows how to know the currently used projection model:
226   \code
227   vpCameraParameters cam;
228   ...
229   vpCameraParameters::vpCameraParametersProjType projModel;
230   projModel = cam.get_projModel(); // Get the projection model type
231   \endcode
232 
233   An XML parser for camera parameters is also provided in vpXmlParserCamera.
234 
235   <b>II. Kannala-Brandt camera model</b>
236 
237   This model \cite KannalaBrandt deals with fish-eye lenses designed to cover
238   the whole hemispherical field in front of the camera and the angle of view
239   is very large. In this case, the inherent distortion of a fish-eye lens should
240   not be considered only as a derivation from the pinhole model.
241 
242   The following projection in the general form is adapted:
243 
244   \f[
245   \begin{array}{lcl}
246   r(\theta) &=& k_1 \theta + k_2 \theta^3 + k_3 \theta^5 + k_4 \theta^7 + k_5 \theta^9
247   \end{array}
248   \f]
249 
250   where:
251   - \f$\theta\f$ is the angle in rad between a point in the real world and the
252   optical axis.
253   - \f$r\f$ is the distance between the image point and the principal point.
254 
255   In ViSP, we only consider radially symmetric distortions (caused by fisheye lenses).
256 
257 */
258 class VISP_EXPORT vpCameraParameters
259 {
260   friend class vpMeterPixelConversion;
261   friend class vpPixelMeterConversion;
262 
263 public:
264   typedef enum {
265     perspectiveProjWithoutDistortion, //!< Perspective projection without
266                                       //!< distortion model
267     perspectiveProjWithDistortion,    //!< Perspective projection with distortion
268                                       //!< model
269     ProjWithKannalaBrandtDistortion   //!< Projection with Kannala-Brandt distortion
270                                       //!< model
271   } vpCameraParametersProjType;
272 
273   // generic functions
274   vpCameraParameters();
275   vpCameraParameters(const vpCameraParameters &c);
276   vpCameraParameters(double px, double py, double u0, double v0);
277   vpCameraParameters(double px, double py, double u0, double v0, double kud, double kdu);
278   vpCameraParameters(double px, double py, double u0, double v0, const std::vector<double> &distortion_coefficients);
279 
280   vpCameraParameters &operator=(const vpCameraParameters &c);
281   bool operator==(const vpCameraParameters &c) const;
282   bool operator!=(const vpCameraParameters &c) const;
283   virtual ~vpCameraParameters();
284 
285   void init();
286   void init(const vpCameraParameters &c);
287   void initFromCalibrationMatrix(const vpMatrix &_K);
288   void initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov);
289   void initPersProjWithoutDistortion(double px, double py, double u0, double v0);
290   void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu);
291   void initProjWithKannalaBrandtDistortion(double px, double py, double u0, double v0, const std::vector<double> &distortion_coefficients);
292 
293   /*!
294     Specify if the fov has been computed.
295 
296     \sa computeFov()
297 
298     \return True if the fov has been computed, False otherwise.
299   */
isFovComputed()300   inline bool isFovComputed() const { return isFov; }
301 
302   void computeFov(const unsigned int &w, const unsigned int &h);
303 
304   /*!
305     Get the horizontal angle in radian of the field of view.
306 
307     \return FOV horizontal angle computed with px and width.
308 
309     \sa computeFov(), getVerticalFovAngle()
310   */
getHorizontalFovAngle()311   inline double getHorizontalFovAngle() const
312   {
313     if (!isFov) {
314       vpTRACE("Warning: The FOV is not computed, getHorizontalFovAngle() "
315               "won't be significant.");
316     }
317     return m_hFovAngle;
318   }
319 
320   /*!
321     Get the vertical angle in radian of the field of view.
322 
323     \return FOV vertical angle computed with py and height.
324 
325     \sa computeFov(), getHorizontalFovAngle()
326   */
getVerticalFovAngle()327   inline double getVerticalFovAngle() const
328   {
329     if (!isFov) {
330       vpTRACE("Warning: The FOV is not computed, getVerticalFovAngle() won't "
331               "be significant.");
332     }
333     return m_vFovAngle;
334   }
335 
336   /*!
337     Get the list of the normals corresponding to planes describing the field
338     of view.
339       - vector[0] : Left Normal.
340       - vector[1] : Right Normal.
341       - vector[2] : Up Normal.
342       - vector[3] : Down Normal.
343 
344     \sa computeFov()
345 
346     \return List of the normals.
347   */
getFovNormals()348   inline std::vector<vpColVector> getFovNormals() const
349   {
350     if (!isFov) {
351       vpTRACE("Warning: The FOV is not computed, getFovNormals() won't be "
352               "significant.");
353     }
354     return fovNormals;
355   }
356 
get_px()357   inline double get_px() const { return px; }
get_px_inverse()358   inline double get_px_inverse() const { return inv_px; }
get_py_inverse()359   inline double get_py_inverse() const { return inv_py; }
get_py()360   inline double get_py() const { return py; }
get_u0()361   inline double get_u0() const { return u0; }
get_v0()362   inline double get_v0() const { return v0; }
get_kud()363   inline double get_kud() const { return kud; }
get_kdu()364   inline double get_kdu() const { return kdu; }
getKannalaBrandtDistortionCoefficients()365   inline std::vector<double> getKannalaBrandtDistortionCoefficients() const { return m_dist_coefs; }
366 
get_projModel()367   inline vpCameraParametersProjType get_projModel() const { return projModel; }
368 
369   vpMatrix get_K() const;
370   vpMatrix get_K_inverse() const;
371 
372   void printParameters();
373   friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam);
374 
375 private:
376   static const double DEFAULT_U0_PARAMETER;
377   static const double DEFAULT_V0_PARAMETER;
378   static const double DEFAULT_PX_PARAMETER;
379   static const double DEFAULT_PY_PARAMETER;
380   static const double DEFAULT_KUD_PARAMETER;
381   static const double DEFAULT_KDU_PARAMETER;
382   static const vpCameraParametersProjType DEFAULT_PROJ_TYPE;
383 
384   double px, py;                       //!< Pixel size
385   double u0, v0;                       //!< Principal point
386   double kud;                          //!< Radial distortion (from undistorted to distorted)
387   double kdu;                          //!< Radial distortion (from distorted to undistorted)
388   std::vector<double> m_dist_coefs;    //!< Coefficients for Kannala-Brandt distorsion model
389 
390   unsigned int width;                  //!< Width of the image used for the fov computation
391   unsigned int height;                 //!< Height of the image used for the fov computation
392   bool isFov;                          //!< Boolean to specify if the fov has been computed
393   double m_hFovAngle;                  //!< Field of view horizontal angle
394   double m_vFovAngle;                  //!< Field of view vertical angle
395   std::vector<vpColVector> fovNormals; //!< Normals of the planes describing the fov
396 
397   double inv_px, inv_py;
398 
399   vpCameraParametersProjType projModel; //!< used projection model
400 };
401 
402 #endif
403