机器人二维地图的各种表示及转换

机器人二维地图表示方式：

• 占据栅格地图 (nav_msgs::OccupancyGrid)
• 多层栅格地图 (grid_map)
• OpenCV 图像表示

占据栅格地图 (nav_msgs::OccupancyGrid)

栅格地图即将一块区域分割成 m*n 个正方形格子，栅格地图的分辨率就表示每个格子的边长表示物理上的距离，比如分辨率为 1 米的栅格地图表示 100 米 * 100 米区域需要 100 * 100 个格子，如下所示：

nav_msgs::OccupancyGrid 是 ROS 中表示二维占据栅格的数据结构，它包括以下内容：

1
2
3
4
5
6
7
8

# 元数据
std_msgs/Header      header

# 地图信息
MapMetaData          info

# 地图数据
int8[]               data

元数据为标准的 ROS 消息头

地图信息包括以下内容：

地图数据是一维数组，长度为 width × height，按行优先（row-major）排列。每个元素取值：

data 的索引 0 对应栅格地图左下角，x 沿宽度方向递增（向右）， y 沿高度方向递增（向上）

1
2
3
4
5
6
7

    y ↑
      |
      |   • P(3, 4)
      |  /
      | /
------+------→ x
      |

坐标转数据： value(x, y) = data[y * width + x]

世界坐标和栅格坐标的转换：

1
2

grid_x = (world_x - origin.position.x) / resolution
grid_y = (world_y - origin.position.y) / resolution

多层栅格地图 (grid_map)

grid_map 用于表示栅格地图和操作栅格地图，支持同一位置存储多层数据。

grid_map 底层使用 Eigen::MatrixXf 来存储每一层的数据

grid_map 的坐标表示：

• (0, 0) 位于地图左下角：
• 行号 → y 方向（从地图底部到顶部）
• 列号 → x 方向（从地图左侧到右侧

栅格访问方式

1. 矩阵索引访问栅格坐标 (x, y) -> (row, col)

   Copy

   1 2 3 4 5 6

   grid_map::Index index(5, 10); //(row = y, col = x) float& val = mat.at("layer", index); // 或者直接拿到 Eigne 的 matrix Eigen::MatrixXf& mat = map["elevation"]; float& val = mat(5, 10);

2. 已知世界坐标 (2.5m, 1.0m) ，使用世界坐标访问

   Copy

   1 2 3 4 5 6 7 8 9

   grid_map::Position worldPos(2.5, 1.0); // 自动将世界坐标按照分辨率转为索引 grid_map::Index index; map.getIndex(worldPos, index); float val = map.at("elevation", index); // 或者一步到位 float val = map.atPosition("elevation", worldPos);

获取地图信息

1. 获取栅格的宽高

   Copy

   1 2 3

   grid_map::Size size = map.getSize(); int cols = size.x(); // (矩阵列数, x 方向栅格数) int rows = size.y(); // (矩阵行数, y 方向栅格数)

2. 获取物理世界的宽高

   Copy

   1 2 3

   grid_map::Length length = map.getLength(); // 单位为 m double lenX = length.x(); // (物理宽) double lenY = length.y(); // (物理高)

3. 地图原点，grid_map 的地图原点不一定是在左下角，即坐标可能为负数

   Copy

   1	grid_map::Position pos = map.getPosition();

OpenCV 图像表示

可以使用 OpenCV 表示栅格地图，一般一个栅格对应一个像素

坐标变换

OpenCV 中的坐标 Y 轴向下，以左上角为坐标原点

1
2
3
4
5
6
7

------+------→ x
    y |
      |
      |   • P(3, height - 1 - 4)
      |
      |
------+------→ x'

及 grid_map 中 (x, y) 对应于 OpenCV 中就是 (x, height - 1 - y)

像素访问

(x, y) 对应 (row, col)

1
2
3
4
5
6

cv::Mat image;

int x = 10;
int y = 5;

image.at<uchar>(5, 10);