diff --git a/extern/libmv/libmv-capi.cc b/extern/libmv/libmv-capi.cc index f02ca0e98e0..95ac2ff703a 100644 --- a/extern/libmv/libmv-capi.cc +++ b/extern/libmv/libmv-capi.cc @@ -1089,8 +1089,8 @@ void libmv_homography2DFromCorrespondencesEuc(double (*x1)[2], double (*x2)[2], LG << "x1: " << x1_mat; LG << "x2: " << x2_mat; - libmv::HomographyEstimationOptions options; - libmv::Homography2DFromCorrespondencesEuc(x1_mat, x2_mat, options, &H_mat); + libmv::EstimateHomographyOptions options; + libmv::EstimateHomography2DFromCorrespondences(x1_mat, x2_mat, options, &H_mat); LG << "H: " << H_mat; diff --git a/extern/libmv/libmv/multiview/fundamental.cc b/extern/libmv/libmv/multiview/fundamental.cc index ec6c70d81cf..7a1433c3419 100644 --- a/extern/libmv/libmv/multiview/fundamental.cc +++ b/extern/libmv/libmv/multiview/fundamental.cc @@ -408,13 +408,16 @@ void FundamentalToEssential(const Mat3 &F, Mat3 *E) { *E = svd.matrixU() * diag.asDiagonal() * svd.matrixV().transpose(); } -FundamentalEstimationOptions::FundamentalEstimationOptions(void) : - use_refine_if_algebraic_fails(true), +// Default settings for fundamental estimation which should be suitable +// for a wide range of use cases. +EstimateFundamentalOptions::EstimateFundamentalOptions(void) : max_num_iterations(50), - parameter_tolerance(1e-16), - function_tolerance(1e-16) { + expected_average_symmetric_distance(1e-16) { } +namespace { +// Cost functor which computes symmetric epipolar distance +// used for fundamental matrix refinement. class FundamentalSymmetricEpipolarCostFunctor { public: FundamentalSymmetricEpipolarCostFunctor(const Vec2 &x, @@ -445,21 +448,60 @@ class FundamentalSymmetricEpipolarCostFunctor { const Mat y_; }; +// Termination checking callback used for fundamental estimation. +// It finished the minimization as soon as actual average of +// symmetric epipolar distance is less or equal to the expected +// average value. +class TerminationCheckingCallback : public ceres::IterationCallback { + public: + TerminationCheckingCallback(const Mat &x1, const Mat &x2, + const EstimateFundamentalOptions &options, + Mat3 *F) + : options_(options), x1_(x1), x2_(x2), F_(F) {} + + virtual ceres::CallbackReturnType operator()( + const ceres::IterationSummary& summary) { + // If the step wasn't successful, there's nothing to do. + if (!summary.step_is_successful) { + return ceres::SOLVER_CONTINUE; + } + + // Calculate average of symmetric epipolar distance. + double average_distance = 0.0; + for (int i = 0; i < x1_.cols(); i++) { + average_distance = SymmetricEpipolarDistance(*F_, + x1_.col(i), + x2_.col(i)); + } + average_distance /= x1_.cols(); + + if (average_distance <= options_.expected_average_symmetric_distance) { + return ceres::SOLVER_TERMINATE_SUCCESSFULLY; + } + + return ceres::SOLVER_CONTINUE; + } + + private: + const EstimateFundamentalOptions &options_; + const Mat &x1_; + const Mat &x2_; + Mat3 *F_; +}; +} // namespace + /* Fundamental transformation estimation. */ -bool FundamentalFromCorrespondencesEuc( +bool EstimateFundamentalFromCorrespondences( const Mat &x1, const Mat &x2, - const FundamentalEstimationOptions &options, + const EstimateFundamentalOptions &options, Mat3 *F) { // Step 1: Algebraic fundamental estimation. - bool algebraic_success = NormalizedEightPointSolver(x1, x2, F); - LG << "Algebraic result " << algebraic_success - << ", estimated matrix:\n" << *F; + // Assume algebraic estiation always succeeds, + NormalizedEightPointSolver(x1, x2, F); - if (!algebraic_success && !options.use_refine_if_algebraic_fails) { - return false; - } + LG << "Estimated matrix after algebraic estimation:\n" << *F; // Step 2: Refine matrix using Ceres minimizer. ceres::Problem problem; @@ -483,8 +525,10 @@ bool FundamentalFromCorrespondencesEuc( solver_options.linear_solver_type = ceres::DENSE_QR; solver_options.max_num_iterations = options.max_num_iterations; solver_options.update_state_every_iteration = true; - solver_options.parameter_tolerance = options.parameter_tolerance; - solver_options.function_tolerance = options.function_tolerance; + + // Terminate if the average symmetric distance is good enough. + TerminationCheckingCallback callback(x1, x2, options, F); + solver_options.callbacks.push_back(&callback); // Run the solve. ceres::Solver::Summary summary; @@ -495,7 +539,8 @@ bool FundamentalFromCorrespondencesEuc( LG << "Final refined matrix:\n" << *F; return !(summary.termination_type == ceres::DID_NOT_RUN || - summary.termination_type == ceres::NUMERICAL_FAILURE); + summary.termination_type == ceres::NUMERICAL_FAILURE || + summary.termination_type == ceres::USER_ABORT); } } // namespace libmv diff --git a/extern/libmv/libmv/multiview/fundamental.h b/extern/libmv/libmv/multiview/fundamental.h index 2961a46cdc4..51067aefc2b 100644 --- a/extern/libmv/libmv/multiview/fundamental.h +++ b/extern/libmv/libmv/multiview/fundamental.h @@ -151,21 +151,22 @@ void FundamentalToEssential(const Mat3 &F, Mat3 *E); * Defaults should be suitable for a wide range of use cases, but * better performance and accuracy might require tweaking/ */ -struct FundamentalEstimationOptions { +struct EstimateFundamentalOptions { // Default constructor which sets up a options for generic usage. - FundamentalEstimationOptions(void); - - // Refine fundamental matrix even if algebraic estimation reported failure. - bool use_refine_if_algebraic_fails; + EstimateFundamentalOptions(void); // Maximal number of iterations for refinement step. int max_num_iterations; - // Paramaneter tolerance used by minimizer termination criteria. - float parameter_tolerance; - - // Function tolerance used by minimizer termination criteria. - float function_tolerance; + // Expected average of symmetric epipolar distance between + // actual destination points and original ones transformed by + // estimated fundamental matrix. + // + // Refinement will finish as soon as average of symmetric + // epipolar distance is less or equal to this value. + // + // This distance is measured in the same units as input points are. + double expected_average_symmetric_distance; }; /** @@ -175,10 +176,10 @@ struct FundamentalEstimationOptions { * correspondences by doing algebraic estimation first followed with result * refinement. */ -bool FundamentalFromCorrespondencesEuc( +bool EstimateFundamentalFromCorrespondences( const Mat &x1, const Mat &x2, - const FundamentalEstimationOptions &options, + const EstimateFundamentalOptions &options, Mat3 *F); } // namespace libmv diff --git a/extern/libmv/libmv/multiview/homography.cc b/extern/libmv/libmv/multiview/homography.cc index 6041849d9fe..cbd3ec78a57 100644 --- a/extern/libmv/libmv/multiview/homography.cc +++ b/extern/libmv/libmv/multiview/homography.cc @@ -22,6 +22,7 @@ #include "ceres/ceres.h" #include "libmv/logging/logging.h" +#include "libmv/multiview/conditioning.h" #include "libmv/multiview/homography_parameterization.h" namespace libmv { @@ -155,14 +156,26 @@ bool Homography2DFromCorrespondencesLinear(const Mat &x1, } } -HomographyEstimationOptions::HomographyEstimationOptions(void) : - expected_algebraic_precision(EigenDouble::dummy_precision()), - use_refine_if_algebraic_fails(true), +// Default settings for homography estimation which should be suitable +// for a wide range of use cases. +EstimateHomographyOptions::EstimateHomographyOptions(void) : + use_normalization(true), max_num_iterations(50), - parameter_tolerance(1e-16), - function_tolerance(1e-16) { + expected_average_symmetric_distance(1e-16) { } +namespace { +void GetNormalizedPoints(const Mat &original_points, + Mat *normalized_points, + Mat3 *normalization_matrix) { + IsotropicPreconditionerFromPoints(original_points, normalization_matrix); + ApplyTransformationToPoints(original_points, + *normalization_matrix, + normalized_points); +} + +// Cost functor which computes symmetric geometric distance +// used for homography matrix refinement. class HomographySymmetricGeometricCostFunctor { public: HomographySymmetricGeometricCostFunctor(const Vec2 &x, @@ -200,13 +213,55 @@ class HomographySymmetricGeometricCostFunctor { const Vec2 y_; }; +// Termination checking callback used for homography estimation. +// It finished the minimization as soon as actual average of +// symmetric geometric distance is less or equal to the expected +// average value. +class TerminationCheckingCallback : public ceres::IterationCallback { + public: + TerminationCheckingCallback(const Mat &x1, const Mat &x2, + const EstimateHomographyOptions &options, + Mat3 *H) + : options_(options), x1_(x1), x2_(x2), H_(H) {} + + virtual ceres::CallbackReturnType operator()( + const ceres::IterationSummary& summary) { + // If the step wasn't successful, there's nothing to do. + if (!summary.step_is_successful) { + return ceres::SOLVER_CONTINUE; + } + + // Calculate average of symmetric geometric distance. + double average_distance = 0.0; + for (int i = 0; i < x1_.cols(); i++) { + average_distance = SymmetricGeometricDistance(*H_, + x1_.col(i), + x2_.col(i)); + } + average_distance /= x1_.cols(); + + if (average_distance <= options_.expected_average_symmetric_distance) { + return ceres::SOLVER_TERMINATE_SUCCESSFULLY; + } + + return ceres::SOLVER_CONTINUE; + } + + private: + const EstimateHomographyOptions &options_; + const Mat &x1_; + const Mat &x2_; + Mat3 *H_; +}; +} // namespace + /** 2D Homography transformation estimation in the case that points are in * euclidean coordinates. */ -bool Homography2DFromCorrespondencesEuc( +bool EstimateHomography2DFromCorrespondences( const Mat &x1, const Mat &x2, - const HomographyEstimationOptions &options, + const EstimateHomographyOptions &options, Mat3 *H) { // TODO(sergey): Support homogenous coordinates, not just euclidean. @@ -215,18 +270,31 @@ bool Homography2DFromCorrespondencesEuc( assert(x1.rows() == x2.rows()); assert(x1.cols() == x2.cols()); + Mat3 T1 = Mat3::Identity(), + T2 = Mat3::Identity(); + // Step 1: Algebraic homography estimation. - bool algebraic_success = - Homography2DFromCorrespondencesLinear(x1, x2, H, - options.expected_algebraic_precision); + Mat x1_normalized, x2_normalized; - LG << "Algebraic result " << algebraic_success - << ", estimated matrix:\n" << *H; - - if (!algebraic_success && !options.use_refine_if_algebraic_fails) { - return false; + if (options.use_normalization) { + LG << "Estimating homography using normalization."; + GetNormalizedPoints(x1, &x1_normalized, &T1); + GetNormalizedPoints(x2, &x2_normalized, &T2); + } else { + x1_normalized = x1; + x2_normalized = x2; } + // Assume algebraic estiation always suceeds, + Homography2DFromCorrespondencesLinear(x1_normalized, x2_normalized, H); + + // Denormalize the homography matrix. + if (options.use_normalization) { + *H = T2.inverse() * (*H) * T1; + } + + LG << "Estimated matrix after algebraic estimation:\n" << *H; + // Step 2: Refine matrix using Ceres minimizer. ceres::Problem problem; for (int i = 0; i < x1.cols(); i++) { @@ -249,8 +317,10 @@ bool Homography2DFromCorrespondencesEuc( solver_options.linear_solver_type = ceres::DENSE_QR; solver_options.max_num_iterations = options.max_num_iterations; solver_options.update_state_every_iteration = true; - solver_options.parameter_tolerance = options.parameter_tolerance; - solver_options.function_tolerance = options.function_tolerance; + + // Terminate if the average symmetric distance is good enough. + TerminationCheckingCallback callback(x1, x2, options, H); + solver_options.callbacks.push_back(&callback); // Run the solve. ceres::Solver::Summary summary; @@ -261,7 +331,8 @@ bool Homography2DFromCorrespondencesEuc( LG << "Final refined matrix:\n" << *H; return !(summary.termination_type == ceres::DID_NOT_RUN || - summary.termination_type == ceres::NUMERICAL_FAILURE); + summary.termination_type == ceres::NUMERICAL_FAILURE || + summary.termination_type == ceres::USER_ABORT); } /** @@ -377,7 +448,9 @@ bool Homography3DFromCorrespondencesLinear(const Mat &x1, } } -double SymmetricGeometricDistance(Mat3 &H, Vec2 &x1, Vec2 &x2) { +double SymmetricGeometricDistance(const Mat3 &H, + const Vec2 &x1, + const Vec2 &x2) { Vec3 x(x1(0), x1(1), 1.0); Vec3 y(x2(0), x2(1), 1.0); diff --git a/extern/libmv/libmv/multiview/homography.h b/extern/libmv/libmv/multiview/homography.h index 1928e39dffb..6d810c845ed 100644 --- a/extern/libmv/libmv/multiview/homography.h +++ b/extern/libmv/libmv/multiview/homography.h @@ -60,24 +60,35 @@ bool Homography2DFromCorrespondencesLinear(const Mat &x1, * Defaults should be suitable for a wide range of use cases, but * better performance and accuracy might require tweaking/ */ -struct HomographyEstimationOptions { +struct EstimateHomographyOptions { // Default constructor which sets up a options for generic usage. - HomographyEstimationOptions(void); + EstimateHomographyOptions(void); - // Expected precision of algebraic estimation. - double expected_algebraic_precision; + // Normalize correspondencies before estimating the homography + // in order to increase estimation stability. + // + // Normaliztion will make it so centroid od correspondences + // is the coordinate origin and their average distance from + // the origin is sqrt(2). + // + // See: + // - R. Hartley and A. Zisserman. Multiple View Geometry in Computer + // Vision. Cambridge University Press, second edition, 2003. + // - https://www.cs.ubc.ca/grads/resources/thesis/May09/Dubrofsky_Elan.pdf + bool use_normalization; - // Refine homography even if algebraic estimation reported failure. - bool use_refine_if_algebraic_fails; - - // Maximal number of iterations for refinement step. + // Maximal number of iterations for the refinement step. int max_num_iterations; - // Paramaneter tolerance used by minimizer termination criteria. - float parameter_tolerance; - - // Function tolerance used by minimizer termination criteria. - float function_tolerance; + // Expected average of symmetric geometric distance between + // actual destination points and original ones transformed by + // estimated homography matrix. + // + // Refinement will finish as soon as average of symmetric + // geometric distance is less or equal to this value. + // + // This distance is measured in the same units as input points are. + double expected_average_symmetric_distance; }; /** @@ -87,10 +98,11 @@ struct HomographyEstimationOptions { * correspondences by doing algebraic estimation first followed with result * refinement. */ -bool Homography2DFromCorrespondencesEuc(const Mat &x1, - const Mat &x2, - const HomographyEstimationOptions &options, - Mat3 *H); +bool EstimateHomography2DFromCorrespondences( + const Mat &x1, + const Mat &x2, + const EstimateHomographyOptions &options, + Mat3 *H); /** * 3D Homography transformation estimation. @@ -124,7 +136,9 @@ bool Homography3DFromCorrespondencesLinear(const Mat &x1, * * D(H * x1, x2)^2 + D(H^-1 * x2, x1) */ -double SymmetricGeometricDistance(Mat3 &H, Vec2 &x1, Vec2 &x2); +double SymmetricGeometricDistance(const Mat3 &H, + const Vec2 &x1, + const Vec2 &x2); } // namespace libmv diff --git a/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc b/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc index d4ddee56fda..6ab9e7eefb6 100644 --- a/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc +++ b/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc @@ -207,19 +207,19 @@ void SelectKeyframesBasedOnGRICAndVariance(const Tracks &tracks, Mat3 H, F; // Estimate homography using default options. - HomographyEstimationOptions homography_estimation_options; - Homography2DFromCorrespondencesEuc(x1, - x2, - homography_estimation_options, - &H); + EstimateHomographyOptions estimate_homography_options; + EstimateHomography2DFromCorrespondences(x1, + x2, + estimate_homography_options, + &H); // Convert homography to original pixel space. H = N_inverse * H * N; - FundamentalEstimationOptions fundamental_estimation_options; - FundamentalFromCorrespondencesEuc(x1, + EstimateFundamentalOptions estimate_fundamental_options; + EstimateFundamentalFromCorrespondences(x1, x2, - fundamental_estimation_options, + estimate_fundamental_options, &F); // Convert fundamental to original pixel space.