struct point ThreePoint(struct point p1,struct point p2,struct point p3)
{
struct point n; //p1,p2,p3确定的平面的法向量
n.x=(p3.y-p2.y)*(p1.z-p2.z)-(p3.z-p2.z)*(p1.y-p2.y);
n.y=(p3.z-p2.z)*(p1.x-p2.x)-(p3.x-p2.x)*(p1.z-p2.z);
n.z=(p3.x-p2.x)*(p1.y-p2.y)-(p3.y-p2.y)*(p1.x-p2.x);
struct point outp; //用来记录p1,p2,p3确定的平面的在参数空间的坐标,x-rho,y-phi,z-theta
outp.x=(n.x*p1.x+n.y*p1.y+n.z*p1.z)/(sqrt(n.x*n.x+n.y*n.y+n.z*n.z));
if (outp.x<0) //我们选择的投票范围 rho-0~size,phi-0~PI,theta-0~2*PI
{
n.x=-n.x;
n.y=-n.y;
n.z=-n.z;
outp.x=-outp.x;
}
outp.y=acos((n.z)/(sqrt(n.x*n.x+n.y*n.y+n.z*n.z)));
outp.z=acos((n.x)/(sqrt(n.x*n.x+n.y*n.y)));
if (n.y<0) outp.z=2*PI-outp.z;
return outp;
}
RTH(Randomized Hough Transform)中计算p1,p2,p3三个点确定的平面在参数空间的坐标
最新推荐文章于 2021-09-23 11:43:50 发布