CMakeLists.txt

@@ -777,6 +777,15 @@ if(BUILD_JAVA)
     include(cmake/android/OpenCVDetectAndroidSDK.cmake)
   else()
     include(cmake/OpenCVDetectApacheAnt.cmake)
+    if(ANT_EXECUTABLE AND NOT OPENCV_JAVA_IGNORE_ANT)
+      ocv_update(OPENCV_JAVA_SDK_BUILD_TYPE "ANT")
+    elseif(NOT ANDROID)
+      find_package(Java)
+      if(Java_FOUND)
+        include(UseJava)
+        ocv_update(OPENCV_JAVA_SDK_BUILD_TYPE "JAVA")
+      endif()
+    endif()
     find_package(JNI)
   endif()
 endif()
@@ -1802,9 +1811,10 @@ if(BUILD_JAVA)
   status("  Java:"            BUILD_FAT_JAVA_LIB  THEN "export all functions"                                      ELSE "")
   status("    ant:"           ANT_EXECUTABLE      THEN "${ANT_EXECUTABLE} (ver ${ANT_VERSION})"                    ELSE NO)
   if(NOT ANDROID)
+    status("    Java:"        Java_FOUND     THEN "YES (ver ${Java_VERSION})"                                      ELSE NO)
     status("    JNI:"         JNI_INCLUDE_DIRS    THEN "${JNI_INCLUDE_DIRS}"                                       ELSE NO)
   endif()
-  status("    Java wrappers:" HAVE_opencv_java                                                            THEN YES ELSE NO)
+  status("    Java wrappers:" HAVE_opencv_java                                                            THEN "YES (${OPENCV_JAVA_SDK_BUILD_TYPE})" ELSE NO)
   status("    Java tests:"    BUILD_TESTS AND opencv_test_java_BINARY_DIR                                 THEN YES ELSE NO)
 endif()
 

modules/calib3d/include/opencv2/calib3d.hpp

@@ -548,12 +548,13 @@ enum RobotWorldHandEyeCalibrationMethod
     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 };
+enum SamplingMethod { SAMPLING_UNIFORM=0, SAMPLING_PROGRESSIVE_NAPSAC=1, SAMPLING_NAPSAC=2,
+        SAMPLING_PROSAC=3 };
+enum LocalOptimMethod {LOCAL_OPTIM_NULL=0, LOCAL_OPTIM_INNER_LO=1, LOCAL_OPTIM_INNER_AND_ITER_LO=2,
+        LOCAL_OPTIM_GC=3, LOCAL_OPTIM_SIGMA=4};
+enum ScoreMethod {SCORE_METHOD_RANSAC=0, SCORE_METHOD_MSAC=1, SCORE_METHOD_MAGSAC=2, SCORE_METHOD_LMEDS=3};
+enum NeighborSearchMethod { NEIGH_FLANN_KNN=0, NEIGH_GRID=1, NEIGH_FLANN_RADIUS=2 };
+enum PolishingMethod { NONE_POLISHER=0, LSQ_POLISHER=1, MAGSAC=2, COV_POLISHER=3 };
 
 struct CV_EXPORTS_W_SIMPLE UsacParams
 { // in alphabetical order
@@ -569,6 +570,8 @@ struct CV_EXPORTS_W_SIMPLE UsacParams
     CV_PROP_RW SamplingMethod sampler;
     CV_PROP_RW ScoreMethod score;
     CV_PROP_RW double threshold;
+    CV_PROP_RW PolishingMethod final_polisher;
+    CV_PROP_RW int final_polisher_iterations;
 };
 
 /** @brief Converts a rotation matrix to a rotation vector or vice versa.

modules/calib3d/src/five-point.cpp

@@ -522,9 +522,9 @@ 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());
+    usac::setParameters(model, usac::EstimationMethod::ESSENTIAL, params, mask.needed());
     Ptr<usac::RansacOutput> ransac_output;
-    if (usac::run(model, points1, points2, model->getRandomGeneratorState(),
+    if (usac::run(model, points1, points2,
             ransac_output, cameraMatrix1, cameraMatrix2, dist_coeff1, dist_coeff2)) {
         usac::saveMask(mask, ransac_output->getInliersMask());
         return ransac_output->getModel();

modules/calib3d/src/fundam.cpp

@@ -451,9 +451,9 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
 cv::Mat cv::findHomography(InputArray srcPoints, InputArray dstPoints, OutputArray mask,
                    const UsacParams &params) {
     Ptr<usac::Model> model;
-    usac::setParameters(model, usac::EstimationMethod::Homography, params, mask.needed());
+    usac::setParameters(model, usac::EstimationMethod::HOMOGRAPHY, params, mask.needed());
     Ptr<usac::RansacOutput> ransac_output;
-    if (usac::run(model, srcPoints, dstPoints, model->getRandomGeneratorState(),
+    if (usac::run(model, srcPoints, dstPoints,
             ransac_output, noArray(), noArray(), noArray(), noArray())) {
         usac::saveMask(mask, ransac_output->getInliersMask());
         return ransac_output->getModel() / ransac_output->getModel().at<double>(2,2);
@@ -913,10 +913,10 @@ cv::Mat cv::findFundamentalMat( cv::InputArray points1, cv::InputArray points2,
 cv::Mat cv::findFundamentalMat( InputArray points1, InputArray points2,
                         OutputArray mask, const UsacParams &params) {
     Ptr<usac::Model> model;
-    setParameters(model, usac::EstimationMethod::Fundamental, params, mask.needed());
+    setParameters(model, usac::EstimationMethod::FUNDAMENTAL, params, mask.needed());
     CV_Assert(model);
     Ptr<usac::RansacOutput> ransac_output;
-    if (usac::run(model, points1, points2, model->getRandomGeneratorState(),
+    if (usac::run(model, points1, points2,
             ransac_output, noArray(), noArray(), noArray(), noArray())) {
         usac::saveMask(mask, ransac_output->getInliersMask());
         return ransac_output->getModel();

modules/calib3d/src/ptsetreg.cpp

@@ -1086,9 +1086,9 @@ Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray _inliers,
 Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray inliers,
                      const UsacParams &params) {
     Ptr<usac::Model> model;
-    usac::setParameters(model, usac::EstimationMethod::Affine, params, inliers.needed());
+    usac::setParameters(model, usac::EstimationMethod::AFFINE, params, inliers.needed());
     Ptr<usac::RansacOutput> ransac_output;
-    if (usac::run(model, _from, _to, model->getRandomGeneratorState(),
+    if (usac::run(model, _from, _to,
             ransac_output, noArray(), noArray(), noArray(), noArray())) {
         usac::saveMask(inliers, ransac_output->getInliersMask());
         return ransac_output->getModel().rowRange(0,2);

modules/calib3d/src/solvepnp.cpp

@@ -199,21 +199,6 @@ public:
     Mat tvec;
 };
 
-UsacParams::UsacParams()
-{
-    confidence = 0.99;
-    isParallel = false;
-    loIterations = 5;
-    loMethod = LocalOptimMethod::LOCAL_OPTIM_INNER_LO;
-    loSampleSize = 14;
-    maxIterations = 5000;
-    neighborsSearch = NeighborSearchMethod::NEIGH_GRID;
-    randomGeneratorState = 0;
-    sampler = SamplingMethod::SAMPLING_UNIFORM;
-    score = ScoreMethod::SCORE_METHOD_MSAC;
-    threshold = 1.5;
-}
-
 bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
                     InputArray _cameraMatrix, InputArray _distCoeffs,
                     OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
@@ -407,7 +392,7 @@ bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
     usac::setParameters(model_params, cameraMatrix.empty() ? usac::EstimationMethod::P6P :
         usac::EstimationMethod::P3P, params, inliers.needed());
     Ptr<usac::RansacOutput> ransac_output;
-    if (usac::run(model_params, imagePoints, objectPoints, model_params->getRandomGeneratorState(),
+    if (usac::run(model_params, imagePoints, objectPoints,
             ransac_output, cameraMatrix, noArray(), distCoeffs, noArray())) {
         if (inliers.needed()) {
             const auto &inliers_mask = ransac_output->getInliersMask();

modules/calib3d/src/usac/bundle.cpp

+// Copyright (c) 2020, Viktor Larsson
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of the copyright holder nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "../precomp.hpp"
+#include "../usac.hpp"
+
+namespace cv { namespace usac {
+class MlesacLoss {
+  public:
+    MlesacLoss(double threshold) : squared_thr(threshold * threshold), norm_thr(squared_thr*3), one_over_thr(1/norm_thr), inv_sq_thr(1/squared_thr) {}
+    double loss(double r2) const {
+        return r2 < norm_thr ? r2 * one_over_thr - 1 : 0;
+    }
+    double weight(double r2) const {
+        // use Cauchly weight
+        return 1.0 / (1.0 + r2 * inv_sq_thr);
+    }
+    const double squared_thr;
+    private:
+    const double norm_thr, one_over_thr, inv_sq_thr;
+};
+
+class RelativePoseJacobianAccumulator {
+private:
+    const Mat* correspondences;
+    const std::vector<int> &sample;
+    const int sample_size;
+    const MlesacLoss &loss_fn;
+    const double *weights;
+
+public:
+    RelativePoseJacobianAccumulator(
+        const Mat& correspondences_,
+        const std::vector<int> &sample_,
+        const int sample_size_,
+        const MlesacLoss &l,
+        const double *w = nullptr) :
+            correspondences(&correspondences_),
+            sample(sample_),
+            sample_size(sample_size_),
+            loss_fn(l),
+            weights(w) {}
+
+    Matx33d essential_from_motion(const CameraPose &pose) const {
+        return Matx33d(0.0, -pose.t(2), pose.t(1),
+            pose.t(2), 0.0, -pose.t(0),
+            -pose.t(1), pose.t(0), 0.0) * pose.R;
+    }
+
+    double residual(const CameraPose &pose) const {
+        const Matx33d E = essential_from_motion(pose);
+        const float m11=static_cast<float>(E(0,0)), m12=static_cast<float>(E(0,1)), m13=static_cast<float>(E(0,2));
+        const float m21=static_cast<float>(E(1,0)), m22=static_cast<float>(E(1,1)), m23=static_cast<float>(E(1,2));
+        const float m31=static_cast<float>(E(2,0)), m32=static_cast<float>(E(2,1)), m33=static_cast<float>(E(2,2));
+        const auto * const pts = (float *) correspondences->data;
+        double cost = 0.0;
+        for (int k = 0; k < sample_size; ++k) {
+            const int idx = 4*sample[k];
+            const float x1=pts[idx], y1=pts[idx+1], x2=pts[idx+2], y2=pts[idx+3];
+            const float F_pt1_x = m11 * x1 + m12 * y1 + m13,
+                        F_pt1_y = m21 * x1 + m22 * y1 + m23;
+            const float pt2_F_x = x2 * m11 + y2 * m21 + m31,
+                        pt2_F_y = x2 * m12 + y2 * m22 + m32;
+            const float pt2_F_pt1 = x2 * F_pt1_x + y2 * F_pt1_y + m31 * x1 + m32 * y1 + m33;
+            const float r2 = pt2_F_pt1 * pt2_F_pt1 / (F_pt1_x * F_pt1_x + F_pt1_y * F_pt1_y +
+                                                      pt2_F_x * pt2_F_x + pt2_F_y * pt2_F_y);
+            if (weights == nullptr)
+                 cost += loss_fn.loss(r2);
+            else cost += weights[k] * loss_fn.loss(r2);
+        }
+        return cost;
+    }
+
+    void accumulate(const CameraPose &pose, Matx<double, 5, 5> &JtJ, Matx<double, 5, 1> &Jtr, Matx<double, 3, 2> &tangent_basis) const {
+        const auto * const pts = (float *) correspondences->data;
+        // We start by setting up a basis for the updates in the translation (orthogonal to t)
+        // We find the minimum element of t and cross product with the corresponding basis vector.
+        // (this ensures that the first cross product is not close to the zero vector)
+        Vec3d tangent_basis_col0;
+        if (std::abs(pose.t(0)) < std::abs(pose.t(1))) {
+            // x < y
+            if (std::abs(pose.t(0)) < std::abs(pose.t(2))) {
+                tangent_basis_col0 = pose.t.cross(Vec3d(1,0,0));
+            } else {
+                tangent_basis_col0 = pose.t.cross(Vec3d(0,0,1));
+            }
+        } else {
+            // x > y
+            if (std::abs(pose.t(1)) < std::abs(pose.t(2))) {
+                tangent_basis_col0 = pose.t.cross(Vec3d(0,1,0));
+            } else {
+                tangent_basis_col0 = pose.t.cross(Vec3d(0,0,1));
+            }
+        }
+        tangent_basis_col0 /= norm(tangent_basis_col0);
+        Vec3d tangent_basis_col1 = pose.t.cross(tangent_basis_col0);
+        tangent_basis_col1 /= norm(tangent_basis_col1);
+        for (int i = 0; i < 3; i++) {
+            tangent_basis(i,0) = tangent_basis_col0(i);
+            tangent_basis(i,1) = tangent_basis_col1(i);
+        }
+
+        const Matx33d E = essential_from_motion(pose);
+
+        // Matrices contain the jacobians of E w.r.t. the rotation and translation parameters
+        // Each column is vec(E*skew(e_k)) where e_k is k:th basis vector
+        const Matx<double, 9, 3> dR = {0., -E(0,2), E(0,1),
+                                       0., -E(1,2), E(1,1),
+                                       0., -E(2,2), E(2,1),
+                                       E(0,2), 0., -E(0,0),
+                                       E(1,2), 0., -E(1,0),
+                                       E(2,2), 0., -E(2,0),
+                                       -E(0,1), E(0,0), 0.,
+                                       -E(1,1), E(1,0), 0.,
+                                       -E(2,1), E(2,0), 0.};
+
+        Matx<double, 9, 2> dt;
+        // Each column is vec(skew(tangent_basis[k])*R)
+        for (int i = 0; i <= 2; i+=1) {
+            const Vec3d r_i(pose.R(0,i), pose.R(1,i), pose.R(2,i));
+            for (int j = 0; j <= 1; j+= 1) {
+                const Vec3d v = (j == 0 ? tangent_basis_col0 : tangent_basis_col1).cross(r_i);
+                for (int k = 0; k < 3; k++) {
+                    dt(3*i+k,j) = v[k];
+                }
+            }
+        }
+
+        for (int k = 0; k < sample_size; ++k) {
+            const auto point_idx = 4*sample[k];
+            const Vec3d pt1 (pts[point_idx], pts[point_idx+1], 1), pt2 (pts[point_idx+2], pts[point_idx+3], 1);
+            const double C = pt2.dot(E * pt1);
+
+            // J_C is the Jacobian of the epipolar constraint w.r.t. the image points
+            const Vec4d J_C ((E.col(0).t() * pt2)[0], (E.col(1).t() * pt2)[0], (E.row(0) * pt1)[0], (E.row(1) * pt1)[0]);
+            const double nJ_C = norm(J_C);
+            const double inv_nJ_C = 1.0 / nJ_C;
+            const double r = C * inv_nJ_C;
+
+            if (r*r > loss_fn.squared_thr) continue;
+
+            // Compute weight from robust loss function (used in the IRLS)
+            double weight = loss_fn.weight(r * r) / sample_size;
+            if (weights != nullptr)
+                weight = weights[k] * weight;
+
+            if(weight < DBL_EPSILON)
+                continue;
+
+            // Compute Jacobian of Sampson error w.r.t the fundamental/essential matrix (3x3)
+            Matx<double, 1, 9> dF (pt1(0) * pt2(0), pt1(0) * pt2(1), pt1(0), pt1(1) * pt2(0), pt1(1) * pt2(1), pt1(1), pt2(0), pt2(1), 1.0);
+            const double s = C * inv_nJ_C * inv_nJ_C;
+            dF(0) -= s * (J_C(2) * pt1(0) + J_C(0) * pt2(0));
+            dF(1) -= s * (J_C(3) * pt1(0) + J_C(0) * pt2(1));
+            dF(2) -= s * (J_C(0));
+            dF(3) -= s * (J_C(2) * pt1(1) + J_C(1) * pt2(0));
+            dF(4) -= s * (J_C(3) * pt1(1) + J_C(1) * pt2(1));
+            dF(5) -= s * (J_C(1));
+            dF(6) -= s * (J_C(2));
+            dF(7) -= s * (J_C(3));
+            dF *= inv_nJ_C;
+
+            // and then w.r.t. the pose parameters (rotation + tangent basis for translation)
+            const Matx13d dFdR = dF * dR;
+            const Matx12d dFdt = dF * dt;
+            const Matx<double, 1, 5> J (dFdR(0), dFdR(1), dFdR(2), dFdt(0), dFdt(1));
+
+            // Accumulate into JtJ and Jtr
+            Jtr += weight * C * inv_nJ_C * J.t();
+            JtJ(0, 0) += weight * (J(0) * J(0));
+            JtJ(1, 0) += weight * (J(1) * J(0));
+            JtJ(1, 1) += weight * (J(1) * J(1));
+            JtJ(2, 0) += weight * (J(2) * J(0));
+            JtJ(2, 1) += weight * (J(2) * J(1));
+            JtJ(2, 2) += weight * (J(2) * J(2));
+            JtJ(3, 0) += weight * (J(3) * J(0));
+            JtJ(3, 1) += weight * (J(3) * J(1));
+            JtJ(3, 2) += weight * (J(3) * J(2));
+            JtJ(3, 3) += weight * (J(3) * J(3));
+            JtJ(4, 0) += weight * (J(4) * J(0));
+            JtJ(4, 1) += weight * (J(4) * J(1));
+            JtJ(4, 2) += weight * (J(4) * J(2));
+            JtJ(4, 3) += weight * (J(4) * J(3));
+            JtJ(4, 4) += weight * (J(4) * J(4));
+        }
+    }
+};
+
+bool satisfyCheirality (const Matx33d& R, const Vec3d &t, const Vec3d &x1, const Vec3d &x2) {
+    // This code assumes that x1 and x2 are unit vectors
+     const auto Rx1 = R * x1;
+    // lambda_2 * x2 = R * ( lambda_1 * x1 ) + t
+    // [1 a; a 1] * [lambda1; lambda2] = [b1; b2]
+    // [lambda1; lambda2] = [1 -a; -a 1] * [b1; b2] / (1 - a*a)
+    const double a = -Rx1.dot(x2), b1 = -Rx1.dot(t), b2 = x2.dot(t);
+    // Note that we drop the factor 1.0/(1-a*a) since it is always positive.
+    return (b1 - a * b2 > 0) && (-a * b1 + b2 > 0);
+}
+
+int refine_relpose(const Mat &correspondences_,
+                    const std::vector<int> &sample_,
+                    const int sample_size_,
+                    CameraPose *pose,
+                    const BundleOptions &opt,
+                    const double* weights) {
+    MlesacLoss loss_fn(opt.loss_scale);
+    RelativePoseJacobianAccumulator accum(correspondences_, sample_, sample_size_, loss_fn, weights);
+    // return lm_5dof_impl(accum, pose, opt);
+
+    Matx<double, 5, 5> JtJ;
+    Matx<double, 5, 1> Jtr;
+    Matx<double, 3, 2> tangent_basis;
+    Matx33d sw = Matx33d::zeros();
+    double lambda = opt.initial_lambda;
+
+    // Compute initial cost
+    double cost = accum.residual(*pose);
+    bool recompute_jac = true;
+    int iter;
+    for (iter = 0; iter < opt.max_iterations; ++iter) {
+        // We only recompute jacobian and residual vector if last step was successful
+        if (recompute_jac) {
+            std::fill(JtJ.val, JtJ.val+25, 0);
+            std::fill(Jtr.val, Jtr.val +5, 0);
+            accum.accumulate(*pose, JtJ, Jtr, tangent_basis);
+            if (norm(Jtr) < opt.gradient_tol)
+                break;
+        }
+
+        // Add dampening
+        JtJ(0, 0) += lambda;
+        JtJ(1, 1) += lambda;
+        JtJ(2, 2) += lambda;
+        JtJ(3, 3) += lambda;
+        JtJ(4, 4) += lambda;
+
+        Matx<double, 5, 1> sol;
+        Matx<double, 5, 5> JtJ_symm = JtJ;
+        for (int i = 0; i < 5; i++)
+            for (int j = i+1; j < 5; j++)
+                JtJ_symm(i,j) = JtJ(j,i);
+
+        const bool success = solve(-JtJ_symm, Jtr, sol);
+        if (!success || norm(sol) < opt.step_tol)
+            break;
+
+        Vec3d w (sol(0,0), sol(1,0), sol(2,0));
+        const double theta = norm(w);
+        w /= theta;
+        const double a = std::sin(theta);
+        const double b = std::cos(theta);
+        sw(0, 1) = -w(2);
+        sw(0, 2) = w(1);
+        sw(1, 2) = -w(0);
+        sw(1, 0) = w(2);
+        sw(2, 0) = -w(1);
+        sw(2, 1) = w(0);
+
+        CameraPose pose_new;
+        pose_new.R = pose->R + pose->R * (a * sw + (1 - b) * sw * sw);
+        // In contrast to the 6dof case, we don't apply R here
+        // (since this can already be added into tangent_basis)
+        pose_new.t = pose->t + Vec3d(Mat(tangent_basis * Matx21d(sol(3,0), sol(4,0))));
+        double cost_new = accum.residual(pose_new);
+
+        if (cost_new < cost) {
+            *pose = pose_new;
+            lambda /= 10;
+            cost = cost_new;
+            recompute_jac = true;
+        } else {
+            JtJ(0, 0) -= lambda;
+            JtJ(1, 1) -= lambda;
+            JtJ(2, 2) -= lambda;
+            JtJ(3, 3) -= lambda;
+            JtJ(4, 4) -= lambda;
+            lambda *= 10;
+            recompute_jac = false;
+        }
+    }
+    return iter;
+}
+}}
\ No newline at end of file

modules/calib3d/src/usac/dls_solver.cpp

@@ -46,29 +46,25 @@
 #endif
 
 namespace cv { namespace usac {
-// This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented
 class DLSPnPImpl : public DLSPnP {
+#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
 private:
     const Mat * points_mat, * calib_norm_points_mat;
-    const Matx33d * K_mat;
-#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
-    const Matx33d &K;
+    const Matx33d K;
     const float * const calib_norm_points, * const points;
-#endif
 public:
-    explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Matx33d &K_) :
-        points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K_mat (&K_)
-#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
-        , K(K_), calib_norm_points((float*)calib_norm_points_.data), points((float*)points_.data)
+    explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Mat &K_)
+        : points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K(K_),
+        calib_norm_points((float*)calib_norm_points_mat->data), points((float*)points_mat->data) {}
+#else
+public:
+    explicit DLSPnPImpl (const Mat &, const Mat &, const Mat &) {}
 #endif
-        {}
+
     // return minimal sample size required for non-minimal estimation.
     int getMinimumRequiredSampleSize() const override { return 3; }
     // return maximum number of possible solutions.
     int getMaxNumberOfSolutions () const override { return 27; }
-    Ptr<NonMinimalSolver> clone () const override {
-        return makePtr<DLSPnPImpl>(*points_mat, *calib_norm_points_mat, *K_mat);
-    }
 #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
     int estimate(const std::vector<int> &sample, int sample_number,
         std::vector<Mat> &models_, const std::vector<double> &/*weights_*/) const override {
@@ -170,7 +166,6 @@ public:
         for (int i = 0; i < sample_number; i++)
             pts_random_shuffle[i] = i;
         randShuffle(pts_random_shuffle);
-
         for (int i = 0; i < 27; i++) {
             // If the rotation solutions are real, treat this as a valid candidate
             // rotation.
@@ -227,6 +222,12 @@ public:
 #endif
     }
 
+    int estimate (const std::vector<bool> &/*mask*/, std::vector<Mat> &/*models*/,
+            const std::vector<double> &/*weights*/) override {
+        return 0;
+    }
+    void enforceRankConstraint (bool /*enforce*/) override {}
+
 protected:
 #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
     const int indices[1968] = {
@@ -871,7 +872,7 @@ protected:
                  2 * D[74] - 2 * D[78]);                              // s1^3
     }
 };
-Ptr<DLSPnP> DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K) {
+Ptr<DLSPnP> DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) {
     return makePtr<DLSPnPImpl>(points_, calib_norm_pts, K);
 }
 }}

modules/calib3d/src/usac/estimator.cpp

@@ -36,10 +36,7 @@ public:
     int getNonMinimalSampleSize () const override {
         return non_min_solver->getMinimumRequiredSampleSize();
     }
-    Ptr<Estimator> clone() const override {
-        return makePtr<HomographyEstimatorImpl>(min_solver->clone(), non_min_solver->clone(),
-                degeneracy->clone(0 /*we don't need state here*/));
-    }
+    void enforceRankConstraint (bool /*enforce*/) override {}
 };
 Ptr<HomographyEstimator> HomographyEstimator::create (const Ptr<MinimalSolver> &min_solver_,
         const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> &degeneracy_) {
@@ -80,13 +77,12 @@ public:
     int getNonMinimalSampleSize () const override {
         return non_min_solver->getMinimumRequiredSampleSize();
     }
+    void enforceRankConstraint (bool enforce) override {
+        non_min_solver->enforceRankConstraint(enforce);
+    }
     int getMaxNumSolutionsNonMinimal () const override {
         return non_min_solver->getMaxNumberOfSolutions();
     }
-    Ptr<Estimator> clone() const override {
-        return makePtr<FundamentalEstimatorImpl>(min_solver->clone(), non_min_solver->clone(),
-                degeneracy->clone(0));
-    }
 };
 Ptr<FundamentalEstimator> FundamentalEstimator::create (const Ptr<MinimalSolver> &min_solver_,
         const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> &degeneracy_) {
@@ -106,7 +102,7 @@ public:
 
     inline int
     estimateModels(const std::vector<int> &sample, std::vector<Mat> &models) const override {
-            std::vector<Mat> E;
+        std::vector<Mat> E;
         const int models_count = min_solver->estimate(sample, E);
         int valid_models_count = 0;
         for (int i = 0; i < models_count; i++)
@@ -115,6 +111,10 @@ public:
         return valid_models_count;
     }
 
+    int estimateModelNonMinimalSample (const Mat &model, const std::vector<int> &sample, int sample_size, std::vector<Mat>
+                &models, const std::vector<double> &weights) const override {
+        return non_min_solver->estimate(model, sample, sample_size, models, weights);
+    }
     int estimateModelNonMinimalSample(const std::vector<int> &sample, int sample_size,
             std::vector<Mat> &models, const std::vector<double> &weights) const override {
         return non_min_solver->estimate(sample, sample_size, models, weights);
@@ -128,13 +128,12 @@ public:
     int getNonMinimalSampleSize () const override {
         return non_min_solver->getMinimumRequiredSampleSize();
     }
+    void enforceRankConstraint (bool enforce) override {
+        non_min_solver->enforceRankConstraint(enforce);
+    }
     int getMaxNumSolutionsNonMinimal () const override {
         return non_min_solver->getMaxNumberOfSolutions();
     }
-    Ptr<Estimator> clone() const override {
-        return makePtr<EssentialEstimatorImpl>(min_solver->clone(), non_min_solver->clone(),
-                degeneracy->clone(0));
-    }
 };
 Ptr<EssentialEstimator> EssentialEstimator::create (const Ptr<MinimalSolver> &min_solver_,
         const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> &degeneracy_) {
@@ -170,9 +169,7 @@ public:
     int getMaxNumSolutionsNonMinimal () const override {
         return non_min_solver->getMaxNumberOfSolutions();
     }
-    Ptr<Estimator> clone() const override {
-        return makePtr<AffineEstimatorImpl>(min_solver->clone(), non_min_solver->clone());
-    }
+    void enforceRankConstraint (bool /*enforce*/) override {}
 };
 Ptr<AffineEstimator> AffineEstimator::create (const Ptr<MinimalSolver> &min_solver_,
         const Ptr<NonMinimalSolver> &non_min_solver_) {
@@ -208,9 +205,7 @@ public:
     int getMaxNumSolutionsNonMinimal () const override {
         return non_min_solver->getMaxNumberOfSolutions();
     }
-    Ptr<Estimator> clone() const override {
-        return makePtr<PnPEstimatorImpl>(min_solver->clone(), non_min_solver->clone());
-    }
+    void enforceRankConstraint (bool /*enforce*/) override {}
 };
 Ptr<PnPEstimator> PnPEstimator::create (const Ptr<MinimalSolver> &min_solver_,
         const Ptr<NonMinimalSolver> &non_min_solver_) {
@@ -253,9 +248,9 @@ public:
         minv21=(float)minv[3]; minv22=(float)minv[4]; minv23=(float)minv[5];
         minv31=(float)minv[6]; minv32=(float)minv[7]; minv33=(float)minv[8];
     }
-    inline float getError (int point_idx) const override {
-        const int smpl = 4*point_idx;
-        const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
+    inline float getError (int idx) const override {
+        idx *= 4;
+        const float x1=points[idx], y1=points[idx+1], x2=points[idx+2], y2=points[idx+3];
         const float est_z2 = 1 / (m31 * x1 + m32 * y1 + m33),
                     dx2 =  x2 -  (m11 * x1 + m12 * y1 + m13) * est_z2,
                     dy2 =  y2 -  (m21 * x1 + m22 * y1 + m23) * est_z2;
@@ -279,9 +274,6 @@ public:
         }
         return errors;
     }
-    Ptr<Error> clone () const override {
-        return makePtr<ReprojectionErrorSymmetricImpl>(*points_mat);
-    }
 };
 Ptr<ReprojectionErrorSymmetric>
 ReprojectionErrorSymmetric::create(const Mat &points) {
@@ -314,9 +306,9 @@ public:
         m21=static_cast<float>(m[3]); m22=static_cast<float>(m[4]); m23=static_cast<float>(m[5]);
         m31=static_cast<float>(m[6]); m32=static_cast<float>(m[7]); m33=static_cast<float>(m[8]);
     }
-    inline float getError (int point_idx) const override {
-        const int smpl = 4*point_idx;
-        const float x1 = points[smpl], y1 = points[smpl+1], x2 = points[smpl+2], y2 = points[smpl+3];
+    inline float getError (int idx) const override {
+        idx *= 4;
+        const float x1 = points[idx], y1 = points[idx+1], x2 = points[idx+2], y2 = points[idx+3];
         const float est_z2 = 1 / (m31 * x1 + m32 * y1 + m33),
                     dx2 =  x2 -  (m11 * x1 + m12 * y1 + m13) * est_z2,
                     dy2 =  y2 -  (m21 * x1 + m22 * y1 + m23) * est_z2;
@@ -334,9 +326,6 @@ public:
         }
         return errors;
     }
-    Ptr<Error> clone () const override {
-        return makePtr<ReprojectionErrorForwardImpl>(*points_mat);
-    }
 };
 Ptr<ReprojectionErrorForward>
 ReprojectionErrorForward::create(const Mat &points) {
@@ -379,9 +368,9 @@ public:
      *               [ F(3,1)  F(3,2)  F(3,3) ]   [ 1  ]
      *
      */
-    inline float getError (int point_idx) const override {
-        const int smpl = 4*point_idx;
-        const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
+    inline float getError (int idx) const override {
+        idx *= 4;
+        const float x1=points[idx], y1=points[idx+1], x2=points[idx+2], y2=points[idx+3];
         const float F_pt1_x = m11 * x1 + m12 * y1 + m13,
                     F_pt1_y = m21 * x1 + m22 * y1 + m23;
         const float pt2_F_x = x2 * m11 + y2 * m21 + m31,
@@ -405,9 +394,6 @@ public:
         }
         return errors;
     }
-    Ptr<Error> clone () const override {
-        return makePtr<SampsonErrorImpl>(*points_mat);
-    }
 };
 Ptr<SampsonError>
 SampsonError::create(const Mat &points) {
@@ -440,9 +426,9 @@ public:
         m31=static_cast<float>(m[6]); m32=static_cast<float>(m[7]); m33=static_cast<float>(m[8]);
     }
 
-    inline float getError (int point_idx) const override {
-        const int smpl = 4*point_idx;
-        const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
+    inline float getError (int idx) const override {
+        idx *= 4;
+        const float x1=points[idx], y1=points[idx+1], x2=points[idx+2], y2=points[idx+3];
         // pt2^T * E, line 1 = [l1 l2]
         const float l1 = x2 * m11 + y2 * m21 + m31,
                     l2 = x2 * m12 + y2 * m22 + m32;
@@ -468,9 +454,6 @@ public:
         }
         return errors;
     }
-    Ptr<Error> clone () const override {
-        return makePtr<SymmetricGeometricDistanceImpl>(*points_mat);
-    }
 };
 Ptr<SymmetricGeometricDistance>
 SymmetricGeometricDistance::create(const Mat &points) {
@@ -504,10 +487,10 @@ public:
         p31 = (float)p[8]; p32 = (float)p[9]; p33 = (float)p[10]; p34 = (float)p[11];
     }
 
-    inline float getError (int point_idx) const override {
-        const int smpl = 5*point_idx;
-        const float u = points[smpl  ], v = points[smpl+1],
-                    x = points[smpl+2], y = points[smpl+3], z = points[smpl+4];
+    inline float getError (int idx) const override {
+        idx *= 5;
+        const float u = points[idx  ], v = points[idx+1],
+                    x = points[idx+2], y = points[idx+3], z = points[idx+4];
         const float depth = 1 / (p31 * x + p32 * y + p33 * z + p34);
         const float du =    u - (p11 * x + p12 * y + p13 * z + p14) * depth;
         const float dv =    v - (p21 * x + p22 * y + p23 * z + p24) * depth;
@@ -526,9 +509,6 @@ public:
         }
         return errors;
     }
-    Ptr<Error> clone () const override {
-        return makePtr<ReprojectionErrorPmatrixImpl>(*points_mat);
-    }
 };
 Ptr<ReprojectionErrorPmatrix> ReprojectionErrorPmatrix::create(const Mat &points) {
     return makePtr<ReprojectionErrorPmatrixImpl>(points);
@@ -565,9 +545,9 @@ public:
         m11 = (float)m[0]; m12 = (float)m[1]; m13 = (float)m[2];
         m21 = (float)m[3]; m22 = (float)m[4]; m23 = (float)m[5];
     }
-    inline float getError (int point_idx) const override {
-        const int smpl = 4*point_idx;
-        const float x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3];
+    inline float getError (int idx) const override {
+        idx *= 4;
+        const float x1=points[idx], y1=points[idx+1], x2=points[idx+2], y2=points[idx+3];
         const float dx2 = x2 - (m11 * x1 + m12 * y1 + m13), dy2 = y2 - (m21 * x1 + m22 * y1 + m23);
         return dx2 * dx2 + dy2 * dy2;
     }
@@ -581,9 +561,6 @@ public:
         }
         return errors;
     }
-    Ptr<Error> clone () const override {
-        return makePtr<ReprojectionDistanceAffineImpl>(*points_mat);
-    }
 };
 Ptr<ReprojectionErrorAffine>
 ReprojectionErrorAffine::create(const Mat &points) {
@@ -606,13 +583,11 @@ public:
         int smpl;
         for (int i = 0; i < sample_size; i++) {
             smpl = 4 * sample[i];
-
             mean_pts1_x += points[smpl    ];
             mean_pts1_y += points[smpl + 1];
             mean_pts2_x += points[smpl + 2];
             mean_pts2_y += points[smpl + 3];
         }
-
         mean_pts1_x /= sample_size; mean_pts1_y /= sample_size;
         mean_pts2_x /= sample_size; mean_pts2_y /= sample_size;
 
@@ -652,9 +627,9 @@ public:
         auto * norm_points_ptr = (float *) norm_points.data;
 
         // Normalize points: Npts = T*pts    3x3 * 3xN
-        const float avg_dist1f = (float)avg_dist1, avg_dist2f = (float)avg_dist2;
-        const float transl_x1f = (float)transl_x1, transl_y1f = (float)transl_y1;
-        const float transl_x2f = (float)transl_x2, transl_y2f = (float)transl_y2;
+        const auto avg_dist1f = (float)avg_dist1, avg_dist2f = (float)avg_dist2;
+        const auto transl_x1f = (float)transl_x1, transl_y1f = (float)transl_y1;
+        const auto transl_x2f = (float)transl_x2, transl_y2f = (float)transl_y2;
         for (int i = 0; i < sample_size; i++) {
             smpl = 4 * sample[i];
             (*norm_points_ptr++) = avg_dist1f * points[smpl    ] + transl_x1f;

modules/calib3d/src/usac/gamma_values.cpp

@@ -7,101 +7,146 @@
 
 namespace cv { namespace usac {
 
-GammaValues::GammaValues()
-    : max_range_complete(4.62)
-    , max_range_gamma(1.52)
-    , max_size_table(3000)
-{
-    /*
-     * Gamma values for degrees of freedom n = 2 and sigma quantile 99% of chi distribution
-     * (squared root of chi-squared distribution), in the range <0; 4.62> for complete values
-     * and <0, 1.52> for gamma values.
-     * Number of anchor points is 50. Other values are approximated using linear interpolation
-     */
-    const int number_of_anchor_points = 50;
-    std::vector<double> gamma_complete_anchor = std::vector<double>
-       {1.7724538509055159, 1.182606138403832, 0.962685372890749, 0.8090013493715409,
-        0.6909325812483967, 0.5961199186942078, 0.5179833984918483, 0.45248091153099873,
-        0.39690029823142897, 0.34930995878395804, 0.3082742109224103, 0.2726914551904204,
-        0.2416954924567404, 0.21459196516027726, 0.190815580770884, 0.16990026519723456,
-        0.15145770273372564, 0.13516150988807635, 0.12073530906427948, 0.10794357255251595,
-        0.0965844793065712, 0.08648426334883624, 0.07749268706639856, 0.06947937608738222,
-        0.062330823249820304, 0.05594791865006951, 0.05024389794830681, 0.045142626552664405,
-        0.040577155977706246, 0.03648850256745103, 0.03282460924226794, 0.029539458909083157,
-        0.02659231432268328, 0.023947063970062663, 0.021571657306774475, 0.01943761564987864,
-        0.017519607407598645, 0.015795078236273064, 0.014243928262247118, 0.012848229767187478,
-        0.011591979769030827, 0.010460882783057988, 0.009442159753944173, 0.008524379737926344,
-        0.007697311406424555, 0.006951791856026042, 0.006279610558635573, 0.005673406581042374,
-        0.005126577454218803, 0.004633198286725555};
+class GammaValuesImpl : public GammaValues {
+    std::vector<double> gamma_complete, gamma_incomplete, gamma;
+    double scale_complete_values, scale_gamma_values;
+    int max_size_table, DoF;
+public:
+    GammaValuesImpl (int DoF_, int max_size_table_) {
+        max_size_table = max_size_table_;
+        max_size_table = max_size_table_;
+        DoF = DoF_;
+        /*
+         * Gamma values for degrees of freedom n = 2 and sigma quantile 99% of chi distribution
+         * (squared root of chi-squared distribution), in the range <0; 4.62> for complete values
+         * and <0, 1.52> for gamma values.
+         * Number of anchor points is 50. Other values are approximated using linear interpolation
+         */
+        const int number_of_anchor_points = 50;
+        std::vector<double> gamma_complete_anchor, gamma_incomplete_anchor, gamma_anchor;
+        if (DoF == 2) {
+            const double max_thr = 7.5, gamma_quantile = 3.04;
+            scale_complete_values = max_size_table_ / max_thr;
+            scale_gamma_values = gamma_quantile * max_size_table_ / max_thr ;
 
-    std::vector<double> gamma_incomplete_anchor = std::vector<double>
-        {0.0, 0.01773096912803939, 0.047486924846289004, 0.08265437835139826, 0.120639343491371,
-         0.15993024714868515, 0.19954558593754865, 0.23881753504915218, 0.2772830648361923,
-         0.3146208784488923, 0.3506114446939783, 0.385110056889967, 0.41802785670077697,
-         0.44931803198258047, 0.47896553567848993, 0.5069792897777948, 0.5333861945970247,
-         0.5582264802664578, 0.581550074874317, 0.6034137543595729, 0.6238789008764282,
-         0.6430097394182639, 0.6608719532994989, 0.6775316015953519, 0.6930542783709592,
-         0.7075044661695132, 0.7209450459078338, 0.733436932830201, 0.7450388140484766,
-         0.7558069678435577, 0.7657951486073097, 0.7750545242776943, 0.7836336555215403,
-         0.7915785078697124, 0.798932489600361, 0.8057365094688473, 0.8120290494534339,
-         0.8178462485678104, 0.8232219945197348, 0.8281880205973585, 0.8327740056635289,
-         0.8370076755516281, 0.8409149044990385, 0.8445198155381767, 0.8478448790000731,
-         0.8509110084798414, 0.8537376537738418, 0.8563428904304485, 0.8587435056647642,
-         0.8609550804762539};
+            gamma_complete_anchor = std::vector<double>
+                    {1.77245385e+00, 1.02824699e+00, 7.69267629e-01, 5.99047749e-01,
+                     4.75998050e-01, 3.83008633e-01, 3.10886473e-01, 2.53983661e-01,
+                     2.08540472e-01, 1.71918718e-01, 1.42197872e-01, 1.17941854e-01,
+                     9.80549104e-02, 8.16877552e-02, 6.81739145e-02, 5.69851046e-02,
+                     4.76991202e-02, 3.99417329e-02, 3.35126632e-02, 2.81470710e-02,
+                     2.36624697e-02, 1.99092598e-02, 1.67644090e-02, 1.41264487e-02,
+                     1.19114860e-02, 1.00500046e-02, 8.48428689e-03, 7.16632498e-03,
+                     6.05612291e-03, 5.12031042e-03, 4.33100803e-03, 3.66489504e-03,
+                     3.10244213e-03, 2.62514027e-03, 2.22385863e-03, 1.88454040e-03,
+                     1.59749690e-03, 1.35457835e-03, 1.14892453e-03, 9.74756909e-04,
+                     8.27205063e-04, 7.02161552e-04, 5.96160506e-04, 5.06275903e-04,
+                     4.30036278e-04, 3.65353149e-04, 3.10460901e-04, 2.63866261e-04,
+                     2.24305797e-04, 1.90558599e-04};
 
-    std::vector<double> gamma_anchor = std::vector<double>
-        {1.7724538509055159, 1.427187162582056, 1.2890382454046982, 1.186244737282388,
-         1.1021938955410173, 1.0303674512016956, 0.9673796229113404, 0.9111932804012203,
-         0.8604640514722175, 0.814246149432561, 0.7718421763436497, 0.7327190195355812,
-         0.6964573670982434, 0.6627197089339725, 0.6312291454822467, 0.6017548373556638,
-         0.5741017071093776, 0.5481029597580317, 0.523614528104858, 0.5005108666212138,
-         0.478681711577816, 0.4580295473431646, 0.43846759792922513, 0.41991821541471996,
-         0.40231157253054745, 0.38558459136185, 0.3696800574963841, 0.3545458813847714,
-         0.340134477710645, 0.32640224021796493, 0.3133090943985706, 0.3008181141790485,
-         0.28889519159238314, 0.2775087506098113, 0.2666294980086962, 0.2562302054837794,
-         0.24628551826026082, 0.2367717863030556, 0.22766691488600885, 0.21895023182476064,
-         0.2106023691144937, 0.2026051570714723, 0.19494152937027823, 0.18759543761063277,
-         0.1805517742482484, 0.17379630289125447, 0.16731559510356395, 0.1610969729740903,
-         0.1551284568099053, 0.14939871739550692};
+            gamma_incomplete_anchor = std::vector<double>
+                    {0.        , 0.0364325 , 0.09423626, 0.15858163, 0.22401622, 0.28773243,
+                     0.34820493, 0.40463362, 0.45665762, 0.50419009, 0.54731575, 0.58622491,
+                     0.62116968, 0.65243473, 0.68031763, 0.70511575, 0.72711773, 0.74668782,
+                     0.76389332, 0.77907386, 0.79244816, 0.80421561, 0.81455692, 0.8236351 ,
+                     0.83159653, 0.83857228, 0.84467927, 0.85002158, 0.85469163, 0.85877132,
+                     0.86233307, 0.86544086, 0.86815108, 0.87052421, 0.87258093, 0.87437198,
+                     0.87593103, 0.87728759, 0.87846752, 0.87949345, 0.88038518, 0.88116002,
+                     0.88183307, 0.88241755, 0.88292497, 0.88336537, 0.88374751, 0.88407901,
+                     0.88436652, 0.88461696};
 
-    // allocate tables
-    gamma_complete = std::vector<double>(max_size_table);
-    gamma_incomplete = std::vector<double>(max_size_table);
-    gamma = std::vector<double>(max_size_table);
+            gamma_anchor = std::vector<double>
+                    {1.77245385e+00, 5.93375722e-01, 3.05833272e-01, 1.68019955e-01,
+                     9.52188705e-02, 5.49876141e-02, 3.21629603e-02, 1.89881161e-02,
+                     1.12897384e-02, 6.75016002e-03, 4.05426969e-03, 2.44422283e-03,
+                     1.47822780e-03, 8.96425642e-04, 5.44879754e-04, 3.31873268e-04,
+                     2.02499478e-04, 1.23458651e-04, 7.55593392e-05, 4.63032752e-05,
+                     2.84078946e-05, 1.74471428e-05, 1.07257506e-05, 6.59955061e-06,
+                     4.06400013e-06, 2.50448635e-06, 1.54449028e-06, 9.53085308e-07,
+                     5.88490160e-07, 3.63571768e-07, 2.24734099e-07, 1.38982938e-07,
+                     8.59913580e-08, 5.31026827e-08, 3.28834964e-08, 2.03707922e-08,
+                     1.26240063e-08, 7.82595771e-09, 4.85312084e-09, 3.01051575e-09,
+                     1.86805770e-09, 1.15947962e-09, 7.19869372e-10, 4.47050615e-10,
+                     2.77694421e-10, 1.72536278e-10, 1.07224039e-10, 6.66497131e-11,
+                     4.14376355e-11, 2.57079508e-11};
+        } else if (DoF == 4) {
+            const double max_thr = 2.5, gamma_quantile = 3.64;
+            scale_complete_values = max_size_table_ / max_thr;
+            scale_gamma_values = gamma_quantile * max_size_table_ / max_thr ;
+            gamma_complete_anchor = std::vector<double>
+                {0.88622693, 0.87877828, 0.86578847, 0.84979442, 0.83179176, 0.81238452,
+                0.79199067, 0.77091934, 0.74940836, 0.72764529, 0.70578051, 0.68393585,
+                0.66221071, 0.64068639, 0.61942952, 0.59849449, 0.57792561, 0.55766078,
+                0.53792634, 0.51864482, 0.49983336, 0.48150466, 0.46366759, 0.44632776,
+                0.42948797, 0.41314862, 0.39730804, 0.38196282, 0.36710806, 0.35273761,
+                0.33884422, 0.32541979, 0.31245545, 0.29988151, 0.28781065, 0.2761701 ,
+                0.26494924, 0.25413723, 0.24372308, 0.23369573, 0.22404405, 0.21475696,
+                0.2058234 , 0.19723241, 0.18897314, 0.18103488, 0.17340708, 0.16607937,
+                0.15904157, 0.15225125};
+            gamma_incomplete_anchor = std::vector<double>
+                {0.00000000e+00, 2.26619558e-04, 1.23631005e-03, 3.28596265e-03,
+                6.50682297e-03, 1.09662062e-02, 1.66907233e-02, 2.36788942e-02,
+                3.19091043e-02, 4.13450655e-02, 5.19397673e-02, 6.36384378e-02,
+                7.63808171e-02, 9.01029320e-02, 1.04738496e-01, 1.20220023e-01,
+                1.36479717e-01, 1.53535010e-01, 1.71152805e-01, 1.89349599e-01,
+                2.08062142e-01, 2.27229225e-01, 2.46791879e-01, 2.66693534e-01,
+                2.86880123e-01, 3.07300152e-01, 3.27904735e-01, 3.48647611e-01,
+                3.69485130e-01, 3.90376227e-01, 4.11282379e-01, 4.32167556e-01,
+                4.52998149e-01, 4.73844336e-01, 4.94473655e-01, 5.14961263e-01,
+                5.35282509e-01, 5.55414767e-01, 5.75337352e-01, 5.95031429e-01,
+                6.14479929e-01, 6.33667460e-01, 6.52580220e-01, 6.71205917e-01,
+                6.89533681e-01, 7.07553988e-01, 7.25258581e-01, 7.42640393e-01,
+                7.59693477e-01, 7.76494059e-01};
+            gamma_anchor = std::vector<double>
+                {8.86226925e-01, 8.38460922e-01, 7.64931722e-01, 6.85680218e-01,
+                6.07663201e-01, 5.34128389e-01, 4.66574835e-01, 4.05560768e-01,
+                3.51114357e-01, 3.02965249e-01, 2.60682396e-01, 2.23758335e-01,
+                1.91661077e-01, 1.63865725e-01, 1.39873108e-01, 1.19220033e-01,
+                1.01484113e-01, 8.62162923e-02, 7.32253576e-02, 6.21314285e-02,
+                5.26713657e-02, 4.46151697e-02, 3.77626859e-02, 3.19403783e-02,
+                2.69982683e-02, 2.28070945e-02, 1.92557199e-02, 1.62487939e-02,
+                1.37046640e-02, 1.15535264e-02, 9.73579631e-03, 8.20068208e-03,
+                6.90494092e-03, 5.80688564e-03, 4.88587254e-03, 4.10958296e-03,
+                3.45555079e-03, 2.90474053e-03, 2.44103551e-03, 2.05079975e-03,
+                1.72250366e-03, 1.44640449e-03, 1.21427410e-03, 1.01916714e-03,
+                8.55224023e-04, 7.17503448e-04, 6.01840372e-04, 5.04725511e-04,
+                4.23203257e-04, 3.54478559e-04};
+        } else CV_Error(cv::Error::StsNotImplemented, "Not implemented for specific DoF!");
+        // allocate tables
+        gamma_complete = std::vector<double>(max_size_table);
+        gamma_incomplete = std::vector<double>(max_size_table);
+        gamma = std::vector<double>(max_size_table);
 
-    const int step = (int)((double)max_size_table / (number_of_anchor_points-1));
-    int arr_cnt = 0;
-    for (int i = 0; i < number_of_anchor_points-1; i++) {
-         const double complete_x0 = gamma_complete_anchor[i], step_complete = (gamma_complete_anchor[i+1] - complete_x0) / step;
-         const double incomplete_x0 = gamma_incomplete_anchor[i], step_incomplete = (gamma_incomplete_anchor[i+1] - incomplete_x0) / step;
-         const double gamma_x0 = gamma_anchor[i], step_gamma = (gamma_anchor[i+1] - gamma_x0) / step;
+        // do linear interpolation of gamma values
+        const int step = (int)((double)max_size_table / (number_of_anchor_points-1));
+        int arr_cnt = 0;
+        for (int i = 0; i < number_of_anchor_points-1; i++) {
+             const double complete_x0 = gamma_complete_anchor[i], step_complete = (gamma_complete_anchor[i+1] - complete_x0) / step;
+             const double incomplete_x0 = gamma_incomplete_anchor[i], step_incomplete = (gamma_incomplete_anchor[i+1] - incomplete_x0) / step;
+             const double gamma_x0 = gamma_anchor[i], step_gamma = (gamma_anchor[i+1] - gamma_x0) / step;
 
-         for (int j = 0; j < step; j++) {
-             gamma_complete[arr_cnt] = complete_x0 + j * step_complete;
-             gamma_incomplete[arr_cnt] = incomplete_x0 + j * step_incomplete;
-             gamma[arr_cnt++] = gamma_x0 + j * step_gamma;
-         }
+             for (int j = 0; j < step; j++) {
+                 gamma_complete[arr_cnt] = complete_x0 + j * step_complete;
+                 gamma_incomplete[arr_cnt] = incomplete_x0 + j * step_incomplete;
+                 gamma[arr_cnt++] = gamma_x0 + j * step_gamma;
+             }
+        }
+        if (arr_cnt < max_size_table) {
+            // if array was not totally filled (in some cases can happen) then copy last values
+            std::fill(gamma_complete.begin()+arr_cnt, gamma_complete.end(), gamma_complete[arr_cnt-1]);
+            std::fill(gamma_incomplete.begin()+arr_cnt, gamma_incomplete.end(), gamma_incomplete[arr_cnt-1]);
+            std::fill(gamma.begin()+arr_cnt, gamma.end(), gamma[arr_cnt-1]);
+        }
     }
-    if (arr_cnt < max_size_table) {
-        // if array was not totally filled (in some cases can happen) then copy last values
-        std::fill(gamma_complete.begin()+arr_cnt, gamma_complete.end(), gamma_complete[arr_cnt-1]);
-        std::fill(gamma_incomplete.begin()+arr_cnt, gamma_incomplete.end(), gamma_incomplete[arr_cnt-1]);
-        std::fill(gamma.begin()+arr_cnt, gamma.end(), gamma[arr_cnt-1]);
-    }
-}
 
-const std::vector<double>& GammaValues::getCompleteGammaValues() const { return gamma_complete; }
-const std::vector<double>& GammaValues::getIncompleteGammaValues() const { return gamma_incomplete; }
-const std::vector<double>& GammaValues::getGammaValues() const { return gamma; }
-double GammaValues::getScaleOfGammaCompleteValues () const { return gamma_complete.size() / max_range_complete; }
-double GammaValues::getScaleOfGammaValues () const { return gamma.size() / max_range_gamma; }
-int GammaValues::getTableSize () const { return max_size_table; }
-
-/* static */
-const GammaValues& GammaValues::getSingleton()
-{
-    static GammaValues g_gammaValues;
-    return g_gammaValues;
+    const std::vector<double> &getCompleteGammaValues() const override { return gamma_complete; }
+    const std::vector<double> &getIncompleteGammaValues() const override { return gamma_incomplete; }
+    const std::vector<double> &getGammaValues() const override { return gamma; }
+    double getScaleOfGammaCompleteValues () const override { return scale_complete_values; }
+    double getScaleOfGammaValues () const override { return scale_gamma_values; }
+    int getTableSize () const override { return max_size_table; }
+};
+Ptr<GammaValues> GammaValues::create(int DoF, int max_size_table) {
+    return makePtr<GammaValuesImpl>(DoF, max_size_table);
 }
-
 }}  // namespace

modules/calib3d/test/test_usac.cpp

@@ -181,12 +181,9 @@ static double getError (TestSolver test_case, int pt_idx, const cv::Mat &pts1, c
     if (test_case == TestSolver::Fundam || test_case == TestSolver::Essen) {
         cv::Mat l2 = model     * pt1;
         cv::Mat l1 = model.t() * pt2;
-        if (test_case == TestSolver::Fundam) // sampson error
-            return fabs(pt2.dot(l2)) / sqrt(pow(l1.at<double>(0), 2) + pow(l1.at<double>(1), 2) +
-                                      pow(l2.at<double>(0), 2) + pow(l2.at<double>(1), 2));
-        else // symmetric geometric distance
-            return sqrt(pow(pt1.dot(l1),2) / (pow(l1.at<double>(0),2) + pow(l1.at<double>(1),2)) +
-                        pow(pt2.dot(l2),2) / (pow(l2.at<double>(0),2) + pow(l2.at<double>(1),2)));
+        // Sampson error
+        return fabs(pt2.dot(l2)) / sqrt(pow(l1.at<double>(0), 2) + pow(l1.at<double>(1), 2) +
+                                  pow(l2.at<double>(0), 2) + pow(l2.at<double>(1), 2));
     } else
     if (test_case == TestSolver::PnP) { // PnP, reprojection error
         cv::Mat img_pt = model * pt2; img_pt /= img_pt.at<double>(2);
@@ -340,7 +337,7 @@ TEST_P(usac_Essential, maxiters) {
     cv::Mat K1 = cv::Mat(cv::Matx33d(1, 0, 0,
                                      0, 1, 0,
                                      0, 0, 1.));
-    const double conf = 0.99, thr = 0.5;
+    const double conf = 0.99, thr = 0.25;
     int roll_results_sum = 0;
 
     for (int iters = 0; iters < 10; iters++) {
@@ -361,8 +358,8 @@ TEST_P(usac_Essential, maxiters) {
                 FAIL() << "Essential matrix estimation failed!\n";
             else continue;
         }
-        EXPECT_NE(roll_results_sum, 0);
     }
+    EXPECT_NE(roll_results_sum, 0);
 }
 
 INSTANTIATE_TEST_CASE_P(Calib3d, usac_Essential, UsacMethod::all());