論文筆記 Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection

# Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection H. Yang, P. Antonante, V. Tzoumas and L. Carlone, *Graduated Non-Convexity for Robust Spatial Perception: From Non-Minimal Solvers to Global Outlier Rejection*, IEEE Robotics and Automation Letters, vol. 5, no. 2, pp. 1127-1134, 2020. [TOC] ## Abstract > 問題： > - non-minimal solvers rely on least squares, but are brittle against outliers > - apply robust cost function, but robust costs introduce non-convexity > > 提出GNC，同時使用robust cost與non-minimal solver > - does not require initial guess > - global optimality is not guaranteed SDP and Sums-of-Squares relaxations have led to non-minimal solvers, however, most non-minimal solvers rely on **least squares and are brittle against outliers**. While a standard approach is to use robust cost functions, the latter typically **introduce other non-convexities**, preventing the use of existing non-minimal solvers. We enable the **simultaneous use of non-minimal solvers and robust estimation** by providing a general-purpose approach for robust global estimation. We leverage the Black-Rangarajan duality between robust estimation and outlier processes, and show that GNC can be used in conjunction with non-minimal solvers to compute robust solutions, without requiring an initial guess. Although GNC’s global optimality cannot be guaranteed, we demonstrate the empirical robustness of the resulting robust non-minimal solvers in applications, including PGO. ### Robust cost > M-estimator least squares ($\mathbf{x}$: state, $\mathbf{y}$: measurement, $r$: residual) $$ \min_{\mathbf{x}\in \mathcal{X}}\sum_{i=1}^Nr^2(\mathbf{x,y}_i) $$ least squares for **outliers** ($\rho$: robust cost) $$ \min_{\mathbf{x}\in \mathcal{X}}\sum_{i=1}^N\rho(r(\mathbf{x,y}_i)) $$ Ex. **convex** Huber loss, **non-convex** truncated least squares, Geman-McClure - non-minimal solver can include convex robust cost - rare robust non-minimal solvers  ## Method ### Black-Rangarajan Duality given a robust cost function $\rho$, define $\phi(z)=\rho(\sqrt{z})$. If $\phi(z)$ satisfies $\lim_{z\rightarrow0}\phi^\prime(z)=1$, $\lim_{z\rightarrow\infty}\phi^\prime(z)=0$ and $\phi^{\prime\prime}(z)<0$, then $$ \min_{\mathbf{x}\in \mathcal{X}}\sum_{i=1}^N\rho(r(\mathbf{x,y}_i))=\min_{\mathbf{x}\in \mathcal{X},w_i\in{[0,1]}}\sum_{i=1}^N[w_ir^2(\mathbf{x,y}_i)+\Phi_\rho(w_i)] $$ where $w_i\in[0,1]$ are weights and $\Phi_\rho(w_i)$ defines a penalty on $w_i$. > SLAM: without acknowledging the connection with robust estimation, i.e., penalty term Black-Rangarajan provide a procedure to compute $\Phi_\rho(w_i)$ (step 1) Define $\phi(z)=\rho(\sqrt{z/\tau})$ where $z$ is possibly scaled by $\tau$ in $\rho$. (step 2) Compute $\phi^\prime(z)$ and $\phi^{\prime\prime}(z)$. (check) If satisfies $\lim_{z\rightarrow0}\phi^\prime(z)=1$, $\lim_{z\rightarrow\infty}\phi^\prime(z)=0$ and $\phi^{\prime\prime}(z)<0$, proceed. (Otherwise, $\rho$ does not have a simple outlier process formulation.) (step 3) Solve $w=\phi^\prime(z)$ to get inverse function $z=(\phi^\prime)^{-1}(w)$. (step 4) Define $\Phi(w)=\phi(z)-wz=\phi((\phi^\prime)^{-1}(w))-w(\phi^\prime)^{-1}(w)$. (step 5) New objective function $\tau wr^2+\Phi(w)$. > 在M-estimator裡$w=\phi^\prime(x)/x$ ### Graduated non-convexity surrogate cost $\rho_\mu$, $\mu$ is the control parameter - for a certain $\mu$, the surrogate function $\rho_\mu$ is convex - for a certain $\mu$, the surrogate function $\rho_\mu$ recovers the original non-convex $\rho$ start from the convex surrogate, gradually recover to the non-convex function Ex. Geman-McClure $$ \rho(r)=\frac{\bar{c}^2r^2}{\bar{c}^2+r^2}\implies\rho_\mu(r)=\frac{\mu\bar{c}^2r^2}{\mu\bar{c}^2+r^2} $$ where $\mu\rightarrow\infty$ is convex and $\mu=1$ is the original function Ex. Truncated Least Squares $$ \rho(r)=\begin{cases}r^2&\text{if }r^2\in[0,\bar{c}^2]\\\bar{c}^2&\text{if }r^2\in[\bar{c}^2,\infty]\end{cases}\implies\rho_\mu(r)=\begin{cases}r^2&\text{if }r^2\in[0,\frac{\mu}{\mu+1}\bar{c}^2]\\2\bar{c}|r|\sqrt{\mu(\mu+1)}-\mu(\bar{c}^2+r^2)&\text{if }r^2\in[\frac{\mu}{\mu+1}\bar{c}^2,\frac{\mu+1}{\mu}\bar{c}^2]\\\bar{c}^2&\text{if }r^2\in[\frac{\mu+1}{\mu}\bar{c}^2,\infty]\end{cases} $$ where $\mu\rightarrow0$ is convex and $\mu\rightarrow\infty$ is the original function ![gnc](https://hackmd.io/_uploads/ByjMJmJrT.gif) ### O**uter iteration** fix $\mu$ and optimize: $$ \min_{\mathbf{x}\in \mathcal{X}}\sum_{i=1}^N\rho_\mu(r(\mathbf{x,y}_i))=\min_{\mathbf{x}\in \mathcal{X},w_i\in{[0,1]}}\sum_{i=1}^N[w_ir^2(\mathbf{x,y}_i)+\Phi_{\rho_\mu}(w_i)] $$ we will discuss solving $\Phi_{\rho_\mu}(w_i)$ later by examples of GM & TLS ### Inner iteration first optimize over $\mathbf{x}$ (fixed $w_i$), then optimize over all $w_i$ (fixed $\mathbf{x}$) **variable update** optimize over $\mathbf{x}$ (fixed $w_i$) $$ \mathbf{x}^{(t)}=\min_{\mathbf{x}\in \mathcal{X}}\sum_{i=1}^Nw_i^{(t-1)}r^2(\mathbf{x,y}_i) $$ omit penalty term since it does not depend on $\mathbf{x}$, solved with non-minimal solvers **weight update** optimize over $\mathbf{w}$ (fixed $\mathbf{x}$) $$ \mathbf{w}^{(t)}=\min_{w_i\in{[0,1]}}\sum_{i=1}^N[w_ir^2(\mathbf{x}^{(t)},\mathbf{y}_i)+\Phi_{\rho_\mu}(w_i)] $$ since $r^2(\mathbf{x}^{(t)},\mathbf{y}_i)$ is constant, solved in closed form the process is repeated for changing $\mu$ and increasing the amount of non-convexity ![image](https://hackmd.io/_uploads/rkfOJm1Ha.png =500x) ## German-McClure example **Black-Rangarajan duality** $$ \rho_\mu(r)=\frac{\mu\bar{c}^2r^2}{\mu\bar{c}^2+r^2} $$ (step 1) $$ \phi(z)=\rho(\sqrt{z})=\frac{\mu\bar{c}^2z}{\mu\bar{c}^2+z} $$ (step 2, check) $$ \phi^\prime(z)=(\frac{\mu\bar{c}^2}{\mu\bar{c}^2+z})^2,\ \ \phi^{\prime\prime}(z)=-2\Big(\frac{\mu\bar{c}^2}{\mu\bar{c}^2+z}\Big)^3 $$ (step 3) Solve $(\phi^\prime)^{-1}:[0,1]\rightarrow \mathbb{R}$ $$ w=\phi^\prime(z)=(\frac{\mu\bar{c}^2}{\mu\bar{c}^2+z})^2\implies z=(\phi^\prime)^{-1}(w)=\frac{\mu\bar{c}^2}{\sqrt{w}}-\mu\bar{c}^2 $$ (step 4) Define $\Phi(w)=\phi(z)-wz$ $$ \begin{aligned}\Phi(w)&=\frac{\mu\bar{c}^2z}{\mu\bar{c}^2+z}-wz\\[10pt]&=\frac{\frac{(\mu\bar{c}^2)^2}{\sqrt{w}}-(\mu\bar{c}^2)^2}{\frac{\mu\bar{c}^2}{\sqrt{w}}}-\sqrt{w}\mu\bar{c}^2-w\mu\bar{c}^2\\[20pt]&=(\mu\bar{c}^2)^2-2\sqrt{w}\mu\bar{c}^2+w\mu\bar{c}^2=\mu\bar{c}^2(\sqrt{w}-1)^2\end{aligned} $$ (step 5) New objective function $wr^2+\Phi(w)$ $$ \rho_\mu(r)=wr^2+\mu\bar{c}^2(\sqrt{w}-1)^2 $$ **variable update** solve with non-minimal solvers such as weighted least squares $$ \mathbf{x}^{(t)}=\min_{\mathbf{x}\in \mathcal{X}}\sum_{i=1}^Nw_i^{(t-1)}r^2 $$ **weight update** derivative with respect to $w^{(t)}$, we get the closed form of $w^{(t)}$ $$ \nabla_w\rho_\mu(r)=r^2+\mu\bar{c}^2(\sqrt{w}-1)w^{-\frac{1}{2}}=r^2+\mu\bar{c}^2-\mu\bar{c}^2w^{-\frac{1}{2}}=0\implies w^{(t)}=\Big(\frac{\mu\bar{c}^2}{r^2+\mu\bar{c}^2}\Big)^2 $$ **others** - initialize $\mu=2(\max_i r^2(\mathbf{x}_0,\mathbf{y}_i))/\bar{c}^2$ - update $\mu\leftarrow\mu/1.4$ - stop when $\mu<1$ - choose $\bar{c}$ based on remark 1-2 in [2] ## Truncated Least Squares example **Black-Rangarajan duality** $$ \rho_\mu(r)=\begin{cases}r^2&\text{if }r^2\in[0,\frac{\mu}{\mu+1}\bar{c}^2]\\2\bar{c}|r|\sqrt{\mu(\mu+1)}-\mu(\bar{c}^2+r^2)&\text{if }r^2\in[\frac{\mu}{\mu+1}\bar{c}^2,\frac{\mu+1}{\mu}\bar{c}^2]\\\bar{c}^2&\text{if }r^2\in[\frac{\mu+1}{\mu}\bar{c}^2,\infty]\end{cases} $$ (step 1) $$ \phi(z)=\rho(\sqrt{z})=\begin{cases}z&\text{if }z\in[0,\frac{\mu}{\mu+1}\bar{c}^2]\\2\bar{c}\sqrt{z\mu(\mu+1)}-\mu(\bar{c}^2+z)&\text{if }z\in [\frac{\mu}{\mu+1}\bar{c}^2,\frac{\mu+1}{\mu}\bar{c}^2]\\\bar{c}^2&\text{if }z\in [\frac{\mu+1}{\mu}\bar{c}^2,\infty]\end{cases} $$ (step 2, check) $$ \phi^\prime(z)=\begin{cases}1&\text{if }z\in [0,\frac{\mu}{\mu+1}\bar{c}^2]\\\bar{c}\sqrt{\mu(\mu+1)}\cdot z^{-\frac{1}{2}}-\mu&\text{if }z\in [\frac{\mu}{\mu+1}\bar{c}^2,\frac{\mu+1}{\mu}\bar{c}^2]\\0&\text{if }z\in [\frac{\mu+1}{\mu}\bar{c}^2,\infty]\end{cases},\ \ \phi^{\prime\prime}(z)=-\frac{1}{2}\bar{c}\sqrt{\mu(\mu+1)}\cdot z^{-\frac{3}{2}},\ z\in [\frac{\mu}{\mu+1}\bar{c}^2,\frac{\mu+1}{\mu}\bar{c}^2] $$ (step 3) Solve $(\phi^\prime)^{-1}:[0,1]\rightarrow [\frac{\mu}{\mu+1}\bar{c}^2,\frac{\mu+1}{\mu}\bar{c}^2]$ $$ w=\phi^\prime(z)=\bar{c}\sqrt{\mu(\mu+1)}\cdot z^{-\frac{1}{2}}-\mu\implies z=(\phi^\prime)^{-1}(w)=\frac{\bar{c}^2\mu(\mu+1)}{(w+\mu)^2} $$ (step 4) Define $\Phi(w)=\phi(z)-wz$ $$ \begin{aligned}\Phi(w)&=2\bar{c}\sqrt{z\mu(\mu+1)}-\mu(\bar{c}^2+z)-wz\\[6pt]&=2\bar{c}^2\frac{\mu(\mu+1)}{(w+\mu)}-\mu(\bar{c}^2+\frac{\bar{c}^2\mu(\mu+1)}{(w+\mu)^2})-\frac{w\bar{c}^2\mu(\mu+1)}{(w+\mu)^2}\\&=\frac{\mu(1-w)}{w+\mu}\bar{c}^2\end{aligned} $$ (step 5) New objective function $wr^2+\Phi(w)$ $$ \rho_\mu(r)=\begin{cases}r^2&\text{if }r^2\in[0,\frac{\mu}{\mu+1}\bar{c}^2]\\wr^2+\frac{\mu(1-w)}{w+\mu}\bar{c}^2&\text{if }r^2\in[\frac{\mu}{\mu+1}\bar{c}^2,\frac{\mu+1}{\mu}\bar{c}^2]\\\bar{c}^2&\text{if }r^2\in[\frac{\mu+1}{\mu}\bar{c}^2,\infty]\end{cases} $$ **variable update** solve with non-minimal solvers such as weighted least squares $$ \mathbf{x}^{(t)}=\min_{\mathbf{x}\in \mathcal{X}}\sum_{i=1}^Nw_i^{(t-1)}r^2 $$ **weight update** derivative with respect to $w^{(t)}$ $$ \nabla_w\rho_\mu(r)=r^2+\bar{c}^2\frac{-\mu(w^{(t)}+\mu)-\mu(1-w^{(t)})}{(w^{(t)}+\mu)^2}=r^2-\frac{\mu(1+\mu)}{(w^{(t)}+\mu)^2}\bar{c}^2=0\implies w^{(t)}=\frac{\sqrt{\mu(1+\mu)}}{r}\bar{c}-\mu $$ Note that $w^{(t)}=1\iff\rho_\mu(r)=r^2$ and $w^{(t)}=0\iff\rho_\mu(r)=\bar{c}^2$, we get the closed form of $w^{(t)}$ $$ w^{(t)}=\begin{cases}1&\text{if }r^2\in[0,\frac{\mu}{\mu+1}\bar{c}]\\\frac{\sqrt{\mu(1+\mu)}}{r}\bar{c}-\mu&\text{if }r^2\in[\frac{\mu}{\mu+1}\bar{c}^2,\frac{\mu+1}{\mu}\bar{c}^2]\\0&\text{if }r^2\in[\frac{\mu+1}{\mu}\bar{c}^2,\infty]\end{cases} $$ **others** - initialize $\mu=\bar{c}^2/(2(\max_i r^2(\mathbf{x}_0,\mathbf{y}_i))-\bar{c}^2)$ - update $\mu\leftarrow1.4\mu$ - stop when the weighted sum of squares $\sum_{i=1}^Nw_ir^2$ converges - choose $\bar{c}$ based on remark 1-2 in [2] [1] Convex relaxations for pose graph optimization with outliers (IEEE RA-L, 2018) [2] A quaternion-based certifiably optimal solution to the Wahba problem with outliers (ICCV, 2019) ## Open Source https://github.com/MIT-SPARK/GNC-and-ADAPT