点云法向量和平面方程

一、平面方程表示法

1.1 一般方程

$$Ax+By+Cz+D=0$$

1.2 点法式

已知平面$\Pi$上的一个点$M_0=(x_0,y_0,z_0)$和它的一个法线向量$n=(A,B,C)$，则可以唯一确定该面的平面方程$A(x-x_0)+B(y-y_0)+C(z-z_0)=0$，推导过程如下：

设$M=(x,y,z)$是平面$\Pi$上任一点，则向量$M{M}_0$和法向量一定垂直，即内积为0：
$$M{M}_0\cdot n=0$$
即：
$$\begin{array}{rl}(x-x_0,y-y_0,z-z_0)\cdot (A,B,C)& =0\\ A(x-x_0)+B(y-y_0)+C(z-z_0)& =0\end{array}$$

1.3 一般方程的系数构成法向量

设平面方程为$Ax+By+Cz+D=0$，任取满足该方程的一点$M_0=(x_0,y_0,z_0)$，则有$A{x}_0+B{y}_0+C_0=0$，两式相减为：
$$A(x-x_0)+B(y-y_0)+C(z-z_0)=0$$
即：
$$M{M}_0\perp (A,B,C)$$
由于$M_0$是平面上任意一点，因此永远有向量$(A,B,C)$垂直于$M{M}_0$，即向量$(A,B,C)$为该平面的一个法向量。

1.4 结论

已知平面方程，则法向量为平面方程的系数。

数学篇22-平面方程（点法式方程与一般方程一定要掌握）

搬运——证明线性空间中，平面方程的系数就是平面的法向量

二、拉格朗日乘数法

求解多元函数求极值的问题，可用拉格朗日乘数法，步骤如下：

（1）构造拉格朗日函数；

（2）拉格朗日函数分别对每个变量及$\lambda$求偏导，并令它们都等于0，构造出一个方程组；

（3）求解方程组上述方程组，即可解得疑似极值点。

多元函数条件极值的求法 拉格朗日乘数法_Norstc的博客-CSDN博客

三、法向量计算

3.1 问题背景

法向量的计算近似于表面相切面法线的问题，因此该问题就是一个最小二乘法平面拟合的问题。平面的一般方程为：
$$Ax+By+Cz+D=0$$
将多元系数进行归一化为：
$$\frac{A}{A^2+B^2+C^2}x+\frac{B}{A^2+B^2+C^2}y+\frac{C}{A^2+B^2+C^2}z+\frac{D}{A^2+B^2+C^2}=0$$
即：
$$ax+by+cz+d=0$$
其中，$(A,B,C)$是该平面的一个法向量，$(a,b,c)$是归一化的法向量。

因此，计算法向量的问题就转化为多元一次方程参数估计的问题。

3.2 推导过程

3.2.1 证明法向量是一个特征向量

（1）构造目标函数

保证参数估计结果尽可能地让更多的点在拟合的平面上或距拟合的平面距离最小，因此目标函数为：
$$d_i=\frac{|a{x}_i+b{y}_i+c{z}_i+d|}{\sqrt{a^2+b^2+c^2}}=|a{x}_i+b{y}_i+c{z}_i+d|$$

$$L=\sum _{i=1}^N{d}_i^2=\sum _{i=1}^N(a{x}_i+b{y}_i+c{z}_i+d{)}^2\to min$$

其中$(a,b,c)$是归一化的法向量，即$a^2+b^2+c^2=1$，则问题转化为多元一次方程求条件极值的问题，使用拉格朗日乘数法：

（2）构造拉格朗日函数：
$$\begin{array}{rl}f& =L+\lambda (a^2+b^2+c^2-1)\\ & =\sum _{i=1}^N{d}_i^2+\lambda (a^2+b^2+c^2-1)\\ & =\sum _{i=1}^N(a{x}_i+b{y}_i+c{z}_i+d{)}^2+\lambda (a^2+b^2+c^2-1)\end{array}$$
（3）目标函数对参数$d$求偏导，并令其等于0：
$$\frac{\partial L}{\partial d}=2\sum _{i=1}^N(a{x}_i+b{y}_i+c{z}_i+d)=0$$
即：
$$\begin{array}{rl}d& =-\frac{1}{N}(a\sum _i{x}_i+b\sum _i{y}_i+c\sum _i{z}_i)\\ & =-\frac{{\sum }_{i=1}^N{x}_i}{N}a-\frac{{\sum }_{i=1}^N{y}_i}{N}b-\frac{{\sum }_{i=1}^N{z}_i}{N}c\end{array}$$
将$d$分别带入点到面的距离公式、拉格朗日函数有：
$$\begin{array}{rl}{d}_i& =|a{x}_i+b{y}_i+c{z}_i+d|\\ & =|a{x}_i+b{y}_i+c{z}_i-\frac{{\sum }_{i=1}^N{x}_i}{N}a-\frac{{\sum }_{i=1}^N{y}_i}{N}b-\frac{{\sum }_{i=1}^N{z}_i}{N}c|\\ & =|a(x_i-\bar{x})+b(y-\bar{y})+c(z-\bar{z})|\end{array}$$

$$\begin{array}{rl}f& =\sum _{i=1}^N(a{x}_i+b{y}_i+c{z}_i-\frac{{\sum }_{i=1}^N{x}_i}{N}a-\frac{{\sum }_{i=1}^N{y}_i}{N}b-\frac{{\sum }_{i=1}^N{z}_i}{N}c{)}^2+\lambda (a^2+b^2+c^2-1)\\ & =\sum _{i=1}^N(a(x_i-\bar{x})+b(y-\bar{y})+c(z-\bar{z}){)}^2+\lambda (a^2+b^2+c^2-1)\end{array}$$

拉格朗日函数对各参数求偏导，并令其均为0：
$$\{\begin{array}{l}\begin{array}{rlrl}& \frac{\partial f}{\partial a}& =2\sum _{i=1}^N(x_i-\bar{x})(a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z}))+2\lambda a& =0\\ & \frac{\partial f}{\partial b}& =2\sum _{i=1}^N(y_i-\bar{y})(a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z}))+2\lambda b& =0\\ & \frac{\partial f}{\partial c}& =2\sum _{i=1}^N(z_i-\bar{z})(a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z}))+2\lambda c& =0\end{array}\end{array}$$
即：
$$\{\begin{array}{l}\begin{array}{rlr}& \sum _{i=1}^{N}a(x_i-\bar{x})(x_i-\bar{x})+b(y_i-\bar{y})(x_i-\bar{x})+c(z_i-\bar{z})(x_i-\bar{x})+\lambda a& =0\\ & \sum _{i=1}^{N}a(x_i-\bar{x})(y_i-\bar{y})+b(y_i-\bar{y})(y_i-\bar{y})+c(z_i-\bar{z})(y_i-\bar{y})+\lambda b& =0\\ & \sum _{i=1}^{N}a(x_i-\bar{x})(z_i-\bar{z})+b(y_i-\bar{y})(z_i-\bar{z})+c(z_i-\bar{z})(z_i-\bar{z})+\lambda c& =0\end{array}\end{array}$$
可用矩阵表示为：
( ∑ ( x i − x ˉ ) ( x i − x ˉ ) ∑ ( x i − x ˉ ) ( y i − y ˉ ) ∑ ( x i − x ˉ ) ( z i − z ˉ ) ∑ ( x i − x ˉ ) ( y i − y ˉ ) ∑ ( y i − y ˉ ) ( y i − y ˉ ) ∑ ( y i − y ˉ ) ( z i − z ˉ ) ∑ ( x i − x ˉ ) ( z i − z ˉ ) ∑ ( y i − y ˉ ) ( z i − z ˉ ) ∑ ( z i − z ˉ ) ( z i − z ˉ ) ) ( a b c ) = − λ ( a b c ) \begin{pmatrix} \sum{(x_i-\bar{x})(x_i-\bar{x})}&\sum{(x_i-\bar{x})(y_i-\bar{y})}&\sum{(x_i-\bar{x})(z_i-\bar{z})}\\ \sum{(x_i-\bar{x})(y_i-\bar{y})}&\sum{(y_i-\bar{y})(y_i-\bar{y})}&\sum{(y_i-\bar{y})(z_i-\bar{z})}\\ \sum{(x_i-\bar{x})(z_i-\bar{z})}&\sum{(y_i-\bar{y})(z_i-\bar{z})}&\sum{(z_i-\bar{z})(z_i-\bar{z})} \end{pmatrix} \begin{pmatrix} a\\b\\c \end{pmatrix} = -\lambda\begin{pmatrix} a\\b\\c \end{pmatrix} ​∑(xi​−xˉ)(xi​−xˉ)∑(xi​−xˉ)(yi​−yˉ​)∑(xi​−xˉ)(zi​−zˉ)​∑(xi​−xˉ)(yi​−yˉ​)∑(yi​−yˉ​)(yi​−yˉ​)∑(yi​−yˉ​)(zi​−zˉ)​∑(xi​−xˉ)(zi​−zˉ)∑(yi​−yˉ​)(zi​−zˉ)∑(zi​−zˉ)(zi​−zˉ)​ ​ ​abc​ ​=−λ ​abc​ ​
即：
$$A^{\prime }{x}^{\prime }={\lambda }^{\prime }{x}^{\prime }$$
其中，矩阵$A^{\prime }$是3×3的协方差矩阵，$x^{\prime }={\left(\begin{array}{ccc}a& b& c\end{array}\right)}^T$为该矩阵的一个特征向量，也就是说法向量是该矩阵的一个特征向量。

3.2.2 证明法向量是最小特征值对应的特征向量

由$a^2+b^2+c^2=1$可知$x^{\prime T}{x}^{\prime }=1$，即内积为1，$x^{\prime }\cdot x^{\prime }=1$，则有：
$$A^{\prime }\times x^{\prime }={\lambda }^{\prime }{x}^{\prime }\Rightarrow A^{\prime }\times x^{\prime }\cdot x^{\prime }={\lambda }^{\prime }{x}^{\prime }\cdot x^{\prime }\Rightarrow {\lambda }^{\prime }=A^{\prime }\times x^{\prime }\cdot x^{\prime }$$
其中符号 “×” 表示矩阵相乘，符号 "·"表示计算向量内积。
λ ′ = ( ∑ ( x i − x ˉ ) ( x i − x ˉ ) ∑ ( x i − x ˉ ) ( y i − y ˉ ) ∑ ( x i − x ˉ ) ( z i − z ˉ ) ∑ ( x i − x ˉ ) ( y i − y ˉ ) ∑ ( y i − y ˉ ) ( y i − y ˉ ) ∑ ( y i − y ˉ ) ( z i − z ˉ ) ∑ ( x i − x ˉ ) ( z i − z ˉ ) ∑ ( y i − y ˉ ) ( z i − z ˉ ) ∑ ( z i − z ˉ ) ( z i − z ˉ ) ) × ( a b c ) ⋅ ( a b c ) = ( a ∑ ( x i − x ˉ ) ( x i − x ˉ ) + b ∑ ( x i − x ˉ ) ( y i − y ˉ ) + c ∑ ( x i − x ˉ ) ( z i − z ˉ ) a ∑ ( x i − x ˉ ) ( y i − y ˉ ) + b ∑ ( y i − y ˉ ) ( y i − y ˉ ) + c ∑ ( y i − y ˉ ) ( z i − z ˉ ) a ∑ ( x i − x ˉ ) ( z i − z ˉ ) + b ∑ ( y i − y ˉ ) ( z i − z ˉ ) + c ∑ ( z i − z ˉ ) ( z i − z ˉ ) ) ⋅ ( a b c ) = ∑ i = 1 N ( a ( x i − x ˉ ) + b ( y i − y ˉ ) + c ( z i − z ˉ ) ) 2 = ∑ i = 1 N d i 2 \begin{align} \lambda'&=\begin{pmatrix} \sum{(x_i-\bar{x})(x_i-\bar{x})}&\sum{(x_i-\bar{x})(y_i-\bar{y})}&\sum{(x_i-\bar{x})(z_i-\bar{z})}\\ \sum{(x_i-\bar{x})(y_i-\bar{y})}&\sum{(y_i-\bar{y})(y_i-\bar{y})}&\sum{(y_i-\bar{y})(z_i-\bar{z})}\\ \sum{(x_i-\bar{x})(z_i-\bar{z})}&\sum{(y_i-\bar{y})(z_i-\bar{z})}&\sum{(z_i-\bar{z})(z_i-\bar{z})} \end{pmatrix} \times \begin{pmatrix} a\\b\\c \end{pmatrix} \cdot \begin{pmatrix} a\\b\\c \end{pmatrix}\notag\\ &= \begin{pmatrix} a\sum{(x_i-\bar{x})(x_i-\bar{x})} +b\sum{(x_i-\bar{x})(y_i-\bar{y})}+c\sum{(x_i-\bar{x})(z_i-\bar{z})}\\ a\sum{(x_i-\bar{x})(y_i-\bar{y})}+b\sum{(y_i-\bar{y})(y_i-\bar{y})}+c\sum{(y_i-\bar{y})(z_i-\bar{z})}\\ a\sum{(x_i-\bar{x})(z_i-\bar{z})}+b\sum{(y_i-\bar{y})(z_i-\bar{z})}+c\sum{(z_i-\bar{z})(z_i-\bar{z})} \end{pmatrix} \cdot \begin{pmatrix} a\\b\\c \end{pmatrix}\notag\\ &=\sum_{i=1}^{N}{(a(x_i-\bar{x})+b(y_i-\bar{y})+c(z_i-\bar{z}))^2}\notag\\ &=\sum_{i=1}^{N}d_i^2\notag \end{align} λ′​= ​∑(xi​−xˉ)(xi​−xˉ)∑(xi​−xˉ)(yi​−yˉ​)∑(xi​−xˉ)(zi​−zˉ)​∑(xi​−xˉ)(yi​−yˉ​)∑(yi​−yˉ​)(yi​−yˉ​)∑(yi​−yˉ​)(zi​−zˉ)​∑(xi​−xˉ)(zi​−zˉ)∑(yi​−yˉ​)(zi​−zˉ)∑(zi​−zˉ)(zi​−zˉ)​ ​× ​abc​ ​⋅ ​abc​ ​= ​a∑(xi​−xˉ)(xi​−xˉ)+b∑(xi​−xˉ)(yi​−yˉ​)+c∑(xi​−xˉ)(zi​−zˉ)a∑(xi​−xˉ)(yi​−yˉ​)+b∑(yi​−yˉ​)(yi​−yˉ​)+c∑(yi​−yˉ​)(zi​−zˉ)a∑(xi​−xˉ)(zi​−zˉ)+b∑(yi​−yˉ​)(zi​−zˉ)+c∑(zi​−zˉ)(zi​−zˉ)​ ​⋅ ​abc​ ​=i=1∑N​(a(xi​−xˉ)+b(yi​−yˉ​)+c(zi​−zˉ))2=i=1∑N​di2​​
因此，要想$L={\sum }_{i=1}^N{d}_i^2\to min$，必须有${\lambda }^{\prime }\to min$，则法向量就是协方差矩阵的最小特征值对应的特征向量。PCA找的是最大特征值对应的特征向量，因为它要的是最重要的方向，而法向量是垂直于原始数据的方向，所以是最不重要的方向，因此是最小特征值对应的特征向量。（引申为最小奇异值对应的奇异向量，因为奇异值是特征值的平方根！！！）

点云法向量估计原理及应用PCL

四、已知三点求平面方程表达式

将三个点坐标$$P_1=(x_1,y_1,z_1),P_2=(x_2,y_2,z_2),P_3=(x_3,y_3,z_3)$$分别带入点法式平面方程：
$$\{\begin{array}{llc}& A(x-x_1)+B(y-y_1)+C(z-z_1)& =0\\ & A(x-x_2)+B(y-y_2)+C(z-z_2)& =0\\ & A(x-x_3)+B(y-y_3)+C(z-z_3)& =0\end{array}$$
利用叉乘性质 “两个向量的叉乘结果为同时垂直于参与叉乘计算的两个向量的一个向量” 可知$P_1{P}_2$和$P_1{P}_3$的叉乘结果同时垂直于这两个向量，所以叉乘结果就是平面的法向量，即法向量为：
n ⃗ = P 1 P 2 →   ×   P 1 P 3 → = ( x 1 − x 2 y 1 − y 2 z 1 − z 2 ) × ( x 1 − x 3 y 1 − y 3 z 1 − z 3 ) = ( ( y 1 − y 2 ) ( z 1 − z 3 ) − ( z 1 − z 2 ) ( y 1 − y 3 ) ( z 1 − z 2 ) ( x 1 − x 3 ) − ( x 1 − x 2 ) ( z 1 − z 3 ) ( x 1 − x 2 ) ( y 1 − y 3 ) − ( y 1 − y 2 ) ( x 1 − x 3 ) ) \begin{align} \vec{n}&=\overrightarrow{P_1P_2}\,\times\,\overrightarrow{P_1P_3}\notag\\ &=\left( \begin{matrix} x_1-x_2 \\ y_1-y_2 \\ z_1-z_2 \end{matrix} \right) \times \left( \begin{matrix} x_1-x_3 \\ y_1-y_3 \\ z_1-z_3 \end{matrix} \right) \notag \\ &= \left(\begin{matrix} (y_1-y_2)(z_1-z_3)-(z_1-z_2)(y_1-y_3)\\ (z_1-z_2)(x_1-x_3)-(x_1-x_2)(z_1-z_3)\\ (x_1-x_2)(y_1-y_3)-(y_1-y_2)(x_1-x_3) \end{matrix}\right) \notag \end{align} n ​=P1​P2​ ​×P1​P3​ ​= ​x1​−x2​y1​−y2​z1​−z2​​ ​× ​x1​−x3​y1​−y3​z1​−z3​​ ​= ​(y1​−y2​)(z1​−z3​)−(z1​−z2​)(y1​−y3​)(z1​−z2​)(x1​−x3​)−(x1​−x2​)(z1​−z3​)(x1​−x2​)(y1​−y3​)−(y1​−y2​)(x1​−x3​)​ ​​
即：
$$\{\begin{array}{l}\begin{array}{rl}& A=(y_1-y_2)(z_1-z_3)-(z_1-z_2)(y_1-y_3)\\ & B=(z_1-z_2)(x_1-x_3)-(x_1-x_2)(z_1-z_3)\\ & C=(x_1-x_2)(y_1-y_3)-(y_1-y_2)(x_1-x_3)\end{array}\end{array}$$
可以选择性进行归一化：
$$\{\begin{array}{l}\begin{array}{rl}& A^{\prime }=\frac{A}{\sqrt{A^2+B^2+C^2}}\\ & B^{\prime }=\frac{B}{\sqrt{A^2+B^2+C^2}}\\ & C^{\prime }=\frac{C}{\sqrt{A^2+B^2+C^2}}\end{array}\end{array}$$
带入可得点法式平面方程表达式。

向量点乘与叉乘的概念及几何意义

【回归本源】1.3-向量叉乘

五、已知协方差矩阵计算法向量

拟合平面一定经过离散点的质心。

$\mathbf{A}=\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^{-1}$，其中当奇异值矩阵中的奇异值按照从大到小排序时，V的最后一列就是协方差A求得的法向量！

3D点云处理:用SVD分解法和最小二乘法拟合平面点云，求解平面方程_svd平面拟合-CSDN博客

SVD 的V 的最后一列是Ax = 0的解_eigen ax=0-CSDN博客

为何SVD分解出来的V最后一列向量为Ax=0的最优解（以orbslam2中求解单应矩阵为例子）

 

（本文完整的pdf请关注公众号“张张学算法”，并回复“022”获取~）