点云配准（Point Cloud Registration）即求一个位姿变换 x = [ R , t ] \mathbf{x}=[\mathbf{R},\mathbf{t}] x=[R,t]，将源点云 Q = { q 1 , ⋯ , q m } Q=\{\mathbf{q}_{1},\cdots,\mathbf{q}_{m}\} Q={q1,⋯,qm}变换到与目标点云 P = { p 1 , ⋯ , p n } P=\{\mathbf{p}_{1},\cdots,\mathbf{p}_{n}\} P={p1,⋯,pn}的相同的坐标系下，也即使变换后的源点云与目标点云间相似的部分重合
1、ICP
ICP（Iterative Closest Point，迭代最近点）算法将点云配准问题问题分为数据关联和位姿估计两个步骤，并迭代执行这两个步骤，直至算法收敛。无论数据关联和位姿估计具体是怎样进行，只要算法中包含了这两步交替的过程，就可以认为其为ICP算法。
数据关联即将源点云中的每个点关联到目标点云中的某统计量。常见ICP方法的数据关联方式有点到点的关联，点到面的关联，点到线的关联，这些方法的主要区别在于所关联的目标点云中的统计量不同。
位姿估计即根据固定的数据关联估计位姿变换。对于固定的数据关联 ( q i , S i ) , i = 1 , ⋯ , m (\mathbf{q}_{i},\mathrm{S}_{i}),i=1,\cdots,m (qi,Si),i=1,⋯,m，其中 M i M_i Mi为目标点云中的某统计量，ICP算法进行位姿估计的方式通常为
[ R , t ] ∗ = arg min R , t ∑ i = 1 m e i T e i \begin{align*} [\mathbf{R},\mathbf{t}]^{\ast} &=\underset{\mathbf{R},\mathbf{t}}{\arg\min}\,\sum_{i=1}^{m}\mathbf{e}_{i}^{T}\mathbf{e}_{i}\tag{1}\\ \end{align*} [R,t]∗=R,targmini=1∑meiTei(1)
上式中省略了 R ∈ S O ( 3 ) \mathbf{R}\in SO(3) R∈SO(3)， t ∈ R 3 \mathbf{t}\in\mathbb{R}^{3} t∈R3的条件，下同， e i \mathbf{e}_{i} ei的形式由关联方式决定。点和统计量的关联关系是在迭代过程中不断调整的，故该问题被反复求解。上式主要通过解析的方式闭式求解或通过构建增量方程求解，很多情况下要对求解过程进行改进，如引入核函数等，在这些改进下，闭式解可能并不存在，而增量方程经调整后仍可写出，这种情况下，通常采用增量方程的方式求解。
1.1、点到点ICP
- 数据关联
首先，对点云 Q Q Q中的每个点 q i \mathbf{q}_{i} qi，查找其经位姿变换得到的点 R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t在点云 P P P中的单点最近邻 p i \mathbf{p}_{i} pi。记全部关联的点对为 ( q i , p i ) , i = 1 , ⋯ , m (\mathbf{q}_{i},\mathbf{p}_{i}),i=1,\cdots,m (qi,pi),i=1,⋯,m
- 计算误差向量
点到点的关联误差为
e i = p i − R q i − t (2) \mathbf{e}_{i}=\mathbf{p}_{i}-\mathbf{R}\mathbf{q}_{i}-\mathbf{t}\tag{2} ei=pi−Rqi−t(2)
此时 ∥ e i ∥ \|\mathbf{e}_{i}\| ∥ei∥表示点 R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t和 p i \mathbf{p}_{i} pi两点间的距离
1.1.1、解析解法
在该关联误差下，优化问题 ( 1 ) (1) (1)可写为
[ R , t ] ∗ = arg min R , t ∑ i = 1 m ∥ p i − R q i − t ∥ 2 \begin{align*} [\mathbf{R},\mathbf{t}]^{\ast} &=\underset{\mathbf{R},\mathbf{t}}{\arg\min}\,\sum_{i=1}^{m}\|\mathbf{p}_{i}-\mathbf{R}\mathbf{q}_{i}-\mathbf{t}\|^{2}\tag{3}\\ \end{align*} [R,t]∗=R,targmini=1∑m∥pi−Rqi−t∥2(3)
计算两组点云的质心
p ‾ = 1 n ∑ i = 1 m p i , q ‾ = 1 n ∑ i = 1 m q i \overline{\mathbf{p}}=\frac{1}{n}\sum^{m}_{i=1}\mathbf{p}_{i},\quad \overline{\mathbf{q}}=\frac{1}{n}\sum^{m}_{i=1}\mathbf{q}_{i} p=n1i=1∑mpi,q=n1i=1∑mqi
目标函数可做如下处理
∑ i = 1 m ∥ p i − R q i − t ∥ 2 = ∑ i = 1 m ∥ p i − R q i − t − ( p ‾ − R q ‾ ) + ( p ‾ − R q ‾ ) ∥ 2 = ∑ i = 1 m ∥ ( p i − p ‾ − R ( q i − q ‾ ) ) + ( p ‾ − R q ‾ − t ) ∥ 2 = ∑ i = 1 m [ ∥ p i − p ‾ − R ( q i − q ‾ ) ∥ 2 + ∥ p ‾ − R q ‾ − t ∥ 2 + 2 ( p i − p ‾ − R ( q i − q ‾ ) ) T ( p ‾ − R q ‾ − t ) ] = ∑ i = 1 m [ ∥ p i − p ‾ − R ( q i − q ‾ ) ∥ 2 + ∥ p ‾ − R q ‾ − t ∥ 2 ] \begin{align*} \sum_{i=1}^{m}\|\mathbf{p}_{i}-\mathbf{R}\mathbf{q}_{i}-\mathbf{t}\|^{2} &=\sum_{i=1}^{m}\|\mathbf{p}_{i}-\mathbf{R}\mathbf{q}_{i}-\mathbf{t}-(\overline{\mathbf{p}}-\mathbf{R}\overline{\mathbf{q}})+(\overline{\mathbf{p}}-\mathbf{R}\overline{\mathbf{q}})\|^{2}\\ &=\sum_{i=1}^{m}\|(\mathbf{p}_{i}-\overline{\mathbf{p}}-\mathbf{R}(\mathbf{q}_{i}-\overline{\mathbf{q}}))+(\overline{\mathbf{p}}-\mathbf{R}\overline{\mathbf{q}}-\mathbf{t})\|^{2}\\ &=\sum_{i=1}^{m}\left[\|\mathbf{p}_{i}-\overline{\mathbf{p}}-\mathbf{R}(\mathbf{q}_{i}-\overline{\mathbf{q}})\|^{2}+\|\overline{\mathbf{p}}-\mathbf{R}\overline{\mathbf{q}}-\mathbf{t}\|^{2}+2(\mathbf{p}_{i}-\overline{\mathbf{p}}-\mathbf{R}(\mathbf{q}_{i}-\overline{\mathbf{q}}))^{T}(\overline{\mathbf{p}}-\mathbf{R}\overline{\mathbf{q}}-\mathbf{t})\right]\\ &=\sum_{i=1}^{m}\left[\|\mathbf{p}_{i}-\overline{\mathbf{p}}-\mathbf{R}(\mathbf{q}_{i}-\overline{\mathbf{q}})\|^{2}+\|\overline{\mathbf{p}}-\mathbf{R}\overline{\mathbf{q}}-\mathbf{t}\|^{2}\right]\tag{4}\\ \end{align*} i=1∑m∥pi−Rqi−t∥2=i=1∑m∥pi−Rqi−t−(p−Rq)+(p−Rq)∥2=i=1∑m∥(pi−p−R(qi−q))+(p−Rq−t)∥2=i=1∑m[∥pi−p−R(qi−q)∥2+∥p−Rq−t∥2+2(pi−p−R(qi−q))T(p−Rq−t)]=i=1∑m[∥pi−p−R(qi−q)∥2+∥p−Rq−t∥2](4)
上式从第三行推导至第四行是根据最后一项求和为零
由 ( 4 ) (4) (4)的形式知道，求和号中第一项仅和 R \mathbf{R} R相关，可据此对 R \mathbf{R} R进行优化，在得到旋转矩阵的最优估计 R ∗ \mathbf{R}^{\ast} R∗后，令第二项等于零，即可得到位移的最优估计 t ∗ \mathbf{t}^{\ast} t∗。下面先对第一项进行优化，即使得目标函数在任意固定的 t \mathbf{t} t值下对 R \mathbf{R} R最小
R ∗ = arg min R ∑ i = 1 m ∥ p i − p ‾ − R ( q i − q ‾ ) ∥ 2 \begin{align*} \mathbf{R}^{\ast} &=\underset{\mathbf{R}}{\arg\min}\,\sum_{i=1}^{m}\|\mathbf{p}_{i}-\overline{\mathbf{p}}-\mathbf{R}(\mathbf{q}_{i}-\overline{\mathbf{q}})\|^{2}\tag{5} \end{align*} R∗=Rargmini=1∑m∥pi−p−R(qi−q)∥2(5)
引入去质心坐标
p i ′ = p i − p ‾ , q i ′ = q i − q ‾ \mathbf{p}^{\prime}_{i}=\mathbf{p}_{i}-\overline{\mathbf{p}},\quad\mathbf{q}^{\prime}_{i}=\mathbf{q}_{i}-\overline{\mathbf{q}} pi′=pi−p,qi′=qi−q
可将 ( 5 ) (5) (5)写为
R ∗ = arg min R ∑ i = 1 m ∥ p i ′ − R q i ′ ∥ 2 = arg min R ∑ i = 1 m ( p i ′ T p i ′ + q i ′ T R T R q i ′ − 2 p i ′ T R q i ′ ) = arg max R ∑ i = 1 m p i ′ T R q i ′ = arg max R ∑ i = 1 m t r ( R q i ′ p i ′ T ) = arg max R t r ( R ∑ i = 1 m q i ′ p i ′ T ) = arg min R − t r ( R W T ) \begin{align*} \mathbf{R}^{\ast} &=\underset{\mathbf{R}}{\arg\min}\,\sum_{i=1}^{m}\|\mathbf{p}^{\prime}_{i}-\mathbf{R}\mathbf{q}^{\prime}_{i}\|^{2}\\ &=\underset{\mathbf{R}}{\arg\min}\,\sum_{i=1}^{m}\left({\mathbf{p}^{\prime}_{i}}^{T}{\mathbf{p}^{\prime}_{i}}+{\mathbf{q}^{\prime}_{i}}^{T}\mathbf{R}^{T}\mathbf{R}\mathbf{q}^{\prime}_{i}-2{\mathbf{p}^{\prime}_{i}}^{T}\mathbf{R}\mathbf{q}^{\prime}_{i}\right)\\ &=\underset{\mathbf{R}}{\arg\max}\,\sum_{i=1}^{m}{\mathbf{p}^{\prime}_{i}}^{T}\mathbf{R}\mathbf{q}^{\prime}_{i}\\ &=\underset{\mathbf{R}}{\arg\max}\,\sum_{i=1}^{m}\mathrm{tr}\left(\mathbf{R}\mathbf{q}^{\prime}_{i}{\mathbf{p}^{\prime}_{i}}^{T}\right)\\ &=\underset{\mathbf{R}}{\arg\max}\,\mathrm{tr}\left(\mathbf{R}\sum_{i=1}^{m}\mathbf{q}^{\prime}_{i}{\mathbf{p}^{\prime}_{i}}^{T}\right)\\ &=\underset{\mathbf{R}}{\arg\min}\,-\mathrm{tr}\left(\mathbf{R}\mathbf{W}^{T}\right)\tag{6}\\ \end{align*} R∗=Rargmini=1∑m∥pi′−Rqi′∥2=Rargmini=1∑m(pi′Tpi′+qi′TRTRqi′−2pi′TRqi′)=Rargmaxi=1∑mpi′TRqi′=Rargmaxi=1∑mtr(Rqi′pi′T)=Rargmaxtr(Ri=1∑mqi′pi′T)=Rargmin−tr(RWT)(6)
其中
W = ∑ i = 1 m p i ′ q i ′ T (7) \mathbf{W}=\sum_{i=1}^{m}{\mathbf{p}^{\prime}_{i}}{\mathbf{q}^{\prime}_{i}}^{T}\tag{7} W=i=1∑mpi′qi′T(7)
考虑到 R ∈ S O ( 3 ) \mathbf{R}\in{SO}(3) R∈SO(3)，可以构造如下优化问题
min − t r ( R W T ) s u b j e c t t o R T R = I det ( R ) = 1 (8) \begin{align*} \min\,\,\,&-\mathrm{tr}\left(\mathbf{R}\mathbf{W}^{T}\right)\\ \mathrm{subject\,to}\,\,\,&\mathbf{R}^T\mathbf{R}=\mathbf{I}\quad\det(\mathbf{R})=1\\ \end{align*}\tag{8} minsubjectto−tr(RWT)RTR=Idet(R)=1(8)
首先对 W \mathbf{W} W进行SVD分解
W = U Σ V T (9) \mathbf{W}=\mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^{T}\tag{9} W=UΣVT(9)
根据§1中KKT条件，可以证明，在任何情况下，使 ( 6 ) (6) (6)中目标函数最优的旋转矩阵 R ∗ \mathbf{R}^{\ast} R∗总可以取为下述值
R ∗ = U [ 1 1 det ( U ) det ( V ) ] V T (10) \begin{align*} \mathbf{R}^{\ast}=\mathbf{U} \begin{bmatrix} 1&&\\ &1&\\ &&\det(\mathbf{U})\det(\mathbf{V}) \end{bmatrix} \mathbf{V}^{T} \end{align*}\tag{10} R∗=U 11det(U)det(V) VT(10)
为使第二项等于零，位移的最优估计 t ∗ \mathbf{t}^{\ast} t∗应取值为
t ∗ = p ‾ − R ∗ q ‾ (11) \mathbf{t}^{\ast}=\overline{\mathbf{p}}-\mathbf{R}^{\ast}\overline{\mathbf{q}}\tag{11} t∗=p−R∗q(11)
1.1.2、增量方程解法
- 计算雅可比矩阵
( 2 ) (2) (2)中 e i \mathbf{e}_{i} ei对旋转矩阵的右扰动雅可比和对平移的雅可比为：
∂ e i ∂ R = R q i ∧ ∂ e i ∂ t = − I (13) \begin{align*} \frac{\partial\mathbf{e}_{i}}{\partial\mathbf{R}}=\mathbf{R}\mathbf{q}_{i}^{\wedge} \quad\quad \frac{\partial\mathbf{e}_{i}}{\partial\mathbf{t}}=-\mathbf{I} \end{align*}\tag{13} ∂R∂ei=Rqi∧∂t∂ei=−I(13)
应用ICP算法求源点云 Q = { q 1 , ⋯ , q m } Q=\{\mathbf{q}_{1},\cdots,\mathbf{q}_{m}\} Q={q1,⋯,qm}到目标点云 P = { p 1 , ⋯ , p n } P=\{\mathbf{p}_{1},\cdots,\mathbf{p}_{n}\} P={p1,⋯,pn}的位姿变换 x = [ R , t ] \mathbf{x}=[\mathbf{R},\mathbf{t}] x=[R,t]的步骤如下：
-
令 k = 0 k=0 k=0，给定初始的位姿估计为 x 0 = [ R 0 , t 0 ] \mathbf{x}_{0}=[\mathbf{R}_{0},\mathbf{t}_{0}] x0=[R0,t0]
-
在当前位姿估计 x k = [ R k , t k ] \mathbf{x}_{k}=[\mathbf{R}_{k},\mathbf{t}_{k}] xk=[Rk,tk]下，将点云 Q Q Q中每个点 q i \mathbf{q}_{i} qi关联到点云 P P P中的某统计量
-
对每个关联，计算误差向量 e i \mathbf{e}_{i} ei和误差向量对状态量的雅可比矩阵 J i = [ ∂ e i ∂ R , ∂ e i ∂ t ] ∣ R k , t k \mathbf{J}_{i}=\left.\left[\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{R}},\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{t}}\right]\right|_{\mathbf{R}_{k},\mathbf{t}_{k}} Ji=[∂R∂ei,∂t∂ei] Rk,tk
-
根据 J i , e i , i = 1 , ⋯ , m \mathbf{J}_{i},\mathbf{e}_{i},i=1,\cdots,m Ji,ei,i=1,⋯,m求出海森矩阵 H k \mathbf{H}_{k} Hk，梯度 b k \mathbf{b}_{k} bk
H k = ∑ i = 1 m J i T J i b k = ∑ i = 1 m J i T e i \begin{align*} \mathbf{H}_{k}=\sum_{i=1}^{m}\mathbf{J}_{i}^{T}\mathbf{J}_{i} \quad\quad\mathbf{b}_{k}&=\sum_{i=1}^{m}\mathbf{J}_{i}^{T}\mathbf{e}_{i} \end{align*} Hk=i=1∑mJiTJibk=i=1∑mJiTei -
求解增量方程
H k Δ x k = − b k \mathbf{H}_{k}\Delta\mathbf{x}_{k}=-\mathbf{b}_{k} HkΔxk=−bk -
若 Δ x k \Delta\mathbf{x}_{k} Δxk足够小，则认为算法已收敛，退出；否则令 x k + 1 = x k ⊕ Δ x k \mathbf{x}_{k+1}=\mathbf{x}_{k}\oplus\Delta\mathbf{x}_{k} xk+1=xk⊕Δxk， k = k + 1 k=k+1 k=k+1，返回2
上述步骤中，不同ICP算法的区别仅在于数据关联和计算关联误差和雅可比两个步骤，下面讨论不同ICP算法解决这两个问题具体方式
1.2、点到面ICP
- 数据关联
对点云 Q Q Q中的每个点 q i \mathbf{q}_{i} qi，查找其经位姿变换得到的点 R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t在点云 P P P中的 k k k点最近邻 { p i 1 , ⋯ , p i k } \{\mathbf{p}_{i_{1}},\cdots,\mathbf{p}_{i_{k}}\} {pi1,⋯,pik}，通过 k k k点最近邻，进行局部平面拟合，得平面 Π i : n i T p + d i = 0 , ∥ n ~ i ∥ = 1 \Pi_{i}:\mathbf{n}_{i}^{T}\mathbf{p}+d_{i}=0,\|\tilde{\mathbf{n}}_{i}\|=1 Πi:niTp+di=0,∥n~i∥=1。记全部关联的点—平面对为 ( q i , Π i ) , i = 1 , ⋯ , m (\mathbf{q}_{i},\Pi_{i}),i=1,\cdots,m (qi,Πi),i=1,⋯,m
- 计算误差向量和雅可比矩阵
点到面的关联误差为
e i = n i T ( R q i + t ) + d i (14) \mathbf{e}_{i}=\mathbf{n}_{i}^{T}(\mathbf{R}\mathbf{q}_{i}+\mathbf{t})+d_{i}\tag{14} ei=niT(Rqi+t)+di(14)
此时 ∥ e i ∥ \|\mathbf{e}_{i}\| ∥ei∥表示点 R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t到平面 Π i \Pi_{i} Πi的距离
e i \mathbf{e}_{i} ei对旋转矩阵的右扰动雅可比和对平移的雅可比为：
∂ e i ∂ R = − n i T R q i ∧ ∂ e i ∂ t = n i T (15) \begin{align*} \frac{\partial\mathbf{e}_{i}}{\partial\mathbf{R}}=-\mathbf{n}_{i}^{T}\mathbf{R}\mathbf{q}_{i}^{\wedge} \quad\quad\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{t}}=\mathbf{n}_{i}^{T} \end{align*}\tag{15} ∂R∂ei=−niTRqi∧∂t∂ei=niT(15)
1.3、点到线ICP
- 数据关联
对点云 Q Q Q中的每个点 q i \mathbf{q}_{i} qi，查找其经位姿变换得到的点 R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t在点云 P P P中的 k k k点最近邻 { p i 1 , ⋯ , p i k } \{\mathbf{p}_{i_{1}},\cdots,\mathbf{p}_{i_{k}}\} {pi1,⋯,pik}，通过这 k k k点最近邻，进行局部直线拟合，得到直线 L i : p = d i t + p i , ∥ d i ∥ = 1 L_{i}:\mathbf{p}=\mathbf{d}_{i}t+\mathbf{p}_{i},\|\mathbf{d}_{i}\|=1 Li:p=dit+pi,∥di∥=1。记全部关联的点—直线对为 ( q i , L i ) , i = 1 , ⋯ , m (\mathbf{q}_{i},L_{i}),i=1,\cdots,m (qi,Li),i=1,⋯,m
- 计算误差向量和雅可比矩阵：
点到直线的关联误差为
e i = d i × ( R q i + t − p i ) = d i ∧ ( R q i + t − p i ) (16) \mathbf{e}_{i}=\mathbf{d}_{i}\times(\mathbf{R}\mathbf{q}_{i}+\mathbf{t}-\mathbf{p}_{i})=\mathbf{d}_{i}^{\wedge}(\mathbf{R}\mathbf{q}_{i}+\mathbf{t}-\mathbf{p}_{i})\tag{16} ei=di×(Rqi+t−pi)=di∧(Rqi+t−pi)(16)
此时 ∥ e i ∥ \|\mathbf{e}_{i}\| ∥ei∥表示点 R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t到直线 L i L_{i} Li的距离
e i \mathbf{e}_{i} ei对旋转矩阵的右扰动雅可比和对平移的雅可比为：
∂ e i ∂ R = − d i ∧ R q i ∧ ∂ e i ∂ t = d i ∧ (17) \begin{align*} \frac{\partial\mathbf{e}_{i}}{\partial\mathbf{R}}=-\mathbf{d}_{i}^{\wedge}\mathbf{R}\mathbf{q}_{i}^{\wedge} \quad\quad\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{t}}=\mathbf{d}_{i}^{\wedge} \end{align*}\tag{17} ∂R∂ei=−di∧Rqi∧∂t∂ei=di∧(17)
2、NDT
NDT（Normal Distribution Transform，正态分布变换）算法认为点云在局部服从正态分布，算法将点云空间划分为体素，并假设在每个体素中的点服从同一正态分布，正态分布的分布参数可以通过目标点云中落入体素的点估计，这样就将每个体素中的目标点云都转变为了正态分布这一更能表达其局部统计信息的统计量，在位姿估计时，只需在位姿变换下，将源点云中的点与正态分布进行关联，并最大化全体源点云经位姿变换后得到的点在其对应的正态分布下的抽取概率的乘积，并不断迭代，从而得到源点云到目标点云的最优位姿变换
NDT算法分为构建体素，数据关联，位姿估计三个步骤，下面分别介绍这三个步骤
一、构建体素
应用体素方法建立目标点云 P P P的哈希表，对哈希表中存在的每个体素 V o x e l \mathrm{Voxel} Voxel，可认为其中的点服从分布 N ( μ , Σ ) \mathcal{N}(\boldsymbol\mu,\boldsymbol\Sigma) N(μ,Σ)
根据目标点云中落入体素 V o x e l \mathrm{Voxel} Voxel中的点，可以估计 V o x e l \mathrm{Voxel} Voxel的正态分布参数 μ , Σ \boldsymbol\mu,\boldsymbol\Sigma μ,Σ
二、数据关联
对点云 Q Q Q中的每个点 q i \mathbf{q}_{i} qi，根据哈希表查找其经位姿变换得到的点 R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t所在的体素 V o x e l i \mathrm{Voxel}_{i} Voxeli，若该体素有效，即其中存在目标点云中的点，则将 q i \mathbf{q}_{i} qi与 N ( μ i , Σ i ) \mathcal{N}(\boldsymbol\mu_{i},\boldsymbol\Sigma_{i}) N(μi,Σi)关联。记全部关联的点—正态分布对为 ( q i , N ( μ i , Σ i ) ) , i = 1 , ⋯ , s (\mathbf{q}_{i},\mathcal{N}(\boldsymbol\mu_{i},\boldsymbol\Sigma_{i})),i=1,\cdots,s (qi,N(μi,Σi)),i=1,⋯,s
三、位姿估计
R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t在体素 V o x e l i \mathrm{Voxel}_{i} Voxeli下的似然为
p ( R q i + t ∣ μ i , Σ i ) ∝ exp ( − 1 2 e i T Σ i − 1 e i ) (18) p(\mathbf{R}\mathbf{q}_{i}+\mathbf{t}|\boldsymbol\mu_{i},\boldsymbol\Sigma_{i})\propto\exp\left(-\frac{1}{2}\mathbf{e}_{i}^{T}\boldsymbol\Sigma^{-1}_{i}\mathbf{e}_{i}\right)\tag{18} p(Rqi+t∣μi,Σi)∝exp(−21eiTΣi−1ei)(18)
其中
e i = R q i + t − μ i (19) \mathbf{e}_{i}=\mathbf{R}\mathbf{q}_{i}+\mathbf{t}-\boldsymbol\mu_{i}\tag{19} ei=Rqi+t−μi(19)
对固定的点和体素的关联，优化目标是最大化全体 R q i + t \mathbf{R}\mathbf{q}_{i}+\mathbf{t} Rqi+t在其所在的体素下的似然的乘积，即
[ R , t ] ∗ = arg max R , t ∏ i = 1 s p ( R q i + t ∣ μ i , Σ i ) = arg min R , t ∑ i = 1 s e i T Σ i − 1 e i (20) \begin{align*} [\mathbf{R},\mathbf{t}]^{\ast} &=\underset{\mathbf{R},\mathbf{t}}{\arg\max}\,\prod_{i=1}^{s}p(\mathbf{R}\mathbf{q}_{i}+\mathbf{t}|\boldsymbol\mu_{i},\boldsymbol\Sigma_{i})\\ &=\underset{\mathbf{R},\mathbf{t}}{\arg\min}\,\sum_{i=1}^{s}\mathbf{e}_{i}^{T}\boldsymbol\Sigma^{-1}_{i}\mathbf{e}_{i}\\ \end{align*}\tag{20} [R,t]∗=R,targmaxi=1∏sp(Rqi+t∣μi,Σi)=R,targmini=1∑seiTΣi−1ei(20)
点和体素的关联关系是在迭代过程中不断调整的，故该问题被反复求解。仍采用增量方程的方式求解上述优化问题。
应用NDT算法求源点云 Q = { q 1 , ⋯ , q m } Q=\{\mathbf{q}_{1},\cdots,\mathbf{q}_{m}\} Q={q1,⋯,qm}到目标点云 P = { p 1 , ⋯ , p n } P=\{\mathbf{p}_{1},\cdots,\mathbf{p}_{n}\} P={p1,⋯,pn}的位姿变换 x = [ R , t ] \mathbf{x}=[\mathbf{R},\mathbf{t}] x=[R,t]的步骤如下：
-
应用体素方法建立目标点云 P P P的哈希表，对每个体素 V o x e l \mathrm{Voxel} Voxel，根据包含在其中的点，计算均值 μ \boldsymbol\mu μ，协方差矩阵 Σ \boldsymbol\Sigma Σ
-
令 k = 0 k=0 k=0，给定初始的位姿估计为 x 0 = [ R 0 , t 0 ] \mathbf{x}_{0}=[\mathbf{R}_{0},\mathbf{t}_{0}] x0=[R0,t0]
-
对点云 Q Q Q中的每个点 q i \mathbf{q}_{i} qi，根据哈希表查找其经位姿变换得到的点 R k q i + t k \mathbf{R}_{k}\mathbf{q}_{i}+\mathbf{t}_{k} Rkqi+tk所在的体素 V o x e l i \mathrm{Voxel}_{i} Voxeli，若该体素有效，则将 q i \mathbf{q}_{i} qi与 N ( μ i , Σ i ) \mathcal{N}(\boldsymbol\mu_{i},\boldsymbol\Sigma_{i}) N(μi,Σi)关联
-
对每个点—体素对 ( q i , N ( μ i , Σ i ) ) (\mathbf{q}_{i},\mathcal{N}(\boldsymbol\mu_{i},\boldsymbol\Sigma_{i})) (qi,N(μi,Σi))
计算关联误差
e i = R k q i + t k − μ i \mathbf{e}_{i}=\mathbf{R}_{k}\mathbf{q}_{i}+\mathbf{t}_{k}-\boldsymbol\mu_{i} ei=Rkqi+tk−μi
计算误差向量对状态量的雅可比矩阵
J i = [ ∂ e i ∂ R , ∂ e i ∂ t ] ∣ R k , t k = [ − R k q i ∧ , I ] \mathbf{J}_{i} =\left.\left[\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{R}},\frac{\partial\mathbf{e}_{i}}{\partial\mathbf{t}}\right]\right|_{\mathbf{R}_{k},\mathbf{t}_{k}} =\left[-\mathbf{R}_{k}\mathbf{q}_{i}^{\wedge},\mathbf{I}\right] Ji=[∂R∂ei,∂t∂ei] Rk,tk=[−Rkqi∧,I]
将信息矩阵赋值为该体素上的信息矩阵
Ω i = ( Σ i + λ I ) − 1 \boldsymbol\Omega_{i}=(\boldsymbol\Sigma_{i}+\lambda\mathbf{I})^{-1} Ωi=(Σi+λI)−1 -
根据 J i , e i , i = 1 , ⋯ , s \mathbf{J}_{i},\mathbf{e}_{i},i=1,\cdots,s Ji,ei,i=1,⋯,s求出海森矩阵 H k \mathbf{H}_{k} Hk，梯度 b k \mathbf{b}_{k} bk
H k = ∑ i = 1 s J i T Ω i J i b k = ∑ i = 1 s J i T Ω i e i \begin{align*} \mathbf{H}_{k}=\sum_{i=1}^{s}\mathbf{J}_{i}^{T}\boldsymbol\Omega_{i}\mathbf{J}_{i} \quad\quad\mathbf{b}_{k}&=\sum_{i=1}^{s}\mathbf{J}_{i}^{T}\boldsymbol\Omega_{i}\mathbf{e}_{i} \end{align*} Hk=i=1∑sJiTΩiJibk=i=1∑sJiTΩiei -
求解增量方程
H k Δ x k = − b k \mathbf{H}_{k}\Delta\mathbf{x}_{k}=-\mathbf{b}_{k} HkΔxk=−bk -
若 Δ x k \Delta\mathbf{x}_{k} Δxk足够小，则认为算法已收敛，退出；否则令 x k + 1 = x k ⊕ Δ x k \mathbf{x}_{k+1}=\mathbf{x}_{k}\oplus\Delta\mathbf{x}_{k} xk+1=xk⊕Δxk， k = k + 1 k=k+1 k=k+1，返回3
实验：点云配准方法对比
现有两组三维点云，采用多线程版本的不同方法求第二组点云到第一组点云中的位姿变换所需的时间，与真实位姿的误差如下
| 点到点ICP | 点到面ICP | 点到线ICP | NDT（体素大小 r = 1 r=1 r=1，仅取中心体素） | |
|---|---|---|---|---|
| 时间（毫秒） | 192.25 192.25 192.25 | 217.618 217.618 217.618 | 381.422 381.422 381.422 | 68.55 68.55 68.55 |
| 误差 | 0.015 0.015 0.015 | 0.000014 0.000014 0.000014 | 0.033 0.033 0.033 | 0.003 0.003 0.003 |
附录
§1、拉格朗日乘子法
求解如下优化问题
min f ( x ) s u b j e c t t o g i ( x ) = 0 i = 1 , ⋯ , m h j ( x ) ⩽ 0 j = 1 , ⋯ , n (A1) \begin{align*} \min\,\,\,&f(\mathbf{x})\\ \mathrm{subject\,to}\,\,\,&g_{i}(\mathbf{x})=0\quad i=1,\cdots,m\\ &h_{j}(\mathbf{x})\leqslant0\quad j=1,\cdots,n \end{align*}\tag{A1} minsubjecttof(x)gi(x)=0i=1,⋯,mhj(x)⩽0j=1,⋯,n(A1)
问题的定义域为 D = { x ∈ R N ∣ g i ( x ) = 0 , h j ( x ) ⩽ 0 } D=\{\mathbf{x}\in\mathbb{R}^{N}|g_{i}(\mathbf{x})=0,h_{j}(\mathbf{x})\leqslant0\} D={x∈RN∣gi(x)=0,hj(x)⩽0}，其中 f , g i , h j f,g_{i},h_{j} f,gi,hj在 R N \mathbb{R}^{N} RN上具有连续的一阶偏导数
首先定义拉格朗日函数
L ( x , λ , μ ) = f ( x ) + ∑ i = 1 m λ i g i ( x ) + ∑ i = 1 n μ j h j ( x ) (A2) L(\mathbf{x},\boldsymbol\lambda,\boldsymbol\mu)=f(\mathbf{x})+\sum_{i=1}^{m}\lambda_{i}g_{i}(\mathbf{x})+\sum_{i=1}^{n}\mu_{j}h_{j}(\mathbf{x})\tag{A2} L(x,λ,μ)=f(x)+i=1∑mλigi(x)+i=1∑nμjhj(x)(A2)
其中 λ = [ λ 1 , ⋯ , λ m ] T \boldsymbol\lambda=[\lambda_{1},\cdots,\lambda_{m}]^{T} λ=[λ1,⋯,λm]T， μ = [ μ 1 , ⋯ , μ n ] T \boldsymbol\mu=[\mu_{1},\cdots,\mu_{n}]^{T} μ=[μ1,⋯,μn]T称为问题 ( A 1 ) (\mathrm{A1}) (A1)的拉格朗日乘子向量。
下列方程组为优化问题取得极值点的必要条件，也称为Karush-Kuhn-Tucker条件，简称KKT条件
{ ∇ x L ( x , λ , μ ) = ∇ x f ( x ) + ∑ j = 1 m λ i ∇ x g i ( x ) + ∑ i = 1 n μ j ∇ x h j ( x ) = 0 g i ( x ) = 0 i = 1 , ⋯ , m h j ( x ) ⩽ 0 j = 1 , ⋯ , n μ j ⩾ 0 j = 1 , ⋯ , n μ j h j ( x ) = 0 j = 1 , ⋯ , n (A3) \left\{ \begin{align*} \nabla_{\mathbf{x}}L(\mathbf{x},\boldsymbol\lambda,\boldsymbol\mu)=\nabla_{\mathbf{x}}f(\mathbf{x})+\sum_{j=1}^{m}\lambda_{i}\nabla_{\mathbf{x}}g_{i}(\mathbf{x})+\sum_{i=1}^{n}\mu_{j}\nabla_{\mathbf{x}}h_{j}(\mathbf{x})=\mathbf{0}\\ g_{i}(\mathbf{x})=0\quad i=1,\cdots,m\\ h_{j}(\mathbf{x})\leqslant0\quad j=1,\cdots,n\\ \mu_{j}\geqslant0\quad j=1,\cdots,n\\ \mu_{j}h_{j}(\mathbf{x})=0\quad j=1,\cdots,n \end{align*} \right.\tag{A3} ⎩ ⎨ ⎧∇xL(x,λ,μ)=∇xf(x)+j=1∑mλi∇xgi(x)+i=1∑nμj∇xhj(x)=0gi(x)=0i=1,⋯,mhj(x)⩽0j=1,⋯,nμj⩾0j=1,⋯,nμjhj(x)=0j=1,⋯,n(A3)
需要注意，通过上式求出的 x ∗ \mathbf{x}^{\ast} x∗可能为约束邻域上的局部极大/极小值点（内部、边界），非局部极大/极小值点的普通驻点（内部，边界），鞍点（内部）
