@[TOC](【Basalt】Basalt void SqrtKeypointVioEstimator<Scalar_>::optimize() VIO优化流程)
0. 执行流程图
(x,l)→linearize→(H,b)→eliminate landmarks→(Hˉ,bˉ)→LM solve (x,l) \rightarrow \text{linearize} \rightarrow (H,b) \rightarrow \text{eliminate landmarks} \rightarrow (\bar H,\bar b) \rightarrow \text{LM solve}(x,l)→linearize→(H,b)→eliminate landmarks→(Hˉ,bˉ)→LM solve
→δx→back-substitute→δl→apply→recompute true cost→accept/reject \rightarrow \delta x \rightarrow \text{back-substitute} \rightarrow \delta l \rightarrow \text{apply} \rightarrow \text{recompute true cost} \rightarrow \text{accept/reject} →δx→back-substitute→δl→apply→recompute true cost→accept/reject
cpp
template <class Scalar_>
void SqrtKeypointVioEstimator<Scalar_>::optimize_and_marg(
const std::map<int64_t, int>& num_points_connected,
const std::unordered_set<KeypointId>& lost_landmaks) {
optimize();
marginalize(num_points_connected, lost_landmaks);
}
template <class Scalar_>
void SqrtKeypointVioEstimator<Scalar_>::optimize()下面我按 4 层来拆这段 optimize()：
- 这段函数整体在干什么
- 逐段逐行解释代码
- 它背后的完整数学推导
- 这份实现里几个最关键的工程点 / 数学点
1. 这段 optimize() 整体在干什么
这段函数本质上是在做一次 VIO 后端非线性优化，用的是：
- 视觉重投影误差
- IMU 预积分误差 / 惯导误差
- bias 正则项
- 边缘化先验（marginalization prior）
共同组成的目标函数。
然后它采用的求解框架可以概括成：
- 先把所有待优化变量整理成一个大状态向量
- 对误差做一阶线性化 / 二阶近似
- 对路标点做 QR / Schur 风格消元
- 得到只关于相机/状态的 reduced system
- 用 Levenberg--Marquardt (LM) 求一个增量
- 回代求路标增量
- 应用增量后重新计算真实误差
- 如果误差下降，则接受；否则拒绝并增大阻尼重新来
所以这是一个非常标准的：
滑窗 VIO 后端 + landmark 消元 + LM 阻尼迭代
2. 逐段逐行解释代码
2.1 函数入口
cpp
void SqrtKeypointVioEstimator<Scalar_>::optimize() {
if (config.vio_debug) {
std::cout << "=================================" << std::endl;
}作用
进入优化函数。如果开了 debug，就打印分隔线。
cpp
if (opt_started || frame_states.size() > 4) {
opt_started = true;作用
只有在下面两种情况才真正开始优化：
- 之前已经启动过优化
- 当前滑窗里的
frame_states数量大于 4
含义
这是个启动条件，避免系统刚开始时状态太少、约束不足就硬优化。
2.2 一些配置和计时器
cpp
// harcoded configs
// bool scale_Jp = config.vio_scale_jacobian && is_qr_solver();
// bool scale_Jl = config.vio_scale_jacobian && is_qr_solver();作用
本来这里可能支持 Jacobian scaling，但现在注释掉了。
含义
作者尝试过列缩放来改善数值条件，但当前版本没启用。
cpp
ExecutionStats stats;
Timer timer_total;
Timer timer_iteration;作用
用于统计总时间、每轮时间和各模块耗时。
2.3 构造线性系统中的变量顺序 AbsOrderMap
cpp
AbsOrderMap aom;aom = absolute order map。
本质是：每个变量块在总状态向量里的起始位置和维度。
2.3.1 先给 frame_poses 编号
cpp
for (const auto& kv : frame_poses) {
aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE);作用
给每个 pose 在大向量里分配起始 index。
例如：
- 第一个 pose 从 0 开始，长度
POSE_SIZE - 第二个 pose 接在后面
- ...
cpp
BASALT_ASSERT(marg_data.order.abs_order_map.at(kv.first) ==
aom.abs_order_map.at(kv.first));作用
检查当前构造的变量顺序和 边缘化先验里记录的顺序 一致。
为什么重要
边缘化先验本质上是对旧状态形成的高斯信息项，变量顺序一旦错了，先验就会加到错误变量上，整个优化直接错。
cpp
aom.total_size += POSE_SIZE;
aom.items++;
}作用
更新总状态维度和变量块数量。
2.3.2 再给 frame_states 编号
cpp
for (const auto& kv : frame_states) {
aom.abs_order_map[kv.first] =
std::make_pair(aom.total_size, POSE_VEL_BIAS_SIZE);作用
给完整状态块编号。这里通常包括：
- pose
- velocity
- gyro bias
- accel bias
cpp
if (aom.items < marg_data.order.abs_order_map.size())
BASALT_ASSERT(marg_data.order.abs_order_map.at(kv.first) ==
aom.abs_order_map.at(kv.first));作用
同样检查和先验顺序一致。
cpp
aom.total_size += POSE_VEL_BIAS_SIZE;
aom.items++;
}含义
此时 aom.total_size 就是 所有相机/状态变量拼起来后的总维数。
2.4 LM 初始阻尼
cpp
lambda = Scalar(config.vio_lm_lambda_initial);作用
设置 LM 初始阻尼参数 lambda。
数学意义
后面求解时不是解
H \\Delta x = b
而是解
(H + \\lambda D)\\Delta x = b
这里 D 近似用对角线。
lambda 大：
- 更像梯度下降
- 更保守
lambda 小：
- 更像 Gauss-Newton
- 步子更激进
2.5 记录基本统计信息
cpp
stats.add("num_cams", this->frame_poses.size()).format("count");
stats.add("num_lms", this->lmdb.numLandmarks()).format("count");
stats.add("num_obs", this->lmdb.numObservations()).format("count");作用
记录：
- 相机/位姿数量
- 路标数量
- 观测数量
2.6 设置 landmark 线性化器参数
cpp
typename LinearizationBase<Scalar, POSE_SIZE>::Options lqr_options;
lqr_options.lb_options.huber_parameter = huber_thresh;
lqr_options.lb_options.obs_std_dev = obs_std_dev;
lqr_options.linearization_type = config.vio_linearization_type;作用
配置线性化模块：
huber_parameter：Huber 鲁棒核阈值obs_std_dev：观测标准差linearization_type：线性化方式
数学含义
视觉 residual 不是直接裸平方，而是可能经过鲁棒核加权。
cpp
std::unique_ptr<LinearizationBase<Scalar, POSE_SIZE>> lqr;作用
lqr 是整个线性化/消元/回代模块的核心对象。
它负责：
- 线性化残差
- 进行 QR / landmark 消元
- 输出 reduced Hessian 和 rhs
- 回代 landmark 增量
2.7 准备 IMU 线性化数据
cpp
ImuLinData<Scalar> ild = {
g, gyro_bias_sqrt_weight, accel_bias_sqrt_weight, {}};作用
准备 IMU 误差项需要的数据：
- 重力
g - 陀螺 bias 权重
- 加计 bias 权重
cpp
for (const auto& kv : imu_meas) {
ild.imu_meas[kv.first] = &kv.second;
}作用
把 IMU 测量按 frame / pair 填到 ild 里。
2.8 创建线性化器
cpp
{
Timer t;
lqr = LinearizationBase<Scalar, POSE_SIZE>::create(this, aom, lqr_options,
&marg_data, &ild);
stats.add("allocateLMB", t.reset()).format("ms");
lqr->log_problem_stats(stats);
}作用
创建线性化问题对象。
本质
把当前 estimator 的所有内容：
- 状态
- 点
- 观测
- 先验
- IMU
打包成一个可线性化求解的问题。
2.9 终止标志
cpp
bool terminated = false;
bool converged = false;
std::string message;
int it = 0;
int it_rejected = 0;作用
优化终止控制：
terminated：强制终止converged：正常收敛it：总迭代数it_rejected：被拒绝步数
2.10 外层主迭代
cpp
for (; it <= config.vio_max_iterations && !terminated;) {
if (it > 0) {
timer_iteration.reset();
}作用
进入主 LM 迭代。
注意这里 for 里没有 it++，因为接受/拒绝步都可能自己控制 it。
2.11 线性化问题
cpp
Scalar error_total = 0;
VecX Jp_column_norm2;
cpp
{
Timer t;
bool numerically_valid;
error_total = lqr->linearizeProblem(&numerically_valid);作用
对当前状态点做线性化，并返回当前总误差 error_total。
数学上做了什么
把非线性目标
F(x,l)
在当前点 ((x_0,l_0)) 附近展开：
r(x_0+\\delta x, l_0+\\delta l) \\approx r_0 + J_x \\delta x + J_l \\delta l
于是代价函数近似为：
\\frac12 \|r_0 + J_x \\delta x + J_l \\delta l\|\^2
cpp
BASALT_ASSERT_STREAM(
numerically_valid,
"did not expect numerical failure during linearization");作用
如果线性化过程数值爆了，直接报错。
cpp
stats.add("linearizeProblem", t.reset()).format("ms");记录线性化耗时。
注释掉的 Jacobian scaling
这些都注释掉了：
cpp
// Jp_column_norm2 = lqr->getJp_diag2();
// lqr->scaleJl_cols();说明作者曾试过：
- pose Jacobian 列归一化
- landmark Jacobian 列归一化
但当前版本没启用。
2.12 对点做 QR / 消元
cpp
lqr->performQR();
stats.add("performQR", t.reset()).format("ms");
}作用
对 landmark 变量做消元。
数学含义
把系统分成：
- 相机/状态变量 (x)
- 路标变量 (l)
线性系统写成：
HxxHxl HlxHll\]\[δx δl\] \\begin{bmatrix} H_{xx} \& H_{xl} \\ H_{lx} \& H_{ll} \\end{bmatrix} \\begin{bmatrix} \\delta x \\ \\delta l \\end{bmatrix}\[HxxHxl HlxHll\]\[δx δl
bx bl\] \\begin{bmatrix} b_x \\ b_l \\end{bmatrix}\[bx bl
消元 landmark 后得到 reduced camera system：
bx−HxlHll−1bl b_x - H_{xl}H_{ll}^{-1}b_l bx−HxlHll−1bl
这里代码实现里叫 performQR()，通常是用 QR 做每个 landmark block 的稳定消元。
cpp
if (config.vio_debug) {
std::cout << "[LINEARIZE] Error: " << error_total
<< " num_landmarks=" << lmdb.numLandmarks()
<< " num_observations=" << lmdb.numObservations() << std::endl;打印当前线性化前误差、点数、观测数。
2.13 内层 backtracking / LM 阻尼循环
cpp
for (int j = 0; it <= config.vio_max_iterations && !terminated; j++) {作用
这是 同一轮线性化结果下 的内层阻尼搜索。
也就是：
- 不重新线性化
- 只改
lambda - 反复尝试一步是否可接受
cpp
if (j > 0) {
timer_iteration.reset();
if (config.vio_debug) {
std::cout << "Iteration " << it << ", backtracking" << std::endl;
}
}含义
如果当前不是第一次尝试，说明前一个步子没通过，要开始 backtracking。
2.14 取 reduced system
cpp
VecX inc;
{
Timer t;
MatX H;
VecX b;
lqr->get_dense_H_b(H, b);作用
拿到消元后的致密 Hessian 和右端项。
数学上
现在拿到的是：
Hδx=b H \delta x = b Hδx=b
这里 H 已经是 只关于相机/状态变量 的 reduced Hessian。
cpp
stats.add("get_dense_H_b", t.reset()).format("ms");2.15 加 LM 阻尼并求解
cpp
int iter = 0;
bool inc_valid = false;
constexpr int max_num_iter = 3;作用
最多试 3 次，直到解出来的增量 inc 数值有效。
cpp
while (iter < max_num_iter && !inc_valid) {
VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
MatX H_copy = H;
H_copy.diagonal() += Hdiag_lambda;数学上
这里构造的是 LM 系统：
(H+Λ)δx=b (H + \Lambda)\delta x = b (H+Λ)δx=b
其中
KaTeX parse error: Expected 'EOF', got '_' at position 56: ...{ii}, \text{min_̲lambda}))
这不是简单加 (\lambda I)，而是 按 Hessian 对角线缩放的阻尼，更接近常见 trust-region 风格。
cpp
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H_copy);
inc = ldlt.solve(b);作用
用 LDLT 分解解线性系统。
数学上
解：
δx=(H+Λ)−1b \delta x = (H+\Lambda)^{-1} b δx=(H+Λ)−1b
cpp
if (!inc.array().isFinite().all()) {
lambda = lambda_vee * lambda;
lambda_vee *= vee_factor;
} else {
inc_valid = true;
}含义
如果解出来有 NaN / Inf，说明阻尼太小或系统病态，就增大 lambda 再试。
cpp
if (!inc_valid) {
std::cerr << "Still invalid inc after " << max_num_iter
<< " iterations." << std::endl;
}含义
3 次还不行，就报错。
2.16 先备份状态
cpp
backup();作用
因为后面要试着应用这个增量，如果失败要恢复。
2.17 回代 landmark 增量
cpp
Scalar l_diff = 0;
{
inc = -inc;这一句非常关键
前面线性系统是按某种符号约定得到的，真正用于 applyInc 前，这里把增量取反。
一般对应两种常见写法之一：
- 解 (H \Delta x = -g)
- 或解 (H \Delta x = b) 其中 (b = -g)
这里说明内部 b 的定义和 applyInc 所需方向相反，所以在应用前取负号。
cpp
Timer t;
l_diff = lqr->backSubstitute(inc);作用
在已知相机/状态增量后，回代求路标增量。
数学上
由 landmark block 的方程：
Hllδl=bl−Hlxδx H_{ll}\delta l = b_l - H_{lx}\delta x Hllδl=bl−Hlxδx
得到：
δl=Hll−1(bl−Hlxδx) \delta l = H_{ll}^{-1}(b_l - H_{lx}\delta x) δl=Hll−1(bl−Hlxδx)
backSubstitute(inc) 就是在做这个。
cpp
stats.add("backSubstitute", t.reset()).format("ms");
}2.18 应用 pose / state 增量
cpp
for (auto& [frame_id, state] : frame_poses) {
int idx = aom.abs_order_map.at(frame_id).first;
state.applyInc(inc.template segment<POSE_SIZE>(idx));
}作用
把大增量向量里属于每个 pose 的那一段取出来，应用到 pose 上。
数学上
如果 pose 在李群上，一般是：
T←T⊞δξ T \leftarrow T \boxplus \delta \xi T←T⊞δξ
例如 SE(3) 上：
T←exp(δξ∧)T T \leftarrow \exp(\delta \xi^\wedge) T T←exp(δξ∧)T
或右乘版本，具体看 applyInc 实现。
cpp
for (auto& [frame_id, state] : frame_states) {
int idx = aom.abs_order_map.at(frame_id).first;
state.applyInc(inc.template segment<POSE_VEL_BIAS_SIZE>(idx));
}作用
对完整状态（pose/vel/bias）应用增量。
2.19 计算步长
cpp
Scalar step_norminf = inc.array().abs().maxCoeff();作用
用无穷范数看增量大小：
∣δx∣∞=maxi∣δxi∣ |\delta x|_\infty = \max_i |\delta x_i| ∣δx∣∞=imax∣δxi∣
后面会用它作为收敛判据之一。
2.20 应用增量后重新计算真实误差
cpp
Scalar after_update_marg_prior_error = 0;
Scalar after_update_vision_and_inertial_error = 0;
cpp
{
Timer t;
computeError(after_update_vision_and_inertial_error);
computeMargPriorError(marg_data, after_update_marg_prior_error);作用
重新计算：
- 视觉 + 某些惯性相关误差
- 边缘化先验误差
关键点
这里是 重新用非线性模型算真实误差，不是只看线性近似。
cpp
Scalar after_update_imu_error = 0, after_bg_error = 0,
after_ba_error = 0;
ScBundleAdjustmentBase<Scalar>::computeImuError(
aom, after_update_imu_error, after_bg_error, after_ba_error,
frame_states, imu_meas, gyro_bias_sqrt_weight.array().square(),
accel_bias_sqrt_weight.array().square(), g);作用
显式计算 IMU 和 bias 项误差。
拆成了：
after_update_imu_errorafter_bg_errorafter_ba_error
即：
- 预积分残差
- gyro bias 正则
- accel bias 正则
cpp
after_update_vision_and_inertial_error +=
after_update_imu_error + after_bg_error + after_ba_error;把 IMU 和 bias 项加进去。
cpp
stats.add("computerError2", t.reset()).format("ms");
}
cpp
Scalar after_error_total = after_update_vision_and_inertial_error +
after_update_marg_prior_error;数学上
更新后总误差：
F(x+δx,l+δl)\[ F(x+\\delta x, l+\\delta l) \[F(x+δx,l+δl)
Fvision+imu+bias+Fmarg prior F_{\\text{vision+imu+bias}} + F_{\\text{marg prior}} Fvision+imu+bias+Fmarg prior
*** ** * ** ***
### 2.21 判断这一步是否值得接受
```cpp
Scalar f_diff;
bool step_is_valid = false;
bool step_is_successful = false;
Scalar relative_decrease = 0;
```
*** ** * ** ***
```cpp
{
f_diff = error_total - after_error_total;
```
#### 数学上
真实误差下降量：
ΔFactual=Fbefore−Fafter \\Delta F_{\\text{actual}} = F_{\\text{before}} - F_{\\text{after}} ΔFactual=Fbefore−Fafter
若 `f_diff > 0`，说明真下降了。
*** ** * ** ***
```cpp
relative_decrease = f_diff / l_diff;
```
#### 数学含义
这是典型 LM / trust-region 思路中的
ρ=actual reductionpredicted reduction \\rho = \\frac{\\text{actual reduction}}{\\text{predicted reduction}} ρ=predicted reductionactual reduction
其中：
* `f_diff` = 实际下降
* `l_diff` = 线性/二次模型预测下降
*** ** * ** ***
```cpp
step_is_valid = l_diff > 0;
step_is_successful = step_is_valid && relative_decrease > 0;
```
#### 含义
只有当：
* 模型预测下降是正的
* 实际上也确实下降
这一步才算成功。
*** ** * ** ***
### 2.22 成功时怎么做
```cpp
if (step_is_successful) {
BASALT_ASSERT(step_is_valid);
```
*** ** * ** ***
```cpp
lambda *= std::max
其中
Hxx=JxTJx,Hxl=JxTJl,Hll=JlTJl H_{xx} = J_x^T J_x,\quad H_{xl} = J_x^T J_l,\quad H_{ll} = J_l^T J_l Hxx=JxTJx,Hxl=JxTJl,Hll=JlTJl
bx=−JxTr0,bl=−JlTr0 b_x = -J_x^T r_0,\quad b_l = -J_l^T r_0 bx=−JxTr0,bl=−JlTr0
实际代码里 b 的符号约定可能不同，所以后面有 inc = -inc。
3.5 landmark 消元：Schur / QR
因为 landmark 数很多，不能把全部变量直接一起 dense solve。
所以对每个 landmark block 消元，得到 reduced system：
δl=Hll−1(bl−Hlxδx) \delta l = H_{ll}^{-1}(b_l - H_{lx}\delta x) δl=Hll−1(bl−Hlxδx)
代入第一行：
Hxxδx+Hxlδl=bx H_{xx}\delta x + H_{xl}\delta l = b_x Hxxδx+Hxlδl=bx
得到：
(Hxx−HxlHll−1Hlx)δx=bx−HxlHll−1bl \left(H_{xx} - H_{xl}H_{ll}^{-1}H_{lx}\right)\delta x= b_x - H_{xl}H_{ll}^{-1}b_l (Hxx−HxlHll−1Hlx)δx=bx−HxlHll−1bl
定义
Hˉ=Hxx−HxlHll−1Hlx \bar H = H_{xx} - H_{xl}H_{ll}^{-1}H_{lx} Hˉ=Hxx−HxlHll−1Hlx
bˉ=bx−HxlHll−1bl \bar b = b_x - H_{xl}H_{ll}^{-1}b_l bˉ=bx−HxlHll−1bl
于是 reduced system 为：
Hˉδx=bˉ \bar H \delta x = \bar b Hˉδx=bˉ
这就是：
performQR()get_dense_H_b(H, b)
最终拿到的 H, b。
3.6 LM 阻尼
Gauss-Newton 直接解：
Hˉδx=bˉ \bar H \delta x = \bar b Hˉδx=bˉ
LM 则解：
(Hˉ+Λ)δx=bˉ (\bar H + \Lambda)\delta x = \bar b (Hˉ+Λ)δx=bˉ
代码里：
KaTeX parse error: Expected 'EOF', got '_' at position 66: ...{ii}, \text{min_̲lambda})\right)...
所以实际求解的是：
(Hˉ+diag(max(λHˉii,ϵ)))δx=bˉ \left(\bar H + \mathrm{diag}(\max(\lambda \bar H_{ii}, \epsilon))\right)\delta x = \bar b (Hˉ+diag(max(λHˉii,ϵ)))δx=bˉ
这比简单的 (\bar H+\lambda I) 更符合尺度自适应。
3.7 求得相机增量后，回代路标
当求出 (\delta x) 后，
δl=Hll−1(bl−Hlxδx) \delta l = H_{ll}^{-1}(b_l - H_{lx}\delta x) δl=Hll−1(bl−Hlxδx)
这就是 backSubstitute(inc)。
3.8 真实代价和模型代价
LM 需要比较：
1）真实代价下降
ΔFactual=F(x,l)−F(x+δx,l+δl) \Delta F_{\text{actual}} = F(x,l) - F(x+\delta x,l+\delta l) ΔFactual=F(x,l)−F(x+δx,l+δl)
代码里就是：
fdiff=errortotal−aftererrortotal f_diff = error_total - after_error_total fdiff=errortotal−aftererrortotal
2）模型预测下降
记为：
ΔFmodel=ldiff \Delta F_{\text{model}} = l_diff ΔFmodel=ldiff
这是 backSubstitute(inc) 返回的值。
它本质上是当前线性/二次模型下预期能下降多少。
3）步质量
ρ=ΔFactualΔFmodel \rho = \frac{\Delta F_{\text{actual}}}{\Delta F_{\text{model}}} ρ=ΔFmodelΔFactual
代码里：
cpp
relative_decrease = f_diff / l_diff;3.9 接受 / 拒绝规则
接受
若
ldiff>0,ρ>0 l_diff > 0,\qquad \rho > 0 ldiff>0,ρ>0
说明：
- 模型预测是下降的
- 实际也确实下降了
则接受该步。
拒绝
否则拒绝，并增大阻尼 (\lambda)。
3.10 阻尼更新规则
若接受，则：
λ←λ⋅max(13,;1−(2ρ−1)3) \lambda \leftarrow \lambda \cdot \max\left( \frac13,; 1-(2\rho-1)^3 \right) λ←λ⋅max(31,;1−(2ρ−1)3)
当 (\rho) 大时，上式会减小 (\lambda)。
若拒绝，则：
λ←λ⋅λ∨ \lambda \leftarrow \lambda \cdot \lambda_{\vee} λ←λ⋅λ∨
并继续放大倍率：
KaTeX parse error: Expected 'EOF', got '_' at position 57: ...\cdot \text{vee_̲factor}
3.11 收敛条件
代码使用两个判据：
1）函数值收敛
0<ΔFactual<10−6 0 < \Delta F_{\text{actual}} < 10^{-6} 0<ΔFactual<10−6
2）参数收敛
∣δx∣∞<10−4 |\delta x|_\infty < 10^{-4} ∣δx∣∞<10−4
4. 这份实现里最关键的点
下面是你真正调试时最该盯的。
4.1 aom 的顺序必须和 marg_data 一致
这是这类系统最容易埋雷的点之一。
因为边缘化先验不是抽象概念，而是一个固定维度、固定顺序的高斯项。
如果 aom.abs_order_map 和 marg_data.order.abs_order_map 不一致：
- 先验会作用在错误状态块
- 误差和 Hessian 全都错位
- 结果可能不是直接崩，而是"能跑但漂"
所以这里的 BASALT_ASSERT 非常关键。
4.2 performQR() 本质是在给相机系统做 Schur reduction
这一步是 BA / VIO 优化加速的核心。
因为如果不先消 landmark，而是直接把所有点和相机一起 solve：
- 系统维度会巨大
- 稠密化严重
- 速度和内存都会炸
所以流程是：
- 线性化所有残差
- 对点 block 做局部消元
- 得到只关于相机/状态的系统
- 只解这个 reduced system
- 再回代点
这就是经典 BA 后端套路。
4.3 inc = -inc 暗示内部 b 的符号约定
这是看代码时最容易迷惑的地方。
正常你可能以为：
Hδx=−g H\delta x = -g Hδx=−g
但这里 ldlt.solve(b) 后，又有
cpp
inc = -inc;说明内部某些模块使用的是另一种定义，比如：
Hδx=b,b=+JTr H\delta x = b,\quad b = +J^Tr Hδx=b,b=+JTr
最后应用前统一取负。
所以调试时不要只看 solve 那一行，要结合：
linearizeProblemget_dense_H_bapplyIncbackSubstitute
一起判断符号。
4.4 l_diff 是模型预测下降量，不是 landmark difference
这个名字很容易误读。
这里的 l_diff 不是 "landmark diff"，而更像：
- linearized diff
- local model predicted decrease
因为后面用的是：
cpp
relative_decrease = f_diff / l_diff;这明显是 LM 的 actual/predicted reduction ratio。
4.5 这段代码是"先固定线性化，再内层调 lambda"的结构
也就是：
- 外层：重新线性化
- 内层：不重线性化，只调阻尼
这比每次 rejected 都重新 linearize 便宜很多。
代价是：
- 内层 backtracking 期间用的是同一个局部二次模型
- 如果当前线性化点离真值较远，模型质量可能一般
所以作者在注释里也在怀疑 old SC loop 和当前实现的精度差异。
5. 你可以把这段函数压缩记成一句话
这段 optimize() 的本质就是：
在滑窗 VIO 中，把视觉+IMU+bias+边缘化先验组成的非线性目标，在当前状态上线性化；对 landmark 做 QR/Schur 消元，形成 reduced camera system；用 LM 阻尼求解状态增量，再回代 landmark，按真实代价下降决定接受还是拒绝该步，直到收敛或阻尼爆掉。
6. 你现在最值得继续看的下一步
下一步最值得看的是这 4 个函数：
linearizeProblem(...)performQR()get_dense_H_b(H, b)backSubstitute(inc)
因为这 4 个函数决定了：
- 视觉残差雅可比怎么写
- IMU 和先验怎么并进来
- landmark 是怎么被消元的
l_diff到底怎么算的- 为什么这里需要
inc = -inc
cpp
void SqrtKeypointVioEstimator<Scalar_>::optimize() {
if (config.vio_debug) {
std::cout << "===============optimize==================" << std::endl;
}
if (opt_started || frame_states.size() > 4) {
opt_started = true;
// step 12. 总入口与预检查
// harcoded configs
// bool scale_Jp = config.vio_scale_jacobian && is_qr_solver();
// bool scale_Jl = config.vio_scale_jacobian && is_qr_solver();
// step 12.1 构建线性系统状态排序
// timing
ExecutionStats stats;
Timer timer_total;
Timer timer_iteration;
// construct order of states in linear system --> sort by ascending
// timestamp
AbsOrderMap aom;
for (const auto& kv : frame_poses) {
aom.abs_order_map[kv.first] = std::make_pair(aom.total_size, POSE_SIZE);
// Check that we have the same order as marginalization
BASALT_ASSERT(marg_data.order.abs_order_map.at(kv.first) ==
aom.abs_order_map.at(kv.first));
aom.total_size += POSE_SIZE;
aom.items++;
}
for (const auto& kv : frame_states) {
aom.abs_order_map[kv.first] =
std::make_pair(aom.total_size, POSE_VEL_BIAS_SIZE);
// Check that we have the same order as marginalization
if (aom.items < marg_data.order.abs_order_map.size())
BASALT_ASSERT(marg_data.order.abs_order_map.at(kv.first) ==
aom.abs_order_map.at(kv.first));
aom.total_size += POSE_VEL_BIAS_SIZE;
aom.items++;
}
// step 12.2 初始化 LM 参数与统计信息
// TODO: Check why we get better accuracy with old SC loop. Possible
// culprits:
// - different initial lambda (based on previous iteration)
// - no landmark damping
// - outlier removal after 4 iterations?
lambda = Scalar(config.vio_lm_lambda_initial);
// record stats
stats.add("num_cams", this->frame_poses.size()).format("count");
stats.add("num_lms", this->lmdb.numLandmarks()).format("count");
stats.add("num_obs", this->lmdb.numObservations()).format("count");
// setup landmark blocks
typename LinearizationBase<Scalar, POSE_SIZE>::Options lqr_options;
lqr_options.lb_options.huber_parameter = huber_thresh;
lqr_options.lb_options.obs_std_dev = obs_std_dev;
lqr_options.linearization_type = config.vio_linearization_type;
std::unique_ptr<LinearizationBase<Scalar, POSE_SIZE>> lqr;
ImuLinData<Scalar> ild = {
g, gyro_bias_sqrt_weight, accel_bias_sqrt_weight, {}};
for (const auto& kv : imu_meas) {
ild.imu_meas[kv.first] = &kv.second;
}
// step 12.3 构造线性化对象
{
Timer t;
lqr = LinearizationBase<Scalar, POSE_SIZE>::create(this, aom, lqr_options,
&marg_data, &ild);
stats.add("allocateLMB", t.reset()).format("ms");
lqr->log_problem_stats(stats);
}
bool terminated = false;
bool converged = false;
std::string message;
int it = 0;
int it_rejected = 0;
// step 12.4 主迭代循环
for (; it <= config.vio_max_iterations && !terminated;) {
if (it > 0) {
timer_iteration.reset();
}
Scalar error_total = 0;
VecX Jp_column_norm2;
{
// step 12.4.1 线性化与 QR 消元
// TODO: execution could be done staged
Timer t;
// linearize residuals
bool numerically_valid;
error_total = lqr->linearizeProblem(&numerically_valid);
BASALT_ASSERT_STREAM(
numerically_valid,
"did not expect numerical failure during linearization");
stats.add("linearizeProblem", t.reset()).format("ms");
// // compute pose jacobian norm squared for Jacobian scaling
// if (scale_Jp) {
// Jp_column_norm2 = lqr->getJp_diag2();
// stats.add("getJp_diag2", t.reset()).format("ms");
// }
// // scale landmark jacobians
// if (scale_Jl) {
// lqr->scaleJl_cols();
// stats.add("scaleJl_cols", t.reset()).format("ms");
// }
// marginalize points in place
//利用surhur QR 消元边缘化掉 landmark 增量，得到 reduced camera system
lqr->performQR();
stats.add("performQR", t.reset()).format("ms");
}
if (config.vio_debug) {
std::cout << "[LINEARIZE] Error: " << error_total
<< " num_landmarks=" << lmdb.numLandmarks()
<< " num_observations=" << lmdb.numObservations() << std::endl;
std::cout << "Iteration " << it << " " << error_total << std::endl;
}
// compute pose jacobian scaling
// VecX jacobian_scaling;
// if (scale_Jp) {
// // TODO: what about jacobian scaling for SC solver?
// // ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
// // we use 1.0 / (eps + sqrt(SquaredColumnNorm))
// jacobian_scaling = (lqr_options.lb_options.jacobi_scaling_eps +
// Jp_column_norm2.array().sqrt())
// .inverse();
// }
// if (config.vio_debug) {
// std::cout << "\t[INFO] Stage 1" << std::endl;
//}
// inner loop for backtracking in LM (still count as main iteration
// though)
// step 12.4.2 LM 内层回溯求解
for (int j = 0; it <= config.vio_max_iterations && !terminated; j++) {
if (j > 0) {
timer_iteration.reset();
if (config.vio_debug) {
std::cout << "Iteration " << it << ", backtracking" << std::endl;
}
}
{
// Timer t;
// TODO: execution could be done staged
// // set (updated) damping for poses
// if (config.vio_lm_pose_damping_variant == 0) {
// lqr->setPoseDamping(lambda);
// stats.add("setPoseDamping", t.reset()).format("ms");
// }
// // scale landmark Jacobians only on the first inner
// iteration. if (scale_Jp && j == 0) {
// lqr->scaleJp_cols(jacobian_scaling);
// stats.add("scaleJp_cols", t.reset()).format("ms");
// }
// // set (updated) damping for landmarks
// if (config.vio_lm_landmark_damping_variant == 0) {
// lqr->setLandmarkDamping(lambda);
// stats.add("setLandmarkDamping", t.reset()).format("ms");
// }
}
// if (config.vio_debug) {
// std::cout << "\t[INFO] Stage 2 " << std::endl;
// }
VecX inc;
{
Timer t;
// step 12.4.2.1 组装并求解线性方程组
// get dense reduced camera system
MatX H;
VecX b;
lqr->get_dense_H_b(H, b);//拿到消元后的致密 Hessian 和右端项。这里 H 已经是 只关于相机/状态变量 的 reduced Hessian。
stats.add("get_dense_H_b", t.reset()).format("ms");
int iter = 0;
bool inc_valid = false;
constexpr int max_num_iter = 3;
//最多试 3 次，直到解出来的增量 inc 数值有效。
while (iter < max_num_iter && !inc_valid) {
VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
MatX H_copy = H;
H_copy.diagonal() += Hdiag_lambda;
// 这里构造的是 LM 系统： (H + lambda * diag(H)) * inc = b
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H_copy);
inc = ldlt.solve(b);//LDLT 分解解线性系统。
stats.add("solve", t.reset()).format("ms");
if (!inc.array().isFinite().all()) {
lambda = lambda_vee * lambda;
lambda_vee *= vee_factor;
} else {
inc_valid = true;
}
iter++;
}//如果解出来有 NaN / Inf，说明阻尼太小或系统病态，就增大 lambda 再试。
if (!inc_valid) {
std::cerr << "Still invalid inc after " << max_num_iter
<< " iterations." << std::endl;
}
}
// step 12.4.3 保存状态并回代 landmark 增量
// backup state (then apply increment and check cost decrease)
backup();
// backsubstitute (with scaled pose increment)
Scalar l_diff = 0;
{
// negate pose increment before point update// 内部 b 的定义和 applyInc 所需方向相反，所以在应用前取负号
inc = -inc;
Timer t;
l_diff = lqr->backSubstitute(inc);
stats.add("backSubstitute", t.reset()).format("ms");
}
// step 12.4.4 应用增量到状态
// undo jacobian scaling before applying increment to poses
// if (scale_Jp) {
// inc.array() *= jacobian_scaling.array();
// }
// apply increment to poses //把大增量向量里属于每个 pose 的那一段取出来，应用到 pose 上。
for (auto& [frame_id, state] : frame_poses) {
int idx = aom.abs_order_map.at(frame_id).first;
state.applyInc(inc.template segment<POSE_SIZE>(idx));
}
for (auto& [frame_id, state] : frame_states) {
int idx = aom.abs_order_map.at(frame_id).first;
state.applyInc(inc.template segment<POSE_VEL_BIAS_SIZE>(idx));
}
// compute stepsize //用无穷范数看增量大小,作为收敛判据之一。
Scalar step_norminf = inc.array().abs().maxCoeff();
// step 12.4.5 重新计算代价
// compute error update applying increment 应用增量后重新计算真实误差
Scalar after_update_marg_prior_error = 0;
Scalar after_update_vision_and_inertial_error = 0;
// 1)视觉 + 某些惯性相关误差, 2)边缘化先验误差,重新用非线性模型算真实误差，不是只看线性近似
{
Timer t;
computeError(after_update_vision_and_inertial_error);
computeMargPriorError(marg_data, after_update_marg_prior_error);
Scalar after_update_imu_error = 0, after_bg_error = 0,
after_ba_error = 0;
ScBundleAdjustmentBase<Scalar>::computeImuError(
aom, after_update_imu_error, after_bg_error, after_ba_error,
frame_states, imu_meas, gyro_bias_sqrt_weight.array().square(),
accel_bias_sqrt_weight.array().square(), g);
//显式计算 IMU 和 bias 项误差。
after_update_vision_and_inertial_error +=
after_update_imu_error + after_bg_error + after_ba_error;
stats.add("computerError2", t.reset()).format("ms");
}
// F(x+δx,l+δl)=F_{vision+imu+bias} +F_{marg prior}
Scalar after_error_total = after_update_vision_and_inertial_error +
after_update_marg_prior_error;
// step 12.4.6 评估步长与模型一致性
// check cost decrease compared to quadratic model cost
Scalar f_diff;
bool step_is_valid = false;
bool step_is_successful = false;
Scalar relative_decrease = 0;
{
// compute actual cost decrease 真实误差下降量：
f_diff = error_total - after_error_total;//若 f_diff > 0，说明真下降了。
relative_decrease = f_diff / l_diff;// 典型 LM / trust-region 思路中的 pho = actual_reduction / predicted_reduction
if (config.vio_debug) {
std::cout << "\t[EVAL] error: {:.4e}, f_diff {:.4e} l_diff {:.4e} "
"step_quality {:.2e} step_size {:.2e}\n"_format(
after_error_total, f_diff, l_diff,
relative_decrease, step_norminf);
}
// TODO: consider to remove assert. For now we want to test if we
// even run into the l_diff <= 0 case ever in practice
// BASALT_ASSERT_STREAM(l_diff > 0, "l_diff " << l_diff);
// l_diff <= 0 is a theoretical possibility if the model cost change
// is tiny and becomes numerically negative (non-positive). It might
// not occur since our linear systems are not that big (compared to
// large scale BA for example) and we also abort optimization quite
// early and usually don't have large damping (== tiny step size).
step_is_valid = l_diff > 0;//f_diff = 实际下降
step_is_successful = step_is_valid && relative_decrease > 0;//l_diff = 线性/二次模型预测下降
}
double iteration_time = timer_iteration.elapsed();
double cumulative_time = timer_total.elapsed();
stats.add("iteration", iteration_time).format("ms");
{
basalt::MemoryInfo mi;
if (get_memory_info(mi)) {
stats.add("resident_memory", mi.resident_memory);
stats.add("resident_memory_peak", mi.resident_memory_peak);
}
}
// step 12.4.7 接受或拒绝步长
if (step_is_successful) {
BASALT_ASSERT(step_is_valid);
if (config.vio_debug) {
// std::cout << "\t[ACCEPTED] lambda:" << lambda
// << " Error: " << after_error_total <<
// std::endl;
std::cout << "\t[ACCEPTED] error: {:.4e}, lambda: {:.1e}, it_time: "
"{:.3f}s, total_time: {:.3f}s\n"
""_format(after_error_total, lambda, iteration_time,
cumulative_time);
}
//如果这一步效果不错，就减小阻尼。ρ 大，说明模型预测靠谱，可以更相信 GN，减小阻尼;ρ 小，说明模型预测不靠谱，要更保守地靠近梯度下降，增大阻尼。
lambda *= std::max<Scalar>(
Scalar(1.0) / 3,
1 - std::pow<Scalar>(2 * relative_decrease - 1, 3));
lambda = std::max(min_lambda, lambda);
lambda_vee = initial_vee;//重置阻尼放大倍率。
it++;
// check function and parameter tolerance 收敛判据 1) 误差下降量足够小，2) 步长足够小
if ((f_diff > 0 && f_diff < Scalar(1e-6)) ||
step_norminf < Scalar(1e-4)) {
converged = true;
terminated = true;
}
// stop inner lm loop
break;
} else {
std::string reason = step_is_valid ? "REJECTED" : "INVALID";
if (config.vio_debug) {
// std::cout << "\t[REJECTED] lambda:" << lambda
// << " Error: " << after_error_total <<
// std::endl;
std::cout << "\t[{}] error: {}, lambda: {:.1e}, it_time:"
"{:.3f}s, total_time: {:.3f}s\n"
""_format(reason, after_error_total, lambda,
iteration_time, cumulative_time);
}
lambda = lambda_vee * lambda;
lambda_vee *= vee_factor;
// lambda = std::max(min_lambda, lambda);
// lambda = std::min(max_lambda, lambda);
restore();
it++;
it_rejected++;
if (lambda > max_lambda) {
terminated = true;
message =
"Solver did not converge and reached maximum damping lambda";
}
}
}
}
// step 12.5 统计与收尾
stats.add("optimize", timer_total.elapsed()).format("ms");
stats.add("num_it", it).format("count");
stats.add("num_it_rejected", it_rejected).format("count");
// TODO: call filterOutliers at least once (also for CG version)
stats_all_.merge_all(stats);
stats_sums_.merge_sums(stats);
if (config.vio_debug) {
if (!converged) {
if (terminated) {
std::cout << "Solver terminated early after {} iterations: {}"_format(
it, message);
} else {
std::cout
<< "Solver did not converge after maximum number of {} iterations"_format(
it);
}
}
stats.print();
std::cout << "=================================" << std::endl;
}
}
}