Lio-sam代码分析（2）-图像预处理imageProjection
Lio-sam代码分析（2）-图像预处理imageProjection
- 概述
- 点云投影成2维图片
- 去畸变
- 有效点云提取
- 简单总结
概述
激光雷达点云获得原数据都是一维的,但是根据实际雷达旋转扫描的特性,将其投影映射成2维数组.例如采用16线的vld雷达,则根据实际扫描逻辑映射成16个1维数组,相当于存在16个1维平面雷达.如此可以更容易提取如地面/平面/角点/垂直线的特征点. 映射和特征提取与之前loam-slam中特征提取方案一致.不再详述,具体看lego-loam代码分析（1）-地面提取和点云类聚.
由于lidar 实际存在运动,因此实际上每帧点云中的每个点的坐标原点并非一致.由于Lego-loam中没有引入imu信息,且假设匀速运动,因此没有进行较为准确的点云去畸变处理.LIO-SAM进行了优化.
点云投影成2维图片
由于点云是一维无序无规律数组,根据机械扫描雷达特性,将其投影成具有空间关系的2维图片,同时存储每个点到center中心的距离.
PointType thisPoint;
thisPoint.x = laserCloudIn->points[i].x;
thisPoint.y = laserCloudIn->points[i].y;
thisPoint.z = laserCloudIn->points[i].z;
thisPoint.intensity = laserCloudIn->points[i].intensity;
// 计算每个点到圆心的距离
float range = pointDistance(thisPoint);
// 计算行索引,从下往上共16,-15度到15度
int rowIdn = laserCloudIn->points[i].ring;
// 计算当前点水平方向夹角, 根据velodyne说明书描述,
// 注意horizonAngle = atan(x/y),就是因为velodyne的说明书便是如此计算的
// 所以夹角是相对于y轴的,而不是x轴
float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
// 计算水平扫描角度分辨率
static float ang_res_x = 360.0/float(Horizon_SCAN);
// 根据夹角和分辨率计算 水平索引
// 之所以下面如此繁琐,主要是因为velodyne输出的点云不是采用ros下标准
// velodyne点云坐标系为前方y,右边为x.
// 在安装时可将雷达的坐标系与baselink一致,如(前x,左y,上z)
columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
去畸变
去畸变实际上就是补偿因为lidar 运动时造成每帧点云中每个点的原点坐标不一致的造成的偏移.因此对lidar本身的运动姿态和速度需要较为准确的估计且高频. LIO-SAM采用的数据为imu原数据和 imu odom进行位置估计.其中IMU odom是经预积分修正后更为精确,但在初始化或开始阶段,仅由IMU原始数据进行估计.
将imu和odom信息进行缓存,并且与cloud时钟对齐,补偿就是cloud一帧的点云(实际所有点并非同一时刻获得,必然存在时间差),都是以cloud第一个点的时刻进行补偿,求出每个点相对于第一个点时刻的相对位置.高频的IMU原数据可以提供高频的姿态变化,imu odom可以提供高频平移变换.具体看代码如下.
// get timestamp
cloudHeader = currentCloudMsg.header;
// 每一帧点云的时间戳,即第一个点的时间戳
timeScanCur = cloudHeader.stamp.toSec();
// 每一帧最后一个点的时间戳,主要是点云中的时间戳都是针对第一个点的相对时间
timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
首先确定每帧点云的时刻,实际为点云中第一个点的时间戳.同时根据相对时间,也能获取每帧点云最后一个点的获取的时间戳.
if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
{
....
// integrate rotation
// 根据角速度进行积分,获取每个时刻的rpy,用于后续的去畸变
// imuRot记录的是点云第一个点,到最后一个点时imu中间每个时刻的相对与第一个时刻的相对姿态
double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
imuTime[imuPointerCur] = currentImuTime;
++imuPointerCur;
}
对应时间段内,对姿态进行积分,可获取每个时刻相对姿态,也就是说获得了一帧cloud中,从第一个点到最后一个点间lidar姿态变化.
*rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
// 根据当前点的时刻,查询IMU旋转量队列中对应的时刻
// (实际上找到 当前时刻前后两个时刻的imu旋转量)
int imuPointerFront = 0;
while (imuPointerFront < imuPointerCur)
{
if (pointTime < imuTime[imuPointerFront])
break;
++imuPointerFront;
}
// imuPointerFront - 1 则为时间早的一刻
// 为更加精确,当前点姿态采用两个时刻间的插值
int imuPointerBack = imuPointerFront - 1;
double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
*rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
*rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
*rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
为获取cloud中每个点的相对姿态,但目前存储的仅有IMU数据对应时刻的姿态,因此可采用插值的方法.
相同方法处理imu odom信息,同样进行时钟对齐,并将cloud中第一个点作为初始位置,计算出整个时间段内每个imuodom时刻的相对位置(绝对位置在初始位置坐标系下的相对位置).这里方案仍然是假设了匀速平移,因此只需要获取cloud最后一个点时刻的相对位置即可.(目前开源代码中这项目前使用,测试都是低速运动,一帧时间非常短,每个点平移太小,因此补偿较小无需补偿)
由于获得了cloud中每个点相对于第一个点的相对位移和相对姿态,进行补偿,完成畸变矫正.
if (firstPointFlag == true)
{
transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
firstPointFlag = false;
}
// transform points to start
// 获取相对于第一个点的相对偏移
Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
Eigen::Affine3f transBt = transStartInverse * transFinal;
// 位置畸变矫正
PointType newPoint;
newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
newPoint.intensity = point->intensity;
有效点云提取
为下一步特征点提取进行简单的预处理, 由于2维投影存在每一个像素,但是会存在无效数据.同时由于每一个扫描环,起始和结束位置属于激光边界区属于不稳定区域.基于此原因,记录一个点云信息较为良好且有效的点云集合.
// 有效点云提取
void cloudExtraction()
{
// 记录点云有效值个数和有效点云顺序索引
int count = 0;
// extract segmented cloud for lidar odometry
// 将点云看成2维图片进行顺序遍历
for (int i = 0; i < N_SCAN; ++i)
{
// 每行开始索引值,从第5个开始
cloudInfo.startRingIndex[i] = count - 1 + 5;
for (int j = 0; j < Horizon_SCAN; ++j)
{
// 仅考虑有效点
if (rangeMat.at(i,j) != FLT_MAX)
{
// mark the points' column index for marking occlusion later
cloudInfo.pointColInd[count] = j;
// save range info
cloudInfo.pointRange[count] = rangeMat.at(i,j);
// save extracted cloud
extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
// size of extracted cloud
++count;
}
}
// 每行结束索引值,抛弃最后5个
cloudInfo.endRingIndex[i] = count -1 - 5;
}
}
简单总结
lio-sam实际是将lego-loam中点云预处理功能一致,增加采用imu和imu odom进行较为准确畸变校正功能.如果在slam中,旋转和平移较为缓慢,且不抖动,则畸变修正功能意义不大,但实际运动中尤其手持,背包等都会使得同一帧点云获取期间,其原点位置发生改变,则会使本帧的点云产生畸变,而校正则就是采用反向补偿进行修正. 由于LIO-SAM中融合了高频的IMU,因此一帧点云产生期间的lidar设备运动可以进行准确估计,从而补偿点云畸变.