lego-loam代码分析（2）-角点和平面点提取
FeatureAssociation.cpp同样为独立线程，主要功能是根据带有聚类后地面和非地面的点云数据进行特征值提取，主要提取其平面特征、角点特征；同时采用匹配算法计算前后两帧激光视角下的位姿转换；从而更新里程计；其流程如下：
- 畸变矫正（未能完全理解）；
- 计算每个点平滑性参数；
- 对每个点进行标记，用于忽略此点特征提取；
- 平面特征点和角点特征提取；
- 计算两帧位姿转换；
- 计算里程计；
坐标修正（adjustDistortion）
lego-loam后续使用的点云坐标系与常见的ros和传感器原始坐标系不一致（loam中的坐标系比较奇怪，中间经常变换，比较难以理解，不清楚作者的具体意图，可能是为了和imu坐标一致，但是imu暂时未使用）。常见的坐标系即右手坐标系，即朝前为X，朝左为Y，朝上为z。而在提特征前，将点云坐标系进行了更改。更改为朝前为Z，朝左为X，朝上为Y。
// 坐标系更改，不是ros中常用的右手坐标系
void FeatureAssociation::adjustDistortion() {
bool halfPassed = false;
int cloudSize = segmentedCloud->points.size(); //对分类
PointType point;
for (int i = 0; i < cloudSize; i++) { // 坐标系做了重新调整
point.x = segmentedCloud->points[i].y; // 朝左为x
point.y = segmentedCloud->points[i].z; // 朝上为y
point.z = segmentedCloud->points[i].x; // 朝前为z
float ori = -atan2(point.x, point.z); // 表明atan2（y，x），表明绕z轴的旋转，范围为-PI～ PI
if (!halfPassed) { // 表明未经过一半的角度
if (ori < segInfo.startOrientation - M_PI / 2)
ori += 2 * M_PI;
else if (ori > segInfo.startOrientation + M_PI * 3 / 2)
ori -= 2 * M_PI;
if (ori - segInfo.startOrientation > M_PI) halfPassed = true;
} else {
ori += 2 * M_PI;
if (ori < segInfo.endOrientation - M_PI * 3 / 2)
ori += 2 * M_PI;
else if (ori > segInfo.endOrientation + M_PI / 2)
ori -= 2 * M_PI;
}
// 计算当前point在整个水平扫描位置比例，或者说扫描的时刻在一圈中的比例
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
// 由于扫描周期为_scan_period，故_scan_period * relTime为当前点扫描的时刻， 而原始强度为扫描索引序号的变形
point.intensity =
int(segmentedCloud->points[i].intensity) + _scan_period * relTime; // 强度信息与随着每个点扫描角度不同变大
segmentedCloud->points[i] = point; //将坐标系进行调整保存
}
}
转换过坐标系后，查找16个水平扫描中每行中每个点距离起始扫描点的角度差，用于计算每个点距离第一个扫描点的时间差，目的为后续因运动带来的运动畸变。
代码稍微难易理解，原因在于需要和雷达的坐标系和驱动有关。反推的水平方向扫描角度，其终点可能会超过360度，即终点可能会超过起始点，为和起点相邻的点云进行区分，则增加了halfPassed标志位进行巧妙区别。
驱动中每个点都是与Y轴的夹角作为索引角度且顺时针旋转（vlp16驱动所导致）。代码就是对arctan四个象限的分别处理。
注：
1.点云中的强度信息，表示的是每个点扫描的时刻，在loam中目的是与采用imu进行匀速修正的，lego-loam暂时未用，因此在真正使用时需注意建图时雷达移动不要太快，且尽量匀速。
2.后面匹配时使用强度信息用于畸变补偿，但是没有理解
计算平滑参数 （calculateSmoothness）
平滑性采用的方法为将当前点的距离与前后各5点计算误差累计和的平方，其平滑度也有称为曲率。从代码思想来看，其平滑度主要考虑单线激光测得距离前后的变化程度，因此更接近一个圆的曲率，而不是真正一个面的曲率。
void FeatureAssociation::calculateSmoothness() {
int cloudSize = segmentedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++) {
float diffRange = segInfo.segmentedCloudRange[i - 5] + // 当前点的10倍与水平方向上前后5个点，距离差。差越大，表明当前点为1边缘点
segInfo.segmentedCloudRange[i - 4] + // 连续点云边缘的点最大
segInfo.segmentedCloudRange[i - 3] +
segInfo.segmentedCloudRange[i - 2] +
segInfo.segmentedCloudRange[i - 1] -
segInfo.segmentedCloudRange[i] * 10 +
segInfo.segmentedCloudRange[i + 1] +
segInfo.segmentedCloudRange[i + 2] +
segInfo.segmentedCloudRange[i + 3] +
segInfo.segmentedCloudRange[i + 4] +
segInfo.segmentedCloudRange[i + 5];
cloudCurvature[i] = diffRange * diffRange;
cloudNeighborPicked[i] = 0; // 初始化，后续临近点标志
cloudLabel[i] = 0; // 特征点分类，后续特征提取赋值
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
经过平滑度的计算，则每个已经分类后的点云的每个点，增加一个平滑度信息。而后续的特征点提取也会根据平滑度进行分类。
点云屏蔽标记（markOccludedPoints）
- 在提取特征点时，防止提取同一位置附近的多个点，或者边缘跳跃点，防止提取远处跳跃近处时的点，由于雷达旋转测距方式，近处障碍的遮挡导致远处边缘的点并不固定。标记的例子如下图，红色点标记为屏蔽点：
- 还有一种情况，即当前点与相邻的前后两个点的测量距离均有明显的出入。这种点是一个平面接近平行于激光光束的点，因此会导致相邻的点距离差较大。此点也会在测量中可能忽隐忽现，不稳定，同样提前屏蔽。
cloudNeighborPicked中记录的是点云中的每个点是否需要提取特征的标记，0可认为需要进行判断提取，而1表明无需提取，或者已提取过的特征点。而上述途中的点在提取时前进行标记为1，无需进行特征提取。详细代码执行如下。
void FeatureAssociation::markOccludedPoints() {
int cloudSize = segmentedCloud->points.size();
for (int i = 5; i < cloudSize - 6; ++i) {
float depth1 = segInfo.segmentedCloudRange[i];
float depth2 = segInfo.segmentedCloudRange[i + 1]; // 遍历提取相邻的两个点的距离
int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i + 1] - // 获取相邻两个点的在水平方向上的索引号差
segInfo.segmentedCloudColInd[i]));
if (columnDiff < 10) { // 如果水平索引在10个点内，将远处的边缘的5个点标记为1,
if (depth1 - depth2 > 0.3) {
cloudNeighborPicked[i - 5] = 1; // 近处的边缘点则为0
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
} else if (depth2 - depth1 > 0.3) {
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 diff1 = std::abs(segInfo.segmentedCloudRange[i - 1] -
segInfo.segmentedCloudRange[i]);
float diff2 = std::abs(segInfo.segmentedCloudRange[i + 1] -
segInfo.segmentedCloudRange[i]);
if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && // 如果两个相邻点的距离超过本点的距离0.02倍时，也标记为1，即孤立的点标记为1
diff2 > 0.02 * segInfo.segmentedCloudRange[i])
cloudNeighborPicked[i] = 1;
}
}
平面点和角点提取 extractFeatures（）
特征提取实际就是从点云中提取平面点，角点等特殊性的特征点，用于后续的匹配。由于特征点相对于所有点云来说个数极少，因此可极大提高后续匹配的性能。将提取的特征点采用之前定义的cloudLabel进行记录。主要包括四种特征点： cornerPointsSharp（角点，cloudLabel=2）、cornerPointsLessSharp（ 轻微角点cloudLabel=1）、surfPointsFlat（地面平面点，cloudLabel=-1）、surfPointsLessFlat(非角点的特征点，cloudLabel<=0)。
平面点及其角点提取时，均是对16线中其中一条激光线点云进行判断是否为连续点还是断开边缘点。因此垂直方向上16条线独立处理。
作者在处理一条激光线即360度的一维点云，考虑到希望在360度中相对平均的提取，因此将360度的点云按照个数平均分成6份（注：并非按照角度进行平均而是按照分类后的有效点个数分成6份），分别进行提取；并在每一份根据每个点按照平滑参数从小到大进行排序，通过设置一个阈值来区分为平面点与角点；
角点特征点提取
将曲率参数从大到小进行遍历，即如此可最先遍历到最大的跳跃点，即边缘特征点。
其中边缘特征点条件：
- 不得是远处跳跃到近处的边缘点；
- 不得是地面上点；
- 并且平滑性需大于一定值；
- 特征点相邻的5个点也不得是特征点；即一个点为角点，则接下来相邻的5个点不得再是角点。
- 仅提取每份中排列在前20个点（即曲率最大的20个点）。
角点示例，其中蓝色点为角点，红色点为屏蔽点：
int ind = cloudSmoothness[k].ind; // 提取索引
if (cloudNeighborPicked[ind] == 0 && // 当前并非远方点变近的边缘的点、并且平滑度大于一定值、非地面数据（结论：就是提取水平方向连续断开的端点，且仅提取断开近处的端点）
cloudCurvature[ind] > _edge_threshold && // ？？？？？？？这里是提取出连续点中断开的端点，即连续点云断开的边缘点？？？？
segInfo.segmentedCloudGroundFlag[ind] == false) {
largestPickedNum++; // 统计满足上条件的个数
if (largestPickedNum <= 2) { // 记录最大的两个点，为最陡的两个点，即一圈最多6*2 = 12个点
cloudLabel[ind] = 2;
cornerPointsSharp->push_back(segmentedCloud->points[ind]);
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else if (largestPickedNum <= 20) { // 前20个点为稍微陡峭的两个点，
cloudLabel[ind] = 1;
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else { // 后面无需考虑
break;
}
cloudNeighborPicked[ind] = 1; // 此点近距离边缘端点，已经处理过并将其设置为1
for (int l = 1; l <= 5; l++) { // 如果当前点在最后5个点内，不处理
if( ind + l >= segInfo.segmentedCloudColInd.size() ) {
continue;
}
int columnDiff =
std::abs(int(segInfo.segmentedCloudColInd[ind + l] -
segInfo.segmentedCloudColInd[ind + l - 1])); // 如果相邻有效索引的两点，在水平上索超出10（即10个水平角度分辨率），便跳出，即不平坦
if (columnDiff > 10) break;
cloudNeighborPicked[ind + l] = 1; // 即与当前点相对平滑的5个点设置为1，
}
for (int l = -1; l >= -5; l--) { // 若当前点在前5个点内，不处理
if( ind + l < 0 ) {
continue;
}
int columnDiff =
std::abs(int(segInfo.segmentedCloudColInd[ind + l] - // 同理
segInfo.segmentedCloudColInd[ind + l + 1]));
if (columnDiff > 10) break;
cloudNeighborPicked[ind + l] = 1;
}
角点中包含 cornerPointsSharp（角点）、cornerPointsLessSharp（ 轻微角点），其中cornerPointsLessSharp即满足以上5个条件即可因此一圈最多120个点;而cornerPointsSharp则是每份提取角点中曲面最大的两个点，由于共6份，因此最多12个点，即最为明显的12个角点。
地平面特征点提取
平面特征则是角点特征的相反思想，即将曲面度参数从小到大进行遍历，即如此可最先遍历到最平滑点，连续平滑中间的点。
其中平滑特征点条件：
- 剔除已标记的；
- 必须是地面上点；
- 并且平滑性需小于一定值；
- 特征点相邻的5个点也不得是特征点，与角点类似，即已经是平面特征点，则接下来相邻的5个不得再是，除非相邻的点在扫描索引相差10个以上；
- 最平滑的前4个点；
for (int k = sp; k <= ep; k++) { // 平滑度从小到大遍历
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < _surf_threshold && // 平滑的点，且是地面上的点
segInfo.segmentedCloudGroundFlag[ind] == true) {
cloudLabel[ind] = -1; // 标记平滑地面的点为-1
surfPointsFlat->push_back(segmentedCloud->points[ind]); // 放入平滑地面点云
smallestPickedNum++;
if (smallestPickedNum >= 4) { // 即一圈最多为6*4 = 24个点
break;
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
if( ind + l >= segInfo.segmentedCloudColInd.size() ) {
continue;
}
int columnDiff =
std::abs(int(segInfo.segmentedCloudColInd.at(ind + l) -
segInfo.segmentedCloudColInd.at(ind + l - 1)));
if (columnDiff > 10) break; // 每选择一个点，则便使相邻的5个点，若是平滑的，则使其标记为1，
cloudNeighborPicked[ind + l] = 1; // 此操作可作为降采样功能，使特征点至少间隔5个点
}
for (int l = -1; l >= -5; l--) {
if (ind + l < 0) {
continue;
}
int columnDiff =
std::abs(int(segInfo.segmentedCloudColInd.at(ind + l) -
segInfo.segmentedCloudColInd.at(ind + l + 1)));
if (columnDiff > 10) break;
cloudNeighborPicked[ind + l] = 1;
}
}
}
相对较平的平面点云
除去两种角点特征点，剩下的所有点云均为相对较平的点。
for (int k = sp; k <= ep; k++) { // 除去陡峭的边缘点，均为，稍微平面的点
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
}
}
总结
lego-loam中之所以可以低性能实时进行slam，主要一个原因耗时的匹配算法并非直接采用原始点云而是提取了特征点进行后续匹配，其个数远远小于原始数据集合。
其loam中采用的特征点即为角点和平面点，采用每个点的曲面度来进行分类，本节即分析了具体提取方法。
2874
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
