mirror/opencv
1
0
Fork 0
mirror of https://github.com/opencv/opencv.git synced 2024-11-24 03:00:14 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Merge pull request #23078 from ivashmak:update_vsac

Browse Source 
Update USAC #23078

### Pull Request Readiness Checklist

- [x] I agree to contribute to the project under Apache 2 License.
- [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV
- [x] The PR is proposed to the proper branch
- [x] There is a reference to the original bug report and related work
- [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable
      Patch to opencv_extra has the same branch name.
- [x] The feature is well documented and sample code can be built with the project CMake
This commit is contained in:
Maksym Ivashechkin 2023-06-16 08:59:13 +01:00 committed by GitHub
parent 3c0b71bcec
commit 44881592c3
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
22 changed files with 4493 additions and 2314 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
modules/calib3d/include/opencv2/calib3d.hpp
View File

@ -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.

4
modules/calib3d/src/five-point.cpp
View File

@ -522,9 +522,9 @@ cv::Mat cv::findEssentialMat( InputArray points1, InputArray points2,
InputArray cameraMatrix1, InputArray cameraMatrix2,
InputArray dist_coeff1, InputArray dist_coeff2, OutputArray mask, const UsacParams &params) {
Ptr<usac::Model> model;
usac::setParameters(model, usac::EstimationMethod::Essential, params, mask.needed());
usac::setParameters(model, usac::EstimationMethod::ESSENTIAL, params, mask.needed());
Ptr<usac::RansacOutput> ransac_output;
if (usac::run(model, points1, points2, model->getRandomGeneratorState(),
if (usac::run(model, points1, points2,
ransac_output, cameraMatrix1, cameraMatrix2, dist_coeff1, dist_coeff2)) {
usac::saveMask(mask, ransac_output->getInliersMask());
return ransac_output->getModel();

8
modules/calib3d/src/fundam.cpp
View File

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

4
modules/calib3d/src/ptsetreg.cpp
View File

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

17
modules/calib3d/src/solvepnp.cpp
View File

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

401
modules/calib3d/src/usac.hpp
View File

@ -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<float> &getErrors (const Mat &model) = 0;
virtual Ptr<Error> clone () const = 0;
};
// Symmetric Reprojection Error for Homography
@ -58,6 +58,11 @@ public:
static Ptr<ReprojectionErrorAffine> create(const Mat &points);
};
class TrifocalTensorReprError : public Error {
public:
static Ptr<TrifocalTensorReprError> 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<MinimalSolver> clone () const = 0;
};
//-------------------------- HOMOGRAPHY MATRIX -----------------------
class HomographyMinimalSolver4ptsGEM : public MinimalSolver {
class HomographyMinimalSolver4pts : public MinimalSolver {
public:
static Ptr<HomographyMinimalSolver4ptsGEM> create(const Mat &points_);
static Ptr<HomographyMinimalSolver4pts> create(const Mat &points, bool use_ge);
};
class PnPSVDSolver : public MinimalSolver {
public:
static Ptr<PnPSVDSolver> create (const Mat &points);
};
//-------------------------- FUNDAMENTAL MATRIX -----------------------
class FundamentalMinimalSolver7pts : public MinimalSolver {
public:
static Ptr<FundamentalMinimalSolver7pts> create(const Mat &points_);
static Ptr<FundamentalMinimalSolver7pts> 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<EssentialMinimalSolverStewenius5pts> create(const Mat &points_);
static Ptr<EssentialMinimalSolver5pts> create(const Mat &points, bool use_svd, bool is_nister);
};
//-------------------------- PNP -----------------------
@ -116,7 +124,7 @@ public:
class P3PSolver : public MinimalSolver {
public:
static Ptr<P3PSolver> create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K);
static Ptr<P3PSolver> create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K);
};
//-------------------------- AFFINE -----------------------
@ -125,9 +133,20 @@ public:
static Ptr<AffineMinimalSolver> create(const Mat &points_);
};
class TrifocalTensorMinimalSolver : public MinimalSolver {
public:
static Ptr<TrifocalTensorMinimalSolver> 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<int> &sample, int sample_size, std::vector<Mat>
&models, const std::vector<double> &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<int> &sample, int sample_size,
std::vector<Mat> &models, const std::vector<double> &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<NonMinimalSolver> clone () const = 0;
virtual int estimate (const std::vector<bool>& mask, std::vector<Mat>& models,
const std::vector<double>& weights) = 0;
virtual void enforceRankConstraint (bool enforce) = 0;
};
//-------------------------- HOMOGRAPHY MATRIX -----------------------
class HomographyNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<HomographyNonMinimalSolver> create(const Mat &points_);
static Ptr<HomographyNonMinimalSolver> create(const Mat &points_, bool use_ge_=false);
static Ptr<HomographyNonMinimalSolver> 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<FundamentalNonMinimalSolver> create(const Mat &points_);
static Ptr<EpipolarNonMinimalSolver> create(const Mat &points_, bool is_fundamental);
static Ptr<EpipolarNonMinimalSolver> 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<EssentialNonMinimalSolver> create(const Mat &points_);
static Ptr<EssentialNonMinimalSolverViaF> create(const Mat &points_, const cv::Mat &K1, const Mat &K2);
};
class EssentialNonMinimalSolverViaT : public NonMinimalSolver {
public:
static Ptr<EssentialNonMinimalSolverViaT> create(const Mat &points_);
};
//-------------------------- PNP -----------------------
@ -164,16 +192,20 @@ public:
class DLSPnP : public NonMinimalSolver {
public:
static Ptr<DLSPnP> create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K);
static Ptr<DLSPnP> create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K);
};
//-------------------------- AFFINE -----------------------
class AffineNonMinimalSolver : public NonMinimalSolver {
public:
static Ptr<AffineNonMinimalSolver> create(const Mat &points_);
static Ptr<AffineNonMinimalSolver> create(const Mat &points, InputArray T1, InputArray T2);
};
class LarssonOptimizer : public NonMinimalSolver {
public:
static Ptr<LarssonOptimizer> 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<double> gamma_complete, gamma_incomplete, gamma;
GammaValues(); // use getSingleton()
class GammaValues : public Algorithm {
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;
virtual ~GammaValues() override = default;
static Ptr<GammaValues> create(int DoF, int max_size_table=500);
virtual const std::vector<double> &getCompleteGammaValues() const = 0;
virtual const std::vector<double> &getIncompleteGammaValues() const = 0;
virtual const std::vector<double> &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<float> &/*errors*/) const {
CV_Error(cv::Error::StsNotImplemented, "getScore(errors)");
}
virtual Score getScore (const std::vector<float>& 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<int> &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<bool> &inliers_mask) const = 0;
virtual int getPointsSize() const = 0;
virtual Ptr<Quality> clone () const = 0;
virtual double getThreshold () const = 0;
virtual Ptr<Error> getErrorFnc () 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);
static int getInliers (const std::vector<float> &errors, std::vector<bool> &inliers,
double threshold);
static int getInliers (const std::vector<float> &errors, std::vector<int> &inliers,
double threshold);
Score selectBest (const std::vector<Mat> &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<MsacQuality> create(int points_size_, double threshold_, const Ptr<Error> &error_);
static Ptr<MsacQuality> create(int points_size_, double threshold_, const Ptr<Error> &error_, double k_msac=2.25);
};
// 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_,
const Ptr<GammaValues> &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<int> &/*sample*/) const {
virtual bool isSampleGood (const std::vector<int>& 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<int> &/*sample*/) const {
virtual bool isModelValid (const Mat& model, const std::vector<int>& 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<int> &/*sample*/,const Mat &/*best_model*/,
Mat &/*non_degenerate_model*/, Score &/*non_degenerate_model_score*/) {
virtual bool recoverIfDegenerate (const std::vector<int> &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<Degeneracy> clone(int /*state*/) const { return makePtr<Degeneracy>(); }
};
class EpipolarGeometryDegeneracy : public Degeneracy {
@ -316,17 +366,31 @@ public:
static Ptr<HomographyDegeneracy> create(const Mat &points_);
};
class FundamentalDegeneracyViaE : public EpipolarGeometryDegeneracy {
public:
static Ptr<FundamentalDegeneracyViaE> create (const Ptr<Quality> &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<bool> &inliers_mask, cv::Mat &F_new, Score &new_score) = 0;
static Ptr<FundamentalDegeneracy> create (int state, const Ptr<Quality> &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<int> &sample, int sample_size, std::vector<Mat>
&models, const std::vector<double> &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<Estimator> 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<EssentialEstimator> create (const Ptr<MinimalSolver> &min_solver_,
const Ptr<NonMinimalSolver> &non_min_solver_, const Ptr<Degeneracy> &degeneracy_);
};
@ -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<float> &getErrors() const = 0;
virtual bool hasErrors () const = 0;
virtual Ptr<ModelVerifier> clone (int state) const = 0;
static Ptr<ModelVerifier> 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<ModelVerifier> create(const Ptr<Quality> &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<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_);
virtual int avgNumCheckedPts () const = 0;
static Ptr<AdaptiveSPRT> create (int state, const Ptr<Quality> &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<int> &sample) = 0;
virtual Ptr<Sampler> 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<int> &getNeighbors(int point_idx_) const = 0;
virtual const std::vector<std::vector<int>> &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<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_);
};
////////////////////////////////////// UNIFORM SAMPLER ////////////////////////////////////////////
class UniformSampler : public Sampler {
@ -488,6 +554,11 @@ public:
static Ptr<UniformSampler> create(int state, int sample_size_, int points_size_);
};
class QuasiUniformSampler : public Sampler {
public:
static Ptr<QuasiUniformSampler> 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<TerminationCriteria> 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<StandardTerminationCriteria> 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<SPRTTermination> create(const std::vector<SPRT_history> &sprt_histories_,
static Ptr<SPRTTermination> create(const Ptr<AdaptiveSPRT> &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<SPRTPNapsacTermination> create(const std::vector<SPRT_history>&
sprt_histories_, double confidence, int points_size_, int sample_size_,
static Ptr<SPRTPNapsacTermination> create(const Ptr<AdaptiveSPRT> &
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<int> &getNonRandomInliers () const = 0;
virtual int updateTerminationLength (const Mat &model, int inliers_size, int &found_termination_length) const = 0;
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);
double inlier_thresh, const std::vector<int> &non_rand_inliers);
};
//////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////// UTILS ////////////////////////////////////////////////
namespace Utils {
void densitySort (const Mat &points, int knn, Mat &sorted_points, std::vector<int> &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<float> &array);
double intersectionOverUnion (const std::vector<bool> &a, const std::vector<bool> &b);
void triangulatePoints (const Mat &points, const Mat &E_, const Mat &K1, const Mat &K2, Mat &points3D, Mat &R, Mat &t,
std::vector<bool> &good_mask, std::vector<double> &depths1, std::vector<double> &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<bool> &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<bool> &good_mask, std::vector<double> &depths1, std::vector<double> &depths2);
int decomposeHomography (const Matx33d &Hnorm, std::vector<Matx33d> &R, std::vector<Vec3d> &t);
double getPoissonCDF (double lambda, int tentative_inliers);
void getClosePoints (const cv::Mat &points, std::vector<std::vector<int>> &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<double> &coeffs, std::vector<double> &real_roots) = 0;
static Ptr<SolverPoly> create();
};
///////////////////////////////////////// RANDOM GENERATOR /////////////////////////////////////
class RandomGenerator : public Algorithm {
public:
@ -609,7 +700,6 @@ public:
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 {
@ -618,6 +708,24 @@ public:
static Ptr<UniformRandomGenerator> create (int state, int max_range, int subset_size_);
};
class WeightFunction : public Algorithm {
public:
virtual int getInliersWeights (const std::vector<float> &errors, std::vector<int> &inliers, std::vector<double> &weights) const = 0;
virtual int getInliersWeights (const std::vector<float> &errors, std::vector<int> &inliers, std::vector<double> &weights, double thr_sqr) const = 0;
virtual double getThreshold() const = 0;
};
class GaussWeightFunction : public WeightFunction {
public:
static Ptr<GaussWeightFunction> create (double thr, double sigma, double outlier_prob);
};
class MagsacWeightFunction : public WeightFunction {
public:
static Ptr<MagsacWeightFunction> create (const Ptr<GammaValues> &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<LocalOptimization> 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<GraphCut>
create(const Ptr<Estimator> &estimator_, const Ptr<Error> &error_,
create(const Ptr<Estimator> &estimator_,
const Ptr<Quality> &quality_, const Ptr<NeighborhoodGraph> &neighborhood_graph_,
const Ptr<RandomGenerator> &lo_sampler_, double threshold_,
double spatial_coherence_term, int gc_iters);
double spatial_coherence_term, int gc_iters, Ptr<Termination> 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<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);
static Ptr<SimpleLocalOptimization> create
(const Ptr<Quality> &quality_, const Ptr<NonMinimalSolver> &estimator_,
const Ptr<Termination> termination_, const Ptr<RandomGenerator> &random_gen,
const Ptr<WeightFunction> 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<LeastSquaresPolishing> create (const Ptr<Estimator> &estimator_,
const Ptr<Quality> &quality_, int lsq_iterations);
static Ptr<NonMinimalPolisher> create(const Ptr<Quality> &quality_, const Ptr<NonMinimalSolver> &solver_,
Ptr<WeightFunction> weight_fnc_, int max_iters_, double iou_thr_);
};
class CovarianceSolver : public NonMinimalSolver {
public:
~CovarianceSolver() override = default;
int estimate (const std::vector<int> &, int , std::vector<Mat> &,
const std::vector<double> &) const override {
assert(false && "not implemeted!");
return 0;
}
virtual int estimate (const std::vector<bool> &new_mask, std::vector<Mat> &models,
const std::vector<double> &weights) override = 0;
virtual void reset() = 0;
};
class CovarianceEpipolarSolver : public CovarianceSolver {
public:
static Ptr<CovarianceEpipolarSolver> create (const Mat &points, bool is_fundamental);
static Ptr<CovarianceEpipolarSolver> create (const Mat &points, const Matx33d &T1, const Matx33d &T2);
};
class CovarianceHomographySolver : public CovarianceSolver {
public:
static Ptr<CovarianceHomographySolver> create (const Mat &points);
static Ptr<CovarianceHomographySolver> create (const Mat &points, const Matx33d &T1, const Matx33d &T2);
};
class CovarianceAffineSolver : public CovarianceSolver {
public:
static Ptr<CovarianceAffineSolver> create (const Mat &points, const Matx33d &T1, const Matx33d &T2);
static Ptr<CovarianceAffineSolver> 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<CameraPose> 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<int> &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<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_);
const std::vector<bool> &inliers_mask_, int number_inliers_,
int number_iterations_, ModelConfidence conf, const std::vector<float> &errors_);
// 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;
virtual int getNumberOfIters() const = 0;
virtual ModelConfidence getConfidence() const = 0;
virtual const std::vector<float> &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<int> &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<int> &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<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,
bool run (const Ptr<const Model> &params, InputArray points1, InputArray points2,
Ptr<RansacOutput> &ransac_output, InputArray K1_, InputArray K2_,
InputArray dist_coeff1, InputArray dist_coeff2);
}}

308
modules/calib3d/src/usac/bundle.cpp Normal file
View File

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

717
modules/calib3d/src/usac/degeneracy.cpp
View File

@ -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<int> &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<Degeneracy> clone(int /*state*/) const override {
return makePtr<EpipolarGeometryDegeneracyImpl>(*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<int> &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<Degeneracy> clone(int /*state*/) const override {
return makePtr<HomographyDegeneracyImpl>(*points_mat);
}
};
Ptr<HomographyDegeneracy> HomographyDegeneracy::create (const Mat &points_) {
return makePtr<HomographyDegeneracyImpl>(points_);
}
class FundamentalDegeneracyViaEImpl : public FundamentalDegeneracyViaE {
private:
bool is_F_objective;
std::vector<std::vector<int>> instances = {{0,1,2,3,4}, {2,3,4,5,6}, {0,1,4,5,6}};
std::vector<int> e_sample;
const Ptr<Quality> quality;
Ptr<EpipolarGeometryDegeneracy> e_degen, f_degen;
Ptr<EssentialMinimalSolver5pts> e_solver;
std::vector<Mat> e_models;
const int E_SAMPLE_SIZE = 5;
Matx33d K2_inv_t, K1_inv;
public:
FundamentalDegeneracyViaEImpl (const Ptr<Quality> &quality_, const Mat &pts, const Mat &calib_pts, const Matx33d &K1, const Matx33d &K2, bool is_f_objective)
: quality(quality_) {
is_F_objective = is_f_objective;
e_solver = EssentialMinimalSolver5pts::create(calib_pts, false/*svd*/, true/*nister*/);
f_degen = is_F_objective ? EpipolarGeometryDegeneracy::create(pts, 7) : EpipolarGeometryDegeneracy::create(calib_pts, 7);
e_degen = EpipolarGeometryDegeneracy::create(calib_pts, E_SAMPLE_SIZE);
e_sample = std::vector<int>(E_SAMPLE_SIZE);
if (is_f_objective) {
K2_inv_t = K2.inv().t();
K1_inv = K1.inv();
}
}
bool isModelValid (const Mat &F, const std::vector<int> &sample) const override {
return f_degen->isModelValid(F, sample);
}
bool recoverIfDegenerate (const std::vector<int> &sample_7pt, const Mat &/*best*/, const Score &best_score,
Mat &out_model, Score &out_score) override {
out_score = Score();
for (const auto &instance : instances) {
for (int i = 0; i < E_SAMPLE_SIZE; i++)
e_sample[i] = sample_7pt[instance[i]];
const int num_models = e_solver->estimate(e_sample, e_models);
for (int i = 0; i < num_models; i++) {
if (e_degen->isModelValid(e_models[i], e_sample)) {
const Mat model = is_F_objective ? Mat(K2_inv_t * Matx33d(e_models[i]) * K1_inv) : e_models[i];
const auto sc = quality->getScore(model);
if (sc.isBetter(out_score)) {
out_score = sc;
model.copyTo(out_model);
}
}
}
if (out_score.isBetter(best_score)) break;
}
return true;
}
};
Ptr<FundamentalDegeneracyViaE> FundamentalDegeneracyViaE::create (const Ptr<Quality> &quality, const Mat &pts,
const Mat &calib_pts, const Matx33d &K1, const Matx33d &K2, bool is_f_objective) {
return makePtr<FundamentalDegeneracyViaEImpl>(quality, pts, calib_pts, K1, K2, is_f_objective);
}
///////////////////////////////// Fundamental Matrix Degeneracy ///////////////////////////////////
class FundamentalDegeneracyImpl : public FundamentalDegeneracy {
private:
RNG rng;
const Ptr<Quality> quality;
const Ptr<Error> f_error;
Ptr<Quality> h_repr_quality;
const float * const points;
const Mat * points_mat;
const Ptr<ReprojectionErrorForward> h_reproj_error;
Ptr<EpipolarNonMinimalSolver> f_non_min_solver;
Ptr<HomographyNonMinimalSolver> h_non_min_solver;
Ptr<UniformRandomGenerator> random_gen_H;
const EpipolarGeometryDegeneracyImpl ep_deg;
// threshold to find inliers for homography model
const double homography_threshold, log_conf = log(0.05);
const double homography_threshold, log_conf = log(0.05), H_SAMPLE_THR_SQR = 49/*7^2*/, MAX_H_THR = 225/*15^2*/;
double f_threshold_sqr, best_focal = -1;
// points (1-7) to verify in sample
std::vector<std::vector<int>> h_sample {{0,1,2},{3,4,5},{0,1,6},{3,4,6},{2,5,6}};
std::vector<int> h_inliers;
std::vector<std::vector<int>> h_sample_ver {{3,4,5,6},{0,1,2,6},{2,3,4,5},{0,1,2,5},{0,1,3,4}};
std::vector<int> non_planar_supports, h_inliers, h_outliers, h_outliers_eval, f_inliers;
std::vector<double> weights;
std::vector<Mat> h_models;
const int points_size, 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> &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<int>{0, 1, 7});
h_sample.emplace_back(std::vector<int>{0, 2, 7});
h_sample.emplace_back(std::vector<int>{3, 5, 7});
h_sample.emplace_back(std::vector<int>{3, 6, 7});
h_sample.emplace_back(std::vector<int>{2, 4, 7});
h_sample.emplace_back(std::vector<int>{0, 1, 7}); h_sample_ver.emplace_back(std::vector<int>{2,3,4,5,6});
h_sample.emplace_back(std::vector<int>{0, 2, 7}); h_sample_ver.emplace_back(std::vector<int>{1,3,4,5,6});
h_sample.emplace_back(std::vector<int>{3, 5, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,2,4,6});
h_sample.emplace_back(std::vector<int>{3, 6, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,2,4,5});
h_sample.emplace_back(std::vector<int>{2, 4, 7}); h_sample_ver.emplace_back(std::vector<int>{0,1,3,5,6});
}
non_planar_supports = std::vector<int>(MAX_MODELS_TO_TEST);
h_inliers = std::vector<int>(points_size);
h_outliers = std::vector<int>(points_size);
h_outliers_eval = std::vector<int>(points_size);
f_inliers = std::vector<int>(points_size);
h_non_min_solver = HomographyNonMinimalSolver::create(points_);
f_non_min_solver = EpipolarNonMinimalSolver::create(points_, true /*F*/);
num_h_outliers_eval = num_h_outliers = points_size;
f_threshold_sqr = f_inlier_thr_sqr;
h_repr_quality = MsacQuality::create(points_.rows, homography_threshold_, h_reproj_error);
true_K_given = ! true_K1_.empty() && ! true_K2_.empty();
if (true_K_given) {
true_K1 = Matx33d((double *)true_K1_.data);
true_K2_inv = Matx33d(Mat(true_K2_.inv()));
true_K2_t = Matx33d(true_K2_).t();
true_K1_inv = true_K1.inv();
true_K2_inv_t = true_K2_inv.t();
}
random_gen_H = UniformRandomGenerator::create(rng.uniform(0, INT_MAX), points_size, MAX_H_SUBSET);
estimated_min_non_planar_support = TENT_MIN_NON_PLANAR_SUPP = std::min(5, (int)(0.05*points_size));
}
bool estimateHfrom3Points (const Mat &F_best, const std::vector<int> &sample, Mat &H_best) {
Score H_best_score;
// find e', null vector of F^T
const Vec3d e_prime = Utils::getLeftEpipole(F_best);
const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best);
bool is_degenerate = false;
int idx = -1;
for (const auto &h_i : h_sample) { // only 5 samples
idx++;
Matx33d H;
if (!getH(A, e_prime, 4 * sample[h_i[0]], 4 * sample[h_i[1]], 4 * sample[h_i[2]], H))
continue;
h_reproj_error->setModelParameters(Mat(H));
const auto &ver_pts = h_sample_ver[idx];
int inliers_in_plane = 3; // 3 points are always inliers
for (int s : ver_pts)
if (h_reproj_error->getError(sample[s]) < homography_threshold) {
if (++inliers_in_plane >= H_INLS_DEGEN_SAMPLE)
break;
}
if (inliers_in_plane >= H_INLS_DEGEN_SAMPLE) {
is_degenerate = true;
const auto h_score = h_repr_quality->getScore(Mat(H));
if (h_score.isBetter(H_best_score)) {
H_best_score = h_score;
H_best = Mat(H);
}
}
}
if (!is_degenerate)
return false;
int h_inls_cnt = optimizeH(H_best, H_best_score);
for (int iter = 0; iter < 2; iter++) {
if (h_non_min_solver->estimate(h_inliers, h_inls_cnt, h_models, weights) == 0)
break;
const auto h_score = h_repr_quality->getScore(h_models[0]);
if (h_score.isBetter(H_best_score)) {
H_best_score = h_score;
h_models[0].copyTo(H_best);
h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
} else break;
}
getOutliersH(H_best);
return true;
}
bool optimizeF (const Mat &F, const Score &score, Mat &F_new, Score &new_score) {
std::vector<Mat> Fs;
if (f_non_min_solver->estimate(f_inliers, quality->getInliers(F, f_inliers), Fs, weights)) {
const auto F_polished_score = quality->getScore(f_error->getErrors(Fs[0]));
if (F_polished_score.isBetter(score)) {
Fs[0].copyTo(F_new); new_score = F_polished_score;
return true;
}
}
return false;
}
int optimizeH (Mat &H_best, Score &H_best_score) {
int h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
random_gen_H->setSubsetSize(h_inls_cnt <= MAX_H_SUBSET ? (int)(0.8*h_inls_cnt) : MAX_H_SUBSET);
if (random_gen_H->getSubsetSize() >= 4/*min H sample size*/) {
for (int iter = 0; iter < MAX_ITERS_H; iter++) {
if (h_non_min_solver->estimate(random_gen_H->generateUniqueRandomSubset(h_inliers, h_inls_cnt), random_gen_H->getSubsetSize(), h_models, weights) == 0)
continue;
const auto h_score = h_repr_quality->getScore(h_models[0]);
if (h_score.isBetter(H_best_score)) {
h_models[0].copyTo(H_best);
// if more inliers than previous best
if (h_score.inlier_number > H_best_score.inlier_number || h_score.inlier_number >= MAX_H_SUBSET) {
h_inls_cnt = h_repr_quality->getInliers(H_best, h_inliers);
random_gen_H->setSubsetSize(h_inls_cnt <= MAX_H_SUBSET ? (int)(0.8*h_inls_cnt) : MAX_H_SUBSET);
}
H_best_score = h_score;
}
}
}
return h_inls_cnt;
}
bool recoverIfDegenerate (const std::vector<int> &sample, const Mat &F_best, const Score &F_best_score,
Mat &non_degenerate_model, Score &non_degenerate_model_score) override {
const auto swapF = [&] (const Mat &_F, const Score &_score) {
const auto non_min_solver = EpipolarNonMinimalSolver::create(*points_mat, true);
if (! optimizeF(_F, _score, non_degenerate_model, non_degenerate_model_score)) {
_F.copyTo(non_degenerate_model);
non_degenerate_model_score = _score;
}
};
Mat F_from_H, F_from_E, H_best; Score F_from_H_score, F_from_E_score;
if (! estimateHfrom3Points(F_best, sample, H_best)) {
return false; // non degenerate
}
if (true_K_given) {
if (getFfromTrueK(H_best, F_from_H, F_from_H_score)) {
if (F_from_H_score.isBetter(F_from_E_score))
swapF(F_from_H, F_from_H_score);
else swapF(F_from_E, F_from_E_score);
return true;
} else {
non_degenerate_model_score = Score();
return true; // no translation
}
}
const int non_planar_support_degen_F = getNonPlanarSupport(F_best);
Score F_pl_par_score, F_calib_score; Mat F_pl_par, F_calib;
if (calibDegensac(H_best, F_calib, F_calib_score, non_planar_support_degen_F, F_best_score)) {
if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, max_iters_plane_and_parallax, true,
F_best_score, non_planar_support_degen_F, F_pl_par, F_pl_par_score) && F_pl_par_score.isBetter(F_calib_score)
&& getNonPlanarSupport(F_pl_par) > getNonPlanarSupport(F_calib)) {
swapF(F_pl_par, F_pl_par_score);
return true;
}
swapF(F_calib, F_calib_score);
return true;
} else {
if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, max_iters_plane_and_parallax, true,
F_best_score, non_planar_support_degen_F, F_pl_par, F_pl_par_score)) {
swapF(F_pl_par, F_pl_par_score);
return true;
}
}
if (! isFDegenerate(non_planar_support_degen_F)) {
return false;
}
non_degenerate_model_score = Score();
return true;
}
// RANSAC with plane-and-parallax to find new Fundamental matrix
bool getFfromTrueK (const Matx33d &H, Mat &F_from_K, Score &F_from_K_score) {
std::vector<Matx33d> R; std::vector<Vec3d> t;
if (decomposeHomographyMat(true_K2_inv * H * true_K1, Matx33d::eye(), R, t, noArray()) == 1) {
// std::cout << "Warning: translation is zero!\n";
return false; // is degenerate
}
// sign of translation does not make difference
const Mat F1 = Mat(true_K2_inv_t * Math::getSkewSymmetric(t[0]) * R[0] * true_K1_inv);
const Mat F2 = Mat(true_K2_inv_t * Math::getSkewSymmetric(t[1]) * R[1] * true_K1_inv);
const auto score_f1 = quality->getScore(f_error->getErrors(F1)), score_f2 = quality->getScore(f_error->getErrors(F2));
if (score_f1.isBetter(score_f2)) {
F_from_K = F1; F_from_K_score = score_f1;
} else {
F_from_K = F2; F_from_K_score = score_f2;
}
return true;
}
bool planeAndParallaxRANSAC (const Matx33d &H, std::vector<int> &non_planar_pts, int num_non_planar_pts,
int max_iters_pl_par, bool use_preemptive, const Score &score_degen_F, int non_planar_support_degen_F,
Mat &F_new, Score &F_new_score) {
if (num_non_planar_pts < 2)
return false;
num_models_used_so_far = 0; // reset estimation of lambda for plane-and-parallax
int max_iters = max_iters_pl_par, non_planar_support = 0, iters = 0;
std::vector<Matx33d> F_good;
const double CLOSE_THR = 1.0;
for (; iters < max_iters; iters++) {
// draw two random points
int h_outlier1 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];
int h_outlier2 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];
while (h_outlier1 == h_outlier2)
h_outlier2 = 4 * non_planar_pts[rng.uniform(0, num_non_planar_pts)];
const auto x1 = points[h_outlier1], y1 = points[h_outlier1+1], X1 = points[h_outlier1+2], Y1 = points[h_outlier1+3];
const auto x2 = points[h_outlier2], y2 = points[h_outlier2+1], X2 = points[h_outlier2+2], Y2 = points[h_outlier2+3];
if ((fabsf(X1 - X2) < CLOSE_THR && fabsf(Y1 - Y2) < CLOSE_THR) ||
(fabsf(x1 - x2) < CLOSE_THR && fabsf(y1 - y2) < CLOSE_THR))
continue;
// do plane and parallax with outliers of H
// F = [(p1' x Hp1) x (p2' x Hp2)]_x H = [e']_x H
Vec3d ep = (Vec3d(X1, Y1, 1).cross(H * Vec3d(x1, y1, 1))) // l1 = p1' x Hp1
.cross((Vec3d(X2, Y2, 1).cross(H * Vec3d(x2, y2, 1))));// l2 = p2' x Hp2
const Matx33d F = Math::getSkewSymmetric(ep) * H;
const auto * const f = F.val;
// check orientation constraint of epipolar lines
const bool valid = (f[0]*x1+f[1]*y1+f[2])*(ep[1]-ep[2]*Y1) * (f[0]*x2+f[1]*y2+f[2])*(ep[1]-ep[2]*Y2) > 0;
if (!valid) continue;
const int num_f_inliers_of_h_outliers = getNonPlanarSupport(Mat(F), num_models_used_so_far >= MAX_MODELS_TO_TEST, non_planar_support);
if (non_planar_support < num_f_inliers_of_h_outliers) {
non_planar_support = num_f_inliers_of_h_outliers;
const double predicted_iters = log_conf / log(1 - pow(static_cast<double>
(getNonPlanarSupport(Mat(F), non_planar_pts, num_non_planar_pts)) / num_non_planar_pts, 2));
if (use_preemptive && ! std::isinf(predicted_iters) && predicted_iters < max_iters)
max_iters = static_cast<int>(predicted_iters);
F_good = { F };
} else if (non_planar_support == num_f_inliers_of_h_outliers) {
F_good.emplace_back(F);
}
}
F_new_score = Score();
for (const auto &F_ : F_good) {
const auto sc = quality->getScore(f_error->getErrors(Mat(F_)));
if (sc.isBetter(F_new_score)) {
F_new_score = sc;
F_new = Mat(F_);
}
}
if (F_new_score.isBetter(score_degen_F) && non_planar_support > non_planar_support_degen_F)
return true;
if (isFDegenerate(non_planar_support))
return false;
return true;
}
bool calibDegensac (const Matx33d &H, Mat &F_new, Score &F_new_score, int non_planar_support_degen_F, const Score &F_degen_score) {
if (! is_principal_pt_set) {
// estimate principal points from coordinates
float px1 = 0, py1 = 0, px2 = 0, py2 = 0;
for (int i = 0; i < points_size; i++) {
const int idx = 4*i;
if (px1 < points[idx ]) px1 = points[idx ];
if (py1 < points[idx+1]) py1 = points[idx+1];
if (px2 < points[idx+2]) px2 = points[idx+2];
if (py2 < points[idx+3]) py2 = points[idx+3];
}
setPrincipalPoint((int)(px1/2)+1, (int)(py1/2)+1, (int)(px2/2)+1, (int)(py2/2)+1);
}
std::vector<Mat> R; std::vector<Mat> t; std::vector<Mat> F_good;
std::vector<double> best_f;
int non_planar_support_out = 0;
for (double f = 300; f <= 3000; f += 150.) {
K(0,0) = K(1,1) = K2(0,0) = K2(1,1) = f;
const double one_over_f = 1/f;
K_inv(0,0) = K_inv(1,1) = K2_inv(0,0) = K2_inv(1,1) = K2_inv_t(0,0) = K2_inv_t(1,1) = one_over_f;
K_inv(0,2) = -K(0,2)*one_over_f; K_inv(1,2) = -K(1,2)*one_over_f;
K2_inv_t(2,0) = K2_inv(0,2) = -K2(0,2)*one_over_f; K2_inv_t(2,1) = K2_inv(1,2) = -K2(1,2)*one_over_f;
if (decomposeHomographyMat(K2_inv * H * K, Matx33d::eye(), R, t, noArray()) == 1) continue;
Mat F1 = Mat(K2_inv_t * Math::getSkewSymmetric(Vec3d(t[0])) * Matx33d(R[0]) * K_inv);
Mat F2 = Mat(K2_inv_t * Math::getSkewSymmetric(Vec3d(t[2])) * Matx33d(R[2]) * K_inv);
int non_planar_f1 = getNonPlanarSupport(F1, true, non_planar_support_out),
non_planar_f2 = getNonPlanarSupport(F2, true, non_planar_support_out);
if (non_planar_f1 < non_planar_f2) {
non_planar_f1 = non_planar_f2; F1 = F2;
}
if (non_planar_support_out < non_planar_f1) {
non_planar_support_out = non_planar_f1;
F_good = {F1};
best_f = { f };
} else if (non_planar_support_out == non_planar_f1) {
F_good.emplace_back(F1);
best_f.emplace_back(f);
}
}
F_new_score = Score();
for (int i = 0; i < (int) F_good.size(); i++) {
const auto sc = quality->getScore(f_error->getErrors(F_good[i]));
if (sc.isBetter(F_new_score)) {
F_new_score = sc;
F_good[i].copyTo(F_new);
if (sc.isBetter(best_focal_score)) {
best_focal = best_f[i]; // save best focal length
best_focal_score = sc;
}
}
}
if (F_degen_score.isBetter(F_new_score) && non_planar_support_out <= non_planar_support_degen_F)
return false;
/*
// logarithmic search -> faster but less accurate
double f_min = 300, f_max = 3500;
while (f_max - f_min > 100) {
const double f_half = (f_max + f_min) * 0.5f, left_half = (f_min + f_half) * 0.5f, right_half = (f_half + f_max) * 0.5f;
const double inl_in_left = eval_f(left_half), inl_in_right = eval_f(right_half);
if (inl_in_left > inl_in_right)
f_max = f_half;
else f_min = f_half;
}
*/
return true;
}
void getOutliersH (const Mat &H_best) {
// get H outliers
num_h_outliers_eval = num_h_outliers = 0;
const auto &h_errors = h_reproj_error->getErrors(H_best);
for (int pt = 0; pt < points_size; pt++)
if (h_errors[pt] > H_SAMPLE_THR_SQR) {
h_outliers[num_h_outliers++] = pt;
if (h_errors[pt] > MAX_H_THR)
h_outliers_eval[num_h_outliers_eval++] = pt;
}
}
bool verifyFundamental (const Mat &F_best, const Score &F_score, const std::vector<bool> &inliers_mask, Mat &F_new, Score &new_score) override {
const int f_sample_size = 3, max_H_iters = 5; // 3.52 = log(0.01) / log(1 - std::pow(0.9, 3));
int num_f_inliers = 0;
std::vector<int> inliers(points_size), f_sample(f_sample_size);
for (int i = 0; i < points_size; i++) if (inliers_mask[i]) inliers[num_f_inliers++] = i;
const auto sampler = UniformSampler::create(0, f_sample_size, num_f_inliers);
// find e', null space of F^T
const Vec3d e_prime = Utils::getLeftEpipole(F_best);
const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best);
Score H_best_score; Mat H_best;
for (int iter = 0; iter < max_H_iters; iter++) {
sampler->generateSample(f_sample);
Matx33d H;
if (!getH(A, e_prime, 4*inliers[f_sample[0]], 4*inliers[f_sample[1]], 4*inliers[f_sample[2]], H))
continue;
const auto h_score = h_repr_quality->getScore(Mat(H));
if (h_score.isBetter(H_best_score)) {
H_best_score = h_score; H_best = Mat(H);
}
}
if (H_best.empty()) return false; // non-degenerate
optimizeH(H_best, H_best_score);
getOutliersH(H_best);
const int non_planar_support_best_F = getNonPlanarSupport(F_best);
const bool is_F_degen = isFDegenerate(non_planar_support_best_F);
Mat F_from_K; Score F_from_K_score;
bool success = false;
// generate non-degenerate F even though so-far-the-best one may not be degenerate
if (true_K_given) {
// use GT calibration
if (getFfromTrueK(H_best, F_from_K, F_from_K_score)) {
new_score = F_from_K_score;
F_from_K.copyTo(F_new);
success = true;
}
} else {
// use calibrated DEGENSAC
if (calibDegensac(H_best, F_from_K, F_from_K_score, non_planar_support_best_F, F_score)) {
new_score = F_from_K_score;
F_from_K.copyTo(F_new);
success = true;
}
}
if (!is_F_degen) {
return false;
} else if (success) // F is degenerate
return true; // but successfully recovered
// recover degenerate F using plane-and-parallax
Score plane_parallax_score; Mat F_plane_parallax;
if (planeAndParallaxRANSAC(H_best, h_outliers, num_h_outliers, 20, true,
F_score, non_planar_support_best_F, F_plane_parallax, plane_parallax_score)) {
new_score = plane_parallax_score;
F_plane_parallax.copyTo(F_new);
return true;
}
// plane-and-parallax failed. A previous non-degenerate so-far-the-best model will be used instead
new_score = Score();
return true;
}
void setPrincipalPoint (double px_, double py_) override {
setPrincipalPoint(px_, py_, 0, 0);
}
void setPrincipalPoint (double px_, double py_, double px2_, double py2_) override {
if (px_ > DBL_EPSILON && py_ > DBL_EPSILON) {
is_principal_pt_set = true;
K = {1, 0, px_, 0, 1, py_, 0, 0, 1};
if (px2_ > DBL_EPSILON && py2_ > DBL_EPSILON) K2 = {1, 0, px2_, 0, 1, py2_, 0, 0, 1};
else K2 = K;
K_inv = K2_inv = K2_inv_t = Matx33d::eye();
}
}
private:
bool getH (const Matx33d &A, const Vec3d &e_prime, int smpl1, int smpl2, int smpl3, Matx33d &H) {
Vec3d p1(points[smpl1 ], points[smpl1+1], 1), p2(points[smpl2 ], points[smpl2+1], 1), p3(points[smpl3 ], points[smpl3+1], 1);
Vec3d P1(points[smpl1+2], points[smpl1+3], 1), P2(points[smpl2+2], points[smpl2+3], 1), P3(points[smpl3+2], points[smpl3+3], 1);
const Matx33d M (p1[0], p1[1], 1, p2[0], p2[1], 1, p3[0], p3[1], 1);
if (p1.cross(p2).dot(p3) * P1.cross(P2).dot(P3) < 0) return false;
// (x′i × e')
const Vec3d P1e = P1.cross(e_prime), P2e = P2.cross(e_prime), P3e = P3.cross(e_prime);
// x′i × (A xi))^T (x′i × e′) / ‖x′i×e′‖^2,
const Vec3d b (P1.cross(A * p1).dot(P1e) / (P1e[0]*P1e[0]+P1e[1]*P1e[1]+P1e[2]*P1e[2]),
P2.cross(A * p2).dot(P2e) / (P2e[0]*P2e[0]+P2e[1]*P2e[1]+P2e[2]*P2e[2]),
P3.cross(A * p3).dot(P3e) / (P3e[0]*P3e[0]+P3e[1]*P3e[1]+P3e[2]*P3e[2]));
H = A - e_prime * (M.inv() * b).t();
return true;
}
int getNonPlanarSupport (const Mat &F, const std::vector<int> &pts, int num_pts) {
int non_rand_support = 0;
f_error->setModelParameters(F);
for (int pt = 0; pt < num_pts; pt++)
if (f_error->getError(pts[pt]) < f_threshold_sqr)
non_rand_support++;
return non_rand_support;
}
int getNonPlanarSupport (const Mat &F, bool preemptive=false, int max_so_far=0) {
int non_rand_support = 0;
f_error->setModelParameters(F);
if (preemptive) {
const auto preemptive_thr = -num_h_outliers_eval + max_so_far;
for (int pt = 0; pt < num_h_outliers_eval; pt++)
if (f_error->getError(h_outliers_eval[pt]) < f_threshold_sqr)
non_rand_support++;
else if (non_rand_support - pt < preemptive_thr)
break;
} else {
for (int pt = 0; pt < num_h_outliers_eval; pt++)
if (f_error->getError(h_outliers_eval[pt]) < f_threshold_sqr)
non_rand_support++;
if (num_models_used_so_far < MAX_MODELS_TO_TEST && !true_K_given/*for K we know that recovered F cannot be degenerate*/) {
non_planar_supports[num_models_used_so_far++] = non_rand_support;
if (num_models_used_so_far == MAX_MODELS_TO_TEST) {
getLambda(non_planar_supports, 2.32, num_h_outliers_eval, 0, false, estimated_min_non_planar_support);
if (estimated_min_non_planar_support < 3) estimated_min_non_planar_support = 3;
}
}
}
return non_rand_support;
}
inline bool isModelValid(const Mat &F, const std::vector<int> &sample) const override {
return ep_deg.isModelValid(F, sample);
}
bool recoverIfDegenerate (const std::vector<int> &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<Degeneracy> clone(int state) const override {
return makePtr<FundamentalDegeneracyImpl>(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<float> &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<double>(score.inlier_number) / points_size, 2));
if (! std::isinf(predicted_iters) && predicted_iters < max_iters)
max_iters = static_cast<int>(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> FundamentalDegeneracy::create (int state, const Ptr<Quality> &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<FundamentalDegeneracyImpl>(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<int> &sample) const override {
return ep_deg.isModelValid(E, sample);
}
Ptr<Degeneracy> clone(int /*state*/) const override {
return makePtr<EssentialDegeneracyImpl>(*points_mat, sample_size);
}
};
Ptr<EssentialDegeneracy> EssentialDegeneracy::create (const Mat &points_, int sample_size_) {
return makePtr<EssentialDegeneracyImpl>(points_, sample_size_);
}
}}
}}

31
modules/calib3d/src/usac/dls_solver.cpp
View File

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

448
modules/calib3d/src/usac/essential_solver.cpp
View File

@ -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<int> &sample, std::vector<Mat> &models) const override {
// (1) Extract 4 null vectors from linear equations of epipolar constraint
std::vector<double> 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<double, 5, 9> 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_<double> constraint_mat(10, 20);
Matx<double, 1, 10> 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<double, 1, 10> 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<double, 1, 10> trace = eet[0][0] + eet[1][1] + eet[2][2];
Mat_<double> 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<double, 1, 10> 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<double, 10, 10> 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<double> 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<Mat>(); 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_<double> 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<double, 1, 10> 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<double, 10, 20, Eigen::RowMajor> 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<double, 10, 10> eliminated_mat_eig = constraint_mat_eig.block<10, 10>(0, 0)
.fullPivLu().solve(constraint_mat_eig.block<10, 10>(0, 10));
const Eigen::Matrix<double, 10, 20, Eigen::RowMajor> 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<double, 10, 10> 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<double, 10, 10> action_mat_eig = Eigen::Matrix<double, 10, 10>::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<double, 10, 10> action_mat_eig = Eigen::Matrix<double, 10, 10>::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<Eigen::Matrix<double, 10, 10>> 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<Eigen::Matrix<double, 10, 10>> eigensolver(action_mat_eig);
const Eigen::VectorXcd &eigenvalues = eigensolver.eigenvalues();
const auto * const eig_vecs_ = (double *) eigensolver.eigenvectors().real().data();
#else
Matx<double, 10, 10> 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<double, 10, 10> 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_<double>::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_<double>::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<Mat>(); models.reserve(10);
models = std::vector<Mat>(); 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_<double> 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_<double> 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_<double> 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_<double> 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<int>(models.size());
#else
int estimate (const std::vector<int> &/*sample*/, std::vector<Mat> &/*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<MinimalSolver> clone () const override {
return makePtr<EssentialMinimalSolverStewenius5ptsImpl>(*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> EssentialMinimalSolverStewenius5pts::create
(const Mat &points_) {
return makePtr<EssentialMinimalSolverStewenius5ptsImpl>(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<int> &sample, int sample_size, std::vector<Mat>
&models, const std::vector<double> &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>{ Mat_<double>(3,3) };
const Eigen::JacobiSVD<Eigen::Matrix<double, 9, 9>> svd((Eigen::Matrix<double, 9, 9>(AtA)),
Eigen::ComputeFullV);
// extract the last nullspace
Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)models[0].data) = svd.matrixV().col(8);
#else
Matx<double, 9, 9> AtA_(AtA), U, Vt;
Vec<double, 9> W;
SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A);
models = std::vector<Mat> { Mat_<double>(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<NonMinimalSolver> clone () const override {
return makePtr<EssentialNonMinimalSolverImpl>(*points_mat);
static inline Matx<double, 1, 20> 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<double, 1, 20>
({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> EssentialNonMinimalSolver::create (const Mat &points_) {
return makePtr<EssentialNonMinimalSolverImpl>(points_);
Ptr<EssentialMinimalSolver5pts> EssentialMinimalSolver5pts::create
(const Mat &points_, bool use_svd, bool is_nister) {
return makePtr<EssentialMinimalSolver5ptsImpl>(points_, use_svd, is_nister);
}
}}

97
modules/calib3d/src/usac/estimator.cpp
View File

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

497
modules/calib3d/src/usac/fundamental_solver.cpp
View File

@ -4,19 +4,20 @@
#include "../precomp.hpp"
#include "../usac.hpp"
#include "../polynom_solver.h"
#ifdef HAVE_EIGEN
#include <Eigen/Eigen>
#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<int> &sample, std::vector<Mat> &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<double, 7, 9> 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_<double> coeffs (1, 4, c);
Mat_<double> 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<Mat>(nroots);
@ -145,12 +151,9 @@ public:
int getMaxNumberOfSolutions () const override { return 3; }
int getSampleSize() const override { return 7; }
Ptr<MinimalSolver> clone () const override {
return makePtr<FundamentalMinimalSolver7ptsImpl>(*points_mat);
}
};
Ptr<FundamentalMinimalSolver7pts> FundamentalMinimalSolver7pts::create(const Mat &points_) {
return makePtr<FundamentalMinimalSolver7ptsImpl>(points_);
Ptr<FundamentalMinimalSolver7pts> FundamentalMinimalSolver7pts::create(const Mat &points, bool use_ge) {
return makePtr<FundamentalMinimalSolver7ptsImpl>(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<int> &sample, std::vector<Mat> &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<MinimalSolver> clone () const override {
return makePtr<FundamentalMinimalSolver8ptsImpl>(*points_mat);
}
};
Ptr<FundamentalMinimalSolver8pts> FundamentalMinimalSolver8pts::create(const Mat &points_) {
return makePtr<FundamentalMinimalSolver8ptsImpl>(points_);
}
class FundamentalNonMinimalSolverImpl : public FundamentalNonMinimalSolver {
class EpipolarNonMinimalSolverImpl : public EpipolarNonMinimalSolver {
private:
const Mat * points_mat;
const Ptr<NormTransform> normTr;
const bool do_norm;
Matx33d _T1, _T2;
Ptr<NormTransform> 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<int> &sample, int sample_size, std::vector<Mat>
&models, const std::vector<double> &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<double> 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>{ Mat_<double>(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>{ Mat_<double>(3,3) };
// extract the last null-vector
Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)models[0].data) = Eigen::JacobiSVD
<Eigen::Matrix<double, 9, 9>> ((Eigen::Matrix<double, 9, 9>(AtA)),
Eigen::ComputeFullV).matrixV().col(8);
#else
Matx<double, 9, 9> AtA_(AtA), U, Vt;
Vec<double, 9> W;
SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A);
models = std::vector<Mat> { Mat_<double>(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<bool> &/*mask*/, std::vector<Mat> &/*models*/,
const std::vector<double> &/*weights*/) override {
return 0;
}
int getMinimumRequiredSampleSize() const override { return 8; }
int getMaxNumberOfSolutions () const override { return 1; }
};
Ptr<EpipolarNonMinimalSolver> EpipolarNonMinimalSolver::create(const Mat &points_, bool is_fundamental) {
return makePtr<EpipolarNonMinimalSolverImpl>(points_, is_fundamental);
}
Ptr<EpipolarNonMinimalSolver> EpipolarNonMinimalSolver::create(const Mat &points_, const Matx33d &T1, const Matx33d &T2, bool use_ge) {
return makePtr<EpipolarNonMinimalSolverImpl>(points_, T1, T2, use_ge);
}
class CovarianceEpipolarSolverImpl : public CovarianceEpipolarSolver {
private:
Mat norm_pts;
Matx33d T1, T2;
float * norm_points;
std::vector<bool> 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<bool>(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<int> sample(points_size);
for (int i = 0; i < points_size; i++) sample[i] = i;
const Ptr<NormTransform> 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<bool>(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<bool> &new_mask, std::vector<Mat> &models,
const std::vector<double> &/*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>{ Mat_<double>(3,3) };
const Eigen::JacobiSVD<Eigen::Matrix<double, 9, 9>> svd((Eigen::Matrix<double, 9, 9>(AtA)),
Eigen::ComputeFullV);
// extract the last nullspace
Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)models[0].data) = svd.matrixV().col(8);
// extract the last null-vector
Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)models[0].data) = Eigen::JacobiSVD
<Eigen::Matrix<double, 9, 9>> ((Eigen::Matrix<double, 9, 9>(covariance)),
Eigen::ComputeFullV).matrixV().col(8);
#else
Matx<double, 9, 9> AtA_(AtA), U, Vt;
Vec<double, 9> W;
SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A);
models = std::vector<Mat> { Mat_<double>(3, 3, Vt.val + 72 /*=8*9*/) };
Matx<double, 9, 9> AtA_(covariance), U, Vt;
Vec<double, 9> W;
SVD::compute(AtA_, W, U, Vt, SVD::FULL_UV + SVD::MODIFY_A);
models = std::vector<Mat> { Mat_<double>(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> CovarianceEpipolarSolver::create (const Mat &points, bool is_fundamental) {
return makePtr<CovarianceEpipolarSolverImpl>(points, is_fundamental);
}
Ptr<CovarianceEpipolarSolver> CovarianceEpipolarSolver::create (const Mat &points, const Matx33d &T1, const Matx33d &T2) {
return makePtr<CovarianceEpipolarSolverImpl>(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<int> &sample, int sample_size, std::vector<Mat>
&models, const std::vector<double> &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> { Mat(model_new) };
return 1;
}
int getMinimumRequiredSampleSize() const override { return 8; }
int getMaxNumberOfSolutions () const override { return 1; }
Ptr<NonMinimalSolver> clone () const override {
return makePtr<FundamentalNonMinimalSolverImpl>(*points_mat);
int estimate (const std::vector<int>&, int, std::vector<Mat>&, const std::vector<double>&) const override {
return 0;
}
int estimate (const std::vector<bool> &/*mask*/, std::vector<Mat> &/*models*/,
const std::vector<double> &/*weights*/) override {
return 0;
}
void enforceRankConstraint (bool /*enforce*/) override {}
int getMinimumRequiredSampleSize() const override { return 5; }
int getMaxNumberOfSolutions () const override { return 1; }
};
Ptr<FundamentalNonMinimalSolver> FundamentalNonMinimalSolver::create(const Mat &points_) {
return makePtr<FundamentalNonMinimalSolverImpl>(points_);
Ptr<LarssonOptimizer> LarssonOptimizer::create(const Mat &calib_points_, const Matx33d &K1, const Matx33d &K2, int max_iters_, bool is_fundamental) {
return makePtr<LarssonOptimizerImpl>(calib_points_, K1, K2, max_iters_, is_fundamental);
}
}}

225
modules/calib3d/src/usac/gamma_values.cpp
View File

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

559
modules/calib3d/src/usac/homography_solver.cpp
View File

@ -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<int>& sample, std::vector<Mat> &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>{ Mat_<double>(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>{ Mat_<double>(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<double, 8, 9> A_svd(&A[0]);
SVD::compute(A_svd, D, U, Vt, SVD::FULL_UV+SVD::MODIFY_A);
models = std::vector<Mat> { Vt.row(Vt.rows-1).reshape(0, 3) };
}
return 1;
}
int getMaxNumberOfSolutions () const override { return 1; }
int getSampleSize() const override { return 4; }
Ptr<MinimalSolver> clone () const override {
return makePtr<HomographyMinimalSolver4ptsGEMImpl>(*points_mat);
}
};
Ptr<HomographyMinimalSolver4ptsGEM> HomographyMinimalSolver4ptsGEM::create(const Mat &points_) {
return makePtr<HomographyMinimalSolver4ptsGEMImpl>(points_);
Ptr<HomographyMinimalSolver4pts> HomographyMinimalSolver4pts::create(const Mat &points, bool use_ge) {
return makePtr<HomographyMinimalSolver4ptsImpl>(points, use_ge);
}
class HomographyNonMinimalSolverImpl : public HomographyNonMinimalSolver {
private:
const Mat * points_mat;
const Ptr<NormTransform> normTr;
const bool do_norm, use_ge;
Ptr<NormTransform> 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<int> &sample, int sample_size, std::vector<Mat> &models,
const std::vector<double> &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<double> 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_<double>(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_<double>(3,3);
// extract the last null-vector
Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)H.data) = Eigen::Matrix<double, 9, 9>
(Eigen::HouseholderQR<Eigen::Matrix<double, 9, 9>> (
(Eigen::Matrix<double, 9, 9> (AtA))).householderQ()).col(8);
#else
Matx<double, 9, 9> Vt;
Vec<double, 9> D;
if (! eigen(Matx<double, 9, 9>(AtA), D, Vt)) return 0;
H = Mat_<double>(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>{ 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<bool> &/*mask*/, std::vector<Mat> &/*models*/,
const std::vector<double> &/*weights*/) override {
return 0;
}
int getMinimumRequiredSampleSize() const override { return 4; }
int getMaxNumberOfSolutions () const override { return 1; }
void enforceRankConstraint (bool /*enforce*/) override {}
};
Ptr<HomographyNonMinimalSolver> HomographyNonMinimalSolver::create(const Mat &points_, bool use_ge_) {
return makePtr<HomographyNonMinimalSolverImpl>(points_, use_ge_);
}
Ptr<HomographyNonMinimalSolver> HomographyNonMinimalSolver::create(const Mat &points_, const Matx33d &T1, const Matx33d &T2, bool use_ge) {
return makePtr<HomographyNonMinimalSolverImpl>(points_, T1, T2, use_ge);
}
class CovarianceHomographySolverImpl : public CovarianceHomographySolver {
private:
Mat norm_pts;
Matx33d T1, T2;
float * norm_points;
std::vector<bool> 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<bool>(points_size, false);
}
explicit CovarianceHomographySolverImpl (const Mat &points_) {
points_size = points_.rows;
// normalize points
std::vector<int> sample(points_size);
for (int i = 0; i < points_size; i++) sample[i] = i;
const Ptr<NormTransform> 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<bool>(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<bool> &new_mask, std::vector<Mat> &models,
const std::vector<double> &/*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_<double>(3,3);
Eigen::HouseholderQR<Eigen::Matrix<double, 9, 9>> qr((Eigen::Matrix<double, 9, 9> (AtA)));
const Eigen::Matrix<double, 9, 9> &Q = qr.householderQ();
// extract the last nullspace
Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)H.data) = Q.col(8);
// extract the last null-vector
Eigen::Map<Eigen::Matrix<double, 9, 1>>((double *)H.data) = Eigen::Matrix<double, 9, 9>
(Eigen::HouseholderQR<Eigen::Matrix<double, 9, 9>> (
(Eigen::Matrix<double, 9, 9> (covariance))).householderQ()).col(8);
#else
Matx<double, 9, 9> Vt;
Vec<double, 9> D;
if (! eigen(Matx<double, 9, 9>(AtA), D, Vt)) return 0;
Mat H = Mat_<double>(3, 3, Vt.val + 72/*=8*9*/);
Matx<double, 9, 9> Vt;
Vec<double, 9> D;
if (! eigen(Matx<double, 9, 9>(covariance), D, Vt)) return 0;
Mat H = Mat_<double>(3, 3, Vt.val + 72/*=8*9*/);
#endif
models = std::vector<Mat>{ T2.inv() * H * T1 };
const auto * const h = (double *) H.data;
// H = T2^-1 H T1
models = std::vector<Mat>{ 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<NonMinimalSolver> clone () const override {
return makePtr<HomographyNonMinimalSolverImpl>(*points_mat);
}
};
Ptr<HomographyNonMinimalSolver> HomographyNonMinimalSolver::create(const Mat &points_) {
return makePtr<HomographyNonMinimalSolverImpl>(points_);
Ptr<CovarianceHomographySolver> CovarianceHomographySolver::create (const Mat &points) {
return makePtr<CovarianceHomographySolverImpl>(points);
}
Ptr<CovarianceHomographySolver> CovarianceHomographySolver::create (const Mat &points, const Matx33d &T1, const Matx33d &T2) {
return makePtr<CovarianceHomographySolverImpl>(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<MinimalSolver> clone () const override {
return makePtr<AffineMinimalSolverImpl>(*points_mat);
}
};
Ptr<AffineMinimalSolver> AffineMinimalSolver::create(const Mat &points_) {
return makePtr<AffineMinimalSolverImpl>(points_);
@ -243,22 +436,32 @@ Ptr<AffineMinimalSolver> AffineMinimalSolver::create(const Mat &points_) {
class AffineNonMinimalSolverImpl : public AffineNonMinimalSolver {
private:
const Mat * points_mat;
const float * const points;
// const NormTransform<double> norm_transform;
Ptr<NormTransform> 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<int> &sample, int sample_size, std::vector<Mat> &models,
const std::vector<double> &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>{ 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<bool> &/*mask*/, std::vector<Mat> &/*models*/,
const std::vector<double> &/*weights*/) override {
return 0;
}
void enforceRankConstraint (bool /*enforce*/) override {}
int getMinimumRequiredSampleSize() const override { return 3; }
int getMaxNumberOfSolutions () const override { return 1; }
Ptr<NonMinimalSolver> clone () const override {
return makePtr<AffineNonMinimalSolverImpl>(*points_mat);
}
};
Ptr<AffineNonMinimalSolver> AffineNonMinimalSolver::create(const Mat &points_) {
return makePtr<AffineNonMinimalSolverImpl>(points_);
Ptr<AffineNonMinimalSolver> AffineNonMinimalSolver::create(const Mat &points_, InputArray T1, InputArray T2) {
return makePtr<AffineNonMinimalSolverImpl>(points_, T1, T2);
}
class CovarianceAffineSolverImpl : public CovarianceAffineSolver {
private:
Mat norm_pts;
Matx33d T1, T2;
float * norm_points;
std::vector<bool> 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<bool>(points_size, false);
}
explicit CovarianceAffineSolverImpl (const Mat &points_) {
points_size = points_.rows;
// normalize points
std::vector<int> sample(points_size);
for (int i = 0; i < points_size; i++) sample[i] = i;
const Ptr<NormTransform> 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<bool>(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<bool> &new_mask, std::vector<Mat> &models,
const std::vector<double> &) 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>{ 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> CovarianceAffineSolver::create (const Mat &points, const Matx33d &T1, const Matx33d &T2) {
return makePtr<CovarianceAffineSolverImpl>(points, T1, T2);
}
Ptr<CovarianceAffineSolver> CovarianceAffineSolver::create (const Mat &points) {
return makePtr<CovarianceAffineSolverImpl>(points);
}
}}

547
modules/calib3d/src/usac/local_optimization.cpp
View File

@ -22,14 +22,18 @@ protected:
std::vector<double> energies, weights;
std::set<int> used_edges;
std::vector<Mat> gc_models;
Ptr<Termination> 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> &estimator_, const Ptr<Error> &error_, const Ptr<Quality> &quality_,
GraphCutImpl (const Ptr<Estimator> &estimator_, const Ptr<Quality> &quality_,
const Ptr<NeighborhoodGraph> &neighborhood_graph_, const Ptr<RandomGenerator> &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> 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<double> 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<LocalOptimization> clone(int state) const override {
return makePtr<GraphCutImpl>(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> GraphCut::create(const Ptr<Estimator> &estimator_, const Ptr<Error> &error_,
Ptr<GraphCut> GraphCut::create(const Ptr<Estimator> &estimator_,
const Ptr<Quality> &quality_, const Ptr<NeighborhoodGraph> &neighborhood_graph_,
const Ptr<RandomGenerator> &lo_sampler_, double threshold_,
double spatial_coherence_term, int gc_inner_iteration_number) {
return makePtr<GraphCutImpl>(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> termination_) {
return makePtr<GraphCutImpl>(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> estimator;
@ -197,23 +196,18 @@ private:
double threshold, new_threshold, threshold_step;
std::vector<double> weights;
public:
InnerIterativeLocalOptimizationImpl (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_=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<Mat>(estimator->getMaxNumSolutionsNonMinimal());
// Allocate max memory to avoid reallocation
@ -327,12 +320,6 @@ public:
}
return true;
}
Ptr<LocalOptimization> clone(int state) const override {
return makePtr<InnerIterativeLocalOptimizationImpl>(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> InnerIterativeLocalOptimization::create
(const Ptr<Estimator> &estimator_, const Ptr<Quality> &quality_,
@ -345,293 +332,275 @@ Ptr<InnerIterativeLocalOptimization> InnerIterativeLocalOptimization::create
lo_inner_iterations_, lo_iter_max_iterations_, threshold_multiplier_);
}
class SigmaConsensusImpl : public SigmaConsensus {
class SimpleLocalOptimizationImpl : public SimpleLocalOptimization {
private:
const Ptr<Estimator> estimator;
const Ptr<Quality> quality;
const Ptr<Error> error;
const Ptr<ModelVerifier> 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<double> sqr_residuals, sigma_weights;
std::vector<int> sqr_residuals_idxs;
// Models fit by weighted least-squares fitting
std::vector<Mat> sigma_models;
// Points used in the weighted least-squares fitting
std::vector<int> 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<double> &stored_gamma_values;
const Ptr<NonMinimalSolver> estimator;
const Ptr<Termination> termination;
const Ptr<RandomGenerator> random_generator;
const Ptr<WeightFunction> 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<RandomGenerator> random_generator_smaller_subset;
int points_size, max_lo_iters, non_min_sample_size, current_ransac_iter;
std::vector<double> weights;
std::vector<int> inliers;
std::vector<cv::Mat> models;
double inlier_threshold_sqr;
int num_lo_optimizations = 0;
bool updated_lo = false;
public:
SigmaConsensusImpl (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) : 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<double>(points_size);
sqr_residuals_idxs = std::vector<int>(points_size);
sigma_inliers = std::vector<int>(points_size);
max_lo_sample_size = max_lo_sample_size_;
sigma_weights = std::vector<double>(points_size);
sigma_models = std::vector<Mat>(estimator->getMaxNumSolutionsNonMinimal());
stored_gamma_number_min1 = gamma_generator.getTableSize()-1;
scale_of_stored_gammas = gamma_generator.getScaleOfGammaValues();
SimpleLocalOptimizationImpl (const Ptr<Quality> &quality_, const Ptr<NonMinimalSolver> &estimator_,
const Ptr<Termination> termination_, const Ptr<RandomGenerator> &random_gen, Ptr<WeightFunction> 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<int>(quality_->getPointsSize());
models = std::vector<cv::Mat>(estimator_->getMaxNumberOfSolutions());
points_size = quality_->getPointsSize();
inlier_threshold_sqr = inlier_threshold_sqr_;
if (weight_fnc != nullptr) weights = std::vector<double>(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<float> &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<LocalOptimization> clone(int state) const override {
return makePtr<SigmaConsensusImpl>(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> SimpleLocalOptimization::create (const Ptr<Quality> &quality_,
const Ptr<NonMinimalSolver> &estimator_, const Ptr<Termination> termination_, const Ptr<RandomGenerator> &random_gen,
const Ptr<WeightFunction> weight_fnc, int max_lo_iters_, double inlier_thr_sqr, bool updated_lo) {
return makePtr<SimpleLocalOptimizationImpl> (quality_, estimator_, termination_, random_gen, weight_fnc, max_lo_iters_, inlier_thr_sqr, updated_lo);
}
class MagsacWeightFunctionImpl : public MagsacWeightFunction {
private:
const std::vector<double> &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<GammaValues> &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<unsigned int>(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<float> &errors, std::vector<int> &inliers, std::vector<double> &weights) const override {
return getInliersWeights(errors, inliers, weights, one_over_sigma, rescale_err, max_sigma_sqr);
}
int getInliersWeights (const std::vector<float> &errors, std::vector<int> &inliers, std::vector<double> &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<float> &errors, std::vector<int> &inliers, std::vector<double> &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<unsigned int>(_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>
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) {
return makePtr<SigmaConsensusImpl>(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> MagsacWeightFunction::create(const Ptr<GammaValues> &gamma_generator_,
int DoF_, double upper_incomplete_of_sigma_quantile, double C_, double max_sigma_) {
return makePtr<MagsacWeightFunctionImpl>(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> estimator;
const Ptr<Quality> quality;
int lsq_iterations;
std::vector<int> inliers;
const Ptr<NonMinimalSolver> solver;
const Ptr<Error> error_fnc;
const Ptr<WeightFunction> weight_fnc;
std::vector<bool> mask, mask_best;
std::vector<Mat> models;
std::vector<double> weights;
std::vector<float> errors_best;
std::vector<int> inliers;
double threshold, iou_thr, max_thr;
int max_iters, points_size;
bool is_covariance, CHANGE_WEIGHTS = true;
public:
LeastSquaresPolishingImpl(const Ptr<Estimator> &estimator_, const Ptr<Quality> &quality_,
int lsq_iterations_) :
estimator(estimator_), quality(quality_) {
lsq_iterations = lsq_iterations_;
// allocate memory for inliers array and models
inliers = std::vector<int>(quality_->getPointsSize());
models = std::vector<Mat>(estimator->getMaxNumSolutionsNonMinimal());
NonMinimalPolisherImpl (const Ptr<Quality> &quality_, const Ptr<NonMinimalSolver> &solver_,
Ptr<WeightFunction> 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<const cv::usac::CovarianceSolver*>(solver_.get()) != nullptr;
mask = std::vector<bool>(points_size);
mask_best = std::vector<bool>(points_size);
inliers = std::vector<int>(points_size);
if (weight_fnc != nullptr) {
weights = std::vector<double>(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<double>(out_score.inlier_number) - static_cast<double>
(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> LeastSquaresPolishing::create (const Ptr<Estimator> &estimator_,
const Ptr<Quality> &quality_, int lsq_iterations_) {
return makePtr<LeastSquaresPolishingImpl>(estimator_, quality_, lsq_iterations_);
Ptr<NonMinimalPolisher> NonMinimalPolisher::create(const Ptr<Quality> &quality_, const Ptr<NonMinimalSolver> &solver_,
Ptr<WeightFunction> weight_fnc_, int max_iters_, double iou_thr_) {
return makePtr<NonMinimalPolisherImpl>(quality_, solver_, weight_fnc_, max_iters_, iou_thr_);
}
}}

62
modules/calib3d/src/usac/pnp_solver.cpp
View File

@ -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<int> &sample, std::vector<Mat> &models) const override {
std::vector<double> A1 (60, 0), A2(56, 0); // 5x12, 7x8
// std::vector<double> 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<Mat>{P};
return 1;
}
Ptr<MinimalSolver> clone () const override {
return makePtr<PnPMinimalSolver6PtsImpl>(*points_mat);
}
};
Ptr<PnPMinimalSolver6Pts> PnPMinimalSolver6Pts::create(const Mat &points_) {
return makePtr<PnPMinimalSolver6PtsImpl>(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<int> &sample, int sample_size,
std::vector<Mat> &models, const std::vector<double> &weights) const override {
@ -236,7 +237,7 @@ public:
models = std::vector<Mat>{ Mat_<double>(3,4) };
Eigen::HouseholderQR<Eigen::Matrix<double, 12, 12>> qr((Eigen::Matrix<double, 12, 12>(AtA)));
const Eigen::Matrix<double, 12, 12> &Q = qr.householderQ();
// extract the last nullspace
// extract the last null-vector
Eigen::Map<Eigen::Matrix<double, 12, 1>>((double *)models[0].data) = Q.col(11);
#else
Matx<double, 12, 12> Vt;
@ -246,17 +247,37 @@ public:
#endif
return 1;
}
int estimate (const std::vector<bool> &/*mask*/, std::vector<Mat> &/*models*/,
const std::vector<double> &/*weights*/) override {
return 0;
}
void enforceRankConstraint (bool /*enforce*/) override {}
int getMinimumRequiredSampleSize() const override { return 6; }
int getMaxNumberOfSolutions () const override { return 1; }
Ptr<NonMinimalSolver> clone () const override {
return makePtr<PnPNonMinimalSolverImpl>(*points_mat);
}
};
Ptr<PnPNonMinimalSolver> PnPNonMinimalSolver::create(const Mat &points) {
return makePtr<PnPNonMinimalSolverImpl>(points);
}
class PnPSVDSolverImpl : public PnPSVDSolver {
private:
std::vector<double> w;
Ptr<PnPNonMinimalSolver> 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<int> &sample, std::vector<Mat> &models) const override {
return pnp_solver->estimate(sample, 6, models, w);
}
};
Ptr<PnPSVDSolver> PnPSVDSolver::create(const Mat &points_) {
return makePtr<PnPSVDSolverImpl>(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<int> &sample, std::vector<Mat> &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<MinimalSolver> clone () const override {
return makePtr<P3PSolverImpl>(*points_mat, *calib_norm_points_mat, *K_mat);
}
};
Ptr<P3PSolver> P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Matx33d &K) {
Ptr<P3PSolver> P3PSolver::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) {
return makePtr<P3PSolverImpl>(points_, calib_norm_pts, K);
}
}}

566
modules/calib3d/src/usac/quality.cpp
View File

@ -25,6 +25,27 @@ int Quality::getInliers(const Ptr<Error> &error, const Mat &model, std::vector<b
}
return num_inliers;
}
int Quality::getInliers (const std::vector<float> &errors, std::vector<bool> &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<float> &errors, std::vector<int> &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<double>(inlier_number));
return {inlier_number, -static_cast<double>(inlier_number)};
}
Score getScore (const std::vector<float> &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<double>(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<bool> &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<Quality> clone () const override {
return makePtr<RansacQualityImpl>(points_size, threshold, error->clone());
}
Ptr<Error> getErrorFnc () const override { return error; }
};
Ptr<RansacQuality> RansacQuality::create(int points_size_, double threshold_,
@ -77,13 +105,13 @@ class MsacQualityImpl : public MsacQuality {
protected:
const Ptr<Error> 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 (error_), points_size (points_size_), threshold (threshold_) {
MsacQualityImpl (int points_size_, double threshold_, const Ptr<Error> &error_, double k_msac_)
: error (error_), points_size (points_size_), threshold (threshold_), k_msac(k_msac_) {
best_score = std::numeric_limits<double>::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<float> &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<bool> &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<Quality> clone () const override {
return makePtr<MsacQualityImpl>(points_size, threshold, error->clone());
}
Ptr<Error> getErrorFnc () const override { return error; }
};
Ptr<MsacQuality> MsacQuality::create(int points_size_, double threshold_,
const Ptr<Error> &error_) {
return makePtr<MsacQualityImpl>(points_size_, threshold_, error_);
const Ptr<Error> &error_, double k_msac) {
return makePtr<MsacQualityImpl>(points_size_, threshold_, error_, k_msac);
}
class MagsacQualityImpl : public MagsacQuality {
private:
const Ptr<Error> error;
const GammaValues& gamma_generator;
const Ptr<GammaValues> 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<double> &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> &error_,
const Ptr<GammaValues> &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<double>::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<unsigned int>(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<unsigned int>(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<unsigned int>(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<float> &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<unsigned int>(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<bool> &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<Quality> clone () const override {
return makePtr<MagsacQualityImpl>(maximum_sigma, points_size, error->clone(),
tentative_inlier_threshold, degrees_of_freedom,
k, gamma_value_of_k, lower_gamma_value_of_k, C);
}
Ptr<Error> getErrorFnc () const override { return error; }
};
Ptr<MagsacQuality> MagsacQuality::create(double maximum_thr, int points_size_, const Ptr<Error> &error_,
const Ptr<GammaValues> &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<MagsacQualityImpl>(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<MagsacQualityImpl>(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<float> &errors_) const override {
std::vector<float> 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<bool> &inliers_mask) const override
{ return Quality::getInliers(error, model, inliers_mask, threshold); }
Ptr<Quality> clone () const override {
return makePtr<LMedsQualityImpl>(points_size, threshold, error->clone());
}
double getThreshold () const override { return threshold; }
Ptr<Error> getErrorFnc () const override { return error; }
};
Ptr<LMedsQuality> LMedsQuality::create(int points_size_, double threshold_, const Ptr<Error> &error_) {
return makePtr<LMedsQualityImpl>(points_size_, threshold_, error_);
@ -335,47 +335,53 @@ Ptr<LMedsQuality> LMedsQuality::create(int points_size_, double threshold_, cons
class ModelVerifierImpl : public ModelVerifier {
private:
std::vector<float> errors;
Ptr<Quality> 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<float> &getErrors() const override { return errors; }
bool hasErrors () const override { return false; }
Ptr<ModelVerifier> clone (int /*state*/) const override { return makePtr<ModelVerifierImpl>();}
ModelVerifierImpl (const Ptr<Quality> &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> ModelVerifier::create() {
return makePtr<ModelVerifierImpl>();
Ptr<ModelVerifier> ModelVerifier::create(const Ptr<Quality> &quality) {
return makePtr<ModelVerifierImpl>(quality);
}
///////////////////////////////////// SPRT VERIFIER //////////////////////////////////////////
class SPRTImpl : public SPRT {
class AdaptiveSPRTImpl : public AdaptiveSPRT {
private:
RNG rng;
const Ptr<Error> err;
const Ptr<Quality> 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_history> sprt_histories;
std::vector<SPRT_history> sprt_histories, empty;
std::vector<int> points_random_pool;
std::vector<float> 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<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_) : 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> &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<int> (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<double>::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<float>(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<float>(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<double>(tested_inliers);
else if (score_type == ScoreMethod::SCORE_METHOD_LMEDS)
score.score = Utils::findMedian(errors);
const double new_epsilon = static_cast<double>(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<double> (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<double>(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<float> &getErrors () const override { return errors; }
const std::vector<SPRT_history> &getSPRTvector () const override { return sprt_histories; }
void update (int highest_inlier_number_) override {
const double new_epsilon = static_cast<double>(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<ModelVerifier> clone (int state) const override {
return makePtr<SPRTImpl>(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<SPRT_history> &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<double>(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<int>(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<double,double> 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> 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_) {
return makePtr<SPRTImpl>(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> AdaptiveSPRT::create (int state, const Ptr<Quality> &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<AdaptiveSPRTImpl>(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);
}
}}

1596
modules/calib3d/src/usac/ransac_solvers.cpp
View File

File diff suppressed because it is too large Load Diff

30
modules/calib3d/src/usac/sampler.cpp
View File

@ -40,9 +40,6 @@ public:
points_random_pool[--random_pool_size]);
}
}
Ptr<Sampler> clone (int state) const override {
return makePtr<UniformSamplerImpl>(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<Sampler> clone (int state) const override {
return makePtr<ProsacSimpleSamplerImpl>(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<Sampler> clone (int state) const override {
return makePtr<ProsacSamplerImpl>(state, points_size, sample_size,
growth_max_samples);
}
};
Ptr<ProsacSampler> 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<Sampler> clone (int state) const override {
return makePtr<ProgressiveNapsacImpl>(state, points_size, sample_size, *layers,
sampler_length);
}
};
Ptr<ProgressiveNapsac> ProgressiveNapsac::create(int state, int points_size_, int sample_size_,
const std::vector<Ptr<NeighborhoodGraph>> &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<Sampler> clone (int state) const override {
return makePtr<NapsacSamplerImpl>(state, points_size, sample_size, neighborhood_graph);
}
};
Ptr<NapsacSampler> NapsacSampler::create(int state, int points_size_, int sample_size_,
const Ptr<NeighborhoodGraph> &neighborhood_graph_) {

148
modules/calib3d/src/usac/termination.cpp
View File

@ -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<double>(inlier_number) / points_size, sample_size));
@ -40,9 +41,11 @@ public:
return MAX_ITERATIONS;
}
Ptr<TerminationCriteria> clone () const override {
return makePtr<StandardTerminationCriteriaImpl>(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<double>(inlier_number)/points_size, sample_size));
if (std::isinf(pred_iters))
return INT_MAX;
return (int) pred_iters + 1;
}
};
Ptr<StandardTerminationCriteria> StandardTerminationCriteria::create(double confidence,
@ -54,13 +57,13 @@ Ptr<StandardTerminationCriteria> StandardTerminationCriteria::create(double conf
/////////////////////////////////////// SPRT TERMINATION //////////////////////////////////////////
class SPRTTerminationImpl : public SPRTTermination {
private:
const std::vector<SPRT_history> &sprt_histories;
const Ptr<AdaptiveSPRT> sprt;
const double log_eta_0;
const int points_size, sample_size, MAX_ITERATIONS;
public:
SPRTTerminationImpl (const std::vector<SPRT_history> &sprt_histories_, double confidence,
SPRTTerminationImpl (const Ptr<AdaptiveSPRT> &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<double>(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<int>(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<TerminationCriteria> clone () const override {
return makePtr<SPRTTerminationImpl>(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> SPRTTermination::create(const std::vector<SPRT_history> &sprt_histories_,
Ptr<SPRTTermination> SPRTTermination::create(const Ptr<AdaptiveSPRT> &sprt_,
double confidence, int points_size_, int sample_size_, int max_iterations_) {
return makePtr<SPRTTerminationImpl>(sprt_histories_, confidence, points_size_, sample_size_,
return makePtr<SPRTTerminationImpl>(sprt_, confidence, points_size_, sample_size_,
max_iterations_);
}
@ -167,21 +165,20 @@ Ptr<SPRTTermination> SPRTTermination::create(const std::vector<SPRT_history> &sp
class SPRTPNapsacTerminationImpl : public SPRTPNapsacTermination {
private:
SPRTTerminationImpl sprt_termination;
const std::vector<SPRT_history> &sprt_histories;
const double relax_coef, log_confidence;
const int points_size, sample_size, MAX_ITERS;
public:
SPRTPNapsacTerminationImpl (const std::vector<SPRT_history> &sprt_histories_,
SPRTPNapsacTerminationImpl (const Ptr<AdaptiveSPRT> &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<double>(inlier_number) / points_size + relax_coef;
@ -192,52 +189,41 @@ public:
if (! std::isinf(predicted_iters) && predicted_iters < predicted_iterations)
return static_cast<int>(predicted_iters);
return predicted_iterations;
}
Ptr<TerminationCriteria> clone () const override {
return makePtr<SPRTPNapsacTerminationImpl>(sprt_histories, 1-exp(log_confidence),
points_size, sample_size, MAX_ITERS, relax_coef);
return std::min(MAX_ITERS, predicted_iterations);
}
};
Ptr<SPRTPNapsacTermination> SPRTPNapsacTermination::create(const std::vector<SPRT_history>&
sprt_histories_, double confidence, int points_size_, int sample_size_,
Ptr<SPRTPNapsacTermination> SPRTPNapsacTermination::create(const Ptr<AdaptiveSPRT> &
sprt, double confidence, int points_size_, int sample_size_,
int max_iterations_, double relax_coef_) {
return makePtr<SPRTPNapsacTerminationImpl>(sprt_histories_, confidence, points_size_,
return makePtr<SPRTPNapsacTerminationImpl>(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<ProsacSampler> sampler;
std::vector<int> non_random_inliers;
const Ptr<Error> error;
public:
ProsacTerminationCriteriaImpl (const Ptr<Error> &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<ProsacSampler> &sampler_,const Ptr<Error> &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<int> &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<int>(points_size, 0);
std::vector<double> pn_i_arr(points_size);
std::vector<double> 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<int> &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<double>(num_inliers_under_termination_len)
const double new_max_samples = log_conf/log(1-pow(static_cast<double>(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<int>(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<double>(inliers_size) / points_size, sample_size));
if (! std::isinf(predicted_iters) && predicted_iters < predicted_iterations)
return static_cast<int>(predicted_iters);
return predicted_iterations;
}
Ptr<TerminationCriteria> clone () const override {
return makePtr<ProsacTerminationCriteriaImpl>(error->clone(),
points_size, sample_size, 1-exp(log_confidence), MAX_ITERATIONS,
min_termination_length, beta, non_randomness_phi, inlier_threshold);
}
};
Ptr<ProsacTerminationCriteria>
ProsacTerminationCriteria::create(const Ptr<ProsacSampler> &sampler, const Ptr<Error> &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<int> &non_rand_inliers) {
return makePtr<ProsacTerminationCriteriaImpl> (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);
}
}}

514
modules/calib3d/src/usac/utils.cpp
View File

@ -8,9 +8,234 @@
#include <map>
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<double> &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<double> 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<int,int> 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<double>(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 &quotient,*/ 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<double>(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<Poly> &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<Poly> &sturm, double min, double max,
int sign_changes_at_min, int sign_changes_at_max, std::vector<double> &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<double> &coeffs, std::vector<double> &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<std::pair<int,int>> 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<Poly> 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> SolverPoly::create() {
return makePtr<SolvePoly>();
}
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<int> &sorted_mask) {
// mask of sorted points (array of indexes)
const int points_size = points.rows, dim = points.cols;
sorted_mask = std::vector<int >(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<double> sum_knn_distances (points_size, 0);
for (int p = 0; p < points_size; p++) {
const std::vector<double> &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<double> &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<double> &a, int m, int n) {
return true;
}
double Utils::intersectionOverUnion (const std::vector<bool> &a, const std::vector<bool> &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<int>(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<int>& 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<RandomGenerator> clone (int state) const override {
return makePtr<UniformRandomGeneratorImpl>(state, max_range, subset_size);
}
};
Ptr<UniformRandomGenerator> UniformRandomGenerator::create (int state) {
@ -338,19 +644,17 @@ float Utils::findMedian (std::vector<float> &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<std::vector<int>> 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<int>(n.size()-1);
int j = 0;
@ -373,7 +679,7 @@ public:
pt++;
}
}
const std::vector<std::vector<int>> &getGraph () const override { return graph; }
inline const std::vector<int> &getNeighbors(int point_idx) const override {
return graph[point_idx];
}
@ -384,9 +690,7 @@ Ptr<RadiusSearchNeighborhoodGraph> RadiusSearchNeighborhoodGraph::create (const
flann_search_params, num_kd_trees);
}
///////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// FLANN Graph /////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////
class FlannNeighborhoodGraphImpl : public FlannNeighborhoodGraph {
private:
std::vector<std::vector<int>> graph;
@ -425,6 +729,7 @@ public:
const std::vector<double>& getNeighborsDistances (int idx) const override {
return distances[idx];
}
const std::vector<std::vector<int>> &getGraph () const override { return graph; }
inline const std::vector<int> &getNeighbors(int point_idx) const override {
// CV_Assert(point_idx_ < num_vertices);
return graph[point_idx];
@ -438,9 +743,7 @@ Ptr<FlannNeighborhoodGraph> 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<CellCoord, std::vector<int >> neighbors_map;
std::vector<std::vector<int>> 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<CellCoord, std::vector<int >> neighbors_map;
const auto * const container = (float *) container_.data;
// <int, int, int, int> -> {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<int>(cell.second.size());
// only one point in cell -> no neighbors
if (neighbors_in_cell < 2) continue;
@ -510,7 +812,7 @@ public:
}
}
}
const std::vector<std::vector<int>> &getGraph () const override { return graph; }
inline const std::vector<int> &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> GridNeighborhoodGraph::create(const Mat &points,
return makePtr<GridNeighborhoodGraphImpl>(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<std::vector<int>> 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<CellCoord, std::vector<int >> 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<std::vector<int>>(points_size);
// store neighbors cells into graph (2D vector)
for (const auto &cell : neighbors_map1) {
const int neighbors_in_cell = static_cast<int>(cell.second.size());
// only one point in cell -> no neighbors
if (neighbors_in_cell < 2) continue;
const std::vector<int> &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<int> &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<std::vector<int>> &getGraph () const override { return graph; }
inline const std::vector<int> &getNeighbors(int point_idx) const override {
// Note, neighbors vector also includes point_idx!
return graph[point_idx];
}
};
Ptr<GridNeighborhoodGraph2Images> 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<GridNeighborhoodGraph2ImagesImpl>(points, points_size,
cell_size_x_img1_, cell_size_y_img1_, cell_size_x_img2_, cell_size_y_img2_);
}
}}

13
modules/calib3d/test/test_usac.cpp
View File

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