激光SLAM（二）：点云拟合和平面参数化

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

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

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

取齐次坐标

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

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

求解的问题可以表示为

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

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

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

带入最小二乘问题得到

注意：这里奇异值的平方等于$A^{T}A$的特征值,V和特征值分解的V是一样的，两种方法等价。
这里奇异值是$A^{T}A$特征值的平方根，详细可以见：特征分解与SVD分解的关系

二、平面参数化

平面实际上只有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$，我们可以利用勾股定理，计算它离直线垂直距离的平方

此时最小二乘问题为

先计算关于p的jacobian

整体的目标函数关于 p 导数为

令其等于0得到，即p是点云中心

已知p后，再求解方向向量d，记
残差变为

等价于

记

则

这个问题的解d为A的最大右奇异值向量，或者$A^{T}A$的最大特征值对应的特征向量。

从图中可以看出，直线拟合是寻找最大值方向，而平面拟合是寻找特征值最小值方向

四、SVD分解

对于下面表示可以理解为矩阵A本身表示一个线性变换，下视表示对X进行线性变换得到向量b的过程

SVD分解过程可以看做

单位正交基V= $[v_1,v_2]$，首先将$x$在V坐标系下表示，对应坐标为$[{\xi }_1,{\xi }_2{]}^T$. 则SVD分解可以看做如下过程

相当于先通过$V^T$把V变为单位正交基$I$，然后做缩放，然后U进行旋转

变换之后的向量为：

一个SVD常见的应用就是PCA,详见：PCA原理
通过特征值分解来进行数据降维，过程如下

从数据降维的角度来理解我们刚刚的平面拟合和直线拟合，对于3D点云

• 平面拟合取最小特征值方向为平面法向量，相当于数据从3D降维到2D平面
• 直线拟合只取最大特征点云方向，相当于从3D降维到1D

五、LOAM中点面特征的计算

在线特征提取需要计算量比较低，因此是通过下述方式确定当前点到底是平面还是线的。
LOAM根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标，判断当前点是边缘点还是平面点，曲率计算如下

在点的左侧和右侧分别取5个点，把5个io加起来，三个轴diff计算模长，曲率最大的为Edge Point，曲率最小的为Planar Point