论文阅读：SuMa++

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

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

语义ICP部分

公式如下：

E(VD,VM,NM)=∑u∈VDωunuT(TCt−1Ct(k)u−vu)2E(\mathcal{V}_D,\mathcal{V}_M,\mathcal{N}_M)=\sum\limits_{\mathbf{u}\in V_D}\omega_\mathbf{u}\mathbf{n_u}^T\biggl(T_{C_{t-1}C_t}^{(k)}u-\mathbf{v}_\mathbf{u}\biggr)^2E(VD,VM,NM)=u∈VD∑ωunuT(TCt−1Ct(k)u−vu)2

其中残差 ru=nuT(TCt−1Ct(k)u−vu)2r_{\mathbf{u}}=\mathbf{n_u}^T\biggl(T_{C_{t-1}C_t}^{(k)}u-\mathbf{v}_\mathbf{u}\biggr)^2ru=nuT(TCt−1Ct(k)u−vu)2 为ICP算法部分, ωu\omega_\mathbf{u}ωu 为语义权重

其中每个顶点 u∈VD\mathbf{u}\in V_Du∈VD 与其参考顶点 vu∈VM\mathbf{v}_\mathbf{u}\in \mathcal{V}_Mvu∈VM 和它的法向量 nu∈NM\mathbf{n}_\mathbf{u}\in \mathcal{N}_Mnu∈NM 关联。

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

vu=VM(Π(TCt−1Ct(k)u))nu=NM(Π(TCt−1Ct(k)u))\begin{aligned} \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{aligned}vunu=VM(Π(TCt−1Ct(k)u))=NM(Π(TCt−1Ct(k)u))

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

δ=(J⊤WJ)−1J⊤Wr\delta=\left(\mathbf{J}^{\top} \mathbf{W} \mathbf{J}\right)^{-1} \mathbf{J}^{\top} \mathbf{W} \mathbf{r}δ=(J⊤WJ)−1J⊤Wr

其中 W∈Rn×n\mathbf{W} \in \mathbb{R}^{n \times n}W∈Rn×n 是由与 rur_{\mathbf{u}}ru 对应的wuw_{\mathbf{u}}wu 组成的，r∈Rn\mathbf{r}\in\mathbb{R}^nr∈Rn rur_{\mathbf{u}}ru 的向量，

J∈Rn×6\mathbf{J}\in\mathbb{R}^{n \times 6}J∈Rn×6 是 r∈Rn\mathbf{r}\in \mathbb{R}^nr∈Rn 的Jacobian。

W\mathbf{W}W 中的 wuw_{\mathbf{u}}wu 的计算是这样的：

wu(k)=ρHuber (ru(k))Csemantic (SD(u),SM(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)wu(k)=ρHuber (ru(k))Csemantic (SD(u),SM(u))

使用了Huber:
ρHuber (r)={1,if ∣r∣<δδ∣r∣−1,otherwise \rho_{\text {Huber }}(r)=\left\{\begin{array}{cl}1 & , \text { if }|r|<\delta \\ \delta|r|^{-1} & , \text {otherwise }\end{array}\right.ρHuber (r)={1δ∣r∣−1, if ∣r∣<δ,otherwise

Csemantic ((yu,Pu),(yvu,Pvu))C_{\text {semantic }}\left(\left(y_{\mathbf{u}}, P_{\mathbf{u}}\right),\left(y_{v_{\mathbf{u}}}, P_{v_{\mathbf{u}}}\right)\right)Csemantic ((yu,Pu),(yvu,Pvu))

Csemantic (⋅,⋅)={P(yu∣u),if yu=yvu1−P(yu∣u),otherwise C_{\text {semantic }}(\cdot, \cdot)=\left\{\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}\right.Csemantic (⋅,⋅)={P(yu∣u)1−P(yu∣u), if yu=yvu, otherwise

其中 yuy_\mathbf{u}yu 对应 u\mathbf{u}u 的label，yvuy_{v_u}yvu 对应 vuv_\mathbf{u}vu 的label

更新中…