1 /*********************************************************************
2 * selectGoodFeatures.c
3 *
4 *********************************************************************/
5
6 /* Standard includes */
7 #include <cassert>
8 #include <cstdlib> /* malloc(), qsort() */
9 #include <cstdio> /* fflush() */
10 #include <cstring> /* memset() */
11 #include <cmath> /* fsqrt() */
12 #define fsqrt(X) sqrt(X)
13
14 /* Our includes */
15 #include "base.h"
16 #include "error.h"
17 #include "convolve.h"
18 #include "klt.h"
19 #include "klt_util.h"
20 #include "pyramid.h"
21
22 int KLT_verbose = 1;
23
24 typedef enum {SELECTING_ALL, REPLACING_SOME} selectionMode;
25
26
27 /*********************************************************************
28 * _quicksort
29 * Replacement for qsort(). Computing time is decreased by taking
30 * advantage of specific knowledge of our array (that there are
31 * three ints associated with each point).
32 *
33 * This routine generously provided by
34 * Manolis Lourakis <lourakis@csi.forth.gr>
35 *
36 * NOTE: The results of this function may be slightly different from
37 * those of qsort(). This is due to the fact that different sort
38 * algorithms have different behaviours when sorting numbers with the
39 * same value: Some leave them in the same relative positions in the
40 * array, while others change their relative positions. For example,
41 * if you have the array [c d b1 a b2] with b1=b2, it may be sorted as
42 * [a b1 b2 c d] or [a b2 b1 c d].
43 */
44
45 #define SWAP3(list, i, j) \
46 { int *pi, *pj, tmp; \
47 pi=list+3*(i); pj=list+3*(j); \
48 \
49 tmp=*pi; \
50 *pi++=*pj; \
51 *pj++=tmp; \
52 \
53 tmp=*pi; \
54 *pi++=*pj; \
55 *pj++=tmp; \
56 \
57 tmp=*pi; \
58 *pi=*pj; \
59 *pj=tmp; \
60 }
61
_quicksort(int * pointlist,int n)62 void _quicksort(int *pointlist, int n)
63 {
64 unsigned int i, j, ln, rn;
65
66 while (n > 1)
67 {
68 SWAP3(pointlist, 0, n/2);
69 for (i = 0, j = n; ; )
70 {
71 do
72 --j;
73 while (pointlist[3*j+2] < pointlist[2]);
74 do
75 ++i;
76 while (i < j && pointlist[3*i+2] > pointlist[2]);
77 if (i >= j)
78 break;
79 SWAP3(pointlist, i, j);
80 }
81 SWAP3(pointlist, j, 0);
82 ln = j;
83 rn = n - ++j;
84 if (ln < rn)
85 {
86 _quicksort(pointlist, ln);
87 pointlist += 3*j;
88 n = rn;
89 }
90 else
91 {
92 _quicksort(pointlist + 3*j, rn);
93 n = ln;
94 }
95 }
96 }
97 #undef SWAP3
98
99
100 /*********************************************************************/
101
_fillFeaturemap(int x,int y,uchar * featuremap,int mindist,int ncols,int nrows)102 static void _fillFeaturemap(
103 int x, int y,
104 uchar *featuremap,
105 int mindist,
106 int ncols,
107 int nrows)
108 {
109 int ix, iy;
110
111 for (iy = y - mindist ; iy <= y + mindist ; iy++)
112 for (ix = x - mindist ; ix <= x + mindist ; ix++)
113 if (ix >= 0 && ix < ncols && iy >= 0 && iy < nrows)
114 featuremap[iy*ncols+ix] = 1;
115 }
116
117
118 /*********************************************************************
119 * _enforceMinimumDistance
120 *
121 * Removes features that are within close proximity to better features.
122 *
123 * INPUTS
124 * featurelist: A list of features. The nFeatures property
125 * is used.
126 *
127 * OUTPUTS
128 * featurelist: Is overwritten. Nearby "redundant" features are removed.
129 * Writes -1's into the remaining elements.
130 *
131 * RETURNS
132 * The number of remaining features.
133 */
134
_enforceMinimumDistance(int * pointlist,int npoints,KLT_FeatureList featurelist,int ncols,int nrows,int mindist,int min_eigenvalue,KLT_BOOL overwriteAllFeatures)135 static void _enforceMinimumDistance(
136 int *pointlist, /* featurepoints */
137 int npoints, /* number of featurepoints */
138 KLT_FeatureList featurelist, /* features */
139 int ncols, int nrows, /* size of images */
140 int mindist, /* min. dist b/w features */
141 int min_eigenvalue, /* min. eigenvalue */
142 KLT_BOOL overwriteAllFeatures)
143 {
144 int indx; /* Index into features */
145 int x, y, val; /* Location and trackability of pixel under consideration */
146 uchar *featuremap; /* Boolean array recording proximity of features */
147 int *ptr;
148
149 /* Cannot add features with an eigenvalue less than one */
150 if (min_eigenvalue < 1) min_eigenvalue = 1;
151
152 /* Allocate memory for feature map and clear it */
153 featuremap = (uchar *) malloc(ncols * nrows * sizeof(uchar));
154 memset(featuremap, 0, ncols*nrows);
155
156 /* Necessary because code below works with (mindist-1) */
157 mindist--;
158
159 /* If we are keeping all old good features, then add them to the featuremap */
160 if (!overwriteAllFeatures)
161 for (indx = 0 ; indx < featurelist->nFeatures ; indx++)
162 if (featurelist->feature[indx]->val >= 0) {
163 x = (int) featurelist->feature[indx]->x;
164 y = (int) featurelist->feature[indx]->y;
165 _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows);
166 }
167
168 /* For each feature point, in descending order of importance, do ... */
169 ptr = pointlist;
170 indx = 0;
171 while (1) {
172
173 /* If we can't add all the points, then fill in the rest
174 of the featurelist with -1's */
175 if (ptr >= pointlist + 3*npoints) {
176 while (indx < featurelist->nFeatures) {
177 if (overwriteAllFeatures ||
178 featurelist->feature[indx]->val < 0) {
179 featurelist->feature[indx]->x = -1;
180 featurelist->feature[indx]->y = -1;
181 featurelist->feature[indx]->val = KLT_NOT_FOUND;
182 featurelist->feature[indx]->aff_img = nullptr;
183 featurelist->feature[indx]->aff_img_gradx = nullptr;
184 featurelist->feature[indx]->aff_img_grady = nullptr;
185 featurelist->feature[indx]->aff_x = -1.0;
186 featurelist->feature[indx]->aff_y = -1.0;
187 featurelist->feature[indx]->aff_Axx = 1.0;
188 featurelist->feature[indx]->aff_Ayx = 0.0;
189 featurelist->feature[indx]->aff_Axy = 0.0;
190 featurelist->feature[indx]->aff_Ayy = 1.0;
191 }
192 indx++;
193 }
194 break;
195 }
196
197 x = *ptr++;
198 y = *ptr++;
199 val = *ptr++;
200
201 /* Ensure that feature is in-bounds */
202 assert(x >= 0);
203 assert(x < ncols);
204 assert(y >= 0);
205 assert(y < nrows);
206
207 while (!overwriteAllFeatures &&
208 indx < featurelist->nFeatures &&
209 featurelist->feature[indx]->val >= 0)
210 indx++;
211
212 if (indx >= featurelist->nFeatures) break;
213
214 /* If no neighbor has been selected, and if the minimum
215 eigenvalue is large enough, then add feature to the current list */
216 if (!featuremap[y*ncols+x] && val >= min_eigenvalue) {
217 featurelist->feature[indx]->x = (KLT_locType) x;
218 featurelist->feature[indx]->y = (KLT_locType) y;
219 featurelist->feature[indx]->val = (int) val;
220 featurelist->feature[indx]->aff_img = nullptr;
221 featurelist->feature[indx]->aff_img_gradx = nullptr;
222 featurelist->feature[indx]->aff_img_grady = nullptr;
223 featurelist->feature[indx]->aff_x = -1.0;
224 featurelist->feature[indx]->aff_y = -1.0;
225 featurelist->feature[indx]->aff_Axx = 1.0;
226 featurelist->feature[indx]->aff_Ayx = 0.0;
227 featurelist->feature[indx]->aff_Axy = 0.0;
228 featurelist->feature[indx]->aff_Ayy = 1.0;
229 indx++;
230
231 /* Fill in surrounding region of feature map, but
232 make sure that pixels are in-bounds */
233 _fillFeaturemap(x, y, featuremap, mindist, ncols, nrows);
234 }
235 }
236
237 /* Free feature map */
238 free(featuremap);
239 }
240
241
242 /*********************************************************************
243 * _comparePoints
244 *
245 * Used by qsort (in _KLTSelectGoodFeatures) to determine
246 * which feature is better.
247 * By switching the '>' with the '<', qsort is fooled into sorting
248 * in descending order.
249 */
250
251 #ifdef KLT_USE_QSORT
_comparePoints(const void * a,const void * b)252 static int _comparePoints(const void *a, const void *b)
253 {
254 int v1 = *(((int *) a) + 2);
255 int v2 = *(((int *) b) + 2);
256
257 if (v1 > v2) return(-1);
258 else if (v1 < v2) return(1);
259 else return(0);
260 }
261 #endif
262
263
264 /*********************************************************************
265 * _sortPointList
266 */
267
_sortPointList(int * pointlist,int npoints)268 static void _sortPointList(
269 int *pointlist,
270 int npoints)
271 {
272 #ifdef KLT_USE_QSORT
273 qsort(pointlist, npoints, 3*sizeof(int), _comparePoints);
274 #else
275 _quicksort(pointlist, npoints);
276 #endif
277 }
278
279
280 /*********************************************************************
281 * _minEigenvalue
282 *
283 * Given the three distinct elements of the symmetric 2x2 matrix
284 * [gxx gxy]
285 * [gxy gyy],
286 * Returns the minimum eigenvalue of the matrix.
287 */
288
_minEigenvalue(float gxx,float gxy,float gyy)289 static float _minEigenvalue(float gxx, float gxy, float gyy)
290 {
291 return (float) ((gxx + gyy - sqrt((gxx - gyy)*(gxx - gyy) + 4*gxy*gxy))/2.0f);
292 }
293
294
295 /*********************************************************************/
296
_KLTSelectGoodFeatures(KLT_TrackingContext tc,KLT_PixelType * img,int ncols,int nrows,KLT_FeatureList featurelist,selectionMode mode)297 void _KLTSelectGoodFeatures(
298 KLT_TrackingContext tc,
299 KLT_PixelType *img,
300 int ncols,
301 int nrows,
302 KLT_FeatureList featurelist,
303 selectionMode mode)
304 {
305 _KLT_FloatImage floatimg, gradx, grady;
306 int window_hw, window_hh;
307 int *pointlist;
308 int npoints = 0;
309 KLT_BOOL overwriteAllFeatures = (mode == SELECTING_ALL) ?
310 TRUE : FALSE;
311 KLT_BOOL floatimages_created = FALSE;
312
313 /* Check window size (and correct if necessary) */
314 if (tc->window_width % 2 != 1) {
315 tc->window_width = tc->window_width+1;
316 KLTWarning("Tracking context's window width must be odd. "
317 "Changing to %d.\n", tc->window_width);
318 }
319 if (tc->window_height % 2 != 1) {
320 tc->window_height = tc->window_height+1;
321 KLTWarning("Tracking context's window height must be odd. "
322 "Changing to %d.\n", tc->window_height);
323 }
324 if (tc->window_width < 3) {
325 tc->window_width = 3;
326 KLTWarning("Tracking context's window width must be at least three. \n"
327 "Changing to %d.\n", tc->window_width);
328 }
329 if (tc->window_height < 3) {
330 tc->window_height = 3;
331 KLTWarning("Tracking context's window height must be at least three. \n"
332 "Changing to %d.\n", tc->window_height);
333 }
334 window_hw = tc->window_width/2;
335 window_hh = tc->window_height/2;
336
337 /* Create pointlist, which is a simplified version of a featurelist, */
338 /* for speed. Contains only integer locations and values. */
339 pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int));
340
341 /* Create temporary images, etc. */
342 if (mode == REPLACING_SOME &&
343 tc->sequentialMode && tc->pyramid_last != nullptr) {
344 floatimg = ((_KLT_Pyramid) tc->pyramid_last)->img[0];
345 gradx = ((_KLT_Pyramid) tc->pyramid_last_gradx)->img[0];
346 grady = ((_KLT_Pyramid) tc->pyramid_last_grady)->img[0];
347 assert(gradx != nullptr);
348 assert(grady != nullptr);
349 } else {
350 floatimages_created = TRUE;
351 floatimg = _KLTCreateFloatImage(ncols, nrows);
352 gradx = _KLTCreateFloatImage(ncols, nrows);
353 grady = _KLTCreateFloatImage(ncols, nrows);
354 if (tc->smoothBeforeSelecting) {
355 _KLT_FloatImage tmpimg;
356 tmpimg = _KLTCreateFloatImage(ncols, nrows);
357 _KLTToFloatImage(img, ncols, nrows, tmpimg);
358 _KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg);
359 _KLTFreeFloatImage(tmpimg);
360 } else _KLTToFloatImage(img, ncols, nrows, floatimg);
361
362 /* Compute gradient of image in x and y direction */
363 _KLTComputeGradients(floatimg, tc->grad_sigma, gradx, grady);
364 }
365
366 /* Write internal images */
367 if (tc->writeInternalImages) {
368 _KLTWriteFloatImageToPGM(floatimg, "kltimg_sgfrlf.pgm");
369 _KLTWriteFloatImageToPGM(gradx, "kltimg_sgfrlf_gx.pgm");
370 _KLTWriteFloatImageToPGM(grady, "kltimg_sgfrlf_gy.pgm");
371 }
372
373 /* Compute trackability of each image pixel as the minimum
374 of the two eigenvalues of the Z matrix */
375 {
376 float gx, gy;
377 float gxx, gxy, gyy;
378 int xx, yy;
379 int *ptr;
380 float val;
381 unsigned int limit = 1;
382 int borderx = tc->borderx; /* Must not touch cols */
383 int bordery = tc->bordery; /* lost by convolution */
384 int x, y;
385
386 if (borderx < window_hw) borderx = window_hw;
387 if (bordery < window_hh) bordery = window_hh;
388
389 /* Find largest value of an int */
390 for (size_t i = 0 ; i < sizeof(int) ; i++) limit *= 256;
391 limit = limit/2 - 1;
392
393 /* For most of the pixels in the image, do ... */
394 ptr = pointlist;
395 for (y = bordery ; y < nrows - bordery ; y += tc->nSkippedPixels + 1)
396 for (x = borderx ; x < ncols - borderx ; x += tc->nSkippedPixels + 1) {
397
398 /* Sum the gradients in the surrounding window */
399 gxx = 0; gxy = 0; gyy = 0;
400 for (yy = y-window_hh ; yy <= y+window_hh ; yy++)
401 for (xx = x-window_hw ; xx <= x+window_hw ; xx++) {
402 gx = *(gradx->data + ncols*yy+xx);
403 gy = *(grady->data + ncols*yy+xx);
404 gxx += gx * gx;
405 gxy += gx * gy;
406 gyy += gy * gy;
407 }
408
409 /* Store the trackability of the pixel as the minimum
410 of the two eigenvalues */
411 *ptr++ = x;
412 *ptr++ = y;
413 val = _minEigenvalue(gxx, gxy, gyy);
414 if (val > limit) {
415 KLTWarning("(_KLTSelectGoodFeatures) minimum eigenvalue %f is "
416 "greater than the capacity of an int; setting "
417 "to maximum value", val);
418 val = (float) limit;
419 }
420 *ptr++ = (int) val;
421 npoints++;
422 }
423 }
424
425 /* Sort the features */
426 _sortPointList(pointlist, npoints);
427
428 /* Check tc->mindist */
429 if (tc->mindist < 0) {
430 KLTWarning("(_KLTSelectGoodFeatures) Tracking context field tc->mindist "
431 "is negative (%d); setting to zero", tc->mindist);
432 tc->mindist = 0;
433 }
434
435 /* Enforce minimum distance between features */
436 _enforceMinimumDistance(
437 pointlist,
438 npoints,
439 featurelist,
440 ncols, nrows,
441 tc->mindist,
442 tc->min_eigenvalue,
443 overwriteAllFeatures);
444
445 /* Free memory */
446 free(pointlist);
447 if (floatimages_created) {
448 _KLTFreeFloatImage(floatimg);
449 _KLTFreeFloatImage(gradx);
450 _KLTFreeFloatImage(grady);
451 }
452 }
453
454
455 /*********************************************************************
456 * KLTSelectGoodFeatures
457 *
458 * Main routine, visible to the outside. Finds the good features in
459 * an image.
460 *
461 * INPUTS
462 * tc: Contains parameters used in computation (size of image,
463 * size of window, min distance b/w features, sigma to compute
464 * image gradients, # of features desired).
465 * img: Pointer to the data of an image (probably unsigned chars).
466 *
467 * OUTPUTS
468 * features: List of features. The member nFeatures is computed.
469 */
470
KLTSelectGoodFeatures(KLT_TrackingContext tc,KLT_PixelType * img,int ncols,int nrows,KLT_FeatureList fl)471 void KLTSelectGoodFeatures(
472 KLT_TrackingContext tc,
473 KLT_PixelType *img,
474 int ncols,
475 int nrows,
476 KLT_FeatureList fl)
477 {
478 if (KLT_verbose >= 1) {
479 fprintf(stderr, "(KLT) Selecting the %d best features "
480 "from a %d by %d image... ", fl->nFeatures, ncols, nrows);
481 fflush(stderr);
482 }
483
484 _KLTSelectGoodFeatures(tc, img, ncols, nrows,
485 fl, SELECTING_ALL);
486
487 if (KLT_verbose >= 1) {
488 fprintf(stderr, "\n\t%d features found.\n",
489 KLTCountRemainingFeatures(fl));
490 if (tc->writeInternalImages)
491 fprintf(stderr, "\tWrote images to 'kltimg_sgfrlf*.pgm'.\n");
492 fflush(stderr);
493 }
494 }
495
496
497 /*********************************************************************
498 * KLTReplaceLostFeatures
499 *
500 * Main routine, visible to the outside. Replaces the lost features
501 * in an image.
502 *
503 * INPUTS
504 * tc: Contains parameters used in computation (size of image,
505 * size of window, min distance b/w features, sigma to compute
506 * image gradients, # of features desired).
507 * img: Pointer to the data of an image (probably unsigned chars).
508 *
509 * OUTPUTS
510 * features: List of features. The member nFeatures is computed.
511 */
512
KLTReplaceLostFeatures(KLT_TrackingContext tc,KLT_PixelType * img,int ncols,int nrows,KLT_FeatureList fl)513 void KLTReplaceLostFeatures(
514 KLT_TrackingContext tc,
515 KLT_PixelType *img,
516 int ncols,
517 int nrows,
518 KLT_FeatureList fl)
519 {
520 int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl);
521
522 if (KLT_verbose >= 1) {
523 fprintf(stderr, "(KLT) Attempting to replace %d features "
524 "in a %d by %d image... ", nLostFeatures, ncols, nrows);
525 fflush(stderr);
526 }
527
528 /* If there are any lost features, replace them */
529 if (nLostFeatures > 0)
530 _KLTSelectGoodFeatures(tc, img, ncols, nrows,
531 fl, REPLACING_SOME);
532
533 if (KLT_verbose >= 1) {
534 fprintf(stderr, "\n\t%d features replaced.\n",
535 nLostFeatures - fl->nFeatures + KLTCountRemainingFeatures(fl));
536 if (tc->writeInternalImages)
537 fprintf(stderr, "\tWrote images to 'kltimg_sgfrlf*.pgm'.\n");
538 fflush(stderr);
539 }
540 }
541
542
543