Merged revisions 60248-60717 from trunk/blendersoc-2013-bge

5 files changed, 297 insertions, 157 deletions

diff --git a/extern/libmv/libmv/multiview/fundamental.cc b/extern/libmv/libmv/multiview/fundamental.cc
index a05ef7907a2..ec6c70d81cf 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,94 @@ 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:\n" << *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:\n" << *F;
+
+  return !(summary.termination_type == ceres::DID_NOT_RUN ||
+           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..2961a46cdc4 100644
--- a/extern/libmv/libmv/multiview/fundamental.h
+++ b/extern/libmv/libmv/multiview/fundamental.h
@@ -144,6 +144,43 @@ 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/
+ */
+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;
+};
+
+/**
+ * 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..6041849d9fe 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,116 @@ 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);
+
+    // This is a forward error.
+    residuals[0] = H_x(0) - T(y_(0));
+    residuals[1] = H_x(1) - T(y_(1));
+
+    // This is a backward error.
+    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:\n" << *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:\n" << *H;
+
+  return !(summary.termination_type == ceres::DID_NOT_RUN ||
+           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..1928e39dffb 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/
+ */
+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;
+};
+
+/**
+ * 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..d4ddee56fda 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,25 @@ 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.
+      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