pcl中计算VFH特征，以及进行特征匹配，计算VFH描述子之间的欧式距离

在聚类分割后，会产生多个pcd文件，我们需要从中找出目标pcd，此时，就可以使用VFH（视点特征直方图），它先计算聚类分割产生的每个pcd的VFH，然后将每个VFH与特征库中的VFH比较（特征库就是目标pcd的VFH）。比较方式为计算两者VFH描述子的欧式距离，距离越小，说明两个VFH相关性越大，即两个pcd文件最相似，从而找出聚类分割后的目标pcd文件。代码如下：

包含头文件：

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/pcl_plotter.h>
#include <iostream>
#include <string>
#include <cmath>
#include <cstdio>

using namespace std;

1. 计算VFH特征

两个参数分别为：cloud指向点云文件的点云指针，vfh_dir为生成的VFH类型的pcd的存储路径

void VFH(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, string vfh_dir){
    //计算点云的VFH
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>());
    ne.setInputCloud(cloud);
    ne.setSearchMethod(tree1);
    ne.setRadiusSearch(0.01);

    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    ne.compute(*normals);


    pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
    vfh.setInputCloud(cloud);
    vfh.setInputNormals(normals);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());
    vfh.setSearchMethod(tree2);
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_ptr(new pcl::PointCloud<pcl::VFHSignature308>());
    vfh.compute(*vfh_ptr);//计算特征值，这是一组特征值
    //cout << vfh_ptr->size() << endl;
    pcl::io::savePCDFile(vfh_dir, *vfh_ptr, false);//将特征值保存
    //显示
    /*pcl::visualization::PCLPlotter plotter;
    plotter.addFeatureHistogram(*vfh_ptr, 200);
    plotter.plot();*/
}

2、VFH特征匹配，通过计算欧式距离来匹配目标文件

void feature_matching(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,int file_count, string file_dir, string vfh_dir) {
    //计算不同点云描述子（特征向量）之间的欧式距离
    /* 参数介绍
    cloud：指向点云文件的指针
    file_count：聚类分割产生的点云文件数量
    file_dir：聚类分割产生的点云文件存储路径
    vfh_dir：VFH存储路径*/
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_lib(new pcl::PointCloud<pcl::VFHSignature308>());
    pcl::PointCloud<pcl::VFHSignature308>::Ptr vfh_ptr(new pcl::PointCloud<pcl::VFHSignature308>());
    double thresh = 15; //设置阈值
    string file_dir1, vfh_dir1;
    int count = 0;//记录符合要求的pcd
    if (-1 == pcl::io::loadPCDFile("C:\\pcd\\library\\1902.pcd", *vfh_lib)) {
        cout << "load pcd file failed. please check it." << endl;
        return;
    }
    for (int j = 1; j <= file_count; j++) {
        file_dir1 = file_dir + to_string(j) + ".pcd";
        if (-1 == pcl::io::loadPCDFile(file_dir1, *cloud)) {
            cout << "load pcd file failed. please check it." << endl;
            return;
        }
        vfh_dir1 = vfh_dir + to_string(j) + ".pcd";
        VFH(cloud, vfh_dir1);
        if (-1 == pcl::io::loadPCDFile(vfh_dir1, *vfh_ptr)) {
            cout << "load pcd file failed. please check it." << endl;
            return;
        }
        double dist = 0; //存最终的距离
        for (int i = 0; i < 308; i++) {
            dist = dist + (static_cast<double>(vfh_lib->points[0].histogram[i]) - static_cast<double>(vfh_ptr->points[0].histogram[i])) * (static_cast<double>(vfh_lib->points[0].histogram[i]) - static_cast<double>(vfh_ptr->points[0].histogram[i]));
        }
        dist = sqrt(dist);
        cout << "特征值之间的欧氏距离" << dist << endl;
        if (dist > thresh) {
            //删除不符合的pcd文件以及vfh
            const char* savePath1 = file_dir1.c_str();
            const char* savePath2 = vfh_dir1.c_str();

            if (remove(savePath1) == 0 && remove(savePath2) == 0)
            {
                cout << "删除成功" << endl;
            }
            else
            {
                cout << "删除失败" << endl;
            }
        }
        else {
            count = j;//取得目标文件索引
        }
    }
    file_dir1 = file_dir + to_string(count) + ".pcd";
    if (-1 == pcl::io::loadPCDFile(file_dir1, *cloud)) {
        cout << "load pcd file failed. please check it." << endl;
        return;
    }
}