论文阅读：SuMa++

论文阅读：SuMa++

论文：《SuMa++: Efficient LiDAR-based Semantic SLAM》

 SuMa++是建立在SuMa基础上的方法，该方法使用了基于RangeNet++的语义分割技术，在ICP的基础上增加的语义约束。
发布本文的时候，网上还没有找到对SuMa++语义ICP的翻译讲解的文章。于是自己翻译出来，以供同样需要语义ICP算法的同学参考。

语义ICP部分

公式如下：

$$E({\mathcal{V}}_D,{\mathcal{V}}_M,{\mathcal{N}}_M)=\sum _{\mathbf{u}\in V_D}{\omega }_{\mathbf{u}}{{\mathbf{n}}_{\mathbf{u}}}^T(T_{C_{t-1}{C}_t}^{(k)}u-{\mathbf{v}}_{\mathbf{u}}{)}^2$$

其中残差 $r_{\mathbf{u}}={{\mathbf{n}}_{\mathbf{u}}}^T(T_{C_{t-1}{C}_t}^{(k)}u-{\mathbf{v}}_{\mathbf{u}}{)}^2$ 为ICP算法部分, ${\omega }_{\mathbf{u}}$ 为语义权重

其中每个顶点 $\mathbf{u}\in V_D$ 与其参考顶点 ${\mathbf{v}}_{\mathbf{u}}\in {\mathcal{V}}_M$ 和它的法向量 ${\mathbf{n}}_{\mathbf{u}}\in {\mathcal{N}}_M$ 关联。

$\mathbf{u}$ 对应的${\mathbf{v}}_{\mathbf{u}}$ 和${\mathbf{n}}_{\mathbf{u}}$ 为：

$$\begin{array}{rl}{\mathbf{v}}_{\mathbf{u}}& ={\mathcal{V}}_M\left(\Pi \left({\mathbf{T}}_{C_{t-1}{C}_t}^{(k)}\mathbf{u}\right)\right)\\ {\mathbf{n}}_{\mathbf{u}}& ={\mathcal{N}}_M\left(\Pi \left({\mathbf{T}}_{C_{t-1}{C}_t}^{(k)}\mathbf{u}\right)\right)\end{array}$$

为了实现最小化，我们使用高斯牛顿，定义增量 $\delta$ 如下：

$$\delta ={\left({\mathbf{J}}^{\top }\mathbf{W}\mathbf{J}\right)}^{-1}{\mathbf{J}}^{\top }\mathbf{W}\mathbf{r}$$

其中 $\mathbf{W}\in {\mathbb{R}}^{n\times n}$ 是由与 $r_{\mathbf{u}}$ 对应的$w_{\mathbf{u}}$ 组成的，$\mathbf{r}\in {\mathbb{R}}^n$ $r_{\mathbf{u}}$ 的向量，

$\mathbf{J}\in {\mathbb{R}}^{n\times 6}$ 是 $\mathbf{r}\in {\mathbb{R}}^n$ 的Jacobian。

$\mathbf{W}$ 中的 $w_{\mathbf{u}}$ 的计算是这样的：

$$w_{\mathbf{u}}^{(k)}={\rho }_{\text{Huber }}\left(r_{\mathbf{u}}^{(k)}\right)C_{\text{semantic }}\left({\mathcal{S}}_D(\mathbf{u}),{\mathcal{S}}_M(\mathbf{u})\right)$$

使用了Huber:
$${\rho }_{\text{Huber }}(r)=\{\begin{array}{cl}1& ,\text{ if }|r|<\delta \\ \delta |r{|}^{-1}& ,\text{otherwise }\end{array}$$

$$C_{\text{semantic }}\left(\left(y_{\mathbf{u}},P_{\mathbf{u}}\right),\left(y_{v_{\mathbf{u}}},P_{v_{\mathbf{u}}}\right)\right)$$

$$C_{\text{semantic }}(\cdot ,\cdot )=\{\begin{array}{cl}P\left(y_{\mathbf{u}}\mid \mathbf{u}\right)& ,\text{ if }{y}_{\mathbf{u}}=y_{v_{\mathbf{u}}}\\ 1-P\left(y_{\mathbf{u}}\mid \mathbf{u}\right)& ,\text{ otherwise }\end{array}$$

其中 $y_{\mathbf{u}}$ 对应 $\mathbf{u}$ 的label，$y_{v_u}$ 对应 $v_{\mathbf{u}}$ 的label

更新中…