lybhit / correlation_scan_match Goto Github PK

This package will abort, because of the load map coordinate is not match to laser scan coordinate. This will result in memory leak. You must confirm the map coordinate and laser scan coordinate is matched. I hope this is helpful for you!

How to convert the standard grid map in ros (OccupancyGrid) to the probability map (ProbabilityGrid) required for matching in cartographer, your code seems to be what is the definition of map in convertMap

ProbabilityGrid*
location_module::convertMap( const nav_msgs::OccupancyGrid& map_msg )
{

// ROS_ASSERT(map);

CellLimits cell_limits(map_msg.info.width, map_msg.info.height);

double resolution = map_msg.info.resolution;

double origin_x = map_msg.info.origin.position.x + (map->size_x) * map->scale;
double origin_y = map_msg.info.origin.position.y + (map->size_y) * map->scale;

Eigen::Vector2d max(origin_x, origin_y);

map_limits = new Maplimits(resolution, max, cell_limits);

grid = new ProbabilityGrid(*map_limits);

// Convert to player format

for(int i = 0; i < map->size_y; i++){
for(int j = 0; j < map->size_x; j++)
{
int map_index = i * map->size_x + j;
int grid_x = map->size_x - j;
int grid_y = map->size_y - i;
int grid_index = grid_y * map->size_x + grid_x;

if(map_msg.data[map_index] == 100)
  grid->*lookupTable_[grid_index] = 1;
else
  grid->*lookupTable_[grid_index] = 0;
}

}

return grid;