DROID-SLAM 夜晚超强（3） 数学模型 latex
10. 为什么这种形式比直接光流更适合 SLAM
因为纯光流只能告诉你“像素动了多少”，但它不天然保证：
- 所有像素对应同一个相机运动
- 同一个三维结构在多帧之间几何一致
- 位姿和深度之间相互协调
而 DROID 这种重投影优化会显式利用：
- 相机位姿
- 深度
- 多帧约束
- 因子图结构
所以它不是单纯估计一个二维流场，而是在估计一个能够解释这个流场的三维场景和相机运动。
11. 一句话概括这部分原理
最合适的概括是：
[
\text{网络负责提出稠密对应，BA负责把这些对应解释成一致的三维几何。}
]
或者论文里更正式一点：
[
\text{通过最小化网络引导的稠密重投影残差，联合优化相机位姿与逆深度。}
]
如果你要，我下一步可以直接把这一整段解释整理成一节可直接放进 Overleaf 的 LaTeX 小节，标题就叫：
Dense Reprojection / Optical-Flow-Like Term 的数学原理
并追加进你刚才那个 .tex 文件里。
% This file is intended to be included in an existing Overleaf project.
% Suggested packages in your main tex file:
% \usepackage{amsmath, amssymb, bm}
% If your main project is not already configured for Chinese,
% use ctex or xeCJK in the main document.\section{DROID-SLAM与GPS/ENU约束的数学模型}需要说明的是，\texttt{run\_droid\_gps\_enu.py} 的实现并不是将视觉项与GPS项一次性写入同一个联合正规方程中统一求解，而是采用交替优化策略：
先执行DROID-SLAM的稠密重投影Bundle Adjustment，再执行一次基于GPS先验的位姿平移修正。因此，从实现角度看，它更准确地属于“视觉BA + GPS先验修正”的交替求解框架。\subsection{状态变量}设第 \(i\) 个关键帧位姿为
\begin{equation}
T_i \in SE(3),
\end{equation}
其逆深度图记为 \(\rho_i(p)\)，其中像素 \(p=(u,v)\)。相机内参记为 \(K_i\)。两帧之间的相对位姿定义为
\begin{equation}
T_{ji} = T_j T_i^{-1}.
\end{equation}记第 \(i\) 帧相机中心在SLAM世界坐标系中的位置为 \(c_i \in \mathbb{R}^3\)，其对应的GPS观测在ENU坐标系下的位置为 \(g_i \in \mathbb{R}^3\)。\subsection{视觉重投影约束}\subsubsection{像素反投影}对第 \(i\) 帧中的像素 \(p=(u,v)\)，记其齐次坐标为
\begin{equation}
\bar{p} = [u,v,1]^\top.
\end{equation}由逆深度反投影得到该像素对应的三维点
\begin{equation}
X_i(p) = \frac{1}{\rho_i(p)} K_i^{-1}\bar{p}.
\end{equation}\subsubsection{跨帧重投影}在当前位姿与深度估计下，该点投影到第 \(j\) 帧后的像素坐标为
\begin{equation}
\hat{p}_{ij}(p) = \pi \bigl( K_j T_{ji} X_i(p) \bigr),
\end{equation}
其中 \(\pi(\cdot)\) 表示针孔投影函数。\subsubsection{网络预测的稠密对应修正}DROID中的更新网络基于相关体和运动特征，为每条边 \((i,j)\) 上的每个像素预测一个二维修正量 \(\Delta_{ij}(p)\) 和像素权重 \(w_{ij}(p)\)。因此，优化目标对应点可写为
\begin{equation}
p^*_{ij}(p) = \hat{p}_{ij}(p) + \Delta_{ij}(p).
\end{equation}\subsubsection{视觉项目标函数}于是，视觉重投影项可以写为
\begin{equation}
E_{\mathrm{flow}}(T,\rho)
=
\sum_{(i,j)\in\mathcal{E}}
\sum_{p\in\Omega_i}
\left\|
W_{ij}(p)^{1/2}
\left(
p^*_{ij}(p) -
\pi \bigl(K_j T_{ji} X_i(p)\bigr)
\right)
\right\|_2^2
+
\sum_i \sum_{p\in\Omega_i}
\eta_i(p)\bigl(\delta \rho_i(p)\bigr)^2,
\end{equation}
其中 \(\mathcal{E}\) 为因子图中的边集合，\(\Omega_i\) 表示第 \(i\) 帧的像素集合，\(W_{ij}(p)\) 为像素级权重矩阵，\(\eta_i(p)\) 为逆深度阻尼项。在实现中，权重矩阵可以写成
\begin{equation}
W_{ij}(p)=10^{-3}\,\mathrm{diag}\bigl(w^x_{ij}(p),\,w^y_{ij}(p)\bigr).
\end{equation}\subsubsection{线性化形式}对残差进行一阶线性化，有
\begin{equation}
r_{ij}(p)
\approx
r^0_{ij}(p)
- J_i(p)\,\delta \xi_i
- J_j(p)\,\delta \xi_j
- J_{\rho}(p)\,\delta \rho_i(p),
\end{equation}
其中 \(\delta \xi_i,\delta \xi_j\) 是位姿在李代数中的增量，\(J_i,J_j,J_{\rho}\) 分别表示对源帧位姿、目标帧位姿和逆深度的雅可比矩阵。代码中基于该线性化模型构建正规方程，并通过Schur Complement对位姿和深度进行联合求解。\subsection{GPS到ENU坐标转换}设原始GPS测量为地理坐标
\begin{equation}
(\phi_i,\lambda_i,h_i),
\end{equation}
其中 \(\phi_i\) 为纬度，\(\lambda_i\) 为经度，\(h_i\) 为高程。以参考点
\begin{equation}
(\phi_0,\lambda_0,h_0)
\end{equation}
建立局部ENU坐标系，则首先将地理坐标转换到ECEF坐标系：
\begin{equation}
x_i^{\mathrm{ecef}} = f_{\mathrm{ecef}}(\phi_i,\lambda_i,h_i).
\end{equation}再转换到ENU坐标系：
\begin{equation}
g_i
=
R_{\mathrm{enu}\leftarrow\mathrm{ecef}}(\phi_0,\lambda_0)
\bigl(x_i^{\mathrm{ecef}} - x_0^{\mathrm{ecef}}\bigr).
\end{equation}其中，ECEF到ENU的旋转矩阵为
\begin{equation}
R_{\mathrm{enu}\leftarrow\mathrm{ecef}}
=
\begin{bmatrix}
-\sin\lambda_0 & \cos\lambda_0 & 0 \\
-\sin\phi_0\cos\lambda_0 & -\sin\phi_0\sin\lambda_0 & \cos\phi_0 \\
\cos\phi_0\cos\lambda_0 & \cos\phi_0\sin\lambda_0 & \sin\phi_0
\end{bmatrix}.
\end{equation}\subsection{ENU与SLAM坐标系之间的Sim(3)对齐}由于单目SLAM轨迹与ENU坐标系之间通常存在尺度、旋转和平移差异，代码先通过匹配的GPS关键帧估计一个相似变换：
\begin{equation}
\mathcal{S}(g) = sRg + t,
\end{equation}
其中 \(s>0\) 为尺度，\(R\in SO(3)\) 为旋转矩阵，\(t\in\mathbb{R}^3\) 为平移向量。该相似变换通过最小化以下误差估计：
\begin{equation}
\min_{s,R,t}
\sum_{k\in\mathcal{M}}
\left\|
c_k - (sRg_k + t)
\right\|_2^2,
\end{equation}
其中 \(\mathcal{M}\) 表示具有有效GPS观测的关键帧集合。实现中使用Umeyama方法估计Sim(3)，并可结合RANSAC对外点进行剔除。\subsection{GPS先验约束}\subsubsection{GPS残差定义}在完成ENU到SLAM的Sim(3)对齐后，第 \(k\) 个关键帧的GPS残差定义为
\begin{equation}
e_k = c_k - \mathcal{S}(g_k).
\end{equation}\subsubsection{竖直方向加权}由于高度方向的GPS误差通常更大，代码对竖直方向施加较小权重，定义加权残差
\begin{equation}
\tilde{e}_k = D_v e_k,
\qquad
D_v = \mathrm{diag}(1,1,\lambda_v),
\end{equation}
其中 \(\lambda_v\) 为竖直方向权重，默认取
\begin{equation}
\lambda_v = 0.25.
\end{equation}\subsubsection{Huber鲁棒核}令加权残差范数为
\begin{equation}
r_k = \|\tilde{e}_k\|_2,
\end{equation}
则代码中引入一个Huber形式的鲁棒缩放因子：
\begin{equation}
\alpha_k =
\begin{cases}
1, & r_k \le \delta_h, \\
\dfrac{\delta_h}{r_k}, & r_k > \delta_h,
\end{cases}
\end{equation}
其中 \(\delta_h\) 为Huber阈值，默认设置为
\begin{equation}
\delta_h = 3.0\ \mathrm{m}.
\end{equation}\subsubsection{理想化的GPS能量表达}若将该先验写成标准能量函数形式，则可表示为
\begin{equation}
E_{\mathrm{gps}}(T)
=
\sum_{k\in\mathcal{I}}
\rho_H
\left(
\left\|
D_v\bigl(c_k-\mathcal{S}(g_k)\bigr)
\right\|_2
\right),
\end{equation}
其中 \(\rho_H(\cdot)\) 表示Huber损失函数，\(\mathcal{I}\) 表示GPS内点集合。\subsection{代码中的实际GPS更新方式}需要强调的是，代码并没有把 \(E_{\mathrm{gps}}\) 直接并入DROID的BA正规方程中联合求解，而是在每次BA之后对位姿平移执行一次梯度式修正。对每个GPS内点关键帧，其平移更新量为
\begin{equation}
\Delta c_k
=
-\eta_{\mathrm{step}}
\, w_{\mathrm{prior}}
\, \alpha_k
\, \tilde{e}_k,
\end{equation}
其中 \(w_{\mathrm{prior}}\) 为GPS先验权重，\(\eta_{\mathrm{step}}\) 为步长系数。为了避免GPS对轨迹施加过大的瞬时扰动，代码还对平移更新步长做了截断：
\begin{equation}
\|\Delta c_k\|_2 \le \Delta_{\max},
\end{equation}
其中 \(\Delta_{\max}\) 表示单次更新允许的最大平移修正量。最终，相机中心位置按如下方式更新：
\begin{equation}
c_k \leftarrow c_k + \Delta c_k.
\end{equation}这一更新只作用于位姿平移，不直接修改姿态旋转。\subsection{整体优化流程}因此，该方法更准确的整体表达应写为交替优化：
\begin{align}
(T,\rho) &\leftarrow \arg\min E_{\mathrm{flow}}(T,\rho), \\
c &\leftarrow c - \eta \nabla E_{\mathrm{gps}}(c).
\end{align}换句话说，每一次后端更新迭代都包括两个阶段：
\begin{enumerate}\item 利用稠密重投影残差执行DROID的视觉Bundle Adjustment；\item 利用GPS先验对关键帧的世界坐标平移进行一次鲁棒修正。
\end{enumerate}因此，在论文中更推荐将该方法表述为：
“基于DROID-SLAM稠密重投影优化，并结合GPS/ENU位置先验进行交替约束优化”，
而不建议表述为“将GPS项直接并入统一BA中做一次性联合求解”。\subsection{默认参数}若采用默认配置，则主要GPS优化参数为
\begin{align}
w_{\mathrm{prior}} &= 0.15, \\
\eta_{\mathrm{step}} &= 0.35, \\
\delta_h &= 3.0\ \mathrm{m}, \\
\Delta_{\max} &= 0.75\ \mathrm{m}, \\
\lambda_v &= 0.25.
\end{align}