一、平面拟合：特征值法和SVD法

平面方程如下，n是法向量，d是截距

当多个点拟合平面时，需要求解下述最小二乘问题

取齐次坐标

最小二乘问题可以表示成如下矩阵形式

转换为通用的线性代数问题:对任意矩阵A,寻找x,使得Ax最小化,其中

求解的问题可以表示为

$A^{T}A$是实对称矩阵，可以对角化,因此可以进行如下特征值分解

对角特征值矩阵，这里认为它们按照从大到小的顺序排列，则：

上面问题也可用SVD分解来解释，A不论什么矩阵形式，都可以进行SVD分解

带入最小二乘问题得到

注意：这里奇异值的平方等于$A^{T}A$的特征值,V和特征值分解的V是一样的，两种方法等价。

二、平面参数化

平面实际上只有3个自由度，这里介绍不同的平面参数化形式，拟合时不需要考虑参数化形式，但如果想对平面进行优化或者将其加入

1）Hesse形式

最常见的一个参数化形式就是Hesse形式，用一个平面的单位法向量和距原点的距离d组成的向量表示：

但存在过参数化问题，平面实际上只有3个自由度，但是Hesse形式有4个参数，导致过参数化知乎，优化过程中计算出的Hessian矩阵不会满秩，不能求逆，解决方法是LM提供正则化。但收敛度会变慢。

2）球坐标

平面的Hesse参数化形式可以被分成两个部分，一个单位法向量部分和一个距离部分。平面的Hesse形式的过参数化就是因为单位法向量部分有3个参数，但是实际只有两个自由度导致的。那么我们就可以从这个法向量着手解决过参的问题。单位法向量可以被看成是一个单位圆球上的一点。那么我们就可以用两个角度 θ和Θ来参数化点，从而表示出这个单位法向量。那么我们就可以把Hesse形式变化成如下的形式：

但是这种形式的参数化虽然是最小参数化形式，但在优化的时候会产生奇异性。

3）最近点

这种方式是用在平面上距离当前表示坐标系原点最近的3D坐标点来表示平面。这种表示方式有两个优点：1.是最小化参数形式；2.具有真实的物理意义。

4) 单位四元数

直接上论文：The homogeneous plane representation is over- parametrized and therefore requires some special care during optimization—we will apply a similar solution as is commonly used for quaternions, and therefore start with a review of the quaternion case.

细节比较多，参考论文：Simultaneous Localization and Mapping with Infinite Planes

三、直线拟合

我们使用直线上一点再加上直线方向矢量来描述直线，直线上的点 x 满足方程：

d为直线的方向，满足 ||d∥ = 1；p为直线 l 上某个点，t 为直线参数。我们想求的是d和p，共 6 个未知数。显然，当给定的点集较大时，这依然是一个超定方程，需要 构造最小二乘问题进行求解。
对于任意一个不在 l 上的点 $x_k$，我们可以利用勾股定理，计算它离直线垂直距离的平方