diff --git a/.gitignore b/.gitignore index c1e46e3..261ecef 100644 --- a/.gitignore +++ b/.gitignore @@ -2,6 +2,7 @@ build docs/_build tests/bin +dist/ # Python *.egg-info diff --git a/src/pymagsac/graph-cut-ransac/include/mathfunc.h b/src/pymagsac/graph-cut-ransac/include/mathfunc.h new file mode 100644 index 0000000..b5e98f4 --- /dev/null +++ b/src/pymagsac/graph-cut-ransac/include/mathfunc.h @@ -0,0 +1,171 @@ +#pragma once +#include +#include +using namespace cv; +/* + Finds real roots of cubic, quadratic or linear equation. + The original code has been taken from Ken Turkowski web page + (http://www.worldserver.com/turk/opensource/) and adopted for OpenCV. + Here is the copyright notice. + + ----------------------------------------------------------------------- + Copyright (C) 1978-1999 Ken Turkowski. + + All rights reserved. + + Warranty Information + Even though I have reviewed this software, I make no warranty + or representation, either express or implied, with respect to this + software, its quality, accuracy, merchantability, or fitness for a + particular purpose. As a result, this software is provided "as is," + and you, its user, are assuming the entire risk as to its quality + and accuracy. + + This code may be used and freely distributed as long as it includes + this copyright notice and the above warranty information. + ----------------------------------------------------------------------- +*/ + +int solveCubic( Mat& coeffs, Mat& roots ) +{ + + const int n0 = 3; + int ctype = coeffs.type(); + + CV_Assert( ctype == CV_32F || ctype == CV_64F ); + CV_Assert( (coeffs.size() == Size(n0, 1) || + coeffs.size() == Size(n0+1, 1) || + coeffs.size() == Size(1, n0) || + coeffs.size() == Size(1, n0+1)) ); + + + int i = -1, n = 0; + double a0 = 1., a1, a2, a3; + double x0 = 0., x1 = 0., x2 = 0.; + int ncoeffs = coeffs.rows + coeffs.cols - 1; + + if( ctype == CV_32FC1 ) + { + if( ncoeffs == 4 ) + a0 = coeffs.at(++i); + + a1 = coeffs.at(i+1); + a2 = coeffs.at(i+2); + a3 = coeffs.at(i+3); + } + else + { + if( ncoeffs == 4 ) + a0 = coeffs.at(++i); + + a1 = coeffs.at(i+1); + a2 = coeffs.at(i+2); + a3 = coeffs.at(i+3); + } + + + if( a0 == 0 ) + { + if( a1 == 0 ) + { + if( a2 == 0 ) + n = a3 == 0 ? -1 : 0; + else + { + // linear equation + x0 = -a3/a2; + n = 1; + } + } + else + { + // quadratic equation + double d = a2*a2 - 4*a1*a3; + if( d >= 0 ) + { + d = std::sqrt(d); + double q1 = (-a2 + d) * 0.5; + double q2 = (a2 + d) * -0.5; + if( fabs(q1) > fabs(q2) ) + { + x0 = q1 / a1; + x1 = a3 / q1; + } + else + { + x0 = q2 / a1; + x1 = a3 / q2; + } + n = d > 0 ? 2 : 1; + } + } + } + else + { + a0 = 1./a0; + a1 *= a0; + a2 *= a0; + a3 *= a0; + + double Q = (a1 * a1 - 3 * a2) * (1./9); + double R = (2 * a1 * a1 * a1 - 9 * a1 * a2 + 27 * a3) * (1./54); + double Qcubed = Q * Q * Q; + double d = Qcubed - R * R; + + if( d > 0 ) + { + double theta = acos(R / sqrt(Qcubed)); + double sqrtQ = sqrt(Q); + double t0 = -2 * sqrtQ; + double t1 = theta * (1./3); + double t2 = a1 * (1./3); + x0 = t0 * cos(t1) - t2; + x1 = t0 * cos(t1 + (2.*CV_PI/3)) - t2; + x2 = t0 * cos(t1 + (4.*CV_PI/3)) - t2; + n = 3; + } + else if( d == 0 ) + { + if(R >= 0) + { + x0 = -2*pow(R, 1./3) - a1/3; + x1 = pow(R, 1./3) - a1/3; + } + else + { + x0 = 2*pow(-R, 1./3) - a1/3; + x1 = -pow(-R, 1./3) - a1/3; + } + x2 = 0; + n = x0 == x1 ? 1 : 2; + x1 = x0 == x1 ? 0 : x1; + } + else + { + double e; + d = sqrt(-d); + e = pow(d + fabs(R), 1./3); + if( R > 0 ) + e = -e; + x0 = (e + Q / e) - a1 * (1./3); + n = 1; + } + } + + if( roots.type() == CV_32FC1 ) + { + roots.at(0) = (float)x0; + roots.at(1) = (float)x1; + roots.at(2) = (float)x2; + } + else + { + roots.at(0) = x0; + roots.at(1) = x1; + roots.at(2) = x2; + } + + return n; +} + + diff --git a/src/pymagsac/graph-cut-ransac/include/solver_fundamental_matrix_seven_point.h b/src/pymagsac/graph-cut-ransac/include/solver_fundamental_matrix_seven_point.h index 089abf4..cebc5e6 100644 --- a/src/pymagsac/graph-cut-ransac/include/solver_fundamental_matrix_seven_point.h +++ b/src/pymagsac/graph-cut-ransac/include/solver_fundamental_matrix_seven_point.h @@ -32,9 +32,9 @@ // Please contact the author of this library if you have any questions. // Author: Daniel Barath (barath.daniel@sztaki.mta.hu) #pragma once - #include "solver_engine.h" #include "fundamental_estimator.h" +#include "mathfunc.h" namespace gcransac { @@ -85,7 +85,9 @@ namespace gcransac Eigen::MatrixXd coefficients(sample_number_, 9); const double *data_ptr = reinterpret_cast(data_.data); const int cols = data_.cols; - double c[4]; + double c[4], r[3] = {0}; + cv::Mat coeffs( 1, 4, CV_64F, c ); + cv::Mat roots( 1, 3, CV_64F, r ); double t0, t1, t2; int i, n; @@ -168,9 +170,9 @@ namespace gcransac t1 = f2[3] * f2[8] - f2[5] * f2[6]; t2 = f2[3] * f2[7] - f2[4] * f2[6]; - c[0] = f2[0] * t0 - f2[1] * t1 + f2[2] * t2; + c[3] = f2[0] * t0 - f2[1] * t1 + f2[2] * t2; - c[1] = f1[0] * t0 - f1[1] * t1 + f1[2] * t2 - + c[2] = f1[0] * t0 - f1[1] * t1 + f1[2] * t2 - f1[3] * (f2[1] * f2[8] - f2[2] * f2[7]) + f1[4] * (f2[0] * f2[8] - f2[2] * f2[6]) - f1[5] * (f2[0] * f2[7] - f2[1] * f2[6]) + @@ -182,7 +184,7 @@ namespace gcransac t1 = f1[3] * f1[8] - f1[5] * f1[6]; t2 = f1[3] * f1[7] - f1[4] * f1[6]; - c[2] = f2[0] * t0 - f2[1] * t1 + f2[2] * t2 - + c[1] = f2[0] * t0 - f2[1] * t1 + f2[2] * t2 - f2[3] * (f1[1] * f1[8] - f1[2] * f1[7]) + f2[4] * (f1[0] * f1[8] - f1[2] * f1[6]) - f2[5] * (f1[0] * f1[7] - f1[1] * f1[6]) + @@ -190,20 +192,16 @@ namespace gcransac f2[7] * (f1[0] * f1[5] - f1[2] * f1[3]) + f2[8] * (f1[0] * f1[4] - f1[1] * f1[3]); - c[3] = f1[0] * t0 - f1[1] * t1 + f1[2] * t2; - - // solve the cubic equation; there can be 1 to 3 roots ... - Eigen::Matrix polynomial; - for (auto i = 0; i < 4; ++i) - polynomial(i) = c[i]; - Eigen::PolynomialSolver psolve(polynomial); + c[0] = f1[0] * t0 - f1[1] * t1 + f1[2] * t2; - std::vector real_roots; - psolve.realRoots(real_roots); + n = solveCubic( coeffs, roots ); - n = real_roots.size(); if (n < 1 || n > 3) return false; + + std::vector real_roots(n); + for(i = 0; i < n; i++) + real_roots[i] = r[i]; double f[8]; for (const double &root : real_roots) @@ -233,4 +231,4 @@ namespace gcransac } } } -} \ No newline at end of file +} diff --git a/src/pymagsac/graph-cut-ransac/include/types.h b/src/pymagsac/graph-cut-ransac/include/types.h index 83543e7..dc3c056 100644 --- a/src/pymagsac/graph-cut-ransac/include/types.h +++ b/src/pymagsac/graph-cut-ransac/include/types.h @@ -142,4 +142,4 @@ namespace gcransac } }; } -} \ No newline at end of file +} diff --git a/src/pymagsac/include/estimators.h b/src/pymagsac/include/estimators.h index b5b1d30..58a1978 100644 --- a/src/pymagsac/include/estimators.h +++ b/src/pymagsac/include/estimators.h @@ -128,7 +128,7 @@ namespace magsac { // The default estimator for essential matrix fitting typedef estimator::EssentialMatrixEstimator // The solver used for fitting a model to a non-minimal sample + gcransac::estimator::solver::FundamentalMatrixEightPointSolver> // The solver used for fitting a model to a non-minimal sample DefaultEssentialMatrixEstimator; // The default estimator for fundamental matrix fitting diff --git a/src/pymagsac/include/magsac.h b/src/pymagsac/include/magsac.h index e27f67e..a271524 100644 --- a/src/pymagsac/include/magsac.h +++ b/src/pymagsac/include/magsac.h @@ -41,7 +41,8 @@ class MAGSAC ModelEstimator& estimator_, gcransac::sampler::Sampler &sampler_, gcransac::Model &obtained_model_, - int &iteration_number_); + int &iteration_number_, + ModelScore &model_score_); // The score of the estimated model bool scoreLess( const ModelScore &score_1_, @@ -59,6 +60,11 @@ class MAGSAC { reference_inlier_outlier_threshold = threshold_; } + + double getReferenceThreshold() + { + return interrupting_threshold; + } void applyPostProcessing(bool value_) { @@ -132,7 +138,8 @@ bool MAGSAC::run( ModelEstimator& estimator_, gcransac::sampler::Sampler &sampler_, gcransac::Model& obtained_model_, - int& iteration_number_) + int& iteration_number_, + ModelScore &model_score_) { // Initialize variables std::chrono::time_point start, end; // Variables for time measuring: start and end times @@ -190,8 +197,8 @@ bool MAGSAC::run( } // If the method was not able to generate any usable models, break the cycle. - if (unsuccessful_model_generations >= max_unsuccessful_model_generations) - break; + //if (unsuccessful_model_generations >= max_unsuccessful_model_generations) + // break; // Select the so-far-the-best from the estimated models for (const auto &model : models) @@ -253,6 +260,7 @@ bool MAGSAC::run( obtained_model_ = so_far_the_best_model; iteration_number_ = iteration; + model_score_ = so_far_the_best_score; return so_far_the_best_score.score > 0; } diff --git a/src/pymagsac/include/magsac_python.hpp b/src/pymagsac/include/magsac_python.hpp index b0c53f6..8033b3e 100644 --- a/src/pymagsac/include/magsac_python.hpp +++ b/src/pymagsac/include/magsac_python.hpp @@ -11,8 +11,22 @@ int findFundamentalMatrix_(std::vector& srcPts, double sigma_th = 3.0, double conf = 0.99, int max_iters = 10000, - int partition_num = 5); + int partition_num = 5, + int core_num = 1); +int findEssentialMatrix_(std::vector& srcPts, + std::vector& dstPts, + std::vector& inliers, + std::vector& E, + std::vector& intrinsics_src, + std::vector& intrinsics_dst, + double sigma_th = 3.0, + double conf = 0.99, + int max_iters = 10000, + int partition_num = 5, + int core_num = 1, + double minimum_inlier_ratio_in_validity_check = 0.1, + double normalizing_multiplier = 1e-3); int findHomography_(std::vector& srcPts, diff --git a/src/pymagsac/src/bindings.cpp b/src/pymagsac/src/bindings.cpp index 7f5e909..a5956f1 100644 --- a/src/pymagsac/src/bindings.cpp +++ b/src/pymagsac/src/bindings.cpp @@ -13,7 +13,8 @@ py::tuple findFundamentalMatrix(py::array_t x1y1_, double sigma_th, double conf, int max_iters, - int partition_num) { + int partition_num, + int core_num) { py::buffer_info buf1 = x1y1_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; @@ -52,7 +53,8 @@ py::tuple findFundamentalMatrix(py::array_t x1y1_, sigma_th, conf, max_iters, - partition_num); + partition_num, + core_num); py::array_t inliers_ = py::array_t(NUM_TENTS); py::buffer_info buf3 = inliers_.request(); @@ -69,6 +71,100 @@ py::tuple findFundamentalMatrix(py::array_t x1y1_, ptr2[i] = F[i]; return py::make_tuple(F_,inliers_); } + +py::tuple findEssentialMatrix(py::array_t x1y1_, + py::array_t x2y2_, + py::array_t K1_, + py::array_t K2_, + double sigma_th, + double conf, + int max_iters, + int partition_num, + int core_num, + double minimum_inlier_ratio_in_validity_check, + double normalizing_multiplier){ + py::buffer_info buf1 = x1y1_.request(); + size_t NUM_TENTS = buf1.shape[0]; + size_t DIM = buf1.shape[1]; + + if (DIM != 2) { + throw std::invalid_argument( "x1y1 should be an array with dims [n,2], n>=7" ); + } + if (NUM_TENTS < 7) { + throw std::invalid_argument( "x1y1 should be an array with dims [n,2], n>=7"); + } + py::buffer_info buf1a = x2y2_.request(); + size_t NUM_TENTSa = buf1a.shape[0]; + size_t DIMa = buf1a.shape[1]; + + if (DIMa != 2) { + throw std::invalid_argument( "x2y2 should be an array with dims [n,2], n>=7" ); + } + if (NUM_TENTSa != NUM_TENTS) { + throw std::invalid_argument( "x1y1 and x2y2 should be the same size"); + } + + py::buffer_info bufk1 = K1_.request(); + if (bufk1.shape[0]!= 3 || bufk1.shape[1] != 3){ + throw std::invalid_argument( "K1 should be an array with dims [3,3]" ); + } + + py::buffer_info bufk2 = K2_.request(); + if (bufk2.shape[0]!= 3 || bufk2.shape[1] != 3){ + throw std::invalid_argument( "K2 should be an array with dims [3,3]" ); + } + + + double *ptr1 = (double *) buf1.ptr; + std::vector x1y1; + x1y1.assign(ptr1, ptr1 + buf1.size); + + double *ptr1a = (double *) buf1a.ptr; + std::vector x2y2; + x2y2.assign(ptr1a, ptr1a + buf1a.size); + + double *ptrk1 = (double *) bufk1.ptr; + std::vector K1; + K1.assign(ptrk1, ptrk1 + bufk1.size); + + double *ptrk2 = (double *) bufk2.ptr; + std::vector K2; + K2.assign(ptrk2, ptrk2 + bufk2.size); + + + + std::vector E(9); + std::vector inliers(NUM_TENTS); + + int num_inl = findEssentialMatrix_(x1y1, + x2y2, + inliers, + E, + K1, + K2, + sigma_th, + conf, + max_iters, + partition_num, + core_num, + minimum_inlier_ratio_in_validity_check, + normalizing_multiplier); + + py::array_t inliers_ = py::array_t(NUM_TENTS); + py::buffer_info buf3 = inliers_.request(); + bool *ptr3 = (bool *)buf3.ptr; + for (size_t i = 0; i < NUM_TENTS; i++) + ptr3[i] = inliers[i]; + if (num_inl == 0){ + return py::make_tuple(pybind11::cast(Py_None),inliers_); + } + py::array_t E_ = py::array_t({3,3}); + py::buffer_info buf2 = E_.request(); + double *ptr2 = (double *)buf2.ptr; + for (size_t i = 0; i < 9; i++) + ptr2[i] = E[i]; + return py::make_tuple(E_,inliers_); +} py::tuple findHomography(py::array_t x1y1_, py::array_t x2y2_, @@ -143,6 +239,7 @@ PYBIND11_PLUGIN(pymagsac) { :toctree: _generate findFundamentalMatrix, + findEssentialMatrix, findHomography, )doc"); @@ -153,7 +250,21 @@ PYBIND11_PLUGIN(pymagsac) { py::arg("sigma_th") = 1.0, py::arg("conf") = 0.99, py::arg("max_iters") = 10000, - py::arg("partition_num") = 2); + py::arg("partition_num") = 2, + py::arg("core_num") = 1); + + m.def("findEssentialMatrix", &findEssentialMatrix, R"doc(some doc)doc", + py::arg("x1y1"), + py::arg("x2y2"), + py::arg("K1"), + py::arg("K2"), + py::arg("sigma_th") = 1e-4, + py::arg("conf") = 0.999999, + py::arg("max_iters") = 10000, + py::arg("partition_num") = 20, + py::arg("core_num") = 1, + py::arg("minimum_inlier_ratio_in_validity_check") = 0.1, + py::arg("normalizing_multiplier") = 1e-3); m.def("findHomography", &findHomography, R"doc(some doc)doc", diff --git a/src/pymagsac/src/magsac_python.cpp b/src/pymagsac/src/magsac_python.cpp index 51c4730..d94226c 100644 --- a/src/pymagsac/src/magsac_python.cpp +++ b/src/pymagsac/src/magsac_python.cpp @@ -16,7 +16,8 @@ int findFundamentalMatrix_(std::vector& srcPts, double sigma_max, double conf, int max_iters, - int partition_num) + int partition_num, + int core_num = 1) { magsac::utils::DefaultFundamentalMatrixEstimator estimator(0.1); // The robust homography estimator class containing the @@ -25,7 +26,7 @@ int findFundamentalMatrix_(std::vector& srcPts, MAGSAC magsac; magsac.setMaximumThreshold(sigma_max); // The maximum noise scale sigma allowed //magsac.setInterruptingThreshold(sigma_th / 3.0f); // The threshold used for speeding up the procedure - magsac.setCoreNumber(1); // The number of cores used to speed up sigma-consensus + magsac.setCoreNumber(core_num); // The number of cores used to speed up sigma-consensus magsac.setPartitionNumber(partition_num); // The number partitions used for speeding up sigma consensus. As the value grows, the algorithm become slower and, usually, more accurate. magsac.setIterationLimit(max_iters); //magsac.setTerminationCriterion(MAGSAC::TerminationCriterion::RansacCriterion, @@ -41,12 +42,14 @@ int findFundamentalMatrix_(std::vector& srcPts, } gcransac::sampler::UniformSampler main_sampler(&points); + ModelScore score; bool success = magsac.run(points, // The data points conf, // The required confidence in the results estimator, // The used estimator main_sampler, // The sampler used for selecting minimal samples in each iteration model, // The estimated model - max_iters); // The number of iterations + max_iters, // The number of iterations + score); // The score of the estimated model inliers.resize(num_tents); if (!success) { for (auto pt_idx = 0; pt_idx < points.rows; ++pt_idx) { @@ -76,6 +79,84 @@ int findFundamentalMatrix_(std::vector& srcPts, return num_inliers; } +int findEssentialMatrix_(std::vector& srcPts, + std::vector& dstPts, + std::vector& inliers, + std::vector& E, + std::vector& intrinsics_src, + std::vector& intrinsics_dst, + double sigma_max, + double conf, + int max_iters, + int partition_num, + int core_num = 1, + double minimum_inlier_ratio_in_validity_check = 0.1, + double normalizing_multiplier = 1e-3) +{ + + magsac::utils::DefaultEssentialMatrixEstimator estimator(Eigen::Map(intrinsics_src.data()), + Eigen::Map(intrinsics_dst.data()), + minimum_inlier_ratio_in_validity_check + ); // The robust homography estimator class containing the + gcransac::EssentialMatrix model; // The estimated model + + MAGSAC magsac; + magsac.setMaximumThreshold(sigma_max); // The maximum noise scale sigma allowed + //magsac.setInterruptingThreshold(sigma_th / 3.0f); // The threshold used for speeding up the procedure + magsac.setCoreNumber(core_num); // The number of cores used to speed up sigma-consensus + magsac.setPartitionNumber(partition_num); // The number partitions used for speeding up sigma consensus. As the value grows, the algorithm become slower and, usually, more accurate. + magsac.setIterationLimit(max_iters); + magsac.setReferenceThreshold(magsac.getReferenceThreshold() * normalizing_multiplier); + //magsac.setTerminationCriterion(MAGSAC::TerminationCriterion::RansacCriterion, + // sigma_th); // Use the standard RANSAC termination criterion since the MAGSAC one is too pessimistic and, thus, runs too long sometimes + + int num_tents = srcPts.size()/2; + cv::Mat points(num_tents, 4, CV_64F); + for (int i = 0; i < num_tents; ++i) { + points.at(i, 0) = srcPts[2*i]; + points.at(i, 1) = srcPts[2*i + 1]; + points.at(i, 2) = dstPts[2*i]; + points.at(i, 3) = dstPts[2*i + 1]; + } + gcransac::sampler::UniformSampler main_sampler(&points); + + ModelScore score; + bool success = magsac.run(points, // The data points + conf, // The required confidence in the results + estimator, // The used estimator + main_sampler, // The sampler used for selecting minimal samples in each iteration + model, // The estimated model + max_iters, // The number of iterations + score); // The score of the estimated model + inliers.resize(num_tents); + if (!success) { + for (auto pt_idx = 0; pt_idx < points.rows; ++pt_idx) { + inliers[pt_idx] = false; + } + E.resize(9); + for (int i = 0; i < 3; i++){ + for (int j = 0; j < 3; j++){ + E[i*3+j] = 0; + } + } + return 0; + } + int num_inliers = 0; + for (auto pt_idx = 0; pt_idx < points.rows; ++pt_idx) { + const int is_inlier = estimator.residual(points.row(pt_idx), model.descriptor) <= sigma_max; + inliers[pt_idx] = (bool)is_inlier; + num_inliers+=is_inlier; + } + + E.resize(9); + for (int i = 0; i < 3; i++){ + for (int j = 0; j < 3; j++){ + E[i*3+j] = (double)model.descriptor(i,j); + } + } + return num_inliers; +} + int findHomography_(std::vector& srcPts, std::vector& dstPts, @@ -106,12 +187,14 @@ int findHomography_(std::vector& srcPts, } gcransac::sampler::UniformSampler main_sampler(&points); + ModelScore score; bool success = magsac.run(points, // The data points conf, // The required confidence in the results estimator, // The used estimator main_sampler, // The sampler used for selecting minimal samples in each iteration model, // The estimated model - max_iters); // The number of iterations + max_iters, // The number of iterations + score); // The score of the estimated model inliers.resize(num_tents); if (!success) { for (auto pt_idx = 0; pt_idx < points.rows; ++pt_idx) {