激光雷达+imu_激光雷达slam-激光点云畸变补偿

本文都是对下述链接的学习以及对其代码中一些部分的理解.

任乾：从零开始做自动驾驶定位(八): 点云畸变补偿​zhuanlan.zhihu.com

点云运动畸变补偿分为以下几步:

1. 计算相对坐标

1)获得激光雷达的运动信息

由于我们得到的运动信息(速度

和角速度

)是IMU的速度

和角速度

,所以需要用lidar2imu的状态转移矩阵将

和

转换为

和

.此时由于lidar和IMU的关系存在刚体旋转的关系,所以不能直接用左乘lidar2imu的方式直接转换,需要参考哥式定理:

掀桌喵：哥氏定理与科里奥利力​zhuanlan.zhihu.com

之后就可以得到

和

.

此处解释了current_velocity_data_.TransformCoordinate(lidar_to_imu_)中的代码.

2)计算激光点相对于一圈儿的起始时刻的时间差

由于是顺序扫描，我们通过atan2(y, x)来计算出该激光点相对于第一个激光点旋转过的角度

，再结合雷达内部旋转一圈儿用了100ms，就可以求出旋转

角度用的时间

。

3)计算各个激光点的相对运动

由于激光雷达旋转一圈的时间极短,所以可以认为车辆在这期间的运动为匀速匀角速度运动.所以各个激光点的相对运动可以用激光雷达的

和

乘以

来计算.

2.转换激光点

用上述计算出的相对运动,组成转换矩阵,左乘激光雷达的坐标点即可.

这里有一个问题: 如车子在激光雷达旋转一圈的时间中,顺时针旋转了角度

(记为正值),那么为什么激光雷达的点也会乘以一个正值(意思是也得顺时针旋转来补偿).

如下图所示

理论上激光雷达顺时针旋转后的点云形状

start点为起始点,end为终止点,在车辆顺时针旋转之后,end点由于该旋转,理论上的角度应该是

(因为这个点位于起始时刻的右边),但是由于他是激光雷达的终止点,所以在激光雷达中,他的角度还是0°,所以需要对其进行顺时针旋转的补偿.

此处解释了distortion_adjust_ptr_->AdjustCloud(current_cloud_data_.cloud_ptr, current_cloud_data_.cloud_ptr)中的代码.