点云（一）计算FPFH特征并显示

最新推荐文章于 2024-09-30 10:07:25 发布

hero_heart

最新推荐文章于 2024-09-30 10:07:25 发布

阅读量109

点赞数 2

文章标签：算法

本文链接：https://blog.csdn.net/hero_heart/article/details/141292711

版权

计算FPFH特征并显示。

代码如下：

#include <iostream>
#include<vector>
#include <string>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/fpfh.h> 
#include <pcl/common/common_headers.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/parse.h>
#include <pcl/visualization/pcl_plotter.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_line.h> // 拟合直线
#include <pcl/filters/extract_indices.h>
#include  <Eigen/SVD>
#include <pcl/filters/radius_outlier_removal.h>

void computeFPFH()
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	// pcl::PCDReader reader;
	// reader.read("/home/zhu/pcl_fpfh/fpfh/bunny.pcd", *cloud);
    cloud->width = 20;
    cloud->height = 1;
    cloud->is_dense = false;
     cloud->points.resize(20);


    FILE* fp_s = nullptr;
    fp_s = fopen("/home/zhu/pcl_fpfh/fpfh/2.txt","r");
    char data[300];
    float x,y,z;
    int valid_index =0;
    for(int i =0; i< 20;i++)
    {
        fgets(data, 300, fp_s);
        sscanf(data,"%f %f %f",&x, &y,&z); 
        cloud->points[valid_index].x = x;
        cloud->points[valid_index].y = y;
        cloud->points[valid_index].z = z;
                std::cout<<valid_index<<"  "<<cloud->points[valid_index].x<<"   "<<cloud->points[valid_index].y<<"  "<<cloud->points[valid_index].z<<std::endl;
        valid_index++;

    }
	//法向量计算
	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
	n.setInputCloud(cloud);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	n.setSearchMethod(tree);
	n.setNumberOfThreads(4);
	n.setKSearch(20);
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	n.compute(*normals);
	//计算特征
	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
	fpfh.setInputCloud(cloud);
	fpfh.setInputNormals(normals);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>());
	fpfh.setSearchMethod(tree2);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_fe(new pcl::PointCloud<pcl::FPFHSignature33>());
	//注意：此处使用的半径必须要大于估计表面法线时使用的半径
	fpfh.setRadiusSearch(1000000);
	fpfh.compute(*fpfh_fe);
    
	cout << "phf feature size : " << fpfh_fe->points.size() << endl;
    pcl::PCDWriter writer;
    writer.write<pcl::FPFHSignature33>("fpfh_cloud.pcd", *fpfh_fe);

    pcl::io::savePCDFileASCII("custom_point_cloud.pcd", *fpfh_fe);
    std::cout << "Saved " << cloud->points.size() << " points to custom_point_cloud.pcd" << std::endl;

	 pcl::visualization::PCLPlotter plotter;
	 plotter.addFeatureHistogram(*fpfh_fe, 200);
     plotter.plot();
}