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

36 篇文章 7 订阅
34 篇文章 26 订阅

一、生成一个平面

 过程:

单有法向量不能确定一个平面,至少还要有平面上的一个点的坐标才行
假如知道法向量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)
画出x,y在 -2~2区间内的平面

[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:

  • 0
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
pcl(点云库)是一个开源的点云处理库,可以用于点云数据的获取、处理、分析等。平面交线并显示实际上是一个几何计算问题。 首先,我们需要确定两个平面的方程。假设平面1的法向量为n1=(a1,b1,c1),平面方程为ax+by+cz+d1=0;平面2的法向量为n2=(a2,b2,c2),平面方程为ax+by+cz+d2=0。 两个平面交线可以通过平面方程组得到。首先我们可以将平面方程转换为参数化方程,即将x和y表示为z的函数。我们令z=t,然后解方程组得到x和y的参数化表达式。 接下来,我们可以通过pcl库中一些相关的功能函数来实现平面交线解和显示。其中,pcl::SampleConsensusModelLine可以用于拟合直线,pcl::ProjectInliers可以用于将点云投影到拟合的直线上,pcl::visualization::PCLVisualizer可以用于显示点云和拟合的直线。 具体的步骤如下: 1. 从点云数据中提取平面的法向量n1和n2。 2. 根据法向量和平面方程的定义,得到平面方程参数。 3. 解平面方程组,得到交线的参数化表达式。 4. 使用pcl::SampleConsensusModelLine进行直线拟合。 5. 使用pcl::ProjectInliers将点云投影到拟合的直线上。 6. 使用pcl::visualization::PCLVisualizer显示点云和拟合的直线。 通过以上步骤,我们可以利用pcl库解两平面交线并显示。这样能更加直观地观察两平面交线,并进行进一步的分析和研究。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值