fix the bug of Eigen::PolynomialSolver

.gitignore

@@ -2,6 +2,7 @@
 build
 docs/_build
 tests/bin
+dist/
 
 # Python
 *.egg-info

src/pymagsac/graph-cut-ransac/include/mathfunc.h

@@ -0,0 +1,171 @@
+#pragma once
+#include <opencv2/core.hpp>
+#include <iostream>
+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. <turk@computer.org>
+
+    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<float>(++i);
+
+        a1 = coeffs.at<float>(i+1);
+        a2 = coeffs.at<float>(i+2);
+        a3 = coeffs.at<float>(i+3);
+    }
+    else
+    {
+        if( ncoeffs == 4 )
+            a0 = coeffs.at<double>(++i);
+
+        a1 = coeffs.at<double>(i+1);
+        a2 = coeffs.at<double>(i+2);
+        a3 = coeffs.at<double>(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<float>(0) = (float)x0;
+        roots.at<float>(1) = (float)x1;
+        roots.at<float>(2) = (float)x2;
+    }
+    else
+    {
+        roots.at<double>(0) = x0;
+        roots.at<double>(1) = x1;
+        roots.at<double>(2) = x2;
+    }
+
+    return n;
+}
+
+

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<double *>(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,28 +184,24 @@ 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]) +
 					f2[6] * (f1[1] * f1[5] - f1[2] * f1[4]) -
 					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<double, 4, 1> polynomial;
-				for (auto i = 0; i < 4; ++i)
-					polynomial(i) = c[i];
-				Eigen::PolynomialSolver<double, 3> psolve(polynomial);
+				c[0] = f1[0] * t0 - f1[1] * t1 + f1[2] * t2;
 
-				std::vector<double> real_roots;
-				psolve.realRoots(real_roots);
+				n = solveCubic( coeffs, roots );
 
-				n = real_roots.size();
 				if (n < 1 || n > 3)
 					return false;
+
+				std::vector<double> 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
 			}
 		}
 	}
-}
+}

src/pymagsac/graph-cut-ransac/include/types.h

@@ -142,4 +142,4 @@ namespace gcransac
 			}
 		};
 	}
-}
+}

src/pymagsac/include/estimators.h

@@ -128,7 +128,7 @@ namespace magsac
 	{
 		// The default estimator for essential matrix fitting
 		typedef estimator::EssentialMatrixEstimator<gcransac::estimator::solver::EssentialMatrixFivePointSteweniusSolver, // The solver used for fitting a model to a minimal sample
-			gcransac::estimator::solver::EssentialMatrixFivePointSteweniusSolver> // 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

src/pymagsac/include/magsac.h

@@ -41,7 +41,8 @@ class MAGSAC
 		ModelEstimator& estimator_,
 		gcransac::sampler::Sampler<cv::Mat, size_t> &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<DatumType, ModelEstimator>::run(
 	ModelEstimator& estimator_,
 	gcransac::sampler::Sampler<cv::Mat, size_t> &sampler_,
 	gcransac::Model& obtained_model_,
-	int& iteration_number_)
+	int& iteration_number_,
+    ModelScore &model_score_)
 {
 	// Initialize variables
 	std::chrono::time_point<std::chrono::system_clock> start, end; // Variables for time measuring: start and end times
@@ -190,8 +197,8 @@ bool MAGSAC<DatumType, ModelEstimator>::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<DatumType, ModelEstimator>::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;
 }

src/pymagsac/include/magsac_python.hpp

@@ -11,8 +11,22 @@ int findFundamentalMatrix_(std::vector<double>& 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<double>& srcPts,
+                           std::vector<double>& dstPts,
+                           std::vector<bool>& inliers,
+                           std::vector<double>&  E,
+                           std::vector<double>& intrinsics_src,
+                           std::vector<double>& 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<double>& srcPts,