SLAM因子边缘化算法（eroam实现）深度解析

1. 问题陈述

SLAM后端优化的信息矩阵（Hessian矩阵）对应滑动窗口内的所有状态变量，当滑动窗口移动时，需要剔除窗口外的旧状态变量，同时保留旧状态对剩余状态的约束关系，避免优化信息丢失。该算法解决的问题为：给定对称的\(n \times n\)信息矩阵\(H\)，以及待边缘化的连续变量块对应的列索引区间\([start, end]\)，输出与\(H\)同维度的边缘化后信息矩阵，待边缘化块对应的行、列全部置零，剩余保留块的约束关系与原矩阵的舒尔补结果一致。

2. 核心直觉

直接删除待边缘化变量对应的矩阵行和列的朴素方法会丢失待边缘化变量关联的保留变量间的约束，导致优化结果精度下降甚至发散。 边缘化的过程相当于把中间变量作为连接两端保留变量的桥梁，拆除桥梁的同时将桥梁两端的约束直接绑定，而非直接删除桥梁和所有关联连接。该实现的核心洞察是通过矩阵重排将任意位置的待边缘化块移动到矩阵右下角，再通过舒尔补将待边缘化变量的约束投影到剩余保留变量上，最后逆重排回原矩阵维度，不需要上层提前调整状态顺序。

3. 形式化定义

设原信息矩阵\(H\)对应的状态向量为\(\boldsymbol{x} = [\boldsymbol{x}_a^T, \boldsymbol{x}_b^T, \boldsymbol{x}_c^T]^T\)，其中\(\boldsymbol{x}_a\)为待边缘化块前的保留状态，\(\boldsymbol{x}_b\)为待边缘化状态，\(\boldsymbol{x}_c\)为待边缘化块后的保留状态。\(H\)的分块形式为：

4. 算法流程

4.1 伪代码

function Marginalize(H, start, end):
    // 输入：对称Hessian矩阵H，待边缘化块的列起始索引start，结束索引end
    // 输出：边缘化后的同大小Hessian矩阵
    a = start                          // 前序保留块的大小
    b = end - start + 1                // 待边缘化块的大小
    c = H.cols() - end - 1             // 后序保留块的大小

    // 步骤1：矩阵重排，将待边缘化块移到右下角
    初始化Hn为与H同大小的零矩阵
    if a > 0:
        Hn[0:a, 0:a] = H[0:a, 0:a]
        Hn[0:a, a+c:a+c+b] = H[0:a, a:a+b]
        Hn[a+c:a+c+b, 0:a] = H[a:a+b, 0:a]
    if a > 0 and c > 0:
        Hn[0:a, a:a+c] = H[0:a, a+b:a+b+c]
        Hn[a:a+c, 0:a] = H[a+b:a+b+c, 0:a]
    if c > 0:
        Hn[a:a+c, a:a+c] = H[a+b:a+b+c, a+b:a+b+c]
        Hn[a:a+c, a+c:a+c+b] = H[a+b:a+b+c, a:a+b]
        Hn[a+c:a+c+b, a:a+c] = H[a:a+b, a+b:a+b+c]
    Hn[a+c:a+c+b, a+c:a+c+b] = H[a:a+b, a:a+b]

    // 步骤2：计算待边缘化块的伪逆
    对Hn右下角b*b块做SVD分解，得到U, S, V
    初始化S_inv为与S同大小的向量
    for i from 0 to b-1:
        if S[i] > 1e-6:
            S_inv[i] = 1 / S[i]
        else:
            S_inv[i] = 0
    invHb = V * diag(S_inv) * U^T

    // 步骤3：计算舒尔补
    Hn[0:a+c, 0:a+c] = Hn[0:a+c, 0:a+c] - Hn[0:a+c, a+c:a+c+b] * invHb * Hn[a+c:a+c+b, 0:a+c]
    将Hn中待边缘化块对应的行和列全部置零

    // 步骤4：逆重排回原顺序
    初始化res为与H同大小的零矩阵
    if a > 0:
        res[0:a, 0:a] = Hn[0:a, 0:a]
        res[0:a, a:a+b] = Hn[0:a, a+c:a+c+b]
        res[a:a+b, 0:a] = Hn[a+c:a+c+b, 0:a]
    if a > 0 and c > 0:
        res[0:a, a+b:a+b+c] = Hn[0:a, a:a+c]
        res[a+b:a+b+c, 0:a] = Hn[a:a+c, 0:a]
    if c > 0:
        res[a+b:a+b+c, a+b:a+b+c] = Hn[a:a+c, a:a+c]
        res[a+b:a+b+c, a:a+b] = Hn[a:a+c, a+c:a+c+b]
        res[a:a+b, a+b:a+b+c] = Hn[a+c:a+c+b, a:a+c]
    res[a:a+b, a:a+b] = Hn[a+c:a+c+b, a+c:a+c+b]

    return res

4.2 执行流程图

5. 复杂度分析

时间复杂度

核心运算分为三部分：

1. 两次矩阵重排的总复杂度为\(O(n^2)\)，\(n\)为输入矩阵的维度；
2. \(b \times b\)矩阵的SVD分解复杂度为\(O(b^3)\)，\(b\)为待边缘化块的大小；
3. 舒尔补计算包含三次矩阵乘法，总复杂度为\(O(b(a+c)^2)\)，其中\(a+c\)为保留状态的总维度。 最好情况为\(b=1\)（边缘化单个状态变量），复杂度为\(O(n^2)\)；最坏情况为\(b \approx n\)（边缘化绝大多数状态变量），复杂度为\(O(n^3)\)；SLAM滑动窗口场景下，每次边缘化的变量数\(b\)固定（通常为10~20），因此平均复杂度为\(O(n^2)\)。

空间复杂度

总空间复杂度为\(O(n^2)\)，仅需要存储两个与输入矩阵同大小的临时矩阵Hn和res，无额外高阶空间开销。

6. 实现说明

该实现与理论推导的差异主要为工程适配优化：

1. 理论上边缘化后仅需要返回\((a+c) \times (a+c)\)的压缩矩阵，该实现返回与输入同维度的矩阵，待边缘化块对应位置全部置零，允许上层代码复用固定大小的Hessian矩阵内存，避免频繁动态内存分配，更适合嵌入式实时场景。
2. 代码未使用Cholesky分解计算\(H\_{bb}\)的逆，而是采用SVD分解求伪逆，奇异值阈值1e-6为工程经验值，可过滤退化场景下的近零奇异值，避免数值溢出，在观测退化场景下仍能稳定运行。
3. 重排步骤拆分了\(a>0\)、\(c>0\)的分支处理，可覆盖待边缘化块位于矩阵开头（\(a=0\)）、末尾（\(c=0\)）、中间的所有场景，避免越界访问。
4. 所有操作维持了输出矩阵的对称性，符合信息矩阵的数学性质，不需要额外做对称化处理。

7. 对比分析

与经典SLAM边缘化实现的差异

1. 调用灵活度：VINS-Mono等经典框架的边缘化实现要求上层提前将待边缘化变量调整到状态向量的两端，该实现将重排逻辑封装在函数内部，支持任意位置的状态块边缘化，上层调用无需调整状态顺序，灵活度更高。
2. 数值稳定性：经典实现多采用Cholesky分解求逆，在非退化场景下运算速度比SVD快2~3倍，但退化场景下会分解失败导致程序崩溃，该实现用SVD伪逆替换Cholesky分解，牺牲约30%的运算速度，换来退化场景下的稳定性，更适合环境特征不足的定位场景。
3. 内存开销：多数经典实现会返回压缩后的低维矩阵，上层需要每次调整Hessian矩阵维度，该实现返回固定大小的矩阵，在滑动窗口固定大小的场景下可减少60%以上的内存分配开销，更适合嵌入式平台部署。