/********************************************************************* * 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);