对于两空间直线来说，计算交点和最小公垂线是一码事，交点即最小公垂线两个垂足的中心。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

pcl::lineToLineSegment() 计算空间直线的交点和最小公垂线

Enjoy！

猜你喜欢