1 // This file is part of OpenCV project. 2 // It is subject to the license terms in the LICENSE file found in the top-level directory 3 // of this distribution and at http://opencv.org/license.html. 4 5 #ifndef OPENCV_USAC_USAC_HPP 6 #define OPENCV_USAC_USAC_HPP 7 8 namespace cv { namespace usac { 9 enum EstimationMethod { Homography, Fundamental, Fundamental8, Essential, Affine, P3P, P6P}; 10 enum VerificationMethod { NullVerifier, SprtVerifier }; 11 enum PolishingMethod { NonePolisher, LSQPolisher }; 12 enum ErrorMetric {DIST_TO_LINE, SAMPSON_ERR, SGD_ERR, SYMM_REPR_ERR, FORW_REPR_ERR, RERPOJ}; 13 14 // Abstract Error class 15 class Error : public Algorithm { 16 public: 17 // set model to use getError() function 18 virtual void setModelParameters (const Mat &model) = 0; 19 // returns error of point wih @point_idx w.r.t. model 20 virtual float getError (int point_idx) const = 0; 21 virtual const std::vector<float> &getErrors (const Mat &model) = 0; 22 virtual Ptr<Error> clone () const = 0; 23 }; 24 25 // Symmetric Reprojection Error for Homography 26 class ReprojectionErrorSymmetric : public Error { 27 public: 28 static Ptr<ReprojectionErrorSymmetric> create(const Mat &points); 29 }; 30 31 // Forward Reprojection Error for Homography 32 class ReprojectionErrorForward : public Error { 33 public: 34 static Ptr<ReprojectionErrorForward> create(const Mat &points); 35 }; 36 37 // Sampson Error for Fundamental matrix 38 class SampsonError : public Error { 39 public: 40 static Ptr<SampsonError> create(const Mat &points); 41 }; 42 43 // Symmetric Geometric Distance (to epipolar lines) for Fundamental and Essential matrix 44 class SymmetricGeometricDistance : public Error { 45 public: 46 static Ptr<SymmetricGeometricDistance> create(const Mat &points); 47 }; 48 49 // Reprojection Error for Projection matrix 50 class ReprojectionErrorPmatrix : public Error { 51 public: 52 static Ptr<ReprojectionErrorPmatrix> create(const Mat &points); 53 }; 54 55 // Reprojection Error for Affine matrix 56 class ReprojectionErrorAffine : public Error { 57 public: 58 static Ptr<ReprojectionErrorAffine> create(const Mat &points); 59 }; 60 61 // Normalizing transformation of data points 62 class NormTransform : public Algorithm { 63 public: 64 /* 65 * @norm_points is output matrix of size pts_size x 4 66 * @sample constains indices of points 67 * @sample_number is number of used points in sample <0; sample_number) 68 * @T1, T2 are output transformation matrices 69 */ 70 virtual void getNormTransformation (Mat &norm_points, const std::vector<int> &sample, 71 int sample_number, Matx33d &T1, Matx33d &T2) const = 0; 72 static Ptr<NormTransform> create (const Mat &points); 73 }; 74 75 ///////////////////////////////////////////////////////////////////////////////////////////// 76 ////////////////////////////////////////// SOLVER /////////////////////////////////////////// 77 class MinimalSolver : public Algorithm { 78 public: 79 // Estimate models from minimal sample. models.size() == number of found solutions 80 virtual int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const = 0; 81 // return minimal sample size required for estimation. 82 virtual int getSampleSize() const = 0; 83 // return maximum number of possible solutions. 84 virtual int getMaxNumberOfSolutions () const = 0; 85 virtual Ptr<MinimalSolver> clone () const = 0; 86 }; 87 88 //-------------------------- HOMOGRAPHY MATRIX ----------------------- 89 class HomographyMinimalSolver4ptsGEM : public MinimalSolver { 90 public: 91 static Ptr<HomographyMinimalSolver4ptsGEM> create(const Mat &points_); 92 }; 93 94 //-------------------------- FUNDAMENTAL MATRIX ----------------------- 95 class FundamentalMinimalSolver7pts : public MinimalSolver { 96 public: 97 static Ptr<FundamentalMinimalSolver7pts> create(const Mat &points_); 98 }; 99 100 class FundamentalMinimalSolver8pts : public MinimalSolver { 101 public: 102 static Ptr<FundamentalMinimalSolver8pts> create(const Mat &points_); 103 }; 104 105 //-------------------------- ESSENTIAL MATRIX ----------------------- 106 class EssentialMinimalSolverStewenius5pts : public MinimalSolver { 107 public: 108 static Ptr<EssentialMinimalSolverStewenius5pts> create(const Mat &points_); 109 }; 110 111 //-------------------------- PNP ----------------------- 112 class PnPMinimalSolver6Pts : public MinimalSolver { 113 public: 114 static Ptr<PnPMinimalSolver6Pts> create(const Mat &points_); 115 }; 116 117 class P3PSolver : public MinimalSolver { 118 public: 119 static Ptr<P3PSolver> create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K); 120 }; 121 122 //-------------------------- AFFINE ----------------------- 123 class AffineMinimalSolver : public MinimalSolver { 124 public: 125 static Ptr<AffineMinimalSolver> create(const Mat &points_); 126 }; 127 128 //////////////////////////////////////// NON MINIMAL SOLVER /////////////////////////////////////// 129 class NonMinimalSolver : public Algorithm { 130 public: 131 // Estimate models from non minimal sample. models.size() == number of found solutions 132 virtual int estimate (const std::vector<int> &sample, int sample_size, 133 std::vector<Mat> &models, const std::vector<double> &weights) const = 0; 134 // return minimal sample size required for non-minimal estimation. 135 virtual int getMinimumRequiredSampleSize() const = 0; 136 // return maximum number of possible solutions. 137 virtual int getMaxNumberOfSolutions () const = 0; 138 virtual Ptr<NonMinimalSolver> clone () const = 0; 139 }; 140 141 //-------------------------- HOMOGRAPHY MATRIX ----------------------- 142 class HomographyNonMinimalSolver : public NonMinimalSolver { 143 public: 144 static Ptr<HomographyNonMinimalSolver> create(const Mat &points_); 145 }; 146 147 //-------------------------- FUNDAMENTAL MATRIX ----------------------- 148 class FundamentalNonMinimalSolver : public NonMinimalSolver { 149 public: 150 static Ptr<FundamentalNonMinimalSolver> create(const Mat &points_); 151 }; 152 153 //-------------------------- ESSENTIAL MATRIX ----------------------- 154 class EssentialNonMinimalSolver : public NonMinimalSolver { 155 public: 156 static Ptr<EssentialNonMinimalSolver> create(const Mat &points_); 157 }; 158 159 //-------------------------- PNP ----------------------- 160 class PnPNonMinimalSolver : public NonMinimalSolver { 161 public: 162 static Ptr<PnPNonMinimalSolver> create(const Mat &points); 163 }; 164 165 class DLSPnP : public NonMinimalSolver { 166 public: 167 static Ptr<DLSPnP> create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K); 168 }; 169 170 //-------------------------- AFFINE ----------------------- 171 class AffineNonMinimalSolver : public NonMinimalSolver { 172 public: 173 static Ptr<AffineNonMinimalSolver> create(const Mat &points_); 174 }; 175 176 ////////////////////////////////////////////////////////////////////////////////////////////// 177 ////////////////////////////////////////// SCORE /////////////////////////////////////////// 178 class Score { 179 public: 180 int inlier_number; 181 double score; Score()182 Score () { // set worst case 183 inlier_number = 0; 184 score = std::numeric_limits<double>::max(); 185 } Score(int inlier_number_,double score_)186 Score (int inlier_number_, double score_) { // copy constructor 187 inlier_number = inlier_number_; 188 score = score_; 189 } 190 // Compare two scores. Objective is minimization of score. Lower score is better. isBetter(const Score & score2) const191 inline bool isBetter (const Score &score2) const { 192 return score < score2.score; 193 } 194 }; 195 196 class GammaValues 197 { 198 const double max_range_complete /*= 4.62*/, max_range_gamma /*= 1.52*/; 199 const int max_size_table /* = 3000 */; 200 201 std::vector<double> gamma_complete, gamma_incomplete, gamma; 202 203 GammaValues(); // use getSingleton() 204 205 public: 206 static const GammaValues& getSingleton(); 207 208 const std::vector<double>& getCompleteGammaValues() const; 209 const std::vector<double>& getIncompleteGammaValues() const; 210 const std::vector<double>& getGammaValues() const; 211 double getScaleOfGammaCompleteValues () const; 212 double getScaleOfGammaValues () const; 213 int getTableSize () const; 214 }; 215 216 ////////////////////////////////////////// QUALITY /////////////////////////////////////////// 217 class Quality : public Algorithm { 218 public: 219 virtual ~Quality() override = default; 220 /* 221 * Calculates number of inliers and score of the @model. 222 * return Score with calculated inlier_number and score. 223 * @model: Mat current model, e.g., H matrix. 224 */ 225 virtual Score getScore (const Mat &model) const = 0; getScore(const std::vector<float> &) const226 virtual Score getScore (const std::vector<float> &/*errors*/) const { 227 CV_Error(cv::Error::StsNotImplemented, "getScore(errors)"); 228 } 229 // get @inliers of the @model. Assume threshold is given 230 // @inliers must be preallocated to maximum points size. 231 virtual int getInliers (const Mat &model, std::vector<int> &inliers) const = 0; 232 // get @inliers of the @model for given threshold 233 virtual int getInliers (const Mat &model, std::vector<int> &inliers, double thr) const = 0; 234 // Set the best score, so evaluation of the model can terminate earlier 235 virtual void setBestScore (double best_score_) = 0; 236 // set @inliers_mask: true if point i is inlier, false - otherwise. 237 virtual int getInliers (const Mat &model, std::vector<bool> &inliers_mask) const = 0; 238 virtual int getPointsSize() const = 0; 239 virtual Ptr<Quality> clone () const = 0; 240 static int getInliers (const Ptr<Error> &error, const Mat &model, 241 std::vector<bool> &inliers_mask, double threshold); 242 static int getInliers (const Ptr<Error> &error, const Mat &model, 243 std::vector<int> &inliers, double threshold); 244 }; 245 246 // RANSAC (binary) quality 247 class RansacQuality : public Quality { 248 public: 249 static Ptr<RansacQuality> create(int points_size_, double threshold_,const Ptr<Error> &error_); 250 }; 251 252 // M-estimator quality - truncated Squared error 253 class MsacQuality : public Quality { 254 public: 255 static Ptr<MsacQuality> create(int points_size_, double threshold_, const Ptr<Error> &error_); 256 }; 257 258 // Marginlizing Sample Consensus quality, D. Barath et al. 259 class MagsacQuality : public Quality { 260 public: 261 static Ptr<MagsacQuality> create(double maximum_thr, int points_size_,const Ptr<Error> &error_, 262 double tentative_inlier_threshold_, int DoF, double sigma_quantile, 263 double upper_incomplete_of_sigma_quantile, 264 double lower_incomplete_of_sigma_quantile, double C_); 265 }; 266 267 // Least Median of Squares Quality 268 class LMedsQuality : public Quality { 269 public: 270 static Ptr<LMedsQuality> create(int points_size_, double threshold_, const Ptr<Error> &error_); 271 }; 272 273 ////////////////////////////////////////////////////////////////////////////////////// 274 //////////////////////////////////////// DEGENERACY ////////////////////////////////// 275 class Degeneracy : public Algorithm { 276 public: 277 virtual ~Degeneracy() override = default; 278 /* 279 * Check if sample causes degenerate configurations. 280 * For example, test if points are collinear. 281 */ isSampleGood(const std::vector<int> &) const282 virtual bool isSampleGood (const std::vector<int> &/*sample*/) const { 283 return true; 284 } 285 /* 286 * Check if model satisfies constraints. 287 * For example, test if epipolar geometry satisfies oriented constraint. 288 */ isModelValid(const Mat &,const std::vector<int> &) const289 virtual bool isModelValid (const Mat &/*model*/, const std::vector<int> &/*sample*/) const { 290 return true; 291 } 292 /* 293 * Fix degenerate model. 294 * Return true if model is degenerate, false - otherwise 295 */ recoverIfDegenerate(const std::vector<int> &,const Mat &,Mat &,Score &)296 virtual bool recoverIfDegenerate (const std::vector<int> &/*sample*/,const Mat &/*best_model*/, 297 Mat &/*non_degenerate_model*/, Score &/*non_degenerate_model_score*/) { 298 return false; 299 } clone(int) const300 virtual Ptr<Degeneracy> clone(int /*state*/) const { return makePtr<Degeneracy>(); } 301 }; 302 303 class EpipolarGeometryDegeneracy : public Degeneracy { 304 public: 305 static void recoverRank (Mat &model, bool is_fundamental_mat); 306 static Ptr<EpipolarGeometryDegeneracy> create (const Mat &points_, int sample_size_); 307 }; 308 309 class EssentialDegeneracy : public EpipolarGeometryDegeneracy { 310 public: 311 static Ptr<EssentialDegeneracy>create (const Mat &points, int sample_size); 312 }; 313 314 class HomographyDegeneracy : public Degeneracy { 315 public: 316 static Ptr<HomographyDegeneracy> create(const Mat &points_); 317 }; 318 319 class FundamentalDegeneracy : public EpipolarGeometryDegeneracy { 320 public: 321 // threshold for homography is squared so is around 2.236 pixels 322 static Ptr<FundamentalDegeneracy> create (int state, const Ptr<Quality> &quality_, 323 const Mat &points_, int sample_size_, double homography_threshold); 324 }; 325 326 ///////////////////////////////////////////////////////////////////////////////////// 327 //////////////////////////////////////// ESTIMATOR ////////////////////////////////// 328 class Estimator : public Algorithm{ 329 public: 330 /* 331 * Estimate models with minimal solver. 332 * Return number of valid solutions after estimation. 333 * Return models accordingly to number of solutions. 334 * Note, vector of models must allocated before. 335 * Note, not all degenerate tests are included in estimation. 336 */ 337 virtual int 338 estimateModels (const std::vector<int> &sample, std::vector<Mat> &models) const = 0; 339 /* 340 * Estimate model with non-minimal solver. 341 * Return number of valid solutions after estimation. 342 * Note, not all degenerate tests are included in estimation. 343 */ 344 virtual int 345 estimateModelNonMinimalSample (const std::vector<int> &sample, int sample_size, 346 std::vector<Mat> &models, const std::vector<double> &weights) const = 0; 347 // return minimal sample size required for minimal estimation. 348 virtual int getMinimalSampleSize () const = 0; 349 // return minimal sample size required for non-minimal estimation. 350 virtual int getNonMinimalSampleSize () const = 0; 351 // return maximum number of possible solutions of minimal estimation. 352 virtual int getMaxNumSolutions () const = 0; 353 // return maximum number of possible solutions of non-minimal estimation. 354 virtual int getMaxNumSolutionsNonMinimal () const = 0; 355 virtual Ptr<Estimator> clone() const = 0; 356 }; 357 358 class HomographyEstimator : public Estimator { 359 public: 360 static Ptr<HomographyEstimator> create (const Ptr<MinimalSolver> &min_solver_, 361 const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> °eneracy_); 362 }; 363 364 class FundamentalEstimator : public Estimator { 365 public: 366 static Ptr<FundamentalEstimator> create (const Ptr<MinimalSolver> &min_solver_, 367 const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> °eneracy_); 368 }; 369 370 class EssentialEstimator : public Estimator { 371 public: 372 static Ptr<EssentialEstimator> create (const Ptr<MinimalSolver> &min_solver_, 373 const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> °eneracy_); 374 }; 375 376 class AffineEstimator : public Estimator { 377 public: 378 static Ptr<AffineEstimator> create (const Ptr<MinimalSolver> &min_solver_, 379 const Ptr<NonMinimalSolver> &non_min_solver_); 380 }; 381 382 class PnPEstimator : public Estimator { 383 public: 384 static Ptr<PnPEstimator> create (const Ptr<MinimalSolver> &min_solver_, 385 const Ptr<NonMinimalSolver> &non_min_solver_); 386 }; 387 388 ////////////////////////////////////////////////////////////////////////////////////////////// 389 ////////////////////////////////////////// MODEL VERIFIER //////////////////////////////////// 390 class ModelVerifier : public Algorithm { 391 public: 392 virtual ~ModelVerifier() override = default; 393 // Return true if model is good, false - otherwise. 394 virtual bool isModelGood(const Mat &model) = 0; 395 // Return true if score was computed during evaluation. 396 virtual bool getScore(Score &score) const = 0; 397 // update verifier by given inlier number 398 virtual void update (int highest_inlier_number) = 0; 399 virtual const std::vector<float> &getErrors() const = 0; 400 virtual bool hasErrors () const = 0; 401 virtual Ptr<ModelVerifier> clone (int state) const = 0; 402 static Ptr<ModelVerifier> create(); 403 }; 404 405 struct SPRT_history { 406 /* 407 * delta: 408 * The probability of a data point being consistent 409 * with a ‘bad’ model is modeled as a probability of 410 * a random event with Bernoulli distribution with parameter 411 * δ : p(1|Hb) = δ. 412 413 * epsilon: 414 * The probability p(1|Hg) = ε 415 * that any randomly chosen data point is consistent with a ‘good’ model 416 * is approximated by the fraction of inliers ε among the data 417 * points 418 419 * A is the decision threshold, the only parameter of the Adapted SPRT 420 */ 421 double epsilon, delta, A; 422 // number of samples processed by test 423 int tested_samples; // k SPRT_historycv::usac::SPRT_history424 SPRT_history () : epsilon(0), delta(0), A(0) { 425 tested_samples = 0; 426 } 427 }; 428 429 ///////////////////////////////// SPRT VERIFIER ///////////////////////////////////////// 430 /* 431 * Matas, Jiri, and Ondrej Chum. "Randomized RANSAC with sequential probability ratio test." 432 * Tenth IEEE International Conference on Computer Vision (ICCV'05) Volume 1. Vol. 2. IEEE, 2005. 433 */ 434 class SPRT : public ModelVerifier { 435 public: 436 // return constant reference of vector of SPRT histories for SPRT termination. 437 virtual const std::vector<SPRT_history> &getSPRTvector () const = 0; 438 static Ptr<SPRT> create (int state, const Ptr<Error> &err_, int points_size_, 439 double inlier_threshold_, double prob_pt_of_good_model, 440 double prob_pt_of_bad_model, double time_sample, double avg_num_models, 441 ScoreMethod score_type_); 442 }; 443 444 ////////////////////////////////////////////////////////////////////////////////////////// 445 ////////////////////////////////////////// SAMPLER /////////////////////////////////////// 446 class Sampler : public Algorithm { 447 public: 448 virtual ~Sampler() override = default; 449 // set new points size 450 virtual void setNewPointsSize (int points_size) = 0; 451 // generate sample. Fill @sample with indices of points. 452 virtual void generateSample (std::vector<int> &sample) = 0; 453 virtual Ptr<Sampler> clone (int state) const = 0; 454 }; 455 456 //////////////////////////////////////////////////////////////////////////////////////////////// 457 /////////////////////////////////// NEIGHBORHOOD GRAPH ///////////////////////////////////////// 458 class NeighborhoodGraph : public Algorithm { 459 public: 460 virtual ~NeighborhoodGraph() override = default; 461 // Return neighbors of the point with index @point_idx_ in the graph. 462 virtual const std::vector<int> &getNeighbors(int point_idx_) const = 0; 463 }; 464 465 class RadiusSearchNeighborhoodGraph : public NeighborhoodGraph { 466 public: 467 static Ptr<RadiusSearchNeighborhoodGraph> create (const Mat &points, int points_size, 468 double radius_, int flann_search_params, int num_kd_trees); 469 }; 470 471 class FlannNeighborhoodGraph : public NeighborhoodGraph { 472 public: 473 static Ptr<FlannNeighborhoodGraph> create(const Mat &points, int points_size, 474 int k_nearest_neighbors_, bool get_distances, int flann_search_params, int num_kd_trees); 475 virtual const std::vector<double> &getNeighborsDistances (int idx) const = 0; 476 }; 477 478 class GridNeighborhoodGraph : public NeighborhoodGraph { 479 public: 480 static Ptr<GridNeighborhoodGraph> create(const Mat &points, int points_size, 481 int cell_size_x_img1_, int cell_size_y_img1_, 482 int cell_size_x_img2_, int cell_size_y_img2_, int max_neighbors); 483 }; 484 485 ////////////////////////////////////// UNIFORM SAMPLER //////////////////////////////////////////// 486 class UniformSampler : public Sampler { 487 public: 488 static Ptr<UniformSampler> create(int state, int sample_size_, int points_size_); 489 }; 490 491 /////////////////////////////////// PROSAC (SIMPLE) SAMPLER /////////////////////////////////////// 492 class ProsacSimpleSampler : public Sampler { 493 public: 494 static Ptr<ProsacSimpleSampler> create(int state, int points_size_, int sample_size_, 495 int max_prosac_samples_count); 496 }; 497 498 ////////////////////////////////////// PROSAC SAMPLER //////////////////////////////////////////// 499 class ProsacSampler : public Sampler { 500 public: 501 static Ptr<ProsacSampler> create(int state, int points_size_, int sample_size_, 502 int growth_max_samples); 503 // return number of samples generated (for prosac termination). 504 virtual int getKthSample () const = 0; 505 // return constant reference of growth function of prosac sampler (for prosac termination) 506 virtual const std::vector<int> &getGrowthFunction () const = 0; 507 virtual void setTerminationLength (int termination_length) = 0; 508 }; 509 510 ////////////////////////// NAPSAC (N adjacent points sample consensus) SAMPLER //////////////////// 511 class NapsacSampler : public Sampler { 512 public: 513 static Ptr<NapsacSampler> create(int state, int points_size_, int sample_size_, 514 const Ptr<NeighborhoodGraph> &neighborhood_graph_); 515 }; 516 517 ////////////////////////////////////// P-NAPSAC SAMPLER ///////////////////////////////////////// 518 class ProgressiveNapsac : public Sampler { 519 public: 520 static Ptr<ProgressiveNapsac> create(int state, int points_size_, int sample_size_, 521 const std::vector<Ptr<NeighborhoodGraph>> &layers, int sampler_length); 522 }; 523 524 ///////////////////////////////////////////////////////////////////////////////////////////////// 525 ///////////////////////////////////////// TERMINATION /////////////////////////////////////////// 526 class TerminationCriteria : public Algorithm { 527 public: 528 // update termination object by given @model and @inlier number. 529 // and return maximum number of predicted iteration 530 virtual int update(const Mat &model, int inlier_number) = 0; 531 // clone termination 532 virtual Ptr<TerminationCriteria> clone () const = 0; 533 }; 534 535 //////////////////////////////// STANDARD TERMINATION /////////////////////////////////////////// 536 class StandardTerminationCriteria : public TerminationCriteria { 537 public: 538 static Ptr<StandardTerminationCriteria> create(double confidence, int points_size_, 539 int sample_size_, int max_iterations_); 540 }; 541 542 ///////////////////////////////////// SPRT TERMINATION ////////////////////////////////////////// 543 class SPRTTermination : public TerminationCriteria { 544 public: 545 static Ptr<SPRTTermination> create(const std::vector<SPRT_history> &sprt_histories_, 546 double confidence, int points_size_, int sample_size_, int max_iterations_); 547 }; 548 549 ///////////////////////////// PROGRESSIVE-NAPSAC-SPRT TERMINATION ///////////////////////////////// 550 class SPRTPNapsacTermination : public TerminationCriteria { 551 public: 552 static Ptr<SPRTPNapsacTermination> create(const std::vector<SPRT_history>& 553 sprt_histories_, double confidence, int points_size_, int sample_size_, 554 int max_iterations_, double relax_coef_); 555 }; 556 557 ////////////////////////////////////// PROSAC TERMINATION ///////////////////////////////////////// 558 class ProsacTerminationCriteria : public TerminationCriteria { 559 public: 560 static Ptr<ProsacTerminationCriteria> create(const Ptr<ProsacSampler> &sampler_, 561 const Ptr<Error> &error_, int points_size_, int sample_size, double confidence, 562 int max_iters, int min_termination_length, double beta, double non_randomness_phi, 563 double inlier_thresh); 564 }; 565 566 ////////////////////////////////////////////////////////////////////////////////////////////////// 567 /////////////////////////////////////////// UTILS //////////////////////////////////////////////// 568 namespace Utils { 569 /* 570 * calibrate points: [x'; 1] = K^-1 [x; 1] 571 * @points is matrix N x 4. 572 * @norm_points is output matrix N x 4 with calibrated points. 573 */ 574 void calibratePoints (const Mat &K1, const Mat &K2, const Mat &points, Mat &norm_points); 575 void calibrateAndNormalizePointsPnP (const Mat &K, const Mat &pts, Mat &calib_norm_pts); 576 void normalizeAndDecalibPointsPnP (const Mat &K, Mat &pts, Mat &calib_norm_pts); 577 void decomposeProjection (const Mat &P, Mat &K_, Mat &R, Mat &t, bool same_focal=false); 578 double getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2); 579 float findMedian (std::vector<float> &array); 580 } 581 namespace Math { 582 // return skew symmetric matrix 583 Matx33d getSkewSymmetric(const Vec3d &v_); 584 // eliminate matrix with m rows and n columns to be upper triangular. 585 bool eliminateUpperTriangular (std::vector<double> &a, int m, int n); 586 Matx33d rotVec2RotMat (const Vec3d &v); 587 Vec3d rotMat2RotVec (const Matx33d &R); 588 } 589 590 ///////////////////////////////////////// RANDOM GENERATOR ///////////////////////////////////// 591 class RandomGenerator : public Algorithm { 592 public: 593 virtual ~RandomGenerator() override = default; 594 // interval is <0, max_range); 595 virtual void resetGenerator (int max_range) = 0; 596 // return sample filled with random numbers 597 virtual void generateUniqueRandomSet (std::vector<int> &sample) = 0; 598 // fill @sample of size @subset_size with random numbers in range <0, @max_range) 599 virtual void generateUniqueRandomSet (std::vector<int> &sample, int subset_size, 600 int max_range) = 0; 601 // fill @sample of size @sample.size() with random numbers in range <0, @max_range) 602 virtual void generateUniqueRandomSet (std::vector<int> &sample, int max_range) = 0; 603 // return subset=sample size 604 virtual void setSubsetSize (int subset_sz) = 0; 605 virtual int getSubsetSize () const = 0; 606 // return random number from <0, max_range), where max_range is from constructor 607 virtual int getRandomNumber () = 0; 608 // return random number from <0, max_rng) 609 virtual int getRandomNumber (int max_rng) = 0; 610 virtual const std::vector<int> &generateUniqueRandomSubset (std::vector<int> &array1, 611 int size1) = 0; 612 virtual Ptr<RandomGenerator> clone (int state) const = 0; 613 }; 614 615 class UniformRandomGenerator : public RandomGenerator { 616 public: 617 static Ptr<UniformRandomGenerator> create (int state); 618 static Ptr<UniformRandomGenerator> create (int state, int max_range, int subset_size_); 619 }; 620 621 /////////////////////////////////////////////////////////////////////////////////////////////////// 622 ////////////////////////////////////// LOCAL OPTIMIZATION ///////////////////////////////////////// 623 class LocalOptimization : public Algorithm { 624 public: 625 virtual ~LocalOptimization() override = default; 626 /* 627 * Refine so-far-the-best RANSAC model in local optimization step. 628 * @best_model: so-far-the-best model 629 * @new_model: output refined new model. 630 * @new_model_score: score of @new_model. 631 * Returns bool if model was refined successfully, false - otherwise 632 */ 633 virtual bool refineModel (const Mat &best_model, const Score &best_model_score, 634 Mat &new_model, Score &new_model_score) = 0; 635 virtual Ptr<LocalOptimization> clone(int state) const = 0; 636 }; 637 638 //////////////////////////////////// GRAPH CUT LO //////////////////////////////////////// 639 class GraphCut : public LocalOptimization { 640 public: 641 static Ptr<GraphCut> 642 create(const Ptr<Estimator> &estimator_, const Ptr<Error> &error_, 643 const Ptr<Quality> &quality_, const Ptr<NeighborhoodGraph> &neighborhood_graph_, 644 const Ptr<RandomGenerator> &lo_sampler_, double threshold_, 645 double spatial_coherence_term, int gc_iters); 646 }; 647 648 //////////////////////////////////// INNER + ITERATIVE LO /////////////////////////////////////// 649 class InnerIterativeLocalOptimization : public LocalOptimization { 650 public: 651 static Ptr<InnerIterativeLocalOptimization> 652 create(const Ptr<Estimator> &estimator_, const Ptr<Quality> &quality_, 653 const Ptr<RandomGenerator> &lo_sampler_, int pts_size, double threshold_, 654 bool is_iterative_, int lo_iter_sample_size_, int lo_inner_iterations, 655 int lo_iter_max_iterations, double threshold_multiplier); 656 }; 657 658 class SigmaConsensus : public LocalOptimization { 659 public: 660 static Ptr<SigmaConsensus> 661 create(const Ptr<Estimator> &estimator_, const Ptr<Error> &error_, 662 const Ptr<Quality> &quality, const Ptr<ModelVerifier> &verifier_, 663 int max_lo_sample_size, int number_of_irwls_iters_, 664 int DoF, double sigma_quantile, double upper_incomplete_of_sigma_quantile, 665 double C_, double maximum_thr); 666 }; 667 668 /////////////////////////////////////////////////////////////////////////////////////////////////// 669 /////////////////////////////////////// FINAL MODEL POLISHER ////////////////////////////////////// 670 class FinalModelPolisher : public Algorithm { 671 public: 672 virtual ~FinalModelPolisher() override = default; 673 /* 674 * Polish so-far-the-best RANSAC model in the end of RANSAC. 675 * @model: input final RANSAC model. 676 * @new_model: output polished model. 677 * @new_score: score of output model. 678 * Return true if polishing was successful, false - otherwise. 679 */ 680 virtual bool polishSoFarTheBestModel (const Mat &model, const Score &best_model_score, 681 Mat &new_model, Score &new_model_score) = 0; 682 }; 683 684 ///////////////////////////////////// LEAST SQUARES POLISHER ////////////////////////////////////// 685 class LeastSquaresPolishing : public FinalModelPolisher { 686 public: 687 static Ptr<LeastSquaresPolishing> create (const Ptr<Estimator> &estimator_, 688 const Ptr<Quality> &quality_, int lsq_iterations); 689 }; 690 691 /////////////////////////////////// RANSAC OUTPUT /////////////////////////////////// 692 class RansacOutput : public Algorithm { 693 public: 694 virtual ~RansacOutput() override = default; 695 static Ptr<RansacOutput> create(const Mat &model_, 696 const std::vector<bool> &inliers_mask_, 697 int time_mcs_, double score_, int number_inliers_, int number_iterations_, 698 int number_estimated_models_, int number_good_models_); 699 700 // Return inliers' indices. size of vector = number of inliers 701 virtual const std::vector<int > &getInliers() = 0; 702 // Return inliers mask. Vector of points size. 1-inlier, 0-outlier. 703 virtual const std::vector<bool> &getInliersMask() const = 0; 704 virtual int getTimeMicroSeconds() const = 0; 705 virtual int getTimeMicroSeconds1() const = 0; 706 virtual int getTimeMilliSeconds2() const = 0; 707 virtual int getTimeSeconds3() const = 0; 708 virtual int getNumberOfInliers() const = 0; 709 virtual int getNumberOfMainIterations() const = 0; 710 virtual int getNumberOfGoodModels () const = 0; 711 virtual int getNumberOfEstimatedModels () const = 0; 712 virtual const Mat &getModel() const = 0; 713 }; 714 715 ////////////////////////////////////////////// MODEL ///////////////////////////////////////////// 716 717 class Model : public Algorithm { 718 public: 719 virtual bool isFundamental () const = 0; 720 virtual bool isHomography () const = 0; 721 virtual bool isEssential () const = 0; 722 virtual bool isPnP () const = 0; 723 724 // getters 725 virtual int getSampleSize () const = 0; 726 virtual bool isParallel() const = 0; 727 virtual int getMaxNumHypothesisToTestBeforeRejection() const = 0; 728 virtual PolishingMethod getFinalPolisher () const = 0; 729 virtual LocalOptimMethod getLO () const = 0; 730 virtual ErrorMetric getError () const = 0; 731 virtual EstimationMethod getEstimator () const = 0; 732 virtual ScoreMethod getScore () const = 0; 733 virtual int getMaxIters () const = 0; 734 virtual double getConfidence () const = 0; 735 virtual double getThreshold () const = 0; 736 virtual VerificationMethod getVerifier () const = 0; 737 virtual SamplingMethod getSampler () const = 0; 738 virtual double getTimeForModelEstimation () const = 0; 739 virtual double getSPRTdelta () const = 0; 740 virtual double getSPRTepsilon () const = 0; 741 virtual double getSPRTavgNumModels () const = 0; 742 virtual NeighborSearchMethod getNeighborsSearch () const = 0; 743 virtual int getKNN () const = 0; 744 virtual int getCellSize () const = 0; 745 virtual int getGraphRadius() const = 0; 746 virtual double getRelaxCoef () const = 0; 747 748 virtual int getFinalLSQIterations () const = 0; 749 virtual int getDegreesOfFreedom () const = 0; 750 virtual double getSigmaQuantile () const = 0; 751 virtual double getUpperIncompleteOfSigmaQuantile () const = 0; 752 virtual double getLowerIncompleteOfSigmaQuantile () const = 0; 753 virtual double getC () const = 0; 754 virtual double getMaximumThreshold () const = 0; 755 virtual double getGraphCutSpatialCoherenceTerm () const = 0; 756 virtual int getLOSampleSize () const = 0; 757 virtual int getLOThresholdMultiplier() const = 0; 758 virtual int getLOIterativeSampleSize() const = 0; 759 virtual int getLOIterativeMaxIters() const = 0; 760 virtual int getLOInnerMaxIters() const = 0; 761 virtual const std::vector<int> &getGridCellNumber () const = 0; 762 virtual int getRandomGeneratorState () const = 0; 763 virtual int getMaxItersBeforeLO () const = 0; 764 765 // setters 766 virtual void setLocalOptimization (LocalOptimMethod lo_) = 0; 767 virtual void setKNearestNeighhbors (int knn_) = 0; 768 virtual void setNeighborsType (NeighborSearchMethod neighbors) = 0; 769 virtual void setCellSize (int cell_size_) = 0; 770 virtual void setParallel (bool is_parallel) = 0; 771 virtual void setVerifier (VerificationMethod verifier_) = 0; 772 virtual void setPolisher (PolishingMethod polisher_) = 0; 773 virtual void setError (ErrorMetric error_) = 0; 774 virtual void setLOIterations (int iters) = 0; 775 virtual void setLOIterativeIters (int iters) = 0; 776 virtual void setLOSampleSize (int lo_sample_size) = 0; 777 virtual void setThresholdMultiplierLO (double thr_mult) = 0; 778 virtual void setRandomGeneratorState (int state) = 0; 779 780 virtual void maskRequired (bool required) = 0; 781 virtual bool isMaskRequired () const = 0; 782 static Ptr<Model> create(double threshold_, EstimationMethod estimator_, SamplingMethod sampler_, 783 double confidence_=0.95, int max_iterations_=5000, ScoreMethod score_ =ScoreMethod::SCORE_METHOD_MSAC); 784 }; 785 786 Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method, 787 double ransacReprojThreshold, OutputArray mask, 788 const int maxIters, const double confidence); 789 790 Mat findFundamentalMat( InputArray points1, InputArray points2, 791 int method, double ransacReprojThreshold, double confidence, 792 int maxIters, OutputArray mask=noArray()); 793 794 bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints, 795 InputArray cameraMatrix, InputArray distCoeffs, 796 OutputArray rvec, OutputArray tvec, 797 bool useExtrinsicGuess, int iterationsCount, 798 float reprojectionError, double confidence, 799 OutputArray inliers, int flags); 800 801 Mat findEssentialMat( InputArray points1, InputArray points2, 802 InputArray cameraMatrix1, 803 int method, double prob, 804 double threshold, OutputArray mask); 805 806 Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers, 807 int method, double ransacReprojThreshold, int maxIters, 808 double confidence, int refineIters); 809 810 void saveMask (OutputArray mask, const std::vector<bool> &inliers_mask); 811 void setParameters (Ptr<Model> ¶ms, EstimationMethod estimator, const UsacParams &usac_params, 812 bool mask_need); 813 bool run (const Ptr<const Model> ¶ms, InputArray points1, InputArray points2, int state, 814 Ptr<RansacOutput> &ransac_output, InputArray K1_, InputArray K2_, 815 InputArray dist_coeff1, InputArray dist_coeff2); 816 }} 817 818 #endif //OPENCV_USAC_USAC_HPP 819