writefeatures.c

学习跟踪的好程序

/*********************************************************************
 * writeFeatures.c
 *
 *********************************************************************
 */

/* Standard includes */
#include <assert.h>
#include <ctype.h>		/* isdigit() */
#include <stdio.h>		/* sprintf(), fprintf(), sscanf(), fscanf() */
#include <stdlib.h>		/* malloc() */
#include <string.h>		/* memcpy(), strcmp() */

/* Our includes */
#include "base.h"
#include "error.h"
#include "pnmio.h"		/* ppmWriteFileRGB() */
#include "klt.h"

#define BINHEADERLENGTH	6

extern int KLT_verbose;

typedef enum {FEATURE_LIST, FEATURE_HISTORY, FEATURE_TABLE} structureType;

static char warning_line[] = "!!! Warning:  This is a KLT data file.  "
                             "Do not modify below this line !!!\n";
static char binheader_fl[BINHEADERLENGTH+1] = "KLTFL1";
static char binheader_fh[BINHEADERLENGTH+1] = "KLTFH1";
static char binheader_ft[BINHEADERLENGTH+1] = "KLTFT1";

/*********************************************************************
 * KLTWriteFeatureListToPPM
 */

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);
}


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;
}


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;
}


static void _printNhyphens(
  FILE *fp,
  int n)
{
  int i;
  for (i = 0 ; i < n ; i++)
    fprintf(fp, "-");
}

static void _printInteger(
  FILE *fp,
  int integer,
  int width)
{
  char fmt[80];
  sprintf(fmt, "%%%dd", width);
  fprintf(fp, fmt, integer);
}


static KLT_BOOL _isCharInString(
  char c,
  char *str)
{
  int width = strlen(str);
  int i;

  for (i = 0 ; i < width ; i++)
    if (c == str[i])  return TRUE;

  return FALSE;
}


/*********************************************************************
 * _findStringWidth
 *
 * Calculates the length of a string after expansion.  E.g., the
 * length of "(%6.1f)" is eight -- six for the floating-point number,
 * and two for the parentheses.
 */

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;
}


static void _printHeader(
  FILE *fp,
  char *format,
  structureType id,
  int nFrames,
  int nFeatures)
{
  int width = _findStringWidth(format);
  int i;
	
  assert(id == FEATURE_LIST || id == FEATURE_HISTORY || id == FEATURE_TABLE);

  if (fp != stderr)  {
    fprintf(fp, "Feel free to place comments here.\n\n\n");
    fprintf(fp, "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");
    fprintf(fp, warning_line);
    fprintf(fp, "\n");
  }
  fprintf(fp, "------------------------------\n");
  switch (id)  {
     case FEATURE_LIST: fprintf(fp, "KLT Feature List\n");    break;
     case FEATURE_HISTORY: fprintf(fp, "KLT Feature History\n"); break;
     case FEATURE_TABLE: fprintf(fp, "KLT Feature Table\n");   break;
  }

  fprintf(fp, "------------------------------\n\n");
  switch (id)  {
     case FEATURE_LIST: fprintf(fp, "nFeatures = %d\n\n", nFeatures);    break;
     case FEATURE_HISTORY: fprintf(fp, "nFrames = %d\n\n", nFrames); break;
     case FEATURE_TABLE: fprintf(fp, "nFrames = %d, nFeatures = %d\n\n",
                                 nFrames, nFeatures);   break;
  }

  switch (id)  {
     case FEATURE_LIST: fprintf(fp, "feature | (x,y)=val\n");
       fprintf(fp, "--------+-");
       _printNhyphens(fp, width);
       fprintf(fp, "\n");   
       break;
     case FEATURE_HISTORY: fprintf(fp, "frame | (x,y)=val\n");
       fprintf(fp, "------+-");
       _printNhyphens(fp, width);
       fprintf(fp, "\n");   
       break;
     case FEATURE_TABLE: fprintf(fp, "feature |          frame\n");   
       fprintf(fp, "        |");   
       for (i = 0 ; i < nFrames ; i++) _printInteger(fp, i, width);
       fprintf(fp, "\n--------+-");   
       for (i = 0 ; i < nFrames ; i++) _printNhyphens(fp, width);
       fprintf(fp, "\n");   
       break;
  }
}


static void _printFeatureTxt(
  FILE *fp,
  KLT_Feature feat,
  char *format,
  char type)
{
  assert(type == 'f' || type == 'd');

  if (type == 'f')
    fprintf(fp, format, (float) feat->x, (float) feat->y, feat->val);
  else if (type == 'd')  {
    /* Round x & y to nearest integer, unless negative */
    float x = feat->x;
    float y = feat->y;
    if (x >= 0.0) x += 0.5;
    if (y >= 0.0) y += 0.5;
    fprintf(fp, format, 
            (int) x, (int) y, feat->val);
  }
}


static void _printFeatureBin(
  FILE *fp,
  KLT_Feature feat)
{
  fwrite(&(feat->x), sizeof(KLT_locType), 1, fp);
  fwrite(&(feat->y), sizeof(KLT_locType), 1, fp);
  fwrite(&(feat->val), sizeof(int), 1, fp);
}


static void _printShutdown(
  FILE *fp)
{
  /* Close file, if necessary */
  if (fp != stderr)
    fclose(fp);
}


/*********************************************************************
 * KLTWriteFeatureList()
 * KLTWriteFeatureHistory()
 * KLTWriteFeatureTable()
 * 
 * Writes features to file or to screen.
 *
 * INPUTS
 * fname: name of file to write data; if NULL, then print to stderr
 * fmt:   format for printing (e.g., "%5.1f" or "%3d");
 *        if NULL, and if fname is not NULL, then write to binary file.
 */

void KLTWriteFeatureList(
  KLT_FeatureList fl,
  char *fname, 
  char *fmt)
{
  FILE *fp;
  char format[100];
  char type;
  int i;

  if (KLT_verbose >= 1 && fname != NULL)  {
    fprintf(stderr,  
            "(KLT) Writing feature list to %s file: '%s'\n", 
            (fmt == NULL ? "binary" : "text"), fname);
  }

  if (fmt != NULL) {  /* text file or stderr */
    fp = _printSetupTxt(fname, fmt, format, &type);
    _printHeader(fp, format, FEATURE_LIST, 0, fl->nFeatures);
	
    for (i = 0 ; i < fl->nFeatures ; i++)  {
      fprintf(fp, "%7d | ", i);
      _printFeatureTxt(fp, fl->feature[i], format, type);
      fprintf(fp, "\n");
    }
    _printShutdown(fp);
  } else {  /* binary file */
    fp = _printSetupBin(fname);
    fwrite(binheader_fl, sizeof(char), BINHEADERLENGTH, fp); 
    fwrite(&(fl->nFeatures), sizeof(int), 1, fp);
    for (i = 0 ; i < fl->nFeatures ; i++)  {
      _printFeatureBin(fp, fl->feature[i]);
    }
    fclose(fp);
  }
}


void KLTWriteFeatureHistory(
  KLT_FeatureHistory fh,
  char *fname, 
  char *fmt)
{
  FILE *fp;
  char format[100];
  char type;
  int i;