3D 感知：BEV 鸟瞰图
我们正在构建一个统一系统，使用YOLO进行检测和Open3D进行点云处理，融合3D LiDAR和ZED2立体相机数据。一切通过Pixi管理，使环境在不同系统上可复现。
在第5部分中，我们成功地将LiDAR深度与YOLO语义标签融合。然而，在3D点云中可视化这些结果对人类和下游机器人系统来说都可能难以处理。在本课中，我们构建一个将3D世界"压平"为俯视网格的节点。这使我们能够将AI检测到的"智能聚类"叠加到一个清晰的几何地图上，其中距离和物体大小一目了然。这种BEV表示是自动驾驶系统中的标准视图，用于路径规划和态势感知。
0、学习目标
完成本课后，你将能够：
- 定义BEV几何： 配置世界到像素的参数，包括范围和分辨率（如每像素5厘米）。
- 掌握坐标映射： 实现将LiDAR (x, y)坐标转换为OpenCV (行, 列)图像索引的数学方法。
- 渲染语义叠加： 使用OpenCV绘图函数将3D边界框和AI类别标签投影到俯视图上。
- 实现高效像素着色： 使用NumPy高级索引以最小的CPU开销将数千个LiDAR点渲染到图像画布上。
1、导入和依赖
我们需要用于图像处理和 ROS 2 通信的工具。
import rclpy
from rclpy.node import Node
from sensor_msgs_py import point_cloud2
from sensor_msgs.msg import PointCloud2, Image
from vision_msgs.msg import Detection3DArray
from cv_bridge import CvBridge
import numpy as np
import cv2
- CvBridge： 将ROS图像消息转换为OpenCV兼容数组的关键链接。
- cv2： 在图像上绘制形状和文本的行业标准库。
- sensor_msgs_py： 包含解析原始二进制LiDAR数据的辅助函数。
2、初始化和BEV几何
BEV本质上是一个网格。我们必须定义每个像素代表多少米。
class BEVVisualizerNode(Node):
def __init__(self):
super().__init__('bev_visualizer_node')
# BEV参数
self.declare_parameter('range_x', 10.0) # +/- 10米
self.declare_parameter('range_y', 10.0) # +/- 10米
self.declare_parameter('resolution', 0.05) # 5厘米/像素
self.range_x = self.get_parameter('range_x').value
self.range_y = self.get_parameter('range_y').value
self.resolution = self.get_parameter('resolution').value
self.width = int(2 * self.range_y / self.resolution)
self.height = int(2 * self.range_x / self.resolution)
- 分辨率： 0.05时，每个像素代表世界中5x5厘米的方块。
- 宽/高： 我们根据范围和分辨率计算图像大小。±10米范围、5厘米分辨率创建400x400像素的图像。
处理150米范围的高分辨率图像需要巨大的内存缓冲区并减慢LiDAR回调速度。通过限制在±10米、5厘米分辨率，我们将机器人的注意力集中在关键的即时障碍物上，同时保持较高的像素密度。
3、订阅者和同步
我们监听原始激光雷达信号以获取背景信息，监听融合后的检测结果以获取标签信息。
self.bridge = CvBridge()
# 订阅
self.lidar_sub = self.create_subscription(PointCloud2, '/rslidar_points', self.lidar_callback, 10)
self.fused_sub = self.create_subscription(Detection3DArray, '/detection/fused_3d', self.fused_callback, 10)
self.latest_fused = None
# 发布者
self.bev_pub = self.create_publisher(Image, '/visualization/bev_image', 10)
self.get_logger().info(f'BEV可视化节点已启动。图像大小: {self.width}x{self.height}')
def fused_callback(self, msg):
self.latest_fused = msg
- 订阅：
/rslidar_points(PointCloud2) — 接收用于灰色"地图"背景的原始3D数据。 - 订阅：
/detection/fused_3d(Detection3DArray) — 接收带有3D位置和AI标签的智能聚类。 - 发布者：
/visualization/bev_image(Image) — 输出最终图像，供RViz或Web仪表板使用。
第4部分：将LiDAR映射到像素
数学方法将现实世界坐标转换为图像索引。
def lidar_callback(self, msg):
# 1. 将PointCloud2转换为numpy
points = self.pointcloud2_to_numpy(msg)
if points.size == 0:
return
# 2. 按范围过滤点
mask = (points[:, 0] > -self.range_x) & (points[:, 0] < self.range_x) & \
(points[:, 1] > -self.range_y) & (points[:, 1] < self.range_y)
points = points[mask]
# 3. 投影到像素
# OpenCV图像坐标：(行=y_pixel, 列=x_pixel)
x_pix = ((self.range_y - points[:, 1]) / self.resolution).astype(np.int32)
y_pix = ((self.range_x - points[:, 0]) / self.resolution).astype(np.int32)
将点云转换为NumPy数组后，我们裁剪掉BEV区域外的数据。使用NumPy的布尔索引，我们创建一个检查每个点是否在边界参数内的掩码：
- X轴过滤： 仅保留前后距离在±range_x内的点。
- Y轴过滤： 仅保留左右距离在±range_y内的点。
为了将3D LiDAR点转换为2D图像坐标，我们应用简单的线性正交投影。这种俯视映射忽略Z（高度）值，只关注XY地平面。将以米为单位的点坐标 (x, y) 转换为像素坐标 (u, v)。这需要两个步骤：平移（将原点移动到图像中心）和缩放（使用分辨率将米转换为像素）。
代码中实现的公式如下：
- 原点偏移：在激光雷达图像中，(0,0,0) 是传感器中心。在 OpenCV 图像中，(0,0) 是左上角。从最大测距中减去激光雷达坐标 (range_y - y) 可以有效地移动我们的“相机”，使机器人位于图像中心。
- 方向：在 ROS 2 的 X 轴正向坐标系中，X 轴正方向指向机器人前方。通过将 X_lidar 映射到图像的 v（垂直/行）轴，Y_lidar 映射到 u（水平/列）轴，我们可以确保 BEV 图像以正确的方向显示。
5、绘制BEV地图
我们创建一个空白画布，然后将这些点“放置”到画布上。
# 4. 绘制BEV
# 创建黑色图像
bev_img = np.zeros((self.height, self.width, 3), dtype=np.uint8)
# 将LiDAR点绘制为灰色像素
valid_mask = (x_pix >= 0) & (x_pix < self.width) & (y_pix >= 0) & (y_pix < self.height)
bev_img[y_pix[valid_mask], x_pix[valid_mask]] = [200, 200, 200]
这使用NumPy的高级索引同时为数千个像素着色，使可视化非常轻量。
6、绘制语义检测结果
最后，我们将来自 Fusion 节点的 3D 盒子叠加起来，并发布 BEV 图像。
# 5. 绘制融合检测结果
if self.latest_fused:
for det in self.latest_fused.detections:
cx = det.bbox.center.position.x
cy = det.bbox.center.position.y
sx = det.bbox.size.x
sy = det.bbox.size.y
# 将中心点转换为像素
px = int((self.range_y - cy) / self.resolution)
py = int((self.range_x - cx) / self.resolution)
# 将大小转换为像素
pw = int(sy / self.resolution)
ph = int(sx / self.resolution)
# 绘制矩形
cv2.rectangle(bev_img, (px - pw//2, py - ph//2), (px + pw//2, py + ph//2), (0, 0, 255), 2)
# 绘制标签
if det.results:
cls_id = det.results[1].hypothesis.class_id
cv2.putText(bev_img, f"ID:{cls_id}", (px - pw//2, py - ph//2 - 5),
cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)
# 6. 发布BEV图像
bev_msg = self.bridge.cv2_to_imgmsg(bev_img, encoding='bgr8')
bev_msg.header = msg.header
self.bev_pub.publish(bev_msg)
在渲染出 LiDAR 点阵的背景“地图”后，我们将从融合节点收集的高级信息叠加到其上。此步骤通过绘制边界框将简单的占用网格转换为语义地图，这些边界框代表检测到的物体（例如车辆或行人）的物理轮廓。
该过程需要两个阶段的转换：定位和缩放。
- 定位：我们获取融合检测的 3D 质心 (x, y)，并使用与原始点阵相同的正交投影公式将其映射到单个像素 (px, py)。
- 缩放：我们获取边界框的物理尺寸——以米为单位的宽度 (sx) 和长度 (sy)——并将它们除以我们的分辨率（例如，0.05 米/像素），以确定矩形的像素大小。
最后，我们使用 OpenCV 的 cv2.rectangle 和 cv2.putText 函数将视觉标识添加到图像上。正是这种合并后的数据使得自主系统能够预测行为——知道“汽车”集群可能会向前移动，而“交通标志”集群将保持静止。
7、辅助函数
接下来，我们有一个非常重要的辅助函数。
def pointcloud2_to_numpy(self, msg):
gen = point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True)
dtype = [('x', np.float32), ('y', np.float32), ('z', np.float32)]
structured_array = np.fromiter(gen, dtype=dtype)
if structured_array.size > 0:
points = structured_array.view(np.float32).reshape(-1, 3)
points = points[np.isfinite(points).all(axis=1)]
return points
return np.zeros((0, 3), dtype=np.float32)
这个高性能转换器将原始二进制LiDAR数据流式传输到结构化的NumPy数组中，同时去除无效坐标。
8、入口点（main）
最后，是启动节点的标准“样板”代码。
def main(args=None):
rclpy.init(args=args)
node = BEVVisualizerNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
rclpy.spin(node) 是一个阻塞调用，使脚本保持活动状态。它等待新的激光雷达数据到达订阅并立即触发listener_callback以再次开始处理周期。
在过去的五篇文章中，我们从头开始构建了一个完整的模块化 3D 感知管道。我们首先处理原始 LiDAR 点云，使用 DBSCAN 聚类将数千个单独的点分组为不同的物理对象。然后，我们集成了基于 YOLO 的 2D 视觉堆栈来提供语义智能，随后开发了 Frustum Fusion 节点，在数学上弥合了这两个传感器之间的差距。通过将 3D 簇投影到 2D 图像平面上，我们成功地与人工智能驱动的分类“握手”空间深度，创建了知道它们在哪里以及它们是什么的“智能簇”。
鸟瞰图 (BEV) 创建的最后一步是从原始感知到可操作情报的关键转变。通过将环境扁平化为自上而下的几何地图，我们提供了统一的表示，这对于路径规划和避障等下游自主任务至关重要。在 BEV 空间中，规划人员可以轻松计算安全轨迹、确定碰撞时间并管理复杂的操作，因为每个“智能集群”的规模和距离都以干净的基于网格的格式保存。您已经从处理原始字节转向生成高级环境地图——这是任何自动驾驶系统的核心要求。
汇智网翻译整理，转载请标明出处