0dac56aeff70908e11d19d670765410307a5fb0c
[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 // Necessary for M_E when building with MSVC.
24 #define _USE_MATH_DEFINES
25
26 #include "libmv/tracking/track_region.h"
27
28 #include <Eigen/SVD>
29 #include <Eigen/QR>
30 #include <iostream>
31 #include "ceres/ceres.h"
32 #include "libmv/logging/logging.h"
33 #include "libmv/image/image.h"
34 #include "libmv/image/sample.h"
35 #include "libmv/image/convolve.h"
36 #include "libmv/multiview/homography.h"
37 #include "libmv/numeric/numeric.h"
38
39 namespace libmv {
40
41 TrackRegionOptions::TrackRegionOptions()
42     : mode(TRANSLATION),
43       minimum_correlation(0),
44       max_iterations(20),
45       use_esm(true),
46       use_brute_initialization(true),
47       sigma(0.9),
48       num_extra_points(0),
49       image1_mask(NULL) {
50 }
51
52 namespace {
53
54 // TODO(keir): Consider adding padding.
55 template<typename T>
56 bool InBounds(const FloatImage &image,
57               const T &x,
58               const T &y) {
59   return 0.0 <= x && x < image.Width() &&
60          0.0 <= y && y < image.Height();
61 }
62
63 bool AllInBounds(const FloatImage &image,
64                  const double *x,
65                  const double *y) {
66   for (int i = 0; i < 4; ++i) {
67     if (!InBounds(image, x[i], y[i])) {
68       return false;
69     }
70   }
71   return true;
72 }
73
74 // Because C++03 doesn't support partial template specializations for
75 // functions, but at the same time member function specializations are not
76 // supported, the sample function must be inside a template-specialized class
77 // with a non-templated static member.
78
79 // The "AutoDiff::Sample()" function allows sampling an image at an x, y
80 // position such that if x and y are jets, then the derivative information is
81 // correctly propagated.
82
83 // Empty default template.
84 template<typename T>
85 struct AutoDiff {
86   // Sample only the image when the coordinates are scalars.
87   static T Sample(const FloatImage &image_and_gradient,
88                   const T &x, const T &y) {
89     return SampleLinear(image_and_gradient, y, x, 0);
90   }
91
92   static void SetScalarPart(double scalar, T *value) {
93     *value = scalar;
94   }
95   static void ScaleDerivative(double scale_by, T *value) {
96     // For double, there is no derivative to scale.
97   }
98 };
99
100 // Sample the image and gradient when the coordinates are jets, applying the
101 // jacobian appropriately to propagate the derivatives from the coordinates.
102 template<>
103 template<typename T, int N>
104 struct AutoDiff<ceres::Jet<T, N> > {
105   static ceres::Jet<T, N> Sample(const FloatImage &image_and_gradient,
106                                  const ceres::Jet<T, N> &x,
107                                  const ceres::Jet<T, N> &y) {
108     // Sample the image and its derivatives in x and y. One way to think of
109     // this is that the image is a scalar function with a single vector
110     // argument, xy, of dimension 2. Call this s(xy).
111     const T s    = SampleLinear(image_and_gradient, y.a, x.a, 0);
112     const T dsdx = SampleLinear(image_and_gradient, y.a, x.a, 1);
113     const T dsdy = SampleLinear(image_and_gradient, y.a, x.a, 2);
114
115     // However, xy is itself a function of another variable ("z"); xy(z) =
116     // [x(z), y(z)]^T. What this function needs to return is "s", but with the
117     // derivative with respect to z attached to the jet. So combine the
118     // derivative part of x and y's jets to form a Jacobian matrix between x, y
119     // and z (i.e. dxy/dz).
120     Eigen::Matrix<T, 2, N> dxydz;
121     dxydz.row(0) = x.v.transpose();
122     dxydz.row(1) = y.v.transpose();
123
124     // Now apply the chain rule to obtain ds/dz. Combine the derivative with
125     // the scalar part to obtain s with full derivative information.
126     ceres::Jet<T, N> jet_s;
127     jet_s.a = s;
128     jet_s.v = Matrix<T, 1, 2>(dsdx, dsdy) * dxydz;
129     return jet_s;
130   }
131
132   static void SetScalarPart(double scalar, ceres::Jet<T, N> *value) {
133     value->a = scalar;
134   }
135   static void ScaleDerivative(double scale_by, ceres::Jet<T, N> *value) {
136     value->v *= scale_by;
137   }
138 };
139
140 template<typename Warp>
141 class BoundaryCheckingCallback : public ceres::IterationCallback {
142  public:
143   BoundaryCheckingCallback(const FloatImage& image2,
144                            const Warp &warp,
145                            const double *x1, const double *y1)
146       : image2_(image2), warp_(warp), x1_(x1), y1_(y1) {}
147
148   virtual ceres::CallbackReturnType operator()(
149       const ceres::IterationSummary& summary) {
150     // Warp the original 4 points with the current warp into image2.
151     double x2[4];
152     double y2[4];
153     for (int i = 0; i < 4; ++i) {
154       warp_.Forward(warp_.parameters, x1_[i], y1_[i], x2 + i, y2 + i);
155     }
156     // Enusre they are all in bounds.
157     if (!AllInBounds(image2_, x2, y2)) {
158       return ceres::SOLVER_ABORT;
159     }
160     return ceres::SOLVER_CONTINUE;
161   }
162
163  private:
164   const FloatImage &image2_;
165   const Warp &warp_;
166   const double *x1_;
167   const double *y1_;
168 };
169
170 template<typename Warp>
171 class WarpCostFunctor {
172  public:
173   WarpCostFunctor(const TrackRegionOptions &options,
174                   const FloatImage &image_and_gradient1,
175                   const FloatImage &image_and_gradient2,
176                   const Mat3 &canonical_to_image1,
177                   int num_samples_x,
178                   int num_samples_y,
179                   const Warp &warp)
180       : options_(options),
181         image_and_gradient1_(image_and_gradient1),       
182         image_and_gradient2_(image_and_gradient2),       
183         canonical_to_image1_(canonical_to_image1),
184         num_samples_x_(num_samples_x),
185         num_samples_y_(num_samples_y),
186         warp_(warp) {}
187
188   template<typename T>
189   bool operator()(const T *warp_parameters, T *residuals) const {
190     for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
191       VLOG(2) << "warp_parameters[" << i << "]: " << warp_parameters[i];
192     }
193
194     int cursor = 0;
195     for (int r = 0; r < num_samples_y_; ++r) {
196       for (int c = 0; c < num_samples_x_; ++c) {
197         // Compute the location of the source pixel (via homography).
198         Vec3 image1_position = canonical_to_image1_ * Vec3(c, r, 1);
199         image1_position /= image1_position(2);
200         
201         // Compute the location of the destination pixel.
202         T image2_position[2];
203         warp_.Forward(warp_parameters,
204                       T(image1_position[0]),
205                       T(image1_position[1]),
206                       &image2_position[0],
207                       &image2_position[1]);
208
209
210         // Sample the destination, propagating derivatives.
211         T dst_sample = AutoDiff<T>::Sample(image_and_gradient2_,
212                                            image2_position[0],
213                                            image2_position[1]);
214
215         // Sample the source. This is made complicated by ESM mode.
216         T src_sample;
217         if (options_.use_esm) {
218           // In ESM mode, the derivative of the source is also taken into
219           // account. This changes the linearization in a way that causes
220           // better convergence. Copy the derivative of the warp parameters
221           // onto the jets for the image1 position. This is the ESM hack.
222           T image1_position_x = image2_position[0];
223           T image1_position_y = image2_position[1];
224           AutoDiff<T>::SetScalarPart(image1_position[0], &image1_position_x);
225           AutoDiff<T>::SetScalarPart(image1_position[1], &image1_position_y);
226           src_sample = AutoDiff<T>::Sample(image_and_gradient1_,
227                                            image1_position_x,
228                                            image1_position_y);
229
230           // The jacobians for these should be averaged. Due to the subtraction
231           // below, flip the sign of the src derivative so that the effect
232           // after subtraction of the jets is that they are averaged.
233           AutoDiff<T>::ScaleDerivative(-0.5, &src_sample);
234           AutoDiff<T>::ScaleDerivative(0.5, &dst_sample);
235         } else {
236           // This is the traditional, forward-mode KLT solution.
237           src_sample = T(AutoDiff<double>::Sample(image_and_gradient1_,
238                                                   image1_position[0],
239                                                   image1_position[1]));
240         }
241
242         // The difference is the error.
243         T error = src_sample - dst_sample;
244
245         // Weight the error by the mask, if one is present.
246         if (options_.image1_mask != NULL) {
247           error *= T(AutoDiff<double>::Sample(*options_.image1_mask,
248                                               image1_position[0],
249                                               image1_position[1]));
250         }
251         residuals[cursor++] = src_sample - dst_sample;
252       }
253     }
254     return true;
255   }
256
257  // TODO(keir): Consider also computing the cost here.
258  double PearsonProductMomentCorrelationCoefficient(
259      const double *warp_parameters) const {
260    for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
261      VLOG(2) << "Correlation warp_parameters[" << i << "]: "
262              << warp_parameters[i];
263    }
264
265    // The single-pass PMCC computation is somewhat numerically unstable, but
266    // it's sufficient for the tracker.
267    double sX = 0, sY = 0, sXX = 0, sYY = 0, sXY = 0;
268
269    // Due to masking, it's important to account for fractional samples.
270    // Samples with a 50% mask get counted as a half sample.
271    double num_samples = 0;
272
273    for (int r = 0; r < num_samples_y_; ++r) {
274      for (int c = 0; c < num_samples_x_; ++c) {
275         // Compute the location of the source pixel (via homography).
276         // TODO(keir): Cache these projections.
277         Vec3 image1_position = canonical_to_image1_ * Vec3(c, r, 1);
278         image1_position /= image1_position(2);
279         
280         // Compute the location of the destination pixel.
281         double image2_position[2];
282         warp_.Forward(warp_parameters,
283                       image1_position[0],
284                       image1_position[1],
285                       &image2_position[0],
286                       &image2_position[1]);
287
288         double x = AutoDiff<double>::Sample(image_and_gradient2_,
289                                             image2_position[0],
290                                             image2_position[1]);
291
292         double y = AutoDiff<double>::Sample(image_and_gradient1_,
293                                             image1_position[0],
294                                             image1_position[1]);
295
296         // Weight the signals by the mask, if one is present.
297         if (options_.image1_mask != NULL) {
298           double mask_value = AutoDiff<double>::Sample(*options_.image1_mask,
299                                                        image1_position[0],
300                                                        image1_position[1]);
301           x *= mask_value;
302           y *= mask_value;
303           num_samples += mask_value;
304         } else {
305           num_samples++;
306         }
307         sX += x;
308         sY += y;
309         sXX += x*x;
310         sYY += y*y;
311         sXY += x*y;
312       }
313     }
314     // Normalize.
315     sX /= num_samples;
316     sY /= num_samples;
317     sXX /= num_samples;
318     sYY /= num_samples;
319     sXY /= num_samples;
320
321     double var_x = sXX - sX*sX;
322     double var_y = sYY - sY*sY;
323     double covariance_xy = sXY - sX*sY;
324
325     double correlation = covariance_xy / sqrt(var_x * var_y);
326     LG << "Covariance xy: " << covariance_xy
327        << ", var 1: " << var_x << ", var 2: " << var_y
328        << ", correlation: " << correlation;
329     return correlation;
330   }
331
332  private:
333   const TrackRegionOptions &options_;
334   const FloatImage &image_and_gradient1_;
335   const FloatImage &image_and_gradient2_;
336   const Mat3 &canonical_to_image1_;
337   int num_samples_x_;
338   int num_samples_y_;
339   const Warp &warp_;
340 };
341
342 // Compute the warp from rectangular coordinates, where one corner is the
343 // origin, and the opposite corner is at (num_samples_x, num_samples_y).
344 Mat3 ComputeCanonicalHomography(const double *x1,
345                                 const double *y1,
346                                 int num_samples_x,
347                                 int num_samples_y) {
348   Mat canonical(2, 4);
349   canonical << 0, num_samples_x, num_samples_x, 0,
350                0, 0,             num_samples_y, num_samples_y;
351
352   Mat xy1(2, 4);
353   xy1 << x1[0], x1[1], x1[2], x1[3],
354          y1[0], y1[1], y1[2], y1[3];
355
356   Mat3 H;
357   if (!Homography2DFromCorrespondencesLinear(canonical, xy1, &H, 1e-12)) {
358     LG << "Couldn't construct homography.";
359   }
360   return H;
361 }
362
363 class Quad {
364  public:
365   Quad(const double *x, const double *y) : x_(x), y_(y) {
366     // Compute the centroid and store it.
367     centroid_ = Vec2(0.0, 0.0);
368     for (int i = 0; i < 4; ++i) {
369       centroid_ += Vec2(x_[i], y_[i]);
370     }
371     centroid_ /= 4.0;
372   }
373
374   // The centroid of the four points representing the quad.
375   const Vec2& Centroid() const {
376     return centroid_;
377   }
378
379   // The average magnitude of the four points relative to the centroid.
380   double Scale() const {
381     double scale = 0.0;
382     for (int i = 0; i < 4; ++i) {
383       scale += (Vec2(x_[i], y_[i]) - Centroid()).norm();
384     }
385     return scale / 4.0;
386   }
387
388   Vec2 CornerRelativeToCentroid(int i) const {
389     return Vec2(x_[i], y_[i]) - centroid_;
390   }
391
392  private:
393   const double *x_;
394   const double *y_;
395   Vec2 centroid_;
396 };
397
398 struct TranslationWarp {
399   TranslationWarp(const double *x1, const double *y1,
400                   const double *x2, const double *y2) {
401     Vec2 t = Quad(x2, y2).Centroid() - Quad(x1, y1).Centroid() ;
402     parameters[0] = t[0];
403     parameters[1] = t[1];
404   }
405
406   template<typename T>
407   void Forward(const T *warp_parameters,
408                const T &x1, const T& y1, T *x2, T* y2) const {
409     *x2 = x1 + warp_parameters[0];
410     *y2 = y1 + warp_parameters[1];
411   }
412
413   template<typename T>
414   void Backward(const T *warp_parameters,
415                 const T &x2, const T& y2, T *x1, T* y1) const {
416     *x1 = x2 - warp_parameters[0];
417     *y1 = y2 - warp_parameters[1];
418   }
419
420   // Translation x, translation y.
421   enum { NUM_PARAMETERS = 2 };
422   double parameters[NUM_PARAMETERS];
423 };
424
425 struct TranslationScaleWarp {
426   TranslationScaleWarp(const double *x1, const double *y1,
427                        const double *x2, const double *y2)
428       : q1(x1, y1) {
429     Quad q2(x2, y2);
430
431     // The difference in centroids is the best guess for translation.
432     Vec2 t = q2.Centroid() - q1.Centroid();
433     parameters[0] = t[0];
434     parameters[1] = t[1];
435
436     // The difference in scales is the estimate for the scale.
437     parameters[2] = 1.0 - q2.Scale() / q1.Scale();
438   }
439
440   // The strange way of parameterizing the translation and scaling is to make
441   // the knobs that the optimizer sees easy to adjust. This is less important
442   // for the scaling case than the rotation case.
443   template<typename T>
444   void Forward(const T *warp_parameters,
445                const T &x1, const T& y1, T *x2, T* y2) const {
446     // Make the centroid of Q1 the origin.
447     const T x1_origin = x1 - q1.Centroid()(0);
448     const T y1_origin = y1 - q1.Centroid()(1);
449
450     // Scale uniformly about the origin.
451     const T scale = 1.0 + warp_parameters[2];
452     const T x1_origin_scaled = scale * x1_origin;
453     const T y1_origin_scaled = scale * y1_origin;
454
455     // Translate back into the space of Q1 (but scaled).
456     const T x1_scaled = x1_origin_scaled + q1.Centroid()(0);
457     const T y1_scaled = y1_origin_scaled + q1.Centroid()(1);
458
459     // Translate into the space of Q2.
460     *x2 = x1_scaled + warp_parameters[0];
461     *y2 = y1_scaled + warp_parameters[1];
462   }
463
464   template<typename T>
465   void Backward(const T *warp_parameters,
466                 const T &x2, const T& y2, T *x1, T* y1) const {
467     // XXX
468   }
469
470   // Translation x, translation y, scale.
471   enum { NUM_PARAMETERS = 3 };
472   double parameters[NUM_PARAMETERS];
473
474   Quad q1;
475 };
476
477 // Assumes the given points are already zero-centroid and the same size.
478 Mat2 OrthogonalProcrustes(const Mat2 &correlation_matrix) {
479   Eigen::JacobiSVD<Mat2> svd(correlation_matrix,
480                              Eigen::ComputeFullU | Eigen::ComputeFullV);
481   return svd.matrixV() * svd.matrixU().transpose();
482 }
483
484 struct TranslationRotationWarp {
485   TranslationRotationWarp(const double *x1, const double *y1,
486                           const double *x2, const double *y2)
487       : q1(x1, y1) {
488     Quad q2(x2, y2);
489
490     // The difference in centroids is the best guess for translation.
491     Vec2 t = q2.Centroid() - q1.Centroid();
492     parameters[0] = t[0];
493     parameters[1] = t[1];
494
495     // Obtain the rotation via orthorgonal procrustes.
496     Mat2 correlation_matrix;
497     for (int i = 0; i < 4; ++i) {
498       correlation_matrix += q1.CornerRelativeToCentroid(i) * 
499                             q2.CornerRelativeToCentroid(i).transpose();
500     }
501     Mat2 R = OrthogonalProcrustes(correlation_matrix);
502     parameters[2] = acos(R(0, 0));
503
504     std::cout << "correlation_matrix:\n" << correlation_matrix << "\n";
505     std::cout << "R:\n" << R << "\n";
506     std::cout << "theta:" << parameters[2] << "\n";
507   }
508
509   // The strange way of parameterizing the translation and rotation is to make
510   // the knobs that the optimizer sees easy to adjust. The reason is that while
511   // it is always the case that it is possible to express composed rotations
512   // and translations as a single translation and rotation, the numerical
513   // values needed for the composition are often large in magnitude. This is
514   // enough to throw off any minimizer, since it must do the equivalent of
515   // compose rotations and translations.
516   //
517   // Instead, use the parameterization below that offers a parameterization
518   // that exposes the degrees of freedom in a way amenable to optimization.
519   template<typename T>
520   void Forward(const T *warp_parameters,
521                       const T &x1, const T& y1, T *x2, T* y2) const {
522     // Make the centroid of Q1 the origin.
523     const T x1_origin = x1 - q1.Centroid()(0);
524     const T y1_origin = y1 - q1.Centroid()(1);
525
526     // Rotate about the origin (i.e. centroid of Q1).
527     const T theta = warp_parameters[2];
528     const T costheta = cos(theta);
529     const T sintheta = sin(theta);
530     const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
531     const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
532
533     // Translate back into the space of Q1 (but scaled).
534     const T x1_rotated = x1_origin_rotated + q1.Centroid()(0);
535     const T y1_rotated = y1_origin_rotated + q1.Centroid()(1);
536
537     // Translate into the space of Q2.
538     *x2 = x1_rotated + warp_parameters[0];
539     *y2 = y1_rotated + warp_parameters[1];
540   }
541
542   template<typename T>
543   void Backward(const T *warp_parameters,
544                 const T &x2, const T& y2, T *x1, T* y1) const {
545     // XXX
546   }
547
548   // Translation x, translation y, rotation about the center of Q1 degrees.
549   enum { NUM_PARAMETERS = 3 };
550   double parameters[NUM_PARAMETERS];
551
552   Quad q1;
553 };
554
555 struct TranslationRotationScaleWarp {
556   TranslationRotationScaleWarp(const double *x1, const double *y1,
557                                const double *x2, const double *y2)
558       : q1(x1, y1) {
559     Quad q2(x2, y2);
560
561     // The difference in centroids is the best guess for translation.
562     Vec2 t = q2.Centroid() - q1.Centroid();
563     parameters[0] = t[0];
564     parameters[1] = t[1];
565
566     // The difference in scales is the estimate for the scale.
567     parameters[2] = 1.0 - q2.Scale() / q1.Scale();
568
569     // Obtain the rotation via orthorgonal procrustes.
570     Mat2 correlation_matrix;
571     for (int i = 0; i < 4; ++i) {
572       correlation_matrix += q1.CornerRelativeToCentroid(i) * 
573                             q2.CornerRelativeToCentroid(i).transpose();
574     }
575     std::cout << "correlation_matrix:\n" << correlation_matrix << "\n";
576     Mat2 R = OrthogonalProcrustes(correlation_matrix);
577     std::cout << "R:\n" << R << "\n";
578     parameters[3] = acos(R(0, 0));
579     std::cout << "theta:" << parameters[3] << "\n";
580   }
581
582   // The strange way of parameterizing the translation and rotation is to make
583   // the knobs that the optimizer sees easy to adjust. The reason is that while
584   // it is always the case that it is possible to express composed rotations
585   // and translations as a single translation and rotation, the numerical
586   // values needed for the composition are often large in magnitude. This is
587   // enough to throw off any minimizer, since it must do the equivalent of
588   // compose rotations and translations.
589   //
590   // Instead, use the parameterization below that offers a parameterization
591   // that exposes the degrees of freedom in a way amenable to optimization.
592   template<typename T>
593   void Forward(const T *warp_parameters,
594                       const T &x1, const T& y1, T *x2, T* y2) const {
595     // Make the centroid of Q1 the origin.
596     const T x1_origin = x1 - q1.Centroid()(0);
597     const T y1_origin = y1 - q1.Centroid()(1);
598
599     // Rotate about the origin (i.e. centroid of Q1).
600     const T theta = warp_parameters[3];
601     const T costheta = cos(theta);
602     const T sintheta = sin(theta);
603     const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
604     const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
605
606     // Scale uniformly about the origin.
607     const T scale = 1.0 + warp_parameters[2];
608     const T x1_origin_rotated_scaled = scale * x1_origin_rotated;
609     const T y1_origin_rotated_scaled = scale * y1_origin_rotated;
610
611     // Translate back into the space of Q1 (but scaled and rotated).
612     const T x1_rotated_scaled = x1_origin_rotated_scaled + q1.Centroid()(0);
613     const T y1_rotated_scaled = y1_origin_rotated_scaled + q1.Centroid()(1);
614
615     // Translate into the space of Q2.
616     *x2 = x1_rotated_scaled + warp_parameters[0];
617     *y2 = y1_rotated_scaled + warp_parameters[1];
618   }
619
620   template<typename T>
621   void Backward(const T *warp_parameters,
622                 const T &x2, const T& y2, T *x1, T* y1) const {
623     // XXX
624   }
625
626   // Translation x, translation y, rotation about the center of Q1 degrees,
627   // scale.
628   enum { NUM_PARAMETERS = 4 };
629   double parameters[NUM_PARAMETERS];
630
631   Quad q1;
632 };
633
634 // TODO(keir): Finish affine warp.
635
636 struct HomographyWarp {
637   HomographyWarp(const double *x1, const double *y1,
638                  const double *x2, const double *y2) {
639     Mat quad1(2, 4);
640     quad1 << x1[0], x1[1], x1[2], x1[3],
641              y1[0], y1[1], y1[2], y1[3];
642
643     Mat quad2(2, 4);
644     quad2 << x2[0], x2[1], x2[2], x2[3],
645              y2[0], y2[1], y2[2], y2[3];
646
647     Mat3 H;
648     if (!Homography2DFromCorrespondencesLinear(quad1, quad2, &H, 1e-12)) {
649       LG << "Couldn't construct homography.";
650     }
651
652     // Assume H(2, 2) != 0, and fix scale at H(2, 2) == 1.0.
653     H /= H(2, 2);
654
655     // Assume H is close to identity, so subtract out the diagonal.
656     H(0, 0) -= 1.0;
657     H(1, 1) -= 1.0;
658
659     CHECK_NE(H(2, 2), 0.0) << H;
660     for (int i = 0; i < 8; ++i) {
661       parameters[i] = H(i / 3, i % 3);
662       LG << "Parameters[" << i << "]: " << parameters[i];
663     }
664   }
665
666   template<typename T>
667   static void Forward(const T *p,
668                       const T &x1, const T& y1, T *x2, T* y2) {
669     // Homography warp with manual 3x3 matrix multiply.
670     const T xx2 = (1.0 + p[0]) * x1 +     p[1]     * y1 + p[2];
671     const T yy2 =     p[3]     * x1 + (1.0 + p[4]) * y1 + p[5];
672     const T zz2 =     p[6]     * x1 +     p[7]     * y1 + 1.0;
673     *x2 = xx2 / zz2;
674     *y2 = yy2 / zz2;
675   }
676
677   template<typename T>
678   void Backward(const T *warp_parameters,
679                 const T &x2, const T& y2, T *x1, T* y1) const {
680     // XXX
681   }
682
683   enum { NUM_PARAMETERS = 8 };
684   double parameters[NUM_PARAMETERS];
685 };
686
687 // Determine the number of samples to use for x and y. Quad winding goes:
688 //
689 //    0 1
690 //    3 2
691 //
692 // The idea is to take the maximum x or y distance. This may be oversampling.
693 // TODO(keir): Investigate the various choices; perhaps average is better?
694 void PickSampling(const double *x1, const double *y1,
695                   const double *x2, const double *y2,
696                   int *num_samples_x, int *num_samples_y) {
697   Vec2 a0(x1[0], y1[0]);
698   Vec2 a1(x1[1], y1[1]);
699   Vec2 a2(x1[2], y1[2]);
700   Vec2 a3(x1[3], y1[3]);
701
702   Vec2 b0(x1[0], y1[0]);
703   Vec2 b1(x1[1], y1[1]);
704   Vec2 b2(x1[2], y1[2]);
705   Vec2 b3(x1[3], y1[3]);
706
707   double x_dimensions[4] = {
708     (a1 - a0).norm(),
709     (a3 - a2).norm(),
710     (b1 - b0).norm(),
711     (b3 - b2).norm()
712   };
713
714   double y_dimensions[4] = {
715     (a3 - a0).norm(),
716     (a1 - a2).norm(),
717     (b3 - b0).norm(),
718     (b1 - b2).norm()
719   };
720   const double kScaleFactor = 1.0;
721   *num_samples_x = static_cast<int>(
722       kScaleFactor * *std::max_element(x_dimensions, x_dimensions + 4));
723   *num_samples_y = static_cast<int>(
724       kScaleFactor * *std::max_element(y_dimensions, y_dimensions + 4));
725   LG << "Automatic num_samples_x: " << *num_samples_x
726      << ", num_samples_y: " << *num_samples_y;
727 }
728
729 bool SearchAreaTooBigForDescent(const FloatImage &image2,
730                                 const double *x2, const double *y2) {
731   // TODO(keir): Check the bounds and enable only when it makes sense.
732   return true;
733 }
734
735 bool PointOnRightHalfPlane(const Vec2 &a, const Vec2 &b, double x, double y) {
736   Vec2 ba = b - a;
737   return ((Vec2(x, y) - b).transpose() * Vec2(-ba.y(), ba.x())) > 0;
738 }
739
740 // Determine if a point is in a quad. The quad is arranged as:
741 //
742 //    +--> x
743 //    |
744 //    |  0 1
745 //    v  3 2
746 //    y
747 //
748 // The idea is to take the maximum x or y distance. This may be oversampling.
749 // TODO(keir): Investigate the various choices; perhaps average is better?
750 bool PointInQuad(const double *xs, const double *ys, double x, double y) {
751   Vec2 a0(xs[0], ys[0]);
752   Vec2 a1(xs[1], ys[1]);
753   Vec2 a2(xs[2], ys[2]);
754   Vec2 a3(xs[3], ys[3]);
755
756   return PointOnRightHalfPlane(a0, a1, x, y) &&
757          PointOnRightHalfPlane(a1, a2, x, y) &&
758          PointOnRightHalfPlane(a2, a3, x, y) &&
759          PointOnRightHalfPlane(a3, a0, x, y);
760 }
761
762 typedef Eigen::Array<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> FloatArray;
763
764 // This creates a pattern in the frame of image2, from the pixel is image1,
765 // based on the initial guess represented by the two quads x1, y1, and x2, y2.
766 template<typename Warp>
767 void CreateBrutePattern(const double *x1, const double *y1,
768                         const double *x2, const double *y2,
769                         const FloatImage &image1,
770                         const FloatImage *image1_mask,
771                         FloatArray *pattern,
772                         FloatArray *mask,
773                         int *origin_x,
774                         int *origin_y) {
775   // Get integer bounding box of quad2 in image2.
776   int min_x = static_cast<int>(floor(*std::min_element(x2, x2 + 4)));
777   int min_y = static_cast<int>(floor(*std::min_element(y2, y2 + 4)));
778   int max_x = static_cast<int>(ceil (*std::max_element(x2, x2 + 4)));
779   int max_y = static_cast<int>(ceil (*std::max_element(y2, y2 + 4)));
780
781   int w = max_x - min_x;
782   int h = max_y - min_y;
783
784   pattern->resize(h, w);
785   mask->resize(h, w);
786
787   Warp inverse_warp(x2, y2, x1, y1);
788
789   // r,c are in the coordinate frame of image2.
790   for (int r = min_y; r < max_y; ++r) {
791     for (int c = min_x; c < max_x; ++c) {
792       // i and j are in the coordinate frame of the pattern in image2.
793       int i = r - min_y;
794       int j = c - min_x;
795
796       double dst_x = c;
797       double dst_y = r;
798       double src_x;
799       double src_y;
800       inverse_warp.Forward(inverse_warp.parameters,
801                            dst_x, dst_y,
802                            &src_x, &src_y);
803       
804       if (PointInQuad(x1, y1, src_x, src_y)) {
805         (*pattern)(i, j) = SampleLinear(image1, src_y, src_x);
806         (*mask)(i, j) = 1.0;
807         if (image1_mask) {
808           (*mask)(i, j) = SampleLinear(*image1_mask, src_y, src_x);;
809         }
810       } else {
811         (*pattern)(i, j) = 0.0;
812         (*mask)(i, j) = 0.0;
813       }
814     }
815   }
816   *origin_x = min_x;
817   *origin_y = min_y;
818 }
819
820 template<typename Warp>
821 void BruteTranslationOnlyInitialize(const FloatImage &image1,
822                                     const FloatImage *image1_mask,
823                                     const FloatImage &image2,
824                                     const int num_extra_points,
825                                     const double *x1, const double *y1,
826                                     double *x2, double *y2) {
827   // Create the pattern to match in the space of image2, assuming our inital
828   // guess isn't too far from the template in image1. If there is no image1
829   // mask, then the resulting mask is binary.
830   FloatArray pattern;
831   FloatArray mask;
832   int origin_x = -1, origin_y = -1;
833   CreateBrutePattern<Warp>(x1, y1, x2, y2, image1, image1_mask,
834                            &pattern, &mask, &origin_x, &origin_y);
835
836   // Use Eigen on the images via maps for strong vectorization.
837   Map<const FloatArray> search(image2.Data(), image2.Height(), image2.Width());
838
839   // Try all possible locations inside the search area. Yes, everywhere.
840   //
841   // TODO(keir): There are a number of possible optimizations here. One choice
842   // is to make a grid and only try one out of every N possible samples.
843   // 
844   // Another, slightly more clever idea, is to compute some sort of spatial
845   // frequency distribution of the pattern patch. If the spatial resolution is
846   // high (e.g. a grating pattern or fine lines) then checking every possible
847   // translation is necessary, since a 1-pixel shift may induce a massive
848   // change in the cost function. If the image is a blob or splotch with blurry
849   // edges, then fewer samples are necessary since a few pixels offset won't
850   // change the cost function much.
851   double best_sad = std::numeric_limits<double>::max();
852   int best_r = -1;
853   int best_c = -1;
854   int w = pattern.cols();
855   int h = pattern.rows();
856   for (int r = 0; r < (image2.Height() - h); ++r) {
857     for (int c = 0; c < (image2.Width() - w); ++c) {
858       // Compute the weighted sum of absolute differences, Eigen style.
859       double sad = (mask * (pattern - search.block(r, c, h, w))).abs().sum();
860       if (sad < best_sad) {
861         best_r = r;
862         best_c = c;
863         best_sad = sad;
864       }
865     }
866   }
867   CHECK_NE(best_r, -1);
868   CHECK_NE(best_c, -1);
869
870   LG << "Brute force translation found a shift. "
871      << "best_c: " << best_c << ", best_r: " << best_r << ", "
872      << "origin_x: " << origin_x << ", origin_y: " << origin_y << ", "
873      << "dc: " << (best_c - origin_x) << ", "
874      << "dr: " << (best_r - origin_y)
875      << ", tried " << ((image2.Height() - h) * (image2.Width() - w))
876      << " shifts.";
877
878   // Apply the shift.
879   for (int i = 0; i < 4 + num_extra_points; ++i) {
880     x2[i] += best_c - origin_x;
881     y2[i] += best_r - origin_y;
882   }
883 }
884
885 }  // namespace
886
887 template<typename Warp>
888 void TemplatedTrackRegion(const FloatImage &image1,
889                           const FloatImage &image2,
890                           const double *x1, const double *y1,
891                           const TrackRegionOptions &options,
892                           double *x2, double *y2,
893                           TrackRegionResult *result) {
894   for (int i = 0; i < 4; ++i) {
895     LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); guess ("
896        << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
897        << (y2[i] - y1[i]) << ").";
898   }
899
900   // Bail early if the points are already outside.
901   if (!AllInBounds(image1, x1, y1)) {
902     result->termination = TrackRegionResult::SOURCE_OUT_OF_BOUNDS;
903     return;
904   }
905   if (!AllInBounds(image2, x2, y2)) {
906     result->termination = TrackRegionResult::DESTINATION_OUT_OF_BOUNDS;
907     return;
908   }
909   // TODO(keir): Check quads to ensure there is some area.
910
911   // Prepare the initial warp parameters from the four correspondences.
912   Warp warp(x1, y1, x2, y2);
913
914   // Decide how many samples to use in the x and y dimensions.
915   int num_samples_x;
916   int num_samples_y;
917   PickSampling(x1, y1, x2, y2, &num_samples_x, &num_samples_y);
918
919   // Compute the warp from rectangular coordinates.
920   Mat3 canonical_homography = ComputeCanonicalHomography(x1, y1,
921                                                          num_samples_x,
922                                                          num_samples_y);
923
924   // Prepare the image and gradient.
925   Array3Df image_and_gradient1;
926   Array3Df image_and_gradient2;
927   BlurredImageAndDerivativesChannels(image1, options.sigma,
928                                      &image_and_gradient1);
929   BlurredImageAndDerivativesChannels(image2, options.sigma,
930                                      &image_and_gradient2);
931
932   // Possibly do a brute-force translation-only initialization.
933   if (SearchAreaTooBigForDescent(image2, x2, y2) &&
934       options.use_brute_initialization) {
935     LG << "Running brute initialization...";
936     BruteTranslationOnlyInitialize<Warp>(image_and_gradient1,
937                                          options.image1_mask,
938                                          image2,
939                                          options.num_extra_points,
940                                          x1, y1, x2, y2);
941   }
942
943   ceres::Solver::Options solver_options;
944   solver_options.linear_solver_type = ceres::DENSE_QR;
945   solver_options.max_num_iterations = options.max_iterations;
946   solver_options.update_state_every_iteration = true;
947   solver_options.parameter_tolerance = 1e-16;
948   solver_options.function_tolerance = 1e-16;
949
950   // TODO(keir): Consider removing these options before committing.
951   solver_options.numeric_derivative_relative_step_size = 1e-3;
952   solver_options.gradient_check_relative_precision = 1e-10;
953   solver_options.minimizer_progress_to_stdout = false;
954
955   // Prevent the corners from going outside the destination image.
956   BoundaryCheckingCallback<Warp> callback(image2, warp, x1, y1);
957   solver_options.callbacks.push_back(&callback);
958
959   // Construct the warp cost function. AutoDiffCostFunction takes ownership.
960   WarpCostFunctor<Warp> *warp_cost_function =
961       new WarpCostFunctor<Warp>(options,
962                                 image_and_gradient1,
963                                 image_and_gradient2,
964                                 canonical_homography,
965                                 num_samples_x,
966                                 num_samples_y,
967                                 warp);
968
969   // Construct the problem with a single residual.
970   ceres::Problem::Options problem_options;
971   problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
972   ceres::Problem problem(problem_options);
973   problem.AddResidualBlock(
974       new ceres::AutoDiffCostFunction<
975           WarpCostFunctor<Warp>,
976           ceres::DYNAMIC,
977           Warp::NUM_PARAMETERS>(warp_cost_function,
978                                 num_samples_x * num_samples_y),
979       NULL,
980       warp.parameters);
981
982   ceres::Solver::Summary summary;
983   ceres::Solve(solver_options, &problem, &summary);
984
985   LG << "Summary:\n" << summary.FullReport();
986
987   // Update the four points with the found solution; if the solver failed, then
988   // the warp parameters are the identity (so ignore failure).
989   //
990   // Also warp any extra points on the end of the array.
991   for (int i = 0; i < 4 + options.num_extra_points; ++i) {
992     warp.Forward(warp.parameters, x1[i], y1[i], x2 + i, y2 + i);
993     LG << "Warped point " << i << ": (" << x1[i] << ", " << y1[i] << ") -> ("
994        << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
995        << (y2[i] - y1[i]) << ").";
996   }
997
998   // TODO(keir): Update the result statistics.
999   // TODO(keir): Add a normalize-cross-correlation variant.
1000
1001   CHECK_NE(summary.termination_type, ceres::USER_ABORT) << "Libmv bug.";
1002   if (summary.termination_type == ceres::USER_ABORT) {
1003     result->termination = TrackRegionResult::FELL_OUT_OF_BOUNDS;
1004     return;
1005   }
1006 #define HANDLE_TERMINATION(termination_enum) \
1007   if (summary.termination_type == ceres::termination_enum) { \
1008     result->termination = TrackRegionResult::termination_enum; \
1009     return; \
1010   }
1011
1012   // Avoid computing correlation for tracking failures.
1013   HANDLE_TERMINATION(DID_NOT_RUN);
1014   HANDLE_TERMINATION(NUMERICAL_FAILURE);
1015
1016   // Otherwise, run a final correlation check.
1017   if (options.minimum_correlation > 0.0) {
1018     result->correlation = warp_cost_function->
1019           PearsonProductMomentCorrelationCoefficient(warp.parameters);
1020     if (result->correlation < options.minimum_correlation) {
1021       LG << "Failing with insufficient correlation.";
1022       result->termination = TrackRegionResult::INSUFFICIENT_CORRELATION;
1023       return;
1024     }
1025   }
1026
1027   HANDLE_TERMINATION(PARAMETER_TOLERANCE);
1028   HANDLE_TERMINATION(FUNCTION_TOLERANCE);
1029   HANDLE_TERMINATION(GRADIENT_TOLERANCE);
1030   HANDLE_TERMINATION(NO_CONVERGENCE);
1031 #undef HANDLE_TERMINATION
1032 };
1033
1034 void TrackRegion(const FloatImage &image1,
1035                  const FloatImage &image2,
1036                  const double *x1, const double *y1,
1037                  const TrackRegionOptions &options,
1038                  double *x2, double *y2,
1039                  TrackRegionResult *result) {
1040   // Enum is necessary due to templated nature of autodiff.
1041 #define HANDLE_MODE(mode_enum, mode_type) \
1042   if (options.mode == TrackRegionOptions::mode_enum) { \
1043     TemplatedTrackRegion<mode_type>(image1, image2, \
1044                                     x1, y1, \
1045                                     options, \
1046                                     x2, y2, \
1047                                     result); \
1048     return; \
1049   }
1050
1051   HANDLE_MODE(TRANSLATION,                TranslationWarp);
1052   HANDLE_MODE(TRANSLATION_SCALE,          TranslationScaleWarp);
1053   HANDLE_MODE(TRANSLATION_ROTATION,       TranslationRotationWarp);
1054   HANDLE_MODE(TRANSLATION_ROTATION_SCALE, TranslationRotationScaleWarp);
1055   //HANDLE_MODE(AFFINE,                     AffineWarp);
1056   HANDLE_MODE(HOMOGRAPHY,                 HomographyWarp);
1057 #undef HANDLE_MODE
1058 }
1059
1060 }  // namespace libmv