LOAM学习-代码解析（三）特征点运动估计 laserOdometry

---

前言

前两篇文章LOAM学习-代码解析（一）点云数据配准和LOAM学习-代码解析（二）点云数据配准 scanRegistration，对点云数据配准的源代码进行详细完整的即系，这里继续LOAM的代码解析工作。

LOAM代码(带中文注释)的地址：https://github.com/cuitaixiang/LOAM_NOTED

LOAM代码(带中文注释)的百度网盘链接：https://pan.baidu.com/s/1tVSNBxNQrxKJyd5c9mWFWw 提取码: wwxr

LOAM论文的百度网盘链接： https://pan.baidu.com/s/10ahqg8O3G2-xOt9QZ1GuEQ 提取码: hnri

LOAM流程：

从LOAM流程图上可一看出，完成点云配准之后，需要进行激光雷达里程计的计算。

---

一、初始化

设置点云周期，lidar从特征点中估计运动频率为10Hz，因此点云周期为0.1s。

const float scanPeriod = 0.1;

发送数据给laser Mapping的频率为 1HZ，因此设置数据发送周期为1s。

const int skipFrameNum = 1;
bool systemInited = false;

设置时间戳信息

double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
double timeImuTrans = 0;

设置消息接收标志

bool newCornerPointsSharp = false;
bool newCornerPointsLessSharp = false;
bool newSurfPointsFlat = false;
bool newSurfPointsLessFlat = false;
bool newLaserCloudFullRes = false;
bool newImuTrans = false;

接收scanRegistration的信息

//获得尖锐点
pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
//获得较少的尖锐点
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
//获得平面点
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
//获得更少的平面点 
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());
//最后一帧的尖锐点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
//最后一帧的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
//保存前一个节点发过来的未经处理过的特征点
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
//获得所有点
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
//接收imu信息
pcl::PointCloud<pcl::PointXYZ>::Ptr imuTrans(new pcl::PointCloud<pcl::PointXYZ>());
//用最后一帧的较少尖锐点建立kd树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<PointType>());
//用最后一帧较少的平面点建立kd树
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<PointType>());

设置计数标志

int laserCloudCornerLastNum;
int laserCloudSurfLastNum;

设置角点和平面点变量

//unused
int pointSelCornerInd[40000];
//保存搜索到的2个角点索引
float pointSearchCornerInd1[40000];
float pointSearchCornerInd2[40000];

//unused
int pointSelSurfInd[40000];
//保存搜索到的3个平面点索引
float pointSearchSurfInd1[40000];
float pointSearchSurfInd2[40000];
float pointSearchSurfInd3[40000];

设置状态转移量

//当前帧相对上一帧的状态转移量，in the local frame
float transform[6] = {
   0};
//当前帧相对于第一帧的状态转移量，in the global frame
float transformSum[6] = {
   0};

设置点云的欧拉角、畸变位移、畸变速度

//点云第一个点的RPY
float imuRollStart = 0, imuPitchStart = 0, imuYawStart = 0;
//点云最后一个点的RPY
float imuRollLast = 0, imuPitchLast = 0, imuYawLast = 0;
//点云最后一个点相对于第一个点由于加减速产生的畸变位移
float imuShiftFromStartX = 0, imuShiftFromStartY = 0, imuShiftFromStartZ = 0;
//点云最后一个点相对于第一个点由于加减速产生的畸变速度
float imuVeloFromStartX = 0, imuVeloFromStartY = 0, imuVeloFromStartZ = 0;

---

二、去除位移畸变 TransformToStart TransformToEnd

将当前帧点云TransformToStart和将上一帧点云TransformToEnd的作用：去除畸变，并将两帧点云数据统一到同一个坐标系下计算。

当前点云中的点相对第一个点去除因匀速运动产生的畸变，效果相当于得到在点云扫描开始位置静止扫描得到的点云。

在这一部分中，要回顾scanRegistration.cpp中intensity的计算代码，是由scanID和scanPeriod * relTime两部分组成。而relTime是点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间。

// scanRegistration.cpp 371-374
//-0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间）
float relTime = (ori - startOri) / (endOri - startOri);
//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
//扫描周期, velodyne频率10Hz，周期0.1s,scanPeriod=0.1
point.intensity = scanID + scanPeriod * relTime;

插值系数的计算公式如下，感觉作者这里写的很比较绕，实际上计算的就是点旋转的角度与整个周期旋转角度的比率，即点的相对时间。
$$s=F_{period}\ast (I_{point}-int(I_{point}))=F_{period}\ast T_{period}\ast T_{rel}=T_{rel}$$

$$s=T_{rel}$$