基于C++的GridMap2D 代码和公式

在这里插入图片描述
膨胀半径

这段代码主要是关于在二维地图上计算点之间距离的几个函数，同时也包含了查询地图上特定坐标点的距离和值的函数。我们逐一来解释每个函数的作用：

在这里插入图片描述

worldDist(unsigned x1, unsigned y1, unsigned x2, unsigned y2):
- 这个函数计算两个地图坐标之间的距离（以米为单位）。它接受两个点的 x 和 y 坐标作为输入，并通过调用 worldDist(const cv::Point& p1, const cv::Point& p2) 实现计算。
worldDist(const cv::Point& p1, const cv::Point& p2):
- 这个函数也是计算两个地图坐标之间的距离，但它接受的是 cv::Point 类型的点。它通过调用 GridMap2D::pointDist(p1, p2) 来计算两点之间的欧几里得距离，然后乘以地图的分辨率 m_mapInfo.resolution，将距离从像素单位转换成米。
pointDist(const cv::Point& p1, const cv::Point& p2):
- 这个函数计算两个点之间的欧几里得距离。它首先调用 pointDist2(p1, p2) 来计算点之间的距离平方，然后取平方根得到实际的距离。
pointDist2(const cv::Point& p1, const cv::Point& p2):
- 这个函数计算两个点之间的距离平方，使用的是欧几里得距离公式的平方形式，避免了计算平方根，可以用于性能敏感的场合。
distanceMapAt(double wx, double wy) 和 distanceMapAtCell(unsigned int mx, unsigned int my):
- 这两个函数分别返回世界坐标下（wx, wy）和地图单元格坐标下（mx, my）的距离值（以米为单位）。如果指定的坐标超出地图范围，返回 -1。
binaryMapAt(double wx, double wy) 和 binaryMapAtCell(unsigned int mx, unsigned int my):
- 这两个函数分别在世界坐标和地图单元格坐标下返回地图的值。如果指定的坐标超出地图范围，返回 0。

公式

对于两点之间的距离平方，给定点 p1 和 p2，其公式为：

距离平方 (Dist^2) = (x1 - x2)^2 + (y1 - y2)^2

这里，x1, y1 是点 p1 的坐标，x2, y2 是点 p2 的坐标。
欧几里得距离，即两点之间的直线距离，可以通过取距离平方的平方根来得到：

欧几里得距离 (Dist) = sqrt((x1 - x2)^2 + (y1 - y2)^2)

当我们要将这个距离转换为实际的世界距离（单位为米）时，我们需要将其与地图的分辨率相乘：

世界距离 (World Dist) = Dist × 分辨率

这里的 “分辨率” 表示每个地图单元（或像素）代表的实际距离，从而使我们能够将像素距离转换为实际的物理距离。

在这里插入图片描述
这段代码定义了一个地图处理类，主要用于机器人导航和地图管理。该类提供了从 ROS 的 OccupancyGrid 消息初始化地图、将地图数据转换回 OccupancyGrid 消息、从 OpenCV 的 Mat 对象初始化地图、更新内部距离图、获取地图信息等功能。下面是每个方法和变量的详细解释：

setMap(const nav_msgs::OccupancyGridConstPtr& grid_map, bool unknown_as_obstacle = false):
- 从 ROS 的 OccupancyGrid 消息初始化地图。如果 unknown_as_obstacle 设置为 true，那么未知区域将被视为障碍物。
toOccupancyGridMsg() const:
- 将当前地图数据转换回 ROS 的 nav_msgs::OccupancyGrid 消息格式。
setMap(const cv::Mat& binary_map):
- 从一个 OpenCV 的 Mat 对象初始化地图。这通常用于从二进制（黑白）图像创建地图，mapInfo（尤其是分辨率）保持不变。
updateDistanceMap():
- 重新计算内部的距离图。在手动更改网格地图数据之后需要调用此方法。
getInfo() const:
- 返回地图的元数据（nav_msgs::MapMetaData），包含分辨率、尺寸等信息。
getResolution() const:
- 返回地图的分辨率，即地图上每个像素代表的实际距离。
getFrameID() const:
- 返回地图的 tf 帧 ID（通常是 “/map”），这是地图在 ROS tf（坐标变换系统）中的标识符。
distanceMap() const 和 binaryMap() const:
- 分别返回内部的距离图和二进制图的 cv::Mat 对象。距离图中的每个像素值代表该点到最近障碍物的距离，二进制图中，通常使用特定的值标记自由空间和障碍物。
size() const:
- 返回二进制图像的尺寸，注意这里的 x/y 坐标是与图像的高度/宽度相反的。
FREE 和 OCCUPIED:
- 分别代表自由空间和障碍物的像素值。自由空间为 255（白色），障碍物为 0（黑色）。
- 因为这里表示的一种概率，同时更加容易分析图像