pcl matlab は、平面と空間三角形の間の交線を計算します。

1. 平面を生成する

 プロセス:

法線ベクトルだけでは平面を決定することはできません。少なくとも平面上の点の座標が必要です。
法線ベクトル n=(A,B,C) が既知で
、平面がある点を通過する場合 M=(x0, y0,z0)
すると、平面の方程式は
A(x-x0)+B(y-y0)+C(z-z0)=0 となります。
図に描きたい場合は、まずイオンを与えなければなりませ
んx,yの範囲と平面法線ベクトル(1,1,1)に
点(0,1,2)を通り-2~2の範囲でx,yで平面を描画

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

平面の法線を計算する 2、3 点

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

3. 平面と三角形の交線


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;
}

ハルコンコード: 

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)

メッシュ平面の切断効果:

PCL:

ファルコン:

 

おすすめ

転載: blog.csdn.net/weixin_39354845/article/details/131416771