map_inflator.cpp

Go to the documentation of this file.

#include <bwi_mapper/map_inflator.h>

namespace bwi_mapper {
  
  void inflateMap(double threshold, const nav_msgs::OccupancyGrid& map, 
      nav_msgs::OccupancyGrid& inflated_map) {

    inflated_map.header = map.header;
    inflated_map.info = map.info;

    // expand the map out based on the circumscribed robot distance
    int expand_pixels = ceil(threshold / map.info.resolution);
    inflated_map.data.resize(map.info.height * map.info.width);
    for (int i = 0; i < (int)map.info.height; ++i) {
      for (int j = 0; j < (int)map.info.width; ++j) {
        int low_i = (i - expand_pixels < 0) ? 0 : i - expand_pixels;
        int high_i = (i + expand_pixels >= (int)map.info.height) ? 
          map.info.height - 1 : i + expand_pixels;
        int max = 0;
        for (int k = low_i; k <= high_i; ++k) {
          int diff_j = floor(sqrtf(expand_pixels * expand_pixels - (i - k) * (i - k)));
          int low_j = (j - diff_j < 0) ? 0 : j - diff_j;
          int high_j = (j + diff_j >= (int)map.info.width) ? 
            map.info.width - 1 : j + diff_j;
          for (int l = low_j; l <= high_j; ++l) {
            if (map.data[k * map.info.width + l] > max) {
              max = map.data[k * map.info.width + l];
            }
          }
        }
        inflated_map.data[i * map.info.width + j] = max;
      }
    }
  }

} /* bwi_mapper */