🐇

TEASER++

Remark

Quaternion

qc=qa∘qb=Ω1(qa)qb=Ω2(qb)qa\boldsymbol{q}_{c}=\boldsymbol{q}_{a} \circ \boldsymbol{q}_{b}=\boldsymbol{\Omega}_{1}\left(\boldsymbol{q}_{a}\right) \boldsymbol{q}_{b}=\boldsymbol{\Omega}_{2}\left(\boldsymbol{q}_{b}\right) \boldsymbol{q}_{a}
Ω1(q)=[q4−q3q2q1q3q4−q1q2−q2q1q4q3−q1−q2−q3q4],Ω2(q)=[q4q3−q2q1−q3q4q1q2q2−q1q4q3−q1−q2−q3q4]\boldsymbol{\Omega}_{1}(\boldsymbol{q})=\left[\begin{array}{cccc} q_{4} & -q_{3} & q_{2} & q_{1} \\ q_{3} & q_{4} & -q_{1} & q_{2} \\ -q_{2} & q_{1} & q_{4} & q_{3} \\ -q_{1} & -q_{2} & -q_{3} & q_{4} \end{array}\right], \quad \boldsymbol{\Omega}_{2}(\boldsymbol{q})=\left[\begin{array}{cccc} q_{4} & q_{3} & -q_{2} & q_{1} \\ -q_{3} & q_{4} & q_{1} & q_{2} \\ q_{2} & -q_{1} & q_{4} & q_{3} \\ -q_{1} & -q_{2} & -q_{3} & q_{4} \end{array}\right]
q−1=[−vs]\boldsymbol{q}^{-1}=\left[\begin{array}{c} -\boldsymbol{v} \\ s \end{array}\right]
[Ra0]=q∘a^∘q−1\left[\begin{array}{c} \boldsymbol{R a} \\ 0 \end{array}\right]=\boldsymbol{q} \circ \hat{\boldsymbol{a}} \circ \boldsymbol{q}^{-1}

Problem formulation

bi=s∘R∘ai+t∘+oi+ϵi\boldsymbol{b}_{i}=s^{\circ} \boldsymbol{R}^{\circ} \boldsymbol{a}_{i}+\boldsymbol{t}^{\circ}+\boldsymbol{o}_{i}+\boldsymbol{\epsilon}_{i}
min⁡s>0,R∈SO(3),t∈R3∑i=1Nmin⁡(1βi2∥bi−sRai−t∥2,cˉ2)\min _{s>0, \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}} \sum_{i=1}^{N} \min \left(\frac{1}{\beta_{i}^{2}}\left\|\boldsymbol{b}_{i}-s \boldsymbol{R} \boldsymbol{a}_{i}-\boldsymbol{t}\right\|^{2}, \bar{c}^{2}\right)
βi is the upper bound of inlier noise ϵi. cˉ is 1, but reserved for discussion.\beta_i \textrm{~is the upper bound of inlier noise } \epsilon_i.~\bar{c}~\textrm{is 1, but reserved for discussion.}

Decoupling

Translation Invariant Measurements (TIMs)

bj−bi=sR(aj−ai)+(oj−oi)+(ϵj−ϵi)\boldsymbol{b}_{j}-\boldsymbol{b}_{i}=s \boldsymbol{R}\left(\boldsymbol{a}_{j}-\boldsymbol{a}_{i}\right)+\left(\boldsymbol{o}_{j}-\boldsymbol{o}_{i}\right)+\left(\boldsymbol{\epsilon}_{j}-\boldsymbol{\epsilon}_{i}\right)
b‾ij=sRa‾ij+oij+ϵij\overline{\boldsymbol{b}}_{i j}=s \boldsymbol{R} \overline{\boldsymbol{a}}_{i j}+\boldsymbol{o}_{i j}+\boldsymbol{\epsilon}_{i j}

Translation and Rotation Invariant Measurements (TRIMs)

∥b‾ij∥=∥sRa‾ij+oij+ϵij∥\left\|\overline{\boldsymbol{b}}_{i j}\right\|=\left\|s \boldsymbol{R} \overline{\boldsymbol{a}}_{i j}+\boldsymbol{o}_{i j}+\boldsymbol{\epsilon}_{i j}\right\|
∥b‾ij∥=∥sRa‾ij∥+o~ij+ϵ~ij\left\|\overline{\boldsymbol{b}}_{i j}\right\|=\left\|s \boldsymbol{R} \overline{\boldsymbol{a}}_{i j}\right\|+\tilde{\boldsymbol{o}}_{i j}+\tilde{\epsilon}_{i j}
sij=s+oijs+ϵijss_{i j}=s+o_{i j}^{s}+\epsilon_{i j}^{s}

TEASER

  1. TRIMs for s — TLS estimation
    s^=arg⁡min⁡s∑k=1Kmin⁡((s−sk)2αk2,cˉ2)\hat{s}=\underset{s}{\arg \min } \sum_{k=1}^{K} \min \left(\frac{\left(s-s_{k}\right)^{2}}{\alpha_{k}^{2}}, \bar{c}^{2}\right)
    • Algorithm: adpative voting, polynomial time, exact
    • The solution is trivial: enumerate, sort, and vote
  1. s + TIMs for R
    R^=arg⁡min⁡R∈SO(3)∑k=1Kmin⁡(∥b‾k−s^Ra‾k∥2δk2,cˉ2)\hat{\boldsymbol{R}}=\underset{\boldsymbol{R} \in \mathrm{SO}(3)}{\arg \min } \sum_{k=1}^{K} \min \left(\frac{\left\|\overline{\boldsymbol{b}}_{k}-\hat{s} \boldsymbol{R} \overline{\boldsymbol{a}}_{k}\right\|^{2}}{\delta_{k}^{2}}, \bar{c}^{2}\right)
    • Algorithm: Robust rotation search via tight semidefinite relaxation, polynomial time, exact
    min⁡q∈U3∑k=1Kmin⁡(∥b^k−q∘a^k∘q−1∥2δk2,cˉ2)\min _{\boldsymbol{q} \in \mathcal{U}^{3}} \sum_{k=1}^{K} \min \left(\frac{\left\|\hat{\boldsymbol{b}}_{k}-\boldsymbol{q} \circ \hat{\boldsymbol{a}}_{k} \circ \boldsymbol{q}^{-1}\right\|^{2}}{\delta_{k}^{2}}, \bar{c}^{2}\right)

    is equivalent with

    min⁡q∈U3θk={±1}∑k=1K1+θk2∥b^k−q∘a^k∘q−1∥2δk2+1−θk2cˉ2\min _{\boldsymbol{q} \in \mathcal{U}^{3} \atop \theta_{k}=\{\pm 1\}} \sum_{k=1}^{K} \frac{1+\theta_{k}}{2} \frac{\left\|\hat{\boldsymbol{b}}_{k}-\boldsymbol{q} \circ \hat{\boldsymbol{a}}_{k} \circ \boldsymbol{q}^{-1}\right\|^{2}}{\delta_{k}^{2}}+\frac{1-\theta_{k}}{2} \bar{c}^{2}

    can be rewritten with

    min⁡q∈U3qk={±q}k=1∥b^k−q∘a^k∘q−1+q⊤qkb^k−q∘a^k∘qk−1∥24δk2+1−q⊤qk2cˉ2\min _{\boldsymbol{q} \in \mathcal{U}^{3} \atop \boldsymbol{q}_{k}=\{\pm \boldsymbol{q}\}^{k}=1} \frac{\left\|\hat{\boldsymbol{b}}_{k}-\boldsymbol{q} \circ \hat{\boldsymbol{a}}_{k} \circ \boldsymbol{q}^{-1}+\boldsymbol{q}^{\top} \boldsymbol{q}_{k} \hat{\boldsymbol{b}}_{k}-\boldsymbol{q} \circ \hat{\boldsymbol{a}}_{k} \circ \boldsymbol{q}_{k}^{-1}\right\|^{2}}{4 \delta_{k}^{2}} + \frac{1-\boldsymbol{q}^{\top} \boldsymbol{q}_{k}}{2} \bar{c}^{2}

    with

    qk≐θkq\boldsymbol{q}_{k} \doteq \theta_{k} \boldsymbol{q}

    Now q_k = q for inliers and -q for outliers. It is equivalent to Quadratically-Constrained Quadratic Program:

    min⁡x∈R4(K+1)x⊤Qxs.t.xq⊤xq=1xqkxqk⊤=xqxq⊤,∀i=1,…,K\begin{array}{ll} \min _{\boldsymbol{x} \in \mathbb{R}^{4(K+1)}} & \boldsymbol{x}^{\top} \boldsymbol{Q} \boldsymbol{x} \\ \text {s.t.} & \boldsymbol{x}_{q}^{\top} \boldsymbol{x}_{q}=1 \\ & \boldsymbol{x}_{q_{k}} \boldsymbol{x}_{q_{k}}^{\top}=\boldsymbol{x}_{q} \boldsymbol{x}_{q}^{\top}, \forall i=1, \ldots, K \end{array}

    Q depends on TIMs. Reformat Z with x,

    Z=xx⊤=[qq⊤qq1⊤⋯qqK⊤⋆q1q1⊤⋯q1qK⊤⋮⋮⋱⋮⋆⋆⋯qKqK⊤]∈S+4(K+1)\boldsymbol{Z}=\boldsymbol{x} \boldsymbol{x}^{\top}=\left[\begin{array}{cccc} \boldsymbol{q} \boldsymbol{q}^{\top} & \boldsymbol{q} \boldsymbol{q}_{1}^{\top} & \cdots & \boldsymbol{q} \boldsymbol{q}_{K}^{\top} \\ \star & \boldsymbol{q}_{1} \boldsymbol{q}_{1}^{\top} & \cdots & \boldsymbol{q}_{1} \boldsymbol{q}_{K}^{\top} \\ \vdots & \vdots & \ddots & \vdots \\ \star & \star & \cdots & \boldsymbol{q}_{K} \boldsymbol{q}_{K}^{\top} \end{array}\right] \in \mathcal{S}_{+}^{4(K+1)}

    this turns to

3. d + R for t

t^j=arg⁡min⁡tj∑i=1Nmin⁡((tj−[bi−s^R^ai]j)2βi2,cˉ2)\hat{t}_{j}=\underset{t_{j}}{\arg \min } \sum_{i=1}^{N} \min \left(\frac{\left(t_{j}-\left[\boldsymbol{b}_{i}-\hat{s} \hat{\boldsymbol{R}} \boldsymbol{a}_{i}\right]_{j}\right)^{2}}{\beta_{i}^{2}}, \bar{c}^{2}\right)

Boost performance