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).

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;
 

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
 }

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;

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

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_

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

extern/libmv/libmv/multiview/homography.h

@@ -53,6 +53,45 @@ bool Homography2DFromCorrespondencesLinear(const Mat &x1,
                                            double expected_precision =
                                              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.
  *

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
 

source/blender/blenkernel/intern/tracking.c

@@ -3340,7 +3340,7 @@ static void track_plane_from_existing_motion(MovieTrackingPlaneTrack *plane_trac
 			break;
 		}
 
-		libmv_homography2DFromCorrespondencesLinear(x1, x2, num_correspondences, H_double, 1e-8);
+		libmv_homography2DFromCorrespondencesEuc(x1, x2, num_correspondences, H_double);
 
 		mat3f_from_mat3d(H, H_double);
 
@@ -3444,7 +3444,7 @@ void BKE_tracking_homography_between_two_quads(/*const*/ float reference_corners
 	float_corners_to_double(reference_corners, x1);
 	float_corners_to_double(corners, x2);
 
-	libmv_homography2DFromCorrespondencesLinear(x1, x2, 4, H_double, 1e-8);
+	libmv_homography2DFromCorrespondencesEuc(x1, x2, 4, H_double);
 
 	mat3f_from_mat3d(H, H_double);
 }