激光点云去畸变:原理及实现

机械式激光雷达产生畸变的原因

Lidar扫描周期内（一般0.1s）自车有一定幅度的旋转(Rotation)和平移(Translation)，因此不同时间点打出去的激光点束并不在严格统一的Lidar坐标系内，需要对同一帧Lidar转化在统一时间戳对应的Lidar坐标系

上(一般转化到第一束激光点对应的自车坐标系上，或者扫描周期的中心时刻)。

附赠自动驾驶最全的学习资料和量产经验：链接

输入的传感器和参数

传感器主要使用IMU和INS, 两者分别提供的参数和频率如下

IMU 惯性测量单元 ： 提供自车xyz加速度和角速度 /100HZ

GPS/INS 惯导测量系统 : 提供自车分别相对于全局坐标系的欧拉角 /100HZ

两个message结构如下：

IMU Message

<code># Header

std_msgs/Header header

# Sensor status

string status

uint8 quality

# Sensor quality enum

uint8 INVALID=0

uint8 DOWNGRADE=1

uint8 GOOD=2

# Linear acceleration in vehicle reference frame (m/s^2)

# Y for forward, X for right, Z for up

geometry_msgs/Vector3 accel

# Angular velocity in vehicle reference frame (deg/s)

# Y across forward axis, X across right axis, Z across up axis

geometry_msgs/Vector3 gyro

INS Message

# Header

std_msgs/Header header

# Sensor status

string status

uint8 quality

# Sensor quality enum

uint8 INVALID=0

uint8 DOWNGRADE=1

uint8 GOOD=2

# Position (WGS84)

float64 longitude

float64 latitude

float64 altitude

# Position standard deviation in degrees

float64 longitude_std_dev

float64 latitude_std_dev

float64 altitude_std_dev

# Velocity (m/s)

# X for east, Y for north, Z for up

geometry_msgs/Vector3 velocity

# Velocity standard deviation in meter per second

geometry_msgs/Vector3 velocity_std_dev

# Attitude (degrees)

# Right-handed rotation from local level around y-axis

float64 roll

# Right-handed rotation from local level around x-axis

float64 pitch

# Right-handed rotation from local level around z-axis

float64 yaw

# Attitude standard deviation in degrees

float64 roll_std_dev

float64 pitch_std_dev

float64 yaw_std_dev

其中ins提供xyz方向的速度, 但是考虑到ins与imu位置并不严格一致，这里没有直接使用ins提供的速度，而是使用imu提供加速度积分求的速度。

于是从ins中获取imu坐标系的三个方向的欧拉角(x-roll, y-pitch, z-yaw, 简称rpy角), 从imu传感器获取imu在xyz三个方向的加速度和角速度。

注意： 这里的roll-pitch-yaw 严格相对于imu坐标系的三个方向xyz而言，imu坐标系的方向与我们的习惯方向不一致(见下图), 因此’roll’和’pitch’并不符合我们习惯的’翻滚’和’点头’的直觉，而是刚好反过来，但这并不影响放射变换的计算。

在纠正之前需要作如下步骤：

Lidar→Imu : 首先，点云数据一定是基于Lidar坐标系(见下图)，而传感器是从imu坐标系获取的，因此需要首先根据给定的标定矩阵将点云投射到imu坐标系上。

Imu→ Global : Imu坐标系和Lidar坐标系一样都是随车体运动的，因此需要根据传感器数据，计算imu坐标系(一般将imu坐标系看作自车坐标系)本身相对于全局坐标系的位姿。

数值计算 ： 根据ins传感器提供的rpy三个欧拉角，获取imu坐标系的欧拉旋转矩阵； 根据Imu传感器获取加速度(acc)，注意这个加速度是imu坐标系上xyz方向的加速度，需要使用第二步得到的旋转矩阵计算_imu坐标系本身_ 相对于全局坐标系的加速度。

积分： 根据3获取的acc, 分别作一次积分和二次积分获取前后imu数据注册时间段内imu坐标系相对于全局坐标系的位移。

欧拉角→ 旋转矩阵 ：

对应代码摘抄如下：

<code>Eigen::AngleAxisf t_Vz(rot_xyz(2)/180 * pi_, Eigen::Vector3f::UnitZ());

Eigen::AngleAxisf t_Vy(rot_xyz(1)/180 * pi_, Eigen::Vector3f::UnitY());

Eigen::AngleAxisf t_Vx(rot_xyz(0)/180 * pi_, Eigen::Vector3f::UnitX());

Eigen::AngleAxisf t_V;

t_V = t_Vz * t_Vy * t_Vx;

Eigen::Matrix3f rot = t_V.matrix();

acc = rot * acc;

acc.z() -= 9.80665 ;

注意imu在全局坐标系下z方向总是有向上加速度为g的速度分量(因为重力的原因)，但这个acc并没有贡献任何位移，因此需要减去这个重力加速度。

acc→ 全局位移 ：

imu_shift_x_[imu_ptr_last_] =

imu_shift_x_[imu_ptr_back] +imu_velo_x_[imu_ptr_back] * time_diff + acc(0) * time_diff * time_diff * 0.5;

imu_shift_y_[imu_ptr_last_] =

imu_shift_y_[imu_ptr_back] + imu_velo_y_[imu_ptr_back] * time_diff + acc(1) * time_diff * time_diff * 0.5;

imu_shift_z_[imu_ptr_last_] =

imu_shift_z_[imu_ptr_back] + imu_velo_z_[imu_ptr_back] * time_diff + acc(2) * time_diff * time_diff * 0.5;

imu_velo_x_[imu_ptr_last_] = imu_velo_x_[imu_ptr_back] + acc(0) * time_diff;

imu_velo_y_[imu_ptr_last_] = imu_velo_y_[imu_ptr_back] + acc(1) * time_diff;

imu_velo_z_[imu_ptr_last_] = imu_velo_z_[imu_ptr_back] + acc(2) * time_diff;

去畸变与运动补偿

时间同步与插值运算 ： 对于点云中任意一点(自带时间戳)，遍历队列中imu数据集合，直到当前点时间戳处于两个Imu时间戳之间，根据线性插值法计算当前点对应的imu位姿：

int imu_ptr_back = (imu_ptr_cur_ - 1 + imu_que_length_) % imu_que_length_;

float ratio_front_a = (scan_time + rel_time - imu_time_[imu_ptr_back]) * 1.0 ;

float ratio_front_b = (imu_time_[imu_ptr_cur_] - imu_time_[imu_ptr_back]) * 1.0;

float ratio_front = ratio_front_a / ratio_front_b;

float ratio_back = 1.0 - ratio_front;

rpy_cur(0) = imu_roll_[imu_ptr_cur_] * ratio_front + imu_roll_[imu_ptr_back] * ratio_back;

rpy_cur(1) = imu_pitch_[imu_ptr_cur_] * ratio_front + imu_pitch_[imu_ptr_back] * ratio_back;

rpy_cur(2) = imu_yaw_[imu_ptr_cur_] * ratio_front + imu_yaw_[imu_ptr_back] * ratio_back;

shift_cur(0) = imu_shift_x_[imu_ptr_cur_] * ratio_front + imu_shift_x_[imu_ptr_back] * ratio_back;

shift_cur(1) = imu_shift_y_[imu_ptr_cur_] * ratio_front + imu_shift_y_[imu_ptr_back] * ratio_back;

shift_cur(2) = imu_shift_z_[imu_ptr_cur_] * ratio_front + imu_shift_z_[imu_ptr_back] * ratio_back;

根据当前欧拉角计算旋转矩阵：

r_c = (

Eigen::AngleAxisf(rpy_cur(2) / 180.0 * pi_, Eigen::Vector3f::UnitZ()) *

Eigen::AngleAxisf(rpy_cur(1) / 180.0 * pi_, Eigen::Vector3f::UnitY()) *

Eigen::AngleAxisf(rpy_cur(0) / 180.0 * pi_, Eigen::Vector3f::UnitX())

).toRotationMatrix();

注意第一个点默认为最早打出去的一个点，我们将其他点转移到初始坐标系中，初始点状态记录为rt_start:

rpy_start = rpy_cur;

shift_start = shift_cur;

velo_start = velo_cur;

rt_start.translate(shift_start);

rt_start.rotate(r_c);

根据上述计算得到的当前点状态，计算当前点在全局坐标系下的状态(imu i->global i)，然后再根据初始点状态，反向计算当前点全局状态相对于初始点的状态(global i->imu0):

adjusted_p = rt_start.inverse() * (r_c * Eigen::Vector3f(p.x, p.y, p.z) + shift_cur );

p.x = adjusted_p.x();

p.y = adjusted_p.y();

p.z = adjusted_p.z();

坐标转换——纠正之后

最后需要将纠正之后的点云统一转移到Lidar坐标系上，至此，点云去畸变流程计算完毕。

<code>pcl::transformPointCloud(*point_cloud_ptr, *point_cloud_ptr, TransInsLidar.inverse());

纠正效果如下:

将相同id两帧点云投射到通一张图上，红色代表去畸变之前的点，蓝色代表去畸变之后的点，黄色箭头代表lidar旋转的方向，

黑箭头代表第一束和最后一束激光点重合的方向，这个方向积累了0.1秒的畸变，所以断层会沿着这个方向出现。

加速的场景如下(黄线代表lidar转动方向，顺时针)：

旋转的场景如下(lidar顺时针旋转的时候，原点云会应对与lidar做逆时针的摆动)：

路崖子断层 ：

运动中的目标车辆断层：

原目标车辆(带强度)

去畸变之后的效果(带强度)：

墙体的去畸变效果