Add a planar tracking implementation to libmv
authorKeir Mierle <mierle@gmail.com>
Sun, 13 May 2012 23:08:56 +0000 (23:08 +0000)
committerKeir Mierle <mierle@gmail.com>
Sun, 13 May 2012 23:08:56 +0000 (23:08 +0000)
This adds a new planar tracking implementation to libmv. The
tracker is based on Ceres[1], the new nonlinear minimizer that
myself and Sameer released from Google as open source. Since
the motion model is more involved, the interface is
different than the RegionTracker interface used previously
in Blender.

The start of a C API in libmv-capi.{cpp,h} is also included.

The ESM tracker, also known as the KLT tracker in the UI, is
temporarily changed to use the new Ceres-based planar
tracker in translation-only mode. Currently it is a bit
slower than ESM and also doesn't have all the bells and
whistles implemented. Those will come soon. Longer term,
both trackers will remain since Ceres is unlikely to be as
fast as ESM for pure translation solving, due to its
generality.

The next step is to implement a new tracking UI. The current
UI assumes a translational motion model; the new one must
support arbitrary perspective transforms of the pattern
regions.

[1] http://code.google.com/p/ceres-solver

extern/libmv/CMakeLists.txt
extern/libmv/libmv-capi.cpp
extern/libmv/libmv-capi.h
extern/libmv/libmv/multiview/homography.cc [new file with mode: 0644]
extern/libmv/libmv/multiview/homography.h [new file with mode: 0644]
extern/libmv/libmv/multiview/homography_parameterization.h [new file with mode: 0644]
extern/libmv/libmv/tracking/esm_region_tracker.cc
extern/libmv/libmv/tracking/track_region.cc [new file with mode: 0644]
extern/libmv/libmv/tracking/track_region.h [new file with mode: 0644]

index cf0ad1102e038a3e874657f8b7fa8ca2464bd7eb..60cd84d89d482ee36d0095a667a56967964a58be 100644 (file)
@@ -62,6 +62,7 @@ set(SRC
        libmv/multiview/fundamental.cc
        libmv/multiview/projection.cc
        libmv/multiview/triangulation.cc
+       libmv/multiview/homography.cc
        libmv/numeric/numeric.cc
        libmv/numeric/poly.cc
        libmv/simple_pipeline/bundle.cc
@@ -84,6 +85,7 @@ set(SRC
        libmv/tracking/pyramid_region_tracker.cc
        libmv/tracking/retrack_region_tracker.cc
        libmv/tracking/trklt_region_tracker.cc
+       libmv/tracking/track_region.cc
 
        third_party/fast/fast_10.c
        third_party/fast/fast_11.c
index 6c20d76eeac900626c1d25437e8e874edf880dc5..ddf69ce5e28fe329cd9b01f6c88a3eb1da654cf9 100644 (file)
@@ -45,6 +45,7 @@
 #include "libmv/tracking/trklt_region_tracker.h"
 #include "libmv/tracking/lmicklt_region_tracker.h"
 #include "libmv/tracking/pyramid_region_tracker.h"
+#include "libmv/tracking/track_region.h"
 
 #include "libmv/simple_pipeline/callbacks.h"
 #include "libmv/simple_pipeline/tracks.h"
@@ -97,7 +98,7 @@ void libmv_initLogging(const char *argv0)
 void libmv_startDebugLogging(void)
 {
        google::SetCommandLineOption("logtostderr", "1");
-       google::SetCommandLineOption("v", "0");
+       google::SetCommandLineOption("v", "2");
        google::SetCommandLineOption("stderrthreshold", "1");
        google::SetCommandLineOption("minloglevel", "0");
        V3D::optimizerVerbosenessLevel = 1;
@@ -329,6 +330,71 @@ void libmv_regionTrackerDestroy(libmv_RegionTracker *libmv_tracker)
        delete region_tracker;
 }
 
+/* ************ Planar tracker ************ */
+
+/* TrackRegion (new planar tracker) */
+int libmv_trackRegion(const struct libmv_trackRegionOptions *options,
+                       const float *image1, const float *image2,
+                       int width, int height, 
+                       const double *x1, const double *y1,
+                       struct libmv_trackRegionResult *result,
+                       double *x2, double *y2) {
+  double xx1[4], yy1[4];
+  double xx2[4], yy2[4];
+
+  // Convert to doubles for the libmv api.
+  for (int i = 0; i < 4; ++i) {
+    xx1[i] = x1[i];
+    yy1[i] = y1[i];
+    xx2[i] = x2[i];
+    yy2[i] = y2[i];
+  }
+
+  libmv::TrackRegionOptions track_region_options;
+  switch (options->motion_model) {
+#define LIBMV_CONVERT(the_model) \
+    case libmv::TrackRegionOptions::the_model: \
+               track_region_options.mode = libmv::TrackRegionOptions::the_model; \
+               break;
+    LIBMV_CONVERT(TRANSLATION)
+    LIBMV_CONVERT(TRANSLATION_ROTATION)
+    LIBMV_CONVERT(TRANSLATION_SCALE)
+    LIBMV_CONVERT(TRANSLATION_ROTATION_SCALE)
+    LIBMV_CONVERT(AFFINE)
+    LIBMV_CONVERT(HOMOGRAPHY)
+#undef LIBMV_CONVERT
+  }
+  track_region_options.num_samples_x = options->num_samples_x;
+  track_region_options.num_samples_y = options->num_samples_y;
+  track_region_options.minimum_correlation = options->minimum_correlation;
+  track_region_options.max_iterations = options->num_iterations;
+  track_region_options.sigma = options->sigma;
+  
+  // Convert from raw float buffers to libmv's FloatImage.
+  libmv::FloatImage old_patch, new_patch;
+  floatBufToImage(image1, width, height, &old_patch);
+  floatBufToImage(image2, width, height, &new_patch);
+
+  libmv::TrackRegionResult track_region_result;
+  libmv::TrackRegion(old_patch, new_patch, xx1, yy1, track_region_options, xx2, yy2, &track_region_result);
+
+  // Convert to floats for the blender api.
+  for (int i = 0; i < 4; ++i) {
+    x2[i] = xx2[i];
+    y2[i] = yy2[i];
+  }
+
+  // TODO(keir): Update the termination string with failure details.
+  if (track_region_result.termination == libmv::TrackRegionResult::PARAMETER_TOLERANCE ||
+      track_region_result.termination == libmv::TrackRegionResult::FUNCTION_TOLERANCE  ||
+      track_region_result.termination == libmv::TrackRegionResult::GRADIENT_TOLERANCE  ||
+      track_region_result.termination == libmv::TrackRegionResult::NO_CONVERGENCE      ||
+      track_region_result.termination == libmv::TrackRegionResult::INSUFFICIENT_CORRELATION) {
+    return true;
+  }
+  return false;
+}
+
 /* ************ Tracks ************ */
 
 libmv_Tracks *libmv_tracksNew(void)
index bccc47068322741badca2c1d68a1c60f1827217f..d7d132d4f21ac0081c31299f13a84df11b192660 100644 (file)
@@ -50,6 +50,27 @@ int libmv_regionTrackerTrack(struct libmv_RegionTracker *libmv_tracker, const fl
                        int width, int height, double  x1, double  y1, double *x2, double *y2);
 void libmv_regionTrackerDestroy(struct libmv_RegionTracker *libmv_tracker);
 
+/* TrackRegion (new planar tracker) */
+struct libmv_trackRegionOptions {
+  int motion_model;
+  int num_samples_x;
+  int num_samples_y;
+  int num_iterations;
+  double minimum_correlation;
+  double sigma;
+};
+struct libmv_trackRegionResult {
+  int termination;
+  const char *termination_reason;
+  double correlation;
+};
+int libmv_trackRegion(const struct libmv_trackRegionOptions *options,
+                      const float *image1, const float *image2,
+                      int width, int height, 
+                      const double *x1, const double *y1,
+                      struct libmv_trackRegionResult *result,
+                      double *x2, double *y2);
+
 /* Tracks */
 struct libmv_Tracks *libmv_tracksNew(void);
 void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track, double x, double y);
diff --git a/extern/libmv/libmv/multiview/homography.cc b/extern/libmv/libmv/multiview/homography.cc
new file mode 100644 (file)
index 0000000..366392f
--- /dev/null
@@ -0,0 +1,267 @@
+// Copyright (c) 2008, 2009 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/logging/logging.h"
+#include "libmv/multiview/homography.h"
+#include "libmv/multiview/homography_parameterization.h"
+
+namespace libmv {
+/** 2D Homography transformation estimation in the case that points are in 
+ * euclidean coordinates.
+ *
+ * x = H y
+ * x and y vector must have the same direction, we could write
+ * crossproduct(|x|, * H * |y| ) = |0|
+ *
+ * | 0 -1  x2|   |a b c|   |y1|    |0|
+ * | 1  0 -x1| * |d e f| * |y2| =  |0|
+ * |-x2  x1 0|   |g h 1|   |1 |    |0|
+ *
+ * That gives :
+ *
+ * (-d+x2*g)*y1    + (-e+x2*h)*y2 + -f+x2          |0|
+ * (a-x1*g)*y1     + (b-x1*h)*y2  + c-x1         = |0|
+ * (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f  |0|
+ */
+bool Homography2DFromCorrespondencesLinearEuc(
+    const Mat &x1,
+    const Mat &x2,
+    Mat3 *H,
+    double expected_precision) {
+  assert(2 == x1.rows());
+  assert(4 <= x1.cols());
+  assert(x1.rows() == x2.rows());
+  assert(x1.cols() == x2.cols());
+
+  int n = x1.cols();
+  MatX8 L = Mat::Zero(n * 3, 8);
+  Mat b = Mat::Zero(n * 3, 1);
+  for (int i = 0; i < n; ++i) {
+    int j = 3 * i;
+    L(j, 0) =  x1(0, i);            // a
+    L(j, 1) =  x1(1, i);            // b
+    L(j, 2) =  1.0;                 // c
+    L(j, 6) = -x2(0, i) * x1(0, i); // g
+    L(j, 7) = -x2(0, i) * x1(1, i); // h
+    b(j, 0) =  x2(0, i);            // i
+
+    ++j;
+    L(j, 3) =  x1(0, i);            // d
+    L(j, 4) =  x1(1, i);            // e
+    L(j, 5) =  1.0;                 // f
+    L(j, 6) = -x2(1, i) * x1(0, i); // g
+    L(j, 7) = -x2(1, i) * x1(1, i); // h
+    b(j, 0) =  x2(1, i);            // i
+    
+    // This ensures better stability
+    // TODO(julien) make a lite version without this 3rd set
+    ++j;
+    L(j, 0) =  x2(1, i) * x1(0, i); // a
+    L(j, 1) =  x2(1, i) * x1(1, i); // b
+    L(j, 2) =  x2(1, i);            // c
+    L(j, 3) = -x2(0, i) * x1(0, i); // d
+    L(j, 4) = -x2(0, i) * x1(1, i); // e
+    L(j, 5) = -x2(0, i) ;           // f
+  }
+  // Solve Lx=B
+  Vec h = L.fullPivLu().solve(b);
+  Homography2DNormalizedParameterization<double>::To(h, H);
+  if ((L * h).isApprox(b, expected_precision))  {
+    return true;
+  } else {
+    return false;
+  }
+}
+
+/** 2D Homography transformation estimation in the case that points are in 
+ * homogeneous coordinates.
+ *
+ * | 0 -x3  x2|   |a b c|   |y1|   -x3*d+x2*g -x3*e+x2*h -x3*f+x2*1    |y1|   (-x3*d+x2*g)*y1 (-x3*e+x2*h)*y2 (-x3*f+x2*1)*y3   |0|
+ * | x3  0 -x1| * |d e f| * |y2| =  x3*a-x1*g  x3*b-x1*h  x3*c-x1*1  * |y2| =  (x3*a-x1*g)*y1  (x3*b-x1*h)*y2  (x3*c-x1*1)*y3 = |0|
+ * |-x2  x1  0|   |g h 1|   |y3|   -x2*a+x1*d -x2*b+x1*e -x2*c+x1*f    |y3|   (-x2*a+x1*d)*y1 (-x2*b+x1*e)*y2 (-x2*c+x1*f)*y3   |0|
+ * X = |a b c d e f g h|^t
+ */
+bool Homography2DFromCorrespondencesLinear(const Mat &x1,
+                                           const Mat &x2,
+                                           Mat3 *H,
+                                           double expected_precision) {
+  if (x1.rows() == 2) {
+    return Homography2DFromCorrespondencesLinearEuc(x1, x2, H, 
+                                                    expected_precision);
+  }
+  assert(3 == x1.rows());
+  assert(4 <= x1.cols());
+  assert(x1.rows() == x2.rows());
+  assert(x1.cols() == x2.cols());
+
+  const int x = 0;
+  const int y = 1;
+  const int w = 2;
+  int n = x1.cols();
+  MatX8 L = Mat::Zero(n * 3, 8);
+  Mat b = Mat::Zero(n * 3, 1);
+  for (int i = 0; i < n; ++i) {
+    int j = 3 * i;
+    L(j, 0) =  x2(w, i) * x1(x, i);//a
+    L(j, 1) =  x2(w, i) * x1(y, i);//b
+    L(j, 2) =  x2(w, i) * x1(w, i);//c
+    L(j, 6) = -x2(x, i) * x1(x, i);//g
+    L(j, 7) = -x2(x, i) * x1(y, i);//h
+    b(j, 0) =  x2(x, i) * x1(w, i);
+
+    ++j;
+    L(j, 3) =  x2(w, i) * x1(x, i);//d
+    L(j, 4) =  x2(w, i) * x1(y, i);//e
+    L(j, 5) =  x2(w, i) * x1(w, i);//f
+    L(j, 6) = -x2(y, i) * x1(x, i);//g
+    L(j, 7) = -x2(y, i) * x1(y, i);//h
+    b(j, 0) =  x2(y, i) * x1(w, i);
+
+    // This ensures better stability
+    ++j;
+    L(j, 0) =  x2(y, i) * x1(x, i);//a
+    L(j, 1) =  x2(y, i) * x1(y, i);//b
+    L(j, 2) =  x2(y, i) * x1(w, i);//c
+    L(j, 3) = -x2(x, i) * x1(x, i);//d
+    L(j, 4) = -x2(x, i) * x1(y, i);//e
+    L(j, 5) = -x2(x, i) * x1(w, i);//f
+  }
+  // Solve Lx=B
+  Vec h = L.fullPivLu().solve(b);
+  if ((L * h).isApprox(b, expected_precision))  {
+    Homography2DNormalizedParameterization<double>::To(h, H);
+    return true;
+  } else {
+    return false;
+  }
+}
+/**
+ * x2 ~ A * x1
+ * x2^t * Hi * A *x1 = 0
+ * H1 =              H2 =               H3 = 
+ * | 0 0 0 1|     |-x2w|  |0 0 0 0|      |  0 |  | 0 0 1 0|     |-x2z|
+ * | 0 0 0 0|  -> |  0 |  |0 0 1 0|   -> |-x2z|  | 0 0 0 0|  -> |  0 |
+ * | 0 0 0 0|     |  0 |  |0-1 0 0|      | x2y|  |-1 0 0 0|     | x2x|
+ * |-1 0 0 0|     | x2x|  |0 0 0 0|      |  0 |  | 0 0 0 0|     |  0 |
+ * H4 =              H5 =               H6 = 
+ *  |0 0 0 0|     |  0 |  | 0 1 0 0|     |-x2y|   |0 0 0 0|     |  0 |
+ *  |0 0 0 1|  -> |-x2w|  |-1 0 0 0|  -> | x2x|   |0 0 0 0|  -> |  0 |
+ *  |0 0 0 0|     |  0 |  | 0 0 0 0|     |  0 |   |0 0 0 1|     |-x2w|
+ *  |0-1 0 0|     | x2y|  | 0 0 0 0|     |  0 |   |0 0-1 0|     | x2z|
+ *     |a b c d| 
+ * A = |e f g h| 
+ *     |i j k l|
+ *     |m n o 1| 
+ *     
+ * x2^t * H1 * A *x1 = (-x2w*a +x2x*m )*x1x + (-x2w*b +x2x*n )*x1y + (-x2w*c +x2x*o )*x1z + (-x2w*d +x2x*1 )*x1w = 0
+ * x2^t * H2 * A *x1 = (-x2z*e +x2y*i )*x1x + (-x2z*f +x2y*j )*x1y + (-x2z*g +x2y*k )*x1z + (-x2z*h +x2y*l )*x1w = 0
+ * x2^t * H3 * A *x1 = (-x2z*a +x2x*i )*x1x + (-x2z*b +x2x*j )*x1y + (-x2z*c +x2x*k )*x1z + (-x2z*d +x2x*l )*x1w = 0
+ * x2^t * H4 * A *x1 = (-x2w*e +x2y*m )*x1x + (-x2w*f +x2y*n )*x1y + (-x2w*g +x2y*o )*x1z + (-x2w*h +x2y*1 )*x1w = 0
+ * x2^t * H5 * A *x1 = (-x2y*a +x2x*e )*x1x + (-x2y*b +x2x*f )*x1y + (-x2y*c +x2x*g )*x1z + (-x2y*d +x2x*h )*x1w = 0
+ * x2^t * H6 * A *x1 = (-x2w*i +x2z*m )*x1x + (-x2w*j +x2z*n )*x1y + (-x2w*k +x2z*o )*x1z + (-x2w*l +x2z*1 )*x1w = 0
+ *
+ * X = |a b c d e f g h i j k l m n o|^t
+*/
+bool Homography3DFromCorrespondencesLinear(const Mat &x1,
+                                           const Mat &x2,
+                                           Mat4 *H,
+                                           double expected_precision) {
+  assert(4 == x1.rows());
+  assert(5 <= x1.cols());
+  assert(x1.rows() == x2.rows());
+  assert(x1.cols() == x2.cols());
+  const int x = 0;
+  const int y = 1;
+  const int z = 2;
+  const int w = 3;
+  int n = x1.cols();
+  MatX15 L = Mat::Zero(n * 6, 15);
+  Mat b = Mat::Zero(n * 6, 1);
+  for (int i = 0; i < n; ++i) {
+    int j = 6 * i;
+    L(j, 0) = -x2(w, i) * x1(x, i);//a
+    L(j, 1) = -x2(w, i) * x1(y, i);//b
+    L(j, 2) = -x2(w, i) * x1(z, i);//c
+    L(j, 3) = -x2(w, i) * x1(w, i);//d
+    L(j,12) =  x2(x, i) * x1(x, i);//m
+    L(j,13) =  x2(x, i) * x1(y, i);//n
+    L(j,14) =  x2(x, i) * x1(z, i);//o
+    b(j, 0) = -x2(x, i) * x1(w, i);
+    
+    ++j;
+    L(j, 4) = -x2(z, i) * x1(x, i);//e
+    L(j, 5) = -x2(z, i) * x1(y, i);//f
+    L(j, 6) = -x2(z, i) * x1(z, i);//g
+    L(j, 7) = -x2(z, i) * x1(w, i);//h
+    L(j, 8) =  x2(y, i) * x1(x, i);//i
+    L(j, 9) =  x2(y, i) * x1(y, i);//j
+    L(j,10) =  x2(y, i) * x1(z, i);//k
+    L(j,11) =  x2(y, i) * x1(w, i);//l
+    
+    ++j;
+    L(j, 0) = -x2(z, i) * x1(x, i);//a
+    L(j, 1) = -x2(z, i) * x1(y, i);//b
+    L(j, 2) = -x2(z, i) * x1(z, i);//c
+    L(j, 3) = -x2(z, i) * x1(w, i);//d
+    L(j, 8) =  x2(x, i) * x1(x, i);//i
+    L(j, 9) =  x2(x, i) * x1(y, i);//j
+    L(j,10) =  x2(x, i) * x1(z, i);//k
+    L(j,11) =  x2(x, i) * x1(w, i);//l
+    
+    ++j;
+    L(j, 4) = -x2(w, i) * x1(x, i);//e
+    L(j, 5) = -x2(w, i) * x1(y, i);//f
+    L(j, 6) = -x2(w, i) * x1(z, i);//g
+    L(j, 7) = -x2(w, i) * x1(w, i);//h
+    L(j,12) =  x2(y, i) * x1(x, i);//m
+    L(j,13) =  x2(y, i) * x1(y, i);//n
+    L(j,14) =  x2(y, i) * x1(z, i);//o
+    b(j, 0) = -x2(y, i) * x1(w, i);
+    
+    ++j;
+    L(j, 0) = -x2(y, i) * x1(x, i);//a
+    L(j, 1) = -x2(y, i) * x1(y, i);//b
+    L(j, 2) = -x2(y, i) * x1(z, i);//c
+    L(j, 3) = -x2(y, i) * x1(w, i);//d
+    L(j, 4) =  x2(x, i) * x1(x, i);//e
+    L(j, 5) =  x2(x, i) * x1(y, i);//f
+    L(j, 6) =  x2(x, i) * x1(z, i);//g
+    L(j, 7) =  x2(x, i) * x1(w, i);//h
+    
+    ++j;
+    L(j, 8) = -x2(w, i) * x1(x, i);//i
+    L(j, 9) = -x2(w, i) * x1(y, i);//j
+    L(j,10) = -x2(w, i) * x1(z, i);//k
+    L(j,11) = -x2(w, i) * x1(w, i);//l
+    L(j,12) =  x2(z, i) * x1(x, i);//m
+    L(j,13) =  x2(z, i) * x1(y, i);//n
+    L(j,14) =  x2(z, i) * x1(z, i);//o
+    b(j, 0) = -x2(z, i) * x1(w, i);
+  }
+  // Solve Lx=B
+  Vec h = L.fullPivLu().solve(b);
+  if ((L * h).isApprox(b, expected_precision))  {
+    Homography3DNormalizedParameterization<double>::To(h, H);
+    return true;
+  } else {
+    return false;
+  }
+}
+}  // namespace libmv
diff --git a/extern/libmv/libmv/multiview/homography.h b/extern/libmv/libmv/multiview/homography.h
new file mode 100644 (file)
index 0000000..786fd3d
--- /dev/null
@@ -0,0 +1,84 @@
+// Copyright (c) 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_MULTIVIEW_HOMOGRAPHY_H_
+#define LIBMV_MULTIVIEW_HOMOGRAPHY_H_
+
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+/**
+ * 2D homography transformation estimation.
+ * 
+ * This function estimates the homography transformation from a list of 2D
+ * correspondences which represents either:
+ *
+ * - 3D points on a plane, with a general moving camera.
+ * - 3D points with a rotating camera (pure rotation).
+ * - 3D points + different planar projections
+ * 
+ * \param x1 The first 2xN or 3xN matrix of euclidean or homogeneous points.
+ * \param x2 The second 2xN or 3xN matrix of euclidean or homogeneous points.
+ * \param  H The 3x3 homography transformation matrix (8 dof) such that
+ *               x2 = H * x1   with       |a b c| 
+ *                                    H = |d e f|
+ *                                        |g h 1| 
+ * \param expected_precision The expected precision in order for instance 
+ *                           to accept almost homography matrices.
+ *
+ * \return True if the transformation estimation has succeeded.
+ * \note There must be at least 4 non-colinear points.
+ */
+bool Homography2DFromCorrespondencesLinear(const Mat &x1,
+                                           const Mat &x2,
+                                           Mat3 *H,
+                                           double expected_precision = 
+                                             EigenDouble::dummy_precision());
+
+/**
+ * 3D Homography transformation estimation.
+ * 
+ * This function can be used in order to estimate the homography transformation
+ * from a list of 3D correspondences.
+ *
+ * \param[in] x1 The first 4xN matrix of homogeneous points
+ * \param[in] x2 The second 4xN matrix of homogeneous points
+ * \param[out] H The 4x4 homography transformation matrix (15 dof) such that
+ *               x2 = H * x1   with       |a b c d| 
+ *                                    H = |e f g h|
+ *                                        |i j k l|
+ *                                        |m n o 1| 
+ * \param[in] expected_precision The expected precision in order for instance 
+ *        to accept almost homography matrices.
+ * 
+ * \return true if the transformation estimation has succeeded
+ * 
+ * \note Need at least 5 non coplanar points 
+ * \note Points coordinates must be in homogeneous coordinates
+ */
+bool Homography3DFromCorrespondencesLinear(const Mat &x1,
+                                           const Mat &x2,
+                                           Mat4 *H,
+                                           double expected_precision = 
+                                             EigenDouble::dummy_precision());
+} // namespace libmv
+
+#endif  // LIBMV_MULTIVIEW_HOMOGRAPHY_H_
diff --git a/extern/libmv/libmv/multiview/homography_parameterization.h b/extern/libmv/libmv/multiview/homography_parameterization.h
new file mode 100644 (file)
index 0000000..b31642e
--- /dev/null
@@ -0,0 +1,91 @@
+// Copyright (c) 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_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_
+#define LIBMV_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_
+
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+/** A parameterization of the 2D homography matrix that uses 8 parameters so 
+ * that the matrix is normalized (H(2,2) == 1).
+ * The homography matrix H is built from a list of 8 parameters (a, b,...g, h)
+ * as follows
+ *         |a b c| 
+ *     H = |d e f|
+ *         |g h 1| 
+ */
+template<typename T = double>
+class Homography2DNormalizedParameterization {
+ public:
+  typedef Eigen::Matrix<T, 8, 1> Parameters;     // a, b, ... g, h
+  typedef Eigen::Matrix<T, 3, 3> Parameterized;  // H
+
+  /// Convert from the 8 parameters to a H matrix.
+  static void To(const Parameters &p, Parameterized *h) {    
+    *h << p(0), p(1), p(2),
+          p(3), p(4), p(5),
+          p(6), p(7), 1.0;
+  }
+
+  /// Convert from a H matrix to the 8 parameters.
+  static void From(const Parameterized &h, Parameters *p) {
+    *p << h(0, 0), h(0, 1), h(0, 2),
+          h(1, 0), h(1, 1), h(1, 2),
+          h(2, 0), h(2, 1);
+  }
+};
+
+/** A parameterization of the 2D homography matrix that uses 15 parameters so 
+ * that the matrix is normalized (H(3,3) == 1).
+ * The homography matrix H is built from a list of 15 parameters (a, b,...n, o)
+ * as follows
+ *          |a b c d| 
+ *      H = |e f g h|
+ *          |i j k l|
+ *          |m n o 1| 
+ */
+template<typename T = double>
+class Homography3DNormalizedParameterization {
+ public:
+  typedef Eigen::Matrix<T, 15, 1> Parameters;     // a, b, ... n, o
+  typedef Eigen::Matrix<T, 4, 4>  Parameterized;  // H
+
+  /// Convert from the 15 parameters to a H matrix.
+  static void To(const Parameters &p, Parameterized *h) {   
+    *h << p(0), p(1), p(2), p(3),
+          p(4), p(5), p(6), p(7),
+          p(8), p(9), p(10), p(11),
+          p(12), p(13), p(14), 1.0;
+  }
+
+  /// Convert from a H matrix to the 15 parameters.
+  static void From(const Parameterized &h, Parameters *p) {
+    *p << h(0, 0), h(0, 1), h(0, 2), h(0, 3),
+          h(1, 0), h(1, 1), h(1, 2), h(1, 3),
+          h(2, 0), h(2, 1), h(2, 2), h(2, 3),
+          h(3, 0), h(3, 1), h(3, 2);
+  }
+};
+
+} // namespace libmv
+
+#endif  // LIBMV_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_
index 221fa4d081b2b914c0e39d7d905afa82afeda91f..2491b15815436a882e02c6558b4b5582559d2f4e 100644 (file)
@@ -29,6 +29,7 @@
 #include "libmv/image/correlation.h"
 #include "libmv/image/sample.h"
 #include "libmv/numeric/numeric.h"
+#include "libmv/tracking/track_region.h"
 
 namespace libmv {
 
@@ -72,6 +73,45 @@ bool EsmRegionTracker::Track(const FloatImage &image1,
     return false;
   }
   
+  // XXX
+  // TODO(keir): Delete the block between the XXX's once the planar tracker is
+  // integrated into blender.
+  //
+  // For now, to test, replace the ESM tracker with the Ceres tracker in
+  // translation mode. In the future, this should get removed and alloed to
+  // co-exist, since Ceres is not as fast as the ESM implementation since it
+  // specializes for translation.
+  double xx1[4], yy1[4];
+  double xx2[4], yy2[4];
+  // Clockwise winding, starting from the "origin" (top-left).
+  xx1[0] = xx2[0] = x1 - half_window_size;
+  yy1[0] = yy2[0] = y1 - half_window_size;
+
+  xx1[1] = xx2[1] = x1 + half_window_size;
+  yy1[1] = yy2[1] = y1 - half_window_size;
+
+  xx1[2] = xx2[2] = x1 + half_window_size;
+  yy1[2] = yy2[2] = y1 + half_window_size;
+
+  xx1[3] = xx2[3] = x1 - half_window_size;
+  yy1[3] = yy2[3] = y1 + half_window_size;
+
+  TrackRegionOptions options;
+  options.mode = TrackRegionOptions::TRANSLATION;
+  options.num_samples_x = 2 * half_window_size + 1;
+  options.num_samples_y = 2 * half_window_size + 1;
+  options.max_iterations = 20;
+  options.sigma = sigma;
+  
+  TrackRegionResult result;
+  TrackRegion(image1, image2, xx1, yy1, options, xx2, yy2, &result);
+
+  *x2 = xx2[0] + half_window_size;
+  *y2 = yy2[0] + half_window_size;
+
+  return true;
+
+  // XXX
   int width = 2 * half_window_size + 1;
 
   // TODO(keir): Avoid recomputing gradients for e.g. the pyramid tracker.
diff --git a/extern/libmv/libmv/tracking/track_region.cc b/extern/libmv/libmv/tracking/track_region.cc
new file mode 100644 (file)
index 0000000..5a46c66
--- /dev/null
@@ -0,0 +1,651 @@
+// 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.
+//
+// Author: mierle@google.com (Keir Mierle)
+
+// Necessary for M_E when building with MSVC.
+#define _USE_MATH_DEFINES
+
+#include "libmv/tracking/track_region.h"
+
+#include <Eigen/SVD>
+#include <Eigen/QR>
+#include <iostream>
+#include "ceres/ceres.h"
+#include "libmv/logging/logging.h"
+#include "libmv/image/image.h"
+#include "libmv/image/sample.h"
+#include "libmv/image/convolve.h"
+#include "libmv/multiview/homography.h"
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+// TODO(keir): Consider adding padding.
+template<typename T>
+bool InBounds(const FloatImage &image,
+              const T &x,
+              const T &y) {
+  return 0.0 <= x && x < image.Width() &&
+         0.0 <= y && y < image.Height();
+}
+
+bool AllInBounds(const FloatImage &image,
+                 const double *x,
+                 const double *y) {
+  for (int i = 0; i < 4; ++i) {
+    if (!InBounds(image, x[i], y[i])) {
+      return false;
+    }
+  }
+  return true;
+}
+
+// Because C++03 doesn't support partial template specializations for
+// functions, but at the same time member function specializations are not
+// supported, the sample function must be inside a template-specialized class
+// with a non-templated static member.
+
+// The "AutoDiffImage::Sample()" function allows sampling an image at an x, y
+// position such that if x and y are jets, then the derivative information is
+// correctly propagated.
+
+// Empty default template.
+template<typename T>
+struct AutoDiffImage {
+  static T Sample(const FloatImage &image_and_gradient,
+                  const T &x, const T &y) {
+    return 0.0;
+  }
+};
+
+// Sample only the image when the coordinates are scalars.
+template<>
+struct AutoDiffImage<double> {
+  static double Sample(const FloatImage &image_and_gradient,
+                       const double& x, const double& y) {
+    return SampleLinear(image_and_gradient, y, x, 0);
+  }
+};
+
+// Sample the image and gradient when the coordinates are jets, applying the
+// jacobian appropriately to propagate the derivatives from the coordinates.
+template<>
+template<typename T, int N>
+struct AutoDiffImage<ceres::Jet<T, N> > {
+  static ceres::Jet<T, N> Sample(const FloatImage &image_and_gradient,
+                                 const ceres::Jet<T, N> &x,
+                                 const ceres::Jet<T, N> &y) {
+    // Sample the image and its derivatives in x and y. One way to think of
+    // this is that the image is a scalar function with a single vector
+    // argument, xy, of dimension 2. Call this s(xy).
+    const T s    = SampleLinear(image_and_gradient, y.a, x.a, 0);
+    const T dsdx = SampleLinear(image_and_gradient, y.a, x.a, 1);
+    const T dsdy = SampleLinear(image_and_gradient, y.a, x.a, 2);
+
+    // However, xy is itself a function of another variable ("z"); xy(z) =
+    // [x(z), y(z)]^T. What this function needs to return is "s", but with the
+    // derivative with respect to z attached to the jet. So combine the
+    // derivative part of x and y's jets to form a Jacobian matrix between x, y
+    // and z (i.e. dxy/dz).
+    Eigen::Matrix<T, 2, N> dxydz;
+    dxydz.row(0) = x.v.transpose();
+    dxydz.row(1) = y.v.transpose();
+
+    // Now apply the chain rule to obtain ds/dz. Combine the derivative with
+    // the scalar part to obtain s with full derivative information.
+    ceres::Jet<T, N> jet_s;
+    jet_s.a = s;
+    jet_s.v = Matrix<T, 1, 2>(dsdx, dsdy) * dxydz;
+    return jet_s;
+  }
+};
+
+template<typename Warp>
+class BoundaryCheckingCallback : public ceres::IterationCallback {
+ public:
+  BoundaryCheckingCallback(const FloatImage& image2,
+                           const Warp &warp,
+                           const double *x1, const double *y1)
+      : image2_(image2), warp_(warp), x1_(x1), y1_(y1) {}
+
+  virtual ceres::CallbackReturnType operator()(
+      const ceres::IterationSummary& summary) {
+    // Warp the original 4 points with the current warp into image2.
+    double x2[4];
+    double y2[4];
+    for (int i = 0; i < 4; ++i) {
+      warp_.Forward(warp_.parameters, x1_[i], y1_[i], x2 + i, y2 + i);
+    }
+    // Enusre they are all in bounds.
+    if (!AllInBounds(image2_, x2, y2)) {
+      return ceres::SOLVER_ABORT;
+    }
+    return ceres::SOLVER_CONTINUE;
+  }
+
+ private:
+  const FloatImage &image2_;
+  const Warp &warp_;
+  const double *x1_;
+  const double *y1_;
+};
+
+template<typename Warp>
+class WarpCostFunctor {
+ public:
+  WarpCostFunctor(const FloatImage &image_and_gradient1,
+                  const FloatImage &image_and_gradient2,
+                  const Mat3 &canonical_to_image1,
+                  const Warp &warp,
+                  int num_samples_x,
+                  int num_samples_y)
+      : image_and_gradient1_(image_and_gradient1),       
+        image_and_gradient2_(image_and_gradient2),       
+        canonical_to_image1_(canonical_to_image1),
+        warp_(warp),
+        num_samples_x_(num_samples_x),  
+        num_samples_y_(num_samples_y) {}
+
+ template<typename T>
+ bool operator()(const T *warp_parameters, T *residuals) const {
+   for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
+     VLOG(2) << "warp_parameters[" << i << "]: " << warp_parameters[i];
+   }
+
+   int cursor = 0;
+   for (int r = 0; r < num_samples_y_; ++r) {
+     for (int c = 0; c < num_samples_x_; ++c) {
+        // Compute the location of the source pixel (via homography).
+        Vec3 image1_position = canonical_to_image1_ * Vec3(c, r, 1);
+        image1_position /= image1_position(2);
+        
+        // Compute the location of the destination pixel.
+        T image2_position[2];
+        warp_.Forward(warp_parameters,
+                      T(image1_position[0]),
+                      T(image1_position[1]),
+                      &image2_position[0],
+                      &image2_position[1]);
+
+        // Sample the source and destination.
+        double src_sample = AutoDiffImage<double>::Sample(image_and_gradient1_,
+                                                          image1_position[0],
+                                                          image1_position[1]);
+        T dst_sample = AutoDiffImage<T>::Sample(image_and_gradient2_,
+                                                image2_position[0],
+                                                image2_position[1]);
+
+        // The difference is the error.
+        residuals[cursor++] = T(src_sample) - dst_sample;
+      }
+    }
+    return true;
+  }
+
+ private:
+  const FloatImage &image_and_gradient1_;
+  const FloatImage &image_and_gradient2_;
+  const Mat3 &canonical_to_image1_;
+  const Warp &warp_;
+  int num_samples_x_;
+  int num_samples_y_;
+};
+
+// Compute the warp from rectangular coordinates, where one corner is the
+// origin, and the opposite corner is at (num_samples_x, num_samples_y).
+Mat3 ComputeCanonicalHomography(const double *x1,
+                                const double *y1,
+                                int num_samples_x,
+                                int num_samples_y) {
+  Mat canonical(2, 4);
+  canonical << 0, num_samples_x, num_samples_x, 0,
+               0, 0,             num_samples_y, num_samples_y;
+
+  Mat xy1(2, 4);
+  xy1 << x1[0], x1[1], x1[2], x1[3],
+         y1[0], y1[1], y1[2], y1[3];
+
+  Mat3 H;
+  if (!Homography2DFromCorrespondencesLinear(canonical, xy1, &H, 1e-12)) {
+    LG << "Couldn't construct homography.";
+  }
+  return H;
+}
+
+class Quad {
+ public:
+  Quad(const double *x, const double *y)
+      : x_(x), y_(y) {
+
+    // Compute the centroid and store it.
+    centroid_ = Vec2(0.0, 0.0);
+    for (int i = 0; i < 4; ++i) {
+      centroid_ += Vec2(x_[i], y_[i]);
+    }
+    centroid_ /= 4.0;
+  }
+
+  // The centroid of the four points representing the quad.
+  const Vec2& Centroid() const {
+    return centroid_;
+  }
+
+  // The average magnitude of the four points relative to the centroid.
+  double Scale() const {
+    double scale = 0.0;
+    for (int i = 0; i < 4; ++i) {
+      scale += (Vec2(x_[i], y_[i]) - Centroid()).norm();
+    }
+    return scale / 4.0;
+  }
+
+  Vec2 CornerRelativeToCentroid(int i) const {
+    return Vec2(x_[i], y_[i]) - centroid_;
+  }
+
+ private:
+  const double *x_;
+  const double *y_;
+  Vec2 centroid_;
+};
+
+struct TranslationWarp {
+  TranslationWarp(const double *x1, const double *y1,
+                  const double *x2, const double *y2) {
+    Vec2 t = Quad(x1, y1).Centroid() - Quad(x2, y2).Centroid();
+    parameters[0] = t[0];
+    parameters[1] = t[1];
+  }
+
+  template<typename T>
+  void Forward(const T *warp_parameters,
+               const T &x1, const T& y1, T *x2, T* y2) const {
+    *x2 = x1 + warp_parameters[0];
+    *y2 = y1 + warp_parameters[1];
+  }
+
+  // Translation x, translation y.
+  enum { NUM_PARAMETERS = 2 };
+  double parameters[NUM_PARAMETERS];
+};
+
+struct TranslationScaleWarp {
+  TranslationScaleWarp(const double *x1, const double *y1,
+                       const double *x2, const double *y2)
+      : q1(x1, y1) {
+    Quad q2(x2, y2);
+
+    // The difference in centroids is the best guess for translation.
+    Vec2 t = q1.Centroid() - q2.Centroid();
+    parameters[0] = t[0];
+    parameters[1] = t[1];
+
+    // The difference in scales is the estimate for the scale.
+    parameters[2] = 1.0 - q2.Scale() / q1.Scale();
+  }
+
+  // The strange way of parameterizing the translation and scaling is to make
+  // the knobs that the optimizer sees easy to adjust. This is less important
+  // for the scaling case than the rotation case.
+  template<typename T>
+  void Forward(const T *warp_parameters,
+               const T &x1, const T& y1, T *x2, T* y2) const {
+    // Make the centroid of Q1 the origin.
+    const T x1_origin = x1 - q1.Centroid()(0);
+    const T y1_origin = y1 - q1.Centroid()(1);
+
+    // Scale uniformly about the origin.
+    const T scale = 1.0 + warp_parameters[2];
+    const T x1_origin_scaled = scale * x1_origin;
+    const T y1_origin_scaled = scale * y1_origin;
+
+    // Translate back into the space of Q1 (but scaled).
+    const T x1_scaled = x1_origin_scaled + q1.Centroid()(0);
+    const T y1_scaled = y1_origin_scaled + q1.Centroid()(1);
+
+    // Translate into the space of Q2.
+    *x2 = x1_scaled + warp_parameters[0];
+    *y2 = y1_scaled + warp_parameters[1];
+  }
+
+  // Translation x, translation y, scale.
+  enum { NUM_PARAMETERS = 3 };
+  double parameters[NUM_PARAMETERS];
+
+  Quad q1;
+};
+
+// Assumes the given points are already zero-centroid and the same size.
+Mat2 OrthogonalProcrustes(const Mat2 &correlation_matrix) {
+  Eigen::JacobiSVD<Mat2> svd(correlation_matrix,
+                             Eigen::ComputeFullU | Eigen::ComputeFullV);
+  return svd.matrixV() * svd.matrixU().transpose();
+}
+
+struct TranslationRotationWarp {
+  TranslationRotationWarp(const double *x1, const double *y1,
+                          const double *x2, const double *y2)
+      : q1(x1, y1) {
+    Quad q2(x2, y2);
+
+    // The difference in centroids is the best guess for translation.
+    Vec2 t = q1.Centroid() - q2.Centroid();
+    parameters[0] = t[0];
+    parameters[1] = t[1];
+
+    // Obtain the rotation via orthorgonal procrustes.
+    Mat2 correlation_matrix;
+    for (int i = 0; i < 4; ++i) {
+      correlation_matrix += q1.CornerRelativeToCentroid(i) * 
+                            q2.CornerRelativeToCentroid(i).transpose();
+    }
+    Mat2 R = OrthogonalProcrustes(correlation_matrix);
+    parameters[2] = acos(R(0, 0));
+  }
+
+  // The strange way of parameterizing the translation and rotation is to make
+  // the knobs that the optimizer sees easy to adjust. The reason is that while
+  // it is always the case that it is possible to express composed rotations
+  // and translations as a single translation and rotation, the numerical
+  // values needed for the composition are often large in magnitude. This is
+  // enough to throw off any minimizer, since it must do the equivalent of
+  // compose rotations and translations.
+  //
+  // Instead, use the parameterization below that offers a parameterization
+  // that exposes the degrees of freedom in a way amenable to optimization.
+  template<typename T>
+  void Forward(const T *warp_parameters,
+                      const T &x1, const T& y1, T *x2, T* y2) const {
+    // Make the centroid of Q1 the origin.
+    const T x1_origin = x1 - q1.Centroid()(0);
+    const T y1_origin = y1 - q1.Centroid()(1);
+
+    // Rotate about the origin (i.e. centroid of Q1).
+    const T theta = warp_parameters[2];
+    const T costheta = cos(theta);
+    const T sintheta = sin(theta);
+    const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
+    const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
+
+    // Translate back into the space of Q1 (but scaled).
+    const T x1_rotated = x1_origin_rotated + q1.Centroid()(0);
+    const T y1_rotated = y1_origin_rotated + q1.Centroid()(1);
+
+    // Translate into the space of Q2.
+    *x2 = x1_rotated + warp_parameters[0];
+    *y2 = y1_rotated + warp_parameters[1];
+  }
+
+  // Translation x, translation y, rotation about the center of Q1 degrees.
+  enum { NUM_PARAMETERS = 3 };
+  double parameters[NUM_PARAMETERS];
+
+  Quad q1;
+};
+
+struct TranslationRotationScaleWarp {
+  TranslationRotationScaleWarp(const double *x1, const double *y1,
+                               const double *x2, const double *y2)
+      : q1(x1, y1) {
+    Quad q2(x2, y2);
+
+    // The difference in centroids is the best guess for translation.
+    Vec2 t = q1.Centroid() - q2.Centroid();
+    parameters[0] = t[0];
+    parameters[1] = t[1];
+
+    // The difference in scales is the estimate for the scale.
+    parameters[2] = 1.0 - q2.Scale() / q1.Scale();
+
+    // Obtain the rotation via orthorgonal procrustes.
+    Mat2 correlation_matrix;
+    for (int i = 0; i < 4; ++i) {
+      correlation_matrix += q1.CornerRelativeToCentroid(i) * 
+                            q2.CornerRelativeToCentroid(i).transpose();
+    }
+    Mat2 R = OrthogonalProcrustes(correlation_matrix);
+    parameters[3] = acos(R(0, 0));
+  }
+
+  // The strange way of parameterizing the translation and rotation is to make
+  // the knobs that the optimizer sees easy to adjust. The reason is that while
+  // it is always the case that it is possible to express composed rotations
+  // and translations as a single translation and rotation, the numerical
+  // values needed for the composition are often large in magnitude. This is
+  // enough to throw off any minimizer, since it must do the equivalent of
+  // compose rotations and translations.
+  //
+  // Instead, use the parameterization below that offers a parameterization
+  // that exposes the degrees of freedom in a way amenable to optimization.
+  template<typename T>
+  void Forward(const T *warp_parameters,
+                      const T &x1, const T& y1, T *x2, T* y2) const {
+    // Make the centroid of Q1 the origin.
+    const T x1_origin = x1 - q1.Centroid()(0);
+    const T y1_origin = y1 - q1.Centroid()(1);
+
+    // Rotate about the origin (i.e. centroid of Q1).
+    const T theta = warp_parameters[2];
+    const T costheta = cos(theta);
+    const T sintheta = sin(theta);
+    const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
+    const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
+
+    // Scale uniformly about the origin.
+    const T scale = 1.0 + warp_parameters[2];
+    const T x1_origin_rotated_scaled = scale * x1_origin_rotated;
+    const T y1_origin_rotated_scaled = scale * y1_origin_rotated;
+
+    // Translate back into the space of Q1 (but scaled and rotated).
+    const T x1_rotated_scaled = x1_origin_rotated_scaled + q1.Centroid()(0);
+    const T y1_rotated_scaled = y1_origin_rotated_scaled + q1.Centroid()(1);
+
+    // Translate into the space of Q2.
+    *x2 = x1_rotated_scaled + warp_parameters[0];
+    *y2 = y1_rotated_scaled + warp_parameters[1];
+  }
+
+  // Translation x, translation y, rotation about the center of Q1 degrees,
+  // scale.
+  enum { NUM_PARAMETERS = 4 };
+  double parameters[NUM_PARAMETERS];
+
+  Quad q1;
+};
+
+// TODO(keir): Finish affine warp.
+
+struct HomographyWarp {
+  HomographyWarp(const double *x1, const double *y1,
+                 const double *x2, const double *y2) {
+    Mat quad1(2, 4);
+    quad1 << x1[0], x1[1], x1[2], x1[3],
+             y1[0], y1[1], y1[2], y1[3];
+
+    Mat quad2(2, 4);
+    quad2 << x2[0], x2[1], x2[2], x2[3],
+             y2[0], y2[1], y2[2], y2[3];
+
+    Mat3 H;
+    if (!Homography2DFromCorrespondencesLinear(quad1, quad2, &H, 1e-12)) {
+      LG << "Couldn't construct homography.";
+    }
+
+    // Assume H(2, 2) != 0, and fix scale at H(2, 2) == 1.0.
+    H /= H(2, 2);
+
+    // Assume H is close to identity, so subtract out the diagonal.
+    H(0, 0) -= 1.0;
+    H(1, 1) -= 1.0;
+
+    CHECK_NE(H(2, 2), 0.0) << H;
+    for (int i = 0; i < 8; ++i) {
+      parameters[i] = H(i / 3, i % 3);
+      LG << "Parameters[" << i << "]: " << parameters[i];
+    }
+  }
+
+  template<typename T>
+  static void Forward(const T *p,
+                      const T &x1, const T& y1, T *x2, T* y2) {
+    // Homography warp with manual 3x3 matrix multiply.
+    const T xx2 = (1.0 + p[0]) * x1 +     p[1]     * y1 + p[2];
+    const T yy2 =     p[3]     * x1 + (1.0 + p[4]) * y1 + p[5];
+    const T zz2 =     p[6]     * x1 +     p[7]     * y1 + 1.0;
+    *x2 = xx2 / zz2;
+    *y2 = yy2 / zz2;
+  }
+
+  enum { NUM_PARAMETERS = 8 };
+  double parameters[NUM_PARAMETERS];
+};
+
+template<typename Warp>
+void TemplatedTrackRegion(const FloatImage &image1,
+                          const FloatImage &image2,
+                          const double *x1, const double *y1,
+                          const TrackRegionOptions &options,
+                          double *x2, double *y2,
+                          TrackRegionResult *result) {
+  // Bail early if the points are already outside.
+  if (!AllInBounds(image1, x1, y1)) {
+    result->termination = TrackRegionResult::SOURCE_OUT_OF_BOUNDS;
+    return;
+  }
+  if (!AllInBounds(image2, x2, y2)) {
+    result->termination = TrackRegionResult::DESTINATION_OUT_OF_BOUNDS;
+    return;
+  }
+  // TODO(keir): Check quads to ensure there is some area.
+
+  // Prepare the image and gradient.
+  Array3Df image_and_gradient1;
+  Array3Df image_and_gradient2;
+  BlurredImageAndDerivativesChannels(image1, options.sigma,
+                                     &image_and_gradient1);
+  BlurredImageAndDerivativesChannels(image2, options.sigma,
+                                     &image_and_gradient2);
+
+  // Prepare the initial warp parameters from the four correspondences.
+  Warp warp(x1, y1, x2, y2);
+
+  ceres::Solver::Options solver_options;
+  solver_options.linear_solver_type = ceres::DENSE_QR;
+  solver_options.max_num_iterations = options.max_iterations;
+  solver_options.update_state_every_iteration = true;
+  solver_options.parameter_tolerance = 1e-16;
+  solver_options.function_tolerance = 1e-16;
+
+  // TODO(keir): Consider removing these options before committing.
+  solver_options.numeric_derivative_relative_step_size = 1e-3;
+  solver_options.gradient_check_relative_precision = 1e-10;
+  solver_options.minimizer_progress_to_stdout = false;
+
+  // Prevent the corners from going outside the destination image.
+  BoundaryCheckingCallback<Warp> callback(image2, warp, x1, y1);
+  solver_options.callbacks.push_back(&callback);
+
+  // Compute the warp from rectangular coordinates.
+  Mat3 canonical_homography = ComputeCanonicalHomography(x1, y1,
+                                                         options.num_samples_x,
+                                                         options.num_samples_y);
+
+  // Construct the warp cost function. AutoDiffCostFunction takes ownership.
+  WarpCostFunctor<Warp> *cost_function =
+      new WarpCostFunctor<Warp>(image_and_gradient1,
+                                image_and_gradient2,
+                                canonical_homography,
+                                warp,
+                                options.num_samples_x,
+                                options.num_samples_y);
+
+  // Construct the problem with a single residual.
+  ceres::Problem::Options problem_options;
+  problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+  ceres::Problem problem(problem_options);
+  problem.AddResidualBlock(
+      new ceres::AutoDiffCostFunction<
+          WarpCostFunctor<Warp>,
+          ceres::DYNAMIC,
+          Warp::NUM_PARAMETERS>(cost_function,
+                                options.num_samples_x * options.num_samples_y),
+      NULL,
+      warp.parameters);
+
+  ceres::Solver::Summary summary;
+  ceres::Solve(solver_options, &problem, &summary);
+
+  LG << "Summary:\n" << summary.FullReport();
+
+  // Update the four points with the found solution; if the solver failed, then
+  // the warp parameters are the identity (so ignore failure).
+  for (int i = 0; i < 4; ++i) {
+    warp.Forward(warp.parameters, x1[i], y1[i], x2 + i, y2 + i);
+  }
+
+  // TODO(keir): Update the result statistics.
+
+  CHECK_NE(summary.termination_type, ceres::USER_ABORT) << "Libmv bug.";
+  if (summary.termination_type == ceres::USER_ABORT) {
+    result->termination = TrackRegionResult::FELL_OUT_OF_BOUNDS;
+    return;
+  }
+#define HANDLE_TERMINATION(termination_enum) \
+  if (summary.termination_type == ceres::termination_enum) { \
+    result->termination = TrackRegionResult::termination_enum; \
+    return; \
+  }
+  HANDLE_TERMINATION(PARAMETER_TOLERANCE);
+  HANDLE_TERMINATION(FUNCTION_TOLERANCE);
+  HANDLE_TERMINATION(GRADIENT_TOLERANCE);
+  HANDLE_TERMINATION(NO_CONVERGENCE);
+  HANDLE_TERMINATION(DID_NOT_RUN);
+  HANDLE_TERMINATION(NUMERICAL_FAILURE);
+#undef HANDLE_TERMINATION
+};
+
+void TrackRegion(const FloatImage &image1,
+                 const FloatImage &image2,
+                 const double *x1, const double *y1,
+                 const TrackRegionOptions &options,
+                 double *x2, double *y2,
+                 TrackRegionResult *result) {
+  // Enum is necessary due to templated nature of autodiff.
+#define HANDLE_MODE(mode_enum, mode_type) \
+  if (options.mode == TrackRegionOptions::mode_enum) { \
+    TemplatedTrackRegion<mode_type>(image1, image2, \
+                                    x1, y1, \
+                                    options, \
+                                    x2, y2, \
+                                    result); \
+    return; \
+  }
+
+  HANDLE_MODE(TRANSLATION,                TranslationWarp);
+  HANDLE_MODE(TRANSLATION_SCALE,          TranslationScaleWarp);
+  HANDLE_MODE(TRANSLATION_ROTATION,       TranslationRotationWarp);
+  HANDLE_MODE(TRANSLATION_ROTATION_SCALE, TranslationRotationScaleWarp);
+  //HANDLE_MODE(AFFINE,                     AffineWarp);
+  HANDLE_MODE(HOMOGRAPHY,                 HomographyWarp);
+#undef HANDLE_MODE
+}
+
+}  // namespace libmv
diff --git a/extern/libmv/libmv/tracking/track_region.h b/extern/libmv/libmv/tracking/track_region.h
new file mode 100644 (file)
index 0000000..ca783c3
--- /dev/null
@@ -0,0 +1,91 @@
+// 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.
+
+#ifndef LIBMV_TRACKING_TRACK_REGION_H_
+
+// Necessary for M_E when building with MSVC.
+#define _USE_MATH_DEFINES
+
+#include "libmv/tracking/esm_region_tracker.h"
+
+#include "libmv/image/image.h"
+#include "libmv/image/sample.h"
+#include "libmv/numeric/numeric.h"
+
+namespace libmv {
+
+struct TrackRegionOptions {
+  enum Mode {
+    TRANSLATION,
+    TRANSLATION_ROTATION,
+    TRANSLATION_SCALE,
+    TRANSLATION_ROTATION_SCALE,
+    AFFINE,
+    HOMOGRAPHY,
+  };
+  Mode mode;
+
+  int num_samples_x;
+  int num_samples_y;
+
+  double minimum_correlation;
+  int max_iterations;
+  bool use_esm;
+
+  double sigma;
+};
+
+struct TrackRegionResult {
+  enum Termination {
+    // Ceres termination types, duplicated.
+    PARAMETER_TOLERANCE = 0,
+    FUNCTION_TOLERANCE,
+    GRADIENT_TOLERANCE,
+    NO_CONVERGENCE,
+    DID_NOT_RUN,
+    NUMERICAL_FAILURE,
+
+    // Libmv specific errors.
+    SOURCE_OUT_OF_BOUNDS,
+    DESTINATION_OUT_OF_BOUNDS,
+    FELL_OUT_OF_BOUNDS,
+    INSUFFICIENT_CORRELATION,
+  };
+  Termination termination;
+
+  int num_iterations;
+  double correlation;
+
+  // Final parameters?
+};
+
+// Always needs 4 correspondences.
+void TrackRegion(const FloatImage &image1,
+                 const FloatImage &image2,
+                 const double *x1, const double *y1,
+                 const TrackRegionOptions &options,
+                 double *x2, double *y2,
+                 TrackRegionResult *result);
+
+// TODO(keir): May need a "samplewarp" function.
+
+}  // namespace libmv
+
+#endif  // LIBMV_TRACKING_TRACK_REGION_H_