Adding SIFT and AC-based solver for absolute pose estimation.

src/pygcransac/include/estimators/solver_pnp_bundle_adjustment.h

@@ -122,6 +122,7 @@ namespace gcransac
 
 				// If there is no initial model provided estimate one
 				std::vector<Model> temp_models;
+				models_.clear();
 				if (models_.size() == 0)
 				{
 					cv::Mat inlier_image_points(sample_number_, 2, CV_64F),

src/pygcransac/include/estimators/solver_up2p.h

@@ -0,0 +1,180 @@
+// Copyright (c) 2020, Viktor Larsson
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//
+//     * Neither the name of the copyright holder nor the
+//       names of its contributors may be used to endorse or promote products
+//       derived from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+#pragma once
+
+#include "solver_engine.h"
+
+namespace gcransac
+{
+	namespace estimator
+	{
+		namespace solver
+		{
+			// This is the estimator class for estimating a homography matrix between two images. A model estimation method and error calculation method are implemented
+			class UP2PSolver : public SolverEngine
+			{
+			public:
+				UP2PSolver()
+				{
+				}
+
+				~UP2PSolver()
+				{
+				}
+
+
+				// Determines if there is a chance of returning multiple models
+				// the function 'estimateModel' is applied.
+				static constexpr bool returnMultipleModels()
+				{
+					return maximumSolutions() > 1;
+				}
+
+				// The maximum number of solutions returned by the estimator
+				static constexpr size_t maximumSolutions()
+				{
+					return 2;
+				}
+
+				// The minimum number of points required for the estimation
+				static constexpr size_t sampleSize()
+				{
+					return 2;
+				}
+
+				// It returns true/false depending on if the solver needs the gravity direction
+				// for the model estimation. 
+				static constexpr bool needsGravity()
+				{
+					return false;
+				}
+
+				// Estimate the model parameters from the given point sample
+				// using weighted fitting if possible.
+				OLGA_INLINE bool estimateModel(
+					const cv::Mat& data_, // The set of data points
+					const size_t *sample_, // The sample used for the estimation
+					size_t sample_number_, // The size of the sample
+					std::vector<Model> &models_, // The estimated model parameters
+					const double *weights_ = nullptr) const; // The weight for each point
+
+			protected:
+				/* Solves the quadratic equation a*x^2 + b*x + c = 0 */
+				int solve_quadratic_real(double a, double b, double c, double roots[2]) const
+				{
+					double b2m4ac = b * b - 4 * a * c;
+					if (b2m4ac < 0)
+						return 0;
+
+					double sq = std::sqrt(b2m4ac);
+
+					// Choose sign to avoid cancellations
+					roots[0] = (b > 0) ? (2 * c) / (-b - sq) : (2 * c) / (-b + sq);
+					roots[1] = c / (a * roots[0]);
+
+					return 2;
+				}
+			};
+
+			OLGA_INLINE bool UP2PSolver::estimateModel(
+				const cv::Mat& data_,
+				const size_t *sample_,
+				size_t sample_number_,
+				std::vector<Model> &models_,
+				const double *weights_) const
+			{
+				if (sample_ == nullptr)
+					sample_number_ = data_.rows;
+
+				const double * data_ptr = reinterpret_cast<double *>(data_.data);
+				const size_t columns = data_.cols;
+
+				Eigen::Vector2d x[2];
+				Eigen::Vector3d X[2];
+
+				for ( int k = 0; k < 2; k++ )
+				{
+					double * data_ptr;
+					if (sample_ == nullptr)
+						data_ptr = reinterpret_cast<double *>(data_.row(k).data);
+					else
+						data_ptr = reinterpret_cast<double *>(data_.row(sample_[k]).data);
+
+					x[k] = Eigen::Vector2d(data_ptr[0], data_ptr[1]);
+					X[k] = Eigen::Vector3d(data_ptr[2], data_ptr[3], data_ptr[4]);
+				}
+
+				Eigen::Matrix<double, 4, 4> A;
+				Eigen::Matrix<double, 4, 2> b;
+
+				A << -x[0](2), 0, x[0](0), X[0](0) * x[0](2) - X[0](2) * x[0](0), 0, -x[0](2), x[0](1),
+					-X[0](1) * x[0](2) - X[0](2) * x[0](1), -x[1](2), 0, x[1](0), X[1](0) * x[1](2) - X[1](2) * x[1](0), 0,
+					-x[1](2), x[1](1), -X[1](1) * x[1](2) - X[1](2) * x[1](1);
+				b << -2 * X[0](0) * x[0](0) - 2 * X[0](2) * x[0](2), X[0](2) * x[0](0) - X[0](0) * x[0](2), -2 * X[0](0) * x[0](1),
+					X[0](2) * x[0](1) - X[0](1) * x[0](2), -2 * X[1](0) * x[1](0) - 2 * X[1](2) * x[1](2),
+					X[1](2) * x[1](0) - X[1](0) * x[1](2), -2 * X[1](0) * x[1](1), X[1](2) * x[1](1) - X[1](1) * x[1](2);
+
+				// b = A.partialPivLu().solve(b);
+				b = A.inverse() * b;
+
+				const double c2 = b(3, 0);
+				const double c3 = b(3, 1);
+
+				double qq[2];
+				const int sols = solve_quadratic_real(1.0, c2, c3, qq);
+
+				models_.clear();
+				for (int i = 0; i < sols; ++i) {
+					const double q = qq[i];
+					const double q2 = q * q;
+					const double inv_norm = 1.0 / (1 + q2);
+					const double cq = (1 - q2) * inv_norm;
+					const double sq = 2 * q * inv_norm;
+
+					Eigen::Matrix3d R;
+					R.setIdentity();
+					R(0, 0) = cq;
+					R(0, 2) = sq;
+					R(2, 0) = -sq;
+					R(2, 2) = cq;
+
+					Eigen::Vector3d t;
+					t = b.block<3, 1>(0, 0) * q + b.block<3, 1>(0, 1);
+					t *= -inv_norm;
+
+					Model model;
+					model.descriptor.resize(3, 4);
+					model.descriptor << R, t;
+					models_.emplace_back(model);
+				}
+
+				return models_.size();
+			}
+		}
+	}
+}

src/pygcransac/include/gcransac_python.h

@@ -47,6 +47,132 @@ int findRigidTransform_(
 	// The number of RANSAC iterations done in the local optimization
 	int lo_number);
 
+// A method for estimating a the absolute pose given 2D-3D correspondences
+int find6DPoseACP1P_(
+	// The 2D-3D correspondences
+	std::vector<double>& correspondences,
+	// The probabilities for each 3D-3D point correspondence if available
+	std::vector<double> &point_probabilities,
+	// Output: the found inliers 
+	std::vector<bool>& inliers, 
+	// Output: the found 6D pose
+	std::vector<double> &pose, 
+	// The spatial coherence weight used in the local optimization
+	double spatial_coherence_weight, 
+	// The inlier-outlier threshold
+	double threshold, 
+	// The RANSAC confidence. Typical values are 0.95, 0.99.
+	double conf, 
+	// Maximum iteration number. I do not suggest setting it to lower than 1000.
+	int max_iters, 
+	// Minimum iteration number.
+	int min_iters, 
+	// A flag to decide if SPRT should be used to speed up the model verification. 
+	// It is not suggested if the inlier ratio is expected to be very low - it will fail in that case.
+	// Otherwise, it leads to a significant speed-up. 
+	bool use_sprt, 
+	// Expected inlier ratio for SPRT. Default: 0.1
+	double min_inlier_ratio_for_sprt,
+	// The identifier of the used sampler. 
+	// Options: 
+	//	(0) Uniform sampler 
+	// 	(1) PROSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. 
+	int sampler_id,
+	// The identifier of the used neighborhood structure. 
+	// 	(0) FLANN-based neighborhood. 
+	int neighborhood_id,
+	// The size of the neighborhood.
+	// If (0) FLANN is used, the size if the Euclidean distance in the correspondence space
+	double neighborhood_size,
+	// The variance parameter of the AR-Sampler. It is only used if that particular sampler is selected.
+	double sampler_variance,
+	// The number of RANSAC iterations done in the local optimization
+	int lo_number);
+
+// A method for estimating a the absolute pose given 2D-3D correspondences
+int find6DPoseSIFTP1P_(
+	// The 2D-3D correspondences
+	std::vector<double>& correspondences,
+	// The probabilities for each 3D-3D point correspondence if available
+	std::vector<double> &point_probabilities,
+	// Output: the found inliers 
+	std::vector<bool>& inliers, 
+	// Output: the found 6D pose
+	std::vector<double> &pose, 
+	// The spatial coherence weight used in the local optimization
+	double spatial_coherence_weight, 
+	// The inlier-outlier threshold
+	double threshold, 
+	// The RANSAC confidence. Typical values are 0.95, 0.99.
+	double conf, 
+	// Maximum iteration number. I do not suggest setting it to lower than 1000.
+	int max_iters, 
+	// Minimum iteration number.
+	int min_iters, 
+	// A flag to decide if SPRT should be used to speed up the model verification. 
+	// It is not suggested if the inlier ratio is expected to be very low - it will fail in that case.
+	// Otherwise, it leads to a significant speed-up. 
+	bool use_sprt, 
+	// Expected inlier ratio for SPRT. Default: 0.1
+	double min_inlier_ratio_for_sprt,
+	// The identifier of the used sampler. 
+	// Options: 
+	//	(0) Uniform sampler 
+	// 	(1) PROSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. 
+	int sampler_id,
+	// The identifier of the used neighborhood structure. 
+	// 	(0) FLANN-based neighborhood. 
+	int neighborhood_id,
+	// The size of the neighborhood.
+	// If (0) FLANN is used, the size if the Euclidean distance in the correspondence space
+	double neighborhood_size,
+	// The variance parameter of the AR-Sampler. It is only used if that particular sampler is selected.
+	double sampler_variance,
+	// The number of RANSAC iterations done in the local optimization
+	int lo_number);
+
+// A method for estimating a the absolute pose given 2D-3D correspondences
+int find6DPoseSIFTP2P_(
+	// The 2D-3D correspondences
+	std::vector<double>& correspondences,
+	// The probabilities for each 3D-3D point correspondence if available
+	std::vector<double> &point_probabilities,
+	// Output: the found inliers 
+	std::vector<bool>& inliers, 
+	// Output: the found 6D pose
+	std::vector<double> &pose, 
+	// The spatial coherence weight used in the local optimization
+	double spatial_coherence_weight, 
+	// The inlier-outlier threshold
+	double threshold, 
+	// The RANSAC confidence. Typical values are 0.95, 0.99.
+	double conf, 
+	// Maximum iteration number. I do not suggest setting it to lower than 1000.
+	int max_iters, 
+	// Minimum iteration number.
+	int min_iters, 
+	// A flag to decide if SPRT should be used to speed up the model verification. 
+	// It is not suggested if the inlier ratio is expected to be very low - it will fail in that case.
+	// Otherwise, it leads to a significant speed-up. 
+	bool use_sprt, 
+	// Expected inlier ratio for SPRT. Default: 0.1
+	double min_inlier_ratio_for_sprt,
+	// The identifier of the used sampler. 
+	// Options: 
+	//	(0) Uniform sampler 
+	// 	(1) PROSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. 
+	int sampler_id,
+	// The identifier of the used neighborhood structure. 
+	// 	(0) FLANN-based neighborhood. 
+	int neighborhood_id,
+	// The size of the neighborhood.
+	// If (0) FLANN is used, the size if the Euclidean distance in the correspondence space
+	double neighborhood_size,
+	// The variance parameter of the AR-Sampler. It is only used if that particular sampler is selected.
+	double sampler_variance,
+	// The number of RANSAC iterations done in the local optimization
+	int lo_number);
+
 // A method for estimating a the absolute pose given 2D-3D correspondences
 int find6DPose_(
 	// The 2D-3D correspondences