Eigen 由三点求平面方程及平面法向量

一、原理

  已知三点$P1(x_1,y_1,z_1)$、$P2(x_2,y_2,z_2)$、$P3(x_2,y_2,z_2)$，要求该三点所确定的平面方程及法向量。我们知道两点共线，三点一定共面，平面的法向量一定垂直于平面中任意两点之间的连线。所以求解的思路就是找到一个向量$n$与平面的两条不平行的直线垂直即可，即计算$n$与平面中两个向量的叉乘。
  做平面中的两个向量$V_1=P_1{P}_2=(x_2-x_1,y_2-y_1,z_2-z_1)$和$V_2=P_1{P}_3=(x_3-x_1,y_3-y_1,z_3-z_1)$，平面法向量和这两个向量都垂直，因此法向量$n$:
n = V 1 × V 2 = ∣ i j k x 2 − x 1 y 2 − y 1 z 2 − z 1 x 3 − x 1 y 3 − y 1 z 3 − z 1 ∣ = a i + b j + c k = ( a , b , c ) n=V_1 \times V_2 = \left| ijkx2−x1y2−y1z2−z1x3−x1y3−y1z3−z1

\right|=ai+bj+ck=(a,b,c) n=V1​×V2​=∣ ∣​ix2​−x1​x3​−x1​​jy2​−y1​y3​−y1​​kz2​−z1​z3​−z1​​∣ ∣​=ai+bj+ck=(a,b,c)
其中:
$$\begin{gathered} a=(y_2-y_1)\ast (z_3-z_1)-(y_3-y_1)\ast (z_2-z_1) \\ b=(z_2-z_1)\ast (x_3-x_1)-(z_3-z_1)\ast (x_2-x_1) \\ c=(x_2-x_1)\ast (y_3-y_1)-(x_3-x_1)\ast (y_2-y_1) \end{gathered}$$

平面方程为:$a(x-x_1)+b(y-y_1)+c(z-z_1)=0$，所以$d=-a\ast x_1-b\ast y_1-c\ast z_1$，平面方程为：
$$ax+by+cz+d=0$$

二、Eigen实现

void getNormalBy3Points(
    const Eigen::Vector3d &P1,
    const Eigen::Vector3d &P2,
    const Eigen::Vector3d &P3,
          Eigen::Vector3d &normal)
{
    Eigen::Vector3d V1=P1-P2;
    Eigen::Vector3d V2=P1-P3;
    normal=V1.cross(V2);
}
void getPlaneBy3Points(
    const Eigen::Vector3d &P1,
    const Eigen::Vector3d &P2,
    const Eigen::Vector3d &P3,
          Eigen::Vector4d &plane_coeff)
{
    Eigen::Vector3d V1=P1-P2;
    Eigen::Vector3d V2=P1-P3;
    Eigen::Vector3d normal;
    normal=V1.cross(V2);
    plane_coeff.block<3,1>(0,0)=normal;
    plane_coeff(3)=-plane_coeff(0)*P1(0)-plane_coeff(1)*P1(1)-plane_coeff(2)*P1(2);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

主函数代码如下：

Eigen::Vector3d P1(0,0,0);
Eigen::Vector3d P2(1,0,0);
Eigen::Vector3d P3(0,1,0);

Eigen::Vector3d normal;
Eigen::Vector4d plane_coeff;

getNormalBy3Points(P1,P2,P3,normal);
getPlaneBy3Points(P1,P2,P3,plane_coeff);

std::cout<<normal.transpose()<<std::endl;
std::cout<<plane_coeff.transpose()<<std::endl;
1
2
3
4
5
6
7
8
9
10
11
12

输出如下：

0 0 1
0  0  1 -0
1
2

最好用别的数据测试一下，这个测试数据太简单了。