假设咱们有两组点集，注意这里的
P
\mathbf{P}
P
Q
\mathbf{Q}
Q
R
\mathbf{R}
R
T
\mathbf{T}
T
P
\mathbf{P}
P
Q
\mathbf{Q}
Q
1
2
∑
i
=
1
n
∥
q
i
−
R
p
i
−
T
∥
2
(1.1)
\frac{1}{2}\sum^n_{i=1}\|\mathbf{q}_i-\mathbf{R}\mathbf{p}_i-\mathbf{T}\|^2 \tag{1.1}
2 1 i = 1 ∑ n ∥ q i − R p i − T ∥ 2 ( 1 . 1 ) html
咱们定义两组点集的中心为
μ
p
=
∑
i
=
1
n
p
i
(1.2)
\mu_p = \sum^n_{i=1}\mathbf{p}_i \tag{1.2}
μ p = i = 1 ∑ n p i ( 1 . 2 )
μ
q
=
∑
i
=
1
n
q
i
(1.3)
\mu_q = \sum^n_{i=1}\mathbf{q}_i \tag{1.3}
μ q = i = 1 ∑ n q i ( 1 . 3 )
∥
q
i
−
R
p
i
−
T
∥
2
=
∥
q
i
−
R
p
i
−
T
−
(
μ
q
−
R
μ
p
)
+
(
μ
q
−
R
μ
p
)
∥
2
=
∥
q
i
−
μ
q
−
R
(
p
i
−
μ
p
)
+
(
μ
q
−
R
μ
p
−
T
)
∥
2
=
∥
q
i
−
μ
q
−
R
(
p
i
−
μ
p
)
∥
2
+
∥
μ
q
−
R
μ
p
−
T
∥
2
+
2
(
q
i
−
μ
q
−
R
(
p
i
−
μ
p
)
)
T
(
μ
q
−
R
μ
p
−
T
)
\|\mathbf{q}_i-\mathbf{R}\mathbf{p}_i-\mathbf{T}\|^2 = \|\mathbf{q}_i-\mathbf{R}\mathbf{p}_i-\mathbf{T} - (\mu_q - \mathbf{R}{\mu}_p) + (\mu_q - \mathbf{R}{\mu}_p)\|^2 \\= \| \mathbf{q}_i - \mu_q -\mathbf{R}( \mathbf{p}_i - \mu_p) +(\mu_q - \mathbf{R}\mu_p -\mathbf{T})\|^2 \\ =\| \mathbf{q}_i - \mu_q -\mathbf{R}( \mathbf{p}_i - \mu_p)\|^2 + \|\mu_q - \mathbf{R}\mu_p -\mathbf{T} \|^2 + 2 (\mathbf{q}_i - \mu_q -\mathbf{R}( \mathbf{p}_i - \mu_p))^T(\mu_q - \mathbf{R}\mu_p -\mathbf{T})
∥ q i − R p i − T ∥ 2 = ∥ q i − R p i − T − ( μ q − R μ p ) + ( μ q − R μ p ) ∥ 2 = ∥ q i − μ q − R ( p i − μ p ) + ( μ q − R μ p − T ) ∥ 2 = ∥ q i − μ q − R ( p i − μ p ) ∥ 2 + ∥ μ q − R μ p − T ∥ 2 + 2 ( q i − μ q − R ( p i − μ p ) ) T ( μ q − R μ p − T )
∑
i
=
1
n
(
q
i
−
μ
q
−
R
(
p
i
−
μ
p
)
)
T
(
μ
q
−
R
μ
p
−
T
)
=
(
μ
q
−
R
μ
p
−
T
)
T
∑
i
=
1
n
(
q
i
−
μ
q
−
R
(
p
i
−
μ
p
)
)
=
(
μ
q
−
R
μ
p
−
T
)
T
0
=
0
\sum^n_{i=1}(\mathbf{q}_i - \mu_q -\mathbf{R}( \mathbf{p}_i - \mu_p))^T(\mu_q - \mathbf{R}\mu_p -\mathbf{T})\\=(\mu_q - \mathbf{R}\mu_p -\mathbf{T})^T \sum^n_{i=1}(\mathbf{q}_i - \mu_q -\mathbf{R}( \mathbf{p}_i - \mu_p)) \\=(\mu_q - \mathbf{R}\mu_p -\mathbf{T})^T\mathbf{0}=\mathbf{0}
i = 1 ∑ n ( q i − μ q − R ( p i − μ p ) ) T ( μ q − R μ p − T ) = ( μ q − R μ p − T ) T i = 1 ∑ n ( q i − μ q − R ( p i − μ p ) ) = ( μ q − R μ p − T ) T 0 = 0
1
2
(
∑
i
=
1
n
∥
q
i
′
−
R
p
i
′
∥
2
+
∥
μ
q
−
R
μ
p
−
T
∥
2
)
(1.4)
\frac{1}{2}(\sum^n_{i=1}\|\mathbf{q}_i'-\mathbf{R}\mathbf{p}_i' \|^2 + \| \mu_q - \mathbf{R}\mu_p -\mathbf{T} \| ^2 )\tag{1.4}
2 1 ( i = 1 ∑ n ∥ q i ′ − R p i ′ ∥ 2 + ∥ μ q − R μ p − T ∥ 2 ) ( 1 . 4 )
q
i
′
=
q
i
−
μ
q
\mathbf{q}_i'=\mathbf{q}_i - \mu_q
q i ′ = q i − μ q
p
i
′
=
p
i
−
μ
p
\mathbf{p}_i'=\mathbf{p}_i - \mu_p
p i ′ = p i − μ p
∥
q
i
′
−
R
p
i
′
∥
2
=
(
q
i
′
−
R
p
i
′
)
T
(
q
i
′
−
R
p
i
′
)
=
(
q
i
′
T
q
i
′
+
p
i
′
T
R
T
R
p
i
′
−
2
q
i
′
T
R
p
i
′
)
=
(
q
i
′
T
q
i
′
+
p
i
′
T
p
i
′
−
2
q
i
′
T
R
p
i
′
)
(1.5)
\|\mathbf{q}_i'-\mathbf{R}\mathbf{p}_i' \|^2 = (\mathbf{q}_i'-\mathbf{R}\mathbf{p}_i')^T(\mathbf{q}_i'-\mathbf{R}\mathbf{p}_i') \\=(\mathbf{q}_i'^T\mathbf{q}_i' + \mathbf{p}_i'^T \mathbf{R}^T \mathbf{R} \mathbf{p}_i' - 2 \mathbf{q}_i'^T \mathbf{R} \mathbf{p}_i') \\= (\mathbf{q}_i'^T\mathbf{q}_i' + \mathbf{p}_i'^T \mathbf{p}_i' - 2 \mathbf{q}_i'^T \mathbf{R} \mathbf{p}_i') \tag{1.5}
∥ q i ′ − R p i ′ ∥ 2 = ( q i ′ − R p i ′ ) T ( q i ′ − R p i ′ ) = ( q i ′ T q i ′ + p i ′ T R T R p i ′ − 2 q i ′ T R p i ′ ) = ( q i ′ T q i ′ + p i ′ T p i ′ − 2 q i ′ T R p i ′ ) ( 1 . 5 )
∑
i
=
1
n
q
i
′
T
R
p
i
′
\sum^n_{i=1}\mathbf{q}_i'^T \mathbf{R} \mathbf{p}_i'
∑ i = 1 n q i ′ T R p i ′ web
在上一步的基础上，有
∑
i
=
1
n
q
i
′
T
R
p
i
′
=
T
r
(
∑
i
=
1
n
R
p
i
′
T
q
i
′
)
≜
T
r
(
R
H
)
(1.6)
\sum^n_{i=1}\mathbf{q}_i'^T \mathbf{R} \mathbf{p}_i'=Tr(\sum^n_{i=1}\mathbf{R} \mathbf{p}_i'^T \mathbf{q}_i')\triangleq Tr(\mathbf{RH}) \tag{1.6}
i = 1 ∑ n q i ′ T R p i ′ = T r ( i = 1 ∑ n R p i ′ T q i ′ ) ≜ T r ( R H ) ( 1 . 6 ) 引理1 ：对任意正定对称阵
A
A
T
AA^T
A A T
B
B
B
T
r
(
A
A
T
)
≥
T
r
(
B
A
A
T
)
(1.7)
Tr(AA^T)\geq Tr(BAA^T) \tag{1.7}
T r ( A A T ) ≥ T r ( B A A T ) ( 1 . 7 )
R
R
R
T
r
(
R
H
)
Tr(RH)
T r ( R H )
R
H
RH
R H
R
R
R
H
H
H
H
=
U
Λ
V
T
H=U\Lambda V^T
H = U Λ V T
X
=
V
U
T
X=VU^T
X = V U T
R
R
R
X
H
=
V
U
T
U
Λ
V
T
=
V
Λ
V
T
(1.8)
XH=VU^TU\Lambda V^T=V\Lambda V^T \tag{1.8}
X H = V U T U Λ V T = V Λ V T ( 1 . 8 )
B
B
B
T
r
(
X
H
)
≥
T
r
(
B
X
H
)
(1.9)
Tr(XH)\geq Tr(BXH) \tag{1.9}
T r ( X H ) ≥ T r ( B X H ) ( 1 . 9 )
X
X
X
R
R
R 算法
实际上刚才咱们只完成了一次计算，而ICP的全称是Iterative Closest Point，即迭代最近点。咱们来理解一下整个过程app
对P中的每一个点，在Q中找到匹配的最近点。这里须要注意，并非每次的点云都是一一匹配，点云的数量是一方面，另外能够预见的是，很容易出现多对一最近点匹配，固然，能够经过一些额外的限定在达到一对一匹配的效果。
根据上述过程计算最优的R和T.
利用获得的位姿做用于P，若是此时的偏差大于阈值，则从新进行迭代，直到迭代次数达到阈值或者偏差小于阈值。
简单的理解，有点像梯度降低寻找极值点的过程，一样的，一个好的初值对加快ICP的收敛过程也十分重要。另外点对点的计算量十分大，复杂度为
O
(
m
n
)
O(mn)
O ( m n ) dom
KD树是每一个节点均为K维数值的二叉树，其上的每一个节点表明一个超平面，该超平面垂直于当前划分维度的坐标轴，并在该维度上将空间一分为二。其构建过程是循环选取数据点的各个维度来做为切分维度，将当前维度的中值做为划分点，递归处理各子树，直到全部数据点挂载完毕。ide
切分维度选择的时候，通常优先选择方差大的维度开始切分。svg
以二维平面点(x,y)的集合(2,3)，(5,4)，(9,6)，(4,7)，(8,1)，(7,2)为例结合下图来讲明k-d tree的构建过程。函数
咱们构建KD树的目的是为了加快最近点搜索过程，那么KD树如何进行最近邻搜索呢？
O
(
l
o
g
(
m
)
)
O(log(m))
O ( l o g ( m ) )
O
(
m
l
o
g
(
n
)
)
O(mlog(n))
O ( m l o g ( n ) )
O
(
m
l
o
g
(
n
)
)
O(mlog(n))
O ( m l o g ( n ) )
O
(
m
n
)
O(mn)
O ( m n ) oop
目前的配准方法，前提都是环境大部分是不变的，可是彻底不变的环境其实也是不多的，好比一辆车飞驰而过，一我的走过等。咱们更多应该考虑的是容许小部分差别的配准，这时候点对点匹配好比ICP就会出现一些问题，而NDT则能够很好地解决细微差。
p
(
x
)
=
1
σ
2
π
e
−
(
x
−
μ
)
2
2
σ
2
p(x)=\frac{1}{\sigma\sqrt{2\pi}}e^{-\frac{(x-\mu)^2}{2\sigma ^2}}
p ( x ) = σ 2 π
1 e − 2 σ 2 ( x − μ ) 2
p
(
x
)
=
1
(
2
π
)
D
2
∣
Σ
∣
e
−
(
x
−
μ
)
→
T
Σ
−
1
(
x
−
μ
)
→
2
σ
2
p(x)=\frac{1}{(2\pi)^\frac{D}{2}\sqrt{|\mathbf{\Sigma|}}}e^{-\frac{(x-\overrightarrow{\mu)}^T\mathbf{\Sigma}^{-1}(x-\overrightarrow{\mu)}}{2\sigma ^2}}
p ( x ) = ( 2 π ) 2 D ∣ Σ ∣
1 e − 2 σ 2 ( x − μ )
T Σ − 1 ( x − μ )
Σ
\mathbf{\Sigma}
Σ post
与ICP不一样，NDT假设点云服从正态分布，咱们的目的是找一个姿态，使得当前扫描点位于参考扫描平面上的可能性最大。假设当前扫描获得的点云为
X
=
{
x
1
→
,
x
2
→
,
.
.
.
,
x
n
→
}
X=\{\overrightarrow{x_1}, \overrightarrow{x_2}, ..., \overrightarrow{x_n}\}
X = { x 1
, x 2
, . . . , x n
}
T
(
p
→
,
x
→
)
T(\overrightarrow{p}, \overrightarrow{x})
T ( p
, x
)
p
→
\overrightarrow{p}
p
x
→
\overrightarrow{x}
x
θ
=
∏
k
=
1
n
p
(
T
(
p
→
,
x
→
)
)
\theta = \prod^n_{k=1}p(T(\overrightarrow{p}, \overrightarrow{x}))
θ = k = 1 ∏ n p ( T ( p
, x
) )
−
log
θ
=
−
∑
k
=
1
n
log
p
(
T
(
p
→
,
x
→
)
-\log{\theta}= - \sum^n_{k=1} \log{p(T(\overrightarrow{p}, \overrightarrow{x})}
− log θ = − k = 1 ∑ n log p ( T ( p
, x
)
在上一部分中咱们没有解释p函数，固然你可能会说不就是几率密度函数吗？是，可是咱们并无先验的几率密度函数，怎么获得的。这里惟一能够利用的即便参考点云，咱们将参考点云网格化，而后计算每一个网格的多维正态分布参数。用
y
k
→
=
1
,
2
,
.
.
.
,
m
\overrightarrow{y_k}=1, 2, ..., m
y k
= 1 , 2 , . . . , m
μ
→
=
1
m
∑
k
=
1
m
y
k
→
\overrightarrow{\mu}=\frac{1}{m}\sum^m_{k=1}\overrightarrow{y_k}
μ
= m 1 ∑ k = 1 m y k
Σ
=
1
m
∑
k
=
1
m
(
y
k
→
−
μ
k
→
)
(
y
k
→
−
μ
k
→
)
T
\Sigma=\frac{1}{m}\sum^m_{k=1}(\overrightarrow{y_k}-\overrightarrow{\mu _k})(\overrightarrow{y_k}-\overrightarrow{\mu _k})^T
Σ = m 1 ∑ k = 1 m ( y k
− μ k
) ( y k
− μ k
) T
p
(
x
)
=
1
(
2
π
)
3
2
∣
Σ
∣
e
−
(
x
−
μ
)
→
T
Σ
−
1
(
x
−
μ
)
→
2
p(x)=\frac{1}{(2\pi)^\frac{3}{2}\sqrt{|\mathbf{\Sigma|}}}e^{-\frac{(x-\overrightarrow{\mu)}^T\mathbf{\Sigma}^{-1}(x-\overrightarrow{\mu)}}{2}}
p ( x ) = ( 2 π ) 2 3 ∣ Σ ∣
1 e − 2 ( x − μ )
T Σ − 1 ( x − μ )
以下图所示，直接取负对数会出现无穷大的点，这样偶然扫到的一些异常点可能会对原本表现很好的结果产生很大的影响致使被舍弃，为了不这种状况，在原函数的基础上作了一些限制。
p
‾
(
x
)
=
c
1
e
x
p
(
−
(
x
−
μ
)
→
T
Σ
−
1
(
x
−
μ
)
→
2
)
+
c
2
p
0
\overline{p}(x)=c_1 exp(-\frac{(x-\overrightarrow{\mu)}^T\mathbf{\Sigma}^{-1}(x-\overrightarrow{\mu)}}{2})+c_2p_0
p ( x ) = c 1 e x p ( − 2 ( x − μ )
T Σ − 1 ( x − μ )
) + c 2 p 0
−
log
(
c
1
e
x
p
(
−
(
x
−
μ
→
)
T
Σ
−
1
(
x
−
μ
→
)
2
)
+
c
2
)
-\log(c_1exp(-\frac{(x-\overrightarrow{\mu})^T\mathbf{\Sigma}^{-1}(x-\overrightarrow{\mu})}{2})+c_2)
− log ( c 1 e x p ( − 2 ( x − μ
) T Σ − 1 ( x − μ
) ) + c 2 )
p
~
(
x
)
=
d
1
e
x
p
(
−
d
2
(
x
−
μ
→
)
T
(
x
−
μ
→
)
2
σ
2
)
+
d
3
\widetilde{p}(x)=d_1 exp(-d_2\frac{(x-\overrightarrow{\mu})^T(x-\overrightarrow{\mu})}{2\sigma ^2})+d_3
p
( x ) = d 1 e x p ( − d 2 2 σ 2 ( x − μ
) T ( x − μ
) ) + d 3
p
‾
(
x
)
=
−
log
(
c
1
e
x
p
(
−
(
x
−
μ
→
)
T
(
x
−
μ
→
)
2
σ
2
)
+
c
2
)
\overline{p}(x)=-\log(c_1exp(-\frac{(x-\overrightarrow{\mu})^T(x-\overrightarrow{\mu})}{2\sigma ^2})+c_2)
p ( x ) = − log ( c 1 e x p ( − 2 σ 2 ( x − μ
) T ( x − μ
) ) + c 2 )
σ
\sigma
σ
∞
\infty
∞
d
3
=
−
log
(
c
2
)
d_3=-\log(c_2)
d 3 = − log ( c 2 )
d
1
=
−
log
(
c
1
+
c
2
)
−
d
3
d_1=-\log(c_1+c_2)-d_3
d 1 = − log ( c 1 + c 2 ) − d 3
d
2
=
−
2
log
(
(
−
log
(
c
1
e
x
p
(
−
1
/
2
)
+
c
2
)
−
d
3
)
/
d
1
)
d_2=-2\log((-\log(c_1exp(-1/2)+c_2)-d_3)/d_1)
d 2 = − 2 log ( ( − log ( c 1 e x p ( − 1 / 2 ) + c 2 ) − d 3 ) / d 1 )
p
~
(
x
k
)
=
−
d
1
e
x
p
(
−
d
2
(
x
k
−
μ
k
→
)
T
Σ
k
−
1
(
x
−
μ
k
→
)
2
)
\widetilde{p}(x_k)=-d_1 exp(-d_2\frac{(x_k-\overrightarrow{\mu_k})^T\mathbf{\Sigma}_k^{-1}(x-\overrightarrow{\mu_k})}{2})
p
( x k ) = − d 1 e x p ( − d 2 2 ( x k − μ k
) T Σ k − 1 ( x − μ k
) )
s
(
p
→
)
=
−
∑
k
=
1
n
p
~
(
T
(
p
→
,
x
k
→
)
)
s(\overrightarrow{p})=- \sum^n_{k=1}\widetilde{p}(T(\overrightarrow{p}, \overrightarrow{x_k}))
s ( p
) = − k = 1 ∑ n p
( T ( p
, x k
) )
这公式实在长，直接截图了，牛顿法的关键就是经过梯度矩阵g和海森矩阵H求解步长
x
k
x_k
x k
p
i
p_i
p i
cell size. 太大了容易致使损失一些局部特征，过小了则会增长不少计算量，过小会致使失去统计普适性，部分数据对结果影响过大。另外，做者直接舍弃掉少于五个数据的网格（由于会形成协方差矩阵不可逆）。
Fixed discretization。固定大小是最多见的划分方式，操做简单，并且容易找到每一个点对应的网格。缺点的话见底下几种方法
Octree discretization。如何快速找到每一个点对应的网格是搜索速度的关键，八叉树结构是常见的三维搜索树。
Iterative discretization。好的初始位置能够加快收敛过程，一个常见的方法就是起始位置迭代，将上一次的终点位姿做为本次的起点位姿
Adaptive clustering。非固定大小网格划分的一种方法，采用K均值聚类（其余聚类方式相似）划分，更能表现出每一个局部数据的特征。
Linked cells。上面提到数据少于五的网格会被舍弃，致使会出现一些空网格，损失了一些完整性，一个改进措施是将这些空网格用指针链接到最近的非空网格上，以该处的PDF代替，因为处于边缘，值仍是很小，可是保证了数据的完整性。
Trilinear interpolation。插值的逻辑就是固定划分实际上会出现边缘不连续的状况，插值法至关于作了一个平滑，计算的时候考虑全部含该数据点的网格取最优值，计算量大约是原来的4倍，但效果也有较大的改善。示意图以下
Iterative dual correspondences (IDC) algorithm。ICP的一种改进，主要是采用极坐标代替笛卡尔坐标系进行最近点搜索
Probabilistic iterative correspondence (PIC) method。这个方法考虑了噪声和初始位姿的不肯定性
Gaussian fields。采用高斯混合模型，相似NDT
Conditional random fields (CRFs) 。条件随机场，还没细看
Branch-and-bound strategy 。分支定界法，没看。
Registration using local geometric features。结合图像的局部特征进行匹配
除此以外最近还有一些结合深度学习的方法，好比百度无人车团队2019CVPR的工做：L3-Net: Towards Learning based LiDAR Localization for Autonomous Driving。输入包括实时激光点云，地图和IMU数据，输出位姿结果。
1.ICP SVD方法论文： Least-Squares Fitting of Two 3-D Point Sets 2. NDT方法详细介绍论文：Magnusson M. The three-dimensional normal-distributions transform: an efficient representation for registration, surface analysis, and loop detection[D]. Örebro universitet, 2009. 3. 论文2部分翻译：https://blog.csdn.net/qq_23225073/article/details/89221098 4. KD树介绍：https://leileiluoluo.com/posts/kdtree-algorithm-and-implementation.html
