//! @{
//! type of the robust estimation algorithm
-enum { LMEDS = 4, //!< least-median of squares algorithm
- RANSAC = 8, //!< RANSAC algorithm
- RHO = 16 //!< RHO algorithm
+enum { LMEDS = 4, //!< least-median of squares algorithm
+ RANSAC = 8, //!< RANSAC algorithm
+ RHO = 16, //!< RHO algorithm
+ USAC_DEFAULT = 32, //!< USAC algorithm, default settings
+ USAC_PARALLEL = 33, //!< USAC, parallel version
+ USAC_FM_8PTS = 34, //!< USAC, fundamental matrix 8 points
+ USAC_FAST = 35, //!< USAC, fast settings
+ USAC_ACCURATE = 36, //!< USAC, accurate settings
+ USAC_PROSAC = 37, //!< USAC, sorted points, runs PROSAC
+ USAC_MAGSAC = 38 //!< USAC, runs MAGSAC++
};
enum SolvePnPMethod {
enum { CALIB_CB_ADAPTIVE_THRESH = 1,
CALIB_CB_NORMALIZE_IMAGE = 2,
CALIB_CB_FILTER_QUADS = 4,
- CALIB_CB_FAST_CHECK = 8
+ CALIB_CB_FAST_CHECK = 8,
+ CALIB_CB_EXHAUSTIVE = 16,
+ CALIB_CB_ACCURACY = 32,
+ CALIB_CB_LARGER = 64,
+ CALIB_CB_MARKER = 128
};
enum { CALIB_CB_SYMMETRIC_GRID = 1,
CALIB_CB_CLUSTERING = 4
};
-enum { CALIB_USE_INTRINSIC_GUESS = 0x00001,
+enum { CALIB_NINTRINSIC = 18,
+ CALIB_USE_INTRINSIC_GUESS = 0x00001,
CALIB_FIX_ASPECT_RATIO = 0x00002,
CALIB_FIX_PRINCIPAL_POINT = 0x00004,
CALIB_ZERO_TANGENT_DIST = 0x00008,
CALIB_HAND_EYE_DANIILIDIS = 4 //!< Hand-Eye Calibration Using Dual Quaternions @cite Daniilidis98
};
+enum RobotWorldHandEyeCalibrationMethod
+{
+ CALIB_ROBOT_WORLD_HAND_EYE_SHAH = 0, //!< Solving the robot-world/hand-eye calibration problem using the kronecker product @cite Shah2013SolvingTR
+ CALIB_ROBOT_WORLD_HAND_EYE_LI = 1 //!< Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product @cite Li2010SimultaneousRA
+};
+
+enum SamplingMethod { SAMPLING_UNIFORM, SAMPLING_PROGRESSIVE_NAPSAC, SAMPLING_NAPSAC,
+ SAMPLING_PROSAC };
+enum LocalOptimMethod {LOCAL_OPTIM_NULL, LOCAL_OPTIM_INNER_LO, LOCAL_OPTIM_INNER_AND_ITER_LO,
+ LOCAL_OPTIM_GC, LOCAL_OPTIM_SIGMA};
+enum ScoreMethod {SCORE_METHOD_RANSAC, SCORE_METHOD_MSAC, SCORE_METHOD_MAGSAC, SCORE_METHOD_LMEDS};
+enum NeighborSearchMethod { NEIGH_FLANN_KNN, NEIGH_GRID, NEIGH_FLANN_RADIUS };
+
+struct CV_EXPORTS_W_SIMPLE UsacParams
+{ // in alphabetical order
+ CV_WRAP UsacParams();
+ CV_PROP_RW double confidence;
+ CV_PROP_RW bool isParallel;
+ CV_PROP_RW int loIterations;
+ CV_PROP_RW LocalOptimMethod loMethod;
+ CV_PROP_RW int loSampleSize;
+ CV_PROP_RW int maxIterations;
+ CV_PROP_RW NeighborSearchMethod neighborsSearch;
+ CV_PROP_RW int randomGeneratorState;
+ CV_PROP_RW SamplingMethod sampler;
+ CV_PROP_RW ScoreMethod score;
+ CV_PROP_RW double threshold;
+};
/** @brief Converts a rotation matrix to a rotation vector or vice versa.
*/
CV_EXPORTS_W void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() );
+
+
+/** Levenberg-Marquardt solver. Starting with the specified vector of parameters it
+ optimizes the target vector criteria "err"
+ (finds local minima of each target vector component absolute value).
+
+ When needed, it calls user-provided callback.
+*/
+class CV_EXPORTS LMSolver : public Algorithm
+{
+public:
+ class CV_EXPORTS Callback
+ {
+ public:
+ virtual ~Callback() {}
+ /**
+ computes error and Jacobian for the specified vector of parameters
+
+ @param param the current vector of parameters
+ @param err output vector of errors: err_i = actual_f_i - ideal_f_i
+ @param J output Jacobian: J_ij = d(err_i)/d(param_j)
+
+ when J=noArray(), it means that it does not need to be computed.
+ Dimensionality of error vector and param vector can be different.
+ The callback should explicitly allocate (with "create" method) each output array
+ (unless it's noArray()).
+ */
+ virtual bool compute(InputArray param, OutputArray err, OutputArray J) const = 0;
+ };
+
+ /**
+ Runs Levenberg-Marquardt algorithm using the passed vector of parameters as the start point.
+ The final vector of parameters (whether the algorithm converged or not) is stored at the same
+ vector. The method returns the number of iterations used. If it's equal to the previously specified
+ maxIters, there is a big chance the algorithm did not converge.
+
+ @param param initial/final vector of parameters.
+
+ Note that the dimensionality of parameter space is defined by the size of param vector,
+ and the dimensionality of optimized criteria is defined by the size of err vector
+ computed by the callback.
+ */
+ virtual int run(InputOutputArray param) const = 0;
+
+ /**
+ Sets the maximum number of iterations
+ @param maxIters the number of iterations
+ */
+ virtual void setMaxIters(int maxIters) = 0;
+ /**
+ Retrieves the current maximum number of iterations
+ */
+ virtual int getMaxIters() const = 0;
+
+ /**
+ Creates Levenberg-Marquard solver
+
+ @param cb callback
+ @param maxIters maximum number of iterations that can be further
+ modified using setMaxIters() method.
+ */
+ static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters);
+ static Ptr<LMSolver> create(const Ptr<LMSolver::Callback>& cb, int maxIters, double eps);
+};
+
+
+
/** @example samples/cpp/tutorial_code/features2D/Homography/pose_from_homography.cpp
An example program about pose estimation from coplanar points
CV_EXPORTS Mat findHomography( InputArray srcPoints, InputArray dstPoints,
OutputArray mask, int method = 0, double ransacReprojThreshold = 3 );
+
+CV_EXPORTS_W Mat findHomography(InputArray srcPoints, InputArray dstPoints, OutputArray mask,
+ const UsacParams ¶ms);
+
/** @brief Computes an RQ decomposition of 3x3 matrices.
@param src 3x3 input matrix.
float reprojectionError = 8.0, double confidence = 0.99,
OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );
+
+/*
+Finds rotation and translation vector.
+If cameraMatrix is given then run P3P. Otherwise run linear P6P and output cameraMatrix too.
+*/
+CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
+ InputOutputArray cameraMatrix, InputArray distCoeffs,
+ OutputArray rvec, OutputArray tvec, OutputArray inliers,
+ const UsacParams ¶ms=UsacParams());
+
/** @brief Finds an object pose from 3 3D-2D point correspondences.
@param objectPoints Array of object points in the object coordinate space, 3x3 1-channel or
@param image Source chessboard view. It must be an 8-bit grayscale or color image.
@param patternSize Number of inner corners per a chessboard row and column
-( patternSize = cvSize(points_per_row,points_per_colum) = cvSize(columns,rows) ).
+( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ).
@param corners Output array of detected corners.
@param flags Various operation flags that can be zero or a combination of the following values:
- **CALIB_CB_ADAPTIVE_THRESH** Use adaptive thresholding to convert the image to black
CV_EXPORTS_W bool findChessboardCorners( InputArray image, Size patternSize, OutputArray corners,
int flags = CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE );
+/*
+ Checks whether the image contains chessboard of the specific size or not.
+ If yes, nonzero value is returned.
+*/
+CV_EXPORTS_W bool checkChessboard(InputArray img, Size size);
+
+/** @brief Finds the positions of internal corners of the chessboard using a sector based approach.
+
+@param image Source chessboard view. It must be an 8-bit grayscale or color image.
+@param patternSize Number of inner corners per a chessboard row and column
+( patternSize = cv::Size(points_per_row,points_per_colum) = cv::Size(columns,rows) ).
+@param corners Output array of detected corners.
+@param flags Various operation flags that can be zero or a combination of the following values:
+- **CALIB_CB_NORMALIZE_IMAGE** Normalize the image gamma with equalizeHist before detection.
+- **CALIB_CB_EXHAUSTIVE** Run an exhaustive search to improve detection rate.
+- **CALIB_CB_ACCURACY** Up sample input image to improve sub-pixel accuracy due to aliasing effects.
+- **CALIB_CB_LARGER** The detected pattern is allowed to be larger than patternSize (see description).
+- **CALIB_CB_MARKER** The detected pattern must have a marker (see description).
+This should be used if an accurate camera calibration is required.
+@param meta Optional output arrray of detected corners (CV_8UC1 and size = cv::Size(columns,rows)).
+Each entry stands for one corner of the pattern and can have one of the following values:
+- 0 = no meta data attached
+- 1 = left-top corner of a black cell
+- 2 = left-top corner of a white cell
+- 3 = left-top corner of a black cell with a white marker dot
+- 4 = left-top corner of a white cell with a black marker dot (pattern origin in case of markers otherwise first corner)
+
+The function is analog to findchessboardCorners but uses a localized radon
+transformation approximated by box filters being more robust to all sort of
+noise, faster on larger images and is able to directly return the sub-pixel
+position of the internal chessboard corners. The Method is based on the paper
+@cite duda2018 "Accurate Detection and Localization of Checkerboard Corners for
+Calibration" demonstrating that the returned sub-pixel positions are more
+accurate than the one returned by cornerSubPix allowing a precise camera
+calibration for demanding applications.
+
+In the case, the flags **CALIB_CB_LARGER** or **CALIB_CB_MARKER** are given,
+the result can be recovered from the optional meta array. Both flags are
+helpful to use calibration patterns exceeding the field of view of the camera.
+These oversized patterns allow more accurate calibrations as corners can be
+utilized, which are as close as possible to the image borders. For a
+consistent coordinate system across all images, the optional marker (see image
+below) can be used to move the origin of the board to the location where the
+black circle is located.
+
+@note The function requires a white boarder with roughly the same width as one
+of the checkerboard fields around the whole board to improve the detection in
+various environments. In addition, because of the localized radon
+transformation it is beneficial to use round corners for the field corners
+which are located on the outside of the board. The following figure illustrates
+a sample checkerboard optimized for the detection. However, any other checkerboard
+can be used as well.
+![Checkerboard](pics/checkerboard_radon.png)
+ */
+CV_EXPORTS_AS(findChessboardCornersSBWithMeta)
+bool findChessboardCornersSB(InputArray image,Size patternSize, OutputArray corners,
+ int flags,OutputArray meta);
+/** @overload */
+CV_EXPORTS_W inline
+bool findChessboardCornersSB(InputArray image, Size patternSize, OutputArray corners,
+ int flags = 0)
+{
+ return findChessboardCornersSB(image, patternSize, corners, flags, noArray());
+}
+
+/** @brief Estimates the sharpness of a detected chessboard.
+
+Image sharpness, as well as brightness, are a critical parameter for accuracte
+camera calibration. For accessing these parameters for filtering out
+problematic calibraiton images, this method calculates edge profiles by traveling from
+black to white chessboard cell centers. Based on this, the number of pixels is
+calculated required to transit from black to white. This width of the
+transition area is a good indication of how sharp the chessboard is imaged
+and should be below ~3.0 pixels.
+
+@param image Gray image used to find chessboard corners
+@param patternSize Size of a found chessboard pattern
+@param corners Corners found by findChessboardCorners(SB)
+@param rise_distance Rise distance 0.8 means 10% ... 90% of the final signal strength
+@param vertical By default edge responses for horizontal lines are calculated
+@param sharpness Optional output array with a sharpness value for calculated edge responses (see description)
+
+The optional sharpness array is of type CV_32FC1 and has for each calculated
+profile one row with the following five entries:
+* 0 = x coordinate of the underlying edge in the image
+* 1 = y coordinate of the underlying edge in the image
+* 2 = width of the transition area (sharpness)
+* 3 = signal strength in the black cell (min brightness)
+* 4 = signal strength in the white cell (max brightness)
+
+@return Scalar(average sharpness, average min brightness, average max brightness,0)
+*/
+CV_EXPORTS_W Scalar estimateChessboardSharpness(InputArray image, Size patternSize, InputArray corners,
+ float rise_distance=0.8F,bool vertical=false,
+ OutputArray sharpness=noArray());
+
+
//! finds subpixel-accurate positions of the chessboard corners
CV_EXPORTS_W bool find4QuadCornerSubpix( InputArray img, InputOutputArray corners, Size region_size );
SYMMETRIC_GRID, ASYMMETRIC_GRID
};
GridType gridType;
-};
-
-struct CV_EXPORTS_W_SIMPLE CirclesGridFinderParameters2 : public CirclesGridFinderParameters
-{
- CV_WRAP CirclesGridFinderParameters2();
CV_PROP_RW float squareSize; //!< Distance between two adjacent points. Used by CALIB_CB_CLUSTERING.
CV_PROP_RW float maxRectifiedDistance; //!< Max deviation from prediction. Used by CALIB_CB_CLUSTERING.
};
+#ifndef DISABLE_OPENCV_3_COMPATIBILITY
+typedef CirclesGridFinderParameters CirclesGridFinderParameters2;
+#endif
+
/** @brief Finds centers in the grid of circles.
@param image grid view of input circles; it must be an 8-bit grayscale or color image.
CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize,
OutputArray centers, int flags,
const Ptr<FeatureDetector> &blobDetector,
- CirclesGridFinderParameters parameters);
-
-/** @overload */
-CV_EXPORTS_W bool findCirclesGrid2( InputArray image, Size patternSize,
- OutputArray centers, int flags,
- const Ptr<FeatureDetector> &blobDetector,
- CirclesGridFinderParameters2 parameters);
+ const CirclesGridFinderParameters& parameters);
/** @overload */
CV_EXPORTS_W bool findCirclesGrid( InputArray image, Size patternSize,
instead of using patternSize=cvSize(cols,rows) in @ref findChessboardCorners.
@sa
- findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate, undistort
+ calibrateCameraRO, findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate,
+ undistort
*/
CV_EXPORTS_AS(calibrateCameraExtended) double calibrateCamera( InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints, Size imageSize,
int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
-/** @overload double calibrateCamera( InputArrayOfArrays objectPoints,
+/** @overload */
+CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
InputArrayOfArrays imagePoints, Size imageSize,
InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
- OutputArray stdDeviations, OutputArray perViewErrors,
int flags = 0, TermCriteria criteria = TermCriteria(
- TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) )
+ TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
+
+/** @brief Finds the camera intrinsic and extrinsic parameters from several views of a calibration pattern.
+
+This function is an extension of calibrateCamera() with the method of releasing object which was
+proposed in @cite strobl2011iccv. In many common cases with inaccurate, unmeasured, roughly planar
+targets (calibration plates), this method can dramatically improve the precision of the estimated
+camera parameters. Both the object-releasing method and standard method are supported by this
+function. Use the parameter **iFixedPoint** for method selection. In the internal implementation,
+calibrateCamera() is a wrapper for this function.
+
+@param objectPoints Vector of vectors of calibration pattern points in the calibration pattern
+coordinate space. See calibrateCamera() for details. If the method of releasing object to be used,
+the identical calibration board must be used in each view and it must be fully visible, and all
+objectPoints[i] must be the same and all points should be roughly close to a plane. **The calibration
+target has to be rigid, or at least static if the camera (rather than the calibration target) is
+shifted for grabbing images.**
+@param imagePoints Vector of vectors of the projections of calibration pattern points. See
+calibrateCamera() for details.
+@param imageSize Size of the image used only to initialize the intrinsic camera matrix.
+@param iFixedPoint The index of the 3D object point in objectPoints[0] to be fixed. It also acts as
+a switch for calibration method selection. If object-releasing method to be used, pass in the
+parameter in the range of [1, objectPoints[0].size()-2], otherwise a value out of this range will
+make standard calibration method selected. Usually the top-right corner point of the calibration
+board grid is recommended to be fixed when object-releasing method being utilized. According to
+\cite strobl2011iccv, two other points are also fixed. In this implementation, objectPoints[0].front
+and objectPoints[0].back.z are used. With object-releasing method, accurate rvecs, tvecs and
+newObjPoints are only possible if coordinates of these three fixed points are accurate enough.
+@param cameraMatrix Output 3x3 floating-point camera matrix. See calibrateCamera() for details.
+@param distCoeffs Output vector of distortion coefficients. See calibrateCamera() for details.
+@param rvecs Output vector of rotation vectors estimated for each pattern view. See calibrateCamera()
+for details.
+@param tvecs Output vector of translation vectors estimated for each pattern view.
+@param newObjPoints The updated output vector of calibration pattern points. The coordinates might
+be scaled based on three fixed points. The returned coordinates are accurate only if the above
+mentioned three fixed points are accurate. If not needed, noArray() can be passed in. This parameter
+is ignored with standard calibration method.
+@param stdDeviationsIntrinsics Output vector of standard deviations estimated for intrinsic parameters.
+See calibrateCamera() for details.
+@param stdDeviationsExtrinsics Output vector of standard deviations estimated for extrinsic parameters.
+See calibrateCamera() for details.
+@param stdDeviationsObjPoints Output vector of standard deviations estimated for refined coordinates
+of calibration pattern points. It has the same size and order as objectPoints[0] vector. This
+parameter is ignored with standard calibration method.
+ @param perViewErrors Output vector of the RMS re-projection error estimated for each pattern view.
+@param flags Different flags that may be zero or a combination of some predefined values. See
+calibrateCamera() for details. If the method of releasing object is used, the calibration time may
+be much longer. CALIB_USE_QR or CALIB_USE_LU could be used for faster calibration with potentially
+less precise and less stable in some rare cases.
+@param criteria Termination criteria for the iterative optimization algorithm.
+
+@return the overall RMS re-projection error.
+
+The function estimates the intrinsic camera parameters and extrinsic parameters for each of the
+views. The algorithm is based on @cite Zhang2000, @cite BouguetMCT and @cite strobl2011iccv. See
+calibrateCamera() for other detailed explanations.
+@sa
+ calibrateCamera, findChessboardCorners, solvePnP, initCameraMatrix2D, stereoCalibrate, undistort
*/
-CV_EXPORTS_W double calibrateCamera( InputArrayOfArrays objectPoints,
- InputArrayOfArrays imagePoints, Size imageSize,
+CV_EXPORTS_AS(calibrateCameraROExtended) double calibrateCameraRO( InputArrayOfArrays objectPoints,
+ InputArrayOfArrays imagePoints, Size imageSize, int iFixedPoint,
InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
+ OutputArray newObjPoints,
+ OutputArray stdDeviationsIntrinsics,
+ OutputArray stdDeviationsExtrinsics,
+ OutputArray stdDeviationsObjPoints,
+ OutputArray perViewErrors,
+ int flags = 0, TermCriteria criteria = TermCriteria(
+ TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
+
+/** @overload */
+CV_EXPORTS_W double calibrateCameraRO( InputArrayOfArrays objectPoints,
+ InputArrayOfArrays imagePoints, Size imageSize, int iFixedPoint,
+ InputOutputArray cameraMatrix, InputOutputArray distCoeffs,
+ OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
+ OutputArray newObjPoints,
int flags = 0, TermCriteria criteria = TermCriteria(
TermCriteria::COUNT + TermCriteria::EPS, 30, DBL_EPSILON) );
- R. Horaud, F. Dornaika Hand-Eye Calibration \cite Horaud95
Another approach consists in estimating simultaneously the rotation and the translation (simultaneous solutions),
-with the following implemented method:
+with the following implemented methods:
- N. Andreff, R. Horaud, B. Espiau On-line Hand-Eye Calibration \cite Andreff99
- K. Daniilidis Hand-Eye Calibration Using Dual Quaternions \cite Daniilidis98
The following picture describes the Hand-Eye calibration problem where the transformation between a camera ("eye")
-mounted on a robot gripper ("hand") has to be estimated.
+mounted on a robot gripper ("hand") has to be estimated. This configuration is called eye-in-hand.
+
+The eye-to-hand configuration consists in a static camera observing a calibration pattern mounted on the robot
+end-effector. The transformation from the camera to the robot base frame can then be estimated by inputting
+the suitable transformations to the function, see below.
![](pics/hand-eye_figure.png)
\f]
This problem is also known as solving the \f$\mathbf{A}\mathbf{X}=\mathbf{X}\mathbf{B}\f$ equation:
+ - for an eye-in-hand configuration
\f[
\begin{align*}
^{b}{\textrm{T}_g}^{(1)} \hspace{0.2em} ^{g}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(1)} &=
\end{align*}
\f]
+ - for an eye-to-hand configuration
+\f[
+ \begin{align*}
+ ^{g}{\textrm{T}_b}^{(1)} \hspace{0.2em} ^{b}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(1)} &=
+ \hspace{0.1em} ^{g}{\textrm{T}_b}^{(2)} \hspace{0.2em} ^{b}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} \\
+
+ (^{g}{\textrm{T}_b}^{(2)})^{-1} \hspace{0.2em} ^{g}{\textrm{T}_b}^{(1)} \hspace{0.2em} ^{b}\textrm{T}_c &=
+ \hspace{0.1em} ^{b}\textrm{T}_c \hspace{0.2em} ^{c}{\textrm{T}_t}^{(2)} (^{c}{\textrm{T}_t}^{(1)})^{-1} \\
+
+ \textrm{A}_i \textrm{X} &= \textrm{X} \textrm{B}_i \\
+ \end{align*}
+\f]
+
\note
Additional information can be found on this [website](http://campar.in.tum.de/Chair/HandEyeCalibration).
\note
OutputArray R_cam2gripper, OutputArray t_cam2gripper,
HandEyeCalibrationMethod method=CALIB_HAND_EYE_TSAI );
+/** @brief Computes Robot-World/Hand-Eye calibration: \f$_{}^{w}\textrm{T}_b\f$ and \f$_{}^{c}\textrm{T}_g\f$
+
+@param[in] R_world2cam Rotation part extracted from the homogeneous matrix that transforms a point
+expressed in the world frame to the camera frame (\f$_{}^{c}\textrm{T}_w\f$).
+This is a vector (`vector<Mat>`) that contains the rotation, `(3x3)` rotation matrices or `(3x1)` rotation vectors,
+for all the transformations from world frame to the camera frame.
+@param[in] t_world2cam Translation part extracted from the homogeneous matrix that transforms a point
+expressed in the world frame to the camera frame (\f$_{}^{c}\textrm{T}_w\f$).
+This is a vector (`vector<Mat>`) that contains the `(3x1)` translation vectors for all the transformations
+from world frame to the camera frame.
+@param[in] R_base2gripper Rotation part extracted from the homogeneous matrix that transforms a point
+expressed in the robot base frame to the gripper frame (\f$_{}^{g}\textrm{T}_b\f$).
+This is a vector (`vector<Mat>`) that contains the rotation, `(3x3)` rotation matrices or `(3x1)` rotation vectors,
+for all the transformations from robot base frame to the gripper frame.
+@param[in] t_base2gripper Rotation part extracted from the homogeneous matrix that transforms a point
+expressed in the robot base frame to the gripper frame (\f$_{}^{g}\textrm{T}_b\f$).
+This is a vector (`vector<Mat>`) that contains the `(3x1)` translation vectors for all the transformations
+from robot base frame to the gripper frame.
+@param[out] R_base2world Estimated `(3x3)` rotation part extracted from the homogeneous matrix that transforms a point
+expressed in the robot base frame to the world frame (\f$_{}^{w}\textrm{T}_b\f$).
+@param[out] t_base2world Estimated `(3x1)` translation part extracted from the homogeneous matrix that transforms a point
+expressed in the robot base frame to the world frame (\f$_{}^{w}\textrm{T}_b\f$).
+@param[out] R_gripper2cam Estimated `(3x3)` rotation part extracted from the homogeneous matrix that transforms a point
+expressed in the gripper frame to the camera frame (\f$_{}^{c}\textrm{T}_g\f$).
+@param[out] t_gripper2cam Estimated `(3x1)` translation part extracted from the homogeneous matrix that transforms a point
+expressed in the gripper frame to the camera frame (\f$_{}^{c}\textrm{T}_g\f$).
+@param[in] method One of the implemented Robot-World/Hand-Eye calibration method, see cv::RobotWorldHandEyeCalibrationMethod
+
+The function performs the Robot-World/Hand-Eye calibration using various methods. One approach consists in estimating the
+rotation then the translation (separable solutions):
+ - M. Shah, Solving the robot-world/hand-eye calibration problem using the kronecker product \cite Shah2013SolvingTR
+
+Another approach consists in estimating simultaneously the rotation and the translation (simultaneous solutions),
+with the following implemented method:
+ - A. Li, L. Wang, and D. Wu, Simultaneous robot-world and hand-eye calibration using dual-quaternions and kronecker product \cite Li2010SimultaneousRA
+
+The following picture describes the Robot-World/Hand-Eye calibration problem where the transformations between a robot and a world frame
+and between a robot gripper ("hand") and a camera ("eye") mounted at the robot end-effector have to be estimated.
+
+![](pics/robot-world_hand-eye_figure.png)
+
+The calibration procedure is the following:
+ - a static calibration pattern is used to estimate the transformation between the target frame
+ and the camera frame
+ - the robot gripper is moved in order to acquire several poses
+ - for each pose, the homogeneous transformation between the gripper frame and the robot base frame is recorded using for
+ instance the robot kinematics
+\f[
+ \begin{bmatrix}
+ X_g\\
+ Y_g\\
+ Z_g\\
+ 1
+ \end{bmatrix}
+ =
+ \begin{bmatrix}
+ _{}^{g}\textrm{R}_b & _{}^{g}\textrm{t}_b \\
+ 0_{1 \times 3} & 1
+ \end{bmatrix}
+ \begin{bmatrix}
+ X_b\\
+ Y_b\\
+ Z_b\\
+ 1
+ \end{bmatrix}
+\f]
+ - for each pose, the homogeneous transformation between the calibration target frame (the world frame) and the camera frame is recorded using
+ for instance a pose estimation method (PnP) from 2D-3D point correspondences
+\f[
+ \begin{bmatrix}
+ X_c\\
+ Y_c\\
+ Z_c\\
+ 1
+ \end{bmatrix}
+ =
+ \begin{bmatrix}
+ _{}^{c}\textrm{R}_w & _{}^{c}\textrm{t}_w \\
+ 0_{1 \times 3} & 1
+ \end{bmatrix}
+ \begin{bmatrix}
+ X_w\\
+ Y_w\\
+ Z_w\\
+ 1
+ \end{bmatrix}
+\f]
+
+The Robot-World/Hand-Eye calibration procedure returns the following homogeneous transformations
+\f[
+ \begin{bmatrix}
+ X_w\\
+ Y_w\\
+ Z_w\\
+ 1
+ \end{bmatrix}
+ =
+ \begin{bmatrix}
+ _{}^{w}\textrm{R}_b & _{}^{w}\textrm{t}_b \\
+ 0_{1 \times 3} & 1
+ \end{bmatrix}
+ \begin{bmatrix}
+ X_b\\
+ Y_b\\
+ Z_b\\
+ 1
+ \end{bmatrix}
+\f]
+\f[
+ \begin{bmatrix}
+ X_c\\
+ Y_c\\
+ Z_c\\
+ 1
+ \end{bmatrix}
+ =
+ \begin{bmatrix}
+ _{}^{c}\textrm{R}_g & _{}^{c}\textrm{t}_g \\
+ 0_{1 \times 3} & 1
+ \end{bmatrix}
+ \begin{bmatrix}
+ X_g\\
+ Y_g\\
+ Z_g\\
+ 1
+ \end{bmatrix}
+\f]
+
+This problem is also known as solving the \f$\mathbf{A}\mathbf{X}=\mathbf{Z}\mathbf{B}\f$ equation, with:
+ - \f$\mathbf{A} \Leftrightarrow \hspace{0.1em} _{}^{c}\textrm{T}_w\f$
+ - \f$\mathbf{X} \Leftrightarrow \hspace{0.1em} _{}^{w}\textrm{T}_b\f$
+ - \f$\mathbf{Z} \Leftrightarrow \hspace{0.1em} _{}^{c}\textrm{T}_g\f$
+ - \f$\mathbf{B} \Leftrightarrow \hspace{0.1em} _{}^{g}\textrm{T}_b\f$
+
+\note
+At least 3 measurements are required (input vectors size must be greater or equal to 3).
+
+ */
+CV_EXPORTS_W void calibrateRobotWorldHandEye( InputArrayOfArrays R_world2cam, InputArrayOfArrays t_world2cam,
+ InputArrayOfArrays R_base2gripper, InputArrayOfArrays t_base2gripper,
+ OutputArray R_base2world, OutputArray t_base2world,
+ OutputArray R_gripper2cam, OutputArray t_gripper2cam,
+ RobotWorldHandEyeCalibrationMethod method=CALIB_ROBOT_WORLD_HAND_EYE_SHAH );
+
/** @brief Converts points from Euclidean to homogeneous space.
@param src Input vector of N-dimensional points.
OutputArray mask, int method = FM_RANSAC,
double ransacReprojThreshold = 3., double confidence = 0.99 );
+
+CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2,
+ OutputArray mask, const UsacParams ¶ms);
+
/** @brief Calculates an essential matrix from the corresponding points in two images.
@param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should
int method = RANSAC, double prob = 0.999,
double threshold = 1.0, OutputArray mask = noArray() );
+/** @brief Calculates an essential matrix from the corresponding points in two images from potentially two different cameras.
+
+@param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should
+be floating-point (single or double precision).
+@param points2 Array of the second image points of the same size and format as points1 .
+@param cameraMatrix1 Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
+Note that this function assumes that points1 and points2 are feature points from cameras with the
+same camera matrix. If this assumption does not hold for your use case, use
+`undistortPoints()` with `P = cv::NoArray()` for both cameras to transform image points
+to normalized image coordinates, which are valid for the identity camera matrix. When
+passing these coordinates, pass the identity matrix for this parameter.
+@param cameraMatrix2 Camera matrix \f$K = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
+Note that this function assumes that points1 and points2 are feature points from cameras with the
+same camera matrix. If this assumption does not hold for your use case, use
+`undistortPoints()` with `P = cv::NoArray()` for both cameras to transform image points
+to normalized image coordinates, which are valid for the identity camera matrix. When
+passing these coordinates, pass the identity matrix for this parameter.
+@param distCoeffs1 Input vector of distortion coefficients
+\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
+of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
+@param distCoeffs2 Input vector of distortion coefficients
+\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
+of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
+@param method Method for computing an essential matrix.
+- **RANSAC** for the RANSAC algorithm.
+- **LMEDS** for the LMedS algorithm.
+@param prob Parameter used for the RANSAC or LMedS methods only. It specifies a desirable level of
+confidence (probability) that the estimated matrix is correct.
+@param threshold Parameter used for RANSAC. It is the maximum distance from a point to an epipolar
+line in pixels, beyond which the point is considered an outlier and is not used for computing the
+final fundamental matrix. It can be set to something like 1-3, depending on the accuracy of the
+point localization, image resolution, and the image noise.
+@param mask Output array of N elements, every element of which is set to 0 for outliers and to 1
+for the other points. The array is computed only in the RANSAC and LMedS methods.
+
+This function estimates essential matrix based on the five-point algorithm solver in @cite Nister03 .
+@cite SteweniusCFS is also a related. The epipolar geometry is described by the following equation:
+
+\f[[p_2; 1]^T K^{-T} E K^{-1} [p_1; 1] = 0\f]
+
+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
+second images, respectively. The result of this function may be passed further to
+decomposeEssentialMat or recoverPose to recover the relative pose between cameras.
+ */
+CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
+ InputArray cameraMatrix1, InputArray distCoeffs1,
+ InputArray cameraMatrix2, InputArray distCoeffs2,
+ int method = RANSAC,
+ double prob = 0.999, double threshold = 1.0,
+ OutputArray mask = noArray() );
+
+
+CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
+ InputArray cameraMatrix1, InputArray cameraMatrix2,
+ InputArray dist_coeff1, InputArray dist_coeff2, OutputArray mask,
+ const UsacParams ¶ms);
+
/** @brief Decompose an essential matrix to possible rotations and translation.
@param E The input essential matrix.
OutputArray out, OutputArray inliers,
double ransacThreshold = 3, double confidence = 0.99);
+/** @brief Computes an optimal translation between two 3D point sets.
+ *
+ * It computes
+ * \f[
+ * \begin{bmatrix}
+ * x\\
+ * y\\
+ * z\\
+ * \end{bmatrix}
+ * =
+ * \begin{bmatrix}
+ * X\\
+ * Y\\
+ * Z\\
+ * \end{bmatrix}
+ * +
+ * \begin{bmatrix}
+ * b_1\\
+ * b_2\\
+ * b_3\\
+ * \end{bmatrix}
+ * \f]
+ *
+ * @param src First input 3D point set containing \f$(X,Y,Z)\f$.
+ * @param dst Second input 3D point set containing \f$(x,y,z)\f$.
+ * @param out Output 3D translation vector \f$3 \times 1\f$ of the form
+ * \f[
+ * \begin{bmatrix}
+ * b_1 \\
+ * b_2 \\
+ * b_3 \\
+ * \end{bmatrix}
+ * \f]
+ * @param inliers Output vector indicating which points are inliers (1-inlier, 0-outlier).
+ * @param ransacThreshold Maximum reprojection error in the RANSAC algorithm to consider a point as
+ * an inlier.
+ * @param confidence Confidence level, between 0 and 1, for the estimated transformation. Anything
+ * between 0.95 and 0.99 is usually good enough. Values too close to 1 can slow down the estimation
+ * significantly. Values lower than 0.8-0.9 can result in an incorrectly estimated transformation.
+ *
+ * The function estimates an optimal 3D translation between two 3D point sets using the
+ * RANSAC algorithm.
+ * */
+CV_EXPORTS_W int estimateTranslation3D(InputArray src, InputArray dst,
+ OutputArray out, OutputArray inliers,
+ double ransacThreshold = 3, double confidence = 0.99);
+
/** @brief Computes an optimal affine transformation between two 2D point sets.
It computes
size_t maxIters = 2000, double confidence = 0.99,
size_t refineIters = 10);
+
+CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray pts1, InputArray pts2, OutputArray inliers,
+ const UsacParams ¶ms);
+
/** @brief Computes an optimal limited affine transformation with 4 degrees of freedom between
two 2D point sets.
int mode = StereoSGBM::MODE_SGBM);
};
+
+//! cv::undistort mode
+enum UndistortTypes
+{
+ PROJ_SPHERICAL_ORTHO = 0,
+ PROJ_SPHERICAL_EQRECT = 1
+};
+
+/** @brief Transforms an image to compensate for lens distortion.
+
+The function transforms an image to compensate radial and tangential lens distortion.
+
+The function is simply a combination of #initUndistortRectifyMap (with unity R ) and #remap
+(with bilinear interpolation). See the former function for details of the transformation being
+performed.
+
+Those pixels in the destination image, for which there is no correspondent pixels in the source
+image, are filled with zeros (black color).
+
+A particular subset of the source image that will be visible in the corrected image can be regulated
+by newCameraMatrix. You can use #getOptimalNewCameraMatrix to compute the appropriate
+newCameraMatrix depending on your requirements.
+
+The camera matrix and the distortion parameters can be determined using #calibrateCamera. If
+the resolution of images is different from the resolution used at the calibration stage, \f$f_x,
+f_y, c_x\f$ and \f$c_y\f$ need to be scaled accordingly, while the distortion coefficients remain
+the same.
+
+@param src Input (distorted) image.
+@param dst Output (corrected) image that has the same size and type as src .
+@param cameraMatrix Input camera matrix \f$A = \vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
+@param distCoeffs Input vector of distortion coefficients
+\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
+of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
+@param newCameraMatrix Camera matrix of the distorted image. By default, it is the same as
+cameraMatrix but you may additionally scale and shift the result by using a different matrix.
+ */
+CV_EXPORTS_W void undistort( InputArray src, OutputArray dst,
+ InputArray cameraMatrix,
+ InputArray distCoeffs,
+ InputArray newCameraMatrix = noArray() );
+
+/** @brief Computes the undistortion and rectification transformation map.
+
+The function computes the joint undistortion and rectification transformation and represents the
+result in the form of maps for remap. The undistorted image looks like original, as if it is
+captured with a camera using the camera matrix =newCameraMatrix and zero distortion. In case of a
+monocular camera, newCameraMatrix is usually equal to cameraMatrix, or it can be computed by
+#getOptimalNewCameraMatrix for a better control over scaling. In case of a stereo camera,
+newCameraMatrix is normally set to P1 or P2 computed by #stereoRectify .
+
+Also, this new camera is oriented differently in the coordinate space, according to R. That, for
+example, helps to align two heads of a stereo camera so that the epipolar lines on both images
+become horizontal and have the same y- coordinate (in case of a horizontally aligned stereo camera).
+
+The function actually builds the maps for the inverse mapping algorithm that is used by remap. That
+is, for each pixel \f$(u, v)\f$ in the destination (corrected and rectified) image, the function
+computes the corresponding coordinates in the source image (that is, in the original image from
+camera). The following process is applied:
+\f[
+\begin{array}{l}
+x \leftarrow (u - {c'}_x)/{f'}_x \\
+y \leftarrow (v - {c'}_y)/{f'}_y \\
+{[X\,Y\,W]} ^T \leftarrow R^{-1}*[x \, y \, 1]^T \\
+x' \leftarrow X/W \\
+y' \leftarrow Y/W \\
+r^2 \leftarrow x'^2 + y'^2 \\
+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}
++ 2p_1 x' y' + p_2(r^2 + 2 x'^2) + s_1 r^2 + s_2 r^4\\
+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}
++ p_1 (r^2 + 2 y'^2) + 2 p_2 x' y' + s_3 r^2 + s_4 r^4 \\
+s\vecthree{x'''}{y'''}{1} =
+\vecthreethree{R_{33}(\tau_x, \tau_y)}{0}{-R_{13}((\tau_x, \tau_y)}
+{0}{R_{33}(\tau_x, \tau_y)}{-R_{23}(\tau_x, \tau_y)}
+{0}{0}{1} R(\tau_x, \tau_y) \vecthree{x''}{y''}{1}\\
+map_x(u,v) \leftarrow x''' f_x + c_x \\
+map_y(u,v) \leftarrow y''' f_y + c_y
+\end{array}
+\f]
+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$
+are the distortion coefficients.
+
+In case of a stereo camera, this function is called twice: once for each camera head, after
+stereoRectify, which in its turn is called after #stereoCalibrate. But if the stereo camera
+was not calibrated, it is still possible to compute the rectification transformations directly from
+the fundamental matrix using #stereoRectifyUncalibrated. For each camera, the function computes
+homography H as the rectification transformation in a pixel domain, not a rotation matrix R in 3D
+space. R can be computed from H as
+\f[\texttt{R} = \texttt{cameraMatrix} ^{-1} \cdot \texttt{H} \cdot \texttt{cameraMatrix}\f]
+where cameraMatrix can be chosen arbitrarily.
+
+@param cameraMatrix Input camera matrix \f$A=\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
+@param distCoeffs Input vector of distortion coefficients
+\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
+of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
+@param R Optional rectification transformation in the object space (3x3 matrix). R1 or R2 ,
+computed by #stereoRectify can be passed here. If the matrix is empty, the identity transformation
+is assumed. In cvInitUndistortMap R assumed to be an identity matrix.
+@param newCameraMatrix New camera matrix \f$A'=\vecthreethree{f_x'}{0}{c_x'}{0}{f_y'}{c_y'}{0}{0}{1}\f$.
+@param size Undistorted image size.
+@param m1type Type of the first output map that can be CV_32FC1, CV_32FC2 or CV_16SC2, see #convertMaps
+@param map1 The first output map.
+@param map2 The second output map.
+ */
+CV_EXPORTS_W
+void initUndistortRectifyMap(InputArray cameraMatrix, InputArray distCoeffs,
+ InputArray R, InputArray newCameraMatrix,
+ Size size, int m1type, OutputArray map1, OutputArray map2);
+
+//! initializes maps for #remap for wide-angle
+CV_EXPORTS
+float initWideAngleProjMap(InputArray cameraMatrix, InputArray distCoeffs,
+ Size imageSize, int destImageWidth,
+ int m1type, OutputArray map1, OutputArray map2,
+ enum UndistortTypes projType = PROJ_SPHERICAL_EQRECT, double alpha = 0);
+static inline
+float initWideAngleProjMap(InputArray cameraMatrix, InputArray distCoeffs,
+ Size imageSize, int destImageWidth,
+ int m1type, OutputArray map1, OutputArray map2,
+ int projType, double alpha = 0)
+{
+ return initWideAngleProjMap(cameraMatrix, distCoeffs, imageSize, destImageWidth,
+ m1type, map1, map2, (UndistortTypes)projType, alpha);
+}
+
+/** @brief Returns the default new camera matrix.
+
+The function returns the camera matrix that is either an exact copy of the input cameraMatrix (when
+centerPrinicipalPoint=false ), or the modified one (when centerPrincipalPoint=true).
+
+In the latter case, the new camera matrix will be:
+
+\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]
+
+where \f$f_x\f$ and \f$f_y\f$ are \f$(0,0)\f$ and \f$(1,1)\f$ elements of cameraMatrix, respectively.
+
+By default, the undistortion functions in OpenCV (see #initUndistortRectifyMap, #undistort) do not
+move the principal point. However, when you work with stereo, it is important to move the principal
+points in both views to the same y-coordinate (which is required by most of stereo correspondence
+algorithms), and may be to the same x-coordinate too. So, you can form the new camera matrix for
+each view where the principal points are located at the center.
+
+@param cameraMatrix Input camera matrix.
+@param imgsize Camera view image size in pixels.
+@param centerPrincipalPoint Location of the principal point in the new camera matrix. The
+parameter indicates whether this location should be at the image center or not.
+ */
+CV_EXPORTS_W
+Mat getDefaultNewCameraMatrix(InputArray cameraMatrix, Size imgsize = Size(),
+ bool centerPrincipalPoint = false);
+
+/** @brief Computes the ideal point coordinates from the observed point coordinates.
+
+The function is similar to #undistort and #initUndistortRectifyMap but it operates on a
+sparse set of points instead of a raster image. Also the function performs a reverse transformation
+to projectPoints. In case of a 3D object, it does not reconstruct its 3D coordinates, but for a
+planar object, it does, up to a translation vector, if the proper R is specified.
+
+For each observed point coordinate \f$(u, v)\f$ the function computes:
+\f[
+\begin{array}{l}
+x^{"} \leftarrow (u - c_x)/f_x \\
+y^{"} \leftarrow (v - c_y)/f_y \\
+(x',y') = undistort(x^{"},y^{"}, \texttt{distCoeffs}) \\
+{[X\,Y\,W]} ^T \leftarrow R*[x' \, y' \, 1]^T \\
+x \leftarrow X/W \\
+y \leftarrow Y/W \\
+\text{only performed if P is specified:} \\
+u' \leftarrow x {f'}_x + {c'}_x \\
+v' \leftarrow y {f'}_y + {c'}_y
+\end{array}
+\f]
+
+where *undistort* is an approximate iterative algorithm that estimates the normalized original
+point coordinates out of the normalized distorted point coordinates ("normalized" means that the
+coordinates do not depend on the camera matrix).
+
+The function can be used for both a stereo camera head or a monocular camera (when R is empty).
+@param src Observed point coordinates, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel (CV_32FC2 or CV_64FC2) (or
+vector\<Point2f\> ).
+@param dst Output ideal point coordinates (1xN/Nx1 2-channel or vector\<Point2f\> ) after undistortion and reverse perspective
+transformation. If matrix P is identity or omitted, dst will contain normalized point coordinates.
+@param cameraMatrix Camera matrix \f$\vecthreethree{f_x}{0}{c_x}{0}{f_y}{c_y}{0}{0}{1}\f$ .
+@param distCoeffs Input vector of distortion coefficients
+\f$(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])\f$
+of 4, 5, 8, 12 or 14 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
+@param R Rectification transformation in the object space (3x3 matrix). R1 or R2 computed by
+#stereoRectify can be passed here. If the matrix is empty, the identity transformation is used.
+@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
+#stereoRectify can be passed here. If the matrix is empty, the identity new camera matrix is used.
+ */
+CV_EXPORTS_W
+void undistortPoints(InputArray src, OutputArray dst,
+ InputArray cameraMatrix, InputArray distCoeffs,
+ InputArray R = noArray(), InputArray P = noArray());
+/** @overload
+ @note Default version of #undistortPoints does 5 iterations to compute undistorted points.
+ */
+CV_EXPORTS_AS(undistortPointsIter)
+void undistortPoints(InputArray src, OutputArray dst,
+ InputArray cameraMatrix, InputArray distCoeffs,
+ InputArray R, InputArray P, TermCriteria criteria);
+
//! @} calib3d
/** @brief The methods in this namespace use a so-called fisheye camera model.
} //end namespace cv
-#ifndef DISABLE_OPENCV_24_COMPATIBILITY
-#include "opencv2/calib3d/calib3d_c.h"
+#if 0 //def __cplusplus
+//////////////////////////////////////////////////////////////////////////////////////////
+class CV_EXPORTS CvLevMarq
+{
+public:
+ CvLevMarq();
+ CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria=
+ cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
+ bool completeSymmFlag=false );
+ ~CvLevMarq();
+ void init( int nparams, int nerrs, CvTermCriteria criteria=
+ cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
+ bool completeSymmFlag=false );
+ bool update( const CvMat*& param, CvMat*& J, CvMat*& err );
+ bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm );
+
+ void clear();
+ void step();
+ enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 };
+
+ cv::Ptr<CvMat> mask;
+ cv::Ptr<CvMat> prevParam;
+ cv::Ptr<CvMat> param;
+ cv::Ptr<CvMat> J;
+ cv::Ptr<CvMat> err;
+ cv::Ptr<CvMat> JtJ;
+ cv::Ptr<CvMat> JtJN;
+ cv::Ptr<CvMat> JtErr;
+ cv::Ptr<CvMat> JtJV;
+ cv::Ptr<CvMat> JtJW;
+ double prevErrNorm, errNorm;
+ int lambdaLg10;
+ CvTermCriteria criteria;
+ int state;
+ int iters;
+ bool completeSymmFlag;
+ int solveMethod;
+};
#endif
#endif