opencv/modules/3d/src/usac.hpp

980 lines
42 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html.
#ifndef OPENCV_USAC_USAC_HPP
#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};
class UsacConfig : public Algorithm {
public:
virtual int getMaxIterations () const = 0;
virtual int getMaxIterationsBeforeLO () const = 0;
virtual int getMaxNumHypothesisToTestBeforeRejection() const = 0;
virtual int getRandomGeneratorState () const = 0;
virtual bool isParallel() const = 0;
virtual NeighborSearchMethod getNeighborsSearchMethod () const = 0;
virtual SamplingMethod getSamplingMethod () const = 0;
virtual ScoreMethod getScoreMethod () const = 0;
virtual LocalOptimMethod getLOMethod () const = 0;
virtual bool isMaskRequired () const = 0;
};
class SimpleUsacConfig : public UsacConfig {
public:
virtual void setMaxIterations(int max_iterations_) = 0;
virtual void setMaxIterationsBeforeLo(int max_iterations_before_lo_) = 0;
virtual void setMaxNumHypothesisToTestBeforeRejection(int max_num_hypothesis_to_test_before_rejection_) = 0;
virtual void setRandomGeneratorState(int random_generator_state_) = 0;
virtual void setParallel(bool is_parallel) = 0;
virtual void setNeighborsSearchMethod(NeighborSearchMethod neighbors_search_method_) = 0;
virtual void setSamplingMethod(SamplingMethod sampling_method_) = 0;
virtual void setScoreMethod(ScoreMethod score_method_) = 0;
virtual void setLoMethod(LocalOptimMethod lo_method_) = 0;
virtual void maskRequired(bool need_mask_) = 0;
static Ptr<SimpleUsacConfig> create();
};
// Abstract Error class
class Error : public Algorithm {
public:
// set model to use getError() function
virtual void setModelParameters (const Mat &model) = 0;
// returns error of point wih @point_idx w.r.t. model
virtual float getError (int point_idx) const = 0;
virtual const std::vector<float> &getErrors (const Mat &model) = 0;
virtual Ptr<Error> clone () const = 0;
};
// Symmetric Reprojection Error for Homography
class ReprojectionErrorSymmetric : public Error {
public:
static Ptr<ReprojectionErrorSymmetric> create(const Mat &points);
};
// Forward Reprojection Error for Homography
class ReprojectionErrorForward : public Error {
public:
static Ptr<ReprojectionErrorForward> create(const Mat &points);
};
// Sampson Error for Fundamental matrix
class SampsonError : public Error {
public:
static Ptr<SampsonError> create(const Mat &points);
};
// Symmetric Geometric Distance (to epipolar lines) for Fundamental and Essential matrix
class SymmetricGeometricDistance : public Error {
public:
static Ptr<SymmetricGeometricDistance> create(const Mat &points);
};
// Reprojection Error for Projection matrix
class ReprojectionErrorPmatrix : public Error {
public:
static Ptr<ReprojectionErrorPmatrix> create(const Mat &points);
};
// Reprojection Error for Affine matrix
class ReprojectionErrorAffine : public Error {
public:
static Ptr<ReprojectionErrorAffine> create(const Mat &points);
};
// Error for plane model
class PlaneModelError : public Error {
public:
static Ptr<PlaneModelError> create(const Mat &points);
};
// Error for sphere model
class SphereModelError : public Error {
public:
static Ptr<SphereModelError> create(const Mat &points);
};
// Normalizing transformation of data points
class NormTransform : public Algorithm {
public:
/*
* @norm_points is output matrix of size pts_size x 4
* @sample constains indices of points
* @sample_number is number of used points in sample <0; sample_number)
* @T1, T2 are output transformation matrices
*/
virtual void getNormTransformation (Mat &norm_points, const std::vector<int> &sample,
int sample_number, Matx33d &T1, Matx33d &T2) const = 0;
static Ptr<NormTransform> create (const Mat &points);
};
/////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////// SOLVER ///////////////////////////////////////////
class MinimalSolver : public Algorithm {
public:
// Estimate models from minimal sample. models.size() == number of found solutions
virtual int estimate (const std::vector<int> &sample, std::vector<Mat> &models) const = 0;
// return minimal sample size required for estimation.
virtual int getSampleSize() const = 0;
// return maximum number of possible solutions.
virtual int getMaxNumberOfSolutions () const = 0;
virtual Ptr<MinimalSolver> clone () const = 0;
};
//-------------------------- HOMOGRAPHY MATRIX -----------------------
class HomographyMinimalSolver4ptsGEM : public MinimalSolver {
public:
static Ptr<HomographyMinimalSolver4ptsGEM> create(const Mat &points_);
};
//-------------------------- FUNDAMENTAL MATRIX -----------------------
class FundamentalMinimalSolver7pts : public MinimalSolver {
public:
static Ptr<FundamentalMinimalSolver7pts> create(const Mat &points_);
};
class FundamentalMinimalSolver8pts : public MinimalSolver {
public:
static Ptr<FundamentalMinimalSolver8pts> create(const Mat &points_);
};
//-------------------------- ESSENTIAL MATRIX -----------------------
class EssentialMinimalSolverStewenius5pts : public MinimalSolver {
public:
static Ptr<EssentialMinimalSolverStewenius5pts> create(const Mat &points_);
};
//-------------------------- PNP -----------------------
class PnPMinimalSolver6Pts : public MinimalSolver {
public:
static Ptr<PnPMinimalSolver6Pts> create(const Mat &points_);
};
class P3PSolver : public MinimalSolver {
public:
static Ptr<P3PSolver> create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K);
};
//-------------------------- AFFINE -----------------------
class AffineMinimalSolver : public MinimalSolver {
public:
static Ptr<AffineMinimalSolver> create(const Mat &points_);
};
//-------------------------- 3D PLANE -----------------------
class PlaneModelMinimalSolver : public MinimalSolver {
public:
static Ptr<PlaneModelMinimalSolver> create(const Mat &points_);
};
//-------------------------- 3D SPHERE -----------------------
class SphereModelMinimalSolver : public MinimalSolver {
public:
static Ptr<SphereModelMinimalSolver> create(const Mat &points_);
};
//////////////////////////////////////// NON MINIMAL SOLVER ///////////////////////////////////////
class NonMinimalSolver : public Algorithm {
public:
// Estimate models from non minimal sample. models.size() == number of found solutions
virtual int estimate (const std::vector<int> &sample, int sample_size,
std::vector<Mat> &models, const std::vector<double> &weights) const = 0;
// return minimal sample size required for non-minimal estimation.
virtual int getMinimumRequiredSampleSize() const = 0;
// return maximum number of possible solutions.
virtual int getMaxNumberOfSolutions () const = 0;
virtual Ptr<NonMinimalSolver> clone () const = 0;
};
//-------------------------- HOMOGRAPHY MATRIX -----------------------
class HomographyNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<HomographyNonMinimalSolver> create(const Mat &points_);
};
//-------------------------- FUNDAMENTAL MATRIX -----------------------
class FundamentalNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<FundamentalNonMinimalSolver> create(const Mat &points_);
};
//-------------------------- ESSENTIAL MATRIX -----------------------
class EssentialNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<EssentialNonMinimalSolver> create(const Mat &points_);
};
//-------------------------- PNP -----------------------
class PnPNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<PnPNonMinimalSolver> create(const Mat &points);
};
class DLSPnP : public NonMinimalSolver {
public:
static Ptr<DLSPnP> create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K);
};
//-------------------------- AFFINE -----------------------
class AffineNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<AffineNonMinimalSolver> create(const Mat &points_);
};
//-------------------------- 3D PLANE -----------------------
class PlaneModelNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<PlaneModelNonMinimalSolver> create(const Mat &points_);
};
//-------------------------- 3D SPHERE -----------------------
class SphereModelNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<SphereModelNonMinimalSolver> create(const Mat &points_);
};
//////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////// SCORE ///////////////////////////////////////////
class Score {
public:
int inlier_number;
double score;
Score () { // set worst case
inlier_number = 0;
score = std::numeric_limits<double>::max();
}
Score (int inlier_number_, double score_) { // copy constructor
inlier_number = inlier_number_;
score = score_;
}
// Compare two scores. Objective is minimization of score. Lower score is better.
inline bool isBetter (const Score &score2) const {
return score < score2.score;
}
};
class GammaValues
{
const double max_range_complete /*= 4.62*/, max_range_gamma /*= 1.52*/;
const int max_size_table /* = 3000 */;
std::vector<double> gamma_complete, gamma_incomplete, gamma;
GammaValues(); // use getSingleton()
public:
static const GammaValues& getSingleton();
const std::vector<double>& getCompleteGammaValues() const;
const std::vector<double>& getIncompleteGammaValues() const;
const std::vector<double>& getGammaValues() const;
double getScaleOfGammaCompleteValues () const;
double getScaleOfGammaValues () const;
int getTableSize () const;
};
////////////////////////////////////////// QUALITY ///////////////////////////////////////////
class Quality : public Algorithm {
public:
virtual ~Quality() override = default;
/*
* Calculates number of inliers and score of the @model.
* return Score with calculated inlier_number and score.
* @model: Mat current model, e.g., H matrix.
*/
virtual Score getScore (const Mat &model) const = 0;
virtual Score getScore (const std::vector<float> &/*errors*/) const {
CV_Error(cv::Error::StsNotImplemented, "getScore(errors)");
}
// 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<int> &inliers) const = 0;
// get @inliers of the @model for given threshold
virtual int getInliers (const Mat &model, std::vector<int> &inliers, double thr) const = 0;
// Set the best score, so evaluation of the model can terminate earlier
virtual void setBestScore (double best_score_) = 0;
// set @inliers_mask: true if point i is inlier, false - otherwise.
virtual int getInliers (const Mat &model, std::vector<bool> &inliers_mask) const = 0;
virtual int getPointsSize() const = 0;
virtual Ptr<Quality> clone () const = 0;
static int getInliers (const Ptr<Error> &error, const Mat &model,
std::vector<bool> &inliers_mask, double threshold);
static int getInliers (const Ptr<Error> &error, const Mat &model,
std::vector<int> &inliers, double threshold);
};
// RANSAC (binary) quality
class RansacQuality : public Quality {
public:
static Ptr<RansacQuality> create(int points_size_, double threshold_,const Ptr<Error> &error_);
};
// M-estimator quality - truncated Squared error
class MsacQuality : public Quality {
public:
static Ptr<MsacQuality> create(int points_size_, double threshold_, const Ptr<Error> &error_);
};
// Marginlizing Sample Consensus quality, D. Barath et al.
class MagsacQuality : public Quality {
public:
static Ptr<MagsacQuality> create(double maximum_thr, int points_size_,const Ptr<Error> &error_,
double tentative_inlier_threshold_, int DoF, double sigma_quantile,
double upper_incomplete_of_sigma_quantile,
double lower_incomplete_of_sigma_quantile, double C_);
};
// Least Median of Squares Quality
class LMedsQuality : public Quality {
public:
static Ptr<LMedsQuality> create(int points_size_, double threshold_, const Ptr<Error> &error_);
};
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////// DEGENERACY //////////////////////////////////
class Degeneracy : public Algorithm {
public:
virtual ~Degeneracy() override = default;
/*
* Check if sample causes degenerate configurations.
* For example, test if points are collinear.
*/
virtual bool isSampleGood (const std::vector<int> &/*sample*/) const {
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<int> &/*sample*/) const {
return true;
}
/*
* Fix degenerate model.
* Return true if model is degenerate, false - otherwise
*/
virtual bool recoverIfDegenerate (const std::vector<int> &/*sample*/,const Mat &/*best_model*/,
Mat &/*non_degenerate_model*/, Score &/*non_degenerate_model_score*/) {
return false;
}
virtual Ptr<Degeneracy> clone(int /*state*/) const { return makePtr<Degeneracy>(); }
};
class EpipolarGeometryDegeneracy : public Degeneracy {
public:
static void recoverRank (Mat &model, bool is_fundamental_mat);
static Ptr<EpipolarGeometryDegeneracy> create (const Mat &points_, int sample_size_);
};
class EssentialDegeneracy : public EpipolarGeometryDegeneracy {
public:
static Ptr<EssentialDegeneracy>create (const Mat &points, int sample_size);
};
class HomographyDegeneracy : public Degeneracy {
public:
static Ptr<HomographyDegeneracy> create(const Mat &points_);
};
class FundamentalDegeneracy : public EpipolarGeometryDegeneracy {
public:
// threshold for homography is squared so is around 2.236 pixels
static Ptr<FundamentalDegeneracy> create (int state, const Ptr<Quality> &quality_,
const Mat &points_, int sample_size_, double homography_threshold);
};
/////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////// ESTIMATOR //////////////////////////////////
class Estimator : public Algorithm{
public:
/*
* Estimate models with minimal solver.
* Return number of valid solutions after estimation.
* Return models accordingly to number of solutions.
* Note, vector of models must allocated before.
* Note, not all degenerate tests are included in estimation.
*/
virtual int
estimateModels (const std::vector<int> &sample, std::vector<Mat> &models) const = 0;
/*
* Estimate model with non-minimal solver.
* Return number of valid solutions after estimation.
* Note, not all degenerate tests are included in estimation.
*/
virtual int
estimateModelNonMinimalSample (const std::vector<int> &sample, int sample_size,
std::vector<Mat> &models, const std::vector<double> &weights) const = 0;
// return minimal sample size required for minimal estimation.
virtual int getMinimalSampleSize () const = 0;
// return minimal sample size required for non-minimal estimation.
virtual int getNonMinimalSampleSize () const = 0;
// return maximum number of possible solutions of minimal estimation.
virtual int getMaxNumSolutions () const = 0;
// return maximum number of possible solutions of non-minimal estimation.
virtual int getMaxNumSolutionsNonMinimal () const = 0;
virtual Ptr<Estimator> clone() const = 0;
};
class HomographyEstimator : public Estimator {
public:
static Ptr<HomographyEstimator> create (const Ptr<MinimalSolver> &min_solver_,
const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> &degeneracy_);
};
class FundamentalEstimator : public Estimator {
public:
static Ptr<FundamentalEstimator> create (const Ptr<MinimalSolver> &min_solver_,
const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> &degeneracy_);
};
class EssentialEstimator : public Estimator {
public:
static Ptr<EssentialEstimator> create (const Ptr<MinimalSolver> &min_solver_,
const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> &degeneracy_);
};
class AffineEstimator : public Estimator {
public:
static Ptr<AffineEstimator> create (const Ptr<MinimalSolver> &min_solver_,
const Ptr<NonMinimalSolver> &non_min_solver_);
};
class PnPEstimator : public Estimator {
public:
static Ptr<PnPEstimator> create (const Ptr<MinimalSolver> &min_solver_,
const Ptr<NonMinimalSolver> &non_min_solver_);
};
class PointCloudModelEstimator : public Estimator {
public:
//! Custom function that take the model coefficients and return whether the model is acceptable or not.
//! Same as cv::SACSegmentation::ModelConstraintFunction in ptcloud.hpp.
using ModelConstraintFunction = std::function<bool(const std::vector<double> &/*model_coefficients*/)>;
/** @brief Methods for creating PointCloudModelEstimator.
*
* @param min_solver_ Minimum solver for estimating the model with minimum samples.
* @param non_min_solver_ Non-minimum solver for estimating the model with non-minimum samples.
* @param custom_model_constraints_ Custom model constraints for filtering the estimated obtained model.
* @return Ptr\<PointCloudModelEstimator\>
*/
static Ptr<PointCloudModelEstimator> create (const Ptr<MinimalSolver> &min_solver_,
const Ptr<NonMinimalSolver> &non_min_solver_,
const ModelConstraintFunction &custom_model_constraints_ = nullptr);
};
//////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////// MODEL VERIFIER ////////////////////////////////////
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;
// update verifier by given inlier number
virtual void update (int highest_inlier_number) = 0;
virtual const std::vector<float> &getErrors() const = 0;
virtual bool hasErrors () const = 0;
virtual Ptr<ModelVerifier> clone (int state) const = 0;
static Ptr<ModelVerifier> create();
};
struct SPRT_history {
/*
* delta:
* The probability of a data point being consistent
* with a 'bad' model is modeled as a probability of
* a random event with Bernoulli distribution with parameter
* delta : p(1|Hb) = delta.
* epsilon:
* The probability p(1|Hg) = epsilon
* that any randomly chosen data point is consistent with a 'good' model
* is approximated by the fraction of inliers epsilon among the data
* points
* A is the decision threshold, the only parameter of the Adapted SPRT
*/
double epsilon, delta, A;
// number of samples processed by test
int tested_samples; // k
SPRT_history () : epsilon(0), delta(0), A(0) {
tested_samples = 0;
}
};
///////////////////////////////// SPRT VERIFIER /////////////////////////////////////////
/*
* 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 {
public:
// return constant reference of vector of SPRT histories for SPRT termination.
virtual const std::vector<SPRT_history> &getSPRTvector () const = 0;
static Ptr<SPRT> create (int state, const Ptr<Error> &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_);
};
//////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////// SAMPLER ///////////////////////////////////////
class Sampler : public Algorithm {
public:
virtual ~Sampler() override = default;
// set new points size
virtual void setNewPointsSize (int points_size) = 0;
// generate sample. Fill @sample with indices of points.
virtual void generateSample (std::vector<int> &sample) = 0;
virtual Ptr<Sampler> clone (int state) const = 0;
};
////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////// NEIGHBORHOOD GRAPH /////////////////////////////////////////
class NeighborhoodGraph : public Algorithm {
public:
virtual ~NeighborhoodGraph() override = default;
// Return neighbors of the point with index @point_idx_ in the graph.
virtual const std::vector<int> &getNeighbors(int point_idx_) const = 0;
};
class RadiusSearchNeighborhoodGraph : public NeighborhoodGraph {
public:
static Ptr<RadiusSearchNeighborhoodGraph> create (const Mat &points, int points_size,
double radius_, int flann_search_params, int num_kd_trees);
};
class FlannNeighborhoodGraph : public NeighborhoodGraph {
public:
static Ptr<FlannNeighborhoodGraph> create(const Mat &points, int points_size,
int k_nearest_neighbors_, bool get_distances, int flann_search_params, int num_kd_trees);
virtual const std::vector<double> &getNeighborsDistances (int idx) const = 0;
};
class GridNeighborhoodGraph : public NeighborhoodGraph {
public:
static Ptr<GridNeighborhoodGraph> create(const Mat &points, 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);
};
////////////////////////////////////// UNIFORM SAMPLER ////////////////////////////////////////////
class UniformSampler : public Sampler {
public:
static Ptr<UniformSampler> create(int state, int sample_size_, int points_size_);
};
/////////////////////////////////// PROSAC (SIMPLE) SAMPLER ///////////////////////////////////////
class ProsacSimpleSampler : public Sampler {
public:
static Ptr<ProsacSimpleSampler> create(int state, int points_size_, int sample_size_,
int max_prosac_samples_count);
};
////////////////////////////////////// PROSAC SAMPLER ////////////////////////////////////////////
class ProsacSampler : public Sampler {
public:
static Ptr<ProsacSampler> create(int state, int points_size_, int sample_size_,
int growth_max_samples);
// return number of samples generated (for prosac termination).
virtual int getKthSample () const = 0;
// return constant reference of growth function of prosac sampler (for prosac termination)
virtual const std::vector<int> &getGrowthFunction () const = 0;
virtual void setTerminationLength (int termination_length) = 0;
};
////////////////////////// NAPSAC (N adjacent points sample consensus) SAMPLER ////////////////////
class NapsacSampler : public Sampler {
public:
static Ptr<NapsacSampler> create(int state, int points_size_, int sample_size_,
const Ptr<NeighborhoodGraph> &neighborhood_graph_);
};
////////////////////////////////////// P-NAPSAC SAMPLER /////////////////////////////////////////
class ProgressiveNapsac : public Sampler {
public:
static Ptr<ProgressiveNapsac> create(int state, int points_size_, int sample_size_,
const std::vector<Ptr<NeighborhoodGraph>> &layers, int sampler_length);
};
/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////// TERMINATION ///////////////////////////////////////////
class TerminationCriteria : 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<TerminationCriteria> clone () const = 0;
};
//////////////////////////////// STANDARD TERMINATION ///////////////////////////////////////////
class StandardTerminationCriteria : public TerminationCriteria {
public:
static Ptr<StandardTerminationCriteria> create(double confidence, int points_size_,
int sample_size_, int max_iterations_);
};
///////////////////////////////////// SPRT TERMINATION //////////////////////////////////////////
class SPRTTermination : public TerminationCriteria {
public:
static Ptr<SPRTTermination> create(const std::vector<SPRT_history> &sprt_histories_,
double confidence, int points_size_, int sample_size_, int max_iterations_);
};
///////////////////////////// PROGRESSIVE-NAPSAC-SPRT TERMINATION /////////////////////////////////
class SPRTPNapsacTermination : public TerminationCriteria {
public:
static Ptr<SPRTPNapsacTermination> create(const std::vector<SPRT_history>&
sprt_histories_, double confidence, int points_size_, int sample_size_,
int max_iterations_, double relax_coef_);
};
////////////////////////////////////// PROSAC TERMINATION /////////////////////////////////////////
class ProsacTerminationCriteria : public TerminationCriteria {
public:
static Ptr<ProsacTerminationCriteria> create(const Ptr<ProsacSampler> &sampler_,
const Ptr<Error> &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);
};
//////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////// UTILS ////////////////////////////////////////////////
namespace Utils {
/*
* 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);
float findMedian (std::vector<float> &array);
}
namespace Math {
// return skew symmetric matrix
Matx33d getSkewSymmetric(const Vec3d &v_);
// eliminate matrix with m rows and n columns to be upper triangular.
bool eliminateUpperTriangular (std::vector<double> &a, int m, int n);
Matx33d rotVec2RotMat (const Vec3d &v);
Vec3d rotMat2RotVec (const Matx33d &R);
}
///////////////////////////////////////// RANDOM GENERATOR /////////////////////////////////////
class RandomGenerator : public Algorithm {
public:
virtual ~RandomGenerator() override = default;
// interval is <0, max_range);
virtual void resetGenerator (int max_range) = 0;
// return sample filled with random numbers
virtual void generateUniqueRandomSet (std::vector<int> &sample) = 0;
// fill @sample of size @subset_size with random numbers in range <0, @max_range)
virtual void generateUniqueRandomSet (std::vector<int> &sample, int subset_size,
int max_range) = 0;
// fill @sample of size @sample.size() with random numbers in range <0, @max_range)
virtual void generateUniqueRandomSet (std::vector<int> &sample, int max_range) = 0;
// return subset=sample size
virtual void setSubsetSize (int subset_sz) = 0;
virtual int getSubsetSize () const = 0;
// return random number from <0, max_range), where max_range is from constructor
virtual int getRandomNumber () = 0;
// return random number from <0, max_rng)
virtual int getRandomNumber (int max_rng) = 0;
virtual const std::vector<int> &generateUniqueRandomSubset (std::vector<int> &array1,
int size1) = 0;
virtual Ptr<RandomGenerator> clone (int state) const = 0;
};
class UniformRandomGenerator : public RandomGenerator {
public:
static Ptr<UniformRandomGenerator> create (int state);
static Ptr<UniformRandomGenerator> create (int state, int max_range, int subset_size_);
};
///////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////// LOCAL OPTIMIZATION /////////////////////////////////////////
class LocalOptimization : public Algorithm {
public:
virtual ~LocalOptimization() override = default;
/*
* Refine so-far-the-best RANSAC model in local optimization step.
* @best_model: so-far-the-best model
* @new_model: output refined new model.
* @new_model_score: score of @new_model.
* Returns bool if model was refined successfully, false - otherwise
*/
virtual bool refineModel (const Mat &best_model, const Score &best_model_score,
Mat &new_model, Score &new_model_score) = 0;
virtual Ptr<LocalOptimization> clone(int state) const = 0;
};
//////////////////////////////////// GRAPH CUT LO ////////////////////////////////////////
class GraphCut : public LocalOptimization {
public:
static Ptr<GraphCut>
create(const Ptr<Estimator> &estimator_, const Ptr<Error> &error_,
const Ptr<Quality> &quality_, const Ptr<NeighborhoodGraph> &neighborhood_graph_,
const Ptr<RandomGenerator> &lo_sampler_, double threshold_,
double spatial_coherence_term, int gc_iters);
};
//////////////////////////////////// INNER + ITERATIVE LO ///////////////////////////////////////
class InnerIterativeLocalOptimization : public LocalOptimization {
public:
static Ptr<InnerIterativeLocalOptimization>
create(const Ptr<Estimator> &estimator_, const Ptr<Quality> &quality_,
const Ptr<RandomGenerator> &lo_sampler_, int pts_size, double threshold_,
bool is_iterative_, int lo_iter_sample_size_, int lo_inner_iterations,
int lo_iter_max_iterations, double threshold_multiplier);
};
class SigmaConsensus : public LocalOptimization {
public:
static Ptr<SigmaConsensus>
create(const Ptr<Estimator> &estimator_, const Ptr<Error> &error_,
const Ptr<Quality> &quality, const Ptr<ModelVerifier> &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);
};
///////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////// FINAL MODEL POLISHER //////////////////////////////////////
class FinalModelPolisher : public Algorithm {
public:
virtual ~FinalModelPolisher() override = default;
/*
* Polish so-far-the-best RANSAC model in the end of RANSAC.
* @model: input final RANSAC model.
* @new_model: output polished model.
* @new_score: score of output model.
* Return true if polishing was successful, false - otherwise.
*/
virtual bool polishSoFarTheBestModel (const Mat &model, const Score &best_model_score,
Mat &new_model, Score &new_model_score) = 0;
};
///////////////////////////////////// LEAST SQUARES POLISHER //////////////////////////////////////
class LeastSquaresPolishing : public FinalModelPolisher {
public:
static Ptr<LeastSquaresPolishing> create (const Ptr<Estimator> &estimator_,
const Ptr<Quality> &quality_, int lsq_iterations);
};
/////////////////////////////////// RANSAC OUTPUT ///////////////////////////////////
class RansacOutput : public Algorithm {
public:
virtual ~RansacOutput() override = default;
static Ptr<RansacOutput> create(const Mat &model_,
const std::vector<bool> &inliers_mask_,
int time_mcs_, double score_, int number_inliers_, int number_iterations_,
int number_estimated_models_, int number_good_models_);
// Return inliers' indices. size of vector = number of inliers
virtual const std::vector<int > &getInliers() = 0;
// Return inliers mask. Vector of points size. 1-inlier, 0-outlier.
virtual const std::vector<bool> &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;
};
////////////////////////////////////////////// MODEL /////////////////////////////////////////////
class Model : public Algorithm {
public:
virtual bool isFundamental () const = 0;
virtual bool isHomography () const = 0;
virtual bool isEssential () const = 0;
virtual bool isPnP () const = 0;
// 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;
virtual EstimationMethod getEstimator () const = 0;
virtual ScoreMethod getScore () const = 0;
virtual int getMaxIters () const = 0;
virtual double getConfidence () const = 0;
virtual double getThreshold () const = 0;
virtual VerificationMethod getVerifier () const = 0;
virtual SamplingMethod getSampler () const = 0;
virtual double getTimeForModelEstimation () const = 0;
virtual double getSPRTdelta () const = 0;
virtual double getSPRTepsilon () const = 0;
virtual double getSPRTavgNumModels () const = 0;
virtual NeighborSearchMethod getNeighborsSearch () const = 0;
virtual int getKNN () const = 0;
virtual int getCellSize () const = 0;
virtual int getGraphRadius() const = 0;
virtual double getRelaxCoef () const = 0;
virtual int getFinalLSQIterations () const = 0;
virtual int getDegreesOfFreedom () const = 0;
virtual double getSigmaQuantile () const = 0;
virtual double getUpperIncompleteOfSigmaQuantile () const = 0;
virtual double getLowerIncompleteOfSigmaQuantile () const = 0;
virtual double getC () const = 0;
virtual double getMaximumThreshold () const = 0;
virtual double getGraphCutSpatialCoherenceTerm () const = 0;
virtual int getLOSampleSize () const = 0;
virtual int getLOThresholdMultiplier() const = 0;
virtual int getLOIterativeSampleSize() const = 0;
virtual int getLOIterativeMaxIters() const = 0;
virtual int getLOInnerMaxIters() const = 0;
virtual const std::vector<int> &getGridCellNumber () const = 0;
virtual int getRandomGeneratorState () const = 0;
virtual int getMaxItersBeforeLO () const = 0;
// setters
virtual void setLocalOptimization (LocalOptimMethod lo_) = 0;
virtual void setKNearestNeighhbors (int knn_) = 0;
virtual void setNeighborsType (NeighborSearchMethod neighbors) = 0;
virtual void setCellSize (int cell_size_) = 0;
virtual void setParallel (bool is_parallel) = 0;
virtual void setVerifier (VerificationMethod verifier_) = 0;
virtual void setPolisher (PolishingMethod polisher_) = 0;
virtual void setError (ErrorMetric error_) = 0;
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 maskRequired (bool required) = 0;
virtual bool isMaskRequired () const = 0;
static Ptr<Model> create(double threshold_, EstimationMethod estimator_, SamplingMethod sampler_,
double confidence_=0.95, int max_iterations_=5000, ScoreMethod score_ =ScoreMethod::SCORE_METHOD_MSAC);
};
///////////////////////////////////////// UniversalRANSAC ////////////////////////////////////////
/** Implementation of the Universal RANSAC algorithm.
UniversalRANSAC represents an implementation of the Universal RANSAC
(Universal RANdom SAmple Consensus) algorithm, as described in:
"USAC: A Universal Framework for Random Sample Consensus", Raguram, R., et al.
IEEE Transactions on Pattern Analysis and Machine Intelligence,
vol. 35, no. 8, 2013, pp. 20222038.
USAC extends the simple hypothesize-and-verify structure of standard RANSAC
to incorporate a number of important practical and computational considerations.
The optimization of RANSAC algorithms such as NAPSAC, GroupSAC, and MAGSAC
can be considered as a special case of the USAC framework.
The algorithm works as following stages:
+ [Stage 0] Pre-filtering
- [**0. Pre-filtering**] Filtering of the input data, e.g. removing some noise points.
+ [Stage 1] Sample minimal subset
- [**1a. Sampling**] Sample minimal subset. It may be possible to incorporate prior information
and bias the sampling with a view toward preferentially generating models that are more
likely to be correct, or like the standard RANSAC, sampling uniformly at random.
- [**1b. Sample check**] Check whether the sample is suitable for computing model parameters.
Note that this simple test requires very little overhead, particularly when compared to
the expensive model generation and verification stages.
+ [Stage 2] Generate minimal-sample model(s)
- [**2a. Model generation**] Using the data points sampled in the previous step to fit the model
(calculate model parameters).
- [**2b. Model check**] A preliminary test that checks the model based on application-specific
constraints and then performs the verification only if required. For example, fitting
a sphere to a limited radius range.
+ [Stage 3] Is the model interesting?
- [**3a. Verification**] Verify that the current model is likely to obtain the maximum
objective function (in other words, better than the current best model), a score can be
used (e.g., the data point's voting support for this model), or conduct a statistical
test on a small number of data points, and discard or accept the model based on the results
of the test. For example, T(d, d) Test, Bail-Out Test, SPRT Test, Preemptive Verification.
- [**3b. Degeneracy**] Determine if sufficient constraints are provided to produce
a unique solution.
+ [Stage 4] Generate non-minimal sample model
- [**4. Model refinement**] Handle the issue of noisy models (by Local Optimization,
Error Propagation, etc).
+ [Stage 5] Confidence in solution achieved?
- [**5. Judgment termination**] Determine whether the specified maximum number of iterations
is reached or whether the desired model is obtained with a certain confidence level.
Stage 1b, 2b, 3a, 3b, 5 may jump back to Stage 1a.
*/
class UniversalRANSAC {
protected:
const Ptr<const UsacConfig> config;
const Ptr<const Estimator> _estimator;
const Ptr<Quality> _quality;
const Ptr<Sampler> _sampler;
const Ptr<TerminationCriteria> _termination_criteria;
const Ptr<ModelVerifier> _model_verifier;
const Ptr<Degeneracy> _degeneracy;
const Ptr<LocalOptimization> _local_optimization;
const Ptr<FinalModelPolisher> _model_polisher;
const int points_size;
public:
UniversalRANSAC (const Ptr<const UsacConfig> &config_, int points_size_, const Ptr<const Estimator> &estimator_, const Ptr<Quality> &quality_,
const Ptr<Sampler> &sampler_, const Ptr<TerminationCriteria> &termination_criteria_,
const Ptr<ModelVerifier> &model_verifier_, const Ptr<Degeneracy> &degeneracy_,
const Ptr<LocalOptimization> &local_optimization_, const Ptr<FinalModelPolisher> &model_polisher_);
bool run(Ptr<RansacOutput> &ransac_output);
};
Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method,
double ransacReprojThreshold, OutputArray mask,
const int maxIters, const double confidence);
Mat findFundamentalMat( InputArray points1, InputArray points2,
int method, double ransacReprojThreshold, double confidence,
int maxIters, OutputArray mask=noArray());
bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
InputArray cameraMatrix, InputArray distCoeffs,
OutputArray rvec, OutputArray tvec,
bool useExtrinsicGuess, int iterationsCount,
float reprojectionError, double confidence,
OutputArray inliers, int flags);
Mat findEssentialMat( InputArray points1, InputArray points2,
InputArray cameraMatrix1,
int method, double prob,
double threshold, OutputArray mask);
Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers,
int method, double ransacReprojThreshold, int maxIters,
double confidence, int refineIters);
void saveMask (OutputArray mask, const std::vector<bool> &inliers_mask);
void setParameters (Ptr<Model> &params, EstimationMethod estimator, const UsacParams &usac_params,
bool mask_need);
bool run (const Ptr<const Model> &params, InputArray points1, InputArray points2, int state,
Ptr<RansacOutput> &ransac_output, InputArray K1_, InputArray K2_,
InputArray dist_coeff1, InputArray dist_coeff2);
}}
#endif //OPENCV_USAC_USAC_HPP