1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
15 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
16 // Third party copyrights are property of their respective owners.
18 // Redistribution and use in source and binary forms, with or without modification,
19 // are permitted provided that the following conditions are met:
21 // * Redistribution's of source code must retain the above copyright notice,
22 // this list of conditions and the following disclaimer.
24 // * Redistribution's in binary form must reproduce the above copyright notice,
25 // this list of conditions and the following disclaimer in the documentation
26 // and/or other materials provided with the distribution.
28 // * The name of the copyright holders may not be used to endorse or promote products
29 // derived from this software without specific prior written permission.
31 // This software is provided by the copyright holders and contributors "as is" and
32 // any express or implied warranties, including, but not limited to, the implied
33 // warranties of merchantability and fitness for a particular purpose are disclaimed.
34 // In no event shall the Intel Corporation or contributors be liable for any direct,
35 // indirect, incidental, special, exemplary, or consequential damages
36 // (including, but not limited to, procurement of substitute goods or services;
37 // loss of use, data, or profits; or business interruption) however caused
38 // and on any theory of liability, whether in contract, strict liability,
39 // or tort (including negligence or otherwise) arising in any way out of
40 // the use of this software, even if advised of the possibility of such damage.
44 #ifndef OPENCV_CALIB3D_HPP
45 #define OPENCV_CALIB3D_HPP
47 #include "opencv2/core.hpp"
48 #include "opencv2/features2d.hpp"
49 #include "opencv2/core/affine.hpp"
52 @defgroup calib3d Camera Calibration and 3D Reconstruction
54 The functions in this section use a so-called pinhole camera model. The view of a scene
55 is obtained by projecting a scene's 3D point \f$P_w\f$ into the image plane using a perspective
56 transformation which forms the corresponding pixel \f$p\f$. Both \f$P_w\f$ and \f$p\f$ are
57 represented in homogeneous coordinates, i.e. as 3D and 2D homogeneous vector respectively. You will
58 find a brief introduction to projective geometry, homogeneous vectors and homogeneous
59 transformations at the end of this section's introduction. For more succinct notation, we often drop
60 the 'homogeneous' and say vector instead of homogeneous vector.
62 The distortion-free projective transformation given by a pinhole camera model is shown below.
64 \f[s \; p = A \begin{bmatrix} R|t \end{bmatrix} P_w,\f]
66 where \f$P_w\f$ is a 3D point expressed with respect to the world coordinate system,
67 \f$p\f$ is a 2D pixel in the image plane, \f$A\f$ is the intrinsic camera matrix,
68 \f$R\f$ and \f$t\f$ are the rotation and translation that describe the change of coordinates from
69 world to camera coordinate systems (or camera frame) and \f$s\f$ is the projective transformation's
70 arbitrary scaling and not part of the camera model.
72 The intrinsic camera matrix \f$A\f$ (notation used as in @cite Zhang2000 and also generally notated
73 as \f$K\f$) projects 3D points given in the camera coordinate system to 2D pixel coordinates, i.e.
77 The camera matrix \f$A\f$ is composed of the focal lengths \f$f_x\f$ and \f$f_y\f$, which are
78 expressed in pixel units, and the principal point \f$(c_x, c_y)\f$, that is usually close to the
81 \f[A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1},\f]
85 \f[s \vecthree{u}{v}{1} = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1} \vecthree{X_c}{Y_c}{Z_c}.\f]
87 The matrix of intrinsic parameters does not depend on the scene viewed. So, once estimated, it can
88 be re-used as long as the focal length is fixed (in case of a zoom lens). Thus, if an image from the
89 camera is scaled by a factor, all of these parameters need to be scaled (multiplied/divided,
90 respectively) by the same factor.
92 The joint rotation-translation matrix \f$[R|t]\f$ is the matrix product of a projective
93 transformation and a homogeneous transformation. The 3-by-4 projective transformation maps 3D points
94 represented in camera coordinates to 2D poins in the image plane and represented in normalized
95 camera coordinates \f$x' = X_c / Z_c\f$ and \f$y' = Y_c / Z_c\f$:
97 \f[Z_c \begin{bmatrix}
101 \end{bmatrix} = \begin{bmatrix}
113 The homogeneous transformation is encoded by the extrinsic parameters \f$R\f$ and \f$t\f$ and
114 represents the change of basis from world coordinate system \f$w\f$ to the camera coordinate sytem
115 \f$c\f$. Thus, given the representation of the point \f$P\f$ in world coordinates, \f$P_w\f$, we
116 obtain \f$P\f$'s representation in the camera coordinate system, \f$P_c\f$, by
118 \f[P_c = \begin{bmatrix}
121 \end{bmatrix} P_w,\f]
123 This homogeneous transformation is composed out of \f$R\f$, a 3-by-3 rotation matrix, and \f$t\f$, a
124 3-by-1 translation vector:
129 \end{bmatrix} = \begin{bmatrix}
130 r_{11} & r_{12} & r_{13} & t_x \\
131 r_{21} & r_{22} & r_{23} & t_y \\
132 r_{31} & r_{32} & r_{33} & t_z \\
144 \end{bmatrix} = \begin{bmatrix}
145 r_{11} & r_{12} & r_{13} & t_x \\
146 r_{21} & r_{22} & r_{23} & t_y \\
147 r_{31} & r_{32} & r_{33} & t_z \\
157 Combining the projective transformation and the homogeneous transformation, we obtain the projective
158 transformation that maps 3D points in world coordinates into 2D points in the image plane and in
159 normalized camera coordinates:
161 \f[Z_c \begin{bmatrix}
165 \end{bmatrix} = \begin{bmatrix} R|t \end{bmatrix} \begin{bmatrix}
170 \end{bmatrix} = \begin{bmatrix}
171 r_{11} & r_{12} & r_{13} & t_x \\
172 r_{21} & r_{22} & r_{23} & t_y \\
173 r_{31} & r_{32} & r_{33} & t_z
182 with \f$x' = X_c / Z_c\f$ and \f$y' = Y_c / Z_c\f$. Putting the equations for instrincs and extrinsics together, we can write out
183 \f$s \; p = A \begin{bmatrix} R|t \end{bmatrix} P_w\f$ as
185 \f[s \vecthree{u}{v}{1} = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}
187 r_{11} & r_{12} & r_{13} & t_x \\
188 r_{21} & r_{22} & r_{23} & t_y \\
189 r_{31} & r_{32} & r_{33} & t_z
198 If \f$Z_c \ne 0\f$, the transformation above is equivalent to the following,
203 \end{bmatrix} = \begin{bmatrix}
210 \f[\vecthree{X_c}{Y_c}{Z_c} = \begin{bmatrix}
212 \end{bmatrix} \begin{bmatrix}
219 The following figure illustrates the pinhole camera model.
221 ![Pinhole camera model](pics/pinhole_camera_model.png)
223 Real lenses usually have some distortion, mostly radial distortion, and slight tangential distortion.
224 So, the above model is extended as:
229 \end{bmatrix} = \begin{bmatrix}
239 \end{bmatrix} = \begin{bmatrix}
240 x' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + 2 p_1 x' y' + p_2(r^2 + 2 x'^2) + s_1 r^2 + s_2 r^4 \\
241 y' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + p_1 (r^2 + 2 y'^2) + 2 p_2 x' y' + s_3 r^2 + s_4 r^4 \\
246 \f[r^2 = x'^2 + y'^2\f]
253 \end{bmatrix} = \begin{bmatrix}
260 The distortion parameters are the radial coefficients \f$k_1\f$, \f$k_2\f$, \f$k_3\f$, \f$k_4\f$, \f$k_5\f$, and \f$k_6\f$
261 ,\f$p_1\f$ and \f$p_2\f$ are the tangential distortion coefficients, and \f$s_1\f$, \f$s_2\f$, \f$s_3\f$, and \f$s_4\f$,
262 are the thin prism distortion coefficients. Higher-order coefficients are not considered in OpenCV.
264 The next figures show two common types of radial distortion: barrel distortion
265 (\f$ 1 + k_1 r^2 + k_2 r^4 + k_3 r^6 \f$ monotonically decreasing)
266 and pincushion distortion (\f$ 1 + k_1 r^2 + k_2 r^4 + k_3 r^6 \f$ monotonically increasing).
267 Radial distortion is always monotonic for real lenses,
268 and if the estimator produces a non-monotonic result,
269 this should be considered a calibration failure.
270 More generally, radial distortion must be monotonic and the distortion function must be bijective.
271 A failed estimation result may look deceptively good near the image center
272 but will work poorly in e.g. AR/SFM applications.
273 The optimization method used in OpenCV camera calibration does not include these constraints as
274 the framework does not support the required integer programming and polynomial inequalities.
275 See [issue #15992](https://github.com/opencv/opencv/issues/15992) for additional information.
277 ![](pics/distortion_examples.png)
278 ![](pics/distortion_examples2.png)
280 In some cases, the image sensor may be tilted in order to focus an oblique plane in front of the
281 camera (Scheimpflug principle). This can be useful for particle image velocimetry (PIV) or
282 triangulation with a laser fan. The tilt causes a perspective distortion of \f$x''\f$ and
283 \f$y''\f$. This distortion can be modeled in the following way, see e.g. @cite Louhichi07.
288 \end{bmatrix} = \begin{bmatrix}
295 \f[s\vecthree{x'''}{y'''}{1} =
296 \vecthreethree{R_{33}(\tau_x, \tau_y)}{0}{-R_{13}(\tau_x, \tau_y)}
297 {0}{R_{33}(\tau_x, \tau_y)}{-R_{23}(\tau_x, \tau_y)}
298 {0}{0}{1} R(\tau_x, \tau_y) \vecthree{x''}{y''}{1}\f]
300 and the matrix \f$R(\tau_x, \tau_y)\f$ is defined by two rotations with angular parameter
301 \f$\tau_x\f$ and \f$\tau_y\f$, respectively,
305 \vecthreethree{\cos(\tau_y)}{0}{-\sin(\tau_y)}{0}{1}{0}{\sin(\tau_y)}{0}{\cos(\tau_y)}
306 \vecthreethree{1}{0}{0}{0}{\cos(\tau_x)}{\sin(\tau_x)}{0}{-\sin(\tau_x)}{\cos(\tau_x)} =
307 \vecthreethree{\cos(\tau_y)}{\sin(\tau_y)\sin(\tau_x)}{-\sin(\tau_y)\cos(\tau_x)}
308 {0}{\cos(\tau_x)}{\sin(\tau_x)}
309 {\sin(\tau_y)}{-\cos(\tau_y)\sin(\tau_x)}{\cos(\tau_y)\cos(\tau_x)}.
312 In the functions below the coefficients are passed or returned as
314 \f[(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f]
316 vector. That is, if the vector contains four elements, it means that \f$k_3=0\f$ . The distortion
317 coefficients do not depend on the scene viewed. Thus, they also belong to the intrinsic camera
318 parameters. And they remain the same regardless of the captured image resolution. If, for example, a
319 camera has been calibrated on images of 320 x 240 resolution, absolutely the same distortion
320 coefficients can be used for 640 x 480 images from the same camera while \f$f_x\f$, \f$f_y\f$,
321 \f$c_x\f$, and \f$c_y\f$ need to be scaled appropriately.
323 The functions below use the above model to do the following:
325 - Project 3D points to the image plane given intrinsic and extrinsic parameters.
326 - Compute extrinsic parameters given intrinsic parameters, a few 3D points, and their
328 - Estimate intrinsic and extrinsic camera parameters from several views of a known calibration
329 pattern (every view is described by several 3D-2D point correspondences).
330 - Estimate the relative position and orientation of the stereo camera "heads" and compute the
331 *rectification* transformation that makes the camera optical axes parallel.
333 <B> Homogeneous Coordinates </B><br>
334 Homogeneous Coordinates are a system of coordinates that are used in projective geometry. Their use
335 allows to represent points at infinity by finite coordinates and simplifies formulas when compared
336 to the cartesian counterparts, e.g. they have the advantage that affine transformations can be
337 expressed as linear homogeneous transformation.
339 One obtains the homogeneous vector \f$P_h\f$ by appending a 1 along an n-dimensional cartesian
340 vector \f$P\f$ e.g. for a 3D cartesian vector the mapping \f$P \rightarrow P_h\f$ is:
346 \end{bmatrix} \rightarrow \begin{bmatrix}
353 For the inverse mapping \f$P_h \rightarrow P\f$, one divides all elements of the homogeneous vector
354 by its last element, e.g. for a 3D homogeneous vector one gets its 2D cartesian counterpart by:
360 \end{bmatrix} \rightarrow \begin{bmatrix}
367 Due to this mapping, all multiples \f$k P_h\f$, for \f$k \ne 0\f$, of a homogeneous point represent
368 the same point \f$P_h\f$. An intuitive understanding of this property is that under a projective
369 transformation, all multiples of \f$P_h\f$ are mapped to the same point. This is the physical
370 observation one does for pinhole cameras, as all points along a ray through the camera's pinhole are
371 projected to the same image point, e.g. all points along the red ray in the image of the pinhole
372 camera model above would be mapped to the same image coordinate. This property is also the source
373 for the scale ambiguity s in the equation of the pinhole camera model.
375 As mentioned, by using homogeneous coordinates we can express any change of basis parameterized by
376 \f$R\f$ and \f$t\f$ as a linear transformation, e.g. for the change of basis from coordinate system
377 0 to coordinate system 1 becomes:
379 \f[P_1 = R P_0 + t \rightarrow P_{h_1} = \begin{bmatrix}
382 \end{bmatrix} P_{h_0}.\f]
385 - Many functions in this module take a camera matrix as an input parameter. Although all
386 functions assume the same structure of this parameter, they may name it differently. The
387 parameter's description, however, will be clear in that a camera matrix with the structure
388 shown above is required.
389 - A calibration sample for 3 cameras in a horizontal position can be found at
390 opencv_source_code/samples/cpp/3calibration.cpp
391 - A calibration sample based on a sequence of images can be found at
392 opencv_source_code/samples/cpp/calibration.cpp
393 - A calibration sample in order to do 3D reconstruction can be found at
394 opencv_source_code/samples/cpp/build3dmodel.cpp
395 - A calibration example on stereo calibration can be found at
396 opencv_source_code/samples/cpp/stereo_calib.cpp
397 - A calibration example on stereo matching can be found at
398 opencv_source_code/samples/cpp/stereo_match.cpp
399 - (Python) A camera calibration sample can be found at
400 opencv_source_code/samples/python/calibrate.py
403 @defgroup calib3d_fisheye Fisheye camera model
405 Definitions: Let P be a point in 3D of coordinates X in the world reference frame (stored in the
406 matrix X) The coordinate vector of P in the camera reference frame is:
410 where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); call x, y
411 and z the 3 coordinates of Xc:
413 \f[x = Xc_1 \\ y = Xc_2 \\ z = Xc_3\f]
415 The pinhole projection coordinates of P is [a; b] where
417 \f[a = x / z \ and \ b = y / z \\ r^2 = a^2 + b^2 \\ \theta = atan(r)\f]
421 \f[\theta_d = \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8)\f]
423 The distorted point coordinates are [x'; y'] where
425 \f[x' = (\theta_d / r) a \\ y' = (\theta_d / r) b \f]
427 Finally, conversion into pixel coordinates: The final pixel coordinates vector [u; v] where:
429 \f[u = f_x (x' + \alpha y') + c_x \\
432 @defgroup calib3d_c C API
440 //! @addtogroup calib3d
443 //! type of the robust estimation algorithm
444 enum { LMEDS = 4, //!< least-median of squares algorithm
445 RANSAC = 8, //!< RANSAC algorithm
446 RHO = 16 //!< RHO algorithm
449 enum SolvePnPMethod {
450 SOLVEPNP_ITERATIVE = 0,
451 SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp
452 SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete
453 SOLVEPNP_DLS = 3, //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct
454 SOLVEPNP_UPNP = 4, //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive
455 SOLVEPNP_AP3P = 5, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17
456 SOLVEPNP_IPPE = 6, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n
457 //!< Object points must be coplanar.
458 SOLVEPNP_IPPE_SQUARE = 7, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n
459 //!< This is a special case suitable for marker pose estimation.\n
460 //!< 4 coplanar object points must be defined in the following order:
461 //!< - point 0: [-squareLength / 2, squareLength / 2, 0]
462 //!< - point 1: [ squareLength / 2, squareLength / 2, 0]
463 //!< - point 2: [ squareLength / 2, -squareLength / 2, 0]
464 //!< - point 3: [-squareLength / 2, -squareLength / 2, 0]
466 SOLVEPNP_MAX_COUNT //!< Used for count
470 enum { CALIB_CB_ADAPTIVE_THRESH = 1,
471 CALIB_CB_NORMALIZE_IMAGE = 2,
472 CALIB_CB_FILTER_QUADS = 4,
473 CALIB_CB_FAST_CHECK = 8,
474 CALIB_CB_EXHAUSTIVE = 16,
475 CALIB_CB_ACCURACY = 32,
476 CALIB_CB_LARGER = 64,
477 CALIB_CB_MARKER = 128
480 enum { CALIB_CB_SYMMETRIC_GRID = 1,
481 CALIB_CB_ASYMMETRIC_GRID = 2,
482 CALIB_CB_CLUSTERING = 4
485 enum { CALIB_NINTRINSIC = 18,
486 CALIB_USE_INTRINSIC_GUESS = 0x00001,
487 CALIB_FIX_ASPECT_RATIO = 0x00002,
488 CALIB_FIX_PRINCIPAL_POINT = 0x00004,
489 CALIB_ZERO_TANGENT_DIST = 0x00008,
490 CALIB_FIX_FOCAL_LENGTH = 0x00010,
491 CALIB_FIX_K1 = 0x00020,
492 CALIB_FIX_K2 = 0x00040,
493 CALIB_FIX_K3 = 0x00080,
494 CALIB_FIX_K4 = 0x00800,
495 CALIB_FIX_K5 = 0x01000,
496 CALIB_FIX_K6 = 0x02000,
497 CALIB_RATIONAL_MODEL = 0x04000,
498 CALIB_THIN_PRISM_MODEL = 0x08000,
499 CALIB_FIX_S1_S2_S3_S4 = 0x10000,
500 CALIB_TILTED_MODEL = 0x40000,
501 CALIB_FIX_TAUX_TAUY = 0x80000,
502 CALIB_USE_QR = 0x100000, //!< use QR instead of SVD decomposition for solving. Faster but potentially less precise
503 CALIB_FIX_TANGENT_DIST = 0x200000,
505 CALIB_FIX_INTRINSIC = 0x00100,
506 CALIB_SAME_FOCAL_LENGTH = 0x00200,
507 // for stereo rectification
508 CALIB_ZERO_DISPARITY = 0x00400,
509 CALIB_USE_LU = (1 << 17), //!< use LU instead of SVD decomposition for solving. much faster but potentially less precise
510 CALIB_USE_EXTRINSIC_GUESS = (1 << 22) //!< for stereoCalibrate
513 //! the algorithm for finding fundamental matrix
514 enum { FM_7POINT = 1, //!< 7-point algorithm
515 FM_8POINT = 2, //!< 8-point algorithm
516 FM_LMEDS = 4, //!< least-median algorithm. 7-point algorithm is used.
517 FM_RANSAC = 8 //!< RANSAC algorithm. It needs at least 15 points. 7-point algorithm is used.
520 enum HandEyeCalibrationMethod
522 CALIB_HAND_EYE_TSAI = 0, //!< A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration @cite Tsai89
523 CALIB_HAND_EYE_PARK = 1, //!< Robot Sensor Calibration: Solving AX = XB on the Euclidean Group @cite Park94
524 CALIB_HAND_EYE_HORAUD = 2, //!< Hand-eye Calibration @cite Horaud95
525 CALIB_HAND_EYE_ANDREFF = 3, //!< On-line Hand-Eye Calibration @cite Andreff99
526 CALIB_HAND_EYE_DANIILIDIS = 4 //!< Hand-Eye Calibration Using Dual Quaternions @cite Daniilidis98
530 /** @brief Converts a rotation matrix to a rotation vector or vice versa.
532 @param src Input rotation vector (3x1 or 1x3) or rotation matrix (3x3).
533 @param dst Output rotation matrix (3x3) or rotation vector (3x1 or 1x3), respectively.
534 @param jacobian Optional output Jacobian matrix, 3x9 or 9x3, which is a matrix of partial
535 derivatives of the output array components with respect to the input array components.
537 \f[\begin{array}{l} \theta \leftarrow norm(r) \\ r \leftarrow r/ \theta \\ R = \cos(\theta) I + (1- \cos{\theta} ) r r^T + \sin(\theta) \vecthreethree{0}{-r_z}{r_y}{r_z}{0}{-r_x}{-r_y}{r_x}{0} \end{array}\f]
539 Inverse transformation can be also done easily, since
541 \f[\sin ( \theta ) \vecthreethree{0}{-r_z}{r_y}{r_z}{0}{-r_x}{-r_y}{r_x}{0} = \frac{R - R^T}{2}\f]
543 A rotation vector is a convenient and most compact representation of a rotation matrix (since any
544 rotation matrix has just 3 degrees of freedom). The representation is used in the global 3D geometry
545 optimization procedures like @ref calibrateCamera, @ref stereoCalibrate, or @ref solvePnP .
547 @note More information about the computation of the derivative of a 3D rotation matrix with respect to its exponential coordinate
549 - A Compact Formula for the Derivative of a 3-D Rotation in Exponential Coordinates, Guillermo Gallego, Anthony J. Yezzi @cite Gallego2014ACF
551 @note Useful information on SE(3) and Lie Groups can be found in:
552 - A tutorial on SE(3) transformation parameterizations and on-manifold optimization, Jose-Luis Blanco @cite blanco2010tutorial
553 - Lie Groups for 2D and 3D Transformation, Ethan Eade @cite Eade17
554 - A micro Lie theory for state estimation in robotics, Joan Solà , Jérémie Deray, Dinesh Atchuthan @cite Sol2018AML
556 CV_EXPORTS_W void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() );
560 /** Levenberg-Marquardt solver. Starting with the specified vector of parameters it
561 optimizes the target vector criteria "err"
562 (finds local minima of each target vector component absolute value).
564 When needed, it calls user-provided callback.
566 class CV_EXPORTS LMSolver : public Algorithm
569 class CV_EXPORTS Callback
572 virtual ~Callback() {}
574 computes error and Jacobian for the specified vector of parameters
576 @param param the current vector of parameters
577 @param err output vector of errors: err_i = actual_f_i - ideal_f_i
578 @param J output Jacobian: J_ij = d(err_i)/d(param_j)
580 when J=noArray(), it means that it does not need to be computed.
581 Dimensionality of error vector and param vector can be different.
582 The callback should explicitly allocate (with "create" method) each output array
583 (unless it's noArray()).
585 virtual bool compute(InputArray param, OutputArray err, OutputArray J) const = 0;
589 Runs Levenberg-Marquardt algorithm using the passed vector of parameters as the start point.
590 The final vector of parameters (whether the algorithm converged or not) is stored at the same
591 vector. The method returns the number of iterations used. If it's equal to the previously specified
592 maxIters, there is a big chance the algorithm did not converge.
594 @param param initial/final vector of parameters.
596 Note that the dimensionality of parameter space is defined by the size of param vector,
597 and the dimensionality of optimized criteria is defined by the size of err vector
598 computed by the callback.
600 virtual int run(InputOutputArray param) const = 0;
603 Sets the maximum number of iterations
604 @param maxIters the number of iterations
606 virtual void setMaxIters(int maxIters) = 0;
608 Retrieves the current maximum number of iterations
610 virtual int getMaxIters() const = 0;
613 Creates Levenberg-Marquard solver
616 @param maxIters maximum number of iterations that can be further
617 modified using setMaxIters() method.
619 static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters);
620 static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);
625 /** @example samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp
626 An example program about pose estimation from coplanar points
628 Check @ref tutorial_homography "the corresponding tutorial" for more details
631 /** @brief Finds a perspective transformation between two planes.
633 @param srcPoints Coordinates of the points in the original plane, a matrix of the type CV_32FC2
634 or vector\<Point2f\> .
635 @param dstPoints Coordinates of the points in the target plane, a matrix of the type CV_32FC2 or
636 a vector\<Point2f\> .
637 @param method Method used to compute a homography matrix. The following methods are possible:
638 - **0** - a regular method using all the points, i.e., the least squares method
639 - **RANSAC** - RANSAC-based robust method
640 - **LMEDS** - Least-Median robust method
641 - **RHO** - PROSAC-based robust method
642 @param ransacReprojThreshold Maximum allowed reprojection error to treat a point pair as an inlier
643 (used in the RANSAC and RHO methods only). That is, if
644 \f[\| \texttt{dstPoints} _i - \texttt{convertPointsHomogeneous} ( \texttt{H} * \texttt{srcPoints} _i) \|_2 > \texttt{ransacReprojThreshold}\f]
645 then the point \f$i\f$ is considered as an outlier. If srcPoints and dstPoints are measured in pixels,
646 it usually makes sense to set this parameter somewhere in the range of 1 to 10.
647 @param mask Optional output mask set by a robust method ( RANSAC or LMEDS ). Note that the input
648 mask values are ignored.
649 @param maxIters The maximum number of RANSAC iterations.
650 @param confidence Confidence level, between 0 and 1.
652 The function finds and returns the perspective transformation \f$H\f$ between the source and the
655 \f[s_i \vecthree{x'_i}{y'_i}{1} \sim H \vecthree{x_i}{y_i}{1}\f]
657 so that the back-projection error
659 \f[\sum _i \left ( x'_i- \frac{h_{11} x_i + h_{12} y_i + h_{13}}{h_{31} x_i + h_{32} y_i + h_{33}} \right )^2+ \left ( y'_i- \frac{h_{21} x_i + h_{22} y_i + h_{23}}{h_{31} x_i + h_{32} y_i + h_{33}} \right )^2\f]
661 is minimized. If the parameter method is set to the default value 0, the function uses all the point
662 pairs to compute an initial homography estimate with a simple least-squares scheme.
664 However, if not all of the point pairs ( \f$srcPoints_i\f$, \f$dstPoints_i\f$ ) fit the rigid perspective
665 transformation (that is, there are some outliers), this initial estimate will be poor. In this case,
666 you can use one of the three robust methods. The methods RANSAC, LMeDS and RHO try many different
667 random subsets of the corresponding point pairs (of four pairs each, collinear pairs are discarded), estimate the homography matrix
668 using this subset and a simple least-squares algorithm, and then compute the quality/goodness of the
669 computed homography (which is the number of inliers for RANSAC or the least median re-projection error for
670 LMeDS). The best subset is then used to produce the initial estimate of the homography matrix and
671 the mask of inliers/outliers.
673 Regardless of the method, robust or not, the computed homography matrix is refined further (using
674 inliers only in case of a robust method) with the Levenberg-Marquardt method to reduce the
675 re-projection error even more.
677 The methods RANSAC and RHO can handle practically any ratio of outliers but need a threshold to
678 distinguish inliers from outliers. The method LMeDS does not need any threshold but it works
679 correctly only when there are more than 50% of inliers. Finally, if there are no outliers and the
680 noise is rather small, use the default method (method=0).
682 The function is used to find initial intrinsic and extrinsic matrices. Homography matrix is
683 determined up to a scale. Thus, it is normalized so that \f$h_{33}=1\f$. Note that whenever an \f$H\f$ matrix
684 cannot be estimated, an empty one will be returned.
687 getAffineTransform, estimateAffine2D, estimateAffinePartial2D, getPerspectiveTransform, warpPerspective,
690 CV_EXPORTS_W Mat findHomography( InputArray srcPoints, InputArray dstPoints,
691 int method = 0, double ransacReprojThreshold = 3,
692 OutputArray mask=noArray(), const int maxIters = 2000,
693 const double confidence = 0.995);
696 CV_EXPORTS Mat findHomography( InputArray srcPoints, InputArray dstPoints,
697 OutputArray mask, int method = 0, double ransacReprojThreshold = 3 );
699 /** @brief Computes an RQ decomposition of 3x3 matrices.
701 @param src 3x3 input matrix.
702 @param mtxR Output 3x3 upper-triangular matrix.
703 @param mtxQ Output 3x3 orthogonal matrix.
704 @param Qx Optional output 3x3 rotation matrix around x-axis.
705 @param Qy Optional output 3x3 rotation matrix around y-axis.
706 @param Qz Optional output 3x3 rotation matrix around z-axis.
708 The function computes a RQ decomposition using the given rotations. This function is used in
709 decomposeProjectionMatrix to decompose the left 3x3 submatrix of a projection matrix into a camera
710 and a rotation matrix.
712 It optionally returns three rotation matrices, one for each axis, and the three Euler angles in
713 degrees (as the return value) that could be used in OpenGL. Note, there is always more than one
714 sequence of rotations about the three principal axes that results in the same orientation of an
715 object, e.g. see @cite Slabaugh . Returned tree rotation matrices and corresponding three Euler angles
716 are only one of the possible solutions.
718 CV_EXPORTS_W Vec3d RQDecomp3x3( InputArray src, OutputArray mtxR, OutputArray mtxQ,
719 OutputArray Qx = noArray(),
720 OutputArray Qy = noArray(),
721 OutputArray Qz = noArray());
723 /** @brief Decomposes a projection matrix into a rotation matrix and a camera matrix.
725 @param projMatrix 3x4 input projection matrix P.
726 @param cameraMatrix Output 3x3 camera matrix K.
727 @param rotMatrix Output 3x3 external rotation matrix R.
728 @param transVect Output 4x1 translation vector T.
729 @param rotMatrixX Optional 3x3 rotation matrix around x-axis.
730 @param rotMatrixY Optional 3x3 rotation matrix around y-axis.
731 @param rotMatrixZ Optional 3x3 rotation matrix around z-axis.
732 @param eulerAngles Optional three-element vector containing three Euler angles of rotation in
735 The function computes a decomposition of a projection matrix into a calibration and a rotation
736 matrix and the position of a camera.
738 It optionally returns three rotation matrices, one for each axis, and three Euler angles that could
739 be used in OpenGL. Note, there is always more than one sequence of rotations about the three
740 principal axes that results in the same orientation of an object, e.g. see @cite Slabaugh . Returned
741 tree rotation matrices and corresponding three Euler angles are only one of the possible solutions.
743 The function is based on RQDecomp3x3 .
745 CV_EXPORTS_W void decomposeProjectionMatrix( InputArray projMatrix, OutputArray cameraMatrix,
746 OutputArray rotMatrix, OutputArray transVect,
747 OutputArray rotMatrixX = noArray(),
748 OutputArray rotMatrixY = noArray(),
749 OutputArray rotMatrixZ = noArray(),
750 OutputArray eulerAngles =noArray() );
752 /** @brief Computes partial derivatives of the matrix product for each multiplied matrix.
754 @param A First multiplied matrix.
755 @param B Second multiplied matrix.
756 @param dABdA First output derivative matrix d(A\*B)/dA of size
757 \f$\texttt{A.rows*B.cols} \times {A.rows*A.cols}\f$ .
758 @param dABdB Second output derivative matrix d(A\*B)/dB of size
759 \f$\texttt{A.rows*B.cols} \times {B.rows*B.cols}\f$ .
761 The function computes partial derivatives of the elements of the matrix product \f$A*B\f$ with regard to
762 the elements of each of the two input matrices. The function is used to compute the Jacobian
763 matrices in stereoCalibrate but can also be used in any other similar optimization function.
765 CV_EXPORTS_W void matMulDeriv( InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB );
767 /** @brief Combines two rotation-and-shift transformations.
769 @param rvec1 First rotation vector.
770 @param tvec1 First translation vector.
771 @param rvec2 Second rotation vector.
772 @param tvec2 Second translation vector.
773 @param rvec3 Output rotation vector of the superposition.
774 @param tvec3 Output translation vector of the superposition.
775 @param dr3dr1 Optional output derivative of rvec3 with regard to rvec1
776 @param dr3dt1 Optional output derivative of rvec3 with regard to tvec1
777 @param dr3dr2 Optional output derivative of rvec3 with regard to rvec2
778 @param dr3dt2 Optional output derivative of rvec3 with regard to tvec2
779 @param dt3dr1 Optional output derivative of tvec3 with regard to rvec1
780 @param dt3dt1 Optional output derivative of tvec3 with regard to tvec1
781 @param dt3dr2 Optional output derivative of tvec3 with regard to rvec2
782 @param dt3dt2 Optional output derivative of tvec3 with regard to tvec2
784 The functions compute:
786 \f[\begin{array}{l} \texttt{rvec3} = \mathrm{rodrigues} ^{-1} \left ( \mathrm{rodrigues} ( \texttt{rvec2} ) \cdot \mathrm{rodrigues} ( \texttt{rvec1} ) \right ) \\ \texttt{tvec3} = \mathrm{rodrigues} ( \texttt{rvec2} ) \cdot \texttt{tvec1} + \texttt{tvec2} \end{array} ,\f]
788 where \f$\mathrm{rodrigues}\f$ denotes a rotation vector to a rotation matrix transformation, and
789 \f$\mathrm{rodrigues}^{-1}\f$ denotes the inverse transformation. See Rodrigues for details.
791 Also, the functions can compute the derivatives of the output vectors with regards to the input
792 vectors (see matMulDeriv ). The functions are used inside stereoCalibrate but can also be used in
793 your own code where Levenberg-Marquardt or another gradient-based solver is used to optimize a
794 function that contains a matrix multiplication.
796 CV_EXPORTS_W void composeRT( InputArray rvec1, InputArray tvec1,
797 InputArray rvec2, InputArray tvec2,
798 OutputArray rvec3, OutputArray tvec3,
799 OutputArray dr3dr1 = noArray(), OutputArray dr3dt1 = noArray(),
800 OutputArray dr3dr2 = noArray(), OutputArray dr3dt2 = noArray(),
801 OutputArray dt3dr1 = noArray(), OutputArray dt3dt1 = noArray(),
802 OutputArray dt3dr2 = noArray(), OutputArray dt3dt2 = noArray() );
804 /** @brief Projects 3D points to an image plane.
806 @param objectPoints Array of object points expressed wrt. the world coordinate frame. A 3xN/Nx3
807 1-channel or 1xN/Nx1 3-channel (or vector\<Point3f\> ), where N is the number of points in the view.
808 @param rvec The rotation vector (@ref Rodrigues) that, together with tvec, performs a change of
809 basis from world to camera coordinate system, see @ref calibrateCamera for details.
810 @param tvec The translation vector, see parameter description above.
811 @param cameraMatrix Camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$ .
812 @param distCoeffs Input vector of distortion coefficients
813 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
814 4, 5, 8, 12 or 14 elements. If the vector is empty, the zero distortion coefficients are assumed.
815 @param imagePoints Output array of image points, 1xN/Nx1 2-channel, or
817 @param jacobian Optional output 2Nx(10+\<numDistCoeffs\>) jacobian matrix of derivatives of image
818 points with respect to components of the rotation vector, translation vector, focal lengths,
819 coordinates of the principal point and the distortion coefficients. In the old interface different
820 components of the jacobian are returned via different output parameters.
821 @param aspectRatio Optional "fixed aspect ratio" parameter. If the parameter is not 0, the
822 function assumes that the aspect ratio (\f$f_x / f_y\f$) is fixed and correspondingly adjusts the
825 The function computes the 2D projections of 3D points to the image plane, given intrinsic and
826 extrinsic camera parameters. Optionally, the function computes Jacobians -matrices of partial
827 derivatives of image points coordinates (as functions of all the input parameters) with respect to
828 the particular parameters, intrinsic and/or extrinsic. The Jacobians are used during the global
829 optimization in @ref calibrateCamera, @ref solvePnP, and @ref stereoCalibrate. The function itself
830 can also be used to compute a re-projection error, given the current intrinsic and extrinsic
833 @note By setting rvec = tvec = \f$[0, 0, 0]\f$, or by setting cameraMatrix to a 3x3 identity matrix,
834 or by passing zero distortion coefficients, one can get various useful partial cases of the
835 function. This means, one can compute the distorted coordinates for a sparse set of points or apply
836 a perspective transformation (and also compute the derivatives) in the ideal zero-distortion setup.
838 CV_EXPORTS_W void projectPoints( InputArray objectPoints,
839 InputArray rvec, InputArray tvec,
840 InputArray cameraMatrix, InputArray distCoeffs,
841 OutputArray imagePoints,
842 OutputArray jacobian = noArray(),
843 double aspectRatio = 0 );
845 /** @example samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp
846 An example program about homography from the camera displacement
848 Check @ref tutorial_homography "the corresponding tutorial" for more details
851 /** @brief Finds an object pose from 3D-2D point correspondences.
852 This function returns the rotation and the translation vectors that transform a 3D point expressed in the object
853 coordinate frame to the camera coordinate frame, using different methods:
854 - P3P methods (@ref SOLVEPNP_P3P, @ref SOLVEPNP_AP3P): need 4 input points to return a unique solution.
855 - @ref SOLVEPNP_IPPE Input points must be >= 4 and object points must be coplanar.
856 - @ref SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation.
857 Number of input points must be 4. Object points must be defined in the following order:
858 - point 0: [-squareLength / 2, squareLength / 2, 0]
859 - point 1: [ squareLength / 2, squareLength / 2, 0]
860 - point 2: [ squareLength / 2, -squareLength / 2, 0]
861 - point 3: [-squareLength / 2, -squareLength / 2, 0]
862 - for all the other flags, number of input points must be >= 4 and object points can be in any configuration.
864 @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or
865 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3d\> can be also passed here.
866 @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
867 where N is the number of points. vector\<Point2d\> can be also passed here.
868 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
869 @param distCoeffs Input vector of distortion coefficients
870 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
871 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
873 @param rvec Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from
874 the model coordinate system to the camera coordinate system.
875 @param tvec Output translation vector.
876 @param useExtrinsicGuess Parameter used for #SOLVEPNP_ITERATIVE. If true (1), the function uses
877 the provided rvec and tvec values as initial approximations of the rotation and translation
878 vectors, respectively, and further optimizes them.
879 @param flags Method for solving a PnP problem:
880 - **SOLVEPNP_ITERATIVE** Iterative method is based on a Levenberg-Marquardt optimization. In
881 this case the function finds such a pose that minimizes reprojection error, that is the sum
882 of squared distances between the observed projections imagePoints and the projected (using
883 projectPoints ) objectPoints .
884 - **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang
885 "Complete Solution Classification for the Perspective-Three-Point Problem" (@cite gao2003complete).
886 In this case the function requires exactly four object and image points.
887 - **SOLVEPNP_AP3P** Method is based on the paper of T. Ke, S. Roumeliotis
888 "An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17).
889 In this case the function requires exactly four object and image points.
890 - **SOLVEPNP_EPNP** Method has been introduced by F. Moreno-Noguer, V. Lepetit and P. Fua in the
891 paper "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" (@cite lepetit2009epnp).
892 - **SOLVEPNP_DLS** Method is based on the paper of J. Hesch and S. Roumeliotis.
893 "A Direct Least-Squares (DLS) Method for PnP" (@cite hesch2011direct).
894 - **SOLVEPNP_UPNP** Method is based on the paper of A. Penate-Sanchez, J. Andrade-Cetto,
895 F. Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length
896 Estimation" (@cite penate2013exhaustive). In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$
897 assuming that both have the same value. Then the cameraMatrix is updated with the estimated
899 - **SOLVEPNP_IPPE** Method is based on the paper of T. Collins and A. Bartoli.
900 "Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method requires coplanar object points.
901 - **SOLVEPNP_IPPE_SQUARE** Method is based on the paper of Toby Collins and Adrien Bartoli.
902 "Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method is suitable for marker pose estimation.
903 It requires 4 coplanar object points defined in the following order:
904 - point 0: [-squareLength / 2, squareLength / 2, 0]
905 - point 1: [ squareLength / 2, squareLength / 2, 0]
906 - point 2: [ squareLength / 2, -squareLength / 2, 0]
907 - point 3: [-squareLength / 2, -squareLength / 2, 0]
909 The function estimates the object pose given a set of object points, their corresponding image
910 projections, as well as the camera matrix and the distortion coefficients, see the figure below
911 (more precisely, the X-axis of the camera frame is pointing to the right, the Y-axis downward
912 and the Z-axis forward).
916 Points expressed in the world frame \f$ \bf{X}_w \f$ are projected into the image plane \f$ \left[ u, v \right] \f$
917 using the perspective projection model \f$ \Pi \f$ and the camera intrinsic parameters matrix \f$ \bf{A} \f$:
926 \bf{A} \hspace{0.1em} \Pi \hspace{0.2em} ^{c}\bf{T}_w
949 r_{11} & r_{12} & r_{13} & t_x \\
950 r_{21} & r_{22} & r_{23} & t_y \\
951 r_{31} & r_{32} & r_{33} & t_z \\
963 The estimated pose is thus the rotation (`rvec`) and the translation (`tvec`) vectors that allow transforming
964 a 3D point expressed in the world frame into the camera frame:
974 \hspace{0.2em} ^{c}\bf{T}_w
988 r_{11} & r_{12} & r_{13} & t_x \\
989 r_{21} & r_{22} & r_{23} & t_y \\
990 r_{31} & r_{32} & r_{33} & t_z \\
1003 - An example of how to use solvePnP for planar augmented reality can be found at
1004 opencv_source_code/samples/python/plane_ar.py
1005 - If you are using Python:
1006 - Numpy array slices won't work as input because solvePnP requires contiguous
1007 arrays (enforced by the assertion using cv::Mat::checkVector() around line 55 of
1008 modules/calib3d/src/solvepnp.cpp version 2.4.9)
1009 - The P3P algorithm requires image points to be in an array of shape (N,1,2) due
1010 to its calling of cv::undistortPoints (around line 75 of modules/calib3d/src/solvepnp.cpp version 2.4.9)
1011 which requires 2-channel information.
1012 - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of
1013 it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints =
1014 np.ascontiguousarray(D[:,:2]).reshape((N,1,2))
1015 - The methods **SOLVEPNP_DLS** and **SOLVEPNP_UPNP** cannot be used as the current implementations are
1016 unstable and sometimes give completely wrong results. If you pass one of these two
1017 flags, **SOLVEPNP_EPNP** method will be used instead.
1018 - The minimum number of points is 4 in the general case. In the case of **SOLVEPNP_P3P** and **SOLVEPNP_AP3P**
1019 methods, it is required to use exactly 4 points (the first 3 points are used to estimate all the solutions
1020 of the P3P problem, the last one is used to retain the best solution that minimizes the reprojection error).
1021 - With **SOLVEPNP_ITERATIVE** method and `useExtrinsicGuess=true`, the minimum number of points is 3 (3 points
1022 are sufficient to compute a pose but there are up to 4 solutions). The initial solution should be close to the
1023 global solution to converge.
1024 - With **SOLVEPNP_IPPE** input points must be >= 4 and object points must be coplanar.
1025 - With **SOLVEPNP_IPPE_SQUARE** this is a special case suitable for marker pose estimation.
1026 Number of input points must be 4. Object points must be defined in the following order:
1027 - point 0: [-squareLength / 2, squareLength / 2, 0]
1028 - point 1: [ squareLength / 2, squareLength / 2, 0]
1029 - point 2: [ squareLength / 2, -squareLength / 2, 0]
1030 - point 3: [-squareLength / 2, -squareLength / 2, 0]
1032 CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints,
1033 InputArray cameraMatrix, InputArray distCoeffs,
1034 OutputArray rvec, OutputArray tvec,
1035 bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );
1037 /** @brief Finds an object pose from 3D-2D point correspondences using the RANSAC scheme.
1039 @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or
1040 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3d\> can be also passed here.
1041 @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
1042 where N is the number of points. vector\<Point2d\> can be also passed here.
1043 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}\f$ .
1044 @param distCoeffs Input vector of distortion coefficients
1045 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
1046 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
1048 @param rvec Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from
1049 the model coordinate system to the camera coordinate system.
1050 @param tvec Output translation vector.
1051 @param useExtrinsicGuess Parameter used for @ref SOLVEPNP_ITERATIVE. If true (1), the function uses
1052 the provided rvec and tvec values as initial approximations of the rotation and translation
1053 vectors, respectively, and further optimizes them.
1054 @param iterationsCount Number of iterations.
1055 @param reprojectionError Inlier threshold value used by the RANSAC procedure. The parameter value
1056 is the maximum allowed distance between the observed and computed point projections to consider it
1058 @param confidence The probability that the algorithm produces a useful result.
1059 @param inliers Output vector that contains indices of inliers in objectPoints and imagePoints .
1060 @param flags Method for solving a PnP problem (see @ref solvePnP ).
1062 The function estimates an object pose given a set of object points, their corresponding image
1063 projections, as well as the camera matrix and the distortion coefficients. This function finds such
1064 a pose that minimizes reprojection error, that is, the sum of squared distances between the observed
1065 projections imagePoints and the projected (using @ref projectPoints ) objectPoints. The use of RANSAC
1066 makes the function resistant to outliers.
1069 - An example of how to use solvePNPRansac for object detection can be found at
1070 opencv_source_code/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/
1071 - The default method used to estimate the camera pose for the Minimal Sample Sets step
1072 is #SOLVEPNP_EPNP. Exceptions are:
1073 - if you choose #SOLVEPNP_P3P or #SOLVEPNP_AP3P, these methods will be used.
1074 - if the number of input points is equal to 4, #SOLVEPNP_P3P is used.
1075 - The method used to estimate the camera pose using all the inliers is defined by the
1076 flags parameters unless it is equal to #SOLVEPNP_P3P or #SOLVEPNP_AP3P. In this case,
1077 the method #SOLVEPNP_EPNP will be used instead.
1079 CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
1080 InputArray cameraMatrix, InputArray distCoeffs,
1081 OutputArray rvec, OutputArray tvec,
1082 bool useExtrinsicGuess = false, int iterationsCount = 100,
1083 float reprojectionError = 8.0, double confidence = 0.99,
1084 OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );
1086 /** @brief Finds an object pose from 3 3D-2D point correspondences.
1088 @param objectPoints Array of object points in the object coordinate space, 3x3 1-channel or
1089 1x3/3x1 3-channel. vector\<Point3f\> can be also passed here.
1090 @param imagePoints Array of corresponding image points, 3x2 1-channel or 1x3/3x1 2-channel.
1091 vector\<Point2f\> can be also passed here.
1092 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
1093 @param distCoeffs Input vector of distortion coefficients
1094 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
1095 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
1097 @param rvecs Output rotation vectors (see @ref Rodrigues ) that, together with tvecs, brings points from
1098 the model coordinate system to the camera coordinate system. A P3P problem has up to 4 solutions.
1099 @param tvecs Output translation vectors.
1100 @param flags Method for solving a P3P problem:
1101 - **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang
1102 "Complete Solution Classification for the Perspective-Three-Point Problem" (@cite gao2003complete).
1103 - **SOLVEPNP_AP3P** Method is based on the paper of T. Ke and S. Roumeliotis.
1104 "An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17).
1106 The function estimates the object pose given 3 object points, their corresponding image
1107 projections, as well as the camera matrix and the distortion coefficients.
1110 The solutions are sorted by reprojection errors (lowest to highest).
1112 CV_EXPORTS_W int solveP3P( InputArray objectPoints, InputArray imagePoints,
1113 InputArray cameraMatrix, InputArray distCoeffs,
1114 OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
1117 /** @brief Refine a pose (the translation and the rotation that transform a 3D point expressed in the object coordinate frame
1118 to the camera coordinate frame) from a 3D-2D point correspondences and starting from an initial solution.
1120 @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel,
1121 where N is the number of points. vector\<Point3d\> can also be passed here.
1122 @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
1123 where N is the number of points. vector\<Point2d\> can also be passed here.
1124 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
1125 @param distCoeffs Input vector of distortion coefficients
1126 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
1127 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
1129 @param rvec Input/Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from
1130 the model coordinate system to the camera coordinate system. Input values are used as an initial solution.
1131 @param tvec Input/Output translation vector. Input values are used as an initial solution.
1132 @param criteria Criteria when to stop the Levenberg-Marquard iterative algorithm.
1134 The function refines the object pose given at least 3 object points, their corresponding image
1135 projections, an initial solution for the rotation and translation vector,
1136 as well as the camera matrix and the distortion coefficients.
1137 The function minimizes the projection error with respect to the rotation and the translation vectors, according
1138 to a Levenberg-Marquardt iterative minimization @cite Madsen04 @cite Eade13 process.
1140 CV_EXPORTS_W void solvePnPRefineLM( InputArray objectPoints, InputArray imagePoints,
1141 InputArray cameraMatrix, InputArray distCoeffs,
1142 InputOutputArray rvec, InputOutputArray tvec,
1143 TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON));
1145 /** @brief Refine a pose (the translation and the rotation that transform a 3D point expressed in the object coordinate frame
1146 to the camera coordinate frame) from a 3D-2D point correspondences and starting from an initial solution.
1148 @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel,
1149 where N is the number of points. vector\<Point3d\> can also be passed here.
1150 @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
1151 where N is the number of points. vector\<Point2d\> can also be passed here.
1152 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
1153 @param distCoeffs Input vector of distortion coefficients
1154 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
1155 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
1157 @param rvec Input/Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from
1158 the model coordinate system to the camera coordinate system. Input values are used as an initial solution.
1159 @param tvec Input/Output translation vector. Input values are used as an initial solution.
1160 @param criteria Criteria when to stop the Levenberg-Marquard iterative algorithm.
1161 @param VVSlambda Gain for the virtual visual servoing control law, equivalent to the \f$\alpha\f$
1162 gain in the Damped Gauss-Newton formulation.
1164 The function refines the object pose given at least 3 object points, their corresponding image
1165 projections, an initial solution for the rotation and translation vector,
1166 as well as the camera matrix and the distortion coefficients.
1167 The function minimizes the projection error with respect to the rotation and the translation vectors, using a
1168 virtual visual servoing (VVS) @cite Chaumette06 @cite Marchand16 scheme.
1170 CV_EXPORTS_W void solvePnPRefineVVS( InputArray objectPoints, InputArray imagePoints,
1171 InputArray cameraMatrix, InputArray distCoeffs,
1172 InputOutputArray rvec, InputOutputArray tvec,
1173 TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON),
1174 double VVSlambda = 1);
1176 /** @brief Finds an object pose from 3D-2D point correspondences.
1177 This function returns a list of all the possible solutions (a solution is a <rotation vector, translation vector>
1178 couple), depending on the number of input points and the chosen method:
1179 - P3P methods (@ref SOLVEPNP_P3P, @ref SOLVEPNP_AP3P): 3 or 4 input points. Number of returned solutions can be between 0 and 4 with 3 input points.
1180 - @ref SOLVEPNP_IPPE Input points must be >= 4 and object points must be coplanar. Returns 2 solutions.
1181 - @ref SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation.
1182 Number of input points must be 4 and 2 solutions are returned. Object points must be defined in the following order:
1183 - point 0: [-squareLength / 2, squareLength / 2, 0]
1184 - point 1: [ squareLength / 2, squareLength / 2, 0]
1185 - point 2: [ squareLength / 2, -squareLength / 2, 0]
1186 - point 3: [-squareLength / 2, -squareLength / 2, 0]
1187 - for all the other flags, number of input points must be >= 4 and object points can be in any configuration.
1188 Only 1 solution is returned.
1190 @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or
1191 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3d\> can be also passed here.
1192 @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel,
1193 where N is the number of points. vector\<Point2d\> can be also passed here.
1194 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
1195 @param distCoeffs Input vector of distortion coefficients
1196 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
1197 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
1199 @param rvecs Vector of output rotation vectors (see @ref Rodrigues ) that, together with tvecs, brings points from
1200 the model coordinate system to the camera coordinate system.
1201 @param tvecs Vector of output translation vectors.
1202 @param useExtrinsicGuess Parameter used for #SOLVEPNP_ITERATIVE. If true (1), the function uses
1203 the provided rvec and tvec values as initial approximations of the rotation and translation
1204 vectors, respectively, and further optimizes them.
1205 @param flags Method for solving a PnP problem:
1206 - **SOLVEPNP_ITERATIVE** Iterative method is based on a Levenberg-Marquardt optimization. In
1207 this case the function finds such a pose that minimizes reprojection error, that is the sum
1208 of squared distances between the observed projections imagePoints and the projected (using
1209 projectPoints ) objectPoints .
1210 - **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang
1211 "Complete Solution Classification for the Perspective-Three-Point Problem" (@cite gao2003complete).
1212 In this case the function requires exactly four object and image points.
1213 - **SOLVEPNP_AP3P** Method is based on the paper of T. Ke, S. Roumeliotis
1214 "An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17).
1215 In this case the function requires exactly four object and image points.
1216 - **SOLVEPNP_EPNP** Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the
1217 paper "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" (@cite lepetit2009epnp).
1218 - **SOLVEPNP_DLS** Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis.
1219 "A Direct Least-Squares (DLS) Method for PnP" (@cite hesch2011direct).
1220 - **SOLVEPNP_UPNP** Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto,
1221 F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length
1222 Estimation" (@cite penate2013exhaustive). In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$
1223 assuming that both have the same value. Then the cameraMatrix is updated with the estimated
1225 - **SOLVEPNP_IPPE** Method is based on the paper of T. Collins and A. Bartoli.
1226 "Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method requires coplanar object points.
1227 - **SOLVEPNP_IPPE_SQUARE** Method is based on the paper of Toby Collins and Adrien Bartoli.
1228 "Infinitesimal Plane-Based Pose Estimation" (@cite Collins14). This method is suitable for marker pose estimation.
1229 It requires 4 coplanar object points defined in the following order:
1230 - point 0: [-squareLength / 2, squareLength / 2, 0]
1231 - point 1: [ squareLength / 2, squareLength / 2, 0]
1232 - point 2: [ squareLength / 2, -squareLength / 2, 0]
1233 - point 3: [-squareLength / 2, -squareLength / 2, 0]
1234 @param rvec Rotation vector used to initialize an iterative PnP refinement algorithm, when flag is SOLVEPNP_ITERATIVE
1235 and useExtrinsicGuess is set to true.
1236 @param tvec Translation vector used to initialize an iterative PnP refinement algorithm, when flag is SOLVEPNP_ITERATIVE
1237 and useExtrinsicGuess is set to true.
1238 @param reprojectionError Optional vector of reprojection error, that is the RMS error
1239 (\f$ \text{RMSE} = \sqrt{\frac{\sum_{i}^{N} \left ( \hat{y_i} - y_i \right )^2}{N}} \f$) between the input image points
1240 and the 3D object points projected with the estimated pose.
1242 The function estimates the object pose given a set of object points, their corresponding image
1243 projections, as well as the camera matrix and the distortion coefficients, see the figure below
1244 (more precisely, the X-axis of the camera frame is pointing to the right, the Y-axis downward
1245 and the Z-axis forward).
1249 Points expressed in the world frame \f$ \bf{X}_w \f$ are projected into the image plane \f$ \left[ u, v \right] \f$
1250 using the perspective projection model \f$ \Pi \f$ and the camera intrinsic parameters matrix \f$ \bf{A} \f$:
1259 \bf{A} \hspace{0.1em} \Pi \hspace{0.2em} ^{c}\bf{T}_w
1282 r_{11} & r_{12} & r_{13} & t_x \\
1283 r_{21} & r_{22} & r_{23} & t_y \\
1284 r_{31} & r_{32} & r_{33} & t_z \\
1296 The estimated pose is thus the rotation (`rvec`) and the translation (`tvec`) vectors that allow transforming
1297 a 3D point expressed in the world frame into the camera frame:
1307 \hspace{0.2em} ^{c}\bf{T}_w
1321 r_{11} & r_{12} & r_{13} & t_x \\
1322 r_{21} & r_{22} & r_{23} & t_y \\
1323 r_{31} & r_{32} & r_{33} & t_z \\
1336 - An example of how to use solvePnP for planar augmented reality can be found at
1337 opencv_source_code/samples/python/plane_ar.py
1338 - If you are using Python:
1339 - Numpy array slices won't work as input because solvePnP requires contiguous
1340 arrays (enforced by the assertion using cv::Mat::checkVector() around line 55 of
1341 modules/calib3d/src/solvepnp.cpp version 2.4.9)
1342 - The P3P algorithm requires image points to be in an array of shape (N,1,2) due
1343 to its calling of cv::undistortPoints (around line 75 of modules/calib3d/src/solvepnp.cpp version 2.4.9)
1344 which requires 2-channel information.
1345 - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of
1346 it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints =
1347 np.ascontiguousarray(D[:,:2]).reshape((N,1,2))
1348 - The methods **SOLVEPNP_DLS** and **SOLVEPNP_UPNP** cannot be used as the current implementations are
1349 unstable and sometimes give completely wrong results. If you pass one of these two
1350 flags, **SOLVEPNP_EPNP** method will be used instead.
1351 - The minimum number of points is 4 in the general case. In the case of **SOLVEPNP_P3P** and **SOLVEPNP_AP3P**
1352 methods, it is required to use exactly 4 points (the first 3 points are used to estimate all the solutions
1353 of the P3P problem, the last one is used to retain the best solution that minimizes the reprojection error).
1354 - With **SOLVEPNP_ITERATIVE** method and `useExtrinsicGuess=true`, the minimum number of points is 3 (3 points
1355 are sufficient to compute a pose but there are up to 4 solutions). The initial solution should be close to the
1356 global solution to converge.
1357 - With **SOLVEPNP_IPPE** input points must be >= 4 and object points must be coplanar.
1358 - With **SOLVEPNP_IPPE_SQUARE** this is a special case suitable for marker pose estimation.
1359 Number of input points must be 4. Object points must be defined in the following order:
1360 - point 0: [-squareLength / 2, squareLength / 2, 0]
1361 - point 1: [ squareLength / 2, squareLength / 2, 0]
1362 - point 2: [ squareLength / 2, -squareLength / 2, 0]
1363 - point 3: [-squareLength / 2, -squareLength / 2, 0]
1365 CV_EXPORTS_W int solvePnPGeneric( InputArray objectPoints, InputArray imagePoints,
1366 InputArray cameraMatrix, InputArray distCoeffs,
1367 OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
1368 bool useExtrinsicGuess = false, SolvePnPMethod flags = SOLVEPNP_ITERATIVE,
1369 InputArray rvec = noArray(), InputArray tvec = noArray(),
1370 OutputArray reprojectionError = noArray() );
1372 /** @brief Finds an initial camera matrix from 3D-2D point correspondences.
1374 @param objectPoints Vector of vectors of the calibration pattern points in the calibration pattern
1375 coordinate space. In the old interface all the per-view vectors are concatenated. See
1376 calibrateCamera for details.
1377 @param imagePoints Vector of vectors of the projections of the calibration pattern points. In the
1378 old interface all the per-view vectors are concatenated.
1379 @param imageSize Image size in pixels used to initialize the principal point.
1380 @param aspectRatio If it is zero or negative, both \f$f_x\f$ and \f$f_y\f$ are estimated independently.
1381 Otherwise, \f$f_x = f_y * \texttt{aspectRatio}\f$ .
1383 The function estimates and returns an initial camera matrix for the camera calibration process.
1384 Currently, the function only supports planar calibration patterns, which are patterns where each
1385 object point has z-coordinate =0.
1387 CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints,
1388 InputArrayOfArrays imagePoints,
1389 Size imageSize, double aspectRatio = 1.0 );
1391 /** @brief Finds the positions of internal corners of the chessboard.
1393 @param image Source chessboard view. It must be an 8-bit grayscale or color image.
1394 @param patternSize Number of inner corners per a chessboard row and column
1395 ( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ).
1396 @param corners Output array of detected corners.
1397 @param flags Various operation flags that can be zero or a combination of the following values:
1398 - **CALIB_CB_ADAPTIVE_THRESH** Use adaptive thresholding to convert the image to black
1399 and white, rather than a fixed threshold level (computed from the average image brightness).
1400 - **CALIB_CB_NORMALIZE_IMAGE** Normalize the image gamma with equalizeHist before
1401 applying fixed or adaptive thresholding.
1402 - **CALIB_CB_FILTER_QUADS** Use additional criteria (like contour area, perimeter,
1403 square-like shape) to filter out false quads extracted at the contour retrieval stage.
1404 - **CALIB_CB_FAST_CHECK** Run a fast check on the image that looks for chessboard corners,
1405 and shortcut the call if none is found. This can drastically speed up the call in the
1406 degenerate condition when no chessboard is observed.
1408 The function attempts to determine whether the input image is a view of the chessboard pattern and
1409 locate the internal chessboard corners. The function returns a non-zero value if all of the corners
1410 are found and they are placed in a certain order (row by row, left to right in every row).
1411 Otherwise, if the function fails to find all the corners or reorder them, it returns 0. For example,
1412 a regular chessboard has 8 x 8 squares and 7 x 7 internal corners, that is, points where the black
1413 squares touch each other. The detected coordinates are approximate, and to determine their positions
1414 more accurately, the function calls cornerSubPix. You also may use the function cornerSubPix with
1415 different parameters if returned coordinates are not accurate enough.
1417 Sample usage of detecting and drawing chessboard corners: :
1419 Size patternsize(8,6); //interior number of corners
1420 Mat gray = ....; //source image
1421 vector<Point2f> corners; //this will be filled by the detected corners
1423 //CALIB_CB_FAST_CHECK saves a lot of time on images
1424 //that do not contain any chessboard corners
1425 bool patternfound = findChessboardCorners(gray, patternsize, corners,
1426 CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE
1427 + CALIB_CB_FAST_CHECK);
1430 cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),
1431 TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
1433 drawChessboardCorners(img, patternsize, Mat(corners), patternfound);
1435 @note The function requires white space (like a square-thick border, the wider the better) around
1436 the board to make the detection more robust in various environments. Otherwise, if there is no
1437 border and the background is dark, the outer black squares cannot be segmented properly and so the
1438 square grouping and ordering algorithm fails.
1440 CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners,
1441 int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE );
1444 Checks whether the image contains chessboard of the specific size or not.
1445 If yes, nonzero value is returned.
1447 CV_EXPORTS_W bool checkChessboard(InputArray img, Size size);
1449 /** @brief Finds the positions of internal corners of the chessboard using a sector based approach.
1451 @param image Source chessboard view. It must be an 8-bit grayscale or color image.
1452 @param patternSize Number of inner corners per a chessboard row and column
1453 ( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ).
1454 @param corners Output array of detected corners.
1455 @param flags Various operation flags that can be zero or a combination of the following values:
1456 - **CALIB_CB_NORMALIZE_IMAGE** Normalize the image gamma with equalizeHist before detection.
1457 - **CALIB_CB_EXHAUSTIVE** Run an exhaustive search to improve detection rate.
1458 - **CALIB_CB_ACCURACY** Up sample input image to improve sub-pixel accuracy due to aliasing effects.
1459 - **CALIB_CB_LARGER** The detected pattern is allowed to be larger than patternSize (see description).
1460 - **CALIB_CB_MARKER** The detected pattern must have a marker (see description).
1461 This should be used if an accurate camera calibration is required.
1462 @param meta Optional output arrray of detected corners (CV_8UC1 and size = cv::Size(columns,rows)).
1463 Each entry stands for one corner of the pattern and can have one of the following values:
1464 - 0 = no meta data attached
1465 - 1 = left-top corner of a black cell
1466 - 2 = left-top corner of a white cell
1467 - 3 = left-top corner of a black cell with a white marker dot
1468 - 4 = left-top corner of a white cell with a black marker dot (pattern origin in case of markers otherwise first corner)
1470 The function is analog to findchessboardCorners but uses a localized radon
1471 transformation approximated by box filters being more robust to all sort of
1472 noise, faster on larger images and is able to directly return the sub-pixel
1473 position of the internal chessboard corners. The Method is based on the paper
1474 @cite duda2018 "Accurate Detection and Localization of Checkerboard Corners for
1475 Calibration" demonstrating that the returned sub-pixel positions are more
1476 accurate than the one returned by cornerSubPix allowing a precise camera
1477 calibration for demanding applications.
1479 In the case, the flags **CALIB_CB_LARGER** or **CALIB_CB_MARKER** are given,
1480 the result can be recovered from the optional meta array. Both flags are
1481 helpful to use calibration patterns exceeding the field of view of the camera.
1482 These oversized patterns allow more accurate calibrations as corners can be
1483 utilized, which are as close as possible to the image borders. For a
1484 consistent coordinate system across all images, the optional marker (see image
1485 below) can be used to move the origin of the board to the location where the
1486 black circle is located.
1488 @note The function requires a white boarder with roughly the same width as one
1489 of the checkerboard fields around the whole board to improve the detection in
1490 various environments. In addition, because of the localized radon
1491 transformation it is beneficial to use round corners for the field corners
1492 which are located on the outside of the board. The following figure illustrates
1493 a sample checkerboard optimized for the detection. However, any other checkerboard
1494 can be used as well.
1495 ![Checkerboard](pics/checkerboard_radon.png)
1497 CV_EXPORTS_AS(findChessboardCornersSBWithMeta)
1498 bool findChessboardCornersSB(InputArray image,Size patternSize, OutputArray corners,
1499 int flags,OutputArray meta);
1502 bool findChessboardCornersSB(InputArray image, Size patternSize, OutputArray corners,
1505 return findChessboardCornersSB(image, patternSize, corners, flags, noArray());
1508 /** @brief Estimates the sharpness of a detected chessboard.
1510 Image sharpness, as well as brightness, are a critical parameter for accuracte
1511 camera calibration. For accessing these parameters for filtering out
1512 problematic calibraiton images, this method calculates edge profiles by traveling from
1513 black to white chessboard cell centers. Based on this, the number of pixels is
1514 calculated required to transit from black to white. This width of the
1515 transition area is a good indication of how sharp the chessboard is imaged
1516 and should be below ~3.0 pixels.
1518 @param image Gray image used to find chessboard corners
1519 @param patternSize Size of a found chessboard pattern
1520 @param corners Corners found by findChessboardCorners(SB)
1521 @param rise_distance Rise distance 0.8 means 10% ... 90% of the final signal strength
1522 @param vertical By default edge responses for horizontal lines are calculated
1523 @param sharpness Optional output array with a sharpness value for calculated edge responses (see description)
1525 The optional sharpness array is of type CV_32FC1 and has for each calculated
1526 profile one row with the following five entries:
1527 * 0 = x coordinate of the underlying edge in the image
1528 * 1 = y coordinate of the underlying edge in the image
1529 * 2 = width of the transition area (sharpness)
1530 * 3 = signal strength in the black cell (min brightness)
1531 * 4 = signal strength in the white cell (max brightness)
1533 @return Scalar(average sharpness, average min brightness, average max brightness,0)
1535 CV_EXPORTS_W Scalar estimateChessboardSharpness(InputArray image, Size patternSize, InputArray corners,
1536 float rise_distance=0.8F,bool vertical=false,
1537 OutputArray sharpness=noArray());
1540 //! finds subpixel-accurate positions of the chessboard corners
1541 CV_EXPORTS_W bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size );
1543 /** @brief Renders the detected chessboard corners.
1545 @param image Destination image. It must be an 8-bit color image.
1546 @param patternSize Number of inner corners per a chessboard row and column
1547 (patternSize = cv::Size(points_per_row,points_per_column)).
1548 @param corners Array of detected corners, the output of findChessboardCorners.
1549 @param patternWasFound Parameter indicating whether the complete board was found or not. The
1550 return value of findChessboardCorners should be passed here.
1552 The function draws individual chessboard corners detected either as red circles if the board was not
1553 found, or as colored corners connected with lines if the board was found.
1555 CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize,
1556 InputArray corners, bool patternWasFound );
1558 /** @brief Draw axes of the world/object coordinate system from pose estimation. @sa solvePnP
1560 @param image Input/output image. It must have 1 or 3 channels. The number of channels is not altered.
1561 @param cameraMatrix Input 3x3 floating-point matrix of camera intrinsic parameters.
1562 \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$
1563 @param distCoeffs Input vector of distortion coefficients
1564 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
1565 4, 5, 8, 12 or 14 elements. If the vector is empty, the zero distortion coefficients are assumed.
1566 @param rvec Rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from
1567 the model coordinate system to the camera coordinate system.
1568 @param tvec Translation vector.
1569 @param length Length of the painted axes in the same unit than tvec (usually in meters).
1570 @param thickness Line thickness of the painted axes.
1572 This function draws the axes of the world/object coordinate system w.r.t. to the camera frame.
1573 OX is drawn in red, OY in green and OZ in blue.
1575 CV_EXPORTS_W void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs,
1576 InputArray rvec, InputArray tvec, float length, int thickness=3);
1578 struct CV_EXPORTS_W_SIMPLE CirclesGridFinderParameters
1580 CV_WRAP CirclesGridFinderParameters();
1581 CV_PROP_RW cv::Size2f densityNeighborhoodSize;
1582 CV_PROP_RW float minDensity;
1583 CV_PROP_RW int kmeansAttempts;
1584 CV_PROP_RW int minDistanceToAddKeypoint;
1585 CV_PROP_RW int keypointScale;
1586 CV_PROP_RW float minGraphConfidence;
1587 CV_PROP_RW float vertexGain;
1588 CV_PROP_RW float vertexPenalty;
1589 CV_PROP_RW float existingVertexGain;
1590 CV_PROP_RW float edgeGain;
1591 CV_PROP_RW float edgePenalty;
1592 CV_PROP_RW float convexHullFactor;
1593 CV_PROP_RW float minRNGEdgeSwitchDist;
1597 SYMMETRIC_GRID, ASYMMETRIC_GRID
1601 CV_PROP_RW float squareSize; //!< Distance between two adjacent points. Used by CALIB_CB_CLUSTERING.
1602 CV_PROP_RW float maxRectifiedDistance; //!< Max deviation from prediction. Used by CALIB_CB_CLUSTERING.
1605 #ifndef DISABLE_OPENCV_3_COMPATIBILITY
1606 typedef CirclesGridFinderParameters CirclesGridFinderParameters2;
1609 /** @brief Finds centers in the grid of circles.
1611 @param image grid view of input circles; it must be an 8-bit grayscale or color image.
1612 @param patternSize number of circles per row and column
1613 ( patternSize = Size(points_per_row, points_per_colum) ).
1614 @param centers output array of detected centers.
1615 @param flags various operation flags that can be one of the following values:
1616 - **CALIB_CB_SYMMETRIC_GRID** uses symmetric pattern of circles.
1617 - **CALIB_CB_ASYMMETRIC_GRID** uses asymmetric pattern of circles.
1618 - **CALIB_CB_CLUSTERING** uses a special algorithm for grid detection. It is more robust to
1619 perspective distortions but much more sensitive to background clutter.
1620 @param blobDetector feature detector that finds blobs like dark circles on light background.
1621 @param parameters struct for finding circles in a grid pattern.
1623 The function attempts to determine whether the input image contains a grid of circles. If it is, the
1624 function locates centers of the circles. The function returns a non-zero value if all of the centers
1625 have been found and they have been placed in a certain order (row by row, left to right in every
1626 row). Otherwise, if the function fails to find all the corners or reorder them, it returns 0.
1628 Sample usage of detecting and drawing the centers of circles: :
1630 Size patternsize(7,7); //number of centers
1631 Mat gray = ....; //source image
1632 vector<Point2f> centers; //this will be filled by the detected centers
1634 bool patternfound = findCirclesGrid(gray, patternsize, centers);
1636 drawChessboardCorners(img, patternsize, Mat(centers), patternfound);
1638 @note The function requires white space (like a square-thick border, the wider the better) around
1639 the board to make the detection more robust in various environments.
1641 CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize,
1642 OutputArray centers, int flags,
1643 const Ptr<FeatureDetector> &blobDetector,
1644 const CirclesGridFinderParameters& parameters);
1647 CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize,
1648 OutputArray centers, int flags = CALIB_CB_SYMMETRIC_GRID,
1649 const Ptr<FeatureDetector> &blobDetector = SimpleBlobDetector::create());
1651 /** @brief Finds the camera intrinsic and extrinsic parameters from several views of a calibration
1654 @param objectPoints In the new interface it is a vector of vectors of calibration pattern points in
1655 the calibration pattern coordinate space (e.g. std::vector<std::vector<cv::Vec3f>>). The outer
1656 vector contains as many elements as the number of pattern views. If the same calibration pattern
1657 is shown in each view and it is fully visible, all the vectors will be the same. Although, it is
1658 possible to use partially occluded patterns or even different patterns in different views. Then,
1659 the vectors will be different. Although the points are 3D, they all lie in the calibration pattern's
1660 XY coordinate plane (thus 0 in the Z-coordinate), if the used calibration pattern is a planar rig.
1661 In the old interface all the vectors of object points from different views are concatenated
1663 @param imagePoints In the new interface it is a vector of vectors of the projections of calibration
1664 pattern points (e.g. std::vector<std::vector<cv::Vec2f>>). imagePoints.size() and
1665 objectPoints.size(), and imagePoints[i].size() and objectPoints[i].size() for each i, must be equal,
1666 respectively. In the old interface all the vectors of object points from different views are
1667 concatenated together.
1668 @param imageSize Size of the image used only to initialize the intrinsic camera matrix.
1669 @param cameraMatrix Input/output 3x3 floating-point camera matrix
1670 \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . If CV\_CALIB\_USE\_INTRINSIC\_GUESS
1671 and/or CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be
1672 initialized before calling the function.
1673 @param distCoeffs Input/output vector of distortion coefficients
1674 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
1675 4, 5, 8, 12 or 14 elements.
1676 @param rvecs Output vector of rotation vectors (@ref Rodrigues ) estimated for each pattern view
1677 (e.g. std::vector<cv::Mat>>). That is, each i-th rotation vector together with the corresponding
1678 i-th translation vector (see the next output parameter description) brings the calibration pattern
1679 from the object coordinate space (in which object points are specified) to the camera coordinate
1680 space. In more technical terms, the tuple of the i-th rotation and translation vector performs
1681 a change of basis from object coordinate space to camera coordinate space. Due to its duality, this
1682 tuple is equivalent to the position of the calibration pattern with respect to the camera coordinate
1684 @param tvecs Output vector of translation vectors estimated for each pattern view, see parameter
1686 @param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic
1687 parameters. Order of deviations values:
1688 \f$(f_x, f_y, c_x, c_y, k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 , s_1, s_2, s_3,
1689 s_4, \tau_x, \tau_y)\f$ If one of parameters is not estimated, it's deviation is equals to zero.
1690 @param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic
1691 parameters. Order of deviations values: \f$(R_0, T_0, \dotsc , R_{M - 1}, T_{M - 1})\f$ where M is
1692 the number of pattern views. \f$R_i, T_i\f$ are concatenated 1x3 vectors.
1693 @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view.
1694 @param flags Different flags that may be zero or a combination of the following values:
1695 - **CALIB_USE_INTRINSIC_GUESS** cameraMatrix contains valid initial values of
1696 fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image
1697 center ( imageSize is used), and focal distances are computed in a least-squares fashion.
1698 Note, that if intrinsic parameters are known, there is no need to use this function just to
1699 estimate extrinsic parameters. Use solvePnP instead.
1700 - **CALIB_FIX_PRINCIPAL_POINT** The principal point is not changed during the global
1701 optimization. It stays at the center or at a different location specified when
1702 CALIB_USE_INTRINSIC_GUESS is set too.
1703 - **CALIB_FIX_ASPECT_RATIO** The functions consider only fy as a free parameter. The
1704 ratio fx/fy stays the same as in the input cameraMatrix . When
1705 CALIB_USE_INTRINSIC_GUESS is not set, the actual input values of fx and fy are
1706 ignored, only their ratio is computed and used further.
1707 - **CALIB_ZERO_TANGENT_DIST** Tangential distortion coefficients \f$(p_1, p_2)\f$ are set
1708 to zeros and stay zero.
1709 - **CALIB_FIX_K1,...,CALIB_FIX_K6** The corresponding radial distortion
1710 coefficient is not changed during the optimization. If CALIB_USE_INTRINSIC_GUESS is
1711 set, the coefficient from the supplied distCoeffs matrix is used. Otherwise, it is set to 0.
1712 - **CALIB_RATIONAL_MODEL** Coefficients k4, k5, and k6 are enabled. To provide the
1713 backward compatibility, this extra flag should be explicitly specified to make the
1714 calibration function use the rational model and return 8 coefficients. If the flag is not
1715 set, the function computes and returns only 5 distortion coefficients.
1716 - **CALIB_THIN_PRISM_MODEL** Coefficients s1, s2, s3 and s4 are enabled. To provide the
1717 backward compatibility, this extra flag should be explicitly specified to make the
1718 calibration function use the thin prism model and return 12 coefficients. If the flag is not
1719 set, the function computes and returns only 5 distortion coefficients.
1720 - **CALIB_FIX_S1_S2_S3_S4** The thin prism distortion coefficients are not changed during
1721 the optimization. If CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the
1722 supplied distCoeffs matrix is used. Otherwise, it is set to 0.
1723 - **CALIB_TILTED_MODEL** Coefficients tauX and tauY are enabled. To provide the
1724 backward compatibility, this extra flag should be explicitly specified to make the
1725 calibration function use the tilted sensor model and return 14 coefficients. If the flag is not
1726 set, the function computes and returns only 5 distortion coefficients.
1727 - **CALIB_FIX_TAUX_TAUY** The coefficients of the tilted sensor model are not changed during
1728 the optimization. If CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the
1729 supplied distCoeffs matrix is used. Otherwise, it is set to 0.
1730 @param criteria Termination criteria for the iterative optimization algorithm.
1732 @return the overall RMS re-projection error.
1734 The function estimates the intrinsic camera parameters and extrinsic parameters for each of the
1735 views. The algorithm is based on @cite Zhang2000 and @cite BouguetMCT . The coordinates of 3D object
1736 points and their corresponding 2D projections in each view must be specified. That may be achieved
1737 by using an object with known geometry and easily detectable feature points. Such an object is
1738 called a calibration rig or calibration pattern, and OpenCV has built-in support for a chessboard as
1739 a calibration rig (see @ref findChessboardCorners). Currently, initialization of intrinsic
1740 parameters (when CALIB_USE_INTRINSIC_GUESS is not set) is only implemented for planar calibration
1741 patterns (where Z-coordinates of the object points must be all zeros). 3D calibration rigs can also
1742 be used as long as initial cameraMatrix is provided.
1744 The algorithm performs the following steps:
1746 - Compute the initial intrinsic parameters (the option only available for planar calibration
1747 patterns) or read them from the input parameters. The distortion coefficients are all set to
1748 zeros initially unless some of CALIB_FIX_K? are specified.
1750 - Estimate the initial camera pose as if the intrinsic parameters have been already known. This is
1751 done using solvePnP .
1753 - Run the global Levenberg-Marquardt optimization algorithm to minimize the reprojection error,
1754 that is, the total sum of squared distances between the observed feature points imagePoints and
1755 the projected (using the current estimates for camera parameters and the poses) object points
1756 objectPoints. See projectPoints for details.
1759 If you use a non-square (i.e. non-N-by-N) grid and @ref findChessboardCorners for calibration,
1760 and @ref calibrateCamera returns bad values (zero distortion coefficients, \f$c_x\f$ and
1761 \f$c_y\f$ very far from the image center, and/or large differences between \f$f_x\f$ and
1762 \f$f_y\f$ (ratios of 10:1 or more)), then you are probably using patternSize=cvSize(rows,cols)
1763 instead of using patternSize=cvSize(cols,rows) in @ref findChessboardCorners.
1766 calibrateCameraRO, findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate,
1769 CV_EXPORTS_AS(calibrateCameraExtended) double calibrateCamera( InputArrayOfArrays objectPoints,
1770 InputArrayOfArrays imagePoints, Size imageSize,
1771 InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
1772 OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
1773 OutputArray stdDeviationsIntrinsics,
1774 OutputArray stdDeviationsExtrinsics,
1775 OutputArray perViewErrors,
1776 int flags = 0, TermCriteria criteria = TermCriteria(
1777 TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
1780 CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
1781 InputArrayOfArrays imagePoints, Size imageSize,
1782 InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
1783 OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
1784 int flags = 0, TermCriteria criteria = TermCriteria(
1785 TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
1787 /** @brief Finds the camera intrinsic and extrinsic parameters from several views of a calibration pattern.
1789 This function is an extension of calibrateCamera() with the method of releasing object which was
1790 proposed in @cite strobl2011iccv. In many common cases with inaccurate, unmeasured, roughly planar
1791 targets (calibration plates), this method can dramatically improve the precision of the estimated
1792 camera parameters. Both the object-releasing method and standard method are supported by this
1793 function. Use the parameter **iFixedPoint** for method selection. In the internal implementation,
1794 calibrateCamera() is a wrapper for this function.
1796 @param objectPoints Vector of vectors of calibration pattern points in the calibration pattern
1797 coordinate space. See calibrateCamera() for details. If the method of releasing object to be used,
1798 the identical calibration board must be used in each view and it must be fully visible, and all
1799 objectPoints[i] must be the same and all points should be roughly close to a plane. **The calibration
1800 target has to be rigid, or at least static if the camera (rather than the calibration target) is
1801 shifted for grabbing images.**
1802 @param imagePoints Vector of vectors of the projections of calibration pattern points. See
1803 calibrateCamera() for details.
1804 @param imageSize Size of the image used only to initialize the intrinsic camera matrix.
1805 @param iFixedPoint The index of the 3D object point in objectPoints[0] to be fixed. It also acts as
1806 a switch for calibration method selection. If object-releasing method to be used, pass in the
1807 parameter in the range of [1, objectPoints[0].size()-2], otherwise a value out of this range will
1808 make standard calibration method selected. Usually the top-right corner point of the calibration
1809 board grid is recommended to be fixed when object-releasing method being utilized. According to
1810 \cite strobl2011iccv, two other points are also fixed. In this implementation, objectPoints[0].front
1811 and objectPoints[0].back.z are used. With object-releasing method, accurate rvecs, tvecs and
1812 newObjPoints are only possible if coordinates of these three fixed points are accurate enough.
1813 @param cameraMatrix Output 3x3 floating-point camera matrix. See calibrateCamera() for details.
1814 @param distCoeffs Output vector of distortion coefficients. See calibrateCamera() for details.
1815 @param rvecs Output vector of rotation vectors estimated for each pattern view. See calibrateCamera()
1817 @param tvecs Output vector of translation vectors estimated for each pattern view.
1818 @param newObjPoints The updated output vector of calibration pattern points. The coordinates might
1819 be scaled based on three fixed points. The returned coordinates are accurate only if the above
1820 mentioned three fixed points are accurate. If not needed, noArray() can be passed in. This parameter
1821 is ignored with standard calibration method.
1822 @param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic parameters.
1823 See calibrateCamera() for details.
1824 @param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic parameters.
1825 See calibrateCamera() for details.
1826 @param stdDeviationsObjPoints Output vector of standard deviations estimated for refined coordinates
1827 of calibration pattern points. It has the same size and order as objectPoints[0] vector. This
1828 parameter is ignored with standard calibration method.
1829 @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view.
1830 @param flags Different flags that may be zero or a combination of some predefined values. See
1831 calibrateCamera() for details. If the method of releasing object is used, the calibration time may
1832 be much longer. CALIB_USE_QR or CALIB_USE_LU could be used for faster calibration with potentially
1833 less precise and less stable in some rare cases.
1834 @param criteria Termination criteria for the iterative optimization algorithm.
1836 @return the overall RMS re-projection error.
1838 The function estimates the intrinsic camera parameters and extrinsic parameters for each of the
1839 views. The algorithm is based on @cite Zhang2000, @cite BouguetMCT and @cite strobl2011iccv. See
1840 calibrateCamera() for other detailed explanations.
1842 calibrateCamera, findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate, undistort
1844 CV_EXPORTS_AS(calibrateCameraROExtended) double calibrateCameraRO( InputArrayOfArrays objectPoints,
1845 InputArrayOfArrays imagePoints, Size imageSize, int iFixedPoint,
1846 InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
1847 OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
1848 OutputArray newObjPoints,
1849 OutputArray stdDeviationsIntrinsics,
1850 OutputArray stdDeviationsExtrinsics,
1851 OutputArray stdDeviationsObjPoints,
1852 OutputArray perViewErrors,
1853 int flags = 0, TermCriteria criteria = TermCriteria(
1854 TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
1857 CV_EXPORTS_W double calibrateCameraRO( InputArrayOfArrays objectPoints,
1858 InputArrayOfArrays imagePoints, Size imageSize, int iFixedPoint,
1859 InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
1860 OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
1861 OutputArray newObjPoints,
1862 int flags = 0, TermCriteria criteria = TermCriteria(
1863 TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
1865 /** @brief Computes useful camera characteristics from the camera matrix.
1867 @param cameraMatrix Input camera matrix that can be estimated by calibrateCamera or
1869 @param imageSize Input image size in pixels.
1870 @param apertureWidth Physical width in mm of the sensor.
1871 @param apertureHeight Physical height in mm of the sensor.
1872 @param fovx Output field of view in degrees along the horizontal sensor axis.
1873 @param fovy Output field of view in degrees along the vertical sensor axis.
1874 @param focalLength Focal length of the lens in mm.
1875 @param principalPoint Principal point in mm.
1876 @param aspectRatio \f$f_y/f_x\f$
1878 The function computes various useful camera characteristics from the previously estimated camera
1882 Do keep in mind that the unity measure 'mm' stands for whatever unit of measure one chooses for
1883 the chessboard pitch (it can thus be any value).
1885 CV_EXPORTS_W void calibrationMatrixValues( InputArray cameraMatrix, Size imageSize,
1886 double apertureWidth, double apertureHeight,
1887 CV_OUT double& fovx, CV_OUT double& fovy,
1888 CV_OUT double& focalLength, CV_OUT Point2d& principalPoint,
1889 CV_OUT double& aspectRatio );
1891 /** @brief Calibrates a stereo camera set up. This function finds the intrinsic parameters
1892 for each of the two cameras and the extrinsic parameters between the two cameras.
1894 @param objectPoints Vector of vectors of the calibration pattern points. The same structure as
1895 in @ref calibrateCamera. For each pattern view, both cameras need to see the same object
1896 points. Therefore, objectPoints.size(), imagePoints1.size(), and imagePoints2.size() need to be
1897 equal as well as objectPoints[i].size(), imagePoints1[i].size(), and imagePoints2[i].size() need to
1898 be equal for each i.
1899 @param imagePoints1 Vector of vectors of the projections of the calibration pattern points,
1900 observed by the first camera. The same structure as in @ref calibrateCamera.
1901 @param imagePoints2 Vector of vectors of the projections of the calibration pattern points,
1902 observed by the second camera. The same structure as in @ref calibrateCamera.
1903 @param cameraMatrix1 Input/output camera matrix for the first camera, the same as in
1904 @ref calibrateCamera. Furthermore, for the stereo case, additional flags may be used, see below.
1905 @param distCoeffs1 Input/output vector of distortion coefficients, the same as in
1906 @ref calibrateCamera.
1907 @param cameraMatrix2 Input/output second camera matrix for the second camera. See description for
1909 @param distCoeffs2 Input/output lens distortion coefficients for the second camera. See
1910 description for distCoeffs1.
1911 @param imageSize Size of the image used only to initialize the intrinsic camera matrices.
1912 @param R Output rotation matrix. Together with the translation vector T, this matrix brings
1913 points given in the first camera's coordinate system to points in the second camera's
1914 coordinate system. In more technical terms, the tuple of R and T performs a change of basis
1915 from the first camera's coordinate system to the second camera's coordinate system. Due to its
1916 duality, this tuple is equivalent to the position of the first camera with respect to the
1917 second camera coordinate system.
1918 @param T Output translation vector, see description above.
1919 @param E Output essential matrix.
1920 @param F Output fundamental matrix.
1921 @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view.
1922 @param flags Different flags that may be zero or a combination of the following values:
1923 - **CALIB_FIX_INTRINSIC** Fix cameraMatrix? and distCoeffs? so that only R, T, E, and F
1924 matrices are estimated.
1925 - **CALIB_USE_INTRINSIC_GUESS** Optimize some or all of the intrinsic parameters
1926 according to the specified flags. Initial values are provided by the user.
1927 - **CALIB_USE_EXTRINSIC_GUESS** R and T contain valid initial values that are optimized further.
1928 Otherwise R and T are initialized to the median value of the pattern views (each dimension separately).
1929 - **CALIB_FIX_PRINCIPAL_POINT** Fix the principal points during the optimization.
1930 - **CALIB_FIX_FOCAL_LENGTH** Fix \f$f^{(j)}_x\f$ and \f$f^{(j)}_y\f$ .
1931 - **CALIB_FIX_ASPECT_RATIO** Optimize \f$f^{(j)}_y\f$ . Fix the ratio \f$f^{(j)}_x/f^{(j)}_y\f$
1933 - **CALIB_SAME_FOCAL_LENGTH** Enforce \f$f^{(0)}_x=f^{(1)}_x\f$ and \f$f^{(0)}_y=f^{(1)}_y\f$ .
1934 - **CALIB_ZERO_TANGENT_DIST** Set tangential distortion coefficients for each camera to
1935 zeros and fix there.
1936 - **CALIB_FIX_K1,...,CALIB_FIX_K6** Do not change the corresponding radial
1937 distortion coefficient during the optimization. If CALIB_USE_INTRINSIC_GUESS is set,
1938 the coefficient from the supplied distCoeffs matrix is used. Otherwise, it is set to 0.
1939 - **CALIB_RATIONAL_MODEL** Enable coefficients k4, k5, and k6. To provide the backward
1940 compatibility, this extra flag should be explicitly specified to make the calibration
1941 function use the rational model and return 8 coefficients. If the flag is not set, the
1942 function computes and returns only 5 distortion coefficients.
1943 - **CALIB_THIN_PRISM_MODEL** Coefficients s1, s2, s3 and s4 are enabled. To provide the
1944 backward compatibility, this extra flag should be explicitly specified to make the
1945 calibration function use the thin prism model and return 12 coefficients. If the flag is not
1946 set, the function computes and returns only 5 distortion coefficients.
1947 - **CALIB_FIX_S1_S2_S3_S4** The thin prism distortion coefficients are not changed during
1948 the optimization. If CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the
1949 supplied distCoeffs matrix is used. Otherwise, it is set to 0.
1950 - **CALIB_TILTED_MODEL** Coefficients tauX and tauY are enabled. To provide the
1951 backward compatibility, this extra flag should be explicitly specified to make the
1952 calibration function use the tilted sensor model and return 14 coefficients. If the flag is not
1953 set, the function computes and returns only 5 distortion coefficients.
1954 - **CALIB_FIX_TAUX_TAUY** The coefficients of the tilted sensor model are not changed during
1955 the optimization. If CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the
1956 supplied distCoeffs matrix is used. Otherwise, it is set to 0.
1957 @param criteria Termination criteria for the iterative optimization algorithm.
1959 The function estimates the transformation between two cameras making a stereo pair. If one computes
1960 the poses of an object relative to the first camera and to the second camera,
1961 ( \f$R_1\f$,\f$T_1\f$ ) and (\f$R_2\f$,\f$T_2\f$), respectively, for a stereo camera where the
1962 relative position and orientation between the two cameras are fixed, then those poses definitely
1963 relate to each other. This means, if the relative position and orientation (\f$R\f$,\f$T\f$) of the
1964 two cameras is known, it is possible to compute (\f$R_2\f$,\f$T_2\f$) when (\f$R_1\f$,\f$T_1\f$) is
1965 given. This is what the described function does. It computes (\f$R\f$,\f$T\f$) such that:
1968 \f[T_2=R T_1 + T.\f]
1970 Therefore, one can compute the coordinate representation of a 3D point for the second camera's
1971 coordinate system when given the point's coordinate representation in the first camera's coordinate
1979 \end{bmatrix} = \begin{bmatrix}
1982 \end{bmatrix} \begin{bmatrix}
1990 Optionally, it computes the essential matrix E:
1992 \f[E= \vecthreethree{0}{-T_2}{T_1}{T_2}{0}{-T_0}{-T_1}{T_0}{0} R\f]
1994 where \f$T_i\f$ are components of the translation vector \f$T\f$ : \f$T=[T_0, T_1, T_2]^T\f$ .
1995 And the function can also compute the fundamental matrix F:
1997 \f[F = cameraMatrix2^{-T}\cdot E \cdot cameraMatrix1^{-1}\f]
1999 Besides the stereo-related information, the function can also perform a full calibration of each of
2000 the two cameras. However, due to the high dimensionality of the parameter space and noise in the
2001 input data, the function can diverge from the correct solution. If the intrinsic parameters can be
2002 estimated with high accuracy for each of the cameras individually (for example, using
2003 calibrateCamera ), you are recommended to do so and then pass CALIB_FIX_INTRINSIC flag to the
2004 function along with the computed intrinsic parameters. Otherwise, if all the parameters are
2005 estimated at once, it makes sense to restrict some parameters, for example, pass
2006 CALIB_SAME_FOCAL_LENGTH and CALIB_ZERO_TANGENT_DIST flags, which is usually a
2007 reasonable assumption.
2009 Similarly to calibrateCamera, the function minimizes the total re-projection error for all the
2010 points in all the available views from both cameras. The function returns the final value of the
2011 re-projection error.
2013 CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate( InputArrayOfArrays objectPoints,
2014 InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
2015 InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1,
2016 InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2,
2017 Size imageSize, InputOutputArray R,InputOutputArray T, OutputArray E, OutputArray F,
2018 OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC,
2019 TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );
2022 CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints,
2023 InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
2024 InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1,
2025 InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2,
2026 Size imageSize, OutputArray R,OutputArray T, OutputArray E, OutputArray F,
2027 int flags = CALIB_FIX_INTRINSIC,
2028 TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) );
2030 /** @brief Computes rectification transforms for each head of a calibrated stereo camera.
2032 @param cameraMatrix1 First camera matrix.
2033 @param distCoeffs1 First camera distortion parameters.
2034 @param cameraMatrix2 Second camera matrix.
2035 @param distCoeffs2 Second camera distortion parameters.
2036 @param imageSize Size of the image used for stereo calibration.
2037 @param R Rotation matrix from the coordinate system of the first camera to the second camera,
2038 see @ref stereoCalibrate.
2039 @param T Translation vector from the coordinate system of the first camera to the second camera,
2040 see @ref stereoCalibrate.
2041 @param R1 Output 3x3 rectification transform (rotation matrix) for the first camera. This matrix
2042 brings points given in the unrectified first camera's coordinate system to points in the rectified
2043 first camera's coordinate system. In more technical terms, it performs a change of basis from the
2044 unrectified first camera's coordinate system to the rectified first camera's coordinate system.
2045 @param R2 Output 3x3 rectification transform (rotation matrix) for the second camera. This matrix
2046 brings points given in the unrectified second camera's coordinate system to points in the rectified
2047 second camera's coordinate system. In more technical terms, it performs a change of basis from the
2048 unrectified second camera's coordinate system to the rectified second camera's coordinate system.
2049 @param P1 Output 3x4 projection matrix in the new (rectified) coordinate systems for the first
2050 camera, i.e. it projects points given in the rectified first camera coordinate system into the
2051 rectified first camera's image.
2052 @param P2 Output 3x4 projection matrix in the new (rectified) coordinate systems for the second
2053 camera, i.e. it projects points given in the rectified first camera coordinate system into the
2054 rectified second camera's image.
2055 @param Q Output \f$4 \times 4\f$ disparity-to-depth mapping matrix (see @ref reprojectImageTo3D).
2056 @param flags Operation flags that may be zero or CALIB_ZERO_DISPARITY . If the flag is set,
2057 the function makes the principal points of each camera have the same pixel coordinates in the
2058 rectified views. And if the flag is not set, the function may still shift the images in the
2059 horizontal or vertical direction (depending on the orientation of epipolar lines) to maximize the
2061 @param alpha Free scaling parameter. If it is -1 or absent, the function performs the default
2062 scaling. Otherwise, the parameter should be between 0 and 1. alpha=0 means that the rectified
2063 images are zoomed and shifted so that only valid pixels are visible (no black areas after
2064 rectification). alpha=1 means that the rectified image is decimated and shifted so that all the
2065 pixels from the original images from the cameras are retained in the rectified images (no source
2066 image pixels are lost). Any intermediate value yields an intermediate result between
2067 those two extreme cases.
2068 @param newImageSize New image resolution after rectification. The same size should be passed to
2069 initUndistortRectifyMap (see the stereo_calib.cpp sample in OpenCV samples directory). When (0,0)
2070 is passed (default), it is set to the original imageSize . Setting it to a larger value can help you
2071 preserve details in the original image, especially when there is a big radial distortion.
2072 @param validPixROI1 Optional output rectangles inside the rectified images where all the pixels
2073 are valid. If alpha=0 , the ROIs cover the whole images. Otherwise, they are likely to be smaller
2074 (see the picture below).
2075 @param validPixROI2 Optional output rectangles inside the rectified images where all the pixels
2076 are valid. If alpha=0 , the ROIs cover the whole images. Otherwise, they are likely to be smaller
2077 (see the picture below).
2079 The function computes the rotation matrices for each camera that (virtually) make both camera image
2080 planes the same plane. Consequently, this makes all the epipolar lines parallel and thus simplifies
2081 the dense stereo correspondence problem. The function takes the matrices computed by stereoCalibrate
2082 as input. As output, it provides two rotation matrices and also two projection matrices in the new
2083 coordinates. The function distinguishes the following two cases:
2085 - **Horizontal stereo**: the first and the second camera views are shifted relative to each other
2086 mainly along the x-axis (with possible small vertical shift). In the rectified images, the
2087 corresponding epipolar lines in the left and right cameras are horizontal and have the same
2088 y-coordinate. P1 and P2 look like:
2090 \f[\texttt{P1} = \begin{bmatrix}
2096 \f[\texttt{P2} = \begin{bmatrix}
2097 f & 0 & cx_2 & T_x*f \\
2102 where \f$T_x\f$ is a horizontal shift between the cameras and \f$cx_1=cx_2\f$ if
2103 CALIB_ZERO_DISPARITY is set.
2105 - **Vertical stereo**: the first and the second camera views are shifted relative to each other
2106 mainly in the vertical direction (and probably a bit in the horizontal direction too). The epipolar
2107 lines in the rectified images are vertical and have the same x-coordinate. P1 and P2 look like:
2109 \f[\texttt{P1} = \begin{bmatrix}
2115 \f[\texttt{P2} = \begin{bmatrix}
2117 0 & f & cy_2 & T_y*f \\
2121 where \f$T_y\f$ is a vertical shift between the cameras and \f$cy_1=cy_2\f$ if
2122 CALIB_ZERO_DISPARITY is set.
2124 As you can see, the first three columns of P1 and P2 will effectively be the new "rectified" camera
2125 matrices. The matrices, together with R1 and R2 , can then be passed to initUndistortRectifyMap to
2126 initialize the rectification map for each camera.
2128 See below the screenshot from the stereo_calib.cpp sample. Some red horizontal lines pass through
2129 the corresponding image regions. This means that the images are well rectified, which is what most
2130 stereo correspondence algorithms rely on. The green rectangles are roi1 and roi2 . You see that
2131 their interiors are all valid pixels.
2133 ![image](pics/stereo_undistort.jpg)
2135 CV_EXPORTS_W void stereoRectify( InputArray cameraMatrix1, InputArray distCoeffs1,
2136 InputArray cameraMatrix2, InputArray distCoeffs2,
2137 Size imageSize, InputArray R, InputArray T,
2138 OutputArray R1, OutputArray R2,
2139 OutputArray P1, OutputArray P2,
2140 OutputArray Q, int flags = CALIB_ZERO_DISPARITY,
2141 double alpha = -1, Size newImageSize = Size(),
2142 CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 );
2144 /** @brief Computes a rectification transform for an uncalibrated stereo camera.
2146 @param points1 Array of feature points in the first image.
2147 @param points2 The corresponding points in the second image. The same formats as in
2148 findFundamentalMat are supported.
2149 @param F Input fundamental matrix. It can be computed from the same set of point pairs using
2150 findFundamentalMat .
2151 @param imgSize Size of the image.
2152 @param H1 Output rectification homography matrix for the first image.
2153 @param H2 Output rectification homography matrix for the second image.
2154 @param threshold Optional threshold used to filter out the outliers. If the parameter is greater
2155 than zero, all the point pairs that do not comply with the epipolar geometry (that is, the points
2156 for which \f$|\texttt{points2[i]}^T*\texttt{F}*\texttt{points1[i]}|>\texttt{threshold}\f$ ) are
2157 rejected prior to computing the homographies. Otherwise, all the points are considered inliers.
2159 The function computes the rectification transformations without knowing intrinsic parameters of the
2160 cameras and their relative position in the space, which explains the suffix "uncalibrated". Another
2161 related difference from stereoRectify is that the function outputs not the rectification
2162 transformations in the object (3D) space, but the planar perspective transformations encoded by the
2163 homography matrices H1 and H2 . The function implements the algorithm @cite Hartley99 .
2166 While the algorithm does not need to know the intrinsic parameters of the cameras, it heavily
2167 depends on the epipolar geometry. Therefore, if the camera lenses have a significant distortion,
2168 it would be better to correct it before computing the fundamental matrix and calling this
2169 function. For example, distortion coefficients can be estimated for each head of stereo camera
2170 separately by using calibrateCamera . Then, the images can be corrected using undistort , or
2171 just the point coordinates can be corrected with undistortPoints .
2173 CV_EXPORTS_W bool stereoRectifyUncalibrated( InputArray points1, InputArray points2,
2174 InputArray F, Size imgSize,
2175 OutputArray H1, OutputArray H2,
2176 double threshold = 5 );
2178 //! computes the rectification transformations for 3-head camera, where all the heads are on the same line.
2179 CV_EXPORTS_W float rectify3Collinear( InputArray cameraMatrix1, InputArray distCoeffs1,
2180 InputArray cameraMatrix2, InputArray distCoeffs2,
2181 InputArray cameraMatrix3, InputArray distCoeffs3,
2182 InputArrayOfArrays imgpt1, InputArrayOfArrays imgpt3,
2183 Size imageSize, InputArray R12, InputArray T12,
2184 InputArray R13, InputArray T13,
2185 OutputArray R1, OutputArray R2, OutputArray R3,
2186 OutputArray P1, OutputArray P2, OutputArray P3,
2187 OutputArray Q, double alpha, Size newImgSize,
2188 CV_OUT Rect* roi1, CV_OUT Rect* roi2, int flags );
2190 /** @brief Returns the new camera matrix based on the free scaling parameter.
2192 @param cameraMatrix Input camera matrix.
2193 @param distCoeffs Input vector of distortion coefficients
2194 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ of
2195 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are
2197 @param imageSize Original image size.
2198 @param alpha Free scaling parameter between 0 (when all the pixels in the undistorted image are
2199 valid) and 1 (when all the source image pixels are retained in the undistorted image). See
2200 stereoRectify for details.
2201 @param newImgSize Image size after rectification. By default, it is set to imageSize .
2202 @param validPixROI Optional output rectangle that outlines all-good-pixels region in the
2203 undistorted image. See roi1, roi2 description in stereoRectify .
2204 @param centerPrincipalPoint Optional flag that indicates whether in the new camera matrix the
2205 principal point should be at the image center or not. By default, the principal point is chosen to
2206 best fit a subset of the source image (determined by alpha) to the corrected image.
2207 @return new_camera_matrix Output new camera matrix.
2209 The function computes and returns the optimal new camera matrix based on the free scaling parameter.
2210 By varying this parameter, you may retrieve only sensible pixels alpha=0 , keep all the original
2211 image pixels if there is valuable information in the corners alpha=1 , or get something in between.
2212 When alpha\>0 , the undistorted result is likely to have some black pixels corresponding to
2213 "virtual" pixels outside of the captured distorted image. The original camera matrix, distortion
2214 coefficients, the computed new camera matrix, and newImageSize should be passed to
2215 initUndistortRectifyMap to produce the maps for remap .
2217 CV_EXPORTS_W Mat getOptimalNewCameraMatrix( InputArray cameraMatrix, InputArray distCoeffs,
2218 Size imageSize, double alpha, Size newImgSize = Size(),
2219 CV_OUT Rect* validPixROI = 0,
2220 bool centerPrincipalPoint = false);
2222 /** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$
2224 @param[in] R_gripper2base Rotation part extracted from the homogeneous matrix that transforms a point
2225 expressed in the gripper frame to the robot base frame (\f$_{}^{b}\textrm{T}_g\f$).
2226 This is a vector (`vector<Mat>`) that contains the rotation matrices for all the transformations
2227 from gripper frame to robot base frame.
2228 @param[in] t_gripper2base Translation part extracted from the homogeneous matrix that transforms a point
2229 expressed in the gripper frame to the robot base frame (\f$_{}^{b}\textrm{T}_g\f$).
2230 This is a vector (`vector<Mat>`) that contains the translation vectors for all the transformations
2231 from gripper frame to robot base frame.
2232 @param[in] R_target2cam Rotation part extracted from the homogeneous matrix that transforms a point
2233 expressed in the target frame to the camera frame (\f$_{}^{c}\textrm{T}_t\f$).
2234 This is a vector (`vector<Mat>`) that contains the rotation matrices for all the transformations
2235 from calibration target frame to camera frame.
2236 @param[in] t_target2cam Rotation part extracted from the homogeneous matrix that transforms a point
2237 expressed in the target frame to the camera frame (\f$_{}^{c}\textrm{T}_t\f$).
2238 This is a vector (`vector<Mat>`) that contains the translation vectors for all the transformations
2239 from calibration target frame to camera frame.
2240 @param[out] R_cam2gripper Estimated rotation part extracted from the homogeneous matrix that transforms a point
2241 expressed in the camera frame to the gripper frame (\f$_{}^{g}\textrm{T}_c\f$).
2242 @param[out] t_cam2gripper Estimated translation part extracted from the homogeneous matrix that transforms a point
2243 expressed in the camera frame to the gripper frame (\f$_{}^{g}\textrm{T}_c\f$).
2244 @param[in] method One of the implemented Hand-Eye calibration method, see cv::HandEyeCalibrationMethod
2246 The function performs the Hand-Eye calibration using various methods. One approach consists in estimating the
2247 rotation then the translation (separable solutions) and the following methods are implemented:
2248 - R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/EyeCalibration \cite Tsai89
2249 - F. Park, B. Martin Robot Sensor Calibration: Solving AX = XB on the Euclidean Group \cite Park94
2250 - R. Horaud, F. Dornaika Hand-Eye Calibration \cite Horaud95
2252 Another approach consists in estimating simultaneously the rotation and the translation (simultaneous solutions),
2253 with the following implemented method:
2254 - N. Andreff, R. Horaud, B. Espiau On-line Hand-Eye Calibration \cite Andreff99
2255 - K. Daniilidis Hand-Eye Calibration Using Dual Quaternions \cite Daniilidis98
2257 The following picture describes the Hand-Eye calibration problem where the transformation between a camera ("eye")
2258 mounted on a robot gripper ("hand") has to be estimated.
2260 ![](pics/hand-eye_figure.png)
2262 The calibration procedure is the following:
2263 - a static calibration pattern is used to estimate the transformation between the target frame
2264 and the camera frame
2265 - the robot gripper is moved in order to acquire several poses
2266 - for each pose, the homogeneous transformation between the gripper frame and the robot base frame is recorded using for
2267 instance the robot kinematics
2277 _{}^{b}\textrm{R}_g & _{}^{b}\textrm{t}_g \\
2287 - for each pose, the homogeneous transformation between the calibration target frame and the camera frame is recorded using
2288 for instance a pose estimation method (PnP) from 2D-3D point correspondences
2298 _{}^{c}\textrm{R}_t & _{}^{c}\textrm{t}_t \\
2309 The Hand-Eye calibration procedure returns the following homogeneous transformation
2319 _{}^{g}\textrm{R}_c & _{}^{g}\textrm{t}_c \\
2330 This problem is also known as solving the \f$\mathbf{A}\mathbf{X}=\mathbf{X}\mathbf{B}\f$ equation:
2333 ^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(1)} &=
2334 \hspace{0.1em} ^{b}{\textrm{T}_g}^{(2)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} \\
2336 (^{b}{\textrm{T}_g}^{(2)})^{-1} \hspace{0.2em} ^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c &=
2337 \hspace{0.1em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} (^{c}{\textrm{T}_t}^{(1)})^{-1} \\
2339 \textrm{A}_i \textrm{X} &= \textrm{X} \textrm{B}_i \\
2344 Additional information can be found on this [website](http://campar.in.tum.de/Chair/HandEyeCalibration).
2346 A minimum of 2 motions with non parallel rotation axes are necessary to determine the hand-eye transformation.
2347 So at least 3 different poses are required, but it is strongly recommended to use many more poses.
2350 CV_EXPORTS_W void calibrateHandEye( InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base,
2351 InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam,
2352 OutputArray R_cam2gripper, OutputArray t_cam2gripper,
2353 HandEyeCalibrationMethod method=CALIB_HAND_EYE_TSAI );
2355 /** @brief Converts points from Euclidean to homogeneous space.
2357 @param src Input vector of N-dimensional points.
2358 @param dst Output vector of N+1-dimensional points.
2360 The function converts points from Euclidean to homogeneous space by appending 1's to the tuple of
2361 point coordinates. That is, each point (x1, x2, ..., xn) is converted to (x1, x2, ..., xn, 1).
2363 CV_EXPORTS_W void convertPointsToHomogeneous( InputArray src, OutputArray dst );
2365 /** @brief Converts points from homogeneous to Euclidean space.
2367 @param src Input vector of N-dimensional points.
2368 @param dst Output vector of N-1-dimensional points.
2370 The function converts points homogeneous to Euclidean space using perspective projection. That is,
2371 each point (x1, x2, ... x(n-1), xn) is converted to (x1/xn, x2/xn, ..., x(n-1)/xn). When xn=0, the
2372 output point coordinates will be (0,0,0,...).
2374 CV_EXPORTS_W void convertPointsFromHomogeneous( InputArray src, OutputArray dst );
2376 /** @brief Converts points to/from homogeneous coordinates.
2378 @param src Input array or vector of 2D, 3D, or 4D points.
2379 @param dst Output vector of 2D, 3D, or 4D points.
2381 The function converts 2D or 3D points from/to homogeneous coordinates by calling either
2382 convertPointsToHomogeneous or convertPointsFromHomogeneous.
2384 @note The function is obsolete. Use one of the previous two functions instead.
2386 CV_EXPORTS void convertPointsHomogeneous( InputArray src, OutputArray dst );
2388 /** @brief Calculates a fundamental matrix from the corresponding points in two images.
2390 @param points1 Array of N points from the first image. The point coordinates should be
2391 floating-point (single or double precision).
2392 @param points2 Array of the second image points of the same size and format as points1 .
2393 @param method Method for computing a fundamental matrix.
2394 - **CV_FM_7POINT** for a 7-point algorithm. \f$N = 7\f$
2395 - **CV_FM_8POINT** for an 8-point algorithm. \f$N \ge 8\f$
2396 - **CV_FM_RANSAC** for the RANSAC algorithm. \f$N \ge 8\f$
2397 - **CV_FM_LMEDS** for the LMedS algorithm. \f$N \ge 8\f$
2398 @param ransacReprojThreshold Parameter used only for RANSAC. It is the maximum distance from a point to an epipolar
2399 line in pixels, beyond which the point is considered an outlier and is not used for computing the
2400 final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the
2401 point localization, image resolution, and the image noise.
2402 @param confidence Parameter used for the RANSAC and LMedS methods only. It specifies a desirable level
2403 of confidence (probability) that the estimated matrix is correct.
2405 @param maxIters The maximum number of robust method iterations.
2407 The epipolar geometry is described by the following equation:
2409 \f[[p_2; 1]^T F [p_1; 1] = 0\f]
2411 where \f$F\f$ is a fundamental matrix, \f$p_1\f$ and \f$p_2\f$ are corresponding points in the first and the
2412 second images, respectively.
2414 The function calculates the fundamental matrix using one of four methods listed above and returns
2415 the found fundamental matrix. Normally just one matrix is found. But in case of the 7-point
2416 algorithm, the function may return up to 3 solutions ( \f$9 \times 3\f$ matrix that stores all 3
2417 matrices sequentially).
2419 The calculated fundamental matrix may be passed further to computeCorrespondEpilines that finds the
2420 epipolar lines corresponding to the specified points. It can also be passed to
2421 stereoRectifyUncalibrated to compute the rectification transformation. :
2423 // Example. Estimation of fundamental matrix using the RANSAC algorithm
2424 int point_count = 100;
2425 vector<Point2f> points1(point_count);
2426 vector<Point2f> points2(point_count);
2428 // initialize the points here ...
2429 for( int i = 0; i < point_count; i++ )
2435 Mat fundamental_matrix =
2436 findFundamentalMat(points1, points2, FM_RANSAC, 3, 0.99);
2439 CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2,
2440 int method, double ransacReprojThreshold, double confidence,
2441 int maxIters, OutputArray mask = noArray() );
2444 CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2,
2445 int method = FM_RANSAC,
2446 double ransacReprojThreshold = 3., double confidence = 0.99,
2447 OutputArray mask = noArray() );
2450 CV_EXPORTS Mat findFundamentalMat( InputArray points1, InputArray points2,
2451 OutputArray mask, int method = FM_RANSAC,
2452 double ransacReprojThreshold = 3., double confidence = 0.99 );
2454 /** @brief Calculates an essential matrix from the corresponding points in two images.
2456 @param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should
2457 be floating-point (single or double precision).
2458 @param points2 Array of the second image points of the same size and format as points1 .
2459 @param cameraMatrix Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
2460 Note that this function assumes that points1 and points2 are feature points from cameras with the
2461 same camera matrix. If this assumption does not hold for your use case, use
2462 `undistortPoints()` with `P = cv::NoArray()` for both cameras to transform image points
2463 to normalized image coordinates, which are valid for the identity camera matrix. When
2464 passing these coordinates, pass the identity matrix for this parameter.
2465 @param method Method for computing an essential matrix.
2466 - **RANSAC** for the RANSAC algorithm.
2467 - **LMEDS** for the LMedS algorithm.
2468 @param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of
2469 confidence (probability) that the estimated matrix is correct.
2470 @param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar
2471 line in pixels, beyond which the point is considered an outlier and is not used for computing the
2472 final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the
2473 point localization, image resolution, and the image noise.
2474 @param mask Output array of N elements, every element of which is set to 0 for outliers and to 1
2475 for the other points. The array is computed only in the RANSAC and LMedS methods.
2477 This function estimates essential matrix based on the five-point algorithm solver in @cite Nister03 .
2478 @cite SteweniusCFS is also a related. The epipolar geometry is described by the following equation:
2480 \f[[p_2; 1]^T K^{-T} E K^{-1} [p_1; 1] = 0\f]
2482 where \f$E\f$ is an essential matrix, \f$p_1\f$ and \f$p_2\f$ are corresponding points in the first and the
2483 second images, respectively. The result of this function may be passed further to
2484 decomposeEssentialMat or recoverPose to recover the relative pose between cameras.
2486 CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
2487 InputArray cameraMatrix, int method = RANSAC,
2488 double prob = 0.999, double threshold = 1.0,
2489 OutputArray mask = noArray() );
2492 @param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should
2493 be floating-point (single or double precision).
2494 @param points2 Array of the second image points of the same size and format as points1 .
2495 @param focal focal length of the camera. Note that this function assumes that points1 and points2
2496 are feature points from cameras with same focal length and principal point.
2497 @param pp principal point of the camera.
2498 @param method Method for computing a fundamental matrix.
2499 - **RANSAC** for the RANSAC algorithm.
2500 - **LMEDS** for the LMedS algorithm.
2501 @param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar
2502 line in pixels, beyond which the point is considered an outlier and is not used for computing the
2503 final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the
2504 point localization, image resolution, and the image noise.
2505 @param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of
2506 confidence (probability) that the estimated matrix is correct.
2507 @param mask Output array of N elements, every element of which is set to 0 for outliers and to 1
2508 for the other points. The array is computed only in the RANSAC and LMedS methods.
2510 This function differs from the one above that it computes camera matrix from focal length and
2520 CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
2521 double focal = 1.0, Point2d pp = Point2d(0, 0),
2522 int method = RANSAC, double prob = 0.999,
2523 double threshold = 1.0, OutputArray mask = noArray() );
2525 /** @brief Calculates an essential matrix from the corresponding points in two images from potentially two different cameras.
2527 @param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should
2528 be floating-point (single or double precision).
2529 @param points2 Array of the second image points of the same size and format as points1 .
2530 @param cameraMatrix1 Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
2531 Note that this function assumes that points1 and points2 are feature points from cameras with the
2532 same camera matrix. If this assumption does not hold for your use case, use
2533 `undistortPoints()` with `P = cv::NoArray()` for both cameras to transform image points
2534 to normalized image coordinates, which are valid for the identity camera matrix. When
2535 passing these coordinates, pass the identity matrix for this parameter.
2536 @param cameraMatrix2 Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
2537 Note that this function assumes that points1 and points2 are feature points from cameras with the
2538 same camera matrix. If this assumption does not hold for your use case, use
2539 `undistortPoints()` with `P = cv::NoArray()` for both cameras to transform image points
2540 to normalized image coordinates, which are valid for the identity camera matrix. When
2541 passing these coordinates, pass the identity matrix for this parameter.
2542 @param distCoeffs1 Input vector of distortion coefficients
2543 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
2544 of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
2545 @param distCoeffs2 Input vector of distortion coefficients
2546 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
2547 of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
2548 @param method Method for computing an essential matrix.
2549 - **RANSAC** for the RANSAC algorithm.
2550 - **LMEDS** for the LMedS algorithm.
2551 @param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of
2552 confidence (probability) that the estimated matrix is correct.
2553 @param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar
2554 line in pixels, beyond which the point is considered an outlier and is not used for computing the
2555 final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the
2556 point localization, image resolution, and the image noise.
2557 @param mask Output array of N elements, every element of which is set to 0 for outliers and to 1
2558 for the other points. The array is computed only in the RANSAC and LMedS methods.
2560 This function estimates essential matrix based on the five-point algorithm solver in @cite Nister03 .
2561 @cite SteweniusCFS is also a related. The epipolar geometry is described by the following equation:
2563 \f[[p_2; 1]^T K^{-T} E K^{-1} [p_1; 1] = 0\f]
2565 where \f$E\f$ is an essential matrix, \f$p_1\f$ and \f$p_2\f$ are corresponding points in the first and the
2566 second images, respectively. The result of this function may be passed further to
2567 decomposeEssentialMat or recoverPose to recover the relative pose between cameras.
2569 CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
2570 InputArray cameraMatrix1, InputArray distCoeffs1,
2571 InputArray cameraMatrix2, InputArray distCoeffs2,
2572 int method = RANSAC,
2573 double prob = 0.999, double threshold = 1.0,
2574 OutputArray mask = noArray() );
2576 /** @brief Decompose an essential matrix to possible rotations and translation.
2578 @param E The input essential matrix.
2579 @param R1 One possible rotation matrix.
2580 @param R2 Another possible rotation matrix.
2581 @param t One possible translation.
2583 This function decomposes the essential matrix E using svd decomposition @cite HartleyZ00. In
2584 general, four possible poses exist for the decomposition of E. They are \f$[R_1, t]\f$,
2585 \f$[R_1, -t]\f$, \f$[R_2, t]\f$, \f$[R_2, -t]\f$.
2587 If E gives the epipolar constraint \f$[p_2; 1]^T A^{-T} E A^{-1} [p_1; 1] = 0\f$ between the image
2588 points \f$p_1\f$ in the first image and \f$p_2\f$ in second image, then any of the tuples
2589 \f$[R_1, t]\f$, \f$[R_1, -t]\f$, \f$[R_2, t]\f$, \f$[R_2, -t]\f$ is a change of basis from the first
2590 camera's coordinate system to the second camera's coordinate system. However, by decomposing E, one
2591 can only get the direction of the translation. For this reason, the translation t is returned with
2594 CV_EXPORTS_W void decomposeEssentialMat( InputArray E, OutputArray R1, OutputArray R2, OutputArray t );
2596 /** @brief Recovers the relative camera rotation and the translation from an estimated essential
2597 matrix and the corresponding points in two images, using cheirality check. Returns the number of
2598 inliers that pass the check.
2600 @param E The input essential matrix.
2601 @param points1 Array of N 2D points from the first image. The point coordinates should be
2602 floating-point (single or double precision).
2603 @param points2 Array of the second image points of the same size and format as points1 .
2604 @param cameraMatrix Camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
2605 Note that this function assumes that points1 and points2 are feature points from cameras with the
2607 @param R Output rotation matrix. Together with the translation vector, this matrix makes up a tuple
2608 that performs a change of basis from the first camera's coordinate system to the second camera's
2609 coordinate system. Note that, in general, t can not be used for this tuple, see the parameter
2611 @param t Output translation vector. This vector is obtained by @ref decomposeEssentialMat and
2612 therefore is only known up to scale, i.e. t is the direction of the translation vector and has unit
2614 @param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks
2615 inliers in points1 and points2 for then given essential matrix E. Only these inliers will be used to
2616 recover pose. In the output mask only inliers which pass the cheirality check.
2618 This function decomposes an essential matrix using @ref decomposeEssentialMat and then verifies
2619 possible pose hypotheses by doing cheirality check. The cheirality check means that the
2620 triangulated 3D points should have positive depth. Some details can be found in @cite Nister03.
2622 This function can be used to process the output E and mask from @ref findEssentialMat. In this
2623 scenario, points1 and points2 are the same input for findEssentialMat.:
2625 // Example. Estimation of fundamental matrix using the RANSAC algorithm
2626 int point_count = 100;
2627 vector<Point2f> points1(point_count);
2628 vector<Point2f> points2(point_count);
2630 // initialize the points here ...
2631 for( int i = 0; i < point_count; i++ )
2637 // cametra matrix with both focal lengths = 1, and principal point = (0, 0)
2638 Mat cameraMatrix = Mat::eye(3, 3, CV_64F);
2642 E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, mask);
2643 recoverPose(E, points1, points2, cameraMatrix, R, t, mask);
2646 CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2,
2647 InputArray cameraMatrix, OutputArray R, OutputArray t,
2648 InputOutputArray mask = noArray() );
2651 @param E The input essential matrix.
2652 @param points1 Array of N 2D points from the first image. The point coordinates should be
2653 floating-point (single or double precision).
2654 @param points2 Array of the second image points of the same size and format as points1 .
2655 @param R Output rotation matrix. Together with the translation vector, this matrix makes up a tuple
2656 that performs a change of basis from the first camera's coordinate system to the second camera's
2657 coordinate system. Note that, in general, t can not be used for this tuple, see the parameter
2659 @param t Output translation vector. This vector is obtained by @ref decomposeEssentialMat and
2660 therefore is only known up to scale, i.e. t is the direction of the translation vector and has unit
2662 @param focal Focal length of the camera. Note that this function assumes that points1 and points2
2663 are feature points from cameras with same focal length and principal point.
2664 @param pp principal point of the camera.
2665 @param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks
2666 inliers in points1 and points2 for then given essential matrix E. Only these inliers will be used to
2667 recover pose. In the output mask only inliers which pass the cheirality check.
2669 This function differs from the one above that it computes camera matrix from focal length and
2679 CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2,
2680 OutputArray R, OutputArray t,
2681 double focal = 1.0, Point2d pp = Point2d(0, 0),
2682 InputOutputArray mask = noArray() );
2685 @param E The input essential matrix.
2686 @param points1 Array of N 2D points from the first image. The point coordinates should be
2687 floating-point (single or double precision).
2688 @param points2 Array of the second image points of the same size and format as points1.
2689 @param cameraMatrix Camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
2690 Note that this function assumes that points1 and points2 are feature points from cameras with the
2692 @param R Output rotation matrix. Together with the translation vector, this matrix makes up a tuple
2693 that performs a change of basis from the first camera's coordinate system to the second camera's
2694 coordinate system. Note that, in general, t can not be used for this tuple, see the parameter
2696 @param t Output translation vector. This vector is obtained by @ref decomposeEssentialMat and
2697 therefore is only known up to scale, i.e. t is the direction of the translation vector and has unit
2699 @param distanceThresh threshold distance which is used to filter out far away points (i.e. infinite
2701 @param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks
2702 inliers in points1 and points2 for then given essential matrix E. Only these inliers will be used to
2703 recover pose. In the output mask only inliers which pass the cheirality check.
2704 @param triangulatedPoints 3D points which were reconstructed by triangulation.
2706 This function differs from the one above that it outputs the triangulated 3D point that are used for
2707 the cheirality check.
2709 CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2,
2710 InputArray cameraMatrix, OutputArray R, OutputArray t, double distanceThresh, InputOutputArray mask = noArray(),
2711 OutputArray triangulatedPoints = noArray());
2713 /** @brief For points in an image of a stereo pair, computes the corresponding epilines in the other image.
2715 @param points Input points. \f$N \times 1\f$ or \f$1 \times N\f$ matrix of type CV_32FC2 or
2717 @param whichImage Index of the image (1 or 2) that contains the points .
2718 @param F Fundamental matrix that can be estimated using findFundamentalMat or stereoRectify .
2719 @param lines Output vector of the epipolar lines corresponding to the points in the other image.
2720 Each line \f$ax + by + c=0\f$ is encoded by 3 numbers \f$(a, b, c)\f$ .
2722 For every point in one of the two images of a stereo pair, the function finds the equation of the
2723 corresponding epipolar line in the other image.
2725 From the fundamental matrix definition (see findFundamentalMat ), line \f$l^{(2)}_i\f$ in the second
2726 image for the point \f$p^{(1)}_i\f$ in the first image (when whichImage=1 ) is computed as:
2728 \f[l^{(2)}_i = F p^{(1)}_i\f]
2730 And vice versa, when whichImage=2, \f$l^{(1)}_i\f$ is computed from \f$p^{(2)}_i\f$ as:
2732 \f[l^{(1)}_i = F^T p^{(2)}_i\f]
2734 Line coefficients are defined up to a scale. They are normalized so that \f$a_i^2+b_i^2=1\f$ .
2736 CV_EXPORTS_W void computeCorrespondEpilines( InputArray points, int whichImage,
2737 InputArray F, OutputArray lines );
2739 /** @brief This function reconstructs 3-dimensional points (in homogeneous coordinates) by using
2740 their observations with a stereo camera.
2742 @param projMatr1 3x4 projection matrix of the first camera, i.e. this matrix projects 3D points
2743 given in the world's coordinate system into the first image.
2744 @param projMatr2 3x4 projection matrix of the second camera, i.e. this matrix projects 3D points
2745 given in the world's coordinate system into the second image.
2746 @param projPoints1 2xN array of feature points in the first image. In the case of the c++ version,
2747 it can be also a vector of feature points or two-channel matrix of size 1xN or Nx1.
2748 @param projPoints2 2xN array of corresponding points in the second image. In the case of the c++
2749 version, it can be also a vector of feature points or two-channel matrix of size 1xN or Nx1.
2750 @param points4D 4xN array of reconstructed points in homogeneous coordinates. These points are
2751 returned in the world's coordinate system.
2754 Keep in mind that all input data should be of float type in order for this function to work.
2757 If the projection matrices from @ref stereoRectify are used, then the returned points are
2758 represented in the first camera's rectified coordinate system.
2763 CV_EXPORTS_W void triangulatePoints( InputArray projMatr1, InputArray projMatr2,
2764 InputArray projPoints1, InputArray projPoints2,
2765 OutputArray points4D );
2767 /** @brief Refines coordinates of corresponding points.
2769 @param F 3x3 fundamental matrix.
2770 @param points1 1xN array containing the first set of points.
2771 @param points2 1xN array containing the second set of points.
2772 @param newPoints1 The optimized points1.
2773 @param newPoints2 The optimized points2.
2775 The function implements the Optimal Triangulation Method (see Multiple View Geometry for details).
2776 For each given point correspondence points1[i] \<-\> points2[i], and a fundamental matrix F, it
2777 computes the corrected correspondences newPoints1[i] \<-\> newPoints2[i] that minimize the geometric
2778 error \f$d(points1[i], newPoints1[i])^2 + d(points2[i],newPoints2[i])^2\f$ (where \f$d(a,b)\f$ is the
2779 geometric distance between points \f$a\f$ and \f$b\f$ ) subject to the epipolar constraint
2780 \f$newPoints2^T * F * newPoints1 = 0\f$ .
2782 CV_EXPORTS_W void correctMatches( InputArray F, InputArray points1, InputArray points2,
2783 OutputArray newPoints1, OutputArray newPoints2 );
2785 /** @brief Filters off small noise blobs (speckles) in the disparity map
2787 @param img The input 16-bit signed disparity image
2788 @param newVal The disparity value used to paint-off the speckles
2789 @param maxSpeckleSize The maximum speckle size to consider it a speckle. Larger blobs are not
2790 affected by the algorithm
2791 @param maxDiff Maximum difference between neighbor disparity pixels to put them into the same
2792 blob. Note that since StereoBM, StereoSGBM and may be other algorithms return a fixed-point
2793 disparity map, where disparity values are multiplied by 16, this scale factor should be taken into
2794 account when specifying this parameter value.
2795 @param buf The optional temporary buffer to avoid memory allocation within the function.
2797 CV_EXPORTS_W void filterSpeckles( InputOutputArray img, double newVal,
2798 int maxSpeckleSize, double maxDiff,
2799 InputOutputArray buf = noArray() );
2801 //! computes valid disparity ROI from the valid ROIs of the rectified images (that are returned by cv::stereoRectify())
2802 CV_EXPORTS_W Rect getValidDisparityROI( Rect roi1, Rect roi2,
2803 int minDisparity, int numberOfDisparities,
2806 //! validates disparity using the left-right check. The matrix "cost" should be computed by the stereo correspondence algorithm
2807 CV_EXPORTS_W void validateDisparity( InputOutputArray disparity, InputArray cost,
2808 int minDisparity, int numberOfDisparities,
2809 int disp12MaxDisp = 1 );
2811 /** @brief Reprojects a disparity image to 3D space.
2813 @param disparity Input single-channel 8-bit unsigned, 16-bit signed, 32-bit signed or 32-bit
2814 floating-point disparity image. The values of 8-bit / 16-bit signed formats are assumed to have no
2815 fractional bits. If the disparity is 16-bit signed format, as computed by @ref StereoBM or
2816 @ref StereoSGBM and maybe other algorithms, it should be divided by 16 (and scaled to float) before
2818 @param _3dImage Output 3-channel floating-point image of the same size as disparity. Each element of
2819 _3dImage(x,y) contains 3D coordinates of the point (x,y) computed from the disparity map. If one
2820 uses Q obtained by @ref stereoRectify, then the returned points are represented in the first
2821 camera's rectified coordinate system.
2822 @param Q \f$4 \times 4\f$ perspective transformation matrix that can be obtained with
2824 @param handleMissingValues Indicates, whether the function should handle missing values (i.e.
2825 points where the disparity was not computed). If handleMissingValues=true, then pixels with the
2826 minimal disparity that corresponds to the outliers (see StereoMatcher::compute ) are transformed
2827 to 3D points with a very large Z value (currently set to 10000).
2828 @param ddepth The optional output array depth. If it is -1, the output image will have CV_32F
2829 depth. ddepth can also be set to CV_16S, CV_32S or CV_32F.
2831 The function transforms a single-channel disparity map to a 3-channel image representing a 3D
2832 surface. That is, for each pixel (x,y) and the corresponding disparity d=disparity(x,y) , it
2840 \end{bmatrix} = Q \begin{bmatrix}
2843 \texttt{disparity} (x,y) \\
2848 To reproject a sparse set of points {(x,y,d),...} to 3D space, use perspectiveTransform.
2850 CV_EXPORTS_W void reprojectImageTo3D( InputArray disparity,
2851 OutputArray _3dImage, InputArray Q,
2852 bool handleMissingValues = false,
2855 /** @brief Calculates the Sampson Distance between two points.
2857 The function cv::sampsonDistance calculates and returns the first order approximation of the geometric error as:
2859 sd( \texttt{pt1} , \texttt{pt2} )=
2860 \frac{(\texttt{pt2}^t \cdot \texttt{F} \cdot \texttt{pt1})^2}
2861 {((\texttt{F} \cdot \texttt{pt1})(0))^2 +
2862 ((\texttt{F} \cdot \texttt{pt1})(1))^2 +
2863 ((\texttt{F}^t \cdot \texttt{pt2})(0))^2 +
2864 ((\texttt{F}^t \cdot \texttt{pt2})(1))^2}
2866 The fundamental matrix may be calculated using the cv::findFundamentalMat function. See @cite HartleyZ00 11.4.3 for details.
2867 @param pt1 first homogeneous 2d point
2868 @param pt2 second homogeneous 2d point
2869 @param F fundamental matrix
2870 @return The computed Sampson distance.
2872 CV_EXPORTS_W double sampsonDistance(InputArray pt1, InputArray pt2, InputArray F);
2874 /** @brief Computes an optimal affine transformation between two 3D point sets.
2885 a_{11} & a_{12} & a_{13}\\
2886 a_{21} & a_{22} & a_{23}\\
2887 a_{31} & a_{32} & a_{33}\\
2902 @param src First input 3D point set containing \f$(X,Y,Z)\f$.
2903 @param dst Second input 3D point set containing \f$(x,y,z)\f$.
2904 @param out Output 3D affine transformation matrix \f$3 \times 4\f$ of the form
2907 a_{11} & a_{12} & a_{13} & b_1\\
2908 a_{21} & a_{22} & a_{23} & b_2\\
2909 a_{31} & a_{32} & a_{33} & b_3\\
2912 @param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier).
2913 @param ransacThreshold Maximum reprojection error in the RANSAC algorithm to consider a point as
2915 @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything
2916 between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation
2917 significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation.
2919 The function estimates an optimal 3D affine transformation between two 3D point sets using the
2922 CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst,
2923 OutputArray out, OutputArray inliers,
2924 double ransacThreshold = 3, double confidence = 0.99);
2926 /** @brief Computes an optimal translation between two 3D point sets.
2949 * @param src First input 3D point set containing \f$(X,Y,Z)\f$.
2950 * @param dst Second input 3D point set containing \f$(x,y,z)\f$.
2951 * @param out Output 3D translation vector \f$3 \times 1\f$ of the form
2959 * @param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier).
2960 * @param ransacThreshold Maximum reprojection error in the RANSAC algorithm to consider a point as
2962 * @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything
2963 * between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation
2964 * significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation.
2966 * The function estimates an optimal 3D translation between two 3D point sets using the
2969 CV_EXPORTS_W int estimateTranslation3D(InputArray src, InputArray dst,
2970 OutputArray out, OutputArray inliers,
2971 double ransacThreshold = 3, double confidence = 0.99);
2973 /** @brief Computes an optimal affine transformation between two 2D point sets.
2997 @param from First input 2D point set containing \f$(X,Y)\f$.
2998 @param to Second input 2D point set containing \f$(x,y)\f$.
2999 @param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier).
3000 @param method Robust method used to compute transformation. The following methods are possible:
3001 - cv::RANSAC - RANSAC-based robust method
3002 - cv::LMEDS - Least-Median robust method
3003 RANSAC is the default method.
3004 @param ransacReprojThreshold Maximum reprojection error in the RANSAC algorithm to consider
3005 a point as an inlier. Applies only to RANSAC.
3006 @param maxIters The maximum number of robust method iterations.
3007 @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything
3008 between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation
3009 significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation.
3010 @param refineIters Maximum number of iterations of refining algorithm (Levenberg-Marquardt).
3011 Passing 0 will disable refining, so the output matrix will be output of robust method.
3013 @return Output 2D affine transformation matrix \f$2 \times 3\f$ or empty matrix if transformation
3014 could not be estimated. The returned matrix has the following form:
3017 a_{11} & a_{12} & b_1\\
3018 a_{21} & a_{22} & b_2\\
3022 The function estimates an optimal 2D affine transformation between two 2D point sets using the
3023 selected robust algorithm.
3025 The computed transformation is then refined further (using only inliers) with the
3026 Levenberg-Marquardt method to reduce the re-projection error even more.
3029 The RANSAC method can handle practically any ratio of outliers but needs a threshold to
3030 distinguish inliers from outliers. The method LMeDS does not need any threshold but it works
3031 correctly only when there are more than 50% of inliers.
3033 @sa estimateAffinePartial2D, getAffineTransform
3035 CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers = noArray(),
3036 int method = RANSAC, double ransacReprojThreshold = 3,
3037 size_t maxIters = 2000, double confidence = 0.99,
3038 size_t refineIters = 10);
3040 /** @brief Computes an optimal limited affine transformation with 4 degrees of freedom between
3043 @param from First input 2D point set.
3044 @param to Second input 2D point set.
3045 @param inliers Output vector indicating which points are inliers.
3046 @param method Robust method used to compute transformation. The following methods are possible:
3047 - cv::RANSAC - RANSAC-based robust method
3048 - cv::LMEDS - Least-Median robust method
3049 RANSAC is the default method.
3050 @param ransacReprojThreshold Maximum reprojection error in the RANSAC algorithm to consider
3051 a point as an inlier. Applies only to RANSAC.
3052 @param maxIters The maximum number of robust method iterations.
3053 @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything
3054 between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation
3055 significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation.
3056 @param refineIters Maximum number of iterations of refining algorithm (Levenberg-Marquardt).
3057 Passing 0 will disable refining, so the output matrix will be output of robust method.
3059 @return Output 2D affine transformation (4 degrees of freedom) matrix \f$2 \times 3\f$ or
3060 empty matrix if transformation could not be estimated.
3062 The function estimates an optimal 2D affine transformation with 4 degrees of freedom limited to
3063 combinations of translation, rotation, and uniform scaling. Uses the selected algorithm for robust
3066 The computed transformation is then refined further (using only inliers) with the
3067 Levenberg-Marquardt method to reduce the re-projection error even more.
3069 Estimated transformation matrix is:
3070 \f[ \begin{bmatrix} \cos(\theta) \cdot s & -\sin(\theta) \cdot s & t_x \\
3071 \sin(\theta) \cdot s & \cos(\theta) \cdot s & t_y
3073 Where \f$ \theta \f$ is the rotation angle, \f$ s \f$ the scaling factor and \f$ t_x, t_y \f$ are
3074 translations in \f$ x, y \f$ axes respectively.
3077 The RANSAC method can handle practically any ratio of outliers but need a threshold to
3078 distinguish inliers from outliers. The method LMeDS does not need any threshold but it works
3079 correctly only when there are more than 50% of inliers.
3081 @sa estimateAffine2D, getAffineTransform
3083 CV_EXPORTS_W cv::Mat estimateAffinePartial2D(InputArray from, InputArray to, OutputArray inliers = noArray(),
3084 int method = RANSAC, double ransacReprojThreshold = 3,
3085 size_t maxIters = 2000, double confidence = 0.99,
3086 size_t refineIters = 10);
3088 /** @example samples/cpp/tutorial_code/features2D/Homography/decompose_homography.cpp
3089 An example program with homography decomposition.
3091 Check @ref tutorial_homography "the corresponding tutorial" for more details.
3094 /** @brief Decompose a homography matrix to rotation(s), translation(s) and plane normal(s).
3096 @param H The input homography matrix between two images.
3097 @param K The input intrinsic camera calibration matrix.
3098 @param rotations Array of rotation matrices.
3099 @param translations Array of translation matrices.
3100 @param normals Array of plane normal matrices.
3102 This function extracts relative camera motion between two views of a planar object and returns up to
3103 four mathematical solution tuples of rotation, translation, and plane normal. The decomposition of
3104 the homography matrix H is described in detail in @cite Malis.
3106 If the homography H, induced by the plane, gives the constraint
3107 \f[s_i \vecthree{x'_i}{y'_i}{1} \sim H \vecthree{x_i}{y_i}{1}\f] on the source image points
3108 \f$p_i\f$ and the destination image points \f$p'_i\f$, then the tuple of rotations[k] and
3109 translations[k] is a change of basis from the source camera's coordinate system to the destination
3110 camera's coordinate system. However, by decomposing H, one can only get the translation normalized
3111 by the (typically unknown) depth of the scene, i.e. its direction but with normalized length.
3113 If point correspondences are available, at least two solutions may further be invalidated, by
3114 applying positive depth constraint, i.e. all points must be in front of the camera.
3116 CV_EXPORTS_W int decomposeHomographyMat(InputArray H,
3118 OutputArrayOfArrays rotations,
3119 OutputArrayOfArrays translations,
3120 OutputArrayOfArrays normals);
3122 /** @brief Filters homography decompositions based on additional information.
3124 @param rotations Vector of rotation matrices.
3125 @param normals Vector of plane normal matrices.
3126 @param beforePoints Vector of (rectified) visible reference points before the homography is applied
3127 @param afterPoints Vector of (rectified) visible reference points after the homography is applied
3128 @param possibleSolutions Vector of int indices representing the viable solution set after filtering
3129 @param pointsMask optional Mat/Vector of 8u type representing the mask for the inliers as given by the findHomography function
3131 This function is intended to filter the output of the decomposeHomographyMat based on additional
3132 information as described in @cite Malis . The summary of the method: the decomposeHomographyMat function
3133 returns 2 unique solutions and their "opposites" for a total of 4 solutions. If we have access to the
3134 sets of points visible in the camera frame before and after the homography transformation is applied,
3135 we can determine which are the true potential solutions and which are the opposites by verifying which
3136 homographies are consistent with all visible reference points being in front of the camera. The inputs
3137 are left unchanged; the filtered solution set is returned as indices into the existing one.
3140 CV_EXPORTS_W void filterHomographyDecompByVisibleRefpoints(InputArrayOfArrays rotations,
3141 InputArrayOfArrays normals,
3142 InputArray beforePoints,
3143 InputArray afterPoints,
3144 OutputArray possibleSolutions,
3145 InputArray pointsMask = noArray());
3147 /** @brief The base class for stereo correspondence algorithms.
3149 class CV_EXPORTS_W StereoMatcher : public Algorithm
3152 enum { DISP_SHIFT = 4,
3153 DISP_SCALE = (1 << DISP_SHIFT)
3156 /** @brief Computes disparity map for the specified stereo pair
3158 @param left Left 8-bit single-channel image.
3159 @param right Right image of the same size and the same type as the left one.
3160 @param disparity Output disparity map. It has the same size as the input images. Some algorithms,
3161 like StereoBM or StereoSGBM compute 16-bit fixed-point disparity map (where each disparity value
3162 has 4 fractional bits), whereas other algorithms output 32-bit floating-point disparity map.
3164 CV_WRAP virtual void compute( InputArray left, InputArray right,
3165 OutputArray disparity ) = 0;
3167 CV_WRAP virtual int getMinDisparity() const = 0;
3168 CV_WRAP virtual void setMinDisparity(int minDisparity) = 0;
3170 CV_WRAP virtual int getNumDisparities() const = 0;
3171 CV_WRAP virtual void setNumDisparities(int numDisparities) = 0;
3173 CV_WRAP virtual int getBlockSize() const = 0;
3174 CV_WRAP virtual void setBlockSize(int blockSize) = 0;
3176 CV_WRAP virtual int getSpeckleWindowSize() const = 0;
3177 CV_WRAP virtual void setSpeckleWindowSize(int speckleWindowSize) = 0;
3179 CV_WRAP virtual int getSpeckleRange() const = 0;
3180 CV_WRAP virtual void setSpeckleRange(int speckleRange) = 0;
3182 CV_WRAP virtual int getDisp12MaxDiff() const = 0;
3183 CV_WRAP virtual void setDisp12MaxDiff(int disp12MaxDiff) = 0;
3187 /** @brief Class for computing stereo correspondence using the block matching algorithm, introduced and
3188 contributed to OpenCV by K. Konolige.
3190 class CV_EXPORTS_W StereoBM : public StereoMatcher
3193 enum { PREFILTER_NORMALIZED_RESPONSE = 0,
3194 PREFILTER_XSOBEL = 1
3197 CV_WRAP virtual int getPreFilterType() const = 0;
3198 CV_WRAP virtual void setPreFilterType(int preFilterType) = 0;
3200 CV_WRAP virtual int getPreFilterSize() const = 0;
3201 CV_WRAP virtual void setPreFilterSize(int preFilterSize) = 0;
3203 CV_WRAP virtual int getPreFilterCap() const = 0;
3204 CV_WRAP virtual void setPreFilterCap(int preFilterCap) = 0;
3206 CV_WRAP virtual int getTextureThreshold() const = 0;
3207 CV_WRAP virtual void setTextureThreshold(int textureThreshold) = 0;
3209 CV_WRAP virtual int getUniquenessRatio() const = 0;
3210 CV_WRAP virtual void setUniquenessRatio(int uniquenessRatio) = 0;
3212 CV_WRAP virtual int getSmallerBlockSize() const = 0;
3213 CV_WRAP virtual void setSmallerBlockSize(int blockSize) = 0;
3215 CV_WRAP virtual Rect getROI1() const = 0;
3216 CV_WRAP virtual void setROI1(Rect roi1) = 0;
3218 CV_WRAP virtual Rect getROI2() const = 0;
3219 CV_WRAP virtual void setROI2(Rect roi2) = 0;
3221 /** @brief Creates StereoBM object
3223 @param numDisparities the disparity search range. For each pixel algorithm will find the best
3224 disparity from 0 (default minimum disparity) to numDisparities. The search range can then be
3225 shifted by changing the minimum disparity.
3226 @param blockSize the linear size of the blocks compared by the algorithm. The size should be odd
3227 (as the block is centered at the current pixel). Larger block size implies smoother, though less
3228 accurate disparity map. Smaller block size gives more detailed disparity map, but there is higher
3229 chance for algorithm to find a wrong correspondence.
3231 The function create StereoBM object. You can then call StereoBM::compute() to compute disparity for
3232 a specific stereo pair.
3234 CV_WRAP static Ptr<StereoBM> create(int numDisparities = 0, int blockSize = 21);
3237 /** @brief The class implements the modified H. Hirschmuller algorithm @cite HH08 that differs from the original
3240 - By default, the algorithm is single-pass, which means that you consider only 5 directions
3241 instead of 8. Set mode=StereoSGBM::MODE_HH in createStereoSGBM to run the full variant of the
3242 algorithm but beware that it may consume a lot of memory.
3243 - The algorithm matches blocks, not individual pixels. Though, setting blockSize=1 reduces the
3244 blocks to single pixels.
3245 - Mutual information cost function is not implemented. Instead, a simpler Birchfield-Tomasi
3246 sub-pixel metric from @cite BT98 is used. Though, the color images are supported as well.
3247 - Some pre- and post- processing steps from K. Konolige algorithm StereoBM are included, for
3248 example: pre-filtering (StereoBM::PREFILTER_XSOBEL type) and post-filtering (uniqueness
3249 check, quadratic interpolation and speckle filtering).
3252 - (Python) An example illustrating the use of the StereoSGBM matching algorithm can be found
3253 at opencv_source_code/samples/python/stereo_match.py
3255 class CV_EXPORTS_W StereoSGBM : public StereoMatcher
3266 CV_WRAP virtual int getPreFilterCap() const = 0;
3267 CV_WRAP virtual void setPreFilterCap(int preFilterCap) = 0;
3269 CV_WRAP virtual int getUniquenessRatio() const = 0;
3270 CV_WRAP virtual void setUniquenessRatio(int uniquenessRatio) = 0;
3272 CV_WRAP virtual int getP1() const = 0;
3273 CV_WRAP virtual void setP1(int P1) = 0;
3275 CV_WRAP virtual int getP2() const = 0;
3276 CV_WRAP virtual void setP2(int P2) = 0;
3278 CV_WRAP virtual int getMode() const = 0;
3279 CV_WRAP virtual void setMode(int mode) = 0;
3281 /** @brief Creates StereoSGBM object
3283 @param minDisparity Minimum possible disparity value. Normally, it is zero but sometimes
3284 rectification algorithms can shift images, so this parameter needs to be adjusted accordingly.
3285 @param numDisparities Maximum disparity minus minimum disparity. The value is always greater than
3286 zero. In the current implementation, this parameter must be divisible by 16.
3287 @param blockSize Matched block size. It must be an odd number \>=1 . Normally, it should be
3288 somewhere in the 3..11 range.
3289 @param P1 The first parameter controlling the disparity smoothness. See below.
3290 @param P2 The second parameter controlling the disparity smoothness. The larger the values are,
3291 the smoother the disparity is. P1 is the penalty on the disparity change by plus or minus 1
3292 between neighbor pixels. P2 is the penalty on the disparity change by more than 1 between neighbor
3293 pixels. The algorithm requires P2 \> P1 . See stereo_match.cpp sample where some reasonably good
3294 P1 and P2 values are shown (like 8\*number_of_image_channels\*blockSize\*blockSize and
3295 32\*number_of_image_channels\*blockSize\*blockSize , respectively).
3296 @param disp12MaxDiff Maximum allowed difference (in integer pixel units) in the left-right
3297 disparity check. Set it to a non-positive value to disable the check.
3298 @param preFilterCap Truncation value for the prefiltered image pixels. The algorithm first
3299 computes x-derivative at each pixel and clips its value by [-preFilterCap, preFilterCap] interval.
3300 The result values are passed to the Birchfield-Tomasi pixel cost function.
3301 @param uniquenessRatio Margin in percentage by which the best (minimum) computed cost function
3302 value should "win" the second best value to consider the found match correct. Normally, a value
3303 within the 5-15 range is good enough.
3304 @param speckleWindowSize Maximum size of smooth disparity regions to consider their noise speckles
3305 and invalidate. Set it to 0 to disable speckle filtering. Otherwise, set it somewhere in the
3307 @param speckleRange Maximum disparity variation within each connected component. If you do speckle
3308 filtering, set the parameter to a positive value, it will be implicitly multiplied by 16.
3309 Normally, 1 or 2 is good enough.
3310 @param mode Set it to StereoSGBM::MODE_HH to run the full-scale two-pass dynamic programming
3311 algorithm. It will consume O(W\*H\*numDisparities) bytes, which is large for 640x480 stereo and
3312 huge for HD-size pictures. By default, it is set to false .
3314 The first constructor initializes StereoSGBM with all the default parameters. So, you only have to
3315 set StereoSGBM::numDisparities at minimum. The second constructor enables you to set each parameter
3318 CV_WRAP static Ptr<StereoSGBM> create(int minDisparity = 0, int numDisparities = 16, int blockSize = 3,
3319 int P1 = 0, int P2 = 0, int disp12MaxDiff = 0,
3320 int preFilterCap = 0, int uniquenessRatio = 0,
3321 int speckleWindowSize = 0, int speckleRange = 0,
3322 int mode = StereoSGBM::MODE_SGBM);
3326 //! cv::undistort mode
3329 PROJ_SPHERICAL_ORTHO = 0,
3330 PROJ_SPHERICAL_EQRECT = 1
3333 /** @brief Transforms an image to compensate for lens distortion.
3335 The function transforms an image to compensate radial and tangential lens distortion.
3337 The function is simply a combination of #initUndistortRectifyMap (with unity R ) and #remap
3338 (with bilinear interpolation). See the former function for details of the transformation being
3341 Those pixels in the destination image, for which there is no correspondent pixels in the source
3342 image, are filled with zeros (black color).
3344 A particular subset of the source image that will be visible in the corrected image can be regulated
3345 by newCameraMatrix. You can use #getOptimalNewCameraMatrix to compute the appropriate
3346 newCameraMatrix depending on your requirements.
3348 The camera matrix and the distortion parameters can be determined using #calibrateCamera. If
3349 the resolution of images is different from the resolution used at the calibration stage, \f$f_x,
3350 f_y, c_x\f$ and \f$c_y\f$ need to be scaled accordingly, while the distortion coefficients remain
3353 @param src Input (distorted) image.
3354 @param dst Output (corrected) image that has the same size and type as src .
3355 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
3356 @param distCoeffs Input vector of distortion coefficients
3357 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
3358 of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
3359 @param newCameraMatrix Camera matrix of the distorted image. By default, it is the same as
3360 cameraMatrix but you may additionally scale and shift the result by using a different matrix.
3362 CV_EXPORTS_W void undistort( InputArray src, OutputArray dst,
3363 InputArray cameraMatrix,
3364 InputArray distCoeffs,
3365 InputArray newCameraMatrix = noArray() );
3367 /** @brief Computes the undistortion and rectification transformation map.
3369 The function computes the joint undistortion and rectification transformation and represents the
3370 result in the form of maps for remap. The undistorted image looks like original, as if it is
3371 captured with a camera using the camera matrix =newCameraMatrix and zero distortion. In case of a
3372 monocular camera, newCameraMatrix is usually equal to cameraMatrix, or it can be computed by
3373 #getOptimalNewCameraMatrix for a better control over scaling. In case of a stereo camera,
3374 newCameraMatrix is normally set to P1 or P2 computed by #stereoRectify .
3376 Also, this new camera is oriented differently in the coordinate space, according to R. That, for
3377 example, helps to align two heads of a stereo camera so that the epipolar lines on both images
3378 become horizontal and have the same y- coordinate (in case of a horizontally aligned stereo camera).
3380 The function actually builds the maps for the inverse mapping algorithm that is used by remap. That
3381 is, for each pixel \f$(u, v)\f$ in the destination (corrected and rectified) image, the function
3382 computes the corresponding coordinates in the source image (that is, in the original image from
3383 camera). The following process is applied:
3386 x \leftarrow (u - {c'}_x)/{f'}_x \\
3387 y \leftarrow (v - {c'}_y)/{f'}_y \\
3388 {[X\,Y\,W]} ^T \leftarrow R^{-1}*[x \, y \, 1]^T \\
3389 x' \leftarrow X/W \\
3390 y' \leftarrow Y/W \\
3391 r^2 \leftarrow x'^2 + y'^2 \\
3392 x'' \leftarrow x' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}
3393 + 2p_1 x' y' + p_2(r^2 + 2 x'^2) + s_1 r^2 + s_2 r^4\\
3394 y'' \leftarrow y' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6}
3395 + p_1 (r^2 + 2 y'^2) + 2 p_2 x' y' + s_3 r^2 + s_4 r^4 \\
3396 s\vecthree{x'''}{y'''}{1} =
3397 \vecthreethree{R_{33}(\tau_x, \tau_y)}{0}{-R_{13}((\tau_x, \tau_y)}
3398 {0}{R_{33}(\tau_x, \tau_y)}{-R_{23}(\tau_x, \tau_y)}
3399 {0}{0}{1} R(\tau_x, \tau_y) \vecthree{x''}{y''}{1}\\
3400 map_x(u,v) \leftarrow x''' f_x + c_x \\
3401 map_y(u,v) \leftarrow y''' f_y + c_y
3404 where \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
3405 are the distortion coefficients.
3407 In case of a stereo camera, this function is called twice: once for each camera head, after
3408 stereoRectify, which in its turn is called after #stereoCalibrate. But if the stereo camera
3409 was not calibrated, it is still possible to compute the rectification transformations directly from
3410 the fundamental matrix using #stereoRectifyUncalibrated. For each camera, the function computes
3411 homography H as the rectification transformation in a pixel domain, not a rotation matrix R in 3D
3412 space. R can be computed from H as
3413 \f[\texttt{R} = \texttt{cameraMatrix} ^{-1} \cdot \texttt{H} \cdot \texttt{cameraMatrix}\f]
3414 where cameraMatrix can be chosen arbitrarily.
3416 @param cameraMatrix Input camera matrix \f$A=\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
3417 @param distCoeffs Input vector of distortion coefficients
3418 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
3419 of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
3420 @param R Optional rectification transformation in the object space (3x3 matrix). R1 or R2 ,
3421 computed by #stereoRectify can be passed here. If the matrix is empty, the identity transformation
3422 is assumed. In cvInitUndistortMap R assumed to be an identity matrix.
3423 @param newCameraMatrix New camera matrix \f$A'=\vecthreethree{f_x'}{0}{c_x'}{0}{f_y'}{c_y'}{0}{0}{1}\f$.
3424 @param size Undistorted image size.
3425 @param m1type Type of the first output map that can be CV_32FC1, CV_32FC2 or CV_16SC2, see #convertMaps
3426 @param map1 The first output map.
3427 @param map2 The second output map.
3430 void initUndistortRectifyMap(InputArray cameraMatrix, InputArray distCoeffs,
3431 InputArray R, InputArray newCameraMatrix,
3432 Size size, int m1type, OutputArray map1, OutputArray map2);
3434 //! initializes maps for #remap for wide-angle
3436 float initWideAngleProjMap(InputArray cameraMatrix, InputArray distCoeffs,
3437 Size imageSize, int destImageWidth,
3438 int m1type, OutputArray map1, OutputArray map2,
3439 enum UndistortTypes projType = PROJ_SPHERICAL_EQRECT, double alpha = 0);
3441 float initWideAngleProjMap(InputArray cameraMatrix, InputArray distCoeffs,
3442 Size imageSize, int destImageWidth,
3443 int m1type, OutputArray map1, OutputArray map2,
3444 int projType, double alpha = 0)
3446 return initWideAngleProjMap(cameraMatrix, distCoeffs, imageSize, destImageWidth,
3447 m1type, map1, map2, (UndistortTypes)projType, alpha);
3450 /** @brief Returns the default new camera matrix.
3452 The function returns the camera matrix that is either an exact copy of the input cameraMatrix (when
3453 centerPrinicipalPoint=false ), or the modified one (when centerPrincipalPoint=true).
3455 In the latter case, the new camera matrix will be:
3457 \f[\begin{bmatrix} f_x && 0 && ( \texttt{imgSize.width} -1)*0.5 \\ 0 && f_y && ( \texttt{imgSize.height} -1)*0.5 \\ 0 && 0 && 1 \end{bmatrix} ,\f]
3459 where \f$f_x\f$ and \f$f_y\f$ are \f$(0,0)\f$ and \f$(1,1)\f$ elements of cameraMatrix, respectively.
3461 By default, the undistortion functions in OpenCV (see #initUndistortRectifyMap, #undistort) do not
3462 move the principal point. However, when you work with stereo, it is important to move the principal
3463 points in both views to the same y-coordinate (which is required by most of stereo correspondence
3464 algorithms), and may be to the same x-coordinate too. So, you can form the new camera matrix for
3465 each view where the principal points are located at the center.
3467 @param cameraMatrix Input camera matrix.
3468 @param imgsize Camera view image size in pixels.
3469 @param centerPrincipalPoint Location of the principal point in the new camera matrix. The
3470 parameter indicates whether this location should be at the image center or not.
3473 Mat getDefaultNewCameraMatrix(InputArray cameraMatrix, Size imgsize = Size(),
3474 bool centerPrincipalPoint = false);
3476 /** @brief Computes the ideal point coordinates from the observed point coordinates.
3478 The function is similar to #undistort and #initUndistortRectifyMap but it operates on a
3479 sparse set of points instead of a raster image. Also the function performs a reverse transformation
3480 to projectPoints. In case of a 3D object, it does not reconstruct its 3D coordinates, but for a
3481 planar object, it does, up to a translation vector, if the proper R is specified.
3483 For each observed point coordinate \f$(u, v)\f$ the function computes:
3486 x^{"} \leftarrow (u - c_x)/f_x \\
3487 y^{"} \leftarrow (v - c_y)/f_y \\
3488 (x',y') = undistort(x^{"},y^{"}, \texttt{distCoeffs}) \\
3489 {[X\,Y\,W]} ^T \leftarrow R*[x' \, y' \, 1]^T \\
3492 \text{only performed if P is specified:} \\
3493 u' \leftarrow x {f'}_x + {c'}_x \\
3494 v' \leftarrow y {f'}_y + {c'}_y
3498 where *undistort* is an approximate iterative algorithm that estimates the normalized original
3499 point coordinates out of the normalized distorted point coordinates ("normalized" means that the
3500 coordinates do not depend on the camera matrix).
3502 The function can be used for both a stereo camera head or a monocular camera (when R is empty).
3503 @param src Observed point coordinates, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel (CV_32FC2 or CV_64FC2) (or
3504 vector\<Point2f\> ).
3505 @param dst Output ideal point coordinates (1xN/Nx1 2-channel or vector\<Point2f\> ) after undistortion and reverse perspective
3506 transformation. If matrix P is identity or omitted, dst will contain normalized point coordinates.
3507 @param cameraMatrix Camera matrix \f$\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
3508 @param distCoeffs Input vector of distortion coefficients
3509 \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
3510 of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
3511 @param R Rectification transformation in the object space (3x3 matrix). R1 or R2 computed by
3512 #stereoRectify can be passed here. If the matrix is empty, the identity transformation is used.
3513 @param P New camera matrix (3x3) or new projection matrix (3x4) \f$\begin{bmatrix} {f'}_x & 0 & {c'}_x & t_x \\ 0 & {f'}_y & {c'}_y & t_y \\ 0 & 0 & 1 & t_z \end{bmatrix}\f$. P1 or P2 computed by
3514 #stereoRectify can be passed here. If the matrix is empty, the identity new camera matrix is used.
3517 void undistortPoints(InputArray src, OutputArray dst,
3518 InputArray cameraMatrix, InputArray distCoeffs,
3519 InputArray R = noArray(), InputArray P = noArray());
3521 @note Default version of #undistortPoints does 5 iterations to compute undistorted points.
3523 CV_EXPORTS_AS(undistortPointsIter)
3524 void undistortPoints(InputArray src, OutputArray dst,
3525 InputArray cameraMatrix, InputArray distCoeffs,
3526 InputArray R, InputArray P, TermCriteria criteria);
3530 /** @brief The methods in this namespace use a so-called fisheye camera model.
3531 @ingroup calib3d_fisheye
3535 //! @addtogroup calib3d_fisheye
3539 CALIB_USE_INTRINSIC_GUESS = 1 << 0,
3540 CALIB_RECOMPUTE_EXTRINSIC = 1 << 1,
3541 CALIB_CHECK_COND = 1 << 2,
3542 CALIB_FIX_SKEW = 1 << 3,
3543 CALIB_FIX_K1 = 1 << 4,
3544 CALIB_FIX_K2 = 1 << 5,
3545 CALIB_FIX_K3 = 1 << 6,
3546 CALIB_FIX_K4 = 1 << 7,
3547 CALIB_FIX_INTRINSIC = 1 << 8,
3548 CALIB_FIX_PRINCIPAL_POINT = 1 << 9
3551 /** @brief Projects points using fisheye model
3553 @param objectPoints Array of object points, 1xN/Nx1 3-channel (or vector\<Point3f\> ), where N is
3554 the number of points in the view.
3555 @param imagePoints Output array of image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, or
3558 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
3559 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$.
3560 @param alpha The skew coefficient.
3561 @param jacobian Optional output 2Nx15 jacobian matrix of derivatives of image points with respect
3562 to components of the focal lengths, coordinates of the principal point, distortion coefficients,
3563 rotation vector, translation vector, and the skew. In the old interface different components of
3564 the jacobian are returned via different output parameters.
3566 The function computes projections of 3D points to the image plane given intrinsic and extrinsic
3567 camera parameters. Optionally, the function computes Jacobians - matrices of partial derivatives of
3568 image points coordinates (as functions of all the input parameters) with respect to the particular
3569 parameters, intrinsic and/or extrinsic.
3571 CV_EXPORTS void projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine,
3572 InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray());
3575 CV_EXPORTS_W void projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec,
3576 InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray());
3578 /** @brief Distorts 2D points using fisheye model.
3580 @param undistorted Array of object points, 1xN/Nx1 2-channel (or vector\<Point2f\> ), where N is
3581 the number of points in the view.
3582 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
3583 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$.
3584 @param alpha The skew coefficient.
3585 @param distorted Output array of image points, 1xN/Nx1 2-channel, or vector\<Point2f\> .
3587 Note that the function assumes the camera matrix of the undistorted points to be identity.
3588 This means if you want to transform back points undistorted with undistortPoints() you have to
3589 multiply them with \f$P^{-1}\f$.
3591 CV_EXPORTS_W void distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0);
3593 /** @brief Undistorts 2D points using fisheye model
3595 @param distorted Array of object points, 1xN/Nx1 2-channel (or vector\<Point2f\> ), where N is the
3596 number of points in the view.
3597 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
3598 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$.
3599 @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3
3600 1-channel or 1x1 3-channel
3601 @param P New camera matrix (3x3) or new projection matrix (3x4)
3602 @param undistorted Output array of image points, 1xN/Nx1 2-channel, or vector\<Point2f\> .
3604 CV_EXPORTS_W void undistortPoints(InputArray distorted, OutputArray undistorted,
3605 InputArray K, InputArray D, InputArray R = noArray(), InputArray P = noArray());
3607 /** @brief Computes undistortion and rectification maps for image transform by cv::remap(). If D is empty zero
3608 distortion is used, if R or P is empty identity matrixes are used.
3610 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
3611 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$.
3612 @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3
3613 1-channel or 1x1 3-channel
3614 @param P New camera matrix (3x3) or new projection matrix (3x4)
3615 @param size Undistorted image size.
3616 @param m1type Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps()
3618 @param map1 The first output map.
3619 @param map2 The second output map.
3621 CV_EXPORTS_W void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P,
3622 const cv::Size& size, int m1type, OutputArray map1, OutputArray map2);
3624 /** @brief Transforms an image to compensate for fisheye lens distortion.
3626 @param distorted image with fisheye lens distortion.
3627 @param undistorted Output image with compensated fisheye lens distortion.
3628 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
3629 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$.
3630 @param Knew Camera matrix of the distorted image. By default, it is the identity matrix but you
3631 may additionally scale and shift the result by using a different matrix.
3632 @param new_size the new size
3634 The function transforms an image to compensate radial and tangential lens distortion.
3636 The function is simply a combination of fisheye::initUndistortRectifyMap (with unity R ) and remap
3637 (with bilinear interpolation). See the former function for details of the transformation being
3640 See below the results of undistortImage.
3641 - a\) result of undistort of perspective camera model (all possible coefficients (k_1, k_2, k_3,
3642 k_4, k_5, k_6) of distortion were optimized under calibration)
3643 - b\) result of fisheye::undistortImage of fisheye camera model (all possible coefficients (k_1, k_2,
3644 k_3, k_4) of fisheye distortion were optimized under calibration)
3645 - c\) original image was captured with fisheye lens
3647 Pictures a) and b) almost the same. But if we consider points of image located far from the center
3648 of image, we can notice that on image a) these points are distorted.
3650 ![image](pics/fisheye_undistorted.jpg)
3652 CV_EXPORTS_W void undistortImage(InputArray distorted, OutputArray undistorted,
3653 InputArray K, InputArray D, InputArray Knew = cv::noArray(), const Size& new_size = Size());
3655 /** @brief Estimates new camera matrix for undistortion or rectification.
3657 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$.
3658 @param image_size Size of the image
3659 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$.
3660 @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3
3661 1-channel or 1x1 3-channel
3662 @param P New camera matrix (3x3) or new projection matrix (3x4)
3663 @param balance Sets the new focal length in range between the min focal length and the max focal
3664 length. Balance is in range of [0, 1].
3665 @param new_size the new size
3666 @param fov_scale Divisor for new focal length.
3668 CV_EXPORTS_W void estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R,
3669 OutputArray P, double balance = 0.0, const Size& new_size = Size(), double fov_scale = 1.0);
3671 /** @brief Performs camera calibaration
3673 @param objectPoints vector of vectors of calibration pattern points in the calibration pattern
3675 @param imagePoints vector of vectors of the projections of calibration pattern points.
3676 imagePoints.size() and objectPoints.size() and imagePoints[i].size() must be equal to
3677 objectPoints[i].size() for each i.
3678 @param image_size Size of the image used only to initialize the intrinsic camera matrix.
3679 @param K Output 3x3 floating-point camera matrix
3680 \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . If
3681 fisheye::CALIB_USE_INTRINSIC_GUESS/ is specified, some or all of fx, fy, cx, cy must be
3682 initialized before calling the function.
3683 @param D Output vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$.
3684 @param rvecs Output vector of rotation vectors (see Rodrigues ) estimated for each pattern view.
3685 That is, each k-th rotation vector together with the corresponding k-th translation vector (see
3686 the next output parameter description) brings the calibration pattern from the model coordinate
3687 space (in which object points are specified) to the world coordinate space, that is, a real
3688 position of the calibration pattern in the k-th pattern view (k=0.. *M* -1).
3689 @param tvecs Output vector of translation vectors estimated for each pattern view.
3690 @param flags Different flags that may be zero or a combination of the following values:
3691 - **fisheye::CALIB_USE_INTRINSIC_GUESS** cameraMatrix contains valid initial values of
3692 fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image
3693 center ( imageSize is used), and focal distances are computed in a least-squares fashion.
3694 - **fisheye::CALIB_RECOMPUTE_EXTRINSIC** Extrinsic will be recomputed after each iteration
3695 of intrinsic optimization.
3696 - **fisheye::CALIB_CHECK_COND** The functions will check validity of condition number.
3697 - **fisheye::CALIB_FIX_SKEW** Skew coefficient (alpha) is set to zero and stay zero.
3698 - **fisheye::CALIB_FIX_K1..fisheye::CALIB_FIX_K4** Selected distortion coefficients
3699 are set to zeros and stay zero.
3700 - **fisheye::CALIB_FIX_PRINCIPAL_POINT** The principal point is not changed during the global
3701 optimization. It stays at the center or at a different location specified when CALIB_USE_INTRINSIC_GUESS is set too.
3702 @param criteria Termination criteria for the iterative optimization algorithm.
3704 CV_EXPORTS_W double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size,
3705 InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0,
3706 TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON));
3708 /** @brief Stereo rectification for fisheye camera model
3710 @param K1 First camera matrix.
3711 @param D1 First camera distortion parameters.
3712 @param K2 Second camera matrix.
3713 @param D2 Second camera distortion parameters.
3714 @param imageSize Size of the image used for stereo calibration.
3715 @param R Rotation matrix between the coordinate systems of the first and the second
3717 @param tvec Translation vector between coordinate systems of the cameras.
3718 @param R1 Output 3x3 rectification transform (rotation matrix) for the first camera.
3719 @param R2 Output 3x3 rectification transform (rotation matrix) for the second camera.
3720 @param P1 Output 3x4 projection matrix in the new (rectified) coordinate systems for the first
3722 @param P2 Output 3x4 projection matrix in the new (rectified) coordinate systems for the second
3724 @param Q Output \f$4 \times 4\f$ disparity-to-depth mapping matrix (see reprojectImageTo3D ).
3725 @param flags Operation flags that may be zero or CALIB_ZERO_DISPARITY . If the flag is set,
3726 the function makes the principal points of each camera have the same pixel coordinates in the
3727 rectified views. And if the flag is not set, the function may still shift the images in the
3728 horizontal or vertical direction (depending on the orientation of epipolar lines) to maximize the
3730 @param newImageSize New image resolution after rectification. The same size should be passed to
3731 initUndistortRectifyMap (see the stereo_calib.cpp sample in OpenCV samples directory). When (0,0)
3732 is passed (default), it is set to the original imageSize . Setting it to larger value can help you
3733 preserve details in the original image, especially when there is a big radial distortion.
3734 @param balance Sets the new focal length in range between the min focal length and the max focal
3735 length. Balance is in range of [0, 1].
3736 @param fov_scale Divisor for new focal length.
3738 CV_EXPORTS_W void stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec,
3739 OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(),
3740 double balance = 0.0, double fov_scale = 1.0);
3742 /** @brief Performs stereo calibration
3744 @param objectPoints Vector of vectors of the calibration pattern points.
3745 @param imagePoints1 Vector of vectors of the projections of the calibration pattern points,
3746 observed by the first camera.
3747 @param imagePoints2 Vector of vectors of the projections of the calibration pattern points,
3748 observed by the second camera.
3749 @param K1 Input/output first camera matrix:
3750 \f$\vecthreethree{f_x^{(j)}}{0}{c_x^{(j)}}{0}{f_y^{(j)}}{c_y^{(j)}}{0}{0}{1}\f$ , \f$j = 0,\, 1\f$ . If
3751 any of fisheye::CALIB_USE_INTRINSIC_GUESS , fisheye::CALIB_FIX_INTRINSIC are specified,
3752 some or all of the matrix components must be initialized.
3753 @param D1 Input/output vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$ of 4 elements.
3754 @param K2 Input/output second camera matrix. The parameter is similar to K1 .
3755 @param D2 Input/output lens distortion coefficients for the second camera. The parameter is
3757 @param imageSize Size of the image used only to initialize intrinsic camera matrix.
3758 @param R Output rotation matrix between the 1st and the 2nd camera coordinate systems.
3759 @param T Output translation vector between the coordinate systems of the cameras.
3760 @param flags Different flags that may be zero or a combination of the following values:
3761 - **fisheye::CALIB_FIX_INTRINSIC** Fix K1, K2? and D1, D2? so that only R, T matrices
3763 - **fisheye::CALIB_USE_INTRINSIC_GUESS** K1, K2 contains valid initial values of
3764 fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image
3765 center (imageSize is used), and focal distances are computed in a least-squares fashion.
3766 - **fisheye::CALIB_RECOMPUTE_EXTRINSIC** Extrinsic will be recomputed after each iteration
3767 of intrinsic optimization.
3768 - **fisheye::CALIB_CHECK_COND** The functions will check validity of condition number.
3769 - **fisheye::CALIB_FIX_SKEW** Skew coefficient (alpha) is set to zero and stay zero.
3770 - **fisheye::CALIB_FIX_K1..4** Selected distortion coefficients are set to zeros and stay
3772 @param criteria Termination criteria for the iterative optimization algorithm.
3774 CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
3775 InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
3776 OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC,
3777 TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON));
3779 //! @} calib3d_fisheye
3780 } // end namespace fisheye
3782 } //end namespace cv
3784 #if 0 //def __cplusplus
3785 //////////////////////////////////////////////////////////////////////////////////////////
3786 class CV_EXPORTS CvLevMarq
3790 CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria=
3791 cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
3792 bool completeSymmFlag=false );
3794 void init( int nparams, int nerrs, CvTermCriteria criteria=
3795 cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
3796 bool completeSymmFlag=false );
3797 bool update( const CvMat*& param, CvMat*& J, CvMat*& err );
3798 bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm );
3802 enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 };
3804 cv::Ptr<CvMat> mask;
3805 cv::Ptr<CvMat> prevParam;
3806 cv::Ptr<CvMat> param;
3810 cv::Ptr<CvMat> JtJN;
3811 cv::Ptr<CvMat> JtErr;
3812 cv::Ptr<CvMat> JtJV;
3813 cv::Ptr<CvMat> JtJW;
3814 double prevErrNorm, errNorm;
3816 CvTermCriteria criteria;
3819 bool completeSymmFlag;