void _KLTComputePyramid(
_KLT_FloatImage img,
_KLT_Pyramid pyramid,
float sigma_fact)
{
_KLT_FloatImage currimg, tmpimg;
int ncols = img->ncols, nrows = img->nrows;
int subsampling = pyramid->subsampling;
int subhalf = subsampling / 2;
float sigma = subsampling * sigma_fact; /* empirically determined */
int oldncols;
int i, x, y;
if (subsampling != 2 && subsampling != 4 &&
subsampling != 8 && subsampling != 16 && subsampling != 32)
KLTError("(_KLTComputePyramid) Pyramid's subsampling must "
"be either 2, 4, 8, 16, or 32");
assert(pyramid->ncols[0] == img->ncols);
assert(pyramid->nrows[0] == img->nrows);
/* Copy original image to level 0 of pyramid */
memcpy(pyramid->img[0]->data, img->data, ncols*nrows*sizeof(float));
currimg = img;
for (i = 1 ; i < pyramid->nLevels ; i++) {
tmpimg = _KLTCreateFloatImage(ncols, nrows);
_KLTComputeSmoothedImage(currimg, sigma, tmpimg);
// int k;
// for (k = 1280*100; k < 1280*100 + 100; k ++) {
// //if(pyramid1->img[1]->data[k] > 0.0) {
// //printf("%f\n", pyramid1->img[0]->data[k]);
// //printf("%f\t", floatimg1->data[k]);
// printf("%f\t", tmpimg->data[k]);
// if ((k - 1280*100) % 5 == 0) {
// printf("\n");
// }
// //}
// }
// printf("\n\n");
// exit(1);
/* Subsample */
oldncols = ncols;
ncols /= subsampling; nrows /= subsampling;
for (y = 0 ; y < nrows ; y++)
for (x = 0 ; x < ncols ; x++)
pyramid->img[i]->data[y*ncols+x] =
tmpimg->data[(subsampling*y+subhalf)*oldncols +
(subsampling*x+subhalf)];
/* Reassign current image */
currimg = pyramid->img[i];
_KLTFreeFloatImage(tmpimg);
}
}
void _KLTFreePyramid(
_KLT_Pyramid pyramid)
{
int i;
/* Free images */
for (i = 0 ; i < pyramid->nLevels ; i++)
_KLTFreeFloatImage(pyramid->img[i]);
/* Free structure */
free(pyramid);
}
static void _convolveSeparate(
_KLT_FloatImage imgin,
ConvolutionKernel horiz_kernel,
ConvolutionKernel vert_kernel,
_KLT_FloatImage imgout)
{
/* Create temporary image */
_KLT_FloatImage tmpimg;
tmpimg = _KLTCreateFloatImage(imgin->ncols, imgin->nrows);
/* Do convolution */
_convolveImageHoriz(imgin, horiz_kernel, tmpimg);
_convolveImageVert(tmpimg, vert_kernel, imgout);
/* Free memory */
_KLTFreeFloatImage(tmpimg);
}
void _KLTSelectGoodFeatures(
KLT_TrackingContext tc,
KLT_PixelType *img,
int ncols,
int nrows,
KLT_FeatureList featurelist,
selectionMode mode)
{
// added timer for this function
//Timer selectTimer;
_KLT_FloatImage floatimg, gradx, grady;
int window_hw, window_hh;
int *pointlist;
int npoints = 0;
KLT_BOOL overwriteAllFeatures = (mode == SELECTING_ALL) ?
TRUE : FALSE;
KLT_BOOL floatimages_created = FALSE;
// Initialize times
//initTimer(&selectTimer, "select Feature Time");
// Start timer
//startTimer(&selectTimer);
/* Check window size (and correct if necessary) */
if (tc->window_width % 2 != 1) {
tc->window_width = tc->window_width+1;
KLTWarning("Tracking context's window width must be odd. "
"Changing to %d.\n", tc->window_width);
}
if (tc->window_height % 2 != 1) {
tc->window_height = tc->window_height+1;
KLTWarning("Tracking context's window height must be odd. "
"Changing to %d.\n", tc->window_height);
}
if (tc->window_width < 3) {
tc->window_width = 3;
KLTWarning("Tracking context's window width must be at least three. \n"
"Changing to %d.\n", tc->window_width);
}
if (tc->window_height < 3) {
tc->window_height = 3;
KLTWarning("Tracking context's window height must be at least three. \n"
"Changing to %d.\n", tc->window_height);
}
window_hw = tc->window_width/2;
window_hh = tc->window_height/2;
// After windows
//stopTimer(&selectTimer);
//printf("Time for checking the window size = %f\n", getTime(selectTimer));
// Restart for point list
//restartTimer(&selectTimer);
/* Create pointlist, which is a simplified version of a featurelist, */
/* for speed. Contains only integer locations and values. */
pointlist = (int *) malloc(ncols * nrows * 3 * sizeof(int));
// stop and print
//stopTimer(&selectTimer);
//printf("Time for initializing pointlist = %f\n", getTime(selectTimer));
// Restart for point list
//restartTimer(&selectTimer);
/* Create temporary images, etc. */
if (mode == REPLACING_SOME &&
tc->sequentialMode && tc->pyramid_last != NULL) {
floatimg = ((_KLT_Pyramid) tc->pyramid_last)->img[0];
gradx = ((_KLT_Pyramid) tc->pyramid_last_gradx)->img[0];
grady = ((_KLT_Pyramid) tc->pyramid_last_grady)->img[0];
assert(gradx != NULL);
assert(grady != NULL);
} else {
floatimages_created = TRUE;
floatimg = _KLTCreateFloatImage(ncols, nrows);
gradx = _KLTCreateFloatImage(ncols, nrows);
grady = _KLTCreateFloatImage(ncols, nrows);
if (tc->smoothBeforeSelecting) {
_KLT_FloatImage tmpimg;
tmpimg = _KLTCreateFloatImage(ncols, nrows);
_KLTToFloatImage(img, ncols, nrows, tmpimg);
_KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg);
_KLTFreeFloatImage(tmpimg);
} else _KLTToFloatImage(img, ncols, nrows, floatimg);
/* Compute gradient of image in x and y direction */
_KLTComputeGradients(floatimg, tc->grad_sigma, gradx, grady);
}
// stop and print
//stopTimer(&selectTimer);
//printf("Time for creating temporary mages = %f\n", getTime(selectTimer));
// Restart for point list
//restartTimer(&selectTimer);
/* Write internal images */
if (tc->writeInternalImages) {
_KLTWriteFloatImageToPGM(floatimg, "kltimg_sgfrlf.pgm");
_KLTWriteFloatImageToPGM(gradx, "kltimg_sgfrlf_gx.pgm");
_KLTWriteFloatImageToPGM(grady, "kltimg_sgfrlf_gy.pgm");
}
// stop and print
//stopTimer(&selectTimer);
//printf("Time for writing internal images = %f\n", getTime(selectTimer));
/* Compute trackability of each image pixel as the minimum
of the two eigenvalues of the Z matrix */
{register int *ptr;
/*
register float gx, gy;
register float gxx, gxy, gyy;
register int xx, yy;
float val;
unsigned int limit = 1;
int borderx = tc->borderx; // Must not touch cols
int bordery = tc->bordery; // lost by convolution
int x, y;
int i;
*/
/*
if (borderx < window_hw) borderx = window_hw;
if (bordery < window_hh) bordery = window_hh;
// Find largest value of an int
for (i = 0 ; i < sizeof(int) ; i++) limit *= 256;
limit = limit/2 - 1;
*/
/* For most of the pixels in the image, do ... */
ptr = pointlist;
point_gradients(ptr, tc, &npoints, nrows, ncols, gradx, grady);
/*
for (y = bordery ; y < nrows - bordery ; y += tc->nSkippedPixels + 1)
for (x = borderx ; x < ncols - borderx ; x += tc->nSkippedPixels + 1) {
// Sum the gradients in the surrounding window
gxx = 0; gxy = 0; gyy = 0;
for (yy = y-window_hh ; yy <= y+window_hh ; yy++)
for (xx = x-window_hw ; xx <= x+window_hw ; xx++) {
gx = *(gradx->data + ncols*yy+xx);
gy = *(grady->data + ncols*yy+xx);
gxx += gx * gx;
gxy += gx * gy;
gyy += gy * gy;
}
// Store the trackability of the pixel as the minimum
// of the two eigenvalues
*ptr++ = x;
*ptr++ = y;
val = _minEigenvalue(gxx, gxy, gyy);
if (val > limit) {
KLTWarning("(_KLTSelectGoodFeatures) minimum eigenvalue %f is "
"greater than the capacity of an int; setting "
"to maximum value", val);
val = (float) limit;
}
*ptr++ = (int) val;
npoints++;
}
*/
}
// Restart for point list
//restartTimer(&selectTimer);
/* Sort the features */
_sortPointList(pointlist, npoints);
// printing this loop
//stopTimer(&selectTimer);
//printf("Time for sorting the pointlist = %f\n", getTime(selectTimer));
// Restart for point list
//restartTimer(&selectTimer);
/* Check tc->mindist */
if (tc->mindist < 0) {
KLTWarning("(_KLTSelectGoodFeatures) Tracking context field tc->mindist "
"is negative (%d); setting to zero", tc->mindist);
tc->mindist = 0;
}
// printing this loop
//stopTimer(&selectTimer);
//printf("Time for checking midlist = %f\n", getTime(selectTimer));
// Restart for point list
//restartTimer(&selectTimer);
/* Enforce minimum distance between features */
_enforceMinimumDistance(
pointlist,
npoints,
featurelist,
ncols, nrows,
tc->mindist,
tc->min_eigenvalue,
overwriteAllFeatures);
// printing this loop
//stopTimer(&selectTimer);
//printf("Time for enforcing minimum distance = %f\n", getTime(selectTimer));
// Restart for point list
//restartTimer(&selectTimer);
/* Free memory */
free(pointlist);
if (floatimages_created) {
_KLTFreeFloatImage(floatimg);
_KLTFreeFloatImage(gradx);
_KLTFreeFloatImage(grady);
}
// printing this loop
//stopTimer(&selectTimer);
//printf("Time for freeing stuff = %f\n", getTime(selectTimer));
}
static int _am_trackFeatureAffine(
float x1, /* location of window in first image */
float y1,
float *x2, /* starting location of search in second image */
float *y2,
_KLT_FloatImage img1,
_KLT_FloatImage gradx1,
_KLT_FloatImage grady1,
_KLT_FloatImage img2,
_KLT_FloatImage gradx2,
_KLT_FloatImage grady2,
int width, /* size of window */
int height,
float step_factor, /* 2.0 comes from equations, 1.0 seems to avoid overshooting */
int max_iterations,
float small, /* determinant threshold for declaring KLT_SMALL_DET */
float th, /* displacement threshold for stopping */
float th_aff,
float max_residue, /* residue threshold for declaring KLT_LARGE_RESIDUE */
int lighting_insensitive, /* whether to normalize for gain and bias */
int affine_map, /* whether to evaluates the consistency of features with affine mapping */
float mdd, /* difference between the displacements */
float *Axx, float *Ayx,
float *Axy, float *Ayy) /* used affine mapping */
{
_FloatWindow imgdiff, gradx, grady;
float gxx, gxy, gyy, ex, ey, dx, dy;
int iteration = 0;
int status = 0;
int hw = width/2;
int hh = height/2;
int nc1 = img1->ncols;
int nr1 = img1->nrows;
int nc2 = img2->ncols;
int nr2 = img2->nrows;
float **a;
float **T;
float one_plus_eps = 1.001f; /* To prevent rounding errors */
float old_x2 = *x2;
float old_y2 = *y2;
KLT_BOOL convergence = FALSE;
#ifdef DEBUG_AFFINE_MAPPING
char fname[80];
_KLT_FloatImage aff_diff_win = _KLTCreateFloatImage(width,height);
printf("starting location x2=%f y2=%f\n", *x2, *y2);
#endif
/* Allocate memory for windows */
imgdiff = _allocateFloatWindow(width, height);
gradx = _allocateFloatWindow(width, height);
grady = _allocateFloatWindow(width, height);
T = _am_matrix(6,6);
a = _am_matrix(6,1);
/* Iteratively update the window position */
do {
if(!affine_map) {
/* pure translation tracker */
/* If out of bounds, exit loop */
if ( x1-hw < 0.0f || nc1-( x1+hw) < one_plus_eps ||
*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps ||
y1-hh < 0.0f || nr1-( y1+hh) < one_plus_eps ||
*y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps) {
status = KLT_OOB;
break;
}
/* Compute gradient and difference windows */
if (lighting_insensitive) {
_computeIntensityDifferenceLightingInsensitive(img1, img2, x1, y1, *x2, *y2,
width, height, imgdiff);
_computeGradientSumLightingInsensitive(gradx1, grady1, gradx2, grady2,
img1, img2, x1, y1, *x2, *y2, width, height, gradx, grady);
} else {
_computeIntensityDifference(img1, img2, x1, y1, *x2, *y2,
width, height, imgdiff);
_computeGradientSum(gradx1, grady1, gradx2, grady2,
x1, y1, *x2, *y2, width, height, gradx, grady);
}
#ifdef DEBUG_AFFINE_MAPPING
aff_diff_win->data = imgdiff;
sprintf(fname, "./debug/kltimg_trans_diff_win%03d.%03d.pgm", glob_index, counter);
printf("%s\n", fname);
_KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0);
printf("iter = %d translation tracker res: %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height));
#endif
/* Use these windows to construct matrices */
_compute2by2GradientMatrix(gradx, grady, width, height,
&gxx, &gxy, &gyy);
_compute2by1ErrorVector(imgdiff, gradx, grady, width, height, step_factor,
&ex, &ey);
/* Using matrices, solve equation for new displacement */
status = _solveEquation(gxx, gxy, gyy, ex, ey, small, &dx, &dy);
convergence = (fabs(dx) < th && fabs(dy) < th);
*x2 += dx;
*y2 += dy;
}else{
/* affine tracker */
float ul_x = *Axx * (-hw) + *Axy * hh + *x2; /* upper left corner */
float ul_y = *Ayx * (-hw) + *Ayy * hh + *y2;
float ll_x = *Axx * (-hw) + *Axy * (-hh) + *x2; /* lower left corner */
float ll_y = *Ayx * (-hw) + *Ayy * (-hh) + *y2;
float ur_x = *Axx * hw + *Axy * hh + *x2; /* upper right corner */
float ur_y = *Ayx * hw + *Ayy * hh + *y2;
float lr_x = *Axx * hw + *Axy * (-hh) + *x2; /* lower right corner */
float lr_y = *Ayx * hw + *Ayy * (-hh) + *y2;
/* If out of bounds, exit loop */
if ( x1-hw < 0.0f || nc1-(x1+hw) < one_plus_eps ||
y1-hh < 0.0f || nr1-(y1+hh) < one_plus_eps ||
ul_x < 0.0f || nc2-(ul_x ) < one_plus_eps ||
ll_x < 0.0f || nc2-(ll_x ) < one_plus_eps ||
ur_x < 0.0f || nc2-(ur_x ) < one_plus_eps ||
lr_x < 0.0f || nc2-(lr_x ) < one_plus_eps ||
ul_y < 0.0f || nr2-(ul_y ) < one_plus_eps ||
ll_y < 0.0f || nr2-(ll_y ) < one_plus_eps ||
ur_y < 0.0f || nr2-(ur_y ) < one_plus_eps ||
lr_y < 0.0f || nr2-(lr_y ) < one_plus_eps) {
status = KLT_OOB;
break;
}
#ifdef DEBUG_AFFINE_MAPPING
counter++;
_am_computeAffineMappedImage(img1, x1, y1, 1.0, 0.0 , 0.0, 1.0, width, height, imgdiff);
aff_diff_win->data = imgdiff;
sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_1.pgm", glob_index, counter);
printf("%s\n", fname);
_KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0);
_am_computeAffineMappedImage(img2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy, width, height, imgdiff);
aff_diff_win->data = imgdiff;
sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_2.pgm", glob_index, counter);
printf("%s\n", fname);
_KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0);
#endif
_am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy,
width, height, imgdiff);
#ifdef DEBUG_AFFINE_MAPPING
aff_diff_win->data = imgdiff;
sprintf(fname, "./debug/kltimg_aff_diff_win%03d.%03d_3.pgm", glob_index,counter);
printf("%s\n", fname);
_KLTWriteAbsFloatImageToPGM(aff_diff_win, fname,256.0);
printf("iter = %d affine tracker res: %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height));
#endif
_am_getGradientWinAffine(gradx2, grady2, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy,
width, height, gradx, grady);
switch(affine_map){
case 1:
_am_compute4by1ErrorVector(imgdiff, gradx, grady, width, height, a);
_am_compute4by4GradientMatrix(gradx, grady, width, height, T);
status = _am_gauss_jordan_elimination(T,4,a,1);
*Axx += a[0][0];
*Ayx += a[1][0];
*Ayy = *Axx;
*Axy = -(*Ayx);
dx = a[2][0];
dy = a[3][0];
break;
case 2:
_am_compute6by1ErrorVector(imgdiff, gradx, grady, width, height, a);
_am_compute6by6GradientMatrix(gradx, grady, width, height, T);
status = _am_gauss_jordan_elimination(T,6,a,1);
*Axx += a[0][0];
*Ayx += a[1][0];
*Axy += a[2][0];
*Ayy += a[3][0];
dx = a[4][0];
dy = a[5][0];
break;
}
*x2 += dx;
*y2 += dy;
/* old upper left corner - new upper left corner */
ul_x -= *Axx * (-hw) + *Axy * hh + *x2;
ul_y -= *Ayx * (-hw) + *Ayy * hh + *y2;
/* old lower left corner - new lower left corner */
ll_x -= *Axx * (-hw) + *Axy * (-hh) + *x2;
ll_y -= *Ayx * (-hw) + *Ayy * (-hh) + *y2;
/* old upper right corner - new upper right corner */
ur_x -= *Axx * hw + *Axy * hh + *x2;
ur_y -= *Ayx * hw + *Ayy * hh + *y2;
/* old lower right corner - new lower right corner */
lr_x -= *Axx * hw + *Axy * (-hh) + *x2;
lr_y -= *Ayx * hw + *Ayy * (-hh) + *y2;
#ifdef DEBUG_AFFINE_MAPPING
printf ("iter = %d, ul_x=%f ul_y=%f ll_x=%f ll_y=%f ur_x=%f ur_y=%f lr_x=%f lr_y=%f \n",
iteration, ul_x, ul_y, ll_x, ll_y, ur_x, ur_y, lr_x, lr_y);
#endif
convergence = (fabs(dx) < th && fabs(dy) < th &&
fabs(ul_x) < th_aff && fabs(ul_y) < th_aff &&
fabs(ll_x) < th_aff && fabs(ll_y) < th_aff &&
fabs(ur_x) < th_aff && fabs(ur_y) < th_aff &&
fabs(lr_x) < th_aff && fabs(lr_y) < th_aff);
}
if (status == KLT_SMALL_DET) break;
iteration++;
#ifdef DEBUG_AFFINE_MAPPING
printf ("iter = %d, x1=%f, y1=%f, x2=%f, y2=%f, Axx=%f, Ayx=%f , Axy=%f, Ayy=%f \n",iteration, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy);
#endif
} while ( !convergence && iteration < max_iterations);
/*} while ( (fabs(dx)>=th || fabs(dy)>=th || (affine_map && iteration < 8) ) && iteration < max_iterations); */
_am_free_matrix(T);
_am_free_matrix(a);
/* Check whether window is out of bounds */
if (*x2-hw < 0.0f || nc2-(*x2+hw) < one_plus_eps ||
*y2-hh < 0.0f || nr2-(*y2+hh) < one_plus_eps)
status = KLT_OOB;
/* Check whether feature point has moved to much during iteration*/
if ( (*x2-old_x2) > mdd || (*y2-old_y2) > mdd )
status = KLT_OOB;
/* Check whether residue is too large */
if (status == KLT_TRACKED) {
if(!affine_map){
_computeIntensityDifference(img1, img2, x1, y1, *x2, *y2,
width, height, imgdiff);
}else{
_am_computeIntensityDifferenceAffine(img1, img2, x1, y1, *x2, *y2, *Axx, *Ayx , *Axy, *Ayy,
width, height, imgdiff);
}
#ifdef DEBUG_AFFINE_MAPPING
printf("iter = %d final_res = %f\n", iteration, _sumAbsFloatWindow(imgdiff, width, height)/(width*height));
#endif
if (_sumAbsFloatWindow(imgdiff, width, height)/(width*height) > max_residue)
status = KLT_LARGE_RESIDUE;
}
/* Free memory */
free(imgdiff); free(gradx); free(grady);
#ifdef DEBUG_AFFINE_MAPPING
printf("iter = %d status=%d\n", iteration, status);
_KLTFreeFloatImage( aff_diff_win );
#endif
/* Return appropriate value */
return status;
}
void KLTTrackFeatures(
KLT_TrackingContext tc,
KLT_PixelType *img1,
KLT_PixelType *img2,
int ncols,
int nrows,
KLT_FeatureList featurelist)
{
_KLT_FloatImage tmpimg, floatimg1, floatimg2;
_KLT_Pyramid pyramid1, pyramid1_gradx, pyramid1_grady,
pyramid2, pyramid2_gradx, pyramid2_grady;
float subsampling = (float) tc->subsampling;
float xloc, yloc, xlocout, ylocout;
int val;
int indx, r;
KLT_BOOL floatimg1_created = FALSE;
int i;
if (KLT_verbose >= 1) {
fprintf(stderr, "(KLT) Tracking %d features in a %d by %d image... ",
KLTCountRemainingFeatures(featurelist), ncols, nrows);
fflush(stderr);
}
/* Check window size (and correct if necessary) */
if (tc->window_width % 2 != 1) {
tc->window_width = tc->window_width+1;
KLTWarning("Tracking context's window width must be odd. "
"Changing to %d.\n", tc->window_width);
}
if (tc->window_height % 2 != 1) {
tc->window_height = tc->window_height+1;
KLTWarning("Tracking context's window height must be odd. "
"Changing to %d.\n", tc->window_height);
}
if (tc->window_width < 3) {
tc->window_width = 3;
KLTWarning("Tracking context's window width must be at least three. \n"
"Changing to %d.\n", tc->window_width);
}
if (tc->window_height < 3) {
tc->window_height = 3;
KLTWarning("Tracking context's window height must be at least three. \n"
"Changing to %d.\n", tc->window_height);
}
/* Create temporary image */
tmpimg = _KLTCreateFloatImage(ncols, nrows);
/* Process first image by converting to float, smoothing, computing */
/* pyramid, and computing gradient pyramids */
if (tc->sequentialMode && tc->pyramid_last != NULL) {
pyramid1 = (_KLT_Pyramid) tc->pyramid_last;
pyramid1_gradx = (_KLT_Pyramid) tc->pyramid_last_gradx;
pyramid1_grady = (_KLT_Pyramid) tc->pyramid_last_grady;
if (pyramid1->ncols[0] != ncols || pyramid1->nrows[0] != nrows)
KLTError("(KLTTrackFeatures) Size of incoming image (%d by %d) "
"is different from size of previous image (%d by %d)\n",
ncols, nrows, pyramid1->ncols[0], pyramid1->nrows[0]);
assert(pyramid1_gradx != NULL);
assert(pyramid1_grady != NULL);
} else {
floatimg1_created = TRUE;
floatimg1 = _KLTCreateFloatImage(ncols, nrows);
_KLTToFloatImage(img1, ncols, nrows, tmpimg);
_KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg1);
pyramid1 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
_KLTComputePyramid(floatimg1, pyramid1, tc->pyramid_sigma_fact);
pyramid1_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
pyramid1_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
for (i = 0 ; i < tc->nPyramidLevels ; i++)
_KLTComputeGradients(pyramid1->img[i], tc->grad_sigma,
pyramid1_gradx->img[i],
pyramid1_grady->img[i]);
}
/* Do the same thing with second image */
floatimg2 = _KLTCreateFloatImage(ncols, nrows);
_KLTToFloatImage(img2, ncols, nrows, tmpimg);
_KLTComputeSmoothedImage(tmpimg, _KLTComputeSmoothSigma(tc), floatimg2);
pyramid2 = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
_KLTComputePyramid(floatimg2, pyramid2, tc->pyramid_sigma_fact);
pyramid2_gradx = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
pyramid2_grady = _KLTCreatePyramid(ncols, nrows, (int) subsampling, tc->nPyramidLevels);
for (i = 0 ; i < tc->nPyramidLevels ; i++)
_KLTComputeGradients(pyramid2->img[i], tc->grad_sigma,
pyramid2_gradx->img[i],
pyramid2_grady->img[i]);
/* Write internal images */
if (tc->writeInternalImages) {
char fname[80];
for (i = 0 ; i < tc->nPyramidLevels ; i++) {
sprintf(fname, "kltimg_tf_i%d.pgm", i);
_KLTWriteFloatImageToPGM(pyramid1->img[i], fname);
sprintf(fname, "kltimg_tf_i%d_gx.pgm", i);
_KLTWriteFloatImageToPGM(pyramid1_gradx->img[i], fname);
sprintf(fname, "kltimg_tf_i%d_gy.pgm", i);
_KLTWriteFloatImageToPGM(pyramid1_grady->img[i], fname);
sprintf(fname, "kltimg_tf_j%d.pgm", i);
_KLTWriteFloatImageToPGM(pyramid2->img[i], fname);
sprintf(fname, "kltimg_tf_j%d_gx.pgm", i);
_KLTWriteFloatImageToPGM(pyramid2_gradx->img[i], fname);
sprintf(fname, "kltimg_tf_j%d_gy.pgm", i);
_KLTWriteFloatImageToPGM(pyramid2_grady->img[i], fname);
}
}
/* For each feature, do ... */
for (indx = 0 ; indx < featurelist->nFeatures ; indx++) {
/* Only track features that are not lost */
if (featurelist->feature[indx]->val >= 0) {
xloc = featurelist->feature[indx]->x;
yloc = featurelist->feature[indx]->y;
/* Transform location to coarsest resolution */
for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) {
xloc /= subsampling; yloc /= subsampling;
}
xlocout = xloc; ylocout = yloc;
/* Beginning with coarsest resolution, do ... */
for (r = tc->nPyramidLevels - 1 ; r >= 0 ; r--) {
/* Track feature at current resolution */
xloc *= subsampling; yloc *= subsampling;
xlocout *= subsampling; ylocout *= subsampling;
val = _trackFeature(xloc, yloc,
&xlocout, &ylocout,
pyramid1->img[r],
pyramid1_gradx->img[r], pyramid1_grady->img[r],
pyramid2->img[r],
pyramid2_gradx->img[r], pyramid2_grady->img[r],
tc->window_width, tc->window_height,
tc->step_factor,
tc->max_iterations,
tc->min_determinant,
tc->min_displacement,
tc->max_residue,
tc->lighting_insensitive);
if (val==KLT_SMALL_DET || val==KLT_OOB)
break;
}
/* Record feature */
if (val == KLT_OOB) {
featurelist->feature[indx]->x = -1.0;
featurelist->feature[indx]->y = -1.0;
featurelist->feature[indx]->val = KLT_OOB;
if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img);
if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx);
if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady);
featurelist->feature[indx]->aff_img = NULL;
featurelist->feature[indx]->aff_img_gradx = NULL;
featurelist->feature[indx]->aff_img_grady = NULL;
} else if (_outOfBounds(xlocout, ylocout, ncols, nrows, tc->borderx, tc->bordery)) {
featurelist->feature[indx]->x = -1.0;
featurelist->feature[indx]->y = -1.0;
featurelist->feature[indx]->val = KLT_OOB;
if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img);
if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx);
if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady);
featurelist->feature[indx]->aff_img = NULL;
featurelist->feature[indx]->aff_img_gradx = NULL;
featurelist->feature[indx]->aff_img_grady = NULL;
} else if (val == KLT_SMALL_DET) {
featurelist->feature[indx]->x = -1.0;
featurelist->feature[indx]->y = -1.0;
featurelist->feature[indx]->val = KLT_SMALL_DET;
if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img);
if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx);
if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady);
featurelist->feature[indx]->aff_img = NULL;
featurelist->feature[indx]->aff_img_gradx = NULL;
featurelist->feature[indx]->aff_img_grady = NULL;
} else if (val == KLT_LARGE_RESIDUE) {
featurelist->feature[indx]->x = -1.0;
featurelist->feature[indx]->y = -1.0;
featurelist->feature[indx]->val = KLT_LARGE_RESIDUE;
if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img);
if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx);
if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady);
featurelist->feature[indx]->aff_img = NULL;
featurelist->feature[indx]->aff_img_gradx = NULL;
featurelist->feature[indx]->aff_img_grady = NULL;
} else if (val == KLT_MAX_ITERATIONS) {
featurelist->feature[indx]->x = -1.0;
featurelist->feature[indx]->y = -1.0;
featurelist->feature[indx]->val = KLT_MAX_ITERATIONS;
if( featurelist->feature[indx]->aff_img ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img);
if( featurelist->feature[indx]->aff_img_gradx ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx);
if( featurelist->feature[indx]->aff_img_grady ) _KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady);
featurelist->feature[indx]->aff_img = NULL;
featurelist->feature[indx]->aff_img_gradx = NULL;
featurelist->feature[indx]->aff_img_grady = NULL;
} else {
featurelist->feature[indx]->x = xlocout;
featurelist->feature[indx]->y = ylocout;
featurelist->feature[indx]->val = KLT_TRACKED;
if (tc->affineConsistencyCheck >= 0 && val == KLT_TRACKED) { /*for affine mapping*/
int border = 2; /* add border for interpolation */
#ifdef DEBUG_AFFINE_MAPPING
glob_index = indx;
#endif
if(!featurelist->feature[indx]->aff_img){
/* save image and gradient for each feature at finest resolution after first successful track */
featurelist->feature[indx]->aff_img = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border));
featurelist->feature[indx]->aff_img_gradx = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border));
featurelist->feature[indx]->aff_img_grady = _KLTCreateFloatImage((tc->affine_window_width+border), (tc->affine_window_height+border));
_am_getSubFloatImage(pyramid1->img[0],xloc,yloc,featurelist->feature[indx]->aff_img);
_am_getSubFloatImage(pyramid1_gradx->img[0],xloc,yloc,featurelist->feature[indx]->aff_img_gradx);
_am_getSubFloatImage(pyramid1_grady->img[0],xloc,yloc,featurelist->feature[indx]->aff_img_grady);
featurelist->feature[indx]->aff_x = xloc - (int) xloc + (tc->affine_window_width+border)/2;
featurelist->feature[indx]->aff_y = yloc - (int) yloc + (tc->affine_window_height+border)/2;;
}else{
/* affine tracking */
val = _am_trackFeatureAffine(featurelist->feature[indx]->aff_x, featurelist->feature[indx]->aff_y,
&xlocout, &ylocout,
featurelist->feature[indx]->aff_img,
featurelist->feature[indx]->aff_img_gradx,
featurelist->feature[indx]->aff_img_grady,
pyramid2->img[0],
pyramid2_gradx->img[0], pyramid2_grady->img[0],
tc->affine_window_width, tc->affine_window_height,
tc->step_factor,
tc->affine_max_iterations,
tc->min_determinant,
tc->min_displacement,
tc->affine_min_displacement,
tc->affine_max_residue,
tc->lighting_insensitive,
tc->affineConsistencyCheck,
tc->affine_max_displacement_differ,
&featurelist->feature[indx]->aff_Axx,
&featurelist->feature[indx]->aff_Ayx,
&featurelist->feature[indx]->aff_Axy,
&featurelist->feature[indx]->aff_Ayy
);
featurelist->feature[indx]->val = val;
if(val != KLT_TRACKED){
featurelist->feature[indx]->x = -1.0;
featurelist->feature[indx]->y = -1.0;
featurelist->feature[indx]->aff_x = -1.0;
featurelist->feature[indx]->aff_y = -1.0;
/* free image and gradient for lost feature */
_KLTFreeFloatImage(featurelist->feature[indx]->aff_img);
_KLTFreeFloatImage(featurelist->feature[indx]->aff_img_gradx);
_KLTFreeFloatImage(featurelist->feature[indx]->aff_img_grady);
featurelist->feature[indx]->aff_img = NULL;
featurelist->feature[indx]->aff_img_gradx = NULL;
featurelist->feature[indx]->aff_img_grady = NULL;
}else{
/*featurelist->feature[indx]->x = xlocout;*/
/*featurelist->feature[indx]->y = ylocout;*/
}
}
}
}
}
}
if (tc->sequentialMode) {
tc->pyramid_last = pyramid2;
tc->pyramid_last_gradx = pyramid2_gradx;
tc->pyramid_last_grady = pyramid2_grady;
} else {
_KLTFreePyramid(pyramid2);
_KLTFreePyramid(pyramid2_gradx);
_KLTFreePyramid(pyramid2_grady);
}
/* Free memory */
_KLTFreeFloatImage(tmpimg);
if (floatimg1_created) _KLTFreeFloatImage(floatimg1);
_KLTFreeFloatImage(floatimg2);
_KLTFreePyramid(pyramid1);
_KLTFreePyramid(pyramid1_gradx);
_KLTFreePyramid(pyramid1_grady);
if (KLT_verbose >= 1) {
fprintf(stderr, "\n\t%d features successfully tracked.\n",
KLTCountRemainingFeatures(featurelist));
if (tc->writeInternalImages)
fprintf(stderr, "\tWrote images to 'kltimg_tf*.pgm'.\n");
fflush(stderr);
}
}
