static FILE* _printSetupTxt(
char *fname, /* Input: filename, or NULL for stderr */
char *fmt, /* Input: format (e.g., %5.1f or %3d) */
char *format, /* Output: format (e.g., (%5.1f,%5.1f)=%3d) */
char *type) /* Output: either 'f' or 'd', based on input format */
{
FILE *fp;
const int val_width = 5;
int i;
/* Either open file or use stderr */
if (fname == NULL) fp = stderr;
else fp = fopen(fname, "wb");
if (fp == NULL)
KLTError("(KLTWriteFeatures) "
"Can't open file '%s' for writing\n", fname);
/* Parse format */
if (fmt[0] != '%')
KLTError("(KLTWriteFeatures) Bad Format: %s\n", fmt);
i = 0; while (fmt[i] != '\0') i++; *type = fmt[i-1];
if (*type != 'f' && *type != 'd')
KLTError("(KLTWriteFeatures) Format must end in 'f' or 'd'.");
/* Construct feature format */
sprintf(format, "(%s,%s)=%%%dd ", fmt, fmt, val_width);
return fp;
}
void pnmReadHeader(
FILE *fp,
int *magic,
int *ncols, int *nrows,
int *maxval)
{
char line[LENGTH];
/* Read magic number */
_getNextString(fp, line);
if (line[0] != 'P')
KLTError("(pnmReadHeader) Magic number does not begin with 'P', "
"but with a '%c'", line[0]);
sscanf(line, "P%d", magic);
/* Read size, skipping comments */
_getNextString(fp, line);
*ncols = atoi(line);
_getNextString(fp, line);
*nrows = atoi(line);
if (*ncols < 0 || *nrows < 0 || *ncols > 10000 || *nrows > 10000)
KLTError("(pnmReadHeader) The dimensions %d x %d are unacceptable",
*ncols, *nrows);
/* Read maxval, skipping comments */
_getNextString(fp, line);
*maxval = atoi(line);
fread(line, 1, 1, fp); /* Read newline which follows maxval */
if (*maxval != 255)
KLTWarning("(pnmReadHeader) Maxval is not 255, but %d", *maxval);
}
KLT_FeatureTable KLTReadFeatureTable(
KLT_FeatureTable ft_in,
char *fname)
{
FILE *fp;
KLT_FeatureTable ft;
int nFrames;
int nFeatures;
structureType id;
int indx;
KLT_BOOL binary; /* whether file is binary or text */
int i, j;
fp = fopen(fname, "rb");
if (fp == NULL) KLTError("(KLTReadFeatureTable) Can't open file '%s' "
"for reading", fname);
if (KLT_verbose >= 1) fprintf(stderr, "(KLT) Reading feature table from '%s'\n", fname);
id = _readHeader(fp, &nFrames, &nFeatures, &binary);
if (id != FEATURE_TABLE) KLTError("(KLTReadFeatureTable) File '%s' does not contain "
"a FeatureTable", fname);
if (ft_in == NULL) {
ft = KLTCreateFeatureTable(nFrames, nFeatures);
ft->nFrames = nFrames;
ft->nFeatures = nFeatures;
}
else {
ft = ft_in;
if (ft->nFrames != nFrames || ft->nFeatures != nFeatures)
KLTError("(KLTReadFeatureTable) The feature table passed "
"does not contain the same number of frames and "
"features as the feature table in file '%s' ", fname);
}
if (!binary) { /* text file */
for (j = 0 ; j < ft->nFeatures ; j++) {
fscanf(fp, "%d |", &indx);
if (indx != j)
KLTError("(KLTReadFeatureTable) Bad index at j = %d"
"-- %d", j, indx);
for (i = 0 ; i < ft->nFrames ; i++)
_readFeatureTxt(fp, ft->feature[j][i]);
}
} else { /* binary file */
for (j = 0 ; j < ft->nFeatures ; j++) {
for (i = 0 ; i < ft->nFrames ; i++)
_readFeatureBin(fp, ft->feature[j][i]);
}
}
fclose(fp);
return ft;
}
static FILE* _printSetupBin(
char *fname) /* Input: filename */
{
FILE *fp;
if (fname == NULL)
KLTError("(KLTWriteFeatures) Can't write binary data to stderr");
fp = fopen(fname, "wb");
if (fp == NULL)
KLTError("(KLTWriteFeatures) "
"Can't open file '%s' for writing", fname);
return fp;
}
KLT_FeatureList KLTReadFeatureList(
KLT_FeatureList fl_in,
char *fname)
{
FILE *fp;
KLT_FeatureList fl;
int nFeatures;
structureType id;
int indx;
KLT_BOOL binary; /* whether file is binary or text */
int i;
fp = fopen(fname, "rb");
if (fp == NULL) KLTError("(KLTReadFeatureList) Can't open file '%s' "
"for reading", fname);
if (KLT_verbose >= 1)
fprintf(stderr, "(KLT) Reading feature list from '%s'\n", fname);
id = _readHeader(fp, NULL, &nFeatures, &binary);
if (id != FEATURE_LIST)
KLTError("(KLTReadFeatureList) File '%s' does not contain "
"a FeatureList", fname);
if (fl_in == NULL) {
fl = KLTCreateFeatureList(nFeatures);
fl->nFeatures = nFeatures;
}
else {
fl = fl_in;
if (fl->nFeatures != nFeatures)
KLTError("(KLTReadFeatureList) The feature list passed "
"does not contain the same number of features as "
"the feature list in file '%s' ", fname);
}
if (!binary) { /* text file */
for (i = 0 ; i < fl->nFeatures ; i++) {
fscanf(fp, "%d |", &indx);
if (indx != i) KLTError("(KLTReadFeatureList) Bad index at i = %d"
"-- %d", i, indx);
_readFeatureTxt(fp, fl->feature[i]);
}
} else { /* binary file */
for (i = 0 ; i < fl->nFeatures ; i++) {
_readFeatureBin(fp, fl->feature[i]);
}
}
fclose(fp);
return fl;
}
unsigned char* pgmRead(
FILE *fp,
unsigned char *img,
int *ncols, int *nrows)
{
unsigned char *ptr;
int magic, maxval;
int i;
/* Read header */
pgmReadHeader(fp, &magic, ncols, nrows, &maxval);
/* Allocate memory, if necessary, and set pointer */
if (img == NULL) {
ptr = (unsigned char *) malloc(*ncols * *nrows * sizeof(char));
if (ptr == NULL)
KLTError("(pgmRead) Memory not allocated");
}
else
ptr = img;
/* Read binary image data */
{
unsigned char *tmpptr = ptr;
for (i = 0 ; i < *nrows ; i++) {
fread(tmpptr, *ncols, 1, fp);
tmpptr += *ncols;
}
}
return ptr;
}
Kernels::Kernels(const float sigma)
{
const float factor = 0.01; /* for truncating tail */
int i;
assert(MAX_KERNEL_WIDTH % 2 == 1);
assert(sigma >= 0.0);
_sigma = sigma;
/* Compute kernels, and automatically determine widths */
{
const int hw = MAX_KERNEL_WIDTH / 2;
float max_gauss = 1.0f, max_gaussderiv = sigma * exp(-0.5f);
/* Compute gauss and deriv */
for (i = -hw; i <= hw; i++)
{
_gauss.data[i + hw] = exp(-i * i / (2 * sigma * sigma));
_gauss_deriv.data[i + hw] = -i * _gauss.data[i + hw];
}
/* Compute widths */
_gauss.width = MAX_KERNEL_WIDTH;
for (i = -hw; fabs(_gauss.data[i + hw] / max_gauss) < factor;
i++, _gauss.width -= 2)
;
_gauss_deriv.width = MAX_KERNEL_WIDTH;
for (i = -hw; fabs(_gauss_deriv.data[i + hw] / max_gaussderiv) < factor;
i++, _gauss_deriv.width -= 2)
;
if (_gauss.width == MAX_KERNEL_WIDTH
|| _gauss_deriv.width == MAX_KERNEL_WIDTH)
KLTError("(_computeKernels) MAX_KERNEL_WIDTH %d is too small for "
"a sigma of %f", MAX_KERNEL_WIDTH, sigma);
}
/* Shift if width less than MAX_KERNEL_WIDTH */
for (i = 0; i < _gauss.width; i++)
_gauss.data[i] = _gauss.data[i + (MAX_KERNEL_WIDTH - _gauss.width) / 2];
for (i = 0; i < _gauss_deriv.width; i++)
_gauss_deriv.data[i] = _gauss_deriv.data[i
+ (MAX_KERNEL_WIDTH - _gauss_deriv.width) / 2];
/* Normalize gauss and deriv */
{
const int hw = _gauss_deriv.width / 2;
float den;
den = 0.0;
for (i = 0; i < _gauss.width; i++)
den += _gauss.data[i];
for (i = 0; i < _gauss.width; i++)
_gauss.data[i] /= den;
den = 0.0;
for (i = -hw; i <= hw; i++)
den -= i * _gauss_deriv.data[i + hw];
for (i = -hw; i <= hw; i++)
_gauss_deriv.data[i + hw] /= den;
}
}
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);
}
}
int ppmReadFile(
char *fname,
unsigned char **r,
unsigned char **g,
unsigned char **b,
int *ncols,
int *nrows)
{
FILE *fp;
unsigned char *rpt, *gpt, *bpt;
int magic, npixels, maxval, i;
unsigned char *rgb, *rgbpt;
/* Open file */
if ( (fp = fopen(fname, "rb")) == NULL)
KLTError("(ppmWriteFileRGB) Can't open file named '%s' for readingng\n", fname);
ppmReadHeader(fp, &magic, ncols, nrows, &maxval);
npixels = *nrows * *ncols;
if(*r == NULL)
{
if ((*r = (unsigned char *)malloc(npixels*3)) == NULL) {
KLTError("Error allocating %d bytes for PGM file: %s\n", npixels, fname);
}
*g = *r + npixels;
*b = *g + npixels;
}
rgb = (unsigned char *)malloc(npixels*3);
fread(rgb, npixels*3, 1, fp);
rpt = *r;
gpt = *g;
bpt = *b;
rgbpt = rgb;
for(i = 0; i < npixels; ++i)
{
*(rpt++)=*(rgbpt++);
*(gpt++)=*(rgbpt++);
*(bpt++)=*(rgbpt++);
}
free(rgb);
fclose(fp);
return 1;
}
static _FloatWindow _allocateFloatWindow(
int width,
int height)
{
_FloatWindow fw;
fw = (_FloatWindow) malloc(width*height*sizeof(float));
if (fw == NULL) KLTError("(_allocateFloatWindow) Out of memory.");
return fw;
}
void ppmReadHeader(
FILE *fp,
int *magic,
int *ncols, int *nrows,
int *maxval)
{
pnmReadHeader(fp, magic, ncols, nrows, maxval);
if (*magic != 6)
KLTError("(ppmReadHeader) Magic number is not 'P6', but 'P%d'", *magic);
}
static void _computeKernels(
float sigma,
ConvolutionKernel *gauss,
ConvolutionKernel *gaussderiv)
{
const float factor = 0.01f; /* for truncating tail */
int i;
assert(MAX_KERNEL_WIDTH % 2 == 1);
assert(sigma >= 0.0);
/* Compute kernels, and automatically determine widths */
{
const int hw = MAX_KERNEL_WIDTH / 2;
float max_gauss = 1.0f, max_gaussderiv = (float) (sigma*exp(-0.5f));
/* Compute gauss and deriv */
for (i = -hw ; i <= hw ; i++) {
gauss->data[i+hw] = (float) exp(-i*i / (2*sigma*sigma));
gaussderiv->data[i+hw] = -i * gauss->data[i+hw];
}
/* Compute widths */
gauss->width = MAX_KERNEL_WIDTH;
for (i = -hw ; fabs(gauss->data[i+hw] / max_gauss) < factor ;
i++, gauss->width -= 2);
gaussderiv->width = MAX_KERNEL_WIDTH;
for (i = -hw ; fabs(gaussderiv->data[i+hw] / max_gaussderiv) < factor ;
i++, gaussderiv->width -= 2);
if (gauss->width == MAX_KERNEL_WIDTH ||
gaussderiv->width == MAX_KERNEL_WIDTH)
KLTError("(_computeKernels) MAX_KERNEL_WIDTH %d is too small for "
"a sigma of %f", MAX_KERNEL_WIDTH, sigma);
}
/* Shift if width less than MAX_KERNEL_WIDTH */
for (i = 0 ; i < gauss->width ; i++)
gauss->data[i] = gauss->data[i+(MAX_KERNEL_WIDTH-gauss->width)/2];
for (i = 0 ; i < gaussderiv->width ; i++)
gaussderiv->data[i] = gaussderiv->data[i+(MAX_KERNEL_WIDTH-gaussderiv->width)/2];
/* Normalize gauss and deriv */
{
const int hw = gaussderiv->width / 2;
float den;
den = 0.0;
for (i = 0 ; i < gauss->width ; i++) den += gauss->data[i];
for (i = 0 ; i < gauss->width ; i++) gauss->data[i] /= den;
den = 0.0;
for (i = -hw ; i <= hw ; i++) den -= i*gaussderiv->data[i+hw];
for (i = -hw ; i <= hw ; i++) gaussderiv->data[i+hw] /= den;
}
sigma_last = sigma;
}
_KLT_Pyramid _KLTCreatePyramid(
int ncols,
int nrows,
int subsampling,
int nlevels)
{
_KLT_Pyramid pyramid;
int nbytes = sizeof(_KLT_PyramidRec) +
nlevels * sizeof(_KLT_FloatImage *) +
nlevels * sizeof(int) +
nlevels * sizeof(int);
int i;
if (subsampling != 2 && subsampling != 4 &&
subsampling != 8 && subsampling != 16 && subsampling != 32)
KLTError("(_KLTCreatePyramid) Pyramid's subsampling must "
"be either 2, 4, 8, 16, or 32");
/* Allocate memory for structure and set parameters */
pyramid = (_KLT_Pyramid) malloc(nbytes);
if (pyramid == NULL)
KLTError("(_KLTCreatePyramid) Out of memory");
/* Set parameters */
pyramid->subsampling = subsampling;
pyramid->nLevels = nlevels;
pyramid->img = (_KLT_FloatImage *) (pyramid + 1);
pyramid->ncols = (int *) (pyramid->img + nlevels);
pyramid->nrows = (int *) (pyramid->ncols + nlevels);
/* Allocate memory for each level of pyramid and assign pointers */
for (i = 0 ; i < nlevels ; i++) {
pyramid->img[i] = _KLTCreateFloatImage(ncols, nrows);
pyramid->ncols[i] = ncols;
pyramid->nrows[i] = nrows;
ncols /= subsampling;
nrows /= subsampling;
}
return pyramid;
}
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 KLTExtractFeatureHistory(
KLT_FeatureHistory fh,
KLT_FeatureTable ft,
int feat)
{
int frame;
if (feat < 0 || feat >= ft->nFeatures)
KLTError("(KLTExtractFeatureHistory) Feature number %d is not between 0 and %d",
feat, ft->nFeatures - 1);
if (fh->nFrames != ft->nFrames)
KLTError("(KLTExtractFeatureHistory) FeatureHistory and FeatureTable must "
"have the same number of frames");
for (frame = 0 ; frame < fh->nFrames ; frame++) {
fh->feature[frame]->x = ft->feature[feat][frame]->x;
fh->feature[frame]->y = ft->feature[feat][frame]->y;
fh->feature[frame]->val = ft->feature[feat][frame]->val;
}
}
void KLTExtractFeatureList(
KLT_FeatureList fl,
KLT_FeatureTable ft,
int frame)
{
int feat;
if (frame < 0 || frame >= ft->nFrames)
KLTError("(KLTExtractFeatures) Frame number %d is not between 0 and %d",
frame, ft->nFrames - 1);
if (fl->nFeatures != ft->nFeatures)
KLTError("(KLTExtractFeatures) FeatureList and FeatureTable must "
"have the same number of features");
for (feat = 0 ; feat < fl->nFeatures ; feat++) {
fl->feature[feat]->x = ft->feature[feat][frame]->x;
fl->feature[feat]->y = ft->feature[feat][frame]->y;
fl->feature[feat]->val = ft->feature[feat][frame]->val;
}
}
static int _findStringWidth(
char *str)
{
int width = 0;
int add;
int maxi = strlen(str) - 1;
int i = 0;
while (str[i] != '\0') {
if (str[i] == '%') {
if (isdigit(str[i+1])) {
sscanf(str+i+1, "%d", &add);
width += add;
i += 2;
while (!_isCharInString(str[i], "diouxefgn")) {
i++;
if (i > maxi)
KLTError("(_findStringWidth) Can't determine length "
"of string '%s'", str);
}
i++;
} else if (str[i+1] == 'c') {
width++;
i += 2;
} else
KLTError("(_findStringWidth) Can't determine length "
"of string '%s'", str);
} else {
i++;
width++;
}
}
return width;
}
_KLT_FloatImage _KLTCreateFloatImage(
int ncols,
int nrows)
{
_KLT_FloatImage floatimg;
unsigned int nbytes = sizeof(_KLT_FloatImageRec) +
(unsigned)(ncols * nrows) * sizeof(float);
floatimg = (_KLT_FloatImage) malloc(nbytes);
if (floatimg == NULL)
KLTError("(_KLTCreateFloatImage) Out of memory");
floatimg->ncols = ncols;
floatimg->nrows = nrows;
floatimg->data = (float *) (floatimg + 1);
return floatimg;
}
void ppmReadHeaderFile(
char *fname,
int *magic,
int *ncols, int *nrows,
int *maxval)
{
FILE *fp;
/* Open file */
if ( (fp = fopen(fname, "rb")) == NULL)
KLTError("(ppmReadHeaderFile) Can't open file named '%s' for reading\n", fname);
/* Read header */
ppmReadHeader(fp, magic, ncols, nrows, maxval);
/* Close file */
fclose(fp);
}
void pgmWriteFile(
char *fname,
unsigned char *img,
int ncols,
int nrows)
{
FILE *fp;
/* Open file */
if ( (fp = fopen(fname, "wb")) == NULL)
KLTError("(pgmWriteFile) Can't open file named '%s' for writing\n", fname);
/* Write to file */
pgmWrite(fp, img, ncols, nrows);
/* Close file */
fclose(fp);
}
unsigned char* pgmReadFile(
char *fname,
unsigned char *img,
int *ncols, int *nrows)
{
unsigned char *ptr;
FILE *fp;
/* Open file */
if ( (fp = fopen(fname, "rb")) == NULL)
KLTError("(pgmReadFile) Can't open file named '%s' for reading\n", fname);
/* Read file */
ptr = pgmRead(fp, img, ncols, nrows);
/* Close file */
fclose(fp);
return ptr;
}
void ppmWriteFileRGB(
char *fname,
unsigned char *redimg,
unsigned char *greenimg,
unsigned char *blueimg,
int ncols,
int nrows)
{
FILE *fp;
/* Open file */
if ( (fp = fopen(fname, "wb")) == NULL)
KLTError("(ppmWriteFileRGB) Can't open file named '%s' for writing\n", fname);
/* Write to file */
ppmWrite(fp, redimg, greenimg, blueimg, ncols, nrows);
/* Close file */
fclose(fp);
}
void KLTStoreFeatureList(
KLT_FeatureList fl,
KLT_FeatureTable ft,
int frame)
{
int feat;
//if (frame < 0 || frame >= ft->nFrames)
// KLTError("(KLTStoreFeatures) Frame number %d is not between 0 and %d",
// frame, ft->nFrames - 1);
if (fl->nFeatures != ft->nFeatures)
KLTError("(KLTStoreFeatures) FeatureList and FeatureTable must "
"have the same number of features");
//for (feat = 0 ; feat < fl->nFeatures ; feat++) {
// ft->feature[feat][frame]->x = fl->feature[feat]->x;
// ft->feature[feat][frame]->y = fl->feature[feat]->y;
// ft->feature[feat][frame]->val = fl->feature[feat]->val;
//}
}
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);
}
}
static structureType _readHeader(
FILE *fp,
int *nFrames,
int *nFeatures,
KLT_BOOL *binary)
{
#define LINELENGTH 100
char line[LINELENGTH];
structureType id;
/* If file is binary, then read data and return */
fread(line, sizeof(char), BINHEADERLENGTH, fp);
line[BINHEADERLENGTH] = 0;
if (strcmp(line, binheader_fl) == 0) {
assert(nFeatures != NULL);
fread(nFeatures, sizeof(int), 1, fp);
*binary = TRUE;
return FEATURE_LIST;
} else if (strcmp(line, binheader_fh) == 0) {
assert(nFrames != NULL);
fread(nFrames, sizeof(int), 1, fp);
*binary = TRUE;
return FEATURE_HISTORY;
} else if (strcmp(line, binheader_ft) == 0) {
assert(nFrames != NULL);
assert(nFeatures != NULL);
fread(nFrames, sizeof(int), 1, fp);
fread(nFeatures, sizeof(int), 1, fp);
*binary = TRUE;
return FEATURE_TABLE;
/* If file is NOT binary, then continue.*/
} else {
rewind(fp);
*binary = FALSE;
}
/* Skip comments until warning line */
while (strcmp(line, warning_line) != 0) {
fgets(line, LINELENGTH, fp);
if (feof(fp))
KLTError("(_readFeatures) File is corrupted -- Couldn't find line:\n"
"\t%s\n", warning_line);
}
/* Read 'Feature List', 'Feature History', or 'Feature Table' */
while (fgetc(fp) != '-');
while (fgetc(fp) != '\n');
fgets(line, LINELENGTH, fp);
if (strcmp(line, "KLT Feature List\n") == 0) id = FEATURE_LIST;
else if (strcmp(line, "KLT Feature History\n") == 0) id = FEATURE_HISTORY;
else if (strcmp(line, "KLT Feature Table\n") == 0) id = FEATURE_TABLE;
else
KLTError("(_readFeatures) File is corrupted -- (Not 'KLT Feature List', "
"'KLT Feature History', or 'KLT Feature Table')");
/* If there's an incompatibility between the type of file */
/* and the parameters passed, exit now before we attempt */
/* to write to non-allocated memory. Higher routine should */
/* detect and handle this error. */
if ((id == FEATURE_LIST && nFeatures == NULL) ||
(id == FEATURE_HISTORY && nFrames == NULL) ||
(id == FEATURE_TABLE && (nFeatures == NULL || nFrames == NULL)))
return id;
/* Read nFeatures and nFrames */
while (fgetc(fp) != '-');
while (fgetc(fp) != '\n');
fscanf(fp, "%s", line);
if (id == FEATURE_LIST) {
if (strcmp(line, "nFeatures") != 0)
KLTError("(_readFeatures) File is corrupted -- "
"(Expected 'nFeatures', found '%s' instead)", line);
} else if (strcmp(line, "nFrames") != 0)
KLTError("(_readFeatures) File is corrupted -- "
"(Expected 'nFrames', found '%s' instead)", line);
fscanf(fp, "%s", line);
if (strcmp(line, "=") != 0)
KLTError("(_readFeatures) File is corrupted -- "
"(Expected '=', found '%s' instead)", line);
if (id == FEATURE_LIST) fscanf(fp, "%d", nFeatures);
else fscanf(fp, "%d", nFrames);
/* If 'Feature Table', then also get nFeatures */
if (id == FEATURE_TABLE) {
fscanf(fp, "%s", line);
if (strcmp(line, ",") != 0)
KLTError("(_readFeatures) File '%s' is corrupted -- "
"(Expected 'comma', found '%s' instead)", line);
fscanf(fp, "%s", line);
if (strcmp(line, "nFeatures") != 0)
KLTError("(_readFeatures) File '%s' is corrupted -- "
"(2 Expected 'nFeatures ', found '%s' instead)", line);
fscanf(fp, "%s", line);
if (strcmp(line, "=") != 0)
KLTError("(_readFeatures) File '%s' is corrupted -- "
"(2 Expected '= ', found '%s' instead)", line);
fscanf(fp, "%d", nFeatures);
}
/* Skip junk before data */
while (fgetc(fp) != '-');
while (fgetc(fp) != '\n');
return id;
#undef LINELENGTH
}
