激光点云去畸变:原理及实现
机械式激光雷达产生畸变的原因
Lidar扫描周期内（一般0.1s）自车有一定幅度的旋转(Rotation)和平移(Translation)，因此不同时间点打出去的激光点束并不在严格统一的Lidar坐标系内，需要对同一帧Lidar转化在统一时间戳对应的Lidar坐标系
上(一般转化到第一束激光点对应的自车坐标系上，或者扫描周期的中心时刻)。
附赠自动驾驶最全的学习资料和量产经验：链接
输入的传感器和参数
传感器主要使用IMU和INS, 两者分别提供的参数和频率如下
IMU 惯性测量单元 ： 提供自车xyz加速度和角速度 /100HZ
GPS/INS 惯导测量系统 : 提供自车分别相对于全局坐标系的欧拉角 /100HZ
两个message结构如下：
IMU Message
# 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坐标系相对于全局坐标系的位移。
欧拉角→ 旋转矩阵 ：
对应代码摘抄如下：
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坐标系上，至此，点云去畸变流程计算完毕。
pcl::transformPointCloud(*point_cloud_ptr, *point_cloud_ptr, TransInsLidar.inverse());
纠正效果如下:
将相同id两帧点云投射到通一张图上，红色代表去畸变之前的点，蓝色代表去畸变之后的点，黄色箭头代表lidar旋转的方向，
黑箭头代表第一束和最后一束激光点重合的方向，这个方向积累了0.1秒的畸变，所以断层会沿着这个方向出现。
加速的场景如下(黄线代表lidar转动方向，顺时针)：
旋转的场景如下(lidar顺时针旋转的时候，原点云会应对与lidar做逆时针的摆动)：
路崖子断层 ：
运动中的目标车辆断层：
原目标车辆(带强度)
去畸变之后的效果(带强度)：
墙体的去畸变效果
