pcl matlab 计算平面与空间三角形的交线

一、生成一个平面

 过程：

[x y]=meshgrid(-2:0.1:2);
z=2-x-(y-1);
mesh(x,y,z)

二、三个点计算平面的法线

p1p2=p2-p1;
p1p3=p3-p1;
n=cross(p1p2,p1p3);
D=-dot(n,p1);
EquationPlane=[n,D];

三、平面与三角形的交线

int Interection_Plane_Triangle(Plane &plane,pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointXYZ &Point1, pcl::PointXYZ  &Point2)
{
	Eigen::Vector3f v_P1{ cloud->points[0].x, cloud->points[0].y, cloud->points[0].z };
	Eigen::Vector3f v_P2{ cloud->points[1].x, cloud->points[1].y, cloud->points[1].z };
	Eigen::Vector3f v_P3{ cloud->points[2].x, cloud->points[2].y, cloud->points[2].z };

	// 三个点在平面的位置（X-x0）*A +(Y-y0)*B +(Z-z0)*C=0  
	float d1 = (v_P1[0] - plane.point_X)*plane.normal_A + (v_P1[1] - plane.point_Y)*plane.normal_B + (v_P1[2] - plane.point_Z)*plane.normal_C;
	float d2 = (v_P2[0] - plane.point_X)*plane.normal_A + (v_P2[1] - plane.point_Y)*plane.normal_B + (v_P2[2] - plane.point_Z)*plane.normal_C;
	float d3 = (v_P3[0] - plane.point_X)*plane.normal_A + (v_P3[1] - plane.point_Y)*plane.normal_B + (v_P3[2] - plane.point_Z)*plane.normal_C;

	// 判断三个点分别三个点和平面不相交
	if ((d1>0 && d2>0 && d3>0) || (d1<0 && d2<0 && d3<0))
	{
		return -1;
	}
	bool s1 = d1<0;
	bool s2 = d2<0;
	bool s3 = d3<0;

	if (s2 != s1)
	{
		Point1.x = (d2*v_P1[0] - d1*v_P2[0]) / (d2 - d1);
		Point1.y = (d2*v_P1[1] - d1*v_P2[1]) / (d2 - d1);
		Point1.z = (d2*v_P1[2] - d1*v_P2[2]) / (d2 - d1);
		if (s1 != s3)
		{
			Point2.x = (d1*v_P3[0] - d3*v_P1[0]) / (d1 - d3);
			Point2.y = (d1*v_P3[1] - d3*v_P1[1]) / (d1 - d3);
			Point2.z = (d1*v_P3[2] - d3*v_P1[2]) / (d1 - d3);
		}
		else {
			Point2.x = (d2*v_P3[0] - d3*v_P2[0]) / (d2 - d3);
			Point2.y = (d2*v_P3[1] - d3*v_P2[1]) / (d2 - d3);
			Point2.z = (d2*v_P3[2] - d3*v_P2[2]) / (d2 - d3);
		}
	}
	else if (s3 != s1)
	{
		Point1.x = (d3*v_P1[0] - d1*v_P3[0]) / (d3 - d1);
		Point1.y = (d3*v_P1[1] - d1*v_P3[1]) / (d3 - d1);
		Point1.z = (d3*v_P1[2] - d1*v_P3[2]) / (d3 - d1);

		Point2.x = (d2*v_P3[0] - d3*v_P2[0]) / (d2 - d3);
		Point2.y = (d2*v_P3[1] - d3*v_P2[1]) / (d2 - d3);
		Point2.z = (d2*v_P3[2] - d3*v_P2[2]) / (d2 - d3);
	}
	return 0;
}

Halcon代码： 

triangulate_object_model_3d (RawPointData3D, 'greedy', ParameterNames, ParameterValues, Surface3DDefault, Info)
write_object_model_3d (Surface3DDefault, 'ply', 'C:/Users/Albert/Desktop/Halcon2PCL/bundy.ply', [], [])
create_pose (0.1, 0.1, 0.1, 90, 90, 90, 'Rp+T', 'gba', 'point', Pose1)

pose2222:=[[69.981483459472656,22.498832702636719,-12.406876564025879,89.999999999999957,-45.000000000000000,44.999999999999972,0]]
pose_to_hom_mat3d (pose2222, HomMat3D2222)

* 生成一个平面
gen_plane_object_model_3d (pose2222,  [-1,-1,1,1] * 180, [-1,1,1,-1] * 180, PlaneObjectModel)
*  用平面切
intersect_plane_object_model_3d (Surface3DDefault,pose2222, PlaneCuting_Model3DIntersection)

mesh 平面切割效果：

pcl:

halcon: