Motion tracking: automatic keyframe selection

Implements an automatic keyframe selection algorithm which uses
couple of approaches to find out best keyframes candidates:

- First, slightly modifier Pollefeys's criteria is used, which
  limits correspondence ration from 80% to 100%. This allows to
  reject keyframe candidate early without doing heavy math in
  cases there're not much common features with first keyframe.

- Second step is based on Geometric Robust Information Criteria
  (aka GRIC), which checks whether features motion between
  candidate keyframes is better defined by homography or
  fundamental matrices.

  To be a good keyframe candidate, fundamental matrix need to
  define motion better than homography (in this case F-GRIC will
  be smaller than H-GRIC).

  This two criteria are well described in this paper:
  http://www.cs.ait.ac.th/~mdailey/papers/Tahir-KeyFrame.pdf

- Final step is based on estimating reconstruction error of
  a full-scene solution using candidate keyframes. This part
  is based on the following paper:

  ftp://ftp.tnt.uni-hannover.de/pub/papers/2004/ECCV2004-TTHBAW.pdf

  This step requires reconstruction using candidate keyframes
  and obtaining covariance matrix of 3D points positions.
  Reconstruction was done pretty much straightforward using
  other simple pipeline routines, and for covariance estimation
  pseudo-inverse of Hessian is used, which is in this case
  (J^T * J)+, where + denotes pseudo-inverse.

  Jacobian matrix is estimating using Ceres evaluate API.

  This is also crucial to get rid of possible gauge ambiguity,
  which is in our case made by zero-ing 7 (by gauge freedoms
  number) eigen values in pseudo-inverse.

  There're still room for improving and optimizing the code,
  but we need some point to start with anyway :)

  Thanks to Keir Mierle and Sameer Agarwal who assisted a lot
  to make this feature working.

6 files changed, 736 insertions, 6 deletions

diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt
index a18011fc599..4c88a4b7cc2 100644
--- a/extern/libmv/CMakeLists.txt
+++ b/extern/libmv/CMakeLists.txt
@@ -67,6 +67,7 @@ if(WITH_LIBMV)
 		libmv/simple_pipeline/detect.cc
 		libmv/simple_pipeline/initialize_reconstruction.cc
 		libmv/simple_pipeline/intersect.cc
+		libmv/simple_pipeline/keyframe_selection.cc
 		libmv/simple_pipeline/modal_solver.cc
 		libmv/simple_pipeline/pipeline.cc
 		libmv/simple_pipeline/reconstruction.cc
@@ -125,6 +126,7 @@ if(WITH_LIBMV)
 		libmv/simple_pipeline/detect.h
 		libmv/simple_pipeline/initialize_reconstruction.h
 		libmv/simple_pipeline/intersect.h
+		libmv/simple_pipeline/keyframe_selection.h
 		libmv/simple_pipeline/modal_solver.h
 		libmv/simple_pipeline/pipeline.h
 		libmv/simple_pipeline/reconstruction.h
diff --git a/extern/libmv/files.txt b/extern/libmv/files.txt
index 426b6db41fc..bfa22bfe898 100644
--- a/extern/libmv/files.txt
+++ b/extern/libmv/files.txt
@@ -46,6 +46,8 @@ libmv/simple_pipeline/initialize_reconstruction.cc
 libmv/simple_pipeline/initialize_reconstruction.h
 libmv/simple_pipeline/intersect.cc
 libmv/simple_pipeline/intersect.h
+libmv/simple_pipeline/keyframe_selection.cc
+libmv/simple_pipeline/keyframe_selection.h
 libmv/simple_pipeline/modal_solver.cc
 libmv/simple_pipeline/modal_solver.h
 libmv/simple_pipeline/pipeline.cc
diff --git a/extern/libmv/libmv-capi.cc b/extern/libmv/libmv-capi.cc
index 0e25650db79..563919e1d7b 100644
--- a/extern/libmv/libmv-capi.cc
+++ b/extern/libmv/libmv-capi.cc
@@ -56,6 +56,7 @@
 #include "libmv/simple_pipeline/camera_intrinsics.h"
 #include "libmv/simple_pipeline/modal_solver.h"
 #include "libmv/simple_pipeline/reconstruction_scale.h"
+#include "libmv/simple_pipeline/keyframe_selection.h"
 
 #ifdef _MSC_VER
 #  define snprintf _snprintf
@@ -497,9 +498,82 @@ static void finishReconstruction(const libmv::Tracks &tracks, const libmv::Camer
 	libmv_reconstruction->error = libmv::EuclideanReprojectionError(tracks, reconstruction, camera_intrinsics);
 }
 
+static bool selectTwoKeyframesBasedOnGRICAndVariance(
+                          libmv::Tracks &tracks,
+                          libmv::Tracks &normalized_tracks,
+                          libmv::CameraIntrinsics &camera_intrinsics,
+                          libmv::ReconstructionOptions &reconstruction_options,
+                          int &keyframe1,
+                          int &keyframe2)
+{
+	libmv::vector<int> keyframes;
+
+	/* Get list of all keyframe candidates first. */
+	SelectkeyframesBasedOnGRICAndVariance(normalized_tracks,
+	                                      camera_intrinsics,
+	                                      keyframes);
+
+	if (keyframes.size() < 2) {
+		LG << "Not enough keyframes detected by GRIC";
+		return false;
+	}
+	else if (keyframes.size() == 2) {
+		keyframe1 = keyframes[0];
+		keyframe2 = keyframes[1];
+		return true;
+	}
+
+	/* Now choose two keyframes with minimal reprojection error after initial
+	 * reconstruction choose keyframes with the least reprojection error after
+	 * solving from two candidate keyframes.
+	 *
+	 * In fact, currently libmv returns single pair only, so this code will
+	 * not actually run. But in the future this could change, so let's stay
+	 * prepared.
+	 */
+	int previous_keyframe = keyframes[0];
+	double best_error = std::numeric_limits<double>::max();
+	for (int i = 1; i < keyframes.size(); i++) {
+		libmv::EuclideanReconstruction reconstruction;
+		int current_keyframe = keyframes[i];
+
+		libmv::vector<libmv::Marker> keyframe_markers =
+			normalized_tracks.MarkersForTracksInBothImages(previous_keyframe,
+			                                               current_keyframe);
+
+		libmv::Tracks keyframe_tracks(keyframe_markers);
+
+		/* get a solution from two keyframes only */
+		libmv::EuclideanReconstructTwoFrames(keyframe_markers, &reconstruction);
+		libmv::EuclideanBundle(keyframe_tracks, &reconstruction);
+		libmv::EuclideanCompleteReconstruction(reconstruction_options,
+		                                       keyframe_tracks,
+		                                       &reconstruction, NULL);
+
+		double current_error =
+			libmv::EuclideanReprojectionError(tracks,
+			                                  reconstruction,
+			                                  camera_intrinsics);
+
+		LG << "Error between " << previous_keyframe
+		   << " and " << current_keyframe
+		   << ": " << current_error;
+
+		if (current_error < best_error) {
+			best_error = current_error;
+			keyframe1 = previous_keyframe;
+			keyframe2 = current_keyframe;
+		}
+
+		previous_keyframe = current_keyframe;
+	}
+
+	return true;
+}
+
 libmv_Reconstruction *libmv_solveReconstruction(const libmv_Tracks *libmv_tracks,
 			const libmv_cameraIntrinsicsOptions *libmv_camera_intrinsics_options,
-			const libmv_reconstructionOptions *libmv_reconstruction_options,
+			libmv_reconstructionOptions *libmv_reconstruction_options,
 			reconstruct_progress_update_cb progress_update_callback,
 			void *callback_customdata)
 {
@@ -512,19 +586,38 @@ libmv_Reconstruction *libmv_solveReconstruction(const libmv_Tracks *libmv_tracks
 	ReconstructUpdateCallback update_callback =
 		ReconstructUpdateCallback(progress_update_callback, callback_customdata);
 
+	/* Retrieve reconstruction options from C-API to libmv API */
 	cameraIntrinsicsFromOptions(libmv_camera_intrinsics_options, &camera_intrinsics);
 
-	/* Invert the camera intrinsics */
-	libmv::Tracks normalized_tracks = getNormalizedTracks(tracks, camera_intrinsics);
-
-	/* actual reconstruction */
 	libmv::ReconstructionOptions reconstruction_options;
 	reconstruction_options.success_threshold = libmv_reconstruction_options->success_threshold;
 	reconstruction_options.use_fallback_reconstruction = libmv_reconstruction_options->use_fallback_reconstruction;
 
+	/* Invert the camera intrinsics */
+	libmv::Tracks normalized_tracks = getNormalizedTracks(tracks, camera_intrinsics);
+
+	/* keyframe selection */
 	int keyframe1 = libmv_reconstruction_options->keyframe1,
 	    keyframe2 = libmv_reconstruction_options->keyframe2;
 
+	if (libmv_reconstruction_options->select_keyframes) {
+		LG << "Using automatic keyframe selection";
+
+		update_callback.invoke(0, "Selecting keyframes");
+
+		selectTwoKeyframesBasedOnGRICAndVariance(tracks,
+		                                         normalized_tracks,
+		                                         camera_intrinsics,
+		                                         reconstruction_options,
+		                                         keyframe1,
+		                                         keyframe2);
+
+		/* so keyframes in the interface would be updated */
+		libmv_reconstruction_options->keyframe1 = keyframe1;
+		libmv_reconstruction_options->keyframe2 = keyframe2;
+	}
+
+	/* actual reconstruction */
 	LG << "frames to init from: " << keyframe1 << " " << keyframe2;
 
 	libmv::vector<libmv::Marker> keyframe_markers =
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index 5fab666e5a8..beac3e85468 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -92,7 +92,9 @@ typedef struct libmv_cameraIntrinsicsOptions {
 } libmv_cameraIntrinsicsOptions;
 
 typedef struct libmv_reconstructionOptions {
+	int select_keyframes;
 	int keyframe1, keyframe2;
+
 	int refine_intrinsics;
 
 	double success_threshold;
@@ -103,7 +105,7 @@ typedef void (*reconstruct_progress_update_cb) (void *customdata, double progres
 
 struct libmv_Reconstruction *libmv_solveReconstruction(const struct libmv_Tracks *libmv_tracks,
 			const libmv_cameraIntrinsicsOptions *libmv_camera_intrinsics_options,
-			const libmv_reconstructionOptions *libmv_reconstruction_options,
+			libmv_reconstructionOptions *libmv_reconstruction_options,
 			reconstruct_progress_update_cb progress_update_callback,
 			void *callback_customdata);
 struct libmv_Reconstruction *libmv_solveModal(const struct libmv_Tracks *libmv_tracks,
diff --git a/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc b/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc
new file mode 100644
index 00000000000..256b056015a
--- /dev/null
+++ b/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc
@@ -0,0 +1,579 @@
+// Copyright (c) 2012 libmv authors.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#include "libmv/simple_pipeline/keyframe_selection.h"
+
+#include "libmv/numeric/numeric.h"
+#include "ceres/ceres.h"
+#include "libmv/logging/logging.h"
+#include "libmv/multiview/homography.h"
+#include "libmv/multiview/fundamental.h"
+#include "libmv/simple_pipeline/intersect.h"
+#include "libmv/simple_pipeline/bundle.h"
+
+#include <Eigen/Eigenvalues>
+
+namespace libmv {
+namespace {
+
+Vec2 NorrmalizedToPixelSpace(Vec2 vec, const CameraIntrinsics &intrinsics) {
+  Vec2 result;
+
+  double focal_length_x = intrinsics.focal_length_x();
+  double focal_length_y = intrinsics.focal_length_y();
+
+  double principal_point_x = intrinsics.principal_point_x();
+  double principal_point_y = intrinsics.principal_point_y();
+
+  result(0) = vec(0) * focal_length_x + principal_point_x;
+  result(1) = vec(1) * focal_length_y + principal_point_y;
+
+  return result;
+}
+
+Mat3 IntrinsicsNormalizationMatrix(const CameraIntrinsics &intrinsics) {
+  Mat3 T = Mat3::Identity(), S = Mat3::Identity();
+
+  T(0, 2) = -intrinsics.principal_point_x();
+  T(1, 2) = -intrinsics.principal_point_y();
+
+  S(0, 0) /= intrinsics.focal_length_x();
+  S(1, 1) /= intrinsics.focal_length_y();
+
+  return S * T;
+}
+
+class HomographySymmetricGeometricCostFunctor {
+ public:
+  HomographySymmetricGeometricCostFunctor(Vec2 x, 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(Vec2 x, 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
+//
+// http://reference.kfupm.edu.sa/content/g/e/geometric_motion_segmentation_and_model__126445.pdf
+//
+// d is the number of dimensions modeled
+//     (d = 3 for a fundamental matrix or 2 for a homography)
+// k is the number of degrees of freedom in the model
+//     (k = 7 for a fundamental matrix or 8 for a homography)
+// r is the dimension of the data
+//     (r = 4 for 2D correspondences between two frames)
+double GRIC(const Vec &e, int d, int k, int r) {
+  int n = e.rows();
+  double lambda1 = log(static_cast<double>(r));
+  double lambda2 = log(static_cast<double>(r * n));
+
+  // lambda3 limits the residual error, and this paper
+  // http://elvera.nue.tu-berlin.de/files/0990Knorr2006.pdf
+  // suggests using lambda3 of 2
+  // same value is used in Torr's Problem of degeneracy in structure
+  // and motion recovery from uncalibrated image sequences
+  // http://www.robots.ox.ac.uk/~vgg/publications/papers/torr99.ps.gz
+  double lambda3 = 2.0;
+
+  // measurement error of tracker
+  double sigma2 = 0.01;
+
+  // Actual GRIC computation
+  double gric_result = 0.0;
+
+  for (int i = 0; i < n; i++) {
+    double rho = std::min(e(i) * e(i) / sigma2, lambda3 * (r - d));
+    gric_result += rho;
+  }
+
+  gric_result += lambda1 * d * n;
+  gric_result += lambda2 * k;
+
+  return gric_result;
+}
+
+// Compute a generalized inverse using eigen value decomposition.
+// It'll actually also zero 7 last eigen values to deal with
+// gauges, since this function is used to compute variance of
+// reconstructed 3D points.
+//
+// TODO(sergey): Could be generalized by making it so number
+//               of values to be zeroed is passed by an argument
+//               and moved to numeric module.
+Mat pseudoInverse(const Mat &matrix) {
+  Eigen::EigenSolver<Mat> eigenSolver(matrix);
+  Mat D = eigenSolver.pseudoEigenvalueMatrix();
+  Mat V = eigenSolver.pseudoEigenvectors();
+
+  double epsilon = std::numeric_limits<double>::epsilon();
+
+  for (int i = 0; i < D.cols(); ++i) {
+    if (D(i, i) > epsilon)
+      D(i, i) = 1.0 / D(i, i);
+    else
+      D(i, i) = 0.0;
+  }
+
+  // Zero last 7 (which corresponds to smallest eigen values).
+  // 7 equals to the number of gauge freedoms.
+  for (int i = D.cols() - 7; i < D.cols(); ++i)
+    D(i, i) = 0.0;
+
+  return V * D * V.inverse();
+}
+}  // namespace
+
+void SelectkeyframesBasedOnGRICAndVariance(const Tracks &tracks,
+                                           CameraIntrinsics &intrinsics,
+                                           vector<int> &keyframes) {
+  // Mirza Tahir Ahmed, Matthew N. Dailey
+  // Robust key frame extraction for 3D reconstruction from video streams
+  //
+  // http://www.cs.ait.ac.th/~mdailey/papers/Tahir-KeyFrame.pdf
+
+  int max_image = tracks.MaxImage();
+  int next_keyframe = 1;
+  int number_keyframes = 0;
+
+  // Limit correspondence ratio from both sides.
+  // On the one hand if number of correspondent features is too low,
+  // triangulation will suffer.
+  // On the other hand high correspondence likely means short baseline.
+  // which also will affect om accuracy
+  const double Tmin = 0.8;
+  const double Tmax = 1.0;
+
+  Mat3 N = IntrinsicsNormalizationMatrix(intrinsics);
+  Mat3 N_inverse = N.inverse();
+
+  double Sc_best = std::numeric_limits<double>::max();
+  double success_intersects_factor_best = 0.0f;
+
+  while (next_keyframe != -1) {
+    int current_keyframe = next_keyframe;
+    double Sc_best_candidate = std::numeric_limits<double>::max();
+
+    LG << "Found keyframe " << next_keyframe;
+
+    number_keyframes++;
+    next_keyframe = -1;
+
+    for (int candidate_image = current_keyframe + 1;
+         candidate_image <= max_image;
+         candidate_image++) {
+      // Conjunction of all markers from both keyframes
+      vector<Marker> all_markers =
+        tracks.MarkersInBothImages(current_keyframe, candidate_image);
+
+      // Match keypoints between frames current_keyframe and candidate_image
+      vector<Marker> tracked_markers =
+        tracks.MarkersForTracksInBothImages(current_keyframe, candidate_image);
+
+      // Correspondences in normalized space
+      Mat x1, x2;
+      CoordinatesForMarkersInImage(tracked_markers, current_keyframe, &x1);
+      CoordinatesForMarkersInImage(tracked_markers, candidate_image, &x2);
+
+      LG << "Found " << x1.cols()
+         << " correspondences between " << current_keyframe
+         << " and " << candidate_image;
+
+      // Not enough points to construct fundamental matrix
+      if (x1.cols() < 8 || x2.cols() < 8)
+        continue;
+
+      // STEP 1: Correspondence ratio constraint
+      int Tc = tracked_markers.size();
+      int Tf = all_markers.size();
+      double Rc = static_cast<double>(Tc) / Tf;
+
+      LG << "Correspondence between " << current_keyframe
+         << " and " << candidate_image
+         << ": " << Rc;
+
+      if (Rc < Tmin || Rc > Tmax)
+        continue;
+
+      Mat3 H, F;
+      ComputeHomographyFromCorrespondences(x1, x2, intrinsics, &H);
+      ComputeFundamentalFromCorrespondences(x1, x2, intrinsics, &F);
+
+      // TODO(sergey): STEP 2: Discard outlier matches
+
+      // STEP 3: Geometric Robust Information Criteria
+
+      // Compute error values for homography and fundamental matrices
+      Vec H_e, F_e;
+      H_e.resize(x1.cols());
+      F_e.resize(x1.cols());
+      for (int i = 0; i < x1.cols(); i++) {
+        Vec2 current_x1 =
+          NorrmalizedToPixelSpace(Vec2(x1(0, i), x1(1, i)), intrinsics);
+        Vec2 current_x2 =
+          NorrmalizedToPixelSpace(Vec2(x2(0, i), x2(1, i)), intrinsics);
+
+        H_e(i) = SymmetricGeometricDistance(H, current_x1, current_x2);
+        F_e(i) = SymmetricEpipolarDistance(F, current_x1, current_x2);
+      }
+
+      LG << "H_e: " << H_e.transpose();
+      LG << "F_e: " << F_e.transpose();
+
+      // Degeneracy constraint
+      double GRIC_H = GRIC(H_e, 2, 8, 4);
+      double GRIC_F = GRIC(F_e, 3, 7, 4);
+
+      LG << "GRIC values for frames " << current_keyframe
+         << " and " << candidate_image
+         << ", H-GRIC: " << GRIC_H
+         << ", F-GRIC: " << GRIC_F;
+
+      if (GRIC_H <= GRIC_F)
+        continue;
+
+      // TODO(sergey): STEP 4: PELC criterion
+
+      // STEP 5: Estimation of reconstruction error
+      //
+      // Uses paper Keyframe Selection for Camera Motion and Structure
+      // Estimation from Multiple Views
+      // Uses ftp://ftp.tnt.uni-hannover.de/pub/papers/2004/ECCV2004-TTHBAW.pdf
+      // Basically, equation (15)
+      //
+      // TODO(sergey): separate all the constraints into functions,
+      //               this one is getting to much cluttered already
+
+      // Definitions in equation (15):
+      // - I is the number of 3D feature points
+      // - A is the number of essential parameters of one camera
+
+      EuclideanReconstruction reconstruction;
+
+      // The F matrix should be an E matrix, but squash it just to be sure
+
+      // Reconstruction should happen using normalized fundamental matrix
+      Mat3 F_normal = N * F * N_inverse;
+
+      Mat3 E;
+      FundamentalToEssential(F_normal, &E);
+
+      // Recover motion between the two images. Since this function assumes a
+      // calibrated camera, use the identity for K
+      Mat3 R;
+      Vec3 t;
+      Mat3 K = Mat3::Identity();
+
+      if (!MotionFromEssentialAndCorrespondence(E,
+                                                K, x1.col(0),
+                                                K, x2.col(0),
+                                                &R, &t)) {
+        LG << "Failed to compute R and t from E and K";
+        continue;
+      }
+
+      LG << "Camera transform between frames " << current_keyframe
+         << " and " << candidate_image
+         << ":\nR:\n" << R
+         << "\nt:" << t.transpose();
+
+      // First camera is identity, second one is relative to it
+      reconstruction.InsertCamera(current_keyframe,
+                                  Mat3::Identity(),
+                                  Vec3::Zero());
+      reconstruction.InsertCamera(candidate_image, R, t);
+
+      // Reconstruct 3D points
+      int intersects_total = 0, intersects_success = 0;
+      for (int i = 0; i < tracked_markers.size(); i++) {
+        if (!reconstruction.PointForTrack(tracked_markers[i].track)) {
+          vector<Marker> reconstructed_markers;
+
+          int track = tracked_markers[i].track;
+
+          reconstructed_markers.push_back(tracked_markers[i]);
+
+          // We know there're always only two markers for a track
+          // Also, we're using brute-force search because we don't
+          // actually know about markers layout in a list, but
+          // at this moment this cycle will run just once, which
+          // is not so big deal
+
+          for (int j = i + 1; j < tracked_markers.size(); j++) {
+            if (tracked_markers[j].track == track) {
+              reconstructed_markers.push_back(tracked_markers[j]);
+              break;
+            }
+          }
+
+          intersects_total++;
+
+          if (EuclideanIntersect(reconstructed_markers, &reconstruction)) {
+            LG << "Ran Intersect() for track " << track;
+            intersects_success++;
+          } else {
+            LG << "Filed to intersect track " << track;
+          }
+        }
+      }
+
+      double success_intersects_factor =
+          (double) intersects_success / intersects_total;
+
+      if (success_intersects_factor < success_intersects_factor_best) {
+        LG << "Skip keyframe candidate because of "
+              "lower successful intersections ratio";
+
+        continue;
+      }
+
+      success_intersects_factor_best = success_intersects_factor;
+
+      Tracks two_frames_tracks(tracked_markers);
+      CameraIntrinsics empty_intrinsics;
+      BundleEvaluation evaluation;
+      evaluation.evaluate_jacobian = true;
+
+      EuclideanBundleCommonIntrinsics(two_frames_tracks,
+                                      BUNDLE_NO_INTRINSICS,
+                                      BUNDLE_NO_CONSTRAINTS,
+                                      &reconstruction,
+                                      &empty_intrinsics,
+                                      &evaluation);
+
+      Mat &jacobian = evaluation.jacobian;
+
+      Mat JT_J = jacobian.transpose() * jacobian;
+      Mat JT_J_inv = pseudoInverse(JT_J);
+
+      Mat temp_derived = JT_J * JT_J_inv * JT_J;
+      bool is_inversed = (temp_derived - JT_J).cwiseAbs2().sum() <
+          1e-4 * std::min(temp_derived.cwiseAbs2().sum(),
+                          JT_J.cwiseAbs2().sum());
+
+      LG << "Check on inversed: " << (is_inversed ? "true" : "false" )
+         << ", det(JT_J): " << JT_J.determinant();
+
+      if (!is_inversed) {
+        LG << "Ignoring candidature due to poor jacobian stability";
+        continue;
+      }
+
+      Mat Sigma_P;
+      Sigma_P = JT_J_inv.bottomRightCorner(evaluation.num_points * 3,
+                                           evaluation.num_points * 3);
+
+      int I = evaluation.num_points;
+      int A = 12;
+
+      double Sc = static_cast<double>(I + A) / Square(3 * I) * Sigma_P.trace();
+
+      LG << "Expected estimation error between "
+         << current_keyframe << " and "
+         << candidate_image << ": " << Sc;
+
+      // Pairing with a lower Sc indicates a better choice
+      if (Sc > Sc_best_candidate)
+        continue;
+
+      Sc_best_candidate = Sc;
+
+      next_keyframe = candidate_image;
+    }
+
+    // This is a bit arbitrary and main reason of having this is to deal
+    // better with situations when there's no keyframes were found for
+    // current keyframe this could happen when there's no so much parallax
+    // in the beginning of image sequence and then most of features are
+    // getting occluded. In this case there could be good keyframe pair in
+    // the middle of the sequence
+    //
+    // However, it's just quick hack and smarter way to do this would be nice
+    if (next_keyframe == -1) {
+      next_keyframe = current_keyframe + 10;
+      number_keyframes = 0;
+
+      if (next_keyframe >= max_image)
+        break;
+
+      LG << "Starting searching for keyframes starting from " << next_keyframe;
+    } else {
+      // New pair's expected reconstruction error is lower
+      // than existing pair's one.
+      //
+      // For now let's store just one candidate, easy to
+      // store more candidates but needs some thoughts
+      // how to choose best one automatically from them
+      // (or allow user to choose pair manually).
+      if (Sc_best > Sc_best_candidate) {
+        keyframes.clear();
+        keyframes.push_back(current_keyframe);
+        keyframes.push_back(next_keyframe);
+        Sc_best = Sc_best_candidate;
+      }
+    }
+  }
+}
+
+}  // namespace libmv
diff --git a/extern/libmv/libmv/simple_pipeline/keyframe_selection.h b/extern/libmv/libmv/simple_pipeline/keyframe_selection.h
new file mode 100644
index 00000000000..16d1e33a6fe
--- /dev/null
+++ b/extern/libmv/libmv/simple_pipeline/keyframe_selection.h
@@ -0,0 +1,52 @@
+// Copyright (c) 2010, 2011 libmv authors.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#ifndef LIBMV_SIMPLE_PIPELINE_KEYFRAME_SELECTION_H_
+#define LIBMV_SIMPLE_PIPELINE_KEYFRAME_SELECTION_H_
+
+#include "libmv/base/vector.h"
+#include "libmv/simple_pipeline/tracks.h"
+#include "libmv/simple_pipeline/camera_intrinsics.h"
+
+namespace libmv {
+
+// Get list of all images which are good enough to be as keyframes from
+// camera reconstruction. Based on GRIC criteria and uses Pollefeys'
+// approach for correspondence ratio constraint.
+//
+// As an additional, additional criteria based on reconstruction
+// variance is used. This means if correspondence and GRIC criteria
+// are passed, two-frames reconstruction using candidate keyframes
+// happens. After reconstruction variance of 3D points is calculating
+// and if expected error estimation is too large, keyframe candidate
+// is rejecting.
+//
+// \param tracks contains all tracked correspondences between frames
+//        expected to be undistorted and normalized
+// \param intrinsics is camera intrinsics
+// \param keyframes will contain all images number which are considered
+//        good to be used for reconstruction
+void SelectkeyframesBasedOnGRICAndVariance(const Tracks &tracks,
+                                           CameraIntrinsics &intrinsics,
+                                           vector<int> &keyframes);
+
+}  // namespace libmv
+
+#endif  // LIBMV_SIMPLE_PIPELINE_KEYFRAME_SELECTION_H_