1 map_server
1.1 加载地图
// 有三种模式
/** Map mode
* Default: TRINARY -
* value >= occ_th - Occupied (100)
* value <= free_th - Free (0)
* otherwise - Unknown
* SCALE -
* alpha < 1.0 - Unknown
* value >= occ_th - Occupied (100)
* value <= free_th - Free (0)
* otherwise - f( (free_th, occ_th) ) = (0, 100)
* (linearly map in between values to (0,100)
* RAW -
* value = value
*/
enum MapMode {TRINARY, SCALE, RAW};
// 默认阈值为 0.65 0.196
private_nh.param("occupied_thresh", occ_th, 0.65);
private_nh.param("free_thresh", free_th, 0.196);
// Copy pixel data into the map structure
pixels = (unsigned char*)(img->pixels);
for(j = 0; j < resp->map.info.height; j++)
{
for (i = 0; i < resp->map.info.width; i++)
{
// Compute mean of RGB for this pixel
p = pixels + j*rowstride + i*n_channels;
color_sum = 0;
for(k=0;k<avg_channels;k++)
color_sum += *(p + (k));
color_avg = color_sum / (double)avg_channels;
if (n_channels == 1)
alpha = 1;
else
alpha = *(p+n_channels-1);
if(negate)
color_avg = 255 - color_avg;
// RAW 模式,直接输出像素值为栅格值
if(mode==RAW){
value = color_avg;
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
continue;
}
// If negate is true, we consider blacker pixels free, and whiter
// pixels occupied. Otherwise, it's vice versa.
occ = (255 - color_avg) / 255.0;
// Apply thresholds to RGB means to determine occupancy values for
// map. Note that we invert the graphics-ordering of the pixels to
// produce a map with cell (0,0) in the lower-left corner.
if(occ > occ_th)
value = +100; // 100 为障碍物
else if(occ < free_th)
value = 0; // 0 为 free ,即没有障碍物
else if(mode==TRINARY || alpha < 1.0)
value = -1; // 如果是TRINARY模式,则其他情况赋值为 -1 ,代表 未知
else { // 如果为SCALE模式,则其他情况赋值 0-99 之间的值
double ratio = (occ - free_th) / (occ_th - free_th);
value = 99 * ratio;
}
resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
}
}
1.2 保存地图
// map_server/src/map_saver.cpp
int threshold_occupied_ = 65;
int threshold_free_ = 25;
for(unsigned int y = 0; y < map->info.height; y++) {
for(unsigned int x = 0; x < map->info.width; x++) {
unsigned int i = x + (map->info.height - y - 1) * map->info.width;
if (map->data[i] >= 0 && map->data[i] <= threshold_free_) { // [0,free)
fputc(254, out); // 白色
} else if (map->data[i] >= threshold_occupied_) { // (occ,255]
fputc(000, out); // 黑色
} else { //occ [0.25,0.65]
fputc(205, out); // 灰色
}
}
}
2 GMapping保存的地图
if(!private_nh_.getParam("occ_thresh", occ_thresh_))
occ_thresh_ = 0.25;
for(int x=0; x < smap.getMapSizeX(); x++)
{
for(int y=0; y < smap.getMapSizeY(); y++)
{
/// @todo Sort out the unknown vs. free vs. obstacle thresholding
GMapping::IntPoint p(x, y);
double occ=smap.cell(p);
assert(occ <= 1.0);
// occ < 0, 地图填值 -1,代表未知
if(occ < 0)
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
// occ > 0.25, 地图填值 100,占用,代表障碍物
else if(occ > occ_thresh_)
{
// 注释里为输出1-100 间的值
//map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
}
// 其他情况, 地图填值 0, free, 代表没有障碍物
else
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
}
}
3 AMCL中的map
/**
* Convert an OccupancyGrid map message into the internal
* representation. This allocates a map_t and returns it.
*/
map_t*
AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg )
{
map_t* map = map_alloc();
ROS_ASSERT(map);
map->size_x = map_msg.info.width;
map->size_y = map_msg.info.height;
map->scale = map_msg.info.resolution;
map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
// Convert to player format
map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
ROS_ASSERT(map->cells);
for(int i=0;i<map->size_x * map->size_y;i++)
{
// 将0,free 转成 -1
if(map_msg.data[i] == 0)
map->cells[i].occ_state = -1;
// 将100,占用 转成 +1
else if(map_msg.data[i] == 100)
map->cells[i].occ_state = +1;
// 将其他情况 转成 0
else
map->cells[i].occ_state = 0;
}
return map;
}
4 cartographer_ros中的地图
4.1 默认实现
如果观测到了,输出 0-100中间的值,如果没观测到,输出 -1
for (int y = height - 1; y >= 0; --y) {
for (int x = 0; x < width; ++x) {
const uint32_t packed = pixel_data[y * width + x];
const unsigned char color = packed >> 16;
const unsigned char observed = packed >> 8;
const int value =
observed == 0
? -1
: ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
CHECK_LE(-1, value);
CHECK_GE(100, value);
occupancy_grid->data.push_back(value);
}
}
4.2 转换成AMCL能用的地图
std::unique_ptr<nav_msgs::OccupancyGrid> CreateOccupancyGridMsg(
...
for (int y = height - 1; y >= 0; --y) {
for (int x = 0; x < width; ++x) {
const uint32_t packed = pixel_data[y * width + x];
const unsigned char color = packed >> 16;
const unsigned char observed = packed >> 8;
int value_temp = ::cartographer::common::RoundToInt((1. - color / 255.) * 100.);
if (value_temp > 100*0.65)
value_temp = 100;
else if (value_temp < 100*0.195)
value_temp = 0;
else
value_temp += 0;
const int value =
observed == 0
? -1
: value_temp;
CHECK_LE(-1, value);
CHECK_GE(100, value);
occupancy_grid->data.push_back(value);
}
}
return occupancy_grid;
}
references
1 https://blog.csdn.net/xiaoma_bk/article/details/82963731 --- cartographer amcl 栅格地图统一