赞
踩
平面方程如下,n是法向量,d是截距
当多个点拟合平面时,需要求解下述最小二乘问题
取齐次坐标
最小二乘问题可以表示成如下矩阵形式
转换为通用的线性代数问题:对任意矩阵A,寻找x,使得Ax最小化,其中
求解的问题可以表示为
A
T
A
A^TA
ATA是实对称矩阵,可以对角化,因此可以进行如下特征值分解
对角特征值矩阵,这里认为它们按照从大到小的顺序排列,则:
上面问题也可用SVD分解来解释,A不论什么矩阵形式,都可以进行SVD分解
带入最小二乘问题得到
注意:这里奇异值的平方等于
A
T
A
A^TA
ATA的特征值,V和特征值分解的V是一样的,两种方法等价。
这里奇异值是
A
T
A
A^TA
ATA特征值的平方根,详细可以见:特征分解与SVD分解的关系
平面实际上只有3个自由度,这里介绍不同的平面参数化形式,拟合时不需要考虑参数化形式,但如果想对平面进行优化或者将其加入
最常见的一个参数化形式就是Hesse形式,用一个平面的单位法向量和距原点的距离d组成的向量表示:
但存在过参数化问题,平面实际上只有3个自由度,但是Hesse形式有4个参数,导致过参数化知乎,优化过程中计算出的Hessian矩阵不会满秩,不能求逆,解决方法是LM提供正则化。但收敛度会变慢。
平面的Hesse参数化形式可以被分成两个部分,一个单位法向量部分和一个距离部分。平面的Hesse形式的过参数化就是因为单位法向量部分有3个参数,但是实际只有两个自由度导致的。那么我们就可以从这个法向量着手解决过参的问题。单位法向量可以被看成是一个单位圆球上的一点。那么我们就可以用两个角度 θ和Θ来参数化点,从而表示出这个单位法向量。那么我们就可以把Hesse形式变化成如下的形式:
但是这种形式的参数化虽然是最小参数化形式,但在优化的时候会产生奇异性。
这种方式是用在平面上距离当前表示坐标系原点最近的3D坐标点来表示平面。这种表示方式有两个优点:1.是最小化参数形式;2.具有真实的物理意义。
直接上论文: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
x_k
xk,我们可以利用勾股定理,计算它离直线垂直距离的平方
此时最小二乘问题为
先计算关于p的jacobian
整体的目标函数关于 p 导数为
令其等于0得到,即p是点云中心
已知p后,再求解方向向量d,记
残差变为
等价于
记
则
这个问题的解d为A的最大右奇异值向量,或者
A
T
A
A^TA
ATA的最大特征值对应的特征向量。
从图中可以看出,直线拟合是寻找最大值方向,而平面拟合是寻找特征值最小值方向
对于下面表示可以理解为矩阵A本身表示一个线性变换,下视表示对X进行线性变换得到向量b的过程
SVD分解过程可以看做
单位正交基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分解可以看做如下过程
相当于先通过
V
T
V^T
VT把V变为单位正交基
I
I
I,然后做缩放,然后U进行旋转
变换之后的向量为:
一个SVD常见的应用就是PCA,详见:PCA原理
通过特征值分解来进行数据降维,过程如下
从数据降维的角度来理解我们刚刚的平面拟合和直线拟合,对于3D点云
在线特征提取需要计算量比较低,因此是通过下述方式确定当前点到底是平面还是线的。
LOAM根据点的曲率来计算平面光滑度作为提取当前帧的特征信息的指标,判断当前点是边缘点还是平面点,曲率计算如下
在点的左侧和右侧分别取5个点,把5个io加起来,三个轴diff计算模长,曲率最大的为Edge Point,曲率最小的为Planar Point
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。