使用PCA拟合平面

背景

本文参考以下两篇平面解析的论文。
《Fast Plane Extraction in Organized Point Clouds Using Agglomerative Hierarchical Clustering》2014 ICRA
《Fast Cylinder and Plane Extraction from Depth Cameras for Visual Odometry》2018 IROS

这两篇论文在为数不多的平面解析中都取得了较好的效果，并且均开源。本文不详细解析论文本身的方法，而是简单的分析代码是如何使用PCA这种方法来拟合平面的。

三点确定一平面

还是回顾一下基础。

平面方程可以由下列两种方式表示。
Ax+By+Cz+D=0 截距式
A(x-x0)+B(y-y0)+C(z-z0)=0 点法式

其中，A、B、C组成的向量为平面的法向量。

为啥呢？

因为平面法向量垂直于平面任何向量，有 ：平面法向量 点乘· 平面向量 = 0 ，对应着 $$A(x-x0)+B(y-y0)+C(z-z0)=0$$
故A,B,C组成的向量为平面法向量。

我们知道，三点可以确定一个平面。假如我们有三个空间点：
$$P_0=(x_0,y_0,z_0)，P_1=(x_1,y_1,z_1)，P_2=(x_2,y_2,z_2)$$
以$P_0$为中心点得到两平面向量：
$$V_0=P_1-P_0,V_1=P_2-P_0$$

平面法向量为：$$n=V_1\times V_2$$

即上面所说的(A,B,C)。

多点拟合平面

基础数学知识

协方差：
$$\begin{array}{rl}Cov(X,Y)& =E[(X-E[X])(Y-E(Y))]\\ & =E[XY]-2E[Y][X]+E[X]E[Y]\\ & =E[XY]-E[X]E[Y]\end{array}$$
协方差矩阵，可描述不同变量关系的矩阵。对角元素是自身的方差，非对角元素是某变量与另一变量的协方差。
C = ( c i j ) n × n = ( c 11 c 12 ⋯ c 1 n c 21 c 22 ⋯ c 2 n ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ c n 1 c n 2 … c n n ) C=\left(c_{i j}\right)_{n \times n}=\left(\begin{array}{cccc}c_{11} & c_{12} & \cdots & c_{1 n} \\ c_{21} & c_{22} & \cdots & c_{2 n} \\ \cdot & \cdot & & \cdot \\ \cdot & \cdot & & \cdot \\ \cdot & \cdot & & \cdot \\ c_{n 1} & c_{n 2} & \ldots & c_{n n}\end{array}\right) C=(cij​)n×n​=⎝⎜⎜