1.原理推导

  给定空间中的一点P(x,y,z)和直线L上两点A(x1,y1,z1)、B(x2,y2,z2),计算P点到直线的距离。

2.求解方法

2.1等面积法

取点P’为P在AB上的垂足，由向量法计算平行四边形面积公式可知

由此，可推出P到直线AB的距离d为

C++代码如下：

float computeDistancePointAndLine(const Eigen::Vector3f point,
                                  const Eigen::Vector3f lineBegin,
                                  const Eigen::Vector3f lineEnd)
{
    //直线方向向量
    Eigen::Vector3f line_dir = lineEnd -lineBegin;

    //直线上某一点的向量到点的向量
    Eigen::Vector3f line_start_to_point = point - lineBegin;

    Eigen::Vector3f cross_dir = line_dir.cross(line_start_to_point);
    return cross_dir.norm() / line_dir.norm();
}

2.2向量法

设向量AB与向量AP的夹角为θ，由向量乘法运算法则可知

由此，可推出P到直线AB的距离d为

C++代码如下：

float computeDistancePointAndLine(const Eigen::Vector3f point,
                                  const Eigen::Vector3f lineBegin,
                                  const Eigen::Vector3f lineEnd)
{
    //直线方向向量
    Eigen::Vector3f line_dir = lineEnd -lineBegin;

    //直线上某一点的向量到点的向量
    Eigen::Vector3f line_start_to_point = point - lineBegin;

    Eigen::Vector3f dot_dir = line_dir.dot(line_start_to_point);
    return dot_dir .norm() / line_dir.norm();
}