FPFH特征匹配不上
Posted
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了FPFH特征匹配不上相关的知识，希望对你有一定的参考价值。
参考技术A 在计算Fpfh特征时，首先需要计算点集的法向量（法向量是点云的一个非常重要的特征，本该单独处理，仅在这里为了方便，少写两行代码，将其封装在FPFH特征的计算中），根据计算好的法向量，计算FPFH特征。计算fpfh特征时，近邻点集个数不易取得过大，，否则一则导致计算量增大，二会使得fpfh的计算失去意义（通其他特征计算一样，过大的近邻点集合不能反映局部特征）。学习LOAM笔记——特征点提取与匹配
学习LOAM笔记——特征点提取与匹配
学习LOAM笔记——特征点提取与匹配
兜兜转转一圈，最近又开始接触一些和SLAM相关的工作，LOAM是一个非常经典的激光SLAM框架，LOAM和VLOAM至今还在kitti榜上有着不错的表现，从这篇博客开始，我开始着手对LOAM以及LOAM相关的算法进行一次深入的学习，欢迎大家来多交流。
在视觉SLAM领域，特征点提取是一个从一开始就有的一个模块，而在激光SLAM领域，以我的理解，前端最开始演进的是类似于ICP和NDT这样一些直接匹配算法，而从LOAM开始，算法开始从激光点中提取3D特征点，3D特征点在我很早之前的一篇博客视觉SLAM总结——视觉特征子综述中就有过总结，但是当时看到的3D特征点大多复杂而实时性不高，在LOAM算法的前端设计的特征点提取算法简洁而快速，保证了精度和实时性。
关于特征点提取算法我主要参考的是A-LOAM的代码，同时还简单过了下Livox-LOAM和M-LOAM的代码：
在A-LOAM中，首先是对输入的激光点按照激光线进行分类，然后在激光线上计算每一个点的曲率，按照曲率提线点和面点，后端则将前后帧的线点和面点转到同一坐标系下后按照最近邻搜索进行匹配；
在Livox-LOAM中，操作大同小异，不同的地方主要在于，在A-LOAM中处理的是圆柱激光，因此激光线是平行线，而Livox-LOAM中处理的是Livox激光，其激光线是花瓣状，两者在分类过程中就有所不同，除此之外，Livox-LOAM中，算法还对接近视角边缘的点、反射强度过大或者过小的点和平面夹角过小的点进行了过滤；
在M-LOAM中，M-LOAM中处理的方式就是分开处理单个激光，然后再将特征点组合到一起，看这个算法我之前，我是想了解下多个激光雷达激光线混在一起后怎么去获得激光点的曲率，是不是必须先对激光点按照激光先进行分类才能计算曲率呢？看样子好像是的哦
下面以A-LOAM为例，整理下特征点提取的算法和代码，具体如下
1. 特征点提取
A-LOAM中特征点提取一共分为如下三步：
1.1 对激光点按线束分类
按照激光雷达的线束模型，每一个线束称为一个scan，一帧线束组成一帧sweep，首先我们需要计算每个激光在激光雷达坐标系下的角度，按照角度阈值对激光点进行线束归类，并同时将特征点归一化到 π \\pi π和 − π -\\pi −π之间，方便后面计算，这个归一化过程说起来简单，但是实际上是比较繁杂的，因为每个sweep不一定是从0度开始，也不一定是在0度结束，扫描一圈的角度差也不一定是 2 π 2\\pi 2π，因此代码中加了很多判断条件来处理该问题。
if (endOri - startOri > 3 * M_PI) //将角度限制在-PI和PI之间
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS); //将激光雷达点按扫描线进行归类
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;
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI; //计算激光雷达点倾斜角度，按照角度进行分类
int scanID = 0;
if (N_SCANS == 16)
{
scanID = int((angle + 15) / 2 + 0.5);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
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);
else
scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
else
{
printf("wrong scan number\\n");
ROS_BREAK();
}
float ori = -atan2(point.y, point.x); //下面应该是对角度进行归一化
if (!halfPassed)
{
if (ori < startOri - M_PI / 2)
{
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
else
{
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI;
}
}
float relTime = (ori - startOri) / (endOri - startOri); //计算相对时间
point.intensity = scanID + scanPeriod * relTime;
laserCloudScans[scanID].push_back(point);
}
1.2 计算激光点曲率
曲率的计算公式如下：
c
=
1
∣
S
∣
⋅
∥
X
(
k
,
i
)
L
∥
∑
j
∈
S
,
j
≠
i
∥
X
(
k
,
i
)
L
−
X
(
k
,
j
)
L
∥
c=\\frac{1}{|S| \\cdot\\left\\|\\mathbf{X}_{(k, i)}^{L}\\right\\|} \\sum_{j \\in S, j \\neq i}\\left\\|\\mathbf{X}_{(k, i)}^{L}-\\mathbf{X}_{(k, j)}^{L}\\right\\|
c=∣S∣⋅∥∥∥X(k,i)L∥∥∥1j∈S,j=i∑∥∥∥X(k,i)L−X(k,j)L∥∥∥
代码中对该公式进行了简化，选取当前点同一线束上左右各5个点进行曲率计算，具体代码如下：
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{ closestPointScanID
scanStartInd[i] = laserCloud->size() + 5;// 记录每个scan的开始index，忽略前5个点
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;// 记录每个scan的结束index，忽略后5个点，开始和结束处的点云scan容易产生不闭合的“接缝”，对提取edge feature不利
}
printf("prepare time %f \\n", t_prepare.toc());
for (int i = 5; i < cloudSize - 5; i++)
{
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 - 10 * laserCloud->points[i].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;
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 - 10 * laserCloud->points[i].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;
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 - 10 * laserCloud->points[i].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;
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; // 这里应该是计算曲率
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;// 点有没有被选选择为feature点
cloudLabel[i] = 0;// Label 2: corner_sharp
// Label 1: corner_less_sharp, 包含Label 2
// Label -1: surf_flat
// Label 0: surf_less_flat， 包含Label -1，因为点太多，最后会降采样
}
1.3 根据曲率提取特征点
为了使得在一周360度上有均匀的约束,，我们将一条激光线平均分为6块， 将块内的点按曲率大小排列，将曲率最大的2个点作为corner sharp点，曲率最大的前20个点作为corner less sharp点，曲率最小的4个点作为surf flat点，除上述三种类型外的其余的点以及surf flat点作为surf less flat点，surf less flat点相对会较多，因此最后还会对surf less flat点进行一次下采样，那么为什么要分为这四类点呢？
原因是在前后帧进行匹配时，当前帧的corner shape点会与前一帧的corner less shape点进行匹配，而surf flat点会与surf less flat点进行匹配，将曲率最大的点与曲率相对大的点进行匹配可以增加匹配成功概率。
for (int i = 0; i < N_SCANS; i++)// 按照scan的顺序提取4种特征点
{
if( scanEndInd[i] - scanStartInd[i] < 6)// 如果该scan的点数少于7个点，就跳过
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
for (int j = 0; j < 6; j++)// 将该scan分成6小段执行特征检测
{
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;// subscan的起始index
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;// subscan的结束index
TicToc t_tmp;
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);// 根据曲率有小到大对subscan的点进行sort
t_q_sort += t_tmp.toc();
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--)// 从后往前，即从曲率大的点开始提取corner feature
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)// 如果该点没有被选择过，并且曲率大于0.1
{
largestPickedNum++;
if (largestPickedNum <= 2)// 该subscan中曲率最大的前2个点认为是corner_sharp特征点
{
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else if (largestPickedNum <= 20)// 该subscan中曲率最大的前20个点认为是corner_less_sharp特征点
{
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
else
{
break;
}
cloudNeighborPicked[ind] = 1;// 标记该点被选择过了
// 与当前点距离的平方 <= 0.05的点标记为选择过，避免特征点密集分布
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<以上是关于FPFH特征匹配不上的主要内容，如果未能解决你的问题，请参考以下文章