1. 束调整（Bundle Adjustment, BA）

• 作用：同时优化相机位姿和三维地图点，最小化重投影误差。
• 算法：基于 Gauss-Newton 或 Levenberg-Marquardt 的非线性优化。
• 实现库：g2o 或 Ceres Solver。
• 关键函数/类：
• stella_vslam::optimize::bundle_adjustment
◦ optimize(): 核心优化函数，构建优化问题并求解。
◦ 使用 g2o::SparseOptimizer 或 ceres::Problem 配置参数块（相机位姿 SE3、地图点 XYZ）和残差项。
• 相关代码文件：src/stella_vslam/optimize/bundle_adjustment.cc

---

2. 位姿图优化（Pose Graph Optimization）

• 作用：优化关键帧之间的位姿约束，保证全局轨迹一致性，减少累积误差。
• 算法：基于位姿图的稀疏优化（如 g2o 中的 EdgeSE3 边）。
• 关键函数/类：
• stella_vslam::optimize::pose_graph_optimizer
◦ optimize(): 构建位姿图（关键帧为节点，相对运动为边），调用 g2o::optimize() 进行优化。
• 相关代码文件：src/stella_vslam/optimize/pose_graph_opt.cc

---

3. 全局闭环校正（Loop Closure Correction）

• 作用：检测到闭环后，对齐历史轨迹并进行全局优化。
• 算法：结合 Sim3 优化（解决尺度漂移）和 Essential Graph 优化。
• 关键函数/类：
• stella_vslam::module::loop_closer
◦ correct_loop(): 主函数，触发闭环校正流程。
◦ optimize_essential_graph(): 优化 Essential Graph（仅连接共视关键帧的稀疏图）。
◦ global_bundle_adjustment(): 全局 BA 优化所有变量。
• 相关代码文件：src/stella_vslam/module/loop_closer.cc

---

4. 全局 BA（Global Bundle Adjustment）

• 作用：在闭环检测后，对所有关键帧和地图点进行全局优化。
• 实现：通常使用 g2o 或 Ceres 的大规模稀疏求解器。
• 关键函数：
• stella_vslam::optimize::global_bundle_adjuster::optimize()
• 相关代码文件：src/stella_vslam/optimize/global_bundle_adjuster.cc

---

5. 关键帧剔除（Keyframe Culling）

• 作用：剔除冗余关键帧以降低计算复杂度。
• 策略：基于观测数量、时间邻近性等条件判断。
• 关键函数/类：
• stella_vslam::module::mapping_module
◦ cull_keyframe(): 判断并剔除冗余关键帧。
• 相关代码文件：src/stella_vslam/module/mapping_module.cc

---

6. 鲁棒核函数（Robust Kernel）

• 作用：抑制异常值对优化的影响（如动态物体、误匹配）。
• 实现：在 BA 或位姿图中使用 Huber 核 或 Cauchy 核。
• 代码示例：

// g2o 中设置 Huber 核
g2o::RobustKernelHuber* robust_kernel = new g2o::RobustKernelHuber;
robust_kernel->setDelta(delta_threshold);
edge->setRobustKernel(robust_kernel);

---

代码结构总结

• 后端优化代码集中在 src/stella_vslam/optimize 目录下：
• bundle_adjustment.cc/h：局部和全局 BA 实现。
• pose_graph_opt.cc/h：位姿图优化。
• global_bundle_adjuster.cc/h：全局 BA 接口。
• 闭环校正逻辑位于 src/stella_vslam/module/loop_closer.cc。

如需更深入分析，可参考 Stella_VSLAM 的 GitHub 仓库：stella_vslam。

1. 束调整（Bundle Adjustment, BA）

数学模型

BA通过最小化重投影误差来优化相机位姿$T_i\in SE(3)$和三维点$p_j\in {\mathbb{R}}^3$，目标函数为非线性最小二乘问题：
$${\Theta }^{\ast }=\arg \underset{\Theta }{\min }\sum _{i,j}\rho \left(\Vert \pi (T_i,p_j)-z_{ij}{\Vert }_{{\Sigma }_{ij}}^2\right)$$
•$\pi (\cdot )$: 相机投影函数，将3D点映射到图像平面。
•$z_{ij}$: 观测到的2D像素坐标。
•${\Sigma }_{ij}$: 协方差矩阵，表征测量噪声。
•$\rho (\cdot )$: 鲁棒核函数（如Huber核），抑制异常值影响。

优化方法

• 参数化：使用李代数${\xi }_i\in \mathfrak{se}(3)$表示位姿$T_i=\exp ({\xi }_i^{\wedge })$，避免过参数化。
• 迭代求解：采用 Gauss-Newton 或 Levenberg-Marquardt 算法，每次迭代求解增量方程：
$$(J^{\top }J+\lambda I)\Delta \Theta =-J^{\top }r$$
其中$J$是残差对参数的雅可比矩阵，$r$是残差向量.

---

2. 位姿图优化（Pose Graph Optimization）

数学模型

优化目标是最小化所有位姿约束的误差：
$$E=\sum _{(i,j)\in \mathcal{E}}\rho \left(\Vert \log (T_{ij}^{-1}{T}_i^{-1}{T}_j){\Vert }_{{\Sigma }_{ij}}^2\right)$$
•$T_{ij}\in SE(3)$: 两关键帧间的相对位姿测量。
•$\log (\cdot )$: 将SE(3)误差映射到李代数空间$\mathfrak{se}(3)$.

优化方法

• 因子图模型：节点为位姿$T_i$，边为约束$T_{ij}$，构建稀疏因子图。
• 稀疏求解：利用 g2o 或 Ceres 中的稀疏Cholesky分解或PCG算法，高效求解大规模问题.

---

3. 全局闭环校正（Loop Closure Correction）

Sim3优化

当检测到回环时，需对齐不同尺度的轨迹，引入 Sim(3) 群优化：
$$S_{ij}=\left[\begin{array}{cc}sR& t\\ 0& 1\end{array}\right]\in Sim(3)$$
优化目标为：
$$\underset{S_i,S_j}{\min }\Vert \log (S_{ij}^{-1}{S}_i^{-1}{S}_j){\Vert }^2+\sum \text{重投影误差}$$
其中$s$为尺度因子，通过联合优化尺度和位姿消除漂移.

---

4. 关键帧剔除（Keyframe Culling）

数学准则

• 信息熵准则：选择对地图不确定性降低最大的关键帧保留。
• 几何显著性：计算共视图中关键帧的观测点数量$N$，若$N<\tau$，则剔除.
• 滑动窗口策略：仅保留最近$N$个关键帧，丢弃旧帧以控制计算规模.

---

5. 鲁棒核函数（Robust Kernel）

数学作用

通过修改损失函数降低异常值影响，例如 Huber核：
$$\rho (r)=\{\begin{array}{ll}\frac{1}{2}{r}^2& \text{if }|r|\le \delta \\ \delta (|r|-\frac{1}{2}\delta )& \text{otherwise}\end{array}$$
或 Cauchy核：
$$\rho (r)=\log (1+r^2/{\delta }^2)$$
其中$\delta$为阈值，控制对异常值的敏感度.

---

6. 滑动窗口滤波（Sliding Window Filter）

数学实现

• 边缘化（Marginalization）：将旧关键帧$T_k$从优化变量中移除，但其观测信息通过Schur补保留为先验：
$$H_{\text{new}}=H_{aa}-H_{ab}{H}_{bb}^{-1}{H}_{ba}$$
其中$H$为Hessian矩阵，$a$为保留变量，$b$为边缘化变量.

---

总结

Stella_VSLAM后端通过 非线性最小二乘优化（BA、位姿图）、 李群/代数理论（SE3/Sim3参数化）、 鲁棒统计（核函数）和 稀疏线性代数（Cholesky分解）等数学工具，实现了高精度、高效率的状态估计。全局优化与局部优化的结合（如滑动窗口）平衡了精度与实时性.