Eigen 求点到平面的距离

一、原理

1.平面的一般方程

$$ax+by+cz+d=0$$
其中$n=(a,b,c)$是平面的法向量，$d$是将平面平移到坐标原点所需的距离，$d=0$时，平面过原点

2.向量的模

  给定一个向量$V(x,y,z)$，其模为$|V|=sqrt(x^2+y^2+z^2)$。

3.向量的点积

  给定两个向量$V_1(x_1,y_1,z_1)$和$V_2(x_2,y_2,z_2)$，则他们的内积是$V_1{V}_2=x_1{x}_2+y_1{y}_2+z_1{z}_2$。其几何意义为一个向量到另一个向量的投影长度，有$V_1{V}_2=|V_1||V_2|\cdot \cos \theta$，其中$\theta$为两个向量的夹角。

4.点到平面的距离

  给定平面方程$ax+by+cz+d=0$，求平面外一点$Q(x_0,y_0,z_0)$到平面的距离$d$。如下图所示：

  取平面中任意一点$P(x,y,z)$，链接$PQ$，过$P$做平面的法向量$\overrightarrow{n}=(a,b,c)$，可知$Q$到平面的距离$d$恰恰是$\overrightarrow{\mathrm{PQ}}=(x_0-x,y_0-y,z_0-z$在法向量$\overrightarrow{n})$上面的投影长度，即：
d = ∣ P Q → ∣ ⋅ cos ⁡ θ = ∣ n → ∣ ∣ n → ∣ ∣ P Q → ∣ ⋅ cos ⁡ θ （分子的三项组合刚好为向量 n → 和向量 P Q → 的点积） = P Q ⋅ n ∣ n → ∣ = a ( x 0 − x ) + b ( y 0 − y ) + c ( z 0 − z ) a 2 + b 2 + c 2 = a x 0 + b y 0 + c z 0 − ( a x + b y + c z ) a 2 + b 2 + c 2 = a x 0 + b y 0 + c z 0 + d a 2 + b 2 + c 2 ( 因为 a x + b y + c z = − d ) = ∣ a x 0 + b y 0 + c z 0 + d ∣ a 2 + b 2 + c 2 ( 为保证结果为正，所以加绝对值 ) d=|→PQ|⋅cosθ=|→n||→n||→PQ|⋅cosθ（分子的三项组合刚好为向量→n和向量→PQ的点积）=PQ⋅n|→n|=a(x0−x)+b(y0−y)+c(z0−z)√a2+b2+c2=ax0+by0+cz0−(ax+by+cz)√a2+b2+c2=ax0+by0+cz0+d√a2+b2+c2(因为ax+by+cz=−d)=|ax0+by0+cz0+d|√a2+b2+c2(为保证结果为正，所以加绝对值)

d​=∣PQ→​∣⋅cosθ=∣n→∣∣n→∣​∣PQ→​∣⋅cosθ（分子的三项组合刚好为向量n→和向量PQ→​的点积）=∣n→∣PQ⋅n​=a2+b2+c2 ​a(x0​−x)+b(y0​−y)+c(z0​−z)​=a2+b2+c2 ​ax0​+by0​+cz0​−(ax+by+cz)​=a2+b2+c2 ​ax0​+by0​+cz0​+d​(因为ax+by+cz=−d)=a2+b2+c2 ​∣ax0​+by0​+cz0​+d∣​(为保证结果为正，所以加绝对值)​
如果法向量是单位向量的话，分母为1，参考。

二、Eigen实现

double point2planeDistance(
    const Eigen::Vector3d &P,
    const Eigen::Vector4d &plane_coeff)
{
    Eigen::Vector4d point;
    point<<P,1;
    return fabs(point.dot(plane_coeff))/plane_coeff.block<3,1>(0,0).norm();
}
1
2
3
4
5
6
7
8

 Eigen::Vector3d p;
 Eigen::Vector4d plane;
 p<<0,0,5;
 plane<<0,0,1,1;
 std::cout<<point2planeDistance(p,plane)<<std::endl;
1
2
3
4
5

输出：6
输出的是点$(0,0,5)$到平面$z=-1$的距离。