Tweaks and improvements to fundamental/homography estimation

- A bit more reasonable name for the estimation option
  structure and estimation functions.

- Get rid of unclear function and parameter tolerance,
  it wasn't clear at all in which units they are.

  Now we've got expected_average_symmetric_distance as
  an early output check and as soon as average symmetric
  error goes below this threshold refining finishes.

  This distance is measured in the same units as input
  points are.

  It is arguable whether we need callback for this or not,
  but seems Ceres doesn't have some kind of absolute
  threshold for function value and function_tolerance
  behaves different from logic behind expected symmetric
  error.

- Added option to normalize correspondences before
  estimating the homography in order to increase estimation
  stability. 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

6 files changed, 206 insertions, 73 deletions

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 (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;
+  }
 
-  if (!algebraic_success && !options.use_refine_if_algebraic_fails) {
-    return false;
+  // 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.