论文地址
论文视频
为什么这次要解读这篇文章？因为上次文章（详见 TEASER | 快速且可证明的点云配准算法和代码解读）旋转求解部分就是用本文中的方法，所以本文算是TEASER方法的前置工作。因此通过理解本文的内容，那么就会对TEASER旋转部分有更深入的理解。本文使用截断最小二乘将Wahba问题转化为优化问题，然后经过一系列操作产生了一个等价非凸二次约束二次规划问题，最终根据该问题的特性提出相应算子求解和证明，这是一个可证明最优性并且可同时处理高外点率的多项式时间方法。
Wahba问题，也称为旋转搜索，寻求最佳的旋转来对齐两组给定假定对应的向量观测值，它是许多计算机视觉和机器人应用中的一个基本例程。本文提出了第一个多项式时间可证明的最优方法来解决当大量的向量观测值是外点时的Wahba问题。我们的第一个贡献是利用截断最小二乘（TLS）代价来描述Wahba问题，该代价对大量的伪对应不敏感。第二个贡献是使用单位四元数重写问题，并且证明了TLS代价可以用二次约束二次规划（QCQP）来描述。因为上述产生的优化问题仍然是强烈非凸以及很难全局求解，因此我们的第三个贡献是开发一个凸半定规划（SDP）松弛方法。我们证明了尽管单纯的松弛方法通常表现不佳，但我们的松弛方法是紧的，甚至在大噪声和大量外点时仍然时紧的。我们在合成和真实数据集上验证提出的算法，该算法称为QUASAR（基于四元数的鲁棒对齐半定松弛算法），结果表明我们的算法超越了RANSAC，鲁棒局部优化方法，全局外点移除方法以及Branch-and-Bound方法。QUASAR甚至在95%的对应都是外点的情况下仍然能够计算可证明的最优解（即松弛是准确的）。
给定两组向量
a
i
,
b
i
∈
R
3
,
i
=
1
,
.
.
.
,
N
a_i, b_i \in\mathbb{R}^3, i=1,...,N
ai,bi∈R3,i=1,...,N，Wahba问题被建模为一个最小二乘问题
m
i
n
R
∈
S
O
(
3
)
∑
i
=
1
N
w
i
2
∣
∣
b
i
−
R
a
i
∣
∣
2
\mathop{min}\limits_{R \in SO(3)}\sum\limits_{i=1}^N w_i^2||b_i-Ra_i||^2
R∈SO(3)mini=1∑Nwi2∣∣bi−Rai∣∣2
使用截断最小二乘（TLS）代价函数，则原始Wahba问题转换为鲁棒Wahba问题 m i n R ∈ S O ( 3 ) ∑ i = 1 N m i n ( 1 σ i 2 ∣ ∣ b i − R a i ∣ ∣ 2 , c ˉ 2 ) \mathop{min}\limits_{R \in SO(3)}\sum\limits_{i=1}^N min( \frac{1}{\sigma_i^2}||b_i-Ra_i||^2, \bar{c}^2) R∈SO(3)mini=1∑Nmin(σi21∣∣bi−Rai∣∣2,cˉ2)
向量
a
∈
R
3
a\in\mathbb{R}^3
a∈R3 的旋转可以用单位四元数乘积来表示，即
[
R
a
0
]
=
q
⊗
a
^
⊗
q
−
1
\begin{bmatrix} Ra \\ 0 \end{bmatrix} = q\otimes\hat{a} \otimes q^{-1}
[Ra0]=q⊗a^⊗q−1其中齐次化
a
^
=
[
a
T
0
]
T
\hat{a}=[a^T 0]^T
a^=[aT0]T
使用单位四元数参数化旋转，基于四元数的鲁棒Wahba问题为
m
i
n
q
∈
S
3
∑
i
=
1
N
m
i
n
(
1
σ
i
2
∣
∣
b
i
^
−
q
⊗
a
^
i
⊗
q
−
1
∣
∣
2
,
c
ˉ
2
)
\mathop{min}\limits_{q \in S^3}\sum\limits_{i=1}^N min( \frac{1}{\sigma_i^2}||\hat{b_i}-q\otimes\hat{a}_i \otimes q^{-1}||^2, \bar{c}^2)
q∈S3mini=1∑Nmin(σi21∣∣bi^−q⊗a^i⊗q−1∣∣2,cˉ2)其中定义
a
^
i
≐
[
a
i
T
0
]
T
\hat{a}_i \doteq [a_i^T 0]^T
a^i≐[aiT0]T 和
b
^
i
≐
[
b
i
T
0
]
T
\hat{b}_i \doteq [b_i^T 0]^T
b^i≐[biT0]T
引入二值变量
θ
i
,
i
=
1
,
.
.
.
,
N
\theta_i, i=1,..., N
θi,i=1,...,N，则基于四元数的鲁棒Wahba问题可以重写为一个混合整数规划问题
m
i
n
q
∈
S
3
θ
i
=
{
±
1
}
∑
i
=
1
N
1
+
θ
i
2
∣
∣
b
^
i
−
q
⊗
a
^
i
⊗
q
−
1
∣
∣
2
σ
i
2
+
1
−
θ
i
2
c
ˉ
2
\mathop{min}\limits_{ q \in S^3 \atop \theta_i=\left \{ \pm1 \right \} }\sum\limits_{i=1}^N \frac{1+\theta_i}{2}\frac{||\hat{b}_i-q\otimes\hat{a}_i \otimes q^{-1}||^2}{\sigma_i^2} + \frac{1-\theta_i}{2}\bar{c}^2
θi={±1}q∈S3mini=1∑N21+θiσi2∣∣b^i−q⊗a^i⊗q−1∣∣2+21−θicˉ2这样就可以去掉TLS代价函数中的 “
m
i
n
min
min”，同时二值变量
{
θ
i
}
i
=
1
N
\left \{ \theta_i\right \}^N_{i=1}
{θi}i=1N决定了测量值是内点还是外点，显然内点时
θ
i
=
+
1
\theta_i=+1
θi=+1，外点时
θ
i
=
−
1
\theta_i = -1
θi=−1
定义N个四元数
q
i
≐
θ
i
q
,
i
=
1
,
.
.
.
,
N
q_i\doteq\theta_i q, i=1,...,N
qi≐θiq,i=1,...,N，则混合整数规划问题可以重写为 N+1 个四元数优化问题
m
i
n
q
∈
S
3
q
i
=
{
±
q
}
∑
i
=
1
N
∣
∣
b
^
i
−
q
⊗
a
^
i
⊗
q
−
1
+
q
T
q
i
b
^
i
−
q
⊗
a
^
i
⊗
q
i
−
1
∣
∣
2
4
σ
i
2
+
1
−
q
T
q
i
2
c
ˉ
2
\mathop{min}\limits_{ q \in S^3 \atop q_i=\left \{ \pm q \right \} }\sum\limits_{i=1}^N \frac{||\hat{b}_i-q\otimes\hat{a}_i \otimes q^{-1}+q^Tq_i \hat{b}_i-q \otimes \hat{a}_i \otimes q_i^{-1}||^2}{4\sigma_i^2} + \frac{1-q^Tq_i}{2}\bar{c}^2
qi={±q}q∈S3mini=1∑N4σi2∣∣b^i−q⊗a^i⊗q−1+qTqib^i−q⊗a^i⊗qi−1∣∣2+21−qTqicˉ2该问题也可以称为 binary cloning，显然内点时
q
i
=
q
q_i=q
qi=q，外点时
q
i
=
−
q
q_i=-q
qi=−q
将上述 N+1 个四元数堆叠起来，定义一个列向量
x
=
[
q
T
q
1
T
.
.
.
q
N
T
]
T
x=[q^T q_1^T ... q_N^T]^T
x=[qTq1T...qNT]T，则 binary cloning 问题等价于一个二次约束二次规划问题
m
i
n
x
∈
R
4
(
N
+
1
)
q
i
=
{
±
q
}
∑
i
=
1
N
x
T
Q
i
x
\mathop{min}\limits_{ x \in \mathbb{R}^{4(N+1)} \atop q_i=\left \{ \pm q \right \} }\sum\limits_{i=1}^N x^TQ_ix
qi={±q}x∈R4(N+1)mini=1∑NxTQix
s
u
b
j
e
c
t
t
o
x
q
T
x
q
=
1
subject \ \ to \quad x^T_qx_q=1
subject toxqTxq=1
x
q
i
x
q
i
T
=
x
q
x
q
T
,
∀
i
=
1
,
.
.
.
,
N
x_{q_i}x^T_{q_i}=x_qx^T_q, \forall i=1, ..., N
xqixqiT=xqxqT,∀i=1,...,N 其中
Q
i
∈
R
4
(
N
+
1
)
×
4
(
N
+
1
)
(
i
=
1
,
.
.
.
,
N
)
Q_i \in \mathbb{R}^{4(N+1) \times 4(N+1)} (i=1, ..., N)
Qi∈R4(N+1)×4(N+1)(i=1,...,N) 是依赖于3D向量
a
i
a_i
ai 和
b
i
b_i
bi 的已知对称矩阵，可以通过推导得出
[
Q
i
]
u
v
=
{
Q
i
i
i
f
u
=
q
i
a
n
d
v
=
q
i
Q
0
i
i
f
u
=
q
a
n
d
v
=
q
i
o
r
u
=
q
i
a
n
d
v
=
q
0
4
×
4
o
t
h
e
r
w
i
s
e
[Q_i]_{uv} = \left\{ \begin{array}{lcl} Q_{ii} \quad if \ \ u=q_i \ \ and \ \ v=q_i \\ Q_{0i} \quad if \ \ u=q \ \ and \ \ v=q_i \ \ or \ \ u=q_i \ \ and \ \ v=q \\ 0_{4 \times 4} \quad otherwise \end{array} \right.
[Qi]uv=⎩⎨⎧Qiiif u=qi and v=qiQ0iif u=q and v=qi or u=qi and v=q04×4otherwise其中
Q
i
i
=
(
∣
∣
b
i
∣
∣
2
+
∣
∣
a
i
∣
∣
2
)
I
4
+
2
Ω
1
(
b
^
i
)
Ω
2
(
a
^
i
)
2
σ
i
2
+
c
ˉ
2
2
I
4
Q_{ii}=\frac{(||b_i||^2+||a_i||^2)I_4+2\Omega_1(\hat{b}_i)\Omega_2(\hat{a}_i)}{2\sigma^2_i}+\frac{\bar{c}^2}{2}I_4
Qii=2σi2(∣∣bi∣∣2+∣∣ai∣∣2)I4+2Ω1(b^i)Ω2(a^i)+2cˉ2I4，
Q
0
i
=
(
∣
∣
b
i
∣
∣
2
+
∣
∣
a
i
∣
∣
2
)
I
4
+
2
Ω
1
(
b
^
i
)
Ω
2
(
a
^
i
)
4
σ
i
2
−
c
ˉ
2
4
I
4
Q_{0i}=\frac{(||b_i||^2+||a_i||^2)I_4+2\Omega_1(\hat{b}_i)\Omega_2(\hat{a}_i)}{4\sigma^2_i}-\frac{\bar{c}^2}{4}I_4
Q0i=4σi2(∣∣bi∣∣2+∣∣ai∣∣2)I4+2Ω1(b^i)Ω2(a^i)−4cˉ2I4，那么
∑
i
=
1
N
q
i
T
Q
i
i
q
i
+
2
q
T
Q
0
i
q
i
=
∑
i
=
1
N
x
T
Q
i
x
\sum\limits_{i=1}^N q^T_iQ_{ii}q_i+2q^TQ_{0i}q_i = \sum\limits_{i=1}^N x^TQ_ix
i=1∑NqiTQiiqi+2qTQ0iqi=i=1∑NxTQix
因为二次等式约束是非凸的，所以上述 QCQP 问题仍然是非凸且NP-hard的，之后就需要对该问题开发一个凸半定规划松弛。
首先定义
Q
≐
∑
i
=
1
N
Q
i
Q \doteq \sum\limits_{i=1}^N Q_i
Q≐i=1∑NQi，然后定义
Z
=
x
x
T
=
[
q
q
T
q
q
1
T
.
.
.
q
q
N
T
∗
q
q
1
T
.
.
.
q
q
N
T
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
∗
∗
.
.
.
q
N
q
N
T
]
Z=xx^T=\begin{bmatrix} &qq^T & qq_1^T & ... & qq_N^T \\ &* &qq_1^T &... &qq_N^T \\ &. &. &... &. \\ &. &. &... &.\\ &. &. &... &.\\ &* &* &... &q_Nq_N^T\end{bmatrix}
Z=xxT=⎣⎢⎢⎢⎢⎢⎢⎡qqT∗...∗qq1Tqq1T...∗..................qqNTqqNT...qNqNT⎦⎥⎥⎥⎥⎥⎥⎤可以得到
∑
i
=
1
N
x
T
Q
i
x
=
x
T
Q
x
=
t
r
(
Q
x
x
T
)
=
t
r
(
Q
Z
)
\sum\limits_{i=1}^Nx^TQ_ix=x^TQx=tr(Qxx^T)=tr(QZ)
i=1∑NxTQix=xTQx=tr(QxxT)=tr(QZ)那么QCQP问题就等价于以下的非凸规划
m
i
n
Z
⪰
0
t
r
(
Q
Z
)
\mathop{min}\limits_{ Z \succeq 0 } \ \ tr(QZ)
Z⪰0min tr(QZ)
s
u
b
j
e
c
t
t
o
t
r
(
[
Z
]
q
q
)
=
1
subject \ to \ \ tr([Z]_{qq})=1
subject to tr([Z]qq)=1
[
Z
]
q
i
q
i
=
[
Z
]
q
q
,
∀
i
=
1
,
.
.
.
,
N
[Z]_{q_iq_i}=[Z]_{qq}, \forall i=1, ..., N
[Z]qiqi=[Z]qq,∀i=1,...,N
r
a
n
k
(
Z
)
=
1
rank(Z)=1
rank(Z)=1上述非凸问题中的非凸性就显而易见了，因为唯一的非凸性就是秩1约束了，所以只需要将秩约束直接去掉就可以得到标准半定规划松弛
m
i
n
Z
⪰
0
t
r
(
Q
Z
)
\mathop{min}\limits_{ Z \succeq 0 } \ \ tr(QZ)
Z⪰0min tr(QZ)
s
u
b
j
e
c
t
t
o
t
r
(
[
Z
]
q
q
)
=
1
subject \ to \ \ tr([Z]_{qq})=1
subject to tr([Z]qq)=1
[
Z
]
q
i
q
i
=
[
Z
]
q
q
,
∀
i
=
1
,
.
.
.
,
N
[Z]_{q_iq_i}=[Z]_{qq}, \forall i=1, ..., N
[Z]qiqi=[Z]qq,∀i=1,...,N但是上述标准半定规划松弛对于存在外点的测量值并不能保持紧度，所以在此基础上增加冗余约束从而加强半定规划松弛的紧度，得到冗余约束半定规划松弛，这里称为 QUASAR 算法
m
i
n
Z
⪰
0
t
r
(
Q
Z
)
\mathop{min}\limits_{ Z \succeq 0 } \ \ tr(QZ)
Z⪰0min tr(QZ)
s
u
b
j
e
c
t
t
o
t
r
(
[
Z
]
q
q
)
=
1
subject \ to \ \ tr([Z]_{qq})=1
subject to tr([Z]qq)=1
[
Z
]
q
i
q
i
=
[
Z
]
q
q
,
∀
i
=
1
,
.
.
.
,
N
[Z]_{q_iq_i}=[Z]_{qq}, \forall i=1, ..., N
[Z]qiqi=[Z]qq,∀i=1,...,N
[
Z
]
q
q
i
=
[
Z
]
q
q
i
T
,
∀
i
=
1
,
.
.
.
,
N
[Z]_{qq_i}=[Z]_{qq_i}^T, \forall i=1, ..., N
[Z]qqi=[Z]qqiT,∀i=1,...,N
[
Z
]
q
i
q
j
=
[
Z
]
q
i
q
j
T
,
∀
1
≤
i
≤
j
≤
N
[Z]_{q_iq_j}=[Z]_{q_iq_j}^T, \forall 1\le i \le j \le N
[Z]qiqj=[Z]qiqjT,∀1≤i≤j≤N
如果上述凸的半定规划松弛的最优解 Z ∗ Z^* Z∗ 秩为1，那么 Z ∗ Z^* Z∗ 可以分解为 Z ∗ = ( x ∗ ) ( x ∗ ) T Z^*=(x^*)(x^*)^T Z∗=(x∗)(x∗)T，且 x ≐ [ ( q ∗ ) T ( q 1 ∗ ) T . . . ( q N ∗ ) T ] T x \doteq [(q^*)^T \ (q_1^*)^T \ ... \ (q_N^*)^T]^T x≐[(q∗)T (q1∗)T ... (qN∗)T]T 是非凸 QCQP 问题的全局最优解。
从上面左图可以看出，在没有外点的时候，两种松弛都是紧的；但当测量值中包含越来越多外点时标准松弛变得越来越松散，并且在40%外点率时直接失效；相反，无论外点率为多大，QUASAR一直能够保持紧度。通过上面右图可以进一步确认紧松弛(也就是QUASAR)能够保持更精准旋转估计
QUASAR和闭式解(Wahba)，RANSAC，FGR(Fast Global Registration)以及GORE(Guaranteed Outlier Removal)方法进行比较
外点率不同(0-90%)，固定内点噪声为较低水平(0.01)的结果：
外点率不同(0-90%)，固定内点噪声为较高水平(0.1)的结果：
极端外点率(91-96%)的结果
使用SURF特征描述子匹配和建立对应点关系，然后使用QUASAR计算两个相机帧之间的相对旋转
R
R
R，结果如下
可以看到，QUASAR在外点很多以及两张图像重叠区域较小时仍然能够进行精准拼接，效果不错。
这个工作提出了一个多项式时间的可证实最优解的算法，用于Wahba问题的求解。虽然该方法很鲁棒，计算效率也比较理想(多项式时间)，但还是有一些问题的，实验中的问题规模最大只是100，这里受限制的原因是通用的SDP算子在大规模问题上会计算很慢，可以预见该方法在大规模点云重定位上会表现不佳，所以有一个未来的研究方向：开发快速专用的SDP算子，可以参考 IJRR18 的经典论文"SE-Sync: a certifiably correct algorithm for synchronization over the Special Euclidean group."
之前介绍过该作者 TRO20 TEASER 的工作，里面的旋转估计框架就是这篇ICCV文章里的方法，所以该工作是 TEASER 的前置工作，并且 Supplementary Material 里有各种引理，定理的证明，包括如何利用四元数的转换推导，QCQP中二次目标和二次约束的构建，Lagrangian duality和Karush-Kuhn-Tucker (KKT) 的使用和证明，再一次说明作者一系列工作都很solid，非常值得学习和研究。之后将会为大家带来 TEASER 中另一个前置工作 RSS2019 “A Polynomial-time Solution for Robust Registration with Extreme Outlier Rates”，敬请期待。
