July 30, 2020

最新推荐文章于 2020-12-20 18:33:51 发布

tunan11

最新推荐文章于 2020-12-20 18:33:51 发布

阅读量160

点赞数

分类专栏：每天300行

本文链接：https://blog.csdn.net/naonao8986421/article/details/107702873

版权

每天300行专栏收录该内容

2 篇文章 0 订阅

订阅专栏

特别说明
1.本专栏（每日300行）里面的代码片段，只是自己学习记录用，如有侵权请联系我，我立即删除！！！
2.本页代码均来源于《点云库pcl从入门到精通》，如需要用于商业用途，请注明版权所有者（《点云库pcl从入门到精通》）。

代码片段1： PCLPlotter可视化特征直方图

/*
Point-Cloud-Processing-example/第五章/4plotter/source/pcl_plotter_demo.cpp
*/
#define _CRT_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#include<pcl/visualization/pcl_plotter.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/console/parse.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/normal_space.h>
#include <pcl/common/eigen.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/visualization/histogram_visualizer.h>


#include<iostream>
#include<vector>
#include<utility>
#include<math.h>  //for abs()

using namespace std;
using namespace pcl::visualization;
using namespace pcl::console;


void generateData(double *ax, double *acos, double *asin, int numPoints)
{
	double inc = 7.5 / (numPoints - 1);
	for (int i = 0; i < numPoints; ++i)
	{
		ax[i] = i*inc;
		acos[i] = cos(i * inc);
		asin[i] = sin(i * inc);
	}
}

//.....................回调函数....................
double step(double val)
{
	if (val > 0)
		return (double)(int)val;
	else
		return (double)((int)val - 1);
}

double identity_i(double val)
{
	return val;
}


int main(int argc,char **argv)
{
	if (argc < 2)
	{
		std::cout<< ".exe source.pcd - r 0.005 - ds 5"<<endl;
		return 0;
	}
	float voxel_re = 0.005, ds_N = 5;
	parse_argument(argc, argv, "-r", voxel_re);
	parse_argument(argc, argv, "-ds", ds_N);
	//调节下采样的分辨率以保持数据处理的速度
	//下采样
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile(argv[1], *cloud_src);
	std::vector<int>indices1;
	pcl::removeNaNFromPointCloud(*cloud_src, *cloud_src, indices1);
	pcl::PointCloud < pcl::PointXYZ >::Ptr ds_src(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::VoxelGrid < pcl::PointXYZ> grid;
	grid.setLeafSize(voxel_re, voxel_re, voxel_re);
	grid.setInputCloud(cloud_src);
	grid.filter(*ds_src);

	//计算法向量
	pcl::PointCloud<pcl::Normal>::Ptr norm_src(new pcl::PointCloud < pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree<pcl::PointXYZ>());
	pcl::NormalEstimation < pcl::PointXYZ, pcl::Normal > ne;
	PCL_INFO("	Normal Estimation - Source \n");
	ne.setInputCloud(ds_src);
	ne.setSearchSurface(cloud_src);
	ne.setSearchMethod(tree_src);
	ne.setRadiusSearch(ds_N * 2 * voxel_re);
	ne.compute(*norm_src);

	//提取关键点
	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_src(new pcl::PointCloud < pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints_tgt(new pcl::PointCloud<pcl::PointXYZ>);
	grid.setLeafSize(ds_N*voxel_re, ds_N*voxel_re, ds_N*voxel_re);
	grid.setInputCloud(ds_src);
	grid.filter(*keypoints_src);

	//Feature_descriptor
	PCL_INFO("FPFH-Feature Descriptor\n");
	pcl::FPFHEstimation < pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est_src;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_fpfh_src(new pcl::search::KdTree<pcl::PointXYZ>);
	fpfh_est_src.setSearchMethod(tree_fpfh_src);
	fpfh_est_src.setSearchSurface(ds_src);
	fpfh_est_src.setInputCloud(keypoints_src);
	fpfh_est_src.setInputNormals(norm_src);
	fpfh_est_src.setRadiusSearch(2 * ds_N*voxel_re);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_src(new pcl::PointCloud<pcl::FPFHSignature33>);
	fpfh_est_src.compute(*fpfh_src);


	//定义绘图器
	PCLPlotter*plotter = new PCLPlotter("My plotter");
	//设置特性
	plotter->setShowLegend(true);
	std::cout << pcl::getFieldsList<pcl::FPFHSignature33>(*fpfh_src);
	plotter->addFeatureHistogram<pcl::FPFHSignature33>(*fpfh_src, "fpfh", 5, "one_fpfh");
	//显示两秒
	plotter->setWindowSize(800, 600);
	plotter->spinOnce(30000000);
	plotter->clearPlots();

	//产生对应点
	int numPoints = 69;
	double ax[100], acos[100],asin[100];
	generateData(ax, acos, asin, numPoints);

	//添加绘图数据
	plotter->addPlotData(ax, acos, numPoints, "cos");
	plotter->addPlotData(ax, acos, numPoints, "sin");

	//显示2s
	plotter->spinOnce(30000);
	plotter->clearPlots();

	//绘制隐式函数
	//设置y轴范围
	plotter->setYRange(-10, 10);

	//定义多项式
	vector<double> func1(1,0);
	func1[0] = 1;
	vector<double> func2(3, 0);
	func2[2] = 1;

	plotter->addPlotData(std::make_pair(func1,func2),-10,-10,"y = 1/x^2",100, vtkChart::POINTS);
	plotter->spinOnce(2000);

	plotter->addPlotData(func2, -10, 10, "y = x^2");
	plotter->spinOnce(2000);

	//回调函数
	plotter->addPlotData(identity_i, -10, 10, "identity");
	plotter->spinOnce(2000);

	plotter->addPlotData(abs, -10, 10, "abs");
	plotter->spinOnce(2000);

	plotter->addPlotData(step, -10, 10, "step", 100, vtkChart::POINTS);
	plotter->spinOnce(2000);

	plotter->clearPlots();

	//一个简单的动画

	vector<double> fsq(3,0);
	fsq[2] = -100;
	while (plotter->wasStopped())
	{
		if (fsq[2] == 100)
			fsq[2] = -100;
		fsq[2]++;
		char str[50];
		sprintf(str, "y = %dx^2", (int)fsq[2]);
		plotter->addPlotData(fsq, -10, 10, str);
		plotter->setYRange(-1, 1);
		plotter->spinOnce(100);
		plotter->clearPlots();
	}
	return 1;
}

代码片段2：使用直通滤波器对点云进行滤波处理

/*
Point-Cloud-Processing-example/第六章/1 passthrough/source/passthrough.cpp
*/
#define _CRT_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#include <iostream>
#include <ctime>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
int main(int argc,char**argv)
{
	srand(time(0));
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	cloud->width = 5;
	cloud->height = 1;
	cloud->points.resize(cloud->width*cloud->height);
	for (size_t i = 0; i < cloud->points.size(); ++i)
	{
		cloud->points[i].x = rand() / (RAND_MAX + 1.0f) - 0.5;
		cloud->points[i].x = rand() / (RAND_MAX + 1.0f) - 0.5;
		cloud->points[i].x = rand() / (RAND_MAX + 1.0f) - 0.5;
	}
	std::cerr << "Cloud before filtering: " << std::endl;
	for (size_t i = 0; i < cloud->points.size(); ++i)
		std::cerr << "    " << cloud->points[i].x << " "
		<< cloud->points[i].y << " "
		<< cloud->points[i].z << std::endl;
	//创建滤波器对象
	pcl::PassThrough<pcl::PointXYZ> pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(0.0, 0.1);
	pass.filter(*cloud_filtered);

	std::cerr << "Cloud after filtering: " << std::endl;
	for (size_t i = 0; i < cloud_filtered->points.size(); ++i)
		std::cerr << "    " << cloud_filtered->points[i].x << " "
		<< cloud_filtered->points[i].y << " "
		<< cloud_filtered->points[i].z << std::endl;

	return(0);

}

代码片段3: 使用VoxelGrid滤波器对点云进行下采样

/*
Point-Cloud-Processing-example/第六章/2 voxel_grid/source/voxel_grid.cpp
*/
#define _CRT_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <sensor_msgs/PointCloud2.h>


int main(int argc, char **argv)
{
	sensor_msgs::PointCloud2::Ptr cloud(new sensor_msgs::PointCloud2());
	sensor_msgs::PointCloud2::Ptr cloud_filtered(new sensor_msg::PointCloud2());
	//填入数据点云
	pcl::PCDReader reader;
	// 把路径改为自己存放文件的路径
	reader.read("C:\\Users\\1987wangsanguo\\Desktop\\PCD_Viewer\\PCD_Viewer\\2f_only_voxel.pcd", *cloud);
	std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
		<< " data points (" << pcl::getFieldsList(*cloud) << ").";
	//创建滤波器对象
	pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);
	sor.filter(*cloud_filtered);
	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
		<< " data points (" << pcl::getFieldsList(*cloud_filtered) << ").";
	pcl::PCDWriter writer;
	writer.write("2f.pcd", *cloud_filtered,
		Eigen::Vector4f::Zero(), Eigen::Quaternionf::Identity(), false);
	return (0);


}

代码片段4：使用StatisticalOutlierRemoval滤波器移除离群点

/*
Point-Cloud-Processing-example/第六章/3 statistical_removal/source/statistical_removal.cpp
*/

#define _CRT_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>

int main(int argc, char **argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud < pcl::PointXYZ>);
	//填入点云数据
	pcl::PCDReader reader;
	//把路径改为自己存放文件的路径
	reader.read<pcl::PointXYZ>("D:\\数据集\\royzou-Point-Cloud-Processing-example-master\\Point-Cloud-Processing-example\\第六章\\3 statistical_removal\\source\\table_scene_lms400.pcd", *cloud);
	std::cerr << "Cloud before filtering: " << std::endl;
	std::cerr << *cloud << std::endl;
	//创建滤波器对象
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud(cloud);
	sor.setMeanK(50);
	sor.setStddevMulThresh(1.0);
	sor.filter(*cloud_filtered);
	std::cerr << "Cloud after filtering: " << std::endl;
	std::cerr << *cloud_filtered << std::endl;
	pcl::PCDWriter writer;
	writer.write<pcl::PointXYZ>("D:\\数据集\\royzou-Point-Cloud-Processing-example-master\\Point-Cloud-Processing-example\\第六章\\3 statistical_removal\\source\\table_scene_lms400_inliers.pcd", *cloud_filtered, false);
	sor.setNegative(true);
	sor.filter(*cloud_filtered);
	writer.write<pcl::PointXYZ>("D:\\数据集\\royzou-Point-Cloud-Processing-example-master\\Point-Cloud-Processing-example\\第六章\\3 statistical_removal\\source\\table_scene_lms400_outliers.pcd", *cloud_filtered, false);
	return (0);
}

代码片段5：使用参数化模型投影点云

/*
Point-Cloud-Processing-example/第六章/4 project_inliers/source/project_inliers.cpp
*/
#define _CRT_SECURE_NO_WARNINGS
#define _SCL_SECURE_NO_WARNINGS
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);
	// 填入点云数据
	cloud->width = 5;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);
	for (size_t i = 0; i < cloud->points.size(); ++i)
	{
		cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
	}
	std::cerr << "Cloud before projection: " << std::endl;
	for (size_t i = 0; i < cloud->points.size(); ++i)
		std::cerr << "    " << cloud->points[i].x << " "
		<< cloud->points[i].y << " "
		<< cloud->points[i].z << std::endl;
	// 创建一个系数为X=Y=0,Z=1的平面
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	coefficients->values.resize(4);
	coefficients->values[0] = coefficients->values[1] = 0;
	coefficients->values[2] = 1.0;
	coefficients->values[3] = 0;
	// 创建滤波器对象
	pcl::ProjectInliers<pcl::PointXYZ> proj;
	proj.setModelType(pcl::SACMODEL_PLANE);
	proj.setInputCloud(cloud);
	proj.setModelCoefficients(coefficients);
	proj.filter(*cloud_projected);

	std::cerr << "Cloud after projection: " << std::endl;
	for (size_t i = 0; i < cloud_projected->points.size(); ++i)
		std::cerr << "    " << cloud_projected->points[i].x << " "
		<< cloud_projected->points[i].y << " "
		<< cloud_projected->points[i].z << std::endl;

	return (0);
}

代码片段6: 从一个点云种提取一个子集

/*
Point-Cloud-Processing-example/第六章/5 extract_indices/source/extract_indices.cpp
*/

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
int main(int argc, char** argv)
{
	sensor_msgs::PointCloud2::Ptr cloud_blob(new sensor_msgs::PointCloud2), cloud_filtered_blob(new sensor_msgs::PointCloud2);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
	// 填入点云数据
	pcl::PCDReader reader;
	reader.read("table_scene_lms400.pcd", *cloud_blob);
	std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;
	// 创建滤波器对象:使用叶大小为1cm的下采样
	pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
	sor.setInputCloud(cloud_blob);
	sor.setLeafSize(0.01f, 0.01f, 0.01f);
	sor.filter(*cloud_filtered_blob);
	// 转化为模板点云
	pcl::fromROSMsg(*cloud_filtered_blob, *cloud_filtered);
	std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
	// 将下采样后的数据存入磁盘
	pcl::PCDWriter writer;
	writer.write<pcl::PointXYZ>("D:\\数据集\\royzou - Point - Cloud - Processing - example - master\\Point - Cloud - Processing - example\\第六章\\5 extract_indices\\source\\table_scene_lms400_downsampled.pcd", *cloud_filtered, false);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
	// 创建分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	// 可选
	seg.setOptimizeCoefficients(true);
	// 必选
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(1000);
	seg.setDistanceThreshold(0.01);
	// 创建滤波器对象
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	int i = 0, nr_points = (int)cloud_filtered->points.size();
	// 当还有30%原始点云数据时
	while (cloud_filtered->points.size() > 0.3 * nr_points)
	{
		// 从余下的点云中分割最大平面组成部分
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
			break;
		}
		// 分离内层
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);
		extract.setNegative(false);
		extract.filter(*cloud_p);
		std::cerr << "PointCloud representing the planar component: " << cloud_p->width * cloud_p->height << " data points." << std::endl;
		std::stringstream ss;
		ss << "table_scene_lms400_plane_" << i << ".pcd";
		writer.write<pcl::PointXYZ>(ss.str(), *cloud_p, false);
		// 创建滤波器对象
		extract.setNegative(true);
		extract.filter(*cloud_f);
		cloud_filtered.swap(cloud_f);
		i++;
	}
	return (0);
}