nav_msgs::OccupancyGrid数据结构,坐标系及索引概述

nav_msgs::OccupancyGrid的数据结构

header: 
  seq: 
  stamp: 
  frame_id:  # 地图所在坐标系
info: 
  map_load_time:
  resolution: # 分辨率
  width:  # 宽度:在x方向的栅格数
  height: # 高度:在y方向的栅格数
  origin:  # 地图原点在frame_id坐标系下的值;
    position:  
    orientation: 
data:  #  int8[] 一维数组,按照索引存放栅格数据;

nav_msgs::OccupancyGrid的坐标系

地图的原点在矩形的一个顶点,如下图所表示前左上(x,y,y)坐标系(俯视图):
原点为右下顶点,x轴向上,y轴向左;
OccupancyGrid坐标系与车辆坐标系的关系,一般按如下方式建立:
                               ^
    ---------------------------| x
    |                          |  
    |              ^           |
    |              | car_x     |
    |              |           |
    |              |           |
    |     <--------*           |
    |   car_y     car_origin   |
    |                          |  
 <-----------------------------|---
    y                   OccupacyGrid_origin

参数计算关系:
OccupacyGrid_origin.x = - car_origin_to_map_back;
OccupacyGrid_origin.y = - 0.5*height*resolution;
OccupacyGrid_origin.yaw = 0;

nav_msgs::OccupancyGrid的数据索引

索引号的排序关系

索引号沿着x轴方向逐行累加,取值如下图所示:
                               ^
    ---------------------------| x
    |   |        ...       |w-1|  
    |---|----------------------|
    |            ...           |  
    |---|--------------|---|---|
    |   |        ...   |w+1| 1 |
    |---|--------------|---|---|
    |   |        ...   | w | 0 |
 <-----------------------------|---
    y                     OccupacyGrid_origin

点在map系的坐标值(x,y)与索引号(idx)的关系

已知坐标求索引:
ix = x/resolution;
iy = y/resolution;
idx = ix + iy * width;

已知索引求坐标:
iy = idx / width;
ix = idx % width;
x = ix * resolution;
y = iy * resolution;
                               ^
    ---------------------------| x
    |                          |  
    |                          |
    |                          |
    |             (x,y)        |
    |               * ---------| ix
    |               |          |
    |               |          |  
 <-----------------------------|---
    y               iy             OccupacyGrid_origin