python 实现pcd点云合并

• 这是一个demo
• 利用PCL提供的接口，从一个自己定义的点云中分割出一个平面
• 先导入头文件

#include <iostream>
#include <pcl/ModelCoefficients.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/visualization/pcl_visualizer.h>

• 先定义两个点云 分别是 in 和 out
• 其中in点云我们进行一个初始化，它里面的点基本上都在一个平面上 0x + 0y + 1z - 1 = 0 上
• 只有三个我们自己设置的离群值。可视化一下。
  

pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_in_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	
	point_cloud_in_ptr->width = 30;
	point_cloud_in_ptr->height = 1;
	point_cloud_in_ptr->resize(point_cloud_in_ptr->width * point_cloud_in_ptr->height);

	//此时点云中的30个点都在平面 0x + 0y + 1z - 1 = 0 上
	for (size_t i = 0; i < point_cloud_in_ptr->size(); i++) {
		point_cloud_in_ptr->points[i].x = 10 * rand() / (RAND_MAX + 1.0f);
		point_cloud_in_ptr->points[i].y = 10 * rand() / (RAND_MAX + 1.0f);
		point_cloud_in_ptr->points[i].z = 1.0;
	}

	// 设置离群值
	point_cloud_in_ptr->points[0].z = 0.0;
	point_cloud_in_ptr->points[1].z = -2.0;
	point_cloud_in_ptr->points[2].z = 3.0;

• 创建两个对象 一个是分割所用的模型对象 一个是分割后所得的那些点的索引对象

//分割时所用的模型的系数
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	//分割都所得的那些点的索引
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

• 创建分割对象 基于采样一致性的算法（SAmple Consensus）
• pcl::SACSegmentationpcl::PointXYZ seg;

//创建分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	//优化模型系数
	seg.setOptimizeCoefficients(true);
	//设置模型类型是随机采样的平面模型 模型里的四个参数是 ax + by + cz + d  = 0
	seg.setModelType(pcl::SACMODEL_PLANE); 
	//设置随机采样一致性方法类型
	seg.setMethodType(pcl::SAC_RANSAC); 
	//设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件
	seg.setDistanceThreshold(0.1);           
	//设置输入点云									  
	seg.setInputCloud(point_cloud_in_ptr);
	//分割，存储分割结果到点几何inliers及存储平面模型的系数coefficients
	seg.segment(*inliers, *coefficients);

• 输出模型的参数

std::cerr << "Model coefficients: " << coefficients->values[0] << " "
		<< coefficients->values[1] << " "
		<< coefficients->values[2] << " "
		<< coefficients->values[3] << " "
		<< coefficients->header    <<std::endl;

• Model coefficients: 0 0 1 -1 seq: 0 stamp: 0 frame_id:
• Model inliers: 27
• 模型的平面方程是 0x+0y+1z-1 = 0 得出的结果是27个点的索引， 也就是排除了3个离群点
  
• 全部代码

#include <iostream>
#include <pcl/ModelCoefficients.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/visualization/pcl_visualizer.h>



int main() {

	pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_in_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_out_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	
	point_cloud_in_ptr->width = 30;
	point_cloud_in_ptr->height = 1;
	point_cloud_in_ptr->resize(point_cloud_in_ptr->width * point_cloud_in_ptr->height);

	//此时点云中的30个点都在平面 0x + 0y + 1z - 1 = 0 上
	for (size_t i = 0; i < point_cloud_in_ptr->size(); i++) {
		point_cloud_in_ptr->points[i].x = 10 * rand() / (RAND_MAX + 1.0f);
		point_cloud_in_ptr->points[i].y = 10 * rand() / (RAND_MAX + 1.0f);
		point_cloud_in_ptr->points[i].z = 1.0;
	}

	// 设置离群值
	point_cloud_in_ptr->points[0].z = 0.0;
	point_cloud_in_ptr->points[1].z = -2.0;
	point_cloud_in_ptr->points[2].z = 3.0;


	//分割时所用的模型的系数
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	//分割都所得结
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

	
	//创建分割对象
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	//优化模型系数
	seg.setOptimizeCoefficients(true);
	//设置模型类型是随机采样的平面模型 模型里的四个参数是 ax + by + cz + d  = 0
	seg.setModelType(pcl::SACMODEL_PLANE); 
	//设置随机采样一致性方法类型
	seg.setMethodType(pcl::SAC_RANSAC); 
	//设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件
	seg.setDistanceThreshold(0.1);           
	//设置输入点云									  
	seg.setInputCloud(point_cloud_in_ptr);
	//分割，存储分割结果到点几何inliers及存储平面模型的系数coefficients
	seg.segment(*inliers, *coefficients);

	if (inliers->indices.size() == 0)
	{
		PCL_ERROR("Could not estimate a planar model for the given dataset.");
		return (-1);
	}
	std::cerr << "Model coefficients: " << coefficients->values[0] << " "
		<< coefficients->values[1] << " "
		<< coefficients->values[2] << " "
		<< coefficients->values[3] << " "
		<< coefficients->header    <<std::endl;
	std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
	for (size_t i = 0; i < inliers->indices.size(); ++i) {
		std::cerr << inliers->indices[i] << "    " << point_cloud_in_ptr->points[inliers->indices[i]].x << " "
			<< point_cloud_in_ptr->points[inliers->indices[i]].y << " "
			<< point_cloud_in_ptr->points[inliers->indices[i]].z << std::endl;
		point_cloud_out_ptr->push_back(point_cloud_in_ptr->points[inliers->indices[i]]);
	}
	std::cout << point_cloud_out_ptr->size() << std::endl;

	pcl::visualization::PCLVisualizer viewer("v1");
	viewer.addPointCloud(point_cloud_in_ptr, "in");
	viewer.addPointCloud(point_cloud_out_ptr, "out");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,255,255,255, "in");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "in");

	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "out");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "out");
	viewer.addCoordinateSystem(2.0);
	viewer.spin();


}