【10】lightning_lm项目-阶段5-地图转换

文章目录阶段五地图转换G2P5一、G2P5 是什么二、核心成员变量三、核心功能流程四、核心函数详解五、核心数据结构G2P5Map六、G2P5 的优化点阶段五总结阶段五地图转换G2P5现在学习一个主要模块——G2P5这是将 3D 点云地图转换为 2.5D 栅格地图方便与 ROS 导航栈集成一、G2P5 是什么G2P5 3D (Grid) → 2.5D (2 and a half)核心思想从雷达中心出发打射线到障碍物计算射线沿途的栅格状态占用/空闲只关注一定高度范围内的障碍物地面以上 0.5~1.2 米二、核心成员变量看 g2p5.h 的 Options 三、核心功能流程看 g2p5.cc3.1 核心流程概览输入关键帧位姿 点云 ↓ 1. 检测地面可选 ↓ 2. 3D → 2.5D 转换 a. 按角度360度组织点云 b. 每个方向取最近的有效障碍物 ↓ 3. 射线追踪SetWhitePoints a. 从雷达中心到障碍物画直线 b. 沿途栅格设为空闲白色 c. 障碍物栅格设为占用黑色 ↓ 4. 更新地图 ↓ 可选回环触发重绘四、核心函数详解4.1 Convert3DTo2DScan() - 3D 转 2.5D 核心步骤 1组织点云按 360 度std::vectorstd::mapdouble,doublerays(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 MatToCV支持导出为 ROS OccupancyGridToROSToROS() 函数 nav_msgs::msg::OccupancyGridToROS();这个用于与 ROS 导航栈集成六、G2P5 的优化点阶段五总结G2P5 模块学完了核心是射线追踪 从雷达中心到障碍物画线前后端双线程 实时回环重绘导出 ROS 格式 方便与导航集成