4. 数据预处理：时间戳同步、坐标变换、去畸变、点云滤波与降采样
数据预处理，说白了就是给原始数据「洗澡」。你采集回来的数据，时间对不上、坐标不统一、图像有畸变、点云全是噪点…… 直接喂给算法，结果肯定一塌糊涂。我刚开始做自动驾驶时，就吃过这个亏——数据没对齐，回放出来的轨迹直接飞到了马路牙子上。所以这一章，咱们把预处理这五个硬骨头啃下来。
4.1 时间戳同步——让所有传感器「说同一种时间」
多传感器融合的第一步，就是时间对齐。相机、激光雷达、IMU、GPS，每个设备都有自己的时钟，漂移个几毫秒，在高速场景下就是几米的误差。我个人习惯用 硬件同步 + 软件插值 双保险。
- 硬件同步：通过PPS信号或IEEE 1588协议，让所有传感器共享同一个时钟源。这是最可靠的方式，但需要硬件支持。
- 软件插值：当硬件同步不可用时，根据时间戳线性插值。比如激光雷达10Hz，相机20Hz，把相机帧插到激光雷达的时间轴上。
💡 我的经验： 我在项目中遇到过GPS时间戳跳变的情况，原因是GPS信号短暂丢失后恢复时，时间戳回跳了100ms。后来我加了异常检测——如果相邻时间戳差超过阈值，直接丢弃该帧，避免污染整个序列。
# 伪代码：基于最近邻的时间戳匹配
def sync_sensors(lidar_ts, camera_ts):
matched = []
for lt in lidar_ts:
# 找到与lt最接近的相机时间戳
idx = np.argmin(np.abs(camera_ts - lt))
if abs(camera_ts[idx] - lt) < 5e-3: # 5ms阈值
matched.append((lt, camera_ts[idx]))
return matched4.2 坐标变换——把数据塞进同一个「世界坐标系」
每个传感器都有自己的坐标系。激光雷达是三维直角坐标，相机是像素坐标，IMU是机体坐标系。你想想看，如果不做变换，算法根本不知道「前方障碍物」在哪个位置。
我一般维护一个 坐标变换树（TF树），把所有传感器都挂到车体坐标系下，再统一转到世界坐标系。核心就是四个变换：
- 传感器 → 车体：外参标定得到的旋转平移矩阵。
- 车体 → 世界：根据GPS/IMU推算的位姿。
- 像素 ↔ 相机坐标系：内参矩阵 + 畸变参数。
- 时间同步后的动态变换：考虑车辆运动，对每个点做运动补偿。
⚠️ 注意：坐标变换一定要用齐次坐标（4x4矩阵），否则平移和旋转混在一起容易出错。我曾经因为忘记转齐次坐标，调试了整整两天……
# 使用齐次矩阵进行坐标变换
def transform_point(point, tf_matrix):
# point: [x, y, z, 1]
return tf_matrix @ point4.3 去畸变——让图像和点云「恢复真实形状」
相机镜头有径向畸变和切向畸变，激光雷达在运动过程中也会产生点云畸变（因为扫描一圈需要时间，车在动）。去畸变就是把这些扭曲拉回来。
- 图像去畸变：用标定得到的内参和畸变系数，对每个像素重新映射。OpenCV的
cv2.undistort()一步搞定。 - 点云去畸变：利用IMU或里程计数据，把每个点的时间戳对应的位姿算出来，然后补偿到起始时刻。说白了就是「运动补偿」。
⚠️ 避坑指南： 我曾经在点云去畸变时，直接用线性插值代替IMU数据，结果弯道场景下点云出现了「拖尾」现象。后来老老实实用了高频IMU，效果才正常。记住：运动补偿的精度直接取决于位姿插值的精度。
4.4 点云滤波——把「脏数据」筛出去
激光雷达点云里有很多噪点：地面反射、雨雾、传感器本身的异常点。不滤波的话，后续的障碍物检测会多出无数假目标。我常用的滤波器有：
| 滤波器 | 作用 | 适用场景 |
|---|---|---|
| 直通滤波 | 保留指定范围（如Z轴 -1.5~3m） | 去除地面和过高物体 |
| 体素滤波 | 用体素网格降采样 + 去噪 | 均匀化点云密度 |
| 统计滤波 | 去除离群点（基于邻域距离统计） | 去除孤立噪点 |
| 半径滤波 | 如果某点周围点数少于阈值则剔除 | 去除稀疏噪点 |
我个人习惯先做直通滤波，把明显不相关的区域切掉，再用统计滤波去除离群点。这样计算量小，效果也干净。
4.5 降采样——给点云「瘦身」
64线激光雷达一帧可能有几十万个点，直接处理太慢了。降采样不是随便丢点，而是要在保持几何结构的前提下减少点数。最常用的是 体素降采样：把空间分成小立方体，每个立方体只保留一个重心点。
import open3d as o3d
pcd = o3d.io.read_point_cloud("frame.pcd")
# 体素降采样，体素边长0.1m
downsampled = pcd.voxel_down_sample(voxel_size=0.1)
print(f"原始点数: {len(pcd.points)}, 降采样后: {len(downsampled.points)}")
💡 小技巧： 体素大小不是固定的。近处场景用0.05m，远处用0.2m，既能保证近处精度，又能减少远处冗余点。我在高速场景下常用自适应体素，效果比固定体素好不少。
好了，数据预处理这五步走完，你的数据就「干净」了。下一阶段就可以放心地喂给真值系统或者回放引擎。嗯，这里要再啰嗦一句：预处理的质量直接决定闭环验证的可信度，千万别图省事跳过某一步。