diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d78584..16419c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(GraphCutRANSAC LANGUAGES CXX) # indicate if OPENMP should be enabled -option(CREATE_SAMPLE_PROJECT "Create the Sample Project" ON) +option(CREATE_SAMPLE_PROJECT "Create the Sample Project" OFF) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -56,11 +56,10 @@ file(GLOB_RECURSE SRCS_GCRANSAC "${SOURCE_DIR}/include/*.cpp" ) - # Generate python module add_subdirectory(lib/pybind11) -pybind11_add_module(pygcransac ${SOURCE_DIR}/src/bindings.cpp ${SOURCE_DIR}/src/gcransac_python.cpp ${HDRS_GCRANSAC} ${SRCS_GCRANSAC} ) +pybind11_add_module(pygcransac ${SOURCE_DIR}/src/bindings.cpp ${HDRS_GCRANSAC} ${SRCS_GCRANSAC}) target_link_libraries(pygcransac PRIVATE ${OpenCV_LIBS} Eigen3::Eigen) install(TARGETS pygcransac DESTINATION .) diff --git a/src/pygcransac/include/estimators/estimator.h b/src/pygcransac/include/estimators/estimator.h index 3dae213..9930109 100644 --- a/src/pygcransac/include/estimators/estimator.h +++ b/src/pygcransac/include/estimators/estimator.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include "GCoptimization.h" namespace gcransac diff --git a/src/pygcransac/include/estimators/solver_acp1p_cayley.h b/src/pygcransac/include/estimators/solver_acp1p_cayley.h index 52c1ff4..63f5ed4 100644 --- a/src/pygcransac/include/estimators/solver_acp1p_cayley.h +++ b/src/pygcransac/include/estimators/solver_acp1p_cayley.h @@ -85,8 +85,8 @@ namespace gcransac const double *weights_ = nullptr) const; // The weight for each point protected: - void cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const; - int re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change = true) const; + OLGA_INLINE void cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const; + OLGA_INLINE int re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change = true) const; inline void refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const; }; @@ -159,7 +159,7 @@ namespace gcransac return models_.size(); } - void ACP1PCayleySolver::cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const + OLGA_INLINE void ACP1PCayleySolver::cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const { const double x = r(0); const double y = r(1); @@ -225,7 +225,7 @@ namespace gcransac * Order of coefficients is: x^2, xy, xz, y^2, yz, z^2, x, y, z, 1.0; * */ - int ACP1PCayleySolver::re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change) const { + OLGA_INLINE int ACP1PCayleySolver::re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change) const { Eigen::Matrix Ax, Ay, Az; Ax << coeffs.col(3), coeffs.col(5), coeffs.col(4); // y^2, z^2, yz @@ -391,4 +391,4 @@ namespace gcransac } } } -} \ No newline at end of file +} diff --git a/src/pygcransac/include/estimators/solver_dls_pnp.h b/src/pygcransac/include/estimators/solver_dls_pnp.h index ae8a9f5..8fb71ea 100644 --- a/src/pygcransac/include/estimators/solver_dls_pnp.h +++ b/src/pygcransac/include/estimators/solver_dls_pnp.h @@ -84,7 +84,7 @@ namespace gcransac const double *weights_ = nullptr) const; // The weight for each point protected: - void createMacaulayMatrix( + OLGA_INLINE void createMacaulayMatrix( const double a[20], const double b[20], const double c[20], @@ -94,11 +94,11 @@ namespace gcransac // 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. - Eigen::Matrix leftMultiplyMatrix(const Eigen::Vector3d& v) const; + OLGA_INLINE Eigen::Matrix leftMultiplyMatrix(const Eigen::Vector3d& v) const; // Extracts the coefficients of the Jacobians of the LS cost function (which is // parameterized by the 3 rotation coefficients s1, s2, s3). - void extractJacobianCoefficients( + OLGA_INLINE void extractJacobianCoefficients( const Eigen::Matrix& D, double f1_coeff[20], double f2_coeff[20], @@ -268,7 +268,7 @@ namespace gcransac // 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. - Eigen::Matrix DLSPnP::leftMultiplyMatrix(const Eigen::Vector3d& v) const + OLGA_INLINE Eigen::Matrix DLSPnP::leftMultiplyMatrix(const Eigen::Vector3d& v) const { Eigen::Matrix left_mult_mat = Eigen::Matrix::Zero(); left_mult_mat.block<1, 3>(0, 0) = v.transpose(); @@ -279,7 +279,7 @@ namespace gcransac // Extracts the coefficients of the Jacobians of the LS cost function (which is // parameterized by the 3 rotation coefficients s1, s2, s3). - void DLSPnP::extractJacobianCoefficients( + OLGA_INLINE void DLSPnP::extractJacobianCoefficients( const Eigen::Matrix& D, double f1_coeff[20], double f2_coeff[20], @@ -557,7 +557,7 @@ namespace gcransac 2 * D(8, 2) - 2 * D(8, 6)); // s1^3 } - void DLSPnP::createMacaulayMatrix(const double a[20], + OLGA_INLINE void DLSPnP::createMacaulayMatrix(const double a[20], const double b[20], const double c[20], const double u[4], @@ -924,4 +924,4 @@ namespace gcransac } } -} \ No newline at end of file +} diff --git a/src/pygcransac/include/estimators/solver_essential_matrix_three_points_gravity.h b/src/pygcransac/include/estimators/solver_essential_matrix_three_points_gravity.h index 069e914..6cd949b 100644 --- a/src/pygcransac/include/estimators/solver_essential_matrix_three_points_gravity.h +++ b/src/pygcransac/include/estimators/solver_essential_matrix_three_points_gravity.h @@ -96,7 +96,7 @@ namespace gcransac Eigen::Matrix3d gravity_source; Eigen::Matrix3d gravity_destination; - Eigen::MatrixXcd solver_3pt_caliess(const Eigen::VectorXd &data_) const; + OLGA_INLINE Eigen::MatrixXcd solver_3pt_caliess(const Eigen::VectorXd &data_) const; }; OLGA_INLINE bool EssentialMatrixThreePointsGravity::estimateModel( @@ -185,7 +185,7 @@ namespace gcransac return models_.size() > 0; } - Eigen::MatrixXcd EssentialMatrixThreePointsGravity::solver_3pt_caliess(const Eigen::VectorXd &data_) const + OLGA_INLINE Eigen::MatrixXcd EssentialMatrixThreePointsGravity::solver_3pt_caliess(const Eigen::VectorXd &data_) const { using namespace Eigen; diff --git a/src/pygcransac/include/estimators/solver_siftp1p.h b/src/pygcransac/include/estimators/solver_siftp1p.h index a7d572b..268330e 100644 --- a/src/pygcransac/include/estimators/solver_siftp1p.h +++ b/src/pygcransac/include/estimators/solver_siftp1p.h @@ -92,7 +92,7 @@ namespace gcransac protected: Eigen::Matrix3d Rxz; - void solver( + OLGA_INLINE void solver( const Eigen::Matrix3d &R_ref, const Eigen::Vector3d &t_ref, const double s1, // sin of angle in first view @@ -108,11 +108,11 @@ namespace gcransac std::vector &tsolns ) const; - int re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change = true) const; - void refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const ; + OLGA_INLINE int re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change = true) const; + OLGA_INLINE void refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const ; - void cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const; - Eigen::MatrixXd build_linear_constraints( + OLGA_INLINE void cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const; + OLGA_INLINE Eigen::MatrixXd build_linear_constraints( const Eigen::Matrix3d &R_ref, const Eigen::Vector3d &t_ref, const double s1, //sin of angle in first view @@ -206,7 +206,7 @@ namespace gcransac return models_.size(); } - Eigen::MatrixXd SIFTP1PSolver::build_linear_constraints( + OLGA_INLINE Eigen::MatrixXd SIFTP1PSolver::build_linear_constraints( const Eigen::Matrix3d &R_ref, const Eigen::Vector3d &t_ref, const double s1, //sin of angle in first view @@ -229,7 +229,7 @@ namespace gcransac return M; } - void SIFTP1PSolver::cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const + OLGA_INLINE void SIFTP1PSolver::cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const { const double x = r(0); const double y = r(1); @@ -253,7 +253,7 @@ namespace gcransac 2*(xz-yw), 2*(xw+yz), ww-xx-yy+zz; } - void SIFTP1PSolver::solver( + OLGA_INLINE void SIFTP1PSolver::solver( const Eigen::Matrix3d &R_ref, const Eigen::Vector3d &t_ref, const double s1, // sin of angle in first view @@ -304,7 +304,7 @@ namespace gcransac } } - void SIFTP1PSolver::refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const + OLGA_INLINE void SIFTP1PSolver::refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const { Eigen::Matrix3d J; Eigen::Vector3d r; @@ -346,7 +346,7 @@ namespace gcransac * Order of coefficients is: x^2, xy, xz, y^2, yz, z^2, x, y, z, 1.0; * */ - int SIFTP1PSolver::re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change) const + OLGA_INLINE int SIFTP1PSolver::re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change) const { Eigen::Matrix Ax, Ay, Az; @@ -513,4 +513,4 @@ namespace gcransac } } } -} \ No newline at end of file +} diff --git a/src/pygcransac/include/estimators/solver_siftp2p.h b/src/pygcransac/include/estimators/solver_siftp2p.h index a5c87dc..3b04907 100644 --- a/src/pygcransac/include/estimators/solver_siftp2p.h +++ b/src/pygcransac/include/estimators/solver_siftp2p.h @@ -85,7 +85,7 @@ namespace gcransac const double *weights_ = nullptr) const; // The weight for each point protected: - void solver( + OLGA_INLINE void solver( const double s1[], // sin of angle in first view const double c1[], // cos of angle in first view const double s2[], // sin of angle in second view @@ -119,11 +119,11 @@ namespace gcransac std::vector &Rsolns, std::vector &tsolns*/ ) const; - int re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change = true) const; - void refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const ; + OLGA_INLINE int re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change = true) const; + OLGA_INLINE void refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const ; - void cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const; - Eigen::MatrixXd build_constraints( + OLGA_INLINE void cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const; + OLGA_INLINE Eigen::MatrixXd build_constraints( const Eigen::Matrix3d &R_ref, const Eigen::Vector3d &t_ref, const double s1, //sin of angle in first view @@ -210,7 +210,7 @@ namespace gcransac return models_.size(); } - Eigen::MatrixXd SIFTP2PQuerySolver::build_constraints( + OLGA_INLINE Eigen::MatrixXd SIFTP2PQuerySolver::build_constraints( const Eigen::Matrix3d &R_ref, const Eigen::Vector3d &t_ref, const double s1, //sin of angle in first view @@ -229,7 +229,7 @@ namespace gcransac return M; } - void SIFTP2PQuerySolver::cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const + OLGA_INLINE void SIFTP2PQuerySolver::cayley(const Eigen::Vector3d &r, Eigen::Matrix3d &R, double &s ) const { const double x = r(0); const double y = r(1); @@ -253,7 +253,7 @@ namespace gcransac 2*(xz-yw), 2*(xw+yz), ww-xx-yy+zz; } - void SIFTP2PQuerySolver::solver( + OLGA_INLINE void SIFTP2PQuerySolver::solver( const double s1[], // sin of angle in first view const double c1[], // cos of angle in first view const double s2[], // sin of angle in second view @@ -296,7 +296,7 @@ namespace gcransac } } - void SIFTP2PQuerySolver::refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const + OLGA_INLINE void SIFTP2PQuerySolver::refine_3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, int n_sols) const { Eigen::Matrix3d J; Eigen::Vector3d r; @@ -338,7 +338,7 @@ namespace gcransac * Order of coefficients is: x^2, xy, xz, y^2, yz, z^2, x, y, z, 1.0; * */ - int SIFTP2PQuerySolver::re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change) const + OLGA_INLINE int SIFTP2PQuerySolver::re3q3(const Eigen::Matrix &coeffs, Eigen::Matrix *solutions, bool try_random_var_change) const { Eigen::Matrix Ax, Ay, Az; @@ -505,4 +505,4 @@ namespace gcransac } } } -} \ No newline at end of file +} diff --git a/src/pygcransac/include/gcransac_python.h b/src/pygcransac/include/gcransac_python.h index c461c05..ee7ef8d 100644 --- a/src/pygcransac/include/gcransac_python.h +++ b/src/pygcransac/include/gcransac_python.h @@ -2,15 +2,18 @@ #include // A method for estimating a rigid translation between two point clouds +template int findRigidTransform_( - // The 3D-3D point correspondences - std::vector& correspondences, + // The first point cloud + std::vector& correspondences, // The probabilities for each 3D-3D point correspondence if available std::vector &point_probabilities, // Output: the found inliers std::vector& inliers, // Output: the found 6D pose std::vector &pose, + // Number of elements per point in the data matrix + int element_number, // The spatial coherence weight used in the local optimization double spatial_coherence_weight, // The inlier-outlier threshold @@ -48,132 +51,7 @@ int findRigidTransform_( int lo_number); // A method for estimating a the absolute pose given 2D-3D correspondences -int find6DPoseACP1P_( - // The 2D-3D correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &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& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &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& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &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 +template int find6DPose_( // The 2D-3D correspondences std::vector& correspondences, @@ -183,6 +61,8 @@ int find6DPose_( std::vector& inliers, // Output: the found 6D pose std::vector &pose, + // Number of elements per point in the data matrix + int element_number, // The spatial coherence weight used in the local optimization double spatial_coherence_weight, // The inlier-outlier threshold @@ -213,9 +93,12 @@ int find6DPose_( // 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); + int lo_number, + // Refine the estimation with bundle adjustment + bool do_bundle_adj); // A method for estimating a 2D line from a set of 2D points +template int findLine2D_( // The 2D points in the image std::vector& points, @@ -227,6 +110,8 @@ int find6DPose_( std::vector &line, // The image size int w, int h, + // Number of elements per point in the data matrix + int element_number, // The spatial coherence weight used in the local optimization double spatial_coherence_weight, // The inlier-outlier threshold @@ -261,6 +146,7 @@ int find6DPose_( int lo_number); // A method for estimating a fundamental matrix given 2D-2D correspondences +template int findFundamentalMatrix_( // The 2D-2D point correspondences std::vector& correspondences, @@ -272,6 +158,8 @@ int findFundamentalMatrix_( std::vector &fundamental_matrix, // The images' sizes int h1, int w1, int h2, int w2, + // Number of elements per point in the data matrix + int element_number, // The spatial coherence weight used in the local optimization double spatial_coherence_weight, // The inlier-outlier threshold @@ -307,107 +195,12 @@ int findFundamentalMatrix_( // 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 fundamental matrix given 2D-2D correspondences -int findFundamentalMatrixAC_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &fundamental_matrix, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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 fundamental matrix given 2D-2D correspondences -int findFundamentalMatrixSIFT_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &fundamental_matrix, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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); + int lo_number, + // Initialize the bundle adjustment optimizer with the model + bool initialize_BA); // A method for estimating an essential matrix given 2D-2D correspondences +template int findEssentialMatrix_( // The 2D-2D point correspondences std::vector& correspondences, @@ -423,6 +216,8 @@ int findEssentialMatrix_( std::vector &destination_intrinsics, // The images' sizes int h1, int w1, int h2, int w2, + // Number of elements per point in the data matrix + int element_number, // The spatial coherence weight used in the local optimization double spatial_coherence_weight, // The inlier-outlier threshold @@ -431,113 +226,7 @@ int findEssentialMatrix_( 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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 an essential matrix given 2D-2D correspondences -int findEssentialMatrixAC_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &essential_matrix, - // The intrinsic camera matrix of the source image - std::vector &source_intrinsics, - // The intrinsic camera matrix of the destination image - std::vector &destination_intrinsics, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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 an essential matrix given 2D-2D correspondences -int findEssentialMatrixSIFT_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &essential_matrix, - // The intrinsic camera matrix of the source image - std::vector &source_intrinsics, - // The intrinsic camera matrix of the destination image - std::vector &destination_intrinsics, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. + // Minimum iteration number. I do not suggest setting it to lower than 50. 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. @@ -564,9 +253,13 @@ int findEssentialMatrixSIFT_( // 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); + int lo_number, + // When usint SPRT + bool do_final_iterated_least_squares); + // A method for estimating an essential matrix given 2D-2D correspondences +template int findGravityEssentialMatrix_( // The 2D-2D point correspondences. std::vector& correspondences, @@ -586,57 +279,8 @@ int findGravityEssentialMatrix_( std::vector &destination_intrinsics, // The images' sizes int h1, int w1, int h2, int w2, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - double neighborhood_size, - // The number of RANSAC iterations done in the local optimization - int lo_number); - -// A method for estimating an essential matrix given 2D-2D correspondences -int findPlanarEssentialMatrix_( - // The 2D-2D point correspondences. - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &essential_matrix, - // The intrinsic camera matrix of the source image - std::vector &source_intrinsics, - // The intrinsic camera matrix of the destination image - std::vector &destination_intrinsics, - // The images' sizes - int h1, int w1, int h2, int w2, + // Number of elements per point in the data matrix + int element_number, // The spatial coherence weight used in the local optimization double spatial_coherence_weight, // The inlier-outlier threshold @@ -672,8 +316,8 @@ int findPlanarEssentialMatrix_( // The number of RANSAC iterations done in the local optimization int lo_number); -// A method for estimating a homography matrix given 2D-2D correspondences -int findHomography_( +template +int findHomography_t_( // The 2D-2D point correspondences. std::vector& correspondences, // The probabilities for each 3D-3D point correspondence if available @@ -684,12 +328,14 @@ int findHomography_( std::vector &homography, // The images' sizes int h1, int w1, int h2, int w2, + // Number of elements per point in the data matrix + int element_number, // 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, + double conf, // Maximum iteration number. I do not suggest setting it to lower than 1000. int max_iters, // Minimum iteration number. I do not suggest setting it to lower than 50. @@ -720,105 +366,7 @@ int findHomography_( // Barath, Daniel, and Gabor Valasek. "Space-Partitioning RANSAC." arXiv preprint arXiv:2111.12385 (2021). // should be used to speed up the model verification. bool use_space_partitioning, - // The variance parameter of the AR-Sampler. It is used only 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 homography matrix given 2D-2D correspondences -int findHomographyAC_( - // The 2D-2D point correspondences. - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &homography, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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 homography matrix given 2D-2D correspondences -int findHomographySIFT_( - // The 2D-2D point correspondences. - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &homography, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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); \ No newline at end of file diff --git a/src/pygcransac/include/neighborhood/flann_neighborhood_graph.h b/src/pygcransac/include/neighborhood/flann_neighborhood_graph.h index 2dfcad2..00a8baf 100644 --- a/src/pygcransac/include/neighborhood/flann_neighborhood_graph.h +++ b/src/pygcransac/include/neighborhood/flann_neighborhood_graph.h @@ -79,7 +79,7 @@ namespace gcransac initialized = initialize(container); } - bool initialize(const cv::Mat * const container_); + inline bool initialize(const cv::Mat * const container_); inline const std::vector &getNeighbors(size_t point_idx_) const; // A function returning the cell sizes @@ -97,7 +97,7 @@ namespace gcransac size_t filledCellNumber() const { return neighbours.size(); } }; - bool FlannNeighborhoodGraph::initialize(const cv::Mat * const container_) + inline bool FlannNeighborhoodGraph::initialize(const cv::Mat * const container_) { // Compute the neighborhood graph // TODO: replace by nanoflann @@ -143,4 +143,4 @@ namespace gcransac return neighbours[point_idx_]; } } -} \ No newline at end of file +} diff --git a/src/pygcransac/include/problem_instances/find6DPose_t.hpp b/src/pygcransac/include/problem_instances/find6DPose_t.hpp new file mode 100644 index 0000000..04a9be1 --- /dev/null +++ b/src/pygcransac/include/problem_instances/find6DPose_t.hpp @@ -0,0 +1,290 @@ +#pragma once +#include "gcransac_python.h" +#include +#include +#include "utils.h" +#include +#include + +#include "GCRANSAC.h" +#include "neighborhood/flann_neighborhood_graph.h" +#include "neighborhood/grid_neighborhood_graph.h" + +#include "samplers/uniform_sampler.h" +#include "samplers/prosac_sampler.h" +#include "samplers/napsac_sampler.h" +#include "samplers/progressive_napsac_sampler.h" +#include "samplers/importance_sampler.h" +#include "samplers/adaptive_reordering_sampler.h" +#include "samplers/single_point_sampler.h" + +#include "preemption/preemption_sprt.h" + +#include "inlier_selectors/empty_inlier_selector.h" +#include "inlier_selectors/space_partitioning_ransac.h" + +#include +#include +#include + +using namespace gcransac; + + +// A method for estimating a the absolute pose given 2D-3D correspondences +template +int find6DPose_( + // The 2D-3D correspondences + std::vector& correspondences, + // The probabilities for each 3D-3D point correspondence if available + std::vector &point_probabilities, + // Output: the found inliers + std::vector& inliers, + // Output: the found 6D pose + std::vector &pose, + // Number of elements per point in the data matrix + int element_number, + // 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, + // Refine the estimation with bundle adjustment + bool do_bundle_adj) +{ + size_t num_tents = correspondences.size() / element_number; + cv::Mat points(num_tents, element_number, CV_64F, &correspondences[0]); + + typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; + std::unique_ptr neighborhood_graph; + + const size_t cell_number_in_neighborhood_graph_ = + static_cast(neighborhood_size); + + // Initializing the neighborhood graph if needed + if (spatial_coherence_weight <= std::numeric_limits::epsilon()) + { + cv::Mat emptyPoints(0, 4, CV_64F); + + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points + { 0, // The cell size along axis X + 0 }, // The cell size along axis Y + 1)); // The cell number along every axis + } else + { + cv::Mat pointsForNeighborhood; + // Copy the first 5 columns of the points to the pointsForNeighborhood + points.colRange(0, 4).copyTo(pointsForNeighborhood); + + if (neighborhood_id == 0) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::FlannNeighborhoodGraph(&pointsForNeighborhood, neighborhood_size)); + else + { + fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (FLANN-based neighborhood)\n", + neighborhood_id); + return 0; + } + } + + // Apply Graph-cut RANSAC + Pose6DEstimator estimator; + Pose6D model; + + // Initialize the samplers + // The main sampler is used for sampling in the main RANSAC loop + typedef sampler::Sampler AbstractSampler; + std::unique_ptr main_sampler; + if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler + main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); + else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. + main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); + else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "Progressive NAPSAC is not usable for the absolute pose problem.\n", + sampler_id); + return 0; + } + else if (sampler_id == 3) + main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, + point_probabilities, + estimator.sampleSize())); + else if (sampler_id == 4) + { + double max_prob = 0; + for (const auto &prob : point_probabilities) + max_prob = MAX(max_prob, prob); + for (auto &prob : point_probabilities) + prob /= max_prob; + main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, + point_probabilities, + estimator.sampleSize(), + sampler_variance)); + } + else if (sampler_id == 5) + main_sampler = std::unique_ptr(new sampler::SinglePointSampler(&points, 1)); + else + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", + sampler_id); + return 0; + } + + sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization + + // Checking if the samplers are initialized successfully. + if (!main_sampler->isInitialized() || + !local_optimization_sampler.isInitialized()) + { + fprintf(stderr, "One of the samplers is not initialized successfully.\n"); + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + return 0; + } + + utils::RANSACStatistics statistics; + + // Initializing the fast inlier selector object + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + if (use_sprt) + { + // Initializing SPRT test + preemption::SPRTPreemptiveVerfication preemptive_verification( + points, + estimator, + min_inlier_ratio_for_sprt); + + GCRANSAC, + preemption::SPRTPreemptiveVerfication> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + else + { + GCRANSAC gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model); + + statistics = gcransac.getRansacStatistics(); + } + + if (do_bundle_adj && statistics.inliers.size() > 3) + { + std::vector models = { model }; + estimator::solver::PnPBundleAdjustment bundle_adjuster; + bundle_adjuster.estimateModel( + points, + &statistics.inliers[0], + statistics.inliers.size(), + models, + nullptr); + model.descriptor = models[0].descriptor; + } + + inliers.resize(num_tents); + + const int num_inliers = statistics.inliers.size(); + for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { + inliers[pt_idx] = 0; + } + + for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { + inliers[statistics.inliers[pt_idx]] = 1; + } + + pose.resize(12); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + pose[i * 4 + j] = (double)model.descriptor(i, j); + } + } + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + // The number of inliers found + return num_inliers; +} + diff --git a/src/pygcransac/include/problem_instances/findEssentialMatrix_t.hpp b/src/pygcransac/include/problem_instances/findEssentialMatrix_t.hpp new file mode 100644 index 0000000..1f6882a --- /dev/null +++ b/src/pygcransac/include/problem_instances/findEssentialMatrix_t.hpp @@ -0,0 +1,382 @@ +#pragma once +#include "gcransac_python.h" +#include +#include +#include "utils.h" +#include +#include + +#include "GCRANSAC.h" +#include "neighborhood/flann_neighborhood_graph.h" +#include "neighborhood/grid_neighborhood_graph.h" + +#include "samplers/uniform_sampler.h" +#include "samplers/prosac_sampler.h" +#include "samplers/napsac_sampler.h" +#include "samplers/progressive_napsac_sampler.h" +#include "samplers/importance_sampler.h" +#include "samplers/adaptive_reordering_sampler.h" +#include "samplers/single_point_sampler.h" + +#include "preemption/preemption_sprt.h" + +#include "inlier_selectors/empty_inlier_selector.h" +#include "inlier_selectors/space_partitioning_ransac.h" + +#include +#include +#include + +using namespace gcransac; + + +// A method for estimating an essential matrix given 2D-2D correspondences +template +int findEssentialMatrix_( + // The 2D-2D point correspondences + std::vector& correspondences, + // The probabilities for each 3D-3D point correspondence if available + std::vector &point_probabilities, + // Output: the found inliers + std::vector& inliers, + // Output: the found 6D pose + std::vector &essential_matrix, + // The intrinsic camera matrix of the source image + std::vector &source_intrinsics, + // The intrinsic camera matrix of the destination image + std::vector &destination_intrinsics, + // The images' sizes + int h1, int w1, int h2, int w2, + // Number of elements per point in the data matrix + int element_number, + // 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. I do not suggest setting it to lower than 50. + 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. + // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. + // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. + // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. + int sampler_id, + // The identifier of the used neighborhood structure. + // (0) FLANN-based neighborhood. + // (1) Grid-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 + // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) + 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, + // When usint SPRT + bool do_final_iterated_least_squares) +{ + int num_tents = correspondences.size() / element_number; + cv::Mat points(num_tents, element_number, CV_64F, &correspondences[0]); + + typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; + std::unique_ptr neighborhood_graph; + + const size_t cell_number_in_neighborhood_graph_ = + static_cast(neighborhood_size); + + // If the spatial weight is 0.0, the neighborhood graph should not be created + if (spatial_coherence_weight <= std::numeric_limits::epsilon()) + { + cv::Mat emptyPoints(0, 4, CV_64F); + + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points + { 0, // The cell size along axis X + 0 }, // The cell size along axis Y + 1)); // The cell number along every axis + } else // Initializing a grid-based neighborhood graph + { + // Using only the point coordinates and not the affine elements when constructing the neighborhood. + cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); + + // Initializing a grid-based neighborhood graph + if (neighborhood_id == 0) + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, + { w1 / static_cast(cell_number_in_neighborhood_graph_), + h1 / static_cast(cell_number_in_neighborhood_graph_), + w2 / static_cast(cell_number_in_neighborhood_graph_), + h2 / static_cast(cell_number_in_neighborhood_graph_) }, + cell_number_in_neighborhood_graph_)); + else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); + else + { + fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", + neighborhood_id); + return 0; + } + + // Checking if the neighborhood graph is initialized successfully. + if (!neighborhood_graph->isInitialized()) + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); + return 0; + } + } + + Eigen::Map> intrinsics_src(&source_intrinsics[0]); + Eigen::Map> intrinsics_dst(&destination_intrinsics[0]); + + // Calculating the maximum image diagonal to be used for setting the threshold + // adaptively for each image pair. + const double threshold_normalizer = + 0.25 * (intrinsics_src(0,0) + intrinsics_src(1,1) + intrinsics_dst(0,0) + intrinsics_dst(1,1)); + + cv::Mat normalized_points(points.size(), CV_64F); + utils::normalizeCorrespondences(points, + intrinsics_src, + intrinsics_dst, + normalized_points); + + // Apply Graph-cut RANSAC + EssentialMatrixEstimator estimator(intrinsics_src, + intrinsics_dst); + EssentialMatrix model; + + // Initialize the samplers + // The main sampler is used for sampling in the main RANSAC loop + typedef sampler::Sampler AbstractSampler; + std::unique_ptr main_sampler; + if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler + main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); + else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. + main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); + else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler + main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, + { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension + // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. + estimator.sampleSize(), // The size of a minimal sample + { static_cast(w1), // The width of the source image + static_cast(h1), // The height of the source image + static_cast(w2), // The width of the destination image + static_cast(h2) }, // The height of the destination image + 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling + else if (sampler_id == 3) + main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, + point_probabilities, + estimator.sampleSize())); + else if (sampler_id == 4) + { + double max_prob = 0; + for (const auto &prob : point_probabilities) + max_prob = MAX(max_prob, prob); + for (auto &prob : point_probabilities) + prob /= max_prob; + main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, + point_probabilities, + estimator.sampleSize(), + sampler_variance)); + } + else + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling), 3 (NG-RANSAC sampler), 4 (AR-Sampler)\n", + sampler_id); + return 0; + } + + // The local optimization sampler is used inside the local optimization + sampler::UniformSampler local_optimization_sampler(&points); + + // Checking if the samplers are initialized successfully. + if (!main_sampler->isInitialized() || + !local_optimization_sampler.isInitialized()) + { + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "One of the samplers is not initialized successfully.\n"); + return 0; + } + + utils::RANSACStatistics statistics; + + // Initializing the fast inlier selector object + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + if (use_sprt) + { + // Initializing SPRT test + preemption::SPRTPreemptiveVerfication preemptive_verification( + points, + estimator, + min_inlier_ratio_for_sprt); + + GCRANSAC, + preemption::SPRTPreemptiveVerfication> gcransac; + gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + if (!do_final_iterated_least_squares) { + gcransac.settings.do_final_iterated_least_squares = false; + gcransac.settings.max_graph_cut_number = 100; + } + + // Start GC-RANSAC + gcransac.run(normalized_points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + else + { + GCRANSAC gcransac; + gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(normalized_points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model); + + statistics = gcransac.getRansacStatistics(); + } + + // Running bundle adjustment minimizing the pose error on the found inliers + const size_t &inlier_number = statistics.inliers.size(); + if (statistics.inliers.size() > 5) + { + // The truncated least-squares threshold + const double truncated_threshold = 3.0 / 2.0 * threshold / threshold_normalizer; + // The squared least-squares threshold + const double squared_truncated_threshold = truncated_threshold * truncated_threshold; + + // Initializing all weights to be zero + std::vector weights(inlier_number, 0.0); + + // Setting a weight for all inliers + for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) + { + const size_t point_idx = statistics.inliers[inlier_idx]; + weights[inlier_idx] = + MAX(0, 1.0 - estimator.squaredResidual(normalized_points.row(point_idx), model) / squared_truncated_threshold); + } + + std::vector models; + estimator::solver::EssentialMatrixBundleAdjustmentSolver bundleOptimizer; + bundleOptimizer.estimateModel( + normalized_points, + &statistics.inliers[0], + statistics.inliers.size(), + models, + &weights[0]); + + // Scoring function + MSACScoringFunction scoring; + scoring.initialize(squared_truncated_threshold, normalized_points.rows); + // Score of the new model + Score score; + // Inliers of the new model + std::vector current_inliers; + + // Select the best model and update the inliers + for (auto &tmp_model : models) + { + current_inliers.clear(); + score = scoring.getScore(normalized_points, // All points + tmp_model, // The current model parameters + estimator, // The estimator + squared_truncated_threshold, // The current threshold + current_inliers); // The current inlier set + + // Check if the updated model is better than the best so far + if (statistics.score < score.value) + { + model.descriptor = tmp_model.descriptor; + statistics.score = score.value; + statistics.inliers.swap(current_inliers); + } + } + } + else + model.descriptor = Eigen::Matrix3d::Identity(); + + inliers.resize(num_tents); + + const int num_inliers = statistics.inliers.size(); + for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { + inliers[pt_idx] = 0; + + } + for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { + inliers[statistics.inliers[pt_idx]] = 1; + } + + essential_matrix.resize(9); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + essential_matrix[i * 3 + j] = (double)model.descriptor(i, j); + } + } + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + // The number of inliers found + return num_inliers; +} diff --git a/src/pygcransac/include/problem_instances/findFundamentalMatrix_t.hpp b/src/pygcransac/include/problem_instances/findFundamentalMatrix_t.hpp new file mode 100644 index 0000000..c09de77 --- /dev/null +++ b/src/pygcransac/include/problem_instances/findFundamentalMatrix_t.hpp @@ -0,0 +1,370 @@ +#pragma once +#include "gcransac_python.h" +#include +#include +#include "utils.h" +#include +#include + +#include "GCRANSAC.h" +#include "neighborhood/flann_neighborhood_graph.h" +#include "neighborhood/grid_neighborhood_graph.h" + +#include "samplers/uniform_sampler.h" +#include "samplers/prosac_sampler.h" +#include "samplers/napsac_sampler.h" +#include "samplers/progressive_napsac_sampler.h" +#include "samplers/importance_sampler.h" +#include "samplers/adaptive_reordering_sampler.h" +#include "samplers/single_point_sampler.h" + +#include "preemption/preemption_sprt.h" + +#include "inlier_selectors/empty_inlier_selector.h" +#include "inlier_selectors/space_partitioning_ransac.h" + +#include +#include +#include + +using namespace gcransac; + + +// A method for estimating a fundamental matrix given 2D-2D correspondences +template +int findFundamentalMatrix_( + // The 2D-2D point correspondences + std::vector& correspondences, + // The probabilities for each 3D-3D point correspondence if available + std::vector &point_probabilities, + // Output: the found inliers + std::vector& inliers, + // Output: the found 6D pose + std::vector &fundamental_matrix, + // The images' sizes + int h1, int w1, int h2, int w2, + // Number of elements per point in the data matrix + int element_number, + // 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. + // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. + // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. + // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. + int sampler_id, + // The identifier of the used neighborhood structure. + // (0) FLANN-based neighborhood. + // (1) Grid-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 + // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) + 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, + // Initialize the bundle adjustment optimizer with the model + bool initialize_BA) +{ + int num_tents = correspondences.size() / element_number; + cv::Mat points(num_tents, element_number, CV_64F, &correspondences[0]); + + // The neighborhood structure used for the graph-cut-based local optimization + typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; + std::unique_ptr neighborhood_graph; + + const size_t cell_number_in_neighborhood_graph_ = + static_cast(neighborhood_size); + + // If the spatial weight is 0.0, the neighborhood graph should not be created + if (spatial_coherence_weight <= std::numeric_limits::epsilon()) + { + cv::Mat emptyPoints(0, 4, CV_64F); + + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points + { 0, // The cell size along axis X + 0 }, // The cell size along axis Y + 1)); // The cell number along every axis + } else // Initializing a grid-based neighborhood graph + { + // Using only the point coordinates and not the affine elements when constructing the neighborhood. + cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); + + // Initializing a grid-based neighborhood graph + if (neighborhood_id == 0) + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, + { w1 / static_cast(cell_number_in_neighborhood_graph_), + h1 / static_cast(cell_number_in_neighborhood_graph_), + w2 / static_cast(cell_number_in_neighborhood_graph_), + h2 / static_cast(cell_number_in_neighborhood_graph_) }, + cell_number_in_neighborhood_graph_)); + else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); + else + { + fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", + neighborhood_id); + return 0; + } + + // Checking if the neighborhood graph is initialized successfully. + if (!neighborhood_graph->isInitialized()) + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); + return 0; + } + } + + // Calculating the maximum image diagonal to be used for setting the threshold + // adaptively for each image pair. + //const double max_image_diagonal = + // sqrt(pow(MAX(w1, w2), 2) + pow(MAX(h1, h2), 2)); + + // Apply Graph-cut RANSAC + FundamentalMatrixEstimator estimator; + FundamentalMatrix model; + + // Initialize the samplers + // The main sampler is used for sampling in the main RANSAC loop + typedef sampler::Sampler AbstractSampler; + std::unique_ptr main_sampler; + if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler + main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); + else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. + main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); + else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler + main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, + { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension + // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. + estimator.sampleSize(), // The size of a minimal sample + { static_cast(w1), // The width of the source image + static_cast(h1), // The height of the source image + static_cast(w2), // The width of the destination image + static_cast(h2) }, // The height of the destination image + 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling + else if (sampler_id == 3) + main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, + point_probabilities, + estimator.sampleSize())); + else if (sampler_id == 4) + { + double max_prob = 0; + for (const auto &prob : point_probabilities) + max_prob = MAX(max_prob, prob); + for (auto &prob : point_probabilities) + prob /= max_prob; + main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, + point_probabilities, + estimator.sampleSize(), + sampler_variance)); + } + else + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling), 3 (NG-RANSAC sampler), 4 (AR-Sampler)\n", + sampler_id); + return 0; + } + + sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization + + // Checking if the samplers are initialized successfully. + if (!main_sampler->isInitialized() || + !local_optimization_sampler.isInitialized()) + { + fprintf(stderr, "One of the samplers is not initialized successfully.\n"); + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + return 0; + } + + utils::RANSACStatistics statistics; + + // Initializing the fast inlier selector object + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + if (use_sprt) + { + // Initializing SPRT test + preemption::SPRTPreemptiveVerfication preemptive_verification( + points, + estimator, + min_inlier_ratio_for_sprt); + + GCRANSAC, + preemption::SPRTPreemptiveVerfication> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + else + { + GCRANSAC gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model); + + statistics = gcransac.getRansacStatistics(); + } + + // Running bundle adjustment minimizing the pose error on the found inliers + const size_t &inlier_number = statistics.inliers.size(); + if (statistics.inliers.size() > 7) + { + // The truncated least-squares threshold + const double truncated_threshold = 3.0 / 2.0 * threshold; + // The squared least-squares threshold + const double squared_truncated_threshold = truncated_threshold * truncated_threshold; + + std::vector models; + double *weights_ptr = nullptr; + if (initialize_BA) + models.push_back(model); + else { + // Initializing all weights to be zero + std::vector weights(inlier_number, 0.0); + + // Setting a weight for all inliers + for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) + { + const size_t point_idx = statistics.inliers[inlier_idx]; + weights[inlier_idx] = + MAX(0, 1.0 - estimator.squaredResidual(points.row(point_idx), model) / squared_truncated_threshold); + } + weights_ptr = &weights[0]; + } + + estimator::solver::FundamentalMatrixBundleAdjustmentSolver bundleOptimizer; + bundleOptimizer.estimateModel( + points, + &statistics.inliers[0], + statistics.inliers.size(), + models, + weights_ptr); + + // Scoring function + MSACScoringFunction scoring; + scoring.initialize(squared_truncated_threshold, points.rows); + // Score of the new model + Score score; + // Inliers of the new model + std::vector current_inliers; + + // Select the best model and update the inliers + for (auto &tmp_model : models) + { + current_inliers.clear(); + score = scoring.getScore(points, // All points + tmp_model, // The current model parameters + estimator, // The estimator + squared_truncated_threshold, // The current threshold + current_inliers); // The current inlier set + + // Check if the updated model is better than the best so far + if (statistics.score < score.value) + { + model.descriptor = tmp_model.descriptor; + statistics.score = score.value; + statistics.inliers.swap(current_inliers); + } + } + } + else + model.descriptor = Eigen::Matrix3d::Identity(); + + inliers.resize(num_tents); + + const int num_inliers = statistics.inliers.size(); + for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { + inliers[pt_idx] = 0; + } + + for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { + inliers[statistics.inliers[pt_idx]] = 1; + } + + fundamental_matrix.resize(9); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + fundamental_matrix[i * 3 + j] = (double)model.descriptor(i, j); + } + } + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + // The number of inliers found + return num_inliers; +} diff --git a/src/pygcransac/include/problem_instances/findGravityEssentialMatrix_t.hpp b/src/pygcransac/include/problem_instances/findGravityEssentialMatrix_t.hpp new file mode 100644 index 0000000..8cab7aa --- /dev/null +++ b/src/pygcransac/include/problem_instances/findGravityEssentialMatrix_t.hpp @@ -0,0 +1,384 @@ +#pragma once +#include "gcransac_python.h" +#include +#include +#include "utils.h" +#include +#include + +#include "GCRANSAC.h" +#include "neighborhood/flann_neighborhood_graph.h" +#include "neighborhood/grid_neighborhood_graph.h" + +#include "samplers/uniform_sampler.h" +#include "samplers/prosac_sampler.h" +#include "samplers/napsac_sampler.h" +#include "samplers/progressive_napsac_sampler.h" +#include "samplers/importance_sampler.h" +#include "samplers/adaptive_reordering_sampler.h" +#include "samplers/single_point_sampler.h" + +#include "preemption/preemption_sprt.h" + +#include "inlier_selectors/empty_inlier_selector.h" +#include "inlier_selectors/space_partitioning_ransac.h" + +#include +#include +#include + +using namespace gcransac; + + +// A method for estimating an essential matrix given 2D-2D correspondences +template +int findGravityEssentialMatrix_( + // The 2D-2D point correspondences. + std::vector& correspondences, + // The 2D-2D point correspondences. + std::vector& gravity_source, + // The 2D-2D point correspondences. + std::vector& gravity_destination, + // The probabilities for each 3D-3D point correspondence if available + std::vector &point_probabilities, + // Output: the found inliers + std::vector& inliers, + // Output: the found 6D pose + std::vector &essential_matrix, + // The intrinsic camera matrix of the source image + std::vector &source_intrinsics, + // The intrinsic camera matrix of the destination image + std::vector &destination_intrinsics, + // The images' sizes + int h1, int w1, int h2, int w2, + // Number of elements per point in the data matrix + int element_number, + // 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. I do not suggest setting it to lower than 50. + 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. + // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. + // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. + // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. + int sampler_id, + // The identifier of the used neighborhood structure. + // (0) FLANN-based neighborhood. + // (1) Grid-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 + // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) + double neighborhood_size, + // The number of RANSAC iterations done in the local optimization + int lo_number) +{ + int num_tents = correspondences.size() / element_number; + cv::Mat points(num_tents, element_number, CV_64F, &correspondences[0]); + + typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; + std::unique_ptr neighborhood_graph; + + const size_t cell_number_in_neighborhood_graph_ = + static_cast(neighborhood_size); + + // If the spatial weight is 0.0, the neighborhood graph should not be created + if (spatial_coherence_weight <= std::numeric_limits::epsilon()) + { + cv::Mat emptyPoints(0, 4, CV_64F); + + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points + { 0, // The cell size along axis X + 0 }, // The cell size along axis Y + 1)); // The cell number along every axis + } else // Initializing a grid-based neighborhood graph + { + // Initializing a grid-based neighborhood graph + if (neighborhood_id == 0) + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&points, + { w1 / static_cast(cell_number_in_neighborhood_graph_), + h1 / static_cast(cell_number_in_neighborhood_graph_), + w2 / static_cast(cell_number_in_neighborhood_graph_), + h2 / static_cast(cell_number_in_neighborhood_graph_) }, + cell_number_in_neighborhood_graph_)); + else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); + else + { + fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", + neighborhood_id); + return 0; + } + + // Checking if the neighborhood graph is initialized successfully. + if (!neighborhood_graph->isInitialized()) + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); + return 0; + } + } + + Eigen::Map> intrinsics_src(&source_intrinsics[0]); + Eigen::Map> intrinsics_dst(&destination_intrinsics[0]); + + Eigen::Map> gravity_src(&gravity_source[0]); + Eigen::Map> gravity_dst(&gravity_destination[0]); + + // Calculating the maximum image diagonal to be used for setting the threshold + // adaptively for each image pair. + const double threshold_normalizer = + 0.25 * (intrinsics_src(0,0) + intrinsics_src(1,1) + intrinsics_dst(0,0) + intrinsics_dst(1,1)); + + cv::Mat normalized_points(points.size(), CV_64F); + utils::normalizeCorrespondences(points, + intrinsics_src, + intrinsics_dst, + normalized_points); + + // Apply Graph-cut RANSAC + GravityEssentialMatrixEstimator estimator(intrinsics_src, + intrinsics_dst); + EssentialMatrix model; + + // Setting the gravity directions to the solver + estimator.getMutableMinimalSolver()->setGravity(gravity_src, gravity_dst); + + // Initialize the samplers + // The main sampler is used for sampling in the main RANSAC loop + typedef sampler::Sampler AbstractSampler; + std::unique_ptr main_sampler; + if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler + main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); + else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. + main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); + else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler + main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, + { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension + // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. + estimator.sampleSize(), // The size of a minimal sample + { static_cast(w1), // The width of the source image + static_cast(h1), // The height of the source image + static_cast(w2), // The width of the destination image + static_cast(h2) }, // The height of the destination image + 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling + else if (sampler_id == 3) + main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, + point_probabilities, + estimator.sampleSize())); + else if (sampler_id == 4) + { + double variance = 0.1; + double max_prob = 0; + for (const auto &prob : point_probabilities) + max_prob = MAX(max_prob, prob); + for (auto &prob : point_probabilities) + prob /= max_prob; + main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, + point_probabilities, + estimator.sampleSize(), + variance)); + } + else + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", + sampler_id); + return 0; + } + + // The local optimization sampler is used inside the local optimization + sampler::UniformSampler local_optimization_sampler(&points); + + // Checking if the samplers are initialized successfully. + if (!main_sampler->isInitialized() || + !local_optimization_sampler.isInitialized()) + { + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "One of the samplers is not initialized successfully.\n"); + return 0; + } + + utils::RANSACStatistics statistics; + + // Initializing the fast inlier selector object + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + if (use_sprt) + { + // Initializing SPRT test + preemption::SPRTPreemptiveVerfication preemptive_verification( + points, + estimator, + min_inlier_ratio_for_sprt); + + GCRANSAC, + preemption::SPRTPreemptiveVerfication> gcransac; + gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + gcransac.settings.do_final_iterated_least_squares = false; + gcransac.settings.max_graph_cut_number = 100; + + // Start GC-RANSAC + gcransac.run(normalized_points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + else + { + + GCRANSAC gcransac; + gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(normalized_points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model); + + statistics = gcransac.getRansacStatistics(); + } + + // Running bundle adjustment minimizing the pose error on the found inliers + const size_t &inlier_number = statistics.inliers.size(); + if (statistics.inliers.size() > 5) + { + // The truncated least-squares threshold + const double truncated_threshold = 3.0 / 2.0 * threshold / threshold_normalizer; + // The squared least-squares threshold + const double squared_truncated_threshold = truncated_threshold * truncated_threshold; + + // Initializing all weights to be zero + std::vector weights(inlier_number, 0.0); + + // Setting a weight for all inliers + for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) + { + const size_t point_idx = statistics.inliers[inlier_idx]; + weights[inlier_idx] = + MAX(0, 1.0 - estimator.squaredResidual(normalized_points.row(point_idx), model) / squared_truncated_threshold); + } + + std::vector models; + estimator::solver::EssentialMatrixBundleAdjustmentSolver bundleOptimizer; + bundleOptimizer.estimateModel( + normalized_points, + &statistics.inliers[0], + statistics.inliers.size(), + models, + &weights[0]); + + // Scoring function + MSACScoringFunction scoring; + scoring.initialize(squared_truncated_threshold, normalized_points.rows); + // Score of the new model + Score score; + // Inliers of the new model + std::vector current_inliers; + + // Select the best model and update the inliers + for (auto &tmp_model : models) + { + current_inliers.clear(); + score = scoring.getScore(normalized_points, // All points + tmp_model, // The current model parameters + estimator, // The estimator + squared_truncated_threshold, // The current threshold + current_inliers); // The current inlier set + + // Check if the updated model is better than the best so far + if (statistics.score < score.value) + { + model.descriptor = tmp_model.descriptor; + statistics.score = score.value; + statistics.inliers.swap(current_inliers); + } + } + } + else + model.descriptor = Eigen::Matrix3d::Identity(); + + inliers.resize(num_tents); + + const int num_inliers = statistics.inliers.size(); + for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { + inliers[pt_idx] = 0; + + } + for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { + inliers[statistics.inliers[pt_idx]] = 1; + } + + essential_matrix.resize(9); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + essential_matrix[i * 3 + j] = (double)model.descriptor(i, j); + } + } + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + // The number of inliers found + return num_inliers; +} diff --git a/src/pygcransac/include/problem_instances/findHomography_t.hpp b/src/pygcransac/include/problem_instances/findHomography_t.hpp new file mode 100644 index 0000000..c4e4192 --- /dev/null +++ b/src/pygcransac/include/problem_instances/findHomography_t.hpp @@ -0,0 +1,383 @@ +#pragma once +#include "gcransac_python.h" +#include +#include +#include "utils.h" +#include +#include + +#include "GCRANSAC.h" +#include "neighborhood/flann_neighborhood_graph.h" +#include "neighborhood/grid_neighborhood_graph.h" + +#include "samplers/uniform_sampler.h" +#include "samplers/prosac_sampler.h" +#include "samplers/napsac_sampler.h" +#include "samplers/progressive_napsac_sampler.h" +#include "samplers/importance_sampler.h" +#include "samplers/adaptive_reordering_sampler.h" +#include "samplers/single_point_sampler.h" + +#include "preemption/preemption_sprt.h" + +#include "inlier_selectors/empty_inlier_selector.h" +#include "inlier_selectors/space_partitioning_ransac.h" + +#include +#include +#include + +using namespace gcransac; + + +// A method for estimating a homography matrix given 2D-2D correspondences +template +int findHomography_t_( + // The 2D-2D point correspondences. + std::vector& correspondences, + // The probabilities for each 3D-3D point correspondence if available + std::vector &point_probabilities, + // Output: the found inliers + std::vector& inliers, + // Output: the found 6D pose + std::vector &homography, + // The images' sizes + int h1, int w1, int h2, int w2, + // Number of elements per point in the data matrix + int element_number, + // 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. I do not suggest setting it to lower than 50. + 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. + // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. + // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. + // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. + int sampler_id, + // The identifier of the used neighborhood structure. + // (0) FLANN-based neighborhood. + // (1) Grid-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 + // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) + double neighborhood_size, + // A flag determining if space partitioning from + // Barath, Daniel, and Gabor Valasek. "Space-Partitioning RANSAC." arXiv preprint arXiv:2111.12385 (2021). + // should be used to speed up the model verification. + bool use_space_partitioning, + // 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) +{ + int num_tents = correspondences.size() / element_number; + cv::Mat points(num_tents, element_number, CV_64F, &correspondences[0]); + + typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; + std::unique_ptr neighborhood_graph; + + const size_t cell_number_in_neighborhood_graph_ = + static_cast(neighborhood_size); + + // If the spatial weight is 0.0, the neighborhood graph should not be created + if (spatial_coherence_weight <= std::numeric_limits::epsilon()) + { + cv::Mat empty_point_matrix(0, 4, CV_64F); + + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&empty_point_matrix, // The input points + { 0, // The cell size along axis X in the source image + 0, // The cell size along axis Y in the source image + 0, // The cell size along axis X in the destination image + 0 }, // The cell size along axis Y in the destination image + 1)); // The cell number along every axis + } else // Initializing a grid-based neighborhood graph + { + if (use_space_partitioning && neighborhood_id != 0) + { + fprintf(stderr, "Space Partitioning only works with Grid neighorbood yet. Thus, setting neighborhood_id = 0.\n"); + neighborhood_id = 0; + } + + // Using only the point coordinates and not the affine elements when constructing the neighborhood. + cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); + + // Initializing a grid-based neighborhood graph + if (neighborhood_id == 0) + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, + { (w1 + std::numeric_limits::epsilon()) / static_cast(cell_number_in_neighborhood_graph_), + (h1 + std::numeric_limits::epsilon()) / static_cast(cell_number_in_neighborhood_graph_), + (w2 + std::numeric_limits::epsilon()) / static_cast(cell_number_in_neighborhood_graph_), + (h2 + std::numeric_limits::epsilon()) / static_cast(cell_number_in_neighborhood_graph_) }, + cell_number_in_neighborhood_graph_)); + else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); + else + { + fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", + neighborhood_id); + return 0; + } + + // Checking if the neighborhood graph is initialized successfully. + if (!neighborhood_graph->isInitialized()) + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); + return 0; + } + } + + // Calculating the maximum image diagonal to be used for setting the threshold + // adaptively for each image pair. + //const double max_image_diagonal = + // sqrt(pow(MAX(w1, w2), 2) + pow(MAX(h1, h2), 2)); + + HomographyEstimator estimator; + Homography model; + + // Initialize the samplers + // The main sampler is used for sampling in the main RANSAC loop + typedef sampler::Sampler AbstractSampler; + std::unique_ptr main_sampler; + if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler + main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); + else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. + main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); + else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler + main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, + { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension + // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. + estimator.sampleSize(), // The size of a minimal sample + { static_cast(w1), // The width of the source image + static_cast(h1), // The height of the source image + static_cast(w2), // The width of the destination image + static_cast(h2) }, // The height of the destination image + 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling + else if (sampler_id == 3) + main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, + point_probabilities, + estimator.sampleSize())); + else if (sampler_id == 4) + { + double max_prob = 0; + for (const auto &prob : point_probabilities) + max_prob = MAX(max_prob, prob); + for (auto &prob : point_probabilities) + prob /= max_prob; + main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, + point_probabilities, + estimator.sampleSize(), + sampler_variance)); + } + else + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling), 3 (NG-RANSAC sampling), 4 (AR-Sampler)\n", + sampler_id); + return 0; + } + + sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization + + // Checking if the samplers are initialized successfully. + if (!main_sampler->isInitialized() || + !local_optimization_sampler.isInitialized()) + { + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "One of the samplers is not initialized successfully.\n"); + return 0; + } + + utils::RANSACStatistics statistics; + + if (use_sprt) + { + // Initializing SPRT test + preemption::SPRTPreemptiveVerfication preemptive_verification( + points, + estimator); + + if (use_space_partitioning) + { + inlier_selector::SpacePartitioningRANSAC inlier_selector(neighborhood_graph.get()); + + GCRANSAC, + preemption::SPRTPreemptiveVerfication, + inlier_selector::SpacePartitioningRANSAC> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } else + { + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + GCRANSAC, + preemption::SPRTPreemptiveVerfication, + inlier_selector::EmptyInlierSelector> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + } + else + { + // Initializing an empty preemption + preemption::EmptyPreemptiveVerfication preemptive_verification; + + if (use_space_partitioning) + { + inlier_selector::SpacePartitioningRANSAC inlier_selector(neighborhood_graph.get()); + + GCRANSAC, + preemption::EmptyPreemptiveVerfication, + inlier_selector::SpacePartitioningRANSAC> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } else + { + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + GCRANSAC, + preemption::EmptyPreemptiveVerfication, + inlier_selector::EmptyInlierSelector> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + } + + homography.resize(9); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + homography[i * 3 + j] = model.descriptor(i, j); + } + } + + inliers.resize(num_tents); + + const int num_inliers = statistics.inliers.size(); + for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { + inliers[pt_idx] = 0; + + } + for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { + inliers[statistics.inliers[pt_idx]] = 1; + } + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + // Return the number of inliers found + return num_inliers; +} diff --git a/src/pygcransac/include/problem_instances/findLine2D_t.hpp b/src/pygcransac/include/problem_instances/findLine2D_t.hpp new file mode 100644 index 0000000..2a6be6b --- /dev/null +++ b/src/pygcransac/include/problem_instances/findLine2D_t.hpp @@ -0,0 +1,273 @@ +#pragma once +#include "gcransac_python.h" +#include +#include +#include "utils.h" +#include +#include + +#include "GCRANSAC.h" +#include "neighborhood/flann_neighborhood_graph.h" +#include "neighborhood/grid_neighborhood_graph.h" + +#include "samplers/uniform_sampler.h" +#include "samplers/prosac_sampler.h" +#include "samplers/napsac_sampler.h" +#include "samplers/progressive_napsac_sampler.h" +#include "samplers/importance_sampler.h" +#include "samplers/adaptive_reordering_sampler.h" +#include "samplers/single_point_sampler.h" + +#include "preemption/preemption_sprt.h" + +#include "inlier_selectors/empty_inlier_selector.h" +#include "inlier_selectors/space_partitioning_ransac.h" + +#include +#include +#include + +using namespace gcransac; + + +// A method for estimating a 2D line from a set of 2D points +template +int findLine2D_( + // The 2D points in the image + std::vector& points, + // The probabilities for each 3D-3D point correspondence if available + std::vector &point_probabilities, + // Output: the found inliers + std::vector& inliers, + // Output: the found 2d line + std::vector &line, + // The image size + int w, int h, + // Number of elements per point in the data matrix + int element_number, + // 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. I do not suggest setting it to lower than 50. + 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. + // (2) Progressive NAPSAC 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. + // (1) Grid-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 + // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) + double neighborhood_size, + // The number of RANSAC iterations done in the local optimization + int lo_number) +{ + // The number of points provided + const size_t &num_points = points.size() / element_number; + // The matrix containing the points that will be passed to GC-RANSAC + cv::Mat point_matrix(num_points, element_number, CV_64F, &points[0]); + + // Initializing the neighborhood structure based on the provided paramereters + typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; + std::unique_ptr neighborhood_graph; + + // The cell size or radius-search radius of the neighborhood graph + const size_t cell_number_in_neighborhood_graph_ = + static_cast(neighborhood_size); + + // If the spatial weight is 0.0, the neighborhood graph should not be created + if (spatial_coherence_weight <= std::numeric_limits::epsilon()) + { + cv::Mat empty_point_matrix(0, 2, CV_64F); + + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<2>(&empty_point_matrix, // The input points + { 0, // The cell size along axis X + 0 }, // The cell size along axis Y + 1)); // The cell number along every axis + } else // Initializing a grid-based neighborhood graph + { + if (neighborhood_id == 0) + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<4>(&point_matrix, // The input points + { w / static_cast(cell_number_in_neighborhood_graph_), // The cell size along axis X + h / static_cast(cell_number_in_neighborhood_graph_) }, // The cell size along axis Y + cell_number_in_neighborhood_graph_)); // The cell number along every axis + else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::FlannNeighborhoodGraph(&point_matrix, neighborhood_size)); + else + { + fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", + neighborhood_id); + return 0; + } + } + + // Checking if the neighborhood graph is initialized successfully. + if (!neighborhood_graph->isInitialized()) + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); + return 0; + } + + // Initializing the line estimator + Line2dEstimator estimator; + + // Initializing the model object + Line2D model; + + // Initialize the samplers + // The main sampler is used for sampling in the main RANSAC loop + typedef sampler::Sampler AbstractSampler; + std::unique_ptr main_sampler; + if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler + main_sampler = std::unique_ptr(new sampler::UniformSampler(&point_matrix)); + else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. + main_sampler = std::unique_ptr(new sampler::ProsacSampler(&point_matrix, estimator.sampleSize())); + else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler + main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&point_matrix, + { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension + // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. + estimator.sampleSize(), // The size of a minimal sample + { static_cast(w), // The width of the image + static_cast(h) }, // The height of the image + 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling + else + { + fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", + sampler_id); + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + return 0; + } + + // The local optimization sampler is used inside the local optimization + sampler::UniformSampler local_optimization_sampler(&point_matrix); + + // Checking if the samplers are initialized successfully. + if (!main_sampler->isInitialized() || + !local_optimization_sampler.isInitialized()) + { + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "One of the samplers is not initialized successfully.\n"); + return 0; + } + + utils::RANSACStatistics statistics; + + // Initializing the fast inlier selector object + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + if (use_sprt) + { + // Initializing SPRT test + preemption::SPRTPreemptiveVerfication preemptive_verification( + point_matrix, + estimator); + + GCRANSAC, + preemption::SPRTPreemptiveVerfication> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(point_matrix, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + else + { + GCRANSAC gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball + + // Start GC-RANSAC + gcransac.run(point_matrix, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model); + + statistics = gcransac.getRansacStatistics(); + } + + line.resize(3); + + for (int i = 0; i < 3; i++) { + line[i] = model.descriptor(i); + } + + inliers.resize(num_points); + + const int num_inliers = statistics.inliers.size(); + for (auto pt_idx = 0; pt_idx < num_points; ++pt_idx) { + inliers[pt_idx] = 0; + + } + for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { + inliers[statistics.inliers[pt_idx]] = 1; + } + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + // The number of inliers found + return num_inliers; +} diff --git a/src/pygcransac/include/problem_instances/findRigidTransform_t.hpp b/src/pygcransac/include/problem_instances/findRigidTransform_t.hpp new file mode 100644 index 0000000..6b7a98d --- /dev/null +++ b/src/pygcransac/include/problem_instances/findRigidTransform_t.hpp @@ -0,0 +1,361 @@ +#pragma once +#include "gcransac_python.h" +#include +#include +#include "utils.h" +#include +#include + +#include "GCRANSAC.h" +#include "neighborhood/flann_neighborhood_graph.h" +#include "neighborhood/grid_neighborhood_graph.h" + +#include "samplers/uniform_sampler.h" +#include "samplers/prosac_sampler.h" +#include "samplers/napsac_sampler.h" +#include "samplers/progressive_napsac_sampler.h" +#include "samplers/importance_sampler.h" +#include "samplers/adaptive_reordering_sampler.h" +#include "samplers/single_point_sampler.h" + +#include "preemption/preemption_sprt.h" + +#include "inlier_selectors/empty_inlier_selector.h" +#include "inlier_selectors/space_partitioning_ransac.h" + +#include +#include +#include + +using namespace gcransac; + + +// A method for estimating a rigid translation between two point clouds +template +int findRigidTransform_( + // The first point cloud + std::vector& correspondences, + // The probabilities for each 3D-3D point correspondence if available + std::vector &point_probabilities, + // Output: the found inliers + std::vector& inliers, + // Output: the found 6D pose + std::vector &pose, + // Number of elements per point in the data matrix + int element_number, + // 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. I do not suggest setting it to lower than 50. + 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, + // A flag determining if space partitioning from + // Barath, Daniel, and Gabor Valasek. "Space-Partitioning RANSAC." arXiv preprint arXiv:2111.12385 (2021). + // should be used to speed up the model verification. + bool use_space_partitioning, + // The number of RANSAC iterations done in the local optimization + int lo_number) +{ + size_t num_tents = correspondences.size() / element_number; + cv::Mat points(num_tents, element_number, CV_64F, &correspondences[0]); + size_t iterations = 0; + double box_size[6]; + + if (neighborhood_id == 1 && + use_space_partitioning) + { + fprintf(stderr, "When space partitioning is turned on, only neighborhood_id == 0 is accepted.\n"); + return 0; + } + + typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; + std::unique_ptr neighborhood_graph; + + const size_t cell_number_in_neighborhood_graph_ = + static_cast(neighborhood_size); + + // If the spatial weight is 0.0, the neighborhood graph should not be created + if (spatial_coherence_weight <= std::numeric_limits::epsilon() && + sampler_id != 2 && + !use_space_partitioning) + { + cv::Mat emptyPoints(0, 6, CV_64F); + + if (neighborhood_id == 0) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<6>(&points, + { 0.0 / static_cast(cell_number_in_neighborhood_graph_), + 0.0 / static_cast(cell_number_in_neighborhood_graph_), + 0.0 / static_cast(cell_number_in_neighborhood_graph_), + 0.0 / static_cast(cell_number_in_neighborhood_graph_), + 0.0 / static_cast(cell_number_in_neighborhood_graph_), + 0.0 / static_cast(cell_number_in_neighborhood_graph_) }, + cell_number_in_neighborhood_graph_)); + else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::FlannNeighborhoodGraph(&emptyPoints, neighborhood_size)); + else + { + fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are: 0 (Grid-based neighborhood), 1 (FLANN-based neighborhood)\n", + neighborhood_id); + return 0; + } + } else // Initializing a grid-based neighborhood graph + { + if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN + neighborhood_graph = std::unique_ptr( + new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); + else if (neighborhood_id == 0) + { + // Calculating the box dimensions. + size_t coord_idx = 0; + for (size_t dimension = 0; dimension < 6; ++dimension) + box_size[dimension] = points.at(0, dimension); //correspondences[coord_idx++]; + for (size_t point_idx = 1; point_idx < num_tents; ++point_idx) + for (size_t dimension = 0; dimension < 6; ++dimension) + box_size[dimension] = MAX(points.at(point_idx, dimension), box_size[dimension]); + + neighborhood_graph = std::unique_ptr( + new neighborhood::GridNeighborhoodGraph<6>(&points, + { box_size[0] / static_cast(cell_number_in_neighborhood_graph_), + box_size[1] / static_cast(cell_number_in_neighborhood_graph_), + box_size[2] / static_cast(cell_number_in_neighborhood_graph_), + box_size[3] / static_cast(cell_number_in_neighborhood_graph_), + box_size[4] / static_cast(cell_number_in_neighborhood_graph_), + box_size[5] / static_cast(cell_number_in_neighborhood_graph_) }, + cell_number_in_neighborhood_graph_)); + } + else + { + fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are: 0 (Grid-based neighborhood), 1 (FLANN-based neighborhood)\n", + neighborhood_id); + return 0; + } + } + + // Apply Graph-cut RANSAC + RigidTransformEstimator estimator; + RigidTransformation model; + + // Initialize the samplers + // The main sampler is used for sampling in the main RANSAC loop + typedef sampler::Sampler AbstractSampler; + std::unique_ptr main_sampler; + if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler + main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); + else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. + main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); + else if (sampler_id == 2) // Initializing a NAPSAC sampler. This requires the points to be ordered according to the quality. + { + if (neighborhood_id != 1 || + neighborhood_size <= std::numeric_limits::epsilon()) + { + fprintf(stderr, "For NAPSAC sampler, the neighborhood type must be 1 and its size >0.\n"); + return 0; + } + + main_sampler = std::unique_ptr(new sampler::NapsacSampler( + &points, dynamic_cast(neighborhood_graph.get()))); + } else if (sampler_id == 3) // Initializing the importance sampler + main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, + point_probabilities, + estimator.sampleSize())); + else if (sampler_id == 4) // Initializing the adaptive re-ordering sampler + { + double max_prob = 0; + for (const auto &prob : point_probabilities) + max_prob = MAX(max_prob, prob); + for (auto &prob : point_probabilities) + prob /= max_prob; + main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, + point_probabilities, + estimator.sampleSize(), + sampler_variance)); + } + else + { + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (NAPSAC sampling), 3 (Importance sampler), 4 (Adaptive re-ordering sampler)\n", + sampler_id); + return 0; + } + + sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization + + // Checking if the samplers are initialized successfully. + if (!main_sampler->isInitialized() || + !local_optimization_sampler.isInitialized()) + { + fprintf(stderr, "One of the samplers is not initialized successfully.\n"); + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + return 0; + } + + utils::RANSACStatistics statistics; + + // Initializing the fast inlier selector object + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + if (use_sprt) + { + // Initializing SPRT test + preemption::SPRTPreemptiveVerfication preemptive_verification( + points, + estimator, + min_inlier_ratio_for_sprt); + + GCRANSAC, + preemption::SPRTPreemptiveVerfication> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + else + { + // Initializing an empty preemption + preemption::EmptyPreemptiveVerfication preemptive_verification; + + if (use_space_partitioning) + { + // The space partitioning algorithm to accelerate inlier selection + inlier_selector::SpacePartitioningRANSAC inlier_selector( + neighborhood_graph.get()); + + GCRANSAC, + preemption::EmptyPreemptiveVerfication, + inlier_selector::SpacePartitioningRANSAC> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } else + { + // Initializing the fast inlier selector object + inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); + + GCRANSAC, + preemption::EmptyPreemptiveVerfication, + inlier_selector::EmptyInlierSelector> gcransac; + gcransac.settings.threshold = threshold; // The inlier-outlier threshold + gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term + gcransac.settings.confidence = conf; // The required confidence in the results + gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations + gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations + gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations + + // Start GC-RANSAC + gcransac.run(points, + estimator, + main_sampler.get(), + &local_optimization_sampler, + neighborhood_graph.get(), + model, + preemptive_verification, + inlier_selector); + + statistics = gcransac.getRansacStatistics(); + } + } + + inliers.resize(num_tents); + + const int num_inliers = statistics.inliers.size(); + for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { + inliers[pt_idx] = 0; + } + + for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { + inliers[statistics.inliers[pt_idx]] = 1; + } + + pose.resize(16); + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + pose[i * 4 + j] = (double)model.descriptor(i, j); + } + } + + // It is ugly: the unique_ptr does not check for virtual descructors in the base class. + // Therefore, the derived class's objects are not deleted automatically. + // This causes a memory leaking. I hate C++. + AbstractSampler *sampler_ptr = main_sampler.release(); + delete sampler_ptr; + + AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); + delete neighborhood_graph_ptr; + + // The number of inliers found + return num_inliers; +} diff --git a/src/pygcransac/include/sturm8.h b/src/pygcransac/include/sturm8.h index 7a0af59..7efe409 100644 --- a/src/pygcransac/include/sturm8.h +++ b/src/pygcransac/include/sturm8.h @@ -39,7 +39,7 @@ namespace re3q3 { // Constructs the quotients needed for evaluating the sturm sequence. -void build_sturm_seq(const double *fvec, double *svec) { +inline void build_sturm_seq(const double *fvec, double *svec) { double f[24]; double *f1 = f; @@ -117,7 +117,7 @@ inline double get_bounds(const double fvec[45]) { } // Applies Ridder's bracketing method until we get close to root, followed by newton iterations -void ridders_method_newton(const double fvec[17], double a, double b, double roots[8], int &n_roots, double tol) { +inline void ridders_method_newton(const double fvec[17], double a, double b, double roots[8], int &n_roots, double tol) { double fa = polyval<8>(fvec, a); double fb = polyval<8>(fvec, b); @@ -173,7 +173,7 @@ void ridders_method_newton(const double fvec[17], double a, double b, double roo roots[n_roots++] = x; } -void isolate_roots(const double fvec[17], const double svec[24], double a, double b, int sa, int sb, double roots[8], int &n_roots, double tol, int depth) { +inline void isolate_roots(const double fvec[17], const double svec[24], double a, double b, int sa, int sb, double roots[8], int &n_roots, double tol, int depth) { if (depth > 30) return; @@ -189,7 +189,7 @@ void isolate_roots(const double fvec[17], const double svec[24], double a, doubl } } -int bisect_sturm(double c0, double c1, double c2, double c3, double c4, double c5, double c6, double c7, double c8, double roots[8], double tol) { +inline int bisect_sturm(double c0, double c1, double c2, double c3, double c4, double c5, double c6, double c7, double c8, double roots[8], double tol) { if (c8 == 0.0) return 0; diff --git a/src/pygcransac/include/types.h b/src/pygcransac/include/types.h index 2290258..e12f7c9 100644 --- a/src/pygcransac/include/types.h +++ b/src/pygcransac/include/types.h @@ -69,6 +69,11 @@ #include "estimators/solver_homography_bundle_adjustment.h" #include "estimators/solver_radial_homography_5pc.h" #include "estimators/solver_radial_homography_6pc.h" +#include "estimators/solver_acp1p_cayley.h" +#include "estimators/solver_acp1p_query.h" +#include "estimators/solver_siftp1p.h" +#include "estimators/solver_siftp2p.h" +#include "estimators/solver_up2p.h" namespace gcransac { @@ -137,6 +142,18 @@ namespace gcransac estimator::solver::PnPBundleAdjustment> // The solver used for fitting a model to a non-minimal sample DefaultPnPEstimator; + typedef estimator::PerspectiveNPointEstimator // The solver used for fitting a model to a non-minimal sample + SIFTP1PEstimator; + + typedef estimator::PerspectiveNPointEstimator // The solver used for fitting a model to a non-minimal sample + ACP1PEstimator; + + typedef estimator::PerspectiveNPointEstimator // The solver used for fitting a model to a non-minimal sample + SIFTP2PEstimator; + // The default estimator for PnP fitting typedef estimator::RigidTransformationEstimator // The solver used for fitting a model to a non-minimal sample @@ -159,4 +176,4 @@ namespace gcransac estimator::solver::RadialHomography6PC> // The solver used for fitting a model to a non-minimal sample DefaultRadialHomographyEstimator; } -} \ No newline at end of file +} diff --git a/src/pygcransac/include/utils.h b/src/pygcransac/include/utils.h index 83ee384..59c2f4d 100644 --- a/src/pygcransac/include/utils.h +++ b/src/pygcransac/include/utils.h @@ -56,13 +56,13 @@ namespace gcransac { namespace utils { - void normalizeCorrespondences(const cv::Mat &points_, + inline void normalizeCorrespondences(const cv::Mat &points_, const Eigen::Matrix3d &intrinsics_src_, const Eigen::Matrix3d &intrinsics_dst_, cv::Mat &normalized_points_); - void normalizeCorrespondences(const cv::Mat &points_, + inline void normalizeCorrespondences(const cv::Mat &points_, const Eigen::Matrix3d &intrinsics_src_, const Eigen::Matrix3d &intrinsics_dst_, cv::Mat &normalized_points_) diff --git a/src/pygcransac/src/bindings.cpp b/src/pygcransac/src/bindings.cpp index 5f55321..5b77b8b 100644 --- a/src/pygcransac/src/bindings.cpp +++ b/src/pygcransac/src/bindings.cpp @@ -3,6 +3,15 @@ #include #include #include "gcransac_python.h" +#include "types.h" + +#include "problem_instances/findRigidTransform_t.hpp" +#include "problem_instances/find6DPose_t.hpp" +#include "problem_instances/findLine2D_t.hpp" +#include "problem_instances/findFundamentalMatrix_t.hpp" +#include "problem_instances/findEssentialMatrix_t.hpp" +#include "problem_instances/findGravityEssentialMatrix_t.hpp" +#include "problem_instances/findHomography_t.hpp" namespace py = pybind11; @@ -24,11 +33,12 @@ py::tuple findRigidTransform( bool use_space_partitioning, double sampler_variance) { + const size_t RIGID_TRANSFORM_DIM = 6; py::buffer_info buf1 = x1y1z1x2y2z2_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (DIM != 6) { + if (DIM != RIGID_TRANSFORM_DIM) { throw std::invalid_argument("x1y1z1x2y2z2 should be an array with dims [n,6], n>=3"); } if (NUM_TENTS < 3) { @@ -50,11 +60,12 @@ py::tuple findRigidTransform( std::vector pose(16); std::vector inliers(NUM_TENTS); - int num_inl = findRigidTransform_( + int num_inl = findRigidTransform_( x1y1z1x2y2z2, probabilities, inliers, pose, + RIGID_TRANSFORM_DIM, spatial_coherence_weight, threshold, conf, @@ -102,11 +113,12 @@ py::tuple find6DPoseSIFT( int lo_number, double sampler_variance) { + const size_t POSE_6DSIFT_DIM = 12; py::buffer_info buf1 = x1y1x2y2z2nxnynza11a12a21a22_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (DIM != 12) + if (DIM != POSE_6DSIFT_DIM) throw std::invalid_argument("x1y1x2y2z2nxnynza11a12a21a22 should be an array with dims [n,12], n>=1"); if (NUM_TENTS < 4) throw std::invalid_argument("x1y1x2y2z2nxnynza11a12a21a22 should be an array with dims [n,12], n>=1"); @@ -129,11 +141,12 @@ py::tuple find6DPoseSIFT( int num_inl; if (solver == 0) - num_inl = find6DPoseSIFTP1P_( + num_inl = find6DPose_( x1y1x2y2z2nxnynza11a12a21a22, probabilities, inliers, pose, + POSE_6DSIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -145,13 +158,15 @@ py::tuple find6DPoseSIFT( neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + false); else if (solver == 1) - num_inl = find6DPoseSIFTP2P_( + num_inl = find6DPose_( x1y1x2y2z2nxnynza11a12a21a22, probabilities, inliers, pose, + POSE_6DSIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -163,13 +178,15 @@ py::tuple find6DPoseSIFT( neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + false); else if (solver == 2) - num_inl = find6DPoseACP1P_( + num_inl = find6DPose_( x1y1x2y2z2nxnynza11a12a21a22, probabilities, inliers, pose, + POSE_6DSIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -181,7 +198,8 @@ py::tuple find6DPoseSIFT( neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + false); else throw std::invalid_argument("Invalid solver type"); @@ -216,11 +234,12 @@ py::tuple find6DPose( int lo_number, double sampler_variance) { + const size_t POSE_6D_DIM = 5; py::buffer_info buf1 = x1y1x2y2z2_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (DIM != 5) { + if (DIM != POSE_6D_DIM) { throw std::invalid_argument("x1y1x2y2z2 should be an array with dims [n,5], n>=4"); } if (NUM_TENTS < 4) { @@ -242,11 +261,12 @@ py::tuple find6DPose( std::vector pose(12); std::vector inliers(NUM_TENTS); - int num_inl = find6DPose_( + int num_inl = find6DPose_( x1y1x2y2z2, probabilities, inliers, pose, + POSE_6D_DIM, spatial_coherence_weight, threshold, conf, @@ -258,7 +278,8 @@ py::tuple find6DPose( neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + true); py::array_t inliers_ = py::array_t(NUM_TENTS); py::buffer_info buf3 = inliers_.request(); @@ -294,14 +315,15 @@ py::tuple findFundamentalMatrix( double sampler_variance, int solver) { + const size_t FUND_MAT_DIM = 4, FUND_MAT_SIFT_DIM = 8; py::buffer_info buf1 = correspondences_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (solver == 0 && DIM != 4) { + if (solver == 0 && DIM != FUND_MAT_DIM) { throw std::invalid_argument( "correspondences should be an array with dims [n,4], n>=7" ); } - if (solver > 0 && DIM != 8) { + if (solver > 0 && DIM != FUND_MAT_SIFT_DIM) { throw std::invalid_argument( "SIFT or affine correspondences should be an array with dims [n,8], n>=7" ); } if (NUM_TENTS < 7) { @@ -326,12 +348,13 @@ py::tuple findFundamentalMatrix( int num_inl = 0; if (solver == 0) // Point correspondence-based fundamental matrix estimation - num_inl = findFundamentalMatrix_( + num_inl = findFundamentalMatrix_( correspondences, probabilities, inliers, F, h1, w1, h2, w2, + FUND_MAT_DIM, spatial_coherence_weight, threshold, conf, @@ -343,14 +366,16 @@ py::tuple findFundamentalMatrix( neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + false); else if (solver == 1) // SIFT correspondence-based fundamental matrix estimation - num_inl = findFundamentalMatrixSIFT_( + num_inl = findFundamentalMatrix_( correspondences, probabilities, inliers, F, h1, w1, h2, w2, + FUND_MAT_SIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -362,14 +387,16 @@ py::tuple findFundamentalMatrix( neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + true); else if (solver == 2) // Affine correspondence-based fundamental matrix estimation - num_inl = findFundamentalMatrixAC_( + num_inl = findFundamentalMatrix_( correspondences, probabilities, inliers, F, h1, w1, h2, w2, + FUND_MAT_SIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -381,7 +408,8 @@ py::tuple findFundamentalMatrix( neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + true); py::array_t inliers_ = py::array_t(NUM_TENTS); py::buffer_info buf3 = inliers_.request(); @@ -415,11 +443,12 @@ py::tuple findLine2D(py::array_t x1y1_, int lo_number, double neighborhood_size) { + const size_t LINED2D_DIM = 2; py::buffer_info buf1 = x1y1_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (DIM != 2) { + if (DIM != LINED2D_DIM) { throw std::invalid_argument("x1y1 should be an array with dims [n,2], n>=2"); } if (NUM_TENTS < 2) { @@ -441,11 +470,12 @@ py::tuple findLine2D(py::array_t x1y1_, std::vector linemodel(3); std::vector inliers(NUM_TENTS); - int num_inl = findLine2D_(x1y1, + int num_inl = findLine2D_(x1y1, probabilities, inliers, linemodel, w1, h1, + LINED2D_DIM, threshold, conf, max_iters, @@ -494,11 +524,12 @@ py::tuple findGravityEssentialMatrix( int lo_number, double neighborhood_size) { + const size_t GRAVITY_ESS_MAT_DIM = 4; py::buffer_info buf1 = correspondences_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (DIM != 4) { + if (DIM != GRAVITY_ESS_MAT_DIM) { throw std::invalid_argument( "correspondences should be an array with dims [n,4], n>=5" ); } if (NUM_TENTS < 3) { @@ -570,14 +601,16 @@ py::tuple findGravityEssentialMatrix( std::vector E(9); std::vector inliers(NUM_TENTS); - int num_inl = findGravityEssentialMatrix_( + int num_inl = findGravityEssentialMatrix_( corrs, gravity_source, gravity_destination, probabilities, inliers, - E, K1, K2, + E, + K1, K2, h1, w1, h2, w2, + GRAVITY_ESS_MAT_DIM, spatial_coherence_weight, threshold, conf, @@ -625,11 +658,12 @@ py::tuple findPlanarEssentialMatrix( int lo_number, double neighborhood_size) { + const size_t PLANAR_ESS_MAT_DIM = 4; py::buffer_info buf1 = correspondences_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (DIM != 4) { + if (DIM != PLANAR_ESS_MAT_DIM) { throw std::invalid_argument( "correspondences should be an array with dims [n,4], n>=5" ); } if (NUM_TENTS < 3) { @@ -673,15 +707,17 @@ py::tuple findPlanarEssentialMatrix( std::vector F(9); std::vector inliers(NUM_TENTS); - int num_inl = findPlanarEssentialMatrix_( + int num_inl = findEssentialMatrix_( corrs, probabilities, - inliers, - F, K1, K2, - h1, w1, h2, w2, + inliers, + F, + K1, K2, + h1, w1, h2, w2, + PLANAR_ESS_MAT_DIM, spatial_coherence_weight, - threshold, - conf, + threshold, + conf, max_iters, min_iters, use_sprt, @@ -689,7 +725,9 @@ py::tuple findPlanarEssentialMatrix( sampler, neighborhood, neighborhood_size, - lo_number); + 0.1, + lo_number, + false); py::array_t inliers_ = py::array_t(NUM_TENTS); py::buffer_info buf3 = inliers_.request(); @@ -726,14 +764,15 @@ py::tuple findEssentialMatrix(py::array_t correspondences_, double sampler_variance, int solver) { + const size_t ESS_MAT_DIM = 4, ESS_MAT_SIFT_DIM = 8; py::buffer_info buf1 = correspondences_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (solver == 0 && DIM != 4) { + if (solver == 0 && DIM != ESS_MAT_DIM) { throw std::invalid_argument( "correspondences should be an array with dims [n,4], n>=5" ); } - if (solver > 0 && DIM != 8) { + if (solver > 0 && DIM != ESS_MAT_SIFT_DIM) { throw std::invalid_argument( "SIFT or affine correspondences should be an array with dims [n,8], n>=5" ); } if (NUM_TENTS < 5) { @@ -780,13 +819,14 @@ py::tuple findEssentialMatrix(py::array_t correspondences_, int num_inl = 0; if (solver == 0) // Point correspondence-based fundamental matrix estimation - num_inl = findEssentialMatrix_( + num_inl = findEssentialMatrix_( correspondences, probabilities, inliers, F, K1, K2, h1, w1, h2, w2, + ESS_MAT_DIM, spatial_coherence_weight, threshold, conf, @@ -798,15 +838,17 @@ py::tuple findEssentialMatrix(py::array_t correspondences_, neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + true); else if (solver == 1) // SIFT correspondence-based fundamental matrix estimation - num_inl = findEssentialMatrixSIFT_( + num_inl = findEssentialMatrix_( correspondences, probabilities, inliers, F, K1, K2, h1, w1, h2, w2, + ESS_MAT_SIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -818,15 +860,17 @@ py::tuple findEssentialMatrix(py::array_t correspondences_, neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + true); else if (solver == 2) // Affine correspondence-based fundamental matrix estimation - num_inl = findEssentialMatrixAC_( + num_inl = findEssentialMatrix_( correspondences, probabilities, inliers, F, K1, K2, h1, w1, h2, w2, + ESS_MAT_SIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -838,7 +882,8 @@ py::tuple findEssentialMatrix(py::array_t correspondences_, neighborhood, neighborhood_size, sampler_variance, - lo_number); + lo_number, + true); py::array_t inliers_ = py::array_t(NUM_TENTS); py::buffer_info buf3 = inliers_.request(); @@ -874,14 +919,15 @@ py::tuple findHomography(py::array_t correspondences_, double sampler_variance, int solver) { + const size_t HOMOGRAPHY_DIM = 4, HOMOGRAPHY_SIFT_DIM = 8; py::buffer_info buf1 = correspondences_.request(); size_t NUM_TENTS = buf1.shape[0]; size_t DIM = buf1.shape[1]; - if (solver == 0 && DIM != 4) { + if (solver == 0 && DIM != HOMOGRAPHY_DIM) { throw std::invalid_argument( "correspondences should be an array with dims [n,4], n>=4" ); } - if (solver > 0 && DIM != 8) { + if (solver > 0 && DIM != HOMOGRAPHY_SIFT_DIM) { throw std::invalid_argument( "SIFT or affine correspondences should be an array with dims [n,8], n>=4" ); } if (NUM_TENTS < 4) { @@ -906,12 +952,13 @@ py::tuple findHomography(py::array_t correspondences_, int num_inl = 0; if (solver == 0) // Point correspondence-based fundamental matrix estimation - num_inl = findHomography_( + num_inl = findHomography_t_( correspondences, probabilities, inliers, H, h1, w1, h2, w2, + HOMOGRAPHY_DIM, spatial_coherence_weight, threshold, conf, @@ -926,12 +973,13 @@ py::tuple findHomography(py::array_t correspondences_, sampler_variance, lo_number); else if (solver == 1) // SIFT correspondence-based fundamental matrix estimation - num_inl = findHomographySIFT_( + num_inl = findHomography_t_( correspondences, probabilities, inliers, H, h1, w1, h2, w2, + HOMOGRAPHY_SIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -942,15 +990,17 @@ py::tuple findHomography(py::array_t correspondences_, sampler, neighborhood, neighborhood_size, + use_space_partitioning, sampler_variance, lo_number); else if (solver == 2) // Affine correspondence-based fundamental matrix estimation - num_inl = findHomographyAC_( + num_inl = findHomography_t_( correspondences, probabilities, inliers, H, h1, w1, h2, w2, + HOMOGRAPHY_SIFT_DIM, spatial_coherence_weight, threshold, conf, @@ -961,6 +1011,7 @@ py::tuple findHomography(py::array_t correspondences_, sampler, neighborhood, neighborhood_size, + use_space_partitioning, sampler_variance, lo_number); @@ -1171,7 +1222,7 @@ PYBIND11_PLUGIN(pygcransac) { py::arg("sampler") = 1, py::arg("neighborhood") = 0, py::arg("neighborhood_size") = 4.0, - py::arg("use_space_partitioning") = true, + py::arg("use_space_partitioning") = false, py::arg("lo_number") = 50, py::arg("sampler_variance") = 0.1, py::arg("solver") = 0); diff --git a/src/pygcransac/src/gcransac_python.cpp b/src/pygcransac/src/gcransac_python.cpp deleted file mode 100644 index 95b59cd..0000000 --- a/src/pygcransac/src/gcransac_python.cpp +++ /dev/null @@ -1,5069 +0,0 @@ -#include "gcransac_python.h" -#include -#include -#include "utils.h" -#include -#include - -#include "GCRANSAC.h" -#include "neighborhood/flann_neighborhood_graph.h" -#include "neighborhood/grid_neighborhood_graph.h" - -#include "samplers/uniform_sampler.h" -#include "samplers/prosac_sampler.h" -#include "samplers/napsac_sampler.h" -#include "samplers/progressive_napsac_sampler.h" -#include "samplers/importance_sampler.h" -#include "samplers/adaptive_reordering_sampler.h" -#include "samplers/single_point_sampler.h" - -#include "estimators/fundamental_estimator.h" -#include "estimators/homography_estimator.h" -#include "estimators/homography_affine_correspondence_estimator.h" -#include "estimators/essential_estimator.h" - -#include "preemption/preemption_sprt.h" - -#include "inlier_selectors/empty_inlier_selector.h" -#include "inlier_selectors/space_partitioning_ransac.h" - -#include "estimators/solver_fundamental_matrix_seven_point.h" -#include "estimators/solver_fundamental_matrix_eight_point.h" -#include "estimators/solver_homography_four_point.h" -#include "estimators/solver_essential_matrix_five_point_stewenius.h" -#include "estimators/solver_acp1p_cayley.h" -#include "estimators/solver_acp1p_query.h" -#include "estimators/solver_siftp1p.h" -#include "estimators/solver_siftp2p.h" -#include "estimators/solver_up2p.h" - -#include -#include -#include - -using namespace gcransac; - -// A method for estimating a 2D line from a set of 2D points -int findLine2D_( - // The 2D points in the image - std::vector& points, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 2d line - std::vector &line, - // The image size - int w, int h, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC 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. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - double neighborhood_size, - // The number of RANSAC iterations done in the local optimization - int lo_number) -{ - // The number of points provided - const size_t &num_points = points.size() / 2; - - // The matrix containing the points that will be passed to GC-RANSAC - cv::Mat point_matrix(num_points, 2, CV_64F, &points[0]); - - // Initializing the neighborhood structure based on the provided paramereters - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - // The cell size or radius-search radius of the neighborhood graph - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat empty_point_matrix(0, 2, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<2>(&empty_point_matrix, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&point_matrix, // The input points - { w / static_cast(cell_number_in_neighborhood_graph_), // The cell size along axis X - h / static_cast(cell_number_in_neighborhood_graph_) }, // The cell size along axis Y - cell_number_in_neighborhood_graph_)); // The cell number along every axis - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&point_matrix, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - - // Initializing the line estimator - utils::Default2DLineEstimator estimator; - - // Initializing the model object - Line2D model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&point_matrix)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&point_matrix, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&point_matrix, - { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension - // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. - estimator.sampleSize(), // The size of a minimal sample - { static_cast(w), // The width of the image - static_cast(h) }, // The height of the image - 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling - else - { - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - // The local optimization sampler is used inside the local optimization - sampler::UniformSampler local_optimization_sampler(&point_matrix); - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - point_matrix, - estimator); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(point_matrix, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(point_matrix, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - line.resize(3); - - for (int i = 0; i < 3; i++) { - line[i] = model.descriptor(i); - } - - inliers.resize(num_points); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_points; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating a the absolute pose given 2D-3D correspondences -int find6DPoseSIFTP1P_( - // The 2D-3D correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &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) -{ - size_t num_tents = correspondences.size() / 12; - cv::Mat points(num_tents, 12, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // Initializing the neighborhood graph if needed - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else - { - cv::Mat pointsForNeighborhood; - // Copy the first 5 columns of the points to the pointsForNeighborhood - points.colRange(0, 4).copyTo(pointsForNeighborhood); - - if (neighborhood_id == 0) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&pointsForNeighborhood, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - } - - // The estimator for PnP fitting - typedef estimator::PerspectiveNPointEstimator // The solver used for fitting a model to a non-minimal sample - SIFTP1PEstimator; - typedef SIFTP1PEstimator CurrentEstimator; - - // Apply Graph-cut RANSAC - CurrentEstimator estimator; - Pose6D model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Progressive NAPSAC is not usable for the absolute pose problem.\n", - sampler_id); - return 0; - } - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else if (sampler_id == 5) - main_sampler = std::unique_ptr(new sampler::SinglePointSampler(&points, 1)); - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - } - - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - pose.resize(12); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 4; j++) { - pose[i * 4 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating a the absolute pose given 2D-3D correspondences -int find6DPoseACP1P_( - // The 2D-3D correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &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) -{ - size_t num_tents = correspondences.size() / 12; - cv::Mat points(num_tents, 12, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // Initializing the neighborhood graph if needed - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else - { - cv::Mat pointsForNeighborhood; - // Copy the first 5 columns of the points to the pointsForNeighborhood - points.colRange(0, 4).copyTo(pointsForNeighborhood); - - if (neighborhood_id == 0) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&pointsForNeighborhood, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - } - - // The estimator for PnP fitting - typedef estimator::PerspectiveNPointEstimator // The solver used for fitting a model to a non-minimal sample - ACP1PEstimator; - typedef ACP1PEstimator CurrentEstimator; - - // Apply Graph-cut RANSAC - CurrentEstimator estimator; - Pose6D model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Progressive NAPSAC is not usable for the absolute pose problem.\n", - sampler_id); - return 0; - } - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else if (sampler_id == 5) - main_sampler = std::unique_ptr(new sampler::SinglePointSampler(&points, 1)); - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - } - - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - pose.resize(12); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 4; j++) { - pose[i * 4 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating a the absolute pose given 2D-3D correspondences -int find6DPoseSIFTP2P_( - // The 2D-3D correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &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) -{ - size_t num_tents = correspondences.size() / 12; - cv::Mat points(num_tents, 12, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // Initializing the neighborhood graph if needed - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else - { - cv::Mat pointsForNeighborhood; - // Copy the first 5 columns of the points to the pointsForNeighborhood - points.colRange(0, 4).copyTo(pointsForNeighborhood); - - if (neighborhood_id == 0) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&pointsForNeighborhood, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - } - - // The estimator for PnP fitting - typedef estimator::PerspectiveNPointEstimator // The solver used for fitting a model to a non-minimal sample - SIFTP2PEstimator; - - typedef SIFTP2PEstimator CurrentEstimator; - - // Apply Graph-cut RANSAC - CurrentEstimator estimator; - Pose6D model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Progressive NAPSAC is not usable for the absolute pose problem.\n", - sampler_id); - return 0; - } - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else if (sampler_id == 5) - main_sampler = std::unique_ptr(new sampler::SinglePointSampler(&points, 1)); - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - } - - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - pose.resize(12); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 4; j++) { - pose[i * 4 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating a the absolute pose given 2D-3D correspondences -int find6DPose_( - // The 2D-3D correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &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) -{ - size_t num_tents = correspondences.size() / 5; - cv::Mat points(num_tents, 5, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // Initializing the neighborhood graph if needed - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else - { - if (neighborhood_id == 0) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - } - - // Apply Graph-cut RANSAC - utils::DefaultPnPEstimator estimator; - Pose6D model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Progressive NAPSAC is not usable for the absolute pose problem.\n", - sampler_id); - return 0; - } - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - if (statistics.inliers.size() > 3) - { - std::vector models = { model }; - estimator::solver::PnPBundleAdjustment bundle_adjuster; - bundle_adjuster.estimateModel( - points, - &statistics.inliers[0], - statistics.inliers.size(), - models, - nullptr); - model.descriptor = models[0].descriptor; - } - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - } - - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - pose.resize(12); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 4; j++) { - pose[i * 4 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating a rigid translation between two point clouds -int findRigidTransform_( - // The first point cloud - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &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. I do not suggest setting it to lower than 50. - 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, - // A flag determining if space partitioning from - // Barath, Daniel, and Gabor Valasek. "Space-Partitioning RANSAC." arXiv preprint arXiv:2111.12385 (2021). - // should be used to speed up the model verification. - bool use_space_partitioning, - // The number of RANSAC iterations done in the local optimization - int lo_number) -{ - size_t num_tents = correspondences.size() / 6; - cv::Mat points(num_tents, 6, CV_64F, &correspondences[0]); - size_t iterations = 0; - double box_size[6]; - - if (neighborhood_id == 1 && - use_space_partitioning) - { - fprintf(stderr, "When space partitioning is turned on, only neighborhood_id == 0 is accepted.\n"); - return 0; - } - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon() && - sampler_id != 2 && - !use_space_partitioning) - { - cv::Mat emptyPoints(0, 6, CV_64F); - - if (neighborhood_id == 0) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<6>(&points, - { 0.0 / static_cast(cell_number_in_neighborhood_graph_), - 0.0 / static_cast(cell_number_in_neighborhood_graph_), - 0.0 / static_cast(cell_number_in_neighborhood_graph_), - 0.0 / static_cast(cell_number_in_neighborhood_graph_), - 0.0 / static_cast(cell_number_in_neighborhood_graph_), - 0.0 / static_cast(cell_number_in_neighborhood_graph_) }, - cell_number_in_neighborhood_graph_)); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&emptyPoints, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are: 0 (Grid-based neighborhood), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - } else // Initializing a grid-based neighborhood graph - { - if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); - else if (neighborhood_id == 0) - { - // Calculating the box dimensions. - size_t coord_idx = 0; - for (size_t dimension = 0; dimension < 6; ++dimension) - box_size[dimension] = points.at(0, dimension); //correspondences[coord_idx++]; - for (size_t point_idx = 1; point_idx < num_tents; ++point_idx) - for (size_t dimension = 0; dimension < 6; ++dimension) - box_size[dimension] = MAX(points.at(point_idx, dimension), box_size[dimension]); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<6>(&points, - { box_size[0] / static_cast(cell_number_in_neighborhood_graph_), - box_size[1] / static_cast(cell_number_in_neighborhood_graph_), - box_size[2] / static_cast(cell_number_in_neighborhood_graph_), - box_size[3] / static_cast(cell_number_in_neighborhood_graph_), - box_size[4] / static_cast(cell_number_in_neighborhood_graph_), - box_size[5] / static_cast(cell_number_in_neighborhood_graph_) }, - cell_number_in_neighborhood_graph_)); - } - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are: 0 (Grid-based neighborhood), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - } - - // Apply Graph-cut RANSAC - utils::DefaultRigidTransformationEstimator estimator; - RigidTransformation model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a NAPSAC sampler. This requires the points to be ordered according to the quality. - { - if (neighborhood_id != 1 || - neighborhood_size <= std::numeric_limits::epsilon()) - { - fprintf(stderr, "For NAPSAC sampler, the neighborhood type must be 1 and its size >0.\n"); - return 0; - } - - main_sampler = std::unique_ptr(new sampler::NapsacSampler( - &points, dynamic_cast(neighborhood_graph.get()))); - } else if (sampler_id == 3) // Initializing the importance sampler - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) // Initializing the adaptive re-ordering sampler - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (NAPSAC sampling), 3 (Importance sampler), 4 (Adaptive re-ordering sampler)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - // Initializing an empty preemption - preemption::EmptyPreemptiveVerfication preemptive_verification; - - if (use_space_partitioning) - { - // The space partitioning algorithm to accelerate inlier selection - inlier_selector::SpacePartitioningRANSAC inlier_selector( - neighborhood_graph.get()); - - GCRANSAC, - preemption::EmptyPreemptiveVerfication, - inlier_selector::SpacePartitioningRANSAC> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } else - { - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - GCRANSAC, - preemption::EmptyPreemptiveVerfication, - inlier_selector::EmptyInlierSelector> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - } - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - } - - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - pose.resize(16); - - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - pose[i * 4 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating a fundamental matrix given 2D-2D correspondences -int findFundamentalMatrix_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &fundamental_matrix, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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) -{ - int num_tents = correspondences.size() / 4; - cv::Mat points(num_tents, 4, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&points, - { w1 / static_cast(cell_number_in_neighborhood_graph_), - h1 / static_cast(cell_number_in_neighborhood_graph_), - w2 / static_cast(cell_number_in_neighborhood_graph_), - h2 / static_cast(cell_number_in_neighborhood_graph_) }, - cell_number_in_neighborhood_graph_)); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - // Calculating the maximum image diagonal to be used for setting the threshold - // adaptively for each image pair. - const double max_image_diagonal = - sqrt(pow(MAX(w1, w2), 2) + pow(MAX(h1, h2), 2)); - - // Apply Graph-cut RANSAC - utils::DefaultFundamentalMatrixEstimator estimator; - FundamentalMatrix model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, - { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension - // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. - estimator.sampleSize(), // The size of a minimal sample - { static_cast(w1), // The width of the source image - static_cast(h1), // The height of the source image - static_cast(w2), // The width of the destination image - static_cast(h2) }, // The height of the destination image - 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - // Running bundle adjustment minimizing the pose error on the found inliers - const size_t &inlier_number = statistics.inliers.size(); - if (statistics.inliers.size() > 7) - { - // The truncated least-squares threshold - const double truncated_threshold = 3.0 / 2.0 * threshold; - // The squared least-squares threshold - const double squared_truncated_threshold = truncated_threshold * truncated_threshold; - - // Initializing all weights to be zero - std::vector weights(inlier_number, 0.0); - - // Setting a weight for all inliers - for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) - { - const size_t point_idx = statistics.inliers[inlier_idx]; - weights[inlier_idx] = - MAX(0, 1.0 - estimator.squaredResidual(points.row(point_idx), model) / squared_truncated_threshold); - } - - std::vector models; - estimator::solver::FundamentalMatrixBundleAdjustmentSolver bundleOptimizer; - bundleOptimizer.estimateModel( - points, - &statistics.inliers[0], - statistics.inliers.size(), - models, - &weights[0]); - - // Scoring function - MSACScoringFunction scoring; - scoring.initialize(squared_truncated_threshold, points.rows); - // Score of the new model - Score score; - // Inliers of the new model - std::vector current_inliers; - - // Select the best model and update the inliers - for (auto &tmp_model : models) - { - current_inliers.clear(); - score = scoring.getScore(points, // All points - tmp_model, // The current model parameters - estimator, // The estimator - squared_truncated_threshold, // The current threshold - current_inliers); // The current inlier set - - // Check if the updated model is better than the best so far - if (statistics.score < score.value) - { - model.descriptor = tmp_model.descriptor; - statistics.score = score.value; - statistics.inliers.swap(current_inliers); - } - } - } - else - model.descriptor = Eigen::Matrix3d::Identity(); - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - } - - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - fundamental_matrix.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - fundamental_matrix[i * 3 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating a fundamental matrix given 2D-2D correspondences -int findFundamentalMatrixAC_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &fundamental_matrix, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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) -{ - // Each row contains 8 elements as [x1, y1, x2, y2, a11, a12, a21, a22] - int num_tents = correspondences.size() / 8; - cv::Mat points(num_tents, 8, CV_64F, &correspondences[0]); - - // The neighborhood structure used for the graph-cut-based local optimization - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Using only the point coordinates and not the affine elements when constructing the neighborhood. - cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); - - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, - { w1 / neighborhood_size, - h1 / neighborhood_size, - w2 / neighborhood_size, - h2 / neighborhood_size }, - static_cast(neighborhood_size))); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - // Apply Graph-cut RANSAC - utils::ACBasedFundamentalMatrixEstimator estimator; - FundamentalMatrix model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 3 (NG-RANSAC sampler), 4 (AR-Sampler)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - // Running bundle adjustment minimizing the pose error on the found inliers - if (statistics.inliers.size() > 7) - { - std::vector models; - estimator::solver::FundamentalMatrixBundleAdjustmentSolver bundleOptimizer; - bundleOptimizer.estimateModel( - points, - &statistics.inliers[0], - statistics.inliers.size(), - models); - - // The truncated least-squares threshold - const double truncated_threshold = 3.0 / 2.0 * threshold; - // The squared least-squares threshold - const double squared_truncated_threshold = truncated_threshold * truncated_threshold; - // Scoring function - MSACScoringFunction scoring; - scoring.initialize(squared_truncated_threshold, points.rows); - // Score of the new model - Score score; - // Inliers of the new model - std::vector inliers; - - // Select the best model and update the inliers - for (auto &tmp_model : models) - { - inliers.clear(); - score = scoring.getScore(points, // All points - tmp_model, // The current model parameters - estimator, // The estimator - squared_truncated_threshold, // The current threshold - inliers); // The current inlier set - - // Check if the updated model is better than the best so far - if (statistics.score < score.value) - { - model.descriptor = tmp_model.descriptor; - statistics.score = score.value; - statistics.inliers.swap(inliers); - } - } - } - else - model.descriptor = Eigen::Matrix3d::Identity(); - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - } - - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - fundamental_matrix.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - fundamental_matrix[i * 3 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - - -// A method for estimating a fundamental matrix given 2D-2D correspondences -int findFundamentalMatrixSIFT_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &fundamental_matrix, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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) -{ - // Each row contains 8 elements as [x1, y1, x2, y2, size1, size2, ori1, ori2] - int num_tents = correspondences.size() / 8; - cv::Mat points(num_tents, 8, CV_64F, &correspondences[0]); - - // The neighborhood structure used for the graph-cut-based local optimization - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Using only the point coordinates and not the affine elements when constructing the neighborhood. - cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); - - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, - { w1 / neighborhood_size, - h1 / neighborhood_size, - w2 / neighborhood_size, - h2 / neighborhood_size }, - static_cast(neighborhood_size))); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - // Apply Graph-cut RANSAC - utils::SIFTBasedFundamentalMatrixEstimator estimator; - FundamentalMatrix model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 3 (NG-RANSAC sampler), 4 (AR-Sampler)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = 8; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - // Running bundle adjustment minimizing the pose error on the found inliers - if (statistics.inliers.size() > 7) - { - std::vector models = { model }; - estimator::solver::FundamentalMatrixBundleAdjustmentSolver bundleOptimizer; - bundleOptimizer.estimateModel( - points, - &statistics.inliers[0], - statistics.inliers.size(), - models); - - // The truncated least-squares threshold - const double truncated_threshold = 3.0 / 2.0 * threshold; - // The squared least-squares threshold - const double squared_truncated_threshold = truncated_threshold * truncated_threshold; - // Scoring function - MSACScoringFunction scoring; - scoring.initialize(squared_truncated_threshold, points.rows); - // Score of the new model - Score score; - // Inliers of the new model - std::vector tmp_inliers; - - // Select the best model and update the inliers - for (auto &tmp_model : models) - { - tmp_inliers.clear(); - score = scoring.getScore(points, // All points - tmp_model, // The current model parameters - estimator, // The estimator - squared_truncated_threshold, // The current threshold - tmp_inliers); // The current inlier set - - // Check if the updated model is better than the best so far - if (statistics.score < score.value) - { - model.descriptor = tmp_model.descriptor; - statistics.score = score.value; - statistics.inliers.swap(tmp_inliers); - } - } - } - else - model.descriptor = Eigen::Matrix3d::Identity(); - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - } - - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - fundamental_matrix.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - fundamental_matrix[i * 3 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating an essential matrix given 2D-2D correspondences -int findEssentialMatrix_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &essential_matrix, - // The intrinsic camera matrix of the source image - std::vector &source_intrinsics, - // The intrinsic camera matrix of the destination image - std::vector &destination_intrinsics, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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) -{ - int num_tents = correspondences.size() / 4; - cv::Mat points(num_tents, 4, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&points, - { w1 / static_cast(cell_number_in_neighborhood_graph_), - h1 / static_cast(cell_number_in_neighborhood_graph_), - w2 / static_cast(cell_number_in_neighborhood_graph_), - h2 / static_cast(cell_number_in_neighborhood_graph_) }, - cell_number_in_neighborhood_graph_)); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - Eigen::Map> intrinsics_src(&source_intrinsics[0]); - Eigen::Map> intrinsics_dst(&destination_intrinsics[0]); - - // Calculating the maximum image diagonal to be used for setting the threshold - // adaptively for each image pair. - const double threshold_normalizer = - 0.25 * (intrinsics_src(0,0) + intrinsics_src(1,1) + intrinsics_dst(0,0) + intrinsics_dst(1,1)); - - cv::Mat normalized_points(points.size(), CV_64F); - utils::normalizeCorrespondences(points, - intrinsics_src, - intrinsics_dst, - normalized_points); - - // Apply Graph-cut RANSAC - utils::DefaultEssentialMatrixEstimator estimator(intrinsics_src, - intrinsics_dst); - EssentialMatrix model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, - { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension - // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. - estimator.sampleSize(), // The size of a minimal sample - { static_cast(w1), // The width of the source image - static_cast(h1), // The height of the source image - static_cast(w2), // The width of the destination image - static_cast(h2) }, // The height of the destination image - 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - // The local optimization sampler is used inside the local optimization - sampler::UniformSampler local_optimization_sampler(&points); - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - // Running bundle adjustment minimizing the pose error on the found inliers - const size_t &inlier_number = statistics.inliers.size(); - if (statistics.inliers.size() > 5) - { - // The truncated least-squares threshold - const double truncated_threshold = 3.0 / 2.0 * threshold / threshold_normalizer; - // The squared least-squares threshold - const double squared_truncated_threshold = truncated_threshold * truncated_threshold; - - // Initializing all weights to be zero - std::vector weights(inlier_number, 0.0); - - // Setting a weight for all inliers - for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) - { - const size_t point_idx = statistics.inliers[inlier_idx]; - weights[inlier_idx] = - MAX(0, 1.0 - estimator.squaredResidual(normalized_points.row(point_idx), model) / squared_truncated_threshold); - } - - std::vector models; - estimator::solver::EssentialMatrixBundleAdjustmentSolver bundleOptimizer; - bundleOptimizer.estimateModel( - normalized_points, - &statistics.inliers[0], - statistics.inliers.size(), - models, - &weights[0]); - - // Scoring function - MSACScoringFunction scoring; - scoring.initialize(squared_truncated_threshold, normalized_points.rows); - // Score of the new model - Score score; - // Inliers of the new model - std::vector current_inliers; - - // Select the best model and update the inliers - for (auto &tmp_model : models) - { - current_inliers.clear(); - score = scoring.getScore(normalized_points, // All points - tmp_model, // The current model parameters - estimator, // The estimator - squared_truncated_threshold, // The current threshold - current_inliers); // The current inlier set - - // Check if the updated model is better than the best so far - if (statistics.score < score.value) - { - model.descriptor = tmp_model.descriptor; - statistics.score = score.value; - statistics.inliers.swap(current_inliers); - } - } - } - else - model.descriptor = Eigen::Matrix3d::Identity(); - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - essential_matrix.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - essential_matrix[i * 3 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating an essential matrix given 2D-2D correspondences -int findEssentialMatrixAC_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &essential_matrix, - // The intrinsic camera matrix of the source image - std::vector &source_intrinsics, - // The intrinsic camera matrix of the destination image - std::vector &destination_intrinsics, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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) -{ - int num_tents = correspondences.size() / 8; - cv::Mat points(num_tents, 8, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Using only the point coordinates and not the affine elements when constructing the neighborhood. - cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); - - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, - { w1 / neighborhood_size, - h1 / neighborhood_size, - w2 / neighborhood_size, - h2 / neighborhood_size }, - static_cast(neighborhood_size))); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - Eigen::Map> - intrinsics_src(&source_intrinsics[0]); - Eigen::Map> - intrinsics_dst(&destination_intrinsics[0]); - - // Calculating the maximum image diagonal to be used for setting the threshold - // adaptively for each image pair. - const double threshold_normalizer = - 0.25 * (intrinsics_src(0,0) + intrinsics_src(1,1) + intrinsics_dst(0,0) + intrinsics_dst(1,1)); - - cv::Mat normalized_points(points.size(), CV_64F); - utils::normalizeCorrespondences(points, - intrinsics_src, - intrinsics_dst, - normalized_points); - - // Apply Graph-cut RANSAC - utils::ACBasedEssentialMatrixEstimator estimator(intrinsics_src, - intrinsics_dst); - EssentialMatrix model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, - { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension - // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. - estimator.sampleSize(), // The size of a minimal sample - { static_cast(w1), // The width of the source image - static_cast(h1), // The height of the source image - static_cast(w2), // The width of the destination image - static_cast(h2) }, // The height of the destination image - 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - // The local optimization sampler is used inside the local optimization - sampler::UniformSampler local_optimization_sampler(&points); - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = neighborhood_size; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = neighborhood_size; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - // Running bundle adjustment minimizing the pose error on the found inliers - const size_t &inlier_number = statistics.inliers.size(); - if (statistics.inliers.size() > 5) - { - // The truncated least-squares threshold - const double truncated_threshold = 3.0 / 2.0 * threshold / threshold_normalizer; - // The squared least-squares threshold - const double squared_truncated_threshold = truncated_threshold * truncated_threshold; - - // Initializing all weights to be zero - std::vector weights(inlier_number, 0.0); - - // Setting a weight for all inliers - for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) - { - const size_t point_idx = statistics.inliers[inlier_idx]; - weights[inlier_idx] = - MAX(0, 1.0 - estimator.squaredResidual(normalized_points.row(point_idx), model) / squared_truncated_threshold); - } - - std::vector models; - estimator::solver::EssentialMatrixBundleAdjustmentSolver bundleOptimizer; - bundleOptimizer.estimateModel( - normalized_points, - &statistics.inliers[0], - statistics.inliers.size(), - models, - &weights[0]); - - // Scoring function - MSACScoringFunction scoring; - scoring.initialize(squared_truncated_threshold, normalized_points.rows); - // Score of the new model - Score score; - // Inliers of the new model - std::vector current_inliers; - - // Select the best model and update the inliers - for (auto &tmp_model : models) - { - current_inliers.clear(); - score = scoring.getScore(normalized_points, // All points - tmp_model, // The current model parameters - estimator, // The estimator - squared_truncated_threshold, // The current threshold - current_inliers); // The current inlier set - - // Check if the updated model is better than the best so far - if (statistics.score < score.value) - { - model.descriptor = tmp_model.descriptor; - statistics.score = score.value; - statistics.inliers.swap(current_inliers); - } - } - } - else - model.descriptor = Eigen::Matrix3d::Identity(); - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - essential_matrix.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - essential_matrix[i * 3 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - - -// A method for estimating an essential matrix given 2D-2D correspondences -int findEssentialMatrixSIFT_( - // The 2D-2D point correspondences - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &essential_matrix, - // The intrinsic camera matrix of the source image - std::vector &source_intrinsics, - // The intrinsic camera matrix of the destination image - std::vector &destination_intrinsics, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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) -{ - int num_tents = correspondences.size() / 8; - cv::Mat points(num_tents, 8, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0, // The cell size along axis Y - 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Using only the point coordinates and not the affine elements when constructing the neighborhood. - cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); - - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, - { w1 / neighborhood_size, - h1 / neighborhood_size, - w2 / neighborhood_size, - h2 / neighborhood_size }, - static_cast(neighborhood_size))); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - Eigen::Map> - intrinsics_src(&source_intrinsics[0]); - Eigen::Map> - intrinsics_dst(&destination_intrinsics[0]); - - // Calculating the maximum image diagonal to be used for setting the threshold - // adaptively for each image pair. - const double threshold_normalizer = - 0.25 * (intrinsics_src(0,0) + intrinsics_src(1,1) + intrinsics_dst(0,0) + intrinsics_dst(1,1)); - - cv::Mat normalized_points(points.size(), CV_64F); - utils::normalizeCorrespondences(points, - intrinsics_src, - intrinsics_dst, - normalized_points); - - // Apply Graph-cut RANSAC - utils::SIFTBasedEssentialMatrixEstimator estimator(intrinsics_src, - intrinsics_dst); - EssentialMatrix model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 3 (NG-RANSAC sampling), 4 (AR-Sampler)\n", - sampler_id); - return 0; - } - - // The local optimization sampler is used inside the local optimization - sampler::UniformSampler local_optimization_sampler(&points); - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = neighborhood_size; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - GCRANSAC gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = neighborhood_size; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - // Running bundle adjustment minimizing the pose error on the found inliers - const size_t &inlier_number = statistics.inliers.size(); - if (statistics.inliers.size() > 5) - { - // The truncated least-squares threshold - const double truncated_threshold = 3.0 / 2.0 * threshold / threshold_normalizer; - // The squared least-squares threshold - const double squared_truncated_threshold = truncated_threshold * truncated_threshold; - - // Initializing all weights to be zero - std::vector weights(inlier_number, 0.0); - - // Setting a weight for all inliers - for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) - { - const size_t point_idx = statistics.inliers[inlier_idx]; - weights[inlier_idx] = - MAX(0, 1.0 - estimator.squaredResidual(normalized_points.row(point_idx), model) / squared_truncated_threshold); - } - - std::vector models; - estimator::solver::EssentialMatrixBundleAdjustmentSolver bundleOptimizer; - bundleOptimizer.estimateModel( - normalized_points, - &statistics.inliers[0], - statistics.inliers.size(), - models, - &weights[0]); - - // Scoring function - MSACScoringFunction scoring; - scoring.initialize(squared_truncated_threshold, normalized_points.rows); - // Score of the new model - Score score; - // Inliers of the new model - std::vector current_inliers; - - // Select the best model and update the inliers - for (auto &tmp_model : models) - { - current_inliers.clear(); - score = scoring.getScore(normalized_points, // All points - tmp_model, // The current model parameters - estimator, // The estimator - squared_truncated_threshold, // The current threshold - current_inliers); // The current inlier set - - // Check if the updated model is better than the best so far - if (statistics.score < score.value) - { - model.descriptor = tmp_model.descriptor; - statistics.score = score.value; - statistics.inliers.swap(current_inliers); - } - } - } - else - model.descriptor = Eigen::Matrix3d::Identity(); - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - essential_matrix.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - essential_matrix[i * 3 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating an essential matrix given 2D-2D correspondences -int findPlanarEssentialMatrix_( - // The 2D-2D point correspondences. - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &essential_matrix, - // The intrinsic camera matrix of the source image - std::vector &source_intrinsics, - // The intrinsic camera matrix of the destination image - std::vector &destination_intrinsics, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - double neighborhood_size, - // The number of RANSAC iterations done in the local optimization - int lo_number) -{ - int num_tents = correspondences.size() / 4; - cv::Mat points(num_tents, 4, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&points, - { w1 / static_cast(cell_number_in_neighborhood_graph_), - h1 / static_cast(cell_number_in_neighborhood_graph_), - w2 / static_cast(cell_number_in_neighborhood_graph_), - h2 / static_cast(cell_number_in_neighborhood_graph_) }, - cell_number_in_neighborhood_graph_)); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - Eigen::Map> intrinsics_src(&source_intrinsics[0]); - Eigen::Map> intrinsics_dst(&destination_intrinsics[0]); - - // Calculating the maximum image diagonal to be used for setting the threshold - // adaptively for each image pair. - const double threshold_normalizer = - 0.25 * (intrinsics_src(0,0) + intrinsics_src(1,1) + intrinsics_dst(0,0) + intrinsics_dst(1,1)); - - cv::Mat normalized_points(points.size(), CV_64F); - utils::normalizeCorrespondences(points, - intrinsics_src, - intrinsics_dst, - normalized_points); - - // Apply Graph-cut RANSAC - utils::DefaultPlanarEssentialMatrixEstimator estimator(intrinsics_src, - intrinsics_dst); - EssentialMatrix model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, - { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension - // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. - estimator.sampleSize(), // The size of a minimal sample - { static_cast(w1), // The width of the source image - static_cast(h1), // The height of the source image - static_cast(w2), // The width of the destination image - static_cast(h2) }, // The height of the destination image - 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double variance = 0.1; - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - // The local optimization sampler is used inside the local optimization - sampler::UniformSampler local_optimization_sampler(&points); - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - gcransac.settings.do_final_iterated_least_squares = false; - gcransac.settings.max_graph_cut_number = 100; - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - - GCRANSAC gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - // Running bundle adjustment minimizing the pose error on the found inliers - const size_t &inlier_number = statistics.inliers.size(); - if (statistics.inliers.size() > 5) - { - // The truncated least-squares threshold - const double truncated_threshold = 3.0 / 2.0 * threshold / threshold_normalizer; - // The squared least-squares threshold - const double squared_truncated_threshold = truncated_threshold * truncated_threshold; - - // Initializing all weights to be zero - std::vector weights(inlier_number, 0.0); - - // Setting a weight for all inliers - for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) - { - const size_t point_idx = statistics.inliers[inlier_idx]; - weights[inlier_idx] = - MAX(0, 1.0 - estimator.squaredResidual(normalized_points.row(point_idx), model) / squared_truncated_threshold); - } - - std::vector models; - estimator::solver::EssentialMatrixBundleAdjustmentSolver bundleOptimizer; - bundleOptimizer.estimateModel( - normalized_points, - &statistics.inliers[0], - statistics.inliers.size(), - models, - &weights[0]); - - // Scoring function - MSACScoringFunction scoring; - scoring.initialize(squared_truncated_threshold, normalized_points.rows); - // Score of the new model - Score score; - // Inliers of the new model - std::vector current_inliers; - - // Select the best model and update the inliers - for (auto &tmp_model : models) - { - current_inliers.clear(); - score = scoring.getScore(normalized_points, // All points - tmp_model, // The current model parameters - estimator, // The estimator - squared_truncated_threshold, // The current threshold - current_inliers); // The current inlier set - - // Check if the updated model is better than the best so far - if (statistics.score < score.value) - { - model.descriptor = tmp_model.descriptor; - statistics.score = score.value; - statistics.inliers.swap(current_inliers); - } - } - } - else - model.descriptor = Eigen::Matrix3d::Identity(); - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - essential_matrix.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - essential_matrix[i * 3 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating an essential matrix given 2D-2D correspondences -int findGravityEssentialMatrix_( - // The 2D-2D point correspondences. - std::vector& correspondences, - // The 2D-2D point correspondences. - std::vector& gravity_source, - // The 2D-2D point correspondences. - std::vector& gravity_destination, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &essential_matrix, - // The intrinsic camera matrix of the source image - std::vector &source_intrinsics, - // The intrinsic camera matrix of the destination image - std::vector &destination_intrinsics, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - double neighborhood_size, - // The number of RANSAC iterations done in the local optimization - int lo_number) -{ - int num_tents = correspondences.size() / 4; - cv::Mat points(num_tents, 4, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat emptyPoints(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&emptyPoints, // The input points - { 0, // The cell size along axis X - 0 }, // The cell size along axis Y - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&points, - { w1 / static_cast(cell_number_in_neighborhood_graph_), - h1 / static_cast(cell_number_in_neighborhood_graph_), - w2 / static_cast(cell_number_in_neighborhood_graph_), - h2 / static_cast(cell_number_in_neighborhood_graph_) }, - cell_number_in_neighborhood_graph_)); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - Eigen::Map> intrinsics_src(&source_intrinsics[0]); - Eigen::Map> intrinsics_dst(&destination_intrinsics[0]); - - Eigen::Map> gravity_src(&gravity_source[0]); - Eigen::Map> gravity_dst(&gravity_destination[0]); - - // Calculating the maximum image diagonal to be used for setting the threshold - // adaptively for each image pair. - const double threshold_normalizer = - 0.25 * (intrinsics_src(0,0) + intrinsics_src(1,1) + intrinsics_dst(0,0) + intrinsics_dst(1,1)); - - cv::Mat normalized_points(points.size(), CV_64F); - utils::normalizeCorrespondences(points, - intrinsics_src, - intrinsics_dst, - normalized_points); - - // Apply Graph-cut RANSAC - utils::DefaultGravityEssentialMatrixEstimator estimator(intrinsics_src, - intrinsics_dst); - EssentialMatrix model; - - // Setting the gravity directions to the solver - estimator.getMutableMinimalSolver()->setGravity(gravity_src, gravity_dst); - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, - { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension - // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. - estimator.sampleSize(), // The size of a minimal sample - { static_cast(w1), // The width of the source image - static_cast(h1), // The height of the source image - static_cast(w2), // The width of the destination image - static_cast(h2) }, // The height of the destination image - 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double variance = 0.1; - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - // The local optimization sampler is used inside the local optimization - sampler::UniformSampler local_optimization_sampler(&points); - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - - // Initializing the fast inlier selector object - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator, - min_inlier_ratio_for_sprt); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication> gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - gcransac.settings.do_final_iterated_least_squares = false; - gcransac.settings.max_graph_cut_number = 100; - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - - GCRANSAC gcransac; - gcransac.settings.threshold = threshold / threshold_normalizer; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(normalized_points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model); - - statistics = gcransac.getRansacStatistics(); - } - - // Running bundle adjustment minimizing the pose error on the found inliers - const size_t &inlier_number = statistics.inliers.size(); - if (statistics.inliers.size() > 5) - { - // The truncated least-squares threshold - const double truncated_threshold = 3.0 / 2.0 * threshold / threshold_normalizer; - // The squared least-squares threshold - const double squared_truncated_threshold = truncated_threshold * truncated_threshold; - - // Initializing all weights to be zero - std::vector weights(inlier_number, 0.0); - - // Setting a weight for all inliers - for (size_t inlier_idx = 0; inlier_idx < inlier_number; ++inlier_idx) - { - const size_t point_idx = statistics.inliers[inlier_idx]; - weights[inlier_idx] = - MAX(0, 1.0 - estimator.squaredResidual(normalized_points.row(point_idx), model) / squared_truncated_threshold); - } - - std::vector models; - estimator::solver::EssentialMatrixBundleAdjustmentSolver bundleOptimizer; - bundleOptimizer.estimateModel( - normalized_points, - &statistics.inliers[0], - statistics.inliers.size(), - models, - &weights[0]); - - // Scoring function - MSACScoringFunction scoring; - scoring.initialize(squared_truncated_threshold, normalized_points.rows); - // Score of the new model - Score score; - // Inliers of the new model - std::vector current_inliers; - - // Select the best model and update the inliers - for (auto &tmp_model : models) - { - current_inliers.clear(); - score = scoring.getScore(normalized_points, // All points - tmp_model, // The current model parameters - estimator, // The estimator - squared_truncated_threshold, // The current threshold - current_inliers); // The current inlier set - - // Check if the updated model is better than the best so far - if (statistics.score < score.value) - { - model.descriptor = tmp_model.descriptor; - statistics.score = score.value; - statistics.inliers.swap(current_inliers); - } - } - } - else - model.descriptor = Eigen::Matrix3d::Identity(); - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - essential_matrix.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - essential_matrix[i * 3 + j] = (double)model.descriptor(i, j); - } - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // The number of inliers found - return num_inliers; -} - -// A method for estimating a homography matrix given 2D-2D correspondences -int findHomography_( - // The 2D-2D point correspondences. - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &homography, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - double neighborhood_size, - // A flag determining if space partitioning from - // Barath, Daniel, and Gabor Valasek. "Space-Partitioning RANSAC." arXiv preprint arXiv:2111.12385 (2021). - // should be used to speed up the model verification. - bool use_space_partitioning, - // 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) -{ - int num_tents = correspondences.size() / 4; - cv::Mat points(num_tents, 4, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - if (use_space_partitioning && neighborhood_id != 0) - { - fprintf(stderr, "Space Partitioning only works with Grid neighorbood yet. Thus, setting neighborhood_id = 0.\n"); - neighborhood_id = 0; - } - - // If the spatial weight is 0.0, the neighborhood graph should not be created - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&points, - { (w1 + std::numeric_limits::epsilon()) / static_cast(cell_number_in_neighborhood_graph_), - (h1 + std::numeric_limits::epsilon()) / static_cast(cell_number_in_neighborhood_graph_), - (w2 + std::numeric_limits::epsilon()) / static_cast(cell_number_in_neighborhood_graph_), - (h2 + std::numeric_limits::epsilon()) / static_cast(cell_number_in_neighborhood_graph_) }, - cell_number_in_neighborhood_graph_)); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&points, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - - // Calculating the maximum image diagonal to be used for setting the threshold - // adaptively for each image pair. - const double max_image_diagonal = - sqrt(pow(MAX(w1, w2), 2) + pow(MAX(h1, h2), 2)); - - utils::DefaultHomographyEstimator estimator; - Homography model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 2) // Initializing a Progressive NAPSAC sampler - main_sampler = std::unique_ptr(new sampler::ProgressiveNapsacSampler<4>(&points, - { 16, 8, 4, 2 }, // The layer of grids. The cells of the finest grid are of dimension - // (source_image_width / 16) * (source_image_height / 16) * (destination_image_width / 16) (destination_image_height / 16), etc. - estimator.sampleSize(), // The size of a minimal sample - { static_cast(w1), // The width of the source image - static_cast(h1), // The height of the source image - static_cast(w2), // The width of the destination image - static_cast(h2) }, // The height of the destination image - 0.5)); // The length (i.e., 0.5 * iterations) of fully blending to global sampling - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 2 (P-NAPSAC sampling)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator); - - if (use_space_partitioning) - { - inlier_selector::SpacePartitioningRANSAC inlier_selector(neighborhood_graph.get()); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication, - inlier_selector::SpacePartitioningRANSAC> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } else - { - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication, - inlier_selector::EmptyInlierSelector> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - } - else - { - // Initializing an empty preemption - preemption::EmptyPreemptiveVerfication preemptive_verification; - - if (use_space_partitioning) - { - inlier_selector::SpacePartitioningRANSAC inlier_selector(neighborhood_graph.get()); - - GCRANSAC, - preemption::EmptyPreemptiveVerfication, - inlier_selector::SpacePartitioningRANSAC> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } else - { - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - GCRANSAC, - preemption::EmptyPreemptiveVerfication, - inlier_selector::EmptyInlierSelector> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - } - - homography.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - homography[i * 3 + j] = model.descriptor(i, j); - } - } - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // Return the number of inliers found - return num_inliers; -} - -// A method for estimating a homography matrix given 2D-2D correspondences -int findHomographyAC_( - // The 2D-2D point correspondences. - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &homography, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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) -{ - int num_tents = correspondences.size() / 8; - cv::Mat points(num_tents, 8, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat empty_point_matrix(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&empty_point_matrix, // The input points - { 0, // The cell size along axis X in the source image - 0, // The cell size along axis Y in the source image - 0, // The cell size along axis X in the destination image - 0 }, // The cell size along axis Y in the destination image - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Using only the point coordinates and not the affine elements when constructing the neighborhood. - cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); - - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, - { w1 / neighborhood_size, - h1 / neighborhood_size, - w2 / neighborhood_size, - h2 / neighborhood_size }, - static_cast(neighborhood_size))); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - utils::ACBasedHomographyEstimator estimator; - Homography model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 3 (NG-RANSAC sampling), 4 (AR-Sampler)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication, - inlier_selector::EmptyInlierSelector> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = neighborhood_size; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - // Initializing an empty preemption - preemption::EmptyPreemptiveVerfication preemptive_verification; - - GCRANSAC, - preemption::EmptyPreemptiveVerfication, - inlier_selector::EmptyInlierSelector> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - - homography.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - homography[i * 3 + j] = model.descriptor(i, j); - } - } - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // Return the number of inliers found - return num_inliers; -} - - -// A method for estimating a homography matrix given 2D-2D correspondences -int findHomographySIFT_( - // The 2D-2D point correspondences. - std::vector& correspondences, - // The probabilities for each 3D-3D point correspondence if available - std::vector &point_probabilities, - // Output: the found inliers - std::vector& inliers, - // Output: the found 6D pose - std::vector &homography, - // The images' sizes - int h1, int w1, int h2, int w2, - // 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. I do not suggest setting it to lower than 50. - 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. - // (2) Progressive NAPSAC sampler. The correspondences should be ordered by quality (e.g., SNN ratio) prior to calling this function. - // (3) Importance sampler from NG-RANSAC. The point probabilities should be provided. - // (4) Adaptive re-ordering sampler from Deep MAGSAC++. The point probabilities should be provided. - int sampler_id, - // The identifier of the used neighborhood structure. - // (0) FLANN-based neighborhood. - // (1) Grid-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 - // If (1) Grid is used, the size is the division number, e.g., 2 if we want to divide the image to 2 in along each axes (2*2 = 4 cells in total) - 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) -{ - int num_tents = correspondences.size() / 8; - cv::Mat points(num_tents, 8, CV_64F, &correspondences[0]); - - typedef neighborhood::NeighborhoodGraph AbstractNeighborhood; - std::unique_ptr neighborhood_graph; - - const size_t cell_number_in_neighborhood_graph_ = - static_cast(neighborhood_size); - - // If the spatial weight is 0.0, the neighborhood graph should not be created - if (spatial_coherence_weight <= std::numeric_limits::epsilon()) - { - cv::Mat empty_point_matrix(0, 4, CV_64F); - - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&empty_point_matrix, // The input points - { 0, // The cell size along axis X in the source image - 0, // The cell size along axis Y in the source image - 0, // The cell size along axis X in the destination image - 0 }, // The cell size along axis Y in the destination image - 1)); // The cell number along every axis - } else // Initializing a grid-based neighborhood graph - { - // Using only the point coordinates and not the affine elements when constructing the neighborhood. - cv::Mat point_coordinates = points(cv::Rect(0, 0, 4, points.rows)); - - // Initializing a grid-based neighborhood graph - if (neighborhood_id == 0) - neighborhood_graph = std::unique_ptr( - new neighborhood::GridNeighborhoodGraph<4>(&point_coordinates, - { w1 / neighborhood_size, - h1 / neighborhood_size, - w2 / neighborhood_size, - h2 / neighborhood_size }, - static_cast(neighborhood_size))); - else if (neighborhood_id == 1) // Initializing the neighbhood graph by FLANN - neighborhood_graph = std::unique_ptr( - new neighborhood::FlannNeighborhoodGraph(&point_coordinates, neighborhood_size)); - else - { - fprintf(stderr, "Unknown neighborhood-graph identifier: %d. The accepted values are 0 (Grid-based), 1 (FLANN-based neighborhood)\n", - neighborhood_id); - return 0; - } - - // Checking if the neighborhood graph is initialized successfully. - if (!neighborhood_graph->isInitialized()) - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "The neighborhood graph is not initialized successfully.\n"); - return 0; - } - } - - utils::SIFTBasedHomographyEstimator estimator; - Homography model; - - // Initialize the samplers - // The main sampler is used for sampling in the main RANSAC loop - typedef sampler::Sampler AbstractSampler; - std::unique_ptr main_sampler; - if (sampler_id == 0) // Initializing a RANSAC-like uniformly random sampler - main_sampler = std::unique_ptr(new sampler::UniformSampler(&points)); - else if (sampler_id == 1) // Initializing a PROSAC sampler. This requires the points to be ordered according to the quality. - main_sampler = std::unique_ptr(new sampler::ProsacSampler(&points, estimator.sampleSize())); - else if (sampler_id == 3) - main_sampler = std::unique_ptr(new gcransac::sampler::ImportanceSampler(&points, - point_probabilities, - estimator.sampleSize())); - else if (sampler_id == 4) - { - double max_prob = 0; - for (const auto &prob : point_probabilities) - max_prob = MAX(max_prob, prob); - for (auto &prob : point_probabilities) - prob /= max_prob; - main_sampler = std::unique_ptr(new gcransac::sampler::AdaptiveReorderingSampler(&points, - point_probabilities, - estimator.sampleSize(), - sampler_variance)); - } - else - { - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "Unknown sampler identifier: %d. The accepted samplers are 0 (uniform sampling), 1 (PROSAC sampling), 3 (NG-RANSAC sampling), 4 (AR-Sampler)\n", - sampler_id); - return 0; - } - - sampler::UniformSampler local_optimization_sampler(&points); // The local optimization sampler is used inside the local optimization - - // Checking if the samplers are initialized successfully. - if (!main_sampler->isInitialized() || - !local_optimization_sampler.isInitialized()) - { - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - fprintf(stderr, "One of the samplers is not initialized successfully.\n"); - return 0; - } - - utils::RANSACStatistics statistics; - inlier_selector::EmptyInlierSelector inlier_selector(neighborhood_graph.get()); - - if (use_sprt) - { - // Initializing SPRT test - preemption::SPRTPreemptiveVerfication preemptive_verification( - points, - estimator); - - GCRANSAC, - preemption::SPRTPreemptiveVerfication, - inlier_selector::EmptyInlierSelector> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = neighborhood_size; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - else - { - // Initializing an empty preemption - preemption::EmptyPreemptiveVerfication preemptive_verification; - - GCRANSAC, - preemption::EmptyPreemptiveVerfication, - inlier_selector::EmptyInlierSelector> gcransac; - gcransac.settings.threshold = threshold; // The inlier-outlier threshold - gcransac.settings.spatial_coherence_weight = spatial_coherence_weight; // The weight of the spatial coherence term - gcransac.settings.confidence = conf; // The required confidence in the results - gcransac.settings.max_local_optimization_number = lo_number; // The maximum number of local optimizations - gcransac.settings.max_iteration_number = max_iters; // The maximum number of iterations - gcransac.settings.min_iteration_number = min_iters; // The minimum number of iterations - gcransac.settings.neighborhood_sphere_radius = cell_number_in_neighborhood_graph_; // The radius of the neighborhood ball - - // Start GC-RANSAC - gcransac.run(points, - estimator, - main_sampler.get(), - &local_optimization_sampler, - neighborhood_graph.get(), - model, - preemptive_verification, - inlier_selector); - - statistics = gcransac.getRansacStatistics(); - } - - homography.resize(9); - - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - homography[i * 3 + j] = model.descriptor(i, j); - } - } - - inliers.resize(num_tents); - - const int num_inliers = statistics.inliers.size(); - for (auto pt_idx = 0; pt_idx < num_tents; ++pt_idx) { - inliers[pt_idx] = 0; - - } - for (auto pt_idx = 0; pt_idx < num_inliers; ++pt_idx) { - inliers[statistics.inliers[pt_idx]] = 1; - } - - // It is ugly: the unique_ptr does not check for virtual descructors in the base class. - // Therefore, the derived class's objects are not deleted automatically. - // This causes a memory leaking. I hate C++. - AbstractSampler *sampler_ptr = main_sampler.release(); - delete sampler_ptr; - - AbstractNeighborhood *neighborhood_graph_ptr = neighborhood_graph.release(); - delete neighborhood_graph_ptr; - - // Return the number of inliers found - return num_inliers; -}