pgm.cpp

机器人SLAM方面

/*
  gridsim2 (c) 2006 Kris Beevers

  This file is part of gridsim2.

  gridsim2 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.

  gridsim2 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 gridsim2; if not, write to the Free Software Foundation,
  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

// modified from mpro (added write_pgm)

#include <stdio.h>
#include <ctype.h>
#include <assert.h>

#include <inttypes.h>
#include <sys/types.h>

inline void skip_white_comment(FILE *in)
{
  static char c;
  while((c = fgetc(in)) != EOF) {
    if(c == '#') // skip to end of line
      fscanf(in, "%*[^\n]");
    else if(!isspace(c)) {
      ungetc(c, in);
      break;
    }
  }
}

bool read_raster_raw(FILE *in, uint8_t *grid, uint32_t size, uint16_t max)
{
  if(max > 0xff) {
    // two bytes per pixel
    uint16_t p;
    for(uint32_t i = 0; i < size; ++i) {
      if(!fread(&p, 2, 1, in))
	return false;
      grid[i] = (uint8_t)(float(0xff) * float(p)/float(max));
    }
    return true;
  } else
    return (bool)fread(grid, 1, size, in);
}

bool read_raster_ascii(FILE *in, uint8_t *grid, uint32_t size, uint16_t max)
{
  uint32_t p;
  for(uint32_t i = 0; i < size; ++i) {
    skip_white_comment(in);
    if(!fscanf(in, "%d", &p))
      return false;
    grid[i] = (uint8_t)(float(0xff) * float(p)/float(max));
  }
  return true;
}

// read a PGM file (magic number P2 or P5)
// returns true on success, false on failure
bool load_pgm(const char *file, uint8_t **grid, uint32_t *w, uint32_t *h)
{
  assert(file && grid && w && h);
  FILE *in = fopen(file, "rb");
  if(!in)
    return false;
  uint32_t size;

  // read magic number
  uint8_t magic[2];
  if(!fread(magic, 1, 2, in) || magic[0] != 'P')
    goto error;
  if(magic[1] != '2' && magic[1] != '5')
    goto error;

  // read header
  skip_white_comment(in);
  if(!fscanf(in, "%d", w) || *w == 0)
    goto error;
  skip_white_comment(in);
  if(!fscanf(in, "%d", h) || *h == 0)
    goto error;
  uint32_t maxval;
  skip_white_comment(in);
  if(!fscanf(in, "%d", &maxval) || maxval == 0 || maxval > 0xffff)
    goto error;
  skip_white_comment(in);

  size = *w * *h;
  *grid = new uint8_t[size];
  if(magic[1] == '2')
    return read_raster_ascii(in, *grid, size, maxval);
  else
    return read_raster_raw(in, *grid, size, maxval);

 error:
  fclose(in);
  return false;
}

bool write_pgm(const char *file, const uint8_t *grid, uint32_t w, uint32_t h)
{
  assert(file && grid);
  FILE *out = fopen(file, "wb");
  if(!out)
    return false;
  fprintf(out, "P5\n%d\n%d\n255\n", w, h);
  fwrite(grid, w*h, 1, out);
  fclose(out);
  return true;
}