点云进行模型拟合分割

原创已于 2023-04-26 00:59:47 修改 · 331 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #计算机视觉 #c++

于 2023-04-26 00:29:32 首次发布

PCL 专栏收录该内容

15 篇文章

订阅专栏

本文介绍了如何利用pcl库的SACSegmentation接口对点云数据进行模型拟合和分割。通过设置最大分割次数为3，可以得到分割后的点云列表、拟合参数以及剩余点云信息。进一步，文章展示了提取地面点云和相关模型参数的过程，同时提供了扩展klib插件的代码示例。

pcl库中，对点云进行模型拟合的接口是 SACSegmentation。

对其进行可视化显示效果如下：

设置最多分割3次拟合平面，得到分割的点云列表、拟合参数、剩余点云

提取地面点云、模型参数、剩余的点云

扩展klib的插件代码如下:

struct SegSACSegmentation
{
    bool random = false;
    int model = pcl::SACMODEL_PLANE;
	int method = pcl::SAC_RANSAC;
    double distanceThreshold = 0.01;
    int max_iterations = 50;
    double probability = 0.99;
    bool optimizeCoefficients = true;
    double min_radius = -(std::numeric_limits<double>::max)();
    double max_radius = (std::numeric_limits<double>::max)();
    double radius = 0.0;
    double samplesMaxDist = 0.0;
    Eigen::Vector3f axis = Eigen::Vector3f::Zero();
    double epsAngle = 0;

	//////////////////////////////////////////////////////////////////////////
	int minPointCount = 100;
	int maxRunCount = 1;
	template<class PT> inline
	KArbit operator()(const pcl::shared_ptr<PointCloud<PT> >& cloud)
	{
		KArbitList resList;
		KShapeModelList modelList;

		SACSegmentation<PT> seg(random);
		seg.setModelType(model);
		seg.setMethodType(method);
		seg.setDistanceThreshold(distanceThreshold);
		seg.setMaxIterations(max_iterations);
		seg.setProbability(probability);
		seg.setOptimizeCoefficients(optimizeCoefficients);
		seg.setRadiusLimits(min_radius, max_radius);
		seg.setAxis(axis);
		seg.setEpsAngle(epsAngle);
		if (samplesMaxDist > 0)
			seg.setSamplesMaxDist(samplesMaxDist, pcl::search::KdTree<PT>::Ptr(new pcl::search::KdTree<PT>));

		ModelCoefficients  coefficients;
		PointIndicesPtr indicesPtr = pcl::make_shared<pcl::PointIndices>();

		pcl::ExtractIndices<PT> extract;
		pcl::shared_ptr<PointCloud<PT>> leftCld = cloud;
		for(int cnt = 0; cnt < maxRunCount; ++cnt)
		{	
			seg.setInputCloud(leftCld);
			seg.segment(*indicesPtr, coefficients);

			if (indicesPtr->indices.size() < minPointCount)
				break;

			extract.setInputCloud(leftCld);
			extract.setIndices(indicesPtr);

			auto cld0 = pcl::make_shared<PointCloud<PT>>();
			extract.setNegative(true);
			extract.filter(*cld0);
			leftCld = cld0;

			auto cld1 = pcl::make_shared<PointCloud<PT>>();
			extract.setNegative(false);
			extract.filter(*cld1);
			resList += cld1;
			KShapeModel shapeModel;
			shapeModel.coeffs = coefficients.values;
			shapeModel.type = model;
			modelList += shapeModel;
		}

		std::sort(resList.begin(), resList.end(), [](const pcl::shared_ptr<pcl::PointCloud<PT>>& v1, const pcl::shared_ptr<pcl::PointCloud<PT>>& v2) {
			return v1->size() > v2->size();
			});
		return KArbitList{ resList, modelList, leftCld };
	}
};

struct KPclSegSacModel : KPlugBase
{
	SegSACSegmentation p;

	KPclSegSacModel()
	{
		addProp("minPointCount").setType(PropTypeInt32).setValueAddr(&p.minPointCount);
		addProp("maxRunCount").setType(PropTypeInt32).setValueAddr(&p.maxRunCount);
		addProp("random").setType(PropTypeBool).setValueAddr(&p.random);
		addProp("model").setType(PropTypeEnumList).setValueAddr(&p.model).setEnumList({
			{"plane",SACMODEL_PLANE,},{"line",SACMODEL_LINE,},{"circle2d",SACMODEL_CIRCLE2D,},{"circle3d",SACMODEL_CIRCLE3D,},
			{"sphere",SACMODEL_SPHERE,},{"cylinder",SACMODEL_CYLINDER,},{"cone",SACMODEL_CONE,},{"torus",SACMODEL_TORUS,},
			{"paral_line",SACMODEL_PARALLEL_LINE,},{"perpendicular_plane",SACMODEL_PERPENDICULAR_PLANE,},{"paral_lines",SACMODEL_PARALLEL_LINES,},{"norm_plane",SACMODEL_NORMAL_PLANE,},
			{"norm_sphere",SACMODEL_NORMAL_SPHERE,},{"registration",SACMODEL_REGISTRATION,},{"registration_2d",SACMODEL_REGISTRATION_2D,},{"paral_plane",SACMODEL_PARALLEL_PLANE,},
			{"norm_paral_plane",SACMODEL_NORMAL_PARALLEL_PLANE,},{"stick",SACMODEL_STICK,},
		});
		addProp("method").setType(PropTypeEnumList).setValueAddr(&p.method).setEnumList({
			{"ransac",SAC_RANSAC,},{"lmeds",SAC_LMEDS,},{"msac",SAC_MSAC,},{"rransac",SAC_RRANSAC,},
			{"rmsac",SAC_RMSAC,},{"mlesac",SAC_MLESAC,},{"prosac",SAC_PROSAC,},
		});
		addProp("distanceThreshold").setType(PropTypeDouble).setValueAddr(&p.distanceThreshold);
		addProp("max_iterations").setType(PropTypeInt32).setValueAddr(&p.max_iterations);
		addProp("probability").setType(PropTypeDouble).setValueAddr(&p.probability);
		addProp("optimizeCoefficients").setType(PropTypeBool).setValueAddr(&p.optimizeCoefficients);
		addProp("min_radius").setType(PropTypeDouble).setValueAddr(&p.min_radius);
		addProp("max_radius").setType(PropTypeDouble).setValueAddr(&p.max_radius);
		addProp("radius").setType(PropTypeDouble).setValueAddr(&p.radius);
		addProp("samplesMaxDist").setType(PropTypeDouble).setValueAddr(&p.samplesMaxDist);
		addProp("epsAngle").setType(PropTypeDouble).setValueAddr(&p.epsAngle);
	}

	int __exec(const KArbit& src, KArbit& dst) override
	{
		dst = cld_op(src, p);
		return true;
	}
};