简介
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]
$$
- 投影对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}
$$ - 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}
$$ - 链式法则组合：
$$
\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)
$$
我是学生，给我钱
