Merge remote-tracking branch 'upstream/3.4' into merge-3.4
authorAlexander Alekhin <alexander.a.alekhin@gmail.com>
Thu, 8 Apr 2021 10:48:51 +0000 (10:48 +0000)
committerAlexander Alekhin <alexander.a.alekhin@gmail.com>
Thu, 8 Apr 2021 11:23:24 +0000 (11:23 +0000)
1  2 
modules/calib3d/include/opencv2/calib3d.hpp
modules/calib3d/src/five-point.cpp
modules/calib3d/src/fundam.cpp
modules/imgproc/src/imgwarp.cpp

@@@ -2700,10 -2252,16 +2701,22 @@@ where \f$E\f$ is an essential matrix, \
  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
@@@ -2734,67 -2293,17 +2748,80 @@@ f & 0 & x_{pp}  \
  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 &params);
  
  /** @brief Decompose an essential matrix to possible rotations and translation.
  
@@@ -461,52 -472,9 +478,52 @@@ cv::Mat cv::findEssentialMat( InputArra
      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 &params) {
 +    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)
Simple merge
Simple merge