3两个点云配准
3.1目标函数
对于俩个点集
P
P
P
和
Q
Q
Q
，我们的目的是找到一个刚体变换，对齐这两个点集。我们的方法是优化关于
P
P
P
和
Q
Q
Q
对应关系的稳健性目标函数。
设
K
=
(
p
,
q
)
K = { ( p , q) }
K
=
(
p
,
q
)
是由匹配点从P和Q中获得的对应集合，我们的目标是优化位姿T，使得对应点之间的距离最小化，同时拒绝
K
K
K
中的的错误对应关系。目标函数具有如下形式：
E
(
T
)
=
∑
(
p
,
q
)
∈
K
ρ
(
∥
p
−
T
q
∥
)
(1)
E(\mathbf{T})=\sum_{(\mathbf{p}, \mathbf{q}) \in \mathcal{K}} \rho(\|\mathbf{p}-\mathbf{T} \mathbf{q}\|)\tag{1}
E
(
T
)
=
(
p
,
q
)
∈
K
∑
ρ
(
∥
p
−
Tq
∥
)
(
1
)
ρ
(
.
)
\rho(.)
ρ
(
.
)
是罚函数。罚函数的选择是至关重要的，一个好的罚函数可以规避错误的匹配关系。我们使用Geman-McClure估计器：
ρ
(
x
)
=
μ
x
2
μ
+
x
2
(2)
\rho(x)=\frac{\mu x^2}{\mu+x^2}\tag{2}
ρ
(
x
)
=
μ
+
x
2
μ
x
2
(
2
)
目标函数1是很难直接进行优化的，我们利用Black -Rangaration对偶（对偶：即等价的）处理目标函数1，Black -Rangaration对偶是稳健估计和线性的。具体地，令
L
=
{
l
p
，
q
}
L = \lbrace l_{p，q} \rbrace
L
=
{
l
p
，
q
}
为对应关系上的线过程。我们在T和L上优化以下联合目标函数：
E
(
T
,
L
)
=
∑
(
p
,
q
)
∈
K
l
p
,
q
∥
p
−
T
q
∥
2
+
∑
(
p
,
q
)
∈
K
Ψ
(
l
p
,
q
)
(3)
E(\mathbf{T}, \mathbb{L})=\sum_{(\mathbf{p}, \mathbf{q}) \in \mathcal{K}} l_{\mathbf{p}, \mathbf{q}}\|\mathbf{p}-\mathbf{T} \mathbf{q}\|^2+\sum_{(\mathbf{p}, \mathbf{q}) \in \mathcal{K}} \Psi\left(l_{\mathbf{p}, \mathbf{q}}\right)\tag{3}
E
(
T
,
L
)
=
(
p
,
q
)
∈
K
∑
l
p
,
q
∥
p
−
Tq
∥
2
+
(
p
,
q
)
∈
K
∑
Ψ
(
l
p
,
q
)
(
3
)
Ψ
(
l
p
,
q
)
\Psi\left(l_{\mathbf{p}, \mathbf{q}}\right)
Ψ
(
l
p
,
q
)
是先验概率：
Ψ
(
l
p
,
q
)
=
μ
(
l
p
,
q
−
1
)
2
(4)
\Psi\left(l_{\mathbf{p}, \mathbf{q}}\right)=\mu\left(\sqrt{l_{\mathbf{p}, \mathbf{q}}}-1\right)^2\tag{4}
Ψ
(
l
p
,
q
)
=
μ
(
l
p
,
q
−
1
)
2
(
4
)
为了最小化
E
(
T
,
L
)
E(\mathbf{T}, \mathbb{L})
E
(
T
,
L
)
，需要对每个
l
p
,
q
l_{\mathbf{p}, \mathbf{q}}
l
p
,
q
求偏导：
∂
E
∂
l
p
,
q
=
∥
p
−
T
q
∥
2
+
μ
l
p
,
q
−
1
l
p
,
q
=
0
(5)
\frac{\partial E}{\partial l_{\mathbf{p}, \mathbf{q}}}=\|\mathbf{p}-\mathbf{T} \mathbf{q}\|^2+\mu \frac{\sqrt{l_{\mathbf{p}, \mathbf{q}}}-1}{\sqrt{l_{\mathbf{p}, \mathbf{q}}}}=0\tag{5}
∂
l
p
,
q
∂
E
=
∥
p
−
Tq
∥
2
+
μ
l
p
,
q
l
p
,
q
−
1
=
0
(
5
)
所以：
l
p
,
q
=
(
μ
μ
+
∥
p
−
T
q
∥
2
)
2
(6)
l_{\mathbf{p}, \mathbf{q}}=\left(\frac{\mu}{\mu+\|\mathbf{p}-\mathbf{T} \mathbf{q}\|^2}\right)^2\tag{6}
l
p
,
q
=
(
μ
+
∥
p
−
Tq
∥
2
μ
)
2
(
6
)
将
l
p
,
q
l_{\mathbf{p}, \mathbf{q}}
l
p
,
q
带入公式3，得到公式1。因此优化目标函数3即是优化目标函数1。
3.2优化
等式3的好处就是可以交替优化T和L。
当L固定时：等式3转变为对应关系的距离的
L
2
L^2
L
2
罚函数的权重之和。通过文献9可以轻松的得到封闭解。但是这种求解方式只适合两两点云配准，不适合求解多个点云同时配准。因此提出一种新的计算方法：假设两次变化矩阵的变化很小，则变换矩阵
T
T
T
用下面公式进行近似：
T
≈
(
1
−
γ
β
a
γ
1
−
α
b
−
β
α
1
c
0
0
0
1
)
T
k
(7)
\mathbf{T} \approx\left(\begin{array}{cccc} 1 & -\gamma & \beta & a \\ \gamma & 1 & -\alpha & b \\ -\beta & \alpha & 1 & c \\ 0 & 0 & 0 & 1 \end{array}\right) \mathbf{T}^k\tag{7}
T
≈
1
γ
−
β
0
−
γ
1
α
0
β
−
α
1
0
a
b
c
1
T
k
(
7
)
α
,
β
,
γ
,
a
,
b
,
c
\alpha,\beta,\gamma,a,b,c
α
,
β
,
γ
,
a
,
b
,
c
分别是三个旋转量和三个偏移量，
T
k
T^k
T
k
是上一次迭代得到的变换矩阵。则通过高斯牛顿法可以求解公式3.
当T固定时，当
l
p
,
q
l_{p,q}
l
p
,
q
满足公式6时，等式3可以求得封闭解。
3.3对应关系
我们选择Fast Point Feature Histogram (FPFH) feature。记
F
(
P
)
=
{
F
(
p
)
:
p
∈
P
}
F(P)=\lbrace{F(p):p\in{P}}\rbrace
F
(
P
)
=
{
F
(
p
)
:
p
∈
P
}
,
F
(
p
)
F(p)
F
(
p
)
是点p对应的FPFH，同样地，记
F
(
Q
)
=
{
F
(
q
)
:
q
∈
Q
}
F(Q)=\lbrace{F(q):q\in{Q}}\rbrace
F
(
Q
)
=
{
F
(
q
)
:
q
∈
Q
}
。对于点p的F§，在F(Q)中找到p点的最近邻点，同样对于点q，在F§中找到q的最近邻点。记
K
1
{K1}
K
1
为对应关系集合，但是这里面有很多错误的匹配对，因此运用一下两个准则进行筛选：
（1）互惠性检测
如果F§和F(q)互为最近邻点，则通过互惠性检验，是一个对应关系对，将所有通过互惠性检验的对应关系集合记为
K
2
{K2}
K
2
.
(2)元组检测
从
K
2
{K2}
K
2
随机选择3个匹配对
(
p
1
,
q
1
)
,
(
p
2
,
q
2
)
,
(
p
3
,
q
3
)
(p1,q1),(p2,q2),(p3,q3)
(
p
1
,
q
1
)
,
(
p
2
,
q
2
)
,
(
p
3
,
q
3
)
,如果满足以下关系上，表明3个匹配对是相容的，将所有通过测试的匹配对集合记为
K
3
{K3}
K
3
，
K
3
{K3}
K
3
即为最终的匹配对集合。
∀
i
≠
j
,
τ
<
∥
p
i
−
p
j
∥
∥
q
i
−
q
j
∥
<
1
/
τ
(8)
\forall i \neq j, \quad \tau<\frac{\left\|\mathbf{p}_i-\mathbf{p}_j\right\|}{\left\|\mathbf{q}_i-\mathbf{q}_j\right\|}<1 / \tau\tag{8}
∀
i
=
j
,
τ
<
∥
q
i
−
q
j
∥
∥
p
i
−
p
j
∥
<
1/
τ
(
8
)
整个算法如下图所示：
4多个点云配准
我们开发一种方法：直接将所有点云同时配准，而不是优化单独的成对的点云，然后优化两两配准结果。我们直接优化所有点云的全局位姿。
4.1目标函数
对于点云集合
{
P
i
}
\lbrace{P_i}\rbrace
{
P
i
}
,其对应的变换矩阵集合为
T
=
{
T
i
}
\mathbb{T}=\lbrace{T_i}\rbrace
T
=
{
T
i
}
.对于所有点云而言，公式1将变为如下等式(i表示点云序号，j表示每个点云中第j个匹配对)：
E
(
T
)
=
λ
∑
i
∑
(
p
,
q
)
∈
K
i
∥
T
i
p
−
T
i
+
1
q
∥
2
+
∑
i
<
j
∑
(
p
,
q
)
∈
K
i
j
ρ
(
∥
T
i
p
−
T
j
q
∥
)
(10)
E(\mathbb{T})=\lambda \sum_i \sum_{(\mathbf{p}, \mathbf{q}) \in \mathcal{K}_i}\left\|\mathbf{T}_i \mathbf{p}-\mathbf{T}_{i+1} \mathbf{q}\right\|^2+\sum_{i<j} \sum_{(\mathbf{p}, \mathbf{q}) \in \mathcal{K}_{i j}} \rho\left(\left\|\mathbf{T}_i \mathbf{p}-\mathbf{T}_j \mathbf{q}\right\|\right)\tag{10}
E
(
T
)
=
λ
i
∑
(
p
,
q
)
∈
K
i
∑
∥
T
i
p
−
T
i
+
1
q
∥
2
+
i
<
j
∑
(
p
,
q
)
∈
K
ij
∑
ρ
(
∥
T
i
p
−
T
j
q
∥
)
(
10
)
根据公式3和公式4，等式10可以变为如下形式：
E
(
T
,
L
)
=
λ
∑
i
∑
(
p
,
q
)
∈
K
i
∥
T
i
p
−
T
i
+
1
q
∥
2
+
∑
i
<
j
(
∑
(
p
,
q
)
∈
K
i
j
l
p
,
q
∥
T
i
p
−
T
j
q
∥
2
+
∑
(
p
,
q
)
∈
K
i
j
Ψ
(
l
p
,
q
)
)
(11)
\begin{aligned} E(\mathbb{T}, \mathbb{L}) & =\lambda \sum_i \sum_{(\mathbf{p}, \mathbf{q}) \in \mathcal{K}_i}\left\|\mathbf{T}_i \mathbf{p}-\mathbf{T}_{i+1} \mathbf{q}\right\|^2 \\ & +\sum_{i<j}\left(\sum_{(\mathbf{p}, \mathbf{q}) \in \mathcal{K}_{i j}} l_{\mathbf{p}, \mathbf{q}}\left\|\mathbf{T}_i \mathbf{p}-\mathbf{T}_j \mathbf{q}\right\|^2+\sum_{(\mathbf{p}, \mathbf{q}) \in \mathcal{K}_{i j}} \Psi\left(l_{\mathbf{p}, \mathbf{q}}\right)\right) \end{aligned}\tag{11}
E
(
T
,
L
)
=
λ
i
∑
(
p
,
q
)
∈
K
i
∑
∥
T
i
p
−
T
i
+
1
q
∥
2
+
i
<
j
∑
(
p
,
q
)
∈
K
ij
∑
l
p
,
q
∥
T
i
p
−
T
j
q
∥
2
+
(
p
,
q
)
∈
K
ij
∑
Ψ
(
l
p
,
q
)
(
11
)
4.2优化
我们再次使用交替优化求解最小化问题。在每次迭代中，首先使用L最小化
E
(
T
,
L
)
E(\mathbb{T}, \mathbb{L})
E
(
T
,
L
)
：
l
p
,
q
=
(
μ
μ
+
∥
T
i
p
−
T
j
q
∥
2
)
2
(12)
l_{\mathbf{p}, \mathbf{q}}=\left(\frac{\mu}{\mu+\left\|\mathbf{T}_i \mathbf{p}-\mathbf{T}_j \mathbf{q}\right\|^2}\right)^2\tag{12}
l
p
,
q
=
(
μ
+
∥
T
i
p
−
T
j
q
∥
2
μ
)
2
(
12
)
然后使用所有变换矩阵
T
\mathbb{T}
T
最小化
E
(
T
,
L
)
E(\mathbb{T}, \mathbb{L})
E
(
T
,
L
)
（这里和两两配准优化顺序是反的）。记
T
i
k
\mathbf{T}_i^k
T
i
k
为上一次迭代计算的变换矩阵.
T
i
\mathbf{T}_i
T
i
可以被线性化为以下形式：
T
i
≈
(
1
−
γ
i
β
i
a
i
γ
i
1
−
α
i
b
i
−
β
i
α
i
1
c
i
0
0
0
1
)
T
i
k
.
(13)
\begin{aligned} \ \mathbf{T}_i & \approx\left(\begin{array}{cccc} 1 & -\gamma_i & \beta_i & a_i \\ \gamma_i & 1 & -\alpha_i & b_i \\ -\beta_i & \alpha_i & 1 & c_i \\ 0 & 0 & 0 & 1 \end{array}\right) \mathbf{T}_i^k . \end{aligned}\tag{13}
T
i
≈
1
γ
i
−
β
i
0
−
γ
i
1
α
i
0
β
i
−
α
i
1
0
a
i
b
i
c
i
1
T
i
k
.
(
13
)
使用最小二乘计算旋转和平移量，然后更新
T
i
\mathbf{T}_i
T
i
。
注意：对应关系只计算一次，并不是每次都更新。每次迭代只更新
l
p
,
q
l_{\mathbf{p}, \mathbf{q}}
l
p
,
q
和
T
T
T
。
文献来源：《Fast Global Registration》
开源代码(两两配准)