DROID-SLAM

简介

DROID-SLAM 是一种基于深度学习的实时单目 SLAM（Simultaneous Localization and Mapping）系统。它结合了传统 SLAM 方法和深度神经网络的优势，能够在动态环境中实现高精度的相机定位和三维地图构建。相比于后来的MASt3R-SLAM，它能够保证全局一致性，本文重点关注后端优化部分。

高斯牛顿法

$$\begin{aligned} &\arg\min_x f(x) \\ f(x)&=\frac{1}{2}\sum_i^m r_i(x)^2=\frac{1}{2}\|r(x)\|^2 \end{aligned}$$

一阶泰勒展开：

$$\begin{aligned} r(x+\Delta x) &\approx r(x)+J(x)\Delta x \\ J(x)&=\frac{\partial r(x)}{\partial x} \end{aligned}$$

近似后的优化问题：

$$\begin{aligned} &\arg\min_{\Delta x} \frac{1}{2}\|r(x)+J(x)\Delta x\|^2 \\ f(x+\Delta x) &\approx \frac{1}{2}(r+J\Delta x)^T(r+J\Delta x) \\ &=\frac{1}{2}(r^Tr + r^TJ\Delta x + \Delta x^TJ^Tr + \Delta x^TJ^TJ\Delta x) \end{aligned}$$

其中：$r^TJ\Delta x+\Delta x^TJ^Tr=2 r^TJ\Delta x$

定理：

$$\begin{aligned} \frac{\partial}{\partial x}(\frac{1}{2}x^TAx)&= Ax \\ (C^Tx)'&=C \end{aligned}$$

令：

$$\frac{\partial f}{\partial\Delta x}=J^Tr+J^TJ\Delta x=0$$

可得正规方程：

$$J^TJ\Delta x=-J^Tr$$

迭代公式：

$$\Delta x=-(J^TJ)^{-1}J^Tr$$

其中，$H=J^TJ$ 称为近似的 Hessian 矩阵，$g=J^Tr$ 称为梯度。

DROID-SLAM 后端优化

原帧$i$，目标帧$j$，坐标系$\mathcal{F_i},\mathcal{F_j}$。

帧$i$中的3D点：

$$X_i= \begin{bmatrix} x_i\\y_i\\z_i\\d_i \end{bmatrix} =\begin{bmatrix} (u-c_x)/f_x\\(v-c_y)/f_y\\1\\d_i \end{bmatrix}$$

$d_i$为逆深度值，使用齐次坐标的好处是能够线性表示：

$$\begin{bmatrix}X\\Y\\Z \end{bmatrix}_{world} =\frac{1}{d_i}\begin{bmatrix} x_i\\y_i\\z_i \end{bmatrix}$$

使用SE3变换：

$$\begin{aligned} T_{ij}&=\begin{bmatrix} R_{ij} & t_{ij} \\ 0 & 1 \end{bmatrix}\in SE(3) \\ X_j&=\begin{bmatrix} x_j\\y_j\\z_j\\d_j \end{bmatrix}= \begin{bmatrix} R_{ij}X_i+d_it_{ij}\\d_i \end{bmatrix} \end{aligned}$$

可以得到：

$$\left\{ \begin{matrix} x_j=R_{ij}x_i+d_it_x\\ y_j=R_{ij}y_i+d_it_y\\ z_j=R_{ij}z_i+d_it_z\\ d_j=d_i \end{matrix} \right.$$

注：$d_j,d_i$仅为占位变量

归一化到平面：

$$\left\{ \begin{matrix} \overline{x}=\frac{x_i}{z_i}=\frac{x_j}{z_j}\\ \overline{y}=\frac{y_i}{z_i}=\frac{y_j}{z_j} \end{matrix} \right.$$

由相机内参得到像素坐标：

$$\left\{ \begin{matrix} \hat{u}=f_x\overline{x}+c_x\\ \hat{v}=f_y\overline{y}+c_y \end{matrix} \right.$$

定义投影函数$\pi$：

$$\pi(T_{ij},d_i,u,v)= \begin{bmatrix} \hat{u}\\\hat{v} \end{bmatrix}$$

要求的变量为：

$$\frac{\partial \pi}{\partial T_{ij}},\frac{\partial \pi}{\partial d_i}$$

SE3李代数参数化

$$\begin{aligned} \zeta&= \begin{bmatrix} \tau\\\phi \end{bmatrix}= \begin{bmatrix} \tau_x\\\tau_y\\\tau_z\\\phi_x\\\phi_y\\\phi_z \end{bmatrix}\in \mathbb{R}^6 \\ T_{ij} &= \exp(\hat{\zeta}) \\ \hat{\zeta}&\in \mathbb{R}^{4\times4} \end{aligned}$$

$\hat{\zeta}$为$\zeta$的反对称矩阵表示,$\exp(\hat{\zeta})$为矩阵指数映射，代表旋转平移矩阵。

在当前变换$T_{ij}$中加一个扰动：

$$T_{ij}(\zeta)=\exp(\hat{\zeta})T_{ij}$$

求：

$$\left. \frac{\partial}{\partial \boldsymbol{\xi}} \right|_{\boldsymbol{\xi}=\mathbf{0}} \left[ \exp(\boldsymbol{\xi}^\wedge) \mathbf{T}_{ij} \cdot \mathbf{X}_i \right]$$

1. 投影对3D点的导数

$$\frac{\partial \hat{u}}{\partial X_j}=\frac{\partial}{\partial X_j}(f_x\frac{x_j}{z_j}+c_x)$$

可以得到：

$$\begin{aligned} \frac{\partial \hat{u}}{\partial x_j}&=\frac{f_x}{z_j},\frac{\partial \hat{u}}{\partial y_j}=0,\frac{\partial \hat{u}}{\partial z_j}=-f_x\frac{x_j}{z_j^2},\frac{\partial \hat{u}}{\partial d_j}=0 \\ \frac{\partial \hat{v}}{\partial x_j}&=0,\frac{\partial \hat{v}}{\partial y_j}=\frac{f_y}{z_j},\frac{\partial \hat{v}}{\partial z_j}=-f_y\frac{y_j}{z_j^2},\frac{\partial \hat{v}}{\partial d_j}=0 \end{aligned}$$

2. 3D点对SE3的导数

$$\frac{\partial X_j}{\partial \zeta}=\frac{\partial}{\partial \zeta}[\exp(\hat{\zeta})T_{ij}X_i]$$

根据李代数的性质，可以得到：

$$\frac{\partial [x_j,y_j,z_j]^T}{\partial \zeta}= \begin{bmatrix} -d_jI_3 & [x_j]_\times \\ \end{bmatrix} \in \mathbb{R}^{3\times6}, \zeta=[\tau,\phi]$$

$[x_j]_\times$为$x_j$的叉乘矩阵：

$$[x_j]_\times=\begin{bmatrix} 0 & -z_j & y_j \\ z_j & 0 & -x_j \\ -y_j & x_j & 0 \end{bmatrix}$$

3. 链式法则组合：

$$\begin{aligned} \frac{\partial \hat{u}}{\partial \zeta}&= \begin{bmatrix} \frac{\partial \hat{u}}{\partial x_j} & \frac{\partial \hat{u}}{\partial y_j} & \frac{\partial \hat{u}}{\partial z_j} \end{bmatrix} \frac{\partial [x_j,y_j,z_j]^T}{\partial \zeta}\\&= \begin{bmatrix} \frac{f_x}{z_j} & 0 & -f_x\frac{x_j}{z_j^2} \end{bmatrix} \begin{bmatrix} -d_j&0&0&0&-z_j&y_j\\ 0&-d_j&0&z_j&0&-x_j\\ 0&0&-d_j&-y_j&x_j&0 \end{bmatrix}\\&= \begin{bmatrix} -f_x\frac{d_i}{z_j}\\ 0\\ f_x\frac{x_j d_i}{z_j^2}\\ f_x\frac{x_j y_j}{z_j^2}\\ -f_x\left(1+\frac{x_j^2}{z_j^2}\right)\\ f_x\frac{y_j}{z_j} \end{bmatrix}^T \end{aligned}$$

由于残差$r=u_{target}-\hat{u}$，所以：

$$\begin{aligned} \frac{\partial r_u}{\partial \zeta}&=-\frac{\partial \hat{u}}{\partial \zeta} \\ \frac{\partial r_v}{\partial \zeta}&=-\frac{\partial \hat{v}}{\partial \zeta} \end{aligned}$$

带有负号，最终得到：

$$\begin{aligned} \frac{\partial r}{\partial \zeta}&= \begin{bmatrix} \frac{\partial r_u}{\partial \zeta}\\ \frac{\partial r_v}{\partial \zeta} \end{bmatrix}\\&= \begin{bmatrix} f_x\frac{d_i}{z_j} & 0 & -f_x\frac{x_j d_i}{z_j^2} & -f_x\frac{x_j y_j}{z_j^2} & f_x\left(1+\frac{x_j^2}{z_j^2}\right) & -f_x\frac{y_j}{z_j} \\ 0 & f_y\frac{d_i}{z_j} & -f_y\frac{y_j d_i}{z_j^2} & -f_y\left(1+\frac{y_j^2}{z_j^2}\right) & f_y\frac{x_j y_j}{z_j^2} & f_y\frac{x_j}{z_j} \end{bmatrix} \end{aligned}$$

源帧$i$的位姿变化用伴随表示：

$$\frac{\partial \hat{u}}{\partial \zeta_i} =-\mathbf{Ad}_{T_{ij}^{-1}}^T(\frac{\partial \hat{u}}{\partial \zeta_j})^T$$

对应adjSE3函数。

深度参数化

投影对深度的导数：

$$\begin{aligned} \frac{\partial \hat{u}}{\partial d_i}&=\frac{\partial}{\partial d_i}(f_x\frac{x_j}{z_j}+c_x) \\ &=f_x(\frac{1}{z_j}\frac{\partial x_j}{\partial d_i}-\frac{x_j}{z_j^2}\frac{\partial z_j}{\partial d_i}) \\ &=f_x(\frac{t_x}{z_j}-\frac{x_j t_z}{z_j^2})\\ \frac{\partial \hat{v}}{\partial d_i}&=f_y(\frac{t_y}{z_j}-\frac{y_j t_z}{z_j^2}) \end{aligned}$$

残差带负号。

计算流程为：

求解

线性化后的正规方程：

$$\begin{bmatrix}H_{xx} & H_{xd} \\ H_{dx} & H_{dd} \end{bmatrix} \begin{bmatrix}\Delta x \\ \Delta d \end{bmatrix} = \begin{bmatrix}b_x \\ b_d \end{bmatrix}$$

通过Schur补消元法，消去深度变量$\Delta d$：

$$\begin{aligned} S&=H_{xx}-H_{xd}H_{dd}^{-1}H_{dx} \\ v&=b_x - H_{xd}H_{dd}^{-1}b_d \end{aligned}$$

得到简化后的方程：

$$S\Delta x = v$$

通过共轭梯度法求解$\Delta x$，然后回代求解$\Delta d$：

$$\Delta d = H_{dd}^{-1}(b_d - H_{dx}\Delta x)$$