ROS 2多传感器标定系统开发与实践

1. 多传感器标定系统概述

在机器人感知系统中，多传感器标定是确保不同传感器数据能够准确融合的关键步骤。本文将详细介绍基于ROS 2环境开发的多传感器标定可视化交互系统，该系统主要用于解决激光雷达(LiDAR)与相机、以及双激光雷达之间的外参标定问题。

1.1 系统核心功能

该系统提供了完整的标定工作流程，主要包含以下核心功能：

• 点云投影：将3D激光雷达点云投影到2D图像平面
• 对应点选取：支持手动选取2D图像与3D点云之间的对应点
• 标定算法：集成PNP、RANSAC和最小二乘法(LSQ)等多种标定算法
• 位姿调整：提供平移和欧拉角的手动微调功能
• 可视化配置：可调整点大小、色彩映射和图像校正参数
• 结果导出：支持将标定结果以YAML格式导出

1.2 技术架构

系统采用PySide作为GUI开发框架，结合Open3D进行点云处理和可视化，主要技术组件包括：

• ROS 2：作为底层通信框架
• Open3D：用于点云处理和3D可视化
• PySide：构建交互式用户界面
• NumPy：进行矩阵运算和数值计算

系统设计遵循模块化原则，将不同功能封装为独立的组件，便于维护和扩展。

2. 单激光雷达标定实现

2.1 核心类初始化

标定系统的核心是CalibrationWidget类，其初始化过程负责建立标定的基础环境：

python复制def __init__(
    self,
    pointcloud_msg,  # 源LiDAR点云的ROS消息
    pointcloud2_msg, # 目标LiDAR点云的ROS消息
    initial_transform=None,  # 初始变换矩阵（可选）
    completion_callback: Callable = None,  # 标定完成后的回调函数
):
    # 保存源和目标点云的ROS消息
    self.source_pointcloud_msg = pointcloud_msg
    self.target_pointcloud_msg = pointcloud2_msg
    
    # 初始化变换矩阵
    self.initial_transform = initial_transform if initial_transform is not None else np.eye(4)
    self.current_transform = self.initial_transform.copy()
    
    # 将ROS点云消息转换为Open3D点云对象
    self.source_cloud = self.ros_to_open3d(pointcloud_msg)
    self.target_cloud = self.ros_to_open3d(pointcloud2_msg)
    
    # 创建目标点云的变换副本
    self.target_cloud_transformed = copy.deepcopy(self.target_cloud)
    self.target_cloud_transformed.transform(self.current_transform)
    
    # 更新点云颜色并初始化GUI
    self.update_point_colors()
    self.setup_ui()

初始化过程中有几个关键点需要注意：

1. 变换矩阵处理：如果没有提供初始变换矩阵，则使用4×4单位矩阵
2. 点云转换：将ROS格式的点云转换为Open3D可处理的格式
3. 点云副本：创建目标点云的深拷贝，避免修改原始数据

2.2 ROS点云格式转换

系统需要处理ROS的PointCloud2消息格式，将其转换为Open3D的点云对象：

python复制def ros_to_open3d(self, pointcloud_msg):
    """将ROS PointCloud2消息转换为Open3D点云对象"""
    points = []  # 存储提取的有效点坐标

    # 解析点云数据格式
    point_step = pointcloud_msg.point_step  # 每个点占用的字节数
    data = pointcloud_msg.data  # 点云的原始字节数据

    # 获取x/y/z字段的偏移量
    x_offset = y_offset = z_offset = None
    for field in pointcloud_msg.fields:
        if field.name == "x":
            x_offset = field.offset
        elif field.name == "y":
            y_offset = field.offset
        elif field.name == "z":
            z_offset = field.offset

    # 遍历所有点数据
    for i in range(0, len(data), point_step):
        if i + point_step <= len(data):
            # 提取x/y/z坐标
            x = struct.unpack_from("f", data, i + x_offset)[0]
            y = struct.unpack_from("f", data, i + y_offset)[0]
            z = struct.unpack_from("f", data, i + z_offset)[0]

            # 过滤无效点
            if not (np.isfinite(x) and np.isfinite(y) and np.isfinite(z)):
                continue

            points.append([x, y, z])

    # 创建Open3D点云对象
    cloud = o3d.geometry.PointCloud()
    cloud.points = o3d.utility.Vector3dVector(np.array(points))
    return cloud

这个转换过程有几个需要注意的技术细节：

1. 字节解析：需要准确获取每个点的x/y/z坐标在数据中的偏移位置
2. 无效点过滤：排除包含NaN或无穷大的无效点
3. 内存效率：对于大规模点云，需要考虑内存使用效率

2.3 GUI界面搭建

系统使用Open3D的GUI框架构建用户界面：

python复制def setup_ui(self):
    """搭建Open3D GUI界面"""
    # 初始化GUI应用
    gui.Application.instance.initialize()

    # 创建主窗口
    self.window = gui.Application.instance.create_window(
        "LiDAR-to-LiDAR Calibration", 1400, 900
    )

    # 配置界面样式
    em = self.window.theme.font_size
    margin = gui.Margins(0.5 * em, 0.5 * em, 0.5 * em, 0.5 * em)
    separation_height = int(round(0.5 * em))

    # 创建3D场景
    self._scene = gui.SceneWidget()
    self._scene.scene = rendering.Open3DScene(self.window.renderer)
    self._scene.scene.set_background([0.1, 0.1, 0.1, 1.0])

    # 创建控制面板
    self._settings_panel = gui.Vert(0, margin)