void KLTReplaceLostFeatures( KLT_TrackingContext tc, KLT_PixelType *img, int ncols, int nrows, KLT_FeatureList fl) { int nLostFeatures = fl->nFeatures - KLTCountRemainingFeatures(fl); if (KLT_verbose >= 1) { fprintf(stderr, "(KLT) Attempting to replace %d features " "in a %d by %d image... ", nLostFeatures, ncols, nrows); fflush(stderr); } /* If there are any lost features, replace them */ if (nLostFeatures > 0) _KLTSelectGoodFeatures(tc, img, ncols, nrows, fl, REPLACING_SOME); if (KLT_verbose >= 1) { fprintf(stderr, "\n\t%d features replaced.\n", nLostFeatures - fl->nFeatures + KLTCountRemainingFeatures(fl)); if (tc->writeInternalImages) fprintf(stderr, "\tWrote images to 'kltimg_sgfrlf*.pgm'.\n"); fflush(stderr); } }
void KLTWriteFeatureListToPPM( KLT_FeatureList featurelist, KLT_PixelType *greyimg, int ncols, int nrows, char *filename) { int nbytes = ncols * nrows * sizeof(char); uchar *redimg, *grnimg, *bluimg; int offset; int x, y, xx, yy; int i; if (KLT_verbose >= 1) fprintf(stderr, "(KLT) Writing %d features to PPM file: '%s'\n", KLTCountRemainingFeatures(featurelist), filename); /* Allocate memory for component images */ redimg = (uchar *) malloc(nbytes); grnimg = (uchar *) malloc(nbytes); bluimg = (uchar *) malloc(nbytes); if (redimg == NULL || grnimg == NULL || bluimg == NULL) KLTError("(KLTWriteFeaturesToPPM) Out of memory\n"); /* Copy grey image to component images */ if (sizeof(KLT_PixelType) != 1) KLTWarning("(KLTWriteFeaturesToPPM) KLT_PixelType is not uchar"); memcpy(redimg, greyimg, nbytes); memcpy(grnimg, greyimg, nbytes); memcpy(bluimg, greyimg, nbytes); /* Overlay features in red */ for (i = 0 ; i < featurelist->nFeatures ; i++) if (featurelist->feature[i]->val >= 0) { x = (int) (featurelist->feature[i]->x + 0.5); y = (int) (featurelist->feature[i]->y + 0.5); for (yy = y - 1 ; yy <= y + 1 ; yy++) for (xx = x - 1 ; xx <= x + 1 ; xx++) if (xx >= 0 && yy >= 0 && xx < ncols && yy < nrows) { offset = yy * ncols + xx; *(redimg + offset) = 255; *(grnimg + offset) = 0; *(bluimg + offset) = 0; } } /* Write to PPM file */ ppmWriteFileRGB(filename, redimg, grnimg, bluimg, ncols, nrows); /* Free memory */ free(redimg); free(grnimg); free(bluimg); }
void KLTSelectGoodFeatures( KLT_TrackingContext tc, KLT_PixelType *img, int ncols, int nrows, KLT_FeatureList fl) { if (KLT_verbose >= 1) { fprintf(stderr, "(KLT) Selecting the %d best features " "from a %d by %d image... ", fl->nFeatures, ncols, nrows); fflush(stderr); } _KLTSelectGoodFeatures(tc, img, ncols, nrows, fl, SELECTING_ALL); if (KLT_verbose >= 1) { fprintf(stderr, "\n\t%d features found.\n", KLTCountRemainingFeatures(fl)); if (tc->writeInternalImages) fprintf(stderr, "\tWrote images to 'kltimg_sgfrlf*.pgm'.\n"); fflush(stderr); } }
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); } }