diff --git a/doc/tutorials/calib3d/usac.markdown b/doc/tutorials/calib3d/usac.markdown
new file mode 100644
index 0000000000..27d590be3a
--- /dev/null
+++ b/doc/tutorials/calib3d/usac.markdown
@@ -0,0 +1,307 @@
+---
+author:
+- Maksym Ivashechkin
+bibliography: 'bibs.bib'
+csl: 'acm-sigchi-proceedings.csl'
+date: August 2020
+title: 'Google Summer of Code: Improvement of Random Sample Consensus in OpenCV'
+...
+
+Contribution
+============
+
+The integrated part to OpenCV `calib3d` module is RANSAC-based universal
+framework USAC (`namespace usac`) written in C++. The framework includes
+different state-of-the-arts methods for sampling, verification or local
+optimization. The main advantage of the framework is its independence to
+any estimation problem and modular structure. Therefore, new solvers or
+methods can be added/removed easily. So far it includes the following
+components:
+
+1. Sampling method:
+
+ 1. Uniform – standard RANSAC sampling proposed in \[8\] which draw
+ minimal subset independently uniformly at random. *The default
+ option in proposed framework*.
+
+ 2. PROSAC – method \[4\] that assumes input data points sorted by
+ quality so sampling can start from the most promising points.
+ Correspondences for this method can be sorted e.g., by ratio of
+ descriptor distances of the best to second match obtained from
+ SIFT detector. *This is method is recommended to use because it
+ can find good model and terminate much earlier*.
+
+ 3. NAPSAC – sampling method \[10\] which takes initial point
+ uniformly at random and the rest of points for minimal sample in
+ the neighborhood of initial point. This is method can be
+ potentially useful when models are localized. For example, for
+ plane fitting. However, in practise struggles from degenerate
+ issues and defining optimal neighborhood size.
+
+ 4. Progressive-NAPSAC – sampler \[2\] which is similar to NAPSAC,
+ although it starts from local and gradually converges to
+ global sampling. This method can be quite useful if local models
+ are expected but distribution of data can be arbitrary. The
+ implemented version assumes data points to be sorted by quality
+ as in PROSAC.
+
+2. Score Method. USAC as well as standard RANSAC finds model which
+ minimizes total loss. Loss can be represented by following
+ functions:
+
+ 1. RANSAC – binary 0 / 1 loss. 1 for outlier, 0 for inlier. *Good
+ option if the goal is to find as many inliers as possible.*
+
+ 2. MSAC – truncated squared error distance of point to model. *The
+ default option in framework*. The model might not have as many
+ inliers as using RANSAC score, however will be more accurate.
+
+ 3. MAGSAC – threshold-free method \[3\] to compute score. Using,
+ although, maximum sigma (standard deviation of noise) level to
+ marginalize residual of point over sigma. Score of the point
+ represents likelihood of point being inlier. *Recommended option
+ when image noise is unknown since method does not require
+ threshold*. However, it is still recommended to provide at least
+ approximated threshold, because termination itself is based on
+ number of points which error is less than threshold. By giving 0
+ threshold the method will output model after maximum number of
+ iterations reached.
+
+ 4. LMeds – the least median of squared error distances. In the
+ framework finding median is efficiently implement with $O(n)$
+ complexity using quick-sort algorithm. Note, LMeds does not have
+ to work properly when inlier ratio is less than 50%, in other
+ cases this method is robust and does not require threshold.
+
+3. Error metric which describes error distance of point to
+ estimated model.
+
+ 1. Re-projection distance – used for affine, homography and
+ projection matrices. For homography also symmetric re-projection
+ distance can be used.
+
+ 2. Sampson distance – used for Fundamental matrix.
+
+ 3. Symmetric Geometric distance – used for Essential matrix.
+
+4. Degeneracy:
+
+ 1. DEGENSAC – method \[7\] which for Fundamental matrix estimation
+ efficiently verifies and recovers model which has at least 5
+ points in minimal sample lying on the dominant plane.
+
+ 2. Collinearity test – for affine and homography matrix estimation
+ checks if no 3 points lying on the line. For homography matrix
+ since points are planar is applied test which checks if points
+ in minimal sample lie on the same side w.r.t. to any line
+ crossing any two points in sample (does not assume reflection).
+
+ 3. Oriented epipolar constraint – method \[6\] for epipolar
+ geometry which verifies model (fundamental and essential matrix)
+ to have points visible in the front of the camera.
+
+5. SPRT verification – method \[9\] which verifies model by its
+ evaluation on randomly shuffled points using statistical properties
+ given by probability of inlier, relative time for estimation,
+ average number of output models etc. Significantly speeding up
+ framework, because bad model can be rejected very quickly without
+ explicitly computing error for every point.
+
+6. Local Optimization:
+
+ 1. Locally Optimized RANSAC – method \[5\] that iteratively
+ improves so-far-the-best model by non-minimal estimation. *The
+ default option in framework. This procedure is the fastest and
+ not worse than others local optimization methods.*
+
+ 2. Graph-Cut RANSAC – method \[1\] that refine so-far-the-best
+ model, however, it exploits spatial coherence of the
+ data points. *This procedure is quite precise however
+ computationally slower.*
+
+ 3. Sigma Consensus – method \[3\] which improves model by applying
+ non-minimal weighted estimation, where weights are computed with
+ the same logic as in MAGSAC score. This method is better to use
+ together with MAGSAC score.
+
+7. Termination:
+
+ 1. Standard – standard equation for independent and
+ uniform sampling.
+
+ 2. PROSAC – termination for PROSAC.
+
+ 3. SPRT – termination for SPRT.
+
+8. Solver. In the framework there are minimal and non-minimal solvers.
+ In minimal solver standard methods for estimation is applied. In
+ non-minimal solver usually the covariance matrix is built and the
+ model is found as the eigen vector corresponding to the highest
+ eigen value.
+
+ 1. Affine2D matrix
+
+ 2. Homography matrix – for minimal solver is used RHO
+ (Gaussian elimination) algorithm from OpenCV.
+
+ 3. Fundamental matrix – for 7-points algorithm two null vectors are
+ found using Gaussian elimination (eliminating to upper
+ triangular matrix and back-substitution) instead of SVD and then
+ solving 3-degrees polynomial. For 8-points solver Gaussian
+ elimination is used too.
+
+ 4. Essential matrix – 4 null vectors are found using
+ Gaussian elimination. Then the solver based on Gröbner basis
+ described in \[11\] is used. Essential matrix can be computed
+ only if LAPACK or
+ Eigen are
+ installed as it requires eigen decomposition with complex
+ eigen values.
+
+ 5. Perspective-n-Point – the minimal solver is classical 3 points
+ with up to 4 solutions. For RANSAC the low number of sample size
+ plays significant role as it requires less iterations,
+ furthermore in average P3P solver has around 1.39
+ estimated models. Also, in new version of `solvePnPRansac(...)`
+ with `UsacParams` there is an options to pass empty intrinsic
+ matrix `InputOutputArray cameraMatrix`. If matrix is empty than
+ using Direct Linear Transformation algorithm (PnP with 6 points)
+ framework outputs not only rotation and translation vector but
+ also calibration matrix.
+
+Also, the framework can be run in parallel. The parallelization is done
+in the way that multiple RANSACs are created and they share two atomic
+variables `bool success` and `int num_hypothesis_tested` which
+determines when all RANSACs must terminate. If one of RANSAC terminated
+successfully then all other RANSAC will terminate as well. In the end
+the best model is synchronized from all threads. If PROSAC sampler is
+used then threads must share the same sampler since sampling is done
+sequentially. However, using default options of framework parallel
+RANSAC is not deterministic since it depends on how often each thread is
+running. The easiest way to make it deterministic is using PROSAC
+sampler without SPRT and Local Optimization and not for Fundamental
+matrix, because they internally use random generators.\
+\
+For NAPSAC, Progressive NAPSAC or Graph-Cut methods is required to build
+a neighborhood graph. In framework there are 3 options to do it:
+
+1. `NEIGH_FLANN_KNN` – estimate neighborhood graph using OpenCV FLANN
+ K nearest-neighbors. The default value for KNN is 7. KNN method may
+ work good for sampling but not good for GC-RANSAC.
+
+2. `NEIGH_FLANN_RADIUS` – similarly as in previous case finds neighbor
+ points which distance is less than 20 pixels.
+
+3. `NEIGH_GRID` – for finding points’ neighborhood tiles points in
+ cells using hash-table. The method is described in \[2\]. Less
+ accurate than `NEIGH_FLANN_RADIUS`, although significantly faster.
+
+Note, `NEIGH_FLANN_RADIUS` and `NEIGH_FLANN_RADIUS` are not able to PnP
+solver, since there are 3D object points.\
+\
+New flags:
+
+1. `USAC_DEFAULT` – has standard LO-RANSAC.
+
+2. `USAC_PARALLEL` – has LO-RANSAC and RANSACs run in parallel.
+
+3. `USAC_ACCURATE` – has GC-RANSAC.
+
+4. `USAC_FAST` – has LO-RANSAC with smaller number iterations in local
+ optimization step. Uses RANSAC score to maximize number of inliers
+ and terminate earlier.
+
+5. `USAC_PROSAC` – has PROSAC sampling. Note, points must be sorted.
+
+6. `USAC_FM_8PTS` – has LO-RANSAC. Only valid for Fundamental matrix
+ with 8-points solver.
+
+7. `USAC_MAGSAC` – has MAGSAC++.
+
+Every flag uses SPRT verification. And in the end the final
+so-far-the-best model is polished by non minimal estimation of all found
+inliers.\
+\
+A few other important parameters:
+
+1. `randomGeneratorState` – since every USAC solver is deterministic in
+ OpenCV (i.e., for the same points and parameters returns the
+ same result) by providing new state it will output new model.
+
+2. `loIterations` – number of iterations for Local Optimization method.
+ *The default value is 10*. By increasing `loIterations` the output
+ model could be more accurate, however, the computationial time may
+ also increase.
+
+3. `loSampleSize` – maximum sample number for Local Optimization. *The
+ default value is 14*. Note, that by increasing `loSampleSize` the
+ accuracy of model can increase as well as the computational time.
+ However, it is recommended to keep value less than 100, because
+ estimation on low number of points is faster and more robust.
+
+Samples:
+
+There are three new sample files in opencv/samples directory.
+
+1. `epipolar_lines.cpp` – input arguments of `main` function are two
+ pathes to images. Then correspondences are found using
+ SIFT detector. Fundamental matrix is found using RANSAC from
+ tentaive correspondences and epipolar lines are plot.
+
+2. `essential_mat_reconstr.cpp` – input arguments are path to data file
+ containing image names and single intrinsic matrix and directory
+ where these images located. Correspondences are found using SIFT.
+ The essential matrix is estimated using RANSAC and decomposed to
+ rotation and translation. Then by building two relative poses with
+ projection matrices image points are triangulated to object points.
+ By running RANSAC with 3D plane fitting object points as well as
+ correspondences are clustered into planes.
+
+3. `essential_mat_reconstr.py` – the same functionality as in .cpp
+ file, however instead of clustering points to plane the 3D map of
+ object points is plot.
+
+References:
+
+1\. Daniel Barath and Jiří Matas. 2018. Graph-Cut RANSAC. In *Proceedings
+of the iEEE conference on computer vision and pattern recognition*,
+6733–6741.
+
+2\. Daniel Barath, Maksym Ivashechkin, and Jiri Matas. 2019. Progressive
+NAPSAC: Sampling from gradually growing neighborhoods. *arXiv preprint
+arXiv:1906.02295*.
+
+3\. Daniel Barath, Jana Noskova, Maksym Ivashechkin, and Jiri Matas.
+2020. MAGSAC++, a fast, reliable and accurate robust estimator. In
+*Proceedings of the iEEE/CVF conference on computer vision and pattern
+recognition (cVPR)*.
+
+4\. O. Chum and J. Matas. 2005. Matching with PROSAC-progressive sample
+consensus. In *Computer vision and pattern recognition*.
+
+5\. O. Chum, J. Matas, and J. Kittler. 2003. Locally optimized RANSAC. In
+*Joint pattern recognition symposium*.
+
+6\. O. Chum, T. Werner, and J. Matas. 2004. Epipolar geometry estimation
+via RANSAC benefits from the oriented epipolar constraint. In
+*International conference on pattern recognition*.
+
+7\. Ondrej Chum, Tomas Werner, and Jiri Matas. 2005. Two-view geometry
+estimation unaffected by a dominant plane. In *2005 iEEE computer
+society conference on computer vision and pattern recognition
+(cVPR’05)*, 772–779.
+
+8\. M. A. Fischler and R. C. Bolles. 1981. Random sample consensus: A
+paradigm for model fitting with applications to image analysis and
+automated cartography. *Communications of the ACM*.
+
+9\. Jiri Matas and Ondrej Chum. 2005. Randomized RANSAC with sequential
+probability ratio test. In *Tenth iEEE international conference on
+computer vision (iCCV’05) volume 1*, 1727–1732.
+
+10\. D. R. Myatt, P. H. S. Torr, S. J. Nasuto, J. M. Bishop, and R.
+Craddock. 2002. NAPSAC: High noise, high dimensional robust estimation.
+In *In bMVC02*, 458–467.
+
+11\. Henrik Stewénius, Christopher Engels, and David Nistér. 2006. Recent
+developments on direct relative orientation.
diff --git a/modules/calib3d/CMakeLists.txt b/modules/calib3d/CMakeLists.txt
index 0953e3cd7e..7b1bace28a 100644
--- a/modules/calib3d/CMakeLists.txt
+++ b/modules/calib3d/CMakeLists.txt
@@ -9,3 +9,4 @@ endif()
ocv_define_module(calib3d opencv_imgproc opencv_features2d opencv_flann ${debug_modules}
WRAP java objc python js
)
+ocv_target_link_libraries(${the_module} ${LAPACK_LIBRARIES})
diff --git a/modules/calib3d/include/opencv2/calib3d.hpp b/modules/calib3d/include/opencv2/calib3d.hpp
index 17544cbf17..54c0beab04 100644
--- a/modules/calib3d/include/opencv2/calib3d.hpp
+++ b/modules/calib3d/include/opencv2/calib3d.hpp
@@ -441,9 +441,16 @@ namespace cv
//! @{
//! type of the robust estimation algorithm
-enum { LMEDS = 4, //!< least-median of squares algorithm
- RANSAC = 8, //!< RANSAC algorithm
- RHO = 16 //!< RHO algorithm
+enum { LMEDS = 4, //!< least-median of squares algorithm
+ RANSAC = 8, //!< RANSAC algorithm
+ RHO = 16, //!< RHO algorithm
+ USAC_DEFAULT = 32, //!< USAC algorithm, default settings
+ USAC_PARALLEL = 33, //!< USAC, parallel version
+ USAC_FM_8PTS = 34, //!< USAC, fundamental matrix 8 points
+ USAC_FAST = 35, //!< USAC, fast settings
+ USAC_ACCURATE = 36, //!< USAC, accurate settings
+ USAC_PROSAC = 37, //!< USAC, sorted points, runs PROSAC
+ USAC_MAGSAC = 38 //!< USAC, sorted points, runs PROSAC
};
enum SolvePnPMethod {
@@ -526,6 +533,27 @@ enum HandEyeCalibrationMethod
CALIB_HAND_EYE_DANIILIDIS = 4 //!< Hand-Eye Calibration Using Dual Quaternions @cite Daniilidis98
};
+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 };
+
+struct CV_EXPORTS_W_SIMPLE UsacParams
+{ // in alphabetical order
+ double confidence = 0.99;
+ bool isParallel = false;
+ int loIterations = 5;
+ LocalOptimMethod loMethod = LocalOptimMethod::LOCAL_OPTIM_INNER_LO;
+ int loSampleSize = 14;
+ int maxIterations = 5000;
+ NeighborSearchMethod neighborsSearch = NeighborSearchMethod::NEIGH_GRID;
+ int randomGeneratorState = 0;
+ SamplingMethod sampler = SamplingMethod::SAMPLING_UNIFORM;
+ ScoreMethod score = ScoreMethod::SCORE_METHOD_MSAC;
+ double threshold = 1.5;
+};
/** @brief Converts a rotation matrix to a rotation vector or vice versa.
@@ -696,6 +724,10 @@ CV_EXPORTS_W Mat findHomography( InputArray srcPoints, InputArray dstPoints,
CV_EXPORTS Mat findHomography( InputArray srcPoints, InputArray dstPoints,
OutputArray mask, int method = 0, double ransacReprojThreshold = 3 );
+
+CV_EXPORTS_W Mat findHomography(InputArray srcPoints, InputArray dstPoints, OutputArray mask,
+ const UsacParams ¶ms);
+
/** @brief Computes an RQ decomposition of 3x3 matrices.
@param src 3x3 input matrix.
@@ -1083,6 +1115,16 @@ CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoint
float reprojectionError = 8.0, double confidence = 0.99,
OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );
+
+/*
+Finds rotation and translation vector.
+If cameraMatrix is given then run P3P. Otherwise run linear P6P and output cameraMatrix too.
+*/
+CV_EXPORTS_W bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
+ InputOutputArray cameraMatrix, InputArray distCoeffs,
+ OutputArray rvec, OutputArray tvec, OutputArray inliers,
+ const UsacParams ¶ms=UsacParams());
+
/** @brief Finds an object pose from 3 3D-2D point correspondences.
@param objectPoints Array of object points in the object coordinate space, 3x3 1-channel or
@@ -2451,6 +2493,10 @@ CV_EXPORTS Mat findFundamentalMat( InputArray points1, InputArray points2,
OutputArray mask, int method = FM_RANSAC,
double ransacReprojThreshold = 3., double confidence = 0.99 );
+
+CV_EXPORTS_W Mat findFundamentalMat( InputArray points1, InputArray points2,
+ OutputArray mask, const UsacParams ¶ms);
+
/** @brief Calculates an essential matrix from the corresponding points in two images.
@param points1 Array of N (N \>= 5) 2D points from the first image. The point coordinates should
@@ -2573,6 +2619,12 @@ CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
double prob = 0.999, double threshold = 1.0,
OutputArray mask = noArray() );
+
+CV_EXPORTS_W Mat findEssentialMat( InputArray points1, InputArray points2,
+ InputArray cameraMatrix1, InputArray cameraMatrix2,
+ InputArray dist_coeff1, InputArray dist_coeff2, OutputArray mask,
+ const UsacParams ¶ms);
+
/** @brief Decompose an essential matrix to possible rotations and translation.
@param E The input essential matrix.
@@ -3037,6 +3089,10 @@ CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray from, InputArray to, OutputArra
size_t maxIters = 2000, double confidence = 0.99,
size_t refineIters = 10);
+
+CV_EXPORTS_W cv::Mat estimateAffine2D(InputArray pts1, InputArray pts2, OutputArray inliers,
+ const UsacParams ¶ms);
+
/** @brief Computes an optimal limited affine transformation with 4 degrees of freedom between
two 2D point sets.
diff --git a/modules/calib3d/src/five-point.cpp b/modules/calib3d/src/five-point.cpp
index c9847b41e3..f35b7bb4b2 100644
--- a/modules/calib3d/src/five-point.cpp
+++ b/modules/calib3d/src/five-point.cpp
@@ -31,6 +31,8 @@
#include "precomp.hpp"
+#include "usac.hpp"
+
namespace cv
{
@@ -407,6 +409,10 @@ cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2, InputArr
{
CV_INSTRUMENT_REGION();
+ if (method >= 32 && method <= 38)
+ return usac::findEssentialMat(_points1, _points2, _cameraMatrix,
+ method, prob, threshold, _mask);
+
Mat points1, points2, cameraMatrix;
_points1.getMat().convertTo(points1, CV_64F);
_points2.getMat().convertTo(points2, CV_64F);
@@ -487,6 +493,20 @@ cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2,
return findEssentialMat(_pointsUntistorted1, _pointsUntistorted2, cm0, method, prob, threshold, _mask);
}
+cv::Mat cv::findEssentialMat( InputArray points1, InputArray points2,
+ InputArray cameraMatrix1, InputArray cameraMatrix2,
+ InputArray dist_coeff1, InputArray dist_coeff2, OutputArray mask, const UsacParams ¶ms) {
+ Ptr model;
+ usac::setParameters(model, usac::EstimationMethod::Essential, params, mask.needed());
+ Ptr ransac_output;
+ if (usac::run(model, points1, points2, model->getRandomGeneratorState(),
+ ransac_output, cameraMatrix1, cameraMatrix2, dist_coeff1, dist_coeff2)) {
+ usac::saveMask(mask, ransac_output->getInliersMask());
+ return ransac_output->getModel();
+ } else return Mat();
+
+}
+
int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2,
InputArray _cameraMatrix, OutputArray _R, OutputArray _t, double distanceThresh,
InputOutputArray _mask, OutputArray triangulatedPoints)
diff --git a/modules/calib3d/src/fundam.cpp b/modules/calib3d/src/fundam.cpp
index 3f19a8923a..0e38eb9a8b 100644
--- a/modules/calib3d/src/fundam.cpp
+++ b/modules/calib3d/src/fundam.cpp
@@ -44,6 +44,8 @@
#include "rho.h"
#include
+#include "usac.hpp"
+
namespace cv
{
@@ -353,6 +355,10 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
{
CV_INSTRUMENT_REGION();
+ if (method >= 32 && method <= 38)
+ return usac::findHomography(_points1, _points2, method, ransacReprojThreshold,
+ _mask, maxIters, confidence);
+
const double defaultRANSACReprojThreshold = 3;
bool result = false;
@@ -439,6 +445,18 @@ cv::Mat cv::findHomography( InputArray _points1, InputArray _points2,
}
+cv::Mat cv::findHomography(InputArray srcPoints, InputArray dstPoints, OutputArray mask,
+ const UsacParams ¶ms) {
+ Ptr model;
+ usac::setParameters(model, usac::EstimationMethod::Homography, params, mask.needed());
+ Ptr ransac_output;
+ if (usac::run(model, srcPoints, dstPoints, model->getRandomGeneratorState(),
+ ransac_output, noArray(), noArray(), noArray(), noArray())) {
+ usac::saveMask(mask, ransac_output->getInliersMask());
+ return ransac_output->getModel() / ransac_output->getModel().at(2,2);
+ } else return Mat();
+}
+
/* Estimation of Fundamental Matrix from point correspondences.
The original code has been written by Valery Mosyagin */
@@ -813,6 +831,10 @@ cv::Mat cv::findFundamentalMat( InputArray _points1, InputArray _points2,
{
CV_INSTRUMENT_REGION();
+ if (method >= 32 && method <= 38)
+ return usac::findFundamentalMat(_points1, _points2, method,
+ ransacReprojThreshold, confidence, maxIters, _mask);
+
Mat points1 = _points1.getMat(), points2 = _points2.getMat();
Mat m1, m2, F;
int npoints = -1;
@@ -885,6 +907,20 @@ cv::Mat cv::findFundamentalMat( cv::InputArray points1, cv::InputArray points2,
return cv::findFundamentalMat(points1, points2, method, ransacReprojThreshold, confidence, 1000, mask);
}
+cv::Mat cv::findFundamentalMat( InputArray points1, InputArray points2,
+ OutputArray mask, const UsacParams ¶ms) {
+ Ptr model;
+ setParameters(model, usac::EstimationMethod::Fundamental, params, mask.needed());
+ Ptr ransac_output;
+ if (usac::run(model, points1, points2, model->getRandomGeneratorState(),
+ ransac_output, noArray(), noArray(), noArray(), noArray())) {
+ usac::saveMask(mask, ransac_output->getInliersMask());
+ return ransac_output->getModel();
+ } else return Mat();
+}
+
+
+
void cv::computeCorrespondEpilines( InputArray _points, int whichImage,
InputArray _Fmat, OutputArray _lines )
diff --git a/modules/calib3d/src/ptsetreg.cpp b/modules/calib3d/src/ptsetreg.cpp
index e8ae3a61fc..f8cc4fe811 100644
--- a/modules/calib3d/src/ptsetreg.cpp
+++ b/modules/calib3d/src/ptsetreg.cpp
@@ -47,6 +47,8 @@
#include
#include
+#include "usac.hpp"
+
namespace cv
{
@@ -927,6 +929,11 @@ Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray _inliers,
const size_t maxIters, const double confidence,
const size_t refineIters)
{
+
+ if (method >= 32 && method <= 38)
+ return cv::usac::estimateAffine2D(_from, _to, _inliers, method,
+ ransacReprojThreshold, (int)maxIters, confidence, (int)refineIters);
+
Mat from = _from.getMat(), to = _to.getMat();
int count = from.checkVector(2);
bool result = false;
@@ -996,6 +1003,18 @@ Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray _inliers,
return H;
}
+Mat estimateAffine2D(InputArray _from, InputArray _to, OutputArray inliers,
+ const UsacParams ¶ms) {
+ Ptr model;
+ usac::setParameters(model, usac::EstimationMethod::Affine, params, inliers.needed());
+ Ptr ransac_output;
+ if (usac::run(model, _from, _to, model->getRandomGeneratorState(),
+ ransac_output, noArray(), noArray(), noArray(), noArray())) {
+ usac::saveMask(inliers, ransac_output->getInliersMask());
+ return ransac_output->getModel().rowRange(0,2);
+ } else return Mat();
+}
+
Mat estimateAffinePartial2D(InputArray _from, InputArray _to, OutputArray _inliers,
const int method, const double ransacReprojThreshold,
const size_t maxIters, const double confidence,
diff --git a/modules/calib3d/src/solvepnp.cpp b/modules/calib3d/src/solvepnp.cpp
index 2c7b27fe13..5cb541c6d5 100644
--- a/modules/calib3d/src/solvepnp.cpp
+++ b/modules/calib3d/src/solvepnp.cpp
@@ -49,6 +49,8 @@
#include "ippe.hpp"
#include "calib3d_c_api.h"
+#include "usac.hpp"
+
namespace cv
{
#if defined _DEBUG || defined CV_STATIC_ANALYSIS
@@ -201,6 +203,11 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
{
CV_INSTRUMENT_REGION();
+ if (flags >= 32 && flags <= 38)
+ return usac::solvePnPRansac(_opoints, _ipoints, _cameraMatrix, _distCoeffs,
+ _rvec, _tvec, useExtrinsicGuess, iterationsCount, reprojectionError,
+ confidence, _inliers, flags);
+
Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat();
Mat opoints, ipoints;
if( opoints0.depth() == CV_64F || !opoints0.isContinuous() )
@@ -342,6 +349,28 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
return true;
}
+
+bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
+ InputOutputArray cameraMatrix, InputArray distCoeffs,
+ OutputArray rvec, OutputArray tvec, OutputArray inliers,
+ const UsacParams ¶ms) {
+ Ptr model_params;
+ usac::setParameters(model_params, cameraMatrix.empty() ? usac::EstimationMethod::P6P :
+ usac::EstimationMethod::P3P, params, inliers.needed());
+ Ptr ransac_output;
+ if (usac::run(model_params, imagePoints, objectPoints, model_params->getRandomGeneratorState(),
+ ransac_output, cameraMatrix, noArray(), distCoeffs, noArray())) {
+ usac::saveMask(inliers, ransac_output->getInliersMask());
+ const Mat &model = ransac_output->getModel();
+ model.col(0).copyTo(rvec);
+ model.col(1).copyTo(tvec);
+ if (cameraMatrix.empty())
+ model.colRange(2, 5).copyTo(cameraMatrix);
+ return true;
+ } else return false;
+}
+
+
int solveP3P( InputArray _opoints, InputArray _ipoints,
InputArray _cameraMatrix, InputArray _distCoeffs,
OutputArrayOfArrays _rvecs, OutputArrayOfArrays _tvecs, int flags) {
diff --git a/modules/calib3d/src/usac.hpp b/modules/calib3d/src/usac.hpp
new file mode 100644
index 0000000000..4fcf58762f
--- /dev/null
+++ b/modules/calib3d/src/usac.hpp
@@ -0,0 +1,800 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#ifndef OPENCV_USAC_USAC_HPP
+#define OPENCV_USAC_USAC_HPP
+
+namespace cv { namespace usac {
+enum EstimationMethod { Homography, Fundamental, Fundamental8, Essential, Affine, P3P, P6P};
+enum VerificationMethod { NullVerifier, SprtVerifier };
+enum PolishingMethod { NonePolisher, LSQPolisher };
+enum ErrorMetric {DIST_TO_LINE, SAMPSON_ERR, SGD_ERR, SYMM_REPR_ERR, FORW_REPR_ERR, RERPOJ};
+
+// Abstract Error class
+class Error : public Algorithm {
+public:
+ // set model to use getError() function
+ virtual void setModelParameters (const Mat &model) = 0;
+ // returns error of point wih @point_idx w.r.t. model
+ virtual float getError (int point_idx) const = 0;
+ virtual const std::vector &getErrors (const Mat &model) = 0;
+ virtual Ptr clone () const = 0;
+};
+
+// Symmetric Reprojection Error for Homography
+class ReprojectionErrorSymmetric : public Error {
+public:
+ static Ptr create(const Mat &points);
+};
+
+// Forward Reprojection Error for Homography
+class ReprojectionErrorForward : public Error {
+public:
+ static Ptr create(const Mat &points);
+};
+
+// Sampson Error for Fundamental matrix
+class SampsonError : public Error {
+public:
+ static Ptr create(const Mat &points);
+};
+
+// Symmetric Geometric Distance (to epipolar lines) for Fundamental and Essential matrix
+class SymmetricGeometricDistance : public Error {
+public:
+ static Ptr create(const Mat &points);
+};
+
+// Reprojection Error for Projection matrix
+class ReprojectionErrorPmatrix : public Error {
+public:
+ static Ptr create(const Mat &points);
+};
+
+// Reprojection Error for Affine matrix
+class ReprojectionErrorAffine : public Error {
+public:
+ static Ptr create(const Mat &points);
+};
+
+// Normalizing transformation of data points
+class NormTransform : public Algorithm {
+public:
+ /*
+ * @norm_points is output matrix of size pts_size x 4
+ * @sample constains indices of points
+ * @sample_number is number of used points in sample <0; sample_number)
+ * @T1, T2 are output transformation matrices
+ */
+ virtual void getNormTransformation (Mat &norm_points, const std::vector &sample,
+ int sample_number, Matx33d &T1, Matx33d &T2) const = 0;
+ static Ptr create (const Mat &points);
+};
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////// SOLVER ///////////////////////////////////////////
+class MinimalSolver : public Algorithm {
+public:
+ // Estimate models from minimal sample. models.size() == number of found solutions
+ virtual int estimate (const std::vector &sample, std::vector &models) const = 0;
+ // return minimal sample size required for estimation.
+ virtual int getSampleSize() const = 0;
+ // return maximum number of possible solutions.
+ virtual int getMaxNumberOfSolutions () const = 0;
+ virtual Ptr clone () const = 0;
+};
+
+//-------------------------- HOMOGRAPHY MATRIX -----------------------
+class HomographyMinimalSolver4ptsGEM : public MinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+//-------------------------- FUNDAMENTAL MATRIX -----------------------
+class FundamentalMinimalSolver7pts : public MinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+class FundamentalMinimalSolver8pts : public MinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+//-------------------------- ESSENTIAL MATRIX -----------------------
+class EssentialMinimalSolverStewenius5pts : public MinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+//-------------------------- PNP -----------------------
+class PnPMinimalSolver6Pts : public MinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+class P3PSolver : public MinimalSolver {
+public:
+ static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K);
+};
+
+//-------------------------- AFFINE -----------------------
+class AffineMinimalSolver : public MinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+//////////////////////////////////////// NON MINIMAL SOLVER ///////////////////////////////////////
+class NonMinimalSolver : public Algorithm {
+public:
+ // Estimate models from non minimal sample. models.size() == number of found solutions
+ virtual int estimate (const std::vector &sample, int sample_size,
+ std::vector &models, const std::vector &weights) const = 0;
+ // return minimal sample size required for non-minimal estimation.
+ virtual int getMinimumRequiredSampleSize() const = 0;
+ // return maximum number of possible solutions.
+ virtual int getMaxNumberOfSolutions () const = 0;
+ virtual Ptr clone () const = 0;
+};
+
+//-------------------------- HOMOGRAPHY MATRIX -----------------------
+class HomographyNonMinimalSolver : public NonMinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+//-------------------------- FUNDAMENTAL MATRIX -----------------------
+class FundamentalNonMinimalSolver : public NonMinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+//-------------------------- ESSENTIAL MATRIX -----------------------
+class EssentialNonMinimalSolver : public NonMinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+//-------------------------- PNP -----------------------
+class PnPNonMinimalSolver : public NonMinimalSolver {
+public:
+ static Ptr create(const Mat &points);
+};
+
+class DLSPnP : public NonMinimalSolver {
+public:
+ static Ptr create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K);
+};
+
+//-------------------------- AFFINE -----------------------
+class AffineNonMinimalSolver : public NonMinimalSolver {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////// SCORE ///////////////////////////////////////////
+class Score {
+public:
+ int inlier_number;
+ double score;
+ Score () { // set worst case
+ inlier_number = 0;
+ score = std::numeric_limits::max();
+ }
+ Score (int inlier_number_, double score_) { // copy constructor
+ inlier_number = inlier_number_;
+ score = score_;
+ }
+ // Compare two scores. Objective is minimization of score. Lower score is better.
+ inline bool isBetter (const Score &score2) const {
+ return score < score2.score;
+ }
+};
+
+////////////////////////////////////////// QUALITY ///////////////////////////////////////////
+class Quality : public Algorithm {
+public:
+ virtual ~Quality() override = default;
+ /*
+ * Calculates number of inliers and score of the @model.
+ * return Score with calculated inlier_number and score.
+ * @model: Mat current model, e.g., H matrix.
+ */
+ virtual Score getScore (const Mat &model) const = 0;
+ virtual Score getScore (const std::vector &/*errors*/) const {
+ CV_Error(cv::Error::StsNotImplemented, "getScore(errors)");
+ }
+ // get @inliers of the @model. Assume threshold is given
+ // @inliers must be preallocated to maximum points size.
+ virtual int getInliers (const Mat &model, std::vector &inliers) const = 0;
+ // get @inliers of the @model for given threshold
+ virtual int getInliers (const Mat &model, std::vector &inliers, double thr) const = 0;
+ // Set the best score, so evaluation of the model can terminate earlier
+ virtual void setBestScore (double best_score_) = 0;
+ // set @inliers_mask: true if point i is inlier, false - otherwise.
+ virtual int getInliers (const Mat &model, std::vector &inliers_mask) const = 0;
+ virtual int getPointsSize() const = 0;
+ virtual Ptr clone () const = 0;
+ static int getInliers (const Ptr &error, const Mat &model,
+ std::vector &inliers_mask, double threshold);
+ static int getInliers (const Ptr &error, const Mat &model,
+ std::vector &inliers, double threshold);
+};
+
+// RANSAC (binary) quality
+class RansacQuality : public Quality {
+public:
+ static Ptr create(int points_size_, double threshold_,const Ptr &error_);
+};
+
+// M-estimator quality - truncated Squared error
+class MsacQuality : public Quality {
+public:
+ static Ptr create(int points_size_, double threshold_, const Ptr &error_);
+};
+
+// Marginlizing Sample Consensus quality, D. Barath et al.
+class MagsacQuality : public Quality {
+public:
+ static Ptr create(double maximum_thr, int points_size_,const Ptr &error_,
+ double tentative_inlier_threshold_, int DoF, double sigma_quantile,
+ double upper_incomplete_of_sigma_quantile,
+ double lower_incomplete_of_sigma_quantile, double C_);
+};
+
+// Least Median of Squares Quality
+class LMedsQuality : public Quality {
+public:
+ static Ptr create(int points_size_, double threshold_, const Ptr &error_);
+};
+
+//////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////// DEGENERACY //////////////////////////////////
+class Degeneracy : public Algorithm {
+public:
+ virtual ~Degeneracy() override = default;
+ /*
+ * Check if sample causes degenerate configurations.
+ * For example, test if points are collinear.
+ */
+ virtual bool isSampleGood (const std::vector &/*sample*/) const {
+ return true;
+ }
+ /*
+ * Check if model satisfies constraints.
+ * For example, test if epipolar geometry satisfies oriented constraint.
+ */
+ virtual bool isModelValid (const Mat &/*model*/, const std::vector &/*sample*/) const {
+ return true;
+ }
+ virtual bool isModelValid (const Mat &/*model*/, const std::vector &/*sample*/,
+ int /*sample_size*/) const {
+ return true;
+ }
+ /*
+ * Fix degenerate model.
+ * Return true if model is degenerate, false - otherwise
+ */
+ virtual bool recoverIfDegenerate (const std::vector &/*sample*/,const Mat &/*best_model*/,
+ Mat &/*non_degenerate_model*/, Score &/*non_degenerate_model_score*/) {
+ return false;
+ }
+ virtual Ptr clone(int /*state*/) const { return makePtr(); }
+};
+
+class EpipolarGeometryDegeneracy : public Degeneracy {
+public:
+ static void recoverRank (Mat &model);
+ static Ptr create (const Mat &points_, int sample_size_);
+};
+
+class EssentialDegeneracy : public EpipolarGeometryDegeneracy {
+public:
+ static Ptrcreate (const Mat &points, int sample_size);
+};
+
+class HomographyDegeneracy : public Degeneracy {
+public:
+ static Ptr create(const Mat &points_);
+};
+
+class FundamentalDegeneracy : public EpipolarGeometryDegeneracy {
+public:
+ // threshold for homography is squared so is around 2.236 pixels
+ static Ptr create (int state, const Ptr &quality_,
+ const Mat &points_, int sample_size_, double homography_threshold);
+};
+
+/////////////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////// ESTIMATOR //////////////////////////////////
+class Estimator : public Algorithm{
+public:
+ /*
+ * Estimate models with minimal solver.
+ * Return number of valid solutions after estimation.
+ * Return models accordingly to number of solutions.
+ * Note, vector of models must allocated before.
+ * Note, not all degenerate tests are included in estimation.
+ */
+ virtual int
+ estimateModels (const std::vector &sample, std::vector &models) const = 0;
+ /*
+ * Estimate model with non-minimal solver.
+ * Return number of valid solutions after estimation.
+ * Note, not all degenerate tests are included in estimation.
+ */
+ virtual int
+ estimateModelNonMinimalSample (const std::vector &sample, int sample_size,
+ std::vector &models, const std::vector &weights) const = 0;
+ // return minimal sample size required for minimal estimation.
+ virtual int getMinimalSampleSize () const = 0;
+ // return minimal sample size required for non-minimal estimation.
+ virtual int getNonMinimalSampleSize () const = 0;
+ // return maximum number of possible solutions of minimal estimation.
+ virtual int getMaxNumSolutions () const = 0;
+ // return maximum number of possible solutions of non-minimal estimation.
+ virtual int getMaxNumSolutionsNonMinimal () const = 0;
+ virtual Ptr clone() const = 0;
+};
+
+class HomographyEstimator : public Estimator {
+public:
+ static Ptr create (const Ptr &min_solver_,
+ const Ptr &non_min_solver_, const Ptr °eneracy_);
+};
+
+class FundamentalEstimator : public Estimator {
+public:
+ static Ptr create (const Ptr &min_solver_,
+ const Ptr &non_min_solver_, const Ptr °eneracy_);
+};
+
+class EssentialEstimator : public Estimator {
+public:
+ static Ptr create (const Ptr &min_solver_,
+ const Ptr &non_min_solver_, const Ptr °eneracy_);
+};
+
+class AffineEstimator : public Estimator {
+public:
+ static Ptr create (const Ptr &min_solver_,
+ const Ptr &non_min_solver_);
+};
+
+class PnPEstimator : public Estimator {
+public:
+ static Ptr create (const Ptr &min_solver_,
+ const Ptr &non_min_solver_);
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////// MODEL VERIFIER ////////////////////////////////////
+class ModelVerifier : public Algorithm {
+public:
+ virtual ~ModelVerifier() override = default;
+ // Return true if model is good, false - otherwise.
+ virtual bool isModelGood(const Mat &model) = 0;
+ // Return true if score was computed during evaluation.
+ virtual bool getScore(Score &score) const = 0;
+ // update verifier by given inlier number
+ virtual void update (int highest_inlier_number) = 0;
+ virtual const std::vector &getErrors() const = 0;
+ virtual bool hasErrors () const = 0;
+ virtual Ptr clone (int state) const = 0;
+ static Ptr create();
+};
+
+struct SPRT_history {
+ /*
+ * delta:
+ * The probability of a data point being consistent
+ * with a ‘bad’ model is modeled as a probability of
+ * a random event with Bernoulli distribution with parameter
+ * δ : p(1|Hb) = δ.
+
+ * epsilon:
+ * The probability p(1|Hg) = ε
+ * that any randomly chosen data point is consistent with a ‘good’ model
+ * is approximated by the fraction of inliers ε among the data
+ * points
+
+ * A is the decision threshold, the only parameter of the Adapted SPRT
+ */
+ double epsilon, delta, A;
+ // number of samples processed by test
+ int tested_samples; // k
+ SPRT_history () {
+ tested_samples = 0;
+ }
+};
+
+///////////////////////////////// SPRT VERIFIER /////////////////////////////////////////
+/*
+* Matas, Jiri, and Ondrej Chum. "Randomized RANSAC with sequential probability ratio test."
+* Tenth IEEE International Conference on Computer Vision (ICCV'05) Volume 1. Vol. 2. IEEE, 2005.
+*/
+class SPRT : public ModelVerifier {
+public:
+ // return constant reference of vector of SPRT histories for SPRT termination.
+ virtual const std::vector &getSPRTvector () const = 0;
+ static Ptr create (int state, const Ptr &err_, int points_size_,
+ double inlier_threshold_, double prob_pt_of_good_model,
+ double prob_pt_of_bad_model, double time_sample, double avg_num_models,
+ ScoreMethod score_type_);
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////// SAMPLER ///////////////////////////////////////
+class Sampler : public Algorithm {
+public:
+ virtual ~Sampler() override = default;
+ // set new points size
+ virtual void setNewPointsSize (int points_size) = 0;
+ // generate sample. Fill @sample with indices of points.
+ virtual void generateSample (std::vector &sample) = 0;
+ virtual Ptr clone (int state) const = 0;
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////// NEIGHBORHOOD GRAPH /////////////////////////////////////////
+class NeighborhoodGraph : public Algorithm {
+public:
+ virtual ~NeighborhoodGraph() override = default;
+ // Return neighbors of the point with index @point_idx_ in the graph.
+ virtual const std::vector &getNeighbors(int point_idx_) const = 0;
+};
+
+class RadiusSearchNeighborhoodGraph : public NeighborhoodGraph {
+public:
+ static Ptr create (const Mat &points, int points_size,
+ double radius_, int flann_search_params, int num_kd_trees);
+};
+
+class FlannNeighborhoodGraph : public NeighborhoodGraph {
+public:
+ static Ptr create(const Mat &points, int points_size,
+ int k_nearest_neighbors_, bool get_distances, int flann_search_params, int num_kd_trees);
+ virtual const std::vector &getNeighborsDistances (int idx) const = 0;
+};
+
+class GridNeighborhoodGraph : public NeighborhoodGraph {
+public:
+ static Ptr create(const Mat &points, int points_size,
+ int cell_size_x_img1_, int cell_size_y_img1_,
+ int cell_size_x_img2_, int cell_size_y_img2_);
+};
+
+////////////////////////////////////// UNIFORM SAMPLER ////////////////////////////////////////////
+class UniformSampler : public Sampler {
+public:
+ static Ptr create(int state, int sample_size_, int points_size_);
+};
+
+/////////////////////////////////// PROSAC (SIMPLE) SAMPLER ///////////////////////////////////////
+class ProsacSimpleSampler : public Sampler {
+public:
+ static Ptr create(int state, int points_size_, int sample_size_,
+ int max_prosac_samples_count);
+};
+
+////////////////////////////////////// PROSAC SAMPLER ////////////////////////////////////////////
+class ProsacSampler : public Sampler {
+public:
+ static Ptr create(int state, int points_size_, int sample_size_,
+ int growth_max_samples);
+ // return number of samples generated (for prosac termination).
+ virtual int getKthSample () const = 0;
+ // return constant reference of growth function of prosac sampler (for prosac termination)
+ virtual const std::vector &getGrowthFunction () const = 0;
+ virtual void setTerminationLength (int termination_length) = 0;
+};
+
+////////////////////////// NAPSAC (N adjacent points sample consensus) SAMPLER ////////////////////
+class NapsacSampler : public Sampler {
+public:
+ static Ptr create(int state, int points_size_, int sample_size_,
+ const Ptr &neighborhood_graph_);
+};
+
+////////////////////////////////////// P-NAPSAC SAMPLER /////////////////////////////////////////
+class ProgressiveNapsac : public Sampler {
+public:
+ static Ptr create(int state, int points_size_, int sample_size_,
+ const std::vector> &layers, int sampler_length);
+};
+
+/////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////// TERMINATION ///////////////////////////////////////////
+class TerminationCriteria : public Algorithm {
+public:
+ // update termination object by given @model and @inlier number.
+ // and return maximum number of predicted iteration
+ virtual int update(const Mat &model, int inlier_number) = 0;
+ // clone termination
+ virtual Ptr clone () const = 0;
+};
+
+//////////////////////////////// STANDARD TERMINATION ///////////////////////////////////////////
+class StandardTerminationCriteria : public TerminationCriteria {
+public:
+ static Ptr create(double confidence, int points_size_,
+ int sample_size_, int max_iterations_);
+};
+
+///////////////////////////////////// SPRT TERMINATION //////////////////////////////////////////
+class SPRTTermination : public TerminationCriteria {
+public:
+ static Ptr create(const std::vector &sprt_histories_,
+ double confidence, int points_size_, int sample_size_, int max_iterations_);
+};
+
+///////////////////////////// PROGRESSIVE-NAPSAC-SPRT TERMINATION /////////////////////////////////
+class SPRTPNapsacTermination : public TerminationCriteria {
+public:
+ static Ptr create(const std::vector&
+ sprt_histories_, double confidence, int points_size_, int sample_size_,
+ int max_iterations_, double relax_coef_);
+};
+
+////////////////////////////////////// PROSAC TERMINATION /////////////////////////////////////////
+class ProsacTerminationCriteria : public TerminationCriteria {
+public:
+ static Ptr create(const Ptr &sampler_,
+ const Ptr &error_, int points_size_, int sample_size, double confidence,
+ int max_iters, int min_termination_length, double beta, double non_randomness_phi,
+ double inlier_thresh);
+};
+
+//////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////////// UTILS ////////////////////////////////////////////////
+namespace Utils {
+ /*
+ * calibrate points: [x'; 1] = K^-1 [x; 1]
+ * @points is matrix N x 4.
+ * @norm_points is output matrix N x 4 with calibrated points.
+ */
+ void calibratePoints (const 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, Mat &K_, Mat &R, Mat &t, bool same_focal=false);
+ double getCalibratedThreshold (double threshold, const Mat &K1, const Mat &K2);
+ float findMedian (std::vector &array);
+}
+namespace Math {
+ // return skew symmetric matrix
+ Matx33d getSkewSymmetric(const Vec3d &v_);
+ // eliminate matrix with m rows and n columns to be upper triangular.
+ void eliminateUpperTriangular (std::vector &a, int m, int n);
+ Matx33d rotVec2RotMat (const Vec3d &v);
+ Vec3d rotMat2RotVec (const Matx33d &R);
+}
+
+///////////////////////////////////////// RANDOM GENERATOR /////////////////////////////////////
+class RandomGenerator : public Algorithm {
+public:
+ virtual ~RandomGenerator() override = default;
+ // interval is <0, max_range);
+ virtual void resetGenerator (int max_range) = 0;
+ // return sample filled with random numbers
+ virtual void generateUniqueRandomSet (std::vector &sample) = 0;
+ // fill @sample of size @subset_size with random numbers in range <0, @max_range)
+ virtual void generateUniqueRandomSet (std::vector &sample, int subset_size,
+ int max_range) = 0;
+ // fill @sample of size @sample.size() with random numbers in range <0, @max_range)
+ virtual void generateUniqueRandomSet (std::vector &sample, int max_range) = 0;
+ // return subset=sample size
+ virtual void setSubsetSize (int subset_sz) = 0;
+ virtual int getSubsetSize () const = 0;
+ // return random number from <0, max_range), where max_range is from constructor
+ virtual int getRandomNumber () = 0;
+ // return random number from <0, max_rng)
+ virtual int getRandomNumber (int max_rng) = 0;
+ virtual const std::vector &generateUniqueRandomSubset (std::vector &array1,
+ int size1) = 0;
+ virtual Ptr clone (int state) const = 0;
+};
+
+class UniformRandomGenerator : public RandomGenerator {
+public:
+ static Ptr create (int state);
+ static Ptr create (int state, int max_range, int subset_size_);
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////// LOCAL OPTIMIZATION /////////////////////////////////////////
+class LocalOptimization : public Algorithm {
+public:
+ virtual ~LocalOptimization() override = default;
+ /*
+ * Refine so-far-the-best RANSAC model in local optimization step.
+ * @best_model: so-far-the-best model
+ * @new_model: output refined new model.
+ * @new_model_score: score of @new_model.
+ * Returns bool if model was refined successfully, false - otherwise
+ */
+ virtual bool refineModel (const Mat &best_model, const Score &best_model_score,
+ Mat &new_model, Score &new_model_score) = 0;
+ virtual Ptr clone(int state) const = 0;
+};
+
+//////////////////////////////////// GRAPH CUT LO ////////////////////////////////////////
+class GraphCut : public LocalOptimization {
+public:
+ static Ptr
+ create(const Ptr &estimator_, const Ptr &error_,
+ const Ptr &quality_, const Ptr &neighborhood_graph_,
+ const Ptr &lo_sampler_, double threshold_,
+ double spatial_coherence_term, int gc_iters);
+};
+
+//////////////////////////////////// INNER + ITERATIVE LO ///////////////////////////////////////
+class InnerIterativeLocalOptimization : public LocalOptimization {
+public:
+ static Ptr
+ create(const Ptr &estimator_, const Ptr &quality_,
+ const Ptr &lo_sampler_, int pts_size, double threshold_,
+ bool is_iterative_, int lo_iter_sample_size_, int lo_inner_iterations,
+ int lo_iter_max_iterations, double threshold_multiplier);
+};
+
+class SigmaConsensus : public LocalOptimization {
+public:
+ static Ptr
+ create(const Ptr &estimator_, const Ptr &error_,
+ const Ptr &quality, const Ptr &verifier_,
+ int max_lo_sample_size, int number_of_irwls_iters_,
+ int DoF, double sigma_quantile, double upper_incomplete_of_sigma_quantile,
+ double C_, double maximum_thr);
+};
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+/////////////////////////////////////// FINAL MODEL POLISHER //////////////////////////////////////
+class FinalModelPolisher : public Algorithm {
+public:
+ virtual ~FinalModelPolisher() override = default;
+ /*
+ * Polish so-far-the-best RANSAC model in the end of RANSAC.
+ * @model: input final RANSAC model.
+ * @new_model: output polished model.
+ * @new_score: score of output model.
+ * Return true if polishing was successful, false - otherwise.
+ */
+ virtual bool polishSoFarTheBestModel (const Mat &model, const Score &best_model_score,
+ Mat &new_model, Score &new_model_score) = 0;
+};
+
+///////////////////////////////////// LEAST SQUARES POLISHER //////////////////////////////////////
+class LeastSquaresPolishing : public FinalModelPolisher {
+public:
+ static Ptr create (const Ptr &estimator_,
+ const Ptr &quality_, int lsq_iterations);
+};
+
+/////////////////////////////////// RANSAC OUTPUT ///////////////////////////////////
+class RansacOutput : public Algorithm {
+public:
+ virtual ~RansacOutput() override = default;
+ static Ptr create(const Mat &model_,
+ const std::vector &inliers_mask_,
+ int time_mcs_, double score_, int number_inliers_, int number_iterations_,
+ int number_estimated_models_, int number_good_models_);
+
+ // Return inliers' indices. size of vector = number of inliers
+ virtual const std::vector &getInliers() = 0;
+ // Return inliers mask. Vector of points size. 1-inlier, 0-outlier.
+ virtual const std::vector &getInliersMask() const = 0;
+ virtual int getTimeMicroSeconds() const = 0;
+ virtual int getTimeMicroSeconds1() const = 0;
+ virtual int getTimeMilliSeconds2() const = 0;
+ virtual int getTimeSeconds3() const = 0;
+ virtual int getNumberOfInliers() const = 0;
+ virtual int getNumberOfMainIterations() const = 0;
+ virtual int getNumberOfGoodModels () const = 0;
+ virtual int getNumberOfEstimatedModels () const = 0;
+ virtual const Mat &getModel() const = 0;
+};
+
+////////////////////////////////////////////// MODEL /////////////////////////////////////////////
+
+class Model : public Algorithm {
+public:
+ virtual bool isFundamental () const = 0;
+ virtual bool isHomography () const = 0;
+ virtual bool isEssential () const = 0;
+ virtual bool isPnP () const = 0;
+
+ // getters
+ virtual int getSampleSize () const = 0;
+ virtual bool isParallel() const = 0;
+ virtual int getMaxNumHypothesisToTestBeforeRejection() const = 0;
+ virtual PolishingMethod getFinalPolisher () const = 0;
+ virtual LocalOptimMethod getLO () const = 0;
+ virtual ErrorMetric getError () const = 0;
+ virtual EstimationMethod getEstimator () const = 0;
+ virtual ScoreMethod getScore () const = 0;
+ virtual int getMaxIters () const = 0;
+ virtual double getConfidence () const = 0;
+ virtual double getThreshold () const = 0;
+ virtual VerificationMethod getVerifier () const = 0;
+ virtual SamplingMethod getSampler () const = 0;
+ virtual double getTimeForModelEstimation () const = 0;
+ virtual double getSPRTdelta () const = 0;
+ virtual double getSPRTepsilon () const = 0;
+ virtual double getSPRTavgNumModels () const = 0;
+ virtual NeighborSearchMethod getNeighborsSearch () const = 0;
+ virtual int getKNN () const = 0;
+ virtual int getCellSize () const = 0;
+ virtual int getGraphRadius() const = 0;
+ virtual double getRelaxCoef () const = 0;
+
+ virtual int getFinalLSQIterations () const = 0;
+ virtual int getDegreesOfFreedom () const = 0;
+ virtual double getSigmaQuantile () const = 0;
+ virtual double getUpperIncompleteOfSigmaQuantile () const = 0;
+ virtual double getLowerIncompleteOfSigmaQuantile () const = 0;
+ virtual double getC () const = 0;
+ virtual double getMaximumThreshold () const = 0;
+ virtual double getGraphCutSpatialCoherenceTerm () const = 0;
+ virtual int getLOSampleSize () const = 0;
+ virtual int getLOThresholdMultiplier() const = 0;
+ virtual int getLOIterativeSampleSize() const = 0;
+ virtual int getLOIterativeMaxIters() const = 0;
+ virtual int getLOInnerMaxIters() const = 0;
+ virtual const std::vector &getGridCellNumber () const = 0;
+ virtual int getRandomGeneratorState () const = 0;
+
+ // setters
+ virtual void setLocalOptimization (LocalOptimMethod lo_) = 0;
+ virtual void setKNearestNeighhbors (int knn_) = 0;
+ virtual void setNeighborsType (NeighborSearchMethod neighbors) = 0;
+ virtual void setCellSize (int cell_size_) = 0;
+ virtual void setParallel (bool is_parallel) = 0;
+ virtual void setVerifier (VerificationMethod verifier_) = 0;
+ virtual void setPolisher (PolishingMethod polisher_) = 0;
+ virtual void setError (ErrorMetric error_) = 0;
+ virtual void setLOIterations (int iters) = 0;
+ virtual void setLOIterativeIters (int iters) = 0;
+ virtual void setLOSampleSize (int lo_sample_size) = 0;
+ virtual void setRandomGeneratorState (int state) = 0;
+
+ virtual void maskRequired (bool required) = 0;
+ virtual bool isMaskRequired () const = 0;
+ static Ptr create(double threshold_, EstimationMethod estimator_, SamplingMethod sampler_,
+ double confidence_=0.95, int max_iterations_=5000, ScoreMethod score_ =ScoreMethod::SCORE_METHOD_MSAC);
+};
+
+Mat findHomography(InputArray srcPoints, InputArray dstPoints, int method,
+ double ransacReprojThreshold, OutputArray mask,
+ const int maxIters, const double confidence);
+
+Mat findFundamentalMat( InputArray points1, InputArray points2,
+ int method, double ransacReprojThreshold, double confidence,
+ int maxIters, OutputArray mask=noArray());
+
+bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,
+ InputArray cameraMatrix, InputArray distCoeffs,
+ OutputArray rvec, OutputArray tvec,
+ bool useExtrinsicGuess, int iterationsCount,
+ float reprojectionError, double confidence,
+ OutputArray inliers, int flags);
+
+Mat findEssentialMat( InputArray points1, InputArray points2,
+ InputArray cameraMatrix1,
+ int method, double prob,
+ double threshold, OutputArray mask);
+
+Mat estimateAffine2D(InputArray from, InputArray to, OutputArray inliers,
+ int method, double ransacReprojThreshold, int maxIters,
+ double confidence, int refineIters);
+
+void saveMask (OutputArray mask, const std::vector &inliers_mask);
+void setParameters (Ptr ¶ms, EstimationMethod estimator, const UsacParams &usac_params,
+ bool mask_need);
+bool run (const Ptr ¶ms, InputArray points1, InputArray points2, int state,
+ Ptr &ransac_output, InputArray K1_, InputArray K2_,
+ InputArray dist_coeff1, InputArray dist_coeff2);
+}}
+
+#endif //OPENCV_USAC_USAC_HPP
diff --git a/modules/calib3d/src/usac/degeneracy.cpp b/modules/calib3d/src/usac/degeneracy.cpp
new file mode 100644
index 0000000000..9378cb1108
--- /dev/null
+++ b/modules/calib3d/src/usac/degeneracy.cpp
@@ -0,0 +1,336 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#include "../precomp.hpp"
+#include "../usac.hpp"
+
+namespace cv { namespace usac {
+class EpipolarGeometryDegeneracyImpl : public EpipolarGeometryDegeneracy {
+private:
+ const Mat * points_mat;
+ const float * const points; // i-th row xi1 yi1 xi2 yi2
+ 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_) {}
+ /*
+ * Do oriented constraint to verify if epipolar geometry is in front or behind the camera.
+ * Return: true if all points are in front of the camers w.r.t. tested epipolar geometry - satisfies constraint.
+ * false - otherwise.
+ */
+ inline bool isModelValid(const Mat &F, const std::vector &sample) const override {
+ return isModelValid(F, sample, min_sample_size);
+ }
+
+ /* Oriented constraint:
+ * x'^T F x = 0
+ * e' × x' ~+ Fx <=> λe' × x' = Fx, λ > 0
+ * e × x ~+ x'^T F
+ */
+ inline bool isModelValid(const Mat &F_, const std::vector &sample, int sample_size_) 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;
+ }
+ // F is 9x1 row-major ordered F matrix. ec is 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
+ // 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]);
+
+ int num_pts_behind = 0;
+ for (int i = 1; i < 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)
+ // if 3 points are behind the camera for non-minimal sample then model is
+ // not valid. Testing by one point as in case for minimal sample is not very
+ // precise. The number 3 was chosen experimentally.
+ if (min_sample_size == sample_size_ || ++num_pts_behind >= 3)
+ return false;
+ }
+ return true;
+ }
+
+ Ptr clone(int /*state*/) const override {
+ return makePtr(*points_mat, min_sample_size);
+ }
+};
+void EpipolarGeometryDegeneracy::recoverRank (Mat &model) {
+ /*
+ * Do singular value decomposition.
+ * Make last eigen value zero of diagonal matrix of singular values.
+ */
+ Matx33d U, Vt;
+ Vec3d w;
+ SVD::compute(model, w, U, Vt, SVD::FULL_UV + SVD::MODIFY_A);
+ model = Mat(U * Matx33d(w(0), 0, 0, 0, w(1), 0, 0, 0, 0) * Vt);
+}
+Ptr EpipolarGeometryDegeneracy::create (const Mat &points_,
+ int sample_size_) {
+ return makePtr(points_, sample_size_);
+}
+
+class HomographyDegeneracyImpl : public HomographyDegeneracy {
+private:
+ const Mat * points_mat;
+ const float * const points;
+public:
+ explicit HomographyDegeneracyImpl (const Mat &points_) :
+ points_mat(&points_), points ((float *)points_.data) {}
+
+ inline bool isSampleGood (const std::vector &sample) const override {
+ const int smpl1 = 4*sample[0], smpl2 = 4*sample[1], smpl3 = 4*sample[2], smpl4 = 4*sample[3];
+ // planar correspondences must lie on the same side of any line from two points in sample
+ const float x1 = points[smpl1], y1 = points[smpl1+1], X1 = points[smpl1+2], Y1 = points[smpl1+3];
+ const float x2 = points[smpl2], y2 = points[smpl2+1], X2 = points[smpl2+2], Y2 = points[smpl2+3];
+ const float x3 = points[smpl3], y3 = points[smpl3+1], X3 = points[smpl3+2], Y3 = points[smpl3+3];
+ const float x4 = points[smpl4], y4 = points[smpl4+1], X4 = points[smpl4+2], Y4 = points[smpl4+3];
+ // line from points 1 and 2
+ const float ab_cross_x = y1 - y2, ab_cross_y = x2 - x1, ab_cross_z = x1 * y2 - y1 * x2;
+ const float AB_cross_x = Y1 - Y2, AB_cross_y = X2 - X1, AB_cross_z = X1 * Y2 - Y1 * X2;
+
+ // check if points 3 and 4 are on the same side of line ab on both images
+ if ((ab_cross_x * x3 + ab_cross_y * y3 + ab_cross_z) *
+ (AB_cross_x * X3 + AB_cross_y * Y3 + AB_cross_z) < 0)
+ return false;
+ if ((ab_cross_x * x4 + ab_cross_y * y4 + ab_cross_z) *
+ (AB_cross_x * X4 + AB_cross_y * Y4 + AB_cross_z) < 0)
+ return false;
+
+ // line from points 3 and 4
+ const float cd_cross_x = y3 - y4, cd_cross_y = x4 - x3, cd_cross_z = x3 * y4 - y3 * x4;
+ const float CD_cross_x = Y3 - Y4, CD_cross_y = X4 - X3, CD_cross_z = X3 * Y4 - Y3 * X4;
+
+ // check if points 1 and 2 are on the same side of line cd on both images
+ if ((cd_cross_x * x1 + cd_cross_y * y1 + cd_cross_z) *
+ (CD_cross_x * X1 + CD_cross_y * Y1 + CD_cross_z) < 0)
+ return false;
+ if ((cd_cross_x * x2 + cd_cross_y * y2 + cd_cross_z) *
+ (CD_cross_x * X2 + CD_cross_y * Y2 + CD_cross_z) < 0)
+ return false;
+
+ // Checks if points are not collinear
+ // If area of triangle constructed with 3 points is less 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|
+ // 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
+ // 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
+
+ return true;
+ }
+ Ptr clone(int /*state*/) const override {
+ return makePtr(*points_mat);
+ }
+};
+Ptr HomographyDegeneracy::create (const Mat &points_) {
+ return makePtr(points_);
+}
+
+///////////////////////////////// Fundamental Matrix Degeneracy ///////////////////////////////////
+class FundamentalDegeneracyImpl : public FundamentalDegeneracy {
+private:
+ RNG rng;
+ const Ptr quality;
+ const float * const points;
+ const Mat * points_mat;
+ const Ptr h_reproj_error;
+ const EpipolarGeometryDegeneracyImpl ep_deg;
+ // threshold to find inliers for homography model
+ const double homography_threshold, log_conf = log(0.05);
+ // points (1-7) to verify in sample
+ std::vector> h_sample {{0,1,2},{3,4,5},{0,1,6},{3,4,6},{2,5,6}};
+ const int points_size, sample_size;
+public:
+
+ FundamentalDegeneracyImpl (int state, const Ptr &quality_, const Mat &points_,
+ int sample_size_, double homography_threshold_) :
+ rng (state), quality(quality_), points((float *) points_.data), points_mat(&points_),
+ h_reproj_error(ReprojectionErrorForward::create(points_)),
+ ep_deg (points_, sample_size_), homography_threshold (homography_threshold_),
+ points_size (quality_->getPointsSize()), sample_size (sample_size_) {
+ if (sample_size_ == 8) {
+ // add more homography samples to test for 8-points F
+ h_sample.emplace_back(std::vector{0, 1, 7});
+ h_sample.emplace_back(std::vector{0, 2, 7});
+ h_sample.emplace_back(std::vector{3, 5, 7});
+ h_sample.emplace_back(std::vector{3, 6, 7});
+ h_sample.emplace_back(std::vector{2, 4, 7});
+ }
+ }
+ inline bool isModelValid(const Mat &F, const std::vector &sample) const override {
+ return ep_deg.isModelValid(F, sample);
+ }
+ inline bool isModelValid(const Mat &F, const std::vector &sample, int sample_size_) const override {
+ return ep_deg.isModelValid(F, sample, sample_size_);
+ }
+
+ bool recoverIfDegenerate (const std::vector &sample, const Mat &F_best,
+ Mat &non_degenerate_model, Score &non_degenerate_model_score) override {
+ non_degenerate_model_score = Score(); // set worst case
+
+ // According to Two-view Geometry Estimation Unaffected by a Dominant Plane
+ // (http://cmp.felk.cvut.cz/~matas/papers/chum-degen-cvpr05.pdf)
+ // only 5 homographies enough to test
+ // triplets {1,2,3}, {4,5,6}, {1,2,7}, {4,5,7} and {3,6,7}
+
+ // H = A - e' (M^-1 b)^T
+ // A = [e']_x F
+ // b_i = (x′i × (A xi))^T (x′i × e′)‖x′i×e′‖^−2,
+ // M is a 3×3 matrix with rows x^T_i
+ // epipole e' is left nullspace of F s.t. e′^T F=0,
+
+ // find e', null space of F^T
+ Vec3d e_prime = F_best.col(0).cross(F_best.col(2));
+ if (fabs(e_prime(0)) < 1e-10 && fabs(e_prime(1)) < 1e-10 &&
+ fabs(e_prime(2)) < 1e-10) // if e' is zero
+ e_prime = F_best.col(1).cross(F_best.col(2));
+
+ const Matx33d A = Math::getSkewSymmetric(e_prime) * Matx33d(F_best);
+
+ Vec3d xi_prime(0,0,1), xi(0,0,1), b;
+ Matx33d M(0,0,1,0,0,1,0,0,1); // last column of M is 1
+
+ bool is_model_degenerate = false;
+ for (const auto &h_i : h_sample) { // only 5 samples
+ for (int pt_i = 0; pt_i < 3; pt_i++) {
+ // find b and M
+ const int smpl = 4*sample[h_i[pt_i]];
+ xi[0] = points[smpl];
+ xi[1] = points[smpl+1];
+ xi_prime[0] = points[smpl+2];
+ xi_prime[1] = points[smpl+3];
+
+ // (x′i × e')
+ const Vec3d xprime_X_eprime = xi_prime.cross(e_prime);
+
+ // (x′i × (A xi))
+ const Vec3d xprime_X_Ax = xi_prime.cross(A * xi);
+
+ // x′i × (A xi))^T (x′i × e′) / ‖x′i×e′‖^2,
+ b[pt_i] = xprime_X_Ax.dot(xprime_X_eprime) /
+ std::pow(norm(xprime_X_eprime), 2);
+
+ // M from x^T
+ M(pt_i, 0) = xi[0];
+ M(pt_i, 1) = xi[1];
+ }
+
+ // compute H
+ const Matx33d H = A - e_prime * (M.inv() * b).t();
+
+ int inliers_on_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)
+ inliers_on_plane++;
+
+ // if there are at least 5 points lying on plane then F is degenerate
+ if (inliers_on_plane >= 5) {
+ is_model_degenerate = true;
+
+ Mat newF;
+ const Score newF_score = planeAndParallaxRANSAC(H, newF);
+ if (newF_score.isBetter(non_degenerate_model_score)) {
+ // store non degenerate model
+ non_degenerate_model_score = newF_score;
+ newF.copyTo(non_degenerate_model);
+ }
+ }
+ }
+ return is_model_degenerate;
+ }
+ Ptr clone(int state) const override {
+ return makePtr(state, quality->clone(), *points_mat,
+ sample_size, homography_threshold);
+ }
+private:
+ // RANSAC with plane-and-parallax to find new Fundamental matrix
+ Score planeAndParallaxRANSAC (const Matx33d &H, Mat &best_F) {
+ 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_reproj_error->getError(h_outlier1) > homography_threshold &&
+ h_reproj_error->getError(h_outlier2) > homography_threshold) {
+
+ // do plane and parallax with outliers of H
+ const Vec3d pt1 (points[4*h_outlier1], points[4*h_outlier1+1], 1);
+ const Vec3d pt2 (points[4*h_outlier2], points[4*h_outlier2+1], 1);
+ const Vec3d pt1_prime (points[4*h_outlier1+2],points[4*h_outlier1+3],1);
+ const Vec3d pt2_prime (points[4*h_outlier2+2],points[4*h_outlier2+3],1);
+
+ // F = [(p1' x Hp1) x (p2' x Hp2)]_x H
+ const Matx33d F = Math::getSkewSymmetric((pt1_prime.cross(H * pt1)).cross
+ (pt2_prime.cross(H * pt2))) * H;
+
+ const Score score = quality->getScore(Mat(F));
+ if (score.isBetter(best_score)) {
+ best_score = score;
+ best_F = Mat(F);
+ const double predicted_iters = log_conf / log(1 - std::pow
+ (static_cast(score.inlier_number) / points_size, 2));
+
+ if (! std::isinf(predicted_iters) && predicted_iters < max_iters)
+ max_iters = static_cast(predicted_iters);
+ }
+ }
+ }
+ return best_score;
+ }
+};
+Ptr FundamentalDegeneracy::create (int state, const Ptr &quality_,
+ const Mat &points_, int sample_size_, double homography_threshold_) {
+ return makePtr(state, quality_, points_, sample_size_,
+ homography_threshold_);
+}
+
+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_) {}
+ inline bool isModelValid(const Mat &E, const std::vector &sample) const override {
+ return ep_deg.isModelValid(E, sample);
+ }
+ Ptr clone(int /*state*/) const override {
+ return makePtr(*points_mat, sample_size);
+ }
+};
+Ptr EssentialDegeneracy::create (const Mat &points_, int sample_size_) {
+ return makePtr(points_, sample_size_);
+}
+}}
diff --git a/modules/calib3d/src/usac/dls_solver.cpp b/modules/calib3d/src/usac/dls_solver.cpp
new file mode 100644
index 0000000000..0bffc6c2d2
--- /dev/null
+++ b/modules/calib3d/src/usac/dls_solver.cpp
@@ -0,0 +1,877 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+// Copyright (C) 2019 Czech Technical University.
+// 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 Czech Technical University 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 THE COPYRIGHT HOLDERS OR CONTRIBUTORS 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.
+//
+// Please contact the author of this library if you have any questions.
+// Author: Daniel Barath (barath.daniel@sztaki.mta.hu)
+// Modification: Maksym Ivashechkin (ivashmak@cmp.felk.cvut.cz)
+
+#include "../precomp.hpp"
+#include "../usac.hpp"
+#if defined(HAVE_EIGEN)
+#include
+#elif defined(HAVE_LAPACK)
+#include "opencv_lapack.h"
+#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 {
+private:
+ const Mat * points_mat, * calib_norm_points_mat, * K_mat;
+#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
+ const Mat &K;
+ const float * const calib_norm_points, * const points;
+#endif
+public:
+ explicit DLSPnPImpl (const Mat &points_, const Mat &calib_norm_points_, const Mat &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)
+#endif
+ {}
+ // return minimal sample size required for non-minimal estimation.
+ int getMinimumRequiredSampleSize() const override { return 3; }
+ // return maximum number of possible solutions.
+ int getMaxNumberOfSolutions () const override { return 27; }
+ Ptr clone () const override {
+ return makePtr(*points_mat, *calib_norm_points_mat, *K_mat);
+ }
+#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
+ int estimate(const std::vector &sample, int sample_number,
+ std::vector &models_, const std::vector &/*weights_*/) const override {
+ if (sample_number < getMinimumRequiredSampleSize())
+ return 0;
+
+ // Estimate the model parameters from the given point sample
+ // using weighted fitting if possible.
+
+ // Holds the normalized feature positions cross multiplied with itself
+ // i.e. n * n^t. This value is used multiple times so it is efficient to
+ // pre-compute it.
+ std::vector normalized_feature_cross(sample_number);
+ std::vector world_points(sample_number);
+ const Matx33d eye = Matx33d::eye();
+
+ // The bottom-right symmetric block matrix of inverse(A^T * A). Matrix H from
+ // Eq. 25 in the Appendix of the DLS paper.
+ Matx33d h_inverse = sample_number * eye;
+
+ // Compute V*W*b with the rotation parameters factored out. This is the
+ // translation parameterized by the 9 entries of the rotation matrix.
+ Matx translation_factor = Matx::zeros();
+
+ for (int i = 0; i < sample_number; i++) {
+ const int idx_world = 5 * sample[i], idx_calib = 3 * sample[i];
+ Vec3d normalized_feature_pos(calib_norm_points[idx_calib],
+ calib_norm_points[idx_calib+1],
+ calib_norm_points[idx_calib+2]);
+ normalized_feature_cross[i] = normalized_feature_pos * normalized_feature_pos.t();
+ world_points[i] = Vec3d(points[idx_world + 2], points[idx_world + 3], points[idx_world + 4]);
+
+ h_inverse -= normalized_feature_cross[i];
+ translation_factor += (normalized_feature_cross[i] - eye) * leftMultiplyMatrix(world_points[i]);
+ }
+
+ const Matx33d h_matrix = h_inverse.inv();
+ translation_factor = h_matrix * translation_factor;
+
+ // Compute the cost function J' of Eq. 17 in DLS paper. This is a factorized
+ // version where the rotation matrix parameters have been pulled out. The
+ // entries to this equation are the coefficients to the cost function which is
+ // a quartic in the rotation parameters.
+ Matx ls_cost_coefficients = Matx::zeros();
+ for (int i = 0; i < sample_number; i++)
+ ls_cost_coefficients +=
+ (leftMultiplyMatrix(world_points[i]) + translation_factor).t() *
+ (eye - normalized_feature_cross[i]) *
+ (leftMultiplyMatrix(world_points[i]) + translation_factor);
+
+ // Extract the coefficients of the jacobian (Eq. 18) from the
+ // ls_cost_coefficients matrix. The jacobian represent 3 monomials in the
+ // rotation parameters. Each entry of the jacobian will be 0 at the roots of
+ // the polynomial, so we can arrange a system of polynomials from these
+ // equations.
+ double f1_coeff[20], f2_coeff[20], f3_coeff[20];
+ extractJacobianCoefficients(ls_cost_coefficients.val, f1_coeff, f2_coeff, f3_coeff);
+
+ // We create one equation with random terms that is generally non-zero at the
+ // roots of our system.
+ RNG rng;
+ const double macaulay_term[4] = { 100 * rng.uniform(-1.,1.), 100 * rng.uniform(-1.,1.),
+ 100 * rng.uniform(-1.,1.), 100 * rng.uniform(-1.,1.) };
+
+ // Create Macaulay matrix that will be used to solve our polynonomial system.
+ Mat macaulay_matrix = Mat_::zeros(120, 120);
+ createMacaulayMatrix(f1_coeff, f2_coeff, f3_coeff, macaulay_term, (double*)macaulay_matrix.data);
+
+ // Via the Schur complement trick, the top-left of the Macaulay matrix
+ // contains a multiplication matrix whose eigenvectors correspond to solutions
+ // to our system of equations.
+ Mat sol;
+ if (!solve(macaulay_matrix.colRange(27, 120).rowRange(27, 120),
+ macaulay_matrix.colRange(0 , 27).rowRange(27, 120), sol, DECOMP_LU))
+ return 0;
+
+ const Mat solution_polynomial = macaulay_matrix.colRange(0,27).rowRange(0,27) -
+ (macaulay_matrix.colRange(27,120).rowRange(0,27) * sol);
+
+ // Extract eigenvectors of the solution polynomial to obtain the roots which
+ // are contained in the entries of the eigenvectors.
+#ifdef HAVE_EIGEN
+ Eigen::Map> sol_poly((double*)solution_polynomial.data);
+ const Eigen::EigenSolver eigen_solver(sol_poly);
+ const auto &eigen_vectors = eigen_solver.eigenvectors();
+ const auto &eigen_values = eigen_solver.eigenvalues();
+#else
+ int mat_order = 27, info, lda = 27, ldvl = 1, ldvr = 27, lwork = 500;
+ double wr[27], wi[27] = {0}; // 27 = mat_order
+ std::vector work(lwork), eig_vecs(729);
+ char jobvl = 'N', jobvr = 'V'; // only left eigen vectors are computed
+ dgeev_(&jobvl, &jobvr, &mat_order, (double*)solution_polynomial.data, &lda, wr, wi, nullptr, &ldvl,
+ &eig_vecs[0], &ldvr, &work[0], &lwork, &info);
+ if (info != 0) return 0;
+#endif
+ models_ = std::vector(); models_.reserve(3);
+ const int max_pts_to_eval = std::min(sample_number, 100);
+ std::vector pts_random_shuffle(sample_number);
+ 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.
+ // The first entry of the eigenvector should equal 1 according to our
+ // polynomial, so we must divide each solution by the first entry.
+
+#ifdef HAVE_EIGEN
+ if (eigen_values(i).imag() != 0)
+ continue;
+ const double eigen_vec_1i = 1 / eigen_vectors(0, i).real();
+ const double s1 = eigen_vectors(9, i).real() * eigen_vec_1i,
+ s2 = eigen_vectors(3, i).real() * eigen_vec_1i,
+ s3 = eigen_vectors(1, i).real() * eigen_vec_1i;
+#else
+ if (wi[i] != 0)
+ continue;
+ const double eigen_vec_1i = 1 / eig_vecs[mat_order*i];
+ const double s1 = eig_vecs[mat_order*i+9] * eigen_vec_1i,
+ s2 = eig_vecs[mat_order*i+3] * eigen_vec_1i,
+ s3 = eig_vecs[mat_order*i+1] * eigen_vec_1i;
+#endif
+ // Compute the rotation (which is the transpose rotation of our solution)
+ // and translation.
+ const double qi = s1, qi2 = qi*qi, qj = s2, qj2 = qj*qj, qk = s3, qk2 = qk*qk;
+ const double s = 1 / (1 + qi2 + qj2 + qk2);
+ const Matx33d rot_mat (1-2*s*(qj2+qk2), 2*s*(qi*qj+qk), 2*s*(qi*qk-qj),
+ 2*s*(qi*qj-qk), 1-2*s*(qi2+qk2), 2*s*(qj*qk+qi),
+ 2*s*(qi*qk+qj), 2*s*(qj*qk-qi), 1-2*s*(qi2+qj2));
+ const Matx31d soln_translation = translation_factor * rot_mat.reshape<9,1>();
+
+ // Check that all points are in front of the camera. Discard the solution
+ // if this is not the case.
+ bool all_points_in_front_of_camera = true;
+ const Vec3d r3 (rot_mat(2,0),rot_mat(2,1),rot_mat(2,2));
+ const double z = soln_translation(2);
+ for (int pt = 0; pt < max_pts_to_eval; pt++) {
+ if (r3.dot(world_points[pts_random_shuffle[pt]]) + z < 0) {
+ all_points_in_front_of_camera = false;
+ break;
+ }
+ }
+
+ if (all_points_in_front_of_camera) {
+ Mat model;
+// hconcat(rot_mat, soln_translation, model);
+ hconcat(Math::rotVec2RotMat(Math::rotMat2RotVec(rot_mat)), soln_translation, model);
+ models_.emplace_back(K * model);
+ }
+ }
+ return static_cast(models_.size());
+#else
+ int estimate(const std::vector &/*sample*/, int /*sample_number*/,
+ std::vector &/*models_*/, const std::vector &/*weights_*/) const override {
+ return 0;
+#endif
+ }
+
+protected:
+#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
+ const int indices[1968] = {
+ 0, 35, 83, 118, 120, 121, 154, 155, 174, 203, 219, 238, 241, 242, 274, 275,
+ 291, 294, 305, 323, 329, 339, 358, 360, 363, 395, 409, 436, 443, 478, 479,
+ 481, 483, 484, 514, 515, 523, 529, 534, 551, 556, 563, 579, 580, 598, 599,
+ 602, 604, 605, 634, 635, 641, 643, 649, 651, 654, 662, 665, 671, 676, 683,
+ 689, 699, 700, 711, 718, 719, 723, 726, 750, 755, 769, 795, 796, 803, 827,
+ 838, 839, 844, 846, 847, 870, 874, 875, 883, 885, 889, 894, 903, 911, 915,
+ 916, 923, 939, 940, 947, 952, 958, 959, 965, 967, 968, 990, 994, 1001, 1003,
+ 1005, 1006, 1009, 1011, 1014, 1022, 1023, 1025, 1026, 1031, 1035, 1036,
+ 1049, 1059, 1060, 1062, 1067, 1071, 1072, 1079, 1080, 1089, 1115, 1116,
+ 1163, 1164, 1168, 1198, 1201, 1209, 1210, 1233, 1234, 1235, 1236, 1254,
+ 1259, 1283, 1284, 1288, 1299, 1317, 1318, 1322, 1330, 1331, 1348, 1353,
+ 1354, 1355, 1356, 1371, 1374, 1377, 1379, 1385, 1403, 1404, 1408, 1409,
+ 1419, 1434, 1437, 1438, 1443, 1449, 1452, 1475, 1476, 1479, 1489, 1516,
+ 1519, 1523, 1524, 1528, 1536, 1558, 1559, 1564, 1570, 1572, 1573, 1593,
+ 1594, 1595, 1596, 1599, 1603, 1607, 1609, 1614, 1619, 1620, 1631, 1636,
+ 1639, 1643, 1644, 1648, 1650, 1656, 1659, 1660, 1677, 1678, 1679, 1685,
+ 1691, 1693, 1694, 1708, 1713, 1714, 1716, 1719, 1721, 1722, 1723, 1727,
+ 1729, 1731, 1734, 1736, 1737, 1739, 1740, 1742, 1745, 1751, 1756, 1759,
+ 1764, 1768, 1769, 1770, 1776, 1779, 1780, 1786, 1791, 1794, 1797, 1799,
+ 1806, 1812, 1815, 1829, 1830, 1835, 1836, 1839, 1849, 1874, 1875, 1876,
+ 1879, 1883, 1884, 1888, 1894, 1896, 1907, 1918, 1919, 1927, 1933, 1935,
+ 1936, 1949, 1950, 1953, 1954, 1956, 1959, 1963, 1964, 1965, 1967, 1969,
+ 1974, 1979, 1980, 1983, 1988, 1991, 1994, 1995, 1996, 1999, 2004, 2008,
+ 2010, 2014, 2016, 2017, 2019, 2020, 2027, 2032, 2037, 2039, 2048, 2054,
+ 2056, 2057, 2068, 2069, 2070, 2073, 2079, 2081, 2082, 2083, 2084, 2085,
+ 2086, 2087, 2091, 2096, 2097, 2099, 2100, 2102, 2103, 2105, 2106, 2108,
+ 2111, 2114, 2115, 2119, 2129, 2130, 2134, 2136, 2137, 2140, 2142, 2146,
+ 2147, 2151, 2152, 2154, 2157, 2169, 2178, 2195, 2196, 2213, 2242, 2243,
+ 2244, 2247, 2248, 2278, 2290, 2298, 2299, 2312, 2313, 2314, 2315, 2316,
+ 2333, 2334, 2339, 2341, 2362, 2363, 2364, 2367, 2368, 2379, 2396, 2397,
+ 2398, 2411, 2419, 2420, 2427, 2428, 2432, 2433, 2434, 2436, 2451, 2453,
+ 2454, 2455, 2457, 2459, 2461, 2465, 2482, 2484, 2487, 2488, 2489, 2499,
+ 2513, 2514, 2516, 2517, 2532, 2538, 2541, 2555, 2556, 2558, 2559, 2569,
+ 2573, 2596, 2598, 2599, 2602, 2603, 2604, 2607, 2608, 2612, 2616, 2638,
+ 2639, 2653, 2659, 2661, 2662, 2672, 2673, 2674, 2676, 2678, 2679, 2680,
+ 2683, 2687, 2689, 2693, 2694, 2699, 2700, 2701, 2711, 2712, 2716, 2718,
+ 2719, 2722, 2724, 2727, 2728, 2730, 2732, 2735, 2736, 2739, 2740, 2756,
+ 2757, 2759, 2774, 2780, 2782, 2783, 2787, 2788, 2792, 2793, 2798, 2799,
+ 2800, 2801, 2802, 2803, 2807, 2811, 2813, 2815, 2816, 2817, 2819, 2820,
+ 2821, 2822, 2825, 2831, 2832, 2838, 2839, 2842, 2847, 2849, 2850, 2852,
+ 2855, 2856, 2860, 2866, 2871, 2873, 2874, 2876, 2877, 2895, 2901, 2904,
+ 2909, 2910, 2916, 2918, 2919, 2929, 2932, 2933, 2953, 2954, 2955, 2956,
+ 2958, 2959, 2962, 2964, 2967, 2968, 2972, 2973, 2974, 2976, 2987, 2999,
+ 3016, 3022, 3024, 3025, 3029, 3030, 3032, 3033, 3038, 3039, 3040, 3043,
+ 3044, 3045, 3047, 3052, 3053, 3059, 3060, 3061, 3063, 3068, 3071, 3072,
+ 3073, 3074, 3075, 3078, 3079, 3082, 3087, 3090, 3092, 3093, 3094, 3095,
+ 3096, 3097, 3100, 3107, 3112, 3116, 3117, 3137, 3143, 3145, 3146, 3147,
+ 3148, 3149, 3152, 3158, 3160, 3161, 3162, 3164, 3165, 3166, 3167, 3172,
+ 3175, 3176, 3177, 3180, 3181, 3182, 3183, 3186, 3188, 3192, 3193, 3194,
+ 3198, 3210, 3212, 3213, 3214, 3215, 3217, 3222, 3226, 3231, 3232, 3233,
+ 3234, 3236, 3255, 3269, 3270, 3276, 3279, 3289, 3309, 3310, 3314, 3315,
+ 3316, 3319, 3324, 3328, 3331, 3334, 3336, 3347, 3350, 3359, 3366, 3390,
+ 3395, 3409, 3429, 3435, 3436, 3443, 3467, 3470, 3478, 3479, 3504, 3509,
+ 3510, 3518, 3519, 3532, 3533, 3549, 3550, 3553, 3554, 3555, 3558, 3559,
+ 3562, 3567, 3571, 3572, 3573, 3574, 3576, 3587, 3590, 3637, 3648, 3652,
+ 3670, 3673, 3677, 3681, 3685, 3691, 3693, 3698, 3749, 3757, 3758, 3770,
+ 3772, 3789, 3790, 3793, 3794, 3797, 3798, 3800, 3806, 3811, 3812, 3813,
+ 3814, 3818, 3830, 3888, 3890, 3893, 3920, 3921, 3922, 3925, 3926, 3927,
+ 3989, 3990, 3999, 4024, 4029, 4030, 4034, 4035, 4039, 4051, 4054, 4056,
+ 4063, 4067, 4070, 4109, 4118, 4132, 4144, 4149, 4150, 4153, 4154, 4158,
+ 4171, 4172, 4173, 4174, 4183, 4190, 4237, 4252, 4264, 4270, 4273, 4277,
+ 4291, 4293, 4298, 4303, 4325, 4354, 4361, 4363, 4369, 4371, 4374, 4382,
+ 4385, 4391, 4396, 4409, 4419, 4420, 4421, 4429, 4431, 4439, 4442, 4474,
+ 4475, 4491, 4494, 4505, 4523, 4529, 4539, 4549, 4558, 4590, 4609, 4624,
+ 4629, 4635, 4636, 4663, 4667, 4670, 4679, 4708, 4713, 4731, 4737, 4739,
+ 4745, 4769, 4785, 4788, 4789, 4794, 4797, 4827, 4828, 4832, 4855, 4857,
+ 4861, 4905, 4908, 4909, 4913, 4914, 4916, 4950, 4984, 4989, 4995, 5023,
+ 5027, 5030, 5067, 5071, 5095, 5098, 5145, 5148, 5153, 5155, 5189, 5224,
+ 5229, 5230, 5234, 5251, 5254, 5263, 5270, 5308, 5337, 5385, 5388, 5389,
+ 5394, 5427, 5455, 5505, 5508, 5513, 5572, 5584, 5590, 5593, 5611, 5613,
+ 5623, 5680, 5684, 5692, 5704, 5707, 5708, 5710, 5712, 5713, 5731, 5733,
+ 5735, 5737, 5743, 5744, 5790, 5803, 5805, 5823, 5824, 5827, 5829, 5831,
+ 5835, 5860, 5863, 5864, 5867, 5870, 5872, 5921, 5925, 5926, 5942, 5943,
+ 5946, 5981, 5982, 5985, 5989, 5991, 5992, 6041, 6062, 6101, 6105, 6109,
+ 6111, 6184, 6190, 6211, 6223, 6281, 6285, 6286, 6302, 6303, 6306, 6307,
+ 6309, 6341, 6342, 6344, 6349, 6350, 6351, 6352, 6424, 6429, 6463, 6470,
+ 6585, 6589, 6644, 6664, 6667, 6668, 6670, 6691, 6697, 6703, 6704, 6825,
+ 6828, 6904, 6907, 6943, 6944, 7006, 7024, 7026, 7027, 7062, 7063, 7064,
+ 7088, 7110, 7121, 7123, 7125, 7126, 7131, 7142, 7143, 7145, 7146, 7151,
+ 7155, 7169, 7180, 7181, 7182, 7187, 7189, 7191, 7192, 7208, 7230, 7241,
+ 7243, 7245, 7246, 7251, 7262, 7263, 7265, 7266, 7267, 7269, 7271, 7275,
+ 7289, 7300, 7302, 7304, 7307, 7310, 7311, 7312, 7362, 7376, 7421, 7425,
+ 7426, 7428, 7504, 7543, 7665, 7726, 7746, 7747, 7781, 7782, 7784, 7785,
+ 7846, 7864, 7866, 7867, 7901, 7902, 7903, 7904, 7966, 7986, 8021, 8022,
+ 8025, 8141, 8145, 8201, 8203, 8211, 8222, 8225, 8231, 8249, 8260, 8261,
+ 8265, 8269, 8271, 8317, 8328, 8332, 8353, 8357, 8361, 8365, 8373, 8378,
+ 8420, 8427, 8428, 8431, 8432, 8433, 8450, 8451, 8453, 8455, 8457, 8458,
+ 8459, 8461, 8465, 8480, 8482, 8486, 8487, 8489, 8513, 8514, 8515, 8516,
+ 8517, 8565, 8583, 8584, 8587, 8589, 8623, 8624, 8630, 8632, 8681, 8685,
+ 8686, 8702, 8703, 8704, 8706, 8707, 8709, 8742, 8743, 8744, 8750, 8751,
+ 8752, 8808, 8810, 8840, 8841, 8845, 8846, 8905, 8909, 8912, 8918, 8920,
+ 8924, 8925, 8927, 8932, 8940, 8941, 8943, 8947, 8948, 8949, 8950, 8952,
+ 8953, 8954, 8958, 8970, 8971, 8972, 8973, 8974, 8975, 8977, 8984, 8990,
+ 8992, 8996, 9021, 9036, 9037, 9038, 9039, 9049, 9050, 9053, 9076, 9077,
+ 9078, 9079, 9080, 9082, 9084, 9086, 9087, 9088, 9092, 9096, 9098, 9119,
+ 9168, 9201, 9205, 9274, 9291, 9294, 9305, 9329, 9339, 9345, 9349, 9387,
+ 9391, 9397, 9400, 9402, 9415, 9416, 9418, 9432, 9437, 9455, 9458, 9461,
+ 9466, 9468, 9473, 9475, 9522, 9524, 9526, 9536, 9546, 9548, 9577, 9581,
+ 9582, 9585, 9586, 9588, 9614, 9628, 9633, 9639, 9641, 9642, 9643, 9647,
+ 9651, 9656, 9657, 9659, 9660, 9662, 9665, 9671, 9679, 9689, 9690, 9696,
+ 9700, 9701, 9706, 9708, 9709, 9711, 9714, 9717, 9751, 9752, 9757, 9758,
+ 9760, 9767, 9768, 9770, 9778, 9780, 9781, 9792, 9797, 9798, 9800, 9801,
+ 9805, 9806, 9810, 9812, 9815, 9818, 9835, 9836, 9869, 9884, 9885, 9887,
+ 9900, 9903, 9904, 9907, 9908, 9909, 9910, 9914, 9930, 9931, 9934, 9937,
+ 9943, 9944, 9950, 9952, 9986, 9987, 9991, 9997, 10000, 10002, 10004, 10006,
+ 10012, 10015, 10016, 10018, 10026, 10028, 10032, 10033, 10037, 10053, 10055,
+ 10057, 10058, 10062, 10066, 10073, 10075, 10096, 10109, 10110, 10113, 10119,
+ 10123, 10124, 10125, 10127, 10139, 10140, 10143, 10147, 10148, 10149, 10150,
+ 10151, 10154, 10155, 10159, 10170, 10171, 10174, 10176, 10177, 10180, 10184,
+ 10187, 10190, 10192, 10197, 10225, 10229, 10231, 10232, 10237, 10238, 10240,
+ 10244, 10245, 10247, 10250, 10252, 10258, 10260, 10261, 10263, 10268, 10272,
+ 10273, 10274, 10277, 10278, 10280, 10286, 10290, 10292, 10293, 10294, 10295,
+ 10297, 10298, 10312, 10315, 10316, 10351, 10357, 10360, 10364, 10368, 10372,
+ 10378, 10388, 10392, 10393, 10397, 10401, 10405, 10413, 10415, 10417, 10418,
+ 10435, 10462, 10471, 10472, 10473, 10477, 10478, 10479, 10480, 10483, 10487,
+ 10490, 10493, 10498, 10499, 10500, 10501, 10511, 10512, 10517, 10518, 10519,
+ 10520, 10522, 10526, 10527, 10530, 10532, 10535, 10536, 10538, 10540, 10555,
+ 10556, 10557, 10587, 10591, 10597, 10600, 10602, 10608, 10615, 10616, 10618,
+ 10632, 10637, 10641, 10645, 10655, 10658, 10666, 10673, 10675, 10711, 10717,
+ 10720, 10724, 10732, 10738, 10747, 10748, 10750, 10752, 10753, 10757, 10771,
+ 10773, 10775, 10777, 10778, 10784, 10795, 10827, 10840, 10842, 10855, 10856,
+ 10872, 10895, 10901, 10905, 10906, 10908, 10913, 10943, 10947, 10948, 10951,
+ 10952, 10957, 10958, 10960, 10961, 10962, 10967, 10970, 10975, 10976, 10977,
+ 10978, 10980, 10981, 10982, 10992, 10997, 10998, 11000, 11006, 11010, 11012,
+ 11015, 11018, 11026, 11031, 11033, 11034, 11035, 11036, 11057, 11068, 11069,
+ 11081, 11082, 11084, 11085, 11086, 11087, 11096, 11097, 11100, 11102, 11103,
+ 11106, 11108, 11114, 11130, 11134, 11137, 11141, 11142, 11146, 11148, 11149,
+ 11151, 11152, 11154, 11177, 11188, 11189, 11201, 11202, 11204, 11205, 11206,
+ 11207, 11216, 11217, 11220, 11222, 11223, 11226, 11227, 11228, 11229, 11230,
+ 11234, 11250, 11251, 11254, 11257, 11262, 11264, 11266, 11270, 11271, 11272,
+ 11274, 11311, 11317, 11320, 11328, 11338, 11352, 11357, 11361, 11365, 11375,
+ 11378, 11395, 11426, 11427, 11440, 11442, 11444, 11446, 11452, 11455, 11456,
+ 11466, 11468, 11472, 11473, 11493, 11495, 11497, 11501, 11502, 11506, 11508,
+ 11513, 11543, 11547, 11548, 11552, 11558, 11560, 11561, 11562, 11567, 11575,
+ 11576, 11577, 11580, 11581, 11582, 11592, 11598, 11610, 11612, 11615, 11621,
+ 11626, 11628, 11629, 11631, 11633, 11634, 11636, 11682, 11684, 11686, 11696,
+ 11706, 11707, 11708, 11710, 11731, 11737, 11741, 11742, 11744, 11746, 11748,
+ 11788, 11801, 11802, 11807, 11816, 11817, 11820, 11822, 11850, 11861, 11865,
+ 11866, 11868, 11869, 11871, 11874, 11922, 11924, 11926, 11936, 11944, 11946,
+ 11947, 11948, 11950, 11971, 11977, 11982, 11983, 11984, 11986, 12051, 12065,
+ 12089, 12105, 12109, 12157, 12158, 12159, 12168, 12170, 12173, 12197, 12198,
+ 12199, 12200, 12201, 12202, 12205, 12206, 12207, 12212, 12216, 12218, 12277,
+ 12278, 12288, 12290, 12317, 12318, 12320, 12321, 12325, 12326, 12332, 12338,
+ 12397, 12408, 12437, 12441, 12445, 12458, 12491, 12508, 12513, 12514, 12516,
+ 12531, 12534, 12537, 12539, 12545, 12564, 12568, 12569, 12579, 12588, 12589,
+ 12594, 12597, 12620, 12627, 12628, 12632, 12633, 12651, 12653, 12655, 12657,
+ 12659, 12661, 12665, 12682, 12687, 12689, 12708, 12709, 12713, 12714, 12716,
+ 12717, 12747, 12748, 12751, 12752, 12770, 12775, 12777, 12778, 12781, 12800,
+ 12806, 12828, 12829, 12833, 12834, 12835, 12836, 12867, 12871, 12888, 12895,
+ 12898, 12921, 12925, 12948, 12953, 12955, 12996, 13008, 13010, 13013, 13040,
+ 13041, 13042, 13044, 13045, 13046, 13047, 13048, 13106, 13107, 13120, 13122,
+ 13124, 13126, 13132, 13135, 13136, 13146, 13147, 13148, 13150, 13152, 13153,
+ 13171, 13173, 13175, 13177, 13182, 13184, 13186, 13193, 13207, 13230, 13234,
+ 13243, 13245, 13249, 13254, 13263, 13267, 13269, 13271, 13275, 13276, 13299,
+ 13300, 13304, 13307, 13310, 13312, 13319, 13338, 13355, 13356, 13370, 13373,
+ 13400, 13402, 13403, 13404, 13406, 13407, 13408, 13438, 13459, 13471, 13472,
+ 13473, 13474, 13476, 13490, 13493, 13494, 13498, 13499, 13501, 13520, 13522,
+ 13524, 13526, 13527, 13528, 13539, 13555, 13556, 13557, 13591, 13592, 13593,
+ 13608, 13610, 13613, 13618, 13619, 13621, 13640, 13641, 13642, 13645, 13646,
+ 13647, 13675, 13676, 13677, 13711, 13712, 13728, 13730, 13738, 13741, 13760,
+ 13761, 13765, 13766, 13795, 13796, 13831, 13848, 13858, 13881, 13885, 13915,
+ 13944, 13949, 13950, 13957, 13958, 13959, 13970, 13972, 13973, 13993, 13994,
+ 13995, 13997, 13998, 13999, 14000, 14002, 14006, 14007, 14012, 14013, 14014,
+ 14016, 14018, 14027, 14069, 14077, 14078, 14088, 14090, 14092, 14113, 14114,
+ 14117, 14118, 14120, 14121, 14125, 14126, 14132, 14133, 14134, 14138, 14187,
+ 14188, 14191, 14192, 14208, 14210, 14215, 14217, 14218, 14221, 14240, 14241,
+ 14245, 14246, 14273, 14274, 14275, 14276, 14307, 14311, 14328, 14335, 14338,
+ 14361, 14365, 14393, 14395
+ };
+ void createMacaulayMatrix(const double a[20], const double b[20],
+ const double c[20], const double u[4], double * macaulay_matrix) const {
+ // The matrix is very large (14400 elements!) and sparse (1968 non-zero
+ // elements) so we load it from pre-computed values calculated in matlab.
+
+ const double values[1968] = {
+ u[0], a[0], b[0], c[0], u[3], u[0], a[0], a[9], b[0], b[9], c[0], c[9],
+ u[3], u[0], a[9], a[13], a[0], b[9], b[0], b[13], c[0], c[9], c[13], u[2],
+ u[0], a[10], a[0], b[0], b[10], c[10], c[0], u[2], u[3], u[0], a[10], a[4],
+ a[0], a[9], b[10], b[0], b[9], b[4], c[10], c[0], c[4], c[9], u[2], u[3],
+ u[0], a[4], a[11], a[0], a[9], a[13], a[10], b[4], b[0], b[10], b[9], b[13],
+ b[11], c[10], c[4], c[9], c[0], c[11], c[13], u[2], u[0], a[0], a[14],
+ a[10], b[0], b[10], b[14], c[0], c[14], c[10], u[2], u[3], u[0], a[9],
+ a[14], a[5], a[10], a[0], a[4], b[14], b[0], b[10], b[9], b[4], b[5], c[14],
+ c[10], c[9], c[0], c[5], c[4], u[2], u[3], u[0], a[13], a[5], a[10], a[4],
+ a[9], a[0], a[11], a[14], b[5], b[10], b[9], b[14], b[0], b[4], b[13],
+ b[11], c[14], c[5], c[4], c[0], c[13], c[10], c[9], c[11], u[1], u[0], a[8],
+ a[0], b[8], b[0], c[0], c[8], u[1], u[3], u[0], a[0], a[8], a[3], a[9],
+ b[8], b[0], b[3], b[9], c[9], c[8], c[0], c[3], u[1], u[3], u[0], a[0],
+ a[9], a[3], a[7], a[13], a[8], b[3], b[0], b[9], b[8], b[7], b[13], c[13],
+ c[8], c[3], c[0], c[9], c[7], u[1], u[2], u[0], a[2], a[10], a[0], a[8],
+ b[8], b[0], b[2], b[10], c[10], c[0], c[2], c[8], u[1], u[2], u[3], u[0],
+ a[10], a[2], a[16], a[4], a[9], a[8], a[0], a[3], b[2], b[10], b[0], b[8],
+ b[3], b[9], b[16], b[4], c[4], c[0], c[9], c[2], c[8], c[10], c[16], c[3],
+ u[1], u[2], u[3], u[0], a[10], a[4], a[16], a[11], a[13], a[8], a[0], a[3],
+ a[9], a[7], a[2], b[16], b[0], b[10], b[4], b[9], b[8], b[2], b[3], b[7],
+ b[13], b[11], c[11], c[2], c[9], c[13], c[16], c[3], c[0], c[8], c[10],
+ c[4], c[7], u[1], u[2], u[0], a[0], a[8], a[17], a[14], a[10], a[2], b[0],
+ b[8], b[2], b[10], b[17], b[14], c[14], c[0], c[10], c[8], c[17], c[2],
+ u[1], u[2], u[3], u[0], a[9], a[3], a[14], a[17], a[5], a[4], a[2], a[0],
+ a[8], a[10], a[16], b[17], b[14], b[10], b[8], b[0], b[2], b[9], b[3],
+ b[16], b[4], b[5], c[5], c[10], c[9], c[4], c[0], c[17], c[2], c[3], c[8],
+ c[14], c[16], u[1], u[2], u[3], u[0], a[14], a[13], a[7], a[5], a[11], a[2],
+ a[10], a[16], a[9], a[3], a[8], a[4], a[17], b[10], b[14], b[5], b[4], b[2],
+ b[3], b[17], b[8], b[9], b[16], b[13], b[7], b[11], c[17], c[4], c[13],
+ c[11], c[9], c[16], c[8], c[10], c[7], c[2], c[3], c[14], c[5], u[1], u[0],
+ a[12], a[8], a[0], b[0], b[12], b[8], c[0], c[8], c[12], u[1], u[3], u[0],
+ a[0], a[8], a[12], a[18], a[3], a[9], b[12], b[8], b[0], b[9], b[18], b[3],
+ c[9], c[3], c[12], c[0], c[8], c[18], u[1], u[3], u[0], a[0], a[8], a[9],
+ a[3], a[18], a[7], a[12], a[13], b[18], b[0], b[8], b[3], b[9], b[12],
+ b[13], b[7], c[13], c[7], c[12], c[18], c[0], c[8], c[9], c[3], u[1], u[2],
+ u[0], a[1], a[2], a[0], a[8], a[12], a[10], b[12], b[0], b[8], b[10], b[1],
+ b[2], c[10], c[2], c[0], c[8], c[1], c[12], u[1], u[2], u[3], u[0], a[10],
+ a[2], a[1], a[16], a[9], a[3], a[0], a[12], a[8], a[18], a[4], b[1], b[2],
+ b[8], b[10], b[12], b[0], b[18], b[9], b[3], b[4], b[16], c[4], c[16], c[8],
+ c[9], c[0], c[3], c[1], c[12], c[10], c[2], c[18], u[1], u[2], u[3], u[0],
+ a[10], a[2], a[4], a[16], a[13], a[7], a[9], a[12], a[8], a[18], a[3], a[1],
+ a[11], b[10], b[8], b[2], b[16], b[3], b[4], b[12], b[1], b[18], b[9],
+ b[13], b[7], b[11], c[11], c[1], c[3], c[13], c[9], c[7], c[18], c[8],
+ c[12], c[10], c[2], c[4], c[16], u[1], u[2], u[0], a[8], a[12], a[17],
+ a[10], a[2], a[1], a[0], a[14], b[0], b[8], b[12], b[1], b[10], b[2], b[14],
+ b[17], c[14], c[17], c[10], c[0], c[8], c[2], c[12], c[1], u[1], u[2], u[3],
+ u[0], a[3], a[18], a[14], a[17], a[4], a[16], a[10], a[1], a[8], a[12],
+ a[2], a[9], a[5], b[17], b[2], b[14], b[12], b[8], b[1], b[10], b[9], b[3],
+ b[18], b[4], b[16], b[5], c[5], c[2], c[4], c[9], c[3], c[10], c[16], c[8],
+ c[1], c[18], c[12], c[14], c[17], u[1], u[2], u[3], u[0], a[14], a[17],
+ a[7], a[5], a[11], a[4], a[1], a[2], a[3], a[18], a[12], a[16], a[13],
+ b[14], b[2], b[17], b[16], b[5], b[1], b[18], b[12], b[3], b[4], b[13],
+ b[7], b[11], c[16], c[11], c[13], c[7], c[4], c[3], c[12], c[2], c[1],
+ c[18], c[14], c[17], c[5], u[2], a[10], a[2], a[6], a[14], a[17], b[8],
+ b[0], b[10], b[2], b[17], b[14], b[6], c[6], c[0], c[10], c[14], c[2], c[8],
+ c[17], u[2], a[10], a[6], a[14], b[0], b[10], b[14], b[6], c[10], c[0],
+ c[6], c[14], u[2], a[2], a[1], a[14], a[17], a[10], a[6], b[12], b[8],
+ b[10], b[2], b[1], b[14], b[17], b[6], c[6], c[8], c[14], c[10], c[2],
+ c[17], c[1], c[12], a[17], a[6], a[1], b[19], b[1], b[17], b[6], c[6],
+ c[19], c[1], c[17], a[1], a[14], a[17], a[6], a[2], b[19], b[12], b[2],
+ b[1], b[14], b[17], b[6], c[6], c[12], c[17], c[2], c[1], c[14], c[19],
+ a[8], a[12], a[19], b[12], b[8], b[19], c[8], c[12], c[19], a[14], a[17],
+ a[6], b[8], b[2], b[10], b[14], b[17], b[6], c[10], c[14], c[6], c[8],
+ c[17], c[2], a[17], a[6], a[14], b[12], b[1], b[2], b[14], b[17], b[6],
+ c[2], c[6], c[14], c[17], c[12], c[1], a[6], a[17], b[19], b[1], b[17],
+ b[6], c[1], c[17], c[6], c[19], u[3], a[11], a[9], a[13], a[15], a[4],
+ b[11], b[9], b[4], b[13], b[15], c[4], c[11], c[13], c[0], c[10], c[9],
+ c[15], u[3], a[13], a[15], a[9], b[13], b[9], b[15], c[9], c[13], c[0],
+ c[15], a[14], a[6], b[0], b[10], b[14], b[6], c[0], c[14], c[10], c[6],
+ a[13], a[15], a[7], b[13], b[15], b[7], c[7], c[8], c[9], c[3], c[13],
+ c[15], a[13], a[7], a[15], b[13], b[7], b[15], c[12], c[3], c[18], c[13],
+ c[7], c[15], a[6], b[10], b[14], b[6], c[10], c[6], c[14], a[7], a[15],
+ b[7], b[15], c[19], c[18], c[7], c[15], a[6], b[2], b[17], b[14], b[6],
+ c[14], c[6], c[2], c[17], a[15], b[15], c[3], c[13], c[7], c[15], a[15],
+ b[15], c[18], c[7], c[15], a[6], b[1], b[17], b[6], c[17], c[6], c[1], a[6],
+ a[17], a[5], b[18], b[1], b[17], b[16], b[6], b[5], c[16], c[5], c[6],
+ c[17], c[18], c[1], a[5], a[6], a[14], b[14], b[9], b[10], b[4], b[6], b[5],
+ c[6], c[9], c[10], c[5], c[4], c[14], a[11], a[15], a[13], b[11], b[15],
+ b[13], c[4], c[13], c[14], c[5], c[11], c[15], a[15], b[15], c[13], c[4],
+ c[11], c[15], b[17], b[6], c[6], c[17], a[5], a[11], a[4], b[5], b[11],
+ b[4], b[13], b[15], c[14], c[4], c[13], c[6], c[15], c[5], c[11], b[14],
+ b[6], c[14], c[6], c[13], c[15], a[6], b[16], b[17], b[6], b[5], c[5], c[6],
+ c[16], c[17], c[7], c[15], b[5], b[6], c[5], c[6], a[6], b[11], b[6], b[5],
+ c[6], c[11], c[5], u[3], a[15], a[4], a[11], a[13], a[9], a[5], b[4], b[13],
+ b[5], b[9], b[11], b[15], c[5], c[11], c[10], c[9], c[15], c[14], c[4],
+ c[13], u[2], a[11], a[14], a[5], a[4], a[10], a[6], b[14], b[4], b[6],
+ b[10], b[9], b[13], b[5], b[11], c[6], c[5], c[10], c[9], c[11], c[13],
+ c[14], c[4], a[15], b[15], c[7], c[16], c[15], c[11], b[6], c[6], c[15],
+ a[11], b[11], b[15], c[5], c[11], c[15], c[6], a[5], b[15], b[5], b[11],
+ c[6], c[5], c[15], c[11], a[15], b[15], c[11], c[15], c[5], c[15], c[11],
+ a[13], a[15], a[11], b[13], b[11], b[15], c[11], c[15], c[9], c[10], c[4],
+ c[13], a[1], a[17], a[19], b[19], b[1], b[17], c[17], c[19], c[1], u[1],
+ a[8], a[12], a[9], a[3], a[18], a[13], a[19], a[7], b[8], b[12], b[9],
+ b[18], b[3], b[19], b[13], b[7], c[13], c[7], c[19], c[8], c[12], c[9],
+ c[3], c[18], a[6], b[6], b[4], b[14], b[5], c[4], c[14], c[5], c[6], a[6],
+ a[5], a[14], b[6], b[5], b[13], b[14], b[4], b[11], c[14], c[13], c[4],
+ c[11], c[6], c[5], a[12], a[19], b[19], b[12], c[12], c[19], u[2], a[16],
+ a[6], a[5], a[14], a[2], a[1], a[17], a[4], b[17], b[6], b[1], b[12], b[2],
+ b[18], b[3], b[14], b[4], b[16], b[5], c[17], c[3], c[5], c[4], c[16],
+ c[14], c[2], c[12], c[18], c[1], c[6], u[1], a[1], a[0], a[8], a[12], a[19],
+ a[10], a[2], b[19], b[0], b[8], b[12], b[10], b[2], b[1], c[10], c[2], c[1],
+ c[8], c[12], c[0], c[19], a[19], b[19], c[19], a[15], a[13], b[15], b[13],
+ c[13], c[15], c[0], c[9], a[16], a[11], a[15], a[7], a[18], b[16], b[18],
+ b[11], b[7], b[15], c[7], c[15], c[19], c[18], c[1], c[16], c[11], a[11],
+ a[15], a[7], b[11], b[7], b[15], c[15], c[16], c[7], c[17], c[11], c[5],
+ u[3], a[4], a[11], a[15], a[3], a[9], a[7], a[13], a[16], b[9], b[4], b[11],
+ b[13], b[3], b[16], b[7], b[15], c[16], c[13], c[15], c[7], c[8], c[9],
+ c[10], c[2], c[3], c[4], c[11], a[2], a[1], a[3], a[18], a[12], a[19], a[4],
+ a[16], b[2], b[19], b[1], b[12], b[3], b[18], b[16], b[4], c[4], c[16],
+ c[19], c[18], c[12], c[3], c[2], c[1], a[5], a[14], a[17], a[6], b[6],
+ b[17], b[3], b[2], b[14], b[16], b[4], b[5], c[6], c[4], c[5], c[14], c[3],
+ c[2], c[16], c[17], u[1], a[17], a[5], a[11], a[16], a[1], a[18], a[19],
+ a[7], b[17], b[1], b[5], b[19], b[18], b[16], b[7], b[11], c[7], c[16],
+ c[18], c[11], c[19], c[1], c[17], c[5], u[2], a[4], a[16], a[6], a[5],
+ a[17], a[10], a[2], a[14], b[6], b[14], b[2], b[8], b[10], b[3], b[9],
+ b[17], b[4], b[16], b[5], c[14], c[9], c[4], c[5], c[10], c[17], c[8],
+ c[16], c[3], c[2], c[6], u[1], a[18], a[14], a[17], a[4], a[16], a[2],
+ a[12], a[19], a[1], a[5], a[3], b[14], b[1], b[17], b[19], b[12], b[2],
+ b[3], b[18], b[4], b[16], b[5], c[5], c[1], c[16], c[3], c[18], c[2], c[12],
+ c[4], c[19], c[14], c[17], a[17], a[16], a[1], a[19], a[5], a[18], b[17],
+ b[19], b[1], b[18], b[16], b[5], c[5], c[18], c[1], c[19], c[16], c[17],
+ u[1], a[10], a[2], a[1], a[9], a[3], a[18], a[8], a[19], a[12], a[4], a[16],
+ b[10], b[1], b[12], b[2], b[19], b[8], b[9], b[3], b[18], b[4], b[16], c[4],
+ c[16], c[12], c[3], c[8], c[18], c[9], c[19], c[10], c[2], c[1], a[1],
+ a[16], a[7], a[18], a[19], a[11], b[1], b[19], b[16], b[18], b[7], b[11],
+ c[11], c[18], c[7], c[19], c[1], c[16], a[6], a[5], a[17], a[1], a[16],
+ b[6], b[19], b[1], b[18], b[17], b[16], b[5], c[18], c[16], c[17], c[1],
+ c[5], c[19], c[6], a[11], a[15], a[7], b[11], b[7], b[15], c[15], c[18],
+ c[1], c[7], c[16], c[11], u[1], a[2], a[1], a[4], a[16], a[13], a[7], a[3],
+ a[19], a[12], a[18], a[11], b[2], b[12], b[1], b[4], b[18], b[16], b[19],
+ b[3], b[13], b[7], b[11], c[11], c[18], c[7], c[3], c[13], c[12], c[19],
+ c[2], c[1], c[4], c[16], u[3], a[5], a[15], a[16], a[4], a[13], a[7], a[3],
+ a[11], b[4], b[5], b[11], b[16], b[7], b[3], b[13], b[15], c[11], c[15],
+ c[13], c[2], c[3], c[4], c[14], c[17], c[16], c[7], c[5], u[2], a[6], a[11],
+ a[17], a[14], a[4], a[16], a[2], a[5], b[14], b[6], b[5], b[17], b[16],
+ b[2], b[3], b[4], b[7], b[13], b[11], c[5], c[13], c[11], c[4], c[2], c[3],
+ c[14], c[7], c[17], c[16], c[6], a[1], a[18], a[19], a[16], b[1], b[19],
+ b[18], b[16], c[16], c[19], c[18], c[1], u[3], a[5], a[11], a[16], a[7],
+ a[18], a[15], b[5], b[16], b[18], b[7], b[11], b[15], c[15], c[11], c[7],
+ c[1], c[18], c[16], c[17], c[5], u[3], a[4], a[16], a[11], a[15], a[13],
+ a[18], a[3], a[7], b[4], b[3], b[16], b[7], b[11], b[18], b[13], b[15],
+ c[7], c[15], c[13], c[12], c[3], c[2], c[1], c[18], c[4], c[16], c[11],
+ a[5], a[11], a[16], b[5], b[16], b[7], b[11], b[15], c[15], c[11], c[17],
+ c[16], c[7], c[5], c[6], a[11], a[7], a[13], a[15], b[13], b[11], b[15],
+ b[7], c[15], c[3], c[2], c[13], c[4], c[16], c[7], c[11], a[6], a[5], a[17],
+ b[6], b[7], b[17], b[16], b[5], b[11], c[11], c[5], c[17], c[7], c[16],
+ c[6], a[15], b[15], c[15], c[9], c[13], a[8], a[12], a[19], a[10], a[2],
+ a[1], b[8], b[12], b[19], b[2], b[10], b[1], c[10], c[2], c[1], c[12],
+ c[19], c[8], a[12], a[19], a[2], a[1], b[12], b[19], b[1], b[2], c[2], c[1],
+ c[19], c[12], a[19], a[1], b[19], b[1], c[1], c[19], u[3], a[9], a[13],
+ a[7], a[15], a[3], b[7], b[9], b[13], b[3], b[15], c[15], c[3], c[7], c[0],
+ c[8], c[9], c[13], u[3], a[9], a[3], a[13], a[7], a[18], a[15], b[9], b[3],
+ b[7], b[13], b[18], b[15], c[15], c[18], c[8], c[12], c[9], c[3], c[13],
+ c[7], a[3], a[18], a[13], a[7], a[15], b[3], b[18], b[13], b[7], b[15],
+ c[15], c[12], c[19], c[3], c[18], c[13], c[7], a[18], a[7], a[15], b[18],
+ b[7], b[15], c[15], c[19], c[18], c[7], a[19], a[0], a[8], a[12], b[8],
+ b[0], b[12], b[19], c[0], c[8], c[12], c[19], u[2], a[6], a[5], a[17],
+ a[16], a[1], a[11], b[6], b[17], b[1], b[18], b[16], b[7], b[5], b[11],
+ c[7], c[11], c[5], c[16], c[1], c[18], c[17], c[6], u[2], a[4], a[6], a[14],
+ a[10], a[5], b[6], b[10], b[0], b[9], b[14], b[4], b[5], c[6], c[14], c[0],
+ c[4], c[9], c[10], c[5], u[1], a[19], a[12], a[0], a[8], b[0], b[8], b[19],
+ b[12], c[0], c[8], c[12], c[19], u[1], a[0], a[8], a[12], a[19], a[18],
+ a[9], a[3], b[19], b[0], b[12], b[8], b[9], b[3], b[18], c[9], c[3], c[18],
+ c[19], c[0], c[8], c[12], a[8], a[12], a[19], a[9], a[3], a[18], b[8],
+ b[19], b[12], b[3], b[9], b[18], c[9], c[3], c[18], c[8], c[12], c[19],
+ a[12], a[19], a[3], a[18], b[12], b[19], b[18], b[3], c[3], c[18], c[12],
+ c[19], a[19], a[18], b[19], b[18], c[18], c[19], u[1], a[12], a[19], a[10],
+ a[2], a[1], a[14], a[8], a[17], b[8], b[12], b[19], b[10], b[2], b[1],
+ b[14], b[17], c[14], c[17], c[2], c[8], c[12], c[1], c[10], c[19], a[19],
+ a[2], a[1], a[14], a[17], a[12], b[12], b[19], b[2], b[1], b[17], b[14],
+ c[14], c[17], c[1], c[12], c[19], c[2], a[12], a[19], a[3], a[18], a[13],
+ a[7], b[12], b[19], b[3], b[18], b[7], b[13], c[13], c[7], c[12], c[19],
+ c[3], c[18], a[19], a[18], a[7], b[19], b[18], b[7], c[7], c[19], c[18]
+ };
+ for (int i = 0; i < 1968; i++)
+ macaulay_matrix[indices[i]] = values[i];
+ }
+#endif
+
+ // Transforms a 3 - vector in a 3x9 matrix such that :
+ // R * v = leftMultiplyMatrix(v) * vec(R)
+ // Where R is a rotation matrix and vec(R) converts R to a 9x1 vector.
+ Matx leftMultiplyMatrix(const Vec3d& v) const {
+ Matx left_mult_mat = Matx::zeros();
+ left_mult_mat(0,0) = v[0]; left_mult_mat(0,1) = v[1]; left_mult_mat(0,2) = v[2];
+ left_mult_mat(1,3) = v[0]; left_mult_mat(1,4) = v[1]; left_mult_mat(1,5) = v[2];
+ left_mult_mat(2,6) = v[0]; left_mult_mat(2,7) = v[1]; left_mult_mat(2,8) = v[2];
+ return left_mult_mat;
+ }
+
+ // Extracts the coefficients of the Jacobians of the LS cost function (which is
+ // parameterized by the 3 rotation coefficients s1, s2, s3).
+ void extractJacobianCoefficients(const double * const D,
+ double f1_coeff[20], double f2_coeff[20], double f3_coeff[20]) const {
+ f1_coeff[0] =
+ 2 * D[5] - 2 * D[7] + 2 * D[41] - 2 * D[43] + 2 * D[45] +
+ 2 * D[49] + 2 * D[53] - 2 * D[63] - 2 * D[67] - 2 * D[71] +
+ 2 * D[77] - 2 * D[79]; // constant term
+ f1_coeff[1] =
+ (6 * D[1] + 6 * D[3] + 6 * D[9] - 6 * D[13] - 6 * D[17] +
+ 6 * D[27] - 6 * D[31] - 6 * D[35] - 6 * D[37] - 6 * D[39] -
+ 6 * D[73] - 6 * D[75]); // s1^2 * s2
+ f1_coeff[2] =
+ (4 * D[6] - 4 * D[2] + 8 * D[14] - 8 * D[16] - 4 * D[18] +
+ 4 * D[22] + 4 * D[26] + 8 * D[32] - 8 * D[34] + 4 * D[38] -
+ 4 * D[42] + 8 * D[46] + 8 * D[48] + 4 * D[54] - 4 * D[58] -
+ 4 * D[62] - 8 * D[64] - 8 * D[66] + 4 * D[74] -
+ 4 * D[78]); // s1 * s2
+ f1_coeff[3] =
+ (4 * D[1] - 4 * D[3] + 4 * D[9] - 4 * D[13] - 4 * D[17] +
+ 8 * D[23] - 8 * D[25] - 4 * D[27] + 4 * D[31] + 4 * D[35] -
+ 4 * D[37] + 4 * D[39] + 8 * D[47] + 8 * D[51] + 8 * D[59] -
+ 8 * D[61] - 8 * D[65] - 8 * D[69] - 4 * D[73] +
+ 4 * D[75]); // s1 * s3
+ f1_coeff[4] = (8 * D[10] - 8 * D[20] - 8 * D[30] + 8 * D[50] +
+ 8 * D[60] - 8 * D[70]); // s2 * s3
+ f1_coeff[5] =
+ (4 * D[14] - 2 * D[6] - 2 * D[2] + 4 * D[16] - 2 * D[18] +
+ 2 * D[22] - 2 * D[26] + 4 * D[32] + 4 * D[34] + 2 * D[38] +
+ 2 * D[42] + 4 * D[46] + 4 * D[48] - 2 * D[54] + 2 * D[58] -
+ 2 * D[62] + 4 * D[64] + 4 * D[66] - 2 * D[74] -
+ 2 * D[78]); // s2^2 * s3
+ f1_coeff[6] = (2 * D[13] - 2 * D[3] - 2 * D[9] - 2 * D[1] -
+ 2 * D[17] - 2 * D[27] + 2 * D[31] - 2 * D[35] +
+ 2 * D[37] + 2 * D[39] - 2 * D[73] - 2 * D[75]); // s2^3
+ f1_coeff[7] =
+ (4 * D[8] - 4 * D[0] + 8 * D[20] + 8 * D[24] + 4 * D[40] +
+ 8 * D[56] + 8 * D[60] + 4 * D[72] - 4 * D[80]); // s1 * s3^2
+ f1_coeff[8] =
+ (4 * D[0] - 4 * D[40] - 4 * D[44] + 8 * D[50] - 8 * D[52] -
+ 8 * D[68] + 8 * D[70] - 4 * D[76] - 4 * D[80]); // s1
+ f1_coeff[9] = (2 * D[2] + 2 * D[6] + 4 * D[14] - 4 * D[16] +
+ 2 * D[18] + 2 * D[22] + 2 * D[26] - 4 * D[32] +
+ 4 * D[34] + 2 * D[38] + 2 * D[42] + 4 * D[46] -
+ 4 * D[48] + 2 * D[54] + 2 * D[58] + 2 * D[62] -
+ 4 * D[64] + 4 * D[66] + 2 * D[74] + 2 * D[78]); // s3
+ f1_coeff[10] = (2 * D[1] + 2 * D[3] + 2 * D[9] + 2 * D[13] +
+ 2 * D[17] - 4 * D[23] + 4 * D[25] + 2 * D[27] +
+ 2 * D[31] + 2 * D[35] + 2 * D[37] + 2 * D[39] -
+ 4 * D[47] + 4 * D[51] + 4 * D[59] - 4 * D[61] +
+ 4 * D[65] - 4 * D[69] + 2 * D[73] + 2 * D[75]); // s2
+ f1_coeff[11] =
+ (2 * D[17] - 2 * D[3] - 2 * D[9] - 2 * D[13] - 2 * D[1] +
+ 4 * D[23] + 4 * D[25] - 2 * D[27] - 2 * D[31] + 2 * D[35] -
+ 2 * D[37] - 2 * D[39] + 4 * D[47] + 4 * D[51] + 4 * D[59] +
+ 4 * D[61] + 4 * D[65] + 4 * D[69] + 2 * D[73] +
+ 2 * D[75]); // s2 * s3^2
+ f1_coeff[12] =
+ (6 * D[5] - 6 * D[7] - 6 * D[41] + 6 * D[43] + 6 * D[45] -
+ 6 * D[49] - 6 * D[53] - 6 * D[63] + 6 * D[67] + 6 * D[71] -
+ 6 * D[77] + 6 * D[79]); // s1^2
+ f1_coeff[13] =
+ (2 * D[7] - 2 * D[5] + 4 * D[11] + 4 * D[15] + 4 * D[19] -
+ 4 * D[21] - 4 * D[29] - 4 * D[33] - 2 * D[41] + 2 * D[43] -
+ 2 * D[45] - 2 * D[49] + 2 * D[53] + 4 * D[55] - 4 * D[57] +
+ 2 * D[63] + 2 * D[67] - 2 * D[71] + 2 * D[77] -
+ 2 * D[79]); // s3^2
+ f1_coeff[14] =
+ (2 * D[7] - 2 * D[5] - 4 * D[11] + 4 * D[15] - 4 * D[19] -
+ 4 * D[21] - 4 * D[29] + 4 * D[33] + 2 * D[41] - 2 * D[43] -
+ 2 * D[45] + 2 * D[49] - 2 * D[53] + 4 * D[55] + 4 * D[57] +
+ 2 * D[63] - 2 * D[67] + 2 * D[71] - 2 * D[77] +
+ 2 * D[79]); // s2^2
+ f1_coeff[15] =
+ (2 * D[26] - 2 * D[6] - 2 * D[18] - 2 * D[22] - 2 * D[2] -
+ 2 * D[38] - 2 * D[42] - 2 * D[54] - 2 * D[58] + 2 * D[62] +
+ 2 * D[74] + 2 * D[78]); // s3^3
+ f1_coeff[16] =
+ (4 * D[5] + 4 * D[7] + 8 * D[11] + 8 * D[15] + 8 * D[19] +
+ 8 * D[21] + 8 * D[29] + 8 * D[33] - 4 * D[41] - 4 * D[43] +
+ 4 * D[45] - 4 * D[49] - 4 * D[53] + 8 * D[55] + 8 * D[57] +
+ 4 * D[63] - 4 * D[67] - 4 * D[71] - 4 * D[77] -
+ 4 * D[79]); // s1 * s2 * s3
+ f1_coeff[17] =
+ (4 * D[4] - 4 * D[0] + 8 * D[10] + 8 * D[12] + 8 * D[28] +
+ 8 * D[30] + 4 * D[36] - 4 * D[40] + 4 * D[80]); // s1 * s2^2
+ f1_coeff[18] =
+ (6 * D[2] + 6 * D[6] + 6 * D[18] - 6 * D[22] - 6 * D[26] -
+ 6 * D[38] - 6 * D[42] + 6 * D[54] - 6 * D[58] - 6 * D[62] -
+ 6 * D[74] - 6 * D[78]); // s1^2 * s3
+ f1_coeff[19] =
+ (4 * D[0] - 4 * D[4] - 4 * D[8] - 4 * D[36] + 4 * D[40] +
+ 4 * D[44] - 4 * D[72] + 4 * D[76] + 4 * D[80]); // s1^3
+
+ f2_coeff[0] =
+ -2 * D[2] + 2 * D[6] - 2 * D[18] - 2 * D[22] - 2 * D[26] -
+ 2 * D[38] + 2 * D[42] + 2 * D[54] + 2 * D[58] + 2 * D[62] -
+ 2 * D[74] + 2 * D[78]; // constant term
+ f2_coeff[1] =
+ (4 * D[4] - 4 * D[0] + 8 * D[10] + 8 * D[12] + 8 * D[28] +
+ 8 * D[30] + 4 * D[36] - 4 * D[40] + 4 * D[80]); // s1^2 * s2
+ f2_coeff[2] =
+ (4 * D[7] - 4 * D[5] - 8 * D[11] + 8 * D[15] - 8 * D[19] -
+ 8 * D[21] - 8 * D[29] + 8 * D[33] + 4 * D[41] - 4 * D[43] -
+ 4 * D[45] + 4 * D[49] - 4 * D[53] + 8 * D[55] + 8 * D[57] +
+ 4 * D[63] - 4 * D[67] + 4 * D[71] - 4 * D[77] +
+ 4 * D[79]); // s1 * s2
+ f2_coeff[3] = (8 * D[10] - 8 * D[20] - 8 * D[30] + 8 * D[50] +
+ 8 * D[60] - 8 * D[70]); // s1 * s3
+ f2_coeff[4] =
+ (4 * D[3] - 4 * D[1] - 4 * D[9] + 4 * D[13] - 4 * D[17] -
+ 8 * D[23] - 8 * D[25] + 4 * D[27] - 4 * D[31] + 4 * D[35] +
+ 4 * D[37] - 4 * D[39] - 8 * D[47] + 8 * D[51] + 8 * D[59] +
+ 8 * D[61] - 8 * D[65] + 8 * D[69] - 4 * D[73] +
+ 4 * D[75]); // s2 * s3
+ f2_coeff[5] =
+ (6 * D[41] - 6 * D[7] - 6 * D[5] + 6 * D[43] - 6 * D[45] +
+ 6 * D[49] - 6 * D[53] - 6 * D[63] + 6 * D[67] - 6 * D[71] -
+ 6 * D[77] - 6 * D[79]); // s2^2 * s3
+ f2_coeff[6] =
+ (4 * D[0] - 4 * D[4] + 4 * D[8] - 4 * D[36] + 4 * D[40] -
+ 4 * D[44] + 4 * D[72] - 4 * D[76] + 4 * D[80]); // s2^3
+ f2_coeff[7] =
+ (2 * D[17] - 2 * D[3] - 2 * D[9] - 2 * D[13] - 2 * D[1] +
+ 4 * D[23] + 4 * D[25] - 2 * D[27] - 2 * D[31] + 2 * D[35] -
+ 2 * D[37] - 2 * D[39] + 4 * D[47] + 4 * D[51] + 4 * D[59] +
+ 4 * D[61] + 4 * D[65] + 4 * D[69] + 2 * D[73] +
+ 2 * D[75]); // s1 * s3^2
+ f2_coeff[8] = (2 * D[1] + 2 * D[3] + 2 * D[9] + 2 * D[13] +
+ 2 * D[17] - 4 * D[23] + 4 * D[25] + 2 * D[27] +
+ 2 * D[31] + 2 * D[35] + 2 * D[37] + 2 * D[39] -
+ 4 * D[47] + 4 * D[51] + 4 * D[59] - 4 * D[61] +
+ 4 * D[65] - 4 * D[69] + 2 * D[73] + 2 * D[75]); // s1
+ f2_coeff[9] = (2 * D[5] + 2 * D[7] - 4 * D[11] + 4 * D[15] -
+ 4 * D[19] + 4 * D[21] + 4 * D[29] - 4 * D[33] +
+ 2 * D[41] + 2 * D[43] + 2 * D[45] + 2 * D[49] +
+ 2 * D[53] + 4 * D[55] - 4 * D[57] + 2 * D[63] +
+ 2 * D[67] + 2 * D[71] + 2 * D[77] + 2 * D[79]); // s3
+ f2_coeff[10] =
+ (8 * D[20] - 4 * D[8] - 4 * D[0] - 8 * D[24] + 4 * D[40] -
+ 8 * D[56] + 8 * D[60] - 4 * D[72] - 4 * D[80]); // s2
+ f2_coeff[11] =
+ (4 * D[0] - 4 * D[40] + 4 * D[44] + 8 * D[50] + 8 * D[52] +
+ 8 * D[68] + 8 * D[70] + 4 * D[76] - 4 * D[80]); // s2 * s3^2
+ f2_coeff[12] =
+ (2 * D[6] - 2 * D[2] + 4 * D[14] - 4 * D[16] - 2 * D[18] +
+ 2 * D[22] + 2 * D[26] + 4 * D[32] - 4 * D[34] + 2 * D[38] -
+ 2 * D[42] + 4 * D[46] + 4 * D[48] + 2 * D[54] - 2 * D[58] -
+ 2 * D[62] - 4 * D[64] - 4 * D[66] + 2 * D[74] -
+ 2 * D[78]); // s1^2
+ f2_coeff[13] =
+ (2 * D[2] - 2 * D[6] + 4 * D[14] + 4 * D[16] + 2 * D[18] +
+ 2 * D[22] - 2 * D[26] - 4 * D[32] - 4 * D[34] + 2 * D[38] -
+ 2 * D[42] + 4 * D[46] - 4 * D[48] - 2 * D[54] - 2 * D[58] +
+ 2 * D[62] + 4 * D[64] - 4 * D[66] - 2 * D[74] +
+ 2 * D[78]); // s3^2
+ f2_coeff[14] =
+ (6 * D[2] - 6 * D[6] + 6 * D[18] - 6 * D[22] + 6 * D[26] -
+ 6 * D[38] + 6 * D[42] - 6 * D[54] + 6 * D[58] - 6 * D[62] +
+ 6 * D[74] - 6 * D[78]); // s2^2
+ f2_coeff[15] =
+ (2 * D[53] - 2 * D[7] - 2 * D[41] - 2 * D[43] - 2 * D[45] -
+ 2 * D[49] - 2 * D[5] - 2 * D[63] - 2 * D[67] + 2 * D[71] +
+ 2 * D[77] + 2 * D[79]); // s3^3
+ f2_coeff[16] =
+ (8 * D[14] - 4 * D[6] - 4 * D[2] + 8 * D[16] - 4 * D[18] +
+ 4 * D[22] - 4 * D[26] + 8 * D[32] + 8 * D[34] + 4 * D[38] +
+ 4 * D[42] + 8 * D[46] + 8 * D[48] - 4 * D[54] + 4 * D[58] -
+ 4 * D[62] + 8 * D[64] + 8 * D[66] - 4 * D[74] -
+ 4 * D[78]); // s1 * s2 * s3
+ f2_coeff[17] =
+ (6 * D[13] - 6 * D[3] - 6 * D[9] - 6 * D[1] - 6 * D[17] -
+ 6 * D[27] + 6 * D[31] - 6 * D[35] + 6 * D[37] + 6 * D[39] -
+ 6 * D[73] - 6 * D[75]); // s1 * s2^2
+ f2_coeff[18] =
+ (2 * D[5] + 2 * D[7] + 4 * D[11] + 4 * D[15] + 4 * D[19] +
+ 4 * D[21] + 4 * D[29] + 4 * D[33] - 2 * D[41] - 2 * D[43] +
+ 2 * D[45] - 2 * D[49] - 2 * D[53] + 4 * D[55] + 4 * D[57] +
+ 2 * D[63] - 2 * D[67] - 2 * D[71] - 2 * D[77] -
+ 2 * D[79]); // s1^2 * s3
+ f2_coeff[19] =
+ (2 * D[1] + 2 * D[3] + 2 * D[9] - 2 * D[13] - 2 * D[17] +
+ 2 * D[27] - 2 * D[31] - 2 * D[35] - 2 * D[37] - 2 * D[39] -
+ 2 * D[73] - 2 * D[75]); // s1^3
+
+ f3_coeff[0] =
+ 2 * D[1] - 2 * D[3] + 2 * D[9] + 2 * D[13] + 2 * D[17] -
+ 2 * D[27] - 2 * D[31] - 2 * D[35] + 2 * D[37] - 2 * D[39] +
+ 2 * D[73] - 2 * D[75]; // constant term
+ f3_coeff[1] =
+ (2 * D[5] + 2 * D[7] + 4 * D[11] + 4 * D[15] + 4 * D[19] +
+ 4 * D[21] + 4 * D[29] + 4 * D[33] - 2 * D[41] - 2 * D[43] +
+ 2 * D[45] - 2 * D[49] - 2 * D[53] + 4 * D[55] + 4 * D[57] +
+ 2 * D[63] - 2 * D[67] - 2 * D[71] - 2 * D[77] -
+ 2 * D[79]); // s1^2 * s2
+ f3_coeff[2] = (8 * D[10] - 8 * D[20] - 8 * D[30] + 8 * D[50] +
+ 8 * D[60] - 8 * D[70]); // s1 * s2
+ f3_coeff[3] =
+ (4 * D[7] - 4 * D[5] + 8 * D[11] + 8 * D[15] + 8 * D[19] -
+ 8 * D[21] - 8 * D[29] - 8 * D[33] - 4 * D[41] + 4 * D[43] -
+ 4 * D[45] - 4 * D[49] + 4 * D[53] + 8 * D[55] - 8 * D[57] +
+ 4 * D[63] + 4 * D[67] - 4 * D[71] + 4 * D[77] -
+ 4 * D[79]); // s1 * s3
+ f3_coeff[4] =
+ (4 * D[2] - 4 * D[6] + 8 * D[14] + 8 * D[16] + 4 * D[18] +
+ 4 * D[22] - 4 * D[26] - 8 * D[32] - 8 * D[34] + 4 * D[38] -
+ 4 * D[42] + 8 * D[46] - 8 * D[48] - 4 * D[54] - 4 * D[58] +
+ 4 * D[62] + 8 * D[64] - 8 * D[66] - 4 * D[74] +
+ 4 * D[78]); // s2 * s3
+ f3_coeff[5] =
+ (4 * D[0] - 4 * D[40] + 4 * D[44] + 8 * D[50] + 8 * D[52] +
+ 8 * D[68] + 8 * D[70] + 4 * D[76] - 4 * D[80]); // s2^2 * s3
+ f3_coeff[6] = (2 * D[41] - 2 * D[7] - 2 * D[5] + 2 * D[43] -
+ 2 * D[45] + 2 * D[49] - 2 * D[53] - 2 * D[63] +
+ 2 * D[67] - 2 * D[71] - 2 * D[77] - 2 * D[79]); // s2^3
+ f3_coeff[7] =
+ (6 * D[26] - 6 * D[6] - 6 * D[18] - 6 * D[22] - 6 * D[2] -
+ 6 * D[38] - 6 * D[42] - 6 * D[54] - 6 * D[58] + 6 * D[62] +
+ 6 * D[74] + 6 * D[78]); // s1 * s3^2
+ f3_coeff[8] = (2 * D[2] + 2 * D[6] + 4 * D[14] - 4 * D[16] +
+ 2 * D[18] + 2 * D[22] + 2 * D[26] - 4 * D[32] +
+ 4 * D[34] + 2 * D[38] + 2 * D[42] + 4 * D[46] -
+ 4 * D[48] + 2 * D[54] + 2 * D[58] + 2 * D[62] -
+ 4 * D[64] + 4 * D[66] + 2 * D[74] + 2 * D[78]); // s1
+ f3_coeff[9] =
+ (8 * D[10] - 4 * D[4] - 4 * D[0] - 8 * D[12] - 8 * D[28] +
+ 8 * D[30] - 4 * D[36] - 4 * D[40] + 4 * D[80]); // s3
+ f3_coeff[10] = (2 * D[5] + 2 * D[7] - 4 * D[11] + 4 * D[15] -
+ 4 * D[19] + 4 * D[21] + 4 * D[29] - 4 * D[33] +
+ 2 * D[41] + 2 * D[43] + 2 * D[45] + 2 * D[49] +
+ 2 * D[53] + 4 * D[55] - 4 * D[57] + 2 * D[63] +
+ 2 * D[67] + 2 * D[71] + 2 * D[77] + 2 * D[79]); // s2
+ f3_coeff[11] =
+ (6 * D[53] - 6 * D[7] - 6 * D[41] - 6 * D[43] - 6 * D[45] -
+ 6 * D[49] - 6 * D[5] - 6 * D[63] - 6 * D[67] + 6 * D[71] +
+ 6 * D[77] + 6 * D[79]); // s2 * s3^2
+ f3_coeff[12] =
+ (2 * D[1] - 2 * D[3] + 2 * D[9] - 2 * D[13] - 2 * D[17] +
+ 4 * D[23] - 4 * D[25] - 2 * D[27] + 2 * D[31] + 2 * D[35] -
+ 2 * D[37] + 2 * D[39] + 4 * D[47] + 4 * D[51] + 4 * D[59] -
+ 4 * D[61] - 4 * D[65] - 4 * D[69] - 2 * D[73] +
+ 2 * D[75]); // s1^2
+ f3_coeff[13] =
+ (6 * D[3] - 6 * D[1] - 6 * D[9] - 6 * D[13] + 6 * D[17] +
+ 6 * D[27] + 6 * D[31] - 6 * D[35] - 6 * D[37] + 6 * D[39] +
+ 6 * D[73] - 6 * D[75]); // s3^2
+ f3_coeff[14] =
+ (2 * D[3] - 2 * D[1] - 2 * D[9] + 2 * D[13] - 2 * D[17] -
+ 4 * D[23] - 4 * D[25] + 2 * D[27] - 2 * D[31] + 2 * D[35] +
+ 2 * D[37] - 2 * D[39] - 4 * D[47] + 4 * D[51] + 4 * D[59] +
+ 4 * D[61] - 4 * D[65] + 4 * D[69] - 2 * D[73] +
+ 2 * D[75]); // s2^2
+ f3_coeff[15] =
+ (4 * D[0] + 4 * D[4] - 4 * D[8] + 4 * D[36] + 4 * D[40] -
+ 4 * D[44] - 4 * D[72] - 4 * D[76] + 4 * D[80]); // s3^3
+ f3_coeff[16] =
+ (4 * D[17] - 4 * D[3] - 4 * D[9] - 4 * D[13] - 4 * D[1] +
+ 8 * D[23] + 8 * D[25] - 4 * D[27] - 4 * D[31] + 4 * D[35] -
+ 4 * D[37] - 4 * D[39] + 8 * D[47] + 8 * D[51] + 8 * D[59] +
+ 8 * D[61] + 8 * D[65] + 8 * D[69] + 4 * D[73] +
+ 4 * D[75]); // s1 * s2 * s3
+ f3_coeff[17] =
+ (4 * D[14] - 2 * D[6] - 2 * D[2] + 4 * D[16] - 2 * D[18] +
+ 2 * D[22] - 2 * D[26] + 4 * D[32] + 4 * D[34] + 2 * D[38] +
+ 2 * D[42] + 4 * D[46] + 4 * D[48] - 2 * D[54] + 2 * D[58] -
+ 2 * D[62] + 4 * D[64] + 4 * D[66] - 2 * D[74] -
+ 2 * D[78]); // s1 * s2^2
+ f3_coeff[18] =
+ (4 * D[8] - 4 * D[0] + 8 * D[20] + 8 * D[24] + 4 * D[40] +
+ 8 * D[56] + 8 * D[60] + 4 * D[72] - 4 * D[80]); // s1^2 * s3
+ f3_coeff[19] =
+ (2 * D[2] + 2 * D[6] + 2 * D[18] - 2 * D[22] - 2 * D[26] -
+ 2 * D[38] - 2 * D[42] + 2 * D[54] - 2 * D[58] - 2 * D[62] -
+ 2 * D[74] - 2 * D[78]); // s1^3
+ }
+};
+Ptr DLSPnP::create(const Mat &points_, const Mat &calib_norm_pts, const Mat &K) {
+ return makePtr(points_, calib_norm_pts, K);
+}
+}}
diff --git a/modules/calib3d/src/usac/essential_solver.cpp b/modules/calib3d/src/usac/essential_solver.cpp
new file mode 100644
index 0000000000..a2e24676ad
--- /dev/null
+++ b/modules/calib3d/src/usac/essential_solver.cpp
@@ -0,0 +1,273 @@
+// This file is part of OpenCV project.
+// It is subject to the license terms in the LICENSE file found in the top-level directory
+// of this distribution and at http://opencv.org/license.html.
+
+#include "../precomp.hpp"
+#include "../usac.hpp"
+#if defined(HAVE_EIGEN)
+#include
+#elif defined(HAVE_LAPACK)
+#include "opencv_lapack.h"
+#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
+*/
+class EssentialMinimalSolverStewenius5ptsImpl : public EssentialMinimalSolverStewenius5pts {
+private:
+ // Points must be calibrated K^-1 x
+ const Mat * points_mat;
+#if defined(HAVE_EIGEN) || defined(HAVE_LAPACK)
+ const float * const pts;
+#endif
+public:
+ explicit EssentialMinimalSolverStewenius5ptsImpl (const Mat &points_) :
+ points_mat(&points_)
+#if defined(HAVE_EIGEN) || defined(HAVE_LAPACK)
+ , pts((float*)points_.data)
+#endif
+ {}
+
+#if defined(HAVE_LAPACK) || defined(HAVE_EIGEN)
+ int estimate (const std::vector &sample, std::vector &models) const override {
+ // (1) Extract 4 null vectors from linear equations of epipolar constraint
+ std::vector coefficients(45); // 5 pts=rows, 9 columns
+ auto *coefficients_ = &coefficients[0];
+ for (int i = 0; i < 5; i++) {
+ const int smpl = 4 * sample[i];
+ const auto x1 = pts[smpl], y1 = pts[smpl+1], x2 = pts[smpl+2], y2 = pts[smpl+3];
+ (*coefficients_++) = x2 * x1;
+ (*coefficients_++) = x2 * y1;
+ (*coefficients_++) = x2;
+ (*coefficients_++) = y2 * x1;
+ (*coefficients_++) = y2 * y1;
+ (*coefficients_++) = y2;
+ (*coefficients_++) = x1;
+ (*coefficients_++) = y1;
+ (*coefficients_++) = 1;
+ }
+
+ const int num_cols = 9, num_e_mat = 4;
+ double ee[36]; // 9*4
+ // eliminate linear equations
+ Math::eliminateUpperTriangular(coefficients, 5, num_cols);
+ 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;
+ }
+ }
+
+ const Matx null_space(ee);
+ const Matx null_space_mat[3][3] = {
+ {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)}};
+
+ // (2) Use the rank constraint and the trace constraint to build ten third-order polynomial
+ // equations in the three unknowns. The monomials are ordered in GrLex order and
+ // represented in a 10×20 matrix, where each row corresponds to an equation and each column
+ // corresponds to a monomial
+ Matx eet[3][3];
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ // compute EE Transpose
+ // Shorthand for multiplying the Essential matrix with its transpose.
+ eet[i][j] = 2 * (multPolysDegOne(null_space_mat[i][0].val, null_space_mat[j][0].val) +
+ multPolysDegOne(null_space_mat[i][1].val, null_space_mat[j][1].val) +
+ multPolysDegOne(null_space_mat[i][2].val, null_space_mat[j][2].val));
+
+ const Matx trace = eet[0][0] + eet[1][1] + eet[2][2];
+ Mat_ constraint_mat(10, 20);
+ // Trace constraint
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ Mat(multPolysDegOneAndTwo(eet[i][0].val, null_space_mat[0][j].val) +
+ multPolysDegOneAndTwo(eet[i][1].val, null_space_mat[1][j].val) +
+ multPolysDegOneAndTwo(eet[i][2].val, null_space_mat[2][j].val) -
+ 0.5 * multPolysDegOneAndTwo(trace.val, null_space_mat[i][j].val))
+ .copyTo(constraint_mat.row(3 * i + j));
+
+ // 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) +
+ 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));
+
+#ifdef HAVE_EIGEN
+ const Eigen::Matrix constraint_mat_eig((double *) constraint_mat.data);
+ // (3) Compute the Gröbner basis. This turns out to be as simple as performing a
+ // Gauss-Jordan elimination on the 10×20 matrix
+ const Eigen::Matrix eliminated_mat_eig = constraint_mat_eig.block<10, 10>(0, 0)
+ .fullPivLu().solve(constraint_mat_eig.block<10, 10>(0, 10));
+
+ // (4) Compute the 10×10 action matrix for multiplication by one of the un-knowns.
+ // This is a simple matter of extracting the correct elements fromthe eliminated
+ // 10×20 matrix and organising them to form the action matrix.
+ Eigen::Matrix action_mat_eig = Eigen::Matrix::Zero();
+ action_mat_eig.block<3, 10>(0, 0) = eliminated_mat_eig.block<3, 10>(0, 0);
+ action_mat_eig.block<2, 10>(3, 0) = eliminated_mat_eig.block<2, 10>(4, 0);
+ action_mat_eig.row(5) = eliminated_mat_eig.row(7);
+ action_mat_eig(6, 0) = -1.0;
+ action_mat_eig(7, 1) = -1.0;
+ action_mat_eig(8, 3) = -1.0;
+ action_mat_eig(9, 6) = -1.0;
+
+ // (5) Compute the left eigenvectors of the action matrix
+ Eigen::EigenSolver> eigensolver(action_mat_eig);
+ const Eigen::VectorXcd &eigenvalues = eigensolver.eigenvalues();
+ const auto * const eig_vecs_ = (double *) eigensolver.eigenvectors().real().data();
+#else
+ Matx A = constraint_mat.colRange(0, 10),
+ B = constraint_mat.colRange(10, 20), eliminated_mat;
+ if (!solve(A, B, eliminated_mat, DECOMP_LU)) return 0;
+
+ Mat eliminated_mat_dyn = Mat(eliminated_mat);
+ Mat action_mat = Mat_::zeros(10, 10);
+ eliminated_mat_dyn.rowRange(0,3).copyTo(action_mat.rowRange(0,3));
+ eliminated_mat_dyn.rowRange(4,6).copyTo(action_mat.rowRange(3,5));
+ eliminated_mat_dyn.row(7).copyTo(action_mat.row(5));
+ auto * action_mat_data = (double *) action_mat.data;
+ action_mat_data[60] = -1.0; // 6 row, 0 col
+ action_mat_data[71] = -1.0; // 7 row, 1 col
+ action_mat_data[83] = -1.0; // 8 row, 3 col
+ action_mat_data[96] = -1.0; // 9 row, 6 col
+
+ int mat_order = 10, info, lda = 10, ldvl = 10, ldvr = 1, lwork = 100;
+ double wr[10], wi[10] = {0}, eig_vecs[100], work[100]; // 10 = mat_order, 100 = lwork
+ char jobvl = 'V', jobvr = 'N'; // only left eigen vectors are computed
+ dgeev_(&jobvl, &jobvr, &mat_order, action_mat_data, &lda, wr, wi, eig_vecs, &ldvl,
+ nullptr, &ldvr, work, &lwork, &info);
+ if (info != 0) return 0;
+#endif
+
+ models = std::vector(); models.reserve(10);
+
+ // Read off the values for the three unknowns at all the solution points and
+ // back-substitute to obtain the solutions for the essential matrix.
+ for (int i = 0; i < 10; i++)
+ // process only real solutions
+#ifdef HAVE_EIGEN
+ if (eigenvalues(i).imag() == 0) {
+ Mat_ model(3, 3);
+ auto * model_data = (double *) model.data;
+ const int eig_i = 20 * i + 12; // eigen stores imaginary values too
+ for (int j = 0; j < 9; j++)
+ model_data[j] = ee[j ] * eig_vecs_[eig_i ] + ee[j+9 ] * eig_vecs_[eig_i+2] +
+ ee[j+18] * eig_vecs_[eig_i+4] + ee[j+27] * eig_vecs_[eig_i+6];
+#else
+ if (wi[i] == 0) {
+ Mat_ model (3,3);
+ auto * model_data = (double *) model.data;
+ const int eig_i = 10 * i + 6;
+ for (int j = 0; j < 9; j++)
+ model_data[j] = ee[j ]*eig_vecs[eig_i ] + ee[j+9 ]*eig_vecs[eig_i+1] +
+ ee[j+18]*eig_vecs[eig_i+2] + ee[j+27]*eig_vecs[eig_i+3];
+#endif
+ models.emplace_back(model);
+ }
+ return static_cast(models.size());
+#else
+ int estimate (const std::vector &/*sample*/, std::vector &/*models*/) const override {
+ CV_Error(cv::Error::StsNotImplemented, "To use essential matrix solver LAPACK or Eigen has to be installed!");
+#endif
+ }
+
+ // number of possible solutions is 0,2,4,6,8,10
+ int getMaxNumberOfSolutions () const override { return 10; }
+ int getSampleSize() const override { return 5; }
+ Ptr clone () const override {
+ return makePtr(*points_mat);
+ }
+private:
+ /*
+ * Multiply two polynomials of degree one with unknowns x y z
+ * @p = (p1 x + p2 y + p3 z + p4) [p1 p2 p3 p4]
+ * @q = (q1 x + q2 y + q3 z + q4) [q1 q2 q3 a4]
+ * @result is a new polynomial in x^2 xy y^2 xz yz z^2 x y z 1 of size 10
+ */
+ static inline Matx multPolysDegOne(const double * const p,
+ const double * const q) {
+ return
+ {p[0]*q[0], p[0]*q[1]+p[1]*q[0], p[1]*q[1], p[0]*q[2]+p[2]*q[0], p[1]*q[2]+p[2]*q[1],
+ p[2]*q[2], p[0]*q[3]+p[3]*q[0], p[1]*q[3]+p[3]*q[1], p[2]*q[3]+p[3]*q[2], p[3]*q[3]};
+ }
+
+ /*
+ * Multiply two polynomials with unknowns x y z
+ * @p is of size 10 and @q is of size 4
+ * @p = (p1 x^2 + p2 xy + p3 y^2 + p4 xz + p5 yz + p6 z^2 + p7 x + p8 y + p9 z + p10)
+ * @q = (q1 x + q2 y + q3 z + a4) [q1 q2 q3 q4]
+ * @result is a new polynomial of size 20
+ * x^3 x^2y xy^2 y^3 x^2z xyz y^2z xz^2 yz^2 z^3 x^2 xy y^2 xz yz z^2 x y z 1
+ */
+ static inline Matx