Assorted camera tracker improvements

- Add support for refining the camera's intrinsic parameters
  during a solve. Currently, refining supports only the following
  combinations of intrinsic parameters:

    f
    f, cx, cy
    f, cx, cy, k1, k2
    f, k1
    f, k1, k2

  This is not the same as autocalibration, since the user must
  still make a reasonable initial guess about the focal length and
  other parameters, whereas true autocalibration would eliminate
  the need for the user specify intrinsic parameters at all.

  However, the solver works well with only rough guesses for the
  focal length, so perhaps full autocalibation is not that
  important.

  Adding support for the last two combinations, (f, k1) and (f,
  k1, k2) required changes to the library libmv depends on for
  bundle adjustment, SSBA. These changes should get ported
  upstream not just to libmv but to SSBA as well.

- Improved the region of convergence for bundle adjustment by
  increasing the number of Levenberg-Marquardt iterations from 50
  to 500. This way, the solver is able to crawl out of the bad
  local minima it gets stuck in when changing from, for example,
  bundling k1 and k2 to just k1 and resetting k2 to 0.

- Add several new region tracker implementations. A region tracker
  is a libmv concept, which refers to tracking a template image
  pattern through frames. The impact to end users is that tracking
  should "just work better". I am reserving a more detailed
  writeup, and maybe a paper, for later.

- Other libmv tweaks, such as detecting that a tracker is headed
  outside of the image bounds.

This includes several changes made directly to the libmv extern
code rather expecting to get those changes through normal libmv
channels, because I, the libmv BDFL, decided it was faster to work
on libmv directly in Blender, then later reverse-port the libmv
changes from Blender back into libmv trunk. The interesting part
is that I added a full Levenberg-Marquardt loop to the region
tracking code, which should lead to a more stable solutions. I
also added a hacky implementation of "Efficient Second-Order
Minimization" for tracking, which works nicely. A more detailed
quantitative evaluation will follow.

18 files changed, 1106 insertions, 85 deletions

diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt
index 41fc39c97ac..8c069375ff0 100644
--- a/extern/libmv/CMakeLists.txt
+++ b/extern/libmv/CMakeLists.txt
@@ -52,8 +52,10 @@ set(SRC
 	libmv/image/array_nd.cc
 	libmv/tracking/pyramid_region_tracker.cc
 	libmv/tracking/sad.cc
+	libmv/tracking/esm_region_tracker.cc
 	libmv/tracking/trklt_region_tracker.cc
 	libmv/tracking/klt_region_tracker.cc
+	libmv/tracking/lmicklt_region_tracker.cc
 	libmv/tracking/retrack_region_tracker.cc
 	libmv/multiview/projection.cc
 	libmv/multiview/conditioning.cc
@@ -99,8 +101,10 @@ set(SRC
 	libmv/tracking/retrack_region_tracker.h
 	libmv/tracking/sad.h
 	libmv/tracking/pyramid_region_tracker.h
+	libmv/tracking/esm_region_tracker.h
 	libmv/tracking/trklt_region_tracker.h
 	libmv/tracking/klt_region_tracker.h
+	libmv/tracking/lmicklt_region_tracker.h
 	libmv/base/id_generator.h
 	libmv/base/vector.h
 	libmv/base/scoped_ptr.h
diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp
index 2e007bb47b2..4987e356ef6 100644
--- a/extern/libmv/libmv-capi.cpp
+++ b/extern/libmv/libmv-capi.cpp
@@ -33,8 +33,10 @@
 #include "glog/logging.h"
 #include "Math/v3d_optimization.h"
 
+#include "libmv/tracking/esm_region_tracker.h"
 #include "libmv/tracking/klt_region_tracker.h"
 #include "libmv/tracking/trklt_region_tracker.h"
+#include "libmv/tracking/lmicklt_region_tracker.h"
 #include "libmv/tracking/pyramid_region_tracker.h"
 
 #include "libmv/tracking/sad.h"
@@ -47,6 +49,7 @@
 #include "libmv/simple_pipeline/camera_intrinsics.h"
 
 #include <stdlib.h>
+#include <assert.h>
 
 #ifdef DUMP_FAILURE
 #  include <png.h>
@@ -59,7 +62,7 @@
 #define DEFAULT_WINDOW_HALFSIZE	5
 
 typedef struct libmv_RegionTracker {
-	libmv::TrkltRegionTracker *trklt_region_tracker;
+	libmv::EsmRegionTracker *klt_region_tracker;
 	libmv::RegionTracker *region_tracker;
 } libmv_RegionTracker;
 
@@ -112,17 +115,17 @@ void libmv_setLoggingVerbosity(int verbosity)
 
 libmv_RegionTracker *libmv_regionTrackerNew(int max_iterations, int pyramid_level)
 {
-	libmv::TrkltRegionTracker *trklt_region_tracker = new libmv::TrkltRegionTracker;
+	libmv::EsmRegionTracker *klt_region_tracker = new libmv::EsmRegionTracker;
 
-	trklt_region_tracker->half_window_size = DEFAULT_WINDOW_HALFSIZE;
-	trklt_region_tracker->max_iterations = max_iterations;
-	trklt_region_tracker->min_determinant = 1e-4;
+	klt_region_tracker->half_window_size = DEFAULT_WINDOW_HALFSIZE;
+	klt_region_tracker->max_iterations = max_iterations;
+	klt_region_tracker->min_determinant = 1e-4;
 
 	libmv::PyramidRegionTracker *region_tracker =
-		new libmv::PyramidRegionTracker(trklt_region_tracker, pyramid_level);
+		new libmv::PyramidRegionTracker(klt_region_tracker, pyramid_level);
 
 	libmv_RegionTracker *configured_region_tracker = new libmv_RegionTracker;
-	configured_region_tracker->trklt_region_tracker = trklt_region_tracker;
+	configured_region_tracker->klt_region_tracker = klt_region_tracker;
 	configured_region_tracker->region_tracker = region_tracker;
 
 	return configured_region_tracker;
@@ -271,13 +274,13 @@ int libmv_regionTrackerTrack(libmv_RegionTracker *libmv_tracker, const float *im
 			 double x1, double y1, double *x2, double *y2)
 {
 	libmv::RegionTracker *region_tracker;
-	libmv::TrkltRegionTracker *trklt_region_tracker;
+	libmv::EsmRegionTracker *klt_region_tracker;
 	libmv::FloatImage old_patch, new_patch;
 
-	trklt_region_tracker = libmv_tracker->trklt_region_tracker;
+	klt_region_tracker = libmv_tracker->klt_region_tracker;
 	region_tracker = libmv_tracker->region_tracker;
 
-	trklt_region_tracker->half_window_size = half_window_size;
+	klt_region_tracker->half_window_size = half_window_size;
 
 	floatBufToImage(ima1, width, height, &old_patch);
 	floatBufToImage(ima2, width, height, &new_patch);
@@ -353,8 +356,24 @@ void libmv_tracksDestroy(libmv_Tracks *libmv_tracks)
 
 /* ************ Reconstruction solver ************ */
 
+int libmv_refineParametersAreValid(int parameters) {
+	return (parameters == (LIBMV_REFINE_FOCAL_LENGTH))         ||
+	       (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
+	                       LIBMV_REFINE_PRINCIPAL_POINT))      ||
+	       (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
+	                       LIBMV_REFINE_PRINCIPAL_POINT        |
+	                       LIBMV_REFINE_RADIAL_DISTORTION_K1   |
+	                       LIBMV_REFINE_RADIAL_DISTORTION_K2)) ||
+	       (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
+	                       LIBMV_REFINE_RADIAL_DISTORTION_K1   |
+	                       LIBMV_REFINE_RADIAL_DISTORTION_K2)) ||
+	       (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
+	                       LIBMV_REFINE_RADIAL_DISTORTION_K1));
+}
+
+
 libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyframe1, int keyframe2,
-			double focal_length, double principal_x, double principal_y, double k1, double k2, double k3)
+		int refine_intrinsics, double focal_length, double principal_x, double principal_y, double k1, double k2, double k3)
 {
 	/* Invert the camera intrinsics. */
 	libmv::vector<libmv::Marker> markers = ((libmv::Tracks*)tracks)->AllMarkers();
@@ -378,13 +397,34 @@ libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyfra
 
 	libmv::Tracks normalized_tracks(markers);
 
+	printf("frames to init from: %d, %d\n", keyframe1, keyframe2);
 	libmv::vector<libmv::Marker> keyframe_markers =
 		normalized_tracks.MarkersForTracksInBothImages(keyframe1, keyframe2);
+	printf("number of markers for init: %d\n", keyframe_markers.size());
 
 	libmv::EuclideanReconstructTwoFrames(keyframe_markers, reconstruction);
 	libmv::EuclideanBundle(normalized_tracks, reconstruction);
+
 	libmv::EuclideanCompleteReconstruction(normalized_tracks, reconstruction);
 
+	if (refine_intrinsics) {
+		/* only a few combinations are supported but trust the caller */
+		int libmv_refine_flags = 0;
+		if (refine_intrinsics & LIBMV_REFINE_FOCAL_LENGTH) {
+			libmv_refine_flags |= libmv::BUNDLE_FOCAL_LENGTH;
+		}
+		if (refine_intrinsics & LIBMV_REFINE_PRINCIPAL_POINT) {
+			libmv_refine_flags |= libmv::BUNDLE_PRINCIPAL_POINT;
+		}
+		if (refine_intrinsics & LIBMV_REFINE_RADIAL_DISTORTION_K1) {
+			libmv_refine_flags |= libmv::BUNDLE_RADIAL_K1;
+		}
+		if (refine_intrinsics & LIBMV_REFINE_RADIAL_DISTORTION_K2) {
+			libmv_refine_flags |= libmv::BUNDLE_RADIAL_K2;
+		}
+		libmv::EuclideanBundleCommonIntrinsics(*(libmv::Tracks *)tracks, libmv_refine_flags, reconstruction, intrinsics);
+	}
+
 	libmv_reconstruction->tracks = *(libmv::Tracks *)tracks;
 	libmv_reconstruction->error = libmv::EuclideanReprojectionError(*(libmv::Tracks *)tracks, *reconstruction, *intrinsics);
 
@@ -608,6 +648,10 @@ void libmv_destroyFeatures(struct libmv_Features *libmv_features)
 
 /* ************ camera intrinsics ************ */
 
+struct libmv_CameraIntrinsics *libmv_ReconstructionExtractIntrinsics(struct libmv_Reconstruction *libmv_Reconstruction) {
+  return (struct libmv_CameraIntrinsics *)&libmv_Reconstruction->intrinsics;
+}
+
 struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsNew(double focal_length, double principal_x, double principal_y,
 			double k1, double k2, double k3, int width, int height)
 {
@@ -654,6 +698,16 @@ void libmv_CameraIntrinsicsUpdate(struct libmv_CameraIntrinsics *libmvIntrinsics
 		intrinsics->SetImageSize(width, height);
 }
 
+void libmv_CameraIntrinsicsExtract(struct libmv_CameraIntrinsics *libmvIntrinsics, double *focal_length,
+			double *principal_x, double *principal_y, double *k1, double *k2, double *k3, int *width, int *height) {
+	libmv::CameraIntrinsics *intrinsics= (libmv::CameraIntrinsics *) libmvIntrinsics;
+	*focal_length = intrinsics->focal_length();
+	*principal_x = intrinsics->principal_point_x();
+	*principal_y = intrinsics->principal_point_y();
+	*k1 = intrinsics->k1();
+	*k2 = intrinsics->k2();
+}
+
 void libmv_CameraIntrinsicsUndistortByte(struct libmv_CameraIntrinsics *libmvIntrinsics,
 			unsigned char *src, unsigned char *dst, int width, int height, float overscan, int channels)
 {
diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h
index b71a66b73a6..d378afc5ea8 100644
--- a/extern/libmv/libmv-capi.h
+++ b/extern/libmv/libmv-capi.h
@@ -61,8 +61,14 @@ void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track,
 void libmv_tracksDestroy(struct libmv_Tracks *libmv_tracks);
 
 /* Reconstruction solver */
+#define LIBMV_REFINE_FOCAL_LENGTH	(1<<0)
+#define LIBMV_REFINE_PRINCIPAL_POINT	(1<<1)
+#define LIBMV_REFINE_RADIAL_DISTORTION_K1 (1<<2)
+#define LIBMV_REFINE_RADIAL_DISTORTION_K2 (1<<4)
+int libmv_refineParametersAreValid(int parameters);
+
 struct libmv_Reconstruction *libmv_solveReconstruction(struct libmv_Tracks *tracks, int keyframe1, int keyframe2,
-			double focal_length, double principal_x, double principal_y, double k1, double k2, double k3);
+			int refine_intrinsics, double focal_length, double principal_x, double principal_y, double k1, double k2, double k3);
 int libmv_reporojectionPointForTrack(struct libmv_Reconstruction *libmv_reconstruction, int track, double pos[3]);
 double libmv_reporojectionErrorForTrack(struct libmv_Reconstruction *libmv_reconstruction, int track);
 double libmv_reporojectionErrorForImage(struct libmv_Reconstruction *libmv_reconstruction, int image);
@@ -80,18 +86,21 @@ void libmv_getFeature(struct libmv_Features *libmv_features, int number, double
 void libmv_destroyFeatures(struct libmv_Features *libmv_features);
 
 /* camera intrinsics */
+struct libmv_CameraIntrinsics *libmv_ReconstructionExtractIntrinsics(struct libmv_Reconstruction *libmv_Reconstruction);
+
 struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsNew(double focal_length, double principal_x, double principal_y,
 			double k1, double k2, double k3, int width, int height);
 
 struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsCopy(struct libmv_CameraIntrinsics *libmvIntrinsics);
 
-struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsCopy(struct libmv_CameraIntrinsics *libmvIntrinsics);
-
 void libmv_CameraIntrinsicsDestroy(struct libmv_CameraIntrinsics *libmvIntrinsics);
 
 void libmv_CameraIntrinsicsUpdate(struct libmv_CameraIntrinsics *libmvIntrinsics, double focal_length,
 			double principal_x, double principal_y, double k1, double k2, double k3, int width, int height);
 
+void libmv_CameraIntrinsicsExtract(struct libmv_CameraIntrinsics *libmvIntrinsics, double *focal_length,
+			double *principal_x, double *principal_y, double *k1, double *k2, double *k3, int *width, int *height);
+
 void libmv_CameraIntrinsicsUndistortByte(struct libmv_CameraIntrinsics *libmvIntrinsics,
 			unsigned char *src, unsigned char *dst, int width, int height, float overscan, int channels);
 
diff --git a/extern/libmv/libmv/simple_pipeline/bundle.cc b/extern/libmv/libmv/simple_pipeline/bundle.cc
index cb8822dcf44..d382cd5a4fc 100644
--- a/extern/libmv/libmv/simple_pipeline/bundle.cc
+++ b/extern/libmv/libmv/simple_pipeline/bundle.cc
@@ -27,6 +27,8 @@
 #include "libmv/multiview/fundamental.h"
 #include "libmv/multiview/projection.h"
 #include "libmv/numeric/numeric.h"
+#include "libmv/simple_pipeline/camera_intrinsics.h"
+#include "libmv/simple_pipeline/bundle.h"
 #include "libmv/simple_pipeline/reconstruction.h"
 #include "libmv/simple_pipeline/tracks.h"
 #include "third_party/ssba/Geometry/v3d_cameramatrix.h"
@@ -38,6 +40,18 @@ namespace libmv {
 
 void EuclideanBundle(const Tracks &tracks,
                      EuclideanReconstruction *reconstruction) {
+  CameraIntrinsics intrinsics;
+  EuclideanBundleCommonIntrinsics(tracks,
+                                  BUNDLE_NO_INTRINSICS,
+                                  reconstruction,
+                                  &intrinsics);
+}
+
+void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
+                                     int bundle_intrinsics,
+                                     EuclideanReconstruction *reconstruction,
+                                     CameraIntrinsics *intrinsics) {
+  LG << "Original intrinsics: " << *intrinsics;
   vector<Marker> markers = tracks.AllMarkers();
 
   // "index" in this context is the index that V3D's optimizer will see. The
@@ -69,18 +83,20 @@ void EuclideanBundle(const Tracks &tracks,
     }
   }
 
-  // Make a V3D identity matrix, needed in a few places for K, since this
-  // assumes a calibrated setup.
-  V3D::Matrix3x3d identity3x3;
-  identity3x3[0][0] = 1.0;
-  identity3x3[0][1] = 0.0;
-  identity3x3[0][2] = 0.0;
-  identity3x3[1][0] = 0.0;
-  identity3x3[1][1] = 1.0;
-  identity3x3[1][2] = 0.0;
-  identity3x3[2][0] = 0.0;
-  identity3x3[2][1] = 0.0;
-  identity3x3[2][2] = 1.0;
+  // Convert libmv's K matrix to V3d's K matrix.
+  V3D::Matrix3x3d v3d_K;
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      v3d_K[i][j] = intrinsics->K()(i, j);
+    }
+  }
+
+  // Convert libmv's distortion to v3d distortion.
+  V3D::StdDistortionFunction v3d_distortion;
+  v3d_distortion.k1 = intrinsics->k1();
+  v3d_distortion.k2 = intrinsics->k2();
+  v3d_distortion.p1 = intrinsics->p1();
+  v3d_distortion.p2 = intrinsics->p2();
 
   // Convert libmv's cameras to V3D's cameras.
   std::vector<V3D::CameraMatrix> v3d_cameras(index_to_camera.size());
@@ -98,7 +114,7 @@ void EuclideanBundle(const Tracks &tracks,
       }
       t[i] = t_libmv(i);
     }
-    v3d_cameras[k].setIntrinsic(identity3x3);
+    v3d_cameras[k].setIntrinsic(v3d_K);
     v3d_cameras[k].setRotation(R);
     v3d_cameras[k].setTranslation(t);
   }
@@ -134,27 +150,81 @@ void EuclideanBundle(const Tracks &tracks,
   }
   LG << "Number of residuals: " << num_residuals;
   
-  // This is calibrated reconstruction, so use zero distortion.
-  V3D::StdDistortionFunction v3d_distortion;
-  v3d_distortion.k1 = 0;
-  v3d_distortion.k2 = 0;
-  v3d_distortion.p1 = 0;
-  v3d_distortion.p2 = 0;
+  // Convert from libmv's specification for which intrinsics to bundle to V3D's.
+  int v3d_bundle_intrinsics;
+  if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
+    LG << "Bundling only camera positions.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_METRIC;
+  } else if (bundle_intrinsics == BUNDLE_FOCAL_LENGTH) {
+    LG << "Bundling f.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_LENGTH;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_PRINCIPAL_POINT)) {
+    LG << "Bundling f, px, py.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_LENGTH_PP;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_PRINCIPAL_POINT |
+                                   BUNDLE_RADIAL)) {
+    LG << "Bundling f, px, py, k1, k2.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_PRINCIPAL_POINT |
+                                   BUNDLE_RADIAL |
+                                   BUNDLE_TANGENTIAL)) {
+    LG << "Bundling f, px, py, k1, k2, p1, p2.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL_TANGENTIAL;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_RADIAL |
+                                   BUNDLE_TANGENTIAL)) {
+    LG << "Bundling f, px, py, k1, k2, p1, p2.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL_TANGENTIAL;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_RADIAL)) {
+    LG << "Bundling f, k1, k2.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_AND_RADIAL;
+  } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
+                                   BUNDLE_RADIAL_K1)) {
+    LG << "Bundling f, k1.";
+    v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_AND_RADIAL_K1;
+  } else {
+    LOG(FATAL) << "Unsupported bundle combination.";
+  }
+
+  // Ignore any outliers; assume supervised tracking.
+  double v3d_inlier_threshold = 500000.0;
 
   // Finally, run the bundle adjustment.
-  double const inlierThreshold = 500000.0;
-  V3D::CommonInternalsMetricBundleOptimizer opt(V3D::FULL_BUNDLE_METRIC,
-                                                inlierThreshold,
-                                                identity3x3,
+  V3D::CommonInternalsMetricBundleOptimizer opt(v3d_bundle_intrinsics,
+                                                v3d_inlier_threshold,
+                                                v3d_K,
                                                 v3d_distortion,
                                                 v3d_cameras,
                                                 v3d_points,
                                                 v3d_measurements,
                                                 v3d_camera_for_measurement,
                                                 v3d_point_for_measurement);
-  opt.maxIterations = 50;
+  opt.maxIterations = 500;
   opt.minimize();
-  LG << "Bundle status: " << opt.status;
+  if (opt.status == V3D::LEVENBERG_OPTIMIZER_TIMEOUT) {
+    LG << "Bundle status: Timed out.";
+  } else if (opt.status == V3D::LEVENBERG_OPTIMIZER_SMALL_UPDATE) {
+    LG << "Bundle status: Small update.";
+  } else if (opt.status == V3D::LEVENBERG_OPTIMIZER_CONVERGED) {
+    LG << "Bundle status: Converged.";
+  }
+
+  // Convert V3D's K matrix back to libmv's K matrix.
+  Mat3 K;
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      K(i, j) = v3d_K[i][j];
+    }
+  }
+  intrinsics->SetK(K);
+
+  // Convert V3D's distortion back to libmv's distortion.
+  intrinsics->SetRadialDistortion(v3d_distortion.k1, v3d_distortion.k2, 0.0);
+  intrinsics->SetTangentialDistortion(v3d_distortion.p1, v3d_distortion.p2);
 
   // Convert V3D's cameras back to libmv's cameras.
   for (int k = 0; k < num_cameras; k++) {
@@ -173,6 +243,7 @@ void EuclideanBundle(const Tracks &tracks,
       index_to_point[k]->X(i) = v3d_points[k][i];
     }
   }
+  LG << "Final intrinsics: " << *intrinsics;
 }
 
 void ProjectiveBundle(const Tracks & /*tracks*/,
diff --git a/extern/libmv/libmv/simple_pipeline/bundle.h b/extern/libmv/libmv/simple_pipeline/bundle.h
index c7fb2a79607..55657cb2d92 100644
--- a/extern/libmv/libmv/simple_pipeline/bundle.h
+++ b/extern/libmv/libmv/simple_pipeline/bundle.h
@@ -23,6 +23,7 @@
 
 namespace libmv {
 
+class CameraIntrinsics;
 class EuclideanReconstruction;
 class ProjectiveReconstruction;
 class Tracks;
@@ -50,6 +51,46 @@ void EuclideanBundle(const Tracks &tracks,
 /*!
     Refine camera poses and 3D coordinates using bundle adjustment.
 
+    This routine adjusts all cameras positions, points, and the camera
+    intrinsics (assumed common across all images) in \a *reconstruction. This
+    assumes a full observation for reconstructed tracks; this implies that if
+    there is a reconstructed 3D point (a bundle) for a track, then all markers
+    for that track will be included in the minimization. \a tracks should
+    contain markers used in the initial reconstruction.
+
+    The cameras, bundles, and intrinsics are refined in-place.
+
+    The only supported combinations of bundle parameters are:
+
+    BUNDLE_NO_INTRINSICS
+    BUNDLE_FOCAL_LENGTH
+    BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT
+    BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT | BUNDLE_RADIAL
+    BUNDLE_FOCAL_LENGTH | BUNDLE_PRINCIPAL_POINT | BUNDLE_RADIAL | BUNDLE_TANGENTIAL
+
+    \note This assumes an outlier-free set of markers.
+
+    \sa EuclideanResect, EuclideanIntersect, EuclideanReconstructTwoFrames
+*/
+enum BundleIntrinsics {
+  BUNDLE_NO_INTRINSICS = 0,
+  BUNDLE_FOCAL_LENGTH = 1,
+  BUNDLE_PRINCIPAL_POINT = 2,
+  BUNDLE_RADIAL_K1 = 4,
+  BUNDLE_RADIAL_K2 = 8,
+  BUNDLE_RADIAL = 12,
+  BUNDLE_TANGENTIAL_P1 = 16,
+  BUNDLE_TANGENTIAL_P2 = 32,
+  BUNDLE_TANGENTIAL = 48,
+};
+void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
+                                     int bundle_intrinsics,
+                                     EuclideanReconstruction *reconstruction,
+                                     CameraIntrinsics *intrinsics);
+
+/*!
+    Refine camera poses and 3D coordinates using bundle adjustment.
+
     This routine adjusts all cameras and points in \a *reconstruction. This
     assumes a full observation for reconstructed tracks; this implies that if
     there is a reconstructed 3D point (a bundle) for a track, then all markers
diff --git a/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc b/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
index 366129dd3d2..917f80e6926 100644
--- a/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
+++ b/extern/libmv/libmv/simple_pipeline/camera_intrinsics.cc
@@ -348,4 +348,26 @@ void CameraIntrinsics::Undistort(const unsigned char* src, unsigned char* dst, i
   //else assert("channels must be between 1 and 4");
 }
 
+std::ostream& operator <<(std::ostream &os,
+                          const CameraIntrinsics &intrinsics) {
+  if (intrinsics.focal_length_x() == intrinsics.focal_length_x()) {
+    os << "f=" << intrinsics.focal_length();
+  } else {
+    os <<  "fx=" << intrinsics.focal_length_x()
+       << " fy=" << intrinsics.focal_length_y();
+  }
+  os << " cx=" << intrinsics.principal_point_x()
+     << " cy=" << intrinsics.principal_point_y()
+     << " w=" << intrinsics.image_width()
+     << " h=" << intrinsics.image_height();
+
+  if (intrinsics.k1() != 0.0) { os << " k1=" << intrinsics.k1(); }
+  if (intrinsics.k2() != 0.0) { os << " k2=" << intrinsics.k2(); }
+  if (intrinsics.k3() != 0.0) { os << " k3=" << intrinsics.k3(); }
+  if (intrinsics.p1() != 0.0) { os << " p1=" << intrinsics.p1(); }
+  if (intrinsics.p2() != 0.0) { os << " p2=" << intrinsics.p2(); }
+
+  return os;
+}
+
 }  // namespace libmv
diff --git a/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h b/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h
index f4bf903c36c..e0556674ad5 100644
--- a/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h
+++ b/extern/libmv/libmv/simple_pipeline/camera_intrinsics.h
@@ -21,11 +21,15 @@
 #ifndef LIBMV_SIMPLE_PIPELINE_CAMERA_INTRINSICS_H_
 #define LIBMV_SIMPLE_PIPELINE_CAMERA_INTRINSICS_H_
 
+#include <iostream>
+#include <string>
+
 #include <Eigen/Core>
-typedef Eigen::Matrix<double, 3, 3> Mat3;
 
 namespace libmv {
 
+typedef Eigen::Matrix<double, 3, 3> Mat3;
+
 struct Grid;
 
 class CameraIntrinsics {
@@ -35,7 +39,6 @@ class CameraIntrinsics {
   ~CameraIntrinsics();
 
   const Mat3 &K()                 const { return K_;            }
-  // FIXME(MatthiasF): these should be CamelCase methods
   double      focal_length()      const { return K_(0, 0);      }
   double      focal_length_x()    const { return K_(0, 0);      }
   double      focal_length_y()    const { return K_(1, 1);      }
@@ -55,8 +58,10 @@ class CameraIntrinsics {
   /// Set both x and y focal length in pixels.
   void SetFocalLength(double focal_x, double focal_y);
 
+  /// Set principal point in pixels.
   void SetPrincipalPoint(double cx, double cy);
 
+  /// Set the image size in pixels.
   void SetImageSize(int width, int height);
 
   void SetRadialDistortion(double k1, double k2, double k3 = 0);
@@ -92,6 +97,7 @@ class CameraIntrinsics {
   */
   void Distort(const float* src, float* dst,
                int width, int height, double overscan, int channels);
+
   /*!
       Distort an image using the current camera instrinsics
 
@@ -102,6 +108,7 @@ class CameraIntrinsics {
   */
   void Distort(const unsigned char* src, unsigned char* dst,
                int width, int height, double overscan, int channels);
+
   /*!
       Undistort an image using the current camera instrinsics
 
@@ -112,6 +119,7 @@ class CameraIntrinsics {
   */
   void Undistort(const float* src, float* dst,
                  int width, int height, double overscan, int channels);
+
   /*!
       Undistort an image using the current camera instrinsics
 
@@ -147,6 +155,10 @@ class CameraIntrinsics {
   struct Grid *undistort_;
 };
 
+/// A human-readable representation of the camera intrinsic parameters.
+std::ostream& operator <<(std::ostream &os,
+                          const CameraIntrinsics &intrinsics);
+
 }  // namespace libmv
 
 #endif  // LIBMV_SIMPLE_PIPELINE_CAMERA_INTRINSICS_H_
diff --git a/extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc b/extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc
index 0597f09f728..77fe2a530c4 100644
--- a/extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc
+++ b/extern/libmv/libmv/simple_pipeline/initialize_reconstruction.cc
@@ -67,6 +67,7 @@ void GetImagesInMarkers(const vector<Marker> &markers,
 bool EuclideanReconstructTwoFrames(const vector<Marker> &markers,
                                    EuclideanReconstruction *reconstruction) {
   if (markers.size() < 16) {
+    LG << "Not enough markers to initialize from two frames: " << markers.size();
     return false;
   }
 
@@ -105,6 +106,7 @@ bool EuclideanReconstructTwoFrames(const vector<Marker> &markers,
                                             K, x1.col(0),
                                             K, x2.col(0),
                                             &R, &t)) {
+    LG << "Failed to compute R and t from E and K.";
     return false;
   }
 
diff --git a/extern/libmv/libmv/simple_pipeline/pipeline.cc b/extern/libmv/libmv/simple_pipeline/pipeline.cc
index 818c24cb5e7..9512a41c00f 100644
--- a/extern/libmv/libmv/simple_pipeline/pipeline.cc
+++ b/extern/libmv/libmv/simple_pipeline/pipeline.cc
@@ -262,7 +262,6 @@ double InternalReprojectionError(const Tracks &image_tracks,
            ex,
            ey,
            sqrt(ex*ex + ey*ey));
-    //LG << line;
     total_error += sqrt(ex*ex + ey*ey);
   }
   LG << "Skipped " << num_skipped << " markers.";
diff --git a/extern/libmv/libmv/tracking/esm_region_tracker.cc b/extern/libmv/libmv/tracking/esm_region_tracker.cc
new file mode 100644
index 00000000000..2e5c255e153
--- /dev/null
+++ b/extern/libmv/libmv/tracking/esm_region_tracker.cc
@@ -0,0 +1,288 @@
+// Copyright (c) 2007, 2008, 2009, 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.
+
+#include "libmv/tracking/esm_region_tracker.h"
+
+#include "libmv/logging/logging.h"
+#include "libmv/image/image.h"
+#include "libmv/image/convolve.h"
+#include "libmv/image/sample.h"
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+// TODO(keir): Reduce duplication between here and the other region trackers.
+bool RegionIsInBounds(const FloatImage &image1,
+                      double x, double y,
+                      int half_window_size) {
+  // Check the minimum coordinates.
+  int min_x = floor(x) - half_window_size - 1;
+  int min_y = floor(y) - half_window_size - 1;
+  if (min_x < 0.0 ||
+      min_y < 0.0) {
+    return false;
+  }
+
+  // Check the maximum coordinates.
+  int max_x = ceil(x) + half_window_size + 1;
+  int max_y = ceil(y) + half_window_size + 1;
+  if (max_x > image1.cols() ||
+      max_y > image1.rows()) {
+    return false;
+  }
+
+  // Ok, we're good.
+  return true;
+}
+
+// Sample a region centered at x,y in image with size extending by half_width
+// from x,y. Channels specifies the number of channels to sample from.
+void SamplePattern(const FloatImage &image,
+                   double x, double y,
+                   int half_width,
+                   int channels,
+                   FloatImage *sampled) {
+  sampled->Resize(2 * half_width + 1, 2 * half_width + 1, channels);
+  for (int r = -half_width; r <= half_width; ++r) {
+    for (int c = -half_width; c <= half_width; ++c) {
+      for (int i = 0; i < channels; ++i) {
+        (*sampled)(r + half_width, c + half_width, i) =
+            SampleLinear(image, y + r, x + c, i);
+      }
+    }
+  }
+}
+
+// Estimate "reasonable" error by computing autocorrelation for a small shift.
+// TODO(keir): Add a facility for 
+double EstimateReasonableError(const FloatImage &image,
+                               double x, double y,
+                               int half_width) {
+  double error = 0.0;
+  for (int r = -half_width; r <= half_width; ++r) {
+    for (int c = -half_width; c <= half_width; ++c) {
+      double s = SampleLinear(image, y + r, x + c, 0);
+      double e1 = SampleLinear(image, y + r + 0.5, x + c, 0) - s;
+      double e2 = SampleLinear(image, y + r, x + c + 0.5, 0) - s;
+      error += e1*e1 + e2*e2;
+    }
+  }
+  // XXX hack
+  return error / 2.0 * 16.0;
+}
+
+// This is implemented from "Lukas and Kanade 20 years on: Part 1. Page 42,
+// figure 14: the Levenberg-Marquardt-Inverse Compositional Algorithm".
+bool EsmRegionTracker::Track(const FloatImage &image1,
+                             const FloatImage &image2,
+                             double  x1, double  y1,
+                             double *x2, double *y2) const {
+  if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
+    LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
+       << ", hw=" << half_window_size << ".";
+    return false;
+  }
+  
+  int width = 2 * half_window_size + 1;
+
+  // TODO(keir): Avoid recomputing gradients for e.g. the pyramid tracker.
+  Array3Df image_and_gradient1;
+  Array3Df image_and_gradient2;
+  BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
+  BlurredImageAndDerivativesChannels(image2, sigma, &image_and_gradient2);
+
+  // Step -1: Resample the template (image1) since it is not pixel aligned.
+  //
+  // Take a sample of the gradient of the pattern area of image1 at the
+  // subpixel position x1, x2. This is reused for each iteration, so
+  // precomputing it saves time.
+  Array3Df image_and_gradient1_sampled;
+  SamplePattern(image_and_gradient1, x1, y1, half_window_size, 3,
+                &image_and_gradient1_sampled);
+
+  // Step 0: Initialize delta = 0.01.
+  // 
+  // Ignored for my "normal" LM loop.
+
+  double reasonable_error =
+      EstimateReasonableError(image1, x1, y1, half_window_size);
+
+  // Step 1: Warp I with W(x, p) to compute I(W(x; p).
+  //
+  // Use two images for accepting / rejecting updates.
+  // XXX is this necessary?
+  int current_image = 0, new_image = 1;
+  Array3Df image_and_gradient2_sampled[2];
+  SamplePattern(image_and_gradient2, *x2, *y2, half_window_size, 3,
+                &image_and_gradient2_sampled[current_image]);
+
+  // Step 2: Compute the squared error I - J.
+  double error = 0;
+  for (int r = 0; r < width; ++r) {
+    for (int c = 0; c < width; ++c) {
+      double e = image_and_gradient1_sampled(r, c, 0) -
+                 image_and_gradient2_sampled[current_image](r, c, 0);
+      error += e*e;
+    }
+  }
+
+  // Step 3: Evaluate the gradient of the template.
+  //
+  // This is done above when sampling the template (step -1).
+
+  // Step 4: Evaluate the jacobian dw/dp at (x; 0).
+  //
+  // The jacobian between dx,dy and the warp is constant 2x2 identity, so it
+  // doesn't have to get computed. The Gauss-Newton Hessian matrix computation
+  // below would normally have to include a multiply by the jacobian.
+
+  // Step 5: Compute the steepest descent images of the template.
+  //
+  // Since the jacobian of the position w.r.t. the sampled template position is
+  // the identity, the steepest descent images are the same as the gradient.
+
+  // Step 6: Compute the Gauss-Newton Hessian for the template (image1).
+  //
+  // This could get rolled into the previous loop, but split it for now for
+  // clarity.
+  Mat2 H_image1 = Mat2::Zero();
+  for (int r = 0; r < width; ++r) {
+    for (int c = 0; c < width; ++c) {
+      Vec2 g(image_and_gradient1_sampled(r, c, 1),
+             image_and_gradient1_sampled(r, c, 2));
+      H_image1 += g * g.transpose();
+    }
+  }
+
+  double tau = 1e-4, eps1, eps2, eps3;
+  eps1 = eps2 = eps3 = 1e-15;
+
+  double mu = tau * std::max(H_image1(0, 0), H_image1(1, 1));
+  double nu = M_E;
+
+  int i;
+  for (i = 0; i < max_iterations; ++i) {
+    // Check that the entire image patch is within the bounds of the images.
+    if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
+      LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
+         << ", hw=" << half_window_size << ".";
+      return false;
+    }
+
+    Mat2 H = Mat2::Zero();
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        Vec2 g1(image_and_gradient1_sampled(r, c, 1),
+                image_and_gradient1_sampled(r, c, 2));
+        Vec2 g2(image_and_gradient2_sampled[current_image](r, c, 1),
+                image_and_gradient2_sampled[current_image](r, c, 2));
+        Vec2 g = g1 + g2; // Should be / 2.0, but do that outside the loop.
+        H += g * g.transpose();
+      }
+    }
+    H /= 4.0;
+
+    // Step 7: Compute z
+    Vec2 z = Vec2::Zero();
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        double e = image_and_gradient2_sampled[current_image](r, c, 0) -
+                   image_and_gradient1_sampled(r, c, 0);
+        z(0) += image_and_gradient1_sampled(r, c, 1) * e;
+        z(1) += image_and_gradient1_sampled(r, c, 2) * e;
+        z(0) += image_and_gradient2_sampled[current_image](r, c, 1) * e;
+        z(1) += image_and_gradient2_sampled[current_image](r, c, 2) * e;
+      }
+    }
+    z /= 2.0;
+
+    // Step 8: Compute Hlm and (dx,dy)
+    Mat2 diag  = H.diagonal().asDiagonal();
+    diag *= mu;
+    Mat2 Hlm = H + diag;
+    Vec2 d = Hlm.lu().solve(z);
+
+    // TODO(keir): Use the usual LM termination and update criterion instead of
+    // this hacky version from the LK 20 years on paper.
+    LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1]
+       << ", mu=" << mu << ", nu=" << nu;
+
+    // Step 9: Update the warp; W(x; p) <-- W(x;p) compose W(x, dp)^-1
+    double new_x2 = *x2 - d[0];
+    double new_y2 = *y2 - d[1];
+
+    // Step 9.1: Sample the image at the new position.
+    SamplePattern(image_and_gradient2, new_x2, new_y2, half_window_size, 3,
+                  &image_and_gradient2_sampled[new_image]);
+
+    // Step 9.2: Compute the new error.
+    // TODO(keir): Eliminate duplication with above code.
+    double new_error = 0;
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        double e = image_and_gradient1_sampled(r, c, 0) -
+                   image_and_gradient2_sampled[new_image](r, c, 0);
+        new_error += e*e;
+      }
+    }
+    //LG << "Old error: " << error << ", new error: " << new_error;
+
+    double rho = (error - new_error) / (d.transpose() * (mu * d + z));
+
+    // Step 10: Accept or reject step.
+    if (rho <= 0) {
+      // This was a bad step, so don't update.
+      mu *= nu;
+
+      // The standard Levenberg-Marquardt update multiplies nu by 2, but
+      // instead chose e. I chose a constant greater than 2 since in typical
+      // tracking examples, I saw that once updates started failing they should
+      // ramp towards steepest descent faster. This has no basis in theory, but
+      // appears to lead to faster tracking overall.
+      nu *= M_E;
+      LG << "Error increased, so reject update.";
+    } else {
+      // The step was good, so update.
+      *x2 = new_x2;
+      *y2 = new_y2;
+      std::swap(new_image, current_image);
+      error = new_error;
+
+      mu *= std::max(1/3., 1 - pow(2*rho - 1, 3));
+      nu = M_E;  // See above for why to use e.
+    }
+
+    // If the step was accepted, then check for termination.
+    if (d.squaredNorm() < min_update_squared_distance) {
+      if (new_error > reasonable_error) {
+        LG << "Update size shrank but reasonable error ("
+           << reasonable_error << ") not achieved; failing.";
+        return true; // XXX
+      }
+      LG << "Successful track in " << (i + 1) << " iterations.";
+      return true;
+    }
+  }
+  // Getting here means we hit max iterations, so tracking failed.
+  LG << "Too many iterations; max is set to " << max_iterations << ".";
+  return false;
+}
+
+}  // namespace libmv
diff --git a/extern/libmv/libmv/tracking/esm_region_tracker.h b/extern/libmv/libmv/tracking/esm_region_tracker.h
new file mode 100644
index 00000000000..c63417201ad
--- /dev/null
+++ b/extern/libmv/libmv/tracking/esm_region_tracker.h
@@ -0,0 +1,61 @@
+// Copyright (c) 2007, 2008, 2009, 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_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
+#define LIBMV_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
+
+#include "libmv/image/image.h"
+#include "libmv/tracking/region_tracker.h"
+
+namespace libmv {
+
+/*!
+    A translation-only Efficient Second-order Minimization ("ESM") tracker
+
+    Based on the paper "Real-time image-based trackiing of planes using
+    Efficient Second-order Minimization"
+*/
+struct EsmRegionTracker : public RegionTracker {
+  EsmRegionTracker()
+      : half_window_size(4),
+        max_iterations(16),
+        min_determinant(1e-6),
+        min_update_squared_distance(1e-4),
+        sigma(0.9) {}
+  
+  virtual ~EsmRegionTracker() {}
+
+  // Tracker interface.
+  virtual bool Track(const FloatImage &image1,
+                     const FloatImage &image2,
+                     double  x1, double  y1,
+                     double *x2, double *y2) const;
+
+  // No point in creating getters or setters.
+  int half_window_size;
+  int max_iterations;
+  double min_determinant;
+  double min_update_squared_distance;
+  double sigma;
+};
+
+}  // namespace libmv
+
+#endif  // LIBMV_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
diff --git a/extern/libmv/libmv/tracking/klt_region_tracker.cc b/extern/libmv/libmv/tracking/klt_region_tracker.cc
index 299077be155..78ce0be603c 100644
--- a/extern/libmv/libmv/tracking/klt_region_tracker.cc
+++ b/extern/libmv/libmv/tracking/klt_region_tracker.cc
@@ -63,24 +63,26 @@ static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
   }
 }
 
-// Solve the tracking equation
-//
-//   [gxx gxy] [dx] = [ex]
-//   [gxy gyy] [dy] = [ey]
-//
-// for dx and dy.  Borrowed from Stan Birchfield's KLT implementation.
-static bool SolveTrackingEquation(float gxx, float gxy, float gyy,
-                                  float ex, float ey,
-                                  float min_determinant,
-                                  float *dx, float *dy) {
-  float det = gxx * gyy - gxy * gxy;
-  if (det < min_determinant) {
-    *dx = 0;
-    *dy = 0;
+bool RegionIsInBounds(const FloatImage &image1,
+                      double x, double y,
+                      int half_window_size) {
+  // Check the minimum coordinates.
+  int min_x = floor(x) - half_window_size - 1;
+  int min_y = floor(y) - half_window_size - 1;
+  if (min_x < 0.0 ||
+      min_y < 0.0) {
+    return false;
+  }
+
+  // Check the maximum coordinates.
+  int max_x = ceil(x) + half_window_size + 1;
+  int max_y = ceil(y) + half_window_size + 1;
+  if (max_x > image1.cols() ||
+      max_y > image1.rows()) {
     return false;
   }
-  *dx = (gyy * ex - gxy * ey) / det;
-  *dy = (gxx * ey - gxy * ex) / det;
+
+  // Ok, we're good.
   return true;
 }
 
@@ -88,6 +90,12 @@ bool KltRegionTracker::Track(const FloatImage &image1,
                              const FloatImage &image2,
                              double  x1, double  y1,
                              double *x2, double *y2) const {
+  if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
+    LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
+       << ", hw=" << half_window_size << ".";
+    return false;
+  }
+
   Array3Df image_and_gradient1;
   Array3Df image_and_gradient2;
   BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
@@ -96,6 +104,13 @@ bool KltRegionTracker::Track(const FloatImage &image1,
   int i;
   float dx = 0, dy = 0;
   for (i = 0; i < max_iterations; ++i) {
+    // Check that the entire image patch is within the bounds of the images.
+    if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
+      LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
+         << ", hw=" << half_window_size << ".";
+      return false;
+    }
+
     // Compute gradient matrix and error vector.
     float gxx, gxy, gyy, ex, ey;
     ComputeTrackingEquation(image_and_gradient1,
@@ -105,19 +120,32 @@ bool KltRegionTracker::Track(const FloatImage &image1,
                             half_window_size,
                             &gxx, &gxy, &gyy, &ex, &ey);
 
-    // Solve the linear system for the best update to x2 and y2.
-    if (!SolveTrackingEquation(gxx, gxy, gyy, ex, ey, min_determinant,
-                               &dx, &dy)) {
-      // The determinant, which indicates the trackiness of the point, is too
-      // small, so fail out.
-      LG << "Determinant too small; failing tracking.";
-      return false;
-    }
+    // Solve the tracking equation
+    //
+    //   [gxx gxy] [dx] = [ex]
+    //   [gxy gyy] [dy] = [ey]
+    //
+    // for dx and dy.  Borrowed from Stan Birchfield's KLT implementation.
+    float determinant = gxx * gyy - gxy * gxy;
+    dx = (gyy * ex - gxy * ey) / determinant;
+    dy = (gxx * ey - gxy * ex) / determinant;
 
     // Update the position with the solved displacement.
     *x2 += dx;
     *y2 += dy;
 
+    // Check for the quality of the solution, but not until having already
+    // updated the position with our best estimate. The reason to do the update
+    // anyway is that the user already knows the position is bad, so we may as
+    // well try our best.
+    if (determinant < min_determinant) {
+      // The determinant, which indicates the trackiness of the point, is too
+      // small, so fail out.
+      LG << "Determinant " << determinant << " is too small; failing tracking.";
+      return false;
+    }
+    LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << dx << ", dy=" << dy << ", det=" << determinant;
+
     // If the update is small, then we probably found the target.
     if (dx * dx + dy * dy < min_update_squared_distance) {
       LG << "Successful track in " << i << " iterations.";
@@ -125,7 +153,7 @@ bool KltRegionTracker::Track(const FloatImage &image1,
     }
   }
   // Getting here means we hit max iterations, so tracking failed.
-  LG << "Too many iterations.";
+  LG << "Too many iterations; max is set to " << max_iterations << ".";
   return false;
 }
 
diff --git a/extern/libmv/libmv/tracking/lmicklt_region_tracker.cc b/extern/libmv/libmv/tracking/lmicklt_region_tracker.cc
new file mode 100644
index 00000000000..5ac96e66175
--- /dev/null
+++ b/extern/libmv/libmv/tracking/lmicklt_region_tracker.cc
@@ -0,0 +1,265 @@
+// Copyright (c) 2007, 2008, 2009, 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.
+
+#include "libmv/tracking/lmicklt_region_tracker.h"
+
+#include "libmv/logging/logging.h"
+#include "libmv/image/image.h"
+#include "libmv/image/convolve.h"
+#include "libmv/image/sample.h"
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+// TODO(keir): Reduce duplication between here and the other region trackers.
+bool RegionIsInBounds(const FloatImage &image1,
+                      double x, double y,
+                      int half_window_size) {
+  // Check the minimum coordinates.
+  int min_x = floor(x) - half_window_size - 1;
+  int min_y = floor(y) - half_window_size - 1;
+  if (min_x < 0.0 ||
+      min_y < 0.0) {
+    return false;
+  }
+
+  // Check the maximum coordinates.
+  int max_x = ceil(x) + half_window_size + 1;
+  int max_y = ceil(y) + half_window_size + 1;
+  if (max_x > image1.cols() ||
+      max_y > image1.rows()) {
+    return false;
+  }
+
+  // Ok, we're good.
+  return true;
+}
+
+// Sample a region centered at x,y in image with size extending by half_width
+// from x,y. Channels specifies the number of channels to sample from.
+void SamplePattern(const FloatImage &image,
+                   double x, double y,
+                   int half_width,
+                   int channels,
+                   FloatImage *sampled) {
+  sampled->Resize(2 * half_width + 1, 2 * half_width + 1, channels);
+  for (int r = -half_width; r <= half_width; ++r) {
+    for (int c = -half_width; c <= half_width; ++c) {
+      for (int i = 0; i < channels; ++i) {
+        (*sampled)(r + half_width, c + half_width, i) =
+            SampleLinear(image, y + r, x + c, i);
+      }
+    }
+  }
+}
+
+// Estimate "reasonable" error by computing autocorrelation for a small shift.
+double EstimateReasonableError(const FloatImage &image,
+                               double x, double y,
+                               int half_width) {
+  double error = 0.0;
+  for (int r = -half_width; r <= half_width; ++r) {
+    for (int c = -half_width; c <= half_width; ++c) {
+      double s = SampleLinear(image, y + r, x + c, 0);
+      double e1 = SampleLinear(image, y + r + 0.5, x + c, 0) - s;
+      double e2 = SampleLinear(image, y + r, x + c + 0.5, 0) - s;
+      error += e1*e1 + e2*e2;
+    }
+  }
+  return error / 2.0 * 8.0;
+}
+
+// This is implemented from "Lukas and Kanade 20 years on: Part 1. Page 42,
+// figure 14: the Levenberg-Marquardt-Inverse Compositional Algorithm".
+bool LmickltRegionTracker::Track(const FloatImage &image1,
+                             const FloatImage &image2,
+                             double  x1, double  y1,
+                             double *x2, double *y2) const {
+  if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
+    LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
+       << ", hw=" << half_window_size << ".";
+    return false;
+  }
+  
+  int width = 2 * half_window_size + 1;
+
+  // TODO(keir): Avoid recomputing gradients for e.g. the pyramid tracker.
+  Array3Df image_and_gradient1;
+  Array3Df image_and_gradient2;
+  BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
+
+  // TODO(keir): Avoid computing the derivative of image2.
+  BlurredImageAndDerivativesChannels(image2, sigma, &image_and_gradient2);
+
+  // Step -1: Resample the template (image1) since it is not pixel aligned.
+  //
+  // Take a sample of the gradient of the pattern area of image1 at the
+  // subpixel position x1, x2. This is reused for each iteration, so
+  // precomputing it saves time.
+  Array3Df image_and_gradient1_sampled;
+  SamplePattern(image_and_gradient1, x1, y1, half_window_size, 3,
+                &image_and_gradient1_sampled);
+
+  // Step 0: Initialize delta = 0.01.
+  // 
+  // Ignored for my "normal" LM loop.
+
+  double reasonable_error =
+      EstimateReasonableError(image1, x1, y1, half_window_size);
+
+  // Step 1: Warp I with W(x, p) to compute I(W(x; p).
+  //
+  // Use two images for accepting / rejecting updates.
+  int current_image = 0, new_image = 1;
+  Array3Df image2_sampled[2];
+  SamplePattern(image_and_gradient2, *x2, *y2, half_window_size, 1,
+                &image2_sampled[current_image]);
+
+  // Step 2: Compute the squared error I - J.
+  double error = 0;
+  for (int r = 0; r < width; ++r) {
+    for (int c = 0; c < width; ++c) {
+      double e = image_and_gradient1_sampled(r, c, 0) -
+                 image2_sampled[current_image](r, c, 0);
+      error += e*e;
+    }
+  }
+
+  // Step 3: Evaluate the gradient of the template.
+  //
+  // This is done above when sampling the template (step -1).
+
+  // Step 4: Evaluate the jacobian dw/dp at (x; 0).
+  //
+  // The jacobian between dx,dy and the warp is constant 2x2 identity, so it
+  // doesn't have to get computed. The Gauss-Newton Hessian matrix computation
+  // below would normally have to include a multiply by the jacobian.
+
+  // Step 5: Compute the steepest descent images of the template.
+  //
+  // Since the jacobian of the position w.r.t. the sampled template position is
+  // the identity, the steepest descent images are the same as the gradient.
+
+  // Step 6: Compute the Gauss-Newton Hessian for the template (image1).
+  //
+  // This could get rolled into the previous loop, but split it for now for
+  // clarity.
+  Mat2 H = Mat2::Zero();
+  for (int r = 0; r < width; ++r) {
+    for (int c = 0; c < width; ++c) {
+      Vec2 g(image_and_gradient1_sampled(r, c, 1),
+             image_and_gradient1_sampled(r, c, 2));
+      H += g * g.transpose();
+    }
+  }
+
+  double tau = 1e-3, eps1, eps2, eps3;
+  eps1 = eps2 = eps3 = 1e-15;
+
+  double mu = tau * std::max(H(0, 0), H(1, 1));
+  double nu = 2.0;
+
+  int i;
+  for (i = 0; i < max_iterations; ++i) {
+    // Check that the entire image patch is within the bounds of the images.
+    if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
+      LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
+         << ", hw=" << half_window_size << ".";
+      return false;
+    }
+
+    // Step 7: Compute z
+    Vec2 z = Vec2::Zero();
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        double e = image2_sampled[current_image](r, c, 0) -
+                   image_and_gradient1_sampled(r, c, 0);
+        z(0) += image_and_gradient1_sampled(r, c, 1) * e;
+        z(1) += image_and_gradient1_sampled(r, c, 2) * e;
+      }
+    }
+
+    // Step 8: Compute Hlm and (dx,dy)
+    Mat2 diag  = H.diagonal().asDiagonal();
+    diag *= mu;
+    Mat2 Hlm = H + diag;
+    Vec2 d = Hlm.lu().solve(z);
+
+    // TODO(keir): Use the usual LM termination and update criterion instead of
+    // this hacky version from the LK 20 years on paper.
+    LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1]
+       << ", mu=" << mu << ", nu=" << nu;
+
+    // Step 9: Update the warp; W(x; p) <-- W(x;p) compose W(x, dp)^-1
+    double new_x2 = *x2 - d[0];
+    double new_y2 = *y2 - d[1];
+
+    // Step 9.1: Sample the image at the new position.
+    SamplePattern(image_and_gradient2, new_x2, new_y2, half_window_size, 1,
+                  &image2_sampled[new_image]);
+
+    // Step 9.2: Compute the new error.
+    // TODO(keir): Eliminate duplication with above code.
+    double new_error = 0;
+    for (int r = 0; r < width; ++r) {
+      for (int c = 0; c < width; ++c) {
+        double e = image_and_gradient1_sampled(r, c, 0) -
+                   image2_sampled[new_image](r, c, 0);
+        new_error += e*e;
+      }
+    }
+    LG << "Old error: " << error << ", new error: " << new_error;
+
+    // If the step was accepted, then check for termination.
+    if (d.squaredNorm() < min_update_squared_distance) {
+      if (new_error > reasonable_error) {
+        LG << "Update size shrank but reasonable error ("
+           << reasonable_error << ") not achieved; failing.";
+        return false;
+      }
+      LG << "Successful track in " << i << " iterations.";
+      return true;
+    }
+
+    double rho = (error - new_error) / (d.transpose() * (mu * d + z));
+
+    // Step 10: Accept or reject step.
+    if (rho <= 0) {
+      // This was a bad step, so don't update.
+      mu *= nu;
+      nu *= 2;
+      LG << "Error increased, so reject update.";
+    } else {
+      // The step was good, so update.
+      *x2 = new_x2;
+      *y2 = new_y2;
+      std::swap(new_image, current_image);
+      error = new_error;
+
+      mu *= std::max(1/3., 1 - pow(2*rho - 1, 3));
+      nu = 2;
+    }
+  }
+  // Getting here means we hit max iterations, so tracking failed.
+  LG << "Too many iterations; max is set to " << max_iterations << ".";
+  return false;
+}
+
+}  // namespace libmv
diff --git a/extern/libmv/libmv/tracking/lmicklt_region_tracker.h b/extern/libmv/libmv/tracking/lmicklt_region_tracker.h
new file mode 100644
index 00000000000..744f2dfe277
--- /dev/null
+++ b/extern/libmv/libmv/tracking/lmicklt_region_tracker.h
@@ -0,0 +1,62 @@
+// Copyright (c) 2007, 2008, 2009, 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_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
+#define LIBMV_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
+
+#include "libmv/image/image.h"
+#include "libmv/tracking/region_tracker.h"
+
+namespace libmv {
+
+/*!
+    Levenberg-Marquardt inverse compositional region tracking
+
+    This tracker implements the Levenberg-Marquardt inverse compositional
+    region tracking algorithm as described in the paper "Lucas and Kanade 20
+    years on: A unifying framework."
+*/
+struct LmickltRegionTracker : public RegionTracker {
+  LmickltRegionTracker()
+      : half_window_size(4),
+        max_iterations(16),
+        min_determinant(1e-6),
+        min_update_squared_distance(1e-6),
+        sigma(0.9) {}
+  
+  virtual ~LmickltRegionTracker() {}
+
+  // Tracker interface.
+  virtual bool Track(const FloatImage &image1,
+                     const FloatImage &image2,
+                     double  x1, double  y1,
+                     double *x2, double *y2) const;
+
+  // No point in creating getters or setters.
+  int half_window_size;
+  int max_iterations;
+  double min_determinant;
+  double min_update_squared_distance;
+  double sigma;
+};
+
+}  // namespace libmv
+
+#endif  // LIBMV_REGION_TRACKING_LMICKLT_REGION_TRACKER_H_
diff --git a/extern/libmv/libmv/tracking/pyramid_region_tracker.cc b/extern/libmv/libmv/tracking/pyramid_region_tracker.cc
index 58f42b26d7c..c177f9c5a83 100644
--- a/extern/libmv/libmv/tracking/pyramid_region_tracker.cc
+++ b/extern/libmv/libmv/tracking/pyramid_region_tracker.cc
@@ -63,15 +63,32 @@ bool PyramidRegionTracker::Track(const FloatImage &image1,
     *x2 *= 2;
     *y2 *= 2;
 
+    // Save the previous best guess for this level, since tracking within this
+    // level might fail.
+    double x2_new = *x2;
+    double y2_new = *y2;
+
     // Track the point on this level with the base tracker.
-    bool succeeded = tracker_->Track(pyramid1[i], pyramid2[i], xx, yy, x2, y2);
+    LG << "Tracking on level " << i;
+    bool succeeded = tracker_->Track(pyramid1[i], pyramid2[i], xx, yy,
+                                     &x2_new, &y2_new);
+
+    if (!succeeded) {
+      if (i == 0) {
+        // Only fail on the highest-resolution level, because a failure on a
+        // coarse level does not mean failure at a lower level (consider
+        // out-of-bounds conditions).
+        LG << "Finest level of pyramid tracking failed; failing.";
+        return false;
+      }
 
-    if (i == 0 && !succeeded) {
-      // Only fail on the highest-resolution level, because a failure on a
-      // coarse level does not mean failure at a lower level (consider
-      // out-of-bounds conditions).
-      LG << "Finest level of pyramid tracking failed; failing.";
-      return false;
+      LG << "Failed to track at level " << i << "; restoring guess.";
+    } else {
+      // Only save the update if the track for this level succeeded. This is a
+      // bit of a hack; the jury remains out on whether this is better than
+      // re-using the previous failed-attempt.
+      *x2 = x2_new;
+      *y2 = y2_new;
     }
   }
   return true;
diff --git a/extern/libmv/libmv/tracking/trklt_region_tracker.cc b/extern/libmv/libmv/tracking/trklt_region_tracker.cc
index 94319fcb7d3..7e51787ebac 100644
--- a/extern/libmv/libmv/tracking/trklt_region_tracker.cc
+++ b/extern/libmv/libmv/tracking/trklt_region_tracker.cc
@@ -28,6 +28,8 @@
 
 namespace libmv {
 
+// TODO(keir): Switch this to use the smarter LM loop like in ESM.
+
 // Computes U and e from the Ud = e equation (number 14) from the paper.
 static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
                                     const Array3Df &image_and_gradient2,
@@ -79,10 +81,39 @@ static void ComputeTrackingEquation(const Array3Df &image_and_gradient1,
   *e = (A + lambda*Mat2f::Identity())*Di*(V - W) + 0.5*(S - R);
 }
 
+bool RegionIsInBounds(const FloatImage &image1,
+                      double x, double y,
+                      int half_window_size) {
+  // Check the minimum coordinates.
+  int min_x = floor(x) - half_window_size - 1;
+  int min_y = floor(y) - half_window_size - 1;
+  if (min_x < 0.0 ||
+      min_y < 0.0) {
+    return false;
+  }
+
+  // Check the maximum coordinates.
+  int max_x = ceil(x) + half_window_size + 1;
+  int max_y = ceil(y) + half_window_size + 1;
+  if (max_x > image1.cols() ||
+      max_y > image1.rows()) {
+    return false;
+  }
+
+  // Ok, we're good.
+  return true;
+}
+
 bool TrkltRegionTracker::Track(const FloatImage &image1,
                                const FloatImage &image2,
                                double  x1, double  y1,
                                double *x2, double *y2) const {
+  if (!RegionIsInBounds(image1, x1, y1, half_window_size)) {
+    LG << "Fell out of image1's window with x1=" << x1 << ", y1=" << y1
+       << ", hw=" << half_window_size << ".";
+    return false;
+  }
+
   Array3Df image_and_gradient1;
   Array3Df image_and_gradient2;
   BlurredImageAndDerivativesChannels(image1, sigma, &image_and_gradient1);
@@ -91,6 +122,13 @@ bool TrkltRegionTracker::Track(const FloatImage &image1,
   int i;
   Vec2f d = Vec2f::Zero();
   for (i = 0; i < max_iterations; ++i) {
+    // Check that the entire image patch is within the bounds of the images.
+    if (!RegionIsInBounds(image2, *x2, *y2, half_window_size)) {
+      LG << "Fell out of image2's window with x2=" << *x2 << ", y2=" << *y2
+         << ", hw=" << half_window_size << ".";
+      return false;
+    }
+
     // Compute gradient matrix and error vector.
     Mat2f U;
     Vec2f e;
@@ -120,6 +158,8 @@ bool TrkltRegionTracker::Track(const FloatImage &image1,
       LG << "Determinant " << determinant << " is too small; failing tracking.";
       return false;
     }
+    LG << "x=" << *x2 << ", y=" << *y2 << ", dx=" << d[0] << ", dy=" << d[1] << ", det=" << determinant;
+
 
     // If the update is small, then we probably found the target.
     if (d.squaredNorm() < min_update_squared_distance) {
diff --git a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp
index 1c1f0cb2627..7eb2dfac5d9 100644
--- a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp
+++ b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.cpp
@@ -158,6 +158,31 @@ namespace V3D
 
       switch (_mode)
       {
+         case FULL_BUNDLE_FOCAL_AND_RADIAL_K1:
+         {
+            // Focal length.
+            Ck[0][0] = xd[0];
+            Ck[1][0] = xd[1];
+
+            // For radial, k1 only.
+            Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
+            Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
+            Ck[0][1] = d_dk1k2[0][0];
+            Ck[1][1] = d_dk1k2[1][0];
+            break;
+         }
+         case FULL_BUNDLE_FOCAL_AND_RADIAL:
+         {
+            // Focal length.
+            Ck[0][0] = xd[0];
+            Ck[1][0] = xd[1];
+
+            // Radial k1 and k2.
+            Matrix2x2d dxd_dk1k2 = _distortion.derivativeWrtRadialParameters(xu);
+            Matrix2x2d d_dk1k2 = dp_dxd * dxd_dk1k2;
+            copyMatrixSlice(d_dk1k2, 0, 0, 2, 2, Ck, 0, 1);
+            break;
+         }
          case FULL_BUNDLE_RADIAL_TANGENTIAL:
          {
             Matrix2x2d dxd_dp1p2 = _distortion.derivativeWrtTangentialParameters(xu);
@@ -194,6 +219,21 @@ namespace V3D
    {
       switch (_mode)
       {
+         case FULL_BUNDLE_FOCAL_AND_RADIAL_K1:
+         {
+            _K[0][0] += deltaC[0];
+            _K[1][1] = _cachedAspectRatio * _K[0][0];
+            _distortion.k1 += deltaC[1];
+            break;
+         }
+         case FULL_BUNDLE_FOCAL_AND_RADIAL:
+         {
+            _K[0][0] += deltaC[0];
+            _K[1][1] = _cachedAspectRatio * _K[0][0];
+            _distortion.k1 += deltaC[1];
+            _distortion.k2 += deltaC[2];
+            break;
+         }
          case FULL_BUNDLE_RADIAL_TANGENTIAL:
          {
             _distortion.p1 += deltaC[5];
diff --git a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h
index 076a9e64346..339e174ed9e 100644
--- a/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h
+++ b/extern/libmv/third_party/ssba/Geometry/v3d_metricbundle.h
@@ -166,7 +166,9 @@ namespace V3D
       FULL_BUNDLE_FOCAL_LENGTH = 1,      // f
       FULL_BUNDLE_FOCAL_LENGTH_PP = 2,   // f, cx, cy
       FULL_BUNDLE_RADIAL = 3,            // f, cx, cy, k1, k2
-      FULL_BUNDLE_RADIAL_TANGENTIAL = 4  // f, cx, cy, k1, k2, p1, p2
+      FULL_BUNDLE_RADIAL_TANGENTIAL = 4, // f, cx, cy, k1, k2, p1, p2
+      FULL_BUNDLE_FOCAL_AND_RADIAL_K1 = 5,      // f, k1
+      FULL_BUNDLE_FOCAL_AND_RADIAL = 6,      // f, k1, k2
    };
 
    struct CommonInternalsMetricBundleOptimizer : public MetricBundleOptimizerBase
@@ -175,11 +177,13 @@ namespace V3D
          {
             switch (mode)
             {
-               case FULL_BUNDLE_METRIC:            return 0;
-               case FULL_BUNDLE_FOCAL_LENGTH:      return 1;
-               case FULL_BUNDLE_FOCAL_LENGTH_PP:   return 3;
-               case FULL_BUNDLE_RADIAL:            return 5;
-               case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7;
+               case FULL_BUNDLE_METRIC:              return 0;
+               case FULL_BUNDLE_FOCAL_LENGTH:        return 1;
+               case FULL_BUNDLE_FOCAL_LENGTH_PP:     return 3;
+               case FULL_BUNDLE_RADIAL:              return 5;
+               case FULL_BUNDLE_RADIAL_TANGENTIAL:   return 7;
+               case FULL_BUNDLE_FOCAL_AND_RADIAL_K1: return 2;
+               case FULL_BUNDLE_FOCAL_AND_RADIAL:    return 3;
             }
             return 0;
          }
@@ -266,11 +270,13 @@ namespace V3D
          {
             switch (mode)
             {
-               case FULL_BUNDLE_METRIC:            return 0;
-               case FULL_BUNDLE_FOCAL_LENGTH:      return 1;
-               case FULL_BUNDLE_FOCAL_LENGTH_PP:   return 3;
-               case FULL_BUNDLE_RADIAL:            return 5;
-               case FULL_BUNDLE_RADIAL_TANGENTIAL: return 7;
+               case FULL_BUNDLE_METRIC:              return 0;
+               case FULL_BUNDLE_FOCAL_LENGTH:        return 1;
+               case FULL_BUNDLE_FOCAL_LENGTH_PP:     return 3;
+               case FULL_BUNDLE_RADIAL:              return 5;
+               case FULL_BUNDLE_RADIAL_TANGENTIAL:   return 7;
+               case FULL_BUNDLE_FOCAL_AND_RADIAL_K1: return 2;
+               case FULL_BUNDLE_FOCAL_AND_RADIAL:    return 3;
             }
             return 0;
          }