本专栏基于深蓝学院《多传感器融合定位》课程基础上进行拓展，对多传感器融合SLAM的学习过程进行记录
第一章 3D激光里程计
激光SLAM整体框架
激光SLAM整体框架如图所示，主要包括以下几个部分：点云预处理，前端里程计，关键帧提取以及后端优化。其中点云预处理部分负责接收传感器数据，进行多传感器的数据同步，去除点云的无效点，进行去畸变处理等工作。其次前端里程计负责接收预处理后的规范化的点云进行两帧之间的配准并输出初步估计的先验位姿。然后关键帧提取部分负责接收先验位姿并根据一定条件选取关键帧，输出关键帧之间的相对位姿约束和点云。最后后端优化部分负责接收来自各种传感器的位姿约束以及关键帧所包含的点云，然后进行位姿优化，最后输出更精准的位姿并构建轨迹和地图。
点云预处理
如何对激光雷达所采集到的2D或3D点云数据进行处理是激光SLAM算法的重要内容。点云预处理部分通常包含点云的滤波、点云的分割、点云的运动补偿等工作。其中点云的滤波目的是去除点云中的噪声点或者因为遮挡等问题造成的离群点，同时也可以对点云进行下采样来减少数据量。点云的聚类点云的运动补偿用于解决由于激光雷达本身的运动所造成的点云坐标的畸变。点云的分割的主要思想是将点云分割聚类成若干个互不相交的子集来进行目标区域和背景点云的分离。本节将主要对以上三种点云预处理办法进行介绍。
点云的滤波
点云的滤波可以实现很多种功能，比如去除噪声点、离群点、平滑点云以及数据压缩等等。本节将对实现以上几种功能的相关的滤波器做简单介绍。
去除噪声点、离群点——统计滤波器
统计滤波器用于去除明显离群点和噪声点。离群点的特征是在空间中分布稀疏，根据这一特征，可以定义某处点云小于某个密度时点云无效。若计算每个点到其最近的k个点平均距离，则点云中所有点的距离应构成高斯分布。根据给定均值与方差，可剔除方差之外的点。即使方差之外的点是正确点，但是由于其太稀疏，因此带来的有效信息也是很少的，因此剔除此类点所产生的负面效应是很小的。
数据压缩、下采样——体素滤波器
体素滤波器是一种下采样的滤波器，它的作用是使用体素化方法减少点云数量，采用体素网格中接近中心点的点替代体素内的所有点云。这种方法比直接使用中心点要慢，但是更加精确。这种方式可以减少点云数据，但同时保存点云的形状特征，在激光SLAM中非常实用。
去噪、平滑——高斯滤波器
高斯滤波是一种线性平滑滤波，适用于消除高斯噪声。采用加权平均方式，根据欧式距离的高斯分布计算权重，再通过权重加权平均的方式得到当前点的滤波后的点。高斯滤波器利用标准差去噪，适用于呈正态分布的数据平滑效果较好，但是边缘角点也会被较大的平滑。
点云的分割
即使经过了点云滤波等处理，目标区域表面信息和背景信息的数据仍然可能是混合在一起的，这给后续的处理带来了一定的难度。点云分割是根据空间，几何和纹理等特征对点云进行划分，使得同一划分内的点云拥有相似的特征。点云的有效分割往往是许多应用的前提，如特征描述和提取。因此有必要采用点云分割技术进行目标区域和背景点云的分离。下面介绍几种常见的点云分割方法。
基于属性的分割方法
该方法是一种鲁棒性较好的分割方法，通常包括两个单独的步骤：首先是基于属性的计算，然后将根据计算点的属性进行聚类。这种聚类方法一般能适应空间关系和点云的各种属性，最终将不同的属性的点云分割出来。但是这种方法的局限性在于高度依赖派生属性的质量，所以要求第一步能够精确地计算点云数据的属性，这样才会在第二步中根据属性的类别分割出最佳的效果。
目前使用的基于属性的分割方法有欧式聚类和密度聚类。下面对这两种方法进行介绍。
欧式聚类与条件欧式聚类
对于欧式聚类来说，距离判断准则为欧氏距离。对于空间某点 p p p，通过KD-Tree近邻搜索算法找到 k k k个离 p p p点最近的点，这些点中距离小于设定阈值的便聚类到集合 Q Q Q中。如果 Q Q Q中元素的数目不再增加，则整个聚类过程结束；否则须在集合Q中选取 p p p点以外的点，重复上述过程，直到 Q Q Q中元素的数目不再增加为止。条件欧式聚类的工作方式与标准的欧式聚类方法基本一样。条件欧式聚类除了要检查距离，还需要施加一个特殊的，可以自定义的限制，以便符合要求的点被添加到一个集群。
密度聚类
与基于距离的聚类算法不同的是，基于密度的聚类算法可以发现任意形状的聚类。在基于密度的聚类算法中，通过在数据集中寻找被低密度区域分离的高密度区域，将分离出的高密度区域作为一个独立的类别。
DBSCAN算法，全称为“Density-Based Spatial Clustering of Applications with Noise”，也就是“基于密度的聚类”。此类算法是假设聚类结构能通过样本分布的紧密程度确定，从样本密度的角度来考察样本之间的可连续性，并基于可连接样本不断扩展聚类簇以获得最终的聚类结果。DBSCAN算法具有抗噪声、无需指定类别个数、可以在空间数据中发现任意形状的聚类等优点，但也有对高维数据处理不佳的缺点。
基于模型的分割方法
该方法是基于几何形状比如球形、圆锥、平面和圆柱形来对点云进行分组。那么根据这些几何形状，具有相同的数学表示的点将会被分割为同一组点。该方法采用RANSAC（Random sample consensus），中文即“随机抽样一致算法”。RANSAC方法通过迭代的方式从一组包含离群点数据（outlier或者错误数据）的被观测数据中估算出数学模型的参数。相比最小二乘方法，它融合了剔除不合格数据的思想，因此对于有部分错误数据的数据样本，能够更快更准的给出辨识结果。这种应用极为广泛且可以认为是模型拟合的最先进技术的母版，当前3D点云的分割的很多改进方法都是继承了这种方法。基于模型的方法具有纯粹的数学原理，快速且强大。
基于深度学习的分割方法
传统的点云分割主要依赖聚类算法和基于随机采样一致性的分割算法，在很多技术上得到了广泛应用，但当点云规模不断增大时，传统的分割算法已经很难满足实际需要，因此就需要结合深度学习进行分割。目前有5种比较前沿的点云分割网络，包括PointNet/PointNet++、PCT、Cylinder以及JSNet网络。这里不做过多赘述。
运动补偿
什么是运动畸变
激光雷达的一帧数据是指在过去的一个周期内激光雷达旋转扫描得到的点的集合，而不是某个时刻的数据。这些数据仅有一个相同的时间戳。因为每个激光点的坐标都是相对于激光雷达的，因此在激光雷达运动时，不同帧的点云的坐标原点就会不同，这样就产生了运动畸变。
![
运动畸变的来源
- 平移运动导致的运动畸变
- 旋转运动导致的运动畸变
运动补偿的方法
对于每个激光坐标点做运动补偿，补偿量应为激光点相对于该帧激光雷达数据起始时刻的位姿变化。假设在一帧点云中，起始时刻雷达位姿为
T
0
=
[
R
0
t
0
0
1
]
T_ 0=\begin{bmatrix} R_0 &t_0 \\ 0 & 1 \end{bmatrix}
T0=[R00t01]
该帧点云的第i个激光点采集时激光雷达的位姿为
T
i
=
[
R
i
t
i
0
1
]
T_i = \begin{bmatrix} R_i & t_i\\0 &1 \end{bmatrix}
Ti=[Ri0ti1]
假设当前获得的第i个激光点的坐标为
P
i
=
[
p
i
x
p
i
y
p
i
z
]
P_i = [p_{ix} \ p_{iy} \ p_{iz}]
Pi=[pix piy piz]，则第i个激光点补偿畸变后的坐标应为
P
i
′
=
T
0
−
1
T
i
P
i
P_i' = T_0^{-1}T_iP_i
Pi′=T0−1TiPi
即只需要计算0到i时刻，激光雷达的相对旋转和相对平移变化即可。实际上，激光雷达点云是局部坐标系下的表示，当以0时刻雷
达的位姿为基准坐标系时，此时初始激光雷达位姿
T
0
T_0
T0即为单位阵，
T
i
T_i
Ti即为0到i时刻的相对旋转和相对平移。此时有
R
i
=
ω
d
t
i
m
e
s
t
i
=
V
d
t
i
m
e
s
R_i = \omega d_{times} \\ t_i = Vd_{times}
Ri=ωdtimesti=Vdtimes
即只需要知道0到i时刻的平均角速度和平均速度即可。
前端里程计
激光SLAM的前端里程计根据方法的不同可以分为基于直接匹配的前端里程计如迭代最近点法、基于特征的视觉里程计如LOAM等，下面主要介绍基于直接匹配和基于特征的前端里程计的原理和算法流程。
基于直接匹配—— 迭代最近点（Iterative Closest Point, 下简称ICP）算法
ICP算法流程
ICP算法是基于EM(Expectation-maximization algorithm)思想的方法，采用交替迭代法优化得到最优值。即ICP分为两步，第一步进行点云匹配，第二步进行运动估计。其中，点云配准是指令两帧点云数据在同一个坐标系下，一帧数据中的点找到另一帧数据最近的点，这对点就作为一对匹配点。运动估计就是根据得到得两帧点云的匹配情况，构建最小二乘方程并求解两帧之间的位姿变换。
假设两帧点云
X
=
{
x
1
,
x
2
,
.
.
.
,
x
N
x
}
,
x
∈
R
3
X = \{x_1,x_2,...,x_{N_x}\},x\in R^3
X={x1,x2,...,xNx},x∈R3和
Y
=
{
y
1
,
y
2
,
.
.
.
,
y
N
y
}
,
y
∈
R
3
Y = \{y_1,y_2,...,y_{N_y}\},y\in R^3
Y={y1,y2,...,yNy},y∈R3已经找到点云之间的匹配关系，并且两帧的点云数目相等即
N
x
=
N
y
N_x=N_y
Nx=Ny，那么运动估计的目标就是找到旋转矩阵
R
R
R与位移
t
t
t，使得两帧点云之间的残差最小，即
min
E
(
R
,
t
)
=
min
1
N
y
∑
i
=
1
N
y
∣
∣
x
i
−
R
y
i
−
t
∣
∣
2
\min \ E(R,t) = \min \frac{1}{N_y}\sum \limits_{i=1}^{N_y}||x_i-Ry_i-t||^2
min E(R,t)=minNy1i=1∑Ny∣∣xi−Ryi−t∣∣2
其中R是
3
×
3
3\times3
3×3的矩阵，t是
3
×
1
3\times1
3×1的向量，
N
x
,
N
y
N_x,N_y
Nx,Ny表示点云中点的个数。
ICP算法流程图如图所示。首先点云预处理获得的是规范化点云的格式比如去除Nan值点和确定点云数量等，然后进入ICP算法的处理。ICP算法中先进行点云匹配，如做两帧点云的最近邻关联点的选取等。其次进行运动估计，即求解位姿变换使得两帧点云匹配的残差最小。最后进行条件判断，即残差或迭代步长足够小时停止循环并输出位姿R和t。
ICP算法流程如下：
- 点云匹配：
- 对于每一个在 X X X内的点 x i x_i xi，找到 Y Y Y内与之相关联的最邻近点。
- 去除超出边界的点（outliers）如 ∣ ∣ x i − y i ∣ ∣ ||x_i-y_i|| ∣∣xi−yi∣∣过大
- 运动估计： R , t = arg R , t min E ( R , t ) = arg R , t min 1 N y ∑ i = 1 N y ∣ ∣ x i − R y i − t ∣ ∣ 2 R,t = \arg_{R,t} \min E(R,t) = \arg_{R,t} \min \frac{1}{N_y}\sum \limits_{i=1}^{N_y}||x_i-Ry_i-t||^2 R,t=argR,tminE(R,t)=argR,tminNy1i=1∑Ny∣∣xi−Ryi−t∣∣2
- 检查是否收敛：
- 收敛标准
- 残差 E ( R , t ) E(R,t) E(R,t)足够小
- Δ R , Δ t \Delta R,\Delta t ΔR,Δt足够小
- 如果不收敛
- Y < − R Y + t Y <- RY+t Y<−RY+t
- 重复步骤1-3
- 收敛标准
运动估计中位姿变换R和t的求解办法
-
基于奇异值分解（Singular Value Decomposition，以下简称SVD）的ICP算法
令 H = ∑ i = 1 N y y i ′ x i ′ T H = \sum \limits_{i=1}^{N_y}y_i'x_i'^T H=i=1∑Nyyi′xi′T，其中 x i ′ = x i − u x , y i ′ = y i − u y x_i' = x_i-u_x, y_i' = y_i-u_y xi′=xi−ux,yi′=yi−uy 且 u x = 1 N x ∑ i = 1 N x x i , u y = 1 N y ∑ i = 1 N y y i u_x = \frac{1}{N_x}\sum \limits_{i=1}^{N_x}x_i,u_y = \frac{1}{N_y}\sum \limits_{i=1}^{N_y}y_i ux=Nx1i=1∑Nxxi,uy=Ny1i=1∑Nyyi，对H做SVD分解有
H = U Σ V T H = U\Sigma V^T H=UΣVT
则根据推导，旋转矩阵 R = V U T R = VU^T R=VUT，平移向量 t = u x − R u y t = u_x - Ru_y t=ux−Ruy。推导过程这里不做详细阐述，读者可参考文献 [ ? ]。（Arun, K. Somani, Thomas S. Huang, and Steven D. Blostein. “Least-squares fitting of two 3-D point
sets.” IEEE Transactions on pattern analysis and machine intelligence 5 (1987): 698-700. ） -
基于凸优化的ICP算法
-
优化任务的目标：
找到变量 x ∗ ∈ R n x^* \in R^n x∗∈Rn，使得损失函数 F ( x ) F(x) F(x)取得局部最小值，即
min x F ( x ) = 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 2 \min \limits_x F(x) = \frac{1}{2}||f(x)||_2^2 xminF(x)=21∣∣f(x)∣∣22
其中局部最小值指对任意的 ∣ ∣ x − x ∗ ∣ ∣ < δ ||x - x^*||<\delta ∣∣x−x∗∣∣<δ都有 F ( x ∗ ) ≤ F ( x ) F(x^*) \leq F(x) F(x∗)≤F(x)， f ( x ) f(x) f(x)代表残差函数。 -
迭代方法流程
（1）给定初值 x 0 x_0 x0
（2）对第k次迭代，计算增量 Δ x k \Delta x_k Δxk，使得 ∣ ∣ f ( x k + Δ x k ) ∣ ∣ 2 2 ||f(x_k+\Delta x_k)||^2_2 ∣∣f(xk+Δxk)∣∣22达到极小值
（3）当 Δ x \Delta x Δx足够小时，则停止
（4）否则，令 x k + 1 = x k + Δ x x_{k+1} = x_k +\Delta x xk+1=xk+Δx，返回第二步
-
数值求解优化问题的方法
损失函数的泰勒展开可表示为
F ( x + Δ x ) ≈ F ( X ) + J Δ x + 1 2 Δ x T H Δ x F(x+\Delta x) \approx F(X) + J\Delta x+\frac{1}{2}\Delta x^TH\Delta x F(x+Δx)≈F(X)+JΔx+21ΔxTHΔx
其中 J J J和 H H H分别为损失函数 F ( x ) F(x) F(x)对变量 x x x的一阶导数（雅克比矩阵）和二阶导数（黑森矩阵）。（1）最速下降法
最速下降法只保留一阶泰勒展开式，取增量为 Δ x = − J T \Delta x = -J^T Δx=−JT，即沿着梯度的反方向取增量。
（2）牛顿法
牛顿法保留损失函数的二阶泰勒展开式，此时的增量方程为
Δ x ∗ = arg min ( F ( x ) + J Δ x + 1 2 Δ x T H Δ x ) \Delta x^* = \arg \min (F(x) + J\Delta x+\frac{1}{2}\Delta x^TH\Delta x) Δx∗=argmin(F(x)+JΔx+21ΔxTHΔx)
求右侧关于 Δ x \Delta x Δx的导数并令其为零即得到
J T + H Δ x = 0 ⇒ H Δ x = − J T J^T + H \Delta x = 0 \Rightarrow H\Delta x = -J^T JT+HΔx=0⇒HΔx=−JT
求解上式即可得到增量 Δ x \Delta x Δx（3）高斯牛顿法
对残差函数 f ( x ) f(x) f(x)而不是损失函数 F ( x ) F(x) F(x)进行泰勒展开，即
f ( x + Δ x ) ≈ f ( x ) + J Δ x f(x+\Delta x) \approx f(x)+J\Delta x f(x+Δx)≈f(x)+JΔx
此时问题为寻找增量 Δ x \Delta x Δx使得 ∣ ∣ f ( x + Δ x ) ∣ ∣ 2 ||f(x+\Delta x)||^2 ∣∣f(x+Δx)∣∣2最小，即
Δ x ∗ = arg min 1 2 ∣ ∣ f ( x ) + J Δ x ∣ ∣ 2 \Delta x^* = \arg \min \frac{1}{2}||f(x)+J\Delta x||^2 Δx∗=argmin21∣∣f(x)+JΔx∣∣2
对右侧求导并令其为零有
J T f ( x ) + J T J Δ x = 0 J^Tf(x) +J^TJ\Delta x = 0 JTf(x)+JTJΔx=0
令 H = J T J ， g = − J T f ( x ) H = J^TJ，g = -J^Tf(x) H=JTJ，g=−JTf(x)，则有增量 Δ x = H − 1 g \Delta x = H^{-1}g Δx=H−1g（4）列文伯格-马夸尔特（Levenberg-Marquardt，以下简称LM）方法
LM的原理推导较为复杂，读者可自行查看最优化问题相关参考，这里直接给出增量方程
( H + λ I ) Δ x = g (H+\lambda I)\Delta x =g (H+λI)Δx=g -
ICP问题的迭代求解方法
ICP问题可重新表示为
T = arg T min 1 2 ∑ i = 1 n ∣ ∣ x i − T y i ∣ ∣ 2 2 T = \arg \limits_{T} \min \frac{1}{2}\sum \limits_{i=1} ^n||x_i-Ty_i||^2_2 T=Targmin21i=1∑n∣∣xi−Tyi∣∣22
其中 T T T为变换矩阵
T = [ R t 0 1 ] T = \begin{bmatrix} R& t \\ %第一行元素 0 & 1 \\ %第二行元素 \end{bmatrix} T=[R0t1]
在李代数下可表示为
T = arg T min 1 2 ∑ i = 1 n ∣ ∣ x i − e x p ( ζ ) ∣ ∣ 2 2 T = \arg \limits_{T} \min \frac{1}{2}\sum \limits_{i=1} ^n||x_i-exp(\zeta )||^2_2 T=Targmin21i=1∑n∣∣xi−exp(ζ)∣∣22
其中 ζ \zeta ζ为 T T T对应的李代数，则残差对应的雅可比矩阵为
J = ∂ e ∂ δ ζ = − ( e x p ( ζ ) y i ) o J=\frac{\partial e}{\partial \delta \zeta} = -(exp(\zeta)y_i)^o J=∂δζ∂e=−(exp(ζ)yi)o
然后即可通过上述优化方法进行求解。这里关于李代数不详细阐述，读者可自行查阅相关文献。
-
ICP方法的改进
在目前主流的ICP算法中根据不同的残差函数，ICP方法可具体分为点到点的ICP方法，点到线的PLICP方法，面到面的GICP方法等。关于求解方法除了上述提到的SVD分解和数值迭代优化还有四元数法等。为了增加算法的鲁棒性，近年又有学者提出NICP的方法。
ICP方法的评价
ICP方法最大的优点就是其简单易实现，同时在有较好的初始化条件时具有不错的性能，但是缺点也同样明显，两帧点云之间的最近邻搜索可能会比较耗时，而且如果初始位姿不太理想，后续也很难纠正过来。
基于直接匹配——正态分布变换（normal distribution transformation，以下简称NDT）方法
NDT算法流程
正态分布变换算法的核心思想是首先将空间离散为方格。如果是二维空间，则划分为栅格，如果是三维空间则划分为立方体。这样就可以将采样的点云划分到不同的网格中，因此可以很方便的描述点云的局部特性，例如点云局部的形状（直线、平面或者球体）、方向（平面法向、直线方向等）。然后构建多维变量的正态分布， 如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度最大的变换参数，此时两帧激光点云数据将匹配得最好。
NDT算法流程如下：
- 对目标点云进行空间网格划分
- 对每个网格计算均值和协方差
- 计算需要的常数
- 初始化位姿参数
- 通过初始位姿变换源点云
- 计算损失函数，雅克比矩阵，黑森矩阵
- 通过最优化方法更新位姿
- 是否收敛
- 收敛则输出当前位姿
- 不收敛则重复步骤2
经典NDT算法原理
假设有目标点云
X
=
{
x
1
,
x
2
,
.
.
.
,
x
N
x
}
,
x
∈
R
3
X = \{x_1,x_2,...,x_{N_x}\},x\in R^3
X={x1,x2,...,xNx},x∈R3和源点云
Y
=
{
y
1
,
y
2
,
.
.
.
,
y
N
y
}
,
y
∈
R
3
Y = \{y_1,y_2,...,y_{N_y}\},y\in R^3
Y={y1,y2,...,yNy},y∈R3，NDT算法的目标是
max
Ψ
=
m
a
x
∏
i
=
1
N
y
f
(
X
,
T
(
p
,
y
i
)
)
\max \Psi = max \prod_{i=1}^{N_y}f(X,T(p,y_i))
maxΨ=maxi=1∏Nyf(X,T(p,yi))
其中
f
(
X
,
T
(
p
,
y
i
)
)
f(X,T(p,y_i))
f(X,T(p,yi))是联合概率分布，对于2D模型，
p
=
[
t
x
t
y
ϕ
z
]
T
p=[t_x \ t_y \ \phi_z]^T
p=[tx ty ϕz]T，对于3D模型，
p
=
[
t
x
t
y
t
z
ϕ
x
ϕ
y
ϕ
z
]
T
p = [t_x \ t_y \ t_z \ \phi_x \ \phi_y \ \phi_z ]^T
p=[tx ty tz ϕx ϕy ϕz]T。
我们令均值为 μ = 1 N x ∑ i = 1 N x x i \mu = \frac{1}{N_x}\sum \limits_{i=1}^{N_x}x_i μ=Nx1i=1∑Nxxi，则协方差为 Σ = 1 N x − 1 ∑ i = 1 N x ( x i − μ ) ( x i − μ ) T \Sigma = \frac{1}{N_x-1}\sum \limits_{i=1}^{N_x}(x_i-\mu)(x_i-\mu)^T Σ=Nx−11i=1∑Nx(xi−μ)(xi−μ)T。
根据初始预测的位姿，对点云
Y
Y
Y中的点进行旋转和平移
y
i
∗
=
T
(
p
,
y
i
)
=
R
y
i
+
t
y_i^* = T(p,y_i)=Ry_i+t
yi∗=T(p,yi)=Ryi+t
旋转和平移后的点与目标点集在同一坐标系下，此时刻计算各点的联合概率密度
N
(
X
,
y
i
∗
)
=
1
(
2
π
)
d
e
t
(
∑
)
e
x
p
(
−
1
2
(
y
i
∗
−
μ
)
T
∑
−
1
(
y
i
∗
−
μ
)
)
N(X,y_i^*)=\sqrt{\frac{1}{(2\pi)det(\sum)}}exp(-\frac{1}{2}(y^*_i-\mu)^T\sum \ ^{-1}(y_i^*-\mu))
N(X,yi∗)=(2π)det(∑)1exp(−21(yi∗−μ)T∑ −1(yi∗−μ))
因此所有点的联合概率为
Ψ
=
∏
i
=
1
N
x
f
(
X
,
T
(
p
,
y
i
)
)
=
∏
i
=
1
N
x
1
(
2
π
)
d
e
t
(
∑
)
e
x
p
(
−
1
2
(
y
i
∗
−
μ
)
T
∑
−
1
(
y
i
∗
−
μ
)
)
\Psi = \prod_{i=1}^{N_x} f(X,T(p,y_i)) = \prod_{i=1}^{N_x}\sqrt{\frac{1}{(2\pi)det(\sum)}}exp(-\frac{1}{2}(y^*_i-\mu)^T\sum \ ^{-1}(y_i^*-\mu))
Ψ=i=1∏Nxf(X,T(p,yi))=i=1∏Nx(2π)det(∑)1exp(−21(yi∗−μ)T∑ −1(yi∗−μ))
取对数有
l
n
Ψ
=
∑
i
=
1
N
x
(
−
1
2
(
y
i
∗
−
μ
)
T
∑
−
1
(
y
i
∗
−
μ
)
+
l
n
(
1
2
π
∣
Σ
∣
)
)
ln\Psi = \sum \limits_{i=1}^{N_x}(-\frac{1}{2}(y^*_i-\mu)^T\sum \ ^{-1}(y_i^*-\mu)+ln(\frac{1}{\sqrt{2\pi|\Sigma|}}))
lnΨ=i=1∑Nx(−21(yi∗−μ)T∑ −1(yi∗−μ)+ln(2π∣Σ∣1))
可以发现上式中对数项为一常数，设$\Psi_1 =(y’_i-\mu)^T\sum \ ^{-1}(y_i’-\mu) $因此我们的优化目标可以简化为
max
Ψ
=
max
l
n
Ψ
=
min
Ψ
1
=
min
(
y
i
∗
−
μ
)
T
∑
−
1
(
y
i
∗
−
μ
)
\max \Psi = \max ln\Psi = \min \Psi_1=\min (y^*_i-\mu)^T\sum \ ^{-1}(y_i^*-\mu)
maxΨ=maxlnΨ=minΨ1=min(yi∗−μ)T∑ −1(yi∗−μ)
则上述问题可以转换为凸优化问题，即目标函数为
min
(
y
i
∗
−
μ
)
T
∑
−
1
(
y
i
∗
−
μ
)
y
i
∗
=
T
(
p
,
y
i
)
=
R
y
i
+
t
\min (y^*_i-\mu)^T\sum \ ^{-1}(y_i^*-\mu) \\ y_i^* = T(p,y_i) = Ry_i+t
min(yi∗−μ)T∑ −1(yi∗−μ)yi∗=T(p,yi)=Ryi+t
其中待求参数为旋转矩阵
R
R
R与平移向量
t
t
t。
因此我们可以定义残差函数
f
i
(
p
)
=
y
i
∗
−
μ
f_i(p) = y_i^* -\mu
fi(p)=yi∗−μ
可得到其雅可比矩阵为
J
i
=
d
f
i
(
p
)
d
p
J_i = \frac{df_i(p)}{dp}
Ji=dpdfi(p)
如果是2D场景即
p
=
[
t
x
t
y
ϕ
z
]
T
p=[t_x \ t_y \ \phi_z]^T
p=[tx ty ϕz]T，则
y
i
∗
=
T
(
p
,
y
i
)
=
[
c
o
s
ϕ
z
−
s
i
n
ϕ
z
s
i
n
ϕ
z
c
o
s
ϕ
z
]
y
i
+
[
t
x
t
y
]
y_i^* = T(p,y_i) = \begin{bmatrix}cos\phi_z &-sin\phi_z \\sin \phi_z&cos\phi_z \end{bmatrix}y_i+ \begin{bmatrix}t_x \\t_y \end{bmatrix}
yi∗=T(p,yi)=[cosϕzsinϕz−sinϕzcosϕz]yi+[txty]
因此可计算雅可比矩阵为
J
i
=
[
1
0
−
y
i
1
s
i
n
ϕ
z
−
y
i
2
c
o
s
ϕ
z
0
1
y
i
1
c
o
s
ϕ
z
−
y
i
2
s
i
n
ϕ
z
]
J_i = \begin{bmatrix} 1 & 0& -y_{i1}sin\phi_z-y_{i2}cos\phi_z\\ 0 & 1 & y_{i1}cos\phi_z-y_{i2}sin\phi_z \end{bmatrix}
Ji=[1001−yi1sinϕz−yi2cosϕzyi1cosϕz−yi2sinϕz]
3D场景可类似求解。
基于特征——LOAM
向量运算基础知识
-
内积，又叫数量级，是向量的点乘
a = ( x 1 , y 1 , z 1 ) a = (x_1,y_1,z_1) a=(x1,y1,z1)， b = ( x 2 , y 2 , z 2 ) b= (x_2,y_2,z_2) b=(x2,y2,z2)
a ⋅ b = x 1 x 2 + y 1 y 2 + z 1 z 2 = ∣ a ∣ ∣ b ∣ c o s θ a \cdot b = x_1x_2+y_1y_2+z_1z_2=|a||b|cos\theta a⋅b=x1x2+y1y2+z1z2=∣a∣∣b∣cosθ
内积的几何意义：当b是单位向量时，内积就是a在b上的投影。
内积的微分性质：从内积的定义出发，有
∂
a
⋅
b
∂
a
=
b
\frac{\partial a \cdot b}{\partial a } = b
∂a∂a⋅b=b
证明：
∂
a
⋅
b
∂
x
1
=
∂
(
x
1
x
2
+
y
1
y
2
+
z
1
z
2
)
∂
x
1
=
x
2
∂
a
⋅
b
∂
y
1
=
∂
(
x
1
x
2
+
y
1
y
2
+
z
1
z
2
)
∂
x
1
=
y
2
∂
a
⋅
b
∂
z
1
=
∂
(
x
1
x
2
+
y
1
y
2
+
z
1
z
2
)
∂
x
1
=
z
2
\frac{\partial a \cdot b}{\partial x_1} =\frac{\partial(x_1x_2+y_1y_2+z_1z_2)}{\partial x_1}=x_2 \\ \frac{\partial a \cdot b}{\partial y_1} =\frac{\partial(x_1x_2+y_1y_2+z_1z_2)}{\partial x_1}=y_2 \\ \frac{\partial a \cdot b}{\partial z_1} =\frac{\partial(x_1x_2+y_1y_2+z_1z_2)}{\partial x_1}=z_2 \\
∂x1∂a⋅b=∂x1∂(x1x2+y1y2+z1z2)=x2∂y1∂a⋅b=∂x1∂(x1x2+y1y2+z1z2)=y2∂z1∂a⋅b=∂x1∂(x1x2+y1y2+z1z2)=z2
-
外积，又叫叉积、向量积，是向量的叉乘运算
a = ( x 1 , y 1 , z 1 ) a = (x_1,y_1,z_1) a=(x1,y1,z1)， b = ( x 2 , y 2 , z 2 ) b= (x_2,y_2,z_2) b=(x2,y2,z2)
a × b = ∣ i j k x 1 y 1 z 1 x 2 y 2 z 2 ∣ = ( y 1 z 2 − y 2 z 1 ) i − ( x 1 z 2 − x 2 z 1 ) j + ( x 1 y 2 − x 2 y 1 ) k a\times b = \begin{vmatrix} i &j&k \\ x_1&y_1&z_1 \\ x_2 &y_2 &z_2 \end{vmatrix} = (y_1z_2-y_2z_1)i-(x_1z_2-x_2z_1)j+(x_1y_2-x_2y_1)k a×b= ix1x2jy1y2kz1z2 =(y1z2−y2z1)i−(x1z2−x2z1)j+(x1y2−x2y1)k
∣ a × b ∣ = ∣ a ∣ ∣ b ∣ s i n θ |a\times b |=|a||b|sin\theta ∣a×b∣=∣a∣∣b∣sinθ
外积的几何意义：外积的模长等于a和b组成的平面四边形的面积，外积的方向满足右手定则。a和b张成的平面的单位法向量为
n = a × b ∣ a × b ∣ n = \frac{a\times b}{|a\times b|} n=∣a×b∣a×b
外积的微分性质：根据外积的定义有
a × b = a ∧ b a ∧ = [ 0 − z 1 y 1 z 1 0 − x 1 − y 1 x 1 0 ] a ∧ b = − b ∧ a ∂ a ∧ b ∂ a = − b ∧ a ∂ a = − b ∧ a\times b = a ^\wedge b\\ a^\wedge = \begin{bmatrix} 0 & -z_1 & y_1 \\ z_1 & 0 & -x_1 \\ -y_1& x_1 &0 \end{bmatrix}\\ a ^\wedge b = -b^\wedge a \\ \frac{\partial a^\wedge b}{\partial a} = -\frac{b^\wedge a}{\partial a}= -b^\wedge a×b=a∧ba∧= 0z1−y1−z10x1y1−x10 a∧b=−b∧a∂a∂a∧b=−∂ab∧a=−b∧
其中 a ∧ a^\wedge a∧是 a a a的反对称矩阵。
线面特征运算
- 点到直线距离
点A到直线BC的距离为
∣
A
D
⃗
∣
=
∣
A
B
⃗
×
A
C
⃗
∣
∣
B
C
⃗
∣
|\vec{AD}| = \frac{|\vec{AB}\times{\vec{AC}}|}{|\vec{BC}|}
∣AD∣=∣BC∣∣AB×AC∣
即平行四边形面积除以对角线长度。
- 点到平面距离
平面BCD的单位法向量为
n
=
B
C
⃗
×
B
D
⃗
∣
B
C
∣
⃗
×
∣
B
D
∣
⃗
n = \frac{\vec{BC}\times \vec{BD}}{\vec{|BC|}\times \vec{|BD|}}
n=∣BC∣×∣BD∣BC×BD
则点A到平面BCD的距离为
∣
A
E
⃗
∣
=
∣
A
B
⃗
∣
c
o
s
θ
=
A
B
⃗
⋅
n
|\vec{AE}| = |\vec{AB}|cos\theta = \vec{AB}\cdot n
∣AE∣=∣AB∣cosθ=AB⋅n
点云线面特征提取
-
按线数分割：根据激光点的坐标值，可以计算该束激光相比于雷达水平角的俯仰角 ω = a r c t a n z x 2 + y 2 \omega = arctan\frac{z}{\sqrt{x^2+y^2}} ω=arctanx2+y2z，再根据俯仰角和激光雷达的内参（指各扫描线的设计倾角）可知当前坐标点属于激光雷达发出的哪根光束线。
-
计算曲率：根据前后各5个点与当前点的距离值（指点到激光雷达的距离）计算曲率大小。
c = 1 ∣ ∣ X ∣ ∣ ∣ ∣ ∑ i ( X − X i ) ∣ ∣ c = \frac{1}{||X||}||\sum_i (X-X_i)|| c=∣∣X∣∣1∣∣i∑(X−Xi)∣∣ -
按曲率大小筛选特征点：曲率特别大的点——边缘点，曲率特别小的点——平面点。
基于线面特征的位姿优化
-
帧间关联
-
点云坐标系转换
第k+1帧与第k帧的相对位姿，即第k帧到第k+1帧的位姿变换矩阵为
T = [ R t 0 1 ] T = \begin{bmatrix} R&t \\ 0&1\end{bmatrix} T=[R0t1]
因此，第k+1帧中的点 p i p_i pi转换到第k帧坐标系有
p i ~ = R p i + t \tilde{p_i} = Rp_i+t pi~=Rpi+t -
线特征关联
当第k+1帧点云中的点 p i p_i pi为边缘点时，在上一帧即第k帧中搜索离 p i ~ \tilde{p_i} pi~最近的线特征点，并在相邻扫描线上再找一个特征点，组成直线。
-
面特征关联
当第k+1帧点云中的点 p i p_i pi为平面点时，在上一帧即第k帧中搜索离 p i ~ \tilde{p_i} pi~最近的面特征点，并在相邻扫描线上找两个面特征点，组成平面。
-
-
残差函数
-
线特征的残差函数
点到直线的距离（矢量形式）
d ϵ = ( p ~ i − p b ) × ( p ~ i − p a ) ∣ p a − p b ∣ d_\epsilon = \frac{(\tilde p_i-p_b)\times (\tilde p_i-p_a)}{|p_a-p_b|} dϵ=∣pa−pb∣(p~i−pb)×(p~i−pa) -
面特征的残差函数
点到平面的距离
d H = ∣ ( p i ~ − p j ) ⋅ ( p l − p j ) × ( p m − p j ) ∣ ( p l − p j ) × ( p m − p j ) ∣ ∣ d_H =| (\tilde{p_i}-p_j)\cdot \frac{(p_l-p_j)\times (p_m-p_j)}{|(p_l-p_j)\times (p_m-p_j)|}| dH=∣(pi~−pj)⋅∣(pl−pj)×(pm−pj)∣(pl−pj)×(pm−pj)∣
-
-
位姿优化
根据凸优化理论，只要求得残差关于待求变量（这里是变换矩阵T，包括旋转矩阵R和平移向量t）的雅克比矩阵，就可以采用例如高斯牛顿法的优化方法进行优化。
-
线特征残差函数的雅克比
J ϵ = ∂ d ϵ ∂ T = ∂ d ϵ ∂ p ~ i ∂ p ~ i ∂ T J_\epsilon = \frac{\partial d_\epsilon}{\partial T} =\frac{\partial d_\epsilon}{\partial \tilde p_i}\frac{\partial \tilde p_i}{\partial T} Jϵ=∂T∂dϵ=∂p~i∂dϵ∂T∂p~i
对于等号右边第一项，根据外积的微分性质，推导得到
∂ d ϵ ∂ p ~ i = 1 ∣ p a − p b ∣ ( ∂ ( p ~ i − p b ) ∧ ( p ~ i − p a ) ∂ p ~ i + ( p ~ i − p b ) ∧ ∂ ( p ~ i − p a ) ∂ p ~ i ) = 1 ∣ p a − p b ∣ ( − ( p ~ i − p a ) ∧ + ( p ~ i − p b ) ∧ ) = ( p a − p b ) ∧ ∣ p a − p b ∣ \frac{\partial d_\epsilon}{\partial \tilde p_i} = \frac{1}{|p_a-p_b|}(\frac{\partial (\tilde p_i-p_b)^\wedge(\tilde p_i-p_a)}{\partial \tilde p_i}+\frac{(\tilde p_i-p_b)^\wedge \partial(\tilde p_i-p_a)}{\partial \tilde p_i})\\ = \frac{1}{|p_a-p_b|}(-(\tilde p_i-p_a)^\wedge+(\tilde p_i-p_b)^\wedge)\\ =\frac{( p_a-p_b)^\wedge}{|p_a-p_b|} ∂p~i∂dϵ=∣pa−pb∣1(∂p~i∂(p~i−pb)∧(p~i−pa)+∂p~i(p~i−pb)∧∂(p~i−pa))=∣pa−pb∣1(−(p~i−pa)∧+(p~i−pb)∧)=∣pa−pb∣(pa−pb)∧
对于等号右边第二项与李代数相关，此处直接给出结论对平移向量的雅可比矩阵为
∂ p ~ i ∂ t = I \frac{\partial \tilde p_i}{\partial t} = I ∂t∂p~i=I
对旋转矩阵的雅可比矩阵为
∂ p ~ i ∂ t = − ( R p i + t ) ∧ \frac{\partial \tilde p_i}{\partial t} = -(Rp_i+t)^\wedge ∂t∂p~i=−(Rpi+t)∧ -
面特征残差函数的雅可比
J H = ∂ d H ∂ T = ∂ d H ∂ p ~ i ∂ p ~ i ∂ T J_H = \frac{\partial d_H}{\partial T} =\frac{\partial d_H}{\partial \tilde p_i}\frac{\partial \tilde p_i}{\partial T} JH=∂T∂dH=∂p~i∂dH∂T∂p~i
对于上式右边第二项与线特征部分一致对平移向量的雅可比矩阵为
∂ p ~ i ∂ t = I \frac{\partial \tilde p_i}{\partial t} = I ∂t∂p~i=I
对旋转矩阵的雅可比矩阵为
∂ p ~ i ∂ t = − ( R p i + t ) ∧ \frac{\partial \tilde p_i}{\partial t} = -(Rp_i+t)^\wedge ∂t∂p~i=−(Rpi+t)∧
对于等号右边第一项，令
X = ( p i ~ − p j ) ⋅ ( p l − p j ) × ( p m − p j ) ∣ ( p l − p j ) × ( p m − p j ) ∣ X= (\tilde{p_i}-p_j)\cdot \frac{(p_l-p_j)\times (p_m-p_j)}{|(p_l-p_j)\times (p_m-p_j)|} X=(pi~−pj)⋅∣(pl−pj)×(pm−pj)∣(pl−pj)×(pm−pj)
则有
∂ d H ∂ p ~ i = ∂ ∣ X ∣ ∂ p ~ i = ∂ ∣ X ∣ ∂ X ∂ X ∂ p ~ i = X ∣ X ∣ ∂ X ∂ p ~ i \frac{\partial d_H}{\partial \tilde p_i} = \frac{\partial |X|}{\partial \tilde p_i} = \frac{\partial |X|}{\partial X} \frac{\partial X}{\partial \tilde p_i} = \frac{X}{|X|}\frac{\partial X}{\partial \tilde p_i} ∂p~i∂dH=∂p~i∂∣X∣=∂X∂∣X∣∂p~i∂X=∣X∣X∂p~i∂X
对于上式的等号右边第二项，根据内积的微分性质有
∂ X ∂ p ~ i = ( p l − p j ) × ( p m − p j ) ∣ ( p l − p j ) × ( p m − p j ) ∣ \frac{\partial X}{\partial \tilde p_i} = \frac{(p_l-p_j)\times (p_m-p_j)}{|(p_l-p_j)\times (p_m-p_j)|} ∂p~i∂X=∣(pl−pj)×(pm−pj)∣(pl−pj)×(pm−pj)
物理意义上它代表的是平面的法向量。
-
627
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
