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 cameraMatrix, int method = RANSAC,
- double prob = 0.999, double threshold = 1.0,
- OutputArray mask = noArray() );
- InputArray cameraMatrix, int method,
- double prob, double threshold,
- int maxIters, OutputArray mask = noArray() );
++CV_EXPORTS_W
++Mat findEssentialMat(
++ InputArray points1, InputArray points2,
++ InputArray cameraMatrix, int method = RANSAC,
++ double prob = 0.999, double threshold = 1.0,
++ int maxIters = 1000, OutputArray mask = noArray()
++);
+
+ /** @overload */
-CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
- InputArray cameraMatrix, int method = RANSAC,
- double prob = 0.999, double threshold = 1.0,
- OutputArray mask = noArray() );
++CV_EXPORTS
++Mat findEssentialMat(
++ InputArray points1, InputArray points2,
++ InputArray cameraMatrix, int method,
++ double prob, double threshold,
++ OutputArray mask
++); // TODO remove from OpenCV 5.0
/** @overload
@param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should
0 & 0 & 1
\end{bmatrix}\f]
*/
- CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
- double focal = 1.0, Point2d pp = Point2d(0, 0),
- int method = RANSAC, double prob = 0.999,
- double threshold = 1.0, OutputArray mask = noArray() );
++CV_EXPORTS_W
++Mat findEssentialMat(
++ InputArray points1, InputArray points2,
++ double focal = 1.0, Point2d pp = Point2d(0, 0),
++ int method = RANSAC, double prob = 0.999,
++ double threshold = 1.0, int maxIters = 1000,
++ OutputArray mask = noArray()
++);
++
++/** @overload */
++CV_EXPORTS
++Mat findEssentialMat(
++ InputArray points1, InputArray points2,
++ double focal, Point2d pp,
++ int method, double prob,
++ double threshold, OutputArray mask
++); // TODO remove from OpenCV 5.0
+
+/** @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.
+- @ref RANSAC for the RANSAC algorithm.
+- @ref 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,
- double focal, Point2d pp,
- int method, double prob,
- double threshold, int maxIters,
+ InputArray cameraMatrix1, InputArray distCoeffs1,
+ InputArray cameraMatrix2, InputArray distCoeffs2,
+ int method = RANSAC,
+ double prob = 0.999, double threshold = 1.0,
OutputArray mask = noArray() );
-/** @overload */
+
CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
- double focal = 1.0, Point2d pp = Point2d(0, 0),
- int method = RANSAC, double prob = 0.999,
- double threshold = 1.0, OutputArray mask = noArray() );
+ 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.
CV_INSTRUMENT_REGION();
Mat cameraMatrix = (Mat_<double>(3,3) << focal, 0, pp.x, 0, focal, pp.y, 0, 0, 1);
- return cv::findEssentialMat(_points1, _points2, cameraMatrix, method, prob, threshold, _mask);
+ return cv::findEssentialMat(_points1, _points2, cameraMatrix, method, prob, threshold, 1000, _mask);
}
+cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2,
+ InputArray cameraMatrix1, InputArray distCoeffs1,
+ InputArray cameraMatrix2, InputArray distCoeffs2,
+ int method, double prob, double threshold, OutputArray _mask)
+{
+ CV_INSTRUMENT_REGION();
+
+ // Undistort image points, bring them to 3x3 identity "camera matrix"
+ Mat _pointsUntistorted1, _pointsUntistorted2;
+ undistortPoints(_points1, _pointsUntistorted1, cameraMatrix1, distCoeffs1);
+ undistortPoints(_points2, _pointsUntistorted2, cameraMatrix2, distCoeffs2);
+
+ // Scale the points back. We use "arithmetic mean" between the supplied two camera matrices.
+ // Thanks to such 2-stage procedure RANSAC threshold still makes sense, because the undistorted
+ // and rescaled points have a similar value range to the original ones.
+ Mat cm1 = cameraMatrix1.getMat(), cm2 = cameraMatrix2.getMat(), cm0;
+ Mat(cm1 + cm2).convertTo(cm0, CV_64F, 0.5);
+ CV_Assert(cm0.rows == 3 && cm0.cols == 3);
+ CV_Assert(std::abs(cm0.at<double>(2, 0)) < 1e-3 &&
+ std::abs(cm0.at<double>(2, 1)) < 1e-3 &&
+ std::abs(cm0.at<double>(2, 2) - 1.) < 1e-3);
+ Mat affine = cm0.rowRange(0, 2);
+
+ transform(_pointsUntistorted1, _pointsUntistorted1, affine);
+ transform(_pointsUntistorted2, _pointsUntistorted2, affine);
+
+ return findEssentialMat(_pointsUntistorted1, _pointsUntistorted2, cm0, method, prob, threshold, _mask);
+}
+
+cv::Mat cv::findEssentialMat( InputArray points1, InputArray points2,
+ InputArray cameraMatrix1, InputArray cameraMatrix2,
+ InputArray dist_coeff1, InputArray dist_coeff2, OutputArray mask, const UsacParams ¶ms) {
+ Ptr<usac::Model> model;
+ usac::setParameters(model, usac::EstimationMethod::Essential, params, mask.needed());
+ Ptr<usac::RansacOutput> ransac_output;
+ if (usac::run(model, points1, points2, model->getRandomGeneratorState(),
+ ransac_output, cameraMatrix1, cameraMatrix2, dist_coeff1, dist_coeff2)) {
+ usac::saveMask(mask, ransac_output->getInliersMask());
+ return ransac_output->getModel();
+ } else return Mat();
+
+}
+
int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2,
InputArray _cameraMatrix, OutputArray _R, OutputArray _t, double distanceThresh,
InputOutputArray _mask, OutputArray triangulatedPoints)