当前位置：首页 > backend >正文

激光雷达点云去畸变

backend 2025/4/30 12:23:48

文章目录

前言
- 什么是点云畸变
- 为什么要做点云去畸变
一、思路与步骤
- 订阅模块
- 处理流程
核心代码

前言

什么是点云畸变

激光雷达是逐点或逐线扫描来生成一帧点云数据的，并非像相机一样瞬时曝光。这意味着：

在一个扫描周期内（比如100ms），激光雷达会旋转扫描，扫描一圈后形成一帧点云，每个点的采集时间都不同。
如果这期间载体发生了平移或旋转，那采集到的点就处在不同的空间参考系。
将这些点统一放在同一个参考系里（比如扫描起始时刻），就会导致点的位置变形、拉伸或错位——这就是点云畸变（Distortion）。

为什么要做点云去畸变

从感知角度讲

Lidar 点云融合：不同雷达采集的数据需统一到同一坐标系
时间戳对齐：尤其在多传感器系统中，硬件同步无法完全解决所有的采样时间差，需通过软件方式进行补偿。
对齐相机时间戳：在多传感器融合（如 Lidar + Camera 前融合）任务中，需将点云校正至图像帧对应的时间点。

一、思路与步骤

订阅模块

ODO消息（里程计）：用于后续对 LiDAR 点云去畸变时提供每个时刻的姿态（平移 & 旋转）。
LiDAR 消息: 支持多个 LiDAR；用于空间信息获取。
Camera 消息:提供目标参考时间戳，作为各类传感器对齐基准。

处理流程

ODO 消息处理
- 接收并缓存每帧里程计数据。
- 用于插值获取某一时间点的车体姿态。
- 后续在点云去畸变中，根据每个点的时间戳查找对应姿态进行插值转换。
LiDAR 消息处理
1. 多雷达时间同步
- 硬件同步。确保雷达硬件时间源一致
- 软件同步。建立 LiDAR 消息缓存队列（按时间戳排序）；检查所有 LiDAR 是否能在目标时间范围内都获取到有效帧。
1. 坐标系统一转换
- 所有 LiDAR 坐标系转换为主雷达坐标系或车体坐标系。
- 输出统一坐标系下的全局点云集合。
Camera 消息处理
- 选取 Camera 消息时间戳为最终融合时间戳。点云去畸变过程将所有点云转换到 camera timestamp 时刻。用于时间基准对齐（前融合）

核心代码

注：在 odom轨迹上根据时间线性插时，如果时间间隔t2 - t1很短会引入较大的误差，从可视化界面看就是点云乱跳。
图片名称

bool undistortPointCloud(std::vector<Point>& cloud,const std::map<double, Eigen::Matrix4d>& odom,double frame_start_time, double frame_end_time)
{Eigen::Matrix4d start_pose;if (!getPoseAtTime(frame_start_time, odom, start_pose)) {cout << "Failed to get start pose" << endl;return false;}Eigen::Matrix4d start_pose_inv = start_pose.inverse();for (auto& pt : cloud) {double point_time = frame_start_time + pt.relative_time * (frame_end_time - frame_start_time);Eigen::Matrix4d point_pose;if (!getPoseAtTime(point_time, odom, point_pose)) {cout << "Failed to get point pose" << endl;continue;}// 计算该点相对于起点的变换Eigen::Matrix4d relative_pose = start_pose_inv * point_pose;// 把点补偿回起始参考系Eigen::Vector4d homo_point(pt.position(0), pt.position(1), pt.position(2), 1.0);Eigen::Vector4d corrected_point = relative_pose * homo_point;// 更新点的位置pt.position = corrected_point.head<3>();}return true;
}