// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.

#include "../precomp.hpp"
#include "../usac.hpp"

namespace cv { namespace usac {
class EpipolarGeometryDegeneracyImpl : public EpipolarGeometryDegeneracy {
private:
    Mat points_mat;
    const int min_sample_size;
public:
    explicit EpipolarGeometryDegeneracyImpl (const Mat &points_, int sample_size_) :
        points_mat(points_), min_sample_size (sample_size_)
    {
        CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
    }
    /*
     * Do oriented constraint to verify if epipolar geometry is in front or behind the camera.
     * Return: true if all points are in front of the camers w.r.t. tested epipolar geometry - satisfies constraint.
     *         false - otherwise.
     * x'^T F x = 0
     * e' × x' ~+ Fx   <=>  λe' × x' = Fx, λ > 0
     * e  × x ~+ x'^T F
     */
    inline bool isModelValid(const Mat &F_, const std::vector<int> &sample) const override {
        const Vec3d ep = Utils::getRightEpipole(F_);
        const auto * const e = ep.val; // of size 3x1
        const auto * const F = (double *) F_.data;
        const float * points = points_mat.ptr<float>();

        // without loss of generality, let the first point in sample be in front of the camera.
        int pt = 4*sample[0];
        // check only two first elements of vectors (e × x) and (x'^T F)
        // s1 = (x'^T F)[0] = x2 * F11 + y2 * F21 + 1 * F31
        // s2 = (e × x)[0] = e'_2 * 1 - e'_3 * y1
        // sign1 = s1 * s2
        const double sign1 = (F[0]*points[pt+2]+F[3]*points[pt+3]+F[6])*(e[1]-e[2]*points[pt+1]);

        for (int i = 1; i < min_sample_size; i++) {
            pt = 4 * sample[i];
            // if signum of the first point and tested point differs
            // then two points are on different sides of the camera.
            if (sign1*(F[0]*points[pt+2]+F[3]*points[pt+3]+F[6])*(e[1]-e[2]*points[pt+1])<0)
                    return false;
        }
        return true;
    }
};
void EpipolarGeometryDegeneracy::recoverRank (Mat &model, bool is_fundamental_mat) {
    /*
     * Do singular value decomposition.
     * Make last eigen value zero of diagonal matrix of singular values.
     */
    Matx33d U, Vt;
    Vec3d w;
    SVD::compute(model, w, U, Vt, SVD::MODIFY_A);
    if (is_fundamental_mat)
        model = Mat(U * Matx33d(w(0), 0, 0, 0, w(1), 0, 0, 0, 0) * Vt);
    else {
        const double mean_singular_val = (w[0] + w[1]) * 0.5;
        model = Mat(U * Matx33d(mean_singular_val, 0, 0, 0, mean_singular_val, 0, 0, 0, 0) * Vt);
    }
}
Ptr<EpipolarGeometryDegeneracy> EpipolarGeometryDegeneracy::create (const Mat &points_,
        int sample_size_) {
    return makePtr<EpipolarGeometryDegeneracyImpl>(points_, sample_size_);
}

class HomographyDegeneracyImpl : public HomographyDegeneracy {
private:
    Mat points_mat;
    const float TOLERANCE = 2 * FLT_EPSILON; // 2 from area of triangle
public:
    explicit HomographyDegeneracyImpl (const Mat &points_) :
            points_mat(points_)
    {
        CV_DbgAssert(!points_mat.empty() && points_mat.isContinuous());
    }

    inline bool isSampleGood (const std::vector<int> &sample) const override {
        const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2], smpl4 = 4*sample[3];
        // planar correspondences must lie on the same side of any line from two points in sample
        const float * points = points_mat.ptr<float>();
        const float x1 = points[smpl1], y1 = points[smpl1+1], X1 = points[smpl1+2], Y1 = points[smpl1+3];
        const float x2 = points[smpl2], y2 = points[smpl2+1], X2 = points[smpl2+2], Y2 = points[smpl2+3];
        const float x3 = points[smpl3], y3 = points[smpl3+1], X3 = points[smpl3+2], Y3 = points[smpl3+3];
        const float x4 = points[smpl4], y4 = points[smpl4+1], X4 = points[smpl4+2], Y4 = points[smpl4+3];
        // line from points 1 and 2
        const float ab_cross_x = y1 - y2, ab_cross_y = x2 - x1, ab_cross_z = x1 * y2 - y1 * x2;
        const float AB_cross_x = Y1 - Y2, AB_cross_y = X2 - X1, AB_cross_z = X1 * Y2 - Y1 * X2;

        // check if points 3 and 4 are on the same side of line ab on both images
        if ((ab_cross_x * x3 + ab_cross_y * y3 + ab_cross_z) *
            (AB_cross_x * X3 + AB_cross_y * Y3 + AB_cross_z) < 0)
            return false;
        if ((ab_cross_x * x4 + ab_cross_y * y4 + ab_cross_z) *
            (AB_cross_x * X4 + AB_cross_y * Y4 + AB_cross_z) < 0)
            return false;

        // line from points 3 and 4
        const float cd_cross_x = y3 - y4, cd_cross_y = x4 - x3, cd_cross_z = x3 * y4 - y3 * x4;
        const float CD_cross_x = Y3 - Y4, CD_cross_y = X4 - X3, CD_cross_z = X3 * Y4 - Y3 * X4;

        // check if points 1 and 2 are on the same side of line cd on both images
        if ((cd_cross_x * x1 + cd_cross_y * y1 + cd_cross_z) *
            (CD_cross_x * X1 + CD_cross_y * Y1 + CD_cross_z) < 0)
            return false;
        if ((cd_cross_x * x2 + cd_cross_y * y2 + cd_cross_z) *
            (CD_cross_x * X2 + CD_cross_y * Y2 + CD_cross_z) < 0)
            return false;

        // Checks if points are not collinear
        // If area of triangle constructed with 3 points is less than threshold then points are collinear:
        //           |x1 y1 1|             |x1      y1      1|
        // (1/2) det |x2 y2 1| = (1/2) det |x2-x1   y2-y1   0| = det |x2-x1   y2-y1| < 2 * threshold
        //           |x3 y3 1|             |x3-x1   y3-y1   0|       |x3-x1   y3-y1|
        // for points on the first image
        if (fabsf((x2-x1) * (y3-y1) - (y2-y1) * (x3-x1)) < TOLERANCE) return false; //1,2,3
        if (fabsf((x2-x1) * (y4-y1) - (y2-y1) * (x4-x1)) < TOLERANCE) return false; //1,2,4
        if (fabsf((x3-x1) * (y4-y1) - (y3-y1) * (x4-x1)) < TOLERANCE) return false; //1,3,4
        if (fabsf((x3-x2) * (y4-y2) - (y3-y2) * (x4-x2)) < TOLERANCE) return false; //2,3,4
        // for points on the second image
        if (fabsf((X2-X1) * (Y3-Y1) - (Y2-Y1) * (X3-X1)) < TOLERANCE) return false; //1,2,3
        if (fabsf((X2-X1) * (Y4-Y1) - (Y2-Y1) * (X4-X1)) < TOLERANCE) return false; //1,2,4
        if (fabsf((X3-X1) * (Y4-Y1) - (Y3-Y1) * (X4-X1)) < TOLERANCE) return false; //1,3,4
        if (fabsf((X3-X2) * (Y4-Y2) - (Y3-Y2) * (X4-X2)) < TOLERANCE) return false; //2,3,4

        return true;
    }
};
Ptr<HomographyDegeneracy> HomographyDegeneracy::create (const Mat &points_) {
    return makePtr<HomographyDegeneracyImpl>(points_);
}

class FundamentalDegeneracyViaEImpl : public FundamentalDegeneracyViaE {
private:
    bool is_F_objective;
    std::vector<std::vector<int>> instances = {{0,1,2,3,4}, {2,3,4,5,6}, {0,1,4,5,6}};
    std::vector<int> e_sample;
    const Ptr<Quality> quality;
    Ptr<EpipolarGeometryDegeneracy> e_degen, f_degen;
    Ptr<EssentialMinimalSolver5pts> e_solver;
    std::vector<Mat> e_models;
    const int E_SAMPLE_SIZE = 5;
    Matx33d K2_inv_t, K1_inv;
public:
    FundamentalDegeneracyViaEImpl (const Ptr<Quality> &quality_, const Mat &pts, const Mat &calib_pts, const Matx33d &K1, const Matx33d &K2, bool is_f_objective)
            : quality(quality_) {
        is_F_objective = is_f_objective;
        e_solver = EssentialMinimalSolver5pts::create(calib_pts, false/*svd*/, true/*nister*/);
        f_degen = is_F_objective ? EpipolarGeometryDegeneracy::create(pts, 7) : EpipolarGeometryDegeneracy::create(calib_pts, 7);
        e_degen = EpipolarGeometryDegeneracy::create(calib_pts, E_SAMPLE_SIZE);
        e_sample = std::vector<int>(E_SAMPLE_SIZE);
        if (is_f_objective) {
            K2_inv_t = K2.inv().t();
            K1_inv = K1.inv();
        }
    }
    bool isModelValid (const Mat &F, const std::vector<int> &sample) const override {
        return f_degen->isModelValid(F, sample);
    }
    bool recoverIfDegenerate (const std::vector<int> &sample_7pt, const Mat &/*best*/, const Score &best_score,
            Mat &out_model, Score &out_score) override {
        out_score = Score();
        for (const auto &instance : instances) {
            for (int i = 0; i < E_SAMPLE_SIZE; i++)
                e_sample[i] = sample_7pt[instance[i]];
            const int num_models = e_solver->estimate(e_sample, e_models);
            for (int i = 0; i < num_models; i++) {
                if (e_degen->isModelValid(e_models[i], e_sample)) {
                    const Mat model = is_F_objective ? Mat(K2_inv_t * Matx33d(e_models[i]) * K1_inv) : e_models[i];
                    const auto sc = quality->getScore(model);
                    if (sc.isBetter(out_score)) {
                        out_score = sc;
                        model.copyTo(out_model);
                    }
                }
            }
            if (out_score.isBetter(best_score)) break;
        }
        return true;
    }
};
Ptr<FundamentalDegeneracyViaE> FundamentalDegeneracyViaE::create (const Ptr<Quality> &quality, const Mat &pts,
                                              const Mat &calib_pts, const Matx33d &K1, const Matx33d &K2, bool is_f_objective) {
    return makePtr<FundamentalDegeneracyViaEImpl>(quality, pts, calib_pts, K1, K2, is_f_objective);
}
///////////////////////////////// Fundamental Matrix Degeneracy ///////////////////////////////////
class FundamentalDegeneracyImpl : public FundamentalDegeneracy {
private:
    RNG rng;
    const Ptr<Quality> quality;
    const Ptr<Error> f_error;
    Ptr<Quality> h_repr_quality;
    Mat points_mat;
    const Ptr<ReprojectionErrorForward> h_reproj_error;
    Ptr<EpipolarNonMinimalSolver> f_non_min_solver;
    Ptr<HomographyNonMinimalSolver> h_non_min_solver;
    Ptr<UniformRandomGenerator> random_gen_H;
    const EpipolarGeometryDegeneracyImpl ep_deg;
    // threshold to find inliers for homography model
    const double homography_threshold, log_conf = log(0.05), H_SAMPLE_THR_SQR = 49/*7^2*/, MAX_H_THR = 225/*15^2*/;
    double f_threshold_sqr, best_focal = -1;
    // points (1-7) to verify in sample
    std::vector<std::vector<int>> h_sample {{0,1,2},{3,4,5},{0,1,6},{3,4,6},{2,5,6}};
    std::vector<std::vector<int>> h_sample_ver {{3,4,5,6},{0,1,2,6},{2,3,4,5},{0,1,2,5},{0,1,3,4}};
    std::vector<int> non_planar_supports, h_inliers, h_outliers, h_outliers_eval, f_inliers;
    std::vector<double> weights;
    std::vector<Mat> h_models;
    const int points_size, max_iters_plane_and_parallax, MAX_H_SUBSET = 50, MAX_ITERS_H = 6;
    int num_h_outliers, num_models_used_so_far = 0, estimated_min_non_planar_support,
        num_h_outliers_eval, TENT_MIN_NON_PLANAR_SUPP;
    const int MAX_MODELS_TO_TEST = 21, H_INLS_DEGEN_SAMPLE = 5; // 5 by DEGENSAC, Chum et al.
    Matx33d K, K2, K_inv, K2_inv, K2_inv_t, true_K2_inv, true_K2_inv_t, true_K1_inv, true_K1, true_K2_t;
    Score best_focal_score;
    bool true_K_given, is_principal_pt_set = false;
public:
    FundamentalDegeneracyImpl (int state, const Ptr<Quality> &quality_, const Mat &points_,
                int sample_size_, int plane_and_parallax_iters, double homography_threshold_,
                double f_inlier_thr_sqr, const Mat true_K1_, const Mat true_K2_) :
            rng (state), quality(quality_), f_error(quality_->getErrorFnc()), points_mat(points_),
            h_reproj_error(ReprojectionErrorForward::create(points_)),
            ep_deg (points_, sample_size_), homography_threshold (homography_threshold_),
            points_size (quality_->getPointsSize()),
            max_iters_plane_and_parallax(plane_and_parallax_iters) {
        if (sample_size_ == 8) {
            // add more homography samples to test for 8-points F
            h_sample.emplace_back(std::vector<int>{0, 1, 7}); h_sample_ver.emplace_back(std::vector<int>{2,3,4,5,6});
            h_sample.emplace_back(std::vector<int>{0, 2, 7}); h_sample_ver.emplace_back(std::vector<int>{1,3,4,5,6});
            h_sample.emplace_back(std::vector<int>{3, 5, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,2,4,6});
            h_sample.emplace_back(std::vector<int>{3, 6, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,2,4,5});
            h_sample.emplace_back(std::vector<int>{2, 4, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,3,5,6});
        }
        non_planar_supports = std::vector<int>(MAX_MODELS_TO_TEST);
        h_inliers = std::vector<int>(points_size);
        h_outliers = std::vector<int>(points_size);
        h_outliers_eval = std::vector<int>(points_size);
        f_inliers = std::vector<int>(points_size);
        h_non_min_solver = HomographyNonMinimalSolver::create(points_);
        f_non_min_solver = EpipolarNonMinimalSolver::create(points_, true /*F*/);
        num_h_outliers_eval = num_h_outliers = points_size;
        f_threshold_sqr = f_inlier_thr_sqr;
        h_repr_quality = MsacQuality::create(points_.rows, homography_threshold_, h_reproj_error);
        true_K_given = ! true_K1_.empty() && ! true_K2_.empty();
        if (true_K_given) {
            true_K1 = Matx33d((double *)true_K1_.data);
            true_K2_inv = Matx33d(Mat(true_K2_.inv()));
            true_K2_t = Matx33d(true_K2_).t();
            true_K1_inv = true_K1.inv();
            true_K2_inv_t = true_K2_inv.t();
        }
        random_gen_H = UniformRandomGenerator::create(rng.uniform(0, INT_MAX), points_size, MAX_H_SUBSET);
        estimated_min_non_planar_support = TENT_MIN_NON_PLANAR_SUPP = std::min(5, (int)(0.05*points_size));
    }
    bool estimateHfrom3Points (const Mat &F_best, const std::vector<int> &sample, Mat &H_best) {
        Score H_best_score;
        // find e', null vector of F^T
        const Vec3d e_prime = Utils::getLeftEpipole(F_best);
        const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best);
        bool is_degenerate = false;
        int idx = -1;
        for (const auto &h_i : h_sample) { // only 5 samples
            idx++;
            Matx33d H;
            if (!getH(A, e_prime, 4 * sample[h_i[0]], 4 * sample[h_i[1]], 4 * sample[h_i[2]], H))
                continue;
            h_reproj_error->setModelParameters(Mat(H));
            const auto &ver_pts = h_sample_ver[idx];
            int inliers_in_plane = 3; // 3 points are always inliers
            for (int s : ver_pts)
                if (h_reproj_error->getError(sample[s]) < homography_threshold) {
                    if (++inliers_in_plane >= H_INLS_DEGEN_SAMPLE)
                        break;
                }
            if (inliers_in_plane >= H_INLS_DEGEN_SAMPLE) {
                is_degenerate = true;
                const auto h_score = h_repr_quality->getScore(Mat(H));
                if (h_score.isBetter(H_best_score)) {
                    H_best_score = h_score;
                    H_best = Mat(H);
                }
            }
        }
        if (!is_degenerate)
            return false;
        int h_inls_cnt = optimizeH(H_best, H_best_score);
        for (int iter = 0; iter < 2; iter++) {
            if (h_non_min_solver->estimate(h_inliers, h_inls_cnt, h_models, weights) == 0)
                break;
            const auto h_score = h_repr_quality->getScore(h_models[0]);
            if (h_score.isBetter(H_best_score)) {
                H_best_score = h_score;
                h_models[0].copyTo(H_best);
                h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
            } else break;
        }
        getOutliersH(H_best);
        return true;
    }
    bool optimizeF (const Mat &F, const Score &score, Mat &F_new, Score &new_score) {
        std::vector<Mat> Fs;
        if (f_non_min_solver->estimate(f_inliers, quality->getInliers(F, f_inliers), Fs, weights)) {
            const auto F_polished_score = quality->getScore(f_error->getErrors(Fs[0]));
            if (F_polished_score.isBetter(score)) {
                Fs[0].copyTo(F_new); new_score = F_polished_score;
                return true;
            }
        }
        return false;
    }
    int optimizeH (Mat &H_best, Score &H_best_score) {
        int h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
        random_gen_H->setSubsetSize(h_inls_cnt <= MAX_H_SUBSET ? (int)(0.8*h_inls_cnt) : MAX_H_SUBSET);
        if (random_gen_H->getSubsetSize() >= 4/*min H sample size*/) {
            for (int iter = 0; iter < MAX_ITERS_H; iter++) {
                if (h_non_min_solver->estimate(random_gen_H->generateUniqueRandomSubset(h_inliers, h_inls_cnt), random_gen_H->getSubsetSize(), h_models, weights) == 0)
                    continue;
                const auto h_score = h_repr_quality->getScore(h_models[0]);
                if (h_score.isBetter(H_best_score)) {
                    h_models[0].copyTo(H_best);
                    // if more inliers than previous best
                    if (h_score.inlier_number > H_best_score.inlier_number || h_score.inlier_number >= MAX_H_SUBSET) {
                        h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
                        random_gen_H->setSubsetSize(h_inls_cnt <= MAX_H_SUBSET ? (int)(0.8*h_inls_cnt) : MAX_H_SUBSET);
                    }
                    H_best_score = h_score;
                }
            }
        }
        return h_inls_cnt;
    }

    bool recoverIfDegenerate (const std::vector<int> &sample, const Mat &F_best, const Score &F_best_score,
                              Mat &non_degenerate_model, Score &non_degenerate_model_score) override {
        const auto swapF = [&] (const Mat &_F, const Score &_score) {
            const auto non_min_solver = EpipolarNonMinimalSolver::create(points_mat, true);
            if (! optimizeF(_F, _score, non_degenerate_model, non_degenerate_model_score)) {
                _F.copyTo(non_degenerate_model);
                non_degenerate_model_score = _score;
            }
        };
        Mat F_from_H, F_from_E, H_best; Score F_from_H_score, F_from_E_score;
        if (! estimateHfrom3Points(F_best, sample, H_best)) {
            return false; // non degenerate
        }
        if (true_K_given) {
            if (getFfromTrueK(H_best, F_from_H, F_from_H_score)) {
                if (F_from_H_score.isBetter(F_from_E_score))
                    swapF(F_from_H, F_from_H_score);
                else swapF(F_from_E, F_from_E_score);
                return true;
            } else {
                non_degenerate_model_score = Score();
                return true; // no translation
            }
        }
        const int non_planar_support_degen_F = getNonPlanarSupport(F_best);
        Score F_pl_par_score, F_calib_score; Mat F_pl_par, F_calib;
        if (calibDegensac(H_best, F_calib, F_calib_score, non_planar_support_degen_F, F_best_score)) {
            if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, max_iters_plane_and_parallax, true,
                    F_best_score, non_planar_support_degen_F, F_pl_par, F_pl_par_score) && F_pl_par_score.isBetter(F_calib_score)
                    && getNonPlanarSupport(F_pl_par) > getNonPlanarSupport(F_calib)) {
                swapF(F_pl_par, F_pl_par_score);
                return true;
            }
            swapF(F_calib, F_calib_score);
            return true;
        } else {
            if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, max_iters_plane_and_parallax, true,
                    F_best_score, non_planar_support_degen_F, F_pl_par, F_pl_par_score)) {
                swapF(F_pl_par, F_pl_par_score);
                return true;
            }
        }
        if (! isFDegenerate(non_planar_support_degen_F)) {
            return false;
        }
        non_degenerate_model_score = Score();
        return true;
    }

    // RANSAC with plane-and-parallax to find new Fundamental matrix
    bool getFfromTrueK (const Matx33d &H, Mat &F_from_K, Score &F_from_K_score) {
        std::vector<Matx33d> R; std::vector<Vec3d> t;
        if (decomposeHomographyMat(true_K2_inv * H * true_K1, Matx33d::eye(), R, t, noArray()) == 1) {
            // std::cout << "Warning: translation is zero!\n";
            return false; // is degenerate
        }
        // sign of translation does not make difference
        const Mat F1 = Mat(true_K2_inv_t * Math::getSkewSymmetric(t[0]) * R[0] * true_K1_inv);
        const Mat F2 = Mat(true_K2_inv_t * Math::getSkewSymmetric(t[1]) * R[1] * true_K1_inv);
        const auto score_f1 = quality->getScore(f_error->getErrors(F1)), score_f2 = quality->getScore(f_error->getErrors(F2));
        if (score_f1.isBetter(score_f2)) {
            F_from_K = F1; F_from_K_score = score_f1;
        } else {
            F_from_K = F2; F_from_K_score = score_f2;
        }
        return true;
    }
    bool planeAndParallaxRANSAC (const Matx33d &H, std::vector<int> &non_planar_pts, int num_non_planar_pts,
            int max_iters_pl_par, bool use_preemptive, const Score &score_degen_F, int non_planar_support_degen_F,
            Mat &F_new, Score &F_new_score) {
        if (num_non_planar_pts < 2)
            return false;
        num_models_used_so_far = 0; // reset estimation of lambda for plane-and-parallax
        int max_iters = max_iters_pl_par, non_planar_support = 0, iters = 0;
        const float * points = points_mat.ptr<float>();
        std::vector<Matx33d> F_good;
        const double CLOSE_THR = 1.0;
        for (; iters < max_iters; iters++) {
            // draw two random points
            int h_outlier1 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];
            int h_outlier2 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];
            while (h_outlier1 == h_outlier2)
                h_outlier2 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];

            const auto x1 = points[h_outlier1], y1 = points[h_outlier1+1], X1 = points[h_outlier1+2], Y1 = points[h_outlier1+3];
            const auto x2 = points[h_outlier2], y2 = points[h_outlier2+1], X2 = points[h_outlier2+2], Y2 = points[h_outlier2+3];
            if ((fabsf(X1 - X2) < CLOSE_THR && fabsf(Y1 - Y2) < CLOSE_THR) ||
                (fabsf(x1 - x2) < CLOSE_THR && fabsf(y1 - y2) < CLOSE_THR))
                continue;

            // do plane and parallax with outliers of H
            // F = [(p1' x Hp1) x (p2' x Hp2)]_x H = [e']_x H
            Vec3d ep = (Vec3d(X1, Y1, 1).cross(H * Vec3d(x1, y1, 1)))  // l1 = p1' x Hp1
                      .cross((Vec3d(X2, Y2, 1).cross(H * Vec3d(x2, y2, 1))));// l2 = p2' x Hp2
            const Matx33d F = Math::getSkewSymmetric(ep) * H;
            const auto * const f = F.val;
            // check orientation constraint of epipolar lines
            const bool valid = (f[0]*x1+f[1]*y1+f[2])*(ep[1]-ep[2]*Y1) * (f[0]*x2+f[1]*y2+f[2])*(ep[1]-ep[2]*Y2) > 0;
            if (!valid) continue;

            const int num_f_inliers_of_h_outliers = getNonPlanarSupport(Mat(F), num_models_used_so_far >= MAX_MODELS_TO_TEST, non_planar_support);
            if (non_planar_support < num_f_inliers_of_h_outliers) {
                non_planar_support = num_f_inliers_of_h_outliers;
                const double predicted_iters = log_conf / log(1 - pow(static_cast<double>
                    (getNonPlanarSupport(Mat(F), non_planar_pts, num_non_planar_pts)) / num_non_planar_pts, 2));
                if (use_preemptive && ! std::isinf(predicted_iters) && predicted_iters < max_iters)
                    max_iters = static_cast<int>(predicted_iters);
                F_good = { F };
            } else if (non_planar_support == num_f_inliers_of_h_outliers) {
                F_good.emplace_back(F);
            }
        }

        F_new_score = Score();
        for (const auto &F_ : F_good) {
            const auto sc = quality->getScore(f_error->getErrors(Mat(F_)));
            if (sc.isBetter(F_new_score)) {
                F_new_score = sc;
                F_new = Mat(F_);
            }
        }
        if (F_new_score.isBetter(score_degen_F) && non_planar_support > non_planar_support_degen_F)
            return true;
        if (isFDegenerate(non_planar_support))
            return false;
        return true;
    }
    bool calibDegensac (const Matx33d &H, Mat &F_new, Score &F_new_score, int non_planar_support_degen_F, const Score &F_degen_score) {
        const float * points = points_mat.ptr<float>();
        if (! is_principal_pt_set) {
            // estimate principal points from coordinates
            float px1 = 0, py1 = 0, px2 = 0, py2 = 0;
            for (int i = 0; i < points_size; i++) {
                const int idx = 4*i;
                if (px1 < points[idx  ]) px1 = points[idx  ];
                if (py1 < points[idx+1]) py1 = points[idx+1];
                if (px2 < points[idx+2]) px2 = points[idx+2];
                if (py2 < points[idx+3]) py2 = points[idx+3];
            }
            setPrincipalPoint((int)(px1/2)+1, (int)(py1/2)+1, (int)(px2/2)+1, (int)(py2/2)+1);
        }
        std::vector<Mat> R; std::vector<Mat> t; std::vector<Mat> F_good;
        std::vector<double> best_f;
        int non_planar_support_out = 0;
        for (double f = 300; f <= 3000; f += 150.) {
            K(0,0) = K(1,1) = K2(0,0) = K2(1,1) = f;
            const double one_over_f = 1/f;
            K_inv(0,0) = K_inv(1,1) = K2_inv(0,0) = K2_inv(1,1) = K2_inv_t(0,0) = K2_inv_t(1,1) = one_over_f;
            K_inv(0,2) = -K(0,2)*one_over_f; K_inv(1,2) = -K(1,2)*one_over_f;
            K2_inv_t(2,0) = K2_inv(0,2) = -K2(0,2)*one_over_f; K2_inv_t(2,1) = K2_inv(1,2) = -K2(1,2)*one_over_f;
            if (decomposeHomographyMat(K2_inv * H * K, Matx33d::eye(), R, t, noArray()) == 1) continue;
            Mat F1 = Mat(K2_inv_t * Math::getSkewSymmetric(Vec3d(t[0])) * Matx33d(R[0]) * K_inv);
            Mat F2 = Mat(K2_inv_t * Math::getSkewSymmetric(Vec3d(t[2])) * Matx33d(R[2]) * K_inv);
            int non_planar_f1 = getNonPlanarSupport(F1, true, non_planar_support_out),
                non_planar_f2 = getNonPlanarSupport(F2, true, non_planar_support_out);
            if (non_planar_f1 < non_planar_f2) {
                non_planar_f1 = non_planar_f2; F1 = F2;
            }
            if (non_planar_support_out < non_planar_f1) {
                non_planar_support_out = non_planar_f1;
                F_good = {F1};
                best_f = { f };
            } else if (non_planar_support_out == non_planar_f1) {
                F_good.emplace_back(F1);
                best_f.emplace_back(f);
            }
        }
        F_new_score = Score();
        for (int i = 0; i < (int) F_good.size(); i++) {
            const auto sc = quality->getScore(f_error->getErrors(F_good[i]));
            if (sc.isBetter(F_new_score)) {
                F_new_score = sc;
                F_good[i].copyTo(F_new);
                if (sc.isBetter(best_focal_score)) {
                    best_focal = best_f[i]; // save best focal length
                    best_focal_score = sc;
                }
            }
        }
        if (F_degen_score.isBetter(F_new_score) && non_planar_support_out <= non_planar_support_degen_F)
            return false;

        /*
        // logarithmic search -> faster but less accurate
        double f_min = 300, f_max = 3500;
        while (f_max - f_min > 100) {
            const double f_half = (f_max + f_min) * 0.5f, left_half = (f_min + f_half) * 0.5f, right_half = (f_half + f_max) * 0.5f;
            const double inl_in_left = eval_f(left_half), inl_in_right = eval_f(right_half);
            if (inl_in_left > inl_in_right)
                f_max = f_half;
            else f_min = f_half;
        }
        */
        return true;
    }
    void getOutliersH (const Mat &H_best) {
        // get H outliers
        num_h_outliers_eval = num_h_outliers = 0;
        const auto &h_errors = h_reproj_error->getErrors(H_best);
        for (int pt = 0; pt < points_size; pt++)
            if (h_errors[pt] > H_SAMPLE_THR_SQR) {
                h_outliers[num_h_outliers++] = pt;
                if (h_errors[pt] > MAX_H_THR)
                    h_outliers_eval[num_h_outliers_eval++] = pt;
            }
    }

    bool verifyFundamental (const Mat &F_best, const Score &F_score, const std::vector<bool> &inliers_mask, Mat &F_new, Score &new_score) override {
        const int f_sample_size = 3, max_H_iters = 5; // 3.52 = log(0.01) / log(1 - std::pow(0.9, 3));
        int num_f_inliers = 0;
        std::vector<int> inliers(points_size), f_sample(f_sample_size);
        for (int i = 0; i < points_size; i++) if (inliers_mask[i]) inliers[num_f_inliers++] = i;
        const auto sampler = UniformSampler::create(0, f_sample_size, num_f_inliers);
        // find e', null space of F^T
        const Vec3d e_prime = Utils::getLeftEpipole(F_best);
        const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best);
        Score H_best_score; Mat H_best;
        for (int iter = 0; iter < max_H_iters; iter++) {
            sampler->generateSample(f_sample);
            Matx33d H;
            if (!getH(A, e_prime, 4*inliers[f_sample[0]], 4*inliers[f_sample[1]], 4*inliers[f_sample[2]], H))
                continue;
            const auto h_score = h_repr_quality->getScore(Mat(H));
            if (h_score.isBetter(H_best_score)) {
                H_best_score = h_score; H_best = Mat(H);
            }
        }
        if (H_best.empty()) return false; // non-degenerate
        optimizeH(H_best, H_best_score);
        getOutliersH(H_best);
        const int non_planar_support_best_F = getNonPlanarSupport(F_best);
        const bool is_F_degen = isFDegenerate(non_planar_support_best_F);
        Mat F_from_K; Score F_from_K_score;
        bool success = false;
        // generate non-degenerate F even though so-far-the-best one may not be degenerate
        if (true_K_given) {
            // use GT calibration
            if (getFfromTrueK(H_best, F_from_K, F_from_K_score)) {
                new_score = F_from_K_score;
                F_from_K.copyTo(F_new);
                success = true;
            }
        } else {
            // use calibrated DEGENSAC
            if (calibDegensac(H_best, F_from_K, F_from_K_score, non_planar_support_best_F, F_score)) {
                new_score = F_from_K_score;
                F_from_K.copyTo(F_new);
                success = true;
            }
        }
        if (!is_F_degen) {
            return false;
        } else if (success) // F is degenerate
            return true; // but successfully recovered

        // recover degenerate F using plane-and-parallax
        Score plane_parallax_score; Mat F_plane_parallax;
        if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, 20, true,
                F_score, non_planar_support_best_F, F_plane_parallax, plane_parallax_score)) {
            new_score = plane_parallax_score;
            F_plane_parallax.copyTo(F_new);
            return true;
        }
        // plane-and-parallax failed. A previous non-degenerate so-far-the-best model will be used instead
        new_score = Score();
        return true;
    }
    void setPrincipalPoint (double px_, double py_) override {
        setPrincipalPoint(px_, py_, 0, 0);
    }
    void setPrincipalPoint (double px_, double py_, double px2_, double py2_) override {
        if (px_ > DBL_EPSILON && py_ > DBL_EPSILON) {
            is_principal_pt_set = true;
            K = {1, 0, px_, 0, 1, py_, 0, 0, 1};
            if (px2_ > DBL_EPSILON && py2_ > DBL_EPSILON) K2 = {1, 0, px2_, 0, 1, py2_, 0, 0, 1};
            else K2 = K;
            K_inv = K2_inv = K2_inv_t = Matx33d::eye();
        }
    }
private:
    bool getH (const Matx33d &A, const Vec3d &e_prime, int smpl1, int smpl2, int smpl3, Matx33d &H) {
        const float * points = points_mat.ptr<float>();
        Vec3d p1(points[smpl1  ], points[smpl1+1], 1), p2(points[smpl2  ], points[smpl2+1], 1), p3(points[smpl3  ], points[smpl3+1], 1);
        Vec3d P1(points[smpl1+2], points[smpl1+3], 1), P2(points[smpl2+2], points[smpl2+3], 1), P3(points[smpl3+2], points[smpl3+3], 1);
        const Matx33d M (p1[0], p1[1], 1, p2[0], p2[1], 1, p3[0], p3[1], 1);
        if (p1.cross(p2).dot(p3) * P1.cross(P2).dot(P3) < 0) return false;
        // (x′i × e')
        const Vec3d P1e = P1.cross(e_prime), P2e = P2.cross(e_prime), P3e = P3.cross(e_prime);
        // x′i × (A xi))^T (x′i × e′) / ‖x′i×e′‖^2,
        const Vec3d b (P1.cross(A * p1).dot(P1e) / (P1e[0]*P1e[0]+P1e[1]*P1e[1]+P1e[2]*P1e[2]),
                       P2.cross(A * p2).dot(P2e) / (P2e[0]*P2e[0]+P2e[1]*P2e[1]+P2e[2]*P2e[2]),
                       P3.cross(A * p3).dot(P3e) / (P3e[0]*P3e[0]+P3e[1]*P3e[1]+P3e[2]*P3e[2]));

        H = A - e_prime * (M.inv() * b).t();
        return true;
    }
    int getNonPlanarSupport (const Mat &F, const std::vector<int> &pts, int num_pts) {
        int non_rand_support = 0;
        f_error->setModelParameters(F);
        for (int pt = 0; pt < num_pts; pt++)
            if (f_error->getError(pts[pt]) < f_threshold_sqr)
                non_rand_support++;
        return non_rand_support;
    }

    int getNonPlanarSupport (const Mat &F, bool preemptive=false, int max_so_far=0) {
        int non_rand_support = 0;
        f_error->setModelParameters(F);
        if (preemptive) {
            const auto preemptive_thr = -num_h_outliers_eval + max_so_far;
            for (int pt = 0; pt < num_h_outliers_eval; pt++)
                if (f_error->getError(h_outliers_eval[pt]) < f_threshold_sqr)
                    non_rand_support++;
                else if (non_rand_support - pt < preemptive_thr)
                        break;
        } else {
            for (int pt = 0; pt < num_h_outliers_eval; pt++)
                if (f_error->getError(h_outliers_eval[pt]) < f_threshold_sqr)
                    non_rand_support++;
            if (num_models_used_so_far < MAX_MODELS_TO_TEST && !true_K_given/*for K we know that recovered F cannot be degenerate*/) {
                non_planar_supports[num_models_used_so_far++] = non_rand_support;
                if (num_models_used_so_far == MAX_MODELS_TO_TEST) {
                    getLambda(non_planar_supports, 2.32, num_h_outliers_eval, 0, false, estimated_min_non_planar_support);
                    if (estimated_min_non_planar_support < 3) estimated_min_non_planar_support = 3;
                }
            }
        }
        return non_rand_support;
    }
    inline bool isModelValid(const Mat &F, const std::vector<int> &sample) const override {
        return ep_deg.isModelValid(F, sample);
    }
    bool isFDegenerate (int num_f_inliers_h_outliers) const {
        if (num_models_used_so_far < MAX_MODELS_TO_TEST)
            // the minimum number of non-planar support has not estimated yet -> use tentative
            return num_f_inliers_h_outliers < std::min(TENT_MIN_NON_PLANAR_SUPP, (int)(0.1 * num_h_outliers_eval));
        return num_f_inliers_h_outliers < estimated_min_non_planar_support;
    }
};
Ptr<FundamentalDegeneracy> FundamentalDegeneracy::create (int state, const Ptr<Quality> &quality_,
        const Mat &points_, int sample_size_, int max_iters_plane_and_parallax, double homography_threshold_,
        double f_inlier_thr_sqr, const Mat true_K1, const Mat true_K2) {
    return makePtr<FundamentalDegeneracyImpl>(state, quality_, points_, sample_size_,
              max_iters_plane_and_parallax, homography_threshold_, f_inlier_thr_sqr, true_K1, true_K2);
}


class EssentialDegeneracyImpl : public EssentialDegeneracy {
private:
    const EpipolarGeometryDegeneracyImpl ep_deg;
public:
    explicit EssentialDegeneracyImpl (const Mat &points, int sample_size_) :
        ep_deg (points, sample_size_) {}
    inline bool isModelValid(const Mat &E, const std::vector<int> &sample) const override {
        return ep_deg.isModelValid(E, sample);
    }
};
Ptr<EssentialDegeneracy> EssentialDegeneracy::create (const Mat &points_, int sample_size_) {
    return makePtr<EssentialDegeneracyImpl>(points_, sample_size_);
}
}}
