激光SLAM(二):点云拟合和平面参数化

点云拟合和平面参数

  • 一、平面拟合:特征值法和SVD法
  • 二、平面参数化
    • 1)Hesse形式
    • 2)球坐标
    • 3)最近点
    • 4) 单位四元数
  • 三、直线拟合
  • 四、SVD分解
  • 五、LOAM中点面特征的计算

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

平面方程如下,n是法向量,d是截距
在这里插入图片描述
当多个点拟合平面时,需要求解下述最小二乘问题
在这里插入图片描述
取齐次坐标
在这里插入图片描述
最小二乘问题可以表示成如下矩阵形式
在这里插入图片描述
转换为通用的线性代数问题:对任意矩阵A,寻找x,使得Ax最小化,其中
在这里插入图片描述
求解的问题可以表示为
在这里插入图片描述
A T A A^TA ATA是实对称矩阵,可以对角化,因此可以进行如下特征值分解

在这里插入图片描述
对角特征值矩阵,这里认为它们按照从大到小的顺序排列,则:
激光SLAM(二):点云拟合和平面参数化_第1张图片

上面问题也可用SVD分解来解释,A不论什么矩阵形式,都可以进行SVD分解
在这里插入图片描述
带入最小二乘问题得到
在这里插入图片描述
注意:这里奇异值的平方等于 A T A A^TA ATA的特征值,V和特征值分解的V是一样的,两种方法等价。
这里奇异值是 A T A A^TA ATA特征值的平方根,详细可以见:特征分解与SVD分解的关系

二、平面参数化

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

1)Hesse形式

最常见的一个参数化形式就是Hesse形式,用一个平面的单位法向量和距原点的距离d组成的向量表示:
在这里插入图片描述
但存在过参数化问题,平面实际上只有3个自由度,但是Hesse形式有4个参数,导致过参数化知乎,优化过程中计算出的Hessian矩阵不会满秩,不能求逆,解决方法是LM提供正则化。但收敛度会变慢。

2)球坐标

平面的Hesse参数化形式可以被分成两个部分,一个单位法向量部分和一个距离部分。平面的Hesse形式的过参数化就是因为单位法向量部分有3个参数,但是实际只有两个自由度导致的。那么我们就可以从这个法向量着手解决过参的问题。单位法向量可以被看成是一个单位圆球上的一点。那么我们就可以用两个角度 θ和Θ来参数化点,从而表示出这个单位法向量。那么我们就可以把Hesse形式变化成如下的形式:
在这里插入图片描述
但是这种形式的参数化虽然是最小参数化形式,但在优化的时候会产生奇异性。

3)最近点

这种方式是用在平面上距离当前表示坐标系原点最近的3D坐标点来表示平面。这种表示方式有两个优点:1.是最小化参数形式;2.具有真实的物理意义。
激光SLAM(二):点云拟合和平面参数化_第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.
激光SLAM(二):点云拟合和平面参数化_第3张图片
细节比较多,参考论文:Simultaneous Localization and Mapping with Infinite Planes

三、直线拟合

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

在这里插入图片描述
d为直线的方向,满足 ||d∥ = 1;p为直线 l 上某个点,t 为直线参数。我们想求的是d和p,共 6 个未知数。显然,当给定的点集较大时,这依然是一个超定方程,需要 构造最小二乘问题进行求解。
对于任意一个不在 l 上的点 x k x_k xk,我们可以利用勾股定理,计算它离直线垂直距离的平方
在这里插入图片描述
此时最小二乘问题为
在这里插入图片描述
先计算关于p的jacobian
激光SLAM(二):点云拟合和平面参数化_第4张图片
整体的目标函数关于 p 导数为
激光SLAM(二):点云拟合和平面参数化_第5张图片
令其等于0得到,即p是点云中心
在这里插入图片描述
已知p后,再求解方向向量d,记在这里插入图片描述
残差变为
在这里插入图片描述
等价于
在这里插入图片描述

在这里插入图片描述

在这里插入图片描述
这个问题的解d为A的最大右奇异值向量,或者 A T A A^TA ATA的最大特征值对应的特征向量。
激光SLAM(二):点云拟合和平面参数化_第6张图片
从图中可以看出,直线拟合是寻找最大值方向,而平面拟合是寻找特征值最小值方向

四、SVD分解

对于下面表示可以理解为矩阵A本身表示一个线性变换,下视表示对X进行线性变换得到向量b的过程
在这里插入图片描述
SVD分解过程可以看做
激光SLAM(二):点云拟合和平面参数化_第7张图片
单位正交基V= [ v 1 , v 2 ] [v_1, v_2] [v1,v2],首先将 x x x在V坐标系下表示,对应坐标为 [ ξ 1 , ξ 2 ] T [\xi_1, \xi_2]^T [ξ1,ξ2]T. 则SVD分解可以看做如下过程
激光SLAM(二):点云拟合和平面参数化_第8张图片
相当于先通过 V T V^T VT把V变为单位正交基 I I I,然后做缩放,然后U进行旋转
激光SLAM(二):点云拟合和平面参数化_第9张图片
变换之后的向量为:
在这里插入图片描述
一个SVD常见的应用就是PCA,详见:PCA原理
通过特征值分解来进行数据降维,过程如下
激光SLAM(二):点云拟合和平面参数化_第10张图片
从数据降维的角度来理解我们刚刚的平面拟合和直线拟合,对于3D点云

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

五、LOAM中点面特征的计算

在线特征提取需要计算量比较低,因此是通过下述方式确定当前点到底是平面还是线的。
LOAM根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标,判断当前点是边缘点还是平面点,曲率计算如下
在这里插入图片描述
在点的左侧和右侧分别取5个点,把5个io加起来,三个轴diff计算模长,曲率最大的为Edge Point,曲率最小的为Planar Point
激光SLAM(二):点云拟合和平面参数化_第11张图片

你可能感兴趣的:(激光SLAM,平面,机器学习,算法)