void KLTStopSequentialMode(
KLT_TrackingContext tc)
{
tc->sequentialMode = FALSE;
_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last);
_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_gradx);
_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_grady);
tc->pyramid_last = NULL;
tc->pyramid_last_gradx = NULL;
tc->pyramid_last_grady = NULL;
}
void KLTFreeTrackingContext(
KLT_TrackingContext tc)
{
if (tc->pyramid_last)
_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last);
if (tc->pyramid_last_gradx)
_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_gradx);
if (tc->pyramid_last_grady)
_KLTFreePyramid((_KLT_Pyramid) tc->pyramid_last_grady);
//free(tc);
delete tc;
}
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);
}
}
