企业官网建设流程全解析

阶段五：地图转换（G2P5）

现在学习一个主要模块——G2P5！这是将 3D 点云地图转换为 2.5D 栅格地图，方便与 ROS 导航栈集成！

一、G2P5 是什么？

G2P5 = 3D (Grid) → 2.5D (2 and a half)

核心思想：

• 从雷达中心出发，打射线到障碍物
• 计算射线沿途的栅格状态（占用/空闲）
• 只关注一定高度范围内的障碍物（地面以上 0.5~1.2 米）

二、核心成员变量

看 g2p5.h 的 Options ：

三、核心功能流程

看 g2p5.cc
3.1 核心流程概览

输入：关键帧（位姿 + 点云） ↓ 1. 检测地面（可选） ↓ 2. 3D → 2.5D 转换 a. 按角度（360度）组织点云 b. 每个方向取最近的有效障碍物 ↓ 3. 射线追踪（SetWhitePoints） a. 从雷达中心到障碍物画直线 b. 沿途栅格设为空闲（白色） c. 障碍物栅格设为占用（黑色） ↓ 4. 更新地图 ↓ （可选）回环触发重绘

四、核心函数详解

4.1 Convert3DTo2DScan() - 3D 转 2.5D 核心

步骤 1：组织点云（按 360 度）

std::vector<std::map<double,double>>rays(360);// rays[角度] = 按距离排序的 (距离, 高度)

步骤 2：筛选有效障碍物

for(每个点){计算距离地面高度 dis_floor;if(dis_floor 在(min_th,max_th)之间){按角度存入 rays;设置黑点（障碍物）：map->SetHitPoint();}}

步骤 3：每个方向取最近有效点

for(每个角度0~359){从远到近遍历该方向的点； 找第一个高度>min_th 的点； 记录(距离,高度);}

4.2 SetWhitePoints() - 射线追踪

输入：每个角度的 (距离, 高度)

for(每个角度0~359){计算2D 点：(r*cosθ,r*sinθ);转到世界坐标系； 调用 map->SetMissPoint();// 射线追踪}

SetMissPoint() 做的事：

• 从雷达原点画直线到障碍物点
• 直线经过的所有栅格设为 空闲 （白色）
• 障碍物点所在栅格设为 占用 （黑色）

4.3 前后端双线程渲染

好处：

• 实时性好（前端先更新）
• 回环后地图全局修正（后端重绘）

五、核心数据结构：G2P5Map

看 g2p5_map.h

特性：

• 分层存储（主网格 + 子网格 SubGrid）
• 支持导出为 OpenCV Mat（ToCV）
• 支持导出为 ROS OccupancyGrid（ToROS）

ToROS() 函数 ：

nav_msgs::msg::OccupancyGridToROS();

这个用于与 ROS 导航栈集成！

六、G2P5 的优化点

阶段五总结

G2P5 模块学完了！核心是：

1. 射线追踪 ：从雷达中心到障碍物画线
2. 前后端双线程 ：实时+回环重绘
3. 导出 ROS 格式 ：方便与导航集成