testing-driver.c

一个外国人网站上的资料

/*
  ratbot-slam (c) 2006 Kris Beevers

  This file is part of ratbot-slam.

  ratbot-slam is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  ratbot-slam is distributed in the hope that it will be useful, but
  WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with ratbot-slam; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA
*/

// this is a driver program for ratbot-slam that runs slam on a pc,
// rather than on the ratbot, using recorded data, for testing
// purposes.

#include "ratbot-slam.h"
#include <stdio.h>
#include <time.h>
#include <stdlib.h>

int main(int argc, char **argv)
{
  if(argc < 2) {
    printf("Usage: %s logfile\n", argv[0]);
    return 1;
  }

  FILE *logfile = fopen(argv[1], "rb");
  if(!logfile) {
    printf("Unable to open %s\n", argv[1]);
    return 1;
  }

  fp_srand(time(0));

  pose_t start;
  //  start.x = start.y = start.t = 0;
  start.x = float2fp(0.5);
  start.y = 0;
  start.t = float2fp(1.57);

  filter_init(&start);

  FILE *pfile = fopen("particles.dat", "w");
  uint32_t count = 0;
  int16_t frame[7];
  while(!feof(logfile)) {
    if(fread(frame, sizeof(int16_t), 7, logfile) != 7)
      break;
    fprintf(stderr, "%d          \r", ++count);

    filter_update(frame[0], frame[1], frame+2);

    write_current_particles(pfile);
  }

  FILE *mfile = fopen("map.dat", "w");
  write_best_map(mfile);
  FILE *tfile = fopen("traj.dat", "w");
  write_best_trajectory(tfile);
  printf("best particle: %d\n", best_particle());

  return 0;
}