点云旋转处理

实现代码为：

//以中心化点进行旋转
	double theta = atan(maindirection.a);//计算的是弧度单位
	for (int i = 0; i < origipts.size(); i++)
	{
		pcl::PointXYZ tempone;
		tempone.x = aftercenerlizepts[i].x*cos(theta) + aftercenerlizepts[i].y*sin(theta) + center.x;
		tempone.y = aftercenerlizepts[i].y*cos(theta) - aftercenerlizepts[i].x*sin(theta) + center.y;
		transpts.push_back(tempone);
	}

3、测试结果

本程序是在PCL环境下运行，测试工程需要先配置好PCL环境，将点云旋转_test.cpp添加到源文件中即可运行。

3.1 轮廓点检测结果

轮廓点提取主函数如下：

//（1）测试边缘点提取结果
void main()
{
	char *filepath = "D:\\testdata\\points.xyz";
	char *savepath = "D:\\testdata\\points_boundpts.xyz";

	vector<pcl::PointXYZ> origipts = ReadPointXYZIntoVector(filepath);
	//假设其z坐标都为0，为平面坐标
	for (int i = 0; i < origipts.size(); i++)
	{
		origipts[i].z = 0;
	}
	
	vector<pcl::PointXYZ> boundpts, nonbounpts;
	double r = 0.8;
	Bounpts(origipts, r, boundpts, nonbounpts);



	ofstream outfile(savepath, ios::out);
	for (int j = 0; j < boundpts.size(); j++)
	{
		outfile << fixed << setprecision(3) << boundpts[j].x << " " << boundpts[j].y << " " << boundpts[j].z << " " << fixed << setprecision(0) << 255 << " " << 0 << " " << 0 << endl;
	}
	for (int j = 0; j < nonbounpts.size(); j++)
	{
		outfile << fixed << setprecision(3) << nonbounpts[j].x << " " << nonbounpts[j].y << " " << nonbounpts[j].z << " " << fixed << setprecision(0) << 255 << " " << 255 << " " << 255 << endl;
	}
	outfile.close();
	cout << "结束" << endl;
	pcl::visualization::PCLVisualizer viewer("点云可视化");
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	new_cloud->width = origipts.size();
	new_cloud->height = 1;
	new_cloud->is_dense = false;
	new_cloud->points.resize(new_cloud->width*new_cloud->height);


	for (int i = 0; i < origipts.size(); i++)
	{
		if (i < boundpts.size())
		{
			new_cloud->points[i].x = boundpts[i].x;
			new_cloud->points[i].y = boundpts[i].y;
			new_cloud->points[i].z = boundpts[i].z;
			new_cloud->points[i].r = 255;
			new_cloud->points[i].g = 0;
			new_cloud->points[i].b = 0;
		}
		else
		{
			new_cloud->points[i].x = nonbounpts[i - boundpts.size()].x;
			new_cloud->points[i].y = nonbounpts[i - boundpts.size()].y;
			new_cloud->points[i].z = nonbounpts[i - boundpts.size()].z;
			new_cloud->points[i].r = 255;
			new_cloud->points[i].g = 255;
			new_cloud->points[i].b = 255;
		}
	}

	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>fildColor(new_cloud);
	viewer.setBackgroundColor(0, 0, 0);
	viewer.addPointCloud<pcl::PointXYZRGB>(new_cloud, fildColor, "inCloud");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "inCloud");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	system("pause");

}

红色点为边缘点，可以看到边缘提取效果比较理想。

3.2 轮廓点分组

轮廓点分组测试结果如下：

//（2）测试边缘点分组
void main()
{
	char *filepath = "D:\\testdata\\points.xyz";
	char *savepath = "D:\\testdata\\points_boundpts_group.xyz";

	vector<pcl::PointXYZ> origipts = ReadPointXYZIntoVector(filepath);
	//假设其z坐标都为0，为平面坐标
	for (int i = 0; i < origipts.size(); i++)
	{
		origipts[i].z = 0;
	}

	vector<pcl::PointXYZ> boundpts, nonbounpts;
	double r = 0.8;
	Bounpts(origipts, r, boundpts, nonbounpts);

	vector<vector<pcl::PointXYZ>> multi_linepoints;
	double ds_thres = 0.35;
	double linefit_knn = 5;
	double growing_knn = 5;
	GroupPts(boundpts, ds_thres, linefit_knn, growing_knn, multi_linepoints);
	srand((int)time(0));
	ofstream outfile(savepath, ios::out);
	for (int i = 0; i < multi_linepoints.size(); i++)
	{
		double R = rand() % 255;
		double G = rand() % 255;
		double B = rand() % 255;
		for (int j = 0; j < multi_linepoints[i].size(); j++)
		{
			outfile << fixed << setprecision(3) << multi_linepoints[i][j].x << " " << multi_linepoints[i][j].y << " " << multi_linepoints[i][j].z << " " << fixed << setprecision(0) << R << " " << G << " " << B << endl;
		}
		
	}
	outfile.close();
	cout << "结束" << endl;
	pcl::visualization::PCLVisualizer viewer("点云可视化");
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	new_cloud->width = origipts.size();
	new_cloud->height = 1;
	new_cloud->is_dense = false;
	new_cloud->points.resize(new_cloud->width*new_cloud->height);

	
	
	int sumid = 0;
	for (int i = 0; i < multi_linepoints.size(); i++)
	{
		double R = rand() % 255;
		double G = rand() % 255;
		double B = rand() % 255;

		for (int j = 0; j < multi_linepoints[i].size(); j++)
		{
			new_cloud->points[sumid].x = multi_linepoints[i][j].x;
			new_cloud->points[sumid].y = multi_linepoints[i][j].y;
			new_cloud->points[sumid].z = multi_linepoints[i][j].z;
			new_cloud->points[sumid].r = R;
			new_cloud->points[sumid].g = G;
			new_cloud->points[sumid].b = B;
			sumid = sumid + 1;
		}

	}
	


	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>fildColor(new_cloud);
	viewer.setBackgroundColor(0, 0, 0);
	viewer.addPointCloud<pcl::PointXYZRGB>(new_cloud, fildColor, "inCloud");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "inCloud");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	system("pause");

}

属于同一直线的轮廓点，分组结果如上，结果比较理想。

3.3 点云旋转

//（3）原始点云进行旋转
void main()
{
	char *filepath = "D:\\testdata\\points.xyz";
	char *savepath = "D:\\testdata\\points_boundpts_transformpt.xyz";

	vector<pcl::PointXYZ> origipts = ReadPointXYZIntoVector(filepath);
	//假设其z坐标都为0，为平面坐标
	for (int i = 0; i < origipts.size(); i++)
	{
		origipts[i].z = 0;
	}

	vector<pcl::PointXYZ> boundpts, nonbounpts;
	double r = 0.8;
	Bounpts(origipts, r, boundpts, nonbounpts);

	pcl::PointXYZ center; 
	vector<pcl::PointXYZ> transpts;
	double ds_thres = 0.35;
	double linefit_knn = 5;
	double growing_knn = 5;
	TransformPts(origipts, r, ds_thres, linefit_knn, growing_knn, center, transpts);
	srand((int)time(0));
	ofstream outfile(savepath, ios::out);
	for (int i = 0; i < transpts.size(); i++)
	{		
		outfile << fixed << setprecision(3) << transpts[i].x << " " << transpts[i].y << " " << transpts[i].z << " " << endl;
	}
	outfile.close();
	cout << "结束" << endl;
	pcl::visualization::PCLVisualizer viewer("点云可视化");
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	new_cloud->width = origipts.size();
	new_cloud->height = 1;
	new_cloud->is_dense = false;
	new_cloud->points.resize(new_cloud->width*new_cloud->height);

	int sumid = 0;
	for (int i = 0; i < transpts.size(); i++)
	{
		new_cloud->points[sumid].x = transpts[i].x;
		new_cloud->points[sumid].y = transpts[i].y;
		new_cloud->points[sumid].z = transpts[i].z;
		new_cloud->points[sumid].r = 255;
		new_cloud->points[sumid].g = 255;
		new_cloud->points[sumid].b = 255;
	}

	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>fildColor(new_cloud);
	viewer.setBackgroundColor(0, 0, 0);
	viewer.addPointCloud<pcl::PointXYZRGB>(new_cloud, fildColor, "inCloud");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "inCloud");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	system("pause");

}

旋转前点云与水平方向存在一定旋转角，旋转后点云水平一致，旋转旋转成功。

代码与测试数据下载链接：https://mp.csdn.net/mp_download/manage/download/UpDetailed