klt.c

KLT: An Implementation of the Kanade-Lucas-Tomasi Feature Tracker KLT: An Implementation of the

/*********************************************************************
 * klt.c
 *
 * Kanade-Lucas-Tomasi tracker
 *********************************************************************/

/* Standard includes */
#include <assert.h>
#include <math.h>    /* logf() */
#include <stdlib.h>  /* malloc() */

/* Our includes */
#include "base.h"
#include "convolve.h"
#include "error.h"
#include "klt.h"
#include "pyramid.h"


static const int mindist = 10;
static const int window_size = 7;
static const int min_eigenvalue = 1;
static const float min_determinant = 0.01f;
static const float min_displacement = 0.1f;
static const int max_iterations = 10;
static const float max_residue = 10.0f;
static const float grad_sigma = 1.0f;
static const float smooth_sigma_fact = 0.1f;
static const float pyramid_sigma_fact = 0.9f;
static const float step_factor = 1.0f;
static const KLT_BOOL sequentialMode = FALSE;
static const KLT_BOOL lighting_insensitive = FALSE;
/* for affine mapping*/
static const int affineConsistencyCheck = -1;
static const int affine_window_size = 15;
static const int affine_max_iterations = 10;
static const float affine_max_residue = 10.0;
static const float affine_min_displacement = 0.02f;
static const float affine_max_displacement_differ = 1.5f;

static const KLT_BOOL smoothBeforeSelecting = TRUE;
static const KLT_BOOL writeInternalImages = FALSE;
static const int search_range = 15;
static const int nSkippedPixels = 0;

extern int KLT_verbose;


/*********************************************************************
 * _createArray2D
 *
 * Creates a two-dimensional array.
 *
 * INPUTS
 * ncols:      no. of columns
 * nrows:      no. of rows
 * nbytes:     no. of bytes per entry
 *
 * RETURNS
 * Pointer to an array.  Must be coerced.
 *
 * EXAMPLE
 * char **ar;
 * ar = (char **) createArray2D(8, 5, sizeof(char));
 */

static void** _createArray2D(int ncols, int nrows, int nbytes)
{
  char **tt;
  int i;

  tt = (char **) malloc(nrows * sizeof(void *) +
                        ncols * nrows * nbytes);
  if (tt == NULL)
    KLTError("(createArray2D) Out of memory");

  for (i = 0 ; i < nrows ; i++)
    tt[i] = ((char *) tt) + (nrows * sizeof(void *) +
                             i * ncols * nbytes);

  return((void **) tt);
}


/*********************************************************************
 * KLTCreateTrackingContext
 *
 */

KLT_TrackingContext KLTCreateTrackingContext()
{
  KLT_TrackingContext tc;

  /* Allocate memory */
  tc = (KLT_TrackingContext)  malloc(sizeof(KLT_TrackingContextRec));

  /* Set values to default values */
  tc->mindist = mindist;
  tc->window_width = window_size;
  tc->window_height = window_size;
  tc->sequentialMode = sequentialMode;
  tc->smoothBeforeSelecting = smoothBeforeSelecting;
  tc->writeInternalImages = writeInternalImages;
  tc->lighting_insensitive = lighting_insensitive;
  tc->min_eigenvalue = min_eigenvalue;
  tc->min_determinant = min_determinant;
  tc->max_iterations = max_iterations;
  tc->min_displacement = min_displacement;
  tc->max_residue = max_residue;
  tc->grad_sigma = grad_sigma;
  tc->smooth_sigma_fact = smooth_sigma_fact;
  tc->pyramid_sigma_fact = pyramid_sigma_fact;
  tc->step_factor = step_factor;
  tc->nSkippedPixels = nSkippedPixels;
  tc->pyramid_last = NULL;
  tc->pyramid_last_gradx = NULL;
  tc->pyramid_last_grady = NULL;
  /* for affine mapping */
  tc->affineConsistencyCheck = affineConsistencyCheck;
  tc->affine_window_width = affine_window_size;
  tc->affine_window_height = affine_window_size;
  tc->affine_max_iterations = affine_max_iterations;
  tc->affine_max_residue = affine_max_residue;
  tc->affine_min_displacement = affine_min_displacement;
  tc->affine_max_displacement_differ = affine_max_displacement_differ;

  /* Change nPyramidLevels and subsampling */
  KLTChangeTCPyramid(tc, search_range);
	
  /* Update border, which is dependent upon  */
  /* smooth_sigma_fact, pyramid_sigma_fact, window_size, and subsampling */
  KLTUpdateTCBorder(tc);

  return(tc);
}


/*********************************************************************
 * KLTCreateFeatureList
 *
 */

KLT_FeatureList KLTCreateFeatureList(
  int nFeatures)
{
  KLT_FeatureList fl;
  KLT_Feature first;
  int nbytes = sizeof(KLT_FeatureListRec) +
    nFeatures * sizeof(KLT_Feature) +
    nFeatures * sizeof(KLT_FeatureRec);
  int i;
	
  /* Allocate memory for feature list */
  fl = (KLT_FeatureList)  malloc(nbytes);
	
  /* Set parameters */
  fl->nFeatures = nFeatures; 

  /* Set pointers */
  fl->feature = (KLT_Feature *) (fl + 1);
  first = (KLT_Feature) (fl->feature + nFeatures);
  for (i = 0 ; i < nFeatures ; i++) {
    fl->feature[i] = first + i;
    fl->feature[i]->aff_img = NULL;           /* initialization fixed by Sinisa Segvic */
    fl->feature[i]->aff_img_gradx = NULL;
    fl->feature[i]->aff_img_grady = NULL;
  }
  /* Return feature list */
  return(fl);
}


/*********************************************************************
 * KLTCreateFeatureHistory
 *
 */

KLT_FeatureHistory KLTCreateFeatureHistory(
  int nFrames)
{
  KLT_FeatureHistory fh;
  KLT_Feature first;
  int nbytes = sizeof(KLT_FeatureHistoryRec) +
    nFrames * sizeof(KLT_Feature) +
    nFrames * sizeof(KLT_FeatureRec);
  int i;
	
  /* Allocate memory for feature history */
  fh = (KLT_FeatureHistory)  malloc(nbytes);
	
  /* Set parameters */
  fh->nFrames = nFrames; 
	
  /* Set pointers */
  fh->feature = (KLT_Feature *) (fh + 1);
  first = (KLT_Feature) (fh->feature + nFrames);
  for (i = 0 ; i < nFrames ; i++)
    fh->feature[i] = first + i;

  /* Return feature history */
  return(fh);
}


/*********************************************************************
 * KLTCreateFeatureTable
 *
 */

KLT_FeatureTable KLTCreateFeatureTable(
  int nFrames,
  int nFeatures)
{
  KLT_FeatureTable ft;
  KLT_Feature first;
  int nbytes = sizeof(KLT_FeatureTableRec);
  int i, j;
	
  /* Allocate memory for feature history */
  ft = (KLT_FeatureTable)  malloc(nbytes);
	
  /* Set parameters */
  ft->nFrames = nFrames; 
  ft->nFeatures = nFeatures; 
	
  /* Set pointers */
  ft->feature = (KLT_Feature **) 
    _createArray2D(nFrames, nFeatures, sizeof(KLT_Feature));
  first = (KLT_Feature) malloc(nFrames * nFeatures * sizeof(KLT_FeatureRec));
  for (j = 0 ; j < nFeatures ; j++)
    for (i = 0 ; i < nFrames ; i++)
      ft->feature[j][i] = first + j*nFrames + i;

  /* Return feature table */
  return(ft);
}


/*********************************************************************
 * KLTPrintTrackingContext
 */

void KLTPrintTrackingContext(
  KLT_TrackingContext tc)
{
  fprintf(stderr, "\n\nTracking context:\n\n");
  fprintf(stderr, "\tmindist = %d\n", tc->mindist);
  fprintf(stderr, "\twindow_width = %d\n", tc->window_width);
  fprintf(stderr, "\twindow_height = %d\n", tc->window_height);
  fprintf(stderr, "\tsequentialMode = %s\n",
          tc->sequentialMode ? "TRUE" : "FALSE");
  fprintf(stderr, "\tsmoothBeforeSelecting = %s\n",
          tc->smoothBeforeSelecting ? "TRUE" : "FALSE");
  fprintf(stderr, "\twriteInternalImages = %s\n",
          tc->writeInternalImages ? "TRUE" : "FALSE");

  fprintf(stderr, "\tmin_eigenvalue = %d\n", tc->min_eigenvalue);
  fprintf(stderr, "\tmin_determinant = %f\n", tc->min_determinant);
  fprintf(stderr, "\tmin_displacement = %f\n", tc->min_displacement);
  fprintf(stderr, "\tmax_iterations = %d\n", tc->max_iterations);
  fprintf(stderr, "\tmax_residue = %f\n", tc->max_residue);
  fprintf(stderr, "\tgrad_sigma = %f\n", tc->grad_sigma);
  fprintf(stderr, "\tsmooth_sigma_fact = %f\n", tc->smooth_sigma_fact);
  fprintf(stderr, "\tpyramid_sigma_fact = %f\n", tc->pyramid_sigma_fact);
  fprintf(stderr, "\tnSkippedPixels = %d\n", tc->nSkippedPixels);
  fprintf(stderr, "\tborderx = %d\n", tc->borderx);