diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp index a760d3636e..ec4c275597 100644 --- a/modules/calib3d/include/opencv2/calib3d.hpp +++ b/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. diff --git a/modules/calib3d/src/five-point.cpp b/modules/calib3d/src/five-point.cpp index 9b82dcb11c..b8d0b43edc 100644 --- a/modules/calib3d/src/five-point.cpp +++ b/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 ¶ms) { Ptr model; - usac::setParameters(model, usac::EstimationMethod::Essential, params, mask.needed()); + usac::setParameters(model, usac::EstimationMethod::ESSENTIAL, params, mask.needed()); Ptr 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(); diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp index f0a7df605b..8e07efa26d 100644 --- a/modules/calib3d/src/fundam.cpp +++ b/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 ¶ms) { Ptr model; - usac::setParameters(model, usac::EstimationMethod::Homography, params, mask.needed()); + usac::setParameters(model, usac::EstimationMethod::HOMOGRAPHY, params, mask.needed()); Ptr 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(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 ¶ms) { Ptr model; - setParameters(model, usac::EstimationMethod::Fundamental, params, mask.needed()); + setParameters(model, usac::EstimationMethod::FUNDAMENTAL, params, mask.needed()); CV_Assert(model); Ptr 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(); diff --git a/modules/calib3d/src/ptsetreg.cpp b/modules/calib3d/src/ptsetreg.cpp index 5c91fff037..c0132a6832 100644 --- a/modules/calib3d/src/ptsetreg.cpp +++ b/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 ¶ms) { Ptr model; - usac::setParameters(model, usac::EstimationMethod::Affine, params, inliers.needed()); + usac::setParameters(model, usac::EstimationMethod::AFFINE, params, inliers.needed()); Ptr 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); diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp index d9e2664877..54e23ca9f8 100644 --- a/modules/calib3d/src/solvepnp.cpp +++ b/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 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(); diff --git a/modules/calib3d/src/usac.hpp b/modules/calib3d/src/usac.hpp index 2a3e0eb7fc..85b2730e4a 100644 --- a/modules/calib3d/src/usac.hpp +++ b/modules/calib3d/src/usac.hpp @@ -6,10 +6,11 @@ #define OPENCV_USAC_USAC_HPP namespace cv { namespace usac { -enum EstimationMethod { Homography, Fundamental, Fundamental8, Essential, Affine, P3P, P6P}; -enum VerificationMethod { NullVerifier, SprtVerifier }; -enum PolishingMethod { NonePolisher, LSQPolisher }; -enum ErrorMetric {DIST_TO_LINE, SAMPSON_ERR, SGD_ERR, SYMM_REPR_ERR, FORW_REPR_ERR, RERPOJ}; +enum EstimationMethod { HOMOGRAPHY=0, FUNDAMENTAL=1, FUNDAMENTAL8=2, ESSENTIAL=3, AFFINE=4, P3P=5, P6P=6}; +enum VerificationMethod { NULL_VERIFIER=0, SPRT_VERIFIER=1, ASPRT=2 }; +enum ErrorMetric {DIST_TO_LINE=0, SAMPSON_ERR=1, SGD_ERR=2, SYMM_REPR_ERR=3, FORW_REPR_ERR=4, RERPOJ=5}; +enum MethodSolver { GEM_SOLVER=0, SVD_SOLVER=1 }; +enum ModelConfidence {RANDOM=0, NON_RANDOM=1, UNKNOWN=2}; // Abstract Error class class Error : public Algorithm { @@ -19,7 +20,6 @@ public: // returns error of point wih @point_idx w.r.t. model virtual float getError (int point_idx) const = 0; virtual const std::vector &getErrors (const Mat &model) = 0; - virtual Ptr clone () const = 0; }; // Symmetric Reprojection Error for Homography @@ -58,6 +58,11 @@ public: static Ptr create(const Mat &points); }; +class TrifocalTensorReprError : public Error { +public: + static Ptr create(const Mat &points); +}; + // Normalizing transformation of data points class NormTransform : public Algorithm { public: @@ -82,19 +87,22 @@ public: virtual int getSampleSize() const = 0; // return maximum number of possible solutions. virtual int getMaxNumberOfSolutions () const = 0; - virtual Ptr clone () const = 0; }; //-------------------------- HOMOGRAPHY MATRIX ----------------------- -class HomographyMinimalSolver4ptsGEM : public MinimalSolver { +class HomographyMinimalSolver4pts : public MinimalSolver { public: - static Ptr create(const Mat &points_); + static Ptr create(const Mat &points, bool use_ge); +}; +class PnPSVDSolver : public MinimalSolver { +public: + static Ptr create (const Mat &points); }; //-------------------------- FUNDAMENTAL MATRIX ----------------------- class FundamentalMinimalSolver7pts : public MinimalSolver { public: - static Ptr create(const Mat &points_); + static Ptr create(const Mat &points, bool use_ge); }; class FundamentalMinimalSolver8pts : public MinimalSolver { @@ -103,9 +111,9 @@ public: }; //-------------------------- ESSENTIAL MATRIX ----------------------- -class EssentialMinimalSolverStewenius5pts : public MinimalSolver { +class EssentialMinimalSolver5pts : public MinimalSolver { public: - static Ptr create(const Mat &points_); + static Ptr create(const Mat &points, bool use_svd, bool is_nister); }; //-------------------------- PNP ----------------------- @@ -116,7 +124,7 @@ public: class P3PSolver : public MinimalSolver { public: - static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K); + static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K); }; //-------------------------- AFFINE ----------------------- @@ -125,9 +133,20 @@ public: static Ptr create(const Mat &points_); }; +class TrifocalTensorMinimalSolver : public MinimalSolver { +public: + static Ptr create(const Mat &points_); + virtual void getFundamentalMatricesFromTensor (const cv::Mat &tensor, cv::Mat &F21, cv::Mat &F31) = 0; +}; + //////////////////////////////////////// NON MINIMAL SOLVER /////////////////////////////////////// class NonMinimalSolver : public Algorithm { public: + virtual int estimate (const Mat &model, const std::vector &sample, int sample_size, std::vector + &models, const std::vector &weights) const { + CV_UNUSED(model); + return estimate(sample, sample_size, models, weights); + } // Estimate models from non minimal sample. models.size() == number of found solutions virtual int estimate (const std::vector &sample, int sample_size, std::vector &models, const std::vector &weights) const = 0; @@ -135,25 +154,34 @@ public: virtual int getMinimumRequiredSampleSize() const = 0; // return maximum number of possible solutions. virtual int getMaxNumberOfSolutions () const = 0; - virtual Ptr clone () const = 0; + virtual int estimate (const std::vector& mask, std::vector& models, + const std::vector& weights) = 0; + virtual void enforceRankConstraint (bool enforce) = 0; }; //-------------------------- HOMOGRAPHY MATRIX ----------------------- class HomographyNonMinimalSolver : public NonMinimalSolver { public: - static Ptr create(const Mat &points_); + static Ptr create(const Mat &points_, bool use_ge_=false); + static Ptr create(const Mat &points_, const Matx33d &T1, const Matx33d &T2, bool use_ge); }; //-------------------------- FUNDAMENTAL MATRIX ----------------------- -class FundamentalNonMinimalSolver : public NonMinimalSolver { +class EpipolarNonMinimalSolver : public NonMinimalSolver { public: - static Ptr create(const Mat &points_); + static Ptr create(const Mat &points_, bool is_fundamental); + static Ptr create(const Mat &points_, const Matx33d &T1, const Matx33d &T2, bool use_ge); }; //-------------------------- ESSENTIAL MATRIX ----------------------- -class EssentialNonMinimalSolver : public NonMinimalSolver { +class EssentialNonMinimalSolverViaF : public NonMinimalSolver { public: - static Ptr create(const Mat &points_); +static Ptr create(const Mat &points_, const cv::Mat &K1, const Mat &K2); +}; + +class EssentialNonMinimalSolverViaT : public NonMinimalSolver { +public: + static Ptr create(const Mat &points_); }; //-------------------------- PNP ----------------------- @@ -164,16 +192,20 @@ public: class DLSPnP : public NonMinimalSolver { public: - static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K); + static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K); }; //-------------------------- AFFINE ----------------------- class AffineNonMinimalSolver : public NonMinimalSolver { public: - static Ptr create(const Mat &points_); + static Ptr create(const Mat &points, InputArray T1, InputArray T2); +}; + +class LarssonOptimizer : public NonMinimalSolver { +public: + static Ptr create(const Mat &calib_points_, const Matx33d &K1_, const Matx33d &K2_, int max_iters_, bool is_fundamental_); }; -////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////// SCORE /////////////////////////////////////////// class Score { public: @@ -193,24 +225,16 @@ public: } }; -class GammaValues -{ - const double max_range_complete /*= 4.62*/, max_range_gamma /*= 1.52*/; - const int max_size_table /* = 3000 */; - - std::vector gamma_complete, gamma_incomplete, gamma; - - GammaValues(); // use getSingleton() - +class GammaValues : public Algorithm { public: - static const GammaValues& getSingleton(); - - const std::vector& getCompleteGammaValues() const; - const std::vector& getIncompleteGammaValues() const; - const std::vector& getGammaValues() const; - double getScaleOfGammaCompleteValues () const; - double getScaleOfGammaValues () const; - int getTableSize () const; + virtual ~GammaValues() override = default; + static Ptr create(int DoF, int max_size_table=500); + virtual const std::vector &getCompleteGammaValues() const = 0; + virtual const std::vector &getIncompleteGammaValues() const = 0; + virtual const std::vector &getGammaValues() const = 0; + virtual double getScaleOfGammaCompleteValues () const = 0; + virtual double getScaleOfGammaValues () const = 0; + virtual int getTableSize () const = 0; }; ////////////////////////////////////////// QUALITY /////////////////////////////////////////// @@ -223,9 +247,7 @@ public: * @model: Mat current model, e.g., H matrix. */ virtual Score getScore (const Mat &model) const = 0; - virtual Score getScore (const std::vector &/*errors*/) const { - CV_Error(cv::Error::StsNotImplemented, "getScore(errors)"); - } + virtual Score getScore (const std::vector& errors) const = 0; // get @inliers of the @model. Assume threshold is given // @inliers must be preallocated to maximum points size. virtual int getInliers (const Mat &model, std::vector &inliers) const = 0; @@ -236,11 +258,30 @@ public: // set @inliers_mask: true if point i is inlier, false - otherwise. virtual int getInliers (const Mat &model, std::vector &inliers_mask) const = 0; virtual int getPointsSize() const = 0; - virtual Ptr clone () const = 0; + virtual double getThreshold () const = 0; + virtual Ptr getErrorFnc () const = 0; static int getInliers (const Ptr &error, const Mat &model, std::vector &inliers_mask, double threshold); static int getInliers (const Ptr &error, const Mat &model, std::vector &inliers, double threshold); + static int getInliers (const std::vector &errors, std::vector &inliers, + double threshold); + static int getInliers (const std::vector &errors, std::vector &inliers, + double threshold); + Score selectBest (const std::vector &models, int num_models, Mat &best) { + if (num_models == 0) return {}; + int best_idx = 0; + Score best_score = getScore(models[0]); + for (int i = 1; i < num_models; i++) { + const auto sc = getScore(models[i]); + if (sc.isBetter(best_score)) { + best_score = sc; + best_idx = i; + } + } + models[best_idx].copyTo(best); + return best_score; + } }; // RANSAC (binary) quality @@ -252,16 +293,16 @@ public: // M-estimator quality - truncated Squared error class MsacQuality : public Quality { public: - static Ptr create(int points_size_, double threshold_, const Ptr &error_); + static Ptr create(int points_size_, double threshold_, const Ptr &error_, double k_msac=2.25); }; // Marginlizing Sample Consensus quality, D. Barath et al. class MagsacQuality : public Quality { public: static Ptr create(double maximum_thr, int points_size_,const Ptr &error_, + const Ptr &gamma_generator, double tentative_inlier_threshold_, int DoF, double sigma_quantile, - double upper_incomplete_of_sigma_quantile, - double lower_incomplete_of_sigma_quantile, double C_); + double upper_incomplete_of_sigma_quantile); }; // Least Median of Squares Quality @@ -273,31 +314,40 @@ public: ////////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////// DEGENERACY ////////////////////////////////// class Degeneracy : public Algorithm { +private: + Mat homogr; public: virtual ~Degeneracy() override = default; /* * Check if sample causes degenerate configurations. * For example, test if points are collinear. */ - virtual bool isSampleGood (const std::vector &/*sample*/) const { + virtual bool isSampleGood (const std::vector& sample) const { + CV_UNUSED(sample); return true; } /* * Check if model satisfies constraints. * For example, test if epipolar geometry satisfies oriented constraint. */ - virtual bool isModelValid (const Mat &/*model*/, const std::vector &/*sample*/) const { + virtual bool isModelValid (const Mat& model, const std::vector& sample) const { + CV_UNUSED(model); + CV_UNUSED(sample); return true; } /* * Fix degenerate model. * Return true if model is degenerate, false - otherwise */ - virtual bool recoverIfDegenerate (const std::vector &/*sample*/,const Mat &/*best_model*/, - Mat &/*non_degenerate_model*/, Score &/*non_degenerate_model_score*/) { + virtual bool recoverIfDegenerate (const std::vector &sample,const Mat &best_model, const Score &score, + Mat &non_degenerate_model, Score &non_degenerate_model_score) { + CV_UNUSED(sample); + CV_UNUSED(best_model); + CV_UNUSED(score); + CV_UNUSED(non_degenerate_model); + CV_UNUSED(non_degenerate_model_score); return false; } - virtual Ptr clone(int /*state*/) const { return makePtr(); } }; class EpipolarGeometryDegeneracy : public Degeneracy { @@ -316,17 +366,31 @@ public: static Ptr create(const Mat &points_); }; +class FundamentalDegeneracyViaE : public EpipolarGeometryDegeneracy { +public: + static Ptr create (const Ptr &quality, const Mat &pts, + const Mat &calib_pts, const Matx33d &K1, const Matx33d &K2, bool is_f_objective); +}; + class FundamentalDegeneracy : public EpipolarGeometryDegeneracy { public: - // threshold for homography is squared so is around 2.236 pixels + virtual void setPrincipalPoint (double px_, double py_) = 0; + virtual void setPrincipalPoint (double px_, double py_, double px2_, double py2_) = 0; + virtual bool verifyFundamental (const Mat &F_best, const Score &F_score, const std::vector &inliers_mask, cv::Mat &F_new, Score &new_score) = 0; static Ptr create (int state, const Ptr &quality_, - const Mat &points_, int sample_size_, double homography_threshold); + const Mat &points_, int sample_size_, int max_iters_plane_and_parallax, + double homography_threshold, double f_inlier_thr_sqr, const Mat true_K1=Mat(), const Mat true_K2=Mat()); }; ///////////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////// ESTIMATOR ////////////////////////////////// class Estimator : public Algorithm{ public: + virtual int estimateModelNonMinimalSample (const Mat& model, const std::vector &sample, int sample_size, std::vector + &models, const std::vector &weights) const { + CV_UNUSED(model); + return estimateModelNonMinimalSample(sample, sample_size, models, weights); + } /* * Estimate models with minimal solver. * Return number of valid solutions after estimation. @@ -352,7 +416,7 @@ public: virtual int getMaxNumSolutions () const = 0; // return maximum number of possible solutions of non-minimal estimation. virtual int getMaxNumSolutionsNonMinimal () const = 0; - virtual Ptr clone() const = 0; + virtual void enforceRankConstraint (bool enforce) = 0; }; class HomographyEstimator : public Estimator { @@ -368,7 +432,7 @@ public: }; class EssentialEstimator : public Estimator { -public: +public : static Ptr create (const Ptr &min_solver_, const Ptr &non_min_solver_, const Ptr °eneracy_); }; @@ -391,15 +455,12 @@ class ModelVerifier : public Algorithm { public: virtual ~ModelVerifier() override = default; // Return true if model is good, false - otherwise. - virtual bool isModelGood(const Mat &model) = 0; - // Return true if score was computed during evaluation. - virtual bool getScore(Score &score) const = 0; + virtual bool isModelGood(const Mat &model, Score &score) = 0; // update verifier by given inlier number - virtual void update (int highest_inlier_number) = 0; - virtual const std::vector &getErrors() const = 0; - virtual bool hasErrors () const = 0; - virtual Ptr clone (int state) const = 0; - static Ptr create(); + virtual void update (const Score &score, int iteration) = 0; + virtual void reset() = 0; + virtual void updateSPRT (double time_model_est, double time_corr_ver, double new_avg_models, double new_delta, double new_epsilon, const Score &best_score) = 0; + static Ptr create(const Ptr &qualtiy); }; struct SPRT_history { @@ -421,7 +482,7 @@ struct SPRT_history { double epsilon, delta, A; // number of samples processed by test int tested_samples; // k - SPRT_history () : epsilon(0), delta(0), A(0) { + SPRT_history () { tested_samples = 0; } }; @@ -431,14 +492,14 @@ struct SPRT_history { * Matas, Jiri, and Ondrej Chum. "Randomized RANSAC with sequential probability ratio test." * Tenth IEEE International Conference on Computer Vision (ICCV'05) Volume 1. Vol. 2. IEEE, 2005. */ -class SPRT : public ModelVerifier { +class AdaptiveSPRT : public ModelVerifier { public: - // return constant reference of vector of SPRT histories for SPRT termination. virtual const std::vector &getSPRTvector () const = 0; - static Ptr create (int state, const Ptr &err_, int points_size_, - double inlier_threshold_, double prob_pt_of_good_model, - double prob_pt_of_bad_model, double time_sample, double avg_num_models, - ScoreMethod score_type_); + virtual int avgNumCheckedPts () const = 0; + static Ptr create (int state, const Ptr &quality, int points_size_, + double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model, + double time_sample, double avg_num_models, ScoreMethod score_type_, + double k_mlesac, bool is_adaptive = true); }; ////////////////////////////////////////////////////////////////////////////////////////// @@ -450,7 +511,6 @@ public: virtual void setNewPointsSize (int points_size) = 0; // generate sample. Fill @sample with indices of points. virtual void generateSample (std::vector &sample) = 0; - virtual Ptr clone (int state) const = 0; }; //////////////////////////////////////////////////////////////////////////////////////////////// @@ -460,6 +520,7 @@ public: virtual ~NeighborhoodGraph() override = default; // Return neighbors of the point with index @point_idx_ in the graph. virtual const std::vector &getNeighbors(int point_idx_) const = 0; + virtual const std::vector> &getGraph () const = 0; }; class RadiusSearchNeighborhoodGraph : public NeighborhoodGraph { @@ -481,6 +542,11 @@ public: int cell_size_x_img1_, int cell_size_y_img1_, int cell_size_x_img2_, int cell_size_y_img2_, int max_neighbors); }; +class GridNeighborhoodGraph2Images : public NeighborhoodGraph { +public: + static Ptr create(const Mat &points, int points_size, + float cell_size_x_img1_, float cell_size_y_img1_, float cell_size_x_img2_, float cell_size_y_img2_); +}; ////////////////////////////////////// UNIFORM SAMPLER //////////////////////////////////////////// class UniformSampler : public Sampler { @@ -488,6 +554,11 @@ public: static Ptr create(int state, int sample_size_, int points_size_); }; +class QuasiUniformSampler : public Sampler { +public: + static Ptr create(int state, int sample_size_, int points_size_); +}; + /////////////////////////////////// PROSAC (SIMPLE) SAMPLER /////////////////////////////////////// class ProsacSimpleSampler : public Sampler { public: @@ -523,60 +594,74 @@ public: ///////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////// TERMINATION /////////////////////////////////////////// -class TerminationCriteria : public Algorithm { +class Termination : public Algorithm { public: // update termination object by given @model and @inlier number. // and return maximum number of predicted iteration - virtual int update(const Mat &model, int inlier_number) = 0; - // clone termination - virtual Ptr clone () const = 0; + virtual int update(const Mat &model, int inlier_number) const = 0; }; //////////////////////////////// STANDARD TERMINATION /////////////////////////////////////////// -class StandardTerminationCriteria : public TerminationCriteria { +class StandardTerminationCriteria : public Termination { public: static Ptr create(double confidence, int points_size_, int sample_size_, int max_iterations_); }; ///////////////////////////////////// SPRT TERMINATION ////////////////////////////////////////// -class SPRTTermination : public TerminationCriteria { +class SPRTTermination : public Termination { public: - static Ptr create(const std::vector &sprt_histories_, + static Ptr create(const Ptr &sprt, double confidence, int points_size_, int sample_size_, int max_iterations_); }; ///////////////////////////// PROGRESSIVE-NAPSAC-SPRT TERMINATION ///////////////////////////////// -class SPRTPNapsacTermination : public TerminationCriteria { +class SPRTPNapsacTermination : public Termination { public: - static Ptr create(const std::vector& - sprt_histories_, double confidence, int points_size_, int sample_size_, + static Ptr create(const Ptr & + sprt, double confidence, int points_size_, int sample_size_, int max_iterations_, double relax_coef_); }; ////////////////////////////////////// PROSAC TERMINATION ///////////////////////////////////////// -class ProsacTerminationCriteria : public TerminationCriteria { +class ProsacTerminationCriteria : public Termination { public: + virtual const std::vector &getNonRandomInliers () const = 0; + virtual int updateTerminationLength (const Mat &model, int inliers_size, int &found_termination_length) const = 0; static Ptr create(const Ptr &sampler_, const Ptr &error_, int points_size_, int sample_size, double confidence, int max_iters, int min_termination_length, double beta, double non_randomness_phi, - double inlier_thresh); + double inlier_thresh, const std::vector &non_rand_inliers); }; ////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////// UTILS //////////////////////////////////////////////// namespace Utils { + void densitySort (const Mat &points, int knn, Mat &sorted_points, std::vector &sorted_mask); /* * calibrate points: [x'; 1] = K^-1 [x; 1] * @points is matrix N x 4. * @norm_points is output matrix N x 4 with calibrated points. */ - void calibratePoints (const Matx33d &K1, const Matx33d &K2, const Mat &points, Mat &norm_points); - void calibrateAndNormalizePointsPnP (const Matx33d &K, const Mat &pts, Mat &calib_norm_pts); - void normalizeAndDecalibPointsPnP (const Matx33d &K, Mat &pts, Mat &calib_norm_pts); - void decomposeProjection (const Mat &P, Matx33d &K_, Mat &R, Mat &t, bool same_focal=false); - double getCalibratedThreshold (double threshold, const Matx33d &K1, const Matx33d &K2); + void calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Mat &norm_points); + void calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &calib_norm_pts); + void normalizeAndDecalibPointsPnP (const Mat &K, Mat &pts, Mat &calib_norm_pts); + void decomposeProjection (const Mat &P, Matx33d &K_, Matx33d &R, Vec3d &t, bool same_focal=false); + double getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2); float findMedian (std::vector &array); + double intersectionOverUnion (const std::vector &a, const std::vector &b); + void triangulatePoints (const Mat &points, const Mat &E_, const Mat &K1, const Mat &K2, Mat &points3D, Mat &R, Mat &t, + std::vector &good_mask, std::vector &depths1, std::vector &depths2); + void triangulatePoints (const Mat &E, const Mat &points1, const Mat &points2, Mat &corr_points1, Mat &corr_points2, + const Mat &K1, const Mat &K2, Mat &points3D, Mat &R, Mat &t, const std::vector &good_point_mask); + int triangulatePointsRt (const Mat &points, Mat &points3D, const Mat &K1_, const Mat &K2_, + const cv::Mat &R, const cv::Mat &t_vec, std::vector &good_mask, std::vector &depths1, std::vector &depths2); + int decomposeHomography (const Matx33d &Hnorm, std::vector &R, std::vector &t); + double getPoissonCDF (double lambda, int tentative_inliers); + void getClosePoints (const cv::Mat &points, std::vector> &close_points, float close_thr_sqr); + Vec3d getLeftEpipole (const Mat &F); + Vec3d getRightEpipole (const Mat &F); + int removeClosePoints (const Mat &points, Mat &new_points, float thr); } namespace Math { // return skew symmetric matrix @@ -587,6 +672,12 @@ namespace Math { Vec3d rotMat2RotVec (const Matx33d &R); } +class SolverPoly: public Algorithm { +public: + virtual int getRealRoots (const std::vector &coeffs, std::vector &real_roots) = 0; + static Ptr create(); +}; + ///////////////////////////////////////// RANDOM GENERATOR ///////////////////////////////////// class RandomGenerator : public Algorithm { public: @@ -609,7 +700,6 @@ public: virtual int getRandomNumber (int max_rng) = 0; virtual const std::vector &generateUniqueRandomSubset (std::vector &array1, int size1) = 0; - virtual Ptr clone (int state) const = 0; }; class UniformRandomGenerator : public RandomGenerator { @@ -618,6 +708,24 @@ public: static Ptr create (int state, int max_range, int subset_size_); }; +class WeightFunction : public Algorithm { +public: + virtual int getInliersWeights (const std::vector &errors, std::vector &inliers, std::vector &weights) const = 0; + virtual int getInliersWeights (const std::vector &errors, std::vector &inliers, std::vector &weights, double thr_sqr) const = 0; + virtual double getThreshold() const = 0; +}; + +class GaussWeightFunction : public WeightFunction { +public: + static Ptr create (double thr, double sigma, double outlier_prob); +}; + +class MagsacWeightFunction : public WeightFunction { +public: + static Ptr create (const Ptr &gamma_generator_, + int DoF_, double upper_incomplete_of_sigma_quantile, double C_, double max_sigma_); +}; + /////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////// LOCAL OPTIMIZATION ///////////////////////////////////////// class LocalOptimization : public Algorithm { @@ -632,17 +740,18 @@ public: */ virtual bool refineModel (const Mat &best_model, const Score &best_model_score, Mat &new_model, Score &new_model_score) = 0; - virtual Ptr clone(int state) const = 0; + virtual void setCurrentRANSACiter (int /*ransac_iter*/) {} + virtual int getNumLOoptimizations () const { return 0; } }; //////////////////////////////////// GRAPH CUT LO //////////////////////////////////////// class GraphCut : public LocalOptimization { public: static Ptr - create(const Ptr &estimator_, const Ptr &error_, + create(const Ptr &estimator_, const Ptr &quality_, const Ptr &neighborhood_graph_, const Ptr &lo_sampler_, double threshold_, - double spatial_coherence_term, int gc_iters); + double spatial_coherence_term, int gc_iters, Ptr termination_= nullptr); }; //////////////////////////////////// INNER + ITERATIVE LO /////////////////////////////////////// @@ -655,14 +764,13 @@ public: int lo_iter_max_iterations, double threshold_multiplier); }; -class SigmaConsensus : public LocalOptimization { +class SimpleLocalOptimization : public LocalOptimization { public: - static Ptr - create(const Ptr &estimator_, const Ptr &error_, - const Ptr &quality, const Ptr &verifier_, - int max_lo_sample_size, int number_of_irwls_iters_, - int DoF, double sigma_quantile, double upper_incomplete_of_sigma_quantile, - double C_, double maximum_thr); + static Ptr create + (const Ptr &quality_, const Ptr &estimator_, + const Ptr termination_, const Ptr &random_gen, + const Ptr weight_fnc_, + int max_lo_iters_, double inlier_thr_sqr, bool updated_lo=false); }; /////////////////////////////////////////////////////////////////////////////////////////////////// @@ -681,35 +789,86 @@ public: Mat &new_model, Score &new_model_score) = 0; }; -///////////////////////////////////// LEAST SQUARES POLISHER ////////////////////////////////////// -class LeastSquaresPolishing : public FinalModelPolisher { +class NonMinimalPolisher : public FinalModelPolisher { public: - static Ptr create (const Ptr &estimator_, - const Ptr &quality_, int lsq_iterations); + static Ptr create(const Ptr &quality_, const Ptr &solver_, + Ptr weight_fnc_, int max_iters_, double iou_thr_); }; +class CovarianceSolver : public NonMinimalSolver { +public: + ~CovarianceSolver() override = default; + int estimate (const std::vector &, int , std::vector &, + const std::vector &) const override { + assert(false && "not implemeted!"); + return 0; + } + virtual int estimate (const std::vector &new_mask, std::vector &models, + const std::vector &weights) override = 0; + virtual void reset() = 0; +}; +class CovarianceEpipolarSolver : public CovarianceSolver { +public: + static Ptr create (const Mat &points, bool is_fundamental); + static Ptr create (const Mat &points, const Matx33d &T1, const Matx33d &T2); +}; +class CovarianceHomographySolver : public CovarianceSolver { +public: + static Ptr create (const Mat &points); + static Ptr create (const Mat &points, const Matx33d &T1, const Matx33d &T2); +}; +class CovarianceAffineSolver : public CovarianceSolver { +public: + static Ptr create (const Mat &points, const Matx33d &T1, const Matx33d &T2); + static Ptr create (const Mat &points); +}; + +/////////////////////////////////// POSE LIB //////////////////////////////////////// +struct CameraPose { + cv::Matx33d R; + cv::Vec3d t; + double alpha = 1.0; // either focal length or scale +}; +typedef std::vector CameraPoseVector; + +struct BundleOptions { + int max_iterations = 100; + enum LossType { + MLESAC, + } loss_type = LossType::MLESAC; // CAUCHY; + double loss_scale = 1.0; + double gradient_tol = 1e-8; + double step_tol = 1e-8; + double initial_lambda = 1e-3; +}; + +bool satisfyCheirality (const cv::Matx33d& R, const cv::Vec3d &t, const cv::Vec3d &x1, const cv::Vec3d &x2); + +// Relative pose refinement. Minimizes Sampson error error. Assumes identity intrinsics (calibrated camera) +// Returns number of iterations. +int refine_relpose(const cv::Mat &correspondences_, + const std::vector &sample_, + const int sample_size_, + CameraPose *pose, + const BundleOptions &opt = BundleOptions(), + const double *weights = nullptr); /////////////////////////////////// RANSAC OUTPUT /////////////////////////////////// class RansacOutput : public Algorithm { public: virtual ~RansacOutput() override = default; static Ptr create(const Mat &model_, - const std::vector &inliers_mask_, - int time_mcs_, double score_, int number_inliers_, int number_iterations_, - int number_estimated_models_, int number_good_models_); + const std::vector &inliers_mask_, int number_inliers_, + int number_iterations_, ModelConfidence conf, const std::vector &errors_); // Return inliers' indices. size of vector = number of inliers virtual const std::vector &getInliers() = 0; // Return inliers mask. Vector of points size. 1-inlier, 0-outlier. virtual const std::vector &getInliersMask() const = 0; - virtual int getTimeMicroSeconds() const = 0; - virtual int getTimeMicroSeconds1() const = 0; - virtual int getTimeMilliSeconds2() const = 0; - virtual int getTimeSeconds3() const = 0; virtual int getNumberOfInliers() const = 0; - virtual int getNumberOfMainIterations() const = 0; - virtual int getNumberOfGoodModels () const = 0; - virtual int getNumberOfEstimatedModels () const = 0; virtual const Mat &getModel() const = 0; + virtual int getNumberOfIters() const = 0; + virtual ModelConfidence getConfidence() const = 0; + virtual const std::vector &getResiduals() const = 0; }; ////////////////////////////////////////////// MODEL ///////////////////////////////////////////// @@ -724,7 +883,6 @@ public: // getters virtual int getSampleSize () const = 0; virtual bool isParallel() const = 0; - virtual int getMaxNumHypothesisToTestBeforeRejection() const = 0; virtual PolishingMethod getFinalPolisher () const = 0; virtual LocalOptimMethod getLO () const = 0; virtual ErrorMetric getError () const = 0; @@ -744,6 +902,7 @@ public: virtual int getCellSize () const = 0; virtual int getGraphRadius() const = 0; virtual double getRelaxCoef () const = 0; + virtual bool isNonRandomnessTest () const = 0; virtual int getFinalLSQIterations () const = 0; virtual int getDegreesOfFreedom () const = 0; @@ -753,6 +912,7 @@ public: virtual double getC () const = 0; virtual double getMaximumThreshold () const = 0; virtual double getGraphCutSpatialCoherenceTerm () const = 0; + virtual double getKmlesac () const = 0; virtual int getLOSampleSize () const = 0; virtual int getLOThresholdMultiplier() const = 0; virtual int getLOIterativeSampleSize() const = 0; @@ -760,9 +920,15 @@ public: virtual int getLOInnerMaxIters() const = 0; virtual const std::vector &getGridCellNumber () const = 0; virtual int getRandomGeneratorState () const = 0; - virtual int getMaxItersBeforeLO () const = 0; + virtual MethodSolver getRansacSolver () const = 0; + virtual int getPlaneAndParallaxIters () const = 0; + virtual int getLevMarqIters () const = 0; + virtual int getLevMarqItersLO () const = 0; + virtual bool isLarssonOptimization () const = 0; + virtual int getProsacMaxSamples() const = 0; // setters + virtual void setNonRandomnessTest (bool set) = 0; virtual void setLocalOptimization (LocalOptimMethod lo_) = 0; virtual void setKNearestNeighhbors (int knn_) = 0; virtual void setNeighborsType (NeighborSearchMethod neighbors) = 0; @@ -774,8 +940,8 @@ public: virtual void setLOIterations (int iters) = 0; virtual void setLOIterativeIters (int iters) = 0; virtual void setLOSampleSize (int lo_sample_size) = 0; - virtual void setThresholdMultiplierLO (double thr_mult) = 0; virtual void setRandomGeneratorState (int state) = 0; + virtual void setFinalLSQ (int iters) = 0; virtual void maskRequired (bool required) = 0; virtual bool isMaskRequired () const = 0; @@ -783,6 +949,9 @@ public: double confidence_=0.95, int max_iterations_=5000, ScoreMethod score_ =ScoreMethod::SCORE_METHOD_MSAC); }; +double getLambda (std::vector &supports, double cdf_thr, int points_size, + int sample_size, bool is_indendent_inliers, int &min_non_random_inliers); + Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method, double ransacReprojThreshold, OutputArray mask, const int maxIters, const double confidence); @@ -811,7 +980,7 @@ Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers, void saveMask (OutputArray mask, const std::vector &inliers_mask); void setParameters (Ptr ¶ms, EstimationMethod estimator, const UsacParams &usac_params, bool mask_need); -bool run (const Ptr ¶ms, InputArray points1, InputArray points2, int state, +bool run (const Ptr ¶ms, InputArray points1, InputArray points2, Ptr &ransac_output, InputArray K1_, InputArray K2_, InputArray dist_coeff1, InputArray dist_coeff2); }} diff --git a/modules/calib3d/src/usac/bundle.cpp b/modules/calib3d/src/usac/bundle.cpp new file mode 100644 index 0000000000..a621490575 --- /dev/null +++ b/modules/calib3d/src/usac/bundle.cpp @@ -0,0 +1,308 @@ +// 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 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 &sample; + const int sample_size; + const MlesacLoss &loss_fn; + const double *weights; + +public: + RelativePoseJacobianAccumulator( + const Mat& correspondences_, + const std::vector &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(E(0,0)), m12=static_cast(E(0,1)), m13=static_cast(E(0,2)); + const float m21=static_cast(E(1,0)), m22=static_cast(E(1,1)), m23=static_cast(E(1,2)); + const float m31=static_cast(E(2,0)), m32=static_cast(E(2,1)), m33=static_cast(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 &JtJ, Matx &Jtr, Matx &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 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 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 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 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 &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 JtJ; + Matx Jtr; + Matx 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 sol; + Matx 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 diff --git a/modules/calib3d/src/usac/degeneracy.cpp b/modules/calib3d/src/usac/degeneracy.cpp index 7963ef3911..a6e028d5f3 100644 --- a/modules/calib3d/src/usac/degeneracy.cpp +++ b/modules/calib3d/src/usac/degeneracy.cpp @@ -13,7 +13,7 @@ private: const int min_sample_size; public: explicit EpipolarGeometryDegeneracyImpl (const Mat &points_, int sample_size_) : - points_mat(&points_), points ((float*) points_.data), min_sample_size (sample_size_) {} + points_mat(&points_), points ((float*) points_mat->data), min_sample_size (sample_size_) {} /* * 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. @@ -23,39 +23,27 @@ public: * e × x ~+ x'^T F */ inline bool isModelValid(const Mat &F_, const std::vector &sample) const override { - // F is of rank 2, taking cross product of two rows we obtain null vector of F - Vec3d ec_mat = F_.row(0).cross(F_.row(2)); - auto * ec = ec_mat.val; // of size 3x1 - - // e is zero vector, recompute e - if (ec[0] <= 1.9984e-15 && ec[0] >= -1.9984e-15 && - ec[1] <= 1.9984e-15 && ec[1] >= -1.9984e-15 && - ec[2] <= 1.9984e-15 && ec[2] >= -1.9984e-15) { - ec_mat = F_.row(1).cross(F_.row(2)); - ec = ec_mat.val; - } + const Vec3d ep = Utils::getRightEpipole(F_); + const auto * const e = ep.val; // of size 3x1 const auto * const F = (double *) F_.data; // without loss of generality, let the first point in sample be in front of the camera. int pt = 4*sample[0]; - // s1 = F11 * x2 + F21 * y2 + F31 * 1 - // s2 = e'_2 * 1 - e'_3 * y1 + // 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])*(ec[1]-ec[2]*points[pt+1]); + 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])*(ec[1]-ec[2]*points[pt+1])<0) - return false; + 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; } - - Ptr clone(int /*state*/) const override { - return makePtr(*points_mat, min_sample_size); - } }; void EpipolarGeometryDegeneracy::recoverRank (Mat &model, bool is_fundamental_mat) { /* @@ -81,9 +69,10 @@ class HomographyDegeneracyImpl : public HomographyDegeneracy { private: const Mat * points_mat; const float * const points; + const float TOLERANCE = 2 * FLT_EPSILON; // 2 from area of triangle public: explicit HomographyDegeneracyImpl (const Mat &points_) : - points_mat(&points_), points ((float *)points_.data) {} + points_mat(&points_), points ((float *)points_mat->data) {} inline bool isSampleGood (const std::vector &sample) const override { const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2], smpl4 = 4*sample[3]; @@ -119,222 +108,580 @@ public: // Checks if points are not collinear // If area of triangle constructed with 3 points is less then 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| = (1/2) det |x2-x1 y2-y1| < threshold - // |x3 y3 1| |x3-x1 y3-y1 0| |x3-x1 y3-y1| + // (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)) * 0.5 < FLT_EPSILON) return false; //1,2,3 - if (fabsf((x2-x1) * (y4-y1) - (y2-y1) * (x4-x1)) * 0.5 < FLT_EPSILON) return false; //1,2,4 - if (fabsf((x3-x1) * (y4-y1) - (y3-y1) * (x4-x1)) * 0.5 < FLT_EPSILON) return false; //1,3,4 - if (fabsf((x3-x2) * (y4-y2) - (y3-y2) * (x4-x2)) * 0.5 < FLT_EPSILON) return false; //2,3,4 + 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)) * 0.5 < FLT_EPSILON) return false; //1,2,3 - if (fabsf((X2-X1) * (Y4-Y1) - (Y2-Y1) * (X4-X1)) * 0.5 < FLT_EPSILON) return false; //1,2,4 - if (fabsf((X3-X1) * (Y4-Y1) - (Y3-Y1) * (X4-X1)) * 0.5 < FLT_EPSILON) return false; //1,3,4 - if (fabsf((X3-X2) * (Y4-Y2) - (Y3-Y2) * (X4-X2)) * 0.5 < FLT_EPSILON) return false; //2,3,4 + 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 clone(int /*state*/) const override { - return makePtr(*points_mat); - } }; Ptr HomographyDegeneracy::create (const Mat &points_) { return makePtr(points_); } +class FundamentalDegeneracyViaEImpl : public FundamentalDegeneracyViaE { +private: + bool is_F_objective; + std::vector> instances = {{0,1,2,3,4}, {2,3,4,5,6}, {0,1,4,5,6}}; + std::vector e_sample; + const Ptr quality; + Ptr e_degen, f_degen; + Ptr e_solver; + std::vector e_models; + const int E_SAMPLE_SIZE = 5; + Matx33d K2_inv_t, K1_inv; +public: + FundamentalDegeneracyViaEImpl (const Ptr &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(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 &sample) const override { + return f_degen->isModelValid(F, sample); + } + bool recoverIfDegenerate (const std::vector &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::create (const Ptr &quality, const Mat &pts, + const Mat &calib_pts, const Matx33d &K1, const Matx33d &K2, bool is_f_objective) { + return makePtr(quality, pts, calib_pts, K1, K2, is_f_objective); +} ///////////////////////////////// Fundamental Matrix Degeneracy /////////////////////////////////// class FundamentalDegeneracyImpl : public FundamentalDegeneracy { private: RNG rng; const Ptr quality; + const Ptr f_error; + Ptr h_repr_quality; const float * const points; const Mat * points_mat; const Ptr h_reproj_error; + Ptr f_non_min_solver; Ptr h_non_min_solver; + Ptr random_gen_H; const EpipolarGeometryDegeneracyImpl ep_deg; // threshold to find inliers for homography model - const double homography_threshold, log_conf = log(0.05); + 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> h_sample {{0,1,2},{3,4,5},{0,1,6},{3,4,6},{2,5,6}}; - std::vector h_inliers; + std::vector> 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 non_planar_supports, h_inliers, h_outliers, h_outliers_eval, f_inliers; std::vector weights; std::vector h_models; - const int points_size, sample_size; + 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_, const Mat &points_, - int sample_size_, double homography_threshold_) : - rng (state), quality(quality_), points((float *) points_.data), points_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((float *) points_.data), points_mat(&points_), h_reproj_error(ReprojectionErrorForward::create(points_)), ep_deg (points_, sample_size_), homography_threshold (homography_threshold_), - points_size (quality_->getPointsSize()), sample_size (sample_size_) { + 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{0, 1, 7}); - h_sample.emplace_back(std::vector{0, 2, 7}); - h_sample.emplace_back(std::vector{3, 5, 7}); - h_sample.emplace_back(std::vector{3, 6, 7}); - h_sample.emplace_back(std::vector{2, 4, 7}); + h_sample.emplace_back(std::vector{0, 1, 7}); h_sample_ver.emplace_back(std::vector{2,3,4,5,6}); + h_sample.emplace_back(std::vector{0, 2, 7}); h_sample_ver.emplace_back(std::vector{1,3,4,5,6}); + h_sample.emplace_back(std::vector{3, 5, 7}); h_sample_ver.emplace_back(std::vector{0,1,2,4,6}); + h_sample.emplace_back(std::vector{3, 6, 7}); h_sample_ver.emplace_back(std::vector{0,1,2,4,5}); + h_sample.emplace_back(std::vector{2, 4, 7}); h_sample_ver.emplace_back(std::vector{0,1,3,5,6}); } + non_planar_supports = std::vector(MAX_MODELS_TO_TEST); h_inliers = std::vector(points_size); + h_outliers = std::vector(points_size); + h_outliers_eval = std::vector(points_size); + f_inliers = std::vector(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 &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 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 &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 R; std::vector 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 &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; + std::vector 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 + (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(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) { + 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 R; std::vector t; std::vector F_good; + std::vector 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 &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 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) { + 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 &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 &sample) const override { return ep_deg.isModelValid(F, sample); } - bool recoverIfDegenerate (const std::vector &sample, const Mat &F_best, - Mat &non_degenerate_model, Score &non_degenerate_model_score) override { - non_degenerate_model_score = Score(); // set worst case - - // According to Two-view Geometry Estimation Unaffected by a Dominant Plane - // (http://cmp.felk.cvut.cz/~matas/papers/chum-degen-cvpr05.pdf) - // only 5 homographies enough to test - // triplets {1,2,3}, {4,5,6}, {1,2,7}, {4,5,7} and {3,6,7} - - // H = A - e' (M^-1 b)^T - // A = [e']_x F - // b_i = (x′i × (A xi))^T (x′i × e′)‖x′i×e′‖^−2, - // M is a 3×3 matrix with rows x^T_i - // epipole e' is left nullspace of F s.t. e′^T F=0, - - // find e', null space of F^T - Vec3d e_prime = F_best.col(0).cross(F_best.col(2)); - if (fabs(e_prime(0)) < 1e-10 && fabs(e_prime(1)) < 1e-10 && - fabs(e_prime(2)) < 1e-10) // if e' is zero - e_prime = F_best.col(1).cross(F_best.col(2)); - - const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best); - - Vec3d xi_prime(0,0,1), xi(0,0,1), b; - Matx33d M(0,0,1,0,0,1,0,0,1); // last column of M is 1 - - bool is_model_degenerate = false; - for (const auto &h_i : h_sample) { // only 5 samples - for (int pt_i = 0; pt_i < 3; pt_i++) { - // find b and M - const int smpl = 4*sample[h_i[pt_i]]; - xi[0] = points[smpl]; - xi[1] = points[smpl+1]; - xi_prime[0] = points[smpl+2]; - xi_prime[1] = points[smpl+3]; - - // (x′i × e') - const Vec3d xprime_X_eprime = xi_prime.cross(e_prime); - - // (x′i × (A xi)) - const Vec3d xprime_X_Ax = xi_prime.cross(A * xi); - - // x′i × (A xi))^T (x′i × e′) / ‖x′i×e′‖^2, - b[pt_i] = xprime_X_Ax.dot(xprime_X_eprime) / - std::pow(norm(xprime_X_eprime), 2); - - // M from x^T - M(pt_i, 0) = xi[0]; - M(pt_i, 1) = xi[1]; - } - - // compute H - Matx33d H = A - e_prime * (M.inv() * b).t(); - - int inliers_out_plane = 0; - h_reproj_error->setModelParameters(Mat(H)); - - // find inliers from sample, points related to H, x' ~ Hx - for (int s = 0; s < sample_size; s++) - if (h_reproj_error->getError(sample[s]) > homography_threshold) - if (++inliers_out_plane > 2) - break; - - // if there are at least 5 points lying on plane then F is degenerate - if (inliers_out_plane <= 2) { - is_model_degenerate = true; - - // update homography by polishing on all inliers - int h_inls_cnt = 0; - const auto &h_errors = h_reproj_error->getErrors(Mat(H)); - for (int pt = 0; pt < points_size; pt++) - if (h_errors[pt] < homography_threshold) - h_inliers[h_inls_cnt++] = pt; - if (h_non_min_solver->estimate(h_inliers, h_inls_cnt, h_models, weights) != 0) - H = Matx33d(h_models[0]); - - Mat newF; - const Score newF_score = planeAndParallaxRANSAC(H, newF, h_errors); - if (newF_score.isBetter(non_degenerate_model_score)) { - // store non degenerate model - non_degenerate_model_score = newF_score; - newF.copyTo(non_degenerate_model); - } - } - } - return is_model_degenerate; - } - Ptr clone(int state) const override { - return makePtr(state, quality->clone(), *points_mat, - sample_size, homography_threshold); - } -private: - // RANSAC with plane-and-parallax to find new Fundamental matrix - Score planeAndParallaxRANSAC (const Matx33d &H, Mat &best_F, const std::vector &h_errors) { - int max_iters = 100; // with 95% confidence assume at least 17% of inliers - Score best_score; - for (int iters = 0; iters < max_iters; iters++) { - // draw two random points - int h_outlier1 = rng.uniform(0, points_size); - int h_outlier2 = rng.uniform(0, points_size); - while (h_outlier1 == h_outlier2) - h_outlier2 = rng.uniform(0, points_size); - - // find outliers of homography H - if (h_errors[h_outlier1] > homography_threshold && - h_errors[h_outlier2] > homography_threshold) { - - // do plane and parallax with outliers of H - // F = [(p1' x Hp1) x (p2' x Hp2)]_x H - const Matx33d F = Math::getSkewSymmetric( - (Vec3d(points[4*h_outlier1+2], points[4*h_outlier1+3], 1).cross // p1' - (H * Vec3d(points[4*h_outlier1 ], points[4*h_outlier1+1], 1))).cross // Hp1 - (Vec3d(points[4*h_outlier2+2], points[4*h_outlier2+3], 1).cross // p2' - (H * Vec3d(points[4*h_outlier2 ], points[4*h_outlier2+1], 1))) // Hp2 - ) * H; - - const Score score = quality->getScore(Mat(F)); - if (score.isBetter(best_score)) { - best_score = score; - best_F = Mat(F); - const double predicted_iters = log_conf / log(1 - std::pow - (static_cast(score.inlier_number) / points_size, 2)); - - if (! std::isinf(predicted_iters) && predicted_iters < max_iters) - max_iters = static_cast(predicted_iters); - } - } - } - return best_score; + 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::create (int state, const Ptr &quality_, - const Mat &points_, int sample_size_, double homography_threshold_) { + 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(state, quality_, points_, sample_size_, - homography_threshold_); + max_iters_plane_and_parallax, homography_threshold_, f_inlier_thr_sqr, true_K1, true_K2); } + class EssentialDegeneracyImpl : public EssentialDegeneracy { private: - const Mat * points_mat; - const int sample_size; const EpipolarGeometryDegeneracyImpl ep_deg; public: explicit EssentialDegeneracyImpl (const Mat &points, int sample_size_) : - points_mat(&points), sample_size(sample_size_), ep_deg (points, sample_size_) {} + ep_deg (points, sample_size_) {} inline bool isModelValid(const Mat &E, const std::vector &sample) const override { return ep_deg.isModelValid(E, sample); } - Ptr clone(int /*state*/) const override { - return makePtr(*points_mat, sample_size); - } }; Ptr EssentialDegeneracy::create (const Mat &points_, int sample_size_) { return makePtr(points_, sample_size_); } -}} +}} \ No newline at end of file diff --git a/modules/calib3d/src/usac/dls_solver.cpp b/modules/calib3d/src/usac/dls_solver.cpp index 8f109d51bf..43b0fc2919 100644 --- a/modules/calib3d/src/usac/dls_solver.cpp +++ b/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 clone () const override { - return makePtr(*points_mat, *calib_norm_points_mat, *K_mat); - } #if defined(HAVE_LAPACK) || defined(HAVE_EIGEN) int estimate(const std::vector &sample, int sample_number, std::vector &models_, const std::vector &/*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 &/*mask*/, std::vector &/*models*/, + const std::vector &/*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::create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K) { +Ptr DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) { return makePtr(points_, calib_norm_pts, K); } }} diff --git a/modules/calib3d/src/usac/essential_solver.cpp b/modules/calib3d/src/usac/essential_solver.cpp index 014cd36f40..e2b59a8c49 100644 --- a/modules/calib3d/src/usac/essential_solver.cpp +++ b/modules/calib3d/src/usac/essential_solver.cpp @@ -11,28 +11,25 @@ #endif namespace cv { namespace usac { -// Essential matrix solver: /* * H. Stewenius, C. Engels, and D. Nister. Recent developments on direct relative orientation. * ISPRS J. of Photogrammetry and Remote Sensing, 60:284,294, 2006 * http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.61.9329&rep=rep1&type=pdf +* +* D. Nister. An efficient solution to the five-point relative pose problem +* IEEE Transactions on Pattern Analysis and Machine Intelligence +* https://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.86.8769&rep=rep1&type=pdf */ -class EssentialMinimalSolverStewenius5ptsImpl : public EssentialMinimalSolverStewenius5pts { +class EssentialMinimalSolver5ptsImpl : public EssentialMinimalSolver5pts { private: // Points must be calibrated K^-1 x const Mat * points_mat; -#if defined(HAVE_EIGEN) || defined(HAVE_LAPACK) const float * const pts; -#endif + const bool use_svd, is_nister; public: - explicit EssentialMinimalSolverStewenius5ptsImpl (const Mat &points_) : - points_mat(&points_) -#if defined(HAVE_EIGEN) || defined(HAVE_LAPACK) - , pts((float*)points_.data) -#endif - {} + explicit EssentialMinimalSolver5ptsImpl (const Mat &points_, bool use_svd_=false, bool is_nister_=false) : + points_mat(&points_), pts((float*)points_mat->data), use_svd(use_svd_), is_nister(is_nister_) {} -#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN) int estimate (const std::vector &sample, std::vector &models) const override { // (1) Extract 4 null vectors from linear equations of epipolar constraint std::vector coefficients(45); // 5 pts=rows, 9 columns @@ -53,25 +50,35 @@ public: const int num_cols = 9, num_e_mat = 4; double ee[36]; // 9*4 - // eliminate linear equations - if (!Math::eliminateUpperTriangular(coefficients, 5, num_cols)) - return 0; - for (int i = 0; i < num_e_mat; i++) - for (int j = 5; j < num_cols; j++) - ee[num_cols * i + j] = (i + 5 == j) ? 1 : 0; - // use back-substitution - for (int e = 0; e < num_e_mat; e++) { - const int curr_e = num_cols * e; - // start from the last row - for (int i = 4; i >= 0; i--) { - const int row_i = i * num_cols; - double acc = 0; - for (int j = i + 1; j < num_cols; j++) - acc -= coefficients[row_i + j] * ee[curr_e + j]; - ee[curr_e + i] = acc / coefficients[row_i + i]; - // due to numerical errors return 0 solutions - if (std::isnan(ee[curr_e + i])) - return 0; + if (use_svd) { + Matx coeffs (&coefficients[0]); + Mat D, U, Vt; + SVDecomp(coeffs, D, U, Vt, SVD::FULL_UV + SVD::MODIFY_A); + const auto * const vt = (double *) Vt.data; + for (int i = 0; i < num_e_mat; i++) + for (int j = 0; j < num_cols; j++) + ee[i * num_cols + j] = vt[(8-i)*num_cols+j]; + } else { + // eliminate linear equations + if (!Math::eliminateUpperTriangular(coefficients, 5, num_cols)) + return 0; + for (int i = 0; i < num_e_mat; i++) + for (int j = 5; j < num_cols; j++) + ee[num_cols * i + j] = (i + 5 == j) ? 1 : 0; + // use back-substitution + for (int e = 0; e < num_e_mat; e++) { + const int curr_e = num_cols * e; + // start from the last row + for (int i = 4; i >= 0; i--) { + const int row_i = i * num_cols; + double acc = 0; + for (int j = i + 1; j < num_cols; j++) + acc -= coefficients[row_i + j] * ee[curr_e + j]; + ee[curr_e + i] = acc / coefficients[row_i + i]; + // due to numerical errors return 0 solutions + if (std::isnan(ee[curr_e + i])) + return 0; + } } } @@ -80,130 +87,215 @@ public: {null_space.col(0), null_space.col(3), null_space.col(6)}, {null_space.col(1), null_space.col(4), null_space.col(7)}, {null_space.col(2), null_space.col(5), null_space.col(8)}}; + Mat_ constraint_mat(10, 20); + Matx eet[3][3]; // (2) Use the rank constraint and the trace constraint to build ten third-order polynomial // equations in the three unknowns. The monomials are ordered in GrLex order and // represented in a 10×20 matrix, where each row corresponds to an equation and each column // corresponds to a monomial - Matx eet[3][3]; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - // compute EE Transpose - // Shorthand for multiplying the Essential matrix with its transpose. - eet[i][j] = 2 * (multPolysDegOne(null_space_mat[i][0].val, null_space_mat[j][0].val) + - multPolysDegOne(null_space_mat[i][1].val, null_space_mat[j][1].val) + - multPolysDegOne(null_space_mat[i][2].val, null_space_mat[j][2].val)); + if (is_nister) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + // compute EE Transpose + // Shorthand for multiplying the Essential matrix with its transpose. + eet[i][j] = multPolysDegOne(null_space_mat[i][0].val, null_space_mat[j][0].val) + + multPolysDegOne(null_space_mat[i][1].val, null_space_mat[j][1].val) + + multPolysDegOne(null_space_mat[i][2].val, null_space_mat[j][2].val); - const Matx trace = eet[0][0] + eet[1][1] + eet[2][2]; - Mat_ constraint_mat(10, 20); - // Trace constraint - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - Mat(multPolysDegOneAndTwo(eet[i][0].val, null_space_mat[0][j].val) + - multPolysDegOneAndTwo(eet[i][1].val, null_space_mat[1][j].val) + - multPolysDegOneAndTwo(eet[i][2].val, null_space_mat[2][j].val) - - 0.5 * multPolysDegOneAndTwo(trace.val, null_space_mat[i][j].val)) - .copyTo(constraint_mat.row(3 * i + j)); + const Matx trace = 0.5*(eet[0][0] + eet[1][1] + eet[2][2]); + // Trace constraint + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + Mat(multPolysDegOneAndTwoNister(i == 0 ? (eet[i][0] - trace).val : eet[i][0].val, null_space_mat[0][j].val) + + multPolysDegOneAndTwoNister(i == 1 ? (eet[i][1] - trace).val : eet[i][1].val, null_space_mat[1][j].val) + + multPolysDegOneAndTwoNister(i == 2 ? (eet[i][2] - trace).val : eet[i][2].val, null_space_mat[2][j].val)).copyTo(constraint_mat.row(1+3 * j + i)); - // Rank = zero determinant constraint - Mat(multPolysDegOneAndTwo( - (multPolysDegOne(null_space_mat[0][1].val, null_space_mat[1][2].val) - - multPolysDegOne(null_space_mat[0][2].val, null_space_mat[1][1].val)).val, - null_space_mat[2][0].val) + + // Rank = zero determinant constraint + Mat(multPolysDegOneAndTwoNister( + (multPolysDegOne(null_space_mat[0][1].val, null_space_mat[1][2].val) - + multPolysDegOne(null_space_mat[0][2].val, null_space_mat[1][1].val)).val, + null_space_mat[2][0].val) + + multPolysDegOneAndTwoNister( + (multPolysDegOne(null_space_mat[0][2].val, null_space_mat[1][0].val) - + multPolysDegOne(null_space_mat[0][0].val, null_space_mat[1][2].val)).val, + null_space_mat[2][1].val) + + multPolysDegOneAndTwoNister( + (multPolysDegOne(null_space_mat[0][0].val, null_space_mat[1][1].val) - + multPolysDegOne(null_space_mat[0][1].val, null_space_mat[1][0].val)).val, + null_space_mat[2][2].val)).copyTo(constraint_mat.row(0)); + + Matx Acoef = constraint_mat.colRange(0, 10), + Bcoef = constraint_mat.colRange(10, 20), A_; + + if (!solve(Acoef, Bcoef, A_, DECOMP_LU)) return 0; + + double b[3 * 13]; + const auto * const a = A_.val; + for (int i = 0; i < 3; i++) { + const int r1_idx = i * 2 + 4, r2_idx = i * 2 + 5; // process from 4th row + for (int j = 0, r1_j = 0, r2_j = 0; j < 13; j++) + b[i*13+j] = ((j == 0 || j == 4 || j == 8) ? 0 : a[r1_idx*A_.cols+r1_j++]) - ((j == 3 || j == 7 || j == 12) ? 0 : a[r2_idx*A_.cols+r2_j++]); + } + + std::vector c(11), rs; + // filling coefficients of 10-degree polynomial satysfying zero-determinant constraint of essential matrix, ie., det(E) = 0 + // based on "An Efficient Solution to the Five-Point Relative Pose Problem" (David Nister) + // same as in five-point.cpp + c[10] = (b[0]*b[17]*b[34]+b[26]*b[4]*b[21]-b[26]*b[17]*b[8]-b[13]*b[4]*b[34]-b[0]*b[21]*b[30]+b[13]*b[30]*b[8]); + c[9] = (b[26]*b[4]*b[22]+b[14]*b[30]*b[8]+b[13]*b[31]*b[8]+b[1]*b[17]*b[34]-b[13]*b[5]*b[34]+b[26]*b[5]*b[21]-b[0]*b[21]*b[31]-b[26]*b[17]*b[9]-b[1]*b[21]*b[30]+b[27]*b[4]*b[21]+b[0]*b[17]*b[35]-b[0]*b[22]*b[30]+b[13]*b[30]*b[9]+b[0]*b[18]*b[34]-b[27]*b[17]*b[8]-b[14]*b[4]*b[34]-b[13]*b[4]*b[35]-b[26]*b[18]*b[8]); + c[8] = (b[14]*b[30]*b[9]+b[14]*b[31]*b[8]+b[13]*b[31]*b[9]-b[13]*b[4]*b[36]-b[13]*b[5]*b[35]+b[15]*b[30]*b[8]-b[13]*b[6]*b[34]+b[13]*b[30]*b[10]+b[13]*b[32]*b[8]-b[14]*b[4]*b[35]-b[14]*b[5]*b[34]+b[26]*b[4]*b[23]+b[26]*b[5]*b[22]+b[26]*b[6]*b[21]-b[26]*b[17]*b[10]-b[15]*b[4]*b[34]-b[26]*b[18]*b[9]-b[26]*b[19]*b[8]+b[27]*b[4]*b[22]+b[27]*b[5]*b[21]-b[27]*b[17]*b[9]-b[27]*b[18]*b[8]-b[1]*b[21]*b[31]-b[0]*b[23]*b[30]-b[0]*b[21]*b[32]+b[28]*b[4]*b[21]-b[28]*b[17]*b[8]+b[2]*b[17]*b[34]+b[0]*b[18]*b[35]-b[0]*b[22]*b[31]+b[0]*b[17]*b[36]+b[0]*b[19]*b[34]-b[1]*b[22]*b[30]+b[1]*b[18]*b[34]+b[1]*b[17]*b[35]-b[2]*b[21]*b[30]); + c[7] = (b[14]*b[30]*b[10]+b[14]*b[32]*b[8]-b[3]*b[21]*b[30]+b[3]*b[17]*b[34]+b[13]*b[32]*b[9]+b[13]*b[33]*b[8]-b[13]*b[4]*b[37]-b[13]*b[5]*b[36]+b[15]*b[30]*b[9]+b[15]*b[31]*b[8]-b[16]*b[4]*b[34]-b[13]*b[6]*b[35]-b[13]*b[7]*b[34]+b[13]*b[30]*b[11]+b[13]*b[31]*b[10]+b[14]*b[31]*b[9]-b[14]*b[4]*b[36]-b[14]*b[5]*b[35]-b[14]*b[6]*b[34]+b[16]*b[30]*b[8]-b[26]*b[20]*b[8]+b[26]*b[4]*b[24]+b[26]*b[5]*b[23]+b[26]*b[6]*b[22]+b[26]*b[7]*b[21]-b[26]*b[17]*b[11]-b[15]*b[4]*b[35]-b[15]*b[5]*b[34]-b[26]*b[18]*b[10]-b[26]*b[19]*b[9]+b[27]*b[4]*b[23]+b[27]*b[5]*b[22]+b[27]*b[6]*b[21]-b[27]*b[17]*b[10]-b[27]*b[18]*b[9]-b[27]*b[19]*b[8]+b[0]*b[17]*b[37]-b[0]*b[23]*b[31]-b[0]*b[24]*b[30]-b[0]*b[21]*b[33]-b[29]*b[17]*b[8]+b[28]*b[4]*b[22]+b[28]*b[5]*b[21]-b[28]*b[17]*b[9]-b[28]*b[18]*b[8]+b[29]*b[4]*b[21]+b[1]*b[19]*b[34]-b[2]*b[21]*b[31]+b[0]*b[20]*b[34]+b[0]*b[19]*b[35]+b[0]*b[18]*b[36]-b[0]*b[22]*b[32]-b[1]*b[23]*b[30]-b[1]*b[21]*b[32]+b[1]*b[18]*b[35]-b[1]*b[22]*b[31]-b[2]*b[22]*b[30]+b[2]*b[17]*b[35]+b[1]*b[17]*b[36]+b[2]*b[18]*b[34]); + c[6] = (-b[14]*b[6]*b[35]-b[14]*b[7]*b[34]-b[3]*b[22]*b[30]-b[3]*b[21]*b[31]+b[3]*b[17]*b[35]+b[3]*b[18]*b[34]+b[13]*b[32]*b[10]+b[13]*b[33]*b[9]-b[13]*b[4]*b[38]-b[13]*b[5]*b[37]-b[15]*b[6]*b[34]+b[15]*b[30]*b[10]+b[15]*b[32]*b[8]-b[16]*b[4]*b[35]-b[13]*b[6]*b[36]-b[13]*b[7]*b[35]+b[13]*b[31]*b[11]+b[13]*b[30]*b[12]+b[14]*b[32]*b[9]+b[14]*b[33]*b[8]-b[14]*b[4]*b[37]-b[14]*b[5]*b[36]+b[16]*b[30]*b[9]+b[16]*b[31]*b[8]-b[26]*b[20]*b[9]+b[26]*b[4]*b[25]+b[26]*b[5]*b[24]+b[26]*b[6]*b[23]+b[26]*b[7]*b[22]-b[26]*b[17]*b[12]+b[14]*b[30]*b[11]+b[14]*b[31]*b[10]+b[15]*b[31]*b[9]-b[15]*b[4]*b[36]-b[15]*b[5]*b[35]-b[26]*b[18]*b[11]-b[26]*b[19]*b[10]-b[27]*b[20]*b[8]+b[27]*b[4]*b[24]+b[27]*b[5]*b[23]+b[27]*b[6]*b[22]+b[27]*b[7]*b[21]-b[27]*b[17]*b[11]-b[27]*b[18]*b[10]-b[27]*b[19]*b[9]-b[16]*b[5]*b[34]-b[29]*b[17]*b[9]-b[29]*b[18]*b[8]+b[28]*b[4]*b[23]+b[28]*b[5]*b[22]+b[28]*b[6]*b[21]-b[28]*b[17]*b[10]-b[28]*b[18]*b[9]-b[28]*b[19]*b[8]+b[29]*b[4]*b[22]+b[29]*b[5]*b[21]-b[2]*b[23]*b[30]+b[2]*b[18]*b[35]-b[1]*b[22]*b[32]-b[2]*b[21]*b[32]+b[2]*b[19]*b[34]+b[0]*b[19]*b[36]-b[0]*b[22]*b[33]+b[0]*b[20]*b[35]-b[0]*b[23]*b[32]-b[0]*b[25]*b[30]+b[0]*b[17]*b[38]+b[0]*b[18]*b[37]-b[0]*b[24]*b[31]+b[1]*b[17]*b[37]-b[1]*b[23]*b[31]-b[1]*b[24]*b[30]-b[1]*b[21]*b[33]+b[1]*b[20]*b[34]+b[1]*b[19]*b[35]+b[1]*b[18]*b[36]+b[2]*b[17]*b[36]-b[2]*b[22]*b[31]); + c[5] = (-b[14]*b[6]*b[36]-b[14]*b[7]*b[35]+b[14]*b[31]*b[11]-b[3]*b[23]*b[30]-b[3]*b[21]*b[32]+b[3]*b[18]*b[35]-b[3]*b[22]*b[31]+b[3]*b[17]*b[36]+b[3]*b[19]*b[34]+b[13]*b[32]*b[11]+b[13]*b[33]*b[10]-b[13]*b[5]*b[38]-b[15]*b[6]*b[35]-b[15]*b[7]*b[34]+b[15]*b[30]*b[11]+b[15]*b[31]*b[10]+b[16]*b[31]*b[9]-b[13]*b[6]*b[37]-b[13]*b[7]*b[36]+b[13]*b[31]*b[12]+b[14]*b[32]*b[10]+b[14]*b[33]*b[9]-b[14]*b[4]*b[38]-b[14]*b[5]*b[37]-b[16]*b[6]*b[34]+b[16]*b[30]*b[10]+b[16]*b[32]*b[8]-b[26]*b[20]*b[10]+b[26]*b[5]*b[25]+b[26]*b[6]*b[24]+b[26]*b[7]*b[23]+b[14]*b[30]*b[12]+b[15]*b[32]*b[9]+b[15]*b[33]*b[8]-b[15]*b[4]*b[37]-b[15]*b[5]*b[36]+b[29]*b[5]*b[22]+b[29]*b[6]*b[21]-b[26]*b[18]*b[12]-b[26]*b[19]*b[11]-b[27]*b[20]*b[9]+b[27]*b[4]*b[25]+b[27]*b[5]*b[24]+b[27]*b[6]*b[23]+b[27]*b[7]*b[22]-b[27]*b[17]*b[12]-b[27]*b[18]*b[11]-b[27]*b[19]*b[10]-b[28]*b[20]*b[8]-b[16]*b[4]*b[36]-b[16]*b[5]*b[35]-b[29]*b[17]*b[10]-b[29]*b[18]*b[9]-b[29]*b[19]*b[8]+b[28]*b[4]*b[24]+b[28]*b[5]*b[23]+b[28]*b[6]*b[22]+b[28]*b[7]*b[21]-b[28]*b[17]*b[11]-b[28]*b[18]*b[10]-b[28]*b[19]*b[9]+b[29]*b[4]*b[23]-b[2]*b[22]*b[32]-b[2]*b[21]*b[33]-b[1]*b[24]*b[31]+b[0]*b[18]*b[38]-b[0]*b[24]*b[32]+b[0]*b[19]*b[37]+b[0]*b[20]*b[36]-b[0]*b[25]*b[31]-b[0]*b[23]*b[33]+b[1]*b[19]*b[36]-b[1]*b[22]*b[33]+b[1]*b[20]*b[35]+b[2]*b[19]*b[35]-b[2]*b[24]*b[30]-b[2]*b[23]*b[31]+b[2]*b[20]*b[34]+b[2]*b[17]*b[37]-b[1]*b[25]*b[30]+b[1]*b[18]*b[37]+b[1]*b[17]*b[38]-b[1]*b[23]*b[32]+b[2]*b[18]*b[36]); + c[4] = (-b[14]*b[6]*b[37]-b[14]*b[7]*b[36]+b[14]*b[31]*b[12]+b[3]*b[17]*b[37]-b[3]*b[23]*b[31]-b[3]*b[24]*b[30]-b[3]*b[21]*b[33]+b[3]*b[20]*b[34]+b[3]*b[19]*b[35]+b[3]*b[18]*b[36]-b[3]*b[22]*b[32]+b[13]*b[32]*b[12]+b[13]*b[33]*b[11]-b[15]*b[6]*b[36]-b[15]*b[7]*b[35]+b[15]*b[31]*b[11]+b[15]*b[30]*b[12]+b[16]*b[32]*b[9]+b[16]*b[33]*b[8]-b[13]*b[6]*b[38]-b[13]*b[7]*b[37]+b[14]*b[32]*b[11]+b[14]*b[33]*b[10]-b[14]*b[5]*b[38]-b[16]*b[6]*b[35]-b[16]*b[7]*b[34]+b[16]*b[30]*b[11]+b[16]*b[31]*b[10]-b[26]*b[19]*b[12]-b[26]*b[20]*b[11]+b[26]*b[6]*b[25]+b[26]*b[7]*b[24]+b[15]*b[32]*b[10]+b[15]*b[33]*b[9]-b[15]*b[4]*b[38]-b[15]*b[5]*b[37]+b[29]*b[5]*b[23]+b[29]*b[6]*b[22]+b[29]*b[7]*b[21]-b[27]*b[20]*b[10]+b[27]*b[5]*b[25]+b[27]*b[6]*b[24]+b[27]*b[7]*b[23]-b[27]*b[18]*b[12]-b[27]*b[19]*b[11]-b[28]*b[20]*b[9]-b[16]*b[4]*b[37]-b[16]*b[5]*b[36]+b[0]*b[19]*b[38]-b[0]*b[24]*b[33]+b[0]*b[20]*b[37]-b[29]*b[17]*b[11]-b[29]*b[18]*b[10]-b[29]*b[19]*b[9]+b[28]*b[4]*b[25]+b[28]*b[5]*b[24]+b[28]*b[6]*b[23]+b[28]*b[7]*b[22]-b[28]*b[17]*b[12]-b[28]*b[18]*b[11]-b[28]*b[19]*b[10]-b[29]*b[20]*b[8]+b[29]*b[4]*b[24]+b[2]*b[18]*b[37]-b[0]*b[25]*b[32]+b[1]*b[18]*b[38]-b[1]*b[24]*b[32]+b[1]*b[19]*b[37]+b[1]*b[20]*b[36]-b[1]*b[25]*b[31]+b[2]*b[17]*b[38]+b[2]*b[19]*b[36]-b[2]*b[24]*b[31]-b[2]*b[22]*b[33]-b[2]*b[23]*b[32]+b[2]*b[20]*b[35]-b[1]*b[23]*b[33]-b[2]*b[25]*b[30]); + c[3] = (-b[14]*b[6]*b[38]-b[14]*b[7]*b[37]+b[3]*b[19]*b[36]-b[3]*b[22]*b[33]+b[3]*b[20]*b[35]-b[3]*b[23]*b[32]-b[3]*b[25]*b[30]+b[3]*b[17]*b[38]+b[3]*b[18]*b[37]-b[3]*b[24]*b[31]-b[15]*b[6]*b[37]-b[15]*b[7]*b[36]+b[15]*b[31]*b[12]+b[16]*b[32]*b[10]+b[16]*b[33]*b[9]+b[13]*b[33]*b[12]-b[13]*b[7]*b[38]+b[14]*b[32]*b[12]+b[14]*b[33]*b[11]-b[16]*b[6]*b[36]-b[16]*b[7]*b[35]+b[16]*b[31]*b[11]+b[16]*b[30]*b[12]+b[15]*b[32]*b[11]+b[15]*b[33]*b[10]-b[15]*b[5]*b[38]+b[29]*b[5]*b[24]+b[29]*b[6]*b[23]-b[26]*b[20]*b[12]+b[26]*b[7]*b[25]-b[27]*b[19]*b[12]-b[27]*b[20]*b[11]+b[27]*b[6]*b[25]+b[27]*b[7]*b[24]-b[28]*b[20]*b[10]-b[16]*b[4]*b[38]-b[16]*b[5]*b[37]+b[29]*b[7]*b[22]-b[29]*b[17]*b[12]-b[29]*b[18]*b[11]-b[29]*b[19]*b[10]+b[28]*b[5]*b[25]+b[28]*b[6]*b[24]+b[28]*b[7]*b[23]-b[28]*b[18]*b[12]-b[28]*b[19]*b[11]-b[29]*b[20]*b[9]+b[29]*b[4]*b[25]-b[2]*b[24]*b[32]+b[0]*b[20]*b[38]-b[0]*b[25]*b[33]+b[1]*b[19]*b[38]-b[1]*b[24]*b[33]+b[1]*b[20]*b[37]-b[2]*b[25]*b[31]+b[2]*b[20]*b[36]-b[1]*b[25]*b[32]+b[2]*b[19]*b[37]+b[2]*b[18]*b[38]-b[2]*b[23]*b[33]); + c[2] = (b[3]*b[18]*b[38]-b[3]*b[24]*b[32]+b[3]*b[19]*b[37]+b[3]*b[20]*b[36]-b[3]*b[25]*b[31]-b[3]*b[23]*b[33]-b[15]*b[6]*b[38]-b[15]*b[7]*b[37]+b[16]*b[32]*b[11]+b[16]*b[33]*b[10]-b[16]*b[5]*b[38]-b[16]*b[6]*b[37]-b[16]*b[7]*b[36]+b[16]*b[31]*b[12]+b[14]*b[33]*b[12]-b[14]*b[7]*b[38]+b[15]*b[32]*b[12]+b[15]*b[33]*b[11]+b[29]*b[5]*b[25]+b[29]*b[6]*b[24]-b[27]*b[20]*b[12]+b[27]*b[7]*b[25]-b[28]*b[19]*b[12]-b[28]*b[20]*b[11]+b[29]*b[7]*b[23]-b[29]*b[18]*b[12]-b[29]*b[19]*b[11]+b[28]*b[6]*b[25]+b[28]*b[7]*b[24]-b[29]*b[20]*b[10]+b[2]*b[19]*b[38]-b[1]*b[25]*b[33]+b[2]*b[20]*b[37]-b[2]*b[24]*b[33]-b[2]*b[25]*b[32]+b[1]*b[20]*b[38]); + c[1] = (b[29]*b[7]*b[24]-b[29]*b[20]*b[11]+b[2]*b[20]*b[38]-b[2]*b[25]*b[33]-b[28]*b[20]*b[12]+b[28]*b[7]*b[25]-b[29]*b[19]*b[12]-b[3]*b[24]*b[33]+b[15]*b[33]*b[12]+b[3]*b[19]*b[38]-b[16]*b[6]*b[38]+b[3]*b[20]*b[37]+b[16]*b[32]*b[12]+b[29]*b[6]*b[25]-b[16]*b[7]*b[37]-b[3]*b[25]*b[32]-b[15]*b[7]*b[38]+b[16]*b[33]*b[11]); + c[0] = -b[29]*b[20]*b[12]+b[29]*b[7]*b[25]+b[16]*b[33]*b[12]-b[16]*b[7]*b[38]+b[3]*b[20]*b[38]-b[3]*b[25]*b[33]; + + const auto poly_solver = SolverPoly::create(); + const int num_roots = poly_solver->getRealRoots(c, rs); + + models = std::vector(); models.reserve(num_roots); + for (int i = 0; i < num_roots; i++) { + const double z1 = rs[i], z2 = z1*z1, z3 = z2*z1, z4 = z3*z1; + double bz[9], norm_bz = 0; + for (int j = 0; j < 3; j++) { + double * const br = b + j * 13, * Bz = bz + 3*j; + Bz[0] = br[0] * z3 + br[1] * z2 + br[2] * z1 + br[3]; + Bz[1] = br[4] * z3 + br[5] * z2 + br[6] * z1 + br[7]; + Bz[2] = br[8] * z4 + br[9] * z3 + br[10] * z2 + br[11] * z1 + br[12]; + norm_bz += Bz[0]*Bz[0] + Bz[1]*Bz[1] + Bz[2]*Bz[2]; + } + + Matx33d Bz(bz); + // Bz is rank 2, matrix, so epipole is its null-vector + Vec3d xy1 = Utils::getRightEpipole(Mat(Bz * (1/sqrt(norm_bz)))); + + if (fabs(xy1(2)) < 1e-10) continue; + Mat_ E(3,3); + double * e_arr = (double *)E.data, x = xy1(0) / xy1(2), y = xy1(1) / xy1(2); + for (int e_i = 0; e_i < 9; e_i++) + e_arr[e_i] = ee[e_i] * x + ee[9+e_i] * y + ee[18+e_i]*z1 + ee[27+e_i]; + models.emplace_back(E); + } + } else { +#if defined(HAVE_EIGEN) || defined(HAVE_LAPACK) + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + // compute EE Transpose + // Shorthand for multiplying the Essential matrix with its transpose. + eet[i][j] = 2 * (multPolysDegOne(null_space_mat[i][0].val, null_space_mat[j][0].val) + + multPolysDegOne(null_space_mat[i][1].val, null_space_mat[j][1].val) + + multPolysDegOne(null_space_mat[i][2].val, null_space_mat[j][2].val)); + + const Matx trace = eet[0][0] + eet[1][1] + eet[2][2]; + // Trace constraint + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + Mat(multPolysDegOneAndTwo(eet[i][0].val, null_space_mat[0][j].val) + + multPolysDegOneAndTwo(eet[i][1].val, null_space_mat[1][j].val) + + multPolysDegOneAndTwo(eet[i][2].val, null_space_mat[2][j].val) - + 0.5 * multPolysDegOneAndTwo(trace.val, null_space_mat[i][j].val)) + .copyTo(constraint_mat.row(3 * i + j)); + + // Rank = zero determinant constraint + Mat(multPolysDegOneAndTwo( + (multPolysDegOne(null_space_mat[0][1].val, null_space_mat[1][2].val) - + multPolysDegOne(null_space_mat[0][2].val, null_space_mat[1][1].val)).val, + null_space_mat[2][0].val) + multPolysDegOneAndTwo( (multPolysDegOne(null_space_mat[0][2].val, null_space_mat[1][0].val) - multPolysDegOne(null_space_mat[0][0].val, null_space_mat[1][2].val)).val, - null_space_mat[2][1].val) + + null_space_mat[2][1].val) + multPolysDegOneAndTwo( (multPolysDegOne(null_space_mat[0][0].val, null_space_mat[1][1].val) - multPolysDegOne(null_space_mat[0][1].val, null_space_mat[1][0].val)).val, - null_space_mat[2][2].val)).copyTo(constraint_mat.row(9)); + null_space_mat[2][2].val)).copyTo(constraint_mat.row(9)); #ifdef HAVE_EIGEN - const Eigen::Matrix constraint_mat_eig((double *) constraint_mat.data); - // (3) Compute the Gröbner basis. This turns out to be as simple as performing a - // Gauss-Jordan elimination on the 10×20 matrix - const Eigen::Matrix eliminated_mat_eig = constraint_mat_eig.block<10, 10>(0, 0) - .fullPivLu().solve(constraint_mat_eig.block<10, 10>(0, 10)); + const Eigen::Matrix constraint_mat_eig((double *) constraint_mat.data); + // (3) Compute the Gröbner basis. This turns out to be as simple as performing a + // Gauss-Jordan elimination on the 10×20 matrix + const Eigen::Matrix eliminated_mat_eig = constraint_mat_eig.block<10, 10>(0, 0) + .fullPivLu().solve(constraint_mat_eig.block<10, 10>(0, 10)); - // (4) Compute the 10×10 action matrix for multiplication by one of the un-knowns. - // This is a simple matter of extracting the correct elements fromthe eliminated - // 10×20 matrix and organising them to form the action matrix. - Eigen::Matrix action_mat_eig = Eigen::Matrix::Zero(); - action_mat_eig.block<3, 10>(0, 0) = eliminated_mat_eig.block<3, 10>(0, 0); - action_mat_eig.block<2, 10>(3, 0) = eliminated_mat_eig.block<2, 10>(4, 0); - action_mat_eig.row(5) = eliminated_mat_eig.row(7); - action_mat_eig(6, 0) = -1.0; - action_mat_eig(7, 1) = -1.0; - action_mat_eig(8, 3) = -1.0; - action_mat_eig(9, 6) = -1.0; + // (4) Compute the 10×10 action matrix for multiplication by one of the unknowns. + // This is a simple matter of extracting the correct elements from the eliminated + // 10×20 matrix and organising them to form the action matrix. + Eigen::Matrix action_mat_eig = Eigen::Matrix::Zero(); + action_mat_eig.block<3, 10>(0, 0) = eliminated_mat_eig.block<3, 10>(0, 0); + action_mat_eig.block<2, 10>(3, 0) = eliminated_mat_eig.block<2, 10>(4, 0); + action_mat_eig.row(5) = eliminated_mat_eig.row(7); + action_mat_eig(6, 0) = -1.0; + action_mat_eig(7, 1) = -1.0; + action_mat_eig(8, 3) = -1.0; + action_mat_eig(9, 6) = -1.0; - // (5) Compute the left eigenvectors of the action matrix - Eigen::EigenSolver> eigensolver(action_mat_eig); - const Eigen::VectorXcd &eigenvalues = eigensolver.eigenvalues(); - const auto * const eig_vecs_ = (double *) eigensolver.eigenvectors().real().data(); + // (5) Compute the left eigenvectors of the action matrix + Eigen::EigenSolver> eigensolver(action_mat_eig); + const Eigen::VectorXcd &eigenvalues = eigensolver.eigenvalues(); + const auto * const eig_vecs_ = (double *) eigensolver.eigenvectors().real().data(); #else - Matx A = constraint_mat.colRange(0, 10), - B = constraint_mat.colRange(10, 20), eliminated_mat; - if (!solve(A, B, eliminated_mat, DECOMP_LU)) return 0; + Matx A = constraint_mat.colRange(0, 10), + B = constraint_mat.colRange(10, 20), eliminated_mat; + if (!solve(A, B, eliminated_mat, DECOMP_LU)) return 0; - Mat eliminated_mat_dyn = Mat(eliminated_mat); - Mat action_mat = Mat_::zeros(10, 10); - eliminated_mat_dyn.rowRange(0,3).copyTo(action_mat.rowRange(0,3)); - eliminated_mat_dyn.rowRange(4,6).copyTo(action_mat.rowRange(3,5)); - eliminated_mat_dyn.row(7).copyTo(action_mat.row(5)); - auto * action_mat_data = (double *) action_mat.data; - action_mat_data[60] = -1.0; // 6 row, 0 col - action_mat_data[71] = -1.0; // 7 row, 1 col - action_mat_data[83] = -1.0; // 8 row, 3 col - action_mat_data[96] = -1.0; // 9 row, 6 col + Mat eliminated_mat_dyn = Mat(eliminated_mat); + Mat action_mat = Mat_::zeros(10, 10); + eliminated_mat_dyn.rowRange(0,3).copyTo(action_mat.rowRange(0,3)); + eliminated_mat_dyn.rowRange(4,6).copyTo(action_mat.rowRange(3,5)); + eliminated_mat_dyn.row(7).copyTo(action_mat.row(5)); + auto * action_mat_data = (double *) action_mat.data; + action_mat_data[60] = -1.0; // 6 row, 0 col + action_mat_data[71] = -1.0; // 7 row, 1 col + action_mat_data[83] = -1.0; // 8 row, 3 col + action_mat_data[96] = -1.0; // 9 row, 6 col - int mat_order = 10, info, lda = 10, ldvl = 10, ldvr = 1, lwork = 100; - double wr[10], wi[10] = {0}, eig_vecs[100], work[100]; // 10 = mat_order, 100 = lwork - char jobvl = 'V', jobvr = 'N'; // only left eigen vectors are computed - OCV_LAPACK_FUNC(dgeev)(&jobvl, &jobvr, &mat_order, action_mat_data, &lda, wr, wi, eig_vecs, &ldvl, - nullptr, &ldvr, work, &lwork, &info); - if (info != 0) return 0; + int mat_order = 10, info, lda = 10, ldvl = 10, ldvr = 1, lwork = 100; + double wr[10], wi[10] = {0}, eig_vecs[100], work[100]; // 10 = mat_order, 100 = lwork + char jobvl = 'V', jobvr = 'N'; // only left eigen vectors are computed + OCV_LAPACK_FUNC(dgeev)(&jobvl, &jobvr, &mat_order, action_mat_data, &lda, wr, wi, eig_vecs, &ldvl, + nullptr, &ldvr, work, &lwork, &info); + if (info != 0) return 0; #endif + models = std::vector(); models.reserve(10); - models = std::vector(); models.reserve(10); - - // Read off the values for the three unknowns at all the solution points and - // back-substitute to obtain the solutions for the essential matrix. - for (int i = 0; i < 10; i++) - // process only real solutions + // Read off the values for the three unknowns at all the solution points and + // back-substitute to obtain the solutions for the essential matrix. + for (int i = 0; i < 10; i++) + // process only real solutions #ifdef HAVE_EIGEN - if (eigenvalues(i).imag() == 0) { - Mat_ model(3, 3); - auto * model_data = (double *) model.data; - const int eig_i = 20 * i + 12; // eigen stores imaginary values too - for (int j = 0; j < 9; j++) - model_data[j] = ee[j ] * eig_vecs_[eig_i ] + ee[j+9 ] * eig_vecs_[eig_i+2] + - ee[j+18] * eig_vecs_[eig_i+4] + ee[j+27] * eig_vecs_[eig_i+6]; + if (eigenvalues(i).imag() == 0) { + Mat_ model(3, 3); + auto * model_data = (double *) model.data; + const int eig_i = 20 * i + 12; // eigen stores imaginary values too + for (int j = 0; j < 9; j++) + model_data[j] = ee[j ] * eig_vecs_[eig_i ] + ee[j+9 ] * eig_vecs_[eig_i+2] + + ee[j+18] * eig_vecs_[eig_i+4] + ee[j+27] * eig_vecs_[eig_i+6]; #else - if (wi[i] == 0) { - Mat_ model (3,3); - auto * model_data = (double *) model.data; - const int eig_i = 10 * i + 6; - for (int j = 0; j < 9; j++) - model_data[j] = ee[j ]*eig_vecs[eig_i ] + ee[j+9 ]*eig_vecs[eig_i+1] + - ee[j+18]*eig_vecs[eig_i+2] + ee[j+27]*eig_vecs[eig_i+3]; + if (wi[i] == 0) { + Mat_ model (3,3); + auto * model_data = (double *) model.data; + const int eig_i = 10 * i + 6; + for (int j = 0; j < 9; j++) + model_data[j] = ee[j ]*eig_vecs[eig_i ] + ee[j+9 ]*eig_vecs[eig_i+1] + + ee[j+18]*eig_vecs[eig_i+2] + ee[j+27]*eig_vecs[eig_i+3]; #endif - models.emplace_back(model); - } + models.emplace_back(model); + } +#else + CV_Error(cv::Error::StsNotImplemented, "To run essential matrix estimation of Stewenius method you need to have either Eigen or LAPACK installed! Or switch to Nister algorithm"); + return 0; +#endif + } return static_cast(models.size()); -#else - int estimate (const std::vector &/*sample*/, std::vector &/*models*/) const override { - CV_Error(cv::Error::StsNotImplemented, "To use essential matrix solver LAPACK or Eigen has to be installed!"); -#endif } // number of possible solutions is 0,2,4,6,8,10 int getMaxNumberOfSolutions () const override { return 10; } int getSampleSize() const override { return 5; } - Ptr clone () const override { - return makePtr(*points_mat); - } private: /* * Multiply two polynomials of degree one with unknowns x y z @@ -236,105 +328,19 @@ private: p[5]*q[3]+p[8]*q[2], p[6]*q[3]+p[9]*q[0], p[7]*q[3]+p[9]*q[1], p[8]*q[3]+p[9]*q[2], p[9]*q[3]}); } -}; -Ptr EssentialMinimalSolverStewenius5pts::create - (const Mat &points_) { - return makePtr(points_); -} - -class EssentialNonMinimalSolverImpl : public EssentialNonMinimalSolver { -private: - const Mat * points_mat; - const float * const points; -public: - /* - * Input calibrated points K^-1 x. - * Linear 8 points algorithm is used for estimation. - */ - explicit EssentialNonMinimalSolverImpl (const Mat &points_) : - points_mat(&points_), points ((float *) points_.data) {} - - int estimate (const std::vector &sample, int sample_size, std::vector - &models, const std::vector &weights) const override { - if (sample_size < getMinimumRequiredSampleSize()) - return 0; - - // ------- 8 points algorithm with Eigen and covariance matrix -------------- - double a[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1}; - double AtA[81] = {0}; // 9x9 - - if (weights.empty()) { - for (int i = 0; i < sample_size; i++) { - const int pidx = 4*sample[i]; - const double x1 = points[pidx ], y1 = points[pidx+1], - x2 = points[pidx+2], y2 = points[pidx+3]; - a[0] = x2*x1; - a[1] = x2*y1; - a[2] = x2; - a[3] = y2*x1; - a[4] = y2*y1; - a[5] = y2; - a[6] = x1; - a[7] = y1; - - // calculate covariance for eigen - for (int row = 0; row < 9; row++) - for (int col = row; col < 9; col++) - AtA[row*9+col] += a[row]*a[col]; - } - } else { - for (int i = 0; i < sample_size; i++) { - const int smpl = 4*sample[i]; - const double weight = weights[i]; - const double x1 = points[smpl ], y1 = points[smpl+1], - x2 = points[smpl+2], y2 = points[smpl+3]; - const double weight_times_x2 = weight * x2, - weight_times_y2 = weight * y2; - - a[0] = weight_times_x2 * x1; - a[1] = weight_times_x2 * y1; - a[2] = weight_times_x2; - a[3] = weight_times_y2 * x1; - a[4] = weight_times_y2 * y1; - a[5] = weight_times_y2; - a[6] = weight * x1; - a[7] = weight * y1; - a[8] = weight; - - // calculate covariance for eigen - for (int row = 0; row < 9; row++) - for (int col = row; col < 9; col++) - AtA[row*9+col] += a[row]*a[col]; - } - } - - // copy symmetric part of covariance matrix - for (int j = 1; j < 9; j++) - for (int z = 0; z < j; z++) - AtA[j*9+z] = AtA[z*9+j]; - -#ifdef HAVE_EIGEN - models = std::vector{ Mat_(3,3) }; - const Eigen::JacobiSVD> svd((Eigen::Matrix(AtA)), - Eigen::ComputeFullV); - // extract the last nullspace - Eigen::Map>((double *)models[0].data) = svd.matrixV().col(8); -#else - Matx AtA_(AtA), U, Vt; - Vec W; - SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A); - models = std::vector { Mat_(3, 3, Vt.val + 72 /*=8*9*/) }; -#endif - FundamentalDegeneracy::recoverRank(models[0], false /*E*/); - return 1; - } - int getMinimumRequiredSampleSize() const override { return 8; } - int getMaxNumberOfSolutions () const override { return 1; } - Ptr clone () const override { - return makePtr(*points_mat); + static inline Matx multPolysDegOneAndTwoNister(const double * const p, + const double * const q) { + // permutation {0, 3, 1, 2, 4, 10, 6, 12, 5, 11, 7, 13, 16, 8, 14, 17, 9, 15, 18, 19}; + return Matx + ({p[0]*q[0], p[2]*q[1], p[0]*q[1]+p[1]*q[0], p[1]*q[1]+p[2]*q[0], p[0]*q[2]+p[3]*q[0], + p[0]*q[3]+p[6]*q[0], p[2]*q[2]+p[4]*q[1], p[2]*q[3]+p[7]*q[1], p[1]*q[2]+p[3]*q[1]+p[4]*q[0], + p[1]*q[3]+p[6]*q[1]+p[7]*q[0], p[3]*q[2]+p[5]*q[0], p[3]*q[3]+p[6]*q[2]+p[8]*q[0], + p[6]*q[3]+p[9]*q[0], p[4]*q[2]+p[5]*q[1], p[4]*q[3]+p[7]*q[2]+p[8]*q[1], p[7]*q[3]+p[9]*q[1], + p[5]*q[2], p[5]*q[3]+p[8]*q[2], p[8]*q[3]+p[9]*q[2], p[9]*q[3]}); } }; -Ptr EssentialNonMinimalSolver::create (const Mat &points_) { - return makePtr(points_); +Ptr EssentialMinimalSolver5pts::create + (const Mat &points_, bool use_svd, bool is_nister) { + return makePtr(points_, use_svd, is_nister); } }} \ No newline at end of file diff --git a/modules/calib3d/src/usac/estimator.cpp b/modules/calib3d/src/usac/estimator.cpp index c5b783b655..f68ebf4cef 100644 --- a/modules/calib3d/src/usac/estimator.cpp +++ b/modules/calib3d/src/usac/estimator.cpp @@ -36,10 +36,7 @@ public: int getNonMinimalSampleSize () const override { return non_min_solver->getMinimumRequiredSampleSize(); } - Ptr clone() const override { - return makePtr(min_solver->clone(), non_min_solver->clone(), - degeneracy->clone(0 /*we don't need state here*/)); - } + void enforceRankConstraint (bool /*enforce*/) override {} }; Ptr HomographyEstimator::create (const Ptr &min_solver_, const Ptr &non_min_solver_, const Ptr °eneracy_) { @@ -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 clone() const override { - return makePtr(min_solver->clone(), non_min_solver->clone(), - degeneracy->clone(0)); - } }; Ptr FundamentalEstimator::create (const Ptr &min_solver_, const Ptr &non_min_solver_, const Ptr °eneracy_) { @@ -106,7 +102,7 @@ public: inline int estimateModels(const std::vector &sample, std::vector &models) const override { - std::vector E; + std::vector 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 &sample, int sample_size, std::vector + &models, const std::vector &weights) const override { + return non_min_solver->estimate(model, sample, sample_size, models, weights); + } int estimateModelNonMinimalSample(const std::vector &sample, int sample_size, std::vector &models, const std::vector &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 clone() const override { - return makePtr(min_solver->clone(), non_min_solver->clone(), - degeneracy->clone(0)); - } }; Ptr EssentialEstimator::create (const Ptr &min_solver_, const Ptr &non_min_solver_, const Ptr °eneracy_) { @@ -170,9 +169,7 @@ public: int getMaxNumSolutionsNonMinimal () const override { return non_min_solver->getMaxNumberOfSolutions(); } - Ptr clone() const override { - return makePtr(min_solver->clone(), non_min_solver->clone()); - } + void enforceRankConstraint (bool /*enforce*/) override {} }; Ptr AffineEstimator::create (const Ptr &min_solver_, const Ptr &non_min_solver_) { @@ -208,9 +205,7 @@ public: int getMaxNumSolutionsNonMinimal () const override { return non_min_solver->getMaxNumberOfSolutions(); } - Ptr clone() const override { - return makePtr(min_solver->clone(), non_min_solver->clone()); - } + void enforceRankConstraint (bool /*enforce*/) override {} }; Ptr PnPEstimator::create (const Ptr &min_solver_, const Ptr &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 clone () const override { - return makePtr(*points_mat); - } }; Ptr ReprojectionErrorSymmetric::create(const Mat &points) { @@ -314,9 +306,9 @@ public: m21=static_cast(m[3]); m22=static_cast(m[4]); m23=static_cast(m[5]); m31=static_cast(m[6]); m32=static_cast(m[7]); m33=static_cast(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 clone () const override { - return makePtr(*points_mat); - } }; Ptr 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 clone () const override { - return makePtr(*points_mat); - } }; Ptr SampsonError::create(const Mat &points) { @@ -440,9 +426,9 @@ public: m31=static_cast(m[6]); m32=static_cast(m[7]); m33=static_cast(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 clone () const override { - return makePtr(*points_mat); - } }; Ptr 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 clone () const override { - return makePtr(*points_mat); - } }; Ptr ReprojectionErrorPmatrix::create(const Mat &points) { return makePtr(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 clone () const override { - return makePtr(*points_mat); - } }; Ptr 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; diff --git a/modules/calib3d/src/usac/fundamental_solver.cpp b/modules/calib3d/src/usac/fundamental_solver.cpp index 00d4feba63..502c3c8c62 100644 --- a/modules/calib3d/src/usac/fundamental_solver.cpp +++ b/modules/calib3d/src/usac/fundamental_solver.cpp @@ -4,19 +4,20 @@ #include "../precomp.hpp" #include "../usac.hpp" +#include "../polynom_solver.h" #ifdef HAVE_EIGEN #include #endif namespace cv { namespace usac { -// Fundamental Matrix Solver: class FundamentalMinimalSolver7ptsImpl: public FundamentalMinimalSolver7pts { private: - const Mat * points_mat; - const float * const points; + const Mat * points_mat; // pointer to OpenCV Mat + const float * const points; // pointer to points_mat->data for faster data access + const bool use_ge; public: - explicit FundamentalMinimalSolver7ptsImpl (const Mat &points_) : - points_mat (&points_), points ((float *) points_.data) {} + explicit FundamentalMinimalSolver7ptsImpl (const Mat &points_, bool use_ge_) : + points_mat (&points_), points ((float *) points_mat->data), use_ge(use_ge_) {} int estimate (const std::vector &sample, std::vector &models) const override { const int m = 7, n = 9; // rows, cols @@ -38,52 +39,57 @@ public: (*a_++) = y1; (*a_++) = 1; } - - if (!Math::eliminateUpperTriangular(a, m, n)) - return 0; - - /* - [a11 a12 a13 a14 a15 a16 a17 a18 a19] - [ 0 a22 a23 a24 a25 a26 a27 a28 a29] - [ 0 0 a33 a34 a35 a36 a37 a38 a39] - [ 0 0 0 a44 a45 a46 a47 a48 a49] - [ 0 0 0 0 a55 a56 a57 a58 a59] - [ 0 0 0 0 0 a66 a67 a68 a69] - [ 0 0 0 0 0 0 a77 a78 a79] - - f9 = 1 - */ double f1[9], f2[9]; - - f1[8] = 1.; - f1[7] = 0.; - f1[6] = -a[6*n+8] / a[6*n+6]; - - f2[8] = 1.; - f2[7] = -a[6*n+8] / a[6*n+7]; - f2[6] = 0.; - - // start from the last row - for (int i = m-2; i >= 0; i--) { - const int row_i = i*n; - double acc1 = 0, acc2 = 0; - for (int j = i+1; j < n; j++) { - acc1 -= a[row_i + j] * f1[j]; - acc2 -= a[row_i + j] * f2[j]; - } - f1[i] = acc1 / a[row_i + i]; - f2[i] = acc2 / a[row_i + i]; - - // due to numerical errors return 0 solutions - if (std::isnan(f1[i]) || std::isnan(f2[i])) + if (use_ge) { + if (!Math::eliminateUpperTriangular(a, m, n)) return 0; + /* + [a11 a12 a13 a14 a15 a16 a17 a18 a19] + [ 0 a22 a23 a24 a25 a26 a27 a28 a29] + [ 0 0 a33 a34 a35 a36 a37 a38 a39] + [ 0 0 0 a44 a45 a46 a47 a48 a49] + [ 0 0 0 0 a55 a56 a57 a58 a59] + [ 0 0 0 0 0 a66 a67 a68 a69] + [ 0 0 0 0 0 0 a77 a78 a79] + */ + + f1[8] = 1.; + f1[7] = 0.; + f1[6] = -a[6*n+8] / a[6*n+6]; + + f2[8] = 0.; + f2[7] = -a[6*n+6] / a[6*n+7]; + f2[6] = 1; + + // start from the last row + for (int i = m-2; i >= 0; i--) { + const int row_i = i*n; + double acc1 = 0, acc2 = 0; + for (int j = i+1; j < n; j++) { + acc1 -= a[row_i + j] * f1[j]; + acc2 -= a[row_i + j] * f2[j]; + } + f1[i] = acc1 / a[row_i + i]; + f2[i] = acc2 / a[row_i + i]; + + if (std::isnan(f1[i]) || std::isnan(f2[i])) + return 0; // due to numerical errors return 0 solutions + } + } else { + Mat U, Vt, D; + cv::Matx A(&a[0]); + SVD::compute(A, D, U, Vt, SVD::FULL_UV+SVD::MODIFY_A); + const auto * const vt = (double *) Vt.data; + int i1 = 8*9, i2 = 7*9; + for (int i = 0; i < 9; i++) { + f1[i] = vt[i1+i]; + f2[i] = vt[i2+i]; + } } // OpenCV: double c[4] = { 0 }, r[3] = { 0 }; double t0 = 0, t1 = 0, t2 = 0; - Mat_ coeffs (1, 4, c); - Mat_ roots (1, 3, r); for (int i = 0; i < 9; i++) f1[i] -= f2[i]; @@ -117,7 +123,7 @@ public: c[0] = f1[0]*t0 - f1[1]*t1 + f1[2]*t2; // solve the cubic equation; there can be 1 to 3 roots ... - int nroots = solveCubic (coeffs, roots); + const int nroots = solve_deg3(c[0], c[1], c[2], c[3], r[0], r[1], r[2]); if (nroots < 1) return 0; models = std::vector(nroots); @@ -145,12 +151,9 @@ public: int getMaxNumberOfSolutions () const override { return 3; } int getSampleSize() const override { return 7; } - Ptr clone () const override { - return makePtr(*points_mat); - } }; -Ptr FundamentalMinimalSolver7pts::create(const Mat &points_) { - return makePtr(points_); +Ptr FundamentalMinimalSolver7pts::create(const Mat &points, bool use_ge) { + return makePtr(points, use_ge); } class FundamentalMinimalSolver8ptsImpl : public FundamentalMinimalSolver8pts { @@ -159,10 +162,8 @@ private: const float * const points; public: explicit FundamentalMinimalSolver8ptsImpl (const Mat &points_) : - points_mat (&points_), points ((float*) points_.data) - { - CV_DbgAssert(points); - } + points_mat (&points_), points ((float*) points_mat->data) + { CV_DbgAssert(points); } int estimate (const std::vector &sample, std::vector &models) const override { const int m = 8, n = 9; // rows, cols @@ -225,22 +226,30 @@ public: int getMaxNumberOfSolutions () const override { return 1; } int getSampleSize() const override { return 8; } - Ptr clone () const override { - return makePtr(*points_mat); - } }; Ptr FundamentalMinimalSolver8pts::create(const Mat &points_) { return makePtr(points_); } -class FundamentalNonMinimalSolverImpl : public FundamentalNonMinimalSolver { +class EpipolarNonMinimalSolverImpl : public EpipolarNonMinimalSolver { private: const Mat * points_mat; - const Ptr normTr; + const bool do_norm; + Matx33d _T1, _T2; + Ptr normTr = nullptr; + bool enforce_rank = true, is_fundamental, use_ge; public: - explicit FundamentalNonMinimalSolverImpl (const Mat &points_) : - points_mat(&points_), normTr (NormTransform::create(points_)) {} - + explicit EpipolarNonMinimalSolverImpl (const Mat &points_, const Matx33d &T1, const Matx33d &T2, bool use_ge_) + : points_mat(&points_), do_norm(false), _T1(T1), _T2(T2), use_ge(use_ge_) { + is_fundamental = true; + } + explicit EpipolarNonMinimalSolverImpl (const Mat &points_, bool is_fundamental_) : + points_mat(&points_), do_norm(is_fundamental_), use_ge(false) { + is_fundamental = is_fundamental_; + if (is_fundamental) + normTr = NormTransform::create(points_); + } + void enforceRankConstraint (bool enforce) override { enforce_rank = enforce; } int estimate (const std::vector &sample, int sample_size, std::vector &models, const std::vector &weights) const override { if (sample_size < getMinimumRequiredSampleSize()) @@ -248,18 +257,210 @@ public: Matx33d T1, T2; Mat norm_points; - normTr->getNormTransformation(norm_points, sample, sample_size, T1, T2); - const auto * const norm_pts = (float *) norm_points.data; + if (do_norm) + normTr->getNormTransformation(norm_points, sample, sample_size, T1, T2); + const auto * const norm_pts = do_norm ? (float *) norm_points.data : (float *) points_mat->data; - // ------- 8 points algorithm with Eigen and covariance matrix -------------- + if (use_ge) { + double a[8]; + std::vector AtAb(72, 0); // 8x9 + if (weights.empty()) { + for (int i = 0; i < sample_size; i++) { + const int idx = do_norm ? 4*i : 4*sample[i]; + const double x1 = norm_pts[idx], y1 = norm_pts[idx+1], x2 = norm_pts[idx+2], y2 = norm_pts[idx+3]; + a[0] = x2*x1; + a[1] = x2*y1; + a[2] = x2; + a[3] = y2*x1; + a[4] = y2*y1; + a[5] = y2; + a[6] = x1; + a[7] = y1; + // calculate covariance for eigen + for (int row = 0; row < 8; row++) { + for (int col = row; col < 8; col++) + AtAb[row * 9 + col] += a[row] * a[col]; + AtAb[row * 9 + 8] += a[row]; + } + } + } else { // use weights + for (int i = 0; i < sample_size; i++) { + const auto weight = weights[i]; + if (weight < FLT_EPSILON) continue; + const int idx = do_norm ? 4*i : 4*sample[i]; + const double x1 = norm_pts[idx], y1 = norm_pts[idx+1], x2 = norm_pts[idx+2], y2 = norm_pts[idx+3]; + const double weight_times_x2 = weight * x2, weight_times_y2 = weight * y2; + a[0] = weight_times_x2 * x1; + a[1] = weight_times_x2 * y1; + a[2] = weight_times_x2; + a[3] = weight_times_y2 * x1; + a[4] = weight_times_y2 * y1; + a[5] = weight_times_y2; + a[6] = weight * x1; + a[7] = weight * y1; + // calculate covariance for eigen + for (int row = 0; row < 8; row++) { + for (int col = row; col < 8; col++) + AtAb[row * 9 + col] += a[row] * a[col]; + AtAb[row * 9 + 8] += a[row]; + } + } + } + // copy symmetric part of covariance matrix + for (int j = 1; j < 8; j++) + for (int z = 0; z < j; z++) + AtAb[j*9+z] = AtAb[z*9+j]; + Math::eliminateUpperTriangular(AtAb, 8, 9); + models = std::vector{ Mat_(3,3) }; + auto * f = (double *) models[0].data; + f[8] = 1.; + const int m = 8, n = 9; + // start from the last row + for (int i = m-1; i >= 0; i--) { + double acc = 0; + for (int j = i+1; j < n; j++) + acc -= AtAb[i*n+j]*f[j]; + f[i] = acc / AtAb[i*n+i]; + // due to numerical errors return 0 solutions + if (std::isnan(f[i])) + return 0; + } + } else { + // ------- 8 points algorithm with Eigen and covariance matrix -------------- + double a[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1}, AtA[81] = {0}; // 9x9 + if (weights.empty()) { + for (int i = 0; i < sample_size; i++) { + const int idx = do_norm ? 4*i : 4*sample[i]; + const auto x1 = norm_pts[idx], y1 = norm_pts[idx+1], x2 = norm_pts[idx+2], y2 = norm_pts[idx+3]; + a[0] = x2*x1; + a[1] = x2*y1; + a[2] = x2; + a[3] = y2*x1; + a[4] = y2*y1; + a[5] = y2; + a[6] = x1; + a[7] = y1; + // calculate covariance matrix + for (int row = 0; row < 9; row++) + for (int col = row; col < 9; col++) + AtA[row*9+col] += a[row]*a[col]; + } + } else { // use weights + for (int i = 0; i < sample_size; i++) { + const auto weight = weights[i]; + if (weight < FLT_EPSILON) continue; + const int smpl = do_norm ? 4*i : 4*sample[i]; + const auto x1 = norm_pts[smpl], y1 = norm_pts[smpl+1], x2 = norm_pts[smpl+2], y2 = norm_pts[smpl+3]; + const double weight_times_x2 = weight * x2, weight_times_y2 = weight * y2; + a[0] = weight_times_x2 * x1; + a[1] = weight_times_x2 * y1; + a[2] = weight_times_x2; + a[3] = weight_times_y2 * x1; + a[4] = weight_times_y2 * y1; + a[5] = weight_times_y2; + a[6] = weight * x1; + a[7] = weight * y1; + a[8] = weight; + for (int row = 0; row < 9; row++) + for (int col = row; col < 9; col++) + AtA[row*9+col] += a[row]*a[col]; + } + } + for (int j = 1; j < 9; j++) + for (int z = 0; z < j; z++) + AtA[j*9+z] = AtA[z*9+j]; +#ifdef HAVE_EIGEN + models = std::vector{ Mat_(3,3) }; + // extract the last null-vector + Eigen::Map>((double *)models[0].data) = Eigen::JacobiSVD + > ((Eigen::Matrix(AtA)), + Eigen::ComputeFullV).matrixV().col(8); +#else + Matx AtA_(AtA), U, Vt; + Vec W; + SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A); + models = std::vector { Mat_(3, 3, Vt.val + 72 /*=8*9*/) }; +#endif + } + + if (enforce_rank) + FundamentalDegeneracy::recoverRank(models[0], is_fundamental); + if (is_fundamental) { + const auto * const f = (double *) models[0].data; + const auto * const t1 = do_norm ? T1.val : _T1.val, * t2 = do_norm ? T2.val : _T2.val; + // F = T2^T F T1 + models[0] = Mat(Matx33d(t1[0]*t2[0]*f[0],t1[0]*t2[0]*f[1], t2[0]*f[2] + t2[0]*f[0]*t1[2] + + t2[0]*f[1]*t1[5], t1[0]*t2[0]*f[3],t1[0]*t2[0]*f[4], t2[0]*f[5] + t2[0]*f[3]*t1[2] + + t2[0]*f[4]*t1[5], t1[0]*(f[6] + f[0]*t2[2] + f[3]*t2[5]), t1[0]*(f[7] + f[1]*t2[2] + + f[4]*t2[5]), f[8] + t1[2]*(f[6] + f[0]*t2[2] + f[3]*t2[5]) + t1[5]*(f[7] + f[1]*t2[2] + + f[4]*t2[5]) + f[2]*t2[2] + f[5]*t2[5])); + } + return 1; + } + int estimate (const std::vector &/*mask*/, std::vector &/*models*/, + const std::vector &/*weights*/) override { + return 0; + } + int getMinimumRequiredSampleSize() const override { return 8; } + int getMaxNumberOfSolutions () const override { return 1; } +}; +Ptr EpipolarNonMinimalSolver::create(const Mat &points_, bool is_fundamental) { + return makePtr(points_, is_fundamental); +} +Ptr EpipolarNonMinimalSolver::create(const Mat &points_, const Matx33d &T1, const Matx33d &T2, bool use_ge) { + return makePtr(points_, T1, T2, use_ge); +} + +class CovarianceEpipolarSolverImpl : public CovarianceEpipolarSolver { +private: + Mat norm_pts; + Matx33d T1, T2; + float * norm_points; + std::vector mask; + int points_size; + double covariance[81] = {0}, * t1, * t2; + bool is_fundamental, enforce_rank = true; +public: + explicit CovarianceEpipolarSolverImpl (const Mat &norm_points_, const Matx33d &T1_, const Matx33d &T2_) + : norm_pts(norm_points_), T1(T1_), T2(T2_) { + points_size = norm_points_.rows; + norm_points = (float *) norm_pts.data; + t1 = T1.val; t2 = T2.val; + mask = std::vector(points_size, false); + is_fundamental = true; + } + explicit CovarianceEpipolarSolverImpl (const Mat &points_, bool is_fundamental_) { + points_size = points_.rows; + is_fundamental = is_fundamental_; + if (is_fundamental) { // normalize image points only for fundmantal matrix + std::vector sample(points_size); + for (int i = 0; i < points_size; i++) sample[i] = i; + const Ptr normTr = NormTransform::create(points_); + normTr->getNormTransformation(norm_pts, sample, points_size, T1, T2); + t1 = T1.val; t2 = T2.val; + } else norm_pts = points_; // otherwise points are normalized by intrinsics + norm_points = (float *)norm_pts.data; + mask = std::vector(points_size, false); + } + void enforceRankConstraint (bool enforce_) override { enforce_rank = enforce_; } + + void reset () override { + std::fill(covariance, covariance+81, 0); + std::fill(mask.begin(), mask.end(), false); + } + /* + * Find fundamental matrix using 8-point algorithm with covariance matrix and PCA + */ + int estimate (const std::vector &new_mask, std::vector &models, + const std::vector &/*weights*/) override { double a[9] = {0, 0, 0, 0, 0, 0, 0, 0, 1}; - double AtA[81] = {0}; // 9x9 - if (weights.empty()) { - for (int i = 0; i < sample_size; i++) { - const int norm_points_idx = 4*i; - const double x1 = norm_pts[norm_points_idx ], y1 = norm_pts[norm_points_idx+1], - x2 = norm_pts[norm_points_idx+2], y2 = norm_pts[norm_points_idx+3]; + for (int i = 0; i < points_size; i++) { + if (mask[i] != new_mask[i]) { + const int smpl = 4*i; + const double x1 = norm_points[smpl ], y1 = norm_points[smpl+1], + x2 = norm_points[smpl+2], y2 = norm_points[smpl+3]; + a[0] = x2*x1; a[1] = x2*y1; a[2] = x2; @@ -269,71 +470,129 @@ public: a[6] = x1; a[7] = y1; - // calculate covariance for eigen - for (int row = 0; row < 9; row++) - for (int col = row; col < 9; col++) - AtA[row*9+col] += a[row]*a[col]; - } - } else { - for (int i = 0; i < sample_size; i++) { - const int smpl = 4*i; - const double weight = weights[i]; - const double x1 = norm_pts[smpl ], y1 = norm_pts[smpl+1], - x2 = norm_pts[smpl+2], y2 = norm_pts[smpl+3]; - const double weight_times_x2 = weight * x2, - weight_times_y2 = weight * y2; - - a[0] = weight_times_x2 * x1; - a[1] = weight_times_x2 * y1; - a[2] = weight_times_x2; - a[3] = weight_times_y2 * x1; - a[4] = weight_times_y2 * y1; - a[5] = weight_times_y2; - a[6] = weight * x1; - a[7] = weight * y1; - a[8] = weight; - - // calculate covariance for eigen - for (int row = 0; row < 9; row++) - for (int col = row; col < 9; col++) - AtA[row*9+col] += a[row]*a[col]; + if (mask[i]) // if mask[i] is true then new_mask[i] must be false + for (int j = 0; j < 9; j++) + for (int z = j; z < 9; z++) + covariance[j*9+z] -= a[j]*a[z]; + else + for (int j = 0; j < 9; j++) + for (int z = j; z < 9; z++) + covariance[j*9+z] += a[j]*a[z]; } } + mask = new_mask; // copy symmetric part of covariance matrix for (int j = 1; j < 9; j++) for (int z = 0; z < j; z++) - AtA[j*9+z] = AtA[z*9+j]; + covariance[j*9+z] = covariance[z*9+j]; #ifdef HAVE_EIGEN models = std::vector{ Mat_(3,3) }; - const Eigen::JacobiSVD> svd((Eigen::Matrix(AtA)), - Eigen::ComputeFullV); - // extract the last nullspace - Eigen::Map>((double *)models[0].data) = svd.matrixV().col(8); + // extract the last null-vector + Eigen::Map>((double *)models[0].data) = Eigen::JacobiSVD + > ((Eigen::Matrix(covariance)), + Eigen::ComputeFullV).matrixV().col(8); #else - Matx AtA_(AtA), U, Vt; - Vec W; - SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A); - models = std::vector { Mat_(3, 3, Vt.val + 72 /*=8*9*/) }; + Matx AtA_(covariance), U, Vt; + Vec W; + SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A); + models = std::vector { Mat_(3, 3, Vt.val + 72 /*=8*9*/) }; #endif - FundamentalDegeneracy::recoverRank(models[0], true/*F*/); + if (enforce_rank) + FundamentalDegeneracy::recoverRank(models[0], is_fundamental); + if (is_fundamental) { + const auto * const f = (double *) models[0].data; + // F = T2^T F T1 + models[0] = Mat(Matx33d(t1[0]*t2[0]*f[0],t1[0]*t2[0]*f[1], t2[0]*f[2] + t2[0]*f[0]*t1[2] + + t2[0]*f[1]*t1[5], t1[0]*t2[0]*f[3],t1[0]*t2[0]*f[4], t2[0]*f[5] + t2[0]*f[3]*t1[2] + + t2[0]*f[4]*t1[5], t1[0]*(f[6] + f[0]*t2[2] + f[3]*t2[5]), t1[0]*(f[7] + f[1]*t2[2] + + f[4]*t2[5]), f[8] + t1[2]*(f[6] + f[0]*t2[2] + f[3]*t2[5]) + t1[5]*(f[7] + f[1]*t2[2] + + f[4]*t2[5]) + f[2]*t2[2] + f[5]*t2[5])); + } + return 1; + } + int getMinimumRequiredSampleSize() const override { return 8; } + int getMaxNumberOfSolutions () const override { return 1; } +}; +Ptr CovarianceEpipolarSolver::create (const Mat &points, bool is_fundamental) { + return makePtr(points, is_fundamental); +} +Ptr CovarianceEpipolarSolver::create (const Mat &points, const Matx33d &T1, const Matx33d &T2) { + return makePtr(points, T1, T2); +} - // Transpose T2 (in T2 the lower diagonal is zero) - T2(2, 0) = T2(0, 2); T2(2, 1) = T2(1, 2); - T2(0, 2) = 0; T2(1, 2) = 0; +class LarssonOptimizerImpl : public LarssonOptimizer { +private: + const Mat &calib_points; + Matx33d K1, K2, K2_t, K1_inv, K2_inv_t; + bool is_fundamental; + BundleOptions opt; +public: + LarssonOptimizerImpl (const Mat &calib_points_, const Matx33d &K1_, const Matx33d &K2_, int max_iters_, bool is_fundamental_) : + calib_points(calib_points_), K1(K1_), K2(K2_){ + is_fundamental = is_fundamental_; + opt.max_iterations = max_iters_; + opt.loss_scale = Utils::getCalibratedThreshold(std::max(1.5, opt.loss_scale), Mat(K1), Mat(K2)); + if (is_fundamental) { + K1_inv = K1.inv(); + K2_t = K2.t(); + K2_inv_t = K2_t.inv(); + } + } - models[0] = T2 * models[0] * T1; + int estimate (const Mat &model, const std::vector &sample, int sample_size, std::vector + &models, const std::vector &weights) const override { + if (sample_size < 5) return 0; + const Matx33d E = is_fundamental ? K2_t * Matx33d(model) * K1 : model; + RNG rng (sample_size); + cv::Matx33d R1, R2; cv::Vec3d t; + cv::decomposeEssentialMat(E, R1, R2, t); + int positive_depth[4] = {0}; + const auto * const pts_ = (float *) calib_points.data; + // a few point are enough to test + // actually due to Sampson error minimization, the input R,t do not really matter + // for a correct pair there is a sligthly faster convergence + for (int i = 0; i < 3; i++) { // could be 1 point + const int rand_inl = 4 * sample[rng.uniform(0, sample_size)]; + Vec3d p1 (pts_[rand_inl], pts_[rand_inl+1], 1), p2(pts_[rand_inl+2], pts_[rand_inl+3], 1); + p1 /= norm(p1); p2 /= norm(p2); + if (satisfyCheirality(R1, t, p1, p2)) positive_depth[0]++; + if (satisfyCheirality(R1, -t, p1, p2)) positive_depth[1]++; + if (satisfyCheirality(R2, t, p1, p2)) positive_depth[2]++; + if (satisfyCheirality(R2, -t, p1, p2)) positive_depth[3]++; + } + int corr_idx = 0, max_good_pts = positive_depth[0]; + for (int i = 1; i < 4; i++) { + if (max_good_pts < positive_depth[i]) { + max_good_pts = positive_depth[i]; + corr_idx = i; + } + } + + CameraPose pose; + pose.R = corr_idx < 2 ? R1 : R2; + pose.t = corr_idx % 2 == 1 ? -t : t; + refine_relpose(calib_points, sample, sample_size, &pose, opt, &weights[0]); + Matx33d model_new = Math::getSkewSymmetric(pose.t) * pose.R; + if (is_fundamental) + model_new = K2_inv_t * model_new * K1_inv; + models = std::vector { Mat(model_new) }; return 1; } - int getMinimumRequiredSampleSize() const override { return 8; } - int getMaxNumberOfSolutions () const override { return 1; } - Ptr clone () const override { - return makePtr(*points_mat); + int estimate (const std::vector&, int, std::vector&, const std::vector&) const override { + return 0; } + int estimate (const std::vector &/*mask*/, std::vector &/*models*/, + const std::vector &/*weights*/) override { + return 0; + } + void enforceRankConstraint (bool /*enforce*/) override {} + int getMinimumRequiredSampleSize() const override { return 5; } + int getMaxNumberOfSolutions () const override { return 1; } }; -Ptr FundamentalNonMinimalSolver::create(const Mat &points_) { - return makePtr(points_); +Ptr LarssonOptimizer::create(const Mat &calib_points_, const Matx33d &K1, const Matx33d &K2, int max_iters_, bool is_fundamental) { + return makePtr(calib_points_, K1, K2, max_iters_, is_fundamental); } }} diff --git a/modules/calib3d/src/usac/gamma_values.cpp b/modules/calib3d/src/usac/gamma_values.cpp index 1e82d8eba7..fa1c03e8ed 100644 --- a/modules/calib3d/src/usac/gamma_values.cpp +++ b/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 gamma_complete_anchor = std::vector - {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 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 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 gamma_incomplete_anchor = std::vector - {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 + {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 gamma_anchor = std::vector - {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 + {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(max_size_table); - gamma_incomplete = std::vector(max_size_table); - gamma = std::vector(max_size_table); + gamma_anchor = std::vector + {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 + {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 + {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 + {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(max_size_table); + gamma_incomplete = std::vector(max_size_table); + gamma = std::vector(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; - } - } - 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]); + 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]); + } } + + const std::vector &getCompleteGammaValues() const override { return gamma_complete; } + const std::vector &getIncompleteGammaValues() const override { return gamma_incomplete; } + const std::vector &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::create(int DoF, int max_size_table) { + return makePtr(DoF, max_size_table); } - -const std::vector& GammaValues::getCompleteGammaValues() const { return gamma_complete; } -const std::vector& GammaValues::getIncompleteGammaValues() const { return gamma_incomplete; } -const std::vector& 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; -} - }} // namespace diff --git a/modules/calib3d/src/usac/homography_solver.cpp b/modules/calib3d/src/usac/homography_solver.cpp index b60e39781a..ba5e233d22 100644 --- a/modules/calib3d/src/usac/homography_solver.cpp +++ b/modules/calib3d/src/usac/homography_solver.cpp @@ -9,13 +9,14 @@ #endif namespace cv { namespace usac { -class HomographyMinimalSolver4ptsGEMImpl : public HomographyMinimalSolver4ptsGEM { +class HomographyMinimalSolver4ptsImpl : public HomographyMinimalSolver4pts { private: const Mat * points_mat; const float * const points; + const bool use_ge; public: - explicit HomographyMinimalSolver4ptsGEMImpl (const Mat &points_) : - points_mat(&points_), points ((float*) points_.data) {} + explicit HomographyMinimalSolver4ptsImpl (const Mat &points_, bool use_ge_) : + points_mat(&points_), points ((float*) points_mat->data), use_ge(use_ge_) {} int estimate (const std::vector& sample, std::vector &models) const override { int m = 8, n = 9; @@ -23,7 +24,7 @@ public: int cnt = 0; for (int i = 0; i < 4; i++) { const int smpl = 4*sample[i]; - const double x1 = points[smpl], y1 = points[smpl+1], x2 = points[smpl+2], y2 = points[smpl+3]; + const auto x1 = points[smpl], y1 = points[smpl+1], x2 = points[smpl+2], y2 = points[smpl+3]; A[cnt++] = -x1; A[cnt++] = -y1; @@ -42,49 +43,53 @@ public: A[cnt++] = y2; } - if (!Math::eliminateUpperTriangular(A, m, n)) - return 0; - - models = std::vector{ Mat_(3,3) }; - auto * h = (double *) models[0].data; - h[8] = 1.; - - // start from the last row - for (int i = m-1; i >= 0; i--) { - double acc = 0; - for (int j = i+1; j < n; j++) - acc -= A[i*n+j]*h[j]; - - h[i] = acc / A[i*n+i]; - // due to numerical errors return 0 solutions - if (std::isnan(h[i])) + if (use_ge) { + if (!Math::eliminateUpperTriangular(A, m, n)) return 0; + + models = std::vector{ Mat_(3,3) }; + auto * h = (double *) models[0].data; + h[8] = 1.; + + // start from the last row + for (int i = m-1; i >= 0; i--) { + double acc = 0; + for (int j = i+1; j < n; j++) + acc -= A[i*n+j]*h[j]; + + h[i] = acc / A[i*n+i]; + // due to numerical errors return 0 solutions + if (std::isnan(h[i])) + return 0; + } + } else { + Mat U, Vt, D; + cv::Matx A_svd(&A[0]); + SVD::compute(A_svd, D, U, Vt, SVD::FULL_UV+SVD::MODIFY_A); + models = std::vector { Vt.row(Vt.rows-1).reshape(0, 3) }; } return 1; } int getMaxNumberOfSolutions () const override { return 1; } int getSampleSize() const override { return 4; } - Ptr clone () const override { - return makePtr(*points_mat); - } }; -Ptr HomographyMinimalSolver4ptsGEM::create(const Mat &points_) { - return makePtr(points_); +Ptr HomographyMinimalSolver4pts::create(const Mat &points, bool use_ge) { + return makePtr(points, use_ge); } class HomographyNonMinimalSolverImpl : public HomographyNonMinimalSolver { private: const Mat * points_mat; - const Ptr normTr; + const bool do_norm, use_ge; + Ptr normTr; + Matx33d _T1, _T2; public: - explicit HomographyNonMinimalSolverImpl (const Mat &points_) : - points_mat(&points_), normTr (NormTransform::create(points_)) {} + explicit HomographyNonMinimalSolverImpl (const Mat &norm_points_, const Matx33d &T1, const Matx33d &T2, bool use_ge_) : + points_mat(&norm_points_), do_norm(false), use_ge(use_ge_), _T1(T1), _T2(T2) {} + explicit HomographyNonMinimalSolverImpl (const Mat &points_, bool use_ge_) : + points_mat(&points_), do_norm(true), use_ge(use_ge_), normTr (NormTransform::create(points_)) {} - /* - * Find Homography matrix using (weighted) non-minimal estimation. - * Use Principal Component Analysis. Use normalized points. - */ int estimate (const std::vector &sample, int sample_size, std::vector &models, const std::vector &weights) const override { if (sample_size < getMinimumRequiredSampleSize()) @@ -92,21 +97,226 @@ public: Matx33d T1, T2; Mat norm_points_; - normTr->getNormTransformation(norm_points_, sample, sample_size, T1, T2); + if (do_norm) + normTr->getNormTransformation(norm_points_, sample, sample_size, T1, T2); + const auto * const npts = do_norm ? (float *) norm_points_.data : (float *) points_mat->data; - /* - * @norm_points is matrix 4 x inlier_size - * @weights is vector of inliers_size - * weights[i] is weight of i-th inlier - */ - const auto * const norm_points = (float *) norm_points_.data; + Mat H; + if (use_ge) { + double a1[8] = {0, 0, -1, 0, 0, 0, 0, 0}, + a2[8] = {0, 0, 0, 0, 0, -1, 0, 0}; + std::vector AtAb(72, 0); // 8x9 + if (weights.empty()) { + for (int i = 0; i < sample_size; i++) { + const int idx = do_norm ? 4*i : 4*sample[i]; + const double x1 = npts[idx], y1 = npts[idx+1], x2 = npts[idx+2], y2 = npts[idx+3]; + a1[0] = -x1; + a1[1] = -y1; + a1[6] = x2*x1; + a1[7] = x2*y1; + a2[3] = -x1; + a2[4] = -y1; + a2[6] = y2*x1; + a2[7] = y2*y1; + + // calculate covariance for eigen + for (int j = 0; j < 8; j++) { + for (int z = j; z < 8; z++) + AtAb[j * 9 + z] += a1[j]*a1[z] + a2[j]*a2[z]; + AtAb[j * 9 + 8] += a1[j]*x2 + a2[j]*y2; + } + } + } else { // use weights + for (int i = 0; i < sample_size; i++) { + const double weight = weights[i]; + if (weight < FLT_EPSILON) continue; + const int idx = do_norm ? 4*i : 4*sample[i]; + const double x1 = npts[idx], y1 = npts[idx+1], x2 = npts[idx+2], y2 = npts[idx+3]; + const double minus_weight_times_x1 = -weight * x1, + minus_weight_times_y1 = -weight * y1, + weight_times_x2 = weight * x2, + weight_times_y2 = weight * y2; + + a1[0] = minus_weight_times_x1; + a1[1] = minus_weight_times_y1; + a1[2] = -weight; + a1[6] = weight_times_x2 * x1; + a1[7] = weight_times_x2 * y1; + + a2[3] = minus_weight_times_x1; + a2[4] = minus_weight_times_y1; + a2[5] = -weight; + a2[6] = weight_times_y2 * x1; + a2[7] = weight_times_y2 * y1; + + for (int j = 0; j < 8; j++) { + for (int z = j; z < 8; z++) + AtAb[j * 9 + z] += a1[j]*a1[z] + a2[j]*a2[z]; + AtAb[j * 9 + 8] += a1[j]*weight_times_x2 + a2[j]*weight_times_y2; + } + } + } + for (int j = 1; j < 8; j++) + for (int z = 0; z < j; z++) + AtAb[j*9+z] = AtAb[z*9+j]; + if (!Math::eliminateUpperTriangular(AtAb, 8, 9)) + return 0; + H = Mat_(3,3); + auto * h = (double *) H.data; + h[8] = 1.; + const int m = 8, n = 9; + // start from the last row + for (int i = m-1; i >= 0; i--) { + double acc = 0; + for (int j = i+1; j < n; j++) + acc -= AtAb[i*n+j]*h[j]; + h[i] = acc / AtAb[i*n+i]; + if (std::isnan(h[i])) + return 0; // numerical imprecision + } + } else { + double a1[9] = {0, 0, -1, 0, 0, 0, 0, 0, 0}, + a2[9] = {0, 0, 0, 0, 0, -1, 0, 0, 0}, AtA[81] = {0}; + if (weights.empty()) { + for (int i = 0; i < sample_size; i++) { + const int smpl = do_norm ? 4*i : 4*sample[i]; + const auto x1 = npts[smpl], y1 = npts[smpl+1], x2 = npts[smpl+2], y2 = npts[smpl+3]; + + a1[0] = -x1; + a1[1] = -y1; + a1[6] = x2*x1; + a1[7] = x2*y1; + a1[8] = x2; + + a2[3] = -x1; + a2[4] = -y1; + a2[6] = y2*x1; + a2[7] = y2*y1; + a2[8] = y2; + + for (int j = 0; j < 9; j++) + for (int z = j; z < 9; z++) + AtA[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z]; + } + } else { // use weights + for (int i = 0; i < sample_size; i++) { + const double weight = weights[i]; + if (weight < FLT_EPSILON) continue; + const int smpl = do_norm ? 4*i : 4*sample[i]; + const auto x1 = npts[smpl], y1 = npts[smpl+1], x2 = npts[smpl+2], y2 = npts[smpl+3]; + const double minus_weight_times_x1 = -weight * x1, + minus_weight_times_y1 = -weight * y1, + weight_times_x2 = weight * x2, + weight_times_y2 = weight * y2; + + a1[0] = minus_weight_times_x1; + a1[1] = minus_weight_times_y1; + a1[2] = -weight; + a1[6] = weight_times_x2 * x1; + a1[7] = weight_times_x2 * y1; + a1[8] = weight_times_x2; + + a2[3] = minus_weight_times_x1; + a2[4] = minus_weight_times_y1; + a2[5] = -weight; + a2[6] = weight_times_y2 * x1; + a2[7] = weight_times_y2 * y1; + a2[8] = weight_times_y2; + + for (int j = 0; j < 9; j++) + for (int z = j; z < 9; z++) + AtA[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z]; + } + } + // copy symmetric part of covariance matrix + for (int j = 1; j < 9; j++) + for (int z = 0; z < j; z++) + AtA[j*9+z] = AtA[z*9+j]; + +#ifdef HAVE_EIGEN + H = Mat_(3,3); + // extract the last null-vector + Eigen::Map>((double *)H.data) = Eigen::Matrix + (Eigen::HouseholderQR> ( + (Eigen::Matrix (AtA))).householderQ()).col(8); +#else + Matx Vt; + Vec D; + if (! eigen(Matx(AtA), D, Vt)) return 0; + H = Mat_(3, 3, Vt.val + 72/*=8*9*/); +#endif + } + const auto * const h = (double *) H.data; + const auto * const t1 = do_norm ? T1.val : _T1.val, * const t2 = do_norm ? T2.val : _T2.val; + // H = T2^-1 H T1 + models = std::vector{ Mat(Matx33d(t1[0]*(h[0]/t2[0] - (h[6]*t2[2])/t2[0]), + t1[0]*(h[1]/t2[0] - (h[7]*t2[2])/t2[0]), h[2]/t2[0] + t1[2]*(h[0]/t2[0] - + (h[6]*t2[2])/t2[0]) + t1[5]*(h[1]/t2[0] - (h[7]*t2[2])/t2[0]) - (h[8]*t2[2])/t2[0], + t1[0]*(h[3]/t2[0] - (h[6]*t2[5])/t2[0]), t1[0]*(h[4]/t2[0] - (h[7]*t2[5])/t2[0]), + h[5]/t2[0] + t1[2]*(h[3]/t2[0] - (h[6]*t2[5])/t2[0]) + t1[5]*(h[4]/t2[0] - + (h[7]*t2[5])/t2[0]) - (h[8]*t2[5])/t2[0], t1[0]*h[6], t1[0]*h[7], + h[8] + h[6]*t1[2] + h[7]*t1[5])) }; + return 1; + } + int estimate (const std::vector &/*mask*/, std::vector &/*models*/, + const std::vector &/*weights*/) override { + return 0; + } + int getMinimumRequiredSampleSize() const override { return 4; } + int getMaxNumberOfSolutions () const override { return 1; } + void enforceRankConstraint (bool /*enforce*/) override {} +}; +Ptr HomographyNonMinimalSolver::create(const Mat &points_, bool use_ge_) { + return makePtr(points_, use_ge_); +} +Ptr HomographyNonMinimalSolver::create(const Mat &points_, const Matx33d &T1, const Matx33d &T2, bool use_ge) { + return makePtr(points_, T1, T2, use_ge); +} + +class CovarianceHomographySolverImpl : public CovarianceHomographySolver { +private: + Mat norm_pts; + Matx33d T1, T2; + float * norm_points; + std::vector mask; + int points_size; + double covariance[81] = {0}, * t1, * t2; +public: + explicit CovarianceHomographySolverImpl (const Mat &norm_points_, const Matx33d &T1_, const Matx33d &T2_) + : norm_pts(norm_points_), T1(T1_), T2(T2_) { + points_size = norm_points_.rows; + norm_points = (float *) norm_pts.data; + t1 = T1.val; t2 = T2.val; + mask = std::vector(points_size, false); + } + explicit CovarianceHomographySolverImpl (const Mat &points_) { + points_size = points_.rows; + // normalize points + std::vector sample(points_size); + for (int i = 0; i < points_size; i++) sample[i] = i; + const Ptr normTr = NormTransform::create(points_); + normTr->getNormTransformation(norm_pts, sample, points_size, T1, T2); + norm_points = (float *) norm_pts.data; + t1 = T1.val; t2 = T2.val; + mask = std::vector(points_size, false); + } + void reset () override { + // reset covariance matrix to zero and mask to false + std::fill(covariance, covariance+81, 0); + std::fill(mask.begin(), mask.end(), false); + } + + /* + * Find homography using 4-point algorithm with covariance matrix and PCA + */ + int estimate (const std::vector &new_mask, std::vector &models, + const std::vector &/*weights*/) override { double a1[9] = {0, 0, -1, 0, 0, 0, 0, 0, 0}, - a2[9] = {0, 0, 0, 0, 0, -1, 0, 0, 0}, - AtA[81] = {0}; + a2[9] = {0, 0, 0, 0, 0, -1, 0, 0, 0}; - if (weights.empty()) { - for (int i = 0; i < sample_size; i++) { + for (int i = 0; i < points_size; i++) { + if (mask[i] != new_mask[i]) { const int smpl = 4*i; const double x1 = norm_points[smpl ], y1 = norm_points[smpl+1], x2 = norm_points[smpl+2], y2 = norm_points[smpl+3]; @@ -123,71 +333,57 @@ public: a2[7] = y2*y1; a2[8] = y2; - for (int j = 0; j < 9; j++) - for (int z = j; z < 9; z++) - AtA[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z]; - } - } else { - for (int i = 0; i < sample_size; i++) { - const int smpl = 4*i; - const double weight = weights[i]; - const double x1 = norm_points[smpl ], y1 = norm_points[smpl+1], - x2 = norm_points[smpl+2], y2 = norm_points[smpl+3]; - const double minus_weight_times_x1 = -weight * x1, - minus_weight_times_y1 = -weight * y1, - weight_times_x2 = weight * x2, - weight_times_y2 = weight * y2; - - a1[0] = minus_weight_times_x1; - a1[1] = minus_weight_times_y1; - a1[2] = -weight; - a1[6] = weight_times_x2 * x1; - a1[7] = weight_times_x2 * y1; - a1[8] = weight_times_x2; - - a2[3] = minus_weight_times_x1; - a2[4] = minus_weight_times_y1; - a2[5] = -weight; - a2[6] = weight_times_y2 * x1; - a2[7] = weight_times_y2 * y1; - a2[8] = weight_times_y2; - - for (int j = 0; j < 9; j++) - for (int z = j; z < 9; z++) - AtA[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z]; + if (mask[i]) // if mask[i] is true then new_mask[i] must be false + for (int j = 0; j < 9; j++) + for (int z = j; z < 9; z++) + covariance[j*9+z] +=-a1[j]*a1[z] - a2[j]*a2[z]; + else + for (int j = 0; j < 9; j++) + for (int z = j; z < 9; z++) + covariance[j*9+z] += a1[j]*a1[z] + a2[j]*a2[z]; } } + mask = new_mask; // copy symmetric part of covariance matrix for (int j = 1; j < 9; j++) for (int z = 0; z < j; z++) - AtA[j*9+z] = AtA[z*9+j]; + covariance[j*9+z] = covariance[z*9+j]; #ifdef HAVE_EIGEN Mat H = Mat_(3,3); - Eigen::HouseholderQR> qr((Eigen::Matrix (AtA))); - const Eigen::Matrix &Q = qr.householderQ(); - // extract the last nullspace - Eigen::Map>((double *)H.data) = Q.col(8); + // extract the last null-vector + Eigen::Map>((double *)H.data) = Eigen::Matrix + (Eigen::HouseholderQR> ( + (Eigen::Matrix (covariance))).householderQ()).col(8); #else - Matx Vt; - Vec D; - if (! eigen(Matx(AtA), D, Vt)) return 0; - Mat H = Mat_(3, 3, Vt.val + 72/*=8*9*/); + Matx Vt; + Vec D; + if (! eigen(Matx(covariance), D, Vt)) return 0; + Mat H = Mat_(3, 3, Vt.val + 72/*=8*9*/); #endif - models = std::vector{ T2.inv() * H * T1 }; + const auto * const h = (double *) H.data; + // H = T2^-1 H T1 + models = std::vector{ Mat(Matx33d(t1[0]*(h[0]/t2[0] - (h[6]*t2[2])/t2[0]), + t1[0]*(h[1]/t2[0] - (h[7]*t2[2])/t2[0]), h[2]/t2[0] + t1[2]*(h[0]/t2[0] - + (h[6]*t2[2])/t2[0]) + t1[5]*(h[1]/t2[0] - (h[7]*t2[2])/t2[0]) - (h[8]*t2[2])/t2[0], + t1[0]*(h[3]/t2[0] - (h[6]*t2[5])/t2[0]), t1[0]*(h[4]/t2[0] - (h[7]*t2[5])/t2[0]), + h[5]/t2[0] + t1[2]*(h[3]/t2[0] - (h[6]*t2[5])/t2[0]) + t1[5]*(h[4]/t2[0] - + (h[7]*t2[5])/t2[0]) - (h[8]*t2[5])/t2[0], t1[0]*h[6], t1[0]*h[7], + h[8] + h[6]*t1[2] + h[7]*t1[5])) }; + return 1; } - + void enforceRankConstraint (bool /*enforce*/) override {} int getMinimumRequiredSampleSize() const override { return 4; } int getMaxNumberOfSolutions () const override { return 1; } - Ptr clone () const override { - return makePtr(*points_mat); - } }; -Ptr HomographyNonMinimalSolver::create(const Mat &points_) { - return makePtr(points_); +Ptr CovarianceHomographySolver::create (const Mat &points) { + return makePtr(points); +} +Ptr CovarianceHomographySolver::create (const Mat &points, const Matx33d &T1, const Matx33d &T2) { + return makePtr(points, T1, T2); } class AffineMinimalSolverImpl : public AffineMinimalSolver { @@ -196,7 +392,7 @@ private: const float * const points; public: explicit AffineMinimalSolverImpl (const Mat &points_) : - points_mat(&points_), points((float *) points_.data) {} + points_mat(&points_), points((float *) points_mat->data) {} /* Affine transformation x1 y1 1 0 0 0 a u1 @@ -232,9 +428,6 @@ public: } int getSampleSize() const override { return 3; } int getMaxNumberOfSolutions () const override { return 1; } - Ptr clone () const override { - return makePtr(*points_mat); - } }; Ptr AffineMinimalSolver::create(const Mat &points_) { return makePtr(points_); @@ -243,22 +436,32 @@ Ptr AffineMinimalSolver::create(const Mat &points_) { class AffineNonMinimalSolverImpl : public AffineNonMinimalSolver { private: const Mat * points_mat; - const float * const points; - // const NormTransform norm_transform; + Ptr normTr; + Matx33d _T1, _T2; + bool do_norm; public: - explicit AffineNonMinimalSolverImpl (const Mat &points_) : - points_mat(&points_), points((float*) points_.data) - /*, norm_transform(points_)*/ {} + explicit AffineNonMinimalSolverImpl (const Mat &points_, InputArray T1, InputArray T2) : + points_mat(&points_) { + if (!T1.empty() && !T2.empty()) { + do_norm = false; + _T1 = T1.getMat(); + _T2 = T2.getMat(); + } else { + do_norm = true; + normTr = NormTransform::create(points_); + } + } int estimate (const std::vector &sample, int sample_size, std::vector &models, const std::vector &weights) const override { - // surprisingly normalization of points does not improve the output model - // Mat norm_points_, T1, T2; - // norm_transform.getNormTransformation(norm_points_, sample, sample_size, T1, T2); - // const auto * const n_pts = (double *) norm_points_.data; - if (sample_size < getMinimumRequiredSampleSize()) return 0; + Matx33d T1, T2; + Mat norm_points_; + if (do_norm) + normTr->getNormTransformation(norm_points_, sample, sample_size, T1, T2); + const auto * const pts = normTr ? (float *) norm_points_.data : (float *) points_mat->data; + // do Least Squares // Ax = b -> A^T Ax = A^T b // x = (A^T A)^-1 A^T b @@ -268,12 +471,8 @@ public: if (weights.empty()) for (int p = 0; p < sample_size; p++) { - // if (weights != nullptr) weight = weights[sample[p]]; - - const int smpl = 4*sample[p]; - const double x1=points[smpl], y1=points[smpl+1], x2=points[smpl+2], y2=points[smpl+3]; - // const double x1=n_pts[smpl], y1=n_pts[smpl+1], x2=n_pts[smpl+2], y2=n_pts[smpl+3]; - + const int idx = do_norm ? 4*p : 4*sample[p]; + const auto x1=pts[idx], y1=pts[idx+1], x2=pts[idx+2], y2=pts[idx+3]; r1[0] = x1; r1[1] = y1; @@ -288,12 +487,13 @@ public: } else for (int p = 0; p < sample_size; p++) { - const int smpl = 4*sample[p]; - const double weight = weights[p]; - const double weight_times_x1 = weight * points[smpl ], - weight_times_y1 = weight * points[smpl+1], - weight_times_x2 = weight * points[smpl+2], - weight_times_y2 = weight * points[smpl+3]; + const auto weight = weights[p]; + if (weight < FLT_EPSILON) continue; + const int idx = do_norm ? 4*p : 4*sample[p]; + const double weight_times_x1 = weight * pts[idx ], + weight_times_y1 = weight * pts[idx+1], + weight_times_x2 = weight * pts[idx+2], + weight_times_y2 = weight * pts[idx+3]; r1[0] = weight_times_x1; r1[1] = weight_times_y1; @@ -318,21 +518,126 @@ public: Vec6d aff; if (!solve(Matx66d(AtA), Vec6d(Ab), aff)) return 0; - models[0] = Mat(Matx33d(aff(0), aff(1), aff(2), - aff(3), aff(4), aff(5), - 0, 0, 1)); - - // models[0] = T2.inv() * models[0] * T1; + const double h[9] = {aff(0), aff(1), aff(2), + aff(3), aff(4), aff(5), + 0, 0, 1}; + const auto * const t1 = normTr ? T1.val : _T1.val, * const t2 = normTr ? T2.val : _T2.val; + // A = T2^-1 A T1 + models = std::vector{ Mat(Matx33d(t1[0]*(h[0]/t2[0] - (h[6]*t2[2])/t2[0]), + t1[0]*(h[1]/t2[0] - (h[7]*t2[2])/t2[0]), h[2]/t2[0] + t1[2]*(h[0]/t2[0] - + (h[6]*t2[2])/t2[0]) + t1[5]*(h[1]/t2[0] - (h[7]*t2[2])/t2[0]) - (h[8]*t2[2])/t2[0], + t1[0]*(h[3]/t2[0] - (h[6]*t2[5])/t2[0]), t1[0]*(h[4]/t2[0] - (h[7]*t2[5])/t2[0]), + h[5]/t2[0] + t1[2]*(h[3]/t2[0] - (h[6]*t2[5])/t2[0]) + t1[5]*(h[4]/t2[0] - + (h[7]*t2[5])/t2[0]) - (h[8]*t2[5])/t2[0], t1[0]*h[6], t1[0]*h[7], + h[8] + h[6]*t1[2] + h[7]*t1[5])) }; return 1; } + int estimate (const std::vector &/*mask*/, std::vector &/*models*/, + const std::vector &/*weights*/) override { + return 0; + } + void enforceRankConstraint (bool /*enforce*/) override {} int getMinimumRequiredSampleSize() const override { return 3; } int getMaxNumberOfSolutions () const override { return 1; } - Ptr clone () const override { - return makePtr(*points_mat); - } }; -Ptr AffineNonMinimalSolver::create(const Mat &points_) { - return makePtr(points_); +Ptr AffineNonMinimalSolver::create(const Mat &points_, InputArray T1, InputArray T2) { + return makePtr(points_, T1, T2); +} + +class CovarianceAffineSolverImpl : public CovarianceAffineSolver { +private: + Mat norm_pts; + Matx33d T1, T2; + float * norm_points; + std::vector mask; + int points_size; + double covariance[36] = {0}, Ab[6] = {0}, * t1, * t2; +public: + explicit CovarianceAffineSolverImpl (const Mat &norm_points_, const Matx33d &T1_, const Matx33d &T2_) + : norm_pts(norm_points_), T1(T1_), T2(T2_) { + points_size = norm_points_.rows; + norm_points = (float *) norm_pts.data; + t1 = T1.val; t2 = T2.val; + mask = std::vector(points_size, false); + } + explicit CovarianceAffineSolverImpl (const Mat &points_) { + points_size = points_.rows; + // normalize points + std::vector sample(points_size); + for (int i = 0; i < points_size; i++) sample[i] = i; + const Ptr normTr = NormTransform::create(points_); + normTr->getNormTransformation(norm_pts, sample, points_size, T1, T2); + norm_points = (float *) norm_pts.data; + t1 = T1.val; t2 = T2.val; + mask = std::vector(points_size, false); + } + void reset () override { + std::fill(covariance, covariance+36, 0); + std::fill(Ab, Ab+6, 0); + std::fill(mask.begin(), mask.end(), false); + } + /* + * Find affine transformation using linear method with covariance matrix and PCA + */ + int estimate (const std::vector &new_mask, std::vector &models, + const std::vector &) override { + double r1[6] = {0, 0, 1, 0, 0, 0}; // row 1 of A + double r2[6] = {0, 0, 0, 0, 0, 1}; // row 2 of A + for (int i = 0; i < points_size; i++) { + if (mask[i] != new_mask[i]) { + const int smpl = 4*i; + const double x1 = norm_points[smpl ], y1 = norm_points[smpl+1], + x2 = norm_points[smpl+2], y2 = norm_points[smpl+3]; + + r1[0] = x1; + r1[1] = y1; + + r2[3] = x1; + r2[4] = y1; + + if (mask[i]) // if mask[i] is true then new_mask[i] must be false + for (int j = 0; j < 6; j++) { + for (int z = j; z < 6; z++) + covariance[j*6+z] +=-r1[j]*r1[z] - r2[j]*r2[z]; + Ab[j] +=-r1[j]*x2 - r2[j]*y2; + } + else + for (int j = 0; j < 6; j++) { + for (int z = j; z < 6; z++) + covariance[j*6+z] += r1[j]*r1[z] + r2[j]*r2[z]; + Ab[j] += r1[j]*x2 + r2[j]*y2; + } + } + } + mask = new_mask; + + // copy symmetric part of covariance matrix + for (int j = 1; j < 6; j++) + for (int z = 0; z < j; z++) + covariance[j*6+z] = covariance[z*6+j]; + + Vec6d aff; + if (!solve(Matx66d(covariance), Vec6d(Ab), aff)) + return 0; + double a[9] = { aff(0), aff(1), aff(2), aff(3), aff(4), aff(5), 0, 0, 1 }; + models = std::vector{ Mat(Matx33d(t1[0]*(a[0]/t2[0] - (a[6]*t2[2])/t2[0]), + t1[0]*(a[1]/t2[0] - (a[7]*t2[2])/t2[0]), a[2]/t2[0] + t1[2]*(a[0]/t2[0] - + (a[6]*t2[2])/t2[0]) + t1[5]*(a[1]/t2[0] - (a[7]*t2[2])/t2[0]) - (a[8]*t2[2])/t2[0], + t1[0]*(a[3]/t2[0] - (a[6]*t2[5])/t2[0]), t1[0]*(a[4]/t2[0] - (a[7]*t2[5])/t2[0]), + a[5]/t2[0] + t1[2]*(a[3]/t2[0] - (a[6]*t2[5])/t2[0]) + t1[5]*(a[4]/t2[0] - + (a[7]*t2[5])/t2[0]) - (a[8]*t2[5])/t2[0], t1[0]*a[6], t1[0]*a[7], + a[8] + a[6]*t1[2] + a[7]*t1[5])) }; + return 1; + } + void enforceRankConstraint (bool /*enforce*/) override {} + int getMinimumRequiredSampleSize() const override { return 3; } + int getMaxNumberOfSolutions () const override { return 1; } +}; +Ptr CovarianceAffineSolver::create (const Mat &points, const Matx33d &T1, const Matx33d &T2) { + return makePtr(points, T1, T2); +} +Ptr CovarianceAffineSolver::create (const Mat &points) { + return makePtr(points); } }} \ No newline at end of file diff --git a/modules/calib3d/src/usac/local_optimization.cpp b/modules/calib3d/src/usac/local_optimization.cpp index 3964edec9a..8c3703940f 100644 --- a/modules/calib3d/src/usac/local_optimization.cpp +++ b/modules/calib3d/src/usac/local_optimization.cpp @@ -22,14 +22,18 @@ protected: std::vector energies, weights; std::set used_edges; std::vector gc_models; + + Ptr termination; + int num_lo_optimizations = 0, current_ransac_iter = 0; public: + void setCurrentRANSACiter (int ransac_iter) override { current_ransac_iter = ransac_iter; } // In lo_sampler_ the sample size should be set and be equal gc_sample_size_ - GraphCutImpl (const Ptr &estimator_, const Ptr &error_, const Ptr &quality_, + GraphCutImpl (const Ptr &estimator_, const Ptr &quality_, const Ptr &neighborhood_graph_, const Ptr &lo_sampler_, - double threshold_, double spatial_coherence_term, int gc_inner_iteration_number_) : + double threshold_, double spatial_coherence_term, int gc_inner_iteration_number_, Ptr termination_) : neighborhood_graph (neighborhood_graph_), estimator (estimator_), quality (quality_), - lo_sampler (lo_sampler_), error (error_) { + lo_sampler (lo_sampler_), error (quality_->getErrorFnc()), termination(termination_) { points_size = quality_->getPointsSize(); spatial_coherence = spatial_coherence_term; @@ -81,9 +85,13 @@ public: gc_models[model_idx].copyTo(new_model); } } + + if (termination != nullptr && is_best_model_updated && current_ransac_iter > termination->update(best_model, best_model_score.inlier_number)) { + is_best_model_updated = false; // to break outer loop + } + } // end of inner GC local optimization } // end of while loop - return true; } @@ -91,7 +99,6 @@ private: // find inliers using graph cut algorithm. int labeling (const Mat& model) { const auto &errors = error->getErrors(model); - detail::GCGraph graph; for (int pt = 0; pt < points_size; pt++) @@ -151,10 +158,8 @@ private: has_edges = true; } } - - if (!has_edges) + if (! has_edges) return quality->getInliers(model, labeling_inliers); - graph.maxFlow(); int inlier_number = 0; @@ -163,23 +168,17 @@ private: labeling_inliers[inlier_number++] = pt; return inlier_number; } - Ptr clone(int state) const override { - return makePtr(estimator->clone(), error->clone(), quality->clone(), - neighborhood_graph,lo_sampler->clone(state), sqr_trunc_thr / 2.25, - spatial_coherence, lo_inner_iterations); - } + int getNumLOoptimizations () const override { return num_lo_optimizations; } }; -Ptr GraphCut::create(const Ptr &estimator_, const Ptr &error_, +Ptr GraphCut::create(const Ptr &estimator_, const Ptr &quality_, const Ptr &neighborhood_graph_, const Ptr &lo_sampler_, double threshold_, - double spatial_coherence_term, int gc_inner_iteration_number) { - return makePtr(estimator_, error_, quality_, neighborhood_graph_, lo_sampler_, - threshold_, spatial_coherence_term, gc_inner_iteration_number); + double spatial_coherence_term, int gc_inner_iteration_number, Ptr termination_) { + return makePtr(estimator_, quality_, neighborhood_graph_, lo_sampler_, + threshold_, spatial_coherence_term, gc_inner_iteration_number, termination_); } -/* -* http://cmp.felk.cvut.cz/~matas/papers/chum-dagm03.pdf -*/ +// http://cmp.felk.cvut.cz/~matas/papers/chum-dagm03.pdf class InnerIterativeLocalOptimizationImpl : public InnerIterativeLocalOptimization { private: const Ptr estimator; @@ -197,23 +196,18 @@ private: double threshold, new_threshold, threshold_step; std::vector weights; public: - InnerIterativeLocalOptimizationImpl (const Ptr &estimator_, const Ptr &quality_, const Ptr &lo_sampler_, int pts_size, double threshold_, bool is_iterative_, int lo_iter_sample_size_, int lo_inner_iterations_=10, int lo_iter_max_iterations_=5, double threshold_multiplier_=4) : estimator (estimator_), quality (quality_), lo_sampler (lo_sampler_) - , lo_iter_sample_size(0) - , new_threshold(0), threshold_step(0) - { + , lo_iter_sample_size(0), new_threshold(0), threshold_step(0) { lo_inner_max_iterations = lo_inner_iterations_; lo_iter_max_iterations = lo_iter_max_iterations_; threshold = threshold_; - lo_sample_size = lo_sampler->getSubsetSize(); - is_iterative = is_iterative_; if (is_iterative) { lo_iter_sample_size = lo_iter_sample_size_; @@ -225,7 +219,6 @@ public: // In the last iteration there be original threshold θ. threshold_step = (new_threshold - threshold) / lo_iter_max_iterations_; } - lo_models = std::vector(estimator->getMaxNumSolutionsNonMinimal()); // Allocate max memory to avoid reallocation @@ -327,12 +320,6 @@ public: } return true; } - Ptr clone(int state) const override { - return makePtr(estimator->clone(), quality->clone(), - lo_sampler->clone(state),(int)inliers_of_best_model.size(), threshold, is_iterative, - lo_iter_sample_size, lo_inner_max_iterations, lo_iter_max_iterations, - new_threshold / threshold); - } }; Ptr InnerIterativeLocalOptimization::create (const Ptr &estimator_, const Ptr &quality_, @@ -345,293 +332,275 @@ Ptr InnerIterativeLocalOptimization::create lo_inner_iterations_, lo_iter_max_iterations_, threshold_multiplier_); } -class SigmaConsensusImpl : public SigmaConsensus { +class SimpleLocalOptimizationImpl : public SimpleLocalOptimization { private: - const Ptr estimator; const Ptr quality; const Ptr error; - const Ptr verifier; - const GammaValues& gamma_generator; - // The degrees of freedom of the data from which the model is estimated. - // E.g., for models coming from point correspondences (x1,y1,x2,y2), it is 4. - const int degrees_of_freedom; - // A 0.99 quantile of the Chi^2-distribution to convert sigma values to residuals - const double k; - // Calculating (DoF - 1) / 2 which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double dof_minus_one_per_two; - const double C; - // The size of a minimal sample used for the estimation - const int sample_size; - // Calculating 2^(DoF - 1) which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double two_ad_dof; - // Calculating C * 2^(DoF - 1) which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double C_times_two_ad_dof; - // Calculating the gamma value of (DoF - 1) / 2 which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double squared_sigma_max_2, one_over_sigma; - // Calculating the upper incomplete gamma value of (DoF - 1) / 2 with k^2 / 2. - const double gamma_k; - // Calculating the lower incomplete gamma value of (DoF - 1) / 2 which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double max_sigma_sqr; - const int points_size, number_of_irwls_iters; - const double maximum_threshold, max_sigma; - - std::vector sqr_residuals, sigma_weights; - std::vector sqr_residuals_idxs; - // Models fit by weighted least-squares fitting - std::vector sigma_models; - // Points used in the weighted least-squares fitting - std::vector sigma_inliers; - // Weights used in the the weighted least-squares fitting - int max_lo_sample_size, stored_gamma_number_min1; - double scale_of_stored_gammas; - RNG rng; - const std::vector &stored_gamma_values; + const Ptr estimator; + const Ptr termination; + const Ptr random_generator; + const Ptr weight_fnc; + // unlike to @random_generator which has fixed subset size + // @random_generator_smaller_subset is used to draw smaller + // amount of points which depends on current number of inliers + Ptr random_generator_smaller_subset; + int points_size, max_lo_iters, non_min_sample_size, current_ransac_iter; + std::vector weights; + std::vector inliers; + std::vector models; + double inlier_threshold_sqr; + int num_lo_optimizations = 0; + bool updated_lo = false; public: - - SigmaConsensusImpl (const Ptr &estimator_, const Ptr &error_, - const Ptr &quality_, const Ptr &verifier_, - int max_lo_sample_size_, int number_of_irwls_iters_, int DoF, - double sigma_quantile, double upper_incomplete_of_sigma_quantile, double C_, - double maximum_thr) : estimator (estimator_), quality(quality_), - error (error_), verifier(verifier_), - gamma_generator(GammaValues::getSingleton()), - degrees_of_freedom(DoF), k (sigma_quantile), C(C_), - sample_size(estimator_->getMinimalSampleSize()), - gamma_k (upper_incomplete_of_sigma_quantile), points_size (quality_->getPointsSize()), - number_of_irwls_iters (number_of_irwls_iters_), - maximum_threshold(maximum_thr), max_sigma (maximum_thr), - stored_gamma_values(gamma_generator.getGammaValues()) - { - dof_minus_one_per_two = (degrees_of_freedom - 1.0) / 2.0; - two_ad_dof = std::pow(2.0, dof_minus_one_per_two); - C_times_two_ad_dof = C * two_ad_dof; - // Calculate 2 * \sigma_{max}^2 a priori - squared_sigma_max_2 = max_sigma * max_sigma * 2.0; - // Divide C * 2^(DoF - 1) by \sigma_{max} a priori - one_over_sigma = C_times_two_ad_dof / max_sigma; - max_sigma_sqr = squared_sigma_max_2 * 0.5; - sqr_residuals = std::vector(points_size); - sqr_residuals_idxs = std::vector(points_size); - sigma_inliers = std::vector(points_size); - max_lo_sample_size = max_lo_sample_size_; - sigma_weights = std::vector(points_size); - sigma_models = std::vector(estimator->getMaxNumSolutionsNonMinimal()); - stored_gamma_number_min1 = gamma_generator.getTableSize()-1; - scale_of_stored_gammas = gamma_generator.getScaleOfGammaValues(); + SimpleLocalOptimizationImpl (const Ptr &quality_, const Ptr &estimator_, + const Ptr termination_, const Ptr &random_gen, Ptr weight_fnc_, + int max_lo_iters_, double inlier_threshold_sqr_, bool update_lo_) : + quality(quality_), error(quality_->getErrorFnc()), estimator(estimator_), termination(termination_), + random_generator(random_gen), weight_fnc(weight_fnc_) { + max_lo_iters = max_lo_iters_; + non_min_sample_size = random_generator->getSubsetSize(); + current_ransac_iter = 0; + inliers = std::vector(quality_->getPointsSize()); + models = std::vector(estimator_->getMaxNumberOfSolutions()); + points_size = quality_->getPointsSize(); + inlier_threshold_sqr = inlier_threshold_sqr_; + if (weight_fnc != nullptr) weights = std::vector(points_size); + random_generator_smaller_subset = nullptr; + updated_lo = update_lo_; } + void setCurrentRANSACiter (int ransac_iter) override { current_ransac_iter = ransac_iter; } + int getNumLOoptimizations () const override { return num_lo_optimizations; } + bool refineModel (const Mat &best_model, const Score &best_model_score, Mat &new_model, Score &new_model_score) override { + new_model_score = best_model_score; + best_model.copyTo(new_model); - // https://github.com/danini/magsac - bool refineModel (const Mat &in_model, const Score &best_model_score, - Mat &new_model, Score &new_model_score) override { - int residual_cnt = 0; + int num_inliers; + if (weights.empty()) + num_inliers = Quality::getInliers(error, best_model, inliers, inlier_threshold_sqr); + else num_inliers = weight_fnc->getInliersWeights(error->getErrors(best_model), inliers, weights); + auto update_generator = [&] (int num_inls) { + if (num_inls <= non_min_sample_size) { + // add new random generator if number of inliers is fewer than non-minimal sample size + const int new_sample_size = (int)(0.6*num_inls); + if (new_sample_size <= estimator->getMinimumRequiredSampleSize()) + return false; + if (random_generator_smaller_subset == nullptr) + random_generator_smaller_subset = UniformRandomGenerator::create(num_inls/*state*/, quality->getPointsSize(), new_sample_size); + else random_generator_smaller_subset->setSubsetSize(new_sample_size); + } + return true; + }; + if (!update_generator(num_inliers)) + return false; - if (verifier->isModelGood(in_model)) { - if (verifier->hasErrors()) { - const std::vector &errors = verifier->getErrors(); - for (int point_idx = 0; point_idx < points_size; ++point_idx) { - // Calculate the residual of the current point - const auto residual = sqrtf(errors[point_idx]); - if (max_sigma > residual) { - // Store the residual of the current point and its index - sqr_residuals[residual_cnt] = residual; - sqr_residuals_idxs[residual_cnt++] = point_idx; + int max_lo_iters_ = max_lo_iters, last_update = 0, last_inliers_update = 0; + for (int iter = 0; iter < max_lo_iters_; iter++) { + int num_models; + if (num_inliers <= non_min_sample_size) + num_models = estimator->estimate(new_model, random_generator_smaller_subset->generateUniqueRandomSubset(inliers, num_inliers), + random_generator_smaller_subset->getSubsetSize(), models, weights); + else num_models = estimator->estimate(new_model, random_generator->generateUniqueRandomSubset(inliers, num_inliers), non_min_sample_size, models, weights); + for (int m = 0; m < num_models; m++) { + const auto score = quality->getScore(models[m]); + if (score.isBetter(new_model_score)) { + last_update = iter; last_inliers_update = new_model_score.inlier_number - score.inlier_number; + if (updated_lo) { + if (max_lo_iters_ < iter + 5 && last_inliers_update >= 1) { + max_lo_iters_ = iter + 5; + } + } + models[m].copyTo(new_model); + new_model_score = score; + if (termination != nullptr && current_ransac_iter > termination->update(new_model, new_model_score.inlier_number)) + return true; // terminate LO if max number of iterations reached + if (score.inlier_number >= best_model_score.inlier_number || + score.inlier_number > non_min_sample_size) { + // update inliers if new model has more than previous best model + if (weights.empty()) + num_inliers = Quality::getInliers(error, best_model, inliers, inlier_threshold_sqr); + else num_inliers = weight_fnc->getInliersWeights(error->getErrors(best_model), inliers, weights); + if (!update_generator(num_inliers)) + return true; } - // Interrupt if there is no chance of being better - if (residual_cnt + points_size - point_idx < best_model_score.inlier_number) - return false; - } - } else { - error->setModelParameters(in_model); - - for (int point_idx = 0; point_idx < points_size; ++point_idx) { - const double sqr_residual = error->getError(point_idx); - if (sqr_residual < max_sigma_sqr) { - // Store the residual of the current point and its index - sqr_residuals[residual_cnt] = sqr_residual; - sqr_residuals_idxs[residual_cnt++] = point_idx; - } - - if (residual_cnt + points_size - point_idx < best_model_score.inlier_number) - return false; } } - } else return false; - - in_model.copyTo(new_model); - new_model_score = Score(); - - // Do the iteratively re-weighted least squares fitting - for (int iterations = 0; iterations < number_of_irwls_iters; iterations++) { - int sigma_inliers_cnt = 0; - // If the current iteration is not the first, the set of possibly inliers - // (i.e., points closer than the maximum threshold) have to be recalculated. - if (iterations > 0) { - // error->setModelParameters(polished_model); - error->setModelParameters(new_model); - // Remove everything from the residual vector - residual_cnt = 0; - - // Collect the points which are closer than the maximum threshold - for (int point_idx = 0; point_idx < points_size; ++point_idx) { - // Calculate the residual of the current point - const double sqr_residual = error->getError(point_idx); - if (sqr_residual < max_sigma_sqr) { - // Store the residual of the current point and its index - sqr_residuals[residual_cnt] = sqr_residual; - sqr_residuals_idxs[residual_cnt++] = point_idx; - } - } - sigma_inliers_cnt = 0; - } - - // Calculate the weight of each point - for (int i = 0; i < residual_cnt; i++) { - // Get the position of the gamma value in the lookup table - int x = (int)round(scale_of_stored_gammas * sqr_residuals[i] - / squared_sigma_max_2); - - // If the sought gamma value is not stored in the lookup, return the closest element - if (x >= stored_gamma_number_min1 || x < 0 /*overflow*/) // actual number of gamma values is 1 more, so >= - x = stored_gamma_number_min1; - - sigma_inliers[sigma_inliers_cnt] = sqr_residuals_idxs[i]; // store index of point for LSQ - sigma_weights[sigma_inliers_cnt++] = one_over_sigma * (stored_gamma_values[x] - gamma_k); - } - - // random shuffle sigma inliers - if (sigma_inliers_cnt > max_lo_sample_size) - for (int i = sigma_inliers_cnt-1; i > 0; i--) { - const int idx = rng.uniform(0, i+1); - std::swap(sigma_inliers[i], sigma_inliers[idx]); - std::swap(sigma_weights[i], sigma_weights[idx]); - } - const int num_est_models = estimator->estimateModelNonMinimalSample - (sigma_inliers, std::min(max_lo_sample_size, sigma_inliers_cnt), - sigma_models, sigma_weights); - - if (num_est_models == 0) - break; // break iterations - - // Update the model parameters - Mat polished_model = sigma_models[0]; - if (num_est_models > 1) { - // find best over other models - Score sigma_best_score = quality->getScore(polished_model); - for (int m = 1; m < num_est_models; m++) { - const Score sc = quality->getScore(sigma_models[m]); - if (sc.isBetter(sigma_best_score)) { - polished_model = sigma_models[m]; - sigma_best_score = sc; - } - } - } - - const Score polished_model_score = quality->getScore(polished_model); - if (polished_model_score.isBetter(new_model_score)){ - new_model_score = polished_model_score; - polished_model.copyTo(new_model); + if (updated_lo && iter - last_update >= 10) { + break; } } - - const Score in_model_score = quality->getScore(in_model); - if (in_model_score.isBetter(new_model_score)) { - new_model_score = in_model_score; - in_model.copyTo(new_model); - } - return true; } - Ptr clone(int state) const override { - return makePtr(estimator->clone(), error->clone(), quality->clone(), - verifier->clone(state), max_lo_sample_size, - number_of_irwls_iters, degrees_of_freedom, k, gamma_k, C, maximum_threshold); +}; +Ptr SimpleLocalOptimization::create (const Ptr &quality_, + const Ptr &estimator_, const Ptr termination_, const Ptr &random_gen, + const Ptr weight_fnc, int max_lo_iters_, double inlier_thr_sqr, bool updated_lo) { + return makePtr (quality_, estimator_, termination_, random_gen, weight_fnc, max_lo_iters_, inlier_thr_sqr, updated_lo); +} + +class MagsacWeightFunctionImpl : public MagsacWeightFunction { +private: + const std::vector &stored_gamma_values; + double C, max_sigma, max_sigma_sqr, scale_of_stored_gammas, one_over_sigma, gamma_k, squared_sigma_max_2, rescale_err; + int DoF; + unsigned int stored_gamma_number_min1; +public: + MagsacWeightFunctionImpl (const Ptr &gamma_generator, + int DoF_, double upper_incomplete_of_sigma_quantile, double C_, double max_sigma_) : + stored_gamma_values (gamma_generator->getGammaValues()) { + gamma_k = upper_incomplete_of_sigma_quantile; + stored_gamma_number_min1 = static_cast(gamma_generator->getTableSize()-1); + scale_of_stored_gammas = gamma_generator->getScaleOfGammaValues(); + DoF = DoF_; C = C_; + max_sigma = max_sigma_; + squared_sigma_max_2 = max_sigma * max_sigma * 2.0; + one_over_sigma = C * pow(2.0, (DoF - 1.0) * 0.5) / max_sigma; + max_sigma_sqr = squared_sigma_max_2 * 0.5; + rescale_err = scale_of_stored_gammas / squared_sigma_max_2; + } + int getInliersWeights (const std::vector &errors, std::vector &inliers, std::vector &weights) const override { + return getInliersWeights(errors, inliers, weights, one_over_sigma, rescale_err, max_sigma_sqr); + } + int getInliersWeights (const std::vector &errors, std::vector &inliers, std::vector &weights, double thr_sqr) const override { + const auto _max_sigma = thr_sqr; + const auto _squared_sigma_max_2 = _max_sigma * _max_sigma * 2.0; + const auto _one_over_sigma = C * pow(2.0, (DoF - 1.0) * 0.5) / _max_sigma; + const auto _max_sigma_sqr = _squared_sigma_max_2 * 0.5; + const auto _rescale_err = scale_of_stored_gammas / _squared_sigma_max_2; + return getInliersWeights(errors, inliers, weights, _one_over_sigma, _rescale_err, _max_sigma_sqr); + } + double getThreshold () const override { + return max_sigma_sqr; + } +private: + int getInliersWeights (const std::vector &errors, std::vector &inliers, std::vector &weights, + double _one_over_sigma, double _rescale_err, double _max_sigma_sqr) const { + int num_inliers = 0, p = 0; + for (const auto &e : errors) { + // Calculate the residual of the current point + if (e < _max_sigma_sqr) { + // Get the position of the gamma value in the lookup table + auto x = static_cast(_rescale_err * e); + if (x > stored_gamma_number_min1) + x = stored_gamma_number_min1; + inliers[num_inliers] = p; // store index of point for LSQ + weights[num_inliers++] = _one_over_sigma * (stored_gamma_values[x] - gamma_k); + } + p++; + } + return num_inliers; } }; -Ptr -SigmaConsensus::create(const Ptr &estimator_, const Ptr &error_, - const Ptr &quality, const Ptr &verifier_, - int max_lo_sample_size, int number_of_irwls_iters_, int DoF, - double sigma_quantile, double upper_incomplete_of_sigma_quantile, double C_, - double maximum_thr) { - return makePtr(estimator_, error_, quality, verifier_, - max_lo_sample_size, number_of_irwls_iters_, DoF, sigma_quantile, - upper_incomplete_of_sigma_quantile, C_, maximum_thr); +Ptr MagsacWeightFunction::create(const Ptr &gamma_generator_, + int DoF_, double upper_incomplete_of_sigma_quantile, double C_, double max_sigma_) { + return makePtr(gamma_generator_, DoF_, upper_incomplete_of_sigma_quantile, C_, max_sigma_); } /////////////////////////////////////////// FINAL MODEL POLISHER //////////////////////// -class LeastSquaresPolishingImpl : public LeastSquaresPolishing { +class NonMinimalPolisherImpl : public NonMinimalPolisher { private: - const Ptr estimator; const Ptr quality; - int lsq_iterations; - std::vector inliers; + const Ptr solver; + const Ptr error_fnc; + const Ptr weight_fnc; + std::vector mask, mask_best; std::vector models; std::vector weights; + std::vector errors_best; + std::vector inliers; + double threshold, iou_thr, max_thr; + int max_iters, points_size; + bool is_covariance, CHANGE_WEIGHTS = true; public: - - LeastSquaresPolishingImpl(const Ptr &estimator_, const Ptr &quality_, - int lsq_iterations_) : - estimator(estimator_), quality(quality_) { - lsq_iterations = lsq_iterations_; - // allocate memory for inliers array and models - inliers = std::vector(quality_->getPointsSize()); - models = std::vector(estimator->getMaxNumSolutionsNonMinimal()); + NonMinimalPolisherImpl (const Ptr &quality_, const Ptr &solver_, + Ptr weight_fnc_, int max_iters_, double iou_thr_) : + quality(quality_), solver(solver_), error_fnc(quality_->getErrorFnc()), weight_fnc(weight_fnc_) { + max_iters = max_iters_; + points_size = quality_->getPointsSize(); + threshold = quality_->getThreshold(); + iou_thr = iou_thr_; + is_covariance = dynamic_cast(solver_.get()) != nullptr; + mask = std::vector(points_size); + mask_best = std::vector(points_size); + inliers = std::vector(points_size); + if (weight_fnc != nullptr) { + weights = std::vector(points_size); + max_thr = weight_fnc->getThreshold(); + if (is_covariance) + CV_Error(cv::Error::StsBadArg, "Covariance polisher cannot be combined with weights!"); + } } - bool polishSoFarTheBestModel(const Mat &model, const Score &best_model_score, - Mat &new_model, Score &out_score) override { - // get inliers from input model - int inlier_number = quality->getInliers(model, inliers); - if (inlier_number < estimator->getMinimalSampleSize()) - return false; - - out_score = Score(); // set the worst case - - // several all-inlier least-squares refines model better than only one but for - // big amount of points may be too time-consuming. - for (int lsq_iter = 0; lsq_iter < lsq_iterations; lsq_iter++) { - bool model_updated = false; - - // estimate non minimal models with all inliers - const int num_models = estimator->estimateModelNonMinimalSample(inliers, - inlier_number, models, weights); - for (int model_idx = 0; model_idx < num_models; model_idx++) { - const Score score = quality->getScore(models[model_idx]); - if (best_model_score.isBetter(score)) - continue; - if (score.isBetter(out_score)) { - models[model_idx].copyTo(new_model); - out_score = score; - model_updated = true; + bool polishSoFarTheBestModel (const Mat &model, const Score &best_model_score, + Mat &new_model, Score &new_model_score) override { + int num_inliers = 0; + if (weights.empty()) { + quality->getInliers(model, mask_best); + if (!is_covariance) + for (int p = 0; p < points_size; p++) + if (mask_best[p]) inliers[num_inliers++] = p; + } else { + errors_best = error_fnc->getErrors(model); + num_inliers = weight_fnc->getInliersWeights(errors_best, inliers, weights, max_thr); + } + new_model_score = best_model_score; + model.copyTo(new_model); + int last_update = -1; + for (int iter = 0; iter < max_iters; iter++) { + int num_sols; + if (is_covariance) num_sols = solver->estimate(mask_best, models, weights); + else num_sols = solver->estimate(new_model, inliers, num_inliers, models, weights); + Score prev_score; + for (int i = 0; i < num_sols; i++) { + const auto &errors = error_fnc->getErrors(models[i]); + const auto score = quality->getScore(errors); + if (score.isBetter(new_model_score)) { + last_update = iter; + models[i].copyTo(new_model); + errors_best = errors; + prev_score = new_model_score; + new_model_score = score; } } - - if (!model_updated) - // if model was not updated at the first iteration then return false - // otherwise if all-inliers LSQ has not updated model then no sense - // to do it again -> return true (model was updated before). - return lsq_iter > 0; - - // if number of inliers doesn't increase more than 5% then break - if (fabs(static_cast(out_score.inlier_number) - static_cast - (best_model_score.inlier_number)) / best_model_score.inlier_number < 0.05) - return true; - - if (lsq_iter != lsq_iterations - 1) - // if not the last LSQ normalization then get inliers for next normalization - inlier_number = quality->getInliers(new_model, inliers); + if (weights.empty()) { + if (iter > last_update) + break; + else { + Quality::getInliers(errors_best, mask, threshold); + if (Utils::intersectionOverUnion(mask, mask_best) >= iou_thr) + return true; + mask_best = mask; + num_inliers = 0; + if (!is_covariance) + for (int p = 0; p < points_size; p++) + if (mask_best[p]) inliers[num_inliers++] = p; + } + } else { + if (iter > last_update) { + // new model is worse + if (CHANGE_WEIGHTS) { + // if failed more than 5 times then break + if (iter - std::max(0, last_update) >= 5) + break; + // try to change weights by changing threshold + if (fabs(new_model_score.score - prev_score.score) < FLT_EPSILON) { + // increase threshold if new model score is the same as the same as previous + max_thr *= 1.05; // increase by 5% + } else if (iter > last_update) { + // decrease max threshold if model is worse + max_thr *= 0.9; // decrease by 10% + } + } else break; // break if not changing weights + } + // generate new weights and inliers + num_inliers = weight_fnc->getInliersWeights(errors_best, inliers, weights, max_thr); + } } - return true; + return last_update >= 0; } }; -Ptr LeastSquaresPolishing::create (const Ptr &estimator_, - const Ptr &quality_, int lsq_iterations_) { - return makePtr(estimator_, quality_, lsq_iterations_); +Ptr NonMinimalPolisher::create(const Ptr &quality_, const Ptr &solver_, + Ptr weight_fnc_, int max_iters_, double iou_thr_) { + return makePtr(quality_, solver_, weight_fnc_, max_iters_, iou_thr_); } }} diff --git a/modules/calib3d/src/usac/pnp_solver.cpp b/modules/calib3d/src/usac/pnp_solver.cpp index 37c649144c..704632a0e8 100644 --- a/modules/calib3d/src/usac/pnp_solver.cpp +++ b/modules/calib3d/src/usac/pnp_solver.cpp @@ -23,7 +23,7 @@ public: int getMaxNumberOfSolutions () const override { return 1; } explicit PnPMinimalSolver6PtsImpl (const Mat &points_) : - points_mat(&points_), points ((float*)points_.data) {} + points_mat(&points_), points ((float*)points_mat->data) {} /* DLT: d x = P X, x = (u, v, 1), X = (X, Y, Z, 1), P = K[R t] @@ -73,6 +73,9 @@ public: int estimate (const std::vector &sample, std::vector &models) const override { std::vector A1 (60, 0), A2(56, 0); // 5x12, 7x8 + // std::vector A_all(11*12, 0); + // int cnt3 = 0; + int cnt1 = 0, cnt2 = 0; for (int i = 0; i < 6; i++) { const int smpl = 5 * sample[i]; @@ -100,14 +103,16 @@ public: A2[cnt2++] = -v * Z; A2[cnt2++] = -v; } - // matrix is sparse -> do not test for singularity + Math::eliminateUpperTriangular(A1, 5, 12); + int offset = 4*12; // add last eliminated row of A1 for (int i = 0; i < 8; i++) A2[cnt2++] = A1[offset + i + 4/* skip 4 first cols*/]; + // must be full-rank if (!Math::eliminateUpperTriangular(A2, 7, 8)) return 0; @@ -139,13 +144,9 @@ public: if (std::isnan(p[i])) return 0; } - models = std::vector{P}; return 1; } - Ptr clone () const override { - return makePtr(*points_mat); - } }; Ptr PnPMinimalSolver6Pts::create(const Mat &points_) { return makePtr(points_); @@ -157,7 +158,7 @@ private: const float * const points; public: explicit PnPNonMinimalSolverImpl (const Mat &points_) : - points_mat(&points_), points ((float*)points_.data){} + points_mat(&points_), points ((float*)points_mat->data){} int estimate (const std::vector &sample, int sample_size, std::vector &models, const std::vector &weights) const override { @@ -236,7 +237,7 @@ public: models = std::vector{ Mat_(3,4) }; Eigen::HouseholderQR> qr((Eigen::Matrix(AtA))); const Eigen::Matrix &Q = qr.householderQ(); - // extract the last nullspace + // extract the last null-vector Eigen::Map>((double *)models[0].data) = Q.col(11); #else Matx Vt; @@ -246,17 +247,37 @@ public: #endif return 1; } - + int estimate (const std::vector &/*mask*/, std::vector &/*models*/, + const std::vector &/*weights*/) override { + return 0; + } + void enforceRankConstraint (bool /*enforce*/) override {} int getMinimumRequiredSampleSize() const override { return 6; } int getMaxNumberOfSolutions () const override { return 1; } - Ptr clone () const override { - return makePtr(*points_mat); - } }; Ptr PnPNonMinimalSolver::create(const Mat &points) { return makePtr(points); } +class PnPSVDSolverImpl : public PnPSVDSolver { +private: + std::vector w; + Ptr pnp_solver; +public: + int getSampleSize() const override { return 6; } + int getMaxNumberOfSolutions () const override { return 1; } + + explicit PnPSVDSolverImpl (const Mat &points_) { + pnp_solver = PnPNonMinimalSolver::create(points_); + } + int estimate (const std::vector &sample, std::vector &models) const override { + return pnp_solver->estimate(sample, 6, models, w); + } +}; +Ptr PnPSVDSolver::create(const Mat &points_) { + return makePtr(points_); +} + class P3PSolverImpl : public P3PSolver { private: /* @@ -264,7 +285,7 @@ private: * K^-1 [u v 1]^T / ||K^-1 [u v 1]^T|| */ const Mat * points_mat, * calib_norm_points_mat; - const Matx33d * K_mat, &K; + const Matx33d K; const float * const calib_norm_points, * const points; const double VAL_THR = 1e-4; public: @@ -272,9 +293,9 @@ public: * @points_ is matrix N x 5 * u v x y z. (u,v) is image point, (x y z) is world point */ - P3PSolverImpl (const Mat &points_, const Mat &calib_norm_points_, const Matx33d &K_) : - points_mat(&points_), calib_norm_points_mat(&calib_norm_points_), K_mat (&K_), - K(K_), calib_norm_points((float*)calib_norm_points_.data), points((float*)points_.data) {} + P3PSolverImpl (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) {} int estimate (const std::vector &sample, std::vector &models) const override { /* @@ -362,8 +383,8 @@ public: zw[2] = Z3crZ1w(0); zw[5] = Z3crZ1w(1); zw[8] = Z3crZ1w(2); const Matx33d R = Math::rotVec2RotMat(Math::rotMat2RotVec(Z * Zw.inv())); - - Mat P, KR = Mat(K * R); + Matx33d KR = K * R; + Matx34d P; hconcat(KR, -KR * (X1 - R.t() * nX1), P); models.emplace_back(P); } @@ -371,11 +392,8 @@ public: } int getSampleSize() const override { return 3; } int getMaxNumberOfSolutions () const override { return 4; } - Ptr clone () const override { - return makePtr(*points_mat, *calib_norm_points_mat, *K_mat); - } }; -Ptr P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K) { +Ptr P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) { return makePtr(points_, calib_norm_pts, K); } }} \ No newline at end of file diff --git a/modules/calib3d/src/usac/quality.cpp b/modules/calib3d/src/usac/quality.cpp index d07c1da29d..89c5760c1d 100644 --- a/modules/calib3d/src/usac/quality.cpp +++ b/modules/calib3d/src/usac/quality.cpp @@ -25,6 +25,27 @@ int Quality::getInliers(const Ptr &error, const Mat &model, std::vector &errors, std::vector &inliers, double threshold) { + std::fill(inliers.begin(), inliers.end(), false); + int cnt = 0, inls = 0; + for (const auto e : errors) { + if (e < threshold) { + inliers[cnt] = true; + inls++; + } + cnt++; + } + return inls; +} +int Quality::getInliers (const std::vector &errors, std::vector &inliers, double threshold) { + int cnt = 0, inls = 0; + for (const auto e : errors) { + if (e < threshold) + inliers[inls++] = cnt; + cnt++; + } + return inls; +} class RansacQualityImpl : public RansacQuality { private: @@ -41,14 +62,23 @@ public: Score getScore (const Mat &model) const override { error->setModelParameters(model); int inlier_number = 0; - for (int point = 0; point < points_size; point++) { + const auto preemptive_thr = -points_size - best_score; + for (int point = 0; point < points_size; point++) if (error->getError(point) < threshold) inlier_number++; - if (inlier_number + (points_size - point) < -best_score) - break; - } + else if (inlier_number - point < preemptive_thr) + break; // score is negative inlier number! If less then better - return Score(inlier_number, -static_cast(inlier_number)); + return {inlier_number, -static_cast(inlier_number)}; + } + + Score getScore (const std::vector &errors) const override { + int inlier_number = 0; + for (int point = 0; point < points_size; point++) + if (errors[point] < threshold) + inlier_number++; + // score is negative inlier number! If less then better + return {inlier_number, -static_cast(inlier_number)}; } void setBestScore(double best_score_) override { @@ -61,11 +91,9 @@ public: { return Quality::getInliers(error, model, inliers, thr); } int getInliers (const Mat &model, std::vector &inliers_mask) const override { return Quality::getInliers(error, model, inliers_mask, threshold); } - + double getThreshold () const override { return threshold; } int getPointsSize () const override { return points_size; } - Ptr clone () const override { - return makePtr(points_size, threshold, error->clone()); - } + Ptr getErrorFnc () const override { return error; } }; Ptr RansacQuality::create(int points_size_, double threshold_, @@ -77,13 +105,13 @@ class MsacQualityImpl : public MsacQuality { protected: const Ptr error; const int points_size; - const double threshold; + const double threshold, k_msac; double best_score, norm_thr, one_over_thr; public: - MsacQualityImpl (int points_size_, double threshold_, const Ptr &error_) - : error (error_), points_size (points_size_), threshold (threshold_) { + MsacQualityImpl (int points_size_, double threshold_, const Ptr &error_, double k_msac_) + : error (error_), points_size (points_size_), threshold (threshold_), k_msac(k_msac_) { best_score = std::numeric_limits::max(); - norm_thr = threshold*9/4; + norm_thr = threshold*k_msac; one_over_thr = 1 / norm_thr; } @@ -91,17 +119,31 @@ public: error->setModelParameters(model); double err, sum_errors = 0; int inlier_number = 0; + const auto preemptive_thr = points_size + best_score; for (int point = 0; point < points_size; point++) { err = error->getError(point); if (err < norm_thr) { sum_errors -= (1 - err * one_over_thr); if (err < threshold) inlier_number++; - } - if (sum_errors - points_size + point > best_score) + } else if (sum_errors + point > preemptive_thr) break; } - return Score(inlier_number, sum_errors); + return {inlier_number, sum_errors}; + } + + Score getScore (const std::vector &errors) const override { + double sum_errors = 0; + int inlier_number = 0; + for (int point = 0; point < points_size; point++) { + const auto err = errors[point]; + if (err < norm_thr) { + sum_errors -= (1 - err * one_over_thr); + if (err < threshold) + inlier_number++; + } + } + return {inlier_number, sum_errors}; } void setBestScore(double best_score_) override { @@ -114,103 +156,61 @@ public: { return Quality::getInliers(error, model, inliers, thr); } int getInliers (const Mat &model, std::vector &inliers_mask) const override { return Quality::getInliers(error, model, inliers_mask, threshold); } - + double getThreshold () const override { return threshold; } int getPointsSize () const override { return points_size; } - Ptr clone () const override { - return makePtr(points_size, threshold, error->clone()); - } + Ptr getErrorFnc () const override { return error; } }; Ptr MsacQuality::create(int points_size_, double threshold_, - const Ptr &error_) { - return makePtr(points_size_, threshold_, error_); + const Ptr &error_, double k_msac) { + return makePtr(points_size_, threshold_, error_, k_msac); } class MagsacQualityImpl : public MagsacQuality { private: const Ptr error; - const GammaValues& gamma_generator; + const Ptr gamma_generator; const int points_size; - // for example, maximum standard deviation of noise. const double maximum_threshold_sqr, tentative_inlier_threshold; - // The degrees of freedom of the data from which the model is estimated. - // E.g., for models coming from point correspondences (x1,y1,x2,y2), it is 4. - const int degrees_of_freedom; - // A 0.99 quantile of the Chi^2-distribution to convert sigma values to residuals - const double k; - // Calculating k^2 / 2 which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double squared_k_per_2; - // Calculating (DoF - 1) / 2 which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double dof_minus_one_per_two; - // Calculating (DoF + 1) / 2 which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double dof_plus_one_per_two; - const double C; - // Calculating 2^(DoF - 1) which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double two_ad_dof_minus_one; - // Calculating 2^(DoF + 1) which will be used for the estimation and, - // due to being constant, it is better to calculate it a priori. - double two_ad_dof_plus_one; // Calculate the gamma value of k const double gamma_value_of_k; - // Calculate the lower incomplete gamma value of k - const double lower_gamma_value_of_k; double previous_best_loss; - // Convert the maximum threshold to a sigma value - float maximum_sigma; - // Calculate the squared maximum sigma - float maximum_sigma_2; - // Calculate \sigma_{max}^2 / 2 float maximum_sigma_2_per_2; - // Calculate 2 * \sigma_{max}^2 - float maximum_sigma_2_times_2; // Calculating 2^(DoF + 1) / \sigma_{max} which will be used for the estimation and, // due to being constant, it is better to calculate it a priori. - double two_ad_dof_plus_one_per_maximum_sigma; - double scale_of_stored_incomplete_gammas; - double max_loss; + double two_ad_dof_plus_one_per_maximum_sigma, rescale_err, norm_loss; const std::vector &stored_complete_gamma_values, &stored_lower_incomplete_gamma_values; - int stored_incomplete_gamma_number_min1; + unsigned int stored_incomplete_gamma_number_min1; public: MagsacQualityImpl (double maximum_thr, int points_size_, const Ptr &error_, + const Ptr &gamma_generator_, double tentative_inlier_threshold_, int DoF, double sigma_quantile, - double upper_incomplete_of_sigma_quantile, - double lower_incomplete_of_sigma_quantile, double C_) - : error (error_), gamma_generator(GammaValues::getSingleton()), points_size(points_size_), + double upper_incomplete_of_sigma_quantile) + : error (error_), gamma_generator(gamma_generator_), points_size(points_size_), maximum_threshold_sqr(maximum_thr*maximum_thr), - tentative_inlier_threshold(tentative_inlier_threshold_), degrees_of_freedom(DoF), - k(sigma_quantile), C(C_), gamma_value_of_k (upper_incomplete_of_sigma_quantile), - lower_gamma_value_of_k (lower_incomplete_of_sigma_quantile), - stored_complete_gamma_values(gamma_generator.getCompleteGammaValues()), - stored_lower_incomplete_gamma_values(gamma_generator.getIncompleteGammaValues()) - { + tentative_inlier_threshold(tentative_inlier_threshold_), + gamma_value_of_k (upper_incomplete_of_sigma_quantile), + stored_complete_gamma_values (gamma_generator->getCompleteGammaValues()), + stored_lower_incomplete_gamma_values (gamma_generator->getIncompleteGammaValues()) { previous_best_loss = std::numeric_limits::max(); - squared_k_per_2 = k * k / 2.0; - dof_minus_one_per_two = (degrees_of_freedom - 1.0) / 2.0; - dof_plus_one_per_two = (degrees_of_freedom + 1.0) / 2.0; - two_ad_dof_minus_one = std::pow(2.0, dof_minus_one_per_two); - two_ad_dof_plus_one = std::pow(2.0, dof_plus_one_per_two); - maximum_sigma = (float)sqrt(maximum_threshold_sqr) / (float) k; - maximum_sigma_2 = maximum_sigma * maximum_sigma; + const auto maximum_sigma = (float)sqrt(maximum_threshold_sqr) / sigma_quantile; + const auto maximum_sigma_2 = (float) (maximum_sigma * maximum_sigma); maximum_sigma_2_per_2 = maximum_sigma_2 / 2.f; - maximum_sigma_2_times_2 = maximum_sigma_2 * 2.f; - two_ad_dof_plus_one_per_maximum_sigma = two_ad_dof_plus_one / maximum_sigma; - scale_of_stored_incomplete_gammas = gamma_generator.getScaleOfGammaCompleteValues(); - stored_incomplete_gamma_number_min1 = gamma_generator.getTableSize()-1; - max_loss = 1e-10; - // MAGSAC maximum / minimum loss does not have to be in extrumum residuals - // make 50 iterations to find maximum loss + const auto maximum_sigma_2_times_2 = maximum_sigma_2 * 2.f; + two_ad_dof_plus_one_per_maximum_sigma = pow(2.0, (DoF + 1.0)*.5)/maximum_sigma; + rescale_err = gamma_generator->getScaleOfGammaCompleteValues() / maximum_sigma_2_times_2; + stored_incomplete_gamma_number_min1 = static_cast(gamma_generator->getTableSize()-1); + + double max_loss = 1e-10; + // MAGSAC maximum / minimum loss does not have to be in extremum residuals + // make 30 iterations to find maximum loss const double step = maximum_threshold_sqr / 30; double sqr_res = 0; while (sqr_res < maximum_threshold_sqr) { - int x=(int)round(scale_of_stored_incomplete_gammas * sqr_res - / maximum_sigma_2_times_2); - if (x >= stored_incomplete_gamma_number_min1 || x < 0 /*overflow*/) - x = stored_incomplete_gamma_number_min1; + auto x= static_cast(rescale_err * sqr_res); + if (x > stored_incomplete_gamma_number_min1) + x = stored_incomplete_gamma_number_min1; const double loss = two_ad_dof_plus_one_per_maximum_sigma * (maximum_sigma_2_per_2 * stored_lower_incomplete_gamma_values[x] + sqr_res * 0.25 * (stored_complete_gamma_values[x] - gamma_value_of_k)); @@ -218,6 +218,7 @@ public: max_loss = loss; sqr_res += step; } + norm_loss = two_ad_dof_plus_one_per_maximum_sigma / max_loss; } // https://github.com/danini/magsac @@ -225,26 +226,25 @@ public: error->setModelParameters(model); double total_loss = 0.0; int num_tentative_inliers = 0; + const auto preemptive_thr = points_size + previous_best_loss; for (int point_idx = 0; point_idx < points_size; point_idx++) { const float squared_residual = error->getError(point_idx); if (squared_residual < tentative_inlier_threshold) num_tentative_inliers++; if (squared_residual < maximum_threshold_sqr) { // consider point as inlier // Get the position of the gamma value in the lookup table - int x=(int)round(scale_of_stored_incomplete_gammas * squared_residual - / maximum_sigma_2_times_2); + auto x = static_cast(rescale_err * squared_residual); // If the sought gamma value is not stored in the lookup, return the closest element - if (x >= stored_incomplete_gamma_number_min1 || x < 0 /*overflow*/) - x = stored_incomplete_gamma_number_min1; + if (x > stored_incomplete_gamma_number_min1) + x = stored_incomplete_gamma_number_min1; // Calculate the loss implied by the current point - total_loss -= (1 - two_ad_dof_plus_one_per_maximum_sigma * (maximum_sigma_2_per_2 * + total_loss -= (1 - (maximum_sigma_2_per_2 * stored_lower_incomplete_gamma_values[x] + squared_residual * 0.25 * - (stored_complete_gamma_values[x] - gamma_value_of_k)) / max_loss); - } - if (total_loss - (points_size - point_idx) > previous_best_loss) + (stored_complete_gamma_values[x] - gamma_value_of_k)) * norm_loss); + } else if (total_loss + point_idx > preemptive_thr) break; } - return Score(num_tentative_inliers, total_loss); + return {num_tentative_inliers, total_loss}; } Score getScore (const std::vector &errors) const override { @@ -255,18 +255,15 @@ public: if (squared_residual < tentative_inlier_threshold) num_tentative_inliers++; if (squared_residual < maximum_threshold_sqr) { - int x=(int)round(scale_of_stored_incomplete_gammas * squared_residual - / maximum_sigma_2_times_2); - if (x >= stored_incomplete_gamma_number_min1 || x < 0 /*overflow*/) - x = stored_incomplete_gamma_number_min1; - total_loss -= (1 - two_ad_dof_plus_one_per_maximum_sigma * (maximum_sigma_2_per_2 * + auto x = static_cast(rescale_err * squared_residual); + if (x > stored_incomplete_gamma_number_min1) + x = stored_incomplete_gamma_number_min1; + total_loss -= (1 - (maximum_sigma_2_per_2 * stored_lower_incomplete_gamma_values[x] + squared_residual * 0.25 * - (stored_complete_gamma_values[x] - gamma_value_of_k)) / max_loss); + (stored_complete_gamma_values[x] - gamma_value_of_k)) * norm_loss); } - if (total_loss - (points_size - point_idx) > previous_best_loss) - break; } - return Score(num_tentative_inliers, total_loss); + return {num_tentative_inliers, total_loss}; } void setBestScore (double best_loss) override { @@ -279,20 +276,16 @@ public: { return Quality::getInliers(error, model, inliers, thr); } int getInliers (const Mat &model, std::vector &inliers_mask) const override { return Quality::getInliers(error, model, inliers_mask, tentative_inlier_threshold); } + double getThreshold () const override { return tentative_inlier_threshold; } int getPointsSize () const override { return points_size; } - Ptr clone () const override { - return makePtr(maximum_sigma, points_size, error->clone(), - tentative_inlier_threshold, degrees_of_freedom, - k, gamma_value_of_k, lower_gamma_value_of_k, C); - } + Ptr getErrorFnc () const override { return error; } }; Ptr MagsacQuality::create(double maximum_thr, int points_size_, const Ptr &error_, + const Ptr &gamma_generator, double tentative_inlier_threshold_, int DoF, double sigma_quantile, - double upper_incomplete_of_sigma_quantile, - double lower_incomplete_of_sigma_quantile, double C_) { - return makePtr(maximum_thr, points_size_, error_, - tentative_inlier_threshold_, DoF, sigma_quantile, upper_incomplete_of_sigma_quantile, - lower_incomplete_of_sigma_quantile, C_); + double upper_incomplete_of_sigma_quantile) { + return makePtr(maximum_thr, points_size_, error_, gamma_generator, + tentative_inlier_threshold_, DoF, sigma_quantile, upper_incomplete_of_sigma_quantile); } class LMedsQualityImpl : public LMedsQuality { @@ -312,7 +305,16 @@ public: if (errors[point] < threshold) inlier_number++; // score is median of errors - return Score(inlier_number, Utils::findMedian (errors)); + return {inlier_number, Utils::findMedian (errors)}; + } + Score getScore (const std::vector &errors_) const override { + std::vector errors = errors_; + int inlier_number = 0; + for (int point = 0; point < points_size; point++) + if (errors[point] < threshold) + inlier_number++; + // score is median of errors + return {inlier_number, Utils::findMedian (errors)}; } void setBestScore (double /*best_score*/) override {} @@ -324,10 +326,8 @@ public: { return Quality::getInliers(error, model, inliers, thr); } int getInliers (const Mat &model, std::vector &inliers_mask) const override { return Quality::getInliers(error, model, inliers_mask, threshold); } - - Ptr clone () const override { - return makePtr(points_size, threshold, error->clone()); - } + double getThreshold () const override { return threshold; } + Ptr getErrorFnc () const override { return error; } }; Ptr LMedsQuality::create(int points_size_, double threshold_, const Ptr &error_) { return makePtr(points_size_, threshold_, error_); @@ -335,47 +335,53 @@ Ptr LMedsQuality::create(int points_size_, double threshold_, cons class ModelVerifierImpl : public ModelVerifier { private: - std::vector errors; + Ptr quality; public: - inline bool isModelGood(const Mat &/*model*/) override { return true; } - inline bool getScore(Score &/*score*/) const override { return false; } - void update (int /*highest_inlier_number*/) override {} - const std::vector &getErrors() const override { return errors; } - bool hasErrors () const override { return false; } - Ptr clone (int /*state*/) const override { return makePtr();} + ModelVerifierImpl (const Ptr &q) : quality(q) {} + inline bool isModelGood(const Mat &model, Score &score) override { + score = quality->getScore(model); + return true; + } + void update (const Score &/*score*/, int /*iteration*/) override {} + void reset() override {} + void updateSPRT (double , double , double , double , double , const Score &) override {} }; -Ptr ModelVerifier::create() { - return makePtr(); +Ptr ModelVerifier::create(const Ptr &quality) { + return makePtr(quality); } -///////////////////////////////////// SPRT VERIFIER ////////////////////////////////////////// -class SPRTImpl : public SPRT { +class AdaptiveSPRTImpl : public AdaptiveSPRT { private: RNG rng; const Ptr err; + const Ptr quality; const int points_size; - int highest_inlier_number, current_sprt_idx; // i + int highest_inlier_number, last_iteration; // time t_M needed to instantiate a model hypothesis given a sample // Let m_S be the number of models that are verified per sample - const double inlier_threshold, norm_thr, one_over_thr, t_M, m_S; + const double inlier_threshold, norm_thr, one_over_thr; - double lowest_sum_errors, current_epsilon, current_delta, current_A, - delta_to_epsilon, complement_delta_to_complement_epsilon; + // alpha is false negative rate, alpha = 1 / A + double t_M, lowest_sum_errors, current_epsilon, current_delta, current_A, + delta_to_epsilon, complement_delta_to_complement_epsilon, + time_ver_corr_sprt = 0, time_ver_corr = 0, + one_over_complement_alpha, avg_num_checked_pts; - std::vector sprt_histories; + std::vector sprt_histories, empty; std::vector points_random_pool; std::vector errors; - Score score; + bool do_sprt, adapt, IS_ADAPTIVE; const ScoreMethod score_type; - bool last_model_is_good, can_compute_score, has_errors; + double m_S; public: - SPRTImpl (int state, const Ptr &err_, int points_size_, - double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model, - double time_sample, double avg_num_models, ScoreMethod score_type_) : rng(state), err(err_), - points_size(points_size_), inlier_threshold (inlier_threshold_), - norm_thr(inlier_threshold_*9/4), one_over_thr (1/norm_thr), t_M (time_sample), - m_S (avg_num_models), score_type (score_type_) { + AdaptiveSPRTImpl (int state, const Ptr &quality_, int points_size_, + double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model, + double time_sample, double avg_num_models, ScoreMethod score_type_, + double k_mlesac_, bool is_adaptive) : rng(state), err(quality_->getErrorFnc()), + quality(quality_), points_size(points_size_), inlier_threshold (quality->getThreshold()), + norm_thr(inlier_threshold_*k_mlesac_), one_over_thr (1/norm_thr), t_M (time_sample), + score_type (score_type_), m_S (avg_num_models) { // Generate array of random points for randomized evaluation points_random_pool = std::vector (points_size_); @@ -387,19 +393,23 @@ public: // reserve (approximately) some space for sprt vector. sprt_histories.reserve(20); - createTest(prob_pt_of_good_model, prob_pt_of_bad_model); - - highest_inlier_number = 0; + highest_inlier_number = last_iteration = 0; lowest_sum_errors = std::numeric_limits::max(); - last_model_is_good = false; - can_compute_score = score_type_ == ScoreMethod::SCORE_METHOD_MSAC - || score_type_ == ScoreMethod::SCORE_METHOD_RANSAC - || score_type_ == ScoreMethod::SCORE_METHOD_LMEDS; - // for MSAC and RANSAC errors not needed - if (score_type_ != ScoreMethod::SCORE_METHOD_MSAC && score_type_ != ScoreMethod::SCORE_METHOD_RANSAC) - errors = std::vector(points_size_); - // however return errors only if we can't compute score - has_errors = !can_compute_score; + if (score_type_ != ScoreMethod::SCORE_METHOD_MSAC) + errors = std::vector(points_size_); + IS_ADAPTIVE = is_adaptive; + delta_to_epsilon = one_over_complement_alpha = complement_delta_to_complement_epsilon = current_A = -1; + avg_num_checked_pts = points_size_; + adapt = IS_ADAPTIVE; + do_sprt = !IS_ADAPTIVE; + if (IS_ADAPTIVE) { + // all these variables will be initialized later + current_epsilon = prob_pt_of_good_model; + current_delta = prob_pt_of_bad_model; + } else { + current_epsilon = current_delta = 1e-5; + createTest(prob_pt_of_good_model, prob_pt_of_bad_model); + } } /* @@ -421,153 +431,143 @@ public: * @current_hypothesis: current RANSAC iteration * Return: true if model is good, false - otherwise. */ - inline bool isModelGood(const Mat& model) override - { - if (model.empty()) - return false; - + inline bool isModelGood (const Mat &model, Score &out_score) override { // update error object with current model - err->setModelParameters(model); - - double lambda = 1, sum_errors = 0; - last_model_is_good = true; - int random_pool_idx = rng.uniform(0, points_size), tested_point, tested_inliers = 0; - for (tested_point = 0; tested_point < points_size; tested_point++) { - if (random_pool_idx >= points_size) - random_pool_idx = 0; - const double error = err->getError (points_random_pool[random_pool_idx++]); - if (error < inlier_threshold) { - tested_inliers++; - lambda *= delta_to_epsilon; - } else { - lambda *= complement_delta_to_complement_epsilon; - // since delta is always higher than epsilon, then lambda can increase only - // when point is not consistent with model - if (lambda > current_A) - break; - } + bool last_model_is_good = true; + double sum_errors = 0; + int tested_inliers = 0; + if (! do_sprt || adapt) { // if adapt or not sprt then compute model score directly + out_score = quality->getScore(model); + tested_inliers = out_score.inlier_number; + sum_errors = out_score.score; + } else { // do sprt and not adapt + err->setModelParameters(model); + double lambda = 1; + int random_pool_idx = rng.uniform(0, points_size), tested_point; if (score_type == ScoreMethod::SCORE_METHOD_MSAC) { - if (error < norm_thr) - sum_errors -= (1 - error * one_over_thr); - if (sum_errors - points_size + tested_point > lowest_sum_errors) - break; - } else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC) { - if (tested_inliers + points_size - tested_point < highest_inlier_number) - break; - } else errors[points_random_pool[random_pool_idx-1]] = (float)error; + const auto preemptive_thr = points_size + lowest_sum_errors; + for (tested_point = 0; tested_point < points_size; tested_point++) { + if (random_pool_idx == points_size) + random_pool_idx = 0; + const float error = err->getError (points_random_pool[random_pool_idx++]); + if (error < inlier_threshold) { + tested_inliers++; + lambda *= delta_to_epsilon; + } else { + lambda *= complement_delta_to_complement_epsilon; + // since delta is always higher than epsilon, then lambda can increase only + // when point is not consistent with model + if (lambda > current_A) + break; + } + if (error < norm_thr) + sum_errors -= (1 - error * one_over_thr); + else if (sum_errors + tested_point > preemptive_thr) + break; + } + } else { // save errors into array here + for (tested_point = 0; tested_point < points_size; tested_point++) { + if (random_pool_idx == points_size) + random_pool_idx = 0; + const int pt = points_random_pool[random_pool_idx++]; + const float error = err->getError (pt); + if (error < inlier_threshold) { + tested_inliers++; + lambda *= delta_to_epsilon; + } else { + lambda *= complement_delta_to_complement_epsilon; + if (lambda > current_A) + break; + } + errors[pt] = error; + } + } + last_model_is_good = tested_point == points_size; } - last_model_is_good = tested_point == points_size; - - // increase number of samples processed by current test - sprt_histories[current_sprt_idx].tested_samples++; - if (last_model_is_good) { - score.inlier_number = tested_inliers; - if (score_type == ScoreMethod::SCORE_METHOD_MSAC) { - score.score = sum_errors; - if (lowest_sum_errors > sum_errors) - lowest_sum_errors = sum_errors; - } else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC) - score.score = -static_cast(tested_inliers); - else if (score_type == ScoreMethod::SCORE_METHOD_LMEDS) - score.score = Utils::findMedian(errors); - - const double new_epsilon = static_cast(tested_inliers) / points_size; - if (new_epsilon > current_epsilon) { - highest_inlier_number = tested_inliers; // update max inlier number - /* - * Model accepted and the largest support so far: - * design (i+1)-th test (εi + 1= εˆ, δi+1 = δ, i := i + 1). - * Store the current model parameters θ - */ - createTest(new_epsilon, current_delta); - } - } else { - /* - * Since almost all tested models are ‘bad’, the probability - * δ can be estimated as the average fraction of consistent data points - * in rejected models. - */ - // add 1 to tested_point, because loop over tested_point starts from 0 - const double delta_estimated = static_cast (tested_inliers) / (tested_point+1); - if (delta_estimated > 0 && fabs(current_delta - delta_estimated) - / current_delta > 0.05) - /* - * Model rejected: re-estimate δ. If the estimate δ_ differs - * from δi by more than 5% design (i+1)-th test (εi+1 = εi, - * δi+1 = δˆ, i := i + 1) - */ - createTest(current_epsilon, delta_estimated); + if (last_model_is_good && do_sprt) { + out_score.inlier_number = tested_inliers; + if (score_type == ScoreMethod::SCORE_METHOD_MSAC) + out_score.score = sum_errors; + else if (score_type == ScoreMethod::SCORE_METHOD_RANSAC) + out_score.score = -static_cast(tested_inliers); + else out_score = quality->getScore(errors); } return last_model_is_good; } - inline bool getScore (Score &score_) const override { - if (!last_model_is_good || !can_compute_score) - return false; - score_ = score; - return true; - } - bool hasErrors () const override { return has_errors; } - const std::vector &getErrors () const override { return errors; } - const std::vector &getSPRTvector () const override { return sprt_histories; } - void update (int highest_inlier_number_) override { - const double new_epsilon = static_cast(highest_inlier_number_) / points_size; - if (new_epsilon > current_epsilon) { - highest_inlier_number = highest_inlier_number_; - if (sprt_histories[current_sprt_idx].tested_samples == 0) - sprt_histories[current_sprt_idx].tested_samples = 1; - // save sprt test and create new one - createTest(new_epsilon, current_delta); + // update SPRT parameters = called only once inside usac + void updateSPRT (double time_model_est, double time_corr_ver, double new_avg_models, double new_delta, double new_epsilon, const Score &best_score) override { + if (adapt) { + adapt = false; + m_S = new_avg_models; + t_M = time_model_est / time_corr_ver; + time_ver_corr = time_corr_ver; + time_ver_corr_sprt = time_corr_ver * 1.05; + createTest(new_epsilon, new_delta); + highest_inlier_number = best_score.inlier_number; + lowest_sum_errors = best_score.score; } } - Ptr clone (int state) const override { - return makePtr(state, err->clone(), points_size, inlier_threshold, - sprt_histories[current_sprt_idx].epsilon, - sprt_histories[current_sprt_idx].delta, t_M, m_S, score_type); + + const std::vector &getSPRTvector () const override { return adapt ? empty : sprt_histories; } + void update (const Score &score, int iteration) override { + if (adapt || highest_inlier_number > score.inlier_number) + return; + + if (sprt_histories.size() == 1 && sprt_histories.back().tested_samples == 0) + sprt_histories.back().tested_samples = iteration; + else if (! sprt_histories.empty()) + sprt_histories.back().tested_samples += iteration - last_iteration; + + SPRT_history new_sprt_history; + new_sprt_history.epsilon = (double)score.inlier_number / points_size; + highest_inlier_number = score.inlier_number; + lowest_sum_errors = score.score; + createTest(static_cast(highest_inlier_number) / points_size, current_delta); + new_sprt_history.delta = current_delta; + new_sprt_history.A = current_A; + sprt_histories.emplace_back(new_sprt_history); + last_iteration = iteration; + } + int avgNumCheckedPts () const override { return do_sprt ? (int)avg_num_checked_pts + 1 : points_size; } + void reset() override { + adapt = true; + do_sprt = false; + highest_inlier_number = last_iteration = 0; + lowest_sum_errors = DBL_MAX; + sprt_histories.clear(); } private: - - // Saves sprt test to sprt history and update current epsilon, delta and threshold. - void createTest (double epsilon, double delta) { + // Update current epsilon, delta and threshold (A). + bool createTest (double epsilon, double delta) { + if (fabs(current_epsilon - epsilon) < FLT_EPSILON && fabs(current_delta - delta) < FLT_EPSILON) + return false; // if epsilon is closed to 1 then set them to 0.99 to avoid numerical problems if (epsilon > 0.999999) epsilon = 0.999; // delta can't be higher than epsilon, because ratio delta / epsilon will be greater than 1 - if (epsilon < delta) delta = epsilon-0.0001; + if (epsilon < delta) delta = epsilon-0.001; // avoid delta going too high as it is very unlikely // e.g., 30% of points are consistent with bad model is not very real if (delta > 0.3) delta = 0.3; - SPRT_history new_sprt_history; - new_sprt_history.epsilon = epsilon; - new_sprt_history.delta = delta; - new_sprt_history.A = estimateThresholdA (epsilon, delta); - - sprt_histories.emplace_back(new_sprt_history); - - current_A = new_sprt_history.A; + const auto AC = estimateThresholdA (epsilon, delta); + current_A = AC.first; + const auto C = AC.second; current_delta = delta; current_epsilon = epsilon; + one_over_complement_alpha = 1 / (1 - 1 / current_A); delta_to_epsilon = delta / epsilon; complement_delta_to_complement_epsilon = (1 - delta) / (1 - epsilon); - current_sprt_idx = static_cast(sprt_histories.size()) - 1; - } - /* - * A(0) = K1/K2 + 1 - * A(n+1) = K1/K2 + 1 + log (A(n)) - * K1 = t_M / P_g - * K2 = m_S/(P_g*C) - * t_M is time needed to instantiate a model hypotheses given a sample - * P_g = epsilon ^ m, m is the number of data point in the Ransac sample. - * m_S is the number of models that are verified per sample. - * p (0|Hb) p (1|Hb) - * C = p(0|Hb) log (---------) + p(1|Hb) log (---------) - * p (0|Hg) p (1|Hg) - */ - double estimateThresholdA (double epsilon, double delta) { - const double C = (1 - delta) * log ((1 - delta) / (1 - epsilon)) + - delta * (log(delta / epsilon)); + if (IS_ADAPTIVE) { + avg_num_checked_pts = std::min((log(current_A) / C) * one_over_complement_alpha, (double)points_size); + do_sprt = time_ver_corr_sprt * avg_num_checked_pts < time_ver_corr * points_size; + } + return true; + } + std::pair estimateThresholdA (double epsilon, double delta) { + const double C = (1 - delta) * log ((1 - delta) / (1 - epsilon)) + delta * log (delta / epsilon); // K = K1/K2 + 1 = (t_M / P_g) / (m_S / (C * P_g)) + 1 = (t_M * C)/m_S + 1 const double K = t_M * C / m_S + 1; double An, An_1 = K; @@ -579,13 +579,13 @@ private: break; An_1 = An; } - return An; + return std::make_pair(An, C); } }; -Ptr SPRT::create (int state, const Ptr &err_, int points_size_, - double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model, - double time_sample, double avg_num_models, ScoreMethod score_type_) { - return makePtr(state, err_, points_size_, inlier_threshold_, - prob_pt_of_good_model, prob_pt_of_bad_model, time_sample, avg_num_models, score_type_); +Ptr AdaptiveSPRT::create (int state, const Ptr &quality, int points_size_, + double inlier_threshold_, double prob_pt_of_good_model, double prob_pt_of_bad_model, + double time_sample, double avg_num_models, ScoreMethod score_type_, double k_mlesac, bool is_adaptive) { + return makePtr(state, quality, points_size_, inlier_threshold_, + prob_pt_of_good_model, prob_pt_of_bad_model, time_sample, avg_num_models, score_type_, k_mlesac, is_adaptive); } }} diff --git a/modules/calib3d/src/usac/ransac_solvers.cpp b/modules/calib3d/src/usac/ransac_solvers.cpp index cca13405bc..1a76353331 100644 --- a/modules/calib3d/src/usac/ransac_solvers.cpp +++ b/modules/calib3d/src/usac/ransac_solvers.cpp @@ -6,51 +6,54 @@ #include "../usac.hpp" #include -namespace cv { namespace usac { +namespace cv { +UsacParams::UsacParams() { + confidence=0.99; + isParallel=false; + loIterations=5; + loMethod=LOCAL_OPTIM_INNER_LO; + loSampleSize=14; + maxIterations=5000; + neighborsSearch=NEIGH_GRID; + randomGeneratorState=0; + sampler=SAMPLING_UNIFORM; + score=SCORE_METHOD_MSAC; + threshold=1.5; + final_polisher=COV_POLISHER; + final_polisher_iterations=3; +} + +namespace usac { int mergePoints (InputArray pts1_, InputArray pts2_, Mat &pts, bool ispnp); void setParameters (int flag, Ptr ¶ms, EstimationMethod estimator, double thr, int max_iters, double conf, bool mask_needed); class RansacOutputImpl : public RansacOutput { private: - Mat model; - // vector of number_inliers size std::vector inliers; - // vector of points size, true if inlier, false-outlier + cv::Mat model, K1, K2; + // vector of number_inliers size + // vector of points size, true if inlier, false - outlier std::vector inliers_mask; // vector of points size, value of i-th index corresponds to error of i-th point if i is inlier. - std::vector errors; - // the best found score of RANSAC - double score; - - int seconds, milliseconds, microseconds; - int time_mcs, number_inliers, number_estimated_models, number_good_models; - int number_iterations; // number of iterations of main RANSAC + std::vector residuals; + int number_inliers, number_iterations; + ModelConfidence conf; public: - RansacOutputImpl (const Mat &model_, const std::vector &inliers_mask_, - int time_mcs_, double score_, int number_inliers_, int number_iterations_, - int number_estimated_models_, int number_good_models_) { - + RansacOutputImpl (const cv::Mat &model_, const std::vector &inliers_mask_, int number_inliers_, + int number_iterations_, ModelConfidence conf_, const std::vector &errors_) { model_.copyTo(model); inliers_mask = inliers_mask_; - time_mcs = time_mcs_; - score = score_; number_inliers = number_inliers_; number_iterations = number_iterations_; - number_estimated_models = number_estimated_models_; - number_good_models = number_good_models_; - microseconds = time_mcs % 1000; - milliseconds = ((time_mcs - microseconds)/1000) % 1000; - seconds = ((time_mcs - 1000*milliseconds - microseconds)/(1000*1000)) % 60; + residuals = errors_; + conf = conf_; } - /* - * Return inliers' indices. - * size of vector = number of inliers - */ + // Return inliers' indices of size = number of inliers const std::vector &getInliers() override { if (inliers.empty()) { - inliers.reserve(inliers_mask.size()); + inliers.reserve(number_inliers); int pt_cnt = 0; for (bool is_inlier : inliers_mask) { if (is_inlier) @@ -60,319 +63,968 @@ public: } return inliers; } - - // Return inliers mask. Vector of points size. 1-inlier, 0-outlier. - const std::vector &getInliersMask() const override { return inliers_mask; } - - int getTimeMicroSeconds() const override {return time_mcs; } - int getTimeMicroSeconds1() const override {return microseconds; } - int getTimeMilliSeconds2() const override {return milliseconds; } - int getTimeSeconds3() const override {return seconds; } - int getNumberOfInliers() const override { return number_inliers; } - int getNumberOfMainIterations() const override { return number_iterations; } - int getNumberOfGoodModels () const override { return number_good_models; } - int getNumberOfEstimatedModels () const override { return number_estimated_models; } - const Mat &getModel() const override { return model; } + const std::vector &getInliersMask() const override { + return inliers_mask; + } + int getNumberOfInliers() const override { + return number_inliers; + } + const Mat &getModel() const override { + return model; + } + int getNumberOfIters() const override { + return number_iterations; + } + ModelConfidence getConfidence() const override { + return conf; + } + const std::vector &getResiduals() const override { + return residuals; + } }; -Ptr RansacOutput::create(const Mat &model_, const std::vector &inliers_mask_, - int time_mcs_, double score_, int number_inliers_, int number_iterations_, - int number_estimated_models_, int number_good_models_) { - return makePtr(model_, inliers_mask_, time_mcs_, score_, number_inliers_, - number_iterations_, number_estimated_models_, number_good_models_); +Ptr RansacOutput::create(const cv::Mat &model_, const std::vector &inliers_mask_, int number_inliers_, + int number_iterations_, ModelConfidence conf, const std::vector &errors_) { + return makePtr(model_, inliers_mask_, number_inliers_, + number_iterations_, conf, errors_); +} + +double getLambda (std::vector &supports, double cdf_thr, int points_size, + int sample_size, bool is_independent, int &min_non_random_inliers) { + std::sort(supports.begin(), supports.end()); + double lambda = supports.size() % 2 ? (supports[supports.size()/2] + supports[supports.size()/2+1])*0.5 : supports[supports.size()/2]; + const double cdf = lambda + cdf_thr*sqrt(lambda * (1 - lambda / (is_independent ? points_size - sample_size : points_size))); + int lower_than_cdf = 0; lambda = 0; + for (const auto &inl : supports) + if (inl < cdf) { + lambda += inl; lower_than_cdf++; + } else break; // list is sorted + lambda /= lower_than_cdf; + if (lambda < 1 || lower_than_cdf == 0) lambda = 1; + // use 0.9999 quantile https://keisan.casio.com/exec/system/14060745333941 + if (! is_independent) // do not calculate it for all inliers + min_non_random_inliers = (int)(lambda + 2.32*sqrt(lambda * (1 - lambda / points_size))) + 1; + return lambda; } class Ransac { -protected: - const Ptr params; - const Ptr _estimator; - const Ptr _quality; - const Ptr _sampler; - const Ptr _termination_criteria; - const Ptr _model_verifier; - const Ptr _degeneracy; - const Ptr _local_optimization; - const Ptr model_polisher; - - const int points_size, state; - const bool parallel; public: + const Ptr params; + Ptr _estimator; + Ptr _error; + Ptr _quality; + Ptr _sampler; + Ptr _termination; + Ptr _model_verifier; + Ptr _degeneracy; + Ptr _local_optimization; + Ptr polisher; + Ptr _gamma_generator; + Ptr _min_solver; + Ptr _lo_solver, _fo_solver; + Ptr _lo_sampler; + Ptr _weight_fnc; - Ransac (const Ptr ¶ms_, int points_size_, const Ptr &estimator_, const Ptr &quality_, - const Ptr &sampler_, const Ptr &termination_criteria_, - const Ptr &model_verifier_, const Ptr °eneracy_, - const Ptr &local_optimization_, const Ptr &model_polisher_, - bool parallel_=false, int state_ = 0) : + int points_size, _state, filtered_points_size; + double threshold, max_thr; + bool parallel; - params (params_), _estimator (estimator_), _quality (quality_), _sampler (sampler_), - _termination_criteria (termination_criteria_), _model_verifier (model_verifier_), - _degeneracy (degeneracy_), _local_optimization (local_optimization_), - model_polisher (model_polisher_), points_size (points_size_), state(state_), - parallel(parallel_) {} + Matx33d T1, T2; + Mat points, K1, K2, calib_points, image_points, norm_points, filtered_points; + Ptr graph; + std::vector> layers; + + Ransac (const Ptr ¶ms_, cv::InputArray points1, cv::InputArray points2, + cv::InputArray K1_, cv::InputArray K2_, cv::InputArray dist_coeff1, cv::InputArray dist_coeff2) : params(params_) { + _state = params->getRandomGeneratorState(); + threshold = params->getThreshold(); + max_thr = std::max(threshold, params->getMaximumThreshold()); + parallel = params->isParallel(); + Mat undist_points1, undist_points2; + if (params->isPnP()) { + if (! K1_.empty()) { + K1 = K1_.getMat().clone(); K1.convertTo(K1, CV_64F); + if (! dist_coeff1.empty()) { + // undistortPoints also calibrate points using K + undistortPoints(points1.isContinuous() ? points1 : points1.getMat().clone(), undist_points1, K1_, dist_coeff1); + points_size = mergePoints(undist_points1, points2, points, true); + Utils::normalizeAndDecalibPointsPnP (K1, points, calib_points); + } else { + points_size = mergePoints(points1, points2, points, true); + Utils::calibrateAndNormalizePointsPnP(K1, points, calib_points); + } + } else points_size = mergePoints(points1, points2, points, true); + } else { + if (params->isEssential()) { + CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!"); + K1 = K1_.getMat(); K1.convertTo(K1, CV_64F); + K2 = K2_.getMat(); K2.convertTo(K2, CV_64F); + if (! dist_coeff1.empty() || ! dist_coeff2.empty()) { + // undistortPoints also calibrate points using K + if (! dist_coeff1.empty()) undistortPoints(points1.isContinuous() ? points1 : points1.getMat().clone(), undist_points1, K1_, dist_coeff1); + else undist_points1 = points1.getMat(); + if (! dist_coeff2.empty()) undistortPoints(points2.isContinuous() ? points2 : points2.getMat().clone(), undist_points2, K2_, dist_coeff2); + else undist_points2 = points2.getMat(); + points_size = mergePoints(undist_points1, undist_points2, calib_points, false); + } else { + points_size = mergePoints(points1, points2, points, false); + Utils::calibratePoints(K1, K2, points, calib_points); + } + threshold = Utils::getCalibratedThreshold(threshold, K1, K2); + max_thr = Utils::getCalibratedThreshold(max_thr, K1, K2); + } else { + points_size = mergePoints(points1, points2, points, false); + if (params->isFundamental() && ! K1_.empty() && ! K2_.empty()) { + K1 = K1_.getMat(); K1.convertTo(K1, CV_64F); + K2 = K2_.getMat(); K2.convertTo(K2, CV_64F); + Utils::calibratePoints(K1, K2, points, calib_points); + } + } + } + + if (params->getSampler() == SamplingMethod::SAMPLING_NAPSAC || params->getLO() == LocalOptimMethod::LOCAL_OPTIM_GC) { + if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_GRID) { + graph = GridNeighborhoodGraph::create(points, points_size, + params->getCellSize(), params->getCellSize(), params->getCellSize(), params->getCellSize(), 10); + } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_KNN) { + graph = FlannNeighborhoodGraph::create(points, points_size,params->getKNN(), false, 5, 1); + } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_RADIUS) { + graph = RadiusSearchNeighborhoodGraph::create(points, points_size, params->getGraphRadius(), 5, 1); + } else CV_Error(cv::Error::StsNotImplemented, "Graph type is not implemented!"); + } + + if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) { + CV_CheckEQ((int)params->isPnP(), 0, "ProgressiveNAPSAC for PnP is not implemented!"); + const auto &cell_number_per_layer = params->getGridCellNumber(); + layers.reserve(cell_number_per_layer.size()); + const auto * const pts = (float *) points.data; + float img1_width = 0, img1_height = 0, img2_width = 0, img2_height = 0; + for (int i = 0; i < 4 * points_size; i += 4) { + if (pts[i ] > img1_width ) img1_width = pts[i ]; + if (pts[i + 1] > img1_height) img1_height = pts[i + 1]; + if (pts[i + 2] > img2_width ) img2_width = pts[i + 2]; + if (pts[i + 3] > img2_height) img2_height = pts[i + 3]; + } + // Create grid graphs (overlapping layes of given cell numbers) + for (int layer_idx = 0; layer_idx < (int)cell_number_per_layer.size(); layer_idx++) { + const int cell_number = cell_number_per_layer[layer_idx]; + if (layer_idx > 0) + if (cell_number_per_layer[layer_idx-1] <= cell_number) + CV_Error(cv::Error::StsError, "Progressive NAPSAC sampler: " + "Cell number in layers must be in decreasing order!"); + layers.emplace_back(GridNeighborhoodGraph::create(points, points_size, + (int)(img1_width / (float)cell_number), (int)(img1_height / (float)cell_number), + (int)(img2_width / (float)cell_number), (int)(img2_height / (float)cell_number), 10)); + } + } + + // update points by calibrated for Essential matrix after graph is calculated + if (params->isEssential()) { + image_points = points; + points = calib_points; + // if maximum calibrated threshold significanlty differs threshold then set upper bound + if (max_thr > 10*threshold) + max_thr = 10*threshold; + } + + // Since error function output squared error distance, so make + // threshold squared as well + threshold *= threshold; + + if ((params->isHomography() || (params->isFundamental() && (K1.empty() || K2.empty() || !params->isLarssonOptimization())) || + params->getEstimator() == EstimationMethod::AFFINE) && (params->getLO() != LOCAL_OPTIM_NULL || params->getFinalPolisher() == COV_POLISHER)) { + const auto normTr = NormTransform::create(points); + std::vector sample (points_size); + for (int i = 0; i < points_size; i++) sample[i] = i; + normTr->getNormTransformation(norm_points, sample, points_size, T1, T2); + } + + if (params->getScore() == SCORE_METHOD_MAGSAC || params->getLO() == LOCAL_OPTIM_SIGMA || params->getFinalPolisher() == MAGSAC) + _gamma_generator = GammaValues::create(params->getDegreesOfFreedom()); // is thread safe + initialize (_state, _min_solver, _lo_solver, _error, _estimator, _degeneracy, _quality, + _model_verifier, _local_optimization, _termination, _sampler, _lo_sampler, _weight_fnc, false/*parallel*/); + if (params->getFinalPolisher() != NONE_POLISHER) { + polisher = NonMinimalPolisher::create(_quality, _fo_solver, + params->getFinalPolisher() == MAGSAC ? _weight_fnc : nullptr, params->getFinalLSQIterations(), 0.99); + } + } + + void initialize (int state, Ptr &min_solver, Ptr &non_min_solver, + Ptr &error, Ptr &estimator, Ptr °eneracy, Ptr &quality, + Ptr &verifier, Ptr &lo, Ptr &termination, + Ptr &sampler, Ptr &lo_sampler, Ptr &weight_fnc, bool parallel_call) { + + const int min_sample_size = params->getSampleSize(), prosac_termination_length = std::min((int)(.5*points_size), 100); + // inner inlier threshold will be used in LO to obtain inliers + // additionally in DEGENSAC for F + double inner_inlier_thr_sqr = threshold; + if (params->isHomography() && inner_inlier_thr_sqr < 5.25) inner_inlier_thr_sqr = 5.25; // at least 2.5 px + else if (params->isFundamental() && inner_inlier_thr_sqr < 4) inner_inlier_thr_sqr = 4; // at least 2 px + + if (params->getFinalPolisher() == MAGSAC || params->getLO() == LOCAL_OPTIM_SIGMA) + weight_fnc = MagsacWeightFunction::create(_gamma_generator, params->getDegreesOfFreedom(), params->getUpperIncompleteOfSigmaQuantile(), params->getC(), params->getMaximumThreshold()); + else weight_fnc = nullptr; + + switch (params->getError()) { + case ErrorMetric::SYMM_REPR_ERR: + error = ReprojectionErrorSymmetric::create(points); break; + case ErrorMetric::FORW_REPR_ERR: + if (params->getEstimator() == EstimationMethod::AFFINE) + error = ReprojectionErrorAffine::create(points); + else error = ReprojectionErrorForward::create(points); + break; + case ErrorMetric::SAMPSON_ERR: + error = SampsonError::create(points); break; + case ErrorMetric::SGD_ERR: + error = SymmetricGeometricDistance::create(points); break; + case ErrorMetric::RERPOJ: + error = ReprojectionErrorPmatrix::create(points); break; + default: CV_Error(cv::Error::StsNotImplemented , "Error metric is not implemented!"); + } + + const double k_mlesac = params->getKmlesac (); + switch (params->getScore()) { + case ScoreMethod::SCORE_METHOD_RANSAC : + quality = RansacQuality::create(points_size, threshold, error); break; + case ScoreMethod::SCORE_METHOD_MSAC : + quality = MsacQuality::create(points_size, threshold, error, k_mlesac); break; + case ScoreMethod::SCORE_METHOD_MAGSAC : + quality = MagsacQuality::create(max_thr, points_size, error, _gamma_generator, + threshold, params->getDegreesOfFreedom(), params->getSigmaQuantile(), + params->getUpperIncompleteOfSigmaQuantile()); break; + case ScoreMethod::SCORE_METHOD_LMEDS : + quality = LMedsQuality::create(points_size, threshold, error); break; + default: CV_Error(cv::Error::StsNotImplemented, "Score is not imeplemeted!"); + } + + const auto is_ge_solver = params->getRansacSolver() == GEM_SOLVER; + if (params->isHomography()) { + degeneracy = HomographyDegeneracy::create(points); + min_solver = HomographyMinimalSolver4pts::create(points, is_ge_solver); + non_min_solver = HomographyNonMinimalSolver::create(norm_points, T1, T2, true); + estimator = HomographyEstimator::create(min_solver, non_min_solver, degeneracy); + if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) { + if (params->getFinalPolisher() == COV_POLISHER) + _fo_solver = CovarianceHomographySolver::create(norm_points, T1, T2); + else _fo_solver = HomographyNonMinimalSolver::create(points); + } + } else if (params->isFundamental()) { + if (K1.empty() || K2.empty()) { + degeneracy = FundamentalDegeneracy::create(state++, quality, points, min_sample_size, + params->getPlaneAndParallaxIters(), std::max(threshold, 8.) /*sqr homogr thr*/, inner_inlier_thr_sqr, K1, K2); + } else degeneracy = FundamentalDegeneracyViaE::create(quality, points, calib_points, K1, K2, true/*is F*/); + if (min_sample_size == 7) { + min_solver = FundamentalMinimalSolver7pts::create(points, is_ge_solver); + } else min_solver = FundamentalMinimalSolver8pts::create(points); + if (params->isLarssonOptimization() && !K1.empty() && !K2.empty()) { + non_min_solver = LarssonOptimizer::create(calib_points, K1, K2, params->getLevMarqItersLO(), true/*F*/); + } else { + if (weight_fnc) + non_min_solver = EpipolarNonMinimalSolver::create(points, true); + else + non_min_solver = EpipolarNonMinimalSolver::create(norm_points, T1, T2, true); + } + estimator = FundamentalEstimator::create(min_solver, non_min_solver, degeneracy); + if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) { + if (params->isLarssonOptimization() && !K1.empty() && !K2.empty()) + _fo_solver = LarssonOptimizer::create(calib_points, K1, K2, params->getLevMarqIters(), true/*F*/); + else if (params->getFinalPolisher() == COV_POLISHER) + _fo_solver = CovarianceEpipolarSolver::create(norm_points, T1, T2); + else _fo_solver = EpipolarNonMinimalSolver::create(points, true); + } + } else if (params->isEssential()) { + if (params->getEstimator() == EstimationMethod::ESSENTIAL) { + min_solver = EssentialMinimalSolver5pts::create(points, !is_ge_solver, true/*Nister*/); + degeneracy = EssentialDegeneracy::create(points, min_sample_size); + } + non_min_solver = LarssonOptimizer::create(calib_points, K1, K2, params->getLevMarqItersLO(), false/*E*/); + estimator = EssentialEstimator::create(min_solver, non_min_solver, degeneracy); + if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) + _fo_solver = LarssonOptimizer::create(calib_points, K1, K2, params->getLevMarqIters(), false/*E*/); + } else if (params->isPnP()) { + degeneracy = makePtr(); + if (min_sample_size == 3) { + min_solver = P3PSolver::create(points, calib_points, K1); + non_min_solver = DLSPnP::create(points, calib_points, K1); + } else { + if (is_ge_solver) + min_solver = PnPMinimalSolver6Pts::create(points); + else min_solver = PnPSVDSolver::create(points); + non_min_solver = PnPNonMinimalSolver::create(points); + } + estimator = PnPEstimator::create(min_solver, non_min_solver); + if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) _fo_solver = non_min_solver; + } else if (params->getEstimator() == EstimationMethod::AFFINE) { + degeneracy = makePtr(); + min_solver = AffineMinimalSolver::create(points); + non_min_solver = AffineNonMinimalSolver::create(points, cv::noArray(), cv::noArray()); + estimator = AffineEstimator::create(min_solver, non_min_solver); + if (!parallel_call && params->getFinalPolisher() != NONE_POLISHER) { + if (params->getFinalPolisher() == COV_POLISHER) + _fo_solver = CovarianceAffineSolver::create(points); + else _fo_solver = non_min_solver; + } + } else CV_Error(cv::Error::StsNotImplemented, "Estimator not implemented!"); + + switch (params->getSampler()) { + case SamplingMethod::SAMPLING_UNIFORM: + sampler = UniformSampler::create(state++, min_sample_size, points_size); + break; + case SamplingMethod::SAMPLING_PROSAC: + if (!parallel_call) // for parallel only one PROSAC sampler + sampler = ProsacSampler::create(state++, points_size, min_sample_size, params->getProsacMaxSamples()); + break; + case SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC: + sampler = ProgressiveNapsac::create(state++, points_size, min_sample_size, layers, 20); break; + case SamplingMethod::SAMPLING_NAPSAC: + sampler = NapsacSampler::create(state++, points_size, min_sample_size, graph); break; + default: CV_Error(cv::Error::StsNotImplemented, "Sampler is not implemented!"); + } + + const bool is_sprt = params->getVerifier() == VerificationMethod::SPRT_VERIFIER || params->getVerifier() == VerificationMethod::ASPRT; + if (is_sprt) + verifier = AdaptiveSPRT::create(state++, quality, points_size, params->getScore() == ScoreMethod ::SCORE_METHOD_MAGSAC ? max_thr : threshold, + params->getSPRTepsilon(), params->getSPRTdelta(), params->getTimeForModelEstimation(), + params->getSPRTavgNumModels(), params->getScore(), k_mlesac, params->getVerifier() == VerificationMethod::ASPRT); + else if (params->getVerifier() == VerificationMethod::NULL_VERIFIER) + verifier = ModelVerifier::create(quality); + else CV_Error(cv::Error::StsNotImplemented, "Verifier is not imeplemented!"); + + if (params->getSampler() == SamplingMethod::SAMPLING_PROSAC) { + if (parallel_call) { + termination = ProsacTerminationCriteria::create(nullptr, error, + points_size, min_sample_size, params->getConfidence(), params->getMaxIters(), prosac_termination_length, 0.05, 0.05, threshold, + _termination.dynamicCast()->getNonRandomInliers()); + } else { + termination = ProsacTerminationCriteria::create(sampler.dynamicCast(), error, + points_size, min_sample_size, params->getConfidence(), params->getMaxIters(), prosac_termination_length, 0.05, 0.05, threshold, + std::vector()); + } + } else if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) { + if (is_sprt) + termination = SPRTPNapsacTermination::create(verifier.dynamicCast(), + params->getConfidence(), points_size, min_sample_size, + params->getMaxIters(), params->getRelaxCoef()); + else termination = StandardTerminationCriteria::create (params->getConfidence(), + points_size, min_sample_size, params->getMaxIters()); + } else if (is_sprt && params->getLO() == LocalOptimMethod::LOCAL_OPTIM_NULL) { + termination = SPRTTermination::create(verifier.dynamicCast(), + params->getConfidence(), points_size, min_sample_size, params->getMaxIters()); + } else { + termination = StandardTerminationCriteria::create + (params->getConfidence(), points_size, min_sample_size, params->getMaxIters()); + } + + // if normal ransac or parallel call, avoid redundant init + if ((! params->isParallel() || parallel_call) && params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL) { + lo_sampler = UniformRandomGenerator::create(state, points_size, params->getLOSampleSize()); + const auto lo_termination = StandardTerminationCriteria::create(params->getConfidence(), points_size, min_sample_size, params->getMaxIters()); + switch (params->getLO()) { + case LocalOptimMethod::LOCAL_OPTIM_INNER_LO: case LocalOptimMethod::LOCAL_OPTIM_SIGMA: + lo = SimpleLocalOptimization::create(quality, non_min_solver, lo_termination, lo_sampler, + weight_fnc, params->getLOInnerMaxIters(), inner_inlier_thr_sqr, true); break; + case LocalOptimMethod::LOCAL_OPTIM_INNER_AND_ITER_LO: + lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler, + points_size, threshold, true, params->getLOIterativeSampleSize(), + params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(), + params->getLOThresholdMultiplier()); break; + case LocalOptimMethod::LOCAL_OPTIM_GC: + lo = GraphCut::create(estimator, quality, graph, lo_sampler, threshold, + params->getGraphCutSpatialCoherenceTerm(), params->getLOInnerMaxIters(), lo_termination); break; + default: CV_Error(cv::Error::StsNotImplemented , "Local Optimization is not implemented!"); + } + } + } + + int getIndependentInliers (const Mat &model_, const std::vector &sample, + std::vector &inliers, const int num_inliers_) { + bool is_F = params->isFundamental(); + Mat model = model_; + int sample_size = 0; + if (is_F) sample_size = 7; + else if (params->isHomography()) sample_size = 4; + else if (params->isEssential()) { + is_F = true; + // convert E to F + model = Mat(Matx33d(K2).inv().t() * Matx33d(model) * Matx33d(K1).inv()); + sample_size = 5; + } else if (params->isPnP() || params->getEstimator() == EstimationMethod::AFFINE) sample_size = 3; + else + CV_Error(cv::Error::StsNotImplemented, "Method for independent inliers is not implemented for this problem"); + if (num_inliers_ <= sample_size) return 0; // minimal sample size generates model + model.convertTo(model, CV_32F); + int num_inliers = num_inliers_, num_pts_near_ep = 0, + num_pts_validatin_or_constr = 0, pt1 = 0; + const auto * const pts = params->isEssential() ? (float *) image_points.data : (float *) points.data; + // scale for thresholds should be used + const float ep_thr_sqr = 0.000001f, line_thr = 0.01f, neigh_thr = 4.0f; + float sign1=0,a1=0, b1=0, c1=0, a2=0, b2=0, c2=0, ep1_x, ep1_y, ep2_x, ep2_y; + const auto * const m = (float *) model.data; + Vec3f ep1; + bool do_or_test = false, ep1_inf = false, ep2_inf = false; + if (is_F) { // compute epipole and sign of the first point for orientation test + model *= (1/norm(model)); + ep1 = Utils::getRightEpipole(model); + const Vec3f ep2 = Utils::getLeftEpipole(model); + if (fabsf(ep1[2]) < DBL_EPSILON) { + ep1_inf = true; + } else { + ep1_x = ep1[0] / ep1[2]; + ep1_y = ep1[1] / ep1[2]; + } + if (fabsf(ep2[2]) < DBL_EPSILON) { + ep2_inf = true; + } else { + ep2_x = ep2[0] / ep2[2]; + ep2_y = ep2[1] / ep2[2]; + } + } + const auto * const e1 = ep1.val; // of size 3x1 + + // we move sample points to the end, so every inlier will be checked by sample point + int num_sample_in_inliers = 0; + if (!sample.empty()) { + num_sample_in_inliers = 0; + int temp_idx = num_inliers; + for (int i = 0; i < temp_idx; i++) { + const int inl = inliers[i]; + for (int s : sample) { + if (inl == s) { + std::swap(inliers[i], inliers[--temp_idx]); + i--; // we need to check inlier that we just swapped + num_sample_in_inliers++; + break; + } + } + } + } + + if (is_F) { + int MIN_TEST = std::min(15, num_inliers); + for (int i = 0; i < MIN_TEST; i++) { + pt1 = 4*inliers[i]; + sign1 = (m[0]*pts[pt1+2]+m[3]*pts[pt1+3]+m[6])*(e1[1]-e1[2]*pts[pt1+1]); + int validate = 0; + for (int j = 0; j < MIN_TEST; j++) { + if (i == j) continue; + const int inl_idx = 4*inliers[j]; + if (sign1*(m[0]*pts[inl_idx+2]+m[3]*pts[inl_idx+3]+m[6])*(e1[1]-e1[2]*pts[inl_idx+1])<0) + validate++; + } + if (validate < MIN_TEST/2) { + do_or_test = true; break; + } + } + } + + // verification does not include sample points as they are surely random + const int max_verify = num_inliers - num_sample_in_inliers; + if (max_verify <= 0) + return 0; + int num_non_random_inliers = num_inliers - sample_size; + auto removeDependentPoints = [&] (bool do_orient_test, bool check_epipoles) { + for (int i = 0; i < max_verify; i++) { + // checks over inliers if they are dependent to other inliers + const int inl_idx = 4*inliers[i]; + const auto x1 = pts[inl_idx], y1 = pts[inl_idx+1], x2 = pts[inl_idx+2], y2 = pts[inl_idx+3]; + if (is_F) { + // epipolar line on image 2 = l2 + a2 = m[0] * x1 + m[1] * y1 + m[2]; + b2 = m[3] * x1 + m[4] * y1 + m[5]; + c2 = m[6] * x1 + m[7] * y1 + m[8]; + // epipolar line on image 1 = l1 + a1 = m[0] * x2 + m[3] * y2 + m[6]; + b1 = m[1] * x2 + m[4] * y2 + m[7]; + c1 = m[2] * x2 + m[5] * y2 + m[8]; + if ((!ep1_inf && fabsf(x1-ep1_x)+fabsf(y1-ep1_y) < neigh_thr) || + (!ep2_inf && fabsf(x2-ep2_x)+fabsf(y2-ep2_y) < neigh_thr)) { + num_non_random_inliers--; + num_pts_near_ep++; + continue; // is dependent, continue to the next point + } else if (check_epipoles) { + if (a2 * a2 + b2 * b2 + c2 * c2 < ep_thr_sqr || + a1 * a1 + b1 * b1 + c1 * c1 < ep_thr_sqr) { + num_non_random_inliers--; + num_pts_near_ep++; + continue; // is dependent, continue to the next point + } + } + else if (do_orient_test && pt1 != inl_idx && sign1*(m[0]*x2+m[3]*y2+m[6])*(e1[1]-e1[2]*y1)<0) { + num_non_random_inliers--; + num_pts_validatin_or_constr++; + continue; + } + const auto mag2 = 1 / sqrt(a2 * a2 + b2 * b2), mag1 = 1/sqrt(a1 * a1 + b1 * b1); + a2 *= mag2; b2 *= mag2; c2 *= mag2; + a1 *= mag1; b1 *= mag1; c1 *= mag1; + } + + for (int j = i+1; j < num_inliers; j++) {// verify through all including sample points + const int inl_idx_j = 4*inliers[j]; + const auto X1 = pts[inl_idx_j], Y1 = pts[inl_idx_j+1], X2 = pts[inl_idx_j+2], Y2 = pts[inl_idx_j+3]; + // use L1 norm instead of L2 for faster evaluation + if (fabsf(X1-x1) + fabsf(Y1 - y1) < neigh_thr || fabsf(X2-x2) + fabsf(Y2 - y2) < neigh_thr) { + num_non_random_inliers--; + // num_pts_bad_conditioning++; + break; // is dependent stop verification + } else if (is_F) { + if (fabsf(a2 * X2 + b2 * Y2 + c2) < line_thr && //|| // xj'^T F xi + fabsf(a1 * X1 + b1 * Y1 + c1) < line_thr) { // xj^T F^T xi' + num_non_random_inliers--; + break; // is dependent stop verification + } + } + } + } + }; + if (params->isPnP()) { + for (int i = 0; i < max_verify; i++) { + const int inl_idx = 5*inliers[i]; + const auto x = pts[inl_idx], y = pts[inl_idx+1], X = pts[inl_idx+2], Y = pts[inl_idx+3], Z = pts[inl_idx+4]; + for (int j = i+1; j < num_inliers; j++) { + const int inl_idx_j = 5*inliers[j]; + if (fabsf(x-pts[inl_idx_j ]) + fabsf(y-pts[inl_idx_j+1]) < neigh_thr || + fabsf(X-pts[inl_idx_j+2]) + fabsf(Y-pts[inl_idx_j+3]) + fabsf(Z-pts[inl_idx_j+4]) < neigh_thr) { + num_non_random_inliers--; + break; + } + } + } + } else { + removeDependentPoints(do_or_test, !ep1_inf && !ep2_inf); + if (is_F) { + const bool is_pts_vald_constr_normal = (double)num_pts_validatin_or_constr / num_inliers < 0.6; + const bool is_pts_near_ep_normal = (double)num_pts_near_ep / num_inliers < 0.6; + if (!is_pts_near_ep_normal || !is_pts_vald_constr_normal) { + num_non_random_inliers = num_inliers-sample_size; + num_pts_near_ep = 0; num_pts_validatin_or_constr = 0; + removeDependentPoints(is_pts_vald_constr_normal, is_pts_near_ep_normal); + } + } + } + return num_non_random_inliers; + } bool run(Ptr &ransac_output) { if (points_size < params->getSampleSize()) return false; + const bool LO = params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL, + IS_FUNDAMENTAL = params->isFundamental(), IS_NON_RAND_TEST = params->isNonRandomnessTest(); + const int MAX_MODELS_ADAPT = 21, MAX_ITERS_ADAPT = MAX_MODELS_ADAPT/*assume at least 1 model from 1 sample*/, + sample_size = params->getSampleSize(); + const double IOU_SIMILARITY_THR = 0.80; + std::vector non_degen_sample, best_sample; - const auto begin_time = std::chrono::steady_clock::now(); + double lambda_non_random_all_inliers = -1; + int final_iters, num_total_tested_models = 0; - // check if LO - const bool LO = params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL; - const bool is_magsac = params->getLO() == LocalOptimMethod::LOCAL_OPTIM_SIGMA; - const int max_hyp_test_before_ver = params->getMaxNumHypothesisToTestBeforeRejection(); - const int repeat_magsac = 10, max_iters_before_LO = params->getMaxItersBeforeLO(); - Score best_score; - Mat best_model; - int final_iters; + // non-random + const int MAX_TEST_MODELS_NONRAND = IS_NON_RAND_TEST ? MAX_MODELS_ADAPT : 0; + std::vector models_for_random_test; models_for_random_test.reserve(MAX_TEST_MODELS_NONRAND); + std::vector> samples_for_random_test; samples_for_random_test.reserve(MAX_TEST_MODELS_NONRAND); + bool last_model_from_LO = false; + Mat best_model, best_model_not_from_LO, K1_approx, K2_approx; + Score best_score, best_score_model_not_from_LO; + std::vector best_inliers_mask(points_size); if (! parallel) { - auto update_best = [&] (const Mat &new_model, const Score &new_score) { - best_score = new_score; - // remember best model - new_model.copyTo(best_model); - // update quality and verifier to save evaluation time of a model - _quality->setBestScore(best_score.score); - // update verifier - _model_verifier->update(best_score.inlier_number); - // update upper bound of iterations - return _termination_criteria->update(best_model, best_score.inlier_number); - }; - bool was_LO_run = false; + // adaptive sprt test + double IoU = 0, mean_num_est_models = 0; + bool adapt = IS_NON_RAND_TEST || params->getVerifier() == VerificationMethod ::ASPRT, was_LO_run = false; + int min_non_random_inliers = 30, iters = 0, num_estimations = 0, max_iters = params->getMaxIters(); Mat non_degenerate_model, lo_model; - Score current_score, lo_score, non_denegenerate_model_score; - - // reallocate memory for models + Score current_score, non_degenerate_model_score, best_score_sample; + std::vector model_inliers_mask (points_size); std::vector models(_estimator->getMaxNumSolutions()); + std::vector sample(_estimator->getMinimalSampleSize()), supports; + supports.reserve(3*MAX_MODELS_ADAPT); // store model supports during adaption + auto update_best = [&] (const Mat &new_model, const Score &new_score, bool from_lo=false) { + _quality->getInliers(new_model, model_inliers_mask); + IoU = Utils::intersectionOverUnion(best_inliers_mask, model_inliers_mask); + best_inliers_mask = model_inliers_mask; + + if (!best_model.empty() && (int)models_for_random_test.size() < MAX_TEST_MODELS_NONRAND && IoU < IOU_SIMILARITY_THR && !from_lo) { // use IoU to not save similar models + // save old best model for non-randomness test if necessary + models_for_random_test.emplace_back(best_model.clone()); + samples_for_random_test.emplace_back(best_sample); + } + + // update score, model, inliers and max iterations + best_score = new_score; + new_model.copyTo(best_model); + + if (!from_lo) { + best_sample = sample; + if (IS_FUNDAMENTAL) { // avoid degeneracy after LO run + // save last model not from LO + best_model.copyTo(best_model_not_from_LO); + best_score_model_not_from_LO = best_score; + } + } + + _model_verifier->update(best_score, iters); + max_iters = _termination->update(best_model, best_score.inlier_number); + // max_iters = std::max(max_iters, std::min(10, params->getMaxIters())); + if (!adapt) // update quality and verifier to save evaluation time of a model + _quality->setBestScore(best_score.score); + last_model_from_LO = from_lo; + }; + + auto run_lo = [&] (const Mat &_model, const Score &_score, bool force_lo) { + was_LO_run = true; + _local_optimization->setCurrentRANSACiter(force_lo ? iters : -1); + Score lo_score; + if (_local_optimization->refineModel(_model, _score, lo_model, lo_score) && lo_score.isBetter(best_score)) + update_best(lo_model, lo_score, true); + }; - // allocate memory for sample - std::vector sample(_estimator->getMinimalSampleSize()); - int iters = 0, max_iters = params->getMaxIters(); for (; iters < max_iters; iters++) { _sampler->generateSample(sample); - const int number_of_models = _estimator->estimateModels(sample, models); - + int number_of_models; + if (adapt) { + number_of_models = _estimator->estimateModels(sample, models); + mean_num_est_models += number_of_models; + num_estimations++; + } else { + number_of_models = _estimator->estimateModels(sample, models); + } for (int i = 0; i < number_of_models; i++) { - if (iters < max_hyp_test_before_ver) { + num_total_tested_models++; + if (adapt) { current_score = _quality->getScore(models[i]); - } else { - if (is_magsac && iters % repeat_magsac == 0) { - if (!_local_optimization->refineModel - (models[i], best_score, models[i], current_score)) - continue; - } else if (_model_verifier->isModelGood(models[i])) { - if (!_model_verifier->getScore(current_score)) { - if (_model_verifier->hasErrors()) - current_score = _quality->getScore(_model_verifier->getErrors()); - else current_score = _quality->getScore(models[i]); - } - } else continue; - } - - if (current_score.isBetter(best_score)) { - if (_degeneracy->recoverIfDegenerate(sample, models[i], - non_degenerate_model, non_denegenerate_model_score)) { - // check if best non degenerate model is better than so far the best model - if (non_denegenerate_model_score.isBetter(best_score)) - max_iters = update_best(non_degenerate_model, non_denegenerate_model_score); - else continue; - } else max_iters = update_best(models[i], current_score); - - if (LO && iters >= max_iters_before_LO) { - // do magsac if it wasn't already run - if (is_magsac && iters % repeat_magsac == 0 && iters >= max_hyp_test_before_ver) continue; // magsac has already run - was_LO_run = true; - // update model by Local optimization - if (_local_optimization->refineModel - (best_model, best_score, lo_model, lo_score)) { - if (lo_score.isBetter(best_score)){ - max_iters = update_best(lo_model, lo_score); - } - } + supports.emplace_back(current_score.inlier_number); + if (IS_NON_RAND_TEST && best_score_sample.isBetter(current_score)) { + models_for_random_test.emplace_back(models[i].clone()); + samples_for_random_test.emplace_back(sample); } - if (iters > max_iters) - break; + } else { + if (! _model_verifier->isModelGood(models[i], current_score)) + continue; + } + if (current_score.isBetter(best_score_sample)) { + if (_degeneracy->recoverIfDegenerate(sample, models[i], current_score, + non_degenerate_model, non_degenerate_model_score)) { + // check if best non degenerate model is better than so far the best model + if (non_degenerate_model_score.isBetter(best_score)) { + update_best(non_degenerate_model, non_degenerate_model_score); + best_score_sample = current_score.isBetter(best_score) ? best_score : current_score; + } else continue; + } else { + best_score_sample = current_score; + update_best(models[i], current_score); + } + + if (LO && ((iters < max_iters && best_score.inlier_number > min_non_random_inliers && IoU < IOU_SIMILARITY_THR))) + run_lo(best_model, best_score, false); + } // end of if so far the best score } // end loop of number of models - if (LO && !was_LO_run && iters >= max_iters_before_LO) { - was_LO_run = true; - if (_local_optimization->refineModel(best_model, best_score, lo_model, lo_score)) - if (lo_score.isBetter(best_score)){ - max_iters = update_best(lo_model, lo_score); - } + if (adapt && iters >= MAX_ITERS_ADAPT && num_total_tested_models >= MAX_MODELS_ADAPT) { + adapt = false; + lambda_non_random_all_inliers = getLambda(supports, 2.32, points_size, sample_size, false, min_non_random_inliers); + _model_verifier->updateSPRT(params->getTimeForModelEstimation(), 1.0, mean_num_est_models/num_estimations, lambda_non_random_all_inliers/points_size,(double)std::max(min_non_random_inliers, best_score.inlier_number)/points_size, best_score); } } // end main while loop - final_iters = iters; - } else { - const int MAX_THREADS = getNumThreads(); + if (! was_LO_run && !best_model.empty() && LO) + run_lo(best_model, best_score, true); + } else { // parallel VSAC + const int MAX_THREADS = getNumThreads(), growth_max_samples = params->getProsacMaxSamples(); const bool is_prosac = params->getSampler() == SamplingMethod::SAMPLING_PROSAC; - std::atomic_bool success(false); - std::atomic_int num_hypothesis_tested(0); - std::atomic_int thread_cnt(0); - std::vector best_scores(MAX_THREADS); - std::vector best_models(MAX_THREADS); - - Mutex mutex; // only for prosac - + std::atomic_int num_hypothesis_tested(0), thread_cnt(0), max_number_inliers(0), subset_size, termination_length; + std::atomic best_score_all(std::numeric_limits::max()); + std::vector best_scores(MAX_THREADS), best_scores_not_LO; + std::vector best_models(MAX_THREADS), best_models_not_LO, K1_apx, K2_apx; + std::vector num_tested_models_threads(MAX_THREADS), growth_function, non_random_inliers; + std::vector> tested_models_threads(MAX_THREADS); + std::vector>> tested_samples_threads(MAX_THREADS); + std::vector> best_samples_threads(MAX_THREADS); + std::vector last_model_from_LO_vec; + std::vector lambda_non_random_all_inliers_vec(MAX_THREADS); + if (IS_FUNDAMENTAL) { + last_model_from_LO_vec = std::vector(MAX_THREADS); + best_models_not_LO = std::vector(MAX_THREADS); + best_scores_not_LO = std::vector(MAX_THREADS); + K1_apx = std::vector(MAX_THREADS); + K2_apx = std::vector(MAX_THREADS); + } + if (is_prosac) { + growth_function = _sampler.dynamicCast()->getGrowthFunction(); + subset_size = 2*sample_size; // n, size of the current sampling pool + termination_length = points_size; + } /////////////////////////////////////////////////////////////////////////////////////////////////////// parallel_for_(Range(0, MAX_THREADS), [&](const Range & /*range*/) { if (!success) { // cover all if not success to avoid thread creating new variables const int thread_rng_id = thread_cnt++; - int thread_state = state + 10*thread_rng_id; - - Ptr estimator = _estimator->clone(); - Ptr degeneracy = _degeneracy->clone(thread_state++); - Ptr quality = _quality->clone(); - Ptr model_verifier = _model_verifier->clone(thread_state++); // update verifier - Ptr local_optimization; - if (LO) - local_optimization = _local_optimization->clone(thread_state++); - Ptr termination_criteria = _termination_criteria->clone(); + bool adapt = params->getVerifier() == VerificationMethod ::ASPRT || IS_NON_RAND_TEST; + int thread_state = _state + thread_rng_id, min_non_random_inliers = 0, num_tested_models = 0, + num_estimations = 0, mean_num_est_models = 0, iters, max_iters = params->getMaxIters(); + double IoU = 0, lambda_non_random_all_inliers_thread = -1; + std::vector tested_models_thread; tested_models_thread.reserve(MAX_TEST_MODELS_NONRAND); + std::vector> tested_samples_thread; tested_samples_thread.reserve(MAX_TEST_MODELS_NONRAND); + Ptr random_gen; + if (is_prosac) random_gen = UniformRandomGenerator::create(thread_state); + Ptr error; + Ptr estimator; + Ptr degeneracy; + Ptr quality; + Ptr model_verifier; Ptr sampler; - if (!is_prosac) - sampler = _sampler->clone(thread_state); - - Mat best_model_thread, non_degenerate_model, lo_model; - Score best_score_thread, current_score, non_denegenerate_model_score, lo_score, - best_score_all_threads; - std::vector sample(estimator->getMinimalSampleSize()); + Ptr lo_sampler; + Ptr termination; + Ptr local_optimization; + Ptr min_solver; + Ptr non_min_solver; + Ptr weight_fnc; + initialize (thread_state, min_solver, non_min_solver, error, estimator, degeneracy, quality, + model_verifier, local_optimization, termination, sampler, lo_sampler, weight_fnc, true); + bool is_last_from_LO_thread = false; + Mat best_model_thread, non_degenerate_model, lo_model, best_not_LO_thread; + Score best_score_thread, current_score, non_denegenerate_model_score, lo_score,best_score_all_threads, best_not_LO_score_thread; + std::vector sample(estimator->getMinimalSampleSize()), best_sample_thread, supports; + supports.reserve(3*MAX_MODELS_ADAPT); // store model supports + std::vector best_inliers_mask_local(points_size, false), model_inliers_mask(points_size, false); std::vector models(estimator->getMaxNumSolutions()); - int iters, max_iters = params->getMaxIters(); - auto update_best = [&] (const Score &new_score, const Mat &new_model) { + auto update_best = [&] (const Score &new_score, const Mat &new_model, bool from_LO=false) { + // update best score of all threads + if (max_number_inliers < new_score.inlier_number) max_number_inliers = new_score.inlier_number; + if (best_score_all > new_score.score) best_score_all = new_score.score; + best_score_all_threads = Score(max_number_inliers, best_score_all); + // + quality->getInliers(new_model, model_inliers_mask); + IoU = Utils::intersectionOverUnion(best_inliers_mask_local, model_inliers_mask); + if (!best_model_thread.empty() && (int)tested_models_thread.size() < MAX_TEST_MODELS_NONRAND && IoU < IOU_SIMILARITY_THR) { + tested_models_thread.emplace_back(best_model_thread.clone()); + tested_samples_thread.emplace_back(best_sample_thread); + } + if (!adapt) { // update quality and verifier + quality->setBestScore(best_score_all); + model_verifier->update(best_score_all_threads, iters); + } // copy new score to best score best_score_thread = new_score; - best_scores[thread_rng_id] = best_score_thread; + best_sample_thread = sample; + best_inliers_mask_local = model_inliers_mask; // remember best model new_model.copyTo(best_model_thread); - best_model_thread.copyTo(best_models[thread_rng_id]); - best_score_all_threads = best_score_thread; - // update upper bound of iterations - return termination_criteria->update - (best_model_thread, best_score_thread.inlier_number); - }; + // update upper bound of iterations + if (is_prosac) { + int new_termination_length; + max_iters = termination.dynamicCast()-> + updateTerminationLength(best_model_thread, best_score_thread.inlier_number, new_termination_length); + // update termination length + if (new_termination_length < termination_length) + termination_length = new_termination_length; + } else max_iters = termination->update(best_model_thread, max_number_inliers); + if (IS_FUNDAMENTAL) { + is_last_from_LO_thread = from_LO; + if (!from_LO) { + best_model_thread.copyTo(best_not_LO_thread); + best_not_LO_score_thread = best_score_thread; + } + } + }; bool was_LO_run = false; + auto runLO = [&] (int current_ransac_iters) { + was_LO_run = true; + local_optimization->setCurrentRANSACiter(current_ransac_iters); + if (local_optimization->refineModel(best_model_thread, best_score_thread, lo_model, + lo_score) && lo_score.isBetter(best_score_thread)) + update_best(lo_score, lo_model, true); + }; for (iters = 0; iters < max_iters && !success; iters++) { success = num_hypothesis_tested++ > max_iters; - - if (iters % 10) { + if (iters % 10 && !adapt) { // Synchronize threads. just to speed verification of model. - int best_thread_idx = thread_rng_id; - bool updated = false; - for (int t = 0; t < MAX_THREADS; t++) { - if (best_scores[t].isBetter(best_score_all_threads)) { - best_score_all_threads = best_scores[t]; - updated = true; - best_thread_idx = t; - } - } - if (updated && best_thread_idx != thread_rng_id) { - quality->setBestScore(best_score_all_threads.score); - model_verifier->update(best_score_all_threads.inlier_number); - } + quality->setBestScore(std::min(best_score_thread.score, (double)best_score_all)); + model_verifier->update(best_score_thread.inlier_number > max_number_inliers ? best_score_thread : best_score_all_threads, iters); } if (is_prosac) { - // use global sampler - mutex.lock(); - _sampler->generateSample(sample); - mutex.unlock(); + if (num_hypothesis_tested > growth_max_samples) { + // if PROSAC has not converged to solution then do uniform sampling. + random_gen->generateUniqueRandomSet(sample, sample_size, points_size); + } else { + if (num_hypothesis_tested >= growth_function[subset_size-1] && subset_size < termination_length-MAX_THREADS) { + subset_size++; + if (subset_size >= points_size) subset_size = points_size-1; + } + if (growth_function[subset_size-1] < num_hypothesis_tested) { + // The sample contains m-1 points selected from U_(n-1) at random and u_n + random_gen->generateUniqueRandomSet(sample, sample_size-1, subset_size-1); + sample[sample_size-1] = subset_size-1; + } else + // Select m points from U_n at random. + random_gen->generateUniqueRandomSet(sample, sample_size, subset_size); + } } else sampler->generateSample(sample); // use local sampler const int number_of_models = estimator->estimateModels(sample, models); + if (adapt) { + num_estimations++; mean_num_est_models += number_of_models; + } for (int i = 0; i < number_of_models; i++) { - if (iters < max_hyp_test_before_ver) { + num_tested_models++; + if (adapt) { current_score = quality->getScore(models[i]); - } else { - if (is_magsac && iters % repeat_magsac == 0) { - if (local_optimization && !local_optimization->refineModel - (models[i], best_score_thread, models[i], current_score)) - continue; - } else if (model_verifier->isModelGood(models[i])) { - if (!model_verifier->getScore(current_score)) { - if (model_verifier->hasErrors()) - current_score = quality->getScore(model_verifier->getErrors()); - else current_score = quality->getScore(models[i]); - } - } else continue; - } + supports.emplace_back(current_score.inlier_number); + } else if (! model_verifier->isModelGood(models[i], current_score)) + continue; if (current_score.isBetter(best_score_all_threads)) { - if (degeneracy->recoverIfDegenerate(sample, models[i], - non_degenerate_model, non_denegenerate_model_score)) { + if (degeneracy->recoverIfDegenerate(sample, models[i], current_score, + non_degenerate_model, non_denegenerate_model_score)) { // check if best non degenerate model is better than so far the best model if (non_denegenerate_model_score.isBetter(best_score_thread)) - max_iters = update_best(non_denegenerate_model_score, non_degenerate_model); + update_best(non_denegenerate_model_score, non_degenerate_model); else continue; - } else - max_iters = update_best(current_score, models[i]); - - if (LO && iters >= max_iters_before_LO) { - // do magsac if it wasn't already run - if (is_magsac && iters % repeat_magsac == 0 && iters >= max_hyp_test_before_ver) continue; - was_LO_run = true; - // update model by Local optimizaion - if (local_optimization->refineModel - (best_model_thread, best_score_thread, lo_model, lo_score)) - if (lo_score.isBetter(best_score_thread)) { - max_iters = update_best(lo_score, lo_model); - } - } - if (num_hypothesis_tested > max_iters) { - success = true; break; - } + } else update_best(current_score, models[i]); + if (LO && num_hypothesis_tested < max_iters && IoU < IOU_SIMILARITY_THR && + best_score_thread.inlier_number > min_non_random_inliers) + runLO(iters); } // end of if so far the best score + else if ((int)tested_models_thread.size() < MAX_TEST_MODELS_NONRAND) { + tested_models_thread.emplace_back(models[i].clone()); + tested_samples_thread.emplace_back(sample); + } + if (num_hypothesis_tested > max_iters) { + success = true; break; + } } // end loop of number of models - if (LO && !was_LO_run && iters >= max_iters_before_LO) { - was_LO_run = true; - if (_local_optimization->refineModel(best_model, best_score, lo_model, lo_score)) - if (lo_score.isBetter(best_score)){ - max_iters = update_best(lo_score, lo_model); - } + if (adapt && iters >= MAX_ITERS_ADAPT && num_tested_models >= MAX_MODELS_ADAPT) { + adapt = false; + lambda_non_random_all_inliers_thread = getLambda(supports, 2.32, points_size, sample_size, false, min_non_random_inliers); + model_verifier->updateSPRT(params->getTimeForModelEstimation(), 1, (double)mean_num_est_models/num_estimations, lambda_non_random_all_inliers_thread/points_size, + (double)std::max(min_non_random_inliers, best_score.inlier_number)/points_size, best_score_all_threads); } + if (!adapt && LO && num_hypothesis_tested < max_iters && !was_LO_run && !best_model_thread.empty() && + best_score_thread.inlier_number > min_non_random_inliers) + runLO(iters); } // end of loop over iters + if (! was_LO_run && !best_model_thread.empty() && LO) + runLO(-1 /*use full iterations of LO*/); + best_model_thread.copyTo(best_models[thread_rng_id]); + best_scores[thread_rng_id] = best_score_thread; + num_tested_models_threads[thread_rng_id] = num_tested_models; + tested_models_threads[thread_rng_id] = tested_models_thread; + tested_samples_threads[thread_rng_id] = tested_samples_thread; + best_samples_threads[thread_rng_id] = best_sample_thread; + if (IS_FUNDAMENTAL) { + best_scores_not_LO[thread_rng_id] = best_not_LO_score_thread; + best_not_LO_thread.copyTo(best_models_not_LO[thread_rng_id]); + last_model_from_LO_vec[thread_rng_id] = is_last_from_LO_thread; + } + lambda_non_random_all_inliers_vec[thread_rng_id] = lambda_non_random_all_inliers_thread; }}); // end parallel /////////////////////////////////////////////////////////////////////////////////////////////////////// // find best model from all threads' models best_score = best_scores[0]; int best_thread_idx = 0; - for (int i = 1; i < MAX_THREADS; i++) { + for (int i = 1; i < MAX_THREADS; i++) if (best_scores[i].isBetter(best_score)) { best_score = best_scores[i]; best_thread_idx = i; } - } best_model = best_models[best_thread_idx]; + if (IS_FUNDAMENTAL) { + last_model_from_LO = last_model_from_LO_vec[best_thread_idx]; + K1_approx = K1_apx[best_thread_idx]; + K2_approx = K2_apx[best_thread_idx]; + } final_iters = num_hypothesis_tested; + best_sample = best_samples_threads[best_thread_idx]; + int num_lambdas = 0; + double avg_lambda = 0; + for (int i = 0; i < MAX_THREADS; i++) { + if (IS_FUNDAMENTAL && best_scores_not_LO[i].isBetter(best_score_model_not_from_LO)) { + best_score_model_not_from_LO = best_scores_not_LO[i]; + best_models_not_LO[i].copyTo(best_model_not_from_LO); + } + if (IS_NON_RAND_TEST && lambda_non_random_all_inliers_vec[i] > 0) { + num_lambdas ++; + avg_lambda += lambda_non_random_all_inliers_vec[i]; + } + num_total_tested_models += num_tested_models_threads[i]; + if ((int)models_for_random_test.size() < MAX_TEST_MODELS_NONRAND) { + for (int m = 0; m < (int)tested_models_threads[i].size(); m++) { + models_for_random_test.emplace_back(tested_models_threads[i][m].clone()); + samples_for_random_test.emplace_back(tested_samples_threads[i][m]); + if ((int)models_for_random_test.size() == MAX_TEST_MODELS_NONRAND) + break; + } + } + } + if (IS_NON_RAND_TEST && num_lambdas > 0 && avg_lambda > 0) + lambda_non_random_all_inliers = avg_lambda / num_lambdas; } - - if (best_model.empty()) + if (best_model.empty()) { + ransac_output = RansacOutput::create(best_model, std::vector(), best_score.inlier_number, final_iters, ModelConfidence::RANDOM, std::vector()); return false; - - // polish final model - if (params->getFinalPolisher() != PolishingMethod::NonePolisher) { + } + if (last_model_from_LO && IS_FUNDAMENTAL && K1.empty() && K2.empty()) { + Score new_score; Mat new_model; + const double INL_THR = 0.80; + if (parallel) + _quality->getInliers(best_model, best_inliers_mask); + // run additional degeneracy check for F: + if (_degeneracy.dynamicCast()->verifyFundamental(best_model, best_score, best_inliers_mask, new_model, new_score)) { + // so-far-the-best F is degenerate + // Update best F using non-degenerate F or the one which is not from LO + if (new_score.isBetter(best_score_model_not_from_LO) && new_score.inlier_number > INL_THR*best_score.inlier_number) { + best_score = new_score; + new_model.copyTo(best_model); + } else if (best_score_model_not_from_LO.inlier_number > INL_THR*best_score.inlier_number) { + best_score = best_score_model_not_from_LO; + best_model_not_from_LO.copyTo(best_model); + } + } else { // so-far-the-best F is not degenerate + if (new_score.isBetter(best_score)) { + // if new model is better then update + best_score = new_score; + new_model.copyTo(best_model); + } + } + } + if (params->getFinalPolisher() != PolishingMethod::NONE_POLISHER) { Mat polished_model; Score polisher_score; - if (model_polisher->polishSoFarTheBestModel(best_model, best_score, - polished_model, polisher_score)) - if (polisher_score.isBetter(best_score)) { - best_score = polisher_score; - polished_model.copyTo(best_model); - } + if (polisher->polishSoFarTheBestModel(best_model, best_score, // polish final model + polished_model, polisher_score) && polisher_score.isBetter(best_score)) { + best_score = polisher_score; + polished_model.copyTo(best_model); + } } - // ================= here is ending ransac main implementation =========================== - std::vector inliers_mask; + + ///////////////// get inliers of the best model and points' residuals /////////////// + std::vector inliers_mask; std::vector residuals; if (params->isMaskRequired()) { inliers_mask = std::vector(points_size); - // get final inliers from the best model - _quality->getInliers(best_model, inliers_mask); + residuals = _error->getErrors(best_model); + _quality->getInliers(residuals, inliers_mask, threshold); } - // Store results - ransac_output = RansacOutput::create(best_model, inliers_mask, - static_cast(std::chrono::duration_cast - (std::chrono::steady_clock::now() - begin_time).count()), best_score.score, - best_score.inlier_number, final_iters, -1, -1); + + ModelConfidence model_conf = ModelConfidence::UNKNOWN; + if (IS_NON_RAND_TEST) { + std::vector temp_inliers(points_size); + const int non_random_inls_best_model = getIndependentInliers(best_model, best_sample, temp_inliers, + _quality->getInliers(best_model, temp_inliers)); + // quick test on lambda from all inliers (= upper bound of independent inliers) + // if model with independent inliers is not random for Poisson with all inliers then it is not random using independent inliers too + if (pow(Utils::getPoissonCDF(lambda_non_random_all_inliers, non_random_inls_best_model), num_total_tested_models) < 0.9999) { + std::vector inliers_list(models_for_random_test.size()); + for (int m = 0; m < (int)models_for_random_test.size(); m++) + inliers_list[m] = getIndependentInliers(models_for_random_test[m], samples_for_random_test[m], + temp_inliers, _quality->getInliers(models_for_random_test[m], temp_inliers)); + int min_non_rand_inliers; + const double lambda = getLambda(inliers_list, 1.644, points_size, sample_size, true, min_non_rand_inliers); + const double cdf_lambda = Utils::getPoissonCDF(lambda, non_random_inls_best_model), cdf_N = pow(cdf_lambda, num_total_tested_models); + model_conf = cdf_N < 0.9999 ? ModelConfidence ::RANDOM : ModelConfidence ::NON_RANDOM; + } else model_conf = ModelConfidence ::NON_RANDOM; + } + ransac_output = RansacOutput::create(best_model, inliers_mask, best_score.inlier_number, final_iters, model_conf, residuals); return true; } }; @@ -469,7 +1121,7 @@ void setParameters (int flag, Ptr ¶ms, EstimationMethod estimator, do params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO); break; case USAC_FM_8PTS: - params = Model::create(thr, EstimationMethod::Fundamental8,SamplingMethod::SAMPLING_UNIFORM, + params = Model::create(thr, EstimationMethod::FUNDAMENTAL8,SamplingMethod::SAMPLING_UNIFORM, conf, max_iters,ScoreMethod::SCORE_METHOD_MSAC); params->setLocalOptimization(LocalOptimMethod ::LOCAL_OPTIM_INNER_LO); break; @@ -477,9 +1129,10 @@ void setParameters (int flag, Ptr ¶ms, EstimationMethod estimator, do } // do not do too many iterations for PnP if (estimator == EstimationMethod::P3P) { - if (params->getLOInnerMaxIters() > 15) - params->setLOIterations(15); + if (params->getLOInnerMaxIters() > 10) + params->setLOIterations(10); params->setLOIterativeIters(0); + params->setFinalLSQ(3); } params->maskRequired(mask_needed); @@ -488,9 +1141,9 @@ void setParameters (int flag, Ptr ¶ms, EstimationMethod estimator, do Mat findHomography (InputArray srcPoints, InputArray dstPoints, int method, double thr, OutputArray mask, const int max_iters, const double confidence) { Ptr params; - setParameters(method, params, EstimationMethod::Homography, thr, max_iters, confidence, mask.needed()); + setParameters(method, params, EstimationMethod::HOMOGRAPHY, thr, max_iters, confidence, mask.needed()); Ptr ransac_output; - if (run(params, srcPoints, dstPoints, params->getRandomGeneratorState(), + if (run(params, srcPoints, dstPoints, ransac_output, noArray(), noArray(), noArray(), noArray())) { saveMask(mask, ransac_output->getInliersMask()); return ransac_output->getModel() / ransac_output->getModel().at(2,2); @@ -505,9 +1158,9 @@ Mat findHomography (InputArray srcPoints, InputArray dstPoints, int method, doub Mat findFundamentalMat( InputArray points1, InputArray points2, int method, double thr, double confidence, int max_iters, OutputArray mask ) { Ptr params; - setParameters(method, params, EstimationMethod::Fundamental, thr, max_iters, confidence, mask.needed()); + setParameters(method, params, EstimationMethod::FUNDAMENTAL, thr, max_iters, confidence, mask.needed()); Ptr ransac_output; - if (run(params, points1, points2, params->getRandomGeneratorState(), + if (run(params, points1, points2, ransac_output, noArray(), noArray(), noArray(), noArray())) { saveMask(mask, ransac_output->getInliersMask()); return ransac_output->getModel(); @@ -522,9 +1175,9 @@ Mat findFundamentalMat( InputArray points1, InputArray points2, int method, doub Mat findEssentialMat (InputArray points1, InputArray points2, InputArray cameraMatrix1, int method, double prob, double thr, OutputArray mask, int maxIters) { Ptr params; - setParameters(method, params, EstimationMethod::Essential, thr, maxIters, prob, mask.needed()); + setParameters(method, params, EstimationMethod::ESSENTIAL, thr, maxIters, prob, mask.needed()); Ptr ransac_output; - if (run(params, points1, points2, params->getRandomGeneratorState(), + if (run(params, points1, points2, ransac_output, cameraMatrix1, cameraMatrix1, noArray(), noArray())) { saveMask(mask, ransac_output->getInliersMask()); return ransac_output->getModel(); @@ -544,7 +1197,7 @@ bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints, setParameters(method, params, cameraMatrix.empty() ? EstimationMethod ::P6P : EstimationMethod ::P3P, thr, max_iters, conf, inliers.needed()); Ptr ransac_output; - if (run(params, imagePoints, objectPoints, params->getRandomGeneratorState(), + if (run(params, imagePoints, objectPoints, ransac_output, cameraMatrix, noArray(), distCoeffs, noArray())) { if (inliers.needed()) { const auto &inliers_mask = ransac_output->getInliersMask(); @@ -565,9 +1218,9 @@ bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints, Mat estimateAffine2D(InputArray from, InputArray to, OutputArray mask, int method, double thr, int max_iters, double conf, int /*refineIters*/) { Ptr params; - setParameters(method, params, EstimationMethod ::Affine, thr, max_iters, conf, mask.needed()); + setParameters(method, params, EstimationMethod::AFFINE, thr, max_iters, conf, mask.needed()); Ptr ransac_output; - if (run(params, from, to, params->getRandomGeneratorState(), + if (run(params, from, to, ransac_output, noArray(), noArray(), noArray(), noArray())) { saveMask(mask, ransac_output->getInliersMask()); return ransac_output->getModel().rowRange(0,2); @@ -582,12 +1235,23 @@ Mat estimateAffine2D(InputArray from, InputArray to, OutputArray mask, int metho class ModelImpl : public Model { private: // main parameters: - double threshold, confidence; - int sample_size, max_iterations; - + double threshold; EstimationMethod estimator; SamplingMethod sampler; + double confidence; + int max_iterations; ScoreMethod score; + int sample_size; + + // Larsson parameters + bool is_larsson_optimization = true; + int larsson_leven_marq_iters_lo = 10, larsson_leven_marq_iters_fo = 15; + + // solver for a null-space extraction + MethodSolver null_solver = GEM_SOLVER; + + // prosac + int prosac_max_samples = 200000; // for neighborhood graph int k_nearest_neighbors = 8;//, flann_search_params = 5, num_kd_trees = 1; // for FLANN @@ -596,23 +1260,24 @@ private: NeighborSearchMethod neighborsType = NeighborSearchMethod::NEIGH_GRID; // Local Optimization parameters - LocalOptimMethod lo = LocalOptimMethod ::LOCAL_OPTIM_INNER_AND_ITER_LO; - int lo_sample_size=16, lo_inner_iterations=15, lo_iterative_iterations=8, - lo_thr_multiplier=15, lo_iter_sample_size = 30; + LocalOptimMethod lo = LocalOptimMethod ::LOCAL_OPTIM_INNER_LO; + int lo_sample_size=12, lo_inner_iterations=20, lo_iterative_iterations=8, + lo_thr_multiplier=10, lo_iter_sample_size = 30; // Graph cut parameters const double spatial_coherence_term = 0.975; // apply polisher for final RANSAC model - PolishingMethod polisher = PolishingMethod ::LSQPolisher; + PolishingMethod polisher = PolishingMethod ::COV_POLISHER; // preemptive verification test - VerificationMethod verifier = VerificationMethod ::SprtVerifier; - const int max_hypothesis_test_before_verification = 15; + VerificationMethod verifier = VerificationMethod ::ASPRT; // sprt parameters - // lower bound estimate is 1% of inliers - double sprt_eps = 0.01, sprt_delta = 0.008, avg_num_models, time_for_model_est; + // lower bound estimate is 2% of inliers + // model estimation to verification time = ratio of time needed to estimate model + // to verification of one point wrt the model + double sprt_eps = 0.02, sprt_delta = 0.008, avg_num_models, model_est_to_ver_time; // estimator error ErrorMetric est_error; @@ -620,67 +1285,71 @@ private: // progressive napsac double relax_coef = 0.1; // for building neighborhood graphs - const std::vector grid_cell_number = {16, 8, 4, 2}; + const std::vector grid_cell_number = {10, 5, 2}; //for final least squares polisher - int final_lsq_iters = 3; + int final_lsq_iters = 7; - bool need_mask = true, is_parallel = false; + bool need_mask = true, // do we need inlier mask in the end + is_parallel = false, // use parallel RANSAC + is_nonrand_test = false; // is test for the final model non-randomness + + // state of pseudo-random number generator int random_generator_state = 0; - const int max_iters_before_LO = 100; + + // number of iterations of plane-and-parallax in DEGENSAC^+ + int plane_and_parallax_max_iters = 300; // magsac parameters: int DoF = 2; double sigma_quantile = 3.04, upper_incomplete_of_sigma_quantile = 0.00419, - lower_incomplete_of_sigma_quantile = 0.8629, C = 0.5, maximum_thr = 7.5; + lower_incomplete_of_sigma_quantile = 0.8629, C = 0.5, maximum_thr = 7.5; + double k_mlesac = 2.25; // parameter for MLESAC model evaluation public: - ModelImpl (double threshold_, EstimationMethod estimator_, SamplingMethod sampler_, double confidence_=0.95, - int max_iterations_=5000, ScoreMethod score_ =ScoreMethod::SCORE_METHOD_MSAC) { - estimator = estimator_; - sampler = sampler_; - confidence = confidence_; - max_iterations = max_iterations_; - score = score_; - + ModelImpl (double threshold_, EstimationMethod estimator_, SamplingMethod sampler_, double confidence_, + int max_iterations_, ScoreMethod score_) : + threshold(threshold_), estimator(estimator_), sampler(sampler_), confidence(confidence_), max_iterations(max_iterations_), score(score_) { switch (estimator_) { - // time for model estimation is basically a ratio of time need to estimate a model to - // time needed to verify if a point is consistent with this model - case (EstimationMethod::Affine): - avg_num_models = 1; time_for_model_est = 50; + case (EstimationMethod::AFFINE): + avg_num_models = 1; model_est_to_ver_time = 50; sample_size = 3; est_error = ErrorMetric ::FORW_REPR_ERR; break; - case (EstimationMethod::Homography): - avg_num_models = 1; time_for_model_est = 150; + case (EstimationMethod::HOMOGRAPHY): + avg_num_models = 0.8; model_est_to_ver_time = 200; sample_size = 4; est_error = ErrorMetric ::FORW_REPR_ERR; break; - case (EstimationMethod::Fundamental): - avg_num_models = 2.38; time_for_model_est = 180; maximum_thr = 2.5; + case (EstimationMethod::FUNDAMENTAL): + DoF = 4; C = 0.25; sigma_quantile = 3.64, upper_incomplete_of_sigma_quantile = 0.003657; lower_incomplete_of_sigma_quantile = 1.3012; + maximum_thr = 2.5; + avg_num_models = 1.5; model_est_to_ver_time = 200; sample_size = 7; est_error = ErrorMetric ::SAMPSON_ERR; break; - case (EstimationMethod::Fundamental8): - avg_num_models = 1; time_for_model_est = 100; maximum_thr = 2.5; + case (EstimationMethod::FUNDAMENTAL8): + avg_num_models = 1; model_est_to_ver_time = 100; maximum_thr = 2.5; sample_size = 8; est_error = ErrorMetric ::SAMPSON_ERR; break; - case (EstimationMethod::Essential): - avg_num_models = 3.93; time_for_model_est = 1000; maximum_thr = 2.5; - sample_size = 5; est_error = ErrorMetric ::SGD_ERR; break; + case (EstimationMethod::ESSENTIAL): + DoF = 4; C = 0.25; sigma_quantile = 3.64, upper_incomplete_of_sigma_quantile = 0.003657; lower_incomplete_of_sigma_quantile = 1.3012; + avg_num_models = 3.93; model_est_to_ver_time = 1000; maximum_thr = 2; + sample_size = 5; est_error = ErrorMetric ::SAMPSON_ERR; break; case (EstimationMethod::P3P): - avg_num_models = 1.38; time_for_model_est = 800; + avg_num_models = 1.38; model_est_to_ver_time = 800; sample_size = 3; est_error = ErrorMetric ::RERPOJ; break; case (EstimationMethod::P6P): - avg_num_models = 1; time_for_model_est = 300; + avg_num_models = 1; model_est_to_ver_time = 300; sample_size = 6; est_error = ErrorMetric ::RERPOJ; break; default: CV_Error(cv::Error::StsNotImplemented, "Estimator has not implemented yet!"); } + if (score_ == ScoreMethod::SCORE_METHOD_MAGSAC) + polisher = PolishingMethod::MAGSAC; + + // for PnP problem we can use only KNN graph if (estimator_ == EstimationMethod::P3P || estimator_ == EstimationMethod::P6P) { + polisher = LSQ_POLISHER; neighborsType = NeighborSearchMethod::NEIGH_FLANN_KNN; k_nearest_neighbors = 2; } - if (estimator == EstimationMethod::Fundamental || estimator == EstimationMethod::Essential) { - lo_sample_size = 21; - lo_thr_multiplier = 10; - } - if (estimator == EstimationMethod::Homography) - maximum_thr = 8.; - threshold = threshold_; } + + // setters + void setNonRandomnessTest (bool set) override { is_nonrand_test = set; } void setVerifier (VerificationMethod verifier_) override { verifier = verifier_; } void setPolisher (PolishingMethod polisher_) override { polisher = polisher_; } void setParallel (bool is_parallel_) override { is_parallel = is_parallel_; } @@ -690,11 +1359,17 @@ public: void setNeighborsType (NeighborSearchMethod neighbors) override { neighborsType = neighbors; } void setCellSize (int cell_size_) override { cell_size = cell_size_; } void setLOIterations (int iters) override { lo_inner_iterations = iters; } - void setLOIterativeIters (int iters) override {lo_iterative_iterations = iters; } void setLOSampleSize (int lo_sample_size_) override { lo_sample_size = lo_sample_size_; } - void setThresholdMultiplierLO (double thr_mult) override { lo_thr_multiplier = (int) round(thr_mult); } void maskRequired (bool need_mask_) override { need_mask = need_mask_; } void setRandomGeneratorState (int state) override { random_generator_state = state; } + void setLOIterativeIters (int iters) override { lo_iterative_iterations = iters; } + void setFinalLSQ (int iters) override { final_lsq_iters = iters; } + + // getters + int getProsacMaxSamples() const override { return prosac_max_samples; } + int getLevMarqIters () const override { return larsson_leven_marq_iters_fo; } + int getLevMarqItersLO () const override { return larsson_leven_marq_iters_lo; } + bool isNonRandomnessTest () const override { return is_nonrand_test; } bool isMaskRequired () const override { return need_mask; } NeighborSearchMethod getNeighborsSearch () const override { return neighborsType; } int getKNN () const override { return k_nearest_neighbors; } @@ -711,17 +1386,17 @@ public: return lower_incomplete_of_sigma_quantile; } double getC () const override { return C; } + double getKmlesac () const override { return k_mlesac; } double getMaximumThreshold () const override { return maximum_thr; } double getGraphCutSpatialCoherenceTerm () const override { return spatial_coherence_term; } int getLOSampleSize () const override { return lo_sample_size; } - int getMaxNumHypothesisToTestBeforeRejection() const override { - return max_hypothesis_test_before_verification; - } + MethodSolver getRansacSolver () const override { return null_solver; } PolishingMethod getFinalPolisher () const override { return polisher; } int getLOThresholdMultiplier() const override { return lo_thr_multiplier; } int getLOIterativeSampleSize() const override { return lo_iter_sample_size; } int getLOIterativeMaxIters() const override { return lo_iterative_iterations; } int getLOInnerMaxIters() const override { return lo_inner_iterations; } + int getPlaneAndParallaxIters () const override { return plane_and_parallax_max_iters; } LocalOptimMethod getLO () const override { return lo; } ScoreMethod getScore () const override { return score; } int getMaxIters () const override { return max_iterations; } @@ -730,22 +1405,22 @@ public: VerificationMethod getVerifier () const override { return verifier; } SamplingMethod getSampler () const override { return sampler; } int getRandomGeneratorState () const override { return random_generator_state; } - int getMaxItersBeforeLO () const override { return max_iters_before_LO; } double getSPRTdelta () const override { return sprt_delta; } double getSPRTepsilon () const override { return sprt_eps; } double getSPRTavgNumModels () const override { return avg_num_models; } int getCellSize () const override { return cell_size; } int getGraphRadius() const override { return radius; } - double getTimeForModelEstimation () const override { return time_for_model_est; } + double getTimeForModelEstimation () const override { return model_est_to_ver_time; } double getRelaxCoef () const override { return relax_coef; } const std::vector &getGridCellNumber () const override { return grid_cell_number; } + bool isLarssonOptimization () const override { return is_larsson_optimization; } bool isParallel () const override { return is_parallel; } bool isFundamental () const override { - return estimator == EstimationMethod ::Fundamental || - estimator == EstimationMethod ::Fundamental8; + return estimator == EstimationMethod::FUNDAMENTAL || + estimator == EstimationMethod::FUNDAMENTAL8; } - bool isHomography () const override { return estimator == EstimationMethod ::Homography; } - bool isEssential () const override { return estimator == EstimationMethod ::Essential; } + bool isHomography () const override { return estimator == EstimationMethod::HOMOGRAPHY; } + bool isEssential () const override { return estimator == EstimationMethod::ESSENTIAL; } bool isPnP() const override { return estimator == EstimationMethod ::P3P || estimator == EstimationMethod ::P6P; } @@ -757,277 +1432,36 @@ Ptr Model::create(double threshold_, EstimationMethod estimator_, Samplin max_iterations_, score_); } -bool run (const Ptr ¶ms, InputArray points1, InputArray points2, int state, +bool run (const Ptr ¶ms, InputArray points1, InputArray points2, Ptr &ransac_output, InputArray K1_, InputArray K2_, InputArray dist_coeff1, InputArray dist_coeff2) { - Ptr error; - Ptr estimator; - Ptr graph; - Ptr degeneracy; - Ptr quality; - Ptr verifier; - Ptr sampler; - Ptr lo_sampler; - Ptr termination; - Ptr lo; - Ptr polisher; - Ptr min_solver; - Ptr non_min_solver; - - Mat points, calib_points, undist_points1, undist_points2; - Matx33d K1, K2; - int points_size; - double threshold = params->getThreshold(), max_thr = params->getMaximumThreshold(); - const int min_sample_size = params->getSampleSize(); - if (params->isPnP()) { - if (! K1_.empty()) { - K1 = K1_.getMat(); - if (! dist_coeff1.empty()) { - // undistortPoints also calibrate points using K - if (points1.isContinuous()) - undistortPoints(points1, undist_points1, K1_, dist_coeff1); - else undistortPoints(points1.getMat().clone(), undist_points1, K1_, dist_coeff1); - points_size = mergePoints(undist_points1, points2, points, true); - Utils::normalizeAndDecalibPointsPnP (K1, points, calib_points); - } else { - points_size = mergePoints(points1, points2, points, true); - Utils::calibrateAndNormalizePointsPnP(K1, points, calib_points); - } - } else - points_size = mergePoints(points1, points2, points, true); - } else { - if (params->isEssential()) { - CV_CheckEQ((int)(!K1_.empty() && !K2_.empty()), 1, "Intrinsic matrix must not be empty!"); - K1 = K1_.getMat(); - K2 = K2_.getMat(); - if (! dist_coeff1.empty() || ! dist_coeff2.empty()) { - // undistortPoints also calibrate points using K - if (points1.isContinuous()) - undistortPoints(points1, undist_points1, K1_, dist_coeff1); - else undistortPoints(points1.getMat().clone(), undist_points1, K1_, dist_coeff1); - if (points2.isContinuous()) - undistortPoints(points2, undist_points2, K2_, dist_coeff2); - else undistortPoints(points2.getMat().clone(), undist_points2, K2_, dist_coeff2); - points_size = mergePoints(undist_points1, undist_points2, calib_points, false); - } else { - points_size = mergePoints(points1, points2, points, false); - Utils::calibratePoints(K1, K2, points, calib_points); - } - threshold = Utils::getCalibratedThreshold(threshold, K1, K2); - max_thr = Utils::getCalibratedThreshold(max_thr, K1, K2); - } else - points_size = mergePoints(points1, points2, points, false); - } - - // Since error function output squared error distance, so make - // threshold squared as well - threshold *= threshold; - - if (params->getSampler() == SamplingMethod::SAMPLING_NAPSAC || params->getLO() == LocalOptimMethod::LOCAL_OPTIM_GC) { - if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_GRID) { - graph = GridNeighborhoodGraph::create(points, points_size, - params->getCellSize(), params->getCellSize(), - params->getCellSize(), params->getCellSize(), 10); - } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_KNN) { - graph = FlannNeighborhoodGraph::create(points, points_size,params->getKNN(), false, 5, 1); - } else if (params->getNeighborsSearch() == NeighborSearchMethod::NEIGH_FLANN_RADIUS) { - graph = RadiusSearchNeighborhoodGraph::create(points, points_size, - params->getGraphRadius(), 5, 1); - } else CV_Error(cv::Error::StsNotImplemented, "Graph type is not implemented!"); - } - - std::vector> layers; - if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) { - CV_CheckEQ((int)params->isPnP(), 0, "ProgressiveNAPSAC for PnP is not implemented!"); - const auto &cell_number_per_layer = params->getGridCellNumber(); - layers.reserve(cell_number_per_layer.size()); - const auto * const pts = (float *) points.data; - float img1_width = 0, img1_height = 0, img2_width = 0, img2_height = 0; - for (int i = 0; i < 4 * points_size; i += 4) { - if (pts[i ] > img1_width ) img1_width = pts[i ]; - if (pts[i + 1] > img1_height) img1_height = pts[i + 1]; - if (pts[i + 2] > img2_width ) img2_width = pts[i + 2]; - if (pts[i + 3] > img2_height) img2_height = pts[i + 3]; - } - // Create grid graphs (overlapping layes of given cell numbers) - for (int layer_idx = 0; layer_idx < (int)cell_number_per_layer.size(); layer_idx++) { - const int cell_number = cell_number_per_layer[layer_idx]; - if (layer_idx > 0) - if (cell_number_per_layer[layer_idx-1] <= cell_number) - CV_Error(cv::Error::StsError, "Progressive NAPSAC sampler: " - "Cell number in layers must be in decreasing order!"); - layers.emplace_back(GridNeighborhoodGraph::create(points, points_size, - (int)(img1_width / (float)cell_number), (int)(img1_height / (float)cell_number), - (int)(img2_width / (float)cell_number), (int)(img2_height / (float)cell_number), 10)); - } - } - - // update points by calibrated for Essential matrix after graph is calculated - if (params->isEssential()) { - points = calib_points; - // if maximum calibrated threshold significanlty differs threshold then set upper bound - if (max_thr > 10*threshold) - max_thr = sqrt(10*threshold); // max thr will be squared after - } - if (max_thr < threshold) - max_thr = threshold; - - switch (params->getError()) { - case ErrorMetric::SYMM_REPR_ERR: - error = ReprojectionErrorSymmetric::create(points); break; - case ErrorMetric::FORW_REPR_ERR: - if (params->getEstimator() == EstimationMethod::Affine) - error = ReprojectionErrorAffine::create(points); - else error = ReprojectionErrorForward::create(points); - break; - case ErrorMetric::SAMPSON_ERR: - error = SampsonError::create(points); break; - case ErrorMetric::SGD_ERR: - error = SymmetricGeometricDistance::create(points); break; - case ErrorMetric::RERPOJ: - error = ReprojectionErrorPmatrix::create(points); break; - default: CV_Error(cv::Error::StsNotImplemented , "Error metric is not implemented!"); - } - - switch (params->getScore()) { - case ScoreMethod::SCORE_METHOD_RANSAC : - quality = RansacQuality::create(points_size, threshold, error); break; - case ScoreMethod::SCORE_METHOD_MSAC : - quality = MsacQuality::create(points_size, threshold, error); break; - case ScoreMethod::SCORE_METHOD_MAGSAC : - quality = MagsacQuality::create(max_thr, points_size, error, - threshold, params->getDegreesOfFreedom(), params->getSigmaQuantile(), - params->getUpperIncompleteOfSigmaQuantile(), - params->getLowerIncompleteOfSigmaQuantile(), params->getC()); break; - case ScoreMethod::SCORE_METHOD_LMEDS : - quality = LMedsQuality::create(points_size, threshold, error); break; - default: CV_Error(cv::Error::StsNotImplemented, "Score is not imeplemeted!"); - } - - if (params->isHomography()) { - degeneracy = HomographyDegeneracy::create(points); - min_solver = HomographyMinimalSolver4ptsGEM::create(points); - non_min_solver = HomographyNonMinimalSolver::create(points); - estimator = HomographyEstimator::create(min_solver, non_min_solver, degeneracy); - } else if (params->isFundamental()) { - degeneracy = FundamentalDegeneracy::create(state++, quality, points, min_sample_size, 5. /*sqr homogr thr*/); - if(min_sample_size == 7) min_solver = FundamentalMinimalSolver7pts::create(points); - else min_solver = FundamentalMinimalSolver8pts::create(points); - non_min_solver = FundamentalNonMinimalSolver::create(points); - estimator = FundamentalEstimator::create(min_solver, non_min_solver, degeneracy); - } else if (params->isEssential()) { - degeneracy = EssentialDegeneracy::create(points, min_sample_size); - min_solver = EssentialMinimalSolverStewenius5pts::create(points); - non_min_solver = EssentialNonMinimalSolver::create(points); - estimator = EssentialEstimator::create(min_solver, non_min_solver, degeneracy); - } else if (params->isPnP()) { - degeneracy = makePtr(); - if (min_sample_size == 3) { - non_min_solver = DLSPnP::create(points, calib_points, K1); - min_solver = P3PSolver::create(points, calib_points, K1); - } else { - min_solver = PnPMinimalSolver6Pts::create(points); - non_min_solver = PnPNonMinimalSolver::create(points); - } - estimator = PnPEstimator::create(min_solver, non_min_solver); - } else if (params->getEstimator() == EstimationMethod::Affine) { - degeneracy = makePtr(); - min_solver = AffineMinimalSolver::create(points); - non_min_solver = AffineNonMinimalSolver::create(points); - estimator = AffineEstimator::create(min_solver, non_min_solver); - } else CV_Error(cv::Error::StsNotImplemented, "Estimator not implemented!"); - - switch (params->getSampler()) { - case SamplingMethod::SAMPLING_UNIFORM: - sampler = UniformSampler::create(state++, min_sample_size, points_size); break; - case SamplingMethod::SAMPLING_PROSAC: - sampler = ProsacSampler::create(state++, points_size, min_sample_size, 200000); break; - case SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC: - sampler = ProgressiveNapsac::create(state++, points_size, min_sample_size, layers, 20); break; - case SamplingMethod::SAMPLING_NAPSAC: - sampler = NapsacSampler::create(state++, points_size, min_sample_size, graph); break; - default: CV_Error(cv::Error::StsNotImplemented, "Sampler is not implemented!"); - } - - switch (params->getVerifier()) { - case VerificationMethod::NullVerifier: verifier = ModelVerifier::create(); break; - case VerificationMethod::SprtVerifier: - verifier = SPRT::create(state++, error, points_size, params->getScore() == ScoreMethod ::SCORE_METHOD_MAGSAC ? max_thr : threshold, - params->getSPRTepsilon(), params->getSPRTdelta(), params->getTimeForModelEstimation(), - params->getSPRTavgNumModels(), params->getScore()); break; - default: CV_Error(cv::Error::StsNotImplemented, "Verifier is not imeplemented!"); - } - - if (params->getSampler() == SamplingMethod::SAMPLING_PROSAC) { - termination = ProsacTerminationCriteria::create(sampler.dynamicCast(), error, - points_size, min_sample_size, params->getConfidence(), - params->getMaxIters(), 100, 0.05, 0.05, threshold); - } else if (params->getSampler() == SamplingMethod::SAMPLING_PROGRESSIVE_NAPSAC) { - if (params->getVerifier() == VerificationMethod::SprtVerifier) - termination = SPRTPNapsacTermination::create(((SPRT *)verifier.get())->getSPRTvector(), - params->getConfidence(), points_size, min_sample_size, - params->getMaxIters(), params->getRelaxCoef()); - else - termination = StandardTerminationCriteria::create (params->getConfidence(), - points_size, min_sample_size, params->getMaxIters()); - } else if (params->getVerifier() == VerificationMethod::SprtVerifier) { - termination = SPRTTermination::create(((SPRT *) verifier.get())->getSPRTvector(), - params->getConfidence(), points_size, min_sample_size, params->getMaxIters()); - } else - termination = StandardTerminationCriteria::create - (params->getConfidence(), points_size, min_sample_size, params->getMaxIters()); - - if (params->getLO() != LocalOptimMethod::LOCAL_OPTIM_NULL) { - lo_sampler = UniformRandomGenerator::create(state++, points_size, params->getLOSampleSize()); - switch (params->getLO()) { - case LocalOptimMethod::LOCAL_OPTIM_INNER_LO: - lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler, - points_size, threshold, false, params->getLOIterativeSampleSize(), - params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(), - params->getLOThresholdMultiplier()); break; - case LocalOptimMethod::LOCAL_OPTIM_INNER_AND_ITER_LO: - lo = InnerIterativeLocalOptimization::create(estimator, quality, lo_sampler, - points_size, threshold, true, params->getLOIterativeSampleSize(), - params->getLOInnerMaxIters(), params->getLOIterativeMaxIters(), - params->getLOThresholdMultiplier()); break; - case LocalOptimMethod::LOCAL_OPTIM_GC: - lo = GraphCut::create(estimator, error, quality, graph, lo_sampler, threshold, - params->getGraphCutSpatialCoherenceTerm(), params->getLOInnerMaxIters()); break; - case LocalOptimMethod::LOCAL_OPTIM_SIGMA: - lo = SigmaConsensus::create(estimator, error, quality, verifier, - params->getLOSampleSize(), params->getLOInnerMaxIters(), - params->getDegreesOfFreedom(), params->getSigmaQuantile(), - params->getUpperIncompleteOfSigmaQuantile(), params->getC(), max_thr); break; - default: CV_Error(cv::Error::StsNotImplemented , "Local Optimization is not implemented!"); - } - } - - if (params->getFinalPolisher() == PolishingMethod::LSQPolisher) - polisher = LeastSquaresPolishing::create(estimator, quality, params->getFinalLSQIterations()); - - Ransac ransac (params, points_size, estimator, quality, sampler, - termination, verifier, degeneracy, lo, polisher, params->isParallel(), state); + Ransac ransac (params, points1, points2, K1_, K2_, dist_coeff1, dist_coeff2); if (ransac.run(ransac_output)) { if (params->isPnP()) { // convert R to rodrigues and back and recalculate inliers which due to numerical // issues can differ - Mat out, R, newR, newP, t, rvec; + Mat out, newP; + Matx33d R, newR, K1; + Vec3d t, rvec; if (K1_.empty()) { usac::Utils::decomposeProjection (ransac_output->getModel(), K1, R, t); Rodrigues(R, rvec); hconcat(rvec, t, out); hconcat(out, K1, out); } else { - const Mat Rt = K1.inv() * ransac_output->getModel(); + K1 = ransac.K1; + const Mat Rt = Mat(Matx33d(K1).inv() * Matx34d(ransac_output->getModel())); t = Rt.col(3); Rodrigues(Rt.colRange(0,3), rvec); hconcat(rvec, t, out); } + // Matx33d _K1(K1); Rodrigues(rvec, newR); - hconcat(K1 * newR, K1 * t, newP); - std::vector inliers_mask(points_size); - quality->getInliers(newP, inliers_mask); - ransac_output = RansacOutput::create(out, inliers_mask, 0,0,0,0,0,0); + hconcat(K1 * Matx33d(newR), K1 * Vec3d(t), newP); + std::vector inliers_mask(ransac.points_size); + ransac._quality->getInliers(newP, inliers_mask); + ransac_output = RansacOutput::create(out, inliers_mask, ransac_output->getNumberOfInliers(), + ransac_output->getNumberOfIters(), ransac_output->getConfidence(), ransac_output->getResiduals()); } return true; } diff --git a/modules/calib3d/src/usac/sampler.cpp b/modules/calib3d/src/usac/sampler.cpp index c44372853e..2095ee8b4d 100644 --- a/modules/calib3d/src/usac/sampler.cpp +++ b/modules/calib3d/src/usac/sampler.cpp @@ -40,9 +40,6 @@ public: points_random_pool[--random_pool_size]); } } - Ptr clone (int state) const override { - return makePtr(state, sample_size, points_size); - } private: void setPointsSize (int points_size_) { CV_Assert (sample_size <= points_size_); @@ -143,10 +140,6 @@ public: points_size = points_size_; initialize (); } - Ptr clone (int state) const override { - return makePtr(state, points_size, sample_size, - max_prosac_samples_count); - } private: void initialize () { largest_sample_size = points_size; // termination length, n* @@ -266,15 +259,19 @@ public: // Choice of the hypothesis generation set // if (t = T'_n) & (n < n*) then n = n + 1 (eqn. 4) - if (kth_sample_number == growth_function[subset_size-1] && subset_size < termination_length) + if (kth_sample_number >= growth_function[subset_size-1] && subset_size < termination_length) subset_size++; // Semi-random sample M_t of size m // if T'n < t then if (growth_function[subset_size-1] < kth_sample_number) { - // The sample contains m-1 points selected from U_(n-1) at random and u_n - random_gen->generateUniqueRandomSet(sample, sample_size-1, subset_size-1); - sample[sample_size-1] = subset_size-1; + if (subset_size >= termination_length) { + random_gen->generateUniqueRandomSet(sample, sample_size, subset_size); + } else { + // The sample contains m-1 points selected from U_(n-1) at random and u_n + random_gen->generateUniqueRandomSet(sample, sample_size-1, subset_size-1); + sample[sample_size-1] = subset_size-1; + } } else { // Select m points from U_n at random. random_gen->generateUniqueRandomSet(sample, sample_size, subset_size); @@ -306,10 +303,6 @@ public: CV_Error(cv::Error::StsError, "Changing points size in PROSAC requires to change also " "termination criteria! Use PROSAC simpler version"); } - Ptr clone (int state) const override { - return makePtr(state, points_size, sample_size, - growth_max_samples); - } }; Ptr ProsacSampler::create(int state, int points_size_, int sample_size_, @@ -465,10 +458,6 @@ public: CV_Error(cv::Error::StsError, "Changing points size requires changing neighborhood graph! " "You must reinitialize P-NAPSAC!"); } - Ptr clone (int state) const override { - return makePtr(state, points_size, sample_size, *layers, - sampler_length); - } }; Ptr ProgressiveNapsac::create(int state, int points_size_, int sample_size_, const std::vector> &layers, int sampler_length_) { @@ -537,9 +526,6 @@ public: CV_Error(cv::Error::StsError, "Changing points size requires changing neighborhood graph!" " You must reinitialize NAPSAC!"); } - Ptr clone (int state) const override { - return makePtr(state, points_size, sample_size, neighborhood_graph); - } }; Ptr NapsacSampler::create(int state, int points_size_, int sample_size_, const Ptr &neighborhood_graph_) { diff --git a/modules/calib3d/src/usac/termination.cpp b/modules/calib3d/src/usac/termination.cpp index 6c341a9366..803b060e41 100644 --- a/modules/calib3d/src/usac/termination.cpp +++ b/modules/calib3d/src/usac/termination.cpp @@ -28,7 +28,8 @@ public: * (1 - w^n) is probability that at least one point of N is outlier. * 1 - p = (1-w^n)^k is probability that in K steps of getting at least one outlier is 1% (5%). */ - int update (const Mat &/*model*/, int inlier_number) override { + int update (const Mat &model, int inlier_number) const override { + CV_UNUSED(model); const double predicted_iters = log_confidence / log(1 - std::pow (static_cast(inlier_number) / points_size, sample_size)); @@ -40,9 +41,11 @@ public: return MAX_ITERATIONS; } - Ptr clone () const override { - return makePtr(1-exp(log_confidence), points_size, - sample_size, MAX_ITERATIONS); + static int getMaxIterations (int inlier_number, int sample_size, int points_size, double conf) { + const double pred_iters = log(1 - conf) / log(1 - pow(static_cast(inlier_number)/points_size, sample_size)); + if (std::isinf(pred_iters)) + return INT_MAX; + return (int) pred_iters + 1; } }; Ptr StandardTerminationCriteria::create(double confidence, @@ -54,13 +57,13 @@ Ptr StandardTerminationCriteria::create(double conf /////////////////////////////////////// SPRT TERMINATION ////////////////////////////////////////// class SPRTTerminationImpl : public SPRTTermination { private: - const std::vector &sprt_histories; + const Ptr sprt; const double log_eta_0; const int points_size, sample_size, MAX_ITERATIONS; public: - SPRTTerminationImpl (const std::vector &sprt_histories_, double confidence, + SPRTTerminationImpl (const Ptr &sprt_, double confidence, int points_size_, int sample_size_, int max_iterations_) - : sprt_histories (sprt_histories_), log_eta_0(log(1-confidence)), + : sprt (sprt_), log_eta_0(log(1-confidence)), points_size (points_size_), sample_size (sample_size_),MAX_ITERATIONS(max_iterations_){} /* @@ -80,9 +83,11 @@ public: * this equation does not have to be evaluated before nR < n0 * nR = (1 - P_g)^k */ - int update (const Mat &/*model*/, int inlier_size) override { - if (sprt_histories.empty()) - return std::min(MAX_ITERATIONS, getStandardUpperBound(inlier_size)); + int update (const Mat &model, int inlier_size) const override { + CV_UNUSED(model); + const auto &sprt_histories = sprt->getSPRTvector(); + if (sprt_histories.size() <= 1) + return getStandardUpperBound(inlier_size); const double epsilon = static_cast(inlier_size) / points_size; // inlier probability const double P_g = pow (epsilon, sample_size); // probability of good sample @@ -90,23 +95,21 @@ public: double log_eta_lmin1 = 0; int total_number_of_tested_samples = 0; - const int sprts_size_min1 = static_cast(sprt_histories.size())-1; - if (sprts_size_min1 < 0) return getStandardUpperBound(inlier_size); // compute log n(l-1), l is number of tests - for (int test = 0; test < sprts_size_min1; test++) { - log_eta_lmin1 += log (1 - P_g * (1 - pow (sprt_histories[test].A, - -computeExponentH(sprt_histories[test].epsilon, epsilon,sprt_histories[test].delta)))) - * sprt_histories[test].tested_samples; - total_number_of_tested_samples += sprt_histories[test].tested_samples; + for (const auto &test : sprt_histories) { + if (test.tested_samples == 0) continue; + log_eta_lmin1 += log (1 - P_g * (1 - pow (test.A, + -computeExponentH(test.epsilon, epsilon,test.delta)))) * test.tested_samples; + total_number_of_tested_samples += test.tested_samples; } // Implementation note: since η > ηR the equation (9) does not have to be evaluated // before ηR < η0 is satisfied. if (std::pow(1 - P_g, total_number_of_tested_samples) < log_eta_0) - return std::min(MAX_ITERATIONS, getStandardUpperBound(inlier_size)); + return getStandardUpperBound(inlier_size); // use decision threshold A for last test (l-th) - const double predicted_iters_sprt = (log_eta_0 - log_eta_lmin1) / - log (1 - P_g * (1 - 1 / sprt_histories[sprts_size_min1].A)); // last A + const double predicted_iters_sprt = total_number_of_tested_samples + (log_eta_0 - log_eta_lmin1) / + log (1 - P_g * (1 - 1 / sprt_histories.back().A)); // last A if (std::isnan(predicted_iters_sprt) || std::isinf(predicted_iters_sprt)) return getStandardUpperBound(inlier_size); @@ -117,11 +120,6 @@ public: getStandardUpperBound(inlier_size)); return getStandardUpperBound(inlier_size); } - - Ptr clone () const override { - return makePtr(sprt_histories, 1-exp(log_eta_0), points_size, - sample_size, MAX_ITERATIONS); - } private: inline int getStandardUpperBound(int inlier_size) const { const double predicted_iters = log_eta_0 / log(1 - std::pow @@ -157,9 +155,9 @@ private: return h; } }; -Ptr SPRTTermination::create(const std::vector &sprt_histories_, +Ptr SPRTTermination::create(const Ptr &sprt_, double confidence, int points_size_, int sample_size_, int max_iterations_) { - return makePtr(sprt_histories_, confidence, points_size_, sample_size_, + return makePtr(sprt_, confidence, points_size_, sample_size_, max_iterations_); } @@ -167,21 +165,20 @@ Ptr SPRTTermination::create(const std::vector &sp class SPRTPNapsacTerminationImpl : public SPRTPNapsacTermination { private: SPRTTerminationImpl sprt_termination; - const std::vector &sprt_histories; const double relax_coef, log_confidence; const int points_size, sample_size, MAX_ITERS; public: - SPRTPNapsacTerminationImpl (const std::vector &sprt_histories_, + SPRTPNapsacTerminationImpl (const Ptr &sprt, double confidence, int points_size_, int sample_size_, int max_iterations_, double relax_coef_) - : sprt_termination (sprt_histories_, confidence, points_size_, sample_size_, - max_iterations_), sprt_histories (sprt_histories_), + : sprt_termination (sprt, confidence, points_size_, sample_size_, + max_iterations_), relax_coef (relax_coef_), log_confidence(log(1-confidence)), points_size (points_size_), sample_size (sample_size_), MAX_ITERS (max_iterations_) {} - int update (const Mat &model, int inlier_number) override { + int update (const Mat &model, int inlier_number) const override { int predicted_iterations = sprt_termination.update(model, inlier_number); const double inlier_prob = static_cast(inlier_number) / points_size + relax_coef; @@ -192,52 +189,41 @@ public: if (! std::isinf(predicted_iters) && predicted_iters < predicted_iterations) return static_cast(predicted_iters); - return predicted_iterations; - } - Ptr clone () const override { - return makePtr(sprt_histories, 1-exp(log_confidence), - points_size, sample_size, MAX_ITERS, relax_coef); + return std::min(MAX_ITERS, predicted_iterations); } }; -Ptr SPRTPNapsacTermination::create(const std::vector& - sprt_histories_, double confidence, int points_size_, int sample_size_, +Ptr SPRTPNapsacTermination::create(const Ptr & + sprt, double confidence, int points_size_, int sample_size_, int max_iterations_, double relax_coef_) { - return makePtr(sprt_histories_, confidence, points_size_, + return makePtr(sprt, confidence, points_size_, sample_size_, max_iterations_, relax_coef_); } -////////////////////////////////////// PROSAC TERMINATION ///////////////////////////////////////// +////////////////////////////////////// PROSAC TERMINATION ///////////////////////////////////////// class ProsacTerminationCriteriaImpl : public ProsacTerminationCriteria { private: - const double log_confidence, beta, non_randomness_phi, inlier_threshold; + const double log_conf, beta, non_randomness_phi, inlier_threshold; const int MAX_ITERATIONS, points_size, min_termination_length, sample_size; const Ptr sampler; - std::vector non_random_inliers; - const Ptr error; public: - ProsacTerminationCriteriaImpl (const Ptr &error_, int points_size_,int sample_size_, - double confidence, int max_iterations, int min_termination_length_, double beta_, - double non_randomness_phi_, double inlier_threshold_) : log_confidence - (log(1-confidence)), beta(beta_), non_randomness_phi(non_randomness_phi_), - inlier_threshold(inlier_threshold_), MAX_ITERATIONS(max_iterations), - points_size (points_size_), min_termination_length (min_termination_length_), - sample_size(sample_size_), error (error_) { init(); } - ProsacTerminationCriteriaImpl (const Ptr &sampler_,const Ptr &error_, int points_size_, int sample_size_, double confidence, int max_iterations, int min_termination_length_, double beta_, double non_randomness_phi_, - double inlier_threshold_) : log_confidence(log(1-confidence)), beta(beta_), + double inlier_threshold_, const std::vector &non_rand_inliers) : log_conf(log(1-confidence)), beta(beta_), non_randomness_phi(non_randomness_phi_), inlier_threshold(inlier_threshold_), MAX_ITERATIONS(max_iterations), points_size (points_size_), min_termination_length (min_termination_length_), sample_size(sample_size_), - sampler(sampler_), error (error_) { init(); } + sampler(sampler_), error (error_) { + CV_Assert(min_termination_length_ <= points_size_ && min_termination_length_ >= 0); + if (non_rand_inliers.empty()) + init(); + else non_random_inliers = non_rand_inliers; + } void init () { - // m is sample_size - // N is points_size - + // m is sample_size, N is points_size // non-randomness constraint // The non-randomness requirement prevents PROSAC // from selecting a solution supported by outliers that are @@ -245,20 +231,15 @@ public: // checked ex-post in standard approaches [1]. The distribution // of the cardinalities of sets of random ‘inliers’ is binomial // i-th entry - inlier counts for termination up to i-th point (term length = i+1) - - // ------------------------------------------------------------------------ // initialize the data structures that determine stopping // see probabilities description below. non_random_inliers = std::vector(points_size, 0); - std::vector pn_i_arr(points_size); + std::vector pn_i_arr(points_size, 0); const double beta2compl_beta = beta / (1-beta); const int step_n = 50, max_n = std::min(points_size, 1200); - for (int n = sample_size; n <= points_size; n+=step_n) { - if (n > max_n) { - // skip expensive calculation - break; - } + for (int n = sample_size; n < points_size; n+=step_n) { + if (n > max_n) break; // skip expensive calculation // P^R_n(i) = β^(i−m) (1−β)^(n−i+m) (n−m i−m). (7) i = m,...,N // initial value for i = m = sample_size @@ -288,7 +269,7 @@ public: non_random_inliers[n-1] = i_min; } - // approximate values of binomial distribution + // approximate values of binomial distribution using linear interpolation for (int n = sample_size; n <= points_size; n+=step_n) { if (n-1+step_n >= max_n) { // copy rest of the values @@ -301,19 +282,24 @@ public: non_random_inliers[n+i] = (int)(non_rand_n + (i+1)*step); } } + const std::vector &getNonRandomInliers () const override { return non_random_inliers; } /* * The PROSAC algorithm terminates if the number of inliers I_n* * within the set U_n* satisfies the following conditions: * - * • non-randomness – the probability that I_n* out of n* (termination_length) + * non-randomness – the probability that I_n* out of n* (termination_length) * data points are by chance inliers to an arbitrary incorrect model - * is smaller than Ψ (typically set to 5%) + * is smaller than Sigma (typically set to 5%) * - * • maximality – the probability that a solution with more than + * maximality – the probability that a solution with more than * In* inliers in U_n* exists and was not found after k - * samples is smaller than η0 (typically set to 5%). + * samples is smaller than eta_0 (typically set to 5%). */ - int update (const Mat &model, int inliers_size) override { + int update (const Mat &model, int inliers_size) const override { + int t; return updateTerminationLength(model, inliers_size, t); + } + int updateTerminationLength (const Mat &model, int inliers_size, int &found_termination_length) const override { + found_termination_length = points_size; int predicted_iterations = MAX_ITERATIONS; /* * The termination length n* is chosen to minimize k_n*(η0) subject to I_n* ≥ I_min n*; @@ -327,23 +313,22 @@ public: for (int pt = 0; pt < min_termination_length; pt++) if (errors[pt] < inlier_threshold) num_inliers_under_termination_len++; - for (int termination_len = min_termination_length; termination_len < points_size;termination_len++){ if (errors[termination_len /* = point*/] < inlier_threshold) { num_inliers_under_termination_len++; // non-random constraint must satisfy I_n* ≥ I_min n*. - if (num_inliers_under_termination_len < non_random_inliers[termination_len]) + if (num_inliers_under_termination_len < non_random_inliers[termination_len] || (double) num_inliers_under_termination_len/(points_size) < 0.2) continue; // add 1 to termination length since num_inliers_under_termination_len is updated - const double new_max_samples = log_confidence / log(1 - - std::pow(static_cast(num_inliers_under_termination_len) + const double new_max_samples = log_conf/log(1-pow(static_cast(num_inliers_under_termination_len) / (termination_len+1), sample_size)); if (! std::isinf(new_max_samples) && predicted_iterations > new_max_samples) { predicted_iterations = static_cast(new_max_samples); if (predicted_iterations == 0) break; + found_termination_length = termination_len; if (sampler != nullptr) sampler->setTerminationLength(termination_len); } @@ -352,27 +337,22 @@ public: // compare also when termination length = points_size, // so inliers under termination length is total number of inliers: - const double predicted_iters = log_confidence / log(1 - std::pow + const double predicted_iters = log_conf / log(1 - std::pow (static_cast(inliers_size) / points_size, sample_size)); if (! std::isinf(predicted_iters) && predicted_iters < predicted_iterations) return static_cast(predicted_iters); return predicted_iterations; } - - Ptr clone () const override { - return makePtr(error->clone(), - points_size, sample_size, 1-exp(log_confidence), MAX_ITERATIONS, - min_termination_length, beta, non_randomness_phi, inlier_threshold); - } }; Ptr ProsacTerminationCriteria::create(const Ptr &sampler, const Ptr &error, int points_size_, int sample_size_, double confidence, int max_iterations, - int min_termination_length_, double beta, double non_randomness_phi, double inlier_thresh) { + int min_termination_length_, double beta, double non_randomness_phi, double inlier_thresh, + const std::vector &non_rand_inliers) { return makePtr (sampler, error, points_size_, sample_size_, confidence, max_iterations, min_termination_length_, - beta, non_randomness_phi, inlier_thresh); + beta, non_randomness_phi, inlier_thresh, non_rand_inliers); } }} diff --git a/modules/calib3d/src/usac/utils.cpp b/modules/calib3d/src/usac/utils.cpp index c4d74eb663..5e2206702f 100644 --- a/modules/calib3d/src/usac/utils.cpp +++ b/modules/calib3d/src/usac/utils.cpp @@ -8,9 +8,234 @@ #include namespace cv { namespace usac { -double Utils::getCalibratedThreshold (double threshold, const Matx33d &K1, const Matx33d &K2) { - return threshold / ((K1(0, 0) + K1(1, 1) + - K2(0, 0) + K2(1, 1)) / 4.0); +/* +SolvePoly is used to find only real roots of N-degree polynomial using Sturm sequence. +It recursively finds interval where a root lies, and the actual root is found using Regula-Falsi method. +*/ +class SolvePoly : public SolverPoly { +private: + static int sgn(double val) { + return (double(0) < val) - (val < double(0)); + } + class Poly { + public: + Poly () = default; + Poly (const std::vector &coef_) { + coef = coef_; + checkDegree(); + } + Poly (const Poly &p) { coef = p.coef; } + // a_n x^n + a_n-1 x^(n-1) + ... + a_1 x + a_0 + // coef[i] = a_i + std::vector coef = {0}; + inline int degree() const { return (int)coef.size()-1; } + void multiplyScalar (double s) { + // multiplies polynom by scalar + if (fabs(s) < DBL_EPSILON) { // check if scalar is 0 + coef = {0}; + return; + } + for (double &c : coef) c *= s; + } + void checkDegree() { + int deg = degree(); // approximate degree + // check if coefficients of the highest power is non-zero + while (fabs(coef[deg]) < DBL_EPSILON) { + coef.pop_back(); // remove last zero element + if (--deg == 0) + break; + } + } + double eval (double x) const { + // Horner method a0 + x (a1 + x (a2 + x (a3 + ... + x (an-1 + x an)))) + const int d = degree(); + double y = coef[d]; + for (int i = d; i >= 1; i--) + y = coef[i-1] + x * y; + return y; + } + // at +inf and -inf + std::pair signsAtInf () const { + // lim x->+-inf p(x) = lim x->+-inf a_n x^n + const int d = degree(); + const int s = sgn(coef[d]); // sign of the highest coefficient + // compare even and odd degree + return std::make_pair(s, d % 2 == 0 ? s : -s); + } + Poly derivative () const { + Poly deriv; + if (degree() == 0) + return deriv; + // derive.degree = poly.degree-1; + deriv.coef = std::vector(coef.size()-1); + for (int i = degree(); i > 0; i--) + // (a_n * x^n)' = n * a_n * x^(n-1) + deriv.coef[i-1] = i * coef[i]; + return deriv; + } + void copyFrom (const Poly &p) { coef = p.coef; } + }; + // return remainder + static void dividePoly (const Poly &p1, const Poly &p2, /*Poly "ient,*/ Poly &remainder) { + remainder.copyFrom(p1); + int p2_degree = p2.degree(), remainder_degree = remainder.degree(); + if (p1.degree() < p2_degree) + return; + if (p2_degree == 0) { // special case for dividing polynomial by constant + remainder.multiplyScalar(1/p2.coef[0]); + // quotient.coef[0] = p2.coef[0]; + return; + } + // quotient.coef = std::vector(p1.degree() - p2_degree + 1, 0); + const double p2_term = 1/p2.coef[p2_degree]; + while (remainder_degree >= p2_degree) { + const double temp = remainder.coef[remainder_degree] * p2_term; + // quotient.coef[remainder_degree-p2_degree] = temp; + // polynoms now have the same degree, but p2 is shorter than remainder + for (int i = p2_degree, j = remainder_degree; i >= 0; i--, j--) + remainder.coef[j] -= temp * p2.coef[i]; + remainder.checkDegree(); + remainder_degree = remainder.degree(); + } + } + + constexpr static int REGULA_FALSI_MAX_ITERS = 500, MAX_POWER = 10, MAX_LEVEL = 200; + constexpr static double TOLERANCE = 1e-10, DIFF_TOLERANCE = 1e-7; + + static bool findRootRegulaFalsi (const Poly &poly, double min, double max, double &root) { + double f_min = poly.eval(min), f_max = poly.eval(max); + if (f_min * f_max > 0 || min > max) {// conditions are not fulfilled + return false; + } + int sign = 0, iter = 0; + for (; iter < REGULA_FALSI_MAX_ITERS; iter++) { + root = (f_min * max - f_max * min) / (f_min - f_max); + const double f_root = poly.eval(root); + if (fabs(f_root) < TOLERANCE || fabs(min - max) < DIFF_TOLERANCE) { + return true; // root is found + } + + if (f_root * f_max > 0) { + max = root; f_max = f_root; + if (sign == -1) + f_min *= 0.5; + sign = -1; + } else if (f_min * f_root > 0) { + min = root; f_min = f_root; + if (sign == 1) + f_max *= 0.5; + sign = 1; + } + } + return false; + } + + static int numberOfSignChanges (const std::vector &sturm, double x) { + int prev_sign = 0, sign_changes = 0; + for (const auto &poly : sturm) { + const int s = sgn(poly.eval(x)); + if (s != 0 && prev_sign != 0 && s != prev_sign) + sign_changes++; + prev_sign = s; + } + return sign_changes; + } + + static void findRootsRecursive (const Poly &poly, const std::vector &sturm, double min, double max, + int sign_changes_at_min, int sign_changes_at_max, std::vector &roots, int level) { + const int num_roots = sign_changes_at_min - sign_changes_at_max; + if (level == MAX_LEVEL) { + // roots are too close + const double mid = (min + max) * 0.5; + if (fabs(poly.eval(mid)) < DBL_EPSILON) { + roots.emplace_back(mid); + } + } else if (num_roots == 1) { + double root; + if (findRootRegulaFalsi(poly, min, max, root)) { + roots.emplace_back(root); + } + } else if (num_roots > 1) { // at least 2 roots + const double mid = (min + max) * 0.5; + const int sign_changes_at_mid = numberOfSignChanges(sturm, mid); + // try to split interval equally for the roots + if (sign_changes_at_min - sign_changes_at_mid > 0) + findRootsRecursive(poly, sturm, min, mid, sign_changes_at_min, sign_changes_at_mid, roots, level+1); + if (sign_changes_at_mid - sign_changes_at_max > 0) + findRootsRecursive(poly, sturm, mid, max, sign_changes_at_mid, sign_changes_at_max, roots, level+1); + } + } +public: + int getRealRoots (const std::vector &coeffs, std::vector &real_roots) override { + if (coeffs.empty()) + return 0; + Poly input(coeffs); + if (input.degree() < 1) + return 0; + // derivative of input polynomial + const Poly input_der = input.derivative(); + /////////// build Sturm sequence ////////// + Poly p (input), q (input_der), remainder; + std::vector> signs_at_inf; signs_at_inf.reserve(p.degree()); // +inf, -inf pair + signs_at_inf.emplace_back(p.signsAtInf()); + signs_at_inf.emplace_back(q.signsAtInf()); + std::vector sturm_sequence; sturm_sequence.reserve(input.degree()); + sturm_sequence.emplace_back(input); + sturm_sequence.emplace_back(input_der); + while (q.degree() > 0) { + dividePoly(p, q, remainder); + remainder.multiplyScalar(-1); + p.copyFrom(q); + q.copyFrom(remainder); + sturm_sequence.emplace_back(remainder); + signs_at_inf.emplace_back(remainder.signsAtInf()); + } + ////////// find changes in signs of Sturm sequence ///////// + int num_sign_changes_at_pos_inf = 0, num_sign_changes_at_neg_inf = 0; + int prev_sign_pos_inf = signs_at_inf[0].first, prev_sign_neg_inf = signs_at_inf[0].second; + for (int i = 1; i < (int)signs_at_inf.size(); i++) { + const auto s_pos_inf = signs_at_inf[i].first, s_neg_inf = signs_at_inf[i].second; + // zeros must be ignored + if (s_pos_inf != 0) { + if (prev_sign_pos_inf != 0 && prev_sign_pos_inf != s_pos_inf) + num_sign_changes_at_pos_inf++; + prev_sign_pos_inf = s_pos_inf; + } + if (s_neg_inf != 0) { + if (prev_sign_neg_inf != 0 && prev_sign_neg_inf != s_neg_inf) + num_sign_changes_at_neg_inf++; + prev_sign_neg_inf = s_neg_inf; + } + } + ////////// find roots' bounds for numerical method for roots finding ///////// + double root_neg_bound = -0.01, root_pos_bound = 0.01; + int num_sign_changes_min_x = -1, num_sign_changes_pos_x = -1; // -1 = unknown, trigger next if condition + for (int i = 0; i < MAX_POWER; i++) { + if (num_sign_changes_min_x != num_sign_changes_at_neg_inf) { + root_neg_bound *= 10; + num_sign_changes_min_x = numberOfSignChanges(sturm_sequence, root_neg_bound); + } + if (num_sign_changes_pos_x != num_sign_changes_at_pos_inf) { + root_pos_bound *= 10; + num_sign_changes_pos_x = numberOfSignChanges(sturm_sequence, root_pos_bound); + } + } + /////////// get real roots ////////// + real_roots.clear(); + findRootsRecursive(input, sturm_sequence, root_neg_bound, root_pos_bound, num_sign_changes_min_x, num_sign_changes_pos_x, real_roots, 0 /*level*/); + /////////////////////////////// + if ((int)real_roots.size() > input.degree()) + real_roots.resize(input.degree()); // must not happen, unless some roots repeat + return (int) real_roots.size(); + } +}; +Ptr SolverPoly::create() { + return makePtr(); +} + +double Utils::getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2) { + const auto * const k1 = (double *) K1.data, * const k2 = (double *) K2.data; + return threshold / ((k1[0] + k1[4] + k2[0] + k2[4]) / 4.0); } /* @@ -20,20 +245,22 @@ double Utils::getCalibratedThreshold (double threshold, const Matx33d &K1, const * 0 k22 k23 * 0 0 1] */ -void Utils::calibratePoints (const Matx33d &K1, const Matx33d &K2, const Mat &points, Mat &calib_points) { - const auto inv1_k11 = float(1 / K1(0, 0)); // 1 / k11 - const auto inv1_k12 = float(-K1(0, 1) / (K1(0, 0)*K1(1, 1))); // -k12 / (k11*k22) - // (-k13*k22 + k12*k23) / (k11*k22) - const auto inv1_k13 = float((-K1(0, 2)*K1(1, 1) + K1(0, 1)*K1(1, 2)) / (K1(0, 0)*K1(1, 1))); - const auto inv1_k22 = float(1 / K1(1, 1)); // 1 / k22 - const auto inv1_k23 = float(-K1(1, 2) / K1(1, 1)); // -k23 / k22 - - const auto inv2_k11 = float(1 / K2(0, 0)); - const auto inv2_k12 = float(-K2(0, 1) / (K2(0, 0)*K2(1, 1))); - const auto inv2_k13 = float((-K2(0, 2)*K2(1, 1) + K2(0, 1)*K2(1, 2)) / (K2(0, 0)*K2(1, 1))); - const auto inv2_k22 = float(1 / K2(1, 1)); - const auto inv2_k23 = float(-K2(1, 2) / K2(1, 1)); +void Utils::calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Mat &calib_points) { const auto * const points_ = (float *) points.data; + const auto * const k1 = (double *) K1.data; + const auto inv1_k11 = float(1 / k1[0]); // 1 / k11 + const auto inv1_k12 = float(-k1[1] / (k1[0]*k1[4])); // -k12 / (k11*k22) + // (-k13*k22 + k12*k23) / (k11*k22) + const auto inv1_k13 = float((-k1[2]*k1[4] + k1[1]*k1[5]) / (k1[0]*k1[4])); + const auto inv1_k22 = float(1 / k1[4]); // 1 / k22 + const auto inv1_k23 = float(-k1[5] / k1[4]); // -k23 / k22 + + const auto * const k2 = (double *) K2.data; + const auto inv2_k11 = float(1 / k2[0]); + const auto inv2_k12 = float(-k2[1] / (k2[0]*k2[4])); + const auto inv2_k13 = float((-k2[2]*k2[4] + k2[1]*k2[5]) / (k2[0]*k2[4])); + const auto inv2_k22 = float(1 / k2[4]); + const auto inv2_k23 = float(-k2[5] / k2[4]); calib_points = Mat ( points.rows, 4, points.type()); auto * calib_points_ = (float *) calib_points.data; @@ -52,13 +279,15 @@ void Utils::calibratePoints (const Matx33d &K1, const Matx33d &K2, const Mat &po * points is matrix of size |N| x 5, first two columns are image points [u_i, v_i] * calib_norm_pts are K^-1 [u v 1]^T / ||K^-1 [u v 1]^T|| */ -void Utils::calibrateAndNormalizePointsPnP (const Matx33d &K, const Mat &pts, Mat &calib_norm_pts) { +void Utils::calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &calib_norm_pts) { const auto * const points = (float *) pts.data; - const auto inv_k11 = float(1 / K(0, 0)); - const auto inv_k12 = float(-K(0, 1) / (K(0, 0)*K(1, 1))); - const auto inv_k13 = float((-K(0, 2)*K(1, 1) + K(0, 1)*K(1, 2)) / (K(0, 0)*K(1, 1))); - const auto inv_k22 = float(1 / K(1, 1)); - const auto inv_k23 = float(-K(1, 2) / K(1, 1)); + const auto * const k = (double *) K.data; + const auto inv_k11 = float(1 / k[0]); + const auto inv_k12 = float(-k[1] / (k[0]*k[4])); + const auto inv_k13 = float((-k[2]*k[4] + k[1]*k[5]) / (k[0]*k[4])); + const auto inv_k22 = float(1 / k[4]); + const auto inv_k23 = float(-k[5] / k[4]); + calib_norm_pts = Mat (pts.rows, 3, pts.type()); auto * calib_norm_pts_ = (float *) calib_norm_pts.data; @@ -73,9 +302,10 @@ void Utils::calibrateAndNormalizePointsPnP (const Matx33d &K, const Mat &pts, Ma } } -void Utils::normalizeAndDecalibPointsPnP (const Matx33d &K_, Mat &pts, Mat &calib_norm_pts) { - const auto k11 = (float)K_(0, 0), k12 = (float)K_(0, 1), k13 = (float)K_(0, 2), - k22 = (float)K_(1, 1), k23 = (float)K_(1, 2); +void Utils::normalizeAndDecalibPointsPnP (const Mat &K_, Mat &pts, Mat &calib_norm_pts) { + const auto * const K = (double *) K_.data; + const auto k11 = (float)K[0], k12 = (float)K[1], k13 = (float)K[2], + k22 = (float)K[4], k23 = (float)K[5]; calib_norm_pts = Mat (pts.rows, 3, pts.type()); auto * points = (float *) pts.data; auto * calib_norm_pts_ = (float *) calib_norm_pts.data; @@ -98,25 +328,95 @@ void Utils::normalizeAndDecalibPointsPnP (const Matx33d &K_, Mat &pts, Mat &cali * 0 fy ty * 0 0 1] */ -void Utils::decomposeProjection (const Mat &P, Matx33d &K_, Mat &R, Mat &t, bool same_focal) { - const Mat M = P.colRange(0,3); +void Utils::decomposeProjection (const Mat &P, Matx33d &K, Matx33d &R, Vec3d &t, bool same_focal) { + const Matx33d M = P.colRange(0,3); double scale = norm(M.row(2)); scale *= scale; - K_ = Matx33d::eye(); - K_(1,2) = M.row(1).dot(M.row(2)) / scale; - K_(0,2) = M.row(0).dot(M.row(2)) / scale; - K_(1,1) = sqrt(M.row(1).dot(M.row(1)) / scale - K_(1,2)*K_(1,2)); - K_(0,0) = sqrt(M.row(0).dot(M.row(0)) / scale - K_(0,2)*K_(0,2)); + K = Matx33d::eye(); + K(1,2) = M.row(1).dot(M.row(2)) / scale; + K(0,2) = M.row(0).dot(M.row(2)) / scale; + K(1,1) = sqrt(M.row(1).dot(M.row(1)) / scale - K(1,2)*K(1,2)); + K(0,0) = sqrt(M.row(0).dot(M.row(0)) / scale - K(0,2)*K(0,2)); if (same_focal) - K_(0,0) = K_(1,1) = (K_(0,0) + K_(1,1)) / 2; - R = K_.inv() * M / sqrt(scale); + K(0,0) = K(1,1) = (K(0,0) + K(1,1)) / 2; + R = K.inv() * M / sqrt(scale); if (determinant(M) < 0) R *= -1; - t = R * M.inv() * P.col(3); + t = R * M.inv() * Vec3d(P.col(3)); +} + +double Utils::getPoissonCDF (double lambda, int inliers) { + double exp_lamda = exp(-lambda), cdf = exp_lamda, lambda_i_div_fact_i = 1; + for (int i = 1; i <= inliers; i++) { + lambda_i_div_fact_i *= (lambda / i); + cdf += exp_lamda * lambda_i_div_fact_i; + if (fabs(cdf - 1) < DBL_EPSILON) // cdf is almost 1 + break; + } + return cdf; +} + +// since F(E) has rank 2 we use cross product to compute epipole, +// since the third column / row is linearly dependent on two first +// this is faster than SVD +// It is recommended to normalize F, such that ||F|| = 1 +Vec3d Utils::getLeftEpipole (const Mat &F/*E*/) { + Vec3d _e = F.col(0).cross(F.col(2)); // F^T e' = 0; e'^T F = 0 + const auto * const e = _e.val; + if (e[0] <= DBL_EPSILON && e[0] > -DBL_EPSILON && + e[1] <= DBL_EPSILON && e[1] > -DBL_EPSILON && + e[2] <= DBL_EPSILON && e[2] > -DBL_EPSILON) + _e = Vec3d(Mat(F.col(1))).cross(F.col(2)); // if e' is zero + return _e; // e' +} +Vec3d Utils::getRightEpipole (const Mat &F/*E*/) { + Vec3d _e = F.row(0).cross(F.row(2)); // Fe = 0 + const auto * const e = _e.val; + if (e[0] <= DBL_EPSILON && e[0] > -DBL_EPSILON && + e[1] <= DBL_EPSILON && e[1] > -DBL_EPSILON && + e[2] <= DBL_EPSILON && e[2] > -DBL_EPSILON) + _e = F.row(1).cross(F.row(2)); // if e is zero + return _e; +} + +void Utils::densitySort (const Mat &points, int knn, Mat &sorted_points, std::vector &sorted_mask) { + // mask of sorted points (array of indexes) + const int points_size = points.rows, dim = points.cols; + sorted_mask = std::vector(points_size); + for (int i = 0; i < points_size; i++) + sorted_mask[i] = i; + + // get neighbors + FlannNeighborhoodGraph &graph = *FlannNeighborhoodGraph::create(points, points_size, knn, + true /*get distances */, 6, 1); + + std::vector sum_knn_distances (points_size, 0); + for (int p = 0; p < points_size; p++) { + const std::vector &dists = graph.getNeighborsDistances(p); + for (int k = 0; k < knn; k++) + sum_knn_distances[p] += dists[k]; + } + + // compare by sum of distances to k nearest neighbors. + std::sort(sorted_mask.begin(), sorted_mask.end(), [&](int a, int b) { + return sum_knn_distances[a] < sum_knn_distances[b]; + }); + + // copy array of points to array with sorted points + // using @sorted_idx mask of sorted points indexes + + sorted_points = Mat(points_size, dim, points.type()); + const auto * const points_ptr = (float *) points.data; + auto * spoints_ptr = (float *) sorted_points.data; + for (int i = 0; i < points_size; i++) { + const int pt2 = sorted_mask[i] * dim; + for (int j = 0; j < dim; j++) + (*spoints_ptr++) = points_ptr[pt2+j]; + } } Matx33d Math::getSkewSymmetric(const Vec3d &v) { - return Matx33d(0, -v[2], v[1], - v[2], 0, -v[0], - -v[1], v[0], 0); + return {0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0}; } Matx33d Math::rotVec2RotMat (const Vec3d &v) { @@ -124,9 +424,9 @@ Matx33d Math::rotVec2RotMat (const Vec3d &v) { const double x = v[0] / phi, y = v[1] / phi, z = v[2] / phi; const double a = sin(phi), b = cos(phi); // R = I + sin(phi) * skew(v) + (1 - cos(phi) * skew(v)^2 - return Matx33d((b - 1)*y*y + (b - 1)*z*z + 1, -a*z - x*y*(b - 1), a*y - x*z*(b - 1), + return {(b - 1)*y*y + (b - 1)*z*z + 1, -a*z - x*y*(b - 1), a*y - x*z*(b - 1), a*z - x*y*(b - 1), (b - 1)*x*x + (b - 1)*z*z + 1, -a*x - y*z*(b - 1), - -a*y - x*z*(b - 1), a*x - y*z*(b - 1), (b - 1)*x*x + (b - 1)*y*y + 1); + -a*y - x*z*(b - 1), a*x - y*z*(b - 1), (b - 1)*x*x + (b - 1)*y*y + 1}; } Vec3d Math::rotMat2RotVec (const Matx33d &R) { @@ -176,7 +476,7 @@ bool Math::eliminateUpperTriangular (std::vector &a, int m, int n) { // if pivot value is 0 continue if (fabs(pivot) < DBL_EPSILON) - return false; // matrix is not full rank -> terminate + continue; // swap row with maximum pivot value with current row for (int c = r; c < n; c++) @@ -194,6 +494,18 @@ bool Math::eliminateUpperTriangular (std::vector &a, int m, int n) { return true; } +double Utils::intersectionOverUnion (const std::vector &a, const std::vector &b) { + int intersects = 0, unions = 0; + for (int i = 0; i < (int)a.size(); i++) + if (a[i] || b[i]) { + unions++; // one value is true + if (a[i] && b[i]) + intersects++; // a[i] == b[i] and if they both true + } + if (unions == 0) return 0.0; + return (double) intersects / unions; +} + //////////////////////////////////////// RANDOM GENERATOR ///////////////////////////// class UniformRandomGeneratorImpl : public UniformRandomGenerator { private: @@ -209,15 +521,12 @@ public: max_range = max_range_; subset = std::vector(subset_size_); } - int getRandomNumber () override { return rng.uniform(0, max_range); } - int getRandomNumber (int max_rng) override { return rng.uniform(0, max_rng); } - // closed range void resetGenerator (int max_range_) override { CV_CheckGE(0, max_range_, "max range must be greater than 0"); @@ -243,7 +552,6 @@ public: // interval is <0; max_range) void generateUniqueRandomSet (std::vector& sample, int max_range_) override { /* - * necessary condition: * if subset size is bigger than range then array cannot be unique, * so function has infinite loop. */ @@ -282,14 +590,12 @@ public: } return subset; } - void setSubsetSize (int subset_size_) override { + if (subset_size < subset_size_) + subset.resize(subset_size_); subset_size = subset_size_; } int getSubsetSize () const override { return subset_size; } - Ptr clone (int state) const override { - return makePtr(state, max_range, subset_size); - } }; Ptr UniformRandomGenerator::create (int state) { @@ -338,19 +644,17 @@ float Utils::findMedian (std::vector &array) { } else { // even: return average return (quicksort_median(array, length/2 , 0, length-1) + - quicksort_median(array, length/2+1, 0, length-1))/2; + quicksort_median(array, length/2+1, 0, length-1))*.5f; } } -/////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////// Radius Search Graph ///////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////////////////////// class RadiusSearchNeighborhoodGraphImpl : public RadiusSearchNeighborhoodGraph { private: std::vector> graph; public: RadiusSearchNeighborhoodGraphImpl (const Mat &container_, int points_size, - double radius, int flann_search_params, int num_kd_trees) { + double radius, int flann_search_params, int num_kd_trees) { // Radius search OpenCV works only with float data CV_Assert(container_.type() == CV_32F); @@ -363,6 +667,8 @@ public: int pt = 0; for (const auto &n : neighbours) { + if (n.size() <= 1) + continue; auto &graph_row = graph[pt]; graph_row = std::vector(n.size()-1); int j = 0; @@ -373,7 +679,7 @@ public: pt++; } } - + const std::vector> &getGraph () const override { return graph; } inline const std::vector &getNeighbors(int point_idx) const override { return graph[point_idx]; } @@ -384,9 +690,7 @@ Ptr RadiusSearchNeighborhoodGraph::create (const flann_search_params, num_kd_trees); } -/////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////// FLANN Graph ///////////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////////////////////// class FlannNeighborhoodGraphImpl : public FlannNeighborhoodGraph { private: std::vector> graph; @@ -425,6 +729,7 @@ public: const std::vector& getNeighborsDistances (int idx) const override { return distances[idx]; } + const std::vector> &getGraph () const override { return graph; } inline const std::vector &getNeighbors(int point_idx) const override { // CV_Assert(point_idx_ < num_vertices); return graph[point_idx]; @@ -438,9 +743,7 @@ Ptr FlannNeighborhoodGraph::create(const Mat &points, k_nearest_neighbors_, get_distances, flann_search_params_, num_kd_trees); } -/////////////////////////////////////////////////////////////////////////////////////////////////// ///////////////////////////////// Grid Neighborhood Graph ///////////////////////////////////////// -/////////////////////////////////////////////////////////////////////////////////////////////////// class GridNeighborhoodGraphImpl : public GridNeighborhoodGraph { private: // This struct is used for the nearest neighbors search by griding two images. @@ -460,13 +763,13 @@ private: } }; - std::map> neighbors_map; std::vector> graph; public: GridNeighborhoodGraphImpl (const Mat &container_, int points_size, int cell_size_x_img1, int cell_size_y_img1, int cell_size_x_img2, int cell_size_y_img2, int max_neighbors) { + std::map> neighbors_map; const auto * const container = (float *) container_.data; // -> {neighbors set} // Key is cell position. The value is indexes of neighbors. @@ -490,7 +793,6 @@ public: // store neighbors cells into graph (2D vector) for (const auto &cell : neighbors_map) { const int neighbors_in_cell = static_cast(cell.second.size()); - // only one point in cell -> no neighbors if (neighbors_in_cell < 2) continue; @@ -510,7 +812,7 @@ public: } } } - + const std::vector> &getGraph () const override { return graph; } inline const std::vector &getNeighbors(int point_idx) const override { // Note, neighbors vector also includes point_idx! // return neighbors_map[vertices_to_cells[point_idx]]; @@ -524,4 +826,94 @@ Ptr GridNeighborhoodGraph::create(const Mat &points, return makePtr(points, points_size, cell_size_x_img1_, cell_size_y_img1_, cell_size_x_img2_, cell_size_y_img2_, max_neighbors); } + +class GridNeighborhoodGraph2ImagesImpl : public GridNeighborhoodGraph2Images { +private: + // This struct is used for the nearest neighbors search by griding two images. + struct CellCoord { + int c1x, c1y; + CellCoord (int c1x_, int c1y_) { + c1x = c1x_; c1y = c1y_; + } + bool operator==(const CellCoord &o) const { + return c1x == o.c1x && c1y == o.c1y; + } + bool operator<(const CellCoord &o) const { + if (c1x < o.c1x) return true; + return c1x == o.c1x && c1y < o.c1y; + } + }; + + std::vector> graph; +public: + GridNeighborhoodGraph2ImagesImpl (const Mat &container_, int points_size, + float cell_size_x_img1, float cell_size_y_img1, float cell_size_x_img2, float cell_size_y_img2) { + + std::map> neighbors_map1, neighbors_map2; + const auto * const container = (float *) container_.data; + // Key is cell position. The value is indexes of neighbors. + + const auto cell_sz_x1 = 1.f / cell_size_x_img1, + cell_sz_y1 = 1.f / cell_size_y_img1, + cell_sz_x2 = 1.f / cell_size_x_img2, + cell_sz_y2 = 1.f / cell_size_y_img2; + const int dimension = container_.cols; + for (int i = 0; i < points_size; i++) { + const int idx = dimension * i; + neighbors_map1[CellCoord((int)(container[idx ] * cell_sz_x1), + (int)(container[idx+1] * cell_sz_y1))].emplace_back(i); + neighbors_map2[CellCoord((int)(container[idx+2] * cell_sz_x2), + (int)(container[idx+3] * cell_sz_y2))].emplace_back(i); + } + + //--------- create a graph ---------- + graph = std::vector>(points_size); + + // store neighbors cells into graph (2D vector) + for (const auto &cell : neighbors_map1) { + const int neighbors_in_cell = static_cast(cell.second.size()); + // only one point in cell -> no neighbors + if (neighbors_in_cell < 2) continue; + + const std::vector &neighbors = cell.second; + // ---------- fill graph ----- + // for speed-up we make no symmetric graph, eg, x has a neighbor y, but y does not have x + const int v_in_cell = neighbors[0]; + // there is always at least one neighbor + auto &graph_row = graph[v_in_cell]; + graph_row.reserve(neighbors_in_cell); + for (int n : neighbors) + if (n != v_in_cell) + graph_row.emplace_back(n); + } + + // fill neighbors of a second image + for (const auto &cell : neighbors_map2) { + if (cell.second.size() < 2) continue; + const std::vector &neighbors = cell.second; + const int v_in_cell = neighbors[0]; + auto &graph_row = graph[v_in_cell]; + for (const int &n : neighbors) + if (n != v_in_cell) { + bool has = false; + for (const int &nn : graph_row) + if (n == nn) { + has = true; break; + } + if (!has) graph_row.emplace_back(n); + } + } + } + const std::vector> &getGraph () const override { return graph; } + inline const std::vector &getNeighbors(int point_idx) const override { + // Note, neighbors vector also includes point_idx! + return graph[point_idx]; + } +}; + +Ptr GridNeighborhoodGraph2Images::create(const Mat &points, + int points_size, float cell_size_x_img1_, float cell_size_y_img1_, float cell_size_x_img2_, float cell_size_y_img2_) { + return makePtr(points, points_size, + cell_size_x_img1_, cell_size_y_img1_, cell_size_x_img2_, cell_size_y_img2_); +} }} \ No newline at end of file diff --git a/modules/calib3d/test/test_usac.cpp b/modules/calib3d/test/test_usac.cpp index f7c4b9e4ce..90dcb76ffc 100644 --- a/modules/calib3d/test/test_usac.cpp +++ b/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(0), 2) + pow(l1.at(1), 2) + - pow(l2.at(0), 2) + pow(l2.at(1), 2)); - else // symmetric geometric distance - return sqrt(pow(pt1.dot(l1),2) / (pow(l1.at(0),2) + pow(l1.at(1),2)) + - pow(pt2.dot(l2),2) / (pow(l2.at(0),2) + pow(l2.at(1),2))); + // Sampson error + return fabs(pt2.dot(l2)) / sqrt(pow(l1.at(0), 2) + pow(l1.at(1), 2) + + pow(l2.at(0), 2) + pow(l2.at(1), 2)); } else if (test_case == TestSolver::PnP) { // PnP, reprojection error cv::Mat img_pt = model * pt2; img_pt /= img_pt.at(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());