Fix for plane track jittering

Jittering was caused by homography not being estimated
accurate enough.

Before this, only algebraic estimation was used, which
is indeed not so much great, Now use algebraic estimation
followed with refinement step using Ceres minimizer.

The code was already there since keyframe selection patch,
made such estimation a generic function in multiview/ and
changed API for estimation in order to pass all additional
options via an options structure (the same way as it's
done fr Ceres).

This includes changes to both homography and fundamental
estimation.

TODO:
- Need to document Ceres functors better.
- Need to support homogeneous coordinates (currently
  only euclidean coords are supported).

8 files changed, 299 insertions, 164 deletions

diff --git a/extern/libmv/libmv-capi.cc b/extern/libmv/libmv-capi.cc
index a8f396d5df1..396928ba8ba 100644
--- a/extern/libmv/libmv-capi.cc
+++ b/extern/libmv/libmv-capi.cc
@@ -1082,8 +1082,7 @@ void libmv_cameraIntrinsicsInvert(const libmv_CameraIntrinsicsOptions *libmv_cam
 	}
 }
 
-void libmv_homography2DFromCorrespondencesLinear(double (*x1)[2], double (*x2)[2], int num_points,
-                                                 double H[3][3], double expected_precision)
+void libmv_homography2DFromCorrespondencesEuc(double (*x1)[2], double (*x2)[2], int num_points, double H[3][3])
 {
 	libmv::Mat x1_mat, x2_mat;
 	libmv::Mat3 H_mat;
@@ -1099,7 +1098,8 @@ void libmv_homography2DFromCorrespondencesLinear(double (*x1)[2], double (*x2)[2
 	LG << "x1: " << x1_mat;
 	LG << "x2: " << x2_mat;
 
-	libmv::Homography2DFromCorrespondencesLinear(x1_mat, x2_mat, &H_mat, expected_precision);
+	libmv::HomographyEstimationOptions options;
+	libmv::Homography2DFromCorrespondencesEuc(x1_mat, x2_mat, options, &H_mat);
 
 	LG << "H: " << H_mat;
 
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index 37aab2465ed..a872eeb60a1 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -159,8 +159,7 @@ void libmv_cameraIntrinsicsApply(const libmv_CameraIntrinsicsOptions *libmv_came
 void libmv_cameraIntrinsicsInvert(const libmv_CameraIntrinsicsOptions *libmv_camera_intrinsics_options,
                                   double x, double y, double *x1, double *y1);
 
-void libmv_homography2DFromCorrespondencesLinear(double (*x1)[2], double (*x2)[2], int num_points,
-                                                 double H[3][3], double expected_precision);
+void libmv_homography2DFromCorrespondencesEuc(double (*x1)[2], double (*x2)[2], int num_points, double H[3][3]);
 
 #ifdef __cplusplus
 }
diff --git a/extern/libmv/libmv-capi_stub.cc b/extern/libmv/libmv-capi_stub.cc
index 4896f4c6949..e6d3753961b 100644
--- a/extern/libmv/libmv-capi_stub.cc
+++ b/extern/libmv/libmv-capi_stub.cc
@@ -277,8 +277,8 @@ void libmv_cameraIntrinsicsInvert(const libmv_CameraIntrinsicsOptions *libmv_cam
 	*y1 = (y - principal_y) / focal_length;
 }
 
-void libmv_homography2DFromCorrespondencesLinear(double (* /* x1 */)[2], double (* /* x2 */)[2], int /* num_points */,
-                                                 double H[3][3], double /* expected_precision */)
+void libmv_homography2DFromCorrespondencesEuc(double (* /* x1 */)[2], double (* /* x2 */)[2], int /* num_points */,
+                                              double H[3][3])
 {
 	memset(H, 0, sizeof(double[3][3]));
 	H[0][0] = 1.0f;
diff --git a/extern/libmv/libmv/multiview/fundamental.cc b/extern/libmv/libmv/multiview/fundamental.cc
index a05ef7907a2..876e3d07db5 100644
--- a/extern/libmv/libmv/multiview/fundamental.cc
+++ b/extern/libmv/libmv/multiview/fundamental.cc
@@ -20,6 +20,7 @@
 
 #include "libmv/multiview/fundamental.h"
 
+#include "ceres/ceres.h"
 #include "libmv/logging/logging.h"
 #include "libmv/numeric/numeric.h"
 #include "libmv/numeric/poly.h"
@@ -407,4 +408,93 @@ void FundamentalToEssential(const Mat3 &F, Mat3 *E) {
   *E = svd.matrixU() * diag.asDiagonal() * svd.matrixV().transpose();
 }
 
+FundamentalEstimationOptions::FundamentalEstimationOptions(void) :
+    use_refine_if_algebraic_fails(true),
+    max_num_iterations(50),
+    parameter_tolerance(1e-16),
+    function_tolerance(1e-16) {
+}
+
+class FundamentalSymmetricEpipolarCostFunctor {
+ public:
+  FundamentalSymmetricEpipolarCostFunctor(const Vec2 &x,
+                                          const Vec2 &y)
+    : x_(x), y_(y) {}
+
+  template<typename T>
+  bool operator()(const T *fundamental_parameters, T *residuals) const {
+    typedef Eigen::Matrix<T, 3, 3> Mat3;
+    typedef Eigen::Matrix<T, 3, 1> Vec3;
+
+    Mat3 F(fundamental_parameters);
+
+    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
+    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));
+
+    Vec3 F_x = F * x;
+    Vec3 Ft_y = F.transpose() * y;
+    T y_F_x = y.dot(F_x);
+
+    residuals[0] = y_F_x * T(1) / F_x.head(2).norm();
+    residuals[1] = y_F_x * T(1) / Ft_y.head(2).norm();
+
+    return true;
+  }
+
+  const Mat x_;
+  const Mat y_;
+};
+
+/* Fundamental transformation estimation. */
+bool FundamentalFromCorrespondencesEuc(const Mat &x1,
+                                       const Mat &x2,
+                                       const FundamentalEstimationOptions &options,
+                                       Mat3 *F) {
+  // Step 1: Algebraic fundamental estimation.
+  bool algebraic_success = NormalizedEightPointSolver(x1, x2, F);
+
+  LG << "Algebraic result " << algebraic_success << ", estimated matrix " << F;
+
+  if (!algebraic_success && !options.use_refine_if_algebraic_fails) {
+    return false;
+  }
+
+  // Step 2: Refine matrix using Ceres minimizer.
+  ceres::Problem problem;
+  for (int i = 0; i < x1.cols(); i++) {
+    FundamentalSymmetricEpipolarCostFunctor
+        *fundamental_symmetric_epipolar_cost_function =
+            new FundamentalSymmetricEpipolarCostFunctor(x1.col(i),
+                                                        x2.col(i));
+
+    problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<
+            FundamentalSymmetricEpipolarCostFunctor,
+            2, /* num_residuals */
+            9>(fundamental_symmetric_epipolar_cost_function),
+        NULL,
+        F->data());
+  }
+
+  // Configure the solve.
+  ceres::Solver::Options solver_options;
+  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;
+
+  // Run the solve.
+  ceres::Solver::Summary summary;
+  ceres::Solve(solver_options, &problem, &summary);
+
+  VLOG(1) << "Summary:\n" << summary.FullReport();
+
+  LG << "Final refined matrix: " << F;
+
+  return !(summary.termination_type == ceres::DID_NOT_RUN ||
+           summary.termination_type == ceres::NO_CONVERGENCE ||
+           summary.termination_type == ceres::NUMERICAL_FAILURE);
+}
+
 }  // namespace libmv
diff --git a/extern/libmv/libmv/multiview/fundamental.h b/extern/libmv/libmv/multiview/fundamental.h
index 1ad184d8314..45b564c27eb 100644
--- a/extern/libmv/libmv/multiview/fundamental.h
+++ b/extern/libmv/libmv/multiview/fundamental.h
@@ -144,6 +144,42 @@ bool MotionFromEssentialAndCorrespondence(const Mat3 &E,
  */
 void FundamentalToEssential(const Mat3 &F, Mat3 *E);
 
+/**
+ * This structure contains options that controls how the fundamental
+ * estimation operates.
+ *
+ * Defaults should be suitable for a wide range of use cases, but
+ * better performance and accuracy might require tweaking/
+ */
+typedef struct FundamentalEstimationOptions {
+  /* 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;
+
+  /* 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;
+} FundamentalEstimationOptions;
+
+/**
+ * Fundamental transformation estimation.
+ *
+ * This function estimates the fundamental transformation from a list of 2D
+ * correspondences by doing algebraic estimation first followed with result
+ * refinement.
+ */
+bool FundamentalFromCorrespondencesEuc(const Mat &x1,
+                                       const Mat &x2,
+                                       const FundamentalEstimationOptions &options,
+                                       Mat3 *F);
+
 }  // namespace libmv
 
 #endif  // LIBMV_MULTIVIEW_FUNDAMENTAL_H_
diff --git a/extern/libmv/libmv/multiview/homography.cc b/extern/libmv/libmv/multiview/homography.cc
index 317eb3f85c6..ef015f829c8 100644
--- a/extern/libmv/libmv/multiview/homography.cc
+++ b/extern/libmv/libmv/multiview/homography.cc
@@ -20,6 +20,7 @@
 
 #include "libmv/multiview/homography.h"
 
+#include "ceres/ceres.h"
 #include "libmv/logging/logging.h"
 #include "libmv/multiview/homography_parameterization.h"
 
@@ -153,6 +154,113 @@ bool Homography2DFromCorrespondencesLinear(const Mat &x1,
     return false;
   }
 }
+
+HomographyEstimationOptions::HomographyEstimationOptions(void) :
+    expected_algebraic_precision(EigenDouble::dummy_precision()),
+    use_refine_if_algebraic_fails(true),
+    max_num_iterations(50),
+    parameter_tolerance(1e-16),
+    function_tolerance(1e-16) {
+}
+
+class HomographySymmetricGeometricCostFunctor {
+ public:
+  HomographySymmetricGeometricCostFunctor(const Vec2 &x,
+                                          const Vec2 &y)
+      : x_(x), y_(y) { }
+
+  template<typename T>
+  bool operator()(const T *homography_parameters, T *residuals) const {
+    typedef Eigen::Matrix<T, 3, 3> Mat3;
+    typedef Eigen::Matrix<T, 3, 1> Vec3;
+
+    Mat3 H(homography_parameters);
+
+    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
+    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));
+
+    Vec3 H_x = H * x;
+    Vec3 Hinv_y = H.inverse() * y;
+
+    H_x /= H_x(2);
+    Hinv_y /= Hinv_y(2);
+
+    residuals[0] = H_x(0) - T(y_(0));
+    residuals[1] = H_x(1) - T(y_(1));
+
+    residuals[2] = Hinv_y(0) - T(x_(0));
+    residuals[3] = Hinv_y(1) - T(x_(1));
+
+    return true;
+  }
+
+  const Vec2 x_;
+  const Vec2 y_;
+};
+
+/** 2D Homography transformation estimation in the case that points are in
+ * euclidean coordinates.
+ */
+bool Homography2DFromCorrespondencesEuc(const Mat &x1,
+                                        const Mat &x2,
+                                        const HomographyEstimationOptions &options,
+                                        Mat3 *H) {
+  /* TODO(sergey): Support homogenous coordinates, not just euclidean. */
+
+  assert(2 == x1.rows());
+  assert(4 <= x1.cols());
+  assert(x1.rows() == x2.rows());
+  assert(x1.cols() == x2.cols());
+
+  // Step 1: Algebraic homography estimation.
+  bool algebraic_success =
+      Homography2DFromCorrespondencesLinear(x1, x2, H,
+          options.expected_algebraic_precision);
+
+  LG << "Algebraic result " << algebraic_success << ", estimated matrix " << H;
+
+  if (!algebraic_success && !options.use_refine_if_algebraic_fails) {
+    return false;
+  }
+
+  // Step 2: Refine matrix using Ceres minimizer.
+  ceres::Problem problem;
+  for (int i = 0; i < x1.cols(); i++) {
+    HomographySymmetricGeometricCostFunctor
+        *homography_symmetric_geometric_cost_function =
+            new HomographySymmetricGeometricCostFunctor(x1.col(i),
+                                                        x2.col(i));
+
+    problem.AddResidualBlock(
+        new ceres::AutoDiffCostFunction<
+            HomographySymmetricGeometricCostFunctor,
+            4, /* num_residuals */
+            9>(homography_symmetric_geometric_cost_function),
+        NULL,
+        H->data());
+  }
+
+  // Configure the solve.
+  ceres::Solver::Options solver_options;
+  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;
+
+  // Run the solve.
+  ceres::Solver::Summary summary;
+  ceres::Solve(solver_options, &problem, &summary);
+
+  VLOG(1) << "Summary:\n" << summary.FullReport();
+
+  LG << "Final refined matrix: " << H;
+
+  return !(summary.termination_type == ceres::DID_NOT_RUN ||
+           summary.termination_type == ceres::NO_CONVERGENCE ||
+           summary.termination_type == ceres::NUMERICAL_FAILURE);
+}
+
 /**
  * x2 ~ A * x1
  * x2^t * Hi * A *x1 = 0
diff --git a/extern/libmv/libmv/multiview/homography.h b/extern/libmv/libmv/multiview/homography.h
index 8d2dff930eb..abad4e0d963 100644
--- a/extern/libmv/libmv/multiview/homography.h
+++ b/extern/libmv/libmv/multiview/homography.h
@@ -54,6 +54,45 @@ bool Homography2DFromCorrespondencesLinear(const Mat &x1,
                                              EigenDouble::dummy_precision());
 
 /**
+ * This structure contains options that controls how the homography
+ * estimation operates.
+ *
+ * Defaults should be suitable for a wide range of use cases, but
+ * better performance and accuracy might require tweaking/
+ */
+typedef struct HomographyEstimationOptions {
+  /* Default constructor which sets up a options for generic usage. */
+  HomographyEstimationOptions(void);
+
+  /* Expected precision of algebraic estimation. */
+  double expected_algebraic_precision;
+
+  /* Refine homography even if algebraic estimation reported failure. */
+  bool use_refine_if_algebraic_fails;
+
+  /* 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;
+} HomographyEstimationOptions;
+
+/**
+ * 2D homography transformation estimation.
+ *
+ * This function estimates the homography transformation from a list of 2D
+ * correspondences by doing algebraic estimation first followed with result
+ * refinement.
+ */
+bool Homography2DFromCorrespondencesEuc(const Mat &x1,
+                                        const Mat &x2,
+                                        const HomographyEstimationOptions &options,
+                                        Mat3 *H);
+
+/**
  * 3D Homography transformation estimation.
  *
  * This function can be used in order to estimate the homography transformation
diff --git a/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc b/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc
index 976adb288b3..299d48d35fd 100644
--- a/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc
+++ b/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc
@@ -61,161 +61,6 @@ Mat3 IntrinsicsNormalizationMatrix(const CameraIntrinsics &intrinsics) {
   return S * T;
 }
 
-class HomographySymmetricGeometricCostFunctor {
- public:
-  HomographySymmetricGeometricCostFunctor(const Vec2 &x,
-                                          const Vec2 &y)
-      : x_(x), y_(y) { }
-
-  template<typename T>
-  bool operator()(const T *homography_parameters, T *residuals) const {
-    typedef Eigen::Matrix<T, 3, 3> Mat3;
-    typedef Eigen::Matrix<T, 3, 1> Vec3;
-
-    Mat3 H(homography_parameters);
-
-    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
-    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));
-
-    Vec3 H_x = H * x;
-    Vec3 Hinv_y = H.inverse() * y;
-
-    H_x /= H_x(2);
-    Hinv_y /= Hinv_y(2);
-
-    residuals[0] = H_x(0) - T(y_(0));
-    residuals[1] = H_x(1) - T(y_(1));
-
-    residuals[2] = Hinv_y(0) - T(x_(0));
-    residuals[3] = Hinv_y(1) - T(x_(1));
-
-    return true;
-  }
-
-  const Vec2 x_;
-  const Vec2 y_;
-};
-
-void ComputeHomographyFromCorrespondences(const Mat &x1, const Mat &x2,
-                                          CameraIntrinsics &intrinsics,
-                                          Mat3 *H) {
-  // Algebraic homography estimation, happens with normalized coordinates
-  Homography2DFromCorrespondencesLinear(x1, x2, H, 1e-12);
-
-  // Refine matrix using Ceres minimizer
-
-  // TODO(sergey): look into refinement in pixel space.
-  ceres::Problem problem;
-
-  for (int i = 0; i < x1.cols(); i++) {
-    HomographySymmetricGeometricCostFunctor
-        *homography_symmetric_geometric_cost_function =
-            new HomographySymmetricGeometricCostFunctor(x1.col(i),
-                                                        x2.col(i));
-
-    problem.AddResidualBlock(
-        new ceres::AutoDiffCostFunction<
-            HomographySymmetricGeometricCostFunctor,
-            4, /* num_residuals */
-            9>(homography_symmetric_geometric_cost_function),
-        NULL,
-        H->data());
-  }
-
-  // Configure the solve.
-  ceres::Solver::Options solver_options;
-  solver_options.linear_solver_type = ceres::DENSE_QR;
-  solver_options.max_num_iterations = 50;
-  solver_options.update_state_every_iteration = true;
-  solver_options.parameter_tolerance = 1e-16;
-  solver_options.function_tolerance = 1e-16;
-
-  // Run the solve.
-  ceres::Solver::Summary summary;
-  ceres::Solve(solver_options, &problem, &summary);
-
-  VLOG(1) << "Summary:\n" << summary.FullReport();
-
-  // Convert homography to original pixel space
-  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
-  *H = N.inverse() * (*H) * N;
-}
-
-class FundamentalSymmetricEpipolarCostFunctor {
- public:
-  FundamentalSymmetricEpipolarCostFunctor(const Vec2 &x,
-                                          const Vec2 &y)
-    : x_(x), y_(y) {}
-
-  template<typename T>
-  bool operator()(const T *fundamental_parameters, T *residuals) const {
-    typedef Eigen::Matrix<T, 3, 3> Mat3;
-    typedef Eigen::Matrix<T, 3, 1> Vec3;
-
-    Mat3 F(fundamental_parameters);
-
-    Vec3 x(T(x_(0)), T(x_(1)), T(1.0));
-    Vec3 y(T(y_(0)), T(y_(1)), T(1.0));
-
-    Vec3 F_x = F * x;
-    Vec3 Ft_y = F.transpose() * y;
-    T y_F_x = y.dot(F_x);
-
-    residuals[0] = y_F_x * T(1) / F_x.head(2).norm();
-    residuals[1] = y_F_x * T(1) / Ft_y.head(2).norm();
-
-    return true;
-  }
-
-  const Mat x_;
-  const Mat y_;
-};
-
-void ComputeFundamentalFromCorrespondences(const Mat &x1, const Mat &x2,
-                                           CameraIntrinsics &intrinsics,
-                                           Mat3 *F) {
-  // Algebraic fundamental estimation, happens with normalized coordinates
-  NormalizedEightPointSolver(x1, x2, F);
-
-  // Refine matrix using Ceres minimizer
-
-  // TODO(sergey): look into refinement in pixel space.
-  ceres::Problem problem;
-
-  for (int i = 0; i < x1.cols(); i++) {
-    FundamentalSymmetricEpipolarCostFunctor
-        *fundamental_symmetric_epipolar_cost_function =
-            new FundamentalSymmetricEpipolarCostFunctor(x1.col(i),
-                                                        x2.col(i));
-
-    problem.AddResidualBlock(
-        new ceres::AutoDiffCostFunction<
-            FundamentalSymmetricEpipolarCostFunctor,
-            2, /* num_residuals */
-            9>(fundamental_symmetric_epipolar_cost_function),
-        NULL,
-        F->data());
-  }
-
-  // Configure the solve.
-  ceres::Solver::Options solver_options;
-  solver_options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
-  solver_options.max_num_iterations = 50;
-  solver_options.update_state_every_iteration = true;
-  solver_options.parameter_tolerance = 1e-16;
-  solver_options.function_tolerance = 1e-16;
-
-  // Run the solve.
-  ceres::Solver::Summary summary;
-  ceres::Solve(solver_options, &problem, &summary);
-
-  VLOG(1) << "Summary:\n" << summary.FullReport();
-
-  // Convert fundamental to original pixel space
-  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
-  *F = N.inverse() * (*F) * N;
-}
-
 // P.H.S. Torr
 // Geometric Motion Segmentation and Model Selection
 //
@@ -360,8 +205,26 @@ void SelectKeyframesBasedOnGRICAndVariance(const Tracks &tracks,
         continue;
 
       Mat3 H, F;
-      ComputeHomographyFromCorrespondences(x1, x2, intrinsics, &H);
-      ComputeFundamentalFromCorrespondences(x1, x2, intrinsics, &F);
+
+      // Estimate homography using default options.
+      HomographyEstimationOptions homography_estimation_options;
+      Homography2DFromCorrespondencesEuc(x1,
+                                         x2,
+                                         homography_estimation_options,
+                                         &H);
+
+      // Convert homography to original pixel space.
+      Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
+      H = N.inverse() * H * N;
+
+      FundamentalEstimationOptions fundamental_estimation_options;
+      FundamentalFromCorrespondencesEuc(x1,
+                                        x2,
+                                        fundamental_estimation_options,
+                                        &F);
+
+      // Convert fundamental to original pixel space.
+      F = N.inverse() * F * N;
 
       // TODO(sergey): STEP 2: Discard outlier matches