点云平面拟合和球面拟合

一、介绍

In this tutorial we learn how to use a RandomSampleConsensus with a plane model to obtain the cloud fitting to this model.

pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));

pcl::SACSegmentation<pcl::PointXYZ> seg;

pcl::ModelOutlierRemoval<pcl::PointXYZ> sphere_filter;

二、代码

#include <iostream>
#include <thread>
#include <pcl/point_types.h>
#include <pcl/common/io.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std::chrono_literals;

pcl::visualization::PCLVisualizer::Ptr simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
	viewer->addCoordinateSystem (1.0, "global");
	viewer->initCameraParameters();
	return (viewer);
}

void ranFit()
{
	bool is_plane = true;
	bool is_show_final = false;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);

	cloud->width = 500;
	cloud->height = 1;
	cloud->is_dense = false;
	cloud->points.resize(cloud->width * cloud->height);
	for (int i = 0; i < cloud->size(); ++i)
	{
		if (is_plane==false)
		{
			// x*x+y*y+z*z=1
			(*cloud)[i].x = 2.0 * rand() / (RAND_MAX + 1.0) - 1.0;
			(*cloud)[i].y = 2.0 * rand() / (RAND_MAX + 1.0) - 1.0;
			if ((*cloud)[i].x * (*cloud)[i].x + ((*cloud)[i].y * (*cloud)[i].y) > 1)
				(*cloud)[i].z = 2.0 * rand() / (RAND_MAX + 1.0) - 1.0;
			else if (i % 2 == 0)
				(*cloud)[i].z = sqrt(1 - ((*cloud)[i].x * (*cloud)[i].x) - ((*cloud)[i].y * (*cloud)[i].y));
			else
				(*cloud)[i].z = -sqrt(1 - ((*cloud)[i].x * (*cloud)[i].x) - ((*cloud)[i].y * (*cloud)[i].y));
		}
		else
		{
			// 0.5x+0.5y-z=0
			(*cloud)[i].x = rand() / (RAND_MAX + 1.0);
			(*cloud)[i].y = rand() / (RAND_MAX + 1.0);
			if (i % 3 == 0)
				(*cloud)[i].z = rand() / (RAND_MAX + 1.0);
			else
				(*cloud)[i].z = 0.5 * (*cloud)[i].x + 0.5 * (*cloud)[i].y;
		}
	}

	std::vector<int> inliers;
	std::vector<int> outliers;
	pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
	Eigen::VectorXf coef;
	if (is_plane)
	{
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
		ransac.setDistanceThreshold(.01);
		ransac.computeModel();
		ransac.getInliers(inliers);
		ransac.getModelCoefficients(coef);
	}
	else
	{
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
		ransac.setDistanceThreshold(.01);
		ransac.computeModel();
		ransac.getInliers(inliers);
		ransac.getModelCoefficients(coef);
	}
	std::cout << coef << std::endl;
	pcl::copyPointCloud(*cloud, inliers, *final);

	pcl::visualization::PCLVisualizer::Ptr viewer;
	if (is_show_final)
		viewer = simpleVis(final);
	else
		viewer = simpleVis(cloud);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		std::this_thread::sleep_for(100ms);
	}
}

int main()
{
	ranFit();
	return (0);
}

 

 

 

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <thread>

using namespace std::chrono_literals;

pcl::visualization::PCLVisualizer::Ptr simpleView(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
	viewer->addCoordinateSystem(1.0, "global");
	viewer->initCameraParameters();
	return viewer;
}

void planeSeg()
{
	// 生成输入点云数据
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	cloud->width = 15;
	cloud->height = 1;
	cloud->points.resize(cloud->width * cloud->height);
	for (auto& point : *cloud)
	{
		point.x = 10 * rand() / (RAND_MAX + 1.0f);
		point.y = 10 * rand() / (RAND_MAX + 1.0f);
		point.z = 1.0;
	}
	(*cloud)[0].z = -2.0;
	(*cloud)[3].z = 2.0;
	(*cloud)[6].z = 4.0;

	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.setDistanceThreshold(0.1);  // 距离门限
	seg.setInputCloud(cloud);
	seg.segment(*inliers, *coefficients);  // 切割

	// 打印模型参数
	std::cout << "Model coefficients: " << coefficients->values[0] 
		<< " " << coefficients->values[1] << " " << coefficients->values[2] 
		<< " " << coefficients->values[3] << std::endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud(*cloud, inliers->indices, *final);  // 过滤非平面点

	pcl::visualization::PCLVisualizer::Ptr viewer = simpleView(final);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		std::this_thread::sleep_for(100ms);
	}
}
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/model_outlier_removal.h>

void sphereFilter()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);

	std::size_t noise_size = 5;
	std::size_t sphere_data_size = 10;
	cloud->width = noise_size + sphere_data_size;
	cloud->height = 1;
	cloud->points.resize(15 * 1);

	for (std::size_t i = 0; i < noise_size; ++i)
	{
		(*cloud)[i].x = rand() / (RAND_MAX + 1.0) * 2.0 - 1.0;
		(*cloud)[i].y = rand() / (RAND_MAX + 1.0) * 2.0 - 1.0;
		(*cloud)[i].z = rand() / (RAND_MAX + 1.0) * 2.0 - 1.0;
	}

	double rand_x1 = 1;
	double rand_x2 = 1;
	for (std::size_t i = noise_size; i < (noise_size + sphere_data_size); ++i)
	{
		while (pow(rand_x1, 2) + pow(rand_x2, 2) >= 1)
		{
			rand_x1 = rand() / (RAND_MAX + 1.0) * 2.0 - 1.0;  // -1 ~ 1
			rand_x2 = rand() / (RAND_MAX + 1.0) * 2.0 - 1.0;  // -1 ~ 1
		}
		double pre_calc = sqrt(1 - pow(rand_x1, 2) - pow(rand_x2, 2));
		(*cloud)[i].x = 2 * rand_x1 * pre_calc;
		(*cloud)[i].y = 2 * rand_x2 * pre_calc;
		(*cloud)[i].z = 1 - 2 * (pow(rand_x1, 2) + pow(rand_x2, 2));
		rand_x1 = 1;
		rand_x2 = 1;
	}

	std::cerr << "Cloud before filtering: " << std::endl;
	for (const auto& point : *cloud)
		std::cout << "    " << point.x << " " << point.y << " " << point.z << std::endl;

	pcl::ModelCoefficients sphere_coeff;
	sphere_coeff.values.resize(4);
	sphere_coeff.values[0] = 0;  // center_x
	sphere_coeff.values[1] = 0;  // center_y
	sphere_coeff.values[2] = 0;  // center_z
	sphere_coeff.values[3] = 1;  // radius

	pcl::ModelOutlierRemoval<pcl::PointXYZ> sphere_filter;
	sphere_filter.setModelCoefficients(sphere_coeff);
	sphere_filter.setThreshold(0.005);
	sphere_filter.setModelType(pcl::SACMODEL_SPHERE); // 球体
	sphere_filter.setInputCloud(cloud);
	sphere_filter.filter(*filtered);

	std::cerr << "Sphere after filtering: " << std::endl;
	for (const auto& point : *filtered)
		std::cout << "    " << point.x << " " << point.y << " " << point.z << std::endl;
}

三、参考

How to use Random Sample Consensus model — Point Cloud Library 0.0 documentation

Plane model segmentation — Point Cloud Library 0.0 documentation

Filtering a PointCloud using ModelOutlierRemoval — Point Cloud Library 0.0 documentation

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值