一种点云聚类分割的处理流程

iQTan

已于 2022-05-25 15:55:24 修改

阅读量1.6k

点赞数 1

分类专栏：计算机视觉文章标签：点云聚类分割

于 2019-11-26 11:09:07 首次发布

本文链接：https://blog.csdn.net/qq_23149979/article/details/103252507

版权

计算机视觉专栏收录该内容

13 篇文章 2 订阅

订阅专栏

本文是在上文基础上，记录了一种点云聚类分割的处理流程。

程序流程：
>初始化：
    >说明命名空间
    >定义计时器（double类型）
    >定义点云类型 PointXYZRGB
>创建图像矩阵
>遍历深度图
>点云滤波
>平面分割(RANSAC)
>提取平面（展示并输出）
>点云聚类分割
>信息处理与输出
>结束程序

经过处理之后，目标点云可以更清晰地被分割出来，如下图（与上文图不是同一场景）。
在这里插入图片描述
代码：

#include <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <boost/thread/thread.hpp>
#include <boost/filesystem.hpp>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/StdVector>
#include <Eigen/Geometry>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/common/angles.h>
#include <pcl/common/common.h>
#include <boost/thread/thread.hpp>
#include <pcl/point_types.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <vtkAutoInit.h>
#include <ctime>
#include <cstdlib>
#include <windows.h>
#include <map>
#include <list>
#include <vector>
#include "atlbase.h"
#include "atlstr.h"

using namespace std;
double tstart, tstop, ttime;

// 定义点云类型
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

typedef pcl::PointXYZ point_OBB;

//随机产生颜色
int *rand_rgb() {
	int *rgb = new int[3];
	rgb[0] = rand() % 255;
	rgb[1] = rand() % 255;
	rgb[2] = rand() % 255;
	//cout << "RGB:"<<rgb[1] << endl;
	return rgb;
}

//输出信息到“输出”窗口
void OutputDebugPrintf(const char *strOutputString, ...)
{
	char strBuffer[4096] = { 0 };
	va_list vlArgs;
	va_start(vlArgs, strOutputString);
	_vsnprintf_s(strBuffer, sizeof(strBuffer) - 1, strOutputString, vlArgs);
	//vsprintf(strBuffer, strOutputString, vlArgs);
	va_end(vlArgs);
	OutputDebugString(CA2A(strBuffer));
}

// 主函数 
extern "C" __declspec(dllexport) int main(int argc, char** argv)
//int main(int argc, char** argv)
{
	//cout << *rand_rgb()<< endl;
	fstream filepath;
	filepath.open("./data/filepath.txt", ios::in);
	vector<string> strVec;
	if (filepath.fail())
	{
		cout << "File ERROR! Cannot find gray.png &pointclod.pcd files!" << endl;
		cout << "Please make sure your files exist!" << endl;
		cout << "You can configure it as follow:" << endl;
		cout << "./data/images/rgb.png" << endl;
		cout << "./data/pcdata/cache.pcd" << endl;

		OutputDebugPrintf("File ERROR!\n");
		return -1;
	}

	while (!filepath.eof())  //行0 - 行lines对应strvect[0] - strvect[lines]
	{
		string inbuf;
		getline(filepath, inbuf, '\n');
		strVec.push_back(inbuf);
	}

	filepath.close();
	读取文件********************************************************
	// 图像矩阵
	cv::Mat gray;
	gray = cv::imread(strVec[0]);
	cout << "Read gray, finished!" << endl;

	//读取点云文件
	PointCloud::Ptr cloud(new PointCloud);
	std::string filename = strVec[1];

	if (pcl::io::loadPCDFile<PointT>((filename), *cloud) == -1) {
		//* load the file
		PCL_ERROR("Couldn't read PCD file \n");
		return -1;
	}

	//参数读取
	fstream paraset;
	paraset.open("./data/paraset.txt", ios::in);
	vector<string> paraVec;
	if (paraset.fail())
	{
		cout << "File ERROR! Cannot find paras!" << endl;

		OutputDebugPrintf("File ERROR!\n");
		return -1;
	}

	while (!paraset.eof())  //行0 - 行lines对应strvect[0] - strvect[lines]
	{
		string parabuf;
		getline(paraset, parabuf, '\n');
		paraVec.push_back(parabuf);
	}

	paraset.close();

	cout << "point cloud size(cloud):" << cloud->points.size() << endl;
	//点云滤波********************************************************
	if (cloud->points.size() == 0) {
		return -1;
	}
	//使用PassThrough滤波器对点云进行滤波
	x y z
	PointCloud::Ptr cloud_filtered_pt(new PointCloud);
	pcl::PassThrough <pcl::PointXYZ> pt;
	pt.setInputCloud(cloud);
	pt.setFilterFieldName("x");
	pt.setFilterLimits(stof(paraVec[0]), stof(paraVec[1]));
	pt.setFilterFieldName("y");
	pt.setFilterLimits(stof(paraVec[2]), stof(paraVec[3]));
	pt.setFilterFieldName("z");
	pt.setFilterLimits(stof(paraVec[4]), stof(paraVec[5]));
    pt.setFilterLimitsNegative (false);
	pt.filter(*cloud_filtered_pt);
	cout << "point cloud size(cloud_filtered_pt):" << cloud_filtered_pt->points.size() << endl;

	//使用VoxelGrid滤波器对点云进行下采样
	if (cloud_filtered_pt->points.size() == 0) {
		return -1;
	}
	PointCloud::Ptr cloud_filtered_vg(new PointCloud);
	pcl::VoxelGrid <pcl::PointXYZ> vg;
	vg.setInputCloud(cloud_filtered_pt);
	vg.setLeafSize(0.8f, 0.8f, 0.8f);
	vg.filter(*cloud_filtered_vg);
	//LeafSize尺寸推荐1.2f 
	cout << "point cloud size(cloud_filtered_vg):" << cloud_filtered_vg->points.size() << endl;

	//使用RadiusOutlierRemoval0滤波去除离散点云
	if (cloud_filtered_vg->points.size() == 0) {
		return -1;
	}
	PointCloud::Ptr cloud_filtered0(new PointCloud);
	pcl::RadiusOutlierRemoval <pcl::PointXYZ> ror0;
	ror0.setInputCloud(cloud_filtered_vg);
	ror0.setRadiusSearch(40);
	ror0.setMinNeighborsInRadius(200);
	ror0.filter(*cloud_filtered0);
	cout << "point cloud size(cloud_filtered0):" << cloud_filtered0->points.size() << endl;

	//使用RadiusOutlierRemoval滤波去除缝隙处点云
	if (cloud_filtered0->points.size() == 0) {
		return -1;
	}
	PointCloud::Ptr cloud_filtered(new PointCloud);
	pcl::RadiusOutlierRemoval <pcl::PointXYZ> ror;
	ror.setInputCloud(cloud_filtered0);
	ror.setRadiusSearch(2);
	ror.setMinNeighborsInRadius(5);
	ror.filter(*cloud_filtered);
	cout << "point cloud size(cloud_filtered):" << cloud_filtered->points.size() << endl;

	//平面分割(RANSAC)********************************************************
	if (cloud_filtered->points.size() == 0) {
		return -1;
	}
	pcl::SACSegmentation <pcl::PointXYZ> seg;
	pcl::PointIndices::Ptr inliner(new pcl::PointIndices);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointCloud<PointT>::Ptr seg_cloud(new pcl::PointCloud<PointT>);
	seg.setInputCloud(cloud_filtered);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMaxIterations(60); //60
	seg.setDistanceThreshold(40);  //20


	//提取平面（展示并输出）********************************************************
	pcl::PointCloud<PointT>::Ptr ext_cloud(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr ext_cloud_rest(new pcl::PointCloud<PointT>);
	pcl::PointCloud<PointT>::Ptr ext_cloud_all(new pcl::PointCloud<PointT>);

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D-View"));

	//在viewer画出点云有效区域
	pcl::PointXYZ pcpt1(stof(paraVec[0]), stof(paraVec[2]), stof(paraVec[4]));
	pcl::PointXYZ pcpt2(stof(paraVec[1]), stof(paraVec[2]), stof(paraVec[4]));
	pcl::PointXYZ pcpt3(stof(paraVec[1]), stof(paraVec[3]), stof(paraVec[4]));
	pcl::PointXYZ pcpt4(stof(paraVec[0]), stof(paraVec[3]), stof(paraVec[4]));
	pcl::PointXYZ pcpt5(stof(paraVec[0]), stof(paraVec[2]), stof(paraVec[5]));
	pcl::PointXYZ pcpt6(stof(paraVec[1]), stof(paraVec[2]), stof(paraVec[5]));
	pcl::PointXYZ pcpt7(stof(paraVec[1]), stof(paraVec[3]), stof(paraVec[5]));
	pcl::PointXYZ pcpt8(stof(paraVec[0]), stof(paraVec[3]), stof(paraVec[5]));

	stringstream Line_ROI;
	//L1
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt1, pcpt2, 1.0, 0.0, 0.0, Line_ROI.str());
	//L2
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt2, pcpt3, 1.0, 0.8, 0.8, Line_ROI.str());
	//L3
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt3, pcpt4, 1.0, 0.8, 0.8, Line_ROI.str());
	//L4
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt4, pcpt1, 1.0, 0.0, 0.0, Line_ROI.str());
	//L5
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt5, pcpt6, 0.8, 1.0, 0.8, Line_ROI.str());
	//L6
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt6, pcpt7, 0.8, 1.0, 0.8, Line_ROI.str());
	//L7
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt7, pcpt8, 0.8, 1.0, 0.8, Line_ROI.str());
	//L8
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt8, pcpt5, 0.8, 1.0, 0.8, Line_ROI.str());
	//L9
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt1, pcpt5, 0.0, 0.0, 1.0, Line_ROI.str());
	//L10
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt2, pcpt6, 0.8, 0.8, 1.0, Line_ROI.str());
	//L11
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt3, pcpt7, 0.8, 0.8, 1.0, Line_ROI.str());	
	//L12
	Line_ROI << 1 << "edge" << rand();
	viewer->addLine(pcpt4, pcpt8, 0.8, 0.8, 1.0, Line_ROI.str());


	int i = cloud_filtered->size(), j = 0;
	pcl::ExtractIndices<PointT> ext;
	srand((unsigned)time(NULL));//刷新时间的种子节点需要放在循环体外面
	while (cloud_filtered->size() > i*0.1)//当提取的点数小于总数的1/10时，跳出循环
	{
		ext.setInputCloud(cloud_filtered);
		seg.segment(*inliner, *coefficients);
		if (inliner->indices.size() == 0)
		{
			break;
		}
		//按照索引提取点云*************
		ext.setIndices(inliner);
		ext.setNegative(false);
		ext.filter(*ext_cloud);
		ext.setNegative(true);
		ext.filter(*ext_cloud_rest);
		//*****************************
		*cloud_filtered = *ext_cloud_rest;

		if (ext_cloud->size() > 100)  //当提取的点云大于100
		{
			*ext_cloud_all += *ext_cloud;
			j++;
		}
	}

	//聚类分割
	//为提取点云时使用的搜素对象利用输入点云cloud_filtered创建Kd树对象tree。
	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
	kdtree->setInputCloud(ext_cloud_all);

	// Euclidean 聚类对象.
	pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
	// 设置聚类的最小值 2cm (small values may cause objects to be divided
	// in several clusters, whereas big values may join objects in a same cluster).
	clustering.setClusterTolerance(1.2);  //2.5
	// 设置聚类的小点数和最大点云数
	clustering.setMinClusterSize(100);
	clustering.setMaxClusterSize(250000);
	clustering.setSearchMethod(kdtree);
	clustering.setInputCloud(ext_cloud_all);
	std::vector<pcl::PointIndices> clusters;
	clustering.extract(clusters);

	int currentClusterNum = 0;
	//pcl::PointXYZRGB minPoint, maxPoint;

	//创建容器，depth_z存放深度值信息，depth_xy存放箱子位置的信息。
	map <int, string> depth_z;
	map <string, string> depth_xy;

	ifstream ins("depth_xy.txt");
	ofstream ous("depth_xy.txt");

    //设置最小点云尺寸
	int setMinCloudSize = 400;
	for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
	{
		//添加所有的点云到一个新的点云中
		pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);
		for (std::vector<int>::const_iterator point = i->indices.begin(); point != i->indices.end(); point++)
			cluster->points.push_back(ext_cloud_all->points[*point]);

		// 保存
		
		if (cluster->points.size() > setMinCloudSize)
		{
			currentClusterNum++;

			//创建OBB包围盒
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_OBB(new pcl::PointCloud<pcl::PointXYZ>());
			for (int i = 0; i < cluster->points.size(); i++)
			{
				point_OBB p_OBB;
				p_OBB.x = cluster->points[i].x;
				p_OBB.y = cluster->points[i].y;
				p_OBB.z = cluster->points[i].z;
				cloud_OBB->points.push_back(p_OBB);
			}

			pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
			feature_extractor.setInputCloud(cloud_OBB);
			feature_extractor.compute();

			std::vector <float> moment_of_inertia;  //惯性距
			std::vector <float> eccentricity;  //离心率
			pcl::PointXYZ min_point_OBB;
			pcl::PointXYZ max_point_OBB;
			pcl::PointXYZ position_OBB;
			Eigen::Matrix3f rotational_matrix_OBB;     //包围盒绕轴旋转的角度
			float major_value, middle_value, minor_value; //eigen是计算矩阵的开源库
			Eigen::Vector3f major_vector, middle_vector, minor_vector;
			Eigen::Vector3f mass_center;   //包围盒的中心点

			feature_extractor.getMomentOfInertia(moment_of_inertia); //特征提取获取惯性距
			feature_extractor.getEccentricity(eccentricity);  //特征提取获取离心率
			feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB); //特征提取OBB ，position是OBB中心相对AABB中心移动的位移
			feature_extractor.getEigenValues(major_value, middle_value, minor_value); //获取特征值
			feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector); //特征向量
			feature_extractor.getMassCenter(mass_center); //获取最大质心

			viewer->setBackgroundColor(1, 1, 1);
			stringstream cloud_OBB_name;
			cloud_OBB_name << currentClusterNum << "cloud_OBB" << rand();
			int *rgb = rand_rgb();
			viewer->addPointCloud<pcl::PointXYZ>(cloud_OBB, pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>(cloud_OBB, rgb[0], rgb[1], rgb[2]), cloud_OBB_name.str());
			delete[]rgb;
			viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, cloud_OBB_name.str());

			Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z); //向量位置
			Eigen::Quaternionf quat(rotational_matrix_OBB);  //四元用法

			stringstream OBB_name;
			OBB_name << "cloud_OBB" << rand();
			viewer->addCube(position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, OBB_name.str());
			viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, OBB_name.str());
			viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.1, OBB_name.str());
			viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 4, OBB_name.str());
			viewer->setRepresentationToWireframeForAllActors();

			Eigen::Vector3f pvertex1(min_point_OBB.x, min_point_OBB.y, min_point_OBB.z*0.5 + max_point_OBB.z*0.5);
			Eigen::Vector3f pvertex2(max_point_OBB.x, min_point_OBB.y, min_point_OBB.z*0.5 + max_point_OBB.z*0.5);
			Eigen::Vector3f pvertex3(max_point_OBB.x, max_point_OBB.y, min_point_OBB.z*0.5 + max_point_OBB.z*0.5);
			Eigen::Vector3f pvertex4(min_point_OBB.x, max_point_OBB.y, min_point_OBB.z*0.5 + max_point_OBB.z*0.5);

			pvertex1 = rotational_matrix_OBB * pvertex1 + position;
			pvertex2 = rotational_matrix_OBB * pvertex2 + position;
			pvertex3 = rotational_matrix_OBB * pvertex3 + position;
			pvertex4 = rotational_matrix_OBB * pvertex4 + position;

			pcl::PointXYZ pt1(pvertex1(0), pvertex1(1), pvertex1(2));
			pcl::PointXYZ pt2(pvertex2(0), pvertex2(1), pvertex2(2));
			pcl::PointXYZ pt3(pvertex3(0), pvertex3(1), pvertex3(2));
			pcl::PointXYZ pt4(pvertex4(0), pvertex4(1), pvertex4(2));

			stringstream Line_name;
			Line_name << currentClusterNum << "edge" << rand();
			viewer->addLine(pt1, pt2, 1.0, 0.0, 0.0, Line_name.str());
			Line_name << currentClusterNum << "edge" << rand();
			viewer->addLine(pt2, pt3, 1.0, 0.0, 0.0, Line_name.str());
			Line_name << currentClusterNum << "edge" << rand();
			viewer->addLine(pt3, pt4, 1.0, 0.0, 0.0, Line_name.str());
			Line_name << currentClusterNum << "edge" << rand();
			viewer->addLine(pt1, pt4, 1.0, 0.0, 0.0, Line_name.str());

			//填充map
			stringstream box_name;
			box_name << "z" << currentClusterNum;
			depth_z[pvertex1(2)] = string(box_name.str());
			cout << depth_z[pvertex1(2)] << " depth:" << pvertex1(2) << endl;

			stringstream location;
			location << int(pvertex1(0)) << "," << int(pvertex1(1)) << "," << int(pvertex2(0)) << "," << int(pvertex2(1)) << "," << int(pvertex3(0)) << "," << int(pvertex3(1)) << "," << int(pvertex4(0)) << "," << int(pvertex4(1)) << "," << int(pvertex1(2));
			depth_xy[box_name.str()] = location.str();

		}
		else
		{
			break;
		}
	}

	map<int, string>::const_iterator it = depth_z.begin();
	while (it != depth_z.end())
	{
		ous << depth_xy[it->second] << endl;

		it++;
	}
	ous.close();
	viewer->spin();


	cout << "PC progress Finished!" << endl;

	//system("pause");
	
	return 0;
}

iQTan

关注

1
点赞
踩
13

收藏

觉得还不错? 一键收藏
22
评论
一种点云聚类分割的处理流程

本文是在上文基础上，记录了一种点云聚类分割的处理流程。程序流程：>初始化： >说明命名空间 >定义计时器（double类型） >定义点云类型 PointXYZRGB>计时开始tstart>创建图像矩阵>读取rgb.png >读取depth.png 采用“2”模式读取深度图像>创建点云cloud>遍...
复制链接

扫一扫