model.cpp

一个很好的粒子滤波算法

/**
 * implementation of a temporal motion trajectory model
 * by Alex Gruenstein
 * for cs223b final project
 */

#include <stdio.h>
#include "scanner.h"
#include "model.h"

#define BUF_LEN 500

/**
 * load the model from a file,
 * uses my tokenizer from cs107
 * file should be of the form:
 * first line = number of entries in the table
 * second line = horitontal velocitiesLeft, tab delimited
 * third line = vertical velocitiesLeft, tab delimited
 * fourth line = horizontal velocitiesRight
 * fifth line = vertical velocitiesRight
 * all numbers should be in *double* format
 */
Model::Model(const char *filename)
{
  Scanner s;
  char buffer[BUF_LEN];
  double temp;

  s = NewScannerFromFilename(filename, "\t\r\n ", true);
  ReadNextToken(s, buffer, BUF_LEN);
  sscanf(buffer, "%le", &temp);
  length = (int)temp;  //store in length

  hvelLeft = ReadDoubleArray(s, length);
  vvelLeft = ReadDoubleArray(s, length);

  hvelRight = ReadDoubleArray(s, length);
  vvelRight = ReadDoubleArray(s, length);

  FreeScanner(s);
  
  /*
  printf("length = %d\n", length);
  
  for(int i = 0; i < length; i++) {
    printf("hvel[%d] = %e\n", i, hvel[i]);
  }

  for(int i = 0; i < length; i++) {
    printf("vvel[%d] = %e\n", i, vvel[i]);
  }
  */
}

/**
 * utility function to read in a double array from file
 */
double *Model::ReadDoubleArray(Scanner s, int length) 
{
  char buffer[BUF_LEN];
  double temp;

  double *array = new double[length];
  for(int i = 0; i < length; i++) {
    ReadNextToken(s, buffer, BUF_LEN);
    sscanf(buffer, "%le", &temp);
    array[i] = temp;
  }

  return array;
}

int Model::GetLength() {
  return length;
}

double Model::InterpolateLeftHvel(double phase)
{
  return LinearInterpolate(hvelLeft, phase);
}

double Model::InterpolateLeftVvel(double phase)
{
  return LinearInterpolate(vvelLeft, phase);
}

double Model::InterpolateRightHvel(double phase)
{
  return LinearInterpolate(hvelRight, phase);
}

double Model::InterpolateRightVvel(double phase)
{
  return LinearInterpolate(vvelRight, phase);
}


/**
 * linearly interpolate the value at array[phase],
 * where phase is not necessarily an integer
 */
double Model::LinearInterpolate(double *array, double phase)
{
  //not sure what to do in these cases...
  if(phase < 0)
    return array[0];
  else if(phase > length-1)
    return array[length-1];

  int left = (int)phase;
  int right = left+1;

  double leftVal = array[left];
  double rightVal = array[right];

  double slope = rightVal - leftVal;
  
  double mod = phase - (double)left;
  
  return leftVal + (mod * slope);
}