用PFH(点特征直方图)描述子对点云中提取出来的特征点进行定量描述。
可以使用PCL库实现,具体代码:
pcl::PFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::PFHSignature125> pfh;
pfh.setInputCloud(FeatureCloud);
pfh.setInputNormals(FeatureNormals);
pcl::PointCloud<pcl::PFHSignature125>::Ptr pfhs(new pcl::PointCloud<pcl::PFHSignature125>());
pfh.setRadiusSearch(20);
pfh.compute(*pfhs);
pfhs就是计算出来的PFH,为输入点云内的所有点都计算出一个125x1的直方图信息。
尝试参考PCL自己实现PFH计算,代码:
//一点的PFH数组
struct PFH
{
float fHistogram[125];
//static int descriptorSize() { return 125; }
//friend std::ostream& operator << (std::ostream& os, const PFHSignature125& p);
}
//pfh特征直方图计算
vector<PFH> FinalPFH;
int iSearchRadius = 400;//搜寻邻域点阈值 距离平方
float fPFHTuples[4] = { 0.0,0.0,0.0,0.0 };//点对PFH四元组
int iFeatureBinIndex[3];//点对四要素标准化的索引
int iBinNum = 5;//特征值范围的子区间bin个数
int iHistogramIndex = 0;//特征值归入直方图的索引
int iMulti = 1;//特征值归入直方图的计算乘数
for (int iter = 0; iter < Cloud->size(); ++iter)
{
PFH PointPFH;
for (int i = 0; i < 125; ++i)
PointPFH.fHistogram[i] = 0.0;
//查找邻域点
vector<int> vNeiborPointIdx;
for (int i = 0; i < FeatureCloud->size(); ++i)
{
if (PointDistance(FeatureCloud->at(iter), FeatureCloud->at(i)) < iSearchRadius)
vNeiborPointIdx.push_back(i);
}
float fHistogramIncrease = 100.0f / static_cast<float> (vNeiborPointIdx.size() * (vNeiborPointIdx.size() - 1) / 2);
//计算该点的PFH
for (int i = 0; i < vNeiborPointIdx.size(); ++i)
{
for (int j = 0; j < i; ++j)
{
Vector4f p1(FeatureCloud->at(vNeiborPointIdx.at(i)).x, FeatureCloud->at(vNeiborPointIdx.at(i)).y, FeatureCloud->at(vNeiborPointIdx.at(i)).z, 1.0f);
Vector4f p2(FeatureCloud->at(vNeiborPointIdx.at(j)).x, FeatureCloud->at(vNeiborPointIdx.at(j)).y, FeatureCloud->at(vNeiborPointIdx.at(j)).z, 1.0f);
Vector4f n1(FeatureNormals->at(vNeiborPointIdx.at(i)).normal_x, FeatureNormals->at(vNeiborPointIdx.at(i)).normal_y, FeatureNormals->at(vNeiborPointIdx.at(i)).normal_z, 0.0f);
Vector4f n2(FeatureNormals->at(vNeiborPointIdx.at(j)).normal_x, FeatureNormals->at(vNeiborPointIdx.at(j)).normal_y, FeatureNormals->at(vNeiborPointIdx.at(j)).normal_z, 0.0f);
computePairFeatures(p1, n1, p2, n2, fPFHTuples[0], fPFHTuples[1], fPFHTuples[2], fPFHTuples[3]);
//标准化点对四要素到0 1 2 3 4
iFeatureBinIndex[0] = static_cast<int> (floor(iBinNum * ((fPFHTuples[0] + M_PI) * (1.0/(2.0*M_PI)))));
if (iFeatureBinIndex[0] < 0) iFeatureBinIndex[0] = 0;
if (iFeatureBinIndex[0] >= iBinNum) iFeatureBinIndex[0] = iBinNum - 1;
iFeatureBinIndex[1] = static_cast<int> (floor(iBinNum * ((fPFHTuples[1] + 1.0) * 0.5)));
if (iFeatureBinIndex[1] < 0) iFeatureBinIndex[1] = 0;
if (iFeatureBinIndex[1] >= iBinNum) iFeatureBinIndex[1] = iBinNum - 1;
iFeatureBinIndex[2] = static_cast<int> (floor(iBinNum * ((fPFHTuples[2] + 1.0) * 0.5)));
if (iFeatureBinIndex[2] < 0) iFeatureBinIndex[2] = 0;
if (iFeatureBinIndex[2] >= iBinNum) iFeatureBinIndex[2] = iBinNum - 1;
// 归入直方图
iHistogramIndex = 0;
iMulti = 1;
for (int d = 0; d < 3; ++d)
{
iHistogramIndex += iMulti * iFeatureBinIndex[d];
iMulti *= iBinNum;
}
PointPFH.fHistogram[iHistogramIndex] += fHistogramIncrease;
}
}
FinalPFH.push_back(PointPFH);
}
结果打印:
FILE* pf;
fopen_s(&pf,"1.txt","w");
for(int i=0;i<Cloud->size();++i)
{
for(int j=0;j<125;++j)
{
fprintf(pf,"%f ",FinalPFH->at(i).histogram[j]);
}
fprintf(pf,"\n");
}
fclose(pf);