1 /*********************************************************************
2 * trackFeatures.c
3 *
4 *********************************************************************/
5
6 /* Standard includes */
7 #include <math.h> /* fabs() */
8 #include <stdlib.h> /* malloc() */
9 #include <stdio.h> /* fflush() */
10
11 /* Our includes */
12 #include "base.h"
13 #include "error.h"
14 #include "convolve.h" /* for computing pyramid */
15 #include "klt.h"
16 #include "klt_util.h" /* _KLT_FloatImage */
17 #include "pyramid.h" /* _KLT_Pyramid */
18
19 typedef float *_FloatWindow;
20
21 /*********************************************************************
22 * _interpolate
23 *
24 * Given a point (x,y) in an image, computes the bilinear interpolated
25 * gray-level value of the point in the image.
26 */
27
_interpolate(float x,float y,_KLT_FloatImage img)28 float _interpolate(float x, float y, _KLT_FloatImage img) {
29
30 int xt = x;
31 int yt = y;
32
33 float ax = x - xt;
34 float ay = y - yt;
35
36 float *ptr = img->data + (img->ncols * yt) + xt;
37
38 return (
39 (1 - ax) * (1 - ay) * (*ptr) +
40 ax * (1 - ay) * (*(ptr + 1)) +
41 (1 - ax) * ay * (*(ptr + img->ncols)) +
42 ax * ay * (*(ptr + img->ncols + 1))
43 );
44 }
45
46 /*********************************************************************
47 * _computeIntensityDifference
48 *
49 * Given two images and the window center in both images,
50 * aligns the images wrt the window and computes the difference
51 * between the two overlaid images.
52 */
53
_computeIntensityDifference(_KLT_FloatImage img1,_KLT_FloatImage img2,float x1,float y1,float x2,float y2,int width,int height,_FloatWindow imgdiff)54 void _computeIntensityDifference(
55 _KLT_FloatImage img1, /* images */
56 _KLT_FloatImage img2,
57 float x1, float y1, /* center of window in 1st img */
58 float x2, float y2, /* center of window in 2nd img */
59 int width, int height, /* size of window */
60 _FloatWindow imgdiff) /* output */
61 {
62 int hw = width/2, hh = height/2;
63 float g1, g2;
64 int i, j;
65
66 /* Compute values */
67 for (j = -hh ; j <= hh ; j++)
68 for (i = -hw ; i <= hw ; i++) {
69 g1 = _interpolate(x1+i, y1+j, img1);
70 g2 = _interpolate(x2+i, y2+j, img2);
71 *imgdiff++ = g1 - g2;
72 }
73 }
74
75
76 /*********************************************************************
77 * _computeGradientSum
78 *
79 * Given two gradients and the window center in both images,
80 * aligns the gradients wrt the window and computes the sum of the two
81 * overlaid gradients.
82 */
83
_computeGradientSum(_KLT_FloatImage gradx1,_KLT_FloatImage grady1,_KLT_FloatImage gradx2,_KLT_FloatImage grady2,float x1,float y1,float x2,float y2,int width,int height,_FloatWindow gradx,_FloatWindow grady)84 void _computeGradientSum(
85 _KLT_FloatImage gradx1, /* gradient images */
86 _KLT_FloatImage grady1,
87 _KLT_FloatImage gradx2,
88 _KLT_FloatImage grady2,
89 float x1, float y1, /* center of window in 1st img */
90 float x2, float y2, /* center of window in 2nd img */
91 int width, int height, /* size of window */
92 _FloatWindow gradx, /* output */
93 _FloatWindow grady) /* " */
94 {
95 int hw = width/2, hh = height/2;
96 float g1, g2;
97 int i, j;
98
99 /* Compute values */
100 for (j = -hh ; j <= hh ; j++)
101 for (i = -hw ; i <= hw ; i++) {
102 g1 = _interpolate(x1+i, y1+j, gradx1);
103 g2 = _interpolate(x2+i, y2+j, gradx2);
104 *gradx++ = g1 + g2;
105 g1 = _interpolate(x1+i, y1+j, grady1);
106 g2 = _interpolate(x2+i, y2+j, grady2);
107 *grady++ = g1 + g2;
108 }
109 }
110
111 /*********************************************************************
112 * _compute2by2GradientMatrix
113 *
114 */
115
_compute2by2GradientMatrix(_FloatWindow gradx,_FloatWindow grady,int width,int height,float * gxx,float * gxy,float * gyy)116 void _compute2by2GradientMatrix(
117 _FloatWindow gradx,
118 _FloatWindow grady,
119 int width, /* size of window */
120 int height,
121 float *gxx, /* return values */
122 float *gxy,
123 float *gyy)
124
125 {
126 float gx, gy;
127 int i;
128
129 /* Compute values */
130 *gxx = 0.0; *gxy = 0.0; *gyy = 0.0;
131 for (i = 0 ; i < width * height ; i++) {
132 gx = *gradx++;
133 gy = *grady++;
134 *gxx += gx*gx;
135 *gxy += gx*gy;
136 *gyy += gy*gy;
137 }
138 }
139
140
141 /*********************************************************************
142 * _compute2by1ErrorVector
143 *
144 */
145
_compute2by1ErrorVector(_FloatWindow imgdiff,_FloatWindow gradx,_FloatWindow grady,int width,int height,float step_factor,float * ex,float * ey)146 void _compute2by1ErrorVector(
147 _FloatWindow imgdiff,
148 _FloatWindow gradx,
149 _FloatWindow grady,
150 int width, /* size of window */
151 int height,
152 float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */
153 float *ex, /* return values */
154 float *ey)
155 {
156 float diff;
157 int i;
158
159 /* Compute values */
160 *ex = 0; *ey = 0;
161 for (i = 0 ; i < width * height ; i++) {
162 diff = *imgdiff++;
163 *ex += diff * (*gradx++);
164 *ey += diff * (*grady++);
165 }
166 *ex *= step_factor;
167 *ey *= step_factor;
168 }
169
170
171 /*********************************************************************
172 * _solveEquation
173 *
174 * Solves the 2x2 matrix equation
175 * [gxx gxy] [dx] = [ex]
176 * [gxy gyy] [dy] = [ey]
177 * for dx and dy.
178 *
179 * Returns KLT_TRACKED on success and KLT_SMALL_DET on failure
180 */
181
_solveEquation(float gxx,float gxy,float gyy,float ex,float ey,float small,float * dx,float * dy)182 int _solveEquation(
183 float gxx, float gxy, float gyy,
184 float ex, float ey,
185 float small,
186 float *dx, float *dy)
187 {
188 float det = gxx*gyy - gxy*gxy;
189
190
191 if (det < small) return KLT_SMALL_DET;
192
193 *dx = (gyy*ex - gxy*ey)/det;
194 *dy = (gxx*ey - gxy*ex)/det;
195 return KLT_TRACKED;
196 }
197
198
199 /*********************************************************************
200 * _allocateFloatWindow
201 */
202
_allocateFloatWindow(int width,int height)203 _FloatWindow _allocateFloatWindow(int width, int height) {
204
205 return (_FloatWindow)malloc(width * height * sizeof(float));
206 }
207
208 /*********************************************************************
209 * _sumAbsFloatWindow
210 */
211
_sumAbsFloatWindow(_FloatWindow fw,int width,int height)212 float _sumAbsFloatWindow(
213 _FloatWindow fw,
214 int width,
215 int height)
216 {
217 float sum = 0.0;
218 int w;
219
220 for ( ; height > 0 ; height--)
221 for (w=0 ; w < width ; w++)
222 sum += (float) fabs(*fw++);
223
224 return sum;
225 }
226
227
228 /*********************************************************************
229 * _trackFeature
230 *
231 * Tracks a feature point from one image to the next.
232 *
233 * RETURNS
234 * KLT_SMALL_DET if feature is lost,
235 * KLT_MAX_ITERATIONS if tracking stopped because iterations timed out,
236 * KLT_TRACKED otherwise.
237 */
238
_trackFeature(float x1,float y1,float * x2,float * y2,_KLT_FloatImage img1,_KLT_FloatImage gradx1,_KLT_FloatImage grady1,_KLT_FloatImage img2,_KLT_FloatImage gradx2,_KLT_FloatImage grady2,int width,int height,float step_factor,int max_iterations,float small,float th,float max_residue)239 int _trackFeature(
240 float x1, /* location of window in first image */
241 float y1,
242 float *x2, /* starting location of search in second image */
243 float *y2,
244 _KLT_FloatImage img1,
245 _KLT_FloatImage gradx1,
246 _KLT_FloatImage grady1,
247 _KLT_FloatImage img2,
248 _KLT_FloatImage gradx2,
249 _KLT_FloatImage grady2,
250 int width, /* size of window */
251 int height,
252 float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */
253 int max_iterations,
254 float small, /* determinant threshold for declaring KLT_SMALL_DET */
255 float th, /* displacement threshold for stopping */
256 float max_residue) /* residue threshold for declaring KLT_LARGE_RESIDUE */
257 {
258 _FloatWindow imgdiff, gradx, grady;
259 float gxx, gxy, gyy, ex, ey, dx, dy;
260 int iteration = 0;
261 int status;
262 int hw = width / 2;
263 int hh = height / 2;
264 int nc = img1->ncols;
265 int nr = img1->nrows;
266 float one_plus_eps = 1.001f; /* To prevent rounding errors */
267
268
269 /* Allocate memory for windows */
270 imgdiff = _allocateFloatWindow(width, height);
271 gradx = _allocateFloatWindow(width, height);
272 grady = _allocateFloatWindow(width, height);
273
274 /* Iteratively update the window position */
275 do {
276
277 /* If out of bounds, exit loop */
278 if ( x1-hw < 0.0f || nc-( x1+hw) < one_plus_eps ||
279 *x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps ||
280 y1-hh < 0.0f || nr-( y1+hh) < one_plus_eps ||
281 *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps) {
282 status = KLT_OOB;
283 break;
284 }
285
286 /* Compute gradient and difference windows */
287 _computeIntensityDifference(img1, img2, x1, y1, *x2, *y2,
288 width, height, imgdiff);
289 _computeGradientSum(gradx1, grady1, gradx2, grady2,
290 x1, y1, *x2, *y2, width, height, gradx, grady);
291
292
293 /* Use these windows to construct matrices */
294 _compute2by2GradientMatrix(gradx, grady, width, height,
295 &gxx, &gxy, &gyy);
296 _compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor,
297 &ex, &ey);
298
299 /* Using matrices, solve equation for new displacement */
300 status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy);
301 if (status == KLT_SMALL_DET) break;
302
303 *x2 += dx;
304 *y2 += dy;
305 iteration++;
306
307 } while ((fabs(dx)>=th || fabs(dy)>=th) && iteration < max_iterations);
308
309 /* Check whether window is out of bounds */
310 if (*x2-hw < 0.0f || nc-(*x2+hw) < one_plus_eps ||
311 *y2-hh < 0.0f || nr-(*y2+hh) < one_plus_eps)
312 status = KLT_OOB;
313
314 /* Check whether residue is too large */
315 if (status == KLT_TRACKED) {
316
317 _computeIntensityDifference(
318 img1, img2, x1, y1, *x2, *y2,
319 width, height, imgdiff);
320
321 if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue)
322 status = KLT_LARGE_RESIDUE;
323 }
324
325 /* Free memory */
326 free(imgdiff);
327 free(gradx);
328 free(grady);
329
330 /* Return appropriate value */
331 if (status == KLT_SMALL_DET)
332 return KLT_SMALL_DET;
333 else if (status == KLT_OOB)
334 return KLT_OOB;
335 else if (status == KLT_LARGE_RESIDUE)
336 return KLT_LARGE_RESIDUE;
337 else if (iteration >= max_iterations)
338 return KLT_MAX_ITERATIONS;
339 else
340 return KLT_TRACKED;
341
342 }
343
344
345 /*********************************************************************/
346
_outOfBounds(float x,float y,int ncols,int nrows,int borderx,int bordery)347 KLT_BOOL _outOfBounds(
348 float x,
349 float y,
350 int ncols,
351 int nrows,
352 int borderx,
353 int bordery)
354 {
355 return (x < borderx || x > ncols-1-borderx ||
356 y < bordery || y > nrows-1-bordery );
357 }
358
359 /*********************************************************************
360 * KLTTrackFeatures
361 *
362 * Tracks feature points from one image to the next.
363 */
364
KLTTrackFeatures(KLT_TrackingContext tc,KLT_PixelType * img1,KLT_PixelType * img2,int ncols,int nrows,KLT_FeatureList featurelist)365 void KLTTrackFeatures(
366 KLT_TrackingContext tc,
367 KLT_PixelType *img1,
368 KLT_PixelType *img2,
369 int ncols,
370 int nrows,
371 KLT_FeatureList featurelist)
372 {
373 _KLT_FloatImage tmpimg = NULL;
374 _KLT_FloatImage floatimg1 = NULL;
375 _KLT_FloatImage floatimg2 = NULL;
376
377 _KLT_Pyramid pyramid1, pyramid1_gradx, pyramid1_grady;
378 _KLT_Pyramid pyramid2, pyramid2_gradx, pyramid2_grady;
379
380 int val = 0;
381 KLT_BOOL floatimg1_created = FALSE;
382
383 float subsampling = (float)tc->subsampling;
384 float xloc, yloc, xlocout, ylocout;
385 int i, indx, r;
386
387 if (tc->verbose >= 1) {
388 fprintf(stderr, "(KLT) Tracking %d features in a %d by %d image... ",
389 KLTCountRemainingFeatures(featurelist), ncols, nrows);
390 fflush(stderr);
391 }
392
393 /* Check window size (and correct if necessary) */
394 if (tc->window_width % 2 != 1) {
395 tc->window_width = tc->window_width+1;
396 KLTWarning("Tracking context's window width must be odd. "
397 "Changing to %d.\n", tc->window_width);
398 }
399 if (tc->window_height % 2 != 1) {
400 tc->window_height = tc->window_height+1;
401 KLTWarning("Tracking context's window height must be odd. "
402 "Changing to %d.\n", tc->window_height);
403 }
404 if (tc->window_width < 3) {
405 tc->window_width = 3;
406 KLTWarning("Tracking context's window width must be at least three. \n"
407 "Changing to %d.\n", tc->window_width);
408 }
409 if (tc->window_height < 3) {
410 tc->window_height = 3;
411 KLTWarning("Tracking context's window height must be at least three. \n"
412 "Changing to %d.\n", tc->window_height);
413 }
414
415 /* Create temporary image */
416 tmpimg = _KLTCreateFloatImage(ncols, nrows);
417
418 /* Process first image by converting to float, smoothing, computing */
419 /* pyramid, and computing gradient pyramids */
420 if (tc->sequentialMode && tc->pyramid_last != NULL) {
421 pyramid1 = (_KLT_Pyramid) tc->pyramid_last;
422 pyramid1_gradx = (_KLT_Pyramid) tc->pyramid_last_gradx;
423 pyramid1_grady = (_KLT_Pyramid) tc->pyramid_last_grady;
424 if (pyramid1->ncols[0] != ncols || pyramid1->nrows[0] != nrows)
425 KLTError("(KLTTrackFeatures) Size of incoming image (%d by %d) "
426 "is different from size of previous image (%d by %d)\n",
427 ncols, nrows, pyramid1->ncols[0], pyramid1->nrows[0]);
428 } else {
429 floatimg1_created = TRUE;
430 floatimg1 = _KLTCreateFloatImage(ncols, nrows);
431 _KLTToFloatImage(img1, ncols, nrows, tmpimg);
432 _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg1);
433 pyramid1 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
434 _KLTComputePyramid(floatimg1, pyramid1, tc->pyramid_sigma_fact);
435 pyramid1_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
436 pyramid1_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
437 for (i = 0 ; i < tc->nPyramidLevels ; i++)
438 _KLTComputeGradients(pyramid1->img[i], tc->grad_sigma,
439 pyramid1_gradx->img[i],
440 pyramid1_grady->img[i]);
441 }
442
443 /* Do the same thing with second image */
444 floatimg2 = _KLTCreateFloatImage(ncols, nrows);
445 _KLTToFloatImage(img2, ncols, nrows, tmpimg);
446 _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg2);
447 pyramid2 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
448 _KLTComputePyramid(floatimg2, pyramid2, tc->pyramid_sigma_fact);
449 pyramid2_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
450 pyramid2_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
451 for (i = 0 ; i < tc->nPyramidLevels ; i++)
452 _KLTComputeGradients(pyramid2->img[i], tc->grad_sigma,
453 pyramid2_gradx->img[i],
454 pyramid2_grady->img[i]);
455
456 /* For each feature, do ... */
457 for (indx = 0 ; indx < featurelist->nFeatures ; indx++) {
458
459 /* Only track features that are not lost */
460 if (featurelist->feature[indx]->val >= 0) {
461
462 xloc = featurelist->feature[indx]->x;
463 yloc = featurelist->feature[indx]->y;
464
465 /* Transform location to coarsest resolution */
466 for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) {
467 xloc /= subsampling; yloc /= subsampling;
468 }
469 xlocout = xloc; ylocout = yloc;
470
471 /* Beginning with coarsest resolution, do ... */
472 for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) {
473
474 /* Track feature at current resolution */
475 xloc *= subsampling; yloc *= subsampling;
476 xlocout *= subsampling; ylocout *= subsampling;
477
478 val = _trackFeature(xloc, yloc,
479 &xlocout, &ylocout,
480 pyramid1->img[r],
481 pyramid1_gradx->img[r], pyramid1_grady->img[r],
482 pyramid2->img[r],
483 pyramid2_gradx->img[r], pyramid2_grady->img[r],
484 tc->window_width, tc->window_height,
485 tc->step_factor,
486 tc->max_iterations,
487 tc->min_determinant,
488 tc->min_displacement,
489 tc->max_residue);
490
491 if (val==KLT_SMALL_DET || val==KLT_OOB)
492 break;
493 }
494
495 /* Record feature */
496 if (val == KLT_OOB) {
497 featurelist->feature[indx]->x = -1.0;
498 featurelist->feature[indx]->y = -1.0;
499 featurelist->feature[indx]->val = KLT_OOB;
500 } else if (_outOfBounds(xlocout, ylocout, ncols, nrows, tc->borderx, tc->bordery)) {
501 featurelist->feature[indx]->x = -1.0;
502 featurelist->feature[indx]->y = -1.0;
503 featurelist->feature[indx]->val = KLT_OOB;
504 } else if (val == KLT_SMALL_DET) {
505 featurelist->feature[indx]->x = -1.0;
506 featurelist->feature[indx]->y = -1.0;
507 featurelist->feature[indx]->val = KLT_SMALL_DET;
508 } else if (val == KLT_LARGE_RESIDUE) {
509 featurelist->feature[indx]->x = -1.0;
510 featurelist->feature[indx]->y = -1.0;
511 featurelist->feature[indx]->val = KLT_LARGE_RESIDUE;
512 } else if (val == KLT_MAX_ITERATIONS) {
513 featurelist->feature[indx]->x = -1.0;
514 featurelist->feature[indx]->y = -1.0;
515 featurelist->feature[indx]->val = KLT_MAX_ITERATIONS;
516 } else {
517 featurelist->feature[indx]->x = xlocout;
518 featurelist->feature[indx]->y = ylocout;
519 featurelist->feature[indx]->val = KLT_TRACKED;
520 }
521 }
522 }
523
524 if (tc->sequentialMode) {
525 tc->pyramid_last = pyramid2;
526 tc->pyramid_last_gradx = pyramid2_gradx;
527 tc->pyramid_last_grady = pyramid2_grady;
528 } else {
529 _KLTFreePyramid(pyramid2);
530 _KLTFreePyramid(pyramid2_gradx);
531 _KLTFreePyramid(pyramid2_grady);
532 }
533
534 /* Free memory */
535 _KLTFreeFloatImage(tmpimg);
536 if (floatimg1_created) _KLTFreeFloatImage(floatimg1);
537 _KLTFreeFloatImage(floatimg2);
538 _KLTFreePyramid(pyramid1);
539 _KLTFreePyramid(pyramid1_gradx);
540 _KLTFreePyramid(pyramid1_grady);
541
542 if (tc->verbose >= 1)
543 fprintf(
544 stderr,
545 "\n\t%d features successfully tracked.\n",
546 KLTCountRemainingFeatures(featurelist)
547 );
548 }
549
550