grid.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
*/

#include "sim.hpp"

uint8_t *grid;
uint32_t W, H;

// compute density of a grid
double grid_density(const uint8_t *grid, uint32_t w, uint32_t h)
{
  uint32_t F = 0;
  for(uint32_t i = 0; i < w*h; ++i)
    if(grid[i] == 0) // occupied
      ++F;
  return double(F)/double(w*h);
}

// compute entropy of a grid with respect to 4-neighbors (i.e., how
// well 4-neighbors predict a cell's occupancy)
inline bool occ(int i, int j, const uint8_t *grid, uint32_t w, uint32_t h)
{
  if(i < 0 || i >= int(w) || j < 0 || j >= int(h))
    return true;
  return grid[w*i+j] == 0;
}
double grid_entropy(const uint8_t *grid, uint32_t w, uint32_t h)
{
  uint32_t cond[32];
  memset(cond, 0, 32*sizeof(uint32_t));
  int8_t idx;
  for(int i = 0; i < int(w); ++i) {
    for(int j = 0; j < int(h); ++j) {
      idx = occ(i,j,grid,w,h);
      idx |= occ(i-1,j,grid,w,h) << 1;
      idx |= occ(i+1,j,grid,w,h) << 2;
      idx |= occ(i,j-1,grid,w,h) << 3;
      idx |= occ(i,j+1,grid,w,h) << 4;
      cond[idx]++;
    }
  }
  double H = 0;
  for(int i = 0; i < 32; i += 2) {
    uint32_t N = cond[i]+cond[i+1];
    if(N == 0) continue;
    double p_neighbors = double(N) / (w*h);
    double H_int = 0;
    if(cond[i+1] > 0) {
      double p_F_given_neighbors = double(cond[i+1])/double(N);
      H_int += p_F_given_neighbors*log(p_F_given_neighbors)/log(2);
    }
    if(cond[i] > 0) {
      double p_E_given_neighbors = double(cond[i])/double(N);
      H_int += p_E_given_neighbors*log(p_E_given_neighbors)/log(2);
    }
    H += p_neighbors * H_int;
  }
  return -H;
}


// independent cells grid generation
void generate_random_grid_unif(double density)
{
  grid = new uint8_t[W*H];
  for(uint32_t i = 0; i < W*H; ++i) {
    if(drand48() < density)
      grid[i] = F;
    else
      grid[i] = E;
  }
}


// markov random field grid generation

std::vector<mrf_pair> mrf;

bool load_mrf_model(const char *file)
{
  // format: each line is a pair, with:
  //   dx dy f_f f_e e_e
  // (see mrf_pair in sim.hpp for explanation of values)
  mrf.clear(); 
  FILE *in = fopen(file, "r");
  if(!in)
    return false;
  mrf_pair p;
  while(!feof(in)) {
    if(fscanf(in, "%d %d %lg %lg %lg\n", &p.dx, &p.dy, &p.psi.f_f, &p.psi.f_e, &p.psi.e_e) != 5)
      break;
    mrf.push_back(p);
  }
  fclose(in);
  return true;
}

// multiply p_f and p_e by likelihoods specified in the mrf model for
// this pair (m,[i,j]) of cells
inline void p_given_neighbor
  (const mrf_psi_function &psi, int i, int j, double &p_f, double &p_e)
{
  if(i < 0 || i >= int(W) || j < 0 || j >= int(H))
    return; // out of bounds [FIXME: make toroidal?]
  const uint8_t &mij = grid[j*W+i];
  if(mij == F) { p_f *= psi.f_f; p_e *= psi.f_e; }
  else { p_f *= psi.f_e; p_e *= psi.e_e; }
}

void generate_random_grid_mrf(double density, uint32_t N)
{
  generate_random_grid_unif(density); // initial grid

  // run N iterations of gibbs sampling
  double p_f, p_e;
  for(uint32_t n = 0; n < N; ++n) {
    for(int i = 0; i < int(W); ++i) {
      for(int j = 0; j < int(H); ++j) {
	// compute p(m_{ij}=F|N(m_{ij})) = p(m_{ij}=F) * \prod_{m_{ab}
	// \in N(m_{ij})} p(m_{ij}=F|m_{ab})
	// uses the neighbors specified by all the pairs in the mrf vector
	p_f = density; p_e = 1-density;
	for(uint32_t k = 0; k < mrf.size(); ++k)
	  p_given_neighbor(mrf[k].psi, i+mrf[k].dx, j+mrf[k].dy, p_f, p_e);
	p_f = p_f/(p_f+p_e); // normalize
	if(drand48() < p_f) grid[j*W+i] = F;
	else grid[j*W+i] = E;
      }
    }
  }
}


// cells with no visibility can never be mapped: generate the best
// possible map with this restriction

inline bool isempty(int i, int j)
{
  if(i < 0 || i >= int(W) || j < 0 || j >= int(H))
    return false;
  return grid[j*W+i]==E;
}

inline bool isimpossible(uint8_t *possible, int i, int j)
{
  if(i < 0 || i >= int(W) || j < 0 || j >= int(H))
    return true;
  return possible[j*W+i]==U;
}

void generate_best_possible_map(uint8_t *possible)
{
  // first pass
  for(int i = 0; i < int(W); ++i)
    for(int j = 0; j < int(H); ++j) {
      if(isempty(i-1,j) || isempty(i+1,j) || isempty(i,j-1) || isempty(i,j+1))
	possible[j*W+i] = grid[i*W+j];
      else
	possible[j*W+i] = U;
    }
  // next pass: verify consistency
  for(int i = 0; i < int(W); ++i)
    for(int j = 0; j < int(H); ++j) {
      if(isimpossible(possible,i,j)) continue;
      if(isimpossible(possible,i-1,j) && isimpossible(possible,i+1,j) &&
	 isimpossible(possible,i,j-1) && isimpossible(possible,i,j+1))
	possible[j*W+i] = U;
    }
}