使用PCL繪製空間圓

參考算法公式：http://blog.csdn.net/menjiawan/article/details/43021507

代碼：

void DisplayMessage::createTheoryCircle(Point3d planeNormal, Point3d centerPoint, double R)
{
	double nx = planeNormal.x, ny = planeNormal.y, nz = planeNormal.z;
	double cx = centerPoint.x, cy = centerPoint.y, cz = centerPoint.z;
	double r = R;

	double ux = ny, uy = -nx, uz = 0;
	double vx = nx*nz,
		vy = ny*nz,
		vz = -nx*nx - ny*ny;

	double sqrtU = sqrt(ux*ux + uy*uy + uz*uz);
	double sqrtV = sqrt(vx*vx + vy*vy + vz*vz);

	double ux_ = (1 / sqrtU)*ux;
	double uy_ = (1 / sqrtU)*uy;
	double uz_ = (1 / sqrtU)*uz;

	double vx_ = (1 / sqrtV)*vx;
	double vy_ = (1 / sqrtV)*vy;
	double vz_ = (1 / sqrtV)*vz;

	double xi, yi, zi;
	double t = 0;
	double angle = (t / 180.0)*Pi;
	vector<double> x, y, z;

	while (t < 360.0)
	{
		xi = cx + r*(ux_*cos(angle) + vx_*sin(angle));
		yi = cy + r*(uy_*cos(angle) + vy_*sin(angle));
		zi = cz + r*(uz_*cos(angle) + vz_*sin(angle));
		x.push_back(xi);
		y.push_back(yi);
		z.push_back(zi);

		t = t + 1;
		angle = (t / 180.0)*Pi;
	}
	pcl::PointCloud<pcl::PointXYZ>::Ptr theroyCirclePoints(new pcl::PointCloud<pcl::PointXYZ>);

	//定義cloudPoints大小,無序點雲
	theroyCirclePoints->resize(x.size());
	for (int i = 0; i < x.size(); i++){
		//將三維座標賦值給PCL點雲座標
		(*theroyCirclePoints)[i].x = x[i];
		(*theroyCirclePoints)[i].y = y[i];
		(*theroyCirclePoints)[i].z = z[i];
	}
	
	//圓心點雲設置
	//設置點雲顏色
	viewer->removePointCloud("theroyCircleCloud");
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> theroyCircleCloud_color(theroyCirclePoints, 255, 0, 0);
	//點雲顏色渲染	
	viewer->addPointCloud(theroyCirclePoints, theroyCircleCloud_color, "theroyCircleCloud");
	//設置點雲大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "theroyCircleCloud");
	viewer->resetCamera();
	ui.widget->update();
	
}

本類用於QT顯示PCL點雲，可參考本欄博客QT顯示PCL點雲