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/core/types.hpp" |
49 | #include "opencv2/features2d.hpp" |
50 | #include "opencv2/core/affine.hpp" |
51 | #include "opencv2/core/utils/logger.hpp" |
52 | |
53 | /** |
54 | @defgroup calib3d Camera Calibration and 3D Reconstruction |
55 | |
56 | The functions in this section use a so-called pinhole camera model. The view of a scene |
57 | is obtained by projecting a scene's 3D point \f$P_w\f$ into the image plane using a perspective |
58 | transformation which forms the corresponding pixel \f$p\f$. Both \f$P_w\f$ and \f$p\f$ are |
59 | represented in homogeneous coordinates, i.e. as 3D and 2D homogeneous vector respectively. You will |
60 | find a brief introduction to projective geometry, homogeneous vectors and homogeneous |
61 | transformations at the end of this section's introduction. For more succinct notation, we often drop |
62 | the 'homogeneous' and say vector instead of homogeneous vector. |
63 | |
64 | The distortion-free projective transformation given by a pinhole camera model is shown below. |
65 | |
66 | \f[s \; p = A \begin{bmatrix} R|t \end{bmatrix} P_w,\f] |
67 | |
68 | where \f$P_w\f$ is a 3D point expressed with respect to the world coordinate system, |
69 | \f$p\f$ is a 2D pixel in the image plane, \f$A\f$ is the camera intrinsic matrix, |
70 | \f$R\f$ and \f$t\f$ are the rotation and translation that describe the change of coordinates from |
71 | world to camera coordinate systems (or camera frame) and \f$s\f$ is the projective transformation's |
72 | arbitrary scaling and not part of the camera model. |
73 | |
74 | The camera intrinsic matrix \f$A\f$ (notation used as in @cite Zhang2000 and also generally notated |
75 | as \f$K\f$) projects 3D points given in the camera coordinate system to 2D pixel coordinates, i.e. |
76 | |
77 | \f[p = A P_c.\f] |
78 | |
79 | The camera intrinsic matrix \f$A\f$ is composed of the focal lengths \f$f_x\f$ and \f$f_y\f$, which are |
80 | expressed in pixel units, and the principal point \f$(c_x, c_y)\f$, that is usually close to the |
81 | image center: |
82 | |
83 | \f[A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1},\f] |
84 | |
85 | and thus |
86 | |
87 | \f[s \vecthree{u}{v}{1} = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1} \vecthree{X_c}{Y_c}{Z_c}.\f] |
88 | |
89 | The matrix of intrinsic parameters does not depend on the scene viewed. So, once estimated, it can |
90 | be re-used as long as the focal length is fixed (in case of a zoom lens). Thus, if an image from the |
91 | camera is scaled by a factor, all of these parameters need to be scaled (multiplied/divided, |
92 | respectively) by the same factor. |
93 | |
94 | The joint rotation-translation matrix \f$[R|t]\f$ is the matrix product of a projective |
95 | transformation and a homogeneous transformation. The 3-by-4 projective transformation maps 3D points |
96 | represented in camera coordinates to 2D points in the image plane and represented in normalized |
97 | camera coordinates \f$x' = X_c / Z_c\f$ and \f$y' = Y_c / Z_c\f$: |
98 | |
99 | \f[Z_c \begin{bmatrix} |
100 | x' \\ |
101 | y' \\ |
102 | 1 |
103 | \end{bmatrix} = \begin{bmatrix} |
104 | 1 & 0 & 0 & 0 \\ |
105 | 0 & 1 & 0 & 0 \\ |
106 | 0 & 0 & 1 & 0 |
107 | \end{bmatrix} |
108 | \begin{bmatrix} |
109 | X_c \\ |
110 | Y_c \\ |
111 | Z_c \\ |
112 | 1 |
113 | \end{bmatrix}.\f] |
114 | |
115 | The homogeneous transformation is encoded by the extrinsic parameters \f$R\f$ and \f$t\f$ and |
116 | represents the change of basis from world coordinate system \f$w\f$ to the camera coordinate sytem |
117 | \f$c\f$. Thus, given the representation of the point \f$P\f$ in world coordinates, \f$P_w\f$, we |
118 | obtain \f$P\f$'s representation in the camera coordinate system, \f$P_c\f$, by |
119 | |
120 | \f[P_c = \begin{bmatrix} |
121 | R & t \\ |
122 | 0 & 1 |
123 | \end{bmatrix} P_w,\f] |
124 | |
125 | This homogeneous transformation is composed out of \f$R\f$, a 3-by-3 rotation matrix, and \f$t\f$, a |
126 | 3-by-1 translation vector: |
127 | |
128 | \f[\begin{bmatrix} |
129 | R & t \\ |
130 | 0 & 1 |
131 | \end{bmatrix} = \begin{bmatrix} |
132 | r_{11} & r_{12} & r_{13} & t_x \\ |
133 | r_{21} & r_{22} & r_{23} & t_y \\ |
134 | r_{31} & r_{32} & r_{33} & t_z \\ |
135 | 0 & 0 & 0 & 1 |
136 | \end{bmatrix}, |
137 | \f] |
138 | |
139 | and therefore |
140 | |
141 | \f[\begin{bmatrix} |
142 | X_c \\ |
143 | Y_c \\ |
144 | Z_c \\ |
145 | 1 |
146 | \end{bmatrix} = \begin{bmatrix} |
147 | r_{11} & r_{12} & r_{13} & t_x \\ |
148 | r_{21} & r_{22} & r_{23} & t_y \\ |
149 | r_{31} & r_{32} & r_{33} & t_z \\ |
150 | 0 & 0 & 0 & 1 |
151 | \end{bmatrix} |
152 | \begin{bmatrix} |
153 | X_w \\ |
154 | Y_w \\ |
155 | Z_w \\ |
156 | 1 |
157 | \end{bmatrix}.\f] |
158 | |
159 | Combining the projective transformation and the homogeneous transformation, we obtain the projective |
160 | transformation that maps 3D points in world coordinates into 2D points in the image plane and in |
161 | normalized camera coordinates: |
162 | |
163 | \f[Z_c \begin{bmatrix} |
164 | x' \\ |
165 | y' \\ |
166 | 1 |
167 | \end{bmatrix} = \begin{bmatrix} R|t \end{bmatrix} \begin{bmatrix} |
168 | X_w \\ |
169 | Y_w \\ |
170 | Z_w \\ |
171 | 1 |
172 | \end{bmatrix} = \begin{bmatrix} |
173 | r_{11} & r_{12} & r_{13} & t_x \\ |
174 | r_{21} & r_{22} & r_{23} & t_y \\ |
175 | r_{31} & r_{32} & r_{33} & t_z |
176 | \end{bmatrix} |
177 | \begin{bmatrix} |
178 | X_w \\ |
179 | Y_w \\ |
180 | Z_w \\ |
181 | 1 |
182 | \end{bmatrix},\f] |
183 | |
184 | with \f$x' = X_c / Z_c\f$ and \f$y' = Y_c / Z_c\f$. Putting the equations for instrincs and extrinsics together, we can write out |
185 | \f$s \; p = A \begin{bmatrix} R|t \end{bmatrix} P_w\f$ as |
186 | |
187 | \f[s \vecthree{u}{v}{1} = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1} |
188 | \begin{bmatrix} |
189 | r_{11} & r_{12} & r_{13} & t_x \\ |
190 | r_{21} & r_{22} & r_{23} & t_y \\ |
191 | r_{31} & r_{32} & r_{33} & t_z |
192 | \end{bmatrix} |
193 | \begin{bmatrix} |
194 | X_w \\ |
195 | Y_w \\ |
196 | Z_w \\ |
197 | 1 |
198 | \end{bmatrix}.\f] |
199 | |
200 | If \f$Z_c \ne 0\f$, the transformation above is equivalent to the following, |
201 | |
202 | \f[\begin{bmatrix} |
203 | u \\ |
204 | v |
205 | \end{bmatrix} = \begin{bmatrix} |
206 | f_x X_c/Z_c + c_x \\ |
207 | f_y Y_c/Z_c + c_y |
208 | \end{bmatrix}\f] |
209 | |
210 | with |
211 | |
212 | \f[\vecthree{X_c}{Y_c}{Z_c} = \begin{bmatrix} |
213 | R|t |
214 | \end{bmatrix} \begin{bmatrix} |
215 | X_w \\ |
216 | Y_w \\ |
217 | Z_w \\ |
218 | 1 |
219 | \end{bmatrix}.\f] |
220 | |
221 | The following figure illustrates the pinhole camera model. |
222 | |
223 |  |
224 | |
225 | Real lenses usually have some distortion, mostly radial distortion, and slight tangential distortion. |
226 | So, the above model is extended as: |
227 | |
228 | \f[\begin{bmatrix} |
229 | u \\ |
230 | v |
231 | \end{bmatrix} = \begin{bmatrix} |
232 | f_x x'' + c_x \\ |
233 | f_y y'' + c_y |
234 | \end{bmatrix}\f] |
235 | |
236 | where |
237 | |
238 | \f[\begin{bmatrix} |
239 | x'' \\ |
240 | y'' |
241 | \end{bmatrix} = \begin{bmatrix} |
242 | 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 \\ |
243 | y' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} + p_1 (r^2 + 2 y'^2) + 2 p_2 x' y' + s_3 r^2 + s_4 r^4 \\ |
244 | \end{bmatrix}\f] |
245 | |
246 | with |
247 | |
248 | \f[r^2 = x'^2 + y'^2\f] |
249 | |
250 | and |
251 | |
252 | \f[\begin{bmatrix} |
253 | x'\\ |
254 | y' |
255 | \end{bmatrix} = \begin{bmatrix} |
256 | X_c/Z_c \\ |
257 | Y_c/Z_c |
258 | \end{bmatrix},\f] |
259 | |
260 | if \f$Z_c \ne 0\f$. |
261 | |
262 | The distortion parameters are the radial coefficients \f$k_1\f$, \f$k_2\f$, \f$k_3\f$, \f$k_4\f$, \f$k_5\f$, and \f$k_6\f$ |
263 | ,\f$p_1\f$ and \f$p_2\f$ are the tangential distortion coefficients, and \f$s_1\f$, \f$s_2\f$, \f$s_3\f$, and \f$s_4\f$, |
264 | are the thin prism distortion coefficients. Higher-order coefficients are not considered in OpenCV. |
265 | |
266 | The next figures show two common types of radial distortion: barrel distortion |
267 | (\f$ 1 + k_1 r^2 + k_2 r^4 + k_3 r^6 \f$ monotonically decreasing) |
268 | and pincushion distortion (\f$ 1 + k_1 r^2 + k_2 r^4 + k_3 r^6 \f$ monotonically increasing). |
269 | Radial distortion is always monotonic for real lenses, |
270 | and if the estimator produces a non-monotonic result, |
271 | this should be considered a calibration failure. |
272 | More generally, radial distortion must be monotonic and the distortion function must be bijective. |
273 | A failed estimation result may look deceptively good near the image center |
274 | but will work poorly in e.g. AR/SFM applications. |
275 | The optimization method used in OpenCV camera calibration does not include these constraints as |
276 | the framework does not support the required integer programming and polynomial inequalities. |
277 | See [issue #15992](https://github.com/opencv/opencv/issues/15992) for additional information. |
278 | |
279 |  |
280 |  |
281 | |
282 | In some cases, the image sensor may be tilted in order to focus an oblique plane in front of the |
283 | camera (Scheimpflug principle). This can be useful for particle image velocimetry (PIV) or |
284 | triangulation with a laser fan. The tilt causes a perspective distortion of \f$x''\f$ and |
285 | \f$y''\f$. This distortion can be modeled in the following way, see e.g. @cite Louhichi07. |
286 | |
287 | \f[\begin{bmatrix} |
288 | u \\ |
289 | v |
290 | \end{bmatrix} = \begin{bmatrix} |
291 | f_x x''' + c_x \\ |
292 | f_y y''' + c_y |
293 | \end{bmatrix},\f] |
294 | |
295 | where |
296 | |
297 | \f[s\vecthree{x'''}{y'''}{1} = |
298 | \vecthreethree{R_{33}(\tau_x, \tau_y)}{0}{-R_{13}(\tau_x, \tau_y)} |
299 | {0}{R_{33}(\tau_x, \tau_y)}{-R_{23}(\tau_x, \tau_y)} |
300 | {0}{0}{1} R(\tau_x, \tau_y) \vecthree{x''}{y''}{1}\f] |
301 | |
302 | and the matrix \f$R(\tau_x, \tau_y)\f$ is defined by two rotations with angular parameter |
303 | \f$\tau_x\f$ and \f$\tau_y\f$, respectively, |
304 | |
305 | \f[ |
306 | R(\tau_x, \tau_y) = |
307 | \vecthreethree{\cos(\tau_y)}{0}{-\sin(\tau_y)}{0}{1}{0}{\sin(\tau_y)}{0}{\cos(\tau_y)} |
308 | \vecthreethree{1}{0}{0}{0}{\cos(\tau_x)}{\sin(\tau_x)}{0}{-\sin(\tau_x)}{\cos(\tau_x)} = |
309 | \vecthreethree{\cos(\tau_y)}{\sin(\tau_y)\sin(\tau_x)}{-\sin(\tau_y)\cos(\tau_x)} |
310 | {0}{\cos(\tau_x)}{\sin(\tau_x)} |
311 | {\sin(\tau_y)}{-\cos(\tau_y)\sin(\tau_x)}{\cos(\tau_y)\cos(\tau_x)}. |
312 | \f] |
313 | |
314 | In the functions below the coefficients are passed or returned as |
315 | |
316 | \f[(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6 [, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f] |
317 | |
318 | vector. That is, if the vector contains four elements, it means that \f$k_3=0\f$ . The distortion |
319 | coefficients do not depend on the scene viewed. Thus, they also belong to the intrinsic camera |
320 | parameters. And they remain the same regardless of the captured image resolution. If, for example, a |
321 | camera has been calibrated on images of 320 x 240 resolution, absolutely the same distortion |
322 | coefficients can be used for 640 x 480 images from the same camera while \f$f_x\f$, \f$f_y\f$, |
323 | \f$c_x\f$, and \f$c_y\f$ need to be scaled appropriately. |
324 | |
325 | The functions below use the above model to do the following: |
326 | |
327 | - Project 3D points to the image plane given intrinsic and extrinsic parameters. |
328 | - Compute extrinsic parameters given intrinsic parameters, a few 3D points, and their |
329 | projections. |
330 | - Estimate intrinsic and extrinsic camera parameters from several views of a known calibration |
331 | pattern (every view is described by several 3D-2D point correspondences). |
332 | - Estimate the relative position and orientation of the stereo camera "heads" and compute the |
333 | *rectification* transformation that makes the camera optical axes parallel. |
334 | |
335 | <B> Homogeneous Coordinates </B><br> |
336 | Homogeneous Coordinates are a system of coordinates that are used in projective geometry. Their use |
337 | allows to represent points at infinity by finite coordinates and simplifies formulas when compared |
338 | to the cartesian counterparts, e.g. they have the advantage that affine transformations can be |
339 | expressed as linear homogeneous transformation. |
340 | |
341 | One obtains the homogeneous vector \f$P_h\f$ by appending a 1 along an n-dimensional cartesian |
342 | vector \f$P\f$ e.g. for a 3D cartesian vector the mapping \f$P \rightarrow P_h\f$ is: |
343 | |
344 | \f[\begin{bmatrix} |
345 | X \\ |
346 | Y \\ |
347 | Z |
348 | \end{bmatrix} \rightarrow \begin{bmatrix} |
349 | X \\ |
350 | Y \\ |
351 | Z \\ |
352 | 1 |
353 | \end{bmatrix}.\f] |
354 | |
355 | For the inverse mapping \f$P_h \rightarrow P\f$, one divides all elements of the homogeneous vector |
356 | by its last element, e.g. for a 3D homogeneous vector one gets its 2D cartesian counterpart by: |
357 | |
358 | \f[\begin{bmatrix} |
359 | X \\ |
360 | Y \\ |
361 | W |
362 | \end{bmatrix} \rightarrow \begin{bmatrix} |
363 | X / W \\ |
364 | Y / W |
365 | \end{bmatrix},\f] |
366 | |
367 | if \f$W \ne 0\f$. |
368 | |
369 | Due to this mapping, all multiples \f$k P_h\f$, for \f$k \ne 0\f$, of a homogeneous point represent |
370 | the same point \f$P_h\f$. An intuitive understanding of this property is that under a projective |
371 | transformation, all multiples of \f$P_h\f$ are mapped to the same point. This is the physical |
372 | observation one does for pinhole cameras, as all points along a ray through the camera's pinhole are |
373 | projected to the same image point, e.g. all points along the red ray in the image of the pinhole |
374 | camera model above would be mapped to the same image coordinate. This property is also the source |
375 | for the scale ambiguity s in the equation of the pinhole camera model. |
376 | |
377 | As mentioned, by using homogeneous coordinates we can express any change of basis parameterized by |
378 | \f$R\f$ and \f$t\f$ as a linear transformation, e.g. for the change of basis from coordinate system |
379 | 0 to coordinate system 1 becomes: |
380 | |
381 | \f[P_1 = R P_0 + t \rightarrow P_{h_1} = \begin{bmatrix} |
382 | R & t \\ |
383 | 0 & 1 |
384 | \end{bmatrix} P_{h_0}.\f] |
385 | |
386 | @note |
387 | - Many functions in this module take a camera intrinsic matrix as an input parameter. Although all |
388 | functions assume the same structure of this parameter, they may name it differently. The |
389 | parameter's description, however, will be clear in that a camera intrinsic matrix with the structure |
390 | shown above is required. |
391 | - A calibration sample for 3 cameras in a horizontal position can be found at |
392 | opencv_source_code/samples/cpp/3calibration.cpp |
393 | - A calibration sample based on a sequence of images can be found at |
394 | opencv_source_code/samples/cpp/calibration.cpp |
395 | - A calibration sample in order to do 3D reconstruction can be found at |
396 | opencv_source_code/samples/cpp/build3dmodel.cpp |
397 | - A calibration example on stereo calibration can be found at |
398 | opencv_source_code/samples/cpp/stereo_calib.cpp |
399 | - A calibration example on stereo matching can be found at |
400 | opencv_source_code/samples/cpp/stereo_match.cpp |
401 | - (Python) A camera calibration sample can be found at |
402 | opencv_source_code/samples/python/calibrate.py |
403 | |
404 | @{ |
405 | @defgroup calib3d_fisheye Fisheye camera model |
406 | |
407 | Definitions: Let P be a point in 3D of coordinates X in the world reference frame (stored in the |
408 | matrix X) The coordinate vector of P in the camera reference frame is: |
409 | |
410 | \f[Xc = R X + T\f] |
411 | |
412 | where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); call x, y |
413 | and z the 3 coordinates of Xc: |
414 | |
415 | \f[\begin{array}{l} x = Xc_1 \\ y = Xc_2 \\ z = Xc_3 \end{array} \f] |
416 | |
417 | The pinhole projection coordinates of P is [a; b] where |
418 | |
419 | \f[\begin{array}{l} a = x / z \ and \ b = y / z \\ r^2 = a^2 + b^2 \\ \theta = atan(r) \end{array} \f] |
420 | |
421 | Fisheye distortion: |
422 | |
423 | \f[\theta_d = \theta (1 + k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8)\f] |
424 | |
425 | The distorted point coordinates are [x'; y'] where |
426 | |
427 | \f[\begin{array}{l} x' = (\theta_d / r) a \\ y' = (\theta_d / r) b \end{array} \f] |
428 | |
429 | Finally, conversion into pixel coordinates: The final pixel coordinates vector [u; v] where: |
430 | |
431 | \f[\begin{array}{l} u = f_x (x' + \alpha y') + c_x \\ |
432 | v = f_y y' + c_y \end{array} \f] |
433 | |
434 | Summary: |
435 | Generic camera model @cite Kannala2006 with perspective projection and without distortion correction |
436 | |
437 | @} |
438 | */ |
439 | |
440 | namespace cv |
441 | { |
442 | |
443 | //! @addtogroup calib3d |
444 | //! @{ |
445 | |
446 | //! type of the robust estimation algorithm |
447 | enum { LMEDS = 4, //!< least-median of squares algorithm |
448 | RANSAC = 8, //!< RANSAC algorithm |
449 | RHO = 16, //!< RHO algorithm |
450 | USAC_DEFAULT = 32, //!< USAC algorithm, default settings |
451 | USAC_PARALLEL = 33, //!< USAC, parallel version |
452 | USAC_FM_8PTS = 34, //!< USAC, fundamental matrix 8 points |
453 | USAC_FAST = 35, //!< USAC, fast settings |
454 | USAC_ACCURATE = 36, //!< USAC, accurate settings |
455 | USAC_PROSAC = 37, //!< USAC, sorted points, runs PROSAC |
456 | USAC_MAGSAC = 38 //!< USAC, runs MAGSAC++ |
457 | }; |
458 | |
459 | enum SolvePnPMethod { |
460 | SOLVEPNP_ITERATIVE = 0, //!< Pose refinement using non-linear Levenberg-Marquardt minimization scheme @cite Madsen04 @cite Eade13 \n |
461 | //!< Initial solution for non-planar "objectPoints" needs at least 6 points and uses the DLT algorithm. \n |
462 | //!< Initial solution for planar "objectPoints" needs at least 4 points and uses pose from homography decomposition. |
463 | SOLVEPNP_EPNP = 1, //!< EPnP: Efficient Perspective-n-Point Camera Pose Estimation @cite lepetit2009epnp |
464 | SOLVEPNP_P3P = 2, //!< Complete Solution Classification for the Perspective-Three-Point Problem @cite gao2003complete |
465 | SOLVEPNP_DLS = 3, //!< **Broken implementation. Using this flag will fallback to EPnP.** \n |
466 | //!< A Direct Least-Squares (DLS) Method for PnP @cite hesch2011direct |
467 | SOLVEPNP_UPNP = 4, //!< **Broken implementation. Using this flag will fallback to EPnP.** \n |
468 | //!< Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation @cite penate2013exhaustive |
469 | SOLVEPNP_AP3P = 5, //!< An Efficient Algebraic Solution to the Perspective-Three-Point Problem @cite Ke17 |
470 | SOLVEPNP_IPPE = 6, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n |
471 | //!< Object points must be coplanar. |
472 | SOLVEPNP_IPPE_SQUARE = 7, //!< Infinitesimal Plane-Based Pose Estimation @cite Collins14 \n |
473 | //!< This is a special case suitable for marker pose estimation.\n |
474 | //!< 4 coplanar object points must be defined in the following order: |
475 | //!< - point 0: [-squareLength / 2, squareLength / 2, 0] |
476 | //!< - point 1: [ squareLength / 2, squareLength / 2, 0] |
477 | //!< - point 2: [ squareLength / 2, -squareLength / 2, 0] |
478 | //!< - point 3: [-squareLength / 2, -squareLength / 2, 0] |
479 | SOLVEPNP_SQPNP = 8, //!< SQPnP: A Consistently Fast and Globally OptimalSolution to the Perspective-n-Point Problem @cite Terzakis2020SQPnP |
480 | #ifndef CV_DOXYGEN |
481 | SOLVEPNP_MAX_COUNT //!< Used for count |
482 | #endif |
483 | }; |
484 | |
485 | enum { CALIB_CB_ADAPTIVE_THRESH = 1, |
486 | CALIB_CB_NORMALIZE_IMAGE = 2, |
487 | CALIB_CB_FILTER_QUADS = 4, |
488 | CALIB_CB_FAST_CHECK = 8, |
489 | CALIB_CB_EXHAUSTIVE = 16, |
490 | CALIB_CB_ACCURACY = 32, |
491 | CALIB_CB_LARGER = 64, |
492 | CALIB_CB_MARKER = 128, |
493 | CALIB_CB_PLAIN = 256 |
494 | }; |
495 | |
496 | enum { CALIB_CB_SYMMETRIC_GRID = 1, |
497 | CALIB_CB_ASYMMETRIC_GRID = 2, |
498 | CALIB_CB_CLUSTERING = 4 |
499 | }; |
500 | |
501 | enum { CALIB_NINTRINSIC = 18, |
502 | CALIB_USE_INTRINSIC_GUESS = 0x00001, |
503 | CALIB_FIX_ASPECT_RATIO = 0x00002, |
504 | CALIB_FIX_PRINCIPAL_POINT = 0x00004, |
505 | CALIB_ZERO_TANGENT_DIST = 0x00008, |
506 | CALIB_FIX_FOCAL_LENGTH = 0x00010, |
507 | CALIB_FIX_K1 = 0x00020, |
508 | CALIB_FIX_K2 = 0x00040, |
509 | CALIB_FIX_K3 = 0x00080, |
510 | CALIB_FIX_K4 = 0x00800, |
511 | CALIB_FIX_K5 = 0x01000, |
512 | CALIB_FIX_K6 = 0x02000, |
513 | CALIB_RATIONAL_MODEL = 0x04000, |
514 | CALIB_THIN_PRISM_MODEL = 0x08000, |
515 | CALIB_FIX_S1_S2_S3_S4 = 0x10000, |
516 | CALIB_TILTED_MODEL = 0x40000, |
517 | CALIB_FIX_TAUX_TAUY = 0x80000, |
518 | CALIB_USE_QR = 0x100000, //!< use QR instead of SVD decomposition for solving. Faster but potentially less precise |
519 | CALIB_FIX_TANGENT_DIST = 0x200000, |
520 | // only for stereo |
521 | CALIB_FIX_INTRINSIC = 0x00100, |
522 | CALIB_SAME_FOCAL_LENGTH = 0x00200, |
523 | // for stereo rectification |
524 | CALIB_ZERO_DISPARITY = 0x00400, |
525 | CALIB_USE_LU = (1 << 17), //!< use LU instead of SVD decomposition for solving. much faster but potentially less precise |
526 | CALIB_USE_EXTRINSIC_GUESS = (1 << 22) //!< for stereoCalibrate |
527 | }; |
528 | |
529 | //! the algorithm for finding fundamental matrix |
530 | enum { FM_7POINT = 1, //!< 7-point algorithm |
531 | FM_8POINT = 2, //!< 8-point algorithm |
532 | FM_LMEDS = 4, //!< least-median algorithm. 7-point algorithm is used. |
533 | FM_RANSAC = 8 //!< RANSAC algorithm. It needs at least 15 points. 7-point algorithm is used. |
534 | }; |
535 | |
536 | enum HandEyeCalibrationMethod |
537 | { |
538 | CALIB_HAND_EYE_TSAI = 0, //!< A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration @cite Tsai89 |
539 | CALIB_HAND_EYE_PARK = 1, //!< Robot Sensor Calibration: Solving AX = XB on the Euclidean Group @cite Park94 |
540 | CALIB_HAND_EYE_HORAUD = 2, //!< Hand-eye Calibration @cite Horaud95 |
541 | CALIB_HAND_EYE_ANDREFF = 3, //!< On-line Hand-Eye Calibration @cite Andreff99 |
542 | CALIB_HAND_EYE_DANIILIDIS = 4 //!< Hand-Eye Calibration Using Dual Quaternions @cite Daniilidis98 |
543 | }; |
544 | |
545 | enum RobotWorldHandEyeCalibrationMethod |
546 | { |
547 | CALIB_ROBOT_WORLD_HAND_EYE_SHAH = 0, //!< Solving the robot-world/hand-eye calibration problem using the kronecker product @cite Shah2013SolvingTR |
548 | CALIB_ROBOT_WORLD_HAND_EYE_LI = 1 //!< Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product @cite Li2010SimultaneousRA |
549 | }; |
550 | |
551 | enum SamplingMethod { SAMPLING_UNIFORM=0, SAMPLING_PROGRESSIVE_NAPSAC=1, SAMPLING_NAPSAC=2, |
552 | SAMPLING_PROSAC=3 }; |
553 | enum LocalOptimMethod {LOCAL_OPTIM_NULL=0, LOCAL_OPTIM_INNER_LO=1, LOCAL_OPTIM_INNER_AND_ITER_LO=2, |
554 | LOCAL_OPTIM_GC=3, LOCAL_OPTIM_SIGMA=4}; |
555 | enum ScoreMethod {SCORE_METHOD_RANSAC=0, SCORE_METHOD_MSAC=1, SCORE_METHOD_MAGSAC=2, SCORE_METHOD_LMEDS=3}; |
556 | enum NeighborSearchMethod { NEIGH_FLANN_KNN=0, NEIGH_GRID=1, NEIGH_FLANN_RADIUS=2 }; |
557 | enum PolishingMethod { NONE_POLISHER=0, LSQ_POLISHER=1, MAGSAC=2, COV_POLISHER=3 }; |
558 | |
559 | struct CV_EXPORTS_W_SIMPLE UsacParams |
560 | { // in alphabetical order |
561 | CV_WRAP UsacParams(); |
562 | CV_PROP_RW double confidence; |
563 | CV_PROP_RW bool isParallel; |
564 | CV_PROP_RW int loIterations; |
565 | CV_PROP_RW LocalOptimMethod loMethod; |
566 | CV_PROP_RW int loSampleSize; |
567 | CV_PROP_RW int maxIterations; |
568 | CV_PROP_RW NeighborSearchMethod ; |
569 | CV_PROP_RW int randomGeneratorState; |
570 | CV_PROP_RW SamplingMethod sampler; |
571 | CV_PROP_RW ScoreMethod score; |
572 | CV_PROP_RW double threshold; |
573 | CV_PROP_RW PolishingMethod final_polisher; |
574 | CV_PROP_RW int final_polisher_iterations; |
575 | }; |
576 | |
577 | /** @brief Converts a rotation matrix to a rotation vector or vice versa. |
578 | |
579 | @param src Input rotation vector (3x1 or 1x3) or rotation matrix (3x3). |
580 | @param dst Output rotation matrix (3x3) or rotation vector (3x1 or 1x3), respectively. |
581 | @param jacobian Optional output Jacobian matrix, 3x9 or 9x3, which is a matrix of partial |
582 | derivatives of the output array components with respect to the input array components. |
583 | |
584 | \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] |
585 | |
586 | Inverse transformation can be also done easily, since |
587 | |
588 | \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] |
589 | |
590 | A rotation vector is a convenient and most compact representation of a rotation matrix (since any |
591 | rotation matrix has just 3 degrees of freedom). The representation is used in the global 3D geometry |
592 | optimization procedures like @ref calibrateCamera, @ref stereoCalibrate, or @ref solvePnP . |
593 | |
594 | @note More information about the computation of the derivative of a 3D rotation matrix with respect to its exponential coordinate |
595 | can be found in: |
596 | - A Compact Formula for the Derivative of a 3-D Rotation in Exponential Coordinates, Guillermo Gallego, Anthony J. Yezzi @cite Gallego2014ACF |
597 | |
598 | @note Useful information on SE(3) and Lie Groups can be found in: |
599 | - A tutorial on SE(3) transformation parameterizations and on-manifold optimization, Jose-Luis Blanco @cite blanco2010tutorial |
600 | - Lie Groups for 2D and 3D Transformation, Ethan Eade @cite Eade17 |
601 | - A micro Lie theory for state estimation in robotics, Joan Solà , Jérémie Deray, Dinesh Atchuthan @cite Sol2018AML |
602 | */ |
603 | CV_EXPORTS_W void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() ); |
604 | |
605 | |
606 | |
607 | /** Levenberg-Marquardt solver. Starting with the specified vector of parameters it |
608 | optimizes the target vector criteria "err" |
609 | (finds local minima of each target vector component absolute value). |
610 | |
611 | When needed, it calls user-provided callback. |
612 | */ |
613 | class CV_EXPORTS LMSolver : public Algorithm |
614 | { |
615 | public: |
616 | class CV_EXPORTS Callback |
617 | { |
618 | public: |
619 | virtual ~Callback() {} |
620 | /** |
621 | computes error and Jacobian for the specified vector of parameters |
622 | |
623 | @param param the current vector of parameters |
624 | @param err output vector of errors: err_i = actual_f_i - ideal_f_i |
625 | @param J output Jacobian: J_ij = d(ideal_f_i)/d(param_j) |
626 | |
627 | when J=noArray(), it means that it does not need to be computed. |
628 | Dimensionality of error vector and param vector can be different. |
629 | The callback should explicitly allocate (with "create" method) each output array |
630 | (unless it's noArray()). |
631 | */ |
632 | virtual bool compute(InputArray param, OutputArray err, OutputArray J) const = 0; |
633 | }; |
634 | |
635 | /** |
636 | Runs Levenberg-Marquardt algorithm using the passed vector of parameters as the start point. |
637 | The final vector of parameters (whether the algorithm converged or not) is stored at the same |
638 | vector. The method returns the number of iterations used. If it's equal to the previously specified |
639 | maxIters, there is a big chance the algorithm did not converge. |
640 | |
641 | @param param initial/final vector of parameters. |
642 | |
643 | Note that the dimensionality of parameter space is defined by the size of param vector, |
644 | and the dimensionality of optimized criteria is defined by the size of err vector |
645 | computed by the callback. |
646 | */ |
647 | virtual int run(InputOutputArray param) const = 0; |
648 | |
649 | /** |
650 | Sets the maximum number of iterations |
651 | @param maxIters the number of iterations |
652 | */ |
653 | virtual void setMaxIters(int maxIters) = 0; |
654 | /** |
655 | Retrieves the current maximum number of iterations |
656 | */ |
657 | virtual int getMaxIters() const = 0; |
658 | |
659 | /** |
660 | Creates Levenberg-Marquard solver |
661 | |
662 | @param cb callback |
663 | @param maxIters maximum number of iterations that can be further |
664 | modified using setMaxIters() method. |
665 | */ |
666 | static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters); |
667 | static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps); |
668 | }; |
669 | |
670 | |
671 | |
672 | /** @example samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp |
673 | An example program about pose estimation from coplanar points |
674 | |
675 | Check @ref tutorial_homography "the corresponding tutorial" for more details |
676 | */ |
677 | |
678 | /** @brief Finds a perspective transformation between two planes. |
679 | |
680 | @param srcPoints Coordinates of the points in the original plane, a matrix of the type CV_32FC2 |
681 | or vector\<Point2f\> . |
682 | @param dstPoints Coordinates of the points in the target plane, a matrix of the type CV_32FC2 or |
683 | a vector\<Point2f\> . |
684 | @param method Method used to compute a homography matrix. The following methods are possible: |
685 | - **0** - a regular method using all the points, i.e., the least squares method |
686 | - @ref RANSAC - RANSAC-based robust method |
687 | - @ref LMEDS - Least-Median robust method |
688 | - @ref RHO - PROSAC-based robust method |
689 | @param ransacReprojThreshold Maximum allowed reprojection error to treat a point pair as an inlier |
690 | (used in the RANSAC and RHO methods only). That is, if |
691 | \f[\| \texttt{dstPoints} _i - \texttt{convertPointsHomogeneous} ( \texttt{H} \cdot \texttt{srcPoints} _i) \|_2 > \texttt{ransacReprojThreshold}\f] |
692 | then the point \f$i\f$ is considered as an outlier. If srcPoints and dstPoints are measured in pixels, |
693 | it usually makes sense to set this parameter somewhere in the range of 1 to 10. |
694 | @param mask Optional output mask set by a robust method ( RANSAC or LMeDS ). Note that the input |
695 | mask values are ignored. |
696 | @param maxIters The maximum number of RANSAC iterations. |
697 | @param confidence Confidence level, between 0 and 1. |
698 | |
699 | The function finds and returns the perspective transformation \f$H\f$ between the source and the |
700 | destination planes: |
701 | |
702 | \f[s_i \vecthree{x'_i}{y'_i}{1} \sim H \vecthree{x_i}{y_i}{1}\f] |
703 | |
704 | so that the back-projection error |
705 | |
706 | \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] |
707 | |
708 | is minimized. If the parameter method is set to the default value 0, the function uses all the point |
709 | pairs to compute an initial homography estimate with a simple least-squares scheme. |
710 | |
711 | However, if not all of the point pairs ( \f$srcPoints_i\f$, \f$dstPoints_i\f$ ) fit the rigid perspective |
712 | transformation (that is, there are some outliers), this initial estimate will be poor. In this case, |
713 | you can use one of the three robust methods. The methods RANSAC, LMeDS and RHO try many different |
714 | random subsets of the corresponding point pairs (of four pairs each, collinear pairs are discarded), estimate the homography matrix |
715 | using this subset and a simple least-squares algorithm, and then compute the quality/goodness of the |
716 | computed homography (which is the number of inliers for RANSAC or the least median re-projection error for |
717 | LMeDS). The best subset is then used to produce the initial estimate of the homography matrix and |
718 | the mask of inliers/outliers. |
719 | |
720 | Regardless of the method, robust or not, the computed homography matrix is refined further (using |
721 | inliers only in case of a robust method) with the Levenberg-Marquardt method to reduce the |
722 | re-projection error even more. |
723 | |
724 | The methods RANSAC and RHO can handle practically any ratio of outliers but need a threshold to |
725 | distinguish inliers from outliers. The method LMeDS does not need any threshold but it works |
726 | correctly only when there are more than 50% of inliers. Finally, if there are no outliers and the |
727 | noise is rather small, use the default method (method=0). |
728 | |
729 | The function is used to find initial intrinsic and extrinsic matrices. Homography matrix is |
730 | determined up to a scale. If \f$h_{33}\f$ is non-zero, the matrix is normalized so that \f$h_{33}=1\f$. |
731 | @note Whenever an \f$H\f$ matrix cannot be estimated, an empty one will be returned. |
732 | |
733 | @sa |
734 | getAffineTransform, estimateAffine2D, estimateAffinePartial2D, getPerspectiveTransform, warpPerspective, |
735 | perspectiveTransform |
736 | */ |
737 | CV_EXPORTS_W Mat findHomography( InputArray srcPoints, InputArray dstPoints, |
738 | int method = 0, double ransacReprojThreshold = 3, |
739 | OutputArray mask=noArray(), const int maxIters = 2000, |
740 | const double confidence = 0.995); |
741 | |
742 | /** @overload */ |
743 | CV_EXPORTS Mat findHomography( InputArray srcPoints, InputArray dstPoints, |
744 | OutputArray mask, int method = 0, double ransacReprojThreshold = 3 ); |
745 | |
746 | |
747 | CV_EXPORTS_W Mat findHomography(InputArray srcPoints, InputArray dstPoints, OutputArray mask, |
748 | const UsacParams ¶ms); |
749 | |
750 | /** @brief Computes an RQ decomposition of 3x3 matrices. |
751 | |
752 | @param src 3x3 input matrix. |
753 | @param mtxR Output 3x3 upper-triangular matrix. |
754 | @param mtxQ Output 3x3 orthogonal matrix. |
755 | @param Qx Optional output 3x3 rotation matrix around x-axis. |
756 | @param Qy Optional output 3x3 rotation matrix around y-axis. |
757 | @param Qz Optional output 3x3 rotation matrix around z-axis. |
758 | |
759 | The function computes a RQ decomposition using the given rotations. This function is used in |
760 | #decomposeProjectionMatrix to decompose the left 3x3 submatrix of a projection matrix into a camera |
761 | and a rotation matrix. |
762 | |
763 | It optionally returns three rotation matrices, one for each axis, and the three Euler angles in |
764 | degrees (as the return value) that could be used in OpenGL. Note, there is always more than one |
765 | sequence of rotations about the three principal axes that results in the same orientation of an |
766 | object, e.g. see @cite Slabaugh . Returned three rotation matrices and corresponding three Euler angles |
767 | are only one of the possible solutions. |
768 | */ |
769 | CV_EXPORTS_W Vec3d RQDecomp3x3( InputArray src, OutputArray mtxR, OutputArray mtxQ, |
770 | OutputArray Qx = noArray(), |
771 | OutputArray Qy = noArray(), |
772 | OutputArray Qz = noArray()); |
773 | |
774 | /** @brief Decomposes a projection matrix into a rotation matrix and a camera intrinsic matrix. |
775 | |
776 | @param projMatrix 3x4 input projection matrix P. |
777 | @param cameraMatrix Output 3x3 camera intrinsic matrix \f$\cameramatrix{A}\f$. |
778 | @param rotMatrix Output 3x3 external rotation matrix R. |
779 | @param transVect Output 4x1 translation vector T. |
780 | @param rotMatrixX Optional 3x3 rotation matrix around x-axis. |
781 | @param rotMatrixY Optional 3x3 rotation matrix around y-axis. |
782 | @param rotMatrixZ Optional 3x3 rotation matrix around z-axis. |
783 | @param eulerAngles Optional three-element vector containing three Euler angles of rotation in |
784 | degrees. |
785 | |
786 | The function computes a decomposition of a projection matrix into a calibration and a rotation |
787 | matrix and the position of a camera. |
788 | |
789 | It optionally returns three rotation matrices, one for each axis, and three Euler angles that could |
790 | be used in OpenGL. Note, there is always more than one sequence of rotations about the three |
791 | principal axes that results in the same orientation of an object, e.g. see @cite Slabaugh . Returned |
792 | three rotation matrices and corresponding three Euler angles are only one of the possible solutions. |
793 | |
794 | The function is based on #RQDecomp3x3 . |
795 | */ |
796 | CV_EXPORTS_W void decomposeProjectionMatrix( InputArray projMatrix, OutputArray cameraMatrix, |
797 | OutputArray rotMatrix, OutputArray transVect, |
798 | OutputArray rotMatrixX = noArray(), |
799 | OutputArray rotMatrixY = noArray(), |
800 | OutputArray rotMatrixZ = noArray(), |
801 | OutputArray eulerAngles =noArray() ); |
802 | |
803 | /** @brief Computes partial derivatives of the matrix product for each multiplied matrix. |
804 | |
805 | @param A First multiplied matrix. |
806 | @param B Second multiplied matrix. |
807 | @param dABdA First output derivative matrix d(A\*B)/dA of size |
808 | \f$\texttt{A.rows*B.cols} \times {A.rows*A.cols}\f$ . |
809 | @param dABdB Second output derivative matrix d(A\*B)/dB of size |
810 | \f$\texttt{A.rows*B.cols} \times {B.rows*B.cols}\f$ . |
811 | |
812 | The function computes partial derivatives of the elements of the matrix product \f$A*B\f$ with regard to |
813 | the elements of each of the two input matrices. The function is used to compute the Jacobian |
814 | matrices in #stereoCalibrate but can also be used in any other similar optimization function. |
815 | */ |
816 | CV_EXPORTS_W void matMulDeriv( InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB ); |
817 | |
818 | /** @brief Combines two rotation-and-shift transformations. |
819 | |
820 | @param rvec1 First rotation vector. |
821 | @param tvec1 First translation vector. |
822 | @param rvec2 Second rotation vector. |
823 | @param tvec2 Second translation vector. |
824 | @param rvec3 Output rotation vector of the superposition. |
825 | @param tvec3 Output translation vector of the superposition. |
826 | @param dr3dr1 Optional output derivative of rvec3 with regard to rvec1 |
827 | @param dr3dt1 Optional output derivative of rvec3 with regard to tvec1 |
828 | @param dr3dr2 Optional output derivative of rvec3 with regard to rvec2 |
829 | @param dr3dt2 Optional output derivative of rvec3 with regard to tvec2 |
830 | @param dt3dr1 Optional output derivative of tvec3 with regard to rvec1 |
831 | @param dt3dt1 Optional output derivative of tvec3 with regard to tvec1 |
832 | @param dt3dr2 Optional output derivative of tvec3 with regard to rvec2 |
833 | @param dt3dt2 Optional output derivative of tvec3 with regard to tvec2 |
834 | |
835 | The functions compute: |
836 | |
837 | \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] |
838 | |
839 | where \f$\mathrm{rodrigues}\f$ denotes a rotation vector to a rotation matrix transformation, and |
840 | \f$\mathrm{rodrigues}^{-1}\f$ denotes the inverse transformation. See #Rodrigues for details. |
841 | |
842 | Also, the functions can compute the derivatives of the output vectors with regards to the input |
843 | vectors (see #matMulDeriv ). The functions are used inside #stereoCalibrate but can also be used in |
844 | your own code where Levenberg-Marquardt or another gradient-based solver is used to optimize a |
845 | function that contains a matrix multiplication. |
846 | */ |
847 | CV_EXPORTS_W void composeRT( InputArray rvec1, InputArray tvec1, |
848 | InputArray rvec2, InputArray tvec2, |
849 | OutputArray rvec3, OutputArray tvec3, |
850 | OutputArray dr3dr1 = noArray(), OutputArray dr3dt1 = noArray(), |
851 | OutputArray dr3dr2 = noArray(), OutputArray dr3dt2 = noArray(), |
852 | OutputArray dt3dr1 = noArray(), OutputArray dt3dt1 = noArray(), |
853 | OutputArray dt3dr2 = noArray(), OutputArray dt3dt2 = noArray() ); |
854 | |
855 | /** @brief Projects 3D points to an image plane. |
856 | |
857 | @param objectPoints Array of object points expressed wrt. the world coordinate frame. A 3xN/Nx3 |
858 | 1-channel or 1xN/Nx1 3-channel (or vector\<Point3f\> ), where N is the number of points in the view. |
859 | @param rvec The rotation vector (@ref Rodrigues) that, together with tvec, performs a change of |
860 | basis from world to camera coordinate system, see @ref calibrateCamera for details. |
861 | @param tvec The translation vector, see parameter description above. |
862 | @param cameraMatrix Camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
863 | @param distCoeffs Input vector of distortion coefficients |
864 | \f$\distcoeffs\f$ . If the vector is empty, the zero distortion coefficients are assumed. |
865 | @param imagePoints Output array of image points, 1xN/Nx1 2-channel, or |
866 | vector\<Point2f\> . |
867 | @param jacobian Optional output 2Nx(10+\<numDistCoeffs\>) jacobian matrix of derivatives of image |
868 | points with respect to components of the rotation vector, translation vector, focal lengths, |
869 | coordinates of the principal point and the distortion coefficients. In the old interface different |
870 | components of the jacobian are returned via different output parameters. |
871 | @param aspectRatio Optional "fixed aspect ratio" parameter. If the parameter is not 0, the |
872 | function assumes that the aspect ratio (\f$f_x / f_y\f$) is fixed and correspondingly adjusts the |
873 | jacobian matrix. |
874 | |
875 | The function computes the 2D projections of 3D points to the image plane, given intrinsic and |
876 | extrinsic camera parameters. Optionally, the function computes Jacobians -matrices of partial |
877 | derivatives of image points coordinates (as functions of all the input parameters) with respect to |
878 | the particular parameters, intrinsic and/or extrinsic. The Jacobians are used during the global |
879 | optimization in @ref calibrateCamera, @ref solvePnP, and @ref stereoCalibrate. The function itself |
880 | can also be used to compute a re-projection error, given the current intrinsic and extrinsic |
881 | parameters. |
882 | |
883 | @note By setting rvec = tvec = \f$[0, 0, 0]\f$, or by setting cameraMatrix to a 3x3 identity matrix, |
884 | or by passing zero distortion coefficients, one can get various useful partial cases of the |
885 | function. This means, one can compute the distorted coordinates for a sparse set of points or apply |
886 | a perspective transformation (and also compute the derivatives) in the ideal zero-distortion setup. |
887 | */ |
888 | CV_EXPORTS_W void projectPoints( InputArray objectPoints, |
889 | InputArray rvec, InputArray tvec, |
890 | InputArray cameraMatrix, InputArray distCoeffs, |
891 | OutputArray imagePoints, |
892 | OutputArray jacobian = noArray(), |
893 | double aspectRatio = 0 ); |
894 | |
895 | /** @example samples/cpp/tutorial_code/features2D/Homography/homography_from_camera_displacement.cpp |
896 | An example program about homography from the camera displacement |
897 | |
898 | Check @ref tutorial_homography "the corresponding tutorial" for more details |
899 | */ |
900 | |
901 | /** @brief Finds an object pose from 3D-2D point correspondences. |
902 | |
903 | @see @ref calib3d_solvePnP |
904 | |
905 | This function returns the rotation and the translation vectors that transform a 3D point expressed in the object |
906 | coordinate frame to the camera coordinate frame, using different methods: |
907 | - P3P methods (@ref SOLVEPNP_P3P, @ref SOLVEPNP_AP3P): need 4 input points to return a unique solution. |
908 | - @ref SOLVEPNP_IPPE Input points must be >= 4 and object points must be coplanar. |
909 | - @ref SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation. |
910 | Number of input points must be 4. Object points must be defined in the following order: |
911 | - point 0: [-squareLength / 2, squareLength / 2, 0] |
912 | - point 1: [ squareLength / 2, squareLength / 2, 0] |
913 | - point 2: [ squareLength / 2, -squareLength / 2, 0] |
914 | - point 3: [-squareLength / 2, -squareLength / 2, 0] |
915 | - for all the other flags, number of input points must be >= 4 and object points can be in any configuration. |
916 | |
917 | @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or |
918 | 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3d\> can be also passed here. |
919 | @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, |
920 | where N is the number of points. vector\<Point2d\> can be also passed here. |
921 | @param cameraMatrix Input camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
922 | @param distCoeffs Input vector of distortion coefficients |
923 | \f$\distcoeffs\f$. If the vector is NULL/empty, the zero distortion coefficients are |
924 | assumed. |
925 | @param rvec Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from |
926 | the model coordinate system to the camera coordinate system. |
927 | @param tvec Output translation vector. |
928 | @param useExtrinsicGuess Parameter used for #SOLVEPNP_ITERATIVE. If true (1), the function uses |
929 | the provided rvec and tvec values as initial approximations of the rotation and translation |
930 | vectors, respectively, and further optimizes them. |
931 | @param flags Method for solving a PnP problem: see @ref calib3d_solvePnP_flags |
932 | |
933 | More information about Perspective-n-Points is described in @ref calib3d_solvePnP |
934 | |
935 | @note |
936 | - An example of how to use solvePnP for planar augmented reality can be found at |
937 | opencv_source_code/samples/python/plane_ar.py |
938 | - If you are using Python: |
939 | - Numpy array slices won't work as input because solvePnP requires contiguous |
940 | arrays (enforced by the assertion using cv::Mat::checkVector() around line 55 of |
941 | modules/calib3d/src/solvepnp.cpp version 2.4.9) |
942 | - The P3P algorithm requires image points to be in an array of shape (N,1,2) due |
943 | to its calling of #undistortPoints (around line 75 of modules/calib3d/src/solvepnp.cpp version 2.4.9) |
944 | which requires 2-channel information. |
945 | - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of |
946 | it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints = |
947 | np.ascontiguousarray(D[:,:2]).reshape((N,1,2)) |
948 | - The methods @ref SOLVEPNP_DLS and @ref SOLVEPNP_UPNP cannot be used as the current implementations are |
949 | unstable and sometimes give completely wrong results. If you pass one of these two |
950 | flags, @ref SOLVEPNP_EPNP method will be used instead. |
951 | - The minimum number of points is 4 in the general case. In the case of @ref SOLVEPNP_P3P and @ref SOLVEPNP_AP3P |
952 | methods, it is required to use exactly 4 points (the first 3 points are used to estimate all the solutions |
953 | of the P3P problem, the last one is used to retain the best solution that minimizes the reprojection error). |
954 | - With @ref SOLVEPNP_ITERATIVE method and `useExtrinsicGuess=true`, the minimum number of points is 3 (3 points |
955 | are sufficient to compute a pose but there are up to 4 solutions). The initial solution should be close to the |
956 | global solution to converge. |
957 | - With @ref SOLVEPNP_IPPE input points must be >= 4 and object points must be coplanar. |
958 | - With @ref SOLVEPNP_IPPE_SQUARE this is a special case suitable for marker pose estimation. |
959 | Number of input points must be 4. Object points must be defined in the following order: |
960 | - point 0: [-squareLength / 2, squareLength / 2, 0] |
961 | - point 1: [ squareLength / 2, squareLength / 2, 0] |
962 | - point 2: [ squareLength / 2, -squareLength / 2, 0] |
963 | - point 3: [-squareLength / 2, -squareLength / 2, 0] |
964 | - With @ref SOLVEPNP_SQPNP input points must be >= 3 |
965 | */ |
966 | CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, |
967 | InputArray cameraMatrix, InputArray distCoeffs, |
968 | OutputArray rvec, OutputArray tvec, |
969 | bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE ); |
970 | |
971 | /** @brief Finds an object pose from 3D-2D point correspondences using the RANSAC scheme. |
972 | |
973 | @see @ref calib3d_solvePnP |
974 | |
975 | @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or |
976 | 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3d\> can be also passed here. |
977 | @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, |
978 | where N is the number of points. vector\<Point2d\> can be also passed here. |
979 | @param cameraMatrix Input camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
980 | @param distCoeffs Input vector of distortion coefficients |
981 | \f$\distcoeffs\f$. If the vector is NULL/empty, the zero distortion coefficients are |
982 | assumed. |
983 | @param rvec Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from |
984 | the model coordinate system to the camera coordinate system. |
985 | @param tvec Output translation vector. |
986 | @param useExtrinsicGuess Parameter used for @ref SOLVEPNP_ITERATIVE. If true (1), the function uses |
987 | the provided rvec and tvec values as initial approximations of the rotation and translation |
988 | vectors, respectively, and further optimizes them. |
989 | @param iterationsCount Number of iterations. |
990 | @param reprojectionError Inlier threshold value used by the RANSAC procedure. The parameter value |
991 | is the maximum allowed distance between the observed and computed point projections to consider it |
992 | an inlier. |
993 | @param confidence The probability that the algorithm produces a useful result. |
994 | @param inliers Output vector that contains indices of inliers in objectPoints and imagePoints . |
995 | @param flags Method for solving a PnP problem (see @ref solvePnP ). |
996 | |
997 | The function estimates an object pose given a set of object points, their corresponding image |
998 | projections, as well as the camera intrinsic matrix and the distortion coefficients. This function finds such |
999 | a pose that minimizes reprojection error, that is, the sum of squared distances between the observed |
1000 | projections imagePoints and the projected (using @ref projectPoints ) objectPoints. The use of RANSAC |
1001 | makes the function resistant to outliers. |
1002 | |
1003 | @note |
1004 | - An example of how to use solvePNPRansac for object detection can be found at |
1005 | opencv_source_code/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/ |
1006 | - The default method used to estimate the camera pose for the Minimal Sample Sets step |
1007 | is #SOLVEPNP_EPNP. Exceptions are: |
1008 | - if you choose #SOLVEPNP_P3P or #SOLVEPNP_AP3P, these methods will be used. |
1009 | - if the number of input points is equal to 4, #SOLVEPNP_P3P is used. |
1010 | - The method used to estimate the camera pose using all the inliers is defined by the |
1011 | flags parameters unless it is equal to #SOLVEPNP_P3P or #SOLVEPNP_AP3P. In this case, |
1012 | the method #SOLVEPNP_EPNP will be used instead. |
1013 | */ |
1014 | CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints, |
1015 | InputArray cameraMatrix, InputArray distCoeffs, |
1016 | OutputArray rvec, OutputArray tvec, |
1017 | bool useExtrinsicGuess = false, int iterationsCount = 100, |
1018 | float reprojectionError = 8.0, double confidence = 0.99, |
1019 | OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE ); |
1020 | |
1021 | |
1022 | /* |
1023 | Finds rotation and translation vector. |
1024 | If cameraMatrix is given then run P3P. Otherwise run linear P6P and output cameraMatrix too. |
1025 | */ |
1026 | CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints, |
1027 | InputOutputArray cameraMatrix, InputArray distCoeffs, |
1028 | OutputArray rvec, OutputArray tvec, OutputArray inliers, |
1029 | const UsacParams ¶ms=UsacParams()); |
1030 | |
1031 | /** @brief Finds an object pose from 3 3D-2D point correspondences. |
1032 | |
1033 | @see @ref calib3d_solvePnP |
1034 | |
1035 | @param objectPoints Array of object points in the object coordinate space, 3x3 1-channel or |
1036 | 1x3/3x1 3-channel. vector\<Point3f\> can be also passed here. |
1037 | @param imagePoints Array of corresponding image points, 3x2 1-channel or 1x3/3x1 2-channel. |
1038 | vector\<Point2f\> can be also passed here. |
1039 | @param cameraMatrix Input camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
1040 | @param distCoeffs Input vector of distortion coefficients |
1041 | \f$\distcoeffs\f$. If the vector is NULL/empty, the zero distortion coefficients are |
1042 | assumed. |
1043 | @param rvecs Output rotation vectors (see @ref Rodrigues ) that, together with tvecs, brings points from |
1044 | the model coordinate system to the camera coordinate system. A P3P problem has up to 4 solutions. |
1045 | @param tvecs Output translation vectors. |
1046 | @param flags Method for solving a P3P problem: |
1047 | - @ref SOLVEPNP_P3P Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang |
1048 | "Complete Solution Classification for the Perspective-Three-Point Problem" (@cite gao2003complete). |
1049 | - @ref SOLVEPNP_AP3P Method is based on the paper of T. Ke and S. Roumeliotis. |
1050 | "An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17). |
1051 | |
1052 | The function estimates the object pose given 3 object points, their corresponding image |
1053 | projections, as well as the camera intrinsic matrix and the distortion coefficients. |
1054 | |
1055 | @note |
1056 | The solutions are sorted by reprojection errors (lowest to highest). |
1057 | */ |
1058 | CV_EXPORTS_W int solveP3P( InputArray objectPoints, InputArray imagePoints, |
1059 | InputArray cameraMatrix, InputArray distCoeffs, |
1060 | OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, |
1061 | int flags ); |
1062 | |
1063 | /** @brief Refine a pose (the translation and the rotation that transform a 3D point expressed in the object coordinate frame |
1064 | to the camera coordinate frame) from a 3D-2D point correspondences and starting from an initial solution. |
1065 | |
1066 | @see @ref calib3d_solvePnP |
1067 | |
1068 | @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel, |
1069 | where N is the number of points. vector\<Point3d\> can also be passed here. |
1070 | @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, |
1071 | where N is the number of points. vector\<Point2d\> can also be passed here. |
1072 | @param cameraMatrix Input camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
1073 | @param distCoeffs Input vector of distortion coefficients |
1074 | \f$\distcoeffs\f$. If the vector is NULL/empty, the zero distortion coefficients are |
1075 | assumed. |
1076 | @param rvec Input/Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from |
1077 | the model coordinate system to the camera coordinate system. Input values are used as an initial solution. |
1078 | @param tvec Input/Output translation vector. Input values are used as an initial solution. |
1079 | @param criteria Criteria when to stop the Levenberg-Marquard iterative algorithm. |
1080 | |
1081 | The function refines the object pose given at least 3 object points, their corresponding image |
1082 | projections, an initial solution for the rotation and translation vector, |
1083 | as well as the camera intrinsic matrix and the distortion coefficients. |
1084 | The function minimizes the projection error with respect to the rotation and the translation vectors, according |
1085 | to a Levenberg-Marquardt iterative minimization @cite Madsen04 @cite Eade13 process. |
1086 | */ |
1087 | CV_EXPORTS_W void solvePnPRefineLM( InputArray objectPoints, InputArray imagePoints, |
1088 | InputArray cameraMatrix, InputArray distCoeffs, |
1089 | InputOutputArray rvec, InputOutputArray tvec, |
1090 | TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON)); |
1091 | |
1092 | /** @brief Refine a pose (the translation and the rotation that transform a 3D point expressed in the object coordinate frame |
1093 | to the camera coordinate frame) from a 3D-2D point correspondences and starting from an initial solution. |
1094 | |
1095 | @see @ref calib3d_solvePnP |
1096 | |
1097 | @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or 1xN/Nx1 3-channel, |
1098 | where N is the number of points. vector\<Point3d\> can also be passed here. |
1099 | @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, |
1100 | where N is the number of points. vector\<Point2d\> can also be passed here. |
1101 | @param cameraMatrix Input camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
1102 | @param distCoeffs Input vector of distortion coefficients |
1103 | \f$\distcoeffs\f$. If the vector is NULL/empty, the zero distortion coefficients are |
1104 | assumed. |
1105 | @param rvec Input/Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from |
1106 | the model coordinate system to the camera coordinate system. Input values are used as an initial solution. |
1107 | @param tvec Input/Output translation vector. Input values are used as an initial solution. |
1108 | @param criteria Criteria when to stop the Levenberg-Marquard iterative algorithm. |
1109 | @param VVSlambda Gain for the virtual visual servoing control law, equivalent to the \f$\alpha\f$ |
1110 | gain in the Damped Gauss-Newton formulation. |
1111 | |
1112 | The function refines the object pose given at least 3 object points, their corresponding image |
1113 | projections, an initial solution for the rotation and translation vector, |
1114 | as well as the camera intrinsic matrix and the distortion coefficients. |
1115 | The function minimizes the projection error with respect to the rotation and the translation vectors, using a |
1116 | virtual visual servoing (VVS) @cite Chaumette06 @cite Marchand16 scheme. |
1117 | */ |
1118 | CV_EXPORTS_W void solvePnPRefineVVS( InputArray objectPoints, InputArray imagePoints, |
1119 | InputArray cameraMatrix, InputArray distCoeffs, |
1120 | InputOutputArray rvec, InputOutputArray tvec, |
1121 | TermCriteria criteria = TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 20, FLT_EPSILON), |
1122 | double VVSlambda = 1); |
1123 | |
1124 | /** @brief Finds an object pose from 3D-2D point correspondences. |
1125 | |
1126 | @see @ref calib3d_solvePnP |
1127 | |
1128 | This function returns a list of all the possible solutions (a solution is a <rotation vector, translation vector> |
1129 | couple), depending on the number of input points and the chosen method: |
1130 | - P3P methods (@ref SOLVEPNP_P3P, @ref SOLVEPNP_AP3P): 3 or 4 input points. Number of returned solutions can be between 0 and 4 with 3 input points. |
1131 | - @ref SOLVEPNP_IPPE Input points must be >= 4 and object points must be coplanar. Returns 2 solutions. |
1132 | - @ref SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation. |
1133 | Number of input points must be 4 and 2 solutions are returned. Object points must be defined in the following order: |
1134 | - point 0: [-squareLength / 2, squareLength / 2, 0] |
1135 | - point 1: [ squareLength / 2, squareLength / 2, 0] |
1136 | - point 2: [ squareLength / 2, -squareLength / 2, 0] |
1137 | - point 3: [-squareLength / 2, -squareLength / 2, 0] |
1138 | - for all the other flags, number of input points must be >= 4 and object points can be in any configuration. |
1139 | Only 1 solution is returned. |
1140 | |
1141 | @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or |
1142 | 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3d\> can be also passed here. |
1143 | @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, |
1144 | where N is the number of points. vector\<Point2d\> can be also passed here. |
1145 | @param cameraMatrix Input camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
1146 | @param distCoeffs Input vector of distortion coefficients |
1147 | \f$\distcoeffs\f$. If the vector is NULL/empty, the zero distortion coefficients are |
1148 | assumed. |
1149 | @param rvecs Vector of output rotation vectors (see @ref Rodrigues ) that, together with tvecs, brings points from |
1150 | the model coordinate system to the camera coordinate system. |
1151 | @param tvecs Vector of output translation vectors. |
1152 | @param useExtrinsicGuess Parameter used for #SOLVEPNP_ITERATIVE. If true (1), the function uses |
1153 | the provided rvec and tvec values as initial approximations of the rotation and translation |
1154 | vectors, respectively, and further optimizes them. |
1155 | @param flags Method for solving a PnP problem: see @ref calib3d_solvePnP_flags |
1156 | @param rvec Rotation vector used to initialize an iterative PnP refinement algorithm, when flag is @ref SOLVEPNP_ITERATIVE |
1157 | and useExtrinsicGuess is set to true. |
1158 | @param tvec Translation vector used to initialize an iterative PnP refinement algorithm, when flag is @ref SOLVEPNP_ITERATIVE |
1159 | and useExtrinsicGuess is set to true. |
1160 | @param reprojectionError Optional vector of reprojection error, that is the RMS error |
1161 | (\f$ \text{RMSE} = \sqrt{\frac{\sum_{i}^{N} \left ( \hat{y_i} - y_i \right )^2}{N}} \f$) between the input image points |
1162 | and the 3D object points projected with the estimated pose. |
1163 | |
1164 | More information is described in @ref calib3d_solvePnP |
1165 | |
1166 | @note |
1167 | - An example of how to use solvePnP for planar augmented reality can be found at |
1168 | opencv_source_code/samples/python/plane_ar.py |
1169 | - If you are using Python: |
1170 | - Numpy array slices won't work as input because solvePnP requires contiguous |
1171 | arrays (enforced by the assertion using cv::Mat::checkVector() around line 55 of |
1172 | modules/calib3d/src/solvepnp.cpp version 2.4.9) |
1173 | - The P3P algorithm requires image points to be in an array of shape (N,1,2) due |
1174 | to its calling of #undistortPoints (around line 75 of modules/calib3d/src/solvepnp.cpp version 2.4.9) |
1175 | which requires 2-channel information. |
1176 | - Thus, given some data D = np.array(...) where D.shape = (N,M), in order to use a subset of |
1177 | it as, e.g., imagePoints, one must effectively copy it into a new array: imagePoints = |
1178 | np.ascontiguousarray(D[:,:2]).reshape((N,1,2)) |
1179 | - The methods @ref SOLVEPNP_DLS and @ref SOLVEPNP_UPNP cannot be used as the current implementations are |
1180 | unstable and sometimes give completely wrong results. If you pass one of these two |
1181 | flags, @ref SOLVEPNP_EPNP method will be used instead. |
1182 | - The minimum number of points is 4 in the general case. In the case of @ref SOLVEPNP_P3P and @ref SOLVEPNP_AP3P |
1183 | methods, it is required to use exactly 4 points (the first 3 points are used to estimate all the solutions |
1184 | of the P3P problem, the last one is used to retain the best solution that minimizes the reprojection error). |
1185 | - With @ref SOLVEPNP_ITERATIVE method and `useExtrinsicGuess=true`, the minimum number of points is 3 (3 points |
1186 | are sufficient to compute a pose but there are up to 4 solutions). The initial solution should be close to the |
1187 | global solution to converge. |
1188 | - With @ref SOLVEPNP_IPPE input points must be >= 4 and object points must be coplanar. |
1189 | - With @ref SOLVEPNP_IPPE_SQUARE this is a special case suitable for marker pose estimation. |
1190 | Number of input points must be 4. Object points must be defined in the following order: |
1191 | - point 0: [-squareLength / 2, squareLength / 2, 0] |
1192 | - point 1: [ squareLength / 2, squareLength / 2, 0] |
1193 | - point 2: [ squareLength / 2, -squareLength / 2, 0] |
1194 | - point 3: [-squareLength / 2, -squareLength / 2, 0] |
1195 | */ |
1196 | CV_EXPORTS_W int solvePnPGeneric( InputArray objectPoints, InputArray imagePoints, |
1197 | InputArray cameraMatrix, InputArray distCoeffs, |
1198 | OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, |
1199 | bool useExtrinsicGuess = false, SolvePnPMethod flags = SOLVEPNP_ITERATIVE, |
1200 | InputArray rvec = noArray(), InputArray tvec = noArray(), |
1201 | OutputArray reprojectionError = noArray() ); |
1202 | |
1203 | /** @brief Finds an initial camera intrinsic matrix from 3D-2D point correspondences. |
1204 | |
1205 | @param objectPoints Vector of vectors of the calibration pattern points in the calibration pattern |
1206 | coordinate space. In the old interface all the per-view vectors are concatenated. See |
1207 | #calibrateCamera for details. |
1208 | @param imagePoints Vector of vectors of the projections of the calibration pattern points. In the |
1209 | old interface all the per-view vectors are concatenated. |
1210 | @param imageSize Image size in pixels used to initialize the principal point. |
1211 | @param aspectRatio If it is zero or negative, both \f$f_x\f$ and \f$f_y\f$ are estimated independently. |
1212 | Otherwise, \f$f_x = f_y \cdot \texttt{aspectRatio}\f$ . |
1213 | |
1214 | The function estimates and returns an initial camera intrinsic matrix for the camera calibration process. |
1215 | Currently, the function only supports planar calibration patterns, which are patterns where each |
1216 | object point has z-coordinate =0. |
1217 | */ |
1218 | CV_EXPORTS_W Mat initCameraMatrix2D( InputArrayOfArrays objectPoints, |
1219 | InputArrayOfArrays imagePoints, |
1220 | Size imageSize, double aspectRatio = 1.0 ); |
1221 | |
1222 | /** @brief Finds the positions of internal corners of the chessboard. |
1223 | |
1224 | @param image Source chessboard view. It must be an 8-bit grayscale or color image. |
1225 | @param patternSize Number of inner corners per a chessboard row and column |
1226 | ( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ). |
1227 | @param corners Output array of detected corners. |
1228 | @param flags Various operation flags that can be zero or a combination of the following values: |
1229 | - @ref CALIB_CB_ADAPTIVE_THRESH Use adaptive thresholding to convert the image to black |
1230 | and white, rather than a fixed threshold level (computed from the average image brightness). |
1231 | - @ref CALIB_CB_NORMALIZE_IMAGE Normalize the image gamma with #equalizeHist before |
1232 | applying fixed or adaptive thresholding. |
1233 | - @ref CALIB_CB_FILTER_QUADS Use additional criteria (like contour area, perimeter, |
1234 | square-like shape) to filter out false quads extracted at the contour retrieval stage. |
1235 | - @ref CALIB_CB_FAST_CHECK Run a fast check on the image that looks for chessboard corners, |
1236 | and shortcut the call if none is found. This can drastically speed up the call in the |
1237 | degenerate condition when no chessboard is observed. |
1238 | - @ref CALIB_CB_PLAIN All other flags are ignored. The input image is taken as is. |
1239 | No image processing is done to improve to find the checkerboard. This has the effect of speeding up the |
1240 | execution of the function but could lead to not recognizing the checkerboard if the image |
1241 | is not previously binarized in the appropriate manner. |
1242 | |
1243 | The function attempts to determine whether the input image is a view of the chessboard pattern and |
1244 | locate the internal chessboard corners. The function returns a non-zero value if all of the corners |
1245 | are found and they are placed in a certain order (row by row, left to right in every row). |
1246 | Otherwise, if the function fails to find all the corners or reorder them, it returns 0. For example, |
1247 | a regular chessboard has 8 x 8 squares and 7 x 7 internal corners, that is, points where the black |
1248 | squares touch each other. The detected coordinates are approximate, and to determine their positions |
1249 | more accurately, the function calls #cornerSubPix. You also may use the function #cornerSubPix with |
1250 | different parameters if returned coordinates are not accurate enough. |
1251 | |
1252 | Sample usage of detecting and drawing chessboard corners: : |
1253 | @code |
1254 | Size patternsize(8,6); //interior number of corners |
1255 | Mat gray = ....; //source image |
1256 | vector<Point2f> corners; //this will be filled by the detected corners |
1257 | |
1258 | //CALIB_CB_FAST_CHECK saves a lot of time on images |
1259 | //that do not contain any chessboard corners |
1260 | bool patternfound = findChessboardCorners(gray, patternsize, corners, |
1261 | CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE |
1262 | + CALIB_CB_FAST_CHECK); |
1263 | |
1264 | if(patternfound) |
1265 | cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1), |
1266 | TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); |
1267 | |
1268 | drawChessboardCorners(img, patternsize, Mat(corners), patternfound); |
1269 | @endcode |
1270 | @note The function requires white space (like a square-thick border, the wider the better) around |
1271 | the board to make the detection more robust in various environments. Otherwise, if there is no |
1272 | border and the background is dark, the outer black squares cannot be segmented properly and so the |
1273 | square grouping and ordering algorithm fails. |
1274 | |
1275 | Use gen_pattern.py (@ref tutorial_camera_calibration_pattern) to create checkerboard. |
1276 | */ |
1277 | CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners, |
1278 | int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE ); |
1279 | |
1280 | /* |
1281 | Checks whether the image contains chessboard of the specific size or not. |
1282 | If yes, nonzero value is returned. |
1283 | */ |
1284 | CV_EXPORTS_W bool checkChessboard(InputArray img, Size size); |
1285 | |
1286 | /** @brief Finds the positions of internal corners of the chessboard using a sector based approach. |
1287 | |
1288 | @param image Source chessboard view. It must be an 8-bit grayscale or color image. |
1289 | @param patternSize Number of inner corners per a chessboard row and column |
1290 | ( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ). |
1291 | @param corners Output array of detected corners. |
1292 | @param flags Various operation flags that can be zero or a combination of the following values: |
1293 | - @ref CALIB_CB_NORMALIZE_IMAGE Normalize the image gamma with equalizeHist before detection. |
1294 | - @ref CALIB_CB_EXHAUSTIVE Run an exhaustive search to improve detection rate. |
1295 | - @ref CALIB_CB_ACCURACY Up sample input image to improve sub-pixel accuracy due to aliasing effects. |
1296 | - @ref CALIB_CB_LARGER The detected pattern is allowed to be larger than patternSize (see description). |
1297 | - @ref CALIB_CB_MARKER The detected pattern must have a marker (see description). |
1298 | This should be used if an accurate camera calibration is required. |
1299 | @param meta Optional output arrray of detected corners (CV_8UC1 and size = cv::Size(columns,rows)). |
1300 | Each entry stands for one corner of the pattern and can have one of the following values: |
1301 | - 0 = no meta data attached |
1302 | - 1 = left-top corner of a black cell |
1303 | - 2 = left-top corner of a white cell |
1304 | - 3 = left-top corner of a black cell with a white marker dot |
1305 | - 4 = left-top corner of a white cell with a black marker dot (pattern origin in case of markers otherwise first corner) |
1306 | |
1307 | The function is analog to #findChessboardCorners but uses a localized radon |
1308 | transformation approximated by box filters being more robust to all sort of |
1309 | noise, faster on larger images and is able to directly return the sub-pixel |
1310 | position of the internal chessboard corners. The Method is based on the paper |
1311 | @cite duda2018 "Accurate Detection and Localization of Checkerboard Corners for |
1312 | Calibration" demonstrating that the returned sub-pixel positions are more |
1313 | accurate than the one returned by cornerSubPix allowing a precise camera |
1314 | calibration for demanding applications. |
1315 | |
1316 | In the case, the flags @ref CALIB_CB_LARGER or @ref CALIB_CB_MARKER are given, |
1317 | the result can be recovered from the optional meta array. Both flags are |
1318 | helpful to use calibration patterns exceeding the field of view of the camera. |
1319 | These oversized patterns allow more accurate calibrations as corners can be |
1320 | utilized, which are as close as possible to the image borders. For a |
1321 | consistent coordinate system across all images, the optional marker (see image |
1322 | below) can be used to move the origin of the board to the location where the |
1323 | black circle is located. |
1324 | |
1325 | @note The function requires a white boarder with roughly the same width as one |
1326 | of the checkerboard fields around the whole board to improve the detection in |
1327 | various environments. In addition, because of the localized radon |
1328 | transformation it is beneficial to use round corners for the field corners |
1329 | which are located on the outside of the board. The following figure illustrates |
1330 | a sample checkerboard optimized for the detection. However, any other checkerboard |
1331 | can be used as well. |
1332 | |
1333 | Use gen_pattern.py (@ref tutorial_camera_calibration_pattern) to create checkerboard. |
1334 |  |
1335 | */ |
1336 | CV_EXPORTS_AS(findChessboardCornersSBWithMeta) |
1337 | bool (InputArray image,Size patternSize, OutputArray corners, |
1338 | int flags,OutputArray meta); |
1339 | /** @overload */ |
1340 | CV_EXPORTS_W inline |
1341 | bool (InputArray image, Size patternSize, OutputArray corners, |
1342 | int flags = 0) |
1343 | { |
1344 | return findChessboardCornersSB(image, patternSize, corners, flags, meta: noArray()); |
1345 | } |
1346 | |
1347 | /** @brief Estimates the sharpness of a detected chessboard. |
1348 | |
1349 | Image sharpness, as well as brightness, are a critical parameter for accuracte |
1350 | camera calibration. For accessing these parameters for filtering out |
1351 | problematic calibraiton images, this method calculates edge profiles by traveling from |
1352 | black to white chessboard cell centers. Based on this, the number of pixels is |
1353 | calculated required to transit from black to white. This width of the |
1354 | transition area is a good indication of how sharp the chessboard is imaged |
1355 | and should be below ~3.0 pixels. |
1356 | |
1357 | @param image Gray image used to find chessboard corners |
1358 | @param patternSize Size of a found chessboard pattern |
1359 | @param corners Corners found by #findChessboardCornersSB |
1360 | @param rise_distance Rise distance 0.8 means 10% ... 90% of the final signal strength |
1361 | @param vertical By default edge responses for horizontal lines are calculated |
1362 | @param sharpness Optional output array with a sharpness value for calculated edge responses (see description) |
1363 | |
1364 | The optional sharpness array is of type CV_32FC1 and has for each calculated |
1365 | profile one row with the following five entries: |
1366 | * 0 = x coordinate of the underlying edge in the image |
1367 | * 1 = y coordinate of the underlying edge in the image |
1368 | * 2 = width of the transition area (sharpness) |
1369 | * 3 = signal strength in the black cell (min brightness) |
1370 | * 4 = signal strength in the white cell (max brightness) |
1371 | |
1372 | @return Scalar(average sharpness, average min brightness, average max brightness,0) |
1373 | */ |
1374 | CV_EXPORTS_W Scalar estimateChessboardSharpness(InputArray image, Size patternSize, InputArray corners, |
1375 | float rise_distance=0.8F,bool vertical=false, |
1376 | OutputArray sharpness=noArray()); |
1377 | |
1378 | |
1379 | //! finds subpixel-accurate positions of the chessboard corners |
1380 | CV_EXPORTS_W bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size ); |
1381 | |
1382 | /** @brief Renders the detected chessboard corners. |
1383 | |
1384 | @param image Destination image. It must be an 8-bit color image. |
1385 | @param patternSize Number of inner corners per a chessboard row and column |
1386 | (patternSize = cv::Size(points_per_row,points_per_column)). |
1387 | @param corners Array of detected corners, the output of #findChessboardCorners. |
1388 | @param patternWasFound Parameter indicating whether the complete board was found or not. The |
1389 | return value of #findChessboardCorners should be passed here. |
1390 | |
1391 | The function draws individual chessboard corners detected either as red circles if the board was not |
1392 | found, or as colored corners connected with lines if the board was found. |
1393 | */ |
1394 | CV_EXPORTS_W void drawChessboardCorners( InputOutputArray image, Size patternSize, |
1395 | InputArray corners, bool patternWasFound ); |
1396 | |
1397 | /** @brief Draw axes of the world/object coordinate system from pose estimation. @sa solvePnP |
1398 | |
1399 | @param image Input/output image. It must have 1 or 3 channels. The number of channels is not altered. |
1400 | @param cameraMatrix Input 3x3 floating-point matrix of camera intrinsic parameters. |
1401 | \f$\cameramatrix{A}\f$ |
1402 | @param distCoeffs Input vector of distortion coefficients |
1403 | \f$\distcoeffs\f$. If the vector is empty, the zero distortion coefficients are assumed. |
1404 | @param rvec Rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from |
1405 | the model coordinate system to the camera coordinate system. |
1406 | @param tvec Translation vector. |
1407 | @param length Length of the painted axes in the same unit than tvec (usually in meters). |
1408 | @param thickness Line thickness of the painted axes. |
1409 | |
1410 | This function draws the axes of the world/object coordinate system w.r.t. to the camera frame. |
1411 | OX is drawn in red, OY in green and OZ in blue. |
1412 | */ |
1413 | CV_EXPORTS_W void drawFrameAxes(InputOutputArray image, InputArray cameraMatrix, InputArray distCoeffs, |
1414 | InputArray rvec, InputArray tvec, float length, int thickness=3); |
1415 | |
1416 | struct CV_EXPORTS_W_SIMPLE CirclesGridFinderParameters |
1417 | { |
1418 | CV_WRAP CirclesGridFinderParameters(); |
1419 | CV_PROP_RW cv::Size2f densityNeighborhoodSize; |
1420 | CV_PROP_RW float minDensity; |
1421 | CV_PROP_RW int kmeansAttempts; |
1422 | CV_PROP_RW int minDistanceToAddKeypoint; |
1423 | CV_PROP_RW int keypointScale; |
1424 | CV_PROP_RW float minGraphConfidence; |
1425 | CV_PROP_RW float vertexGain; |
1426 | CV_PROP_RW float vertexPenalty; |
1427 | CV_PROP_RW float existingVertexGain; |
1428 | CV_PROP_RW float edgeGain; |
1429 | CV_PROP_RW float edgePenalty; |
1430 | CV_PROP_RW float convexHullFactor; |
1431 | CV_PROP_RW float minRNGEdgeSwitchDist; |
1432 | |
1433 | enum GridType |
1434 | { |
1435 | SYMMETRIC_GRID, ASYMMETRIC_GRID |
1436 | }; |
1437 | GridType gridType; |
1438 | |
1439 | CV_PROP_RW float squareSize; //!< Distance between two adjacent points. Used by CALIB_CB_CLUSTERING. |
1440 | CV_PROP_RW float maxRectifiedDistance; //!< Max deviation from prediction. Used by CALIB_CB_CLUSTERING. |
1441 | }; |
1442 | |
1443 | #ifndef DISABLE_OPENCV_3_COMPATIBILITY |
1444 | typedef CirclesGridFinderParameters CirclesGridFinderParameters2; |
1445 | #endif |
1446 | |
1447 | /** @brief Finds centers in the grid of circles. |
1448 | |
1449 | @param image grid view of input circles; it must be an 8-bit grayscale or color image. |
1450 | @param patternSize number of circles per row and column |
1451 | ( patternSize = Size(points_per_row, points_per_colum) ). |
1452 | @param centers output array of detected centers. |
1453 | @param flags various operation flags that can be one of the following values: |
1454 | - @ref CALIB_CB_SYMMETRIC_GRID uses symmetric pattern of circles. |
1455 | - @ref CALIB_CB_ASYMMETRIC_GRID uses asymmetric pattern of circles. |
1456 | - @ref CALIB_CB_CLUSTERING uses a special algorithm for grid detection. It is more robust to |
1457 | perspective distortions but much more sensitive to background clutter. |
1458 | @param blobDetector feature detector that finds blobs like dark circles on light background. |
1459 | If `blobDetector` is NULL then `image` represents Point2f array of candidates. |
1460 | @param parameters struct for finding circles in a grid pattern. |
1461 | |
1462 | The function attempts to determine whether the input image contains a grid of circles. If it is, the |
1463 | function locates centers of the circles. The function returns a non-zero value if all of the centers |
1464 | have been found and they have been placed in a certain order (row by row, left to right in every |
1465 | row). Otherwise, if the function fails to find all the corners or reorder them, it returns 0. |
1466 | |
1467 | Sample usage of detecting and drawing the centers of circles: : |
1468 | @code |
1469 | Size patternsize(7,7); //number of centers |
1470 | Mat gray = ...; //source image |
1471 | vector<Point2f> centers; //this will be filled by the detected centers |
1472 | |
1473 | bool patternfound = findCirclesGrid(gray, patternsize, centers); |
1474 | |
1475 | drawChessboardCorners(img, patternsize, Mat(centers), patternfound); |
1476 | @endcode |
1477 | @note The function requires white space (like a square-thick border, the wider the better) around |
1478 | the board to make the detection more robust in various environments. |
1479 | */ |
1480 | CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize, |
1481 | OutputArray centers, int flags, |
1482 | const Ptr<FeatureDetector> &blobDetector, |
1483 | const CirclesGridFinderParameters& parameters); |
1484 | |
1485 | /** @overload */ |
1486 | CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize, |
1487 | OutputArray centers, int flags = CALIB_CB_SYMMETRIC_GRID, |
1488 | const Ptr<FeatureDetector> &blobDetector = SimpleBlobDetector::create()); |
1489 | |
1490 | /** @brief Finds the camera intrinsic and extrinsic parameters from several views of a calibration |
1491 | pattern. |
1492 | |
1493 | @param objectPoints In the new interface it is a vector of vectors of calibration pattern points in |
1494 | the calibration pattern coordinate space (e.g. std::vector<std::vector<cv::Vec3f>>). The outer |
1495 | vector contains as many elements as the number of pattern views. If the same calibration pattern |
1496 | is shown in each view and it is fully visible, all the vectors will be the same. Although, it is |
1497 | possible to use partially occluded patterns or even different patterns in different views. Then, |
1498 | the vectors will be different. Although the points are 3D, they all lie in the calibration pattern's |
1499 | XY coordinate plane (thus 0 in the Z-coordinate), if the used calibration pattern is a planar rig. |
1500 | In the old interface all the vectors of object points from different views are concatenated |
1501 | together. |
1502 | @param imagePoints In the new interface it is a vector of vectors of the projections of calibration |
1503 | pattern points (e.g. std::vector<std::vector<cv::Vec2f>>). imagePoints.size() and |
1504 | objectPoints.size(), and imagePoints[i].size() and objectPoints[i].size() for each i, must be equal, |
1505 | respectively. In the old interface all the vectors of object points from different views are |
1506 | concatenated together. |
1507 | @param imageSize Size of the image used only to initialize the camera intrinsic matrix. |
1508 | @param cameraMatrix Input/output 3x3 floating-point camera intrinsic matrix |
1509 | \f$\cameramatrix{A}\f$ . If @ref CALIB_USE_INTRINSIC_GUESS |
1510 | and/or @ref CALIB_FIX_ASPECT_RATIO, @ref CALIB_FIX_PRINCIPAL_POINT or @ref CALIB_FIX_FOCAL_LENGTH |
1511 | are specified, some or all of fx, fy, cx, cy must be initialized before calling the function. |
1512 | @param distCoeffs Input/output vector of distortion coefficients |
1513 | \f$\distcoeffs\f$. |
1514 | @param rvecs Output vector of rotation vectors (@ref Rodrigues ) estimated for each pattern view |
1515 | (e.g. std::vector<cv::Mat>>). That is, each i-th rotation vector together with the corresponding |
1516 | i-th translation vector (see the next output parameter description) brings the calibration pattern |
1517 | from the object coordinate space (in which object points are specified) to the camera coordinate |
1518 | space. In more technical terms, the tuple of the i-th rotation and translation vector performs |
1519 | a change of basis from object coordinate space to camera coordinate space. Due to its duality, this |
1520 | tuple is equivalent to the position of the calibration pattern with respect to the camera coordinate |
1521 | space. |
1522 | @param tvecs Output vector of translation vectors estimated for each pattern view, see parameter |
1523 | describtion above. |
1524 | @param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic |
1525 | parameters. Order of deviations values: |
1526 | \f$(f_x, f_y, c_x, c_y, k_1, k_2, p_1, p_2, k_3, k_4, k_5, k_6 , s_1, s_2, s_3, |
1527 | s_4, \tau_x, \tau_y)\f$ If one of parameters is not estimated, it's deviation is equals to zero. |
1528 | @param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic |
1529 | parameters. Order of deviations values: \f$(R_0, T_0, \dotsc , R_{M - 1}, T_{M - 1})\f$ where M is |
1530 | the number of pattern views. \f$R_i, T_i\f$ are concatenated 1x3 vectors. |
1531 | @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view. |
1532 | @param flags Different flags that may be zero or a combination of the following values: |
1533 | - @ref CALIB_USE_INTRINSIC_GUESS cameraMatrix contains valid initial values of |
1534 | fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image |
1535 | center ( imageSize is used), and focal distances are computed in a least-squares fashion. |
1536 | Note, that if intrinsic parameters are known, there is no need to use this function just to |
1537 | estimate extrinsic parameters. Use @ref solvePnP instead. |
1538 | - @ref CALIB_FIX_PRINCIPAL_POINT The principal point is not changed during the global |
1539 | optimization. It stays at the center or at a different location specified when |
1540 | @ref CALIB_USE_INTRINSIC_GUESS is set too. |
1541 | - @ref CALIB_FIX_ASPECT_RATIO The functions consider only fy as a free parameter. The |
1542 | ratio fx/fy stays the same as in the input cameraMatrix . When |
1543 | @ref CALIB_USE_INTRINSIC_GUESS is not set, the actual input values of fx and fy are |
1544 | ignored, only their ratio is computed and used further. |
1545 | - @ref CALIB_ZERO_TANGENT_DIST Tangential distortion coefficients \f$(p_1, p_2)\f$ are set |
1546 | to zeros and stay zero. |
1547 | - @ref CALIB_FIX_FOCAL_LENGTH The focal length is not changed during the global optimization if |
1548 | @ref CALIB_USE_INTRINSIC_GUESS is set. |
1549 | - @ref CALIB_FIX_K1,..., @ref CALIB_FIX_K6 The corresponding radial distortion |
1550 | coefficient is not changed during the optimization. If @ref CALIB_USE_INTRINSIC_GUESS is |
1551 | set, the coefficient from the supplied distCoeffs matrix is used. Otherwise, it is set to 0. |
1552 | - @ref CALIB_RATIONAL_MODEL Coefficients k4, k5, and k6 are enabled. To provide the |
1553 | backward compatibility, this extra flag should be explicitly specified to make the |
1554 | calibration function use the rational model and return 8 coefficients or more. |
1555 | - @ref CALIB_THIN_PRISM_MODEL Coefficients s1, s2, s3 and s4 are enabled. To provide the |
1556 | backward compatibility, this extra flag should be explicitly specified to make the |
1557 | calibration function use the thin prism model and return 12 coefficients or more. |
1558 | - @ref CALIB_FIX_S1_S2_S3_S4 The thin prism distortion coefficients are not changed during |
1559 | the optimization. If @ref CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the |
1560 | supplied distCoeffs matrix is used. Otherwise, it is set to 0. |
1561 | - @ref CALIB_TILTED_MODEL Coefficients tauX and tauY are enabled. To provide the |
1562 | backward compatibility, this extra flag should be explicitly specified to make the |
1563 | calibration function use the tilted sensor model and return 14 coefficients. |
1564 | - @ref CALIB_FIX_TAUX_TAUY The coefficients of the tilted sensor model are not changed during |
1565 | the optimization. If @ref CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the |
1566 | supplied distCoeffs matrix is used. Otherwise, it is set to 0. |
1567 | @param criteria Termination criteria for the iterative optimization algorithm. |
1568 | |
1569 | @return the overall RMS re-projection error. |
1570 | |
1571 | The function estimates the intrinsic camera parameters and extrinsic parameters for each of the |
1572 | views. The algorithm is based on @cite Zhang2000 and @cite BouguetMCT . The coordinates of 3D object |
1573 | points and their corresponding 2D projections in each view must be specified. That may be achieved |
1574 | by using an object with known geometry and easily detectable feature points. Such an object is |
1575 | called a calibration rig or calibration pattern, and OpenCV has built-in support for a chessboard as |
1576 | a calibration rig (see @ref findChessboardCorners). Currently, initialization of intrinsic |
1577 | parameters (when @ref CALIB_USE_INTRINSIC_GUESS is not set) is only implemented for planar calibration |
1578 | patterns (where Z-coordinates of the object points must be all zeros). 3D calibration rigs can also |
1579 | be used as long as initial cameraMatrix is provided. |
1580 | |
1581 | The algorithm performs the following steps: |
1582 | |
1583 | - Compute the initial intrinsic parameters (the option only available for planar calibration |
1584 | patterns) or read them from the input parameters. The distortion coefficients are all set to |
1585 | zeros initially unless some of CALIB_FIX_K? are specified. |
1586 | |
1587 | - Estimate the initial camera pose as if the intrinsic parameters have been already known. This is |
1588 | done using @ref solvePnP . |
1589 | |
1590 | - Run the global Levenberg-Marquardt optimization algorithm to minimize the reprojection error, |
1591 | that is, the total sum of squared distances between the observed feature points imagePoints and |
1592 | the projected (using the current estimates for camera parameters and the poses) object points |
1593 | objectPoints. See @ref projectPoints for details. |
1594 | |
1595 | @note |
1596 | If you use a non-square (i.e. non-N-by-N) grid and @ref findChessboardCorners for calibration, |
1597 | and @ref calibrateCamera returns bad values (zero distortion coefficients, \f$c_x\f$ and |
1598 | \f$c_y\f$ very far from the image center, and/or large differences between \f$f_x\f$ and |
1599 | \f$f_y\f$ (ratios of 10:1 or more)), then you are probably using patternSize=cvSize(rows,cols) |
1600 | instead of using patternSize=cvSize(cols,rows) in @ref findChessboardCorners. |
1601 | |
1602 | @note |
1603 | The function may throw exceptions, if unsupported combination of parameters is provided or |
1604 | the system is underconstrained. |
1605 | |
1606 | @sa |
1607 | calibrateCameraRO, findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate, |
1608 | undistort |
1609 | */ |
1610 | CV_EXPORTS_AS(calibrateCameraExtended) double calibrateCamera( InputArrayOfArrays objectPoints, |
1611 | InputArrayOfArrays imagePoints, Size imageSize, |
1612 | InputOutputArray cameraMatrix, InputOutputArray distCoeffs, |
1613 | OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, |
1614 | OutputArray stdDeviationsIntrinsics, |
1615 | OutputArray stdDeviationsExtrinsics, |
1616 | OutputArray perViewErrors, |
1617 | int flags = 0, TermCriteria criteria = TermCriteria( |
1618 | TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); |
1619 | |
1620 | /** @overload */ |
1621 | CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints, |
1622 | InputArrayOfArrays imagePoints, Size imageSize, |
1623 | InputOutputArray cameraMatrix, InputOutputArray distCoeffs, |
1624 | OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, |
1625 | int flags = 0, TermCriteria criteria = TermCriteria( |
1626 | TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); |
1627 | |
1628 | /** @brief Finds the camera intrinsic and extrinsic parameters from several views of a calibration pattern. |
1629 | |
1630 | This function is an extension of #calibrateCamera with the method of releasing object which was |
1631 | proposed in @cite strobl2011iccv. In many common cases with inaccurate, unmeasured, roughly planar |
1632 | targets (calibration plates), this method can dramatically improve the precision of the estimated |
1633 | camera parameters. Both the object-releasing method and standard method are supported by this |
1634 | function. Use the parameter **iFixedPoint** for method selection. In the internal implementation, |
1635 | #calibrateCamera is a wrapper for this function. |
1636 | |
1637 | @param objectPoints Vector of vectors of calibration pattern points in the calibration pattern |
1638 | coordinate space. See #calibrateCamera for details. If the method of releasing object to be used, |
1639 | the identical calibration board must be used in each view and it must be fully visible, and all |
1640 | objectPoints[i] must be the same and all points should be roughly close to a plane. **The calibration |
1641 | target has to be rigid, or at least static if the camera (rather than the calibration target) is |
1642 | shifted for grabbing images.** |
1643 | @param imagePoints Vector of vectors of the projections of calibration pattern points. See |
1644 | #calibrateCamera for details. |
1645 | @param imageSize Size of the image used only to initialize the intrinsic camera matrix. |
1646 | @param iFixedPoint The index of the 3D object point in objectPoints[0] to be fixed. It also acts as |
1647 | a switch for calibration method selection. If object-releasing method to be used, pass in the |
1648 | parameter in the range of [1, objectPoints[0].size()-2], otherwise a value out of this range will |
1649 | make standard calibration method selected. Usually the top-right corner point of the calibration |
1650 | board grid is recommended to be fixed when object-releasing method being utilized. According to |
1651 | \cite strobl2011iccv, two other points are also fixed. In this implementation, objectPoints[0].front |
1652 | and objectPoints[0].back.z are used. With object-releasing method, accurate rvecs, tvecs and |
1653 | newObjPoints are only possible if coordinates of these three fixed points are accurate enough. |
1654 | @param cameraMatrix Output 3x3 floating-point camera matrix. See #calibrateCamera for details. |
1655 | @param distCoeffs Output vector of distortion coefficients. See #calibrateCamera for details. |
1656 | @param rvecs Output vector of rotation vectors estimated for each pattern view. See #calibrateCamera |
1657 | for details. |
1658 | @param tvecs Output vector of translation vectors estimated for each pattern view. |
1659 | @param newObjPoints The updated output vector of calibration pattern points. The coordinates might |
1660 | be scaled based on three fixed points. The returned coordinates are accurate only if the above |
1661 | mentioned three fixed points are accurate. If not needed, noArray() can be passed in. This parameter |
1662 | is ignored with standard calibration method. |
1663 | @param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic parameters. |
1664 | See #calibrateCamera for details. |
1665 | @param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic parameters. |
1666 | See #calibrateCamera for details. |
1667 | @param stdDeviationsObjPoints Output vector of standard deviations estimated for refined coordinates |
1668 | of calibration pattern points. It has the same size and order as objectPoints[0] vector. This |
1669 | parameter is ignored with standard calibration method. |
1670 | @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view. |
1671 | @param flags Different flags that may be zero or a combination of some predefined values. See |
1672 | #calibrateCamera for details. If the method of releasing object is used, the calibration time may |
1673 | be much longer. CALIB_USE_QR or CALIB_USE_LU could be used for faster calibration with potentially |
1674 | less precise and less stable in some rare cases. |
1675 | @param criteria Termination criteria for the iterative optimization algorithm. |
1676 | |
1677 | @return the overall RMS re-projection error. |
1678 | |
1679 | The function estimates the intrinsic camera parameters and extrinsic parameters for each of the |
1680 | views. The algorithm is based on @cite Zhang2000, @cite BouguetMCT and @cite strobl2011iccv. See |
1681 | #calibrateCamera for other detailed explanations. |
1682 | @sa |
1683 | calibrateCamera, findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate, undistort |
1684 | */ |
1685 | CV_EXPORTS_AS(calibrateCameraROExtended) double calibrateCameraRO( InputArrayOfArrays objectPoints, |
1686 | InputArrayOfArrays imagePoints, Size imageSize, int iFixedPoint, |
1687 | InputOutputArray cameraMatrix, InputOutputArray distCoeffs, |
1688 | OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, |
1689 | OutputArray newObjPoints, |
1690 | OutputArray stdDeviationsIntrinsics, |
1691 | OutputArray stdDeviationsExtrinsics, |
1692 | OutputArray stdDeviationsObjPoints, |
1693 | OutputArray perViewErrors, |
1694 | int flags = 0, TermCriteria criteria = TermCriteria( |
1695 | TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); |
1696 | |
1697 | /** @overload */ |
1698 | CV_EXPORTS_W double calibrateCameraRO( InputArrayOfArrays objectPoints, |
1699 | InputArrayOfArrays imagePoints, Size imageSize, int iFixedPoint, |
1700 | InputOutputArray cameraMatrix, InputOutputArray distCoeffs, |
1701 | OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, |
1702 | OutputArray newObjPoints, |
1703 | int flags = 0, TermCriteria criteria = TermCriteria( |
1704 | TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) ); |
1705 | |
1706 | /** @brief Computes useful camera characteristics from the camera intrinsic matrix. |
1707 | |
1708 | @param cameraMatrix Input camera intrinsic matrix that can be estimated by #calibrateCamera or |
1709 | #stereoCalibrate . |
1710 | @param imageSize Input image size in pixels. |
1711 | @param apertureWidth Physical width in mm of the sensor. |
1712 | @param apertureHeight Physical height in mm of the sensor. |
1713 | @param fovx Output field of view in degrees along the horizontal sensor axis. |
1714 | @param fovy Output field of view in degrees along the vertical sensor axis. |
1715 | @param focalLength Focal length of the lens in mm. |
1716 | @param principalPoint Principal point in mm. |
1717 | @param aspectRatio \f$f_y/f_x\f$ |
1718 | |
1719 | The function computes various useful camera characteristics from the previously estimated camera |
1720 | matrix. |
1721 | |
1722 | @note |
1723 | Do keep in mind that the unity measure 'mm' stands for whatever unit of measure one chooses for |
1724 | the chessboard pitch (it can thus be any value). |
1725 | */ |
1726 | CV_EXPORTS_W void calibrationMatrixValues( InputArray cameraMatrix, Size imageSize, |
1727 | double apertureWidth, double apertureHeight, |
1728 | CV_OUT double& fovx, CV_OUT double& fovy, |
1729 | CV_OUT double& focalLength, CV_OUT Point2d& principalPoint, |
1730 | CV_OUT double& aspectRatio ); |
1731 | |
1732 | /** @brief Calibrates a stereo camera set up. This function finds the intrinsic parameters |
1733 | for each of the two cameras and the extrinsic parameters between the two cameras. |
1734 | |
1735 | @param objectPoints Vector of vectors of the calibration pattern points. The same structure as |
1736 | in @ref calibrateCamera. For each pattern view, both cameras need to see the same object |
1737 | points. Therefore, objectPoints.size(), imagePoints1.size(), and imagePoints2.size() need to be |
1738 | equal as well as objectPoints[i].size(), imagePoints1[i].size(), and imagePoints2[i].size() need to |
1739 | be equal for each i. |
1740 | @param imagePoints1 Vector of vectors of the projections of the calibration pattern points, |
1741 | observed by the first camera. The same structure as in @ref calibrateCamera. |
1742 | @param imagePoints2 Vector of vectors of the projections of the calibration pattern points, |
1743 | observed by the second camera. The same structure as in @ref calibrateCamera. |
1744 | @param cameraMatrix1 Input/output camera intrinsic matrix for the first camera, the same as in |
1745 | @ref calibrateCamera. Furthermore, for the stereo case, additional flags may be used, see below. |
1746 | @param distCoeffs1 Input/output vector of distortion coefficients, the same as in |
1747 | @ref calibrateCamera. |
1748 | @param cameraMatrix2 Input/output second camera intrinsic matrix for the second camera. See description for |
1749 | cameraMatrix1. |
1750 | @param distCoeffs2 Input/output lens distortion coefficients for the second camera. See |
1751 | description for distCoeffs1. |
1752 | @param imageSize Size of the image used only to initialize the camera intrinsic matrices. |
1753 | @param R Output rotation matrix. Together with the translation vector T, this matrix brings |
1754 | points given in the first camera's coordinate system to points in the second camera's |
1755 | coordinate system. In more technical terms, the tuple of R and T performs a change of basis |
1756 | from the first camera's coordinate system to the second camera's coordinate system. Due to its |
1757 | duality, this tuple is equivalent to the position of the first camera with respect to the |
1758 | second camera coordinate system. |
1759 | @param T Output translation vector, see description above. |
1760 | @param E Output essential matrix. |
1761 | @param F Output fundamental matrix. |
1762 | @param rvecs Output vector of rotation vectors ( @ref Rodrigues ) estimated for each pattern view in the |
1763 | coordinate system of the first camera of the stereo pair (e.g. std::vector<cv::Mat>). More in detail, each |
1764 | i-th rotation vector together with the corresponding i-th translation vector (see the next output parameter |
1765 | description) brings the calibration pattern from the object coordinate space (in which object points are |
1766 | specified) to the camera coordinate space of the first camera of the stereo pair. In more technical terms, |
1767 | the tuple of the i-th rotation and translation vector performs a change of basis from object coordinate space |
1768 | to camera coordinate space of the first camera of the stereo pair. |
1769 | @param tvecs Output vector of translation vectors estimated for each pattern view, see parameter description |
1770 | of previous output parameter ( rvecs ). |
1771 | @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view. |
1772 | @param flags Different flags that may be zero or a combination of the following values: |
1773 | - @ref CALIB_FIX_INTRINSIC Fix cameraMatrix? and distCoeffs? so that only R, T, E, and F |
1774 | matrices are estimated. |
1775 | - @ref CALIB_USE_INTRINSIC_GUESS Optimize some or all of the intrinsic parameters |
1776 | according to the specified flags. Initial values are provided by the user. |
1777 | - @ref CALIB_USE_EXTRINSIC_GUESS R and T contain valid initial values that are optimized further. |
1778 | Otherwise R and T are initialized to the median value of the pattern views (each dimension separately). |
1779 | - @ref CALIB_FIX_PRINCIPAL_POINT Fix the principal points during the optimization. |
1780 | - @ref CALIB_FIX_FOCAL_LENGTH Fix \f$f^{(j)}_x\f$ and \f$f^{(j)}_y\f$ . |
1781 | - @ref CALIB_FIX_ASPECT_RATIO Optimize \f$f^{(j)}_y\f$ . Fix the ratio \f$f^{(j)}_x/f^{(j)}_y\f$ |
1782 | . |
1783 | - @ref CALIB_SAME_FOCAL_LENGTH Enforce \f$f^{(0)}_x=f^{(1)}_x\f$ and \f$f^{(0)}_y=f^{(1)}_y\f$ . |
1784 | - @ref CALIB_ZERO_TANGENT_DIST Set tangential distortion coefficients for each camera to |
1785 | zeros and fix there. |
1786 | - @ref CALIB_FIX_K1,..., @ref CALIB_FIX_K6 Do not change the corresponding radial |
1787 | distortion coefficient during the optimization. If @ref CALIB_USE_INTRINSIC_GUESS is set, |
1788 | the coefficient from the supplied distCoeffs matrix is used. Otherwise, it is set to 0. |
1789 | - @ref CALIB_RATIONAL_MODEL Enable coefficients k4, k5, and k6. To provide the backward |
1790 | compatibility, this extra flag should be explicitly specified to make the calibration |
1791 | function use the rational model and return 8 coefficients. If the flag is not set, the |
1792 | function computes and returns only 5 distortion coefficients. |
1793 | - @ref CALIB_THIN_PRISM_MODEL Coefficients s1, s2, s3 and s4 are enabled. To provide the |
1794 | backward compatibility, this extra flag should be explicitly specified to make the |
1795 | calibration function use the thin prism model and return 12 coefficients. If the flag is not |
1796 | set, the function computes and returns only 5 distortion coefficients. |
1797 | - @ref CALIB_FIX_S1_S2_S3_S4 The thin prism distortion coefficients are not changed during |
1798 | the optimization. If @ref CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the |
1799 | supplied distCoeffs matrix is used. Otherwise, it is set to 0. |
1800 | - @ref CALIB_TILTED_MODEL Coefficients tauX and tauY are enabled. To provide the |
1801 | backward compatibility, this extra flag should be explicitly specified to make the |
1802 | calibration function use the tilted sensor model and return 14 coefficients. If the flag is not |
1803 | set, the function computes and returns only 5 distortion coefficients. |
1804 | - @ref CALIB_FIX_TAUX_TAUY The coefficients of the tilted sensor model are not changed during |
1805 | the optimization. If @ref CALIB_USE_INTRINSIC_GUESS is set, the coefficient from the |
1806 | supplied distCoeffs matrix is used. Otherwise, it is set to 0. |
1807 | @param criteria Termination criteria for the iterative optimization algorithm. |
1808 | |
1809 | The function estimates the transformation between two cameras making a stereo pair. If one computes |
1810 | the poses of an object relative to the first camera and to the second camera, |
1811 | ( \f$R_1\f$,\f$T_1\f$ ) and (\f$R_2\f$,\f$T_2\f$), respectively, for a stereo camera where the |
1812 | relative position and orientation between the two cameras are fixed, then those poses definitely |
1813 | relate to each other. This means, if the relative position and orientation (\f$R\f$,\f$T\f$) of the |
1814 | two cameras is known, it is possible to compute (\f$R_2\f$,\f$T_2\f$) when (\f$R_1\f$,\f$T_1\f$) is |
1815 | given. This is what the described function does. It computes (\f$R\f$,\f$T\f$) such that: |
1816 | |
1817 | \f[R_2=R R_1\f] |
1818 | \f[T_2=R T_1 + T.\f] |
1819 | |
1820 | Therefore, one can compute the coordinate representation of a 3D point for the second camera's |
1821 | coordinate system when given the point's coordinate representation in the first camera's coordinate |
1822 | system: |
1823 | |
1824 | \f[\begin{bmatrix} |
1825 | X_2 \\ |
1826 | Y_2 \\ |
1827 | Z_2 \\ |
1828 | 1 |
1829 | \end{bmatrix} = \begin{bmatrix} |
1830 | R & T \\ |
1831 | 0 & 1 |
1832 | \end{bmatrix} \begin{bmatrix} |
1833 | X_1 \\ |
1834 | Y_1 \\ |
1835 | Z_1 \\ |
1836 | 1 |
1837 | \end{bmatrix}.\f] |
1838 | |
1839 | |
1840 | Optionally, it computes the essential matrix E: |
1841 | |
1842 | \f[E= \vecthreethree{0}{-T_2}{T_1}{T_2}{0}{-T_0}{-T_1}{T_0}{0} R\f] |
1843 | |
1844 | where \f$T_i\f$ are components of the translation vector \f$T\f$ : \f$T=[T_0, T_1, T_2]^T\f$ . |
1845 | And the function can also compute the fundamental matrix F: |
1846 | |
1847 | \f[F = cameraMatrix2^{-T}\cdot E \cdot cameraMatrix1^{-1}\f] |
1848 | |
1849 | Besides the stereo-related information, the function can also perform a full calibration of each of |
1850 | the two cameras. However, due to the high dimensionality of the parameter space and noise in the |
1851 | input data, the function can diverge from the correct solution. If the intrinsic parameters can be |
1852 | estimated with high accuracy for each of the cameras individually (for example, using |
1853 | #calibrateCamera ), you are recommended to do so and then pass @ref CALIB_FIX_INTRINSIC flag to the |
1854 | function along with the computed intrinsic parameters. Otherwise, if all the parameters are |
1855 | estimated at once, it makes sense to restrict some parameters, for example, pass |
1856 | @ref CALIB_SAME_FOCAL_LENGTH and @ref CALIB_ZERO_TANGENT_DIST flags, which is usually a |
1857 | reasonable assumption. |
1858 | |
1859 | Similarly to #calibrateCamera, the function minimizes the total re-projection error for all the |
1860 | points in all the available views from both cameras. The function returns the final value of the |
1861 | re-projection error. |
1862 | */ |
1863 | CV_EXPORTS_AS(stereoCalibrateExtended) double stereoCalibrate( InputArrayOfArrays objectPoints, |
1864 | InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, |
1865 | InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, |
1866 | InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, |
1867 | Size imageSize, InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F, |
1868 | OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, |
1869 | TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); |
1870 | |
1871 | /// @overload |
1872 | CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, |
1873 | InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, |
1874 | InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, |
1875 | InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, |
1876 | Size imageSize, OutputArray R,OutputArray T, OutputArray E, OutputArray F, |
1877 | int flags = CALIB_FIX_INTRINSIC, |
1878 | TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); |
1879 | |
1880 | /// @overload |
1881 | CV_EXPORTS_W double stereoCalibrate( InputArrayOfArrays objectPoints, |
1882 | InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, |
1883 | InputOutputArray cameraMatrix1, InputOutputArray distCoeffs1, |
1884 | InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, |
1885 | Size imageSize, InputOutputArray R, InputOutputArray T, OutputArray E, OutputArray F, |
1886 | OutputArray perViewErrors, int flags = CALIB_FIX_INTRINSIC, |
1887 | TermCriteria criteria = TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6) ); |
1888 | |
1889 | /** @brief Computes rectification transforms for each head of a calibrated stereo camera. |
1890 | |
1891 | @param cameraMatrix1 First camera intrinsic matrix. |
1892 | @param distCoeffs1 First camera distortion parameters. |
1893 | @param cameraMatrix2 Second camera intrinsic matrix. |
1894 | @param distCoeffs2 Second camera distortion parameters. |
1895 | @param imageSize Size of the image used for stereo calibration. |
1896 | @param R Rotation matrix from the coordinate system of the first camera to the second camera, |
1897 | see @ref stereoCalibrate. |
1898 | @param T Translation vector from the coordinate system of the first camera to the second camera, |
1899 | see @ref stereoCalibrate. |
1900 | @param R1 Output 3x3 rectification transform (rotation matrix) for the first camera. This matrix |
1901 | brings points given in the unrectified first camera's coordinate system to points in the rectified |
1902 | first camera's coordinate system. In more technical terms, it performs a change of basis from the |
1903 | unrectified first camera's coordinate system to the rectified first camera's coordinate system. |
1904 | @param R2 Output 3x3 rectification transform (rotation matrix) for the second camera. This matrix |
1905 | brings points given in the unrectified second camera's coordinate system to points in the rectified |
1906 | second camera's coordinate system. In more technical terms, it performs a change of basis from the |
1907 | unrectified second camera's coordinate system to the rectified second camera's coordinate system. |
1908 | @param P1 Output 3x4 projection matrix in the new (rectified) coordinate systems for the first |
1909 | camera, i.e. it projects points given in the rectified first camera coordinate system into the |
1910 | rectified first camera's image. |
1911 | @param P2 Output 3x4 projection matrix in the new (rectified) coordinate systems for the second |
1912 | camera, i.e. it projects points given in the rectified first camera coordinate system into the |
1913 | rectified second camera's image. |
1914 | @param Q Output \f$4 \times 4\f$ disparity-to-depth mapping matrix (see @ref reprojectImageTo3D). |
1915 | @param flags Operation flags that may be zero or @ref CALIB_ZERO_DISPARITY . If the flag is set, |
1916 | the function makes the principal points of each camera have the same pixel coordinates in the |
1917 | rectified views. And if the flag is not set, the function may still shift the images in the |
1918 | horizontal or vertical direction (depending on the orientation of epipolar lines) to maximize the |
1919 | useful image area. |
1920 | @param alpha Free scaling parameter. If it is -1 or absent, the function performs the default |
1921 | scaling. Otherwise, the parameter should be between 0 and 1. alpha=0 means that the rectified |
1922 | images are zoomed and shifted so that only valid pixels are visible (no black areas after |
1923 | rectification). alpha=1 means that the rectified image is decimated and shifted so that all the |
1924 | pixels from the original images from the cameras are retained in the rectified images (no source |
1925 | image pixels are lost). Any intermediate value yields an intermediate result between |
1926 | those two extreme cases. |
1927 | @param newImageSize New image resolution after rectification. The same size should be passed to |
1928 | #initUndistortRectifyMap (see the stereo_calib.cpp sample in OpenCV samples directory). When (0,0) |
1929 | is passed (default), it is set to the original imageSize . Setting it to a larger value can help you |
1930 | preserve details in the original image, especially when there is a big radial distortion. |
1931 | @param validPixROI1 Optional output rectangles inside the rectified images where all the pixels |
1932 | are valid. If alpha=0 , the ROIs cover the whole images. Otherwise, they are likely to be smaller |
1933 | (see the picture below). |
1934 | @param validPixROI2 Optional output rectangles inside the rectified images where all the pixels |
1935 | are valid. If alpha=0 , the ROIs cover the whole images. Otherwise, they are likely to be smaller |
1936 | (see the picture below). |
1937 | |
1938 | The function computes the rotation matrices for each camera that (virtually) make both camera image |
1939 | planes the same plane. Consequently, this makes all the epipolar lines parallel and thus simplifies |
1940 | the dense stereo correspondence problem. The function takes the matrices computed by #stereoCalibrate |
1941 | as input. As output, it provides two rotation matrices and also two projection matrices in the new |
1942 | coordinates. The function distinguishes the following two cases: |
1943 | |
1944 | - **Horizontal stereo**: the first and the second camera views are shifted relative to each other |
1945 | mainly along the x-axis (with possible small vertical shift). In the rectified images, the |
1946 | corresponding epipolar lines in the left and right cameras are horizontal and have the same |
1947 | y-coordinate. P1 and P2 look like: |
1948 | |
1949 | \f[\texttt{P1} = \begin{bmatrix} |
1950 | f & 0 & cx_1 & 0 \\ |
1951 | 0 & f & cy & 0 \\ |
1952 | 0 & 0 & 1 & 0 |
1953 | \end{bmatrix}\f] |
1954 | |
1955 | \f[\texttt{P2} = \begin{bmatrix} |
1956 | f & 0 & cx_2 & T_x \cdot f \\ |
1957 | 0 & f & cy & 0 \\ |
1958 | 0 & 0 & 1 & 0 |
1959 | \end{bmatrix} ,\f] |
1960 | |
1961 | \f[\texttt{Q} = \begin{bmatrix} |
1962 | 1 & 0 & 0 & -cx_1 \\ |
1963 | 0 & 1 & 0 & -cy \\ |
1964 | 0 & 0 & 0 & f \\ |
1965 | 0 & 0 & -\frac{1}{T_x} & \frac{cx_1 - cx_2}{T_x} |
1966 | \end{bmatrix} \f] |
1967 | |
1968 | where \f$T_x\f$ is a horizontal shift between the cameras and \f$cx_1=cx_2\f$ if |
1969 | @ref CALIB_ZERO_DISPARITY is set. |
1970 | |
1971 | - **Vertical stereo**: the first and the second camera views are shifted relative to each other |
1972 | mainly in the vertical direction (and probably a bit in the horizontal direction too). The epipolar |
1973 | lines in the rectified images are vertical and have the same x-coordinate. P1 and P2 look like: |
1974 | |
1975 | \f[\texttt{P1} = \begin{bmatrix} |
1976 | f & 0 & cx & 0 \\ |
1977 | 0 & f & cy_1 & 0 \\ |
1978 | 0 & 0 & 1 & 0 |
1979 | \end{bmatrix}\f] |
1980 | |
1981 | \f[\texttt{P2} = \begin{bmatrix} |
1982 | f & 0 & cx & 0 \\ |
1983 | 0 & f & cy_2 & T_y \cdot f \\ |
1984 | 0 & 0 & 1 & 0 |
1985 | \end{bmatrix},\f] |
1986 | |
1987 | \f[\texttt{Q} = \begin{bmatrix} |
1988 | 1 & 0 & 0 & -cx \\ |
1989 | 0 & 1 & 0 & -cy_1 \\ |
1990 | 0 & 0 & 0 & f \\ |
1991 | 0 & 0 & -\frac{1}{T_y} & \frac{cy_1 - cy_2}{T_y} |
1992 | \end{bmatrix} \f] |
1993 | |
1994 | where \f$T_y\f$ is a vertical shift between the cameras and \f$cy_1=cy_2\f$ if |
1995 | @ref CALIB_ZERO_DISPARITY is set. |
1996 | |
1997 | As you can see, the first three columns of P1 and P2 will effectively be the new "rectified" camera |
1998 | matrices. The matrices, together with R1 and R2 , can then be passed to #initUndistortRectifyMap to |
1999 | initialize the rectification map for each camera. |
2000 | |
2001 | See below the screenshot from the stereo_calib.cpp sample. Some red horizontal lines pass through |
2002 | the corresponding image regions. This means that the images are well rectified, which is what most |
2003 | stereo correspondence algorithms rely on. The green rectangles are roi1 and roi2 . You see that |
2004 | their interiors are all valid pixels. |
2005 | |
2006 |  |
2007 | */ |
2008 | CV_EXPORTS_W void stereoRectify( InputArray cameraMatrix1, InputArray distCoeffs1, |
2009 | InputArray cameraMatrix2, InputArray distCoeffs2, |
2010 | Size imageSize, InputArray R, InputArray T, |
2011 | OutputArray R1, OutputArray R2, |
2012 | OutputArray P1, OutputArray P2, |
2013 | OutputArray Q, int flags = CALIB_ZERO_DISPARITY, |
2014 | double alpha = -1, Size newImageSize = Size(), |
2015 | CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 ); |
2016 | |
2017 | /** @brief Computes a rectification transform for an uncalibrated stereo camera. |
2018 | |
2019 | @param points1 Array of feature points in the first image. |
2020 | @param points2 The corresponding points in the second image. The same formats as in |
2021 | #findFundamentalMat are supported. |
2022 | @param F Input fundamental matrix. It can be computed from the same set of point pairs using |
2023 | #findFundamentalMat . |
2024 | @param imgSize Size of the image. |
2025 | @param H1 Output rectification homography matrix for the first image. |
2026 | @param H2 Output rectification homography matrix for the second image. |
2027 | @param threshold Optional threshold used to filter out the outliers. If the parameter is greater |
2028 | than zero, all the point pairs that do not comply with the epipolar geometry (that is, the points |
2029 | for which \f$|\texttt{points2[i]}^T \cdot \texttt{F} \cdot \texttt{points1[i]}|>\texttt{threshold}\f$ ) |
2030 | are rejected prior to computing the homographies. Otherwise, all the points are considered inliers. |
2031 | |
2032 | The function computes the rectification transformations without knowing intrinsic parameters of the |
2033 | cameras and their relative position in the space, which explains the suffix "uncalibrated". Another |
2034 | related difference from #stereoRectify is that the function outputs not the rectification |
2035 | transformations in the object (3D) space, but the planar perspective transformations encoded by the |
2036 | homography matrices H1 and H2 . The function implements the algorithm @cite Hartley99 . |
2037 | |
2038 | @note |
2039 | While the algorithm does not need to know the intrinsic parameters of the cameras, it heavily |
2040 | depends on the epipolar geometry. Therefore, if the camera lenses have a significant distortion, |
2041 | it would be better to correct it before computing the fundamental matrix and calling this |
2042 | function. For example, distortion coefficients can be estimated for each head of stereo camera |
2043 | separately by using #calibrateCamera . Then, the images can be corrected using #undistort , or |
2044 | just the point coordinates can be corrected with #undistortPoints . |
2045 | */ |
2046 | CV_EXPORTS_W bool stereoRectifyUncalibrated( InputArray points1, InputArray points2, |
2047 | InputArray F, Size imgSize, |
2048 | OutputArray H1, OutputArray H2, |
2049 | double threshold = 5 ); |
2050 | |
2051 | //! computes the rectification transformations for 3-head camera, where all the heads are on the same line. |
2052 | CV_EXPORTS_W float rectify3Collinear( InputArray cameraMatrix1, InputArray distCoeffs1, |
2053 | InputArray cameraMatrix2, InputArray distCoeffs2, |
2054 | InputArray cameraMatrix3, InputArray distCoeffs3, |
2055 | InputArrayOfArrays imgpt1, InputArrayOfArrays imgpt3, |
2056 | Size imageSize, InputArray R12, InputArray T12, |
2057 | InputArray R13, InputArray T13, |
2058 | OutputArray R1, OutputArray R2, OutputArray R3, |
2059 | OutputArray P1, OutputArray P2, OutputArray P3, |
2060 | OutputArray Q, double alpha, Size newImgSize, |
2061 | CV_OUT Rect* roi1, CV_OUT Rect* roi2, int flags ); |
2062 | |
2063 | /** @brief Returns the new camera intrinsic matrix based on the free scaling parameter. |
2064 | |
2065 | @param cameraMatrix Input camera intrinsic matrix. |
2066 | @param distCoeffs Input vector of distortion coefficients |
2067 | \f$\distcoeffs\f$. If the vector is NULL/empty, the zero distortion coefficients are |
2068 | assumed. |
2069 | @param imageSize Original image size. |
2070 | @param alpha Free scaling parameter between 0 (when all the pixels in the undistorted image are |
2071 | valid) and 1 (when all the source image pixels are retained in the undistorted image). See |
2072 | #stereoRectify for details. |
2073 | @param newImgSize Image size after rectification. By default, it is set to imageSize . |
2074 | @param validPixROI Optional output rectangle that outlines all-good-pixels region in the |
2075 | undistorted image. See roi1, roi2 description in #stereoRectify . |
2076 | @param centerPrincipalPoint Optional flag that indicates whether in the new camera intrinsic matrix the |
2077 | principal point should be at the image center or not. By default, the principal point is chosen to |
2078 | best fit a subset of the source image (determined by alpha) to the corrected image. |
2079 | @return new_camera_matrix Output new camera intrinsic matrix. |
2080 | |
2081 | The function computes and returns the optimal new camera intrinsic matrix based on the free scaling parameter. |
2082 | By varying this parameter, you may retrieve only sensible pixels alpha=0 , keep all the original |
2083 | image pixels if there is valuable information in the corners alpha=1 , or get something in between. |
2084 | When alpha\>0 , the undistorted result is likely to have some black pixels corresponding to |
2085 | "virtual" pixels outside of the captured distorted image. The original camera intrinsic matrix, distortion |
2086 | coefficients, the computed new camera intrinsic matrix, and newImageSize should be passed to |
2087 | #initUndistortRectifyMap to produce the maps for #remap . |
2088 | */ |
2089 | CV_EXPORTS_W Mat getOptimalNewCameraMatrix( InputArray cameraMatrix, InputArray distCoeffs, |
2090 | Size imageSize, double alpha, Size newImgSize = Size(), |
2091 | CV_OUT Rect* validPixROI = 0, |
2092 | bool centerPrincipalPoint = false); |
2093 | |
2094 | /** @brief Computes Hand-Eye calibration: \f$_{}^{g}\textrm{T}_c\f$ |
2095 | |
2096 | @param[in] R_gripper2base Rotation part extracted from the homogeneous matrix that transforms a point |
2097 | expressed in the gripper frame to the robot base frame (\f$_{}^{b}\textrm{T}_g\f$). |
2098 | This is a vector (`vector<Mat>`) that contains the rotation, `(3x3)` rotation matrices or `(3x1)` rotation vectors, |
2099 | for all the transformations from gripper frame to robot base frame. |
2100 | @param[in] t_gripper2base Translation part extracted from the homogeneous matrix that transforms a point |
2101 | expressed in the gripper frame to the robot base frame (\f$_{}^{b}\textrm{T}_g\f$). |
2102 | This is a vector (`vector<Mat>`) that contains the `(3x1)` translation vectors for all the transformations |
2103 | from gripper frame to robot base frame. |
2104 | @param[in] R_target2cam Rotation part extracted from the homogeneous matrix that transforms a point |
2105 | expressed in the target frame to the camera frame (\f$_{}^{c}\textrm{T}_t\f$). |
2106 | This is a vector (`vector<Mat>`) that contains the rotation, `(3x3)` rotation matrices or `(3x1)` rotation vectors, |
2107 | for all the transformations from calibration target frame to camera frame. |
2108 | @param[in] t_target2cam Rotation part extracted from the homogeneous matrix that transforms a point |
2109 | expressed in the target frame to the camera frame (\f$_{}^{c}\textrm{T}_t\f$). |
2110 | This is a vector (`vector<Mat>`) that contains the `(3x1)` translation vectors for all the transformations |
2111 | from calibration target frame to camera frame. |
2112 | @param[out] R_cam2gripper Estimated `(3x3)` rotation part extracted from the homogeneous matrix that transforms a point |
2113 | expressed in the camera frame to the gripper frame (\f$_{}^{g}\textrm{T}_c\f$). |
2114 | @param[out] t_cam2gripper Estimated `(3x1)` translation part extracted from the homogeneous matrix that transforms a point |
2115 | expressed in the camera frame to the gripper frame (\f$_{}^{g}\textrm{T}_c\f$). |
2116 | @param[in] method One of the implemented Hand-Eye calibration method, see cv::HandEyeCalibrationMethod |
2117 | |
2118 | The function performs the Hand-Eye calibration using various methods. One approach consists in estimating the |
2119 | rotation then the translation (separable solutions) and the following methods are implemented: |
2120 | - R. Tsai, R. Lenz A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/EyeCalibration \cite Tsai89 |
2121 | - F. Park, B. Martin Robot Sensor Calibration: Solving AX = XB on the Euclidean Group \cite Park94 |
2122 | - R. Horaud, F. Dornaika Hand-Eye Calibration \cite Horaud95 |
2123 | |
2124 | Another approach consists in estimating simultaneously the rotation and the translation (simultaneous solutions), |
2125 | with the following implemented methods: |
2126 | - N. Andreff, R. Horaud, B. Espiau On-line Hand-Eye Calibration \cite Andreff99 |
2127 | - K. Daniilidis Hand-Eye Calibration Using Dual Quaternions \cite Daniilidis98 |
2128 | |
2129 | The following picture describes the Hand-Eye calibration problem where the transformation between a camera ("eye") |
2130 | mounted on a robot gripper ("hand") has to be estimated. This configuration is called eye-in-hand. |
2131 | |
2132 | The eye-to-hand configuration consists in a static camera observing a calibration pattern mounted on the robot |
2133 | end-effector. The transformation from the camera to the robot base frame can then be estimated by inputting |
2134 | the suitable transformations to the function, see below. |
2135 | |
2136 |  |
2137 | |
2138 | The calibration procedure is the following: |
2139 | - a static calibration pattern is used to estimate the transformation between the target frame |
2140 | and the camera frame |
2141 | - the robot gripper is moved in order to acquire several poses |
2142 | - for each pose, the homogeneous transformation between the gripper frame and the robot base frame is recorded using for |
2143 | instance the robot kinematics |
2144 | \f[ |
2145 | \begin{bmatrix} |
2146 | X_b\\ |
2147 | Y_b\\ |
2148 | Z_b\\ |
2149 | 1 |
2150 | \end{bmatrix} |
2151 | = |
2152 | \begin{bmatrix} |
2153 | _{}^{b}\textrm{R}_g & _{}^{b}\textrm{t}_g \\ |
2154 | 0_{1 \times 3} & 1 |
2155 | \end{bmatrix} |
2156 | \begin{bmatrix} |
2157 | X_g\\ |
2158 | Y_g\\ |
2159 | Z_g\\ |
2160 | 1 |
2161 | \end{bmatrix} |
2162 | \f] |
2163 | - for each pose, the homogeneous transformation between the calibration target frame and the camera frame is recorded using |
2164 | for instance a pose estimation method (PnP) from 2D-3D point correspondences |
2165 | \f[ |
2166 | \begin{bmatrix} |
2167 | X_c\\ |
2168 | Y_c\\ |
2169 | Z_c\\ |
2170 | 1 |
2171 | \end{bmatrix} |
2172 | = |
2173 | \begin{bmatrix} |
2174 | _{}^{c}\textrm{R}_t & _{}^{c}\textrm{t}_t \\ |
2175 | 0_{1 \times 3} & 1 |
2176 | \end{bmatrix} |
2177 | \begin{bmatrix} |
2178 | X_t\\ |
2179 | Y_t\\ |
2180 | Z_t\\ |
2181 | 1 |
2182 | \end{bmatrix} |
2183 | \f] |
2184 | |
2185 | The Hand-Eye calibration procedure returns the following homogeneous transformation |
2186 | \f[ |
2187 | \begin{bmatrix} |
2188 | X_g\\ |
2189 | Y_g\\ |
2190 | Z_g\\ |
2191 | 1 |
2192 | \end{bmatrix} |
2193 | = |
2194 | \begin{bmatrix} |
2195 | _{}^{g}\textrm{R}_c & _{}^{g}\textrm{t}_c \\ |
2196 | 0_{1 \times 3} & 1 |
2197 | \end{bmatrix} |
2198 | \begin{bmatrix} |
2199 | X_c\\ |
2200 | Y_c\\ |
2201 | Z_c\\ |
2202 | 1 |
2203 | \end{bmatrix} |
2204 | \f] |
2205 | |
2206 | This problem is also known as solving the \f$\mathbf{A}\mathbf{X}=\mathbf{X}\mathbf{B}\f$ equation: |
2207 | - for an eye-in-hand configuration |
2208 | \f[ |
2209 | \begin{align*} |
2210 | ^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(1)} &= |
2211 | \hspace{0.1em} ^{b}{\textrm{T}_g}^{(2)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} \\ |
2212 | |
2213 | (^{b}{\textrm{T}_g}^{(2)})^{-1} \hspace{0.2em} ^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c &= |
2214 | \hspace{0.1em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} (^{c}{\textrm{T}_t}^{(1)})^{-1} \\ |
2215 | |
2216 | \textrm{A}_i \textrm{X} &= \textrm{X} \textrm{B}_i \\ |
2217 | \end{align*} |
2218 | \f] |
2219 | |
2220 | - for an eye-to-hand configuration |
2221 | \f[ |
2222 | \begin{align*} |
2223 | ^{g}{\textrm{T}_b}^{(1)} \hspace{0.2em} ^{b}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(1)} &= |
2224 | \hspace{0.1em} ^{g}{\textrm{T}_b}^{(2)} \hspace{0.2em} ^{b}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} \\ |
2225 | |
2226 | (^{g}{\textrm{T}_b}^{(2)})^{-1} \hspace{0.2em} ^{g}{\textrm{T}_b}^{(1)} \hspace{0.2em} ^{b}\textrm{T}_c &= |
2227 | \hspace{0.1em} ^{b}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} (^{c}{\textrm{T}_t}^{(1)})^{-1} \\ |
2228 | |
2229 | \textrm{A}_i \textrm{X} &= \textrm{X} \textrm{B}_i \\ |
2230 | \end{align*} |
2231 | \f] |
2232 | |
2233 | \note |
2234 | Additional information can be found on this [website](http://campar.in.tum.de/Chair/HandEyeCalibration). |
2235 | \note |
2236 | A minimum of 2 motions with non parallel rotation axes are necessary to determine the hand-eye transformation. |
2237 | So at least 3 different poses are required, but it is strongly recommended to use many more poses. |
2238 | |
2239 | */ |
2240 | CV_EXPORTS_W void calibrateHandEye( InputArrayOfArrays R_gripper2base, InputArrayOfArrays t_gripper2base, |
2241 | InputArrayOfArrays R_target2cam, InputArrayOfArrays t_target2cam, |
2242 | OutputArray R_cam2gripper, OutputArray t_cam2gripper, |
2243 | HandEyeCalibrationMethod method=CALIB_HAND_EYE_TSAI ); |
2244 | |
2245 | /** @brief Computes Robot-World/Hand-Eye calibration: \f$_{}^{w}\textrm{T}_b\f$ and \f$_{}^{c}\textrm{T}_g\f$ |
2246 | |
2247 | @param[in] R_world2cam Rotation part extracted from the homogeneous matrix that transforms a point |
2248 | expressed in the world frame to the camera frame (\f$_{}^{c}\textrm{T}_w\f$). |
2249 | This is a vector (`vector<Mat>`) that contains the rotation, `(3x3)` rotation matrices or `(3x1)` rotation vectors, |
2250 | for all the transformations from world frame to the camera frame. |
2251 | @param[in] t_world2cam Translation part extracted from the homogeneous matrix that transforms a point |
2252 | expressed in the world frame to the camera frame (\f$_{}^{c}\textrm{T}_w\f$). |
2253 | This is a vector (`vector<Mat>`) that contains the `(3x1)` translation vectors for all the transformations |
2254 | from world frame to the camera frame. |
2255 | @param[in] R_base2gripper Rotation part extracted from the homogeneous matrix that transforms a point |
2256 | expressed in the robot base frame to the gripper frame (\f$_{}^{g}\textrm{T}_b\f$). |
2257 | This is a vector (`vector<Mat>`) that contains the rotation, `(3x3)` rotation matrices or `(3x1)` rotation vectors, |
2258 | for all the transformations from robot base frame to the gripper frame. |
2259 | @param[in] t_base2gripper Rotation part extracted from the homogeneous matrix that transforms a point |
2260 | expressed in the robot base frame to the gripper frame (\f$_{}^{g}\textrm{T}_b\f$). |
2261 | This is a vector (`vector<Mat>`) that contains the `(3x1)` translation vectors for all the transformations |
2262 | from robot base frame to the gripper frame. |
2263 | @param[out] R_base2world Estimated `(3x3)` rotation part extracted from the homogeneous matrix that transforms a point |
2264 | expressed in the robot base frame to the world frame (\f$_{}^{w}\textrm{T}_b\f$). |
2265 | @param[out] t_base2world Estimated `(3x1)` translation part extracted from the homogeneous matrix that transforms a point |
2266 | expressed in the robot base frame to the world frame (\f$_{}^{w}\textrm{T}_b\f$). |
2267 | @param[out] R_gripper2cam Estimated `(3x3)` rotation part extracted from the homogeneous matrix that transforms a point |
2268 | expressed in the gripper frame to the camera frame (\f$_{}^{c}\textrm{T}_g\f$). |
2269 | @param[out] t_gripper2cam Estimated `(3x1)` translation part extracted from the homogeneous matrix that transforms a point |
2270 | expressed in the gripper frame to the camera frame (\f$_{}^{c}\textrm{T}_g\f$). |
2271 | @param[in] method One of the implemented Robot-World/Hand-Eye calibration method, see cv::RobotWorldHandEyeCalibrationMethod |
2272 | |
2273 | The function performs the Robot-World/Hand-Eye calibration using various methods. One approach consists in estimating the |
2274 | rotation then the translation (separable solutions): |
2275 | - M. Shah, Solving the robot-world/hand-eye calibration problem using the kronecker product \cite Shah2013SolvingTR |
2276 | |
2277 | Another approach consists in estimating simultaneously the rotation and the translation (simultaneous solutions), |
2278 | with the following implemented method: |
2279 | - A. Li, L. Wang, and D. Wu, Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product \cite Li2010SimultaneousRA |
2280 | |
2281 | The following picture describes the Robot-World/Hand-Eye calibration problem where the transformations between a robot and a world frame |
2282 | and between a robot gripper ("hand") and a camera ("eye") mounted at the robot end-effector have to be estimated. |
2283 | |
2284 |  |
2285 | |
2286 | The calibration procedure is the following: |
2287 | - a static calibration pattern is used to estimate the transformation between the target frame |
2288 | and the camera frame |
2289 | - the robot gripper is moved in order to acquire several poses |
2290 | - for each pose, the homogeneous transformation between the gripper frame and the robot base frame is recorded using for |
2291 | instance the robot kinematics |
2292 | \f[ |
2293 | \begin{bmatrix} |
2294 | X_g\\ |
2295 | Y_g\\ |
2296 | Z_g\\ |
2297 | 1 |
2298 | \end{bmatrix} |
2299 | = |
2300 | \begin{bmatrix} |
2301 | _{}^{g}\textrm{R}_b & _{}^{g}\textrm{t}_b \\ |
2302 | 0_{1 \times 3} & 1 |
2303 | \end{bmatrix} |
2304 | \begin{bmatrix} |
2305 | X_b\\ |
2306 | Y_b\\ |
2307 | Z_b\\ |
2308 | 1 |
2309 | \end{bmatrix} |
2310 | \f] |
2311 | - for each pose, the homogeneous transformation between the calibration target frame (the world frame) and the camera frame is recorded using |
2312 | for instance a pose estimation method (PnP) from 2D-3D point correspondences |
2313 | \f[ |
2314 | \begin{bmatrix} |
2315 | X_c\\ |
2316 | Y_c\\ |
2317 | Z_c\\ |
2318 | 1 |
2319 | \end{bmatrix} |
2320 | = |
2321 | \begin{bmatrix} |
2322 | _{}^{c}\textrm{R}_w & _{}^{c}\textrm{t}_w \\ |
2323 | 0_{1 \times 3} & 1 |
2324 | \end{bmatrix} |
2325 | \begin{bmatrix} |
2326 | X_w\\ |
2327 | Y_w\\ |
2328 | Z_w\\ |
2329 | 1 |
2330 | \end{bmatrix} |
2331 | \f] |
2332 | |
2333 | The Robot-World/Hand-Eye calibration procedure returns the following homogeneous transformations |
2334 | \f[ |
2335 | \begin{bmatrix} |
2336 | X_w\\ |
2337 | Y_w\\ |
2338 | Z_w\\ |
2339 | 1 |
2340 | \end{bmatrix} |
2341 | = |
2342 | \begin{bmatrix} |
2343 | _{}^{w}\textrm{R}_b & _{}^{w}\textrm{t}_b \\ |
2344 | 0_{1 \times 3} & 1 |
2345 | \end{bmatrix} |
2346 | \begin{bmatrix} |
2347 | X_b\\ |
2348 | Y_b\\ |
2349 | Z_b\\ |
2350 | 1 |
2351 | \end{bmatrix} |
2352 | \f] |
2353 | \f[ |
2354 | \begin{bmatrix} |
2355 | X_c\\ |
2356 | Y_c\\ |
2357 | Z_c\\ |
2358 | 1 |
2359 | \end{bmatrix} |
2360 | = |
2361 | \begin{bmatrix} |
2362 | _{}^{c}\textrm{R}_g & _{}^{c}\textrm{t}_g \\ |
2363 | 0_{1 \times 3} & 1 |
2364 | \end{bmatrix} |
2365 | \begin{bmatrix} |
2366 | X_g\\ |
2367 | Y_g\\ |
2368 | Z_g\\ |
2369 | 1 |
2370 | \end{bmatrix} |
2371 | \f] |
2372 | |
2373 | This problem is also known as solving the \f$\mathbf{A}\mathbf{X}=\mathbf{Z}\mathbf{B}\f$ equation, with: |
2374 | - \f$\mathbf{A} \Leftrightarrow \hspace{0.1em} _{}^{c}\textrm{T}_w\f$ |
2375 | - \f$\mathbf{X} \Leftrightarrow \hspace{0.1em} _{}^{w}\textrm{T}_b\f$ |
2376 | - \f$\mathbf{Z} \Leftrightarrow \hspace{0.1em} _{}^{c}\textrm{T}_g\f$ |
2377 | - \f$\mathbf{B} \Leftrightarrow \hspace{0.1em} _{}^{g}\textrm{T}_b\f$ |
2378 | |
2379 | \note |
2380 | At least 3 measurements are required (input vectors size must be greater or equal to 3). |
2381 | |
2382 | */ |
2383 | CV_EXPORTS_W void calibrateRobotWorldHandEye( InputArrayOfArrays R_world2cam, InputArrayOfArrays t_world2cam, |
2384 | InputArrayOfArrays R_base2gripper, InputArrayOfArrays t_base2gripper, |
2385 | OutputArray R_base2world, OutputArray t_base2world, |
2386 | OutputArray R_gripper2cam, OutputArray t_gripper2cam, |
2387 | RobotWorldHandEyeCalibrationMethod method=CALIB_ROBOT_WORLD_HAND_EYE_SHAH ); |
2388 | |
2389 | /** @brief Converts points from Euclidean to homogeneous space. |
2390 | |
2391 | @param src Input vector of N-dimensional points. |
2392 | @param dst Output vector of N+1-dimensional points. |
2393 | |
2394 | The function converts points from Euclidean to homogeneous space by appending 1's to the tuple of |
2395 | point coordinates. That is, each point (x1, x2, ..., xn) is converted to (x1, x2, ..., xn, 1). |
2396 | */ |
2397 | CV_EXPORTS_W void convertPointsToHomogeneous( InputArray src, OutputArray dst ); |
2398 | |
2399 | /** @brief Converts points from homogeneous to Euclidean space. |
2400 | |
2401 | @param src Input vector of N-dimensional points. |
2402 | @param dst Output vector of N-1-dimensional points. |
2403 | |
2404 | The function converts points homogeneous to Euclidean space using perspective projection. That is, |
2405 | each point (x1, x2, ... x(n-1), xn) is converted to (x1/xn, x2/xn, ..., x(n-1)/xn). When xn=0, the |
2406 | output point coordinates will be (0,0,0,...). |
2407 | */ |
2408 | CV_EXPORTS_W void convertPointsFromHomogeneous( InputArray src, OutputArray dst ); |
2409 | |
2410 | /** @brief Converts points to/from homogeneous coordinates. |
2411 | |
2412 | @param src Input array or vector of 2D, 3D, or 4D points. |
2413 | @param dst Output vector of 2D, 3D, or 4D points. |
2414 | |
2415 | The function converts 2D or 3D points from/to homogeneous coordinates by calling either |
2416 | #convertPointsToHomogeneous or #convertPointsFromHomogeneous. |
2417 | |
2418 | @note The function is obsolete. Use one of the previous two functions instead. |
2419 | */ |
2420 | CV_EXPORTS void convertPointsHomogeneous( InputArray src, OutputArray dst ); |
2421 | |
2422 | /** @brief Calculates a fundamental matrix from the corresponding points in two images. |
2423 | |
2424 | @param points1 Array of N points from the first image. The point coordinates should be |
2425 | floating-point (single or double precision). |
2426 | @param points2 Array of the second image points of the same size and format as points1 . |
2427 | @param method Method for computing a fundamental matrix. |
2428 | - @ref FM_7POINT for a 7-point algorithm. \f$N = 7\f$ |
2429 | - @ref FM_8POINT for an 8-point algorithm. \f$N \ge 8\f$ |
2430 | - @ref FM_RANSAC for the RANSAC algorithm. \f$N \ge 8\f$ |
2431 | - @ref FM_LMEDS for the LMedS algorithm. \f$N \ge 8\f$ |
2432 | @param ransacReprojThreshold Parameter used only for RANSAC. It is the maximum distance from a point to an epipolar |
2433 | line in pixels, beyond which the point is considered an outlier and is not used for computing the |
2434 | final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the |
2435 | point localization, image resolution, and the image noise. |
2436 | @param confidence Parameter used for the RANSAC and LMedS methods only. It specifies a desirable level |
2437 | of confidence (probability) that the estimated matrix is correct. |
2438 | @param[out] mask optional output mask |
2439 | @param maxIters The maximum number of robust method iterations. |
2440 | |
2441 | The epipolar geometry is described by the following equation: |
2442 | |
2443 | \f[[p_2; 1]^T F [p_1; 1] = 0\f] |
2444 | |
2445 | 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 |
2446 | second images, respectively. |
2447 | |
2448 | The function calculates the fundamental matrix using one of four methods listed above and returns |
2449 | the found fundamental matrix. Normally just one matrix is found. But in case of the 7-point |
2450 | algorithm, the function may return up to 3 solutions ( \f$9 \times 3\f$ matrix that stores all 3 |
2451 | matrices sequentially). |
2452 | |
2453 | The calculated fundamental matrix may be passed further to #computeCorrespondEpilines that finds the |
2454 | epipolar lines corresponding to the specified points. It can also be passed to |
2455 | #stereoRectifyUncalibrated to compute the rectification transformation. : |
2456 | @code |
2457 | // Example. Estimation of fundamental matrix using the RANSAC algorithm |
2458 | int point_count = 100; |
2459 | vector<Point2f> points1(point_count); |
2460 | vector<Point2f> points2(point_count); |
2461 | |
2462 | // initialize the points here ... |
2463 | for( int i = 0; i < point_count; i++ ) |
2464 | { |
2465 | points1[i] = ...; |
2466 | points2[i] = ...; |
2467 | } |
2468 | |
2469 | Mat fundamental_matrix = |
2470 | findFundamentalMat(points1, points2, FM_RANSAC, 3, 0.99); |
2471 | @endcode |
2472 | */ |
2473 | CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2, |
2474 | int method, double ransacReprojThreshold, double confidence, |
2475 | int maxIters, OutputArray mask = noArray() ); |
2476 | |
2477 | /** @overload */ |
2478 | CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2, |
2479 | int method = FM_RANSAC, |
2480 | double ransacReprojThreshold = 3., double confidence = 0.99, |
2481 | OutputArray mask = noArray() ); |
2482 | |
2483 | /** @overload */ |
2484 | CV_EXPORTS Mat findFundamentalMat( InputArray points1, InputArray points2, |
2485 | OutputArray mask, int method = FM_RANSAC, |
2486 | double ransacReprojThreshold = 3., double confidence = 0.99 ); |
2487 | |
2488 | |
2489 | CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2, |
2490 | OutputArray mask, const UsacParams ¶ms); |
2491 | |
2492 | /** @brief Calculates an essential matrix from the corresponding points in two images. |
2493 | |
2494 | @param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should |
2495 | be floating-point (single or double precision). |
2496 | @param points2 Array of the second image points of the same size and format as points1. |
2497 | @param cameraMatrix Camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
2498 | Note that this function assumes that points1 and points2 are feature points from cameras with the |
2499 | same camera intrinsic matrix. If this assumption does not hold for your use case, use another |
2500 | function overload or #undistortPoints with `P = cv::NoArray()` for both cameras to transform image |
2501 | points to normalized image coordinates, which are valid for the identity camera intrinsic matrix. |
2502 | When passing these coordinates, pass the identity matrix for this parameter. |
2503 | @param method Method for computing an essential matrix. |
2504 | - @ref RANSAC for the RANSAC algorithm. |
2505 | - @ref LMEDS for the LMedS algorithm. |
2506 | @param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of |
2507 | confidence (probability) that the estimated matrix is correct. |
2508 | @param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar |
2509 | line in pixels, beyond which the point is considered an outlier and is not used for computing the |
2510 | final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the |
2511 | point localization, image resolution, and the image noise. |
2512 | @param mask Output array of N elements, every element of which is set to 0 for outliers and to 1 |
2513 | for the other points. The array is computed only in the RANSAC and LMedS methods. |
2514 | @param maxIters The maximum number of robust method iterations. |
2515 | |
2516 | This function estimates essential matrix based on the five-point algorithm solver in @cite Nister03 . |
2517 | @cite SteweniusCFS is also a related. The epipolar geometry is described by the following equation: |
2518 | |
2519 | \f[[p_2; 1]^T K^{-T} E K^{-1} [p_1; 1] = 0\f] |
2520 | |
2521 | 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 |
2522 | second images, respectively. The result of this function may be passed further to |
2523 | #decomposeEssentialMat or #recoverPose to recover the relative pose between cameras. |
2524 | */ |
2525 | CV_EXPORTS_W |
2526 | Mat findEssentialMat( |
2527 | InputArray points1, InputArray points2, |
2528 | InputArray cameraMatrix, int method = RANSAC, |
2529 | double prob = 0.999, double threshold = 1.0, |
2530 | int maxIters = 1000, OutputArray mask = noArray() |
2531 | ); |
2532 | |
2533 | /** @overload */ |
2534 | CV_EXPORTS |
2535 | Mat findEssentialMat( |
2536 | InputArray points1, InputArray points2, |
2537 | InputArray cameraMatrix, int method, |
2538 | double prob, double threshold, |
2539 | OutputArray mask |
2540 | ); // TODO remove from OpenCV 5.0 |
2541 | |
2542 | /** @overload |
2543 | @param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should |
2544 | be floating-point (single or double precision). |
2545 | @param points2 Array of the second image points of the same size and format as points1 . |
2546 | @param focal focal length of the camera. Note that this function assumes that points1 and points2 |
2547 | are feature points from cameras with same focal length and principal point. |
2548 | @param pp principal point of the camera. |
2549 | @param method Method for computing a fundamental matrix. |
2550 | - @ref RANSAC for the RANSAC algorithm. |
2551 | - @ref LMEDS for the LMedS algorithm. |
2552 | @param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar |
2553 | line in pixels, beyond which the point is considered an outlier and is not used for computing the |
2554 | final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the |
2555 | point localization, image resolution, and the image noise. |
2556 | @param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of |
2557 | confidence (probability) that the estimated matrix is correct. |
2558 | @param mask Output array of N elements, every element of which is set to 0 for outliers and to 1 |
2559 | for the other points. The array is computed only in the RANSAC and LMedS methods. |
2560 | @param maxIters The maximum number of robust method iterations. |
2561 | |
2562 | This function differs from the one above that it computes camera intrinsic matrix from focal length and |
2563 | principal point: |
2564 | |
2565 | \f[A = |
2566 | \begin{bmatrix} |
2567 | f & 0 & x_{pp} \\ |
2568 | 0 & f & y_{pp} \\ |
2569 | 0 & 0 & 1 |
2570 | \end{bmatrix}\f] |
2571 | */ |
2572 | CV_EXPORTS_W |
2573 | Mat findEssentialMat( |
2574 | InputArray points1, InputArray points2, |
2575 | double focal = 1.0, Point2d pp = Point2d(0, 0), |
2576 | int method = RANSAC, double prob = 0.999, |
2577 | double threshold = 1.0, int maxIters = 1000, |
2578 | OutputArray mask = noArray() |
2579 | ); |
2580 | |
2581 | /** @overload */ |
2582 | CV_EXPORTS |
2583 | Mat findEssentialMat( |
2584 | InputArray points1, InputArray points2, |
2585 | double focal, Point2d pp, |
2586 | int method, double prob, |
2587 | double threshold, OutputArray mask |
2588 | ); // TODO remove from OpenCV 5.0 |
2589 | |
2590 | /** @brief Calculates an essential matrix from the corresponding points in two images from potentially two different cameras. |
2591 | |
2592 | @param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should |
2593 | be floating-point (single or double precision). |
2594 | @param points2 Array of the second image points of the same size and format as points1. |
2595 | @param cameraMatrix1 Camera matrix for the first camera \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . |
2596 | @param cameraMatrix2 Camera matrix for the second camera \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . |
2597 | @param distCoeffs1 Input vector of distortion coefficients for the first camera |
2598 | \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ |
2599 | of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. |
2600 | @param distCoeffs2 Input vector of distortion coefficients for the second camera |
2601 | \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ |
2602 | of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. |
2603 | @param method Method for computing an essential matrix. |
2604 | - @ref RANSAC for the RANSAC algorithm. |
2605 | - @ref LMEDS for the LMedS algorithm. |
2606 | @param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of |
2607 | confidence (probability) that the estimated matrix is correct. |
2608 | @param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar |
2609 | line in pixels, beyond which the point is considered an outlier and is not used for computing the |
2610 | final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the |
2611 | point localization, image resolution, and the image noise. |
2612 | @param mask Output array of N elements, every element of which is set to 0 for outliers and to 1 |
2613 | for the other points. The array is computed only in the RANSAC and LMedS methods. |
2614 | |
2615 | This function estimates essential matrix based on the five-point algorithm solver in @cite Nister03 . |
2616 | @cite SteweniusCFS is also a related. The epipolar geometry is described by the following equation: |
2617 | |
2618 | \f[[p_2; 1]^T K^{-T} E K^{-1} [p_1; 1] = 0\f] |
2619 | |
2620 | 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 |
2621 | second images, respectively. The result of this function may be passed further to |
2622 | #decomposeEssentialMat or #recoverPose to recover the relative pose between cameras. |
2623 | */ |
2624 | CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2, |
2625 | InputArray cameraMatrix1, InputArray distCoeffs1, |
2626 | InputArray cameraMatrix2, InputArray distCoeffs2, |
2627 | int method = RANSAC, |
2628 | double prob = 0.999, double threshold = 1.0, |
2629 | OutputArray mask = noArray() ); |
2630 | |
2631 | |
2632 | CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2, |
2633 | InputArray cameraMatrix1, InputArray cameraMatrix2, |
2634 | InputArray dist_coeff1, InputArray dist_coeff2, OutputArray mask, |
2635 | const UsacParams ¶ms); |
2636 | |
2637 | /** @brief Decompose an essential matrix to possible rotations and translation. |
2638 | |
2639 | @param E The input essential matrix. |
2640 | @param R1 One possible rotation matrix. |
2641 | @param R2 Another possible rotation matrix. |
2642 | @param t One possible translation. |
2643 | |
2644 | This function decomposes the essential matrix E using svd decomposition @cite HartleyZ00. In |
2645 | general, four possible poses exist for the decomposition of E. They are \f$[R_1, t]\f$, |
2646 | \f$[R_1, -t]\f$, \f$[R_2, t]\f$, \f$[R_2, -t]\f$. |
2647 | |
2648 | If E gives the epipolar constraint \f$[p_2; 1]^T A^{-T} E A^{-1} [p_1; 1] = 0\f$ between the image |
2649 | points \f$p_1\f$ in the first image and \f$p_2\f$ in second image, then any of the tuples |
2650 | \f$[R_1, t]\f$, \f$[R_1, -t]\f$, \f$[R_2, t]\f$, \f$[R_2, -t]\f$ is a change of basis from the first |
2651 | camera's coordinate system to the second camera's coordinate system. However, by decomposing E, one |
2652 | can only get the direction of the translation. For this reason, the translation t is returned with |
2653 | unit length. |
2654 | */ |
2655 | CV_EXPORTS_W void decomposeEssentialMat( InputArray E, OutputArray R1, OutputArray R2, OutputArray t ); |
2656 | |
2657 | /** @brief Recovers the relative camera rotation and the translation from corresponding points in two images from two different cameras, using cheirality check. Returns the number of |
2658 | inliers that pass the check. |
2659 | |
2660 | @param points1 Array of N 2D points from the first image. The point coordinates should be |
2661 | floating-point (single or double precision). |
2662 | @param points2 Array of the second image points of the same size and format as points1 . |
2663 | @param cameraMatrix1 Input/output camera matrix for the first camera, the same as in |
2664 | @ref calibrateCamera. Furthermore, for the stereo case, additional flags may be used, see below. |
2665 | @param distCoeffs1 Input/output vector of distortion coefficients, the same as in |
2666 | @ref calibrateCamera. |
2667 | @param cameraMatrix2 Input/output camera matrix for the first camera, the same as in |
2668 | @ref calibrateCamera. Furthermore, for the stereo case, additional flags may be used, see below. |
2669 | @param distCoeffs2 Input/output vector of distortion coefficients, the same as in |
2670 | @ref calibrateCamera. |
2671 | @param E The output essential matrix. |
2672 | @param R Output rotation matrix. Together with the translation vector, this matrix makes up a tuple |
2673 | that performs a change of basis from the first camera's coordinate system to the second camera's |
2674 | coordinate system. Note that, in general, t can not be used for this tuple, see the parameter |
2675 | described below. |
2676 | @param t Output translation vector. This vector is obtained by @ref decomposeEssentialMat and |
2677 | therefore is only known up to scale, i.e. t is the direction of the translation vector and has unit |
2678 | length. |
2679 | @param method Method for computing an essential matrix. |
2680 | - @ref RANSAC for the RANSAC algorithm. |
2681 | - @ref LMEDS for the LMedS algorithm. |
2682 | @param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of |
2683 | confidence (probability) that the estimated matrix is correct. |
2684 | @param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar |
2685 | line in pixels, beyond which the point is considered an outlier and is not used for computing the |
2686 | final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the |
2687 | point localization, image resolution, and the image noise. |
2688 | @param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks |
2689 | inliers in points1 and points2 for then given essential matrix E. Only these inliers will be used to |
2690 | recover pose. In the output mask only inliers which pass the cheirality check. |
2691 | |
2692 | This function decomposes an essential matrix using @ref decomposeEssentialMat and then verifies |
2693 | possible pose hypotheses by doing cheirality check. The cheirality check means that the |
2694 | triangulated 3D points should have positive depth. Some details can be found in @cite Nister03. |
2695 | |
2696 | This function can be used to process the output E and mask from @ref findEssentialMat. In this |
2697 | scenario, points1 and points2 are the same input for findEssentialMat.: |
2698 | @code |
2699 | // Example. Estimation of fundamental matrix using the RANSAC algorithm |
2700 | int point_count = 100; |
2701 | vector<Point2f> points1(point_count); |
2702 | vector<Point2f> points2(point_count); |
2703 | |
2704 | // initialize the points here ... |
2705 | for( int i = 0; i < point_count; i++ ) |
2706 | { |
2707 | points1[i] = ...; |
2708 | points2[i] = ...; |
2709 | } |
2710 | |
2711 | // Input: camera calibration of both cameras, for example using intrinsic chessboard calibration. |
2712 | Mat cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2; |
2713 | |
2714 | // Output: Essential matrix, relative rotation and relative translation. |
2715 | Mat E, R, t, mask; |
2716 | |
2717 | recoverPose(points1, points2, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, E, R, t, mask); |
2718 | @endcode |
2719 | */ |
2720 | CV_EXPORTS_W int recoverPose( InputArray points1, InputArray points2, |
2721 | InputArray cameraMatrix1, InputArray distCoeffs1, |
2722 | InputArray cameraMatrix2, InputArray distCoeffs2, |
2723 | OutputArray E, OutputArray R, OutputArray t, |
2724 | int method = cv::RANSAC, double prob = 0.999, double threshold = 1.0, |
2725 | InputOutputArray mask = noArray()); |
2726 | |
2727 | /** @brief Recovers the relative camera rotation and the translation from an estimated essential |
2728 | matrix and the corresponding points in two images, using chirality check. Returns the number of |
2729 | inliers that pass the check. |
2730 | |
2731 | @param E The input essential matrix. |
2732 | @param points1 Array of N 2D points from the first image. The point coordinates should be |
2733 | floating-point (single or double precision). |
2734 | @param points2 Array of the second image points of the same size and format as points1 . |
2735 | @param cameraMatrix Camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
2736 | Note that this function assumes that points1 and points2 are feature points from cameras with the |
2737 | same camera intrinsic matrix. |
2738 | @param R Output rotation matrix. Together with the translation vector, this matrix makes up a tuple |
2739 | that performs a change of basis from the first camera's coordinate system to the second camera's |
2740 | coordinate system. Note that, in general, t can not be used for this tuple, see the parameter |
2741 | described below. |
2742 | @param t Output translation vector. This vector is obtained by @ref decomposeEssentialMat and |
2743 | therefore is only known up to scale, i.e. t is the direction of the translation vector and has unit |
2744 | length. |
2745 | @param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks |
2746 | inliers in points1 and points2 for the given essential matrix E. Only these inliers will be used to |
2747 | recover pose. In the output mask only inliers which pass the chirality check. |
2748 | |
2749 | This function decomposes an essential matrix using @ref decomposeEssentialMat and then verifies |
2750 | possible pose hypotheses by doing chirality check. The chirality check means that the |
2751 | triangulated 3D points should have positive depth. Some details can be found in @cite Nister03. |
2752 | |
2753 | This function can be used to process the output E and mask from @ref findEssentialMat. In this |
2754 | scenario, points1 and points2 are the same input for #findEssentialMat : |
2755 | @code |
2756 | // Example. Estimation of fundamental matrix using the RANSAC algorithm |
2757 | int point_count = 100; |
2758 | vector<Point2f> points1(point_count); |
2759 | vector<Point2f> points2(point_count); |
2760 | |
2761 | // initialize the points here ... |
2762 | for( int i = 0; i < point_count; i++ ) |
2763 | { |
2764 | points1[i] = ...; |
2765 | points2[i] = ...; |
2766 | } |
2767 | |
2768 | // cametra matrix with both focal lengths = 1, and principal point = (0, 0) |
2769 | Mat cameraMatrix = Mat::eye(3, 3, CV_64F); |
2770 | |
2771 | Mat E, R, t, mask; |
2772 | |
2773 | E = findEssentialMat(points1, points2, cameraMatrix, RANSAC, 0.999, 1.0, mask); |
2774 | recoverPose(E, points1, points2, cameraMatrix, R, t, mask); |
2775 | @endcode |
2776 | */ |
2777 | CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2, |
2778 | InputArray cameraMatrix, OutputArray R, OutputArray t, |
2779 | InputOutputArray mask = noArray() ); |
2780 | |
2781 | /** @overload |
2782 | @param E The input essential matrix. |
2783 | @param points1 Array of N 2D points from the first image. The point coordinates should be |
2784 | floating-point (single or double precision). |
2785 | @param points2 Array of the second image points of the same size and format as points1 . |
2786 | @param R Output rotation matrix. Together with the translation vector, this matrix makes up a tuple |
2787 | that performs a change of basis from the first camera's coordinate system to the second camera's |
2788 | coordinate system. Note that, in general, t can not be used for this tuple, see the parameter |
2789 | description below. |
2790 | @param t Output translation vector. This vector is obtained by @ref decomposeEssentialMat and |
2791 | therefore is only known up to scale, i.e. t is the direction of the translation vector and has unit |
2792 | length. |
2793 | @param focal Focal length of the camera. Note that this function assumes that points1 and points2 |
2794 | are feature points from cameras with same focal length and principal point. |
2795 | @param pp principal point of the camera. |
2796 | @param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks |
2797 | inliers in points1 and points2 for the given essential matrix E. Only these inliers will be used to |
2798 | recover pose. In the output mask only inliers which pass the chirality check. |
2799 | |
2800 | This function differs from the one above that it computes camera intrinsic matrix from focal length and |
2801 | principal point: |
2802 | |
2803 | \f[A = |
2804 | \begin{bmatrix} |
2805 | f & 0 & x_{pp} \\ |
2806 | 0 & f & y_{pp} \\ |
2807 | 0 & 0 & 1 |
2808 | \end{bmatrix}\f] |
2809 | */ |
2810 | CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2, |
2811 | OutputArray R, OutputArray t, |
2812 | double focal = 1.0, Point2d pp = Point2d(0, 0), |
2813 | InputOutputArray mask = noArray() ); |
2814 | |
2815 | /** @overload |
2816 | @param E The input essential matrix. |
2817 | @param points1 Array of N 2D points from the first image. The point coordinates should be |
2818 | floating-point (single or double precision). |
2819 | @param points2 Array of the second image points of the same size and format as points1. |
2820 | @param cameraMatrix Camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
2821 | Note that this function assumes that points1 and points2 are feature points from cameras with the |
2822 | same camera intrinsic matrix. |
2823 | @param R Output rotation matrix. Together with the translation vector, this matrix makes up a tuple |
2824 | that performs a change of basis from the first camera's coordinate system to the second camera's |
2825 | coordinate system. Note that, in general, t can not be used for this tuple, see the parameter |
2826 | description below. |
2827 | @param t Output translation vector. This vector is obtained by @ref decomposeEssentialMat and |
2828 | therefore is only known up to scale, i.e. t is the direction of the translation vector and has unit |
2829 | length. |
2830 | @param distanceThresh threshold distance which is used to filter out far away points (i.e. infinite |
2831 | points). |
2832 | @param mask Input/output mask for inliers in points1 and points2. If it is not empty, then it marks |
2833 | inliers in points1 and points2 for the given essential matrix E. Only these inliers will be used to |
2834 | recover pose. In the output mask only inliers which pass the chirality check. |
2835 | @param triangulatedPoints 3D points which were reconstructed by triangulation. |
2836 | |
2837 | This function differs from the one above that it outputs the triangulated 3D point that are used for |
2838 | the chirality check. |
2839 | */ |
2840 | CV_EXPORTS_W int recoverPose( InputArray E, InputArray points1, InputArray points2, |
2841 | InputArray cameraMatrix, OutputArray R, OutputArray t, double distanceThresh, InputOutputArray mask = noArray(), |
2842 | OutputArray triangulatedPoints = noArray()); |
2843 | |
2844 | /** @brief For points in an image of a stereo pair, computes the corresponding epilines in the other image. |
2845 | |
2846 | @param points Input points. \f$N \times 1\f$ or \f$1 \times N\f$ matrix of type CV_32FC2 or |
2847 | vector\<Point2f\> . |
2848 | @param whichImage Index of the image (1 or 2) that contains the points . |
2849 | @param F Fundamental matrix that can be estimated using #findFundamentalMat or #stereoRectify . |
2850 | @param lines Output vector of the epipolar lines corresponding to the points in the other image. |
2851 | Each line \f$ax + by + c=0\f$ is encoded by 3 numbers \f$(a, b, c)\f$ . |
2852 | |
2853 | For every point in one of the two images of a stereo pair, the function finds the equation of the |
2854 | corresponding epipolar line in the other image. |
2855 | |
2856 | From the fundamental matrix definition (see #findFundamentalMat ), line \f$l^{(2)}_i\f$ in the second |
2857 | image for the point \f$p^{(1)}_i\f$ in the first image (when whichImage=1 ) is computed as: |
2858 | |
2859 | \f[l^{(2)}_i = F p^{(1)}_i\f] |
2860 | |
2861 | And vice versa, when whichImage=2, \f$l^{(1)}_i\f$ is computed from \f$p^{(2)}_i\f$ as: |
2862 | |
2863 | \f[l^{(1)}_i = F^T p^{(2)}_i\f] |
2864 | |
2865 | Line coefficients are defined up to a scale. They are normalized so that \f$a_i^2+b_i^2=1\f$ . |
2866 | */ |
2867 | CV_EXPORTS_W void computeCorrespondEpilines( InputArray points, int whichImage, |
2868 | InputArray F, OutputArray lines ); |
2869 | |
2870 | /** @brief This function reconstructs 3-dimensional points (in homogeneous coordinates) by using |
2871 | their observations with a stereo camera. |
2872 | |
2873 | @param projMatr1 3x4 projection matrix of the first camera, i.e. this matrix projects 3D points |
2874 | given in the world's coordinate system into the first image. |
2875 | @param projMatr2 3x4 projection matrix of the second camera, i.e. this matrix projects 3D points |
2876 | given in the world's coordinate system into the second image. |
2877 | @param projPoints1 2xN array of feature points in the first image. In the case of the c++ version, |
2878 | it can be also a vector of feature points or two-channel matrix of size 1xN or Nx1. |
2879 | @param projPoints2 2xN array of corresponding points in the second image. In the case of the c++ |
2880 | version, it can be also a vector of feature points or two-channel matrix of size 1xN or Nx1. |
2881 | @param points4D 4xN array of reconstructed points in homogeneous coordinates. These points are |
2882 | returned in the world's coordinate system. |
2883 | |
2884 | @note |
2885 | Keep in mind that all input data should be of float type in order for this function to work. |
2886 | |
2887 | @note |
2888 | If the projection matrices from @ref stereoRectify are used, then the returned points are |
2889 | represented in the first camera's rectified coordinate system. |
2890 | |
2891 | @sa |
2892 | reprojectImageTo3D |
2893 | */ |
2894 | CV_EXPORTS_W void triangulatePoints( InputArray projMatr1, InputArray projMatr2, |
2895 | InputArray projPoints1, InputArray projPoints2, |
2896 | OutputArray points4D ); |
2897 | |
2898 | /** @brief Refines coordinates of corresponding points. |
2899 | |
2900 | @param F 3x3 fundamental matrix. |
2901 | @param points1 1xN array containing the first set of points. |
2902 | @param points2 1xN array containing the second set of points. |
2903 | @param newPoints1 The optimized points1. |
2904 | @param newPoints2 The optimized points2. |
2905 | |
2906 | The function implements the Optimal Triangulation Method (see Multiple View Geometry @cite HartleyZ00 for details). |
2907 | For each given point correspondence points1[i] \<-\> points2[i], and a fundamental matrix F, it |
2908 | computes the corrected correspondences newPoints1[i] \<-\> newPoints2[i] that minimize the geometric |
2909 | error \f$d(points1[i], newPoints1[i])^2 + d(points2[i],newPoints2[i])^2\f$ (where \f$d(a,b)\f$ is the |
2910 | geometric distance between points \f$a\f$ and \f$b\f$ ) subject to the epipolar constraint |
2911 | \f$newPoints2^T \cdot F \cdot newPoints1 = 0\f$ . |
2912 | */ |
2913 | CV_EXPORTS_W void correctMatches( InputArray F, InputArray points1, InputArray points2, |
2914 | OutputArray newPoints1, OutputArray newPoints2 ); |
2915 | |
2916 | /** @brief Filters off small noise blobs (speckles) in the disparity map |
2917 | |
2918 | @param img The input 16-bit signed disparity image |
2919 | @param newVal The disparity value used to paint-off the speckles |
2920 | @param maxSpeckleSize The maximum speckle size to consider it a speckle. Larger blobs are not |
2921 | affected by the algorithm |
2922 | @param maxDiff Maximum difference between neighbor disparity pixels to put them into the same |
2923 | blob. Note that since StereoBM, StereoSGBM and may be other algorithms return a fixed-point |
2924 | disparity map, where disparity values are multiplied by 16, this scale factor should be taken into |
2925 | account when specifying this parameter value. |
2926 | @param buf The optional temporary buffer to avoid memory allocation within the function. |
2927 | */ |
2928 | CV_EXPORTS_W void filterSpeckles( InputOutputArray img, double newVal, |
2929 | int maxSpeckleSize, double maxDiff, |
2930 | InputOutputArray buf = noArray() ); |
2931 | |
2932 | //! computes valid disparity ROI from the valid ROIs of the rectified images (that are returned by #stereoRectify) |
2933 | CV_EXPORTS_W Rect getValidDisparityROI( Rect roi1, Rect roi2, |
2934 | int minDisparity, int numberOfDisparities, |
2935 | int blockSize ); |
2936 | |
2937 | //! validates disparity using the left-right check. The matrix "cost" should be computed by the stereo correspondence algorithm |
2938 | CV_EXPORTS_W void validateDisparity( InputOutputArray disparity, InputArray cost, |
2939 | int minDisparity, int numberOfDisparities, |
2940 | int disp12MaxDisp = 1 ); |
2941 | |
2942 | /** @brief Reprojects a disparity image to 3D space. |
2943 | |
2944 | @param disparity Input single-channel 8-bit unsigned, 16-bit signed, 32-bit signed or 32-bit |
2945 | floating-point disparity image. The values of 8-bit / 16-bit signed formats are assumed to have no |
2946 | fractional bits. If the disparity is 16-bit signed format, as computed by @ref StereoBM or |
2947 | @ref StereoSGBM and maybe other algorithms, it should be divided by 16 (and scaled to float) before |
2948 | being used here. |
2949 | @param _3dImage Output 3-channel floating-point image of the same size as disparity. Each element of |
2950 | _3dImage(x,y) contains 3D coordinates of the point (x,y) computed from the disparity map. If one |
2951 | uses Q obtained by @ref stereoRectify, then the returned points are represented in the first |
2952 | camera's rectified coordinate system. |
2953 | @param Q \f$4 \times 4\f$ perspective transformation matrix that can be obtained with |
2954 | @ref stereoRectify. |
2955 | @param handleMissingValues Indicates, whether the function should handle missing values (i.e. |
2956 | points where the disparity was not computed). If handleMissingValues=true, then pixels with the |
2957 | minimal disparity that corresponds to the outliers (see StereoMatcher::compute ) are transformed |
2958 | to 3D points with a very large Z value (currently set to 10000). |
2959 | @param ddepth The optional output array depth. If it is -1, the output image will have CV_32F |
2960 | depth. ddepth can also be set to CV_16S, CV_32S or CV_32F. |
2961 | |
2962 | The function transforms a single-channel disparity map to a 3-channel image representing a 3D |
2963 | surface. That is, for each pixel (x,y) and the corresponding disparity d=disparity(x,y) , it |
2964 | computes: |
2965 | |
2966 | \f[\begin{bmatrix} |
2967 | X \\ |
2968 | Y \\ |
2969 | Z \\ |
2970 | W |
2971 | \end{bmatrix} = Q \begin{bmatrix} |
2972 | x \\ |
2973 | y \\ |
2974 | \texttt{disparity} (x,y) \\ |
2975 | 1 |
2976 | \end{bmatrix}.\f] |
2977 | |
2978 | @sa |
2979 | To reproject a sparse set of points {(x,y,d),...} to 3D space, use perspectiveTransform. |
2980 | */ |
2981 | CV_EXPORTS_W void reprojectImageTo3D( InputArray disparity, |
2982 | OutputArray _3dImage, InputArray Q, |
2983 | bool handleMissingValues = false, |
2984 | int ddepth = -1 ); |
2985 | |
2986 | /** @brief Calculates the Sampson Distance between two points. |
2987 | |
2988 | The function cv::sampsonDistance calculates and returns the first order approximation of the geometric error as: |
2989 | \f[ |
2990 | sd( \texttt{pt1} , \texttt{pt2} )= |
2991 | \frac{(\texttt{pt2}^t \cdot \texttt{F} \cdot \texttt{pt1})^2} |
2992 | {((\texttt{F} \cdot \texttt{pt1})(0))^2 + |
2993 | ((\texttt{F} \cdot \texttt{pt1})(1))^2 + |
2994 | ((\texttt{F}^t \cdot \texttt{pt2})(0))^2 + |
2995 | ((\texttt{F}^t \cdot \texttt{pt2})(1))^2} |
2996 | \f] |
2997 | The fundamental matrix may be calculated using the #findFundamentalMat function. See @cite HartleyZ00 11.4.3 for details. |
2998 | @param pt1 first homogeneous 2d point |
2999 | @param pt2 second homogeneous 2d point |
3000 | @param F fundamental matrix |
3001 | @return The computed Sampson distance. |
3002 | */ |
3003 | CV_EXPORTS_W double sampsonDistance(InputArray pt1, InputArray pt2, InputArray F); |
3004 | |
3005 | /** @brief Computes an optimal affine transformation between two 3D point sets. |
3006 | |
3007 | It computes |
3008 | \f[ |
3009 | \begin{bmatrix} |
3010 | x\\ |
3011 | y\\ |
3012 | z\\ |
3013 | \end{bmatrix} |
3014 | = |
3015 | \begin{bmatrix} |
3016 | a_{11} & a_{12} & a_{13}\\ |
3017 | a_{21} & a_{22} & a_{23}\\ |
3018 | a_{31} & a_{32} & a_{33}\\ |
3019 | \end{bmatrix} |
3020 | \begin{bmatrix} |
3021 | X\\ |
3022 | Y\\ |
3023 | Z\\ |
3024 | \end{bmatrix} |
3025 | + |
3026 | \begin{bmatrix} |
3027 | b_1\\ |
3028 | b_2\\ |
3029 | b_3\\ |
3030 | \end{bmatrix} |
3031 | \f] |
3032 | |
3033 | @param src First input 3D point set containing \f$(X,Y,Z)\f$. |
3034 | @param dst Second input 3D point set containing \f$(x,y,z)\f$. |
3035 | @param out Output 3D affine transformation matrix \f$3 \times 4\f$ of the form |
3036 | \f[ |
3037 | \begin{bmatrix} |
3038 | a_{11} & a_{12} & a_{13} & b_1\\ |
3039 | a_{21} & a_{22} & a_{23} & b_2\\ |
3040 | a_{31} & a_{32} & a_{33} & b_3\\ |
3041 | \end{bmatrix} |
3042 | \f] |
3043 | @param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier). |
3044 | @param ransacThreshold Maximum reprojection error in the RANSAC algorithm to consider a point as |
3045 | an inlier. |
3046 | @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything |
3047 | between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation |
3048 | significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation. |
3049 | |
3050 | The function estimates an optimal 3D affine transformation between two 3D point sets using the |
3051 | RANSAC algorithm. |
3052 | */ |
3053 | CV_EXPORTS_W int estimateAffine3D(InputArray src, InputArray dst, |
3054 | OutputArray out, OutputArray inliers, |
3055 | double ransacThreshold = 3, double confidence = 0.99); |
3056 | |
3057 | /** @brief Computes an optimal affine transformation between two 3D point sets. |
3058 | |
3059 | It computes \f$R,s,t\f$ minimizing \f$\sum{i} dst_i - c \cdot R \cdot src_i \f$ |
3060 | where \f$R\f$ is a 3x3 rotation matrix, \f$t\f$ is a 3x1 translation vector and \f$s\f$ is a |
3061 | scalar size value. This is an implementation of the algorithm by Umeyama \cite umeyama1991least . |
3062 | The estimated affine transform has a homogeneous scale which is a subclass of affine |
3063 | transformations with 7 degrees of freedom. The paired point sets need to comprise at least 3 |
3064 | points each. |
3065 | |
3066 | @param src First input 3D point set. |
3067 | @param dst Second input 3D point set. |
3068 | @param scale If null is passed, the scale parameter c will be assumed to be 1.0. |
3069 | Else the pointed-to variable will be set to the optimal scale. |
3070 | @param force_rotation If true, the returned rotation will never be a reflection. |
3071 | This might be unwanted, e.g. when optimizing a transform between a right- and a |
3072 | left-handed coordinate system. |
3073 | @return 3D affine transformation matrix \f$3 \times 4\f$ of the form |
3074 | \f[T = |
3075 | \begin{bmatrix} |
3076 | R & t\\ |
3077 | \end{bmatrix} |
3078 | \f] |
3079 | |
3080 | */ |
3081 | CV_EXPORTS_W cv::Mat estimateAffine3D(InputArray src, InputArray dst, |
3082 | CV_OUT double* scale = nullptr, bool force_rotation = true); |
3083 | |
3084 | /** @brief Computes an optimal translation between two 3D point sets. |
3085 | * |
3086 | * It computes |
3087 | * \f[ |
3088 | * \begin{bmatrix} |
3089 | * x\\ |
3090 | * y\\ |
3091 | * z\\ |
3092 | * \end{bmatrix} |
3093 | * = |
3094 | * \begin{bmatrix} |
3095 | * X\\ |
3096 | * Y\\ |
3097 | * Z\\ |
3098 | * \end{bmatrix} |
3099 | * + |
3100 | * \begin{bmatrix} |
3101 | * b_1\\ |
3102 | * b_2\\ |
3103 | * b_3\\ |
3104 | * \end{bmatrix} |
3105 | * \f] |
3106 | * |
3107 | * @param src First input 3D point set containing \f$(X,Y,Z)\f$. |
3108 | * @param dst Second input 3D point set containing \f$(x,y,z)\f$. |
3109 | * @param out Output 3D translation vector \f$3 \times 1\f$ of the form |
3110 | * \f[ |
3111 | * \begin{bmatrix} |
3112 | * b_1 \\ |
3113 | * b_2 \\ |
3114 | * b_3 \\ |
3115 | * \end{bmatrix} |
3116 | * \f] |
3117 | * @param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier). |
3118 | * @param ransacThreshold Maximum reprojection error in the RANSAC algorithm to consider a point as |
3119 | * an inlier. |
3120 | * @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything |
3121 | * between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation |
3122 | * significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation. |
3123 | * |
3124 | * The function estimates an optimal 3D translation between two 3D point sets using the |
3125 | * RANSAC algorithm. |
3126 | * */ |
3127 | CV_EXPORTS_W int estimateTranslation3D(InputArray src, InputArray dst, |
3128 | OutputArray out, OutputArray inliers, |
3129 | double ransacThreshold = 3, double confidence = 0.99); |
3130 | |
3131 | /** @brief Computes an optimal affine transformation between two 2D point sets. |
3132 | |
3133 | It computes |
3134 | \f[ |
3135 | \begin{bmatrix} |
3136 | x\\ |
3137 | y\\ |
3138 | \end{bmatrix} |
3139 | = |
3140 | \begin{bmatrix} |
3141 | a_{11} & a_{12}\\ |
3142 | a_{21} & a_{22}\\ |
3143 | \end{bmatrix} |
3144 | \begin{bmatrix} |
3145 | X\\ |
3146 | Y\\ |
3147 | \end{bmatrix} |
3148 | + |
3149 | \begin{bmatrix} |
3150 | b_1\\ |
3151 | b_2\\ |
3152 | \end{bmatrix} |
3153 | \f] |
3154 | |
3155 | @param from First input 2D point set containing \f$(X,Y)\f$. |
3156 | @param to Second input 2D point set containing \f$(x,y)\f$. |
3157 | @param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier). |
3158 | @param method Robust method used to compute transformation. The following methods are possible: |
3159 | - @ref RANSAC - RANSAC-based robust method |
3160 | - @ref LMEDS - Least-Median robust method |
3161 | RANSAC is the default method. |
3162 | @param ransacReprojThreshold Maximum reprojection error in the RANSAC algorithm to consider |
3163 | a point as an inlier. Applies only to RANSAC. |
3164 | @param maxIters The maximum number of robust method iterations. |
3165 | @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything |
3166 | between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation |
3167 | significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation. |
3168 | @param refineIters Maximum number of iterations of refining algorithm (Levenberg-Marquardt). |
3169 | Passing 0 will disable refining, so the output matrix will be output of robust method. |
3170 | |
3171 | @return Output 2D affine transformation matrix \f$2 \times 3\f$ or empty matrix if transformation |
3172 | could not be estimated. The returned matrix has the following form: |
3173 | \f[ |
3174 | \begin{bmatrix} |
3175 | a_{11} & a_{12} & b_1\\ |
3176 | a_{21} & a_{22} & b_2\\ |
3177 | \end{bmatrix} |
3178 | \f] |
3179 | |
3180 | The function estimates an optimal 2D affine transformation between two 2D point sets using the |
3181 | selected robust algorithm. |
3182 | |
3183 | The computed transformation is then refined further (using only inliers) with the |
3184 | Levenberg-Marquardt method to reduce the re-projection error even more. |
3185 | |
3186 | @note |
3187 | The RANSAC method can handle practically any ratio of outliers but needs a threshold to |
3188 | distinguish inliers from outliers. The method LMeDS does not need any threshold but it works |
3189 | correctly only when there are more than 50% of inliers. |
3190 | |
3191 | @sa estimateAffinePartial2D, getAffineTransform |
3192 | */ |
3193 | CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers = noArray(), |
3194 | int method = RANSAC, double ransacReprojThreshold = 3, |
3195 | size_t maxIters = 2000, double confidence = 0.99, |
3196 | size_t refineIters = 10); |
3197 | |
3198 | |
3199 | CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray pts1, InputArray pts2, OutputArray inliers, |
3200 | const UsacParams ¶ms); |
3201 | |
3202 | /** @brief Computes an optimal limited affine transformation with 4 degrees of freedom between |
3203 | two 2D point sets. |
3204 | |
3205 | @param from First input 2D point set. |
3206 | @param to Second input 2D point set. |
3207 | @param inliers Output vector indicating which points are inliers. |
3208 | @param method Robust method used to compute transformation. The following methods are possible: |
3209 | - @ref RANSAC - RANSAC-based robust method |
3210 | - @ref LMEDS - Least-Median robust method |
3211 | RANSAC is the default method. |
3212 | @param ransacReprojThreshold Maximum reprojection error in the RANSAC algorithm to consider |
3213 | a point as an inlier. Applies only to RANSAC. |
3214 | @param maxIters The maximum number of robust method iterations. |
3215 | @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything |
3216 | between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation |
3217 | significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation. |
3218 | @param refineIters Maximum number of iterations of refining algorithm (Levenberg-Marquardt). |
3219 | Passing 0 will disable refining, so the output matrix will be output of robust method. |
3220 | |
3221 | @return Output 2D affine transformation (4 degrees of freedom) matrix \f$2 \times 3\f$ or |
3222 | empty matrix if transformation could not be estimated. |
3223 | |
3224 | The function estimates an optimal 2D affine transformation with 4 degrees of freedom limited to |
3225 | combinations of translation, rotation, and uniform scaling. Uses the selected algorithm for robust |
3226 | estimation. |
3227 | |
3228 | The computed transformation is then refined further (using only inliers) with the |
3229 | Levenberg-Marquardt method to reduce the re-projection error even more. |
3230 | |
3231 | Estimated transformation matrix is: |
3232 | \f[ \begin{bmatrix} \cos(\theta) \cdot s & -\sin(\theta) \cdot s & t_x \\ |
3233 | \sin(\theta) \cdot s & \cos(\theta) \cdot s & t_y |
3234 | \end{bmatrix} \f] |
3235 | Where \f$ \theta \f$ is the rotation angle, \f$ s \f$ the scaling factor and \f$ t_x, t_y \f$ are |
3236 | translations in \f$ x, y \f$ axes respectively. |
3237 | |
3238 | @note |
3239 | The RANSAC method can handle practically any ratio of outliers but need a threshold to |
3240 | distinguish inliers from outliers. The method LMeDS does not need any threshold but it works |
3241 | correctly only when there are more than 50% of inliers. |
3242 | |
3243 | @sa estimateAffine2D, getAffineTransform |
3244 | */ |
3245 | CV_EXPORTS_W cv::Mat estimateAffinePartial2D(InputArray from, InputArray to, OutputArray inliers = noArray(), |
3246 | int method = RANSAC, double ransacReprojThreshold = 3, |
3247 | size_t maxIters = 2000, double confidence = 0.99, |
3248 | size_t refineIters = 10); |
3249 | |
3250 | /** @example samples/cpp/tutorial_code/features2D/Homography/decompose_homography.cpp |
3251 | An example program with homography decomposition. |
3252 | |
3253 | Check @ref tutorial_homography "the corresponding tutorial" for more details. |
3254 | */ |
3255 | |
3256 | /** @brief Decompose a homography matrix to rotation(s), translation(s) and plane normal(s). |
3257 | |
3258 | @param H The input homography matrix between two images. |
3259 | @param K The input camera intrinsic matrix. |
3260 | @param rotations Array of rotation matrices. |
3261 | @param translations Array of translation matrices. |
3262 | @param normals Array of plane normal matrices. |
3263 | |
3264 | This function extracts relative camera motion between two views of a planar object and returns up to |
3265 | four mathematical solution tuples of rotation, translation, and plane normal. The decomposition of |
3266 | the homography matrix H is described in detail in @cite Malis2007. |
3267 | |
3268 | If the homography H, induced by the plane, gives the constraint |
3269 | \f[s_i \vecthree{x'_i}{y'_i}{1} \sim H \vecthree{x_i}{y_i}{1}\f] on the source image points |
3270 | \f$p_i\f$ and the destination image points \f$p'_i\f$, then the tuple of rotations[k] and |
3271 | translations[k] is a change of basis from the source camera's coordinate system to the destination |
3272 | camera's coordinate system. However, by decomposing H, one can only get the translation normalized |
3273 | by the (typically unknown) depth of the scene, i.e. its direction but with normalized length. |
3274 | |
3275 | If point correspondences are available, at least two solutions may further be invalidated, by |
3276 | applying positive depth constraint, i.e. all points must be in front of the camera. |
3277 | */ |
3278 | CV_EXPORTS_W int decomposeHomographyMat(InputArray H, |
3279 | InputArray K, |
3280 | OutputArrayOfArrays rotations, |
3281 | OutputArrayOfArrays translations, |
3282 | OutputArrayOfArrays normals); |
3283 | |
3284 | /** @brief Filters homography decompositions based on additional information. |
3285 | |
3286 | @param rotations Vector of rotation matrices. |
3287 | @param normals Vector of plane normal matrices. |
3288 | @param beforePoints Vector of (rectified) visible reference points before the homography is applied |
3289 | @param afterPoints Vector of (rectified) visible reference points after the homography is applied |
3290 | @param possibleSolutions Vector of int indices representing the viable solution set after filtering |
3291 | @param pointsMask optional Mat/Vector of 8u type representing the mask for the inliers as given by the #findHomography function |
3292 | |
3293 | This function is intended to filter the output of the #decomposeHomographyMat based on additional |
3294 | information as described in @cite Malis2007 . The summary of the method: the #decomposeHomographyMat function |
3295 | returns 2 unique solutions and their "opposites" for a total of 4 solutions. If we have access to the |
3296 | sets of points visible in the camera frame before and after the homography transformation is applied, |
3297 | we can determine which are the true potential solutions and which are the opposites by verifying which |
3298 | homographies are consistent with all visible reference points being in front of the camera. The inputs |
3299 | are left unchanged; the filtered solution set is returned as indices into the existing one. |
3300 | |
3301 | */ |
3302 | CV_EXPORTS_W void filterHomographyDecompByVisibleRefpoints(InputArrayOfArrays rotations, |
3303 | InputArrayOfArrays normals, |
3304 | InputArray beforePoints, |
3305 | InputArray afterPoints, |
3306 | OutputArray possibleSolutions, |
3307 | InputArray pointsMask = noArray()); |
3308 | |
3309 | /** @brief The base class for stereo correspondence algorithms. |
3310 | */ |
3311 | class CV_EXPORTS_W StereoMatcher : public Algorithm |
3312 | { |
3313 | public: |
3314 | enum { DISP_SHIFT = 4, |
3315 | DISP_SCALE = (1 << DISP_SHIFT) |
3316 | }; |
3317 | |
3318 | /** @brief Computes disparity map for the specified stereo pair |
3319 | |
3320 | @param left Left 8-bit single-channel image. |
3321 | @param right Right image of the same size and the same type as the left one. |
3322 | @param disparity Output disparity map. It has the same size as the input images. Some algorithms, |
3323 | like StereoBM or StereoSGBM compute 16-bit fixed-point disparity map (where each disparity value |
3324 | has 4 fractional bits), whereas other algorithms output 32-bit floating-point disparity map. |
3325 | */ |
3326 | CV_WRAP virtual void compute( InputArray left, InputArray right, |
3327 | OutputArray disparity ) = 0; |
3328 | |
3329 | CV_WRAP virtual int getMinDisparity() const = 0; |
3330 | CV_WRAP virtual void setMinDisparity(int minDisparity) = 0; |
3331 | |
3332 | CV_WRAP virtual int getNumDisparities() const = 0; |
3333 | CV_WRAP virtual void setNumDisparities(int numDisparities) = 0; |
3334 | |
3335 | CV_WRAP virtual int getBlockSize() const = 0; |
3336 | CV_WRAP virtual void setBlockSize(int blockSize) = 0; |
3337 | |
3338 | CV_WRAP virtual int getSpeckleWindowSize() const = 0; |
3339 | CV_WRAP virtual void setSpeckleWindowSize(int speckleWindowSize) = 0; |
3340 | |
3341 | CV_WRAP virtual int getSpeckleRange() const = 0; |
3342 | CV_WRAP virtual void setSpeckleRange(int speckleRange) = 0; |
3343 | |
3344 | CV_WRAP virtual int getDisp12MaxDiff() const = 0; |
3345 | CV_WRAP virtual void setDisp12MaxDiff(int disp12MaxDiff) = 0; |
3346 | }; |
3347 | |
3348 | |
3349 | /** @brief Class for computing stereo correspondence using the block matching algorithm, introduced and |
3350 | contributed to OpenCV by K. Konolige. |
3351 | */ |
3352 | class CV_EXPORTS_W StereoBM : public StereoMatcher |
3353 | { |
3354 | public: |
3355 | enum { PREFILTER_NORMALIZED_RESPONSE = 0, |
3356 | PREFILTER_XSOBEL = 1 |
3357 | }; |
3358 | |
3359 | CV_WRAP virtual int getPreFilterType() const = 0; |
3360 | CV_WRAP virtual void setPreFilterType(int preFilterType) = 0; |
3361 | |
3362 | CV_WRAP virtual int getPreFilterSize() const = 0; |
3363 | CV_WRAP virtual void setPreFilterSize(int preFilterSize) = 0; |
3364 | |
3365 | CV_WRAP virtual int getPreFilterCap() const = 0; |
3366 | CV_WRAP virtual void setPreFilterCap(int preFilterCap) = 0; |
3367 | |
3368 | CV_WRAP virtual int getTextureThreshold() const = 0; |
3369 | CV_WRAP virtual void setTextureThreshold(int textureThreshold) = 0; |
3370 | |
3371 | CV_WRAP virtual int getUniquenessRatio() const = 0; |
3372 | CV_WRAP virtual void setUniquenessRatio(int uniquenessRatio) = 0; |
3373 | |
3374 | CV_WRAP virtual int getSmallerBlockSize() const = 0; |
3375 | CV_WRAP virtual void setSmallerBlockSize(int blockSize) = 0; |
3376 | |
3377 | CV_WRAP virtual Rect getROI1() const = 0; |
3378 | CV_WRAP virtual void setROI1(Rect roi1) = 0; |
3379 | |
3380 | CV_WRAP virtual Rect getROI2() const = 0; |
3381 | CV_WRAP virtual void setROI2(Rect roi2) = 0; |
3382 | |
3383 | /** @brief Creates StereoBM object |
3384 | |
3385 | @param numDisparities the disparity search range. For each pixel algorithm will find the best |
3386 | disparity from 0 (default minimum disparity) to numDisparities. The search range can then be |
3387 | shifted by changing the minimum disparity. |
3388 | @param blockSize the linear size of the blocks compared by the algorithm. The size should be odd |
3389 | (as the block is centered at the current pixel). Larger block size implies smoother, though less |
3390 | accurate disparity map. Smaller block size gives more detailed disparity map, but there is higher |
3391 | chance for algorithm to find a wrong correspondence. |
3392 | |
3393 | The function create StereoBM object. You can then call StereoBM::compute() to compute disparity for |
3394 | a specific stereo pair. |
3395 | */ |
3396 | CV_WRAP static Ptr<StereoBM> create(int numDisparities = 0, int blockSize = 21); |
3397 | }; |
3398 | |
3399 | /** @brief The class implements the modified H. Hirschmuller algorithm @cite HH08 that differs from the original |
3400 | one as follows: |
3401 | |
3402 | - By default, the algorithm is single-pass, which means that you consider only 5 directions |
3403 | instead of 8. Set mode=StereoSGBM::MODE_HH in createStereoSGBM to run the full variant of the |
3404 | algorithm but beware that it may consume a lot of memory. |
3405 | - The algorithm matches blocks, not individual pixels. Though, setting blockSize=1 reduces the |
3406 | blocks to single pixels. |
3407 | - Mutual information cost function is not implemented. Instead, a simpler Birchfield-Tomasi |
3408 | sub-pixel metric from @cite BT98 is used. Though, the color images are supported as well. |
3409 | - Some pre- and post- processing steps from K. Konolige algorithm StereoBM are included, for |
3410 | example: pre-filtering (StereoBM::PREFILTER_XSOBEL type) and post-filtering (uniqueness |
3411 | check, quadratic interpolation and speckle filtering). |
3412 | |
3413 | @note |
3414 | - (Python) An example illustrating the use of the StereoSGBM matching algorithm can be found |
3415 | at opencv_source_code/samples/python/stereo_match.py |
3416 | */ |
3417 | class CV_EXPORTS_W StereoSGBM : public StereoMatcher |
3418 | { |
3419 | public: |
3420 | enum |
3421 | { |
3422 | MODE_SGBM = 0, |
3423 | MODE_HH = 1, |
3424 | MODE_SGBM_3WAY = 2, |
3425 | MODE_HH4 = 3 |
3426 | }; |
3427 | |
3428 | CV_WRAP virtual int getPreFilterCap() const = 0; |
3429 | CV_WRAP virtual void setPreFilterCap(int preFilterCap) = 0; |
3430 | |
3431 | CV_WRAP virtual int getUniquenessRatio() const = 0; |
3432 | CV_WRAP virtual void setUniquenessRatio(int uniquenessRatio) = 0; |
3433 | |
3434 | CV_WRAP virtual int getP1() const = 0; |
3435 | CV_WRAP virtual void setP1(int P1) = 0; |
3436 | |
3437 | CV_WRAP virtual int getP2() const = 0; |
3438 | CV_WRAP virtual void setP2(int P2) = 0; |
3439 | |
3440 | CV_WRAP virtual int getMode() const = 0; |
3441 | CV_WRAP virtual void setMode(int mode) = 0; |
3442 | |
3443 | /** @brief Creates StereoSGBM object |
3444 | |
3445 | @param minDisparity Minimum possible disparity value. Normally, it is zero but sometimes |
3446 | rectification algorithms can shift images, so this parameter needs to be adjusted accordingly. |
3447 | @param numDisparities Maximum disparity minus minimum disparity. The value is always greater than |
3448 | zero. In the current implementation, this parameter must be divisible by 16. |
3449 | @param blockSize Matched block size. It must be an odd number \>=1 . Normally, it should be |
3450 | somewhere in the 3..11 range. |
3451 | @param P1 The first parameter controlling the disparity smoothness. See below. |
3452 | @param P2 The second parameter controlling the disparity smoothness. The larger the values are, |
3453 | the smoother the disparity is. P1 is the penalty on the disparity change by plus or minus 1 |
3454 | between neighbor pixels. P2 is the penalty on the disparity change by more than 1 between neighbor |
3455 | pixels. The algorithm requires P2 \> P1 . See stereo_match.cpp sample where some reasonably good |
3456 | P1 and P2 values are shown (like 8\*number_of_image_channels\*blockSize\*blockSize and |
3457 | 32\*number_of_image_channels\*blockSize\*blockSize , respectively). |
3458 | @param disp12MaxDiff Maximum allowed difference (in integer pixel units) in the left-right |
3459 | disparity check. Set it to a non-positive value to disable the check. |
3460 | @param preFilterCap Truncation value for the prefiltered image pixels. The algorithm first |
3461 | computes x-derivative at each pixel and clips its value by [-preFilterCap, preFilterCap] interval. |
3462 | The result values are passed to the Birchfield-Tomasi pixel cost function. |
3463 | @param uniquenessRatio Margin in percentage by which the best (minimum) computed cost function |
3464 | value should "win" the second best value to consider the found match correct. Normally, a value |
3465 | within the 5-15 range is good enough. |
3466 | @param speckleWindowSize Maximum size of smooth disparity regions to consider their noise speckles |
3467 | and invalidate. Set it to 0 to disable speckle filtering. Otherwise, set it somewhere in the |
3468 | 50-200 range. |
3469 | @param speckleRange Maximum disparity variation within each connected component. If you do speckle |
3470 | filtering, set the parameter to a positive value, it will be implicitly multiplied by 16. |
3471 | Normally, 1 or 2 is good enough. |
3472 | @param mode Set it to StereoSGBM::MODE_HH to run the full-scale two-pass dynamic programming |
3473 | algorithm. It will consume O(W\*H\*numDisparities) bytes, which is large for 640x480 stereo and |
3474 | huge for HD-size pictures. By default, it is set to false . |
3475 | |
3476 | The first constructor initializes StereoSGBM with all the default parameters. So, you only have to |
3477 | set StereoSGBM::numDisparities at minimum. The second constructor enables you to set each parameter |
3478 | to a custom value. |
3479 | */ |
3480 | CV_WRAP static Ptr<StereoSGBM> create(int minDisparity = 0, int numDisparities = 16, int blockSize = 3, |
3481 | int P1 = 0, int P2 = 0, int disp12MaxDiff = 0, |
3482 | int preFilterCap = 0, int uniquenessRatio = 0, |
3483 | int speckleWindowSize = 0, int speckleRange = 0, |
3484 | int mode = StereoSGBM::MODE_SGBM); |
3485 | }; |
3486 | |
3487 | |
3488 | //! cv::undistort mode |
3489 | enum UndistortTypes |
3490 | { |
3491 | PROJ_SPHERICAL_ORTHO = 0, |
3492 | PROJ_SPHERICAL_EQRECT = 1 |
3493 | }; |
3494 | |
3495 | /** @brief Transforms an image to compensate for lens distortion. |
3496 | |
3497 | The function transforms an image to compensate radial and tangential lens distortion. |
3498 | |
3499 | The function is simply a combination of #initUndistortRectifyMap (with unity R ) and #remap |
3500 | (with bilinear interpolation). See the former function for details of the transformation being |
3501 | performed. |
3502 | |
3503 | Those pixels in the destination image, for which there is no correspondent pixels in the source |
3504 | image, are filled with zeros (black color). |
3505 | |
3506 | A particular subset of the source image that will be visible in the corrected image can be regulated |
3507 | by newCameraMatrix. You can use #getOptimalNewCameraMatrix to compute the appropriate |
3508 | newCameraMatrix depending on your requirements. |
3509 | |
3510 | The camera matrix and the distortion parameters can be determined using #calibrateCamera. If |
3511 | the resolution of images is different from the resolution used at the calibration stage, \f$f_x, |
3512 | f_y, c_x\f$ and \f$c_y\f$ need to be scaled accordingly, while the distortion coefficients remain |
3513 | the same. |
3514 | |
3515 | @param src Input (distorted) image. |
3516 | @param dst Output (corrected) image that has the same size and type as src . |
3517 | @param cameraMatrix Input camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . |
3518 | @param distCoeffs Input vector of distortion coefficients |
3519 | \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ |
3520 | of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. |
3521 | @param newCameraMatrix Camera matrix of the distorted image. By default, it is the same as |
3522 | cameraMatrix but you may additionally scale and shift the result by using a different matrix. |
3523 | */ |
3524 | CV_EXPORTS_W void undistort( InputArray src, OutputArray dst, |
3525 | InputArray cameraMatrix, |
3526 | InputArray distCoeffs, |
3527 | InputArray newCameraMatrix = noArray() ); |
3528 | |
3529 | /** @brief Computes the undistortion and rectification transformation map. |
3530 | |
3531 | The function computes the joint undistortion and rectification transformation and represents the |
3532 | result in the form of maps for #remap. The undistorted image looks like original, as if it is |
3533 | captured with a camera using the camera matrix =newCameraMatrix and zero distortion. In case of a |
3534 | monocular camera, newCameraMatrix is usually equal to cameraMatrix, or it can be computed by |
3535 | #getOptimalNewCameraMatrix for a better control over scaling. In case of a stereo camera, |
3536 | newCameraMatrix is normally set to P1 or P2 computed by #stereoRectify . |
3537 | |
3538 | Also, this new camera is oriented differently in the coordinate space, according to R. That, for |
3539 | example, helps to align two heads of a stereo camera so that the epipolar lines on both images |
3540 | become horizontal and have the same y- coordinate (in case of a horizontally aligned stereo camera). |
3541 | |
3542 | The function actually builds the maps for the inverse mapping algorithm that is used by #remap. That |
3543 | is, for each pixel \f$(u, v)\f$ in the destination (corrected and rectified) image, the function |
3544 | computes the corresponding coordinates in the source image (that is, in the original image from |
3545 | camera). The following process is applied: |
3546 | \f[ |
3547 | \begin{array}{l} |
3548 | x \leftarrow (u - {c'}_x)/{f'}_x \\ |
3549 | y \leftarrow (v - {c'}_y)/{f'}_y \\ |
3550 | {[X\,Y\,W]} ^T \leftarrow R^{-1}*[x \, y \, 1]^T \\ |
3551 | x' \leftarrow X/W \\ |
3552 | y' \leftarrow Y/W \\ |
3553 | r^2 \leftarrow x'^2 + y'^2 \\ |
3554 | x'' \leftarrow x' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} |
3555 | + 2p_1 x' y' + p_2(r^2 + 2 x'^2) + s_1 r^2 + s_2 r^4\\ |
3556 | y'' \leftarrow y' \frac{1 + k_1 r^2 + k_2 r^4 + k_3 r^6}{1 + k_4 r^2 + k_5 r^4 + k_6 r^6} |
3557 | + p_1 (r^2 + 2 y'^2) + 2 p_2 x' y' + s_3 r^2 + s_4 r^4 \\ |
3558 | s\vecthree{x'''}{y'''}{1} = |
3559 | \vecthreethree{R_{33}(\tau_x, \tau_y)}{0}{-R_{13}((\tau_x, \tau_y)} |
3560 | {0}{R_{33}(\tau_x, \tau_y)}{-R_{23}(\tau_x, \tau_y)} |
3561 | {0}{0}{1} R(\tau_x, \tau_y) \vecthree{x''}{y''}{1}\\ |
3562 | map_x(u,v) \leftarrow x''' f_x + c_x \\ |
3563 | map_y(u,v) \leftarrow y''' f_y + c_y |
3564 | \end{array} |
3565 | \f] |
3566 | where \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ |
3567 | are the distortion coefficients. |
3568 | |
3569 | In case of a stereo camera, this function is called twice: once for each camera head, after |
3570 | #stereoRectify, which in its turn is called after #stereoCalibrate. But if the stereo camera |
3571 | was not calibrated, it is still possible to compute the rectification transformations directly from |
3572 | the fundamental matrix using #stereoRectifyUncalibrated. For each camera, the function computes |
3573 | homography H as the rectification transformation in a pixel domain, not a rotation matrix R in 3D |
3574 | space. R can be computed from H as |
3575 | \f[\texttt{R} = \texttt{cameraMatrix} ^{-1} \cdot \texttt{H} \cdot \texttt{cameraMatrix}\f] |
3576 | where cameraMatrix can be chosen arbitrarily. |
3577 | |
3578 | @param cameraMatrix Input camera matrix \f$A=\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . |
3579 | @param distCoeffs Input vector of distortion coefficients |
3580 | \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ |
3581 | of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. |
3582 | @param R Optional rectification transformation in the object space (3x3 matrix). R1 or R2 , |
3583 | computed by #stereoRectify can be passed here. If the matrix is empty, the identity transformation |
3584 | is assumed. In #initUndistortRectifyMap R assumed to be an identity matrix. |
3585 | @param newCameraMatrix New camera matrix \f$A'=\vecthreethree{f_x'}{0}{c_x'}{0}{f_y'}{c_y'}{0}{0}{1}\f$. |
3586 | @param size Undistorted image size. |
3587 | @param m1type Type of the first output map that can be CV_32FC1, CV_32FC2 or CV_16SC2, see #convertMaps |
3588 | @param map1 The first output map. |
3589 | @param map2 The second output map. |
3590 | */ |
3591 | CV_EXPORTS_W |
3592 | void initUndistortRectifyMap(InputArray cameraMatrix, InputArray distCoeffs, |
3593 | InputArray R, InputArray newCameraMatrix, |
3594 | Size size, int m1type, OutputArray map1, OutputArray map2); |
3595 | |
3596 | /** @brief Computes the projection and inverse-rectification transformation map. In essense, this is the inverse of |
3597 | #initUndistortRectifyMap to accomodate stereo-rectification of projectors ('inverse-cameras') in projector-camera pairs. |
3598 | |
3599 | The function computes the joint projection and inverse rectification transformation and represents the |
3600 | result in the form of maps for #remap. The projected image looks like a distorted version of the original which, |
3601 | once projected by a projector, should visually match the original. In case of a monocular camera, newCameraMatrix |
3602 | is usually equal to cameraMatrix, or it can be computed by |
3603 | #getOptimalNewCameraMatrix for a better control over scaling. In case of a projector-camera pair, |
3604 | newCameraMatrix is normally set to P1 or P2 computed by #stereoRectify . |
3605 | |
3606 | The projector is oriented differently in the coordinate space, according to R. In case of projector-camera pairs, |
3607 | this helps align the projector (in the same manner as #initUndistortRectifyMap for the camera) to create a stereo-rectified pair. This |
3608 | allows epipolar lines on both images to become horizontal and have the same y-coordinate (in case of a horizontally aligned projector-camera pair). |
3609 | |
3610 | The function builds the maps for the inverse mapping algorithm that is used by #remap. That |
3611 | is, for each pixel \f$(u, v)\f$ in the destination (projected and inverse-rectified) image, the function |
3612 | computes the corresponding coordinates in the source image (that is, in the original digital image). The following process is applied: |
3613 | |
3614 | \f[ |
3615 | \begin{array}{l} |
3616 | \text{newCameraMatrix}\\ |
3617 | x \leftarrow (u - {c'}_x)/{f'}_x \\ |
3618 | y \leftarrow (v - {c'}_y)/{f'}_y \\ |
3619 | |
3620 | \\\text{Undistortion} |
3621 | \\\scriptsize{\textit{though equation shown is for radial undistortion, function implements cv::undistortPoints()}}\\ |
3622 | r^2 \leftarrow x^2 + y^2 \\ |
3623 | \theta \leftarrow \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}\\ |
3624 | x' \leftarrow \frac{x}{\theta} \\ |
3625 | y' \leftarrow \frac{y}{\theta} \\ |
3626 | |
3627 | \\\text{Rectification}\\ |
3628 | {[X\,Y\,W]} ^T \leftarrow R*[x' \, y' \, 1]^T \\ |
3629 | x'' \leftarrow X/W \\ |
3630 | y'' \leftarrow Y/W \\ |
3631 | |
3632 | \\\text{cameraMatrix}\\ |
3633 | map_x(u,v) \leftarrow x'' f_x + c_x \\ |
3634 | map_y(u,v) \leftarrow y'' f_y + c_y |
3635 | \end{array} |
3636 | \f] |
3637 | where \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ |
3638 | are the distortion coefficients vector distCoeffs. |
3639 | |
3640 | In case of a stereo-rectified projector-camera pair, this function is called for the projector while #initUndistortRectifyMap is called for the camera head. |
3641 | This is done after #stereoRectify, which in turn is called after #stereoCalibrate. If the projector-camera pair |
3642 | is not calibrated, it is still possible to compute the rectification transformations directly from |
3643 | the fundamental matrix using #stereoRectifyUncalibrated. For the projector and camera, the function computes |
3644 | homography H as the rectification transformation in a pixel domain, not a rotation matrix R in 3D |
3645 | space. R can be computed from H as |
3646 | \f[\texttt{R} = \texttt{cameraMatrix} ^{-1} \cdot \texttt{H} \cdot \texttt{cameraMatrix}\f] |
3647 | where cameraMatrix can be chosen arbitrarily. |
3648 | |
3649 | @param cameraMatrix Input camera matrix \f$A=\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . |
3650 | @param distCoeffs Input vector of distortion coefficients |
3651 | \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ |
3652 | of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. |
3653 | @param R Optional rectification transformation in the object space (3x3 matrix). R1 or R2, |
3654 | computed by #stereoRectify can be passed here. If the matrix is empty, the identity transformation |
3655 | is assumed. |
3656 | @param newCameraMatrix New camera matrix \f$A'=\vecthreethree{f_x'}{0}{c_x'}{0}{f_y'}{c_y'}{0}{0}{1}\f$. |
3657 | @param size Distorted image size. |
3658 | @param m1type Type of the first output map. Can be CV_32FC1, CV_32FC2 or CV_16SC2, see #convertMaps |
3659 | @param map1 The first output map for #remap. |
3660 | @param map2 The second output map for #remap. |
3661 | */ |
3662 | CV_EXPORTS_W |
3663 | void initInverseRectificationMap( InputArray cameraMatrix, InputArray distCoeffs, |
3664 | InputArray R, InputArray newCameraMatrix, |
3665 | const Size& size, int m1type, OutputArray map1, OutputArray map2 ); |
3666 | |
3667 | //! initializes maps for #remap for wide-angle |
3668 | CV_EXPORTS |
3669 | float initWideAngleProjMap(InputArray cameraMatrix, InputArray distCoeffs, |
3670 | Size imageSize, int destImageWidth, |
3671 | int m1type, OutputArray map1, OutputArray map2, |
3672 | enum UndistortTypes projType = PROJ_SPHERICAL_EQRECT, double alpha = 0); |
3673 | static inline |
3674 | float initWideAngleProjMap(InputArray cameraMatrix, InputArray distCoeffs, |
3675 | Size imageSize, int destImageWidth, |
3676 | int m1type, OutputArray map1, OutputArray map2, |
3677 | int projType, double alpha = 0) |
3678 | { |
3679 | return initWideAngleProjMap(cameraMatrix, distCoeffs, imageSize, destImageWidth, |
3680 | m1type, map1, map2, projType: (UndistortTypes)projType, alpha); |
3681 | } |
3682 | |
3683 | /** @brief Returns the default new camera matrix. |
3684 | |
3685 | The function returns the camera matrix that is either an exact copy of the input cameraMatrix (when |
3686 | centerPrinicipalPoint=false ), or the modified one (when centerPrincipalPoint=true). |
3687 | |
3688 | In the latter case, the new camera matrix will be: |
3689 | |
3690 | \f[\begin{bmatrix} f_x && 0 && ( \texttt{imgSize.width} -1)*0.5 \\ 0 && f_y && ( \texttt{imgSize.height} -1)*0.5 \\ 0 && 0 && 1 \end{bmatrix} ,\f] |
3691 | |
3692 | where \f$f_x\f$ and \f$f_y\f$ are \f$(0,0)\f$ and \f$(1,1)\f$ elements of cameraMatrix, respectively. |
3693 | |
3694 | By default, the undistortion functions in OpenCV (see #initUndistortRectifyMap, #undistort) do not |
3695 | move the principal point. However, when you work with stereo, it is important to move the principal |
3696 | points in both views to the same y-coordinate (which is required by most of stereo correspondence |
3697 | algorithms), and may be to the same x-coordinate too. So, you can form the new camera matrix for |
3698 | each view where the principal points are located at the center. |
3699 | |
3700 | @param cameraMatrix Input camera matrix. |
3701 | @param imgsize Camera view image size in pixels. |
3702 | @param centerPrincipalPoint Location of the principal point in the new camera matrix. The |
3703 | parameter indicates whether this location should be at the image center or not. |
3704 | */ |
3705 | CV_EXPORTS_W |
3706 | Mat getDefaultNewCameraMatrix(InputArray cameraMatrix, Size imgsize = Size(), |
3707 | bool centerPrincipalPoint = false); |
3708 | |
3709 | /** @brief Computes the ideal point coordinates from the observed point coordinates. |
3710 | |
3711 | The function is similar to #undistort and #initUndistortRectifyMap but it operates on a |
3712 | sparse set of points instead of a raster image. Also the function performs a reverse transformation |
3713 | to #projectPoints. In case of a 3D object, it does not reconstruct its 3D coordinates, but for a |
3714 | planar object, it does, up to a translation vector, if the proper R is specified. |
3715 | |
3716 | For each observed point coordinate \f$(u, v)\f$ the function computes: |
3717 | \f[ |
3718 | \begin{array}{l} |
3719 | x^{"} \leftarrow (u - c_x)/f_x \\ |
3720 | y^{"} \leftarrow (v - c_y)/f_y \\ |
3721 | (x',y') = undistort(x^{"},y^{"}, \texttt{distCoeffs}) \\ |
3722 | {[X\,Y\,W]} ^T \leftarrow R*[x' \, y' \, 1]^T \\ |
3723 | x \leftarrow X/W \\ |
3724 | y \leftarrow Y/W \\ |
3725 | \text{only performed if P is specified:} \\ |
3726 | u' \leftarrow x {f'}_x + {c'}_x \\ |
3727 | v' \leftarrow y {f'}_y + {c'}_y |
3728 | \end{array} |
3729 | \f] |
3730 | |
3731 | where *undistort* is an approximate iterative algorithm that estimates the normalized original |
3732 | point coordinates out of the normalized distorted point coordinates ("normalized" means that the |
3733 | coordinates do not depend on the camera matrix). |
3734 | |
3735 | The function can be used for both a stereo camera head or a monocular camera (when R is empty). |
3736 | @param src Observed point coordinates, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel (CV_32FC2 or CV_64FC2) (or |
3737 | vector\<Point2f\> ). |
3738 | @param dst Output ideal point coordinates (1xN/Nx1 2-channel or vector\<Point2f\> ) after undistortion and reverse perspective |
3739 | transformation. If matrix P is identity or omitted, dst will contain normalized point coordinates. |
3740 | @param cameraMatrix Camera matrix \f$\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . |
3741 | @param distCoeffs Input vector of distortion coefficients |
3742 | \f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$ |
3743 | of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed. |
3744 | @param R Rectification transformation in the object space (3x3 matrix). R1 or R2 computed by |
3745 | #stereoRectify can be passed here. If the matrix is empty, the identity transformation is used. |
3746 | @param P New camera matrix (3x3) or new projection matrix (3x4) \f$\begin{bmatrix} {f'}_x & 0 & {c'}_x & t_x \\ 0 & {f'}_y & {c'}_y & t_y \\ 0 & 0 & 1 & t_z \end{bmatrix}\f$. P1 or P2 computed by |
3747 | #stereoRectify can be passed here. If the matrix is empty, the identity new camera matrix is used. |
3748 | */ |
3749 | CV_EXPORTS_W |
3750 | void undistortPoints(InputArray src, OutputArray dst, |
3751 | InputArray cameraMatrix, InputArray distCoeffs, |
3752 | InputArray R = noArray(), InputArray P = noArray()); |
3753 | /** @overload |
3754 | @note Default version of #undistortPoints does 5 iterations to compute undistorted points. |
3755 | */ |
3756 | CV_EXPORTS_AS(undistortPointsIter) |
3757 | void undistortPoints(InputArray src, OutputArray dst, |
3758 | InputArray cameraMatrix, InputArray distCoeffs, |
3759 | InputArray R, InputArray P, TermCriteria criteria); |
3760 | |
3761 | /** |
3762 | * @brief Compute undistorted image points position |
3763 | * |
3764 | * @param src Observed points position, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel (CV_32FC2 or |
3765 | CV_64FC2) (or vector\<Point2f\> ). |
3766 | * @param dst Output undistorted points position (1xN/Nx1 2-channel or vector\<Point2f\> ). |
3767 | * @param cameraMatrix Camera matrix \f$\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ . |
3768 | * @param distCoeffs Distortion coefficients |
3769 | */ |
3770 | CV_EXPORTS_W |
3771 | void undistortImagePoints(InputArray src, OutputArray dst, InputArray cameraMatrix, |
3772 | InputArray distCoeffs, |
3773 | TermCriteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 5, |
3774 | 0.01)); |
3775 | |
3776 | //! @} calib3d |
3777 | |
3778 | /** @brief The methods in this namespace use a so-called fisheye camera model. |
3779 | @ingroup calib3d_fisheye |
3780 | */ |
3781 | namespace fisheye |
3782 | { |
3783 | //! @addtogroup calib3d_fisheye |
3784 | //! @{ |
3785 | |
3786 | enum{ |
3787 | CALIB_USE_INTRINSIC_GUESS = 1 << 0, |
3788 | CALIB_RECOMPUTE_EXTRINSIC = 1 << 1, |
3789 | CALIB_CHECK_COND = 1 << 2, |
3790 | CALIB_FIX_SKEW = 1 << 3, |
3791 | CALIB_FIX_K1 = 1 << 4, |
3792 | CALIB_FIX_K2 = 1 << 5, |
3793 | CALIB_FIX_K3 = 1 << 6, |
3794 | CALIB_FIX_K4 = 1 << 7, |
3795 | CALIB_FIX_INTRINSIC = 1 << 8, |
3796 | CALIB_FIX_PRINCIPAL_POINT = 1 << 9, |
3797 | CALIB_ZERO_DISPARITY = 1 << 10, |
3798 | CALIB_FIX_FOCAL_LENGTH = 1 << 11 |
3799 | }; |
3800 | |
3801 | /** @brief Projects points using fisheye model |
3802 | |
3803 | @param objectPoints Array of object points, 1xN/Nx1 3-channel (or vector\<Point3f\> ), where N is |
3804 | the number of points in the view. |
3805 | @param imagePoints Output array of image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, or |
3806 | vector\<Point2f\>. |
3807 | @param affine |
3808 | @param K Camera intrinsic matrix \f$\cameramatrix{K}\f$. |
3809 | @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$. |
3810 | @param alpha The skew coefficient. |
3811 | @param jacobian Optional output 2Nx15 jacobian matrix of derivatives of image points with respect |
3812 | to components of the focal lengths, coordinates of the principal point, distortion coefficients, |
3813 | rotation vector, translation vector, and the skew. In the old interface different components of |
3814 | the jacobian are returned via different output parameters. |
3815 | |
3816 | The function computes projections of 3D points to the image plane given intrinsic and extrinsic |
3817 | camera parameters. Optionally, the function computes Jacobians - matrices of partial derivatives of |
3818 | image points coordinates (as functions of all the input parameters) with respect to the particular |
3819 | parameters, intrinsic and/or extrinsic. |
3820 | */ |
3821 | CV_EXPORTS void projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine, |
3822 | InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()); |
3823 | |
3824 | /** @overload */ |
3825 | CV_EXPORTS_W void projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec, |
3826 | InputArray K, InputArray D, double alpha = 0, OutputArray jacobian = noArray()); |
3827 | |
3828 | /** @brief Distorts 2D points using fisheye model. |
3829 | |
3830 | @param undistorted Array of object points, 1xN/Nx1 2-channel (or vector\<Point2f\> ), where N is |
3831 | the number of points in the view. |
3832 | @param K Camera intrinsic matrix \f$\cameramatrix{K}\f$. |
3833 | @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$. |
3834 | @param alpha The skew coefficient. |
3835 | @param distorted Output array of image points, 1xN/Nx1 2-channel, or vector\<Point2f\> . |
3836 | |
3837 | Note that the function assumes the camera intrinsic matrix of the undistorted points to be identity. |
3838 | This means if you want to distort image points you have to multiply them with \f$K^{-1}\f$ or |
3839 | use another function overload. |
3840 | */ |
3841 | CV_EXPORTS_W void distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha = 0); |
3842 | |
3843 | /** @overload |
3844 | Overload of distortPoints function to handle cases when undistorted points are obtained with non-identity |
3845 | camera matrix, e.g. output of #estimateNewCameraMatrixForUndistortRectify. |
3846 | @param undistorted Array of object points, 1xN/Nx1 2-channel (or vector\<Point2f\> ), where N is |
3847 | the number of points in the view. |
3848 | @param Kundistorted Camera intrinsic matrix used as new camera matrix for undistortion. |
3849 | @param K Camera intrinsic matrix \f$\cameramatrix{K}\f$. |
3850 | @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$. |
3851 | @param alpha The skew coefficient. |
3852 | @param distorted Output array of image points, 1xN/Nx1 2-channel, or vector\<Point2f\> . |
3853 | @sa estimateNewCameraMatrixForUndistortRectify |
3854 | */ |
3855 | CV_EXPORTS_W void distortPoints(InputArray undistorted, OutputArray distorted, InputArray Kundistorted, InputArray K, InputArray D, double alpha = 0); |
3856 | |
3857 | /** @brief Undistorts 2D points using fisheye model |
3858 | |
3859 | @param distorted Array of object points, 1xN/Nx1 2-channel (or vector\<Point2f\> ), where N is the |
3860 | number of points in the view. |
3861 | @param K Camera intrinsic matrix \f$\cameramatrix{K}\f$. |
3862 | @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$. |
3863 | @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 |
3864 | 1-channel or 1x1 3-channel |
3865 | @param P New camera intrinsic matrix (3x3) or new projection matrix (3x4) |
3866 | @param criteria Termination criteria |
3867 | @param undistorted Output array of image points, 1xN/Nx1 2-channel, or vector\<Point2f\> . |
3868 | */ |
3869 | CV_EXPORTS_W void undistortPoints(InputArray distorted, OutputArray undistorted, |
3870 | InputArray K, InputArray D, InputArray R = noArray(), InputArray P = noArray(), |
3871 | TermCriteria criteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 10, 1e-8)); |
3872 | |
3873 | /** @brief Computes undistortion and rectification maps for image transform by #remap. If D is empty zero |
3874 | distortion is used, if R or P is empty identity matrixes are used. |
3875 | |
3876 | @param K Camera intrinsic matrix \f$\cameramatrix{K}\f$. |
3877 | @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$. |
3878 | @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 |
3879 | 1-channel or 1x1 3-channel |
3880 | @param P New camera intrinsic matrix (3x3) or new projection matrix (3x4) |
3881 | @param size Undistorted image size. |
3882 | @param m1type Type of the first output map that can be CV_32FC1 or CV_16SC2 . See #convertMaps |
3883 | for details. |
3884 | @param map1 The first output map. |
3885 | @param map2 The second output map. |
3886 | */ |
3887 | CV_EXPORTS_W void initUndistortRectifyMap(InputArray K, InputArray D, InputArray R, InputArray P, |
3888 | const cv::Size& size, int m1type, OutputArray map1, OutputArray map2); |
3889 | |
3890 | /** @brief Transforms an image to compensate for fisheye lens distortion. |
3891 | |
3892 | @param distorted image with fisheye lens distortion. |
3893 | @param undistorted Output image with compensated fisheye lens distortion. |
3894 | @param K Camera intrinsic matrix \f$\cameramatrix{K}\f$. |
3895 | @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$. |
3896 | @param Knew Camera intrinsic matrix of the distorted image. By default, it is the identity matrix but you |
3897 | may additionally scale and shift the result by using a different matrix. |
3898 | @param new_size the new size |
3899 | |
3900 | The function transforms an image to compensate radial and tangential lens distortion. |
3901 | |
3902 | The function is simply a combination of #fisheye::initUndistortRectifyMap (with unity R ) and #remap |
3903 | (with bilinear interpolation). See the former function for details of the transformation being |
3904 | performed. |
3905 | |
3906 | See below the results of undistortImage. |
3907 | - a\) result of undistort of perspective camera model (all possible coefficients (k_1, k_2, k_3, |
3908 | k_4, k_5, k_6) of distortion were optimized under calibration) |
3909 | - b\) result of #fisheye::undistortImage of fisheye camera model (all possible coefficients (k_1, k_2, |
3910 | k_3, k_4) of fisheye distortion were optimized under calibration) |
3911 | - c\) original image was captured with fisheye lens |
3912 | |
3913 | Pictures a) and b) almost the same. But if we consider points of image located far from the center |
3914 | of image, we can notice that on image a) these points are distorted. |
3915 | |
3916 |  |
3917 | */ |
3918 | CV_EXPORTS_W void undistortImage(InputArray distorted, OutputArray undistorted, |
3919 | InputArray K, InputArray D, InputArray Knew = cv::noArray(), const Size& new_size = Size()); |
3920 | |
3921 | /** @brief Estimates new camera intrinsic matrix for undistortion or rectification. |
3922 | |
3923 | @param K Camera intrinsic matrix \f$\cameramatrix{K}\f$. |
3924 | @param image_size Size of the image |
3925 | @param D Input vector of distortion coefficients \f$\distcoeffsfisheye\f$. |
3926 | @param R Rectification transformation in the object space: 3x3 1-channel, or vector: 3x1/1x3 |
3927 | 1-channel or 1x1 3-channel |
3928 | @param P New camera intrinsic matrix (3x3) or new projection matrix (3x4) |
3929 | @param balance Sets the new focal length in range between the min focal length and the max focal |
3930 | length. Balance is in range of [0, 1]. |
3931 | @param new_size the new size |
3932 | @param fov_scale Divisor for new focal length. |
3933 | */ |
3934 | CV_EXPORTS_W void estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R, |
3935 | OutputArray P, double balance = 0.0, const Size& new_size = Size(), double fov_scale = 1.0); |
3936 | |
3937 | /** @brief Performs camera calibration |
3938 | |
3939 | @param objectPoints vector of vectors of calibration pattern points in the calibration pattern |
3940 | coordinate space. |
3941 | @param imagePoints vector of vectors of the projections of calibration pattern points. |
3942 | imagePoints.size() and objectPoints.size() and imagePoints[i].size() must be equal to |
3943 | objectPoints[i].size() for each i. |
3944 | @param image_size Size of the image used only to initialize the camera intrinsic matrix. |
3945 | @param K Output 3x3 floating-point camera intrinsic matrix |
3946 | \f$\cameramatrix{A}\f$ . If |
3947 | @ref fisheye::CALIB_USE_INTRINSIC_GUESS is specified, some or all of fx, fy, cx, cy must be |
3948 | initialized before calling the function. |
3949 | @param D Output vector of distortion coefficients \f$\distcoeffsfisheye\f$. |
3950 | @param rvecs Output vector of rotation vectors (see @ref Rodrigues ) estimated for each pattern view. |
3951 | That is, each k-th rotation vector together with the corresponding k-th translation vector (see |
3952 | the next output parameter description) brings the calibration pattern from the model coordinate |
3953 | space (in which object points are specified) to the world coordinate space, that is, a real |
3954 | position of the calibration pattern in the k-th pattern view (k=0.. *M* -1). |
3955 | @param tvecs Output vector of translation vectors estimated for each pattern view. |
3956 | @param flags Different flags that may be zero or a combination of the following values: |
3957 | - @ref fisheye::CALIB_USE_INTRINSIC_GUESS cameraMatrix contains valid initial values of |
3958 | fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image |
3959 | center ( imageSize is used), and focal distances are computed in a least-squares fashion. |
3960 | - @ref fisheye::CALIB_RECOMPUTE_EXTRINSIC Extrinsic will be recomputed after each iteration |
3961 | of intrinsic optimization. |
3962 | - @ref fisheye::CALIB_CHECK_COND The functions will check validity of condition number. |
3963 | - @ref fisheye::CALIB_FIX_SKEW Skew coefficient (alpha) is set to zero and stay zero. |
3964 | - @ref fisheye::CALIB_FIX_K1,..., @ref fisheye::CALIB_FIX_K4 Selected distortion coefficients |
3965 | are set to zeros and stay zero. |
3966 | - @ref fisheye::CALIB_FIX_PRINCIPAL_POINT The principal point is not changed during the global |
3967 | optimization. It stays at the center or at a different location specified when @ref fisheye::CALIB_USE_INTRINSIC_GUESS is set too. |
3968 | - @ref fisheye::CALIB_FIX_FOCAL_LENGTH The focal length is not changed during the global |
3969 | optimization. It is the \f$max(width,height)/\pi\f$ or the provided \f$f_x\f$, \f$f_y\f$ when @ref fisheye::CALIB_USE_INTRINSIC_GUESS is set too. |
3970 | @param criteria Termination criteria for the iterative optimization algorithm. |
3971 | */ |
3972 | CV_EXPORTS_W double calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size, |
3973 | InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = 0, |
3974 | TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); |
3975 | |
3976 | /** @brief Stereo rectification for fisheye camera model |
3977 | |
3978 | @param K1 First camera intrinsic matrix. |
3979 | @param D1 First camera distortion parameters. |
3980 | @param K2 Second camera intrinsic matrix. |
3981 | @param D2 Second camera distortion parameters. |
3982 | @param imageSize Size of the image used for stereo calibration. |
3983 | @param R Rotation matrix between the coordinate systems of the first and the second |
3984 | cameras. |
3985 | @param tvec Translation vector between coordinate systems of the cameras. |
3986 | @param R1 Output 3x3 rectification transform (rotation matrix) for the first camera. |
3987 | @param R2 Output 3x3 rectification transform (rotation matrix) for the second camera. |
3988 | @param P1 Output 3x4 projection matrix in the new (rectified) coordinate systems for the first |
3989 | camera. |
3990 | @param P2 Output 3x4 projection matrix in the new (rectified) coordinate systems for the second |
3991 | camera. |
3992 | @param Q Output \f$4 \times 4\f$ disparity-to-depth mapping matrix (see #reprojectImageTo3D ). |
3993 | @param flags Operation flags that may be zero or @ref fisheye::CALIB_ZERO_DISPARITY . If the flag is set, |
3994 | the function makes the principal points of each camera have the same pixel coordinates in the |
3995 | rectified views. And if the flag is not set, the function may still shift the images in the |
3996 | horizontal or vertical direction (depending on the orientation of epipolar lines) to maximize the |
3997 | useful image area. |
3998 | @param newImageSize New image resolution after rectification. The same size should be passed to |
3999 | #initUndistortRectifyMap (see the stereo_calib.cpp sample in OpenCV samples directory). When (0,0) |
4000 | is passed (default), it is set to the original imageSize . Setting it to larger value can help you |
4001 | preserve details in the original image, especially when there is a big radial distortion. |
4002 | @param balance Sets the new focal length in range between the min focal length and the max focal |
4003 | length. Balance is in range of [0, 1]. |
4004 | @param fov_scale Divisor for new focal length. |
4005 | */ |
4006 | CV_EXPORTS_W void stereoRectify(InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size &imageSize, InputArray R, InputArray tvec, |
4007 | OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, const Size &newImageSize = Size(), |
4008 | double balance = 0.0, double fov_scale = 1.0); |
4009 | |
4010 | /** @brief Performs stereo calibration |
4011 | |
4012 | @param objectPoints Vector of vectors of the calibration pattern points. |
4013 | @param imagePoints1 Vector of vectors of the projections of the calibration pattern points, |
4014 | observed by the first camera. |
4015 | @param imagePoints2 Vector of vectors of the projections of the calibration pattern points, |
4016 | observed by the second camera. |
4017 | @param K1 Input/output first camera intrinsic matrix: |
4018 | \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 |
4019 | any of @ref fisheye::CALIB_USE_INTRINSIC_GUESS , @ref fisheye::CALIB_FIX_INTRINSIC are specified, |
4020 | some or all of the matrix components must be initialized. |
4021 | @param D1 Input/output vector of distortion coefficients \f$\distcoeffsfisheye\f$ of 4 elements. |
4022 | @param K2 Input/output second camera intrinsic matrix. The parameter is similar to K1 . |
4023 | @param D2 Input/output lens distortion coefficients for the second camera. The parameter is |
4024 | similar to D1 . |
4025 | @param imageSize Size of the image used only to initialize camera intrinsic matrix. |
4026 | @param R Output rotation matrix between the 1st and the 2nd camera coordinate systems. |
4027 | @param T Output translation vector between the coordinate systems of the cameras. |
4028 | @param rvecs Output vector of rotation vectors ( @ref Rodrigues ) estimated for each pattern view in the |
4029 | coordinate system of the first camera of the stereo pair (e.g. std::vector<cv::Mat>). More in detail, each |
4030 | i-th rotation vector together with the corresponding i-th translation vector (see the next output parameter |
4031 | description) brings the calibration pattern from the object coordinate space (in which object points are |
4032 | specified) to the camera coordinate space of the first camera of the stereo pair. In more technical terms, |
4033 | the tuple of the i-th rotation and translation vector performs a change of basis from object coordinate space |
4034 | to camera coordinate space of the first camera of the stereo pair. |
4035 | @param tvecs Output vector of translation vectors estimated for each pattern view, see parameter description |
4036 | of previous output parameter ( rvecs ). |
4037 | @param flags Different flags that may be zero or a combination of the following values: |
4038 | - @ref fisheye::CALIB_FIX_INTRINSIC Fix K1, K2? and D1, D2? so that only R, T matrices |
4039 | are estimated. |
4040 | - @ref fisheye::CALIB_USE_INTRINSIC_GUESS K1, K2 contains valid initial values of |
4041 | fx, fy, cx, cy that are optimized further. Otherwise, (cx, cy) is initially set to the image |
4042 | center (imageSize is used), and focal distances are computed in a least-squares fashion. |
4043 | - @ref fisheye::CALIB_RECOMPUTE_EXTRINSIC Extrinsic will be recomputed after each iteration |
4044 | of intrinsic optimization. |
4045 | - @ref fisheye::CALIB_CHECK_COND The functions will check validity of condition number. |
4046 | - @ref fisheye::CALIB_FIX_SKEW Skew coefficient (alpha) is set to zero and stay zero. |
4047 | - @ref fisheye::CALIB_FIX_K1,..., @ref fisheye::CALIB_FIX_K4 Selected distortion coefficients are set to zeros and stay |
4048 | zero. |
4049 | @param criteria Termination criteria for the iterative optimization algorithm. |
4050 | */ |
4051 | CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, |
4052 | InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, |
4053 | OutputArray R, OutputArray T, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags = fisheye::CALIB_FIX_INTRINSIC, |
4054 | TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); |
4055 | |
4056 | /// @overload |
4057 | CV_EXPORTS_W double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, |
4058 | InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, |
4059 | OutputArray R, OutputArray T, int flags = fisheye::CALIB_FIX_INTRINSIC, |
4060 | TermCriteria criteria = TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, DBL_EPSILON)); |
4061 | |
4062 | /** |
4063 | @brief Finds an object pose from 3D-2D point correspondences for fisheye camera moodel. |
4064 | |
4065 | @param objectPoints Array of object points in the object coordinate space, Nx3 1-channel or |
4066 | 1xN/Nx1 3-channel, where N is the number of points. vector\<Point3d\> can be also passed here. |
4067 | @param imagePoints Array of corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, |
4068 | where N is the number of points. vector\<Point2d\> can be also passed here. |
4069 | @param cameraMatrix Input camera intrinsic matrix \f$\cameramatrix{A}\f$ . |
4070 | @param distCoeffs Input vector of distortion coefficients (4x1/1x4). |
4071 | @param rvec Output rotation vector (see @ref Rodrigues ) that, together with tvec, brings points from |
4072 | the model coordinate system to the camera coordinate system. |
4073 | @param tvec Output translation vector. |
4074 | @param useExtrinsicGuess Parameter used for #SOLVEPNP_ITERATIVE. If true (1), the function uses |
4075 | the provided rvec and tvec values as initial approximations of the rotation and translation |
4076 | vectors, respectively, and further optimizes them. |
4077 | @param flags Method for solving a PnP problem: see @ref calib3d_solvePnP_flags |
4078 | This function returns the rotation and the translation vectors that transform a 3D point expressed in the object |
4079 | coordinate frame to the camera coordinate frame, using different methods: |
4080 | - P3P methods (@ref SOLVEPNP_P3P, @ref SOLVEPNP_AP3P): need 4 input points to return a unique solution. |
4081 | - @ref SOLVEPNP_IPPE Input points must be >= 4 and object points must be coplanar. |
4082 | - @ref SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation. |
4083 | Number of input points must be 4. Object points must be defined in the following order: |
4084 | - point 0: [-squareLength / 2, squareLength / 2, 0] |
4085 | - point 1: [ squareLength / 2, squareLength / 2, 0] |
4086 | - point 2: [ squareLength / 2, -squareLength / 2, 0] |
4087 | - point 3: [-squareLength / 2, -squareLength / 2, 0] |
4088 | - for all the other flags, number of input points must be >= 4 and object points can be in any configuration. |
4089 | @param criteria Termination criteria for internal undistortPoints call. |
4090 | The function interally undistorts points with @ref undistortPoints and call @ref cv::solvePnP, |
4091 | thus the input are very similar. More information about Perspective-n-Points is described in @ref calib3d_solvePnP |
4092 | for more information. |
4093 | */ |
4094 | CV_EXPORTS_W bool solvePnP( InputArray objectPoints, InputArray imagePoints, |
4095 | InputArray cameraMatrix, InputArray distCoeffs, |
4096 | OutputArray rvec, OutputArray tvec, |
4097 | bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE, |
4098 | TermCriteria criteria = TermCriteria(TermCriteria::MAX_ITER + TermCriteria::EPS, 10, 1e-8) |
4099 | ); |
4100 | |
4101 | //! @} calib3d_fisheye |
4102 | } // end namespace fisheye |
4103 | |
4104 | } //end namespace cv |
4105 | |
4106 | #if 0 //def __cplusplus |
4107 | ////////////////////////////////////////////////////////////////////////////////////////// |
4108 | class CV_EXPORTS CvLevMarq |
4109 | { |
4110 | public: |
4111 | CvLevMarq(); |
4112 | CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria= |
4113 | cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON), |
4114 | bool completeSymmFlag=false ); |
4115 | ~CvLevMarq(); |
4116 | void init( int nparams, int nerrs, CvTermCriteria criteria= |
4117 | cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON), |
4118 | bool completeSymmFlag=false ); |
4119 | bool update( const CvMat*& param, CvMat*& J, CvMat*& err ); |
4120 | bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm ); |
4121 | |
4122 | void clear(); |
4123 | void step(); |
4124 | enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 }; |
4125 | |
4126 | cv::Ptr<CvMat> mask; |
4127 | cv::Ptr<CvMat> prevParam; |
4128 | cv::Ptr<CvMat> param; |
4129 | cv::Ptr<CvMat> J; |
4130 | cv::Ptr<CvMat> err; |
4131 | cv::Ptr<CvMat> JtJ; |
4132 | cv::Ptr<CvMat> JtJN; |
4133 | cv::Ptr<CvMat> JtErr; |
4134 | cv::Ptr<CvMat> JtJV; |
4135 | cv::Ptr<CvMat> JtJW; |
4136 | double prevErrNorm, errNorm; |
4137 | int lambdaLg10; |
4138 | CvTermCriteria criteria; |
4139 | int state; |
4140 | int iters; |
4141 | bool completeSymmFlag; |
4142 | int solveMethod; |
4143 | }; |
4144 | #endif |
4145 | |
4146 | #endif |
4147 | |