1
// Copyright (c) 2012 libmv authors.
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:
10
// The above copyright notice and this permission notice shall be included in
11
// all copies or substantial portions of the Software.
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
21
// Author: mierle@gmail.com (Keir Mierle)
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
28
// Necessary for M_E when building with MSVC.
29
#define _USE_MATH_DEFINES
31
#include "libmv/tracking/track_region.h"
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"
44
// Expand the Jet functionality of Ceres to allow mixed numeric/autodiff.
46
// TODO(keir): Push this (or something similar) into upstream Ceres.
49
// A jet traits class to make it easier to work with mixed auto / numeric diff.
52
static bool IsScalar() {
55
static T GetScalar(const T& t) {
58
static void SetScalar(const T& scalar, T* t) {
61
static void ScaleDerivative(double scale_by, T *value) {
62
// For double, there is no derivative to scale.
66
template<typename T, int N>
67
struct JetOps<Jet<T, N> > {
68
static bool IsScalar() {
71
static T GetScalar(const Jet<T, N>& t) {
74
static void SetScalar(const T& scalar, Jet<T, N>* t) {
77
static void ScaleDerivative(double scale_by, Jet<T, N> *value) {
82
template<typename FunctionType, int kNumArgs, typename ArgumentType>
84
static ArgumentType Rule(const FunctionType &f,
85
const FunctionType dfdx[kNumArgs],
86
const ArgumentType x[kNumArgs]) {
87
// In the default case of scalars, there's nothing to do since there are no
88
// derivatives to propagate.
93
// XXX Add documentation here!
94
template<typename FunctionType, int kNumArgs, typename T, int N>
95
struct Chain<FunctionType, kNumArgs, Jet<T, N> > {
96
static Jet<T, N> Rule(const FunctionType &f,
97
const FunctionType dfdx[kNumArgs],
98
const Jet<T, N> x[kNumArgs]) {
99
// x is itself a function of another variable ("z"); what this function
100
// needs to return is "f", but with the derivative with respect to z
101
// attached to the jet. So combine the derivative part of x's jets to form
102
// a Jacobian matrix between x and z (i.e. dx/dz).
103
Eigen::Matrix<T, kNumArgs, N> dxdz;
104
for (int i = 0; i < kNumArgs; ++i) {
105
dxdz.row(i) = x[i].v.transpose();
108
// Map the input gradient dfdx into an Eigen row vector.
109
Eigen::Map<const Eigen::Matrix<FunctionType, 1, kNumArgs> >
110
vector_dfdx(dfdx, 1, kNumArgs);
112
// Now apply the chain rule to obtain df/dz. Combine the derivative with
113
// the scalar part to obtain f with full derivative information.
116
jet_f.v = vector_dfdx.template cast<T>() * dxdz; // Also known as dfdz.
129
TrackRegionOptions::TrackRegionOptions()
131
minimum_correlation(0),
134
use_brute_initialization(true),
135
use_normalized_intensities(false),
138
regularization_coefficient(0.0),
139
minimum_corner_shift_tolerance_pixels(0.005),
145
// TODO(keir): Consider adding padding.
147
bool InBounds(const FloatImage &image,
150
return 0.0 <= x && x < image.Width() &&
151
0.0 <= y && y < image.Height();
154
bool AllInBounds(const FloatImage &image,
157
for (int i = 0; i < 4; ++i) {
158
if (!InBounds(image, x[i], y[i])) {
165
// Sample the image at position (x, y) but use the gradient, if present, to
166
// propagate derivatives from x and y. This is needed to integrate the numeric
167
// image gradients with Ceres's autodiff framework.
169
static T SampleWithDerivative(const FloatImage &image_and_gradient,
172
float scalar_x = JetOps<T>::GetScalar(x);
173
float scalar_y = JetOps<T>::GetScalar(y);
175
// Note that sample[1] and sample[2] will be uninitialized in the scalar
176
// case, but that is not an issue because the Chain::Rule below will not read
177
// the uninitialized values.
179
if (JetOps<T>::IsScalar()) {
180
// For the scalar case, only sample the image.
181
sample[0] = SampleLinear(image_and_gradient, scalar_y, scalar_x, 0);
183
// For the derivative case, sample the gradient as well.
184
SampleLinear(image_and_gradient, scalar_y, scalar_x, sample);
187
return Chain<float, 2, T>::Rule(sample[0], sample + 1, xy);
190
template<typename Warp>
191
class TerminationCheckingCallback : public ceres::IterationCallback {
193
TerminationCheckingCallback(const TrackRegionOptions &options,
194
const FloatImage& image2,
196
const double *x1, const double *y1)
197
: options_(options), image2_(image2), warp_(warp), x1_(x1), y1_(y1),
198
have_last_successful_step_(false) {}
200
virtual ceres::CallbackReturnType operator()(
201
const ceres::IterationSummary& summary) {
202
// If the step wasn't successful, there's nothing to do.
203
if (!summary.step_is_successful) {
204
return ceres::SOLVER_CONTINUE;
206
// Warp the original 4 points with the current warp into image2.
209
for (int i = 0; i < 4; ++i) {
210
warp_.Forward(warp_.parameters, x1_[i], y1_[i], x2 + i, y2 + i);
212
// Ensure the corners are all in bounds.
213
if (!AllInBounds(image2_, x2, y2)) {
214
LG << "Successful step fell outside of the pattern bounds; aborting.";
215
return ceres::SOLVER_ABORT;
218
// Ensure the minimizer is making large enough shifts to bother continuing.
219
// Ideally, this check would happen on the parameters themselves which
220
// Ceres supports directly; however, the mapping from parameter change
221
// magnitude to corner movement in pixels is not a simple norm. Hence, the
222
// need for a stateful callback which tracks the last successful set of
223
// parameters (and the position of the projected patch corners).
224
if (have_last_successful_step_) {
225
// Compute the maximum shift of any corner in pixels since the last
226
// successful iteration.
227
double max_change_pixels = 0;
228
for (int i = 0; i < 4; ++i) {
229
double dx = x2[i] - x2_last_successful_[i];
230
double dy = y2[i] - y2_last_successful_[i];
231
double change_pixels = dx*dx + dy*dy;
232
if (change_pixels > max_change_pixels) {
233
max_change_pixels = change_pixels;
236
max_change_pixels = sqrt(max_change_pixels);
237
LG << "Max patch corner shift is " << max_change_pixels;
239
// Bail if the shift is too small.
240
if (max_change_pixels < options_.minimum_corner_shift_tolerance_pixels) {
241
LG << "Max patch corner shift is " << max_change_pixels
242
<< " from the last iteration; returning success.";
243
return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
247
// Save the projected corners for checking on the next successful iteration.
248
for (int i = 0; i < 4; ++i) {
249
x2_last_successful_[i] = x2[i];
250
y2_last_successful_[i] = y2[i];
252
have_last_successful_step_ = true;
253
return ceres::SOLVER_CONTINUE;
257
const TrackRegionOptions &options_;
258
const FloatImage &image2_;
263
bool have_last_successful_step_;
264
double x2_last_successful_[4];
265
double y2_last_successful_[4];
268
template<typename Warp>
269
class PixelDifferenceCostFunctor {
271
PixelDifferenceCostFunctor(const TrackRegionOptions &options,
272
const FloatImage &image_and_gradient1,
273
const FloatImage &image_and_gradient2,
274
const Mat3 &canonical_to_image1,
279
image_and_gradient1_(image_and_gradient1),
280
image_and_gradient2_(image_and_gradient2),
281
canonical_to_image1_(canonical_to_image1),
282
num_samples_x_(num_samples_x),
283
num_samples_y_(num_samples_y),
285
pattern_and_gradient_(num_samples_y_, num_samples_x_, 3),
286
pattern_positions_(num_samples_y_, num_samples_x_, 2),
287
pattern_mask_(num_samples_y_, num_samples_x_, 1) {
288
ComputeCanonicalPatchAndNormalizer();
291
void ComputeCanonicalPatchAndNormalizer() {
293
double num_samples = 0.0;
294
for (int r = 0; r < num_samples_y_; ++r) {
295
for (int c = 0; c < num_samples_x_; ++c) {
296
// Compute the position; cache it.
297
Vec3 image_position = canonical_to_image1_ * Vec3(c, r, 1);
298
image_position /= image_position(2);
299
pattern_positions_(r, c, 0) = image_position(0);
300
pattern_positions_(r, c, 1) = image_position(1);
302
// Sample the pattern and gradients.
303
SampleLinear(image_and_gradient1_,
304
image_position(1), // SampleLinear is r, c.
306
&pattern_and_gradient_(r, c, 0));
308
// Sample sample the mask.
309
double mask_value = 1.0;
310
if (options_.image1_mask != NULL) {
311
SampleLinear(*options_.image1_mask,
312
image_position(1), // SampleLinear is r, c.
314
&pattern_mask_(r, c, 0));
315
mask_value = pattern_mask_(r, c);
317
src_mean_ += pattern_and_gradient_(r, c, 0) * mask_value;
318
num_samples += mask_value;
321
src_mean_ /= num_samples;
325
bool operator()(const T *warp_parameters, T *residuals) const {
326
if (options_.image1_mask != NULL) {
327
VLOG(2) << "Using a mask.";
329
for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
330
VLOG(2) << "warp_parameters[" << i << "]: " << warp_parameters[i];
334
if (options_.use_normalized_intensities) {
335
ComputeNormalizingCoefficient(warp_parameters,
340
for (int r = 0; r < num_samples_y_; ++r) {
341
for (int c = 0; c < num_samples_x_; ++c) {
342
// Use the pre-computed image1 position.
343
Vec2 image1_position(pattern_positions_(r, c, 0),
344
pattern_positions_(r, c, 1));
346
// Sample the mask early; if it's zero, this pixel has no effect. This
347
// allows early bailout from the expensive sampling that happens below.
349
// Note that partial masks are not short circuited. To see why short
350
// circuiting produces bitwise-exact same results, consider that the
351
// residual for each pixel is
353
// residual = mask * (src - dst) ,
355
// and for jets, multiplying by a scalar multiplies the derivative
356
// components by the scalar as well. Therefore, if the mask is exactly
357
// zero, then so too will the final residual and derivatives.
358
double mask_value = 1.0;
359
if (options_.image1_mask != NULL) {
360
mask_value = pattern_mask_(r, c);
361
if (mask_value == 0.0) {
362
residuals[cursor++] = T(0.0);
367
// Compute the location of the destination pixel.
368
T image2_position[2];
369
warp_.Forward(warp_parameters,
370
T(image1_position[0]),
371
T(image1_position[1]),
373
&image2_position[1]);
375
// Sample the destination, propagating derivatives.
376
T dst_sample = SampleWithDerivative(image_and_gradient2_,
380
// Sample the source. This is made complicated by ESM mode.
382
if (options_.use_esm && !JetOps<T>::IsScalar()) {
383
// In ESM mode, the derivative of the source is also taken into
384
// account. This changes the linearization in a way that causes
385
// better convergence. Copy the derivative of the warp parameters
386
// onto the jets for the image1 position. This is the ESM hack.
387
T image1_position_jet[2] = {
388
image2_position[0], // Order is x, y. This matches the
389
image2_position[1] // derivative order in the patch.
391
JetOps<T>::SetScalar(image1_position[0], image1_position_jet + 0);
392
JetOps<T>::SetScalar(image1_position[1], image1_position_jet + 1);
394
// Now that the image1 positions have the jets applied from the
395
// image2 position (the ESM hack), chain the image gradients to
396
// obtain a sample with the derivative with respect to the warp
397
// parameters attached.
398
src_sample = Chain<float, 2, T>::Rule(pattern_and_gradient_(r, c),
399
&pattern_and_gradient_(r, c, 1),
400
image1_position_jet);
402
// The jacobians for these should be averaged. Due to the subtraction
403
// below, flip the sign of the src derivative so that the effect
404
// after subtraction of the jets is that they are averaged.
405
JetOps<T>::ScaleDerivative(-0.5, &src_sample);
406
JetOps<T>::ScaleDerivative(0.5, &dst_sample);
408
// This is the traditional, forward-mode KLT solution.
409
src_sample = T(pattern_and_gradient_(r, c));
412
// Normalize the samples by the mean values of each signal. The typical
413
// light model assumes multiplicative intensity changes with changing
414
// light, so this is a reasonable choice. Note that dst_mean has
415
// derivative information attached thanks to autodiff.
416
if (options_.use_normalized_intensities) {
417
src_sample /= T(src_mean_);
418
dst_sample /= dst_mean;
421
// The difference is the error.
422
T error = src_sample - dst_sample;
424
// Weight the error by the mask, if one is present.
425
if (options_.image1_mask != NULL) {
426
error *= T(mask_value);
428
residuals[cursor++] = error;
434
// For normalized matching, the average and
436
void ComputeNormalizingCoefficient(const T *warp_parameters,
440
double num_samples = 0.0;
441
for (int r = 0; r < num_samples_y_; ++r) {
442
for (int c = 0; c < num_samples_x_; ++c) {
443
// Use the pre-computed image1 position.
444
Vec2 image1_position(pattern_positions_(r, c, 0),
445
pattern_positions_(r, c, 1));
447
// Sample the mask early; if it's zero, this pixel has no effect. This
448
// allows early bailout from the expensive sampling that happens below.
449
double mask_value = 1.0;
450
if (options_.image1_mask != NULL) {
451
mask_value = pattern_mask_(r, c);
452
if (mask_value == 0.0) {
457
// Compute the location of the destination pixel.
458
T image2_position[2];
459
warp_.Forward(warp_parameters,
460
T(image1_position[0]),
461
T(image1_position[1]),
463
&image2_position[1]);
466
// Sample the destination, propagating derivatives.
467
// TODO(keir): This accumulation can, surprisingly, be done as a
468
// pre-pass by using integral images. This is complicated by the need
469
// to store the jets in the integral image, but it is possible.
470
T dst_sample = SampleWithDerivative(image_and_gradient2_,
474
// Weight the sample by the mask, if one is present.
475
if (options_.image1_mask != NULL) {
476
dst_sample *= T(mask_value);
479
*dst_mean += dst_sample;
480
num_samples += mask_value;
483
*dst_mean /= T(num_samples);
484
LG << "Normalization for dst:" << *dst_mean;
487
// TODO(keir): Consider also computing the cost here.
488
double PearsonProductMomentCorrelationCoefficient(
489
const double *warp_parameters) const {
490
for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) {
491
VLOG(2) << "Correlation warp_parameters[" << i << "]: "
492
<< warp_parameters[i];
495
// The single-pass PMCC computation is somewhat numerically unstable, but
496
// it's sufficient for the tracker.
497
double sX = 0, sY = 0, sXX = 0, sYY = 0, sXY = 0;
499
// Due to masking, it's important to account for fractional samples.
500
// For example, samples with a 50% mask are counted as a half sample.
501
double num_samples = 0;
503
for (int r = 0; r < num_samples_y_; ++r) {
504
for (int c = 0; c < num_samples_x_; ++c) {
505
// Use the pre-computed image1 position.
506
Vec2 image1_position(pattern_positions_(r, c, 0),
507
pattern_positions_(r, c, 1));
509
double mask_value = 1.0;
510
if (options_.image1_mask != NULL) {
511
mask_value = pattern_mask_(r, c);
512
if (mask_value == 0.0) {
517
// Compute the location of the destination pixel.
518
double image2_position[2];
519
warp_.Forward(warp_parameters,
523
&image2_position[1]);
525
double x = pattern_and_gradient_(r, c);
526
double y = SampleLinear(image_and_gradient2_,
527
image2_position[1], // SampleLinear is r, c.
530
// Weight the signals by the mask, if one is present.
531
if (options_.image1_mask != NULL) {
534
num_samples += mask_value;
552
double var_x = sXX - sX*sX;
553
double var_y = sYY - sY*sY;
554
double covariance_xy = sXY - sX*sY;
556
double correlation = covariance_xy / sqrt(var_x * var_y);
557
LG << "Covariance xy: " << covariance_xy
558
<< ", var 1: " << var_x << ", var 2: " << var_y
559
<< ", correlation: " << correlation;
564
const TrackRegionOptions &options_;
565
const FloatImage &image_and_gradient1_;
566
const FloatImage &image_and_gradient2_;
567
const Mat3 &canonical_to_image1_;
572
FloatImage pattern_and_gradient_;
574
// This contains the position from where the cached pattern samples were
575
// taken from. This is also used to warp from src to dest without going from
576
// canonical pixels to src first.
577
FloatImage pattern_positions_;
579
FloatImage pattern_mask_;
582
template<typename Warp>
583
class WarpRegularizingCostFunctor {
585
WarpRegularizingCostFunctor(const TrackRegionOptions &options,
588
const double *x2_original,
589
const double *y2_original,
594
x2_original_(x2_original),
595
y2_original_(y2_original),
597
// Compute the centroid of the first guess quad.
598
// TODO(keir): Use Quad class here.
599
original_centroid_[0] = 0.0;
600
original_centroid_[1] = 0.0;
601
for (int i = 0; i < 4; ++i) {
602
original_centroid_[0] += x2_original[i];
603
original_centroid_[1] += y2_original[i];
605
original_centroid_[0] /= 4;
606
original_centroid_[1] /= 4;
610
bool operator()(const T *warp_parameters, T *residuals) const {
611
T dst_centroid[2] = { T(0.0), T(0.0) };
612
for (int i = 0; i < 4; ++i) {
613
T image1_position[2] = { T(x1_[i]), T(y1_[i]) };
614
T image2_position[2];
615
warp_.Forward(warp_parameters,
619
&image2_position[1]);
621
// Subtract the positions. Note that this ignores the centroids.
622
residuals[2 * i + 0] = image2_position[0] - image1_position[0];
623
residuals[2 * i + 1] = image2_position[1] - image1_position[1];
625
// Accumulate the dst centroid.
626
dst_centroid[0] += image2_position[0];
627
dst_centroid[1] += image2_position[1];
629
dst_centroid[0] /= T(4.0);
630
dst_centroid[1] /= T(4.0);
632
// Adjust for the centroids.
633
for (int i = 0; i < 4; ++i) {
634
residuals[2 * i + 0] += T(original_centroid_[0]) - dst_centroid[0];
635
residuals[2 * i + 1] += T(original_centroid_[1]) - dst_centroid[1];
638
// Reweight the residuals.
639
for (int i = 0; i < 8; ++i) {
640
residuals[i] *= T(options_.regularization_coefficient);
646
const TrackRegionOptions &options_;
649
const double *x2_original_;
650
const double *y2_original_;
651
double original_centroid_[2];
655
// Compute the warp from rectangular coordinates, where one corner is the
656
// origin, and the opposite corner is at (num_samples_x, num_samples_y).
657
Mat3 ComputeCanonicalHomography(const double *x1,
662
canonical << 0, num_samples_x, num_samples_x, 0,
663
0, 0, num_samples_y, num_samples_y;
666
xy1 << x1[0], x1[1], x1[2], x1[3],
667
y1[0], y1[1], y1[2], y1[3];
670
if (!Homography2DFromCorrespondencesLinear(canonical, xy1, &H, 1e-12)) {
671
LG << "Couldn't construct homography.";
678
Quad(const double *x, const double *y) : x_(x), y_(y) {
679
// Compute the centroid and store it.
680
centroid_ = Vec2(0.0, 0.0);
681
for (int i = 0; i < 4; ++i) {
682
centroid_ += Vec2(x_[i], y_[i]);
687
// The centroid of the four points representing the quad.
688
const Vec2& Centroid() const {
692
// The average magnitude of the four points relative to the centroid.
693
double Scale() const {
695
for (int i = 0; i < 4; ++i) {
696
scale += (Vec2(x_[i], y_[i]) - Centroid()).norm();
701
Vec2 CornerRelativeToCentroid(int i) const {
702
return Vec2(x_[i], y_[i]) - centroid_;
711
struct TranslationWarp {
712
TranslationWarp(const double *x1, const double *y1,
713
const double *x2, const double *y2) {
714
Vec2 t = Quad(x2, y2).Centroid() - Quad(x1, y1).Centroid();
715
parameters[0] = t[0];
716
parameters[1] = t[1];
720
void Forward(const T *warp_parameters,
721
const T &x1, const T& y1, T *x2, T* y2) const {
722
*x2 = x1 + warp_parameters[0];
723
*y2 = y1 + warp_parameters[1];
726
// Translation x, translation y.
727
enum { NUM_PARAMETERS = 2 };
728
double parameters[NUM_PARAMETERS];
731
struct TranslationScaleWarp {
732
TranslationScaleWarp(const double *x1, const double *y1,
733
const double *x2, const double *y2)
737
// The difference in centroids is the best guess for translation.
738
Vec2 t = q2.Centroid() - q1.Centroid();
739
parameters[0] = t[0];
740
parameters[1] = t[1];
742
// The difference in scales is the estimate for the scale.
743
parameters[2] = 1.0 - q2.Scale() / q1.Scale();
746
// The strange way of parameterizing the translation and scaling is to make
747
// the knobs that the optimizer sees easy to adjust. This is less important
748
// for the scaling case than the rotation case.
750
void Forward(const T *warp_parameters,
751
const T &x1, const T& y1, T *x2, T* y2) const {
752
// Make the centroid of Q1 the origin.
753
const T x1_origin = x1 - q1.Centroid()(0);
754
const T y1_origin = y1 - q1.Centroid()(1);
756
// Scale uniformly about the origin.
757
const T scale = 1.0 + warp_parameters[2];
758
const T x1_origin_scaled = scale * x1_origin;
759
const T y1_origin_scaled = scale * y1_origin;
761
// Translate back into the space of Q1 (but scaled).
762
const T x1_scaled = x1_origin_scaled + q1.Centroid()(0);
763
const T y1_scaled = y1_origin_scaled + q1.Centroid()(1);
765
// Translate into the space of Q2.
766
*x2 = x1_scaled + warp_parameters[0];
767
*y2 = y1_scaled + warp_parameters[1];
770
// Translation x, translation y, scale.
771
enum { NUM_PARAMETERS = 3 };
772
double parameters[NUM_PARAMETERS];
777
// Assumes the given points are already zero-centroid and the same size.
778
Mat2 OrthogonalProcrustes(const Mat2 &correlation_matrix) {
779
Eigen::JacobiSVD<Mat2> svd(correlation_matrix,
780
Eigen::ComputeFullU | Eigen::ComputeFullV);
781
return svd.matrixV() * svd.matrixU().transpose();
784
struct TranslationRotationWarp {
785
TranslationRotationWarp(const double *x1, const double *y1,
786
const double *x2, const double *y2)
790
// The difference in centroids is the best guess for translation.
791
Vec2 t = q2.Centroid() - q1.Centroid();
792
parameters[0] = t[0];
793
parameters[1] = t[1];
795
// Obtain the rotation via orthorgonal procrustes.
796
Mat2 correlation_matrix;
797
for (int i = 0; i < 4; ++i) {
798
correlation_matrix += q1.CornerRelativeToCentroid(i) *
799
q2.CornerRelativeToCentroid(i).transpose();
801
Mat2 R = OrthogonalProcrustes(correlation_matrix);
802
parameters[2] = atan2(R(1, 0), R(0, 0));
804
LG << "Correlation_matrix:\n" << correlation_matrix;
806
LG << "Theta:" << parameters[2];
809
// The strange way of parameterizing the translation and rotation is to make
810
// the knobs that the optimizer sees easy to adjust. The reason is that while
811
// it is always the case that it is possible to express composed rotations
812
// and translations as a single translation and rotation, the numerical
813
// values needed for the composition are often large in magnitude. This is
814
// enough to throw off any minimizer, since it must do the equivalent of
815
// compose rotations and translations.
817
// Instead, use the parameterization below that offers a parameterization
818
// that exposes the degrees of freedom in a way amenable to optimization.
820
void Forward(const T *warp_parameters,
821
const T &x1, const T& y1, T *x2, T* y2) const {
822
// Make the centroid of Q1 the origin.
823
const T x1_origin = x1 - q1.Centroid()(0);
824
const T y1_origin = y1 - q1.Centroid()(1);
826
// Rotate about the origin (i.e. centroid of Q1).
827
const T theta = warp_parameters[2];
828
const T costheta = cos(theta);
829
const T sintheta = sin(theta);
830
const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
831
const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
833
// Translate back into the space of Q1 (but scaled).
834
const T x1_rotated = x1_origin_rotated + q1.Centroid()(0);
835
const T y1_rotated = y1_origin_rotated + q1.Centroid()(1);
837
// Translate into the space of Q2.
838
*x2 = x1_rotated + warp_parameters[0];
839
*y2 = y1_rotated + warp_parameters[1];
842
// Translation x, translation y, rotation about the center of Q1 degrees.
843
enum { NUM_PARAMETERS = 3 };
844
double parameters[NUM_PARAMETERS];
849
struct TranslationRotationScaleWarp {
850
TranslationRotationScaleWarp(const double *x1, const double *y1,
851
const double *x2, const double *y2)
855
// The difference in centroids is the best guess for translation.
856
Vec2 t = q2.Centroid() - q1.Centroid();
857
parameters[0] = t[0];
858
parameters[1] = t[1];
860
// The difference in scales is the estimate for the scale.
861
parameters[2] = 1.0 - q2.Scale() / q1.Scale();
863
// Obtain the rotation via orthorgonal procrustes.
864
Mat2 correlation_matrix;
865
for (int i = 0; i < 4; ++i) {
866
correlation_matrix += q1.CornerRelativeToCentroid(i) *
867
q2.CornerRelativeToCentroid(i).transpose();
869
Mat2 R = OrthogonalProcrustes(correlation_matrix);
870
parameters[3] = atan2(R(1, 0), R(0, 0));
872
LG << "Correlation_matrix:\n" << correlation_matrix;
874
LG << "Theta:" << parameters[3];
877
// The strange way of parameterizing the translation and rotation is to make
878
// the knobs that the optimizer sees easy to adjust. The reason is that while
879
// it is always the case that it is possible to express composed rotations
880
// and translations as a single translation and rotation, the numerical
881
// values needed for the composition are often large in magnitude. This is
882
// enough to throw off any minimizer, since it must do the equivalent of
883
// compose rotations and translations.
885
// Instead, use the parameterization below that offers a parameterization
886
// that exposes the degrees of freedom in a way amenable to optimization.
888
void Forward(const T *warp_parameters,
889
const T &x1, const T& y1, T *x2, T* y2) const {
890
// Make the centroid of Q1 the origin.
891
const T x1_origin = x1 - q1.Centroid()(0);
892
const T y1_origin = y1 - q1.Centroid()(1);
894
// Rotate about the origin (i.e. centroid of Q1).
895
const T theta = warp_parameters[3];
896
const T costheta = cos(theta);
897
const T sintheta = sin(theta);
898
const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin;
899
const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin;
901
// Scale uniformly about the origin.
902
const T scale = 1.0 + warp_parameters[2];
903
const T x1_origin_rotated_scaled = scale * x1_origin_rotated;
904
const T y1_origin_rotated_scaled = scale * y1_origin_rotated;
906
// Translate back into the space of Q1 (but scaled and rotated).
907
const T x1_rotated_scaled = x1_origin_rotated_scaled + q1.Centroid()(0);
908
const T y1_rotated_scaled = y1_origin_rotated_scaled + q1.Centroid()(1);
910
// Translate into the space of Q2.
911
*x2 = x1_rotated_scaled + warp_parameters[0];
912
*y2 = y1_rotated_scaled + warp_parameters[1];
915
// Translation x, translation y, rotation about the center of Q1 degrees,
917
enum { NUM_PARAMETERS = 4 };
918
double parameters[NUM_PARAMETERS];
924
AffineWarp(const double *x1, const double *y1,
925
const double *x2, const double *y2)
929
// The difference in centroids is the best guess for translation.
930
Vec2 t = q2.Centroid() - q1.Centroid();
931
parameters[0] = t[0];
932
parameters[1] = t[1];
934
// Estimate the four affine parameters with the usual least squares.
937
for (int i = 0; i < 4; ++i) {
938
Vec2 v1 = q1.CornerRelativeToCentroid(i);
939
Vec2 v2 = q2.CornerRelativeToCentroid(i);
941
Q1.row(2 * i + 0) << v1[0], v1[1], 0, 0 ;
942
Q1.row(2 * i + 1) << 0, 0, v1[0], v1[1];
944
Q2(2 * i + 0) = v2[0];
945
Q2(2 * i + 1) = v2[1];
948
// TODO(keir): Check solution quality.
949
Vec4 a = Q1.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Q2);
950
parameters[2] = a[0];
951
parameters[3] = a[1];
952
parameters[4] = a[2];
953
parameters[5] = a[3];
955
LG << "a:" << a.transpose();
956
LG << "t:" << t.transpose();
959
// See comments in other parameterizations about why the centroid is used.
961
void Forward(const T *p, const T &x1, const T& y1, T *x2, T* y2) const {
962
// Make the centroid of Q1 the origin.
963
const T x1_origin = x1 - q1.Centroid()(0);
964
const T y1_origin = y1 - q1.Centroid()(1);
966
// Apply the affine transformation.
967
const T x1_origin_affine = p[2] * x1_origin + p[3] * y1_origin;
968
const T y1_origin_affine = p[4] * x1_origin + p[5] * y1_origin;
970
// Translate back into the space of Q1 (but affine transformed).
971
const T x1_affine = x1_origin_affine + q1.Centroid()(0);
972
const T y1_affine = y1_origin_affine + q1.Centroid()(1);
974
// Translate into the space of Q2.
975
*x2 = x1_affine + p[0];
976
*y2 = y1_affine + p[1];
979
// Translation x, translation y, rotation about the center of Q1 degrees,
981
enum { NUM_PARAMETERS = 6 };
982
double parameters[NUM_PARAMETERS];
987
struct HomographyWarp {
988
HomographyWarp(const double *x1, const double *y1,
989
const double *x2, const double *y2) {
991
quad1 << x1[0], x1[1], x1[2], x1[3],
992
y1[0], y1[1], y1[2], y1[3];
995
quad2 << x2[0], x2[1], x2[2], x2[3],
996
y2[0], y2[1], y2[2], y2[3];
999
if (!Homography2DFromCorrespondencesLinear(quad1, quad2, &H, 1e-12)) {
1000
LG << "Couldn't construct homography.";
1003
// Assume H(2, 2) != 0, and fix scale at H(2, 2) == 1.0.
1006
// Assume H is close to identity, so subtract out the diagonal.
1010
CHECK_NE(H(2, 2), 0.0) << H;
1011
for (int i = 0; i < 8; ++i) {
1012
parameters[i] = H(i / 3, i % 3);
1013
LG << "Parameters[" << i << "]: " << parameters[i];
1017
template<typename T>
1018
static void Forward(const T *p,
1019
const T &x1, const T& y1, T *x2, T* y2) {
1020
// Homography warp with manual 3x3 matrix multiply.
1021
const T xx2 = (1.0 + p[0]) * x1 + p[1] * y1 + p[2];
1022
const T yy2 = p[3] * x1 + (1.0 + p[4]) * y1 + p[5];
1023
const T zz2 = p[6] * x1 + p[7] * y1 + 1.0;
1028
enum { NUM_PARAMETERS = 8 };
1029
double parameters[NUM_PARAMETERS];
1032
// Determine the number of samples to use for x and y. Quad winding goes:
1037
// The idea is to take the maximum x or y distance. This may be oversampling.
1038
// TODO(keir): Investigate the various choices; perhaps average is better?
1039
void PickSampling(const double *x1, const double *y1,
1040
const double *x2, const double *y2,
1041
int *num_samples_x, int *num_samples_y) {
1042
Vec2 a0(x1[0], y1[0]);
1043
Vec2 a1(x1[1], y1[1]);
1044
Vec2 a2(x1[2], y1[2]);
1045
Vec2 a3(x1[3], y1[3]);
1047
Vec2 b0(x1[0], y1[0]);
1048
Vec2 b1(x1[1], y1[1]);
1049
Vec2 b2(x1[2], y1[2]);
1050
Vec2 b3(x1[3], y1[3]);
1052
double x_dimensions[4] = {
1059
double y_dimensions[4] = {
1065
const double kScaleFactor = 1.0;
1066
*num_samples_x = static_cast<int>(
1067
kScaleFactor * *std::max_element(x_dimensions, x_dimensions + 4));
1068
*num_samples_y = static_cast<int>(
1069
kScaleFactor * *std::max_element(y_dimensions, y_dimensions + 4));
1070
LG << "Automatic num_samples_x: " << *num_samples_x
1071
<< ", num_samples_y: " << *num_samples_y;
1074
bool SearchAreaTooBigForDescent(const FloatImage &image2,
1075
const double *x2, const double *y2) {
1076
// TODO(keir): Check the bounds and enable only when it makes sense.
1080
bool PointOnRightHalfPlane(const Vec2 &a, const Vec2 &b, double x, double y) {
1082
return ((Vec2(x, y) - b).transpose() * Vec2(-ba.y(), ba.x())) > 0;
1085
// Determine if a point is in a quad. The quad is arranged as:
1097
// The implementation does up to four half-plane comparisons.
1098
bool PointInQuad(const double *xs, const double *ys, double x, double y) {
1099
Vec2 a0(xs[0], ys[0]);
1100
Vec2 a1(xs[1], ys[1]);
1101
Vec2 a2(xs[2], ys[2]);
1102
Vec2 a3(xs[3], ys[3]);
1104
return PointOnRightHalfPlane(a0, a1, x, y) &&
1105
PointOnRightHalfPlane(a1, a2, x, y) &&
1106
PointOnRightHalfPlane(a2, a3, x, y) &&
1107
PointOnRightHalfPlane(a3, a0, x, y);
1110
// This makes it possible to map between Eigen float arrays and FloatImage
1111
// without using comparisons.
1112
typedef Eigen::Array<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> FloatArray;
1114
// This creates a pattern in the frame of image2, from the pixel is image1,
1115
// based on the initial guess represented by the two quads x1, y1, and x2, y2.
1116
template<typename Warp>
1117
void CreateBrutePattern(const double *x1, const double *y1,
1118
const double *x2, const double *y2,
1119
const FloatImage &image1,
1120
const FloatImage *image1_mask,
1121
FloatArray *pattern,
1125
// Get integer bounding box of quad2 in image2.
1126
int min_x = static_cast<int>(floor(*std::min_element(x2, x2 + 4)));
1127
int min_y = static_cast<int>(floor(*std::min_element(y2, y2 + 4)));
1128
int max_x = static_cast<int>(ceil (*std::max_element(x2, x2 + 4)));
1129
int max_y = static_cast<int>(ceil (*std::max_element(y2, y2 + 4)));
1131
int w = max_x - min_x;
1132
int h = max_y - min_y;
1134
pattern->resize(h, w);
1137
Warp inverse_warp(x2, y2, x1, y1);
1139
// r,c are in the coordinate frame of image2.
1140
for (int r = min_y; r < max_y; ++r) {
1141
for (int c = min_x; c < max_x; ++c) {
1142
// i and j are in the coordinate frame of the pattern in image2.
1150
inverse_warp.Forward(inverse_warp.parameters,
1154
if (PointInQuad(x1, y1, src_x, src_y)) {
1155
(*pattern)(i, j) = SampleLinear(image1, src_y, src_x);
1156
(*mask)(i, j) = 1.0;
1158
(*mask)(i, j) = SampleLinear(*image1_mask, src_y, src_x);;
1161
(*pattern)(i, j) = 0.0;
1162
(*mask)(i, j) = 0.0;
1170
// Compute a translation-only estimate of the warp, using brute force search. A
1171
// smarter implementation would use the FFT to compute the normalized cross
1172
// correlation. Instead, this is a dumb implementation. Surprisingly, it is
1173
// fast enough in practice.
1175
// Returns true if any alignment was found, and false if the projected pattern
1178
// TODO(keir): The normalization is less effective for the brute force search
1179
// than it is with the Ceres solver. It's unclear if this is a bug or due to
1180
// the original frame being too different from the reprojected reference in the
1181
// destination frame.
1183
// The likely solution is to use the previous frame, instead of the original
1184
// pattern, when doing brute initialization. Unfortunately that implies a
1185
// totally different warping interface, since access to more than a the source
1186
// and current destination frame is necessary.
1187
template<typename Warp>
1188
bool BruteTranslationOnlyInitialize(const FloatImage &image1,
1189
const FloatImage *image1_mask,
1190
const FloatImage &image2,
1191
const int num_extra_points,
1192
const bool use_normalized_intensities,
1193
const double *x1, const double *y1,
1194
double *x2, double *y2) {
1195
// Create the pattern to match in the space of image2, assuming our inital
1196
// guess isn't too far from the template in image1. If there is no image1
1197
// mask, then the resulting mask is binary.
1200
int origin_x = -1, origin_y = -1;
1201
CreateBrutePattern<Warp>(x1, y1, x2, y2,
1202
image1, image1_mask,
1204
&origin_x, &origin_y);
1206
// For normalization, premultiply the pattern by the inverse pattern mean.
1207
double mask_sum = 1.0;
1208
if (use_normalized_intensities) {
1209
mask_sum = mask.sum();
1210
double inverse_pattern_mean = mask_sum / ((mask * pattern).sum());
1211
pattern *= inverse_pattern_mean;
1214
// Use Eigen on the images via maps for strong vectorization.
1215
Map<const FloatArray> search(image2.Data(), image2.Height(), image2.Width());
1217
// Try all possible locations inside the search area. Yes, everywhere.
1219
// TODO(keir): There are a number of possible optimizations here. One choice
1220
// is to make a grid and only try one out of every N possible samples.
1222
// Another, slightly more clever idea, is to compute some sort of spatial
1223
// frequency distribution of the pattern patch. If the spatial resolution is
1224
// high (e.g. a grating pattern or fine lines) then checking every possible
1225
// translation is necessary, since a 1-pixel shift may induce a massive
1226
// change in the cost function. If the image is a blob or splotch with blurry
1227
// edges, then fewer samples are necessary since a few pixels offset won't
1228
// change the cost function much.
1229
double best_sad = std::numeric_limits<double>::max();
1232
int w = pattern.cols();
1233
int h = pattern.rows();
1235
for (int r = 0; r < (image2.Height() - h); ++r) {
1236
for (int c = 0; c < (image2.Width() - w); ++c) {
1237
// Compute the weighted sum of absolute differences, Eigen style. Note
1238
// that the block from the search image is never stored in a variable, to
1239
// avoid copying overhead and permit inlining.
1241
if (use_normalized_intensities) {
1242
// TODO(keir): It's really dumb to recompute the search mean for every
1243
// shift. A smarter implementation would use summed area tables
1244
// instead, reducing the mean calculation to an O(1) operation.
1245
double inverse_search_mean =
1246
mask_sum / ((mask * search.block(r, c, h, w)).sum());
1247
sad = (mask * (pattern - (search.block(r, c, h, w) *
1248
inverse_search_mean))).abs().sum();
1250
sad = (mask * (pattern - search.block(r, c, h, w))).abs().sum();
1252
if (sad < best_sad) {
1260
// This mean the effective pattern area is zero. This check could go earlier,
1261
// but this is less code.
1262
if (best_r == -1 || best_c == -1) {
1266
LG << "Brute force translation found a shift. "
1267
<< "best_c: " << best_c << ", best_r: " << best_r << ", "
1268
<< "origin_x: " << origin_x << ", origin_y: " << origin_y << ", "
1269
<< "dc: " << (best_c - origin_x) << ", "
1270
<< "dr: " << (best_r - origin_y)
1271
<< ", tried " << ((image2.Height() - h) * (image2.Width() - w))
1275
for (int i = 0; i < 4 + num_extra_points; ++i) {
1276
x2[i] += best_c - origin_x;
1277
y2[i] += best_r - origin_y;
1284
template<typename Warp>
1285
void TemplatedTrackRegion(const FloatImage &image1,
1286
const FloatImage &image2,
1287
const double *x1, const double *y1,
1288
const TrackRegionOptions &options,
1289
double *x2, double *y2,
1290
TrackRegionResult *result) {
1291
for (int i = 0; i < 4; ++i) {
1292
LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); guess ("
1293
<< x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
1294
<< (y2[i] - y1[i]) << ").";
1296
if (options.use_normalized_intensities) {
1297
LG << "Using normalized intensities.";
1300
// Bail early if the points are already outside.
1301
if (!AllInBounds(image1, x1, y1)) {
1302
result->termination = TrackRegionResult::SOURCE_OUT_OF_BOUNDS;
1305
if (!AllInBounds(image2, x2, y2)) {
1306
result->termination = TrackRegionResult::DESTINATION_OUT_OF_BOUNDS;
1309
// TODO(keir): Check quads to ensure there is some area.
1311
// Keep a copy of the "original" guess for regularization.
1312
double x2_original[4];
1313
double y2_original[4];
1314
for (int i = 0; i < 4; ++i) {
1315
x2_original[i] = x2[i];
1316
y2_original[i] = y2[i];
1319
// Prepare the image and gradient.
1320
Array3Df image_and_gradient1;
1321
Array3Df image_and_gradient2;
1322
BlurredImageAndDerivativesChannels(image1, options.sigma,
1323
&image_and_gradient1);
1324
BlurredImageAndDerivativesChannels(image2, options.sigma,
1325
&image_and_gradient2);
1327
// Possibly do a brute-force translation-only initialization.
1328
if (SearchAreaTooBigForDescent(image2, x2, y2) &&
1329
options.use_brute_initialization) {
1330
LG << "Running brute initialization...";
1331
bool found_any_alignment = BruteTranslationOnlyInitialize<Warp>(
1332
image_and_gradient1,
1333
options.image1_mask,
1335
options.num_extra_points,
1336
options.use_normalized_intensities,
1338
if (!found_any_alignment) {
1339
LG << "Brute failed to find an alignment; pattern too small. "
1340
<< "Failing entire track operation.";
1341
result->termination = TrackRegionResult::INSUFFICIENT_PATTERN_AREA;
1344
for (int i = 0; i < 4; ++i) {
1345
LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); brute ("
1346
<< x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i])
1347
<< ", " << (y2[i] - y1[i]) << ").";
1351
// Prepare the initial warp parameters from the four correspondences.
1352
// Note: This must happen after the brute initialization runs, since the
1353
// brute initialization mutates x2 and y2 in place.
1354
Warp warp(x1, y1, x2, y2);
1356
// Decide how many samples to use in the x and y dimensions.
1359
PickSampling(x1, y1, x2, y2, &num_samples_x, &num_samples_y);
1362
// Compute the warp from rectangular coordinates.
1363
Mat3 canonical_homography = ComputeCanonicalHomography(x1, y1,
1367
ceres::Problem problem;
1369
// Construct the warp cost function. AutoDiffCostFunction takes ownership.
1370
PixelDifferenceCostFunctor<Warp> *pixel_difference_cost_function =
1371
new PixelDifferenceCostFunctor<Warp>(options,
1372
image_and_gradient1,
1373
image_and_gradient2,
1374
canonical_homography,
1378
problem.AddResidualBlock(
1379
new ceres::AutoDiffCostFunction<
1380
PixelDifferenceCostFunctor<Warp>,
1382
Warp::NUM_PARAMETERS>(pixel_difference_cost_function,
1383
num_samples_x * num_samples_y),
1387
// Construct the regularizing cost function
1388
if (options.regularization_coefficient != 0.0) {
1389
WarpRegularizingCostFunctor<Warp> *regularizing_warp_cost_function =
1390
new WarpRegularizingCostFunctor<Warp>(options,
1396
problem.AddResidualBlock(
1397
new ceres::AutoDiffCostFunction<
1398
WarpRegularizingCostFunctor<Warp>,
1399
8 /* num_residuals */,
1400
Warp::NUM_PARAMETERS>(regularizing_warp_cost_function),
1405
// Configure the solve.
1406
ceres::Solver::Options solver_options;
1407
solver_options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
1408
solver_options.max_num_iterations = options.max_iterations;
1409
solver_options.update_state_every_iteration = true;
1410
solver_options.parameter_tolerance = 1e-16;
1411
solver_options.function_tolerance = 1e-16;
1413
// Prevent the corners from going outside the destination image and
1414
// terminate if the optimizer is making tiny moves (converged).
1415
TerminationCheckingCallback<Warp> callback(options, image2, warp, x1, y1);
1416
solver_options.callbacks.push_back(&callback);
1419
ceres::Solver::Summary summary;
1420
ceres::Solve(solver_options, &problem, &summary);
1422
LG << "Summary:\n" << summary.FullReport();
1424
// Update the four points with the found solution; if the solver failed, then
1425
// the warp parameters are the identity (so ignore failure).
1427
// Also warp any extra points on the end of the array.
1428
for (int i = 0; i < 4 + options.num_extra_points; ++i) {
1429
warp.Forward(warp.parameters, x1[i], y1[i], x2 + i, y2 + i);
1430
LG << "Warped point " << i << ": (" << x1[i] << ", " << y1[i] << ") -> ("
1431
<< x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", "
1432
<< (y2[i] - y1[i]) << ").";
1435
// TODO(keir): Update the result statistics.
1436
// TODO(keir): Add a normalize-cross-correlation variant.
1438
if (summary.termination_type == ceres::USER_ABORT) {
1439
result->termination = TrackRegionResult::FELL_OUT_OF_BOUNDS;
1443
// This happens when the minimum corner shift tolerance is reached. Due to
1444
// how the tolerance is computed this can't be done by Ceres. So return the
1445
// same termination enum as Ceres, even though this is slightly different
1446
// than Ceres's parameter tolerance, which operates on the raw parameter
1447
// values rather than the pixel shifts of the patch corners.
1448
if (summary.termination_type == ceres::USER_SUCCESS) {
1449
result->termination = TrackRegionResult::PARAMETER_TOLERANCE;
1453
#define HANDLE_TERMINATION(termination_enum) \
1454
if (summary.termination_type == ceres::termination_enum) { \
1455
result->termination = TrackRegionResult::termination_enum; \
1459
// Avoid computing correlation for tracking failures.
1460
HANDLE_TERMINATION(DID_NOT_RUN);
1461
HANDLE_TERMINATION(NUMERICAL_FAILURE);
1463
// Otherwise, run a final correlation check.
1464
if (options.minimum_correlation > 0.0) {
1465
result->correlation = pixel_difference_cost_function->
1466
PearsonProductMomentCorrelationCoefficient(warp.parameters);
1467
if (result->correlation < options.minimum_correlation) {
1468
LG << "Failing with insufficient correlation.";
1469
result->termination = TrackRegionResult::INSUFFICIENT_CORRELATION;
1474
HANDLE_TERMINATION(PARAMETER_TOLERANCE);
1475
HANDLE_TERMINATION(FUNCTION_TOLERANCE);
1476
HANDLE_TERMINATION(GRADIENT_TOLERANCE);
1477
HANDLE_TERMINATION(NO_CONVERGENCE);
1478
#undef HANDLE_TERMINATION
1481
void TrackRegion(const FloatImage &image1,
1482
const FloatImage &image2,
1483
const double *x1, const double *y1,
1484
const TrackRegionOptions &options,
1485
double *x2, double *y2,
1486
TrackRegionResult *result) {
1487
// Enum is necessary due to templated nature of autodiff.
1488
#define HANDLE_MODE(mode_enum, mode_type) \
1489
if (options.mode == TrackRegionOptions::mode_enum) { \
1490
TemplatedTrackRegion<mode_type>(image1, image2, \
1497
HANDLE_MODE(TRANSLATION, TranslationWarp);
1498
HANDLE_MODE(TRANSLATION_SCALE, TranslationScaleWarp);
1499
HANDLE_MODE(TRANSLATION_ROTATION, TranslationRotationWarp);
1500
HANDLE_MODE(TRANSLATION_ROTATION_SCALE, TranslationRotationScaleWarp);
1501
HANDLE_MODE(AFFINE, AffineWarp);
1502
HANDLE_MODE(HOMOGRAPHY, HomographyWarp);
1506
bool SamplePlanarPatch(const FloatImage &image,
1507
const double *xs, const double *ys,
1508
int num_samples_x, int num_samples_y,
1509
FloatImage *mask, FloatImage *patch,
1510
double *warped_position_x, double *warped_position_y) {
1511
// Bail early if the points are outside the image.
1512
if (!AllInBounds(image, xs, ys)) {
1513
LG << "Can't sample patch: out of bounds.";
1517
// Make the patch have the appropriate size, and match the depth of image.
1518
patch->Resize(num_samples_y, num_samples_x, image.Depth());
1520
// Compute the warp from rectangular coordinates.
1521
Mat3 canonical_homography = ComputeCanonicalHomography(xs, ys,
1525
// Walk over the coordinates in the canonical space, sampling from the image
1526
// in the original space and copying the result into the patch.
1527
for (int r = 0; r < num_samples_y; ++r) {
1528
for (int c = 0; c < num_samples_x; ++c) {
1529
Vec3 image_position = canonical_homography * Vec3(c, r, 1);
1530
image_position /= image_position(2);
1531
SampleLinear(image, image_position(1),
1533
&(*patch)(r, c, 0));
1535
float mask_value = SampleLinear(*mask, image_position(1),
1536
image_position(0), 0);
1538
for (int d = 0; d < image.Depth(); d++)
1539
(*patch)(r, c, d) *= mask_value;
1544
Vec3 warped_position = canonical_homography.inverse() * Vec3(xs[4], ys[4], 1);
1545
warped_position /= warped_position(2);
1547
*warped_position_x = warped_position(0);
1548
*warped_position_y = warped_position(1);
1553
} // namespace libmv
added
removed
extern/libmv/libmv/tracking/track_region.cc
