# Convex global 3d registration with lagrangian duality

J. Briales and J. Gonzalez-Jimenez, *Convex global 3d registration with lagrangian duality*, CVPR, 2017.

[toc]

## Abstract

The registration of 3D models by SE(3) is non-convex due to rotational constraints, making local optimization methods stuck in local minima. This paper addresses finding the globally optimal transformation by a unified formulation that integrates common geometric registration modalities (point-to-point, point-to-line, point-to-plane). This formulation renders the optimization problem independent of both the number and nature of the correspondences.

We introduce a strengthened Lagrangian dual relaxation for this problem, which surpasses previous approaches [32] in effectiveness. Even though with no theoretical guarantees, experiments always resulted on a tight relaxation that allowed to recover a guaranteed globally optimal solution by exploiting duality theory.

> - formulate problem by point-to-point, point-to-line, point-to-plane residual
> - convex relaxation by Lagrangian dual, solve convex SDP
> - no theoretical guarantee, however, always resulted on a tight relaxation in practice
>
> [32] Solving quadratically constrained geometrical problems using Lagrangian duality.
> 為主要比較對象

## Formulation

$$
T^*=\min_{T\in\text{SE}(3)}\sum_{i=1}^nd_{P_i}^2(T\oplus x_i)
$$

- $\{x_i\},\{P_i\}$ are point clouds
- $T\oplus x$ denotes the Euclidean transformation
- $d_{P_i}$ denotes the distance to $P_i$

### generalized distance function

> 定義 point-point, point-line, point-plane 距離公式，也是 objective function
>

$$
d_{P}^2=\min_{y^\prime\in P}\|x-y^\prime\|^2_2=\|x-y\|^2_C=(x-y)^TC(x-y)
$$


> $P$ can be point（紅）, line（綠） or plane（藍）


$$
\begin{align}\min_{y^\prime\in P}\|x-y^\prime\|^2_2&=\|x-y\|^2_{I_3}=(x-y)^TI_3(x-y)\tag{point}\\&=\|x-y\|^2_{(I-vv^T)}=(x-y)^T(I-vv^T)(x-y)\tag{line}\\&=\|x-y\|^2_{nn^T}=(x-y)^Tnn^T(x-y)\tag{plane}\end{align}
$$

> **point**
>
>
> $$
> d^2_\text{point}=\|x-y\|^2_2=(x-y)^T\underbrace{I_3}_C(x-y)
> $$
>
> **line** $y^*=y+(v^T(x-y))v$：$y$往$v$ 走$\langle v,x-y\rangle$量的位移
>
> $$
> d^2_\text{line}=\|x-y^*\|^2_2=\|x-(y+(v^T(x-y))v)\|^2_2=\|(I_3-vv^T)(x-y)\|^2_2
> $$
>
> since $(I-vv^T)^2=I-vv^T$ and $(I-vv^T)^T=I-vv^T$:
>
> $$
> d^2_\text{line}=(x-y)^T(I-vv^T)^T(I-vv^T)(x-y)=(x-y)^T\underbrace{(I-vv^T)}_C(x-y)
> $$
>
> **plane** $x-y$ 沿著unit normal vector投影
>
> $$
> d^2_\text{plane}=(n^T(x-y))^2=(x-y)^T\underbrace{nn^T}_C(x-y)
> $$
>

### quadratic formulation

> 將 objective function 寫成 quadratic form
>

$$
d_{P_i}^2(T\oplus x_i)=(T\oplus x_i-y_i)^TC_i(T\oplus x_i-y_i)
$$

let $\tilde{x}=\begin{bmatrix}x\\1\end{bmatrix},\ \tau=\text{vec}(T)=\begin{bmatrix}\text{vec}(R)\\t\end{bmatrix}$ (column-wise vectorization of rotation):

$$
T\oplus x_i=Rx_i+t=(\tilde{x}^T\otimes I_3)\tau
$$

> **pf)**
>
>
> $$
> \begin{pmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{pmatrix}\begin{pmatrix}x_1\\x_2\\x_3\end{pmatrix}+\begin{pmatrix}t_1\\t_2\\t_3\end{pmatrix}=\begin{pmatrix}r_{11}x_1+r_{12}x_2+r_{13}x_3+t_1\\r_{21}x_1+r_{22}x_2+r_{23}x_3+t_2\\r_{31}x_1+r_{32}x_2+r_{33}x_3+t_3\end{pmatrix}=\begin{pmatrix}x_1&0&0&x_2&0&0&x_3&0&0&1&0&0\\0&x_1&0&0&x_2&0&0&x_3&0&0&1&0\\0&0&x_1&0&0&x_2&0&0&x_3&0&0&1\end{pmatrix}\begin{pmatrix}r_{11}\\r_{21}\\r_{31}\\r_{12}\\r_{22}\\r_{32}\\r_{13}\\r_{23}\\r_{33}\\t_1\\t_2\\t_3\end{pmatrix}
> $$
>

let $N_i=\begin{bmatrix}\tilde{x}_i^T\otimes I_3&-y_i\end{bmatrix}$ and $\tilde{\tau}=\begin{bmatrix}\tau\\1\end{bmatrix}$

> $\tilde{\tau}$ should be column vector instead of row vector $\begin{bmatrix}\tau^T&1\end{bmatrix}$ (paper)
>

$$
d_{P_i}^2(T\oplus x_i)=\tilde{\tau}^T\underbrace{N_i^TC_iN_i}_{M_i}\tilde{\tau}
$$

objective function now becomes a quadratic function:

$$
f(T)=\sum_{i=1}^nd^2_{P_i}(T\oplus x_i)=\tilde{\tau}^T\underbrace{\Bigg(\sum_{i=1}^nM_i\Bigg)}_M\tilde{\tau}
$$

rewrite to problem:

$$
\min_{R\in\text{SO}(3),t\in\mathbb{R}^3}\tilde{\tau}^TM\tilde{\tau},\ \tilde{\tau}=\begin{bmatrix}\text{vec}(R)\\t\\1\end{bmatrix}\in\mathbb{R}^{13}\tag{QP}
$$

### marginalization

> 將問題寫成變數只有rotation，translation用rotation表示
>

by Lagrangian multipliers:

$$
\mathcal{L}(R,t,\lambda)=\underbrace{\bar{\tau}^TM\bar{\tau}}_{f(R,t)}+\lambda^T\text{constraint}_{\text{SO}(3)}(R)
$$

> let $!t=\tilde{\tau}\setminus t=\begin{bmatrix}\text{vec}(R)\\1\end{bmatrix}=\bar{\tau}$ be the complementary set of variables that are not in $t$
>
>
> $$
> f(R,t)=\begin{bmatrix}!t&t\end{bmatrix}\begin{bmatrix}M_{!t,!t}&M_{!t,t}\\M_{t,!t}&M_{t,t}\end{bmatrix}\begin{bmatrix}!t\\t\end{bmatrix}=!t^TM_{!t,!t}!t+2t^TM_{t,!t}!t+t^TM_{t,t}t
> $$
>

compute gradient and set it to 0, SO(3) constraints are independent to translation:

$$
\begin{aligned}\frac{\partial}{\partial t}\mathcal{L}(R,t,\lambda)&=\frac{\partial f(R,t)}{\partial t}+0_{3\times1}\\&=2M_{t,!t}!t+2M_{t,t}t=0_{3\times1}\end{aligned}
$$

rewrite $t$ in terms of $R$:

$$
t^*=-M_{t,t}^{-1}M_{t,!t}\bar{\tau},\ \bar{\tau}=\begin{bmatrix}\text{vec}(R)\\1\end{bmatrix}\in\mathbb{R}^{10}\tag{translation equation}
$$

> 解釋：the optimal translation given a fixed rotation
>

$$
\begin{aligned}f(R,t)&=\bar{\tau}^TM_{!t,!t}\bar{\tau}-2\bar{\tau}^TM_{t,!t}^T(M_{t,t}^{-1})^TM_{t,!t}\bar{\tau}+\bar{\tau}^TM_{t,!t}^T(M_{t,t}^{-1})^TM_{t,t}M_{t,t}^{-1}M_{t,!t}\bar{\tau}\\
&=\bar{\tau}^TM_{!t,!t}\bar{\tau}-\bar{\tau}^TM_{t,!t}M_{t,t}^{-1}M_{t,!t}\bar{\tau}\\&=\bar{\tau}^TQ\bar{\tau}\end{aligned}
$$

where $Q=M_{!t,!t}-M_{t,!t}M_{t,t}^{-1}M_{t,!t}=M\setminus M_{t,t}$ is also a Schur complement, problem becomes:

$$
\min_{\bar\tau\in\mathbb{R}^{10}}\bar{\tau}^TQ\bar{\tau},\ \bar{\tau}=\begin{bmatrix}\text{vec}(R)\\1\end{bmatrix}\in\mathbb{R}^{10}\tag{QP, rotation problem}
$$

> 成功將原本$\mathbb{R}^{13}$變數包括rotation,translation的問題reformulate成$\mathbb{R}^{10}$變數只有rotation的問題

### algorithm

1. compute $M$
2. compute $Q$ by Schur complement
3. solve $R$ by rotation algorithm
4. solve $t$ by translation equation

## Tight dual relaxation

> rotation problem is non-convex, reformulate to convex dual problem by Lagrangian duality
>

$$
\text{SO}(3)=\{R\in\mathbb{R}^{3\times3}\mid R^TR=RR^T=I_3,\ \det(R)=\pm1\}
$$

### duality strengthening

> add scalar constraints to deal with rotation constraints
>
- adding new scalar constraints $c_{k+1}(\cdot)$ introduces new Lagrangian multiplier $\lambda_{k+1}$
- new dual problem is at least as good as the previous one $d^*_k\leq d^*_{k+1}\leq p^*$
- adding redundant constraints can improve the quality of dual relaxations

$$
RR^T=I=\begin{bmatrix}r_1&r_2&r_3\\r_4&r_5&r_6\\r_7&r_8&r_9\end{bmatrix}\begin{bmatrix}r_1&r_4&r_7\\r_2&r_5&r_8\\r_3&r_6&r_9\end{bmatrix}\implies\begin{cases}r_1^2+r_2^2+r_3^2=1\\r_4^2+r_5^2+r_6^2=1\\r_7^2+r_8^2+r_9^2=1\\r_1r_4+r_2r_5+r_3r_6=0\\r_1r_7+r_2r_8+r_3r_9=0\\r_4r_7+r_5r_8+r_6r_9=0\end{cases}\tag{6 constrainsts}
$$

$$
R^TR=I=\begin{bmatrix}r_1&r_4&r_7\\r_2&r_5&r_8\\r_3&r_6&r_9\end{bmatrix}\begin{bmatrix}r_1&r_2&r_3\\r_4&r_5&r_6\\r_7&r_8&r_9\end{bmatrix}\implies\begin{cases}r_1^2+r_4^2+r_7^2=1\\r_2^2+r_5^2+r_8^2=1\\r_3^2+r_6^2+r_9^2=1\\r_1r_2+r_4r_5+r_7r_8=0\\r_1r_3+r_4r_6+r_7r_9=0\\r_2r_3+r_5r_6+r_8r_9=0\end{cases}\tag{6 constrainsts}
$$

$$
\det(R)=1\iff R^{(i)}\times R^{(j)}=R^{(k)},\ i,j,k=\text{cyc}(1,2,3)\implies\begin{cases}r_5r_9-r_8r_6=r_1\\r_6r_7-r_9r_4=r_2\\r_4r_8-r_7r_5=r_3\\r_8r_3-r_2r_9=r_4\\r_9r_1-r_3r_7=r_5\\r_7r_2-r_1r_8=r_6\\r_2r_6-r_5r_3=r_7\\r_3r_4-r_6r_1=r_8\\r_1r_5-r_4r_2=r_9\end{cases}\tag{9 constraints}
$$

*homogeneize problem by auxiliary variable $y$ forms an equivalent, homogeneous, strengthened primal problem:

$$
\begin{aligned}\min_{\bar\tau\in\mathbb{R}^{10}}\ &\bar{\tau}^TQ\bar{\tau},\ \bar{\tau}=\begin{bmatrix}\text{vec}(R)\\y\end{bmatrix}\in\mathbb{R}^{10}\\\text{s.t. }\ &RR^T=y^2I_3&\text{(6 constraints)}\\&R^TR=y^2I_3&\text{(6 constraints)}\\&R^{(i)}\times R^{(j)}=yR^{(k)},\ i,j,k=\text{cyc}(1,2,3)&\text{(9 constraints)}\\&y^2=1&\text{(1 constraint)}\end{aligned}\tag{QCQP, primal problem}
$$

### Daul problem

by Lagrange multiplier:

$$
\mathcal{L}(\bar{\tau},\bar{\lambda})=\bar{\tau}^TQ\bar{\tau}+\Lambda_r(y^2I_3-RR^T)+\Lambda_c(y^2I_3-R^TR)+\sum_{i,j,k\in(1,2,3)}\lambda_{d_{ijk}}^T(R^{(i)}\times R^{(j)}-yR^{(k)})+\gamma(1-y^2)
$$

where $\bar{\lambda}\in\mathbb{R}^{22},\ \Lambda_r=\begin{bmatrix}\lambda_1&\lambda_6&\lambda_5\\\lambda_6&\lambda_2&\lambda_4\\\lambda_5&\lambda_4&\lambda_3\end{bmatrix},\ \Lambda_c=\begin{bmatrix}\lambda_7&\lambda_{12}&\lambda_{11}\\\lambda_{12}&\lambda_8&\lambda_{10}\\\lambda_{11}&\lambda_{10}&\lambda_9\end{bmatrix},\ \lambda_{d_{123}}=\begin{bmatrix}\lambda_{13}\\\lambda_{14}\\\lambda_{15}\end{bmatrix},\ \lambda_{d_{231}}=\begin{bmatrix}\lambda_{16}\\\lambda_{17}\\\lambda_{18}\end{bmatrix},\ \lambda_{d_{312}}=\begin{bmatrix}\lambda_{19}\\\lambda_{20}\\\lambda_{21}\end{bmatrix},\ \gamma=\lambda_{22}$

rewrite penalty terms with $\bar\tau$:

$$
\begin{aligned}\Lambda_r(y^2I_3-RR^T)&=\text{tr}(\Lambda_r^T(y^2I_3-RR^T))\\&=y^2\text{tr}(\Lambda_r)-\text{tr}(\Lambda_r^TRR^T)\\&=y^2\text{tr}(\Lambda_r)-\text{vec}(R)(I_3\otimes\Lambda_r)\text{vec}(R)\\&=\begin{bmatrix}\text{vec}(R)&y\end{bmatrix}\underbrace{\begin{bmatrix}-I_3\otimes\Lambda_r&0_{9\times1}\\0_{1\times0}&\text{tr}(\Lambda_r)\end{bmatrix}}_{P_r}\begin{bmatrix}\text{vec}(R)\\y\end{bmatrix}\\&=\bar\tau^TP_r\bar\tau\end{aligned}
$$

$$
\Lambda_c(y^2I_3-R^TR)=\begin{bmatrix}\text{vec}(R)&y\end{bmatrix}\underbrace{\begin{bmatrix}-\Lambda_c\otimes I_3&0_{9\times1}\\0_{1\times0}&\text{tr}(\Lambda_c)\end{bmatrix}}_{P_c}\begin{bmatrix}\text{vec}(R)\\y\end{bmatrix}=\bar\tau^TP_c\bar\tau
$$

$$
\lambda_{d_{ijk}}^T(R^{(i)}\times R^{(j)}-yR^{(k)})=\cdots=\begin{bmatrix}\text{vec}(R)&y\end{bmatrix}\underbrace{\begin{bmatrix}-e_{ij}\otimes\begin{bmatrix}\lambda_{d_{ijk}}\end{bmatrix}&-(e_k\otimes d_{ijk})\\0_{1\times9}&0\end{bmatrix}}_{P_{d_{ijk}}}\begin{bmatrix}\text{vec}(R)\\y\end{bmatrix}=\bar\tau^TP_{d_{ijk}}\bar\tau
$$

$$
\gamma(1-y^2)=\gamma-\gamma y^2=\gamma+\begin{bmatrix}\text{vec}(R)&y\end{bmatrix}\underbrace{\begin{bmatrix}0_{9\times9}&0_{9\times1}\\0_{1\times9}&-\gamma\end{bmatrix}}_{P_h}\begin{bmatrix}\text{vec}(R)\\y\end{bmatrix}=\gamma+\bar\tau^TP_h\bar\tau
$$


rewrite Lagrangian:

$$
\begin{aligned}\mathcal{L}(\bar{\tau},\bar{\lambda})&=\bar\tau^TQ\bar\tau+\bar\tau^TP_r\bar\tau+\bar\tau^TP_c\bar\tau+\sum_{i,j,k\in(1,2,3)}\bar\tau^TP_{d_{ijk}}\bar\tau+\bar\tau^TP_{h}\bar\tau+\gamma\\&=\gamma+\bar{\tau}^T\underbrace{(Q+P(\bar\lambda))}_Z\bar\tau\\&=\gamma+\bar\tau^TZ\bar\tau\end{aligned}
$$

where $P(\bar\lambda)=P_r+P_c+P_{d_{123}}+P_{d_{231}}+P_{d_{312}}+P_h$ is a quadratic function

dual function:

$$
d(\bar\lambda)=\inf_{\bar\tau}\mathcal{L}(\bar\tau,\bar\lambda)=\begin{cases}\gamma&\text{if }Z\succeq0\\-\infty&\text{otherwise}\end{cases}
$$

daul problem:

$$
\begin{aligned}\max_{\bar\lambda\in\mathbb{R}^{22}}\ &e_{22}^T\bar\lambda\\\text{s.t. }\ &Q+P(\bar\lambda)\succeq0\end{aligned}\tag{SDP}
$$

### recover

assume that the duality gap is 0

\begin{aligned}
f_0(\bar\tau^*)=d(\bar\lambda^*)&\implies \bar\tau^*=\inf_{\bar\tau}\mathcal{L}(\bar\tau,\bar\lambda)\\&\implies (\bar\tau^*)^TZ\bar\tau^*=0\\&\implies\bar\tau^*\in\text{null}(Z)
\end{aligned}

if $\text{rank}(\text{null}(Z))=1$, we recover $\bar\tau^*$ up to a scalar, determine $\bar\tau^*$ by setting $y=\bar\tau^*_{10}=1$

### rotation algorithm

1. solve SDP to get $\bar\lambda^*,Z=Q+P(\bar\lambda^*)$
2. compute $V=\text{null}(Z)$

assert $\text{rank}(V)=1$, i.e., $V=\text{span}\{v\}$

1. dehomogeneize $\bar\tau^*=\frac{v}{v_{10}}$
2. reshape $\bar\tau^*$ to $R$

> no theoretical guarantee, however, always resulted on a tight relaxation in practice
>