Planar tracker polish.
[blender.git] / extern / libmv / libmv / tracking / track_region.cc
1 // Copyright (c) 2012 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20 //
21 // Author: mierle@google.com (Keir Mierle)
22 //
23 // TODO(keir): While this tracking code works rather well, it has some
24 // outragous inefficiencies. There is probably a 5-10x speedup to be had if a
25 // smart coder went through the TODO's and made the suggested performance
26 // enhancements.
27
28 // Necessary for M_E when building with MSVC.
29 #define _USE_MATH_DEFINES
30
31 #include "libmv/tracking/track_region.h"
32
33 #include <Eigen/SVD>
34 #include <Eigen/QR>
35 #include <iostream>
36 #include "ceres/ceres.h"
37 #include "libmv/logging/logging.h"
38 #include "libmv/image/image.h"
39 #include "libmv/image/sample.h"
40 #include "libmv/image/convolve.h"
41 #include "libmv/multiview/homography.h"
42 #include "libmv/numeric/numeric.h"
43
44 namespace libmv {
45
46 using ceres::Jet;
47 using ceres::JetOps;
48 using ceres::Chain;
49
50 TrackRegionOptions::TrackRegionOptions()
51     : mode(TRANSLATION),
52       minimum_correlation(0),
53       max_iterations(20),
54       use_esm(true),
55       use_brute_initialization(true),
56       use_normalized_intensities(false),
57       sigma(0.9),
58       num_extra_points(0),
59       image1_mask(NULL) {
60 }
61
62 namespace {
63
64 // TODO(keir): Consider adding padding.
65 template<typename T>
66 bool InBounds(const FloatImage &image,
67               const T &x,
68               const T &y) {
69   return 0.0 <= x && x < image.Width() &&
70          0.0 <= y && y < image.Height();
71 }
72
73 bool AllInBounds(const FloatImage &image,
74                  const double *x,
75                  const double *y) {
76   for (int i = 0; i < 4; ++i) {
77     if (!InBounds(image, x[i], y[i])) {
78       return false;
79     }
80   }
81   return true;
82 }
83
84 // Sample the image at position (x, y) but use the gradient, if present, to
85 // propagate derivatives from x and y. This is needed to integrate the numeric
86 // image gradients with Ceres's autodiff framework.
87 template<typename T>
88 static T SampleWithDerivative(const FloatImage &image_and_gradient,
89                               const T &x,
90                               const T &y) {
91   float scalar_x = JetOps<T>::GetScalar(x);
92   float scalar_y = JetOps<T>::GetScalar(y);
93
94   // Note that sample[1] and sample[2] will be uninitialized in the scalar
95   // case, but that is not an issue because the Chain::Rule below will not read
96   // the uninitialized values.
97   float sample[3];
98   if (JetOps<T>::IsScalar()) {
99     // For the scalar case, only sample the image.
100     sample[0] = SampleLinear(image_and_gradient, scalar_y, scalar_x, 0);
101   } else {
102     // For the derivative case, sample the gradient as well.
103     SampleLinear(image_and_gradient, scalar_y, scalar_x, sample);
104   }
105   T xy[2] = { x, y };
106   return Chain<float, 2, T>::Rule(sample[0], sample + 1, xy);
107 }
108
109 template<typename Warp>
110 class BoundaryCheckingCallback : public ceres::IterationCallback {
111  public:
112   BoundaryCheckingCallback(const FloatImage& image2,
113                            const Warp &warp,
114                            const double *x1, const double *y1)
115       : image2_(image2), warp_(warp), x1_(x1), y1_(y1) {}
116
117   virtual ceres::CallbackReturnType operator()(
118       const ceres::IterationSummary& summary) {
119     // Warp the original 4 points with the current warp into image2.
120     double x2[4];
121     double y2[4];
122     for (int i = 0; i < 4; ++i) {
123       warp_.Forward(warp_.parameters, x1_[i], y1_[i], x2 + i, y2 + i);
124     }
125     // Enusre they are all in bounds.
126     if (!AllInBounds(image2_, x2, y2)) {
127       return ceres::SOLVER_ABORT;
128     }
129     return ceres::SOLVER_CONTINUE;
130   }
131
132  private:
133   const FloatImage &image2_;
134   const Warp &warp_;
135   const double *x1_;
136   const double *y1_;
137 };
138
139 template<typename Warp>
140 class WarpCostFunctor {
141  public:
142   WarpCostFunctor(const TrackRegionOptions &options,
143                   const FloatImage &image_and_gradient1,
144                   const FloatImage &image_and_gradient2,
145                   const Mat3 &canonical_to_image1,
146                   int num_samples_x,
147                   int num_samples_y,
148                   const Warp &warp)
149       : options_(options),
150         image_and_gradient1_(image_and_gradient1),       
151         image_and_gradient2_(image_and_gradient2),       
152         canonical_to_image1_(canonical_to_image1),
153         num_samples_x_(num_samples_x),
154         num_samples_y_(num_samples_y),
155         warp_(warp),
156         pattern_and_gradient_(num_samples_y_, num_samples_x_, 3),
157         pattern_positions_(num_samples_y_, num_samples_x_, 2),
158         pattern_mask_(num_samples_y_, num_samples_x_, 1) {
159     ComputeCanonicalPatchAndNormalizer();
160   }
161
162   void ComputeCanonicalPatchAndNormalizer() {
163     src_mean_ = 0.0;
164     double num_samples = 0.0;
165     for (int r = 0; r < num_samples_y_; ++r) {
166       for (int c = 0; c < num_samples_x_; ++c) {
167         // Compute the position; cache it.
168         Vec3 image_position = canonical_to_image1_ * Vec3(c, r, 1);
169         image_position /= image_position(2);
170         pattern_positions_(r, c, 0) = image_position(0);
171         pattern_positions_(r, c, 1) = image_position(1);
172
173         // Sample the pattern and gradients.
174         SampleLinear(image_and_gradient1_,
175                      image_position(1),  // SampleLinear is r, c.
176                      image_position(0),
177                      &pattern_and_gradient_(r, c, 0));
178
179         // Sample sample the mask.
180         double mask_value = 1.0;
181         if (options_.image1_mask != NULL) {
182           SampleLinear(*options_.image1_mask,
183                        image_position(1),  // SampleLinear is r, c.
184                        image_position(0),
185                        &pattern_mask_(r, c, 0));
186           mask_value = pattern_mask_(r, c);
187         }
188         src_mean_ += pattern_and_gradient_(r, c, 0) * mask_value;
189         num_samples += mask_value;
190       }
191     }
192     src_mean_ /= num_samples;
193   }
194
195   template<typename T>
196   bool operator()(const T *warp_parameters, T *residuals) const {
197     if (options_.image1_mask != NULL) {
198       VLOG(2) << "Using a mask.";
199     }
200     for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
201       VLOG(2) << "warp_parameters[" << i << "]: " << warp_parameters[i];
202     }
203
204     T dst_mean = T(1.0);
205     if (options_.use_normalized_intensities) {
206       ComputeNormalizingCoefficient(warp_parameters,
207                                     &dst_mean);
208     }
209
210     int cursor = 0;
211     for (int r = 0; r < num_samples_y_; ++r) {
212       for (int c = 0; c < num_samples_x_; ++c) {
213         // Use the pre-computed image1 position.
214         Vec2 image1_position(pattern_positions_(r, c, 0),
215                              pattern_positions_(r, c, 1));
216
217         // Sample the mask early; if it's zero, this pixel has no effect. This
218         // allows early bailout from the expensive sampling that happens below.
219         //
220         // Note that partial masks are not short circuited. To see why short
221         // circuiting produces bitwise-exact same results, consider that the
222         // residual for each pixel is 
223         //
224         //    residual = mask * (src - dst)  ,
225         //
226         // and for jets, multiplying by a scalar multiplies the derivative
227         // components by the scalar as well. Therefore, if the mask is exactly
228         // zero, then so too will the final residual and derivatives.
229         double mask_value = 1.0;
230         if (options_.image1_mask != NULL) {
231           mask_value = pattern_mask_(r, c);
232           if (mask_value == 0.0) {
233             residuals[cursor++] = T(0.0);
234             continue;
235           }
236         }
237
238         // Compute the location of the destination pixel.
239         T image2_position[2];
240         warp_.Forward(warp_parameters,
241                       T(image1_position[0]),
242                       T(image1_position[1]),
243                       &image2_position[0],
244                       &image2_position[1]);
245
246         // Sample the destination, propagating derivatives.
247         T dst_sample = SampleWithDerivative(image_and_gradient2_,
248                                             image2_position[0],
249                                             image2_position[1]);
250
251         // Sample the source. This is made complicated by ESM mode.
252         T src_sample;
253         if (options_.use_esm && !JetOps<T>::IsScalar()) {
254           // In ESM mode, the derivative of the source is also taken into
255           // account. This changes the linearization in a way that causes
256           // better convergence. Copy the derivative of the warp parameters
257           // onto the jets for the image1 position. This is the ESM hack.
258           T image1_position_jet[2] = {
259             image2_position[0],  // Order is x, y. This matches the
260             image2_position[1]   // derivative order in the patch.
261           };
262           JetOps<T>::SetScalar(image1_position[0], image1_position_jet + 0);
263           JetOps<T>::SetScalar(image1_position[1], image1_position_jet + 1);
264
265           // Now that the image1 positions have the jets applied from the
266           // image2 position (the ESM hack), chain the image gradients to
267           // obtain a sample with the derivative with respect to the warp
268           // parameters attached.
269           src_sample = Chain<float, 2, T>::Rule(pattern_and_gradient_(r, c),
270                                                 &pattern_and_gradient_(r, c, 1),
271                                                 image1_position_jet);
272
273           // The jacobians for these should be averaged. Due to the subtraction
274           // below, flip the sign of the src derivative so that the effect
275           // after subtraction of the jets is that they are averaged.
276           JetOps<T>::ScaleDerivative(-0.5, &src_sample);
277           JetOps<T>::ScaleDerivative(0.5, &dst_sample);
278         } else {
279           // This is the traditional, forward-mode KLT solution.
280           src_sample = T(pattern_and_gradient_(r, c));
281         }
282
283         // Normalize the samples by the mean values of each signal. The typical
284         // light model assumes multiplicative intensity changes with changing
285         // light, so this is a reasonable choice. Note that dst_mean has
286         // derivative information attached thanks to autodiff.
287         if (options_.use_normalized_intensities) {
288           src_sample /= T(src_mean_);
289           dst_sample /= dst_mean;
290         }
291
292         // The difference is the error.
293         T error = src_sample - dst_sample;
294
295         // Weight the error by the mask, if one is present.
296         if (options_.image1_mask != NULL) {
297           error *= T(mask_value);
298         }
299         residuals[cursor++] = error;
300       }
301     }
302     return true;
303   }
304
305   // For normalized matching, the average and 
306   template<typename T>
307   void ComputeNormalizingCoefficient(const T *warp_parameters,
308                                      T *dst_mean) const {
309
310     *dst_mean = T(0.0);
311     double num_samples = 0.0;
312     for (int r = 0; r < num_samples_y_; ++r) {
313       for (int c = 0; c < num_samples_x_; ++c) {
314         // Use the pre-computed image1 position.
315         Vec2 image1_position(pattern_positions_(r, c, 0),
316                              pattern_positions_(r, c, 1));
317         
318         // Sample the mask early; if it's zero, this pixel has no effect. This
319         // allows early bailout from the expensive sampling that happens below.
320         double mask_value = 1.0;
321         if (options_.image1_mask != NULL) {
322           mask_value = pattern_mask_(r, c);
323           if (mask_value == 0.0) {
324             continue;
325           }
326         }
327
328         // Compute the location of the destination pixel.
329         T image2_position[2];
330         warp_.Forward(warp_parameters,
331                       T(image1_position[0]),
332                       T(image1_position[1]),
333                       &image2_position[0],
334                       &image2_position[1]);
335
336
337         // Sample the destination, propagating derivatives.
338         // TODO(keir): This accumulation can, surprisingly, be done as a
339         // pre-pass by using integral images. This is complicated by the need
340         // to store the jets in the integral image, but it is possible.
341         T dst_sample = SampleWithDerivative(image_and_gradient2_,
342                                             image2_position[0],
343                                             image2_position[1]);
344
345         // Weight the sample by the mask, if one is present.
346         if (options_.image1_mask != NULL) {
347           dst_sample *= T(mask_value);
348         }
349
350         *dst_mean += dst_sample;
351         num_samples += mask_value;
352       }
353     }
354     *dst_mean /= T(num_samples);
355     LG << "Normalization for dst:" << *dst_mean;
356   }
357
358  // TODO(keir): Consider also computing the cost here.
359  double PearsonProductMomentCorrelationCoefficient(
360      const double *warp_parameters) const {
361    for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
362      VLOG(2) << "Correlation warp_parameters[" << i << "]: "
363              << warp_parameters[i];
364    }
365
366    // The single-pass PMCC computation is somewhat numerically unstable, but
367    // it's sufficient for the tracker.
368    double sX = 0, sY = 0, sXX = 0, sYY = 0, sXY = 0;
369
370    // Due to masking, it's important to account for fractional samples.
371    // For example, samples with a 50% mask are counted as a half sample.
372    double num_samples = 0;
373
374    for (int r = 0; r < num_samples_y_; ++r) {
375      for (int c = 0; c < num_samples_x_; ++c) {
376         // Use the pre-computed image1 position.
377         Vec2 image1_position(pattern_positions_(r, c, 0),
378                              pattern_positions_(r, c, 1));
379         
380         double mask_value = 1.0;
381         if (options_.image1_mask != NULL) {
382           mask_value = pattern_mask_(r, c);
383           if (mask_value == 0.0) {
384             continue;
385           }
386         }
387
388         // Compute the location of the destination pixel.
389         double image2_position[2];
390         warp_.Forward(warp_parameters,
391                       image1_position[0],
392                       image1_position[1],
393                       &image2_position[0],
394                       &image2_position[1]);
395
396         double x = pattern_and_gradient_(r, c);
397         double y = SampleLinear(image_and_gradient2_,
398                                 image2_position[1],  // SampleLinear is r, c.
399                                 image2_position[0]);
400
401         // Weight the signals by the mask, if one is present.
402         if (options_.image1_mask != NULL) {
403           x *= mask_value;
404           y *= mask_value;
405           num_samples += mask_value;
406         } else {
407           num_samples++;
408         }
409         sX += x;
410         sY += y;
411         sXX += x*x;
412         sYY += y*y;
413         sXY += x*y;
414       }
415     }
416     // Normalize.
417     sX /= num_samples;
418     sY /= num_samples;
419     sXX /= num_samples;
420     sYY /= num_samples;
421     sXY /= num_samples;
422
423     double var_x = sXX - sX*sX;
424     double var_y = sYY - sY*sY;
425     double covariance_xy = sXY - sX*sY;
426
427     double correlation = covariance_xy / sqrt(var_x * var_y);
428     LG << "Covariance xy: " << covariance_xy
429        << ", var 1: " << var_x << ", var 2: " << var_y
430        << ", correlation: " << correlation;
431     return correlation;
432   }
433
434  private:
435   const TrackRegionOptions &options_;
436   const FloatImage &image_and_gradient1_;
437   const FloatImage &image_and_gradient2_;
438   const Mat3 &canonical_to_image1_;
439   int num_samples_x_;
440   int num_samples_y_;
441   const Warp &warp_;
442   double src_mean_;
443   FloatImage pattern_and_gradient_;
444
445   // This contains the position from where the cached pattern samples were
446   // taken from. This is also used to warp from src to dest without going from
447   // canonical pixels to src first.
448   FloatImage pattern_positions_;
449
450   FloatImage pattern_mask_;
451 };
452
453 // Compute the warp from rectangular coordinates, where one corner is the
454 // origin, and the opposite corner is at (num_samples_x, num_samples_y).
455 Mat3 ComputeCanonicalHomography(const double *x1,
456                                 const double *y1,
457                                 int num_samples_x,
458                                 int num_samples_y) {
459   Mat canonical(2, 4);
460   canonical << 0, num_samples_x, num_samples_x, 0,
461                0, 0,             num_samples_y, num_samples_y;
462
463   Mat xy1(2, 4);
464   xy1 << x1[0], x1[1], x1[2], x1[3],
465          y1[0], y1[1], y1[2], y1[3];
466
467   Mat3 H;
468   if (!Homography2DFromCorrespondencesLinear(canonical, xy1, &H, 1e-12)) {
469     LG << "Couldn't construct homography.";
470   }
471   return H;
472 }
473
474 class Quad {
475  public:
476   Quad(const double *x, const double *y) : x_(x), y_(y) {
477     // Compute the centroid and store it.
478     centroid_ = Vec2(0.0, 0.0);
479     for (int i = 0; i < 4; ++i) {
480       centroid_ += Vec2(x_[i], y_[i]);
481     }
482     centroid_ /= 4.0;
483   }
484
485   // The centroid of the four points representing the quad.
486   const Vec2& Centroid() const {
487     return centroid_;
488   }
489
490   // The average magnitude of the four points relative to the centroid.
491   double Scale() const {
492     double scale = 0.0;
493     for (int i = 0; i < 4; ++i) {
494       scale += (Vec2(x_[i], y_[i]) - Centroid()).norm();
495     }
496     return scale / 4.0;
497   }
498
499   Vec2 CornerRelativeToCentroid(int i) const {
500     return Vec2(x_[i], y_[i]) - centroid_;
501   }
502
503  private:
504   const double *x_;
505   const double *y_;
506   Vec2 centroid_;
507 };
508
509 struct TranslationWarp {
510   TranslationWarp(const double *x1, const double *y1,
511                   const double *x2, const double *y2) {
512     Vec2 t = Quad(x2, y2).Centroid() - Quad(x1, y1).Centroid();
513     parameters[0] = t[0];
514     parameters[1] = t[1];
515   }
516
517   template<typename T>
518   void Forward(const T *warp_parameters,
519                const T &x1, const T& y1, T *x2, T* y2) const {
520     *x2 = x1 + warp_parameters[0];
521     *y2 = y1 + warp_parameters[1];
522   }
523
524   // Translation x, translation y.
525   enum { NUM_PARAMETERS = 2 };
526   double parameters[NUM_PARAMETERS];
527 };
528
529 struct TranslationScaleWarp {
530   TranslationScaleWarp(const double *x1, const double *y1,
531                        const double *x2, const double *y2)
532       : q1(x1, y1) {
533     Quad q2(x2, y2);
534
535     // The difference in centroids is the best guess for translation.
536     Vec2 t = q2.Centroid() - q1.Centroid();
537     parameters[0] = t[0];
538     parameters[1] = t[1];
539
540     // The difference in scales is the estimate for the scale.
541     parameters[2] = 1.0 - q2.Scale() / q1.Scale();
542   }
543
544   // The strange way of parameterizing the translation and scaling is to make
545   // the knobs that the optimizer sees easy to adjust. This is less important
546   // for the scaling case than the rotation case.
547   template<typename T>
548   void Forward(const T *warp_parameters,
549                const T &x1, const T& y1, T *x2, T* y2) const {
550     // Make the centroid of Q1 the origin.
551     const T x1_origin = x1 - q1.Centroid()(0);
552     const T y1_origin = y1 - q1.Centroid()(1);
553
554     // Scale uniformly about the origin.
555     const T scale = 1.0 + warp_parameters[2];
556     const T x1_origin_scaled = scale * x1_origin;
557     const T y1_origin_scaled = scale * y1_origin;
558
559     // Translate back into the space of Q1 (but scaled).
560     const T x1_scaled = x1_origin_scaled + q1.Centroid()(0);
561     const T y1_scaled = y1_origin_scaled + q1.Centroid()(1);
562
563     // Translate into the space of Q2.
564     *x2 = x1_scaled + warp_parameters[0];
565     *y2 = y1_scaled + warp_parameters[1];
566   }
567
568   // Translation x, translation y, scale.
569   enum { NUM_PARAMETERS = 3 };
570   double parameters[NUM_PARAMETERS];
571
572   Quad q1;
573 };
574
575 // Assumes the given points are already zero-centroid and the same size.
576 Mat2 OrthogonalProcrustes(const Mat2 &correlation_matrix) {
577   Eigen::JacobiSVD<Mat2> svd(correlation_matrix,
578                              Eigen::ComputeFullU | Eigen::ComputeFullV);
579   return svd.matrixV() * svd.matrixU().transpose();
580 }
581
582 struct TranslationRotationWarp {
583   TranslationRotationWarp(const double *x1, const double *y1,
584                           const double *x2, const double *y2)
585       : q1(x1, y1) {
586     Quad q2(x2, y2);
587
588     // The difference in centroids is the best guess for translation.
589     Vec2 t = q2.Centroid() - q1.Centroid();
590     parameters[0] = t[0];
591     parameters[1] = t[1];
592
593     // Obtain the rotation via orthorgonal procrustes.
594     Mat2 correlation_matrix;
595     for (int i = 0; i < 4; ++i) {
596       correlation_matrix += q1.CornerRelativeToCentroid(i) * 
597                             q2.CornerRelativeToCentroid(i).transpose();
598     }
599     Mat2 R = OrthogonalProcrustes(correlation_matrix);
600     parameters[2] = atan2(R(1, 0), R(0, 0));
601
602     LG << "Correlation_matrix:\n" << correlation_matrix;
603     LG << "R:\n" << R;
604     LG << "Theta:" << parameters[2];
605   }
606
607   // The strange way of parameterizing the translation and rotation is to make
608   // the knobs that the optimizer sees easy to adjust. The reason is that while
609   // it is always the case that it is possible to express composed rotations
610   // and translations as a single translation and rotation, the numerical
611   // values needed for the composition are often large in magnitude. This is
612   // enough to throw off any minimizer, since it must do the equivalent of
613   // compose rotations and translations.
614   //
615   // Instead, use the parameterization below that offers a parameterization
616   // that exposes the degrees of freedom in a way amenable to optimization.
617   template<typename T>
618   void Forward(const T *warp_parameters,
619                       const T &x1, const T& y1, T *x2, T* y2) const {
620     // Make the centroid of Q1 the origin.
621     const T x1_origin = x1 - q1.Centroid()(0);
622     const T y1_origin = y1 - q1.Centroid()(1);
623
624     // Rotate about the origin (i.e. centroid of Q1).
625     const T theta = warp_parameters[2];
626     const T costheta = cos(theta);
627     const T sintheta = sin(theta);
628     const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
629     const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
630
631     // Translate back into the space of Q1 (but scaled).
632     const T x1_rotated = x1_origin_rotated + q1.Centroid()(0);
633     const T y1_rotated = y1_origin_rotated + q1.Centroid()(1);
634
635     // Translate into the space of Q2.
636     *x2 = x1_rotated + warp_parameters[0];
637     *y2 = y1_rotated + warp_parameters[1];
638   }
639
640   // Translation x, translation y, rotation about the center of Q1 degrees.
641   enum { NUM_PARAMETERS = 3 };
642   double parameters[NUM_PARAMETERS];
643
644   Quad q1;
645 };
646
647 struct TranslationRotationScaleWarp {
648   TranslationRotationScaleWarp(const double *x1, const double *y1,
649                                const double *x2, const double *y2)
650       : q1(x1, y1) {
651     Quad q2(x2, y2);
652
653     // The difference in centroids is the best guess for translation.
654     Vec2 t = q2.Centroid() - q1.Centroid();
655     parameters[0] = t[0];
656     parameters[1] = t[1];
657
658     // The difference in scales is the estimate for the scale.
659     parameters[2] = 1.0 - q2.Scale() / q1.Scale();
660
661     // Obtain the rotation via orthorgonal procrustes.
662     Mat2 correlation_matrix;
663     for (int i = 0; i < 4; ++i) {
664       correlation_matrix += q1.CornerRelativeToCentroid(i) * 
665                             q2.CornerRelativeToCentroid(i).transpose();
666     }
667     Mat2 R = OrthogonalProcrustes(correlation_matrix);
668     parameters[3] = atan2(R(1, 0), R(0, 0));
669
670     LG << "Correlation_matrix:\n" << correlation_matrix;
671     LG << "R:\n" << R;
672     LG << "Theta:" << parameters[3];
673   }
674
675   // The strange way of parameterizing the translation and rotation is to make
676   // the knobs that the optimizer sees easy to adjust. The reason is that while
677   // it is always the case that it is possible to express composed rotations
678   // and translations as a single translation and rotation, the numerical
679   // values needed for the composition are often large in magnitude. This is
680   // enough to throw off any minimizer, since it must do the equivalent of
681   // compose rotations and translations.
682   //
683   // Instead, use the parameterization below that offers a parameterization
684   // that exposes the degrees of freedom in a way amenable to optimization.
685   template<typename T>
686   void Forward(const T *warp_parameters,
687                       const T &x1, const T& y1, T *x2, T* y2) const {
688     // Make the centroid of Q1 the origin.
689     const T x1_origin = x1 - q1.Centroid()(0);
690     const T y1_origin = y1 - q1.Centroid()(1);
691
692     // Rotate about the origin (i.e. centroid of Q1).
693     const T theta = warp_parameters[3];
694     const T costheta = cos(theta);
695     const T sintheta = sin(theta);
696     const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
697     const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
698
699     // Scale uniformly about the origin.
700     const T scale = 1.0 + warp_parameters[2];
701     const T x1_origin_rotated_scaled = scale * x1_origin_rotated;
702     const T y1_origin_rotated_scaled = scale * y1_origin_rotated;
703
704     // Translate back into the space of Q1 (but scaled and rotated).
705     const T x1_rotated_scaled = x1_origin_rotated_scaled + q1.Centroid()(0);
706     const T y1_rotated_scaled = y1_origin_rotated_scaled + q1.Centroid()(1);
707
708     // Translate into the space of Q2.
709     *x2 = x1_rotated_scaled + warp_parameters[0];
710     *y2 = y1_rotated_scaled + warp_parameters[1];
711   }
712
713   // Translation x, translation y, rotation about the center of Q1 degrees,
714   // scale.
715   enum { NUM_PARAMETERS = 4 };
716   double parameters[NUM_PARAMETERS];
717
718   Quad q1;
719 };
720
721 struct AffineWarp {
722   AffineWarp(const double *x1, const double *y1,
723              const double *x2, const double *y2)
724       : q1(x1, y1) {
725     Quad q2(x2, y2);
726
727     // The difference in centroids is the best guess for translation.
728     Vec2 t = q2.Centroid() - q1.Centroid();
729     parameters[0] = t[0];
730     parameters[1] = t[1];
731
732     // Estimate the four affine parameters with the usual least squares.
733     Mat Q1(8, 4);
734     Vec Q2(8);
735     for (int i = 0; i < 4; ++i) {
736       Vec2 v1 = q1.CornerRelativeToCentroid(i);
737       Vec2 v2 = q2.CornerRelativeToCentroid(i);
738
739       Q1.row(2 * i + 0) << v1[0], v1[1],   0,     0  ;
740       Q1.row(2 * i + 1) <<   0,     0,   v1[0], v1[1];
741
742       Q2(2 * i + 0) = v2[0];
743       Q2(2 * i + 1) = v2[1];
744     }
745
746     // TODO(keir): Check solution quality.
747     Vec4 a = Q1.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Q2);
748     parameters[2] = a[0];
749     parameters[3] = a[1];
750     parameters[4] = a[2];
751     parameters[5] = a[3];
752
753     LG << "a:" << a.transpose();
754     LG << "t:" << t.transpose();
755   }
756
757   // See comments in other parameterizations about why the centroid is used.
758   template<typename T>
759   void Forward(const T *p, const T &x1, const T& y1, T *x2, T* y2) const {
760     // Make the centroid of Q1 the origin.
761     const T x1_origin = x1 - q1.Centroid()(0);
762     const T y1_origin = y1 - q1.Centroid()(1);
763
764     // Apply the affine transformation.
765     const T x1_origin_affine = p[2] * x1_origin + p[3] * y1_origin;
766     const T y1_origin_affine = p[4] * x1_origin + p[5] * y1_origin;
767
768     // Translate back into the space of Q1 (but affine transformed).
769     const T x1_affine = x1_origin_affine + q1.Centroid()(0);
770     const T y1_affine = y1_origin_affine + q1.Centroid()(1);
771
772     // Translate into the space of Q2.
773     *x2 = x1_affine + p[0];
774     *y2 = y1_affine + p[1];
775   }
776
777   // Translation x, translation y, rotation about the center of Q1 degrees,
778   // scale.
779   enum { NUM_PARAMETERS = 6 };
780   double parameters[NUM_PARAMETERS];
781
782   Quad q1;
783 };
784
785 struct HomographyWarp {
786   HomographyWarp(const double *x1, const double *y1,
787                  const double *x2, const double *y2) {
788     Mat quad1(2, 4);
789     quad1 << x1[0], x1[1], x1[2], x1[3],
790              y1[0], y1[1], y1[2], y1[3];
791
792     Mat quad2(2, 4);
793     quad2 << x2[0], x2[1], x2[2], x2[3],
794              y2[0], y2[1], y2[2], y2[3];
795
796     Mat3 H;
797     if (!Homography2DFromCorrespondencesLinear(quad1, quad2, &H, 1e-12)) {
798       LG << "Couldn't construct homography.";
799     }
800
801     // Assume H(2, 2) != 0, and fix scale at H(2, 2) == 1.0.
802     H /= H(2, 2);
803
804     // Assume H is close to identity, so subtract out the diagonal.
805     H(0, 0) -= 1.0;
806     H(1, 1) -= 1.0;
807
808     CHECK_NE(H(2, 2), 0.0) << H;
809     for (int i = 0; i < 8; ++i) {
810       parameters[i] = H(i / 3, i % 3);
811       LG << "Parameters[" << i << "]: " << parameters[i];
812     }
813   }
814
815   template<typename T>
816   static void Forward(const T *p,
817                       const T &x1, const T& y1, T *x2, T* y2) {
818     // Homography warp with manual 3x3 matrix multiply.
819     const T xx2 = (1.0 + p[0]) * x1 +     p[1]     * y1 + p[2];
820     const T yy2 =     p[3]     * x1 + (1.0 + p[4]) * y1 + p[5];
821     const T zz2 =     p[6]     * x1 +     p[7]     * y1 + 1.0;
822     *x2 = xx2 / zz2;
823     *y2 = yy2 / zz2;
824   }
825
826   enum { NUM_PARAMETERS = 8 };
827   double parameters[NUM_PARAMETERS];
828 };
829
830 // Determine the number of samples to use for x and y. Quad winding goes:
831 //
832 //    0 1
833 //    3 2
834 //
835 // The idea is to take the maximum x or y distance. This may be oversampling.
836 // TODO(keir): Investigate the various choices; perhaps average is better?
837 void PickSampling(const double *x1, const double *y1,
838                   const double *x2, const double *y2,
839                   int *num_samples_x, int *num_samples_y) {
840   Vec2 a0(x1[0], y1[0]);
841   Vec2 a1(x1[1], y1[1]);
842   Vec2 a2(x1[2], y1[2]);
843   Vec2 a3(x1[3], y1[3]);
844
845   Vec2 b0(x1[0], y1[0]);
846   Vec2 b1(x1[1], y1[1]);
847   Vec2 b2(x1[2], y1[2]);
848   Vec2 b3(x1[3], y1[3]);
849
850   double x_dimensions[4] = {
851     (a1 - a0).norm(),
852     (a3 - a2).norm(),
853     (b1 - b0).norm(),
854     (b3 - b2).norm()
855   };
856
857   double y_dimensions[4] = {
858     (a3 - a0).norm(),
859     (a1 - a2).norm(),
860     (b3 - b0).norm(),
861     (b1 - b2).norm()
862   };
863   const double kScaleFactor = 1.0;
864   *num_samples_x = static_cast<int>(
865       kScaleFactor * *std::max_element(x_dimensions, x_dimensions + 4));
866   *num_samples_y = static_cast<int>(
867       kScaleFactor * *std::max_element(y_dimensions, y_dimensions + 4));
868   LG << "Automatic num_samples_x: " << *num_samples_x
869      << ", num_samples_y: " << *num_samples_y;
870 }
871
872 bool SearchAreaTooBigForDescent(const FloatImage &image2,
873                                 const double *x2, const double *y2) {
874   // TODO(keir): Check the bounds and enable only when it makes sense.
875   return true;
876 }
877
878 bool PointOnRightHalfPlane(const Vec2 &a, const Vec2 &b, double x, double y) {
879   Vec2 ba = b - a;
880   return ((Vec2(x, y) - b).transpose() * Vec2(-ba.y(), ba.x())) > 0;
881 }
882
883 // Determine if a point is in a quad. The quad is arranged as:
884 //
885 //    +-------> x
886 //    |
887 //    |  a0------a1
888 //    |   |       |
889 //    |   |       |
890 //    |   |       |
891 //    |  a3------a2
892 //    v
893 //    y
894 //
895 // The implementation does up to four half-plane comparisons.
896 bool PointInQuad(const double *xs, const double *ys, double x, double y) {
897   Vec2 a0(xs[0], ys[0]);
898   Vec2 a1(xs[1], ys[1]);
899   Vec2 a2(xs[2], ys[2]);
900   Vec2 a3(xs[3], ys[3]);
901
902   return PointOnRightHalfPlane(a0, a1, x, y) &&
903          PointOnRightHalfPlane(a1, a2, x, y) &&
904          PointOnRightHalfPlane(a2, a3, x, y) &&
905          PointOnRightHalfPlane(a3, a0, x, y);
906 }
907
908 // This makes it possible to map between Eigen float arrays and FloatImage
909 // without using comparisons.
910 typedef Eigen::Array<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> FloatArray;
911
912 // This creates a pattern in the frame of image2, from the pixel is image1,
913 // based on the initial guess represented by the two quads x1, y1, and x2, y2.
914 template<typename Warp>
915 void CreateBrutePattern(const double *x1, const double *y1,
916                         const double *x2, const double *y2,
917                         const FloatImage &image1,
918                         const FloatImage *image1_mask,
919                         FloatArray *pattern,
920                         FloatArray *mask,
921                         int *origin_x,
922                         int *origin_y) {
923   // Get integer bounding box of quad2 in image2.
924   int min_x = static_cast<int>(floor(*std::min_element(x2, x2 + 4)));
925   int min_y = static_cast<int>(floor(*std::min_element(y2, y2 + 4)));
926   int max_x = static_cast<int>(ceil (*std::max_element(x2, x2 + 4)));
927   int max_y = static_cast<int>(ceil (*std::max_element(y2, y2 + 4)));
928
929   int w = max_x - min_x;
930   int h = max_y - min_y;
931
932   pattern->resize(h, w);
933   mask->resize(h, w);
934
935   Warp inverse_warp(x2, y2, x1, y1);
936
937   // r,c are in the coordinate frame of image2.
938   for (int r = min_y; r < max_y; ++r) {
939     for (int c = min_x; c < max_x; ++c) {
940       // i and j are in the coordinate frame of the pattern in image2.
941       int i = r - min_y;
942       int j = c - min_x;
943
944       double dst_x = c;
945       double dst_y = r;
946       double src_x;
947       double src_y;
948       inverse_warp.Forward(inverse_warp.parameters,
949                            dst_x, dst_y,
950                            &src_x, &src_y);
951       
952       if (PointInQuad(x1, y1, src_x, src_y)) {
953         (*pattern)(i, j) = SampleLinear(image1, src_y, src_x);
954         (*mask)(i, j) = 1.0;
955         if (image1_mask) {
956           (*mask)(i, j) = SampleLinear(*image1_mask, src_y, src_x);;
957         }
958       } else {
959         (*pattern)(i, j) = 0.0;
960         (*mask)(i, j) = 0.0;
961       }
962     }
963   }
964   *origin_x = min_x;
965   *origin_y = min_y;
966 }
967
968 // Compute a translation-only estimate of the warp, using brute force search. A
969 // smarter implementation would use the FFT to compute the normalized cross
970 // correlation. Instead, this is a dumb implementation. Surprisingly, it is
971 // fast enough in practice.
972 //
973 // TODO(keir): The normalization is less effective for the brute force search
974 // than it is with the Ceres solver. It's unclear if this is a bug or due to
975 // the original frame being too different from the reprojected reference in the
976 // destination frame.
977 //
978 // The likely solution is to use the previous frame, instead of the original
979 // pattern, when doing brute initialization. Unfortunately that implies a
980 // totally different warping interface, since access to more than a the source
981 // and current destination frame is necessary.
982 template<typename Warp>
983 void BruteTranslationOnlyInitialize(const FloatImage &image1,
984                                     const FloatImage *image1_mask,
985                                     const FloatImage &image2,
986                                     const int num_extra_points,
987                                     const bool use_normalized_intensities,
988                                     const double *x1, const double *y1,
989                                     double *x2, double *y2) {
990   // Create the pattern to match in the space of image2, assuming our inital
991   // guess isn't too far from the template in image1. If there is no image1
992   // mask, then the resulting mask is binary.
993   FloatArray pattern;
994   FloatArray mask;
995   int origin_x = -1, origin_y = -1;
996   CreateBrutePattern<Warp>(x1, y1, x2, y2,
997                            image1, image1_mask,
998                            &pattern, &mask,
999                            &origin_x, &origin_y);
1000
1001   // For normalization, premultiply the pattern by the inverse pattern mean.
1002   double mask_sum = 1.0;
1003   if (use_normalized_intensities) {
1004     mask_sum = mask.sum();
1005     double inverse_pattern_mean = mask_sum / ((mask * pattern).sum());
1006     pattern *= inverse_pattern_mean;
1007   }
1008
1009   // Use Eigen on the images via maps for strong vectorization.
1010   Map<const FloatArray> search(image2.Data(), image2.Height(), image2.Width());
1011
1012   // Try all possible locations inside the search area. Yes, everywhere.
1013   //
1014   // TODO(keir): There are a number of possible optimizations here. One choice
1015   // is to make a grid and only try one out of every N possible samples.
1016   // 
1017   // Another, slightly more clever idea, is to compute some sort of spatial
1018   // frequency distribution of the pattern patch. If the spatial resolution is
1019   // high (e.g. a grating pattern or fine lines) then checking every possible
1020   // translation is necessary, since a 1-pixel shift may induce a massive
1021   // change in the cost function. If the image is a blob or splotch with blurry
1022   // edges, then fewer samples are necessary since a few pixels offset won't
1023   // change the cost function much.
1024   double best_sad = std::numeric_limits<double>::max();
1025   int best_r = -1;
1026   int best_c = -1;
1027   int w = pattern.cols();
1028   int h = pattern.rows();
1029   for (int r = 0; r < (image2.Height() - h); ++r) {
1030     for (int c = 0; c < (image2.Width() - w); ++c) {
1031       // Compute the weighted sum of absolute differences, Eigen style. Note
1032       // that the block from the search image is never stored in a variable, to
1033       // avoid copying overhead and permit inlining.
1034       double sad;
1035       if (use_normalized_intensities) {
1036         // TODO(keir): It's really dumb to recompute the search mean for every
1037         // shift. A smarter implementation would use summed area tables
1038         // instead, reducing the mean calculation to an O(1) operation.
1039         double inverse_search_mean =
1040             mask_sum / ((mask * search.block(r, c, h, w)).sum());
1041         sad = (mask * (pattern - (search.block(r, c, h, w) *
1042                                   inverse_search_mean))).abs().sum();
1043       } else {
1044         sad = (mask * (pattern - search.block(r, c, h, w))).abs().sum();
1045       }
1046       if (sad < best_sad) {
1047         best_r = r;
1048         best_c = c;
1049         best_sad = sad;
1050       }
1051     }
1052   }
1053   CHECK_NE(best_r, -1);
1054   CHECK_NE(best_c, -1);
1055
1056   LG << "Brute force translation found a shift. "
1057      << "best_c: " << best_c << ", best_r: " << best_r << ", "
1058      << "origin_x: " << origin_x << ", origin_y: " << origin_y << ", "
1059      << "dc: " << (best_c - origin_x) << ", "
1060      << "dr: " << (best_r - origin_y)
1061      << ", tried " << ((image2.Height() - h) * (image2.Width() - w))
1062      << " shifts.";
1063
1064   // Apply the shift.
1065   for (int i = 0; i < 4 + num_extra_points; ++i) {
1066     x2[i] += best_c - origin_x;
1067     y2[i] += best_r - origin_y;
1068   }
1069 }
1070
1071 }  // namespace
1072
1073 template<typename Warp>
1074 void TemplatedTrackRegion(const FloatImage &image1,
1075                           const FloatImage &image2,
1076                           const double *x1, const double *y1,
1077                           const TrackRegionOptions &options,
1078                           double *x2, double *y2,
1079                           TrackRegionResult *result) {
1080   for (int i = 0; i < 4; ++i) {
1081     LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); guess ("
1082        << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
1083        << (y2[i] - y1[i]) << ").";
1084   }
1085   if (options.use_normalized_intensities) {
1086     LG << "Using normalized intensities.";
1087   }
1088
1089   // Bail early if the points are already outside.
1090   if (!AllInBounds(image1, x1, y1)) {
1091     result->termination = TrackRegionResult::SOURCE_OUT_OF_BOUNDS;
1092     return;
1093   }
1094   if (!AllInBounds(image2, x2, y2)) {
1095     result->termination = TrackRegionResult::DESTINATION_OUT_OF_BOUNDS;
1096     return;
1097   }
1098   // TODO(keir): Check quads to ensure there is some area.
1099
1100   // Prepare the image and gradient.
1101   Array3Df image_and_gradient1;
1102   Array3Df image_and_gradient2;
1103   BlurredImageAndDerivativesChannels(image1, options.sigma,
1104                                      &image_and_gradient1);
1105   BlurredImageAndDerivativesChannels(image2, options.sigma,
1106                                      &image_and_gradient2);
1107
1108   // Possibly do a brute-force translation-only initialization.
1109   if (SearchAreaTooBigForDescent(image2, x2, y2) &&
1110       options.use_brute_initialization) {
1111     LG << "Running brute initialization...";
1112     BruteTranslationOnlyInitialize<Warp>(image_and_gradient1,
1113                                          options.image1_mask,
1114                                          image2,
1115                                          options.num_extra_points,
1116                                          options.use_normalized_intensities,
1117                                          x1, y1, x2, y2);
1118     for (int i = 0; i < 4; ++i) {
1119       LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); brute ("
1120          << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i])
1121          << ", " << (y2[i] - y1[i]) << ").";
1122     }
1123   }
1124
1125   // Prepare the initial warp parameters from the four correspondences.
1126   // Note: This must happen after the brute initialization runs, since the
1127   // brute initialization mutates x2 and y2 in place.
1128   Warp warp(x1, y1, x2, y2);
1129
1130   // Decide how many samples to use in the x and y dimensions.
1131   int num_samples_x;
1132   int num_samples_y;
1133   PickSampling(x1, y1, x2, y2, &num_samples_x, &num_samples_y);
1134
1135
1136   // Compute the warp from rectangular coordinates.
1137   Mat3 canonical_homography = ComputeCanonicalHomography(x1, y1,
1138                                                          num_samples_x,
1139                                                          num_samples_y);
1140
1141   // Construct the warp cost function. AutoDiffCostFunction takes ownership.
1142   WarpCostFunctor<Warp> *warp_cost_function =
1143       new WarpCostFunctor<Warp>(options,
1144                                 image_and_gradient1,
1145                                 image_and_gradient2,
1146                                 canonical_homography,
1147                                 num_samples_x,
1148                                 num_samples_y,
1149                                 warp);
1150
1151   // Construct the problem with a single residual.
1152   ceres::Problem problem;
1153   problem.AddResidualBlock(
1154       new ceres::AutoDiffCostFunction<
1155           WarpCostFunctor<Warp>,
1156           ceres::DYNAMIC,
1157           Warp::NUM_PARAMETERS>(warp_cost_function,
1158                                 num_samples_x * num_samples_y),
1159       NULL,
1160       warp.parameters);
1161
1162   // Configure the solve.
1163   ceres::Solver::Options solver_options;
1164   solver_options.linear_solver_type = ceres::DENSE_QR;
1165   solver_options.max_num_iterations = options.max_iterations;
1166   solver_options.update_state_every_iteration = true;
1167   solver_options.parameter_tolerance = 1e-16;
1168   solver_options.function_tolerance = 1e-16;
1169
1170   // Prevent the corners from going outside the destination image.
1171   BoundaryCheckingCallback<Warp> callback(image2, warp, x1, y1);
1172   solver_options.callbacks.push_back(&callback);
1173
1174   // Run the solve.
1175   ceres::Solver::Summary summary;
1176   ceres::Solve(solver_options, &problem, &summary);
1177
1178   LG << "Summary:\n" << summary.FullReport();
1179
1180   // Update the four points with the found solution; if the solver failed, then
1181   // the warp parameters are the identity (so ignore failure).
1182   //
1183   // Also warp any extra points on the end of the array.
1184   for (int i = 0; i < 4 + options.num_extra_points; ++i) {
1185     warp.Forward(warp.parameters, x1[i], y1[i], x2 + i, y2 + i);
1186     LG << "Warped point " << i << ": (" << x1[i] << ", " << y1[i] << ") -> ("
1187        << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
1188        << (y2[i] - y1[i]) << ").";
1189   }
1190
1191   // TODO(keir): Update the result statistics.
1192   // TODO(keir): Add a normalize-cross-correlation variant.
1193
1194   CHECK_NE(summary.termination_type, ceres::USER_ABORT) << "Libmv bug.";
1195   if (summary.termination_type == ceres::USER_ABORT) {
1196     result->termination = TrackRegionResult::FELL_OUT_OF_BOUNDS;
1197     return;
1198   }
1199 #define HANDLE_TERMINATION(termination_enum) \
1200   if (summary.termination_type == ceres::termination_enum) { \
1201     result->termination = TrackRegionResult::termination_enum; \
1202     return; \
1203   }
1204
1205   // Avoid computing correlation for tracking failures.
1206   HANDLE_TERMINATION(DID_NOT_RUN);
1207   HANDLE_TERMINATION(NUMERICAL_FAILURE);
1208
1209   // Otherwise, run a final correlation check.
1210   if (options.minimum_correlation > 0.0) {
1211     result->correlation = warp_cost_function->
1212           PearsonProductMomentCorrelationCoefficient(warp.parameters);
1213     if (result->correlation < options.minimum_correlation) {
1214       LG << "Failing with insufficient correlation.";
1215       result->termination = TrackRegionResult::INSUFFICIENT_CORRELATION;
1216       return;
1217     }
1218   }
1219
1220   HANDLE_TERMINATION(PARAMETER_TOLERANCE);
1221   HANDLE_TERMINATION(FUNCTION_TOLERANCE);
1222   HANDLE_TERMINATION(GRADIENT_TOLERANCE);
1223   HANDLE_TERMINATION(NO_CONVERGENCE);
1224 #undef HANDLE_TERMINATION
1225 };
1226
1227 void TrackRegion(const FloatImage &image1,
1228                  const FloatImage &image2,
1229                  const double *x1, const double *y1,
1230                  const TrackRegionOptions &options,
1231                  double *x2, double *y2,
1232                  TrackRegionResult *result) {
1233   // Enum is necessary due to templated nature of autodiff.
1234 #define HANDLE_MODE(mode_enum, mode_type) \
1235   if (options.mode == TrackRegionOptions::mode_enum) { \
1236     TemplatedTrackRegion<mode_type>(image1, image2, \
1237                                     x1, y1, \
1238                                     options, \
1239                                     x2, y2, \
1240                                     result); \
1241     return; \
1242   }
1243   HANDLE_MODE(TRANSLATION,                TranslationWarp);
1244   HANDLE_MODE(TRANSLATION_SCALE,          TranslationScaleWarp);
1245   HANDLE_MODE(TRANSLATION_ROTATION,       TranslationRotationWarp);
1246   HANDLE_MODE(TRANSLATION_ROTATION_SCALE, TranslationRotationScaleWarp);
1247   HANDLE_MODE(AFFINE,                     AffineWarp);
1248   HANDLE_MODE(HOMOGRAPHY,                 HomographyWarp);
1249 #undef HANDLE_MODE
1250 }
1251
1252 bool SamplePlanarPatch(const FloatImage &image,
1253                        const double *xs, const double *ys,
1254                        int num_samples_x, int num_samples_y,
1255                        FloatImage *patch) {
1256   // Bail early if the points are outside the image.
1257   if (!AllInBounds(image, xs, ys)) {
1258     LG << "Can't sample patch: out of bounds.";
1259     return false;
1260   }
1261
1262   // Make the patch have the appropriate size, and match the depth of image.
1263   patch->Resize(num_samples_y, num_samples_x, image.Depth());
1264
1265   // Compute the warp from rectangular coordinates.
1266   Mat3 canonical_homography = ComputeCanonicalHomography(xs, ys,
1267                                                          num_samples_x,
1268                                                          num_samples_y);
1269
1270   // Walk over the coordinates in the canonical space, sampling from the image
1271   // in the original space and copying the result into the patch.
1272   for (int r = 0; r < num_samples_y; ++r) {
1273     for (int c = 0; c < num_samples_x; ++c) {
1274       Vec3 image_position = canonical_homography * Vec3(c, r, 1);
1275       image_position /= image_position(2);
1276       SampleLinear(image, image_position(1),
1277                    image_position(0),
1278                    &(*patch)(r, c, 0));
1279     }
1280   }
1281   return true;
1282 }
1283
1284 }  // namespace libmv