1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 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. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 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. 17 // 18 // Redistribution and use in source and binary forms, with or without modification, 19 // are permitted provided that the following conditions are met: 20 // 21 // * Redistribution's of source code must retain the above copyright notice, 22 // this list of conditions and the following disclaimer. 23 // 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. 27 // 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. 30 // 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. 41 // 42 //M*/ 43 44 #ifndef __OPENCV_CALIB3D_HPP__ 45 #define __OPENCV_CALIB3D_HPP__ 46 47 #include "opencv2/core.hpp" 48 #include "opencv2/features2d.hpp" 49 #include "opencv2/core/affine.hpp" 50 51 /** 52 @defgroup calib3d Camera Calibration and 3D Reconstruction 53 54 The functions in this section use a so-called pinhole camera model. In this model, a scene view is 55 formed by projecting 3D points into the image plane using a perspective transformation. 56 57 \f[s \; m' = A [R|t] M'\f] 58 59 or 60 61 \f[s \vecthree{u}{v}{1} = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1} 62 \begin{bmatrix} 63 r_{11} & r_{12} & r_{13} & t_1 \\ 64 r_{21} & r_{22} & r_{23} & t_2 \\ 65 r_{31} & r_{32} & r_{33} & t_3 66 \end{bmatrix} 67 \begin{bmatrix} 68 X \\ 69 Y \\ 70 Z \\ 71 1 72 \end{bmatrix}\f] 73 74 where: 75 76 - \f$(X, Y, Z)\f$ are the coordinates of a 3D point in the world coordinate space 77 - \f$(u, v)\f$ are the coordinates of the projection point in pixels 78 - \f$A\f$ is a camera matrix, or a matrix of intrinsic parameters 79 - \f$(cx, cy)\f$ is a principal point that is usually at the image center 80 - \f$fx, fy\f$ are the focal lengths expressed in pixel units. 81 82 Thus, if an image from the camera is scaled by a factor, all of these parameters should be scaled 83 (multiplied/divided, respectively) by the same factor. The matrix of intrinsic parameters does not 84 depend on the scene viewed. So, once estimated, it can be re-used as long as the focal length is 85 fixed (in case of zoom lens). The joint rotation-translation matrix \f$[R|t]\f$ is called a matrix of 86 extrinsic parameters. It is used to describe the camera motion around a static scene, or vice versa, 87 rigid motion of an object in front of a still camera. That is, \f$[R|t]\f$ translates coordinates of a 88 point \f$(X, Y, Z)\f$ to a coordinate system, fixed with respect to the camera. The transformation above 89 is equivalent to the following (when \f$z \ne 0\f$ ): 90 91 \f[\begin{array}{l} 92 \vecthree{x}{y}{z} = R \vecthree{X}{Y}{Z} + t \\ 93 x' = x/z \\ 94 y' = y/z \\ 95 u = f_x*x' + c_x \\ 96 v = f_y*y' + c_y 97 \end{array}\f] 98 99 Real lenses usually have some distortion, mostly radial distortion and slight tangential distortion. 100 So, the above model is extended as: 101 102 \f[\begin{array}{l} \vecthree{x}{y}{z} = R \vecthree{X}{Y}{Z} + t \\ x' = x/z \\ y' = y/z \\ x'' = 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 \\ y'' = 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_1 r^2 + s_2 r^4 \\ \text{where} \quad r^2 = x'^2 + y'^2 \\ u = f_x*x'' + c_x \\ v = f_y*y'' + c_y \end{array}\f] 103 104 \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$ are radial distortion coefficients. \f$p_1\f$ and \f$p_2\f$ are 105 tangential distortion coefficients. \f$s_1\f$, \f$s_2\f$, \f$s_3\f$, and \f$s_4\f$, are the thin prism distortion 106 coefficients. Higher-order coefficients are not considered in OpenCV. In the functions below the 107 coefficients are passed or returned as 108 109 \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]])\f] 110 111 vector. That is, if the vector contains four elements, it means that \f$k_3=0\f$ . The distortion 112 coefficients do not depend on the scene viewed. Thus, they also belong to the intrinsic camera 113 parameters. And they remain the same regardless of the captured image resolution. If, for example, a 114 camera has been calibrated on images of 320 x 240 resolution, absolutely the same distortion 115 coefficients can be used for 640 x 480 images from the same camera while \f$f_x\f$, \f$f_y\f$, \f$c_x\f$, and 116 \f$c_y\f$ need to be scaled appropriately. 117 118 The functions below use the above model to do the following: 119 120 - Project 3D points to the image plane given intrinsic and extrinsic parameters. 121 - Compute extrinsic parameters given intrinsic parameters, a few 3D points, and their 122 projections. 123 - Estimate intrinsic and extrinsic camera parameters from several views of a known calibration 124 pattern (every view is described by several 3D-2D point correspondences). 125 - Estimate the relative position and orientation of the stereo camera "heads" and compute the 126 *rectification* transformation that makes the camera optical axes parallel. 127 128 @note 129 - A calibration sample for 3 cameras in horizontal position can be found at 130 opencv_source_code/samples/cpp/3calibration.cpp 131 - A calibration sample based on a sequence of images can be found at 132 opencv_source_code/samples/cpp/calibration.cpp 133 - A calibration sample in order to do 3D reconstruction can be found at 134 opencv_source_code/samples/cpp/build3dmodel.cpp 135 - A calibration sample of an artificially generated camera and chessboard patterns can be 136 found at opencv_source_code/samples/cpp/calibration_artificial.cpp 137 - A calibration example on stereo calibration can be found at 138 opencv_source_code/samples/cpp/stereo_calib.cpp 139 - A calibration example on stereo matching can be found at 140 opencv_source_code/samples/cpp/stereo_match.cpp 141 - (Python) A camera calibration sample can be found at 142 opencv_source_code/samples/python2/calibrate.py 143 144 @{ 145 @defgroup calib3d_fisheye Fisheye camera model 146 147 Definitions: Let P be a point in 3D of coordinates X in the world reference frame (stored in the 148 matrix X) The coordinate vector of P in the camera reference frame is: 149 150 \f[Xc = R X + T\f] 151 152 where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); call x, y 153 and z the 3 coordinates of Xc: 154 155 \f[x = Xc_1 \\ y = Xc_2 \\ z = Xc_3\f] 156 157 The pinehole projection coordinates of P is [a; b] where 158 159 \f[a = x / z \ and \ b = y / z \\ r^2 = a^2 + b^2 \\ \theta = atan(r)\f] 160 161 Fisheye distortion: 162 163 \f[\theta_d = \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8)\f] 164 165 The distorted point coordinates are [x'; y'] where 166 167 \f[x' = (\theta_d / r) x \\ y' = (\theta_d / r) y \f] 168 169 Finally, conversion into pixel coordinates: The final pixel coordinates vector [u; v] where: 170 171 \f[u = f_x (x' + \alpha y') + c_x \\ 172 v = f_y yy + c_y\f] 173 174 @defgroup calib3d_c C API 175 176 @} 177 */ 178 179 namespace cv 180 { 181 182 //! @addtogroup calib3d 183 //! @{ 184 185 //! type of the robust estimation algorithm 186 enum { LMEDS = 4, //!< least-median algorithm 187 RANSAC = 8, //!< RANSAC algorithm 188 RHO = 16 //!< RHO algorithm 189 }; 190 191 enum { SOLVEPNP_ITERATIVE = 0, 192 SOLVEPNP_EPNP = 1, // F.Moreno-Noguer, V.Lepetit and P.Fua "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" 193 SOLVEPNP_P3P = 2, // X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang; "Complete Solution Classification for the Perspective-Three-Point Problem" 194 SOLVEPNP_DLS = 3, // Joel A. Hesch and Stergios I. Roumeliotis. "A Direct Least-Squares (DLS) Method for PnP" 195 SOLVEPNP_UPNP = 4 // A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation" 196 197 }; 198 199 enum { CALIB_CB_ADAPTIVE_THRESH = 1, 200 CALIB_CB_NORMALIZE_IMAGE = 2, 201 CALIB_CB_FILTER_QUADS = 4, 202 CALIB_CB_FAST_CHECK = 8 203 }; 204 205 enum { CALIB_CB_SYMMETRIC_GRID = 1, 206 CALIB_CB_ASYMMETRIC_GRID = 2, 207 CALIB_CB_CLUSTERING = 4 208 }; 209 210 enum { CALIB_USE_INTRINSIC_GUESS = 0x00001, 211 CALIB_FIX_ASPECT_RATIO = 0x00002, 212 CALIB_FIX_PRINCIPAL_POINT = 0x00004, 213 CALIB_ZERO_TANGENT_DIST = 0x00008, 214 CALIB_FIX_FOCAL_LENGTH = 0x00010, 215 CALIB_FIX_K1 = 0x00020, 216 CALIB_FIX_K2 = 0x00040, 217 CALIB_FIX_K3 = 0x00080, 218 CALIB_FIX_K4 = 0x00800, 219 CALIB_FIX_K5 = 0x01000, 220 CALIB_FIX_K6 = 0x02000, 221 CALIB_RATIONAL_MODEL = 0x04000, 222 CALIB_THIN_PRISM_MODEL = 0x08000, 223 CALIB_FIX_S1_S2_S3_S4 = 0x10000, 224 // only for stereo 225 CALIB_FIX_INTRINSIC = 0x00100, 226 CALIB_SAME_FOCAL_LENGTH = 0x00200, 227 // for stereo rectification 228 CALIB_ZERO_DISPARITY = 0x00400 229 }; 230 231 //! the algorithm for finding fundamental matrix 232 enum { FM_7POINT = 1, //!< 7-point algorithm 233 FM_8POINT = 2, //!< 8-point algorithm 234 FM_LMEDS = 4, //!< least-median algorithm 235 FM_RANSAC = 8 //!< RANSAC algorithm 236 }; 237 238 239 240 /** @brief Converts a rotation matrix to a rotation vector or vice versa. 241 242 @param src Input rotation vector (3x1 or 1x3) or rotation matrix (3x3). 243 @param dst Output rotation matrix (3x3) or rotation vector (3x1 or 1x3), respectively. 244 @param jacobian Optional output Jacobian matrix, 3x9 or 9x3, which is a matrix of partial 245 derivatives of the output array components with respect to the input array components. 246 247 \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] 248 249 Inverse transformation can be also done easily, since 250 251 \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] 252 253 A rotation vector is a convenient and most compact representation of a rotation matrix (since any 254 rotation matrix has just 3 degrees of freedom). The representation is used in the global 3D geometry 255 optimization procedures like calibrateCamera, stereoCalibrate, or solvePnP . 256 */ 257 CV_EXPORTS_W void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() ); 258 259 /** @brief Finds a perspective transformation between two planes. 260 261 @param srcPoints Coordinates of the points in the original plane, a matrix of the type CV_32FC2 262 or vector\<Point2f\> . 263 @param dstPoints Coordinates of the points in the target plane, a matrix of the type CV_32FC2 or 264 a vector\<Point2f\> . 265 @param method Method used to computed a homography matrix. The following methods are possible: 266 - **0** - a regular method using all the points 267 - **RANSAC** - RANSAC-based robust method 268 - **LMEDS** - Least-Median robust method 269 - **RHO** - PROSAC-based robust method 270 @param ransacReprojThreshold Maximum allowed reprojection error to treat a point pair as an inlier 271 (used in the RANSAC and RHO methods only). That is, if 272 \f[\| \texttt{dstPoints} _i - \texttt{convertPointsHomogeneous} ( \texttt{H} * \texttt{srcPoints} _i) \| > \texttt{ransacReprojThreshold}\f] 273 then the point \f$i\f$ is considered an outlier. If srcPoints and dstPoints are measured in pixels, 274 it usually makes sense to set this parameter somewhere in the range of 1 to 10. 275 @param mask Optional output mask set by a robust method ( RANSAC or LMEDS ). Note that the input 276 mask values are ignored. 277 @param maxIters The maximum number of RANSAC iterations, 2000 is the maximum it can be. 278 @param confidence Confidence level, between 0 and 1. 279 280 The functions find and return the perspective transformation \f$H\f$ between the source and the 281 destination planes: 282 283 \f[s_i \vecthree{x'_i}{y'_i}{1} \sim H \vecthree{x_i}{y_i}{1}\f] 284 285 so that the back-projection error 286 287 \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] 288 289 is minimized. If the parameter method is set to the default value 0, the function uses all the point 290 pairs to compute an initial homography estimate with a simple least-squares scheme. 291 292 However, if not all of the point pairs ( \f$srcPoints_i\f$, \f$dstPoints_i\f$ ) fit the rigid perspective 293 transformation (that is, there are some outliers), this initial estimate will be poor. In this case, 294 you can use one of the three robust methods. The methods RANSAC, LMeDS and RHO try many different 295 random subsets of the corresponding point pairs (of four pairs each), estimate the homography matrix 296 using this subset and a simple least-square algorithm, and then compute the quality/goodness of the 297 computed homography (which is the number of inliers for RANSAC or the median re-projection error for 298 LMeDs). The best subset is then used to produce the initial estimate of the homography matrix and 299 the mask of inliers/outliers. 300 301 Regardless of the method, robust or not, the computed homography matrix is refined further (using 302 inliers only in case of a robust method) with the Levenberg-Marquardt method to reduce the 303 re-projection error even more. 304 305 The methods RANSAC and RHO can handle practically any ratio of outliers but need a threshold to 306 distinguish inliers from outliers. The method LMeDS does not need any threshold but it works 307 correctly only when there are more than 50% of inliers. Finally, if there are no outliers and the 308 noise is rather small, use the default method (method=0). 309 310 The function is used to find initial intrinsic and extrinsic matrices. Homography matrix is 311 determined up to a scale. Thus, it is normalized so that \f$h_{33}=1\f$. Note that whenever an H matrix 312 cannot be estimated, an empty one will be returned. 313 314 @sa 315 getAffineTransform, getPerspectiveTransform, estimateRigidTransform, warpPerspective, 316 perspectiveTransform 317 318 @note 319 - A example on calculating a homography for image matching can be found at 320 opencv_source_code/samples/cpp/video_homography.cpp 321 322 */ 323 CV_EXPORTS_W Mat findHomography( InputArray srcPoints, InputArray dstPoints, 324 int method = 0, double ransacReprojThreshold = 3, 325 OutputArray mask=noArray(), const int maxIters = 2000, 326 const double confidence = 0.995); 327 328 /** @overload */ 329 CV_EXPORTS Mat findHomography( InputArray srcPoints, InputArray dstPoints, 330 OutputArray mask, int method = 0, double ransacReprojThreshold = 3 ); 331 332 /** @brief Computes an RQ decomposition of 3x3 matrices. 333 334 @param src 3x3 input matrix. 335 @param mtxR Output 3x3 upper-triangular matrix. 336 @param mtxQ Output 3x3 orthogonal matrix. 337 @param Qx Optional output 3x3 rotation matrix around x-axis. 338 @param Qy Optional output 3x3 rotation matrix around y-axis. 339 @param Qz Optional output 3x3 rotation matrix around z-axis. 340 341 The function computes a RQ decomposition using the given rotations. This function is used in 342 decomposeProjectionMatrix to decompose the left 3x3 submatrix of a projection matrix into a camera 343 and a rotation matrix. 344 345 It optionally returns three rotation matrices, one for each axis, and the three Euler angles in 346 degrees (as the return value) that could be used in OpenGL. Note, there is always more than one 347 sequence of rotations about the three principle axes that results in the same orientation of an 348 object, eg. see @cite Slabaugh . Returned tree rotation matrices and corresponding three Euler angules 349 are only one of the possible solutions. 350 */ 351 CV_EXPORTS_W Vec3d RQDecomp3x3( InputArray src, OutputArray mtxR, OutputArray mtxQ, 352 OutputArray Qx = noArray(), 353 OutputArray Qy = noArray(), 354 OutputArray Qz = noArray()); 355 356 /** @brief Decomposes a projection matrix into a rotation matrix and a camera matrix. 357 358 @param projMatrix 3x4 input projection matrix P. 359 @param cameraMatrix Output 3x3 camera matrix K. 360 @param rotMatrix Output 3x3 external rotation matrix R. 361 @param transVect Output 4x1 translation vector T. 362 @param rotMatrixX Optional 3x3 rotation matrix around x-axis. 363 @param rotMatrixY Optional 3x3 rotation matrix around y-axis. 364 @param rotMatrixZ Optional 3x3 rotation matrix around z-axis. 365 @param eulerAngles Optional three-element vector containing three Euler angles of rotation in 366 degrees. 367 368 The function computes a decomposition of a projection matrix into a calibration and a rotation 369 matrix and the position of a camera. 370 371 It optionally returns three rotation matrices, one for each axis, and three Euler angles that could 372 be used in OpenGL. Note, there is always more than one sequence of rotations about the three 373 principle axes that results in the same orientation of an object, eg. see @cite Slabaugh . Returned 374 tree rotation matrices and corresponding three Euler angules are only one of the possible solutions. 375 376 The function is based on RQDecomp3x3 . 377 */ 378 CV_EXPORTS_W void decomposeProjectionMatrix( InputArray projMatrix, OutputArray cameraMatrix, 379 OutputArray rotMatrix, OutputArray transVect, 380 OutputArray rotMatrixX = noArray(), 381 OutputArray rotMatrixY = noArray(), 382 OutputArray rotMatrixZ = noArray(), 383 OutputArray eulerAngles =noArray() ); 384 385 /** @brief Computes partial derivatives of the matrix product for each multiplied matrix. 386 387 @param A First multiplied matrix. 388 @param B Second multiplied matrix. 389 @param dABdA First output derivative matrix d(A\*B)/dA of size 390 \f$\texttt{A.rows*B.cols} \times {A.rows*A.cols}\f$ . 391 @param dABdB Second output derivative matrix d(A\*B)/dB of size 392 \f$\texttt{A.rows*B.cols} \times {B.rows*B.cols}\f$ . 393 394 The function computes partial derivatives of the elements of the matrix product \f$A*B\f$ with regard to 395 the elements of each of the two input matrices. The function is used to compute the Jacobian 396 matrices in stereoCalibrate but can also be used in any other similar optimization function. 397 */ 398 CV_EXPORTS_W void matMulDeriv( InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB ); 399 400 /** @brief Combines two rotation-and-shift transformations. 401 402 @param rvec1 First rotation vector. 403 @param tvec1 First translation vector. 404 @param rvec2 Second rotation vector. 405 @param tvec2 Second translation vector. 406 @param rvec3 Output rotation vector of the superposition. 407 @param tvec3 Output translation vector of the superposition. 408 @param dr3dr1 409 @param dr3dt1 410 @param dr3dr2 411 @param dr3dt2 412 @param dt3dr1 413 @param dt3dt1 414 @param dt3dr2 415 @param dt3dt2 Optional output derivatives of rvec3 or tvec3 with regard to rvec1, rvec2, tvec1 and 416 tvec2, respectively. 417 418 The functions compute: 419 420 \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] 421 422 where \f$\mathrm{rodrigues}\f$ denotes a rotation vector to a rotation matrix transformation, and 423 \f$\mathrm{rodrigues}^{-1}\f$ denotes the inverse transformation. See Rodrigues for details. 424 425 Also, the functions can compute the derivatives of the output vectors with regards to the input 426 vectors (see matMulDeriv ). The functions are used inside stereoCalibrate but can also be used in 427 your own code where Levenberg-Marquardt or another gradient-based solver is used to optimize a 428 function that contains a matrix multiplication. 429 */ 430 CV_EXPORTS_W void composeRT( InputArray rvec1, InputArray tvec1, 431 InputArray rvec2, InputArray tvec2, 432 OutputArray rvec3, OutputArray tvec3, 433 OutputArray dr3dr1 = noArray(), OutputArray dr3dt1 = noArray(), 434 OutputArray dr3dr2 = noArray(), OutputArray dr3dt2 = noArray(), 435 OutputArray dt3dr1 = noArray(), OutputArray dt3dt1 = noArray(), 436 OutputArray dt3dr2 = noArray(), OutputArray dt3dt2 = noArray() ); 437 438 /** @brief Projects 3D points to an image plane. 439 440 @param objectPoints Array of object points, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel (or 441 vector\<Point3f\> ), where N is the number of points in the view. 442 @param rvec Rotation vector. See Rodrigues for details. 443 @param tvec Translation vector. 444 @param cameraMatrix Camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$ . 445 @param distCoeffs Input vector of distortion coefficients 446 \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]])\f$ of 4, 5, 8 or 12 elements. If 447 the vector is NULL/empty, the zero distortion coefficients are assumed. 448 @param imagePoints Output array of image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, or 449 vector\<Point2f\> . 450 @param jacobian Optional output 2Nx(10+\<numDistCoeffs\>) jacobian matrix of derivatives of image 451 points with respect to components of the rotation vector, translation vector, focal lengths, 452 coordinates of the principal point and the distortion coefficients. In the old interface different 453 components of the jacobian are returned via different output parameters. 454 @param aspectRatio Optional "fixed aspect ratio" parameter. If the parameter is not 0, the 455 function assumes that the aspect ratio (*fx/fy*) is fixed and correspondingly adjusts the jacobian 456 matrix. 457 458 The function computes projections of 3D points to the image plane given intrinsic and extrinsic 459 camera parameters. Optionally, the function computes Jacobians - matrices of partial derivatives of 460 image points coordinates (as functions of all the input parameters) with respect to the particular 461 parameters, intrinsic and/or extrinsic. The Jacobians are used during the global optimization in 462 calibrateCamera, solvePnP, and stereoCalibrate . The function itself can also be used to compute a 463 re-projection error given the current intrinsic and extrinsic parameters. 464 465 @note By setting rvec=tvec=(0,0,0) or by setting cameraMatrix to a 3x3 identity matrix, or by 466 passing zero distortion coefficients, you can get various useful partial cases of the function. This 467 means that you can compute the distorted coordinates for a sparse set of points or apply a 468 perspective transformation (and also compute the derivatives) in the ideal zero-distortion setup. 469 */ 470 CV_EXPORTS_W void projectPoints( InputArray objectPoints, 471 InputArray rvec, InputArray tvec, 472 InputArray cameraMatrix, InputArray distCoeffs, 473 OutputArray imagePoints, 474 OutputArray jacobian = noArray(), 475 double aspectRatio = 0 ); 476 477 /** @brief Finds an object pose from 3D-2D point correspondences. 478 479 @param objectPoints Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 480 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3f\> can be also passed here. 481 @param imagePoints Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, 482 where N is the number of points. vector\<Point2f\> can be also passed here. 483 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}\f$ . 484 @param distCoeffs Input vector of distortion coefficients 485 \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]])\f$ of 4, 5, 8 or 12 elements. If 486 the vector is NULL/empty, the zero distortion coefficients are assumed. 487 @param rvec Output rotation vector (see Rodrigues ) that, together with tvec , brings points from 488 the model coordinate system to the camera coordinate system. 489 @param tvec Output translation vector. 490 @param useExtrinsicGuess Parameter used for SOLVEPNP_ITERATIVE. If true (1), the function uses 491 the provided rvec and tvec values as initial approximations of the rotation and translation 492 vectors, respectively, and further optimizes them. 493 @param flags Method for solving a PnP problem: 494 - **SOLVEPNP_ITERATIVE** Iterative method is based on Levenberg-Marquardt optimization. In 495 this case the function finds such a pose that minimizes reprojection error, that is the sum 496 of squared distances between the observed projections imagePoints and the projected (using 497 projectPoints ) objectPoints . 498 - **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang 499 "Complete Solution Classification for the Perspective-Three-Point Problem". In this case the 500 function requires exactly four object and image points. 501 - **SOLVEPNP_EPNP** Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the 502 paper "EPnP: Efficient Perspective-n-Point Camera Pose Estimation". 503 - **SOLVEPNP_DLS** Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. 504 "A Direct Least-Squares (DLS) Method for PnP". 505 - **SOLVEPNP_UPNP** Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, 506 F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length 507 Estimation". In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$ 508 assuming that both have the same value. Then the cameraMatrix is updated with the estimated 509 focal length. 510 511 The function estimates the object pose given a set of object points, their corresponding image 512 projections, as well as the camera matrix and the distortion coefficients. 513 514 @note 515 - An example of how to use solvePnP for planar augmented reality can be found at 516 opencv_source_code/samples/python2/plane_ar.py 517 - If you are using Python: 518 - Numpy array slices won't work as input because solvePnP requires contiguous 519 arrays (enforced by the assertion using cv::Mat::checkVector() around line 55 of 520 modules/calib3d/src/solvepnp.cpp version 2.4.9) 521 - The P3P algorithm requires image points to be in an array of shape (N,1,2) due 522 to its calling of cv::undistortPoints (around line 75 of modules/calib3d/src/solvepnp.cpp version 2.4.9) 523 which requires 2-channel information. 524 - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of 525 it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints = 526 np.ascontiguousarray(D[:,:2]).reshape((N,1,2)) 527 */ 528 CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, 529 InputArray cameraMatrix, InputArray distCoeffs, 530 OutputArray rvec, OutputArray tvec, 531 bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE ); 532 533 /** @brief Finds an object pose from 3D-2D point correspondences using the RANSAC scheme. 534 535 @param objectPoints Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 536 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3f\> can be also passed here. 537 @param imagePoints Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, 538 where N is the number of points. vector\<Point2f\> can be also passed here. 539 @param cameraMatrix Input camera matrix \f$A = \vecthreethree{fx}{0}{cx}{0}{fy}{cy}{0}{0}{1}\f$ . 540 @param distCoeffs Input vector of distortion coefficients 541 \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]])\f$ of 4, 5, 8 or 12 elements. If 542 the vector is NULL/empty, the zero distortion coefficients are assumed. 543 @param rvec Output rotation vector (see Rodrigues ) that, together with tvec , brings points from 544 the model coordinate system to the camera coordinate system. 545 @param tvec Output translation vector. 546 @param useExtrinsicGuess Parameter used for SOLVEPNP_ITERATIVE. If true (1), the function uses 547 the provided rvec and tvec values as initial approximations of the rotation and translation 548 vectors, respectively, and further optimizes them. 549 @param iterationsCount Number of iterations. 550 @param reprojectionError Inlier threshold value used by the RANSAC procedure. The parameter value 551 is the maximum allowed distance between the observed and computed point projections to consider it 552 an inlier. 553 @param confidence The probability that the algorithm produces a useful result. 554 @param inliers Output vector that contains indices of inliers in objectPoints and imagePoints . 555 @param flags Method for solving a PnP problem (see solvePnP ). 556 557 The function estimates an object pose given a set of object points, their corresponding image 558 projections, as well as the camera matrix and the distortion coefficients. This function finds such 559 a pose that minimizes reprojection error, that is, the sum of squared distances between the observed 560 projections imagePoints and the projected (using projectPoints ) objectPoints. The use of RANSAC 561 makes the function resistant to outliers. 562 563 @note 564 - An example of how to use solvePNPRansac for object detection can be found at 565 opencv_source_code/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/ 566 */ 567 CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints, 568 InputArray cameraMatrix, InputArray distCoeffs, 569 OutputArray rvec, OutputArray tvec, 570 bool useExtrinsicGuess = false, int iterationsCount = 100, 571 float reprojectionError = 8.0, double confidence = 0.99, 572 OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE ); 573 574 /** @brief Finds an initial camera matrix from 3D-2D point correspondences. 575 576 @param objectPoints Vector of vectors of the calibration pattern points in the calibration pattern 577 coordinate space. In the old interface all the per-view vectors are concatenated. See 578 calibrateCamera for details. 579 @param imagePoints Vector of vectors of the projections of the calibration pattern points. In the 580 old interface all the per-view vectors are concatenated. 581 @param imageSize Image size in pixels used to initialize the principal point. 582 @param aspectRatio If it is zero or negative, both \f$f_x\f$ and \f$f_y\f$ are estimated independently. 583 Otherwise, \f$f_x = f_y * \texttt{aspectRatio}\f$ . 584 585 The function estimates and returns an initial camera matrix for the camera calibration process. 586 Currently, the function only supports planar calibration patterns, which are patterns where each 587 object point has z-coordinate =0. 588 */ 589 CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, 590 InputArrayOfArrays imagePoints, 591 Size imageSize, double aspectRatio = 1.0 ); 592 593 /** @brief Finds the positions of internal corners of the chessboard. 594 595 @param image Source chessboard view. It must be an 8-bit grayscale or color image. 596 @param patternSize Number of inner corners per a chessboard row and column 597 ( patternSize = cvSize(points_per_row,points_per_colum) = cvSize(columns,rows) ). 598 @param corners Output array of detected corners. 599 @param flags Various operation flags that can be zero or a combination of the following values: 600 - **CV_CALIB_CB_ADAPTIVE_THRESH** Use adaptive thresholding to convert the image to black 601 and white, rather than a fixed threshold level (computed from the average image brightness). 602 - **CV_CALIB_CB_NORMALIZE_IMAGE** Normalize the image gamma with equalizeHist before 603 applying fixed or adaptive thresholding. 604 - **CV_CALIB_CB_FILTER_QUADS** Use additional criteria (like contour area, perimeter, 605 square-like shape) to filter out false quads extracted at the contour retrieval stage. 606 - **CALIB_CB_FAST_CHECK** Run a fast check on the image that looks for chessboard corners, 607 and shortcut the call if none is found. This can drastically speed up the call in the 608 degenerate condition when no chessboard is observed. 609 610 The function attempts to determine whether the input image is a view of the chessboard pattern and 611 locate the internal chessboard corners. The function returns a non-zero value if all of the corners 612 are found and they are placed in a certain order (row by row, left to right in every row). 613 Otherwise, if the function fails to find all the corners or reorder them, it returns 0. For example, 614 a regular chessboard has 8 x 8 squares and 7 x 7 internal corners, that is, points where the black 615 squares touch each other. The detected coordinates are approximate, and to determine their positions 616 more accurately, the function calls cornerSubPix. You also may use the function cornerSubPix with 617 different parameters if returned coordinates are not accurate enough. 618 619 Sample usage of detecting and drawing chessboard corners: : 620 @code 621 Size patternsize(8,6); //interior number of corners 622 Mat gray = ....; //source image 623 vector<Point2f> corners; //this will be filled by the detected corners 624 625 //CALIB_CB_FAST_CHECK saves a lot of time on images 626 //that do not contain any chessboard corners 627 bool patternfound = findChessboardCorners(gray, patternsize, corners, 628 CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE 629 + CALIB_CB_FAST_CHECK); 630 631 if(patternfound) 632 cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1), 633 TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); 634 635 drawChessboardCorners(img, patternsize, Mat(corners), patternfound); 636 @endcode 637 @note The function requires white space (like a square-thick border, the wider the better) around 638 the board to make the detection more robust in various environments. Otherwise, if there is no 639 border and the background is dark, the outer black squares cannot be segmented properly and so the 640 square grouping and ordering algorithm fails. 641 */ 642 CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners, 643 int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE ); 644 645 //! finds subpixel-accurate positions of the chessboard corners 646 CV_EXPORTS bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size ); 647 648 /** @brief Renders the detected chessboard corners. 649 650 @param image Destination image. It must be an 8-bit color image. 651 @param patternSize Number of inner corners per a chessboard row and column 652 (patternSize = cv::Size(points_per_row,points_per_column)). 653 @param corners Array of detected corners, the output of findChessboardCorners. 654 @param patternWasFound Parameter indicating whether the complete board was found or not. The 655 return value of findChessboardCorners should be passed here. 656 657 The function draws individual chessboard corners detected either as red circles if the board was not 658 found, or as colored corners connected with lines if the board was found. 659 */ 660 CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize, 661 InputArray corners, bool patternWasFound ); 662 663 /** @brief Finds centers in the grid of circles. 664 665 @param image grid view of input circles; it must be an 8-bit grayscale or color image. 666 @param patternSize number of circles per row and column 667 ( patternSize = Size(points_per_row, points_per_colum) ). 668 @param centers output array of detected centers. 669 @param flags various operation flags that can be one of the following values: 670 - **CALIB_CB_SYMMETRIC_GRID** uses symmetric pattern of circles. 671 - **CALIB_CB_ASYMMETRIC_GRID** uses asymmetric pattern of circles. 672 - **CALIB_CB_CLUSTERING** uses a special algorithm for grid detection. It is more robust to 673 perspective distortions but much more sensitive to background clutter. 674 @param blobDetector feature detector that finds blobs like dark circles on light background. 675 676 The function attempts to determine whether the input image contains a grid of circles. If it is, the 677 function locates centers of the circles. The function returns a non-zero value if all of the centers 678 have been found and they have been placed in a certain order (row by row, left to right in every 679 row). Otherwise, if the function fails to find all the corners or reorder them, it returns 0. 680 681 Sample usage of detecting and drawing the centers of circles: : 682 @code 683 Size patternsize(7,7); //number of centers 684 Mat gray = ....; //source image 685 vector<Point2f> centers; //this will be filled by the detected centers 686 687 bool patternfound = findCirclesGrid(gray, patternsize, centers); 688 689 drawChessboardCorners(img, patternsize, Mat(centers), patternfound); 690 @endcode 691 @note The function requires white space (like a square-thick border, the wider the better) around 692 the board to make the detection more robust in various environments. 693 */ 694 CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize, 695 OutputArray centers, int flags = CALIB_CB_SYMMETRIC_GRID, 696 const Ptr<FeatureDetector> &blobDetector = SimpleBlobDetector::create()); 697 698 /** @brief Finds the camera intrinsic and extrinsic parameters from several views of a calibration pattern. 699 700 @param objectPoints In the new interface it is a vector of vectors of calibration pattern points in 701 the calibration pattern coordinate space (e.g. std::vector<std::vector<cv::Vec3f>>). The outer 702 vector contains as many elements as the number of the pattern views. If the same calibration pattern 703 is shown in each view and it is fully visible, all the vectors will be the same. Although, it is 704 possible to use partially occluded patterns, or even different patterns in different views. Then, 705 the vectors will be different. The points are 3D, but since they are in a pattern coordinate system, 706 then, if the rig is planar, it may make sense to put the model to a XY coordinate plane so that 707 Z-coordinate of each input object point is 0. 708 In the old interface all the vectors of object points from different views are concatenated 709 together. 710 @param imagePoints In the new interface it is a vector of vectors of the projections of calibration 711 pattern points (e.g. std::vector<std::vector<cv::Vec2f>>). imagePoints.size() and 712 objectPoints.size() and imagePoints[i].size() must be equal to objectPoints[i].size() for each i. 713 In the old interface all the vectors of object points from different views are concatenated 714 together. 715 @param imageSize Size of the image used only to initialize the intrinsic camera matrix. 716 @param cameraMatrix Output 3x3 floating-point camera matrix 717 \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . If CV\_CALIB\_USE\_INTRINSIC\_GUESS 718 and/or CV_CALIB_FIX_ASPECT_RATIO are specified, some or all of fx, fy, cx, cy must be 719 initialized before calling the function. 720 @param distCoeffs Output vector of distortion coefficients 721 \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]])\f$ of 4, 5, 8 or 12 elements. 722 @param rvecs Output vector of rotation vectors (see Rodrigues ) estimated for each pattern view 723 (e.g. std::vector<cv::Mat>>). That is, each k-th rotation vector together with the corresponding 724 k-th translation vector (see the next output parameter description) brings the calibration pattern 725 from the model coordinate space (in which object points are specified) to the world coordinate 726 space, that is, a real position of the calibration pattern in the k-th pattern view (k=0.. *M* -1). 727 @param tvecs Output vector of translation vectors estimated for each pattern view. 728 @param flags Different flags that may be zero or a combination of the following values: 729 - **CV_CALIB_USE_INTRINSIC_GUESS** cameraMatrix contains valid initial values of 730 fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image 731 center ( imageSize is used), and focal distances are computed in a least-squares fashion. 732 Note, that if intrinsic parameters are known, there is no need to use this function just to 733 estimate extrinsic parameters. Use solvePnP instead. 734 - **CV_CALIB_FIX_PRINCIPAL_POINT** The principal point is not changed during the global 735 optimization. It stays at the center or at a different location specified when 736 CV_CALIB_USE_INTRINSIC_GUESS is set too. 737 - **CV_CALIB_FIX_ASPECT_RATIO** The functions considers only fy as a free parameter. The 738 ratio fx/fy stays the same as in the input cameraMatrix . When 739 CV_CALIB_USE_INTRINSIC_GUESS is not set, the actual input values of fx and fy are 740 ignored, only their ratio is computed and used further. 741 - **CV_CALIB_ZERO_TANGENT_DIST** Tangential distortion coefficients \f$(p_1, p_2)\f$ are set 742 to zeros and stay zero. 743 - **CV_CALIB_FIX_K1,...,CV_CALIB_FIX_K6** The corresponding radial distortion 744 coefficient is not changed during the optimization. If CV_CALIB_USE_INTRINSIC_GUESS is 745 set, the coefficient from the supplied distCoeffs matrix is used. Otherwise, it is set to 0. 746 - **CV_CALIB_RATIONAL_MODEL** Coefficients k4, k5, and k6 are enabled. To provide the 747 backward compatibility, this extra flag should be explicitly specified to make the 748 calibration function use the rational model and return 8 coefficients. If the flag is not 749 set, the function computes and returns only 5 distortion coefficients. 750 - **CALIB_THIN_PRISM_MODEL** Coefficients s1, s2, s3 and s4 are enabled. To provide the 751 backward compatibility, this extra flag should be explicitly specified to make the 752 calibration function use the thin prism model and return 12 coefficients. If the flag is not 753 set, the function computes and returns only 5 distortion coefficients. 754 - **CALIB_FIX_S1_S2_S3_S4** The thin prism distortion coefficients are not changed during 755 the optimization. If CV_CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the 756 supplied distCoeffs matrix is used. Otherwise, it is set to 0. 757 @param criteria Termination criteria for the iterative optimization algorithm. 758 759 The function estimates the intrinsic camera parameters and extrinsic parameters for each of the 760 views. The algorithm is based on @cite Zhang2000 and @cite BouguetMCT . The coordinates of 3D object 761 points and their corresponding 2D projections in each view must be specified. That may be achieved 762 by using an object with a known geometry and easily detectable feature points. Such an object is 763 called a calibration rig or calibration pattern, and OpenCV has built-in support for a chessboard as 764 a calibration rig (see findChessboardCorners ). Currently, initialization of intrinsic parameters 765 (when CV_CALIB_USE_INTRINSIC_GUESS is not set) is only implemented for planar calibration 766 patterns (where Z-coordinates of the object points must be all zeros). 3D calibration rigs can also 767 be used as long as initial cameraMatrix is provided. 768 769 The algorithm performs the following steps: 770 771 - Compute the initial intrinsic parameters (the option only available for planar calibration 772 patterns) or read them from the input parameters. The distortion coefficients are all set to 773 zeros initially unless some of CV_CALIB_FIX_K? are specified. 774 775 - Estimate the initial camera pose as if the intrinsic parameters have been already known. This is 776 done using solvePnP . 777 778 - Run the global Levenberg-Marquardt optimization algorithm to minimize the reprojection error, 779 that is, the total sum of squared distances between the observed feature points imagePoints and 780 the projected (using the current estimates for camera parameters and the poses) object points 781 objectPoints. See projectPoints for details. 782 783 The function returns the final re-projection error. 784 785 @note 786 If you use a non-square (=non-NxN) grid and findChessboardCorners for calibration, and 787 calibrateCamera returns bad values (zero distortion coefficients, an image center very far from 788 (w/2-0.5,h/2-0.5), and/or large differences between \f$f_x\f$ and \f$f_y\f$ (ratios of 10:1 or more)), 789 then you have probably used patternSize=cvSize(rows,cols) instead of using 790 patternSize=cvSize(cols,rows) in findChessboardCorners . 791 792 @sa 793 findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate, undistort 794 */ 795 CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints, 796 InputArrayOfArrays imagePoints, Size imageSize, 797 InputOutputArray cameraMatrix, InputOutputArray distCoeffs, 798 OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, 799 int flags = 0, TermCriteria criteria = TermCriteria( 800 TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); 801 802 /** @brief Computes useful camera characteristics from the camera matrix. 803 804 @param cameraMatrix Input camera matrix that can be estimated by calibrateCamera or 805 stereoCalibrate . 806 @param imageSize Input image size in pixels. 807 @param apertureWidth Physical width in mm of the sensor. 808 @param apertureHeight Physical height in mm of the sensor. 809 @param fovx Output field of view in degrees along the horizontal sensor axis. 810 @param fovy Output field of view in degrees along the vertical sensor axis. 811 @param focalLength Focal length of the lens in mm. 812 @param principalPoint Principal point in mm. 813 @param aspectRatio \f$f_y/f_x\f$ 814 815 The function computes various useful camera characteristics from the previously estimated camera 816 matrix. 817 818 @note 819 Do keep in mind that the unity measure 'mm' stands for whatever unit of measure one chooses for 820 the chessboard pitch (it can thus be any value). 821 */ 822 CV_EXPORTS_W void calibrationMatrixValues( InputArray cameraMatrix, Size imageSize, 823 double apertureWidth, double apertureHeight, 824 CV_OUT double& fovx, CV_OUT double& fovy, 825 CV_OUT double& focalLength, CV_OUT Point2d& principalPoint, 826 CV_OUT double& aspectRatio ); 827 828 /** @brief Calibrates the stereo camera. 829 830 @param objectPoints Vector of vectors of the calibration pattern points. 831 @param imagePoints1 Vector of vectors of the projections of the calibration pattern points, 832 observed by the first camera. 833 @param imagePoints2 Vector of vectors of the projections of the calibration pattern points, 834 observed by the second camera. 835 @param cameraMatrix1 Input/output first camera matrix: 836 \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 837 any of CV_CALIB_USE_INTRINSIC_GUESS , CV_CALIB_FIX_ASPECT_RATIO , 838 CV_CALIB_FIX_INTRINSIC , or CV_CALIB_FIX_FOCAL_LENGTH are specified, some or all of the 839 matrix components must be initialized. See the flags description for details. 840 @param distCoeffs1 Input/output vector of distortion coefficients 841 \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]])\f$ of 4, 5, 8 ot 12 elements. The 842 output vector length depends on the flags. 843 @param cameraMatrix2 Input/output second camera matrix. The parameter is similar to cameraMatrix1 844 @param distCoeffs2 Input/output lens distortion coefficients for the second camera. The parameter 845 is similar to distCoeffs1 . 846 @param imageSize Size of the image used only to initialize intrinsic camera matrix. 847 @param R Output rotation matrix between the 1st and the 2nd camera coordinate systems. 848 @param T Output translation vector between the coordinate systems of the cameras. 849 @param E Output essential matrix. 850 @param F Output fundamental matrix. 851 @param flags Different flags that may be zero or a combination of the following values: 852 - **CV_CALIB_FIX_INTRINSIC** Fix cameraMatrix? and distCoeffs? so that only R, T, E , and F 853 matrices are estimated. 854 - **CV_CALIB_USE_INTRINSIC_GUESS** Optimize some or all of the intrinsic parameters 855 according to the specified flags. Initial values are provided by the user. 856 - **CV_CALIB_FIX_PRINCIPAL_POINT** Fix the principal points during the optimization. 857 - **CV_CALIB_FIX_FOCAL_LENGTH** Fix \f$f^{(j)}_x\f$ and \f$f^{(j)}_y\f$ . 858 - **CV_CALIB_FIX_ASPECT_RATIO** Optimize \f$f^{(j)}_y\f$ . Fix the ratio \f$f^{(j)}_x/f^{(j)}_y\f$ 859 . 860 - **CV_CALIB_SAME_FOCAL_LENGTH** Enforce \f$f^{(0)}_x=f^{(1)}_x\f$ and \f$f^{(0)}_y=f^{(1)}_y\f$ . 861 - **CV_CALIB_ZERO_TANGENT_DIST** Set tangential distortion coefficients for each camera to 862 zeros and fix there. 863 - **CV_CALIB_FIX_K1,...,CV_CALIB_FIX_K6** Do not change the corresponding radial 864 distortion coefficient during the optimization. If CV_CALIB_USE_INTRINSIC_GUESS is set, 865 the coefficient from the supplied distCoeffs matrix is used. Otherwise, it is set to 0. 866 - **CV_CALIB_RATIONAL_MODEL** Enable coefficients k4, k5, and k6. To provide the backward 867 compatibility, this extra flag should be explicitly specified to make the calibration 868 function use the rational model and return 8 coefficients. If the flag is not set, the 869 function computes and returns only 5 distortion coefficients. 870 - **CALIB_THIN_PRISM_MODEL** Coefficients s1, s2, s3 and s4 are enabled. To provide the 871 backward compatibility, this extra flag should be explicitly specified to make the 872 calibration function use the thin prism model and return 12 coefficients. If the flag is not 873 set, the function computes and returns only 5 distortion coefficients. 874 - **CALIB_FIX_S1_S2_S3_S4** The thin prism distortion coefficients are not changed during 875 the optimization. If CV_CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the 876 supplied distCoeffs matrix is used. Otherwise, it is set to 0. 877 @param criteria Termination criteria for the iterative optimization algorithm. 878 879 The function estimates transformation between two cameras making a stereo pair. If you have a stereo 880 camera where the relative position and orientation of two cameras is fixed, and if you computed 881 poses of an object relative to the first camera and to the second camera, (R1, T1) and (R2, T2), 882 respectively (this can be done with solvePnP ), then those poses definitely relate to each other. 883 This means that, given ( \f$R_1\f$,\f$T_1\f$ ), it should be possible to compute ( \f$R_2\f$,\f$T_2\f$ ). You only 884 need to know the position and orientation of the second camera relative to the first camera. This is 885 what the described function does. It computes ( \f$R\f$,\f$T\f$ ) so that: 886 887 \f[R_2=R*R_1 888 T_2=R*T_1 + T,\f] 889 890 Optionally, it computes the essential matrix E: 891 892 \f[E= \vecthreethree{0}{-T_2}{T_1}{T_2}{0}{-T_0}{-T_1}{T_0}{0} *R\f] 893 894 where \f$T_i\f$ are components of the translation vector \f$T\f$ : \f$T=[T_0, T_1, T_2]^T\f$ . And the function 895 can also compute the fundamental matrix F: 896 897 \f[F = cameraMatrix2^{-T} E cameraMatrix1^{-1}\f] 898 899 Besides the stereo-related information, the function can also perform a full calibration of each of 900 two cameras. However, due to the high dimensionality of the parameter space and noise in the input 901 data, the function can diverge from the correct solution. If the intrinsic parameters can be 902 estimated with high accuracy for each of the cameras individually (for example, using 903 calibrateCamera ), you are recommended to do so and then pass CV_CALIB_FIX_INTRINSIC flag to the 904 function along with the computed intrinsic parameters. Otherwise, if all the parameters are 905 estimated at once, it makes sense to restrict some parameters, for example, pass 906 CV_CALIB_SAME_FOCAL_LENGTH and CV_CALIB_ZERO_TANGENT_DIST flags, which is usually a 907 reasonable assumption. 908 909 Similarly to calibrateCamera , the function minimizes the total re-projection error for all the 910 points in all the available views from both cameras. The function returns the final value of the 911 re-projection error. 912 */ 913 CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, 914 InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, 915 InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, 916 InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, 917 Size imageSize, OutputArray R,OutputArray T, OutputArray E, OutputArray F, 918 int flags = CALIB_FIX_INTRINSIC, 919 TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); 920 921 922 /** @brief Computes rectification transforms for each head of a calibrated stereo camera. 923 924 @param cameraMatrix1 First camera matrix. 925 @param cameraMatrix2 Second camera matrix. 926 @param distCoeffs1 First camera distortion parameters. 927 @param distCoeffs2 Second camera distortion parameters. 928 @param imageSize Size of the image used for stereo calibration. 929 @param R Rotation matrix between the coordinate systems of the first and the second cameras. 930 @param T Translation vector between coordinate systems of the cameras. 931 @param R1 Output 3x3 rectification transform (rotation matrix) for the first camera. 932 @param R2 Output 3x3 rectification transform (rotation matrix) for the second camera. 933 @param P1 Output 3x4 projection matrix in the new (rectified) coordinate systems for the first 934 camera. 935 @param P2 Output 3x4 projection matrix in the new (rectified) coordinate systems for the second 936 camera. 937 @param Q Output \f$4 \times 4\f$ disparity-to-depth mapping matrix (see reprojectImageTo3D ). 938 @param flags Operation flags that may be zero or CV_CALIB_ZERO_DISPARITY . If the flag is set, 939 the function makes the principal points of each camera have the same pixel coordinates in the 940 rectified views. And if the flag is not set, the function may still shift the images in the 941 horizontal or vertical direction (depending on the orientation of epipolar lines) to maximize the 942 useful image area. 943 @param alpha Free scaling parameter. If it is -1 or absent, the function performs the default 944 scaling. Otherwise, the parameter should be between 0 and 1. alpha=0 means that the rectified 945 images are zoomed and shifted so that only valid pixels are visible (no black areas after 946 rectification). alpha=1 means that the rectified image is decimated and shifted so that all the 947 pixels from the original images from the cameras are retained in the rectified images (no source 948 image pixels are lost). Obviously, any intermediate value yields an intermediate result between 949 those two extreme cases. 950 @param newImageSize New image resolution after rectification. The same size should be passed to 951 initUndistortRectifyMap (see the stereo_calib.cpp sample in OpenCV samples directory). When (0,0) 952 is passed (default), it is set to the original imageSize . Setting it to larger value can help you 953 preserve details in the original image, especially when there is a big radial distortion. 954 @param validPixROI1 Optional output rectangles inside the rectified images where all the pixels 955 are valid. If alpha=0 , the ROIs cover the whole images. Otherwise, they are likely to be smaller 956 (see the picture below). 957 @param validPixROI2 Optional output rectangles inside the rectified images where all the pixels 958 are valid. If alpha=0 , the ROIs cover the whole images. Otherwise, they are likely to be smaller 959 (see the picture below). 960 961 The function computes the rotation matrices for each camera that (virtually) make both camera image 962 planes the same plane. Consequently, this makes all the epipolar lines parallel and thus simplifies 963 the dense stereo correspondence problem. The function takes the matrices computed by stereoCalibrate 964 as input. As output, it provides two rotation matrices and also two projection matrices in the new 965 coordinates. The function distinguishes the following two cases: 966 967 - **Horizontal stereo**: the first and the second camera views are shifted relative to each other 968 mainly along the x axis (with possible small vertical shift). In the rectified images, the 969 corresponding epipolar lines in the left and right cameras are horizontal and have the same 970 y-coordinate. P1 and P2 look like: 971 972 \f[\texttt{P1} = \begin{bmatrix} f & 0 & cx_1 & 0 \\ 0 & f & cy & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}\f] 973 974 \f[\texttt{P2} = \begin{bmatrix} f & 0 & cx_2 & T_x*f \\ 0 & f & cy & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} ,\f] 975 976 where \f$T_x\f$ is a horizontal shift between the cameras and \f$cx_1=cx_2\f$ if 977 CV_CALIB_ZERO_DISPARITY is set. 978 979 - **Vertical stereo**: the first and the second camera views are shifted relative to each other 980 mainly in vertical direction (and probably a bit in the horizontal direction too). The epipolar 981 lines in the rectified images are vertical and have the same x-coordinate. P1 and P2 look like: 982 983 \f[\texttt{P1} = \begin{bmatrix} f & 0 & cx & 0 \\ 0 & f & cy_1 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}\f] 984 985 \f[\texttt{P2} = \begin{bmatrix} f & 0 & cx & 0 \\ 0 & f & cy_2 & T_y*f \\ 0 & 0 & 1 & 0 \end{bmatrix} ,\f] 986 987 where \f$T_y\f$ is a vertical shift between the cameras and \f$cy_1=cy_2\f$ if CALIB_ZERO_DISPARITY is 988 set. 989 990 As you can see, the first three columns of P1 and P2 will effectively be the new "rectified" camera 991 matrices. The matrices, together with R1 and R2 , can then be passed to initUndistortRectifyMap to 992 initialize the rectification map for each camera. 993 994 See below the screenshot from the stereo_calib.cpp sample. Some red horizontal lines pass through 995 the corresponding image regions. This means that the images are well rectified, which is what most 996 stereo correspondence algorithms rely on. The green rectangles are roi1 and roi2 . You see that 997 their interiors are all valid pixels. 998 999 ![image](pics/stereo_undistort.jpg) 1000 */ 1001 CV_EXPORTS_W void stereoRectify( InputArray cameraMatrix1, InputArray distCoeffs1, 1002 InputArray cameraMatrix2, InputArray distCoeffs2, 1003 Size imageSize, InputArray R, InputArray T, 1004 OutputArray R1, OutputArray R2, 1005 OutputArray P1, OutputArray P2, 1006 OutputArray Q, int flags = CALIB_ZERO_DISPARITY, 1007 double alpha = -1, Size newImageSize = Size(), 1008 CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 ); 1009 1010 /** @brief Computes a rectification transform for an uncalibrated stereo camera. 1011 1012 @param points1 Array of feature points in the first image. 1013 @param points2 The corresponding points in the second image. The same formats as in 1014 findFundamentalMat are supported. 1015 @param F Input fundamental matrix. It can be computed from the same set of point pairs using 1016 findFundamentalMat . 1017 @param imgSize Size of the image. 1018 @param H1 Output rectification homography matrix for the first image. 1019 @param H2 Output rectification homography matrix for the second image. 1020 @param threshold Optional threshold used to filter out the outliers. If the parameter is greater 1021 than zero, all the point pairs that do not comply with the epipolar geometry (that is, the points 1022 for which \f$|\texttt{points2[i]}^T*\texttt{F}*\texttt{points1[i]}|>\texttt{threshold}\f$ ) are 1023 rejected prior to computing the homographies. Otherwise,all the points are considered inliers. 1024 1025 The function computes the rectification transformations without knowing intrinsic parameters of the 1026 cameras and their relative position in the space, which explains the suffix "uncalibrated". Another 1027 related difference from stereoRectify is that the function outputs not the rectification 1028 transformations in the object (3D) space, but the planar perspective transformations encoded by the 1029 homography matrices H1 and H2 . The function implements the algorithm @cite Hartley99 . 1030 1031 @note 1032 While the algorithm does not need to know the intrinsic parameters of the cameras, it heavily 1033 depends on the epipolar geometry. Therefore, if the camera lenses have a significant distortion, 1034 it would be better to correct it before computing the fundamental matrix and calling this 1035 function. For example, distortion coefficients can be estimated for each head of stereo camera 1036 separately by using calibrateCamera . Then, the images can be corrected using undistort , or 1037 just the point coordinates can be corrected with undistortPoints . 1038 */ 1039 CV_EXPORTS_W bool stereoRectifyUncalibrated( InputArray points1, InputArray points2, 1040 InputArray F, Size imgSize, 1041 OutputArray H1, OutputArray H2, 1042 double threshold = 5 ); 1043 1044 //! computes the rectification transformations for 3-head camera, where all the heads are on the same line. 1045 CV_EXPORTS_W float rectify3Collinear( InputArray cameraMatrix1, InputArray distCoeffs1, 1046 InputArray cameraMatrix2, InputArray distCoeffs2, 1047 InputArray cameraMatrix3, InputArray distCoeffs3, 1048 InputArrayOfArrays imgpt1, InputArrayOfArrays imgpt3, 1049 Size imageSize, InputArray R12, InputArray T12, 1050 InputArray R13, InputArray T13, 1051 OutputArray R1, OutputArray R2, OutputArray R3, 1052 OutputArray P1, OutputArray P2, OutputArray P3, 1053 OutputArray Q, double alpha, Size newImgSize, 1054 CV_OUT Rect* roi1, CV_OUT Rect* roi2, int flags ); 1055 1056 /** @brief Returns the new camera matrix based on the free scaling parameter. 1057 1058 @param cameraMatrix Input camera matrix. 1059 @param distCoeffs Input vector of distortion coefficients 1060 \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]])\f$ of 4, 5, 8 or 12 elements. If 1061 the vector is NULL/empty, the zero distortion coefficients are assumed. 1062 @param imageSize Original image size. 1063 @param alpha Free scaling parameter between 0 (when all the pixels in the undistorted image are 1064 valid) and 1 (when all the source image pixels are retained in the undistorted image). See 1065 stereoRectify for details. 1066 @param newImgSize Image size after rectification. By default,it is set to imageSize . 1067 @param validPixROI Optional output rectangle that outlines all-good-pixels region in the 1068 undistorted image. See roi1, roi2 description in stereoRectify . 1069 @param centerPrincipalPoint Optional flag that indicates whether in the new camera matrix the 1070 principal point should be at the image center or not. By default, the principal point is chosen to 1071 best fit a subset of the source image (determined by alpha) to the corrected image. 1072 @return new_camera_matrix Output new camera matrix. 1073 1074 The function computes and returns the optimal new camera matrix based on the free scaling parameter. 1075 By varying this parameter, you may retrieve only sensible pixels alpha=0 , keep all the original 1076 image pixels if there is valuable information in the corners alpha=1 , or get something in between. 1077 When alpha\>0 , the undistortion result is likely to have some black pixels corresponding to 1078 "virtual" pixels outside of the captured distorted image. The original camera matrix, distortion 1079 coefficients, the computed new camera matrix, and newImageSize should be passed to 1080 initUndistortRectifyMap to produce the maps for remap . 1081 */ 1082 CV_EXPORTS_W Mat getOptimalNewCameraMatrix( InputArray cameraMatrix, InputArray distCoeffs, 1083 Size imageSize, double alpha, Size newImgSize = Size(), 1084 CV_OUT Rect* validPixROI = 0, 1085 bool centerPrincipalPoint = false); 1086 1087 /** @brief Converts points from Euclidean to homogeneous space. 1088 1089 @param src Input vector of N-dimensional points. 1090 @param dst Output vector of N+1-dimensional points. 1091 1092 The function converts points from Euclidean to homogeneous space by appending 1's to the tuple of 1093 point coordinates. That is, each point (x1, x2, ..., xn) is converted to (x1, x2, ..., xn, 1). 1094 */ 1095 CV_EXPORTS_W void convertPointsToHomogeneous( InputArray src, OutputArray dst ); 1096 1097 /** @brief Converts points from homogeneous to Euclidean space. 1098 1099 @param src Input vector of N-dimensional points. 1100 @param dst Output vector of N-1-dimensional points. 1101 1102 The function converts points homogeneous to Euclidean space using perspective projection. That is, 1103 each point (x1, x2, ... x(n-1), xn) is converted to (x1/xn, x2/xn, ..., x(n-1)/xn). When xn=0, the 1104 output point coordinates will be (0,0,0,...). 1105 */ 1106 CV_EXPORTS_W void convertPointsFromHomogeneous( InputArray src, OutputArray dst ); 1107 1108 /** @brief Converts points to/from homogeneous coordinates. 1109 1110 @param src Input array or vector of 2D, 3D, or 4D points. 1111 @param dst Output vector of 2D, 3D, or 4D points. 1112 1113 The function converts 2D or 3D points from/to homogeneous coordinates by calling either 1114 convertPointsToHomogeneous or convertPointsFromHomogeneous. 1115 1116 @note The function is obsolete. Use one of the previous two functions instead. 1117 */ 1118 CV_EXPORTS void convertPointsHomogeneous( InputArray src, OutputArray dst ); 1119 1120 /** @brief Calculates a fundamental matrix from the corresponding points in two images. 1121 1122 @param points1 Array of N points from the first image. The point coordinates should be 1123 floating-point (single or double precision). 1124 @param points2 Array of the second image points of the same size and format as points1 . 1125 @param method Method for computing a fundamental matrix. 1126 - **CV_FM_7POINT** for a 7-point algorithm. \f$N = 7\f$ 1127 - **CV_FM_8POINT** for an 8-point algorithm. \f$N \ge 8\f$ 1128 - **CV_FM_RANSAC** for the RANSAC algorithm. \f$N \ge 8\f$ 1129 - **CV_FM_LMEDS** for the LMedS algorithm. \f$N \ge 8\f$ 1130 @param param1 Parameter used for RANSAC. It is the maximum distance from a point to an epipolar 1131 line in pixels, beyond which the point is considered an outlier and is not used for computing the 1132 final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the 1133 point localization, image resolution, and the image noise. 1134 @param param2 Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level 1135 of confidence (probability) that the estimated matrix is correct. 1136 @param mask 1137 1138 The epipolar geometry is described by the following equation: 1139 1140 \f[[p_2; 1]^T F [p_1; 1] = 0\f] 1141 1142 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 1143 second images, respectively. 1144 1145 The function calculates the fundamental matrix using one of four methods listed above and returns 1146 the found fundamental matrix. Normally just one matrix is found. But in case of the 7-point 1147 algorithm, the function may return up to 3 solutions ( \f$9 \times 3\f$ matrix that stores all 3 1148 matrices sequentially). 1149 1150 The calculated fundamental matrix may be passed further to computeCorrespondEpilines that finds the 1151 epipolar lines corresponding to the specified points. It can also be passed to 1152 stereoRectifyUncalibrated to compute the rectification transformation. : 1153 @code 1154 // Example. Estimation of fundamental matrix using the RANSAC algorithm 1155 int point_count = 100; 1156 vector<Point2f> points1(point_count); 1157 vector<Point2f> points2(point_count); 1158 1159 // initialize the points here ... 1160 for( int i = 0; i < point_count; i++ ) 1161 { 1162 points1[i] = ...; 1163 points2[i] = ...; 1164 } 1165 1166 Mat fundamental_matrix = 1167 findFundamentalMat(points1, points2, FM_RANSAC, 3, 0.99); 1168 @endcode 1169 */ 1170 CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2, 1171 int method = FM_RANSAC, 1172 double param1 = 3., double param2 = 0.99, 1173 OutputArray mask = noArray() ); 1174 1175 /** @overload */ 1176 CV_EXPORTS Mat findFundamentalMat( InputArray points1, InputArray points2, 1177 OutputArray mask, int method = FM_RANSAC, 1178 double param1 = 3., double param2 = 0.99 ); 1179 1180 /** @brief Calculates an essential matrix from the corresponding points in two images. 1181 1182 @param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should 1183 be floating-point (single or double precision). 1184 @param points2 Array of the second image points of the same size and format as points1 . 1185 @param focal focal length of the camera. Note that this function assumes that points1 and points2 1186 are feature points from cameras with same focal length and principle point. 1187 @param pp principle point of the camera. 1188 @param method Method for computing a fundamental matrix. 1189 - **RANSAC** for the RANSAC algorithm. 1190 - **MEDS** for the LMedS algorithm. 1191 @param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar 1192 line in pixels, beyond which the point is considered an outlier and is not used for computing the 1193 final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the 1194 point localization, image resolution, and the image noise. 1195 @param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of 1196 confidence (probability) that the estimated matrix is correct. 1197 @param mask Output array of N elements, every element of which is set to 0 for outliers and to 1 1198 for the other points. The array is computed only in the RANSAC and LMedS methods. 1199 1200 This function estimates essential matrix based on the five-point algorithm solver in @cite Nister03 . 1201 @cite SteweniusCFS is also a related. The epipolar geometry is described by the following equation: 1202 1203 \f[[p_2; 1]^T K^{-T} E K^{-1} [p_1; 1] = 0 \\\f]\f[K = 1204 \begin{bmatrix} 1205 f & 0 & x_{pp} \\ 1206 0 & f & y_{pp} \\ 1207 0 & 0 & 1 1208 \end{bmatrix}\f] 1209 1210 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 1211 second images, respectively. The result of this function may be passed further to 1212 decomposeEssentialMat or recoverPose to recover the relative pose between cameras. 1213 */ 1214 CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2, 1215 double focal = 1.0, Point2d pp = Point2d(0, 0), 1216 int method = RANSAC, double prob = 0.999, 1217 double threshold = 1.0, OutputArray mask = noArray() ); 1218 1219 /** @brief Decompose an essential matrix to possible rotations and translation. 1220 1221 @param E The input essential matrix. 1222 @param R1 One possible rotation matrix. 1223 @param R2 Another possible rotation matrix. 1224 @param t One possible translation. 1225 1226 This function decompose an essential matrix E using svd decomposition @cite HartleyZ00 . Generally 4 1227 possible poses exists for a given E. They are \f$[R_1, t]\f$, \f$[R_1, -t]\f$, \f$[R_2, t]\f$, \f$[R_2, -t]\f$. By 1228 decomposing E, you can only get the direction of the translation, so the function returns unit t. 1229 */ 1230 CV_EXPORTS_W void decomposeEssentialMat( InputArray E, OutputArray R1, OutputArray R2, OutputArray t ); 1231 1232 /** @brief Recover relative camera rotation and translation from an estimated essential matrix and the 1233 corresponding points in two images, using cheirality check. Returns the number of inliers which pass 1234 the check. 1235 1236 @param E The input essential matrix. 1237 @param points1 Array of N 2D points from the first image. The point coordinates should be 1238 floating-point (single or double precision). 1239 @param points2 Array of the second image points of the same size and format as points1 . 1240 @param R Recovered relative rotation. 1241 @param t Recoverd relative translation. 1242 @param focal Focal length of the camera. Note that this function assumes that points1 and points2 1243 are feature points from cameras with same focal length and principle point. 1244 @param pp Principle point of the camera. 1245 @param mask Input/output mask for inliers in points1 and points2. 1246 : If it is not empty, then it marks inliers in points1 and points2 for then given essential 1247 matrix E. Only these inliers will be used to recover pose. In the output mask only inliers 1248 which pass the cheirality check. 1249 This function decomposes an essential matrix using decomposeEssentialMat and then verifies possible 1250 pose hypotheses by doing cheirality check. The cheirality check basically means that the 1251 triangulated 3D points should have positive depth. Some details can be found in @cite Nister03 . 1252 1253 This function can be used to process output E and mask from findEssentialMat. In this scenario, 1254 points1 and points2 are the same input for findEssentialMat. : 1255 @code 1256 // Example. Estimation of fundamental matrix using the RANSAC algorithm 1257 int point_count = 100; 1258 vector<Point2f> points1(point_count); 1259 vector<Point2f> points2(point_count); 1260 1261 // initialize the points here ... 1262 for( int i = 0; i < point_count; i++ ) 1263 { 1264 points1[i] = ...; 1265 points2[i] = ...; 1266 } 1267 1268 double focal = 1.0; 1269 cv::Point2d pp(0.0, 0.0); 1270 Mat E, R, t, mask; 1271 1272 E = findEssentialMat(points1, points2, focal, pp, RANSAC, 0.999, 1.0, mask); 1273 recoverPose(E, points1, points2, R, t, focal, pp, mask); 1274 @endcode 1275 */ 1276 CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2, 1277 OutputArray R, OutputArray t, 1278 double focal = 1.0, Point2d pp = Point2d(0, 0), 1279 InputOutputArray mask = noArray() ); 1280 1281 1282 /** @brief For points in an image of a stereo pair, computes the corresponding epilines in the other image. 1283 1284 @param points Input points. \f$N \times 1\f$ or \f$1 \times N\f$ matrix of type CV_32FC2 or 1285 vector\<Point2f\> . 1286 @param whichImage Index of the image (1 or 2) that contains the points . 1287 @param F Fundamental matrix that can be estimated using findFundamentalMat or stereoRectify . 1288 @param lines Output vector of the epipolar lines corresponding to the points in the other image. 1289 Each line \f$ax + by + c=0\f$ is encoded by 3 numbers \f$(a, b, c)\f$ . 1290 1291 For every point in one of the two images of a stereo pair, the function finds the equation of the 1292 corresponding epipolar line in the other image. 1293 1294 From the fundamental matrix definition (see findFundamentalMat ), line \f$l^{(2)}_i\f$ in the second 1295 image for the point \f$p^{(1)}_i\f$ in the first image (when whichImage=1 ) is computed as: 1296 1297 \f[l^{(2)}_i = F p^{(1)}_i\f] 1298 1299 And vice versa, when whichImage=2, \f$l^{(1)}_i\f$ is computed from \f$p^{(2)}_i\f$ as: 1300 1301 \f[l^{(1)}_i = F^T p^{(2)}_i\f] 1302 1303 Line coefficients are defined up to a scale. They are normalized so that \f$a_i^2+b_i^2=1\f$ . 1304 */ 1305 CV_EXPORTS_W void computeCorrespondEpilines( InputArray points, int whichImage, 1306 InputArray F, OutputArray lines ); 1307 1308 /** @brief Reconstructs points by triangulation. 1309 1310 @param projMatr1 3x4 projection matrix of the first camera. 1311 @param projMatr2 3x4 projection matrix of the second camera. 1312 @param projPoints1 2xN array of feature points in the first image. In case of c++ version it can 1313 be also a vector of feature points or two-channel matrix of size 1xN or Nx1. 1314 @param projPoints2 2xN array of corresponding points in the second image. In case of c++ version 1315 it can be also a vector of feature points or two-channel matrix of size 1xN or Nx1. 1316 @param points4D 4xN array of reconstructed points in homogeneous coordinates. 1317 1318 The function reconstructs 3-dimensional points (in homogeneous coordinates) by using their 1319 observations with a stereo camera. Projections matrices can be obtained from stereoRectify. 1320 1321 @note 1322 Keep in mind that all input data should be of float type in order for this function to work. 1323 1324 @sa 1325 reprojectImageTo3D 1326 */ 1327 CV_EXPORTS_W void triangulatePoints( InputArray projMatr1, InputArray projMatr2, 1328 InputArray projPoints1, InputArray projPoints2, 1329 OutputArray points4D ); 1330 1331 /** @brief Refines coordinates of corresponding points. 1332 1333 @param F 3x3 fundamental matrix. 1334 @param points1 1xN array containing the first set of points. 1335 @param points2 1xN array containing the second set of points. 1336 @param newPoints1 The optimized points1. 1337 @param newPoints2 The optimized points2. 1338 1339 The function implements the Optimal Triangulation Method (see Multiple View Geometry for details). 1340 For each given point correspondence points1[i] \<-\> points2[i], and a fundamental matrix F, it 1341 computes the corrected correspondences newPoints1[i] \<-\> newPoints2[i] that minimize the geometric 1342 error \f$d(points1[i], newPoints1[i])^2 + d(points2[i],newPoints2[i])^2\f$ (where \f$d(a,b)\f$ is the 1343 geometric distance between points \f$a\f$ and \f$b\f$ ) subject to the epipolar constraint 1344 \f$newPoints2^T * F * newPoints1 = 0\f$ . 1345 */ 1346 CV_EXPORTS_W void correctMatches( InputArray F, InputArray points1, InputArray points2, 1347 OutputArray newPoints1, OutputArray newPoints2 ); 1348 1349 /** @brief Filters off small noise blobs (speckles) in the disparity map 1350 1351 @param img The input 16-bit signed disparity image 1352 @param newVal The disparity value used to paint-off the speckles 1353 @param maxSpeckleSize The maximum speckle size to consider it a speckle. Larger blobs are not 1354 affected by the algorithm 1355 @param maxDiff Maximum difference between neighbor disparity pixels to put them into the same 1356 blob. Note that since StereoBM, StereoSGBM and may be other algorithms return a fixed-point 1357 disparity map, where disparity values are multiplied by 16, this scale factor should be taken into 1358 account when specifying this parameter value. 1359 @param buf The optional temporary buffer to avoid memory allocation within the function. 1360 */ 1361 CV_EXPORTS_W void filterSpeckles( InputOutputArray img, double newVal, 1362 int maxSpeckleSize, double maxDiff, 1363 InputOutputArray buf = noArray() ); 1364 1365 //! computes valid disparity ROI from the valid ROIs of the rectified images (that are returned by cv::stereoRectify()) 1366 CV_EXPORTS_W Rect getValidDisparityROI( Rect roi1, Rect roi2, 1367 int minDisparity, int numberOfDisparities, 1368 int SADWindowSize ); 1369 1370 //! validates disparity using the left-right check. The matrix "cost" should be computed by the stereo correspondence algorithm 1371 CV_EXPORTS_W void validateDisparity( InputOutputArray disparity, InputArray cost, 1372 int minDisparity, int numberOfDisparities, 1373 int disp12MaxDisp = 1 ); 1374 1375 /** @brief Reprojects a disparity image to 3D space. 1376 1377 @param disparity Input single-channel 8-bit unsigned, 16-bit signed, 32-bit signed or 32-bit 1378 floating-point disparity image. 1379 @param _3dImage Output 3-channel floating-point image of the same size as disparity . Each 1380 element of _3dImage(x,y) contains 3D coordinates of the point (x,y) computed from the disparity 1381 map. 1382 @param Q \f$4 \times 4\f$ perspective transformation matrix that can be obtained with stereoRectify. 1383 @param handleMissingValues Indicates, whether the function should handle missing values (i.e. 1384 points where the disparity was not computed). If handleMissingValues=true, then pixels with the 1385 minimal disparity that corresponds to the outliers (see StereoMatcher::compute ) are transformed 1386 to 3D points with a very large Z value (currently set to 10000). 1387 @param ddepth The optional output array depth. If it is -1, the output image will have CV_32F 1388 depth. ddepth can also be set to CV_16S, CV_32S or CV_32F. 1389 1390 The function transforms a single-channel disparity map to a 3-channel image representing a 3D 1391 surface. That is, for each pixel (x,y) andthe corresponding disparity d=disparity(x,y) , it 1392 computes: 1393 1394 \f[\begin{array}{l} [X \; Y \; Z \; W]^T = \texttt{Q} *[x \; y \; \texttt{disparity} (x,y) \; 1]^T \\ \texttt{\_3dImage} (x,y) = (X/W, \; Y/W, \; Z/W) \end{array}\f] 1395 1396 The matrix Q can be an arbitrary \f$4 \times 4\f$ matrix (for example, the one computed by 1397 stereoRectify). To reproject a sparse set of points {(x,y,d),...} to 3D space, use 1398 perspectiveTransform . 1399 */ 1400 CV_EXPORTS_W void reprojectImageTo3D( InputArray disparity, 1401 OutputArray _3dImage, InputArray Q, 1402 bool handleMissingValues = false, 1403 int ddepth = -1 ); 1404 1405 /** @brief Computes an optimal affine transformation between two 3D point sets. 1406 1407 @param src First input 3D point set. 1408 @param dst Second input 3D point set. 1409 @param out Output 3D affine transformation matrix \f$3 \times 4\f$ . 1410 @param inliers Output vector indicating which points are inliers. 1411 @param ransacThreshold Maximum reprojection error in the RANSAC algorithm to consider a point as 1412 an inlier. 1413 @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything 1414 between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation 1415 significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation. 1416 1417 The function estimates an optimal 3D affine transformation between two 3D point sets using the 1418 RANSAC algorithm. 1419 */ 1420 CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst, 1421 OutputArray out, OutputArray inliers, 1422 double ransacThreshold = 3, double confidence = 0.99); 1423 1424 /** @brief Decompose a homography matrix to rotation(s), translation(s) and plane normal(s). 1425 1426 @param H The input homography matrix between two images. 1427 @param K The input intrinsic camera calibration matrix. 1428 @param rotations Array of rotation matrices. 1429 @param translations Array of translation matrices. 1430 @param normals Array of plane normal matrices. 1431 1432 This function extracts relative camera motion between two views observing a planar object from the 1433 homography H induced by the plane. The intrinsic camera matrix K must also be provided. The function 1434 may return up to four mathematical solution sets. At least two of the solutions may further be 1435 invalidated if point correspondences are available by applying positive depth constraint (all points 1436 must be in front of the camera). The decomposition method is described in detail in @cite Malis . 1437 */ 1438 CV_EXPORTS_W int decomposeHomographyMat(InputArray H, 1439 InputArray K, 1440 OutputArrayOfArrays rotations, 1441 OutputArrayOfArrays translations, 1442 OutputArrayOfArrays normals); 1443 1444 /** @brief The base class for stereo correspondence algorithms. 1445 */ 1446 class CV_EXPORTS_W StereoMatcher : public Algorithm 1447 { 1448 public: 1449 enum { DISP_SHIFT = 4, 1450 DISP_SCALE = (1 << DISP_SHIFT) 1451 }; 1452 1453 /** @brief Computes disparity map for the specified stereo pair 1454 1455 @param left Left 8-bit single-channel image. 1456 @param right Right image of the same size and the same type as the left one. 1457 @param disparity Output disparity map. It has the same size as the input images. Some algorithms, 1458 like StereoBM or StereoSGBM compute 16-bit fixed-point disparity map (where each disparity value 1459 has 4 fractional bits), whereas other algorithms output 32-bit floating-point disparity map. 1460 */ 1461 CV_WRAP virtual void compute( InputArray left, InputArray right, 1462 OutputArray disparity ) = 0; 1463 1464 CV_WRAP virtual int getMinDisparity() const = 0; 1465 CV_WRAP virtual void setMinDisparity(int minDisparity) = 0; 1466 1467 CV_WRAP virtual int getNumDisparities() const = 0; 1468 CV_WRAP virtual void setNumDisparities(int numDisparities) = 0; 1469 1470 CV_WRAP virtual int getBlockSize() const = 0; 1471 CV_WRAP virtual void setBlockSize(int blockSize) = 0; 1472 1473 CV_WRAP virtual int getSpeckleWindowSize() const = 0; 1474 CV_WRAP virtual void setSpeckleWindowSize(int speckleWindowSize) = 0; 1475 1476 CV_WRAP virtual int getSpeckleRange() const = 0; 1477 CV_WRAP virtual void setSpeckleRange(int speckleRange) = 0; 1478 1479 CV_WRAP virtual int getDisp12MaxDiff() const = 0; 1480 CV_WRAP virtual void setDisp12MaxDiff(int disp12MaxDiff) = 0; 1481 }; 1482 1483 1484 /** @brief Class for computing stereo correspondence using the block matching algorithm, introduced and 1485 contributed to OpenCV by K. Konolige. 1486 */ 1487 class CV_EXPORTS_W StereoBM : public StereoMatcher 1488 { 1489 public: 1490 enum { PREFILTER_NORMALIZED_RESPONSE = 0, 1491 PREFILTER_XSOBEL = 1 1492 }; 1493 1494 CV_WRAP virtual int getPreFilterType() const = 0; 1495 CV_WRAP virtual void setPreFilterType(int preFilterType) = 0; 1496 1497 CV_WRAP virtual int getPreFilterSize() const = 0; 1498 CV_WRAP virtual void setPreFilterSize(int preFilterSize) = 0; 1499 1500 CV_WRAP virtual int getPreFilterCap() const = 0; 1501 CV_WRAP virtual void setPreFilterCap(int preFilterCap) = 0; 1502 1503 CV_WRAP virtual int getTextureThreshold() const = 0; 1504 CV_WRAP virtual void setTextureThreshold(int textureThreshold) = 0; 1505 1506 CV_WRAP virtual int getUniquenessRatio() const = 0; 1507 CV_WRAP virtual void setUniquenessRatio(int uniquenessRatio) = 0; 1508 1509 CV_WRAP virtual int getSmallerBlockSize() const = 0; 1510 CV_WRAP virtual void setSmallerBlockSize(int blockSize) = 0; 1511 1512 CV_WRAP virtual Rect getROI1() const = 0; 1513 CV_WRAP virtual void setROI1(Rect roi1) = 0; 1514 1515 CV_WRAP virtual Rect getROI2() const = 0; 1516 CV_WRAP virtual void setROI2(Rect roi2) = 0; 1517 1518 /** @brief Creates StereoBM object 1519 1520 @param numDisparities the disparity search range. For each pixel algorithm will find the best 1521 disparity from 0 (default minimum disparity) to numDisparities. The search range can then be 1522 shifted by changing the minimum disparity. 1523 @param blockSize the linear size of the blocks compared by the algorithm. The size should be odd 1524 (as the block is centered at the current pixel). Larger block size implies smoother, though less 1525 accurate disparity map. Smaller block size gives more detailed disparity map, but there is higher 1526 chance for algorithm to find a wrong correspondence. 1527 1528 The function create StereoBM object. You can then call StereoBM::compute() to compute disparity for 1529 a specific stereo pair. 1530 */ 1531 CV_WRAP static Ptr<StereoBM> create(int numDisparities = 0, int blockSize = 21); 1532 }; 1533 1534 /** @brief The class implements the modified H. Hirschmuller algorithm @cite HH08 that differs from the original 1535 one as follows: 1536 1537 - By default, the algorithm is single-pass, which means that you consider only 5 directions 1538 instead of 8. Set mode=StereoSGBM::MODE_HH in createStereoSGBM to run the full variant of the 1539 algorithm but beware that it may consume a lot of memory. 1540 - The algorithm matches blocks, not individual pixels. Though, setting blockSize=1 reduces the 1541 blocks to single pixels. 1542 - Mutual information cost function is not implemented. Instead, a simpler Birchfield-Tomasi 1543 sub-pixel metric from @cite BT98 is used. Though, the color images are supported as well. 1544 - Some pre- and post- processing steps from K. Konolige algorithm StereoBM are included, for 1545 example: pre-filtering (StereoBM::PREFILTER_XSOBEL type) and post-filtering (uniqueness 1546 check, quadratic interpolation and speckle filtering). 1547 1548 @note 1549 - (Python) An example illustrating the use of the StereoSGBM matching algorithm can be found 1550 at opencv_source_code/samples/python2/stereo_match.py 1551 */ 1552 class CV_EXPORTS_W StereoSGBM : public StereoMatcher 1553 { 1554 public: 1555 enum 1556 { 1557 MODE_SGBM = 0, 1558 MODE_HH = 1 1559 }; 1560 1561 CV_WRAP virtual int getPreFilterCap() const = 0; 1562 CV_WRAP virtual void setPreFilterCap(int preFilterCap) = 0; 1563 1564 CV_WRAP virtual int getUniquenessRatio() const = 0; 1565 CV_WRAP virtual void setUniquenessRatio(int uniquenessRatio) = 0; 1566 1567 CV_WRAP virtual int getP1() const = 0; 1568 CV_WRAP virtual void setP1(int P1) = 0; 1569 1570 CV_WRAP virtual int getP2() const = 0; 1571 CV_WRAP virtual void setP2(int P2) = 0; 1572 1573 CV_WRAP virtual int getMode() const = 0; 1574 CV_WRAP virtual void setMode(int mode) = 0; 1575 1576 /** @brief Creates StereoSGBM object 1577 1578 @param minDisparity Minimum possible disparity value. Normally, it is zero but sometimes 1579 rectification algorithms can shift images, so this parameter needs to be adjusted accordingly. 1580 @param numDisparities Maximum disparity minus minimum disparity. The value is always greater than 1581 zero. In the current implementation, this parameter must be divisible by 16. 1582 @param blockSize Matched block size. It must be an odd number \>=1 . Normally, it should be 1583 somewhere in the 3..11 range. 1584 @param P1 The first parameter controlling the disparity smoothness. See below. 1585 @param P2 The second parameter controlling the disparity smoothness. The larger the values are, 1586 the smoother the disparity is. P1 is the penalty on the disparity change by plus or minus 1 1587 between neighbor pixels. P2 is the penalty on the disparity change by more than 1 between neighbor 1588 pixels. The algorithm requires P2 \> P1 . See stereo_match.cpp sample where some reasonably good 1589 P1 and P2 values are shown (like 8\*number_of_image_channels\*SADWindowSize\*SADWindowSize and 1590 32\*number_of_image_channels\*SADWindowSize\*SADWindowSize , respectively). 1591 @param disp12MaxDiff Maximum allowed difference (in integer pixel units) in the left-right 1592 disparity check. Set it to a non-positive value to disable the check. 1593 @param preFilterCap Truncation value for the prefiltered image pixels. The algorithm first 1594 computes x-derivative at each pixel and clips its value by [-preFilterCap, preFilterCap] interval. 1595 The result values are passed to the Birchfield-Tomasi pixel cost function. 1596 @param uniquenessRatio Margin in percentage by which the best (minimum) computed cost function 1597 value should "win" the second best value to consider the found match correct. Normally, a value 1598 within the 5-15 range is good enough. 1599 @param speckleWindowSize Maximum size of smooth disparity regions to consider their noise speckles 1600 and invalidate. Set it to 0 to disable speckle filtering. Otherwise, set it somewhere in the 1601 50-200 range. 1602 @param speckleRange Maximum disparity variation within each connected component. If you do speckle 1603 filtering, set the parameter to a positive value, it will be implicitly multiplied by 16. 1604 Normally, 1 or 2 is good enough. 1605 @param mode Set it to StereoSGBM::MODE_HH to run the full-scale two-pass dynamic programming 1606 algorithm. It will consume O(W\*H\*numDisparities) bytes, which is large for 640x480 stereo and 1607 huge for HD-size pictures. By default, it is set to false . 1608 1609 The first constructor initializes StereoSGBM with all the default parameters. So, you only have to 1610 set StereoSGBM::numDisparities at minimum. The second constructor enables you to set each parameter 1611 to a custom value. 1612 */ 1613 CV_WRAP static Ptr<StereoSGBM> create(int minDisparity, int numDisparities, int blockSize, 1614 int P1 = 0, int P2 = 0, int disp12MaxDiff = 0, 1615 int preFilterCap = 0, int uniquenessRatio = 0, 1616 int speckleWindowSize = 0, int speckleRange = 0, 1617 int mode = StereoSGBM::MODE_SGBM); 1618 }; 1619 1620 //! @} calib3d 1621 1622 /** @brief The methods in this namespace use a so-called fisheye camera model. 1623 @ingroup calib3d_fisheye 1624 */ 1625 namespace fisheye 1626 { 1627 //! @addtogroup calib3d_fisheye 1628 //! @{ 1629 1630 enum{ 1631 CALIB_USE_INTRINSIC_GUESS = 1, 1632 CALIB_RECOMPUTE_EXTRINSIC = 2, 1633 CALIB_CHECK_COND = 4, 1634 CALIB_FIX_SKEW = 8, 1635 CALIB_FIX_K1 = 16, 1636 CALIB_FIX_K2 = 32, 1637 CALIB_FIX_K3 = 64, 1638 CALIB_FIX_K4 = 128, 1639 CALIB_FIX_INTRINSIC = 256 1640 }; 1641 1642 /** @brief Projects points using fisheye model 1643 1644 @param objectPoints Array of object points, 1xN/Nx1 3-channel (or vector\<Point3f\> ), where N is 1645 the number of points in the view. 1646 @param imagePoints Output array of image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, or 1647 vector\<Point2f\>. 1648 @param affine 1649 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. 1650 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$. 1651 @param alpha The skew coefficient. 1652 @param jacobian Optional output 2Nx15 jacobian matrix of derivatives of image points with respect 1653 to components of the focal lengths, coordinates of the principal point, distortion coefficients, 1654 rotation vector, translation vector, and the skew. In the old interface different components of 1655 the jacobian are returned via different output parameters. 1656 1657 The function computes projections of 3D points to the image plane given intrinsic and extrinsic 1658 camera parameters. Optionally, the function computes Jacobians - matrices of partial derivatives of 1659 image points coordinates (as functions of all the input parameters) with respect to the particular 1660 parameters, intrinsic and/or extrinsic. 1661 */ 1662 CV_EXPORTS void projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine, 1663 InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()); 1664 1665 /** @overload */ 1666 CV_EXPORTS_W void projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec, 1667 InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()); 1668 1669 /** @brief Distorts 2D points using fisheye model. 1670 1671 @param undistorted Array of object points, 1xN/Nx1 2-channel (or vector\<Point2f\> ), where N is 1672 the number of points in the view. 1673 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. 1674 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$. 1675 @param alpha The skew coefficient. 1676 @param distorted Output array of image points, 1xN/Nx1 2-channel, or vector\<Point2f\> . 1677 */ 1678 CV_EXPORTS_W void distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0); 1679 1680 /** @brief Undistorts 2D points using fisheye model 1681 1682 @param distorted Array of object points, 1xN/Nx1 2-channel (or vector\<Point2f\> ), where N is the 1683 number of points in the view. 1684 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. 1685 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$. 1686 @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1687 1-channel or 1x1 3-channel 1688 @param P New camera matrix (3x3) or new projection matrix (3x4) 1689 @param undistorted Output array of image points, 1xN/Nx1 2-channel, or vector\<Point2f\> . 1690 */ 1691 CV_EXPORTS_W void undistortPoints(InputArray distorted, OutputArray undistorted, 1692 InputArray K, InputArray D, InputArray R = noArray(), InputArray P = noArray()); 1693 1694 /** @brief Computes undistortion and rectification maps for image transform by cv::remap(). If D is empty zero 1695 distortion is used, if R or P is empty identity matrixes are used. 1696 1697 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. 1698 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$. 1699 @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1700 1-channel or 1x1 3-channel 1701 @param P New camera matrix (3x3) or new projection matrix (3x4) 1702 @param size Undistorted image size. 1703 @param m1type Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps() 1704 for details. 1705 @param map1 The first output map. 1706 @param map2 The second output map. 1707 */ 1708 CV_EXPORTS_W void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P, 1709 const cv::Size& size, int m1type, OutputArray map1, OutputArray map2); 1710 1711 /** @brief Transforms an image to compensate for fisheye lens distortion. 1712 1713 @param distorted image with fisheye lens distortion. 1714 @param undistorted Output image with compensated fisheye lens distortion. 1715 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. 1716 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$. 1717 @param Knew Camera matrix of the distorted image. By default, it is the identity matrix but you 1718 may additionally scale and shift the result by using a different matrix. 1719 @param new_size 1720 1721 The function transforms an image to compensate radial and tangential lens distortion. 1722 1723 The function is simply a combination of fisheye::initUndistortRectifyMap (with unity R ) and remap 1724 (with bilinear interpolation). See the former function for details of the transformation being 1725 performed. 1726 1727 See below the results of undistortImage. 1728 - a\) result of undistort of perspective camera model (all possible coefficients (k_1, k_2, k_3, 1729 k_4, k_5, k_6) of distortion were optimized under calibration) 1730 - b\) result of fisheye::undistortImage of fisheye camera model (all possible coefficients (k_1, k_2, 1731 k_3, k_4) of fisheye distortion were optimized under calibration) 1732 - c\) original image was captured with fisheye lens 1733 1734 Pictures a) and b) almost the same. But if we consider points of image located far from the center 1735 of image, we can notice that on image a) these points are distorted. 1736 1737 ![image](pics/fisheye_undistorted.jpg) 1738 */ 1739 CV_EXPORTS_W void undistortImage(InputArray distorted, OutputArray undistorted, 1740 InputArray K, InputArray D, InputArray Knew = cv::noArray(), const Size& new_size = Size()); 1741 1742 /** @brief Estimates new camera matrix for undistortion or rectification. 1743 1744 @param K Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. 1745 @param image_size 1746 @param D Input vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$. 1747 @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 1748 1-channel or 1x1 3-channel 1749 @param P New camera matrix (3x3) or new projection matrix (3x4) 1750 @param balance Sets the new focal length in range between the min focal length and the max focal 1751 length. Balance is in range of [0, 1]. 1752 @param new_size 1753 @param fov_scale Divisor for new focal length. 1754 */ 1755 CV_EXPORTS_W void estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R, 1756 OutputArray P, double balance = 0.0, const Size& new_size = Size(), double fov_scale = 1.0); 1757 1758 /** @brief Performs camera calibaration 1759 1760 @param objectPoints vector of vectors of calibration pattern points in the calibration pattern 1761 coordinate space. 1762 @param imagePoints vector of vectors of the projections of calibration pattern points. 1763 imagePoints.size() and objectPoints.size() and imagePoints[i].size() must be equal to 1764 objectPoints[i].size() for each i. 1765 @param image_size Size of the image used only to initialize the intrinsic camera matrix. 1766 @param K Output 3x3 floating-point camera matrix 1767 \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . If 1768 fisheye::CALIB_USE_INTRINSIC_GUESS/ is specified, some or all of fx, fy, cx, cy must be 1769 initialized before calling the function. 1770 @param D Output vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$. 1771 @param rvecs Output vector of rotation vectors (see Rodrigues ) estimated for each pattern view. 1772 That is, each k-th rotation vector together with the corresponding k-th translation vector (see 1773 the next output parameter description) brings the calibration pattern from the model coordinate 1774 space (in which object points are specified) to the world coordinate space, that is, a real 1775 position of the calibration pattern in the k-th pattern view (k=0.. *M* -1). 1776 @param tvecs Output vector of translation vectors estimated for each pattern view. 1777 @param flags Different flags that may be zero or a combination of the following values: 1778 - **fisheye::CALIB_USE_INTRINSIC_GUESS** cameraMatrix contains valid initial values of 1779 fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image 1780 center ( imageSize is used), and focal distances are computed in a least-squares fashion. 1781 - **fisheye::CALIB_RECOMPUTE_EXTRINSIC** Extrinsic will be recomputed after each iteration 1782 of intrinsic optimization. 1783 - **fisheye::CALIB_CHECK_COND** The functions will check validity of condition number. 1784 - **fisheye::CALIB_FIX_SKEW** Skew coefficient (alpha) is set to zero and stay zero. 1785 - **fisheye::CALIB_FIX_K1..4** Selected distortion coefficients are set to zeros and stay 1786 zero. 1787 @param criteria Termination criteria for the iterative optimization algorithm. 1788 */ 1789 CV_EXPORTS_W double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size, 1790 InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0, 1791 TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); 1792 1793 /** @brief Stereo rectification for fisheye camera model 1794 1795 @param K1 First camera matrix. 1796 @param D1 First camera distortion parameters. 1797 @param K2 Second camera matrix. 1798 @param D2 Second camera distortion parameters. 1799 @param imageSize Size of the image used for stereo calibration. 1800 @param R Rotation matrix between the coordinate systems of the first and the second 1801 cameras. 1802 @param tvec Translation vector between coordinate systems of the cameras. 1803 @param R1 Output 3x3 rectification transform (rotation matrix) for the first camera. 1804 @param R2 Output 3x3 rectification transform (rotation matrix) for the second camera. 1805 @param P1 Output 3x4 projection matrix in the new (rectified) coordinate systems for the first 1806 camera. 1807 @param P2 Output 3x4 projection matrix in the new (rectified) coordinate systems for the second 1808 camera. 1809 @param Q Output \f$4 \times 4\f$ disparity-to-depth mapping matrix (see reprojectImageTo3D ). 1810 @param flags Operation flags that may be zero or CV_CALIB_ZERO_DISPARITY . If the flag is set, 1811 the function makes the principal points of each camera have the same pixel coordinates in the 1812 rectified views. And if the flag is not set, the function may still shift the images in the 1813 horizontal or vertical direction (depending on the orientation of epipolar lines) to maximize the 1814 useful image area. 1815 @param newImageSize New image resolution after rectification. The same size should be passed to 1816 initUndistortRectifyMap (see the stereo_calib.cpp sample in OpenCV samples directory). When (0,0) 1817 is passed (default), it is set to the original imageSize . Setting it to larger value can help you 1818 preserve details in the original image, especially when there is a big radial distortion. 1819 @param balance Sets the new focal length in range between the min focal length and the max focal 1820 length. Balance is in range of [0, 1]. 1821 @param fov_scale Divisor for new focal length. 1822 */ 1823 CV_EXPORTS_W void stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec, 1824 OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(), 1825 double balance = 0.0, double fov_scale = 1.0); 1826 1827 /** @brief Performs stereo calibration 1828 1829 @param objectPoints Vector of vectors of the calibration pattern points. 1830 @param imagePoints1 Vector of vectors of the projections of the calibration pattern points, 1831 observed by the first camera. 1832 @param imagePoints2 Vector of vectors of the projections of the calibration pattern points, 1833 observed by the second camera. 1834 @param K1 Input/output first camera matrix: 1835 \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 1836 any of fisheye::CALIB_USE_INTRINSIC_GUESS , fisheye::CV_CALIB_FIX_INTRINSIC are specified, 1837 some or all of the matrix components must be initialized. 1838 @param D1 Input/output vector of distortion coefficients \f$(k_1, k_2, k_3, k_4)\f$ of 4 elements. 1839 @param K2 Input/output second camera matrix. The parameter is similar to K1 . 1840 @param D2 Input/output lens distortion coefficients for the second camera. The parameter is 1841 similar to D1 . 1842 @param imageSize Size of the image used only to initialize intrinsic camera matrix. 1843 @param R Output rotation matrix between the 1st and the 2nd camera coordinate systems. 1844 @param T Output translation vector between the coordinate systems of the cameras. 1845 @param flags Different flags that may be zero or a combination of the following values: 1846 - **fisheye::CV_CALIB_FIX_INTRINSIC** Fix K1, K2? and D1, D2? so that only R, T matrices 1847 are estimated. 1848 - **fisheye::CALIB_USE_INTRINSIC_GUESS** K1, K2 contains valid initial values of 1849 fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image 1850 center (imageSize is used), and focal distances are computed in a least-squares fashion. 1851 - **fisheye::CALIB_RECOMPUTE_EXTRINSIC** Extrinsic will be recomputed after each iteration 1852 of intrinsic optimization. 1853 - **fisheye::CALIB_CHECK_COND** The functions will check validity of condition number. 1854 - **fisheye::CALIB_FIX_SKEW** Skew coefficient (alpha) is set to zero and stay zero. 1855 - **fisheye::CALIB_FIX_K1..4** Selected distortion coefficients are set to zeros and stay 1856 zero. 1857 @param criteria Termination criteria for the iterative optimization algorithm. 1858 */ 1859 CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, 1860 InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, 1861 OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC, 1862 TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); 1863 1864 //! @} calib3d_fisheye 1865 } 1866 1867 } // cv 1868 1869 #ifndef DISABLE_OPENCV_24_COMPATIBILITY 1870 #include "opencv2/calib3d/calib3d_c.h" 1871 #endif 1872 1873 #endif 1874