论文阅读: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
更新中…