对于两空间直线来说,计算交点和最小公垂线是一码事,交点即最小公垂线两个垂足的中心。PCL中源码中包含计算空间直线最小公垂线的函数
PCL_EXPORTS void pcl::lineToLineSegment (const Eigen::VectorXf & line_a,
const Eigen::VectorXf & line_b,
Eigen::Vector4f & pt1_seg,
Eigen::Vector4f & pt2_seg
)
Parameters:
line_a the coefficients of the first line (point, direction)
line_b the coefficients of the second line (point, direction)
pt1_seg the first point on the line segment
pt2_seg the second point on the line segment
该函数位于common/src/distances.cpp 中,源码如下,增加了我的一些注释
/*
输入:line_a、line_b,均为6维向量,前三维表示直线上一点的坐标,后三维表示直线的方向向量
输出:pt1_seg、pt2_seg,最小公垂线的两个垂足,4维向量,前三维表示空间坐标,第四维是零
*/
pcl::lineToLineSegment (const Eigen::VectorXf &line_a, const Eigen::VectorXf &line_b,
Eigen::Vector4f &pt1_seg, Eigen::Vector4f &pt2_seg)
{
// point + direction = 2nd point
Eigen::Vector4f p1 = Eigen::Vector4f::Zero ();
Eigen::Vector4f p2 = Eigen::Vector4f::Zero ();
Eigen::Vector4f dir1 = Eigen::Vector4f::Zero ();
p1.head<3> () = line_a.head<3> (); //直线上点的坐标
dir1.head<3> () = line_a.segment<3> (3); //方向向量
p2 = p1 + dir1; //直线上一点加上直线方向向量,等于直线上另外一点;直线上两个点的差等于方向向量
// point + direction = 2nd point
Eigen::Vector4f q1 = Eigen::Vector4f::Zero ();
Eigen::Vector4f q2 = Eigen::Vector4f::Zero ();
Eigen::Vector4f dir2 = Eigen::Vector4f::Zero ();
q1.head<3> () = line_b.head<3> ();
dir2.head<3> () = line_b.segment<3> (3);
q2 = q1 + dir2;
// a = x2 - x1 = line_a[1] - line_a[0]
Eigen::Vector4f u = dir1;
// b = x4 - x3 = line_b[1] - line_b[0]
Eigen::Vector4f v = dir2;
// c = x2 - x3 = line_a[1] - line_b[0]
Eigen::Vector4f w = p2 - q1;
float a = u.dot (u);
float b = u.dot (v);
float c = v.dot (v);
float d = u.dot (w);
float e = v.dot (w);
float denominator = a*c - b*b;
float sc, tc;
// Compute the line parameters of the two closest points
if (denominator < 1e-5) // The lines are almost parallel
{
sc = 0.0;
tc = (b > c ? d / b : e / c); // Use the largest denominator
}
else
{
sc = (b*e - c*d) / denominator;
tc = (a*e - b*d) / denominator;
}
// Get the closest points
pt1_seg = Eigen::Vector4f::Zero ();
pt1_seg = p2 + sc * u;
pt2_seg = Eigen::Vector4f::Zero ();
pt2_seg = q1 + tc * v;
}
具体原理如下:
已知两条异面直线AB、CD上两点坐标分别为A()、B()、C()、D()。公垂线上在两个垂足坐标的计算公式推导过程如下:
定义直线AB上的垂足M(Xm,Ym,Zm), 直线CD上的垂足N(Xn,Yn,Zn)。定义 AM = t1*AB => (Xm-Xa,Y-Ya,Z-Za)=t1*(Xb-Xa,Yb-Ya,Zb-Za)。则 M坐标定义为
M( t1(Xb-Xa)+Xa , t1(Yb-Ya)+Ya , t1(Zb-Za)+Za );
同理 N坐标定义为
N( t2(Xd-Xc)+Xc , t2(Yd-Yc)+Yc , t2(Zd-Zc)+Zc );
所以 向量MN定义为
MN(t2(Xd-Xc)+Xc-t1(Xb-Xa)-Xa, t2(Yd-Yc)+Yc-t1(Yb-Ya)-Ya, t2(Zd-Zc)+Zc-t1(Zb-Za)-Za);
因为 MN垂直于AB,MN垂直于CD,根据空间矢量点积特性,有
(Xb-Xa)[t2(Xd-Xc)+Xc-t1(Xb-Xa)-Xa]+(Yb-Ya)[t2(Yd-Yc)+Yc-t1(Yb-Ya)-Ya]+(Zb-Za)[t2(Zd-Zc)+Zc-t1(Zb-Za)-Za]=0;
(Xd-Xc)[t2(Xd-Xc)+Xc-t1(Xb-Xa)-Xa]+(Yd-Yc)[t2(Yd-Yc)+Yc-t1(Yb-Ya)-Ya]+(Zd-Zc)[t2(Zd-Zc)+Zc-t1(Zb-Za)-Za]=0;
分别推导得到:
式1:
t2[(Xb-Xa)*(Xd-Xc)+(Yb-Ya)*(Yd-Yc)+(Zb-Za)*(Zd-Zc)]-t1[(Xb-Xa)* (Xb-Xa)+(Yb-Ya)*(Yb-Ya)+(Zb-Za)*(Zb-Za)]+[(Xb-Xa)*(Xc-Xa)+(Yb-Ya)*(Yc-Ya)+(Zb-Za)*(Zc-Za)]=0
式2:
t2[(Xd-Xc)*(Xd-Xc)+(Yd-Yc)*(Yd-Yc)+(Zd-Zc)*(Zd-Zc)]-t1[(Xb-Xa)*(Xd-Xc)+(Yb-Ya)*(Yd-Yc)+(Zb-Za)*(Zd-Zc)]+[(Xd-Xc)*(Xc-Xa)+(Yd-Yd)*(Yc-Ya)+(Zd-Zc)*(Zc-Za)]=0
令:
F1(a,b)=[(Xb-Xa)*(Xb-Xa)+(Yb-Ya)*(Yb-Ya)+(Zb-Za)*(Zb-Za)]
F1(c,d)= [(Xd-Xc)*(Xd-Xc)+(Yd-Yc)*(Yd-Yc)+(Zd-Zc)*(Zd-Zc)]
F2()=[(Xb-Xa)*(Xd-Xc)+(Yb-Ya)*(Yd-Yc)+(Zb-Za)*(Zd-Zc)]
F3(a,b)=[(Xb-Xa)*(Xc-Xa)+(Yb-Ya)*(Yc-Ya)+(Zb-Za)*(Zc-Za)]
F3(c,d)=[(Xd-Xc)*(Xc-Xa)+(Yd-Yc)*(Yc-Ya)+(Zd-Zc)*(Zc-Za)]
则:
式1=>式3:t2*F2()-t1*F1(a,b)+F3(a,b)=0;
式2=> 式4:t2*F1(c,d)-t1*F2()+F3(c,d)=0;
即:
t1=[F3(a,b)*F1(c,d)-F3(c,d)*F2()]/[F1(a,b)*F1(c,d)-F2()*F2()]
t2=[F3(c,d)*F1(a,b)-F2()*F3(a,b)]/[F2()*F2()-F1(a,b)*F1(c,d)]
由此得到两个垂足点的坐标:
M(Xm,Ym,Zm):
Xm=t1*(Xb-Xa)+Xa=(Xb-Xa)*[F3(a,b)*F1(c,d)-F3(c,d)*F2()]/[F1(a,b)*F1(c,d)-F2()*F2()]+Xa;
Ym=t1*(Yb-Ya)+Ya=(Yb-Ya)*[F3(a,b)*F1(c,d)-F3(c,d)*F2()]/[F1(a,b)*F1(c,d)-F2()*F2()]+Ya;
Zm=t1*(Zb-Za)+Za=(Zb-Za)*[F3(a,b)*F1(c,d)-F3(c,d)*F2()]/[F1(a,b)*F1(c,d)-F2()*F2()]+Za;
N(Xn,Yn,Zn):
Xn=t2*(Xd-Xc)+Xc=(Xd-Xc)*[F3(c,d)*F1(a,b)-F3(a,b)*F2()]/[F2()*F2()-F1(a,b)*F1(c,d)]+Xc;
Yn=t2*(Yd-Yc)+Yc=(Yd-Yc)*[F3(c,d)*F1(a,b)-F3(a,b)*F2()]/[F2()*F2()-F1(a,b)*F1(c,d)]+Yc;
Zn=t2*(Zd-Zc)+Zc=(Zd-Zc)*[F3(c,d)*F1(a,b)-F3(a,b)*F2()]/[F2()*F2()-F1(a,b)*F1(c,d)]+Zc;
Enjoy!
参考:https://www.cnblogs.com/chuzhouGIS/archive/2011/12/12/2284774.html