3. 传感器数据同步与融合

任务说明

本案例实现相机图像与激光雷达点云的时间戳对齐与空间融合，生成可用于端到端自动驾驶模型的多模态输入。输入为两路传感器数据流（各含时间戳与数据），输出为对齐后的相机-点云数据对，并将点云投影至图像平面生成稀疏深度图，与RGB图像拼接为多模态张量。

实现方案

时间对齐采用最近邻策略：为每个相机帧寻找时间差最小的激光雷达帧；空间融合利用相机内参与外参将3D点投影到2D像素坐标，生成与图像同尺寸的稀疏深度图。最终输出形状为[H, W, 4]的张量（RGB三通道+深度通道）。

核心代码

import numpy as np
from dataclasses import dataclass

@dataclass
class CameraFrame:
    timestamp: float
    image: np.ndarray  # shape [H, W, 3], uint8

@dataclass
class LidarFrame:
    timestamp: float
    points: np.ndarray  # shape [N, 3], xyz in LiDAR coordinate

class SensorFusion:
    def __init__(self, K: np.ndarray, T_lidar2cam: np.ndarray, img_size: tuple):
        """
        K: 相机内参矩阵 [3,3]
        T_lidar2cam: 激光雷达到相机的变换矩阵 [4,4]
        img_size: (H, W)
        """
        self.K = K
        self.T = T_lidar2cam
        self.H, self.W = img_size

    def align_by_timestamp(self, cam_frames, lidar_frames):
        """为每个相机帧匹配最近时间戳的激光雷达帧"""
        lidar_ts = np.array([f.timestamp for f in lidar_frames])
        aligned_pairs = []
        for cam in cam_frames:
            idx = np.argmin(np.abs(lidar_ts - cam.timestamp))
            aligned_pairs.append((cam, lidar_frames[idx]))
        return aligned_pairs

    def project_points(self, points_lidar):
        """将LiDAR点云投影到图像平面，返回像素坐标与深度"""
        # 齐次坐标变换到相机系
        pts_cam = (self.T @ np.hstack([points_lidar, np.ones((len(points_lidar),1))]).T).T[:, :3]
        # 投影到像素
        pts_img = (self.K @ pts_cam.T).T
        depth = pts_img[:, 2]
        u = pts_img[:, 0] / depth
        v = pts_img[:, 1] / depth
        # 滤除深度非正或超出图像范围的点
        valid = (depth > 0) & (u >= 0) & (u < self.W) & (v >= 0) & (v < self.H)
        return u[valid].astype(int), v[valid].astype(int), depth[valid]

    def generate_sparse_depth(self, points_lidar):
        """生成与图像同尺寸的稀疏深度图"""
        depth_map = np.zeros((self.H, self.W), dtype=np.float32)
        u, v, d = self.project_points(points_lidar)
        depth_map[v, u] = d
        return depth_map

    def fuse(self, cam_frame, lidar_frame):
        """生成多模态张量 [H, W, 4]"""
        depth = self.generate_sparse_depth(lidar_frame.points)
        rgb = cam_frame.image.astype(np.float32) / 255.0
        multimodal = np.concatenate([rgb, depth[:, :, np.newaxis]], axis=-1)
        return multimodal

# ===== 使用示例 =====
if __name__ == "__main__":
    # 模拟数据（仅演示，实际数据由传感器驱动提供）
    H, W = 64, 64
    fx = fy = 50.0
    cx, cy = W/2, H/2
    K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1.0]])
    # 简单外参：LiDAR坐标系绕Z轴旋转90度并平移
    theta = np.pi/2
    R = np.array([[np.cos(theta), -np.sin(theta), 0],
                  [np.sin(theta), np.cos(theta), 0],
                  [0, 0, 1]])
    t = np.array([0.5, 0, 0])
    T = np.eye(4)
    T[:3,:3] = R
    T[:3,3] = t

    fusion = SensorFusion(K, T, (H, W))

    # 构造两帧测试数据
    cam_frames = [CameraFrame(timestamp=1.0, image=np.random.randint(0,255,(H,W,3),dtype=np.uint8)),
                  CameraFrame(timestamp=2.0, image=np.random.randint(0,255,(H,W,3),dtype=np.uint8))]
    lidar_frames = [LidarFrame(timestamp=0.9, points=np.random.randn(100,3)*2),
                    LidarFrame(timestamp=2.1, points=np.random.randn(100,3)*2)]

    pairs = fusion.align_by_timestamp(cam_frames, lidar_frames)
    for cam, lidar in pairs:
        out = fusion.fuse(cam, lidar)
        print(f"对齐时间差: {abs(cam.timestamp - lidar.timestamp):.3f}s, 输出形状: {out.shape}")

落地与验证

运行方式：将代码保存为sensor_fusion.py，执行python sensor_fusion.py。代码使用NumPy，无其他第三方依赖。

验证方法：观察输出张量形状是否为(64, 64, 4)；检查对齐时间差是否小于模拟数据中的最大间隔（本示例中为0.1s）；深度通道中非零像素数量应与投影后落在图像范围内的点云数量一致。

判定标准：程序无异常退出，输出形状正确，且深度通道非零值数量与project_points返回的有效点数量相同，即视为通过。