LOAM 之 scanRegistration

LOAM系列之scanRegistration

前言
ALOAM - scanRegistration
- laserCloudHandler回调函数
LOAM - scanRegistration
- imuHandler
- laserCloudHandler
- - IMU去畸变
  - - ShiftToStartIMU
    - TransformToStartIMU
  - 坏点去除

前言

先从代码结构优化后的ALOAM开始分析，再比较LOAM。

loam 推荐一个博客：
LOAM论文和程序代码的解读

ALOAM - scanRegistration

scanRegistration负责对激光雷达点云数据进行 1、数据预处理 2、激光雷达数据模型化 3、特征提取
ALOAM的scanRegistration通过单独一个ROS节点完成，这个节点在scanRegistration.cpp中实现，该cpp比较简单，只有两个主要部分，一个是main函数，另一个是点云数据的回调函数。进入到该节点的main函数中可以看到：

ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
// 滤波处理后的全部点云
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
// 大曲率特征点
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
// 小曲率特征点 包含 大曲率
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
// 平面点
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
// 除了角点之外全部点  经过了滤波处理
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
// 没用
pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);

可以看到scanRegistration节点订阅了velodyne_points话题，通过这个话题来获取原始的激光雷达数据，并且发布了一下5个话题：
velodyne_cloud_2： 将原始点云滤波，模型化后重组成的点云 - laserCloud
laser_cloud_sharp： 大曲率特征点 cornerPointsSharp
laser_cloud_less_sharp： 小曲率特征点也包含大曲率特征点 cornerPointsLessSharp
laser_cloud_flat： 平面点 surfPointsFlat
laser_cloud_less_flat： 除了角点之外全部点，后面又进行了滤波处理 surfPointsLessFlat
laser_remove_points： 没有发布任何消息

laserCloudHandler回调函数

根据的话题机制，每当激光雷达向velodyne_points话题发送一个sweep扫描数据时，便会进入该回调函数中处理，该回调函数中主要执行一下几步：
1、数据的预处理
接收到velodyne的一个sweep数据后，便会将它由ROS msg转换成PCL数据 PointCloud, 并且通过一个vector points存储任意一点的值，于是就有一个疑问，一个完整sweep点云的点在points中的存放是无序的还是有序的呢？通过查阅知道，points中点的存放是有序的，从前往后按照激光雷达的扫描顺序，即先从初始旋转角度开始，依次获取垂直方向上所有的点，然后按旋转顺序，获取下一个旋转角度下每个垂直方向的点，直到旋转一周完成。明白了这个，就能理解之后的点云数据了。
然后开始基本的预处理，主要策略是最初的一帧数据抛弃，等待数据完整，另外是会执行去NaN点与距离滤波

pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); // 去除无效点
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); // 去除距离传感器太近的点

2、激光雷达数据模型化
根据激光雷达模型，获得每个激光点的scanID以及扫描时间，并将每个点按照帧的scanID划分到laserCloudScans中，根据激光雷达的数据存放顺序，我们也能知道，每一scanID的laserCloudScans点依然是按照旋转顺序排列的。
(1)、首先求出一完整sweep数据中激光的起始角度startOri和终止角度endOri。
(2)、求出每一个激光点的垂直俯仰角，从而利用这个俯仰角求出该激光点属于哪一个scan，即scanID，然后求出该点的旋转角ori，并利用该旋转角ori以及起始startOri和终点角度endOri和一个扫描周期的时间scanPeriod计算该点的时间，最后设置intensity参数：
float relTime = (ori - startOri) / (endOri - startOri);
point.intensity = scanID + scanPeriod * relTime;
最后将激光点放置于laserCloudScans[scanID]中，
这部分完整代码如下：

/***********************************************数据模型化********************************************/
int cloudSize = laserCloudIn.points.size();
//lidar scan开始点的旋转角,atan2范围[-pi,+pi],计算旋转角时取负号是因为velodyne是顺时针旋转,atan2逆时针为正角
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
//lidar scan结束点的旋转角，加2*pi使点云旋转周期为2*pi   ？？？？？？？？？？？？？？？？？？？？？？
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,laserCloudIn.points[cloudSize - 1].x) +2 * M_PI;
// 处理  保证  M_PI < endOri - startOri < 3 * M_PI
if (endOri - startOri > 3 * M_PI)   // startOri<0
{endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{endOri += 2 * M_PI;
}
//printf("end Ori %f\n", endOri);
//   std::cout<<"startOri: "<<startOri<<" endOri:"<<endOri<<std::endl;
//   std::cout<<"start angle: "<<startOri*180/M_PI<<" end angle:"<<endOri*180/M_PI<<std::endl;//lidar扫描线是否旋转过半
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
// 遍历当前扫描帧全部点     求出每个点的旋转角和scanID  根据旋转角确定时间
for (int i = 0; i < cloudSize; i++)
{// 读取每个点的坐标point.x = laserCloudIn.points[i].x;point.y = laserCloudIn.points[i].y;point.z = laserCloudIn.points[i].z;/*********** 计算 scanID 即垂直激光帧的序号 (根据lidar文档垂直角计算公式),根据仰角排列激光线号，velodyne每两个scan之间间隔2度）**************/// 计算点的仰角float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;int scanID = 0;if (N_SCANS == 16){   // + 0.5 是用于四舍五入   因为 int 只会保留整数部分  如 int(3.7) = 3  scanID = int((angle + 15) / 2 + 0.5);    // +0.5是为了四舍五入, /2是每两个scan之间的间隔为2度，+15是过滤垂直上为[-,15,15]范围内if (scanID > (N_SCANS - 1) || scanID < 0){   // 说明该点所处于的位置有问题  舍弃count--;continue;}}                                // 下面两种为 32线与64线的   用的少else if (N_SCANS == 32){scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);if (scanID > (N_SCANS - 1) || scanID < 0){count--;continue;}}else if (N_SCANS == 64){   if (angle >= -8.83)scanID = int((2 - angle) * 3.0 + 0.5);elsescanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);// use [0 50]  > 50 remove outlies if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0){count--;continue;}}else{printf("wrong scan number\n");ROS_BREAK();}//printf("angle %f scanID %d \n", angle, scanID);/************************求出该点的旋转角*************************/float ori = -atan2(point.y, point.x);
//    std::cout<<"ori: "<<ori<<std::endl;if (!halfPassed)    // false{// 确保-pi/2 < ori - startOri < 3*pi/2if (ori < startOri - M_PI / 2){ori += 2 * M_PI;}else if (ori > startOri + M_PI * 3 / 2){ ori -= 2 * M_PI;     // 结果为   ori - startOri > - M_PI / 2}if (ori - startOri > M_PI){halfPassed = true;}}else    // true{ori += 2 * M_PI;//确保-3*pi/2 < ori - endOri < pi/2if (ori < endOri - M_PI * 3 / 2){ori += 2 * M_PI;}else if (ori > endOri + M_PI / 2){ori -= 2 * M_PI;}}// -0.5 < relTime < 1.5（点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间）float relTime = (ori - startOri) / (endOri - startOri);//点强度=线号+点相对时间（即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间）,匀速扫描：根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间point.intensity = scanID + scanPeriod * relTime;         // scanPeriod每一帧的时间   laserCloudScans[scanID].push_back(point);                // 将该点放入  scanID 帧中
}

3、特征提取
处理步骤如下：
(1)、首先将按scanID划分的点云laserCloudScans整合到一个PointCloud laserCloud中，并依据每一scan中前后5个点不提取特征点的原则，记录每一scan中有效特征点在 laserCloud中的序号-scanStartInd、scanEndInd，这个序号是很重要的，因为，在后面计算曲率时，只排除了laserCloud的前后5个点，剩下的scan的前后5个点也都计算了曲率，但是由于记录了有效特征点的序号，这些无效点在后续计算特征时就可以排除。

// 将scan从1-16按顺序排列组成的点云
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{   // 记录每个scan的可用曲率的序号  scanStartInd[i] = laserCloud->size() + 5;      // 可以纳入计算曲率的有效开始点从第五个开始，符合论文所述*laserCloud += laserCloudScans[i];             // 将scanID为i的scan放入laserCloud  scanEndInd[i] = laserCloud->size() - 6;        // 记录记录曲率的终止点
}

(2)、计算曲率
对laserCloud除前后5个点外所有点计算曲率，出现了几个数组：
float cloudCurvature[]：保存每个点曲率
int cloudSortInd[]：
int cloudNeighborPicked[]: 该点可否被选为特征点
int cloudLabel[]:记录每个点的曲率的程度大:2 小：1 平面：-1

for (int i = 5; i < cloudSize - 5; i++)
{//使用每个点的前后五个点计算曲率，因此前五个与最后五个点跳过//但是中间scan的前后5个点没有跳过啊    中间scan的前后5个点会把前后scan的点算进去  这些跨scan的点存在高度差  float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x- 10 * laserCloud->points[i].x ;float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y+ laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y- 10 * laserCloud->points[i].y ;float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z+ laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z- 10 * laserCloud->points[i].z ;//曲率计算cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;// 记录曲率点索引   cloudSortInd[i] = i;//初始时，点全未筛选过cloudNeighborPicked[i] = 0;//初始化为less flat点cloudLabel[i] = 0;
}

(3)、特征筛选
首先要明白什么是特征，在loam中，特征也是点，不过这些点的曲率比较特殊，要么是曲率比较大的点即曲线特征点，要么是曲率比较小的点即平面特征点， loam中选择了4类特征点：
cornerPointsSharp ：曲率很大的特征点 ——每scan的6等份中曲率最大的前2个
cornerPointsLessSharp：曲率稍微小的特征点 —— 每scan的6等份中曲率前20 且曲率要大于0.1的点注意也包括cornerPointsSharp的点
surfPointsFlat：平面特征点，挑选每scan的6等份中曲率最小的4个特征点且曲率小于0.1
surfPointsLessFlat：包含所有非曲线特征的点。

筛选特征时，以一个scan为单位，每个scan对其有效曲率点的部分均匀分成6个子部分，对每一个子部分进行筛选。

 // 遍历每个scanfor (int i = 0; i < N_SCANS; i++){if( scanEndInd[i] - scanStartInd[i] < 6)                 // 点的个数大于6才行，不然不能6等分continue;pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);// 对于每个scan进行6等分      每个等分单独提取特征                              for (int j = 0; j < 6; j++){// 获取该等份的点的序号范围int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;            // 6等分,当前份的起点  int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;  // 6等分，当前份的终点TicToc t_tmp;// 对cloudSortInd进行排序   依据的是序号cloudSortInd[i]的点的曲率从小到大   std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);        //按照曲率从小到大排序   注意sort函数的排序范围为  [，)t_q_sort += t_tmp.toc();........

排序部分 std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
comp是比较函数 bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }
这里要注意，首先sort()函数前两个参数分别是排序起始位置指针以及指向最后一个参与排序的元素后一位置的指针，即排序的范围是[cloudSortInd + sp,cloudSortInd + ep + 1)，也就是数组[cloudSortInd + sp,cloudSortInd + ep]部分参与排序。
另外，这里cloudSortInd + sp是一种数组名+i的表示方法，数组名可以等效成指向数组首元素的指针，那么* (cloudSortInd + sp) = cloudSortInd[sp]。
通过这个排序函数，效果可以等效成

cloudCurvature[*(cloudSortInd + i)] < cloudCurvature[*(cloudSortInd + j)]  , i<j

对这里6等分计算做一个解释，首先由于int精度的问题这里可能并不是严格6等分的！！另外，关于ep为什么要减一，这是避免前后两份存在重叠的部分，这样每一份提出的特征点都不同，可以想到，若不减一，下一份的sp与上一份的ep处于相同位置，这样可能会重复提取同一个特征点！！
特征提取完整代码如下：

/*************开始提取特征**********************/
TicToc t_pts;pcl::PointCloud<PointType> cornerPointsSharp;           // 包含曲率很大的特征点  ——每scan中 曲率最大的前2个
pcl::PointCloud<PointType> cornerPointsLessSharp;       // 曲率稍微小的特征点 —— 每scan中曲率前20 且曲率要大于0.1的点  注意包括cornerPointsSharp的点
pcl::PointCloud<PointType> surfPointsFlat;              // 挑选曲率最小的4个特征点 且曲率小于0.1
pcl::PointCloud<PointType> surfPointsLessFlat;          // 包含所有非sharp的点 float t_q_sort = 0;
// 遍历每个scan
for (int i = 0; i < N_SCANS; i++)
{if( scanEndInd[i] - scanStartInd[i] < 6)                 // 点的个数大于6才行，不然不能6等分continue;pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);// 对于每个scan进行6等分      每个等分单独提取特征                              for (int j = 0; j < 6; j++){// 获取该等份的点的序号范围int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;            // 6等分,当前份的起点  int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;   // 6等分，当前份的终点   为什么要减1呢？ 避免两份重叠  提取重复特征点  //if(j==5) ep++;TicToc t_tmp;// 对cloudSortInd进行排序   依据的是序号cloudSortInd[i]的点的曲率从小到大 std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);        //按照曲率从小到大排序   注意sort函数的排序范围为  [，)t_q_sort += t_tmp.toc();//挑选每个分段的曲率很大和比较大的点int largestPickedNum = 0;for (int k = ep; k >= sp; k--){int ind = cloudSortInd[k];           //曲率最大点的序号   //如果曲率大的点，曲率的确比较大，并且未被筛选过滤掉  曲率的阈值为 0.1    if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] > 0.1){largestPickedNum++;if (largestPickedNum <= 2)      //挑选曲率最大的前2个点放入sharp点集合{                        cloudLabel[ind] = 2;        // 2 代表曲率很大cornerPointsSharp.push_back(laserCloud->points[ind]);cornerPointsLessSharp.push_back(laserCloud->points[ind]);     // cornerPointsLessSharp容器也包括cornerPointsSharp的点 ！！！！！！}else if (largestPickedNum <= 20)   //挑选曲率最大的前20个点放入less sharp点集合{                        cloudLabel[ind] = 1;           //1代表曲率比较尖锐cornerPointsLessSharp.push_back(laserCloud->points[ind]);}else{break;                           // 数量足够了  直接跳出循环   }cloudNeighborPicked[ind] = 1;        //过滤标志置位// 考虑该特征点序号前后5个点，如果有两两之间距离过于接近的点  则不考虑将其前一个作为下一个特征点的候选点  这是防止特征点聚集，使得特征点在每个方向上尽量分布均匀  // 注意这里特征点也是普通的点云中的点   for (int l = 1; l <= 5; l++){float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}// 该点直接取消特征点的候选资格cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--){float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}cloudNeighborPicked[ind + l] = 1;}}}// 墙面同理int smallestPickedNum = 0;// 这里循环注意是按曲率从小到大for (int k = sp; k <= ep; k++){int ind = cloudSortInd[k];// 平面的标准是 曲率要小于0.1   if (cloudNeighborPicked[ind] == 0 &&cloudCurvature[ind] < 0.1){cloudLabel[ind] = -1;     surfPointsFlat.push_back(laserCloud->points[ind]);smallestPickedNum++;if (smallestPickedNum >= 4)               // 只选最小的四个，剩下的Label==0,就都是曲率比较小的{ break;}cloudNeighborPicked[ind] = 1;// 同样前后5个点  不能两两之间不能太密集  for (int l = 1; l <= 5; l++){ float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}cloudNeighborPicked[ind + l] = 1;}for (int l = -1; l >= -5; l--){float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}cloudNeighborPicked[ind + l] = 1;}}}//将剩余的点（包括之前被排除的点以及平面点 ）全部归入平面点中less flat类别中for (int k = sp; k <= ep; k++){if (cloudLabel[k] <= 0){surfPointsLessFlatScan->push_back(laserCloud->points[k]);}}}

另外，感觉在防止特征点聚集的处理上不太合理，可以再优化一下

for (int l = 1; l <= 5; l++)
{float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}// 该点直接取消特征点的候选资格cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){break;}cloudNeighborPicked[ind + l] = 1;
}

LOAM - scanRegistration

分析完ALOAM ，下面来分析LOAM的scanRegistration节点。
LOAM比ALOAM多了对IMU的处理的部分，以及增加了对一些不好的特征点的检测与剔除。但大致的框架还是和ALOAM一致的，现在分析LOAM应该就简单多啦。

imuHandler

先进入LOAM的IMU回调函数中

//接收imu消息，imu坐标系为x轴向前，y轴向右，z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{double roll, pitch, yaw;
tf::Quaternion orientation;
//convert Quaternion msg to Quaternion
tf::quaternionMsgToTF(imuIn->orientation, orientation);   //这个IMU有姿态输出的功能
//This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. That's R = Rz(yaw)*Ry(pitch)*Rx(roll).
//Here roll pitch yaw is in the global frame
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);      // 转换为欧拉角   // a' - Riw*gw  获得局部坐标下的真实运动     再转换坐标   y->x   z->Y  x->z
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;//循环移位效果，形成环形数组   从0加到200 从0到199 然后再回0
imuPointerLast = (imuPointerLast + 1) % imuQueLength;        // 初始化为-1 imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;
// 更新IMU 在世界坐标下的P、V
AccumulateIMUShift();
}

LOAM对IMU的使用比较特别，它使用了一个能输出姿态角的IMU，整个系统使用的IMU数据是姿态角orientation和加速度，但是没有用到角速度。imuHandler中，求出的accX，accY，accZ是在局部坐标系转换为z超前，x超左，y朝上的坐标下的加速度值。
loam中使用一个长度为200的环形数组来对IMU数据进行储存，imuPointerLast指示当前位置。

AccumulateIMUShift()

// 积分速度与位移
void AccumulateIMUShift()
{float roll = imuRoll[imuPointerLast];float pitch = imuPitch[imuPointerLast];float yaw = imuYaw[imuPointerLast];float accX = imuAccX[imuPointerLast];float accY = imuAccY[imuPointerLast];float accZ = imuAccZ[imuPointerLast];//将当前时刻的加速度值绕交换过的ZXY固定轴（原XYZ）分别旋转(roll, pitch, yaw)角，转换得到世界坐标系下的加速度值(right hand rule)//绕z轴旋转(roll)float x1 = cos(roll) * accX - sin(roll) * accY;float y1 = sin(roll) * accX + cos(roll) * accY;float z1 = accZ;//绕x轴旋转(pitch)float x2 = x1;float y2 = cos(pitch) * y1 - sin(pitch) * z1;float z2 = sin(pitch) * y1 + cos(pitch) * z1;//绕y轴旋转(yaw)      这里的accX 是原Y轴的加速度   accY是原z轴   accZ 是原x轴 accX = cos(yaw) * x2 + sin(yaw) * z2;accY = y2;accZ = -sin(yaw) * x2 + cos(yaw) * z2;//上一个imu点    当 imuPointerLast = 0 时  这样能得到199  int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;//上一个点到当前点所经历的时间，即计算imu测量周期double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];//要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义if (timeDiff < scanPeriod) {         //（隐含从静止开始运动）//求每个imu时间点的位移与速度,两点之间视为匀加速直线运动// 更新 PimuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff + accX * timeDiff * timeDiff / 2;imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff + accY * timeDiff * timeDiff / 2;imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff + accZ * timeDiff * timeDiff / 2;// 更新V imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;}
}

这个函数将局部坐标下的加速度值转换到世界坐标下，并进行积分得到世界坐标下的运动。
局部坐标转换到世界坐标的可以看成世界转局部的逆过程，世界转局部的可以用ZYX欧拉角旋转表示，即先绕Z轴转yaw->绕Y轴转Pitch->绕X轴转Roll,逆过程就是绕X轴转-Roll->绕Y轴转-Pitch->绕Z轴转-yaw
但由于局部坐标进行了转换 X->Z ,Y->X, Z->Y ,因此这个转换过程就是
绕Z轴转-Roll->绕X轴转-Pitch->绕Y轴转-yaw
注意最后转换后的世界坐标系也同样是 Z朝前 ,Y朝左, Z朝上，和局部坐标系朝向相同。
这个函数利用IMU的测量值(姿态角、加速度)递推的求解IMU在世界坐标下的P、V，但是，由于噪声的影响，这个递推值必然会导致相当大的误差，那么LOAM里是怎么处理的呢？？？？？？？？？？？？？
我认为IMU积分的数据应该要在后面进行修正，不然任凭它积分肯定不一会就飘很远了

laserCloudHandler

laserCloudHandler()是点云处理的主要函数，它的大体流程和ALOAM的基本相同，但是多出了使用IMU去除畸变以及特征点坏点剔除的环节，因此我们需要对这部分进行补充学习。
先梳理一下大体流程：
1、丢弃最开始的20个数据。
2、移去NULL点。
2、遍历所有激光点数据：
(1)、激光数据模型化( 和ALOAM相同 )，求出每个点的所在垂直scan的 scanID，以及旋转角 ori，和相对时间relTime，最后将 point.intensity = scanID + scanPeriod * relTime。
(2)、如果有IMU数据则利用IMU去除畸变。待会重点分析。
3、激光点曲率计算。
4、激光点筛选(ALOAM没有的)。重点分析。
5、对剩余的全部点划分6块，每块单独提取特征。

IMU去畸变

if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;     //计算点的周期时间
//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFront
while (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;    // imuPointerFront ++
}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {   //没找到,此时imuPointerFront==imtPointerLast,只能以当前收到的最新的IMU的速度，位移，欧拉角作为当前点的速度，位移，欧拉角使用// 我觉得如果IMU小于该点的时间   这样可以干脆抛弃这个点  否则不精确也没用imuRollCur = imuRoll[imuPointerFront];imuPitchCur = imuPitch[imuPointerFront];imuYawCur = imuYaw[imuPointerFront];imuVeloXCur = imuVeloX[imuPointerFront];imuVeloYCur = imuVeloY[imuPointerFront];imuVeloZCur = imuVeloZ[imuPointerFront];imuShiftXCur = imuShiftX[imuPointerFront];imuShiftYCur = imuShiftY[imuPointerFront];imuShiftZCur = imuShiftZ[imuPointerFront];
} else {//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间，据此线性插值，计算点云点的速度，位移和欧拉角int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;//按时间距离计算权重分配比率,也即线性插值float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);// 计算插值后 激光点处的 欧拉角 imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;// Yaw 认为可能会运动超过180度    要特殊处理  if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;} else {imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;}//本质:imuVeloXCur = imuVeloX[imuPointerback] + (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFrontimuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
if (imuPointerLast >= 0) {
float pointTime = relTime * scanPeriod;     //计算点的周期时间
//寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFront
while (imuPointerFront != imuPointerLast) {if (timeScanCur + pointTime < imuTime[imuPointerFront]) {break;}imuPointerFront = (imuPointerFront + 1) % imuQueLength;    // imuPointerFront ++
}if (timeScanCur + pointTime > imuTime[imuPointerFront]) {   //没找到,此时imuPointerFront==imtPointerLast,只能以当前收到的最新的IMU的速度，位移，欧拉角作为当前点的速度，位移，欧拉角使用// 我觉得如果IMU小于该点的时间   这样可以干脆抛弃这个点  否则不精确也没用imuRollCur = imuRoll[imuPointerFront];imuPitchCur = imuPitch[imuPointerFront];imuYawCur = imuYaw[imuPointerFront];imuVeloXCur = imuVeloX[imuPointerFront];imuVeloYCur = imuVeloY[imuPointerFront];imuVeloZCur = imuVeloZ[imuPointerFront];imuShiftXCur = imuShiftX[imuPointerFront];imuShiftYCur = imuShiftY[imuPointerFront];imuShiftZCur = imuShiftZ[imuPointerFront];
} else {//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和imuPointerFront之间，据此线性插值，计算点云点的速度，位移和欧拉角int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;//按时间距离计算权重分配比率,也即线性插值float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);// 计算插值后 激光点处的 欧拉角 imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;// Yaw 认为可能会运动超过180度    要特殊处理  if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;} else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;} else {imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;}//本质:imuVeloXCur = imuVeloX[imuPointerback] + (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFrontimuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
}if (i == 0) {                     //如果是第一个点,记住点云起始位置的速度，位移，欧拉角imuRollStart = imuRollCur;imuPitchStart = imuPitchCur;imuYawStart = imuYawCur;imuVeloXStart = imuVeloXCur;imuVeloYStart = imuVeloYCur;imuVeloZStart = imuVeloZCur;imuShiftXStart = imuShiftXCur;imuShiftYStart = imuShiftYCur;imuShiftZStart = imuShiftZCur;
} else {     //计算之后每个点相对于第一个点的由于加减速非匀速运动产生的位移速度畸变，并对点云中的每个点位置信息重新补偿矫正ShiftToStartIMU(pointTime);VeloToStartIMU();TransformToStartIMU(&point);
}
}
laserCloudScans[scanID].push_back(point);              //将每个补偿矫正的点放入对应线号的容器
}

流程：
1、对于要去畸变的点，首先要知道它相对与该帧第一个点经过的时间，因为，我们要将每个点转到该帧的起始点坐标上。计算方式：float pointTime = relTime * scanPeriod 。相对比例 ×一帧的时间。
2、找到位于该点时间戳前后两个IMU先验数据，进行插值得到该点的先验运动：(代码里的操作有待商榷)imuRollCur、imuPitchCur、imuYawCur、imuVeloXCur、imuVeloYCur、imuVeloZCur
imuShiftXCur、imuShiftYCur、imuShiftZCur。
3、记录该帧起始点的先验运动信息。
4、求取激光雷达坐标系在该点处相对于起始点由于非匀速运动产生的位移 ShiftToStartIMU()。
5、计算激光雷达在当前点处相对于起始点处的速度变化。VeloToStartIMU()。(下面去畸变没用到)
6、根据激光雷达坐标系当前点和起始点姿态角变化与位移变化矫正畸变void TransformToStartIMU()。

ShiftToStartIMU

// 计算局部坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变
// pointTime： 与第一个点的时间差
void ShiftToStartIMU(float pointTime)
{//计算相对于第一个点由于加减速产生的畸变位移(全局坐标系下畸变位移量delta_Tg)//imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)   注意这里减去 匀速运动的位移 则得到的是非匀速运动的部分imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;/********************************************************************************Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tgtransfrom from the global frame to the local frame*********************************************************************************/// 上面得到的是全局坐标系的位移  下面转换到第一个点时的局部坐标系// 旋转方向  Z-Y-X   // 绕y轴旋转( imuYawStart )  float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;float y1 = imuShiftFromStartYCur;float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;//绕x轴旋转( imuPitchStart ) float x2 = x1;float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;//绕z轴旋转( imuRollStart ) imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;imuShiftFromStartZCur = z2;
}

TransformToStartIMU

//去除点云加减速产生的位移畸变
void TransformToStartIMU(PointType *p)
{/********************************************************************************Ry*Rx*Rz*Pl, transform point to the global frame*********************************************************************************/// 首先将局部坐标系下的测量值转换到全局坐标下  // 绕z轴旋转(-imuRollCur)float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;float z1 = p->z;//绕x轴旋转(-imuPitchCur)float x2 = x1;float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;//绕y轴旋转(-imuYawCur)float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;float y3 = y2;float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;/********************************************************************************Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pgtransfrom global points to the local frame*********************************************************************************/// 然后将全局坐标下的测量转换到第一个点对应的局部坐标系下// 绕y轴旋转(-imuYawStart)float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;float y4 = y3;float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;//绕x轴旋转(-imuPitchStart)float x5 = x4;float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;//绕z轴旋转(-imuRollStart)，然后叠加平移量p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;p->z = z5 + imuShiftFromStartZCur;
}

去畸变的问题其实就是，将当前点坐标转换为起始点坐标系上，它们两坐标系相差一个旋转与平移，不过这两个量我们通过IMU测出来了非匀速运动的部分，于是整个转换过程就是：
当前点局部坐标->世界坐标->起始点局部坐标。
关于为什么loam要用IMU去除非匀速运动部分的畸变，而不直接用IMU去除整个运动畸变，我的理解是IMU测量整个运动由于噪声的影响是很不准的，但IMU计算短时间内速度变化带来的运动是比较准的，而匀速运动的部分loam的激光里程计要准于IMU，所以匀速运动的畸变就采用激光里程计来去除。

坏点去除

剔除两类坏点

//挑选点，排除容易被斜面挡住的点以及离群点，有些点容易被斜面挡住，而离群点可能出现带有偶然性，这些情况都可能导致前后两次扫描不能被同时看到
for (int i = 5; i < cloudSize - 6; i++) {   //与后一个点差值，所以减6
float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
//计算有效曲率点与后一个点之间的距离平方和
float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;if (diff > 0.1) {           //前提:两个点之间距离要大于0.1//点的深度diffY2
float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + laserCloud->points[i].y * laserCloud->points[i].y +laserCloud->points[i].z * laserCloud->points[i].z);//后一个点的深度
float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);//按照两点的深度的比例，将深度较大的点拉回后计算距离
if (depth1 > depth2) {diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;//边长比也即是弧度值，若小于0.1，说明夹角比较小，斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {//排除容易被斜面挡住的点//该点及前面五个点（大致都在斜面上）全部置为筛选过cloudNeighborPicked[i - 5] = 1;cloudNeighborPicked[i - 4] = 1;cloudNeighborPicked[i - 3] = 1;cloudNeighborPicked[i - 2] = 1;cloudNeighborPicked[i - 1] = 1;cloudNeighborPicked[i] = 1;}
} else {diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {cloudNeighborPicked[i + 1] = 1;cloudNeighborPicked[i + 2] = 1;cloudNeighborPicked[i + 3] = 1;cloudNeighborPicked[i + 4] = 1;cloudNeighborPicked[i + 5] = 1;cloudNeighborPicked[i + 6] = 1;}
}
}
//与前一个点的距离平方和
float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;//点深度的平方和
float dis = laserCloud->points[i].x * laserCloud->points[i].x+ laserCloud->points[i].y * laserCloud->points[i].y+ laserCloud->points[i].z * laserCloud->points[i].z;//与前后点的平方和都大于深度平方和的万分之二，这些点视为离群点，包括陡斜面上的点，强烈凸凹点和空旷区域中的某些点，置为筛选过，弃用
if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {cloudNeighborPicked[i] = 1;
}
}

待续 …