PCL拟合并绘制平面和柱面(三)

在使用PCL拟合平面和柱面后，需要绘制自定义大小和位置的平面和柱面以便于可视化。

//拟合类型
enum FitType
{
	FitPlane = 1,    //平面
	FitCylinder = 2  //圆柱
};

主要函数：

void PointCloudViewer(PointCloudT::Ptr cloudseg, PointCloudT::Ptr cloudfiltered, PointCloudT::Ptr cloudOut, pcl::ModelCoefficients::Ptr coefficients)
{
	pcl::visualization::PCLVisualizer viewer("viewer");
	float bckgr_gray_level = 0.0;
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	int v1(0);
	viewer.createViewPort(0.0, 0.0, 1.0, 1.0, v1);
	//分割点云
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_seg_color_h(cloudseg, 0, 255, 0);
	viewer.addPointCloud(cloudseg, cloud_seg_color_h, "cloudseg", v1);
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloudseg");
	//剩余点云
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_filter_color_h(cloudfiltered, 255, 255, 0);
	viewer.addPointCloud(cloudfiltered, cloud_filter_color_h, "cloudfilter", v1);
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloudfilter");
	//模型
	Eigen::Vector3f mass_center;
	if (mFitType == FitPlane)
	{
		float width = 300.0, height = 300.0;  //x,y
		GetPointsBoundingBoxAABB(cloudseg, width, height, mass_center);

		//分割平面：自定义的平面大小
		float scale[2] = { width, height };
		//利用PCL函数计算质心
		Eigen::Vector4f centroid;					    // 质心
		pcl::compute3DCentroid(*cloudseg, centroid);	// 齐次坐标，（c0,c1,c2,1）
		auto plane = createPlane(*coefficients, centroid[0] - width / 2.0, centroid[1] - height / 2.0, centroid[2], scale);
		viewer.addModelFromPolyData(plane, "plane_1");
		viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 1.0, "plane_1", v1);
		viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.6, "plane_1", v1);
		viewer.addText("point cloud plane", 10, 940, 16, bckgr_gray_level, txt_gray_lvl, txt_gray_lvl, "plane", v1);
	}
	else if (mFitType == FitCylinder)
	{
		float width = 300.0, height = 300.0;  //x,y
		GetPointsBoundingBoxAABB(cloudseg, width, height, mass_center);

		float scale = std::max(width / 2.0, height / 2.0);
		
		auto cylinder = createCylinder(*coefficients, mass_center, 100, scale);
		viewer.addModelFromPolyData(cylinder, "cylinder_1");
		viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 1.0, "cylinder_1", v1);
		viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.6, "cylinder_1", v1);
		viewer.addText("point cloud cylinder", 10, 940, 16, bckgr_gray_level, txt_gray_lvl, txt_gray_lvl, "cylinder", v1);
	}
	//描述
	viewer.addText("point cloud seg", 10, 1000, 16, bckgr_gray_level, txt_gray_lvl, bckgr_gray_level, "cloudseg", v1);
	viewer.addText("point cloud filter", 10, 980, 16, txt_gray_lvl, txt_gray_lvl, bckgr_gray_level, "cloudfilter", v1);
	//背景 
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
	//相机位置
	if (mFitType == FitPlane)
		viewer.setCameraPosition(0.0, 0.0, coefficients->values[3] / 2.0, mass_center[0], mass_center[1], mass_center[2], 0.0, 1.0, 0.0);
	if (mFitType == FitCylinder)
		viewer.setCameraPosition(0.0, 0.0, coefficients->values[2] / 20.0, mass_center[0], mass_center[1], mass_center[2], 1.0, 0.0, 0.0);
	viewer.setSize(1280, 1024);
	//显示
	while (!viewer.wasStopped())
		viewer.spinOnce();
}

获取点云质心等参数：

void GetPointsBoundingBoxAABB(PointCloudT::Ptr cloudSource, float& width, float& height, Eigen::Vector3f& mass_center)
{
	// 创建惯性矩估算对象，设置输入点云，并进行计算
	pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
	feature_extractor.setInputCloud(cloudSource);
	feature_extractor.compute();

	std::vector <float> moment_of_inertia;
	std::vector <float> eccentricity;
	pcl::PointXYZ min_point_AABB;
	pcl::PointXYZ max_point_AABB;
	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::Vector3f major_vector, middle_vector, minor_vector;

	// 获取惯性矩
	feature_extractor.getMomentOfInertia(moment_of_inertia);
	// 获取离心率
	feature_extractor.getEccentricity(eccentricity);
	// 获取AABB盒子
	feature_extractor.getAABB(min_point_AABB, max_point_AABB);
	// 获取OBB盒子
	feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
	feature_extractor.getEigenValues(major_value, middle_value, minor_value);
	// 获取主轴major_vector，中轴middle_vector，辅助轴minor_vector
	feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
	// 获取质心
	feature_extractor.getMassCenter(mass_center);
	//获取高度和宽度
	width = (max_point_AABB.x - min_point_AABB.x)*1.2;
	height = (max_point_AABB.y - min_point_AABB.y)*1.2;
}

创建一个自定义平面(源代码地址)：

vtkSmartPointer<vtkPolyData> VisionTrajectory::createPlane(const pcl::ModelCoefficients &coefficients, double x, double y, double z, float scale[2])
{
	vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New();

	double norm_sqr = 1.0 / (coefficients.values[0] * coefficients.values[0] +
		coefficients.values[1] * coefficients.values[1] +
		coefficients.values[2] * coefficients.values[2]);

	plane->SetNormal(coefficients.values[0], coefficients.values[1], coefficients.values[2]);
	double t = x * coefficients.values[0] + y * coefficients.values[1] + z * coefficients.values[2] + coefficients.values[3];
	x -= coefficients.values[0] * t * norm_sqr;
	y -= coefficients.values[1] * t * norm_sqr;
	z -= coefficients.values[2] * t * norm_sqr;

	plane->SetCenter(x, y, z);

	{
		double pt1[3], pt2[3], orig[3], center[3];
		plane->GetPoint1(pt1);
		plane->GetPoint2(pt2);
		plane->GetOrigin(orig);
		plane->GetCenter(center);

		float scale1 = 3.0;
		float scale2 = 3.0;
		if (scale != nullptr)
		{
			scale1 = scale[0];
			scale2 = scale[1];
		}
		//延长pt1,pt2. 延长origin到pt1连线的方向向量
		double _pt1[3], _pt2[3];
		for (int i = 0; i < 3; i++) {
			_pt1[i] = scale1 * (pt1[i] - orig[i]);
			_pt2[i] = scale2 * (pt2[i] - orig[i]);
		}
		//pt1相对于原坐标系下的坐标值
		for (int i = 0; i < 3; ++i)
		{
			pt1[i] = orig[i] + _pt1[i];
			pt2[i] = orig[i] + _pt2[i];
		}
		plane->SetPoint1(pt1);
		plane->SetPoint2(pt2);
	}
	plane->Update();

	return (plane->GetOutput());

创建一个自定义柱面：

vtkSmartPointer<vtkPolyData> createCylinder(const pcl::ModelCoefficients &coefficients, Eigen::Vector3f mass_center, int numsides, float scale)
{
	if (coefficients.values.size() != 7)
	{
		PCL_WARN("[addCylinder] Coefficients size does not match expected size (expected 7).\n");
		return (false);
	}

	//质心在轴线上的投影点P2
	double P0[3], P2[3];
	P0[0] = coefficients.values[0];
	P0[1] = coefficients.values[1];
	P0[2] = coefficients.values[2];
	double dlambda = (mass_center[0] - P0[0])*coefficients.values[3] + (mass_center[1] - P0[1])*coefficients.values[4] + (mass_center[2] - P0[2])*coefficients.values[5];
	P2[0] = P0[0] + dlambda * coefficients.values[3];
	P2[1] = P0[1] + dlambda * coefficients.values[4];
	P2[2] = P0[2] + dlambda * coefficients.values[5];

	//投影点平移
	double pt[3];
	pt[0] = coefficients.values[3] * scale;
	pt[1] = coefficients.values[4] * scale;
	pt[2] = coefficients.values[5] * scale;
	vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New();
	line->SetPoint1(P2[0] - pt[0], P2[1] - pt[1], P2[2] - pt[2]);
	line->SetPoint2(P2[0] + pt[0], P2[1] + pt[1], P2[2] + pt[2]);

	vtkSmartPointer<vtkTubeFilter> tuber = vtkSmartPointer<vtkTubeFilter>::New();
	tuber->SetInputConnection(line->GetOutputPort());
	tuber->SetRadius(coefficients.values[6]);
	tuber->SetNumberOfSides(numsides);
	tuber->Update();

	return (tuber->GetOutput());
}