滑动窗口模型与边缘化

1.边缘化的需求来源

2.图优化模型

3.边缘化实现原理

4.一个滑动窗口模型实例

[4.1 将节点与约束保存到队列](#4.1 将节点与约束保存到队列)

[4.1.1 外部调用](#4.1.1 外部调用)

[4.1.2 保存节点到节点队列](#4.1.2 保存节点到节点队列)

[4.1.3 保存地图匹配先验位姿约束到队列](#4.1.3 保存地图匹配先验位姿约束到队列)

[4.1.4 保存帧间里程计约束到队列：](#4.1.4 保存帧间里程计约束到队列：)

[4.1.5 保存帧间imu积分约束到队列](#4.1.5 保存帧间imu积分约束到队列)

[4.2 窗口滑动，将窗口范围内的节点添加进优化器](#4.2 窗口滑动，将窗口范围内的节点添加进优化器)

[4.3 当窗口正常滑动时，构造边缘化先验因子](#4.3 当窗口正常滑动时，构造边缘化先验因子)

[4.4 特征分解得到边缘化先验因子对应的雅可比矩阵与残差](#4.4 特征分解得到边缘化先验因子对应的雅可比矩阵与残差)

注：本篇笔记的主体内容来源于对深蓝学院《多传感器融合定位》课程的学习，推荐有一定基础的 SLAM 初学者学习该课程。课程中提供了一个完整的滑动窗口优化框架，可供体会从理论到代码实现的过程。

1.边缘化的需求来源

边缘化的需求来源十分清晰。如下图是一个因子图优化模型：

上图中有两类节点（待优化状态）：1）雷达位姿T 和 2）imu的bias、速度 。这二者也可以放在同一个节点状态中表示：，其中p为位置、q为姿态、v为速度、ba为加速度计bias、bw为陀螺仪bias。此时，先将其分为两类节点来表示。

上图中有三类约束，分别为：1）激光里程计因子 （两个雷达帧之间的相对位姿）作为一个二元边约束两个相邻雷达位姿；2）imu积分因子 作为一个四元边约束两个相邻雷达帧位姿与两个相邻帧的imu的零偏和速度；3）先验位姿因子作为一个一元边约束一帧雷达位姿。

因为这个图优化模型的目的是进行在已有先验地图上的定位，所以这里的先验位姿因子是scan to map得到的全局位姿。待优化节点中的lidar位姿是scan to local map得到的一个有轻微漂移的位姿。在建图过程中，该一元边约束也可以改为GPS位置，来约束雷达节点的位置。

随着时间的进行，图中添加的节点（待优化状态）和边（节点的约束）的数量越来越多，图模型会越来越大，导致优化问题求解耗时递增，无法达到实时性。这一点在课程第9章的代码运行时得到了充分地体现。

在8分钟2千米的SLAM过程中，当没有为优化器提供雅可比的解析式求导时，即优化器使用自动求导时，后端完成一次优化迭代的耗时从1、2秒增长到20多秒；当提供雅可比的解析式求导时，后端完成一次优化迭代的耗时会从0.015秒增长到2秒多。可见图模型大小对优化耗时的巨大影响。

一种解决思路就是维持一个滑动窗口，不断删除旧的帧（认为旧帧已经得到了足够的优化），只优化最新的几帧，从而减少优化问题求解的耗时。滑动窗口模型的表示如下：

如上图，由于激光里程计因子和imu预积分因子连接了待删除的旧帧和窗口中的保留帧，如果直接将待删除的旧帧与其相关的约束边从图模型中删除，会导致系统割裂，损失信息。

解决办法就是通过"边缘化"的操作把旧帧的约束传递下来，作为一个"边缘化先验因子"继续参与优化，约束窗口中剩余的所有与被边缘化变量曾有共同约束的状态变量。

2.图优化模型

按照高斯牛顿法求解非线性最小二乘问题的流程，图优化问题的求解模型可以表示为：

其中，为第i 个约束产生的残差，为残差关于状态量变化量的雅可比矩阵，为第 i 个约束的信息矩阵（权重）（协方差的逆）。模型也可以表示为：

先看下面这个执行边缘化之前的图优化模型：

其中残差类型包括：

1）地图匹配先验位姿对优化变量的残差 ；

2）激光里程计相对位姿对优化变量的残差 ；

3）IMU积分项对优化变量的残差 ；

4）边缘化形成的先验因子对应的残差 。

状态变量，其中，T 表示待优化节点位姿，M表示待优化节点对应的IMU零偏和速度等。其中：

（1）对于地图匹配先验因子，一个因子仅约束一个位姿：

上边右图为其雅可比矩阵的可视化，则其对应的Hessian矩阵可以表示为：

对应的b矩阵可以表示为：

（2）对于激光里程计因子，一个因子约束两个位姿：

上边右图为其雅可比矩阵的可视化，则其对应的Hessian矩阵可以表示为：

对应的b矩阵可以表示为：

（3）对于IMU因子，一个因子约束两个位姿，并约束两个时刻IMU的速度与bias：

上边右图为其雅可比矩阵的可视化，则其对应的Hessian矩阵可以表示为：

对应的b矩阵可以表示为：

优化模型完整的Hessian矩阵最终表示成累加形式：

即为以上各因子对应Hessian矩阵的累加：

上图中可视化时按照优先显示数量少的色块的原则。完整的b矩阵，即为以上各因子对应b矩阵的累加：

3.边缘化实现原理

假设此时认为第一帧的状态已经得到了充分的优化，要边缘化掉第一帧的状态，则图优化模型中的因子可以分为两类，一类是与有关的，如下图中用红色圆圈圈住的：

另一类是与没有关联的，如上图中用红色方框框住的。边缘化过程分为两步。

（1）第一步：因为因子间具有独立性，所以可以先使用和要边缘化掉的量无关的因子，构建剩余变量对应的Hessian矩阵：

（这里默认删除了对应的维度，不影响计算过程，并且Hessian矩阵的维度太大了就是要做边缘化的原因之一。）

（2）第二步：挑出和要边缘化掉的量相关的因子，构建待边缘化的Hessian矩阵，并使用舒尔补做边缘化提取出先验约束因子。

1）构建待边缘化的Hessian矩阵：

2）边缘化：

对于，可拆解为如下形式：

其中，对应要边缘化掉的。拆解的目的是通过一系列操作，把从状态量里删除掉，并将与它相关的因子的约束保留下来。

借助舒尔补理论对矩阵H进行三角化，即：

化简可得：

此时不想再优化，那就只计算，可以利用等式第2行直接得到：

这一步形成的约束（上式）就叫先验因子，它包含了边缘化掉的量对剩余变量的约束关系。第二步过程可图形化表示为：

最终两步叠加，Hessian矩阵叠加的可视化如下：

对应的完整公式为：

其中：

4.一个滑动窗口模型实例

课程作业中的图优化模型如上图所示，其中，为状态节点，代表地图匹配先验位姿约束，代表关键帧间相对位姿约束，代表关键帧间IMU预积分约束。这三种约束都会产生残差，并与节点计算雅可比：

则每种约束会对每个节点计算雅可比：

三种约束对应的Hessian矩阵的形式为（暂时忽略信息矩阵，即权重）：

叠加得到的完整Hessian矩阵形式为：

计算边缘化先验因子的Hessian矩阵：

在实际实现中，SLAM过程中产生的所有待优化状态变量（参数块）与约束（残差因子）都会被按顺序保存在各自的队列中。在窗口滑动时，每次会按照当前窗口的覆盖范围将参与优化的变量与约束重新添加到优化器中。所以，与待边缘化变量无关联的变量的Hessian矩阵 会交由优化器自己去计算 ，而边缘化先验因子的Hessian矩阵需要我们手动计算。

下面是部分代码实例：

4.1 将节点与约束保存到队列

4.1.1 外部调用

cpp 复制代码

bool SlidingWindow::Update(void) {
    static KeyFrame last_key_frame_ = current_key_frame_;

    // add node for new key frame pose
    sliding_window_ptr_->AddPRVAGParam(current_key_frame_);

    // get num. of vertices:
    const int N = sliding_window_ptr_->GetNumParamBlocks();
    // get param block ID, current:
    const int param_index_j = N - 1;

    // add unary constraints
    // a. map matching / GNSS position:
    if ( N > 0 && measurement_config_.source.map_matching ) {
        // get prior position measurement:
        Eigen::Matrix4d prior_pose = current_map_matching_pose_.pose.cast<double>();

        // add constraint, GNSS position:
        sliding_window_ptr_->AddPRVAGMapMatchingPoseFactor(param_index_j, prior_pose, 
                                                                measurement_config_.noise.map_matching);
    }

    // add binary constraints:
    if ( N > 1 ) {
        // get param block ID, previous:
        const int param_index_i = N - 2;
        
        // a. lidar frontend:
        // get relative pose measurement:
        Eigen::Matrix4d relative_pose = (last_key_frame_.pose.inverse() * current_key_frame_.pose).cast<double>();
        // TODO: add constraint, lidar frontend / loop closure detection:
        sliding_window_ptr_->AddPRVAGRelativePoseFactor(param_index_i, param_index_j, relative_pose, 
                                                            measurement_config_.noise.lidar_odometry);
        
        // b. IMU pre-integration:
        if ( measurement_config_.source.imu_pre_integration ) {
            // TODO: add constraint, IMU pre-integraion:
            sliding_window_ptr_->AddPRVAGIMUPreIntegrationFactor(param_index_i, param_index_j, 
                                                                    imu_pre_integration_);
        }
    }

    // move forward:
    last_key_frame_ = current_key_frame_;

    return true;
}

4.1.2 保存节点到节点队列

cpp 复制代码

void CeresSlidingWindow::AddPRVAGParam(
    const KeyFrame &lio_key_frame, const bool fixed
) {
    // create new key frame:
    OptimizedKeyFrame optimized_key_frame;

    // a. set timestamp:
    optimized_key_frame.time = lio_key_frame.time;

    // b. shall the param block be fixed:
    optimized_key_frame.fixed = fixed;

    // c. set init values:
    Eigen::Map<Eigen::Vector3d>     pos(optimized_key_frame.prvag + INDEX_P);
    Eigen::Map<Eigen::Vector3d> log_ori(optimized_key_frame.prvag + INDEX_R);
    Eigen::Map<Eigen::Vector3d>     vel(optimized_key_frame.prvag + INDEX_V);
    Eigen::Map<Eigen::Vector3d>     b_a(optimized_key_frame.prvag + INDEX_A);
    Eigen::Map<Eigen::Vector3d>     b_g(optimized_key_frame.prvag + INDEX_G);

    // d. get values from lio_key_frame
    pos = lio_key_frame.pose.block<3, 1>(0, 3).cast<double>();

    Sophus::SO3d ori(
        Eigen::Quaterniond(lio_key_frame.pose.block<3, 3>(0, 0).cast<double>())
    );
    log_ori = ori.log();

    vel = ori * lio_key_frame.vel.v.cast<double>();
    b_a = lio_key_frame.bias.accel.cast<double>();
    b_g = lio_key_frame.bias.gyro.cast<double>();

    // add to data buffer:
    optimized_key_frames_.push_back(optimized_key_frame);
}

4.1.3 保存地图匹配先验位姿约束到队列

cpp 复制代码

void CeresSlidingWindow::AddPRVAGMapMatchingPoseFactor(
    const int param_index,
    const Eigen::Matrix4d &prior_pose, const Eigen::VectorXd &noise
) {
    // create new residual block:
    ResidualMapMatchingPose residual_map_matching_pose;

    // a. set param block ID:
    residual_map_matching_pose.param_index = param_index;

    // b. set measurement
    residual_map_matching_pose.m = Eigen::VectorXd::Zero(6);
    // b.1. position:
    residual_map_matching_pose.m.block<3, 1>(INDEX_P, 0) = prior_pose.block<3, 1>(0, 3);
    // b.2. orientation, so3:
    residual_map_matching_pose.m.block<3, 1>(INDEX_R, 0) = Sophus::SO3d(
        Eigen::Quaterniond(prior_pose.block<3, 3>(0, 0).cast<double>())
    ).log();

    // c. set information matrix:
    residual_map_matching_pose.I = GetInformationMatrix(noise);

    // add to data buffer:
    residual_blocks_.map_matching_pose.push_back(residual_map_matching_pose);
}

4.1.4 保存帧间里程计约束到队列：

cpp 复制代码

void CeresSlidingWindow::AddPRVAGRelativePoseFactor(
    const int param_index_i, const int param_index_j,
    const Eigen::Matrix4d &relative_pose, const Eigen::VectorXd &noise
) {
    // create new residual block:
    ResidualRelativePose residual_relative_pose;

    // a. set param block IDs:
    residual_relative_pose.param_index_i = param_index_i;
    residual_relative_pose.param_index_j = param_index_j;

    // b. set measurement
    residual_relative_pose.m = Eigen::VectorXd::Zero(6);
    // b.1. position:
    residual_relative_pose.m.block<3, 1>(INDEX_P, 0) = relative_pose.block<3, 1>(0, 3);
    // b.2. orientation, so3:
    residual_relative_pose.m.block<3, 1>(INDEX_R, 0) = Sophus::SO3d(
        Eigen::Quaterniond(relative_pose.block<3, 3>(0, 0).cast<double>())
    ).log();

    // c. set information matrix:
    residual_relative_pose.I = GetInformationMatrix(noise);

    // add to data buffer:
    residual_blocks_.relative_pose.push_back(residual_relative_pose);
}

4.1.5 保存帧间imu积分约束到队列

cpp 复制代码

void CeresSlidingWindow::AddPRVAGIMUPreIntegrationFactor(
    const int param_index_i, const int param_index_j,
    const IMUPreIntegrator::IMUPreIntegration &imu_pre_integration
) {
    // create new residual block:
    ResidualIMUPreIntegration residual_imu_pre_integration;

    // a. set param block IDs:
    residual_imu_pre_integration.param_index_i = param_index_i;
    residual_imu_pre_integration.param_index_j = param_index_j;

    // b. set measurement
    // b.1. integration interval:
    residual_imu_pre_integration.T = imu_pre_integration.GetT();
    // b.2. gravity constant:
    residual_imu_pre_integration.g = imu_pre_integration.GetGravity();
    // b.3. measurement:
    residual_imu_pre_integration.m = imu_pre_integration.GetMeasurement();
    // b.4. information:
    residual_imu_pre_integration.I = imu_pre_integration.GetInformation();
    // b.5. Jacobian:
    residual_imu_pre_integration.J = imu_pre_integration.GetJacobian();

    // add to data buffer:
    residual_blocks_.imu_pre_integration.push_back(residual_imu_pre_integration);
}

4.2 窗口滑动，将窗口范围内的节点添加进优化器

cpp 复制代码

bool CeresSlidingWindow::Optimize() {
    static int optimization_count = 0;
    
    // get key frames count:
    const int N = GetNumParamBlocks();
    if(N < 2) return true;

    // 当节点总数小于20时，将所有节点加入优化器
    // 当节点总数大于等于20时，只添加20个节点（窗口大小）到优化器
    static int k_ = 20;
    if(N < k_) kWindowSize = N;
    else kWindowSize = k_;

    // create new sliding window optimization problem:
    ceres::Problem problem;
    auto start = std::chrono::steady_clock::now();

    // a. add parameter blocks:
    // 从窗口末尾到窗口头部反向将参数块添加到优化器
    for ( int i = 0; i < kWindowSize; ++i) {
        auto &target_key_frame = optimized_key_frames_.at(N - i - 1);

        ceres::LocalParameterization *local_parameterization = new sliding_window::ParamPRVAG();

        // add parameter block:
        problem.AddParameterBlock(target_key_frame.prvag, 15, local_parameterization);
    }
    // 当队列较短时，将窗口中第一个节点设置为固定
    if(N <= k_) problem.SetParameterBlockConstant(optimized_key_frames_.front().prvag);

因为窗口始终覆盖队列的尾部，所以可以从队列尾部开始，反向将节点添加到优化器中。这个操作不会造成Ceres错误识别节点的顺序：

唯一标识符是内存地址： Ceres 是通过你传入的 double* 指针（即代码里的 target_key_frame.prvag 数组的内存首地址）来唯一识别状态变量的。

图的连接：当你调用 AddResidualBlock 时，Ceres 仅仅是在内部记录"这个残差函数连着这几个 double* 地址"。

只要你的内存地址没乱，无论你先告诉 Ceres 参数在哪，还是先告诉 Ceres 约束在哪，最终构建出来的雅可比矩阵（Jacobian）结构和Hessian矩阵都是完全一模一样的。

4.3 当窗口正常滑动时，构造边缘化先验因子

cpp 复制代码

    // 用于存储上一帧的边缘化先验因子
    static struct MInfo last_marg_info;

    // add residual blocks:
    // b.1. marginalization constraint:
    if (N > k_) {                       // 队列长度大于窗口长度
        int marg_index = N - kWindowSize - 1;
        auto &key_frame_m = optimized_key_frames_.at(marg_index);  // 要被边缘化的帧
        auto &key_frame_r = optimized_key_frames_.at(marg_index + 1);

        sliding_window::FactorPRVAGMarginalization *factor_marginalization = nullptr;
        // 如果上一帧有边缘化先验因子产生，则用其来初始化本帧的边缘化先验因子
        // （边缘化先验因子的传递，参与后续优化）
        if(last_marg_info.valid)
        {
            factor_marginalization = new sliding_window::FactorPRVAGMarginalization(
                last_marg_info.H_prior, last_marg_info.b_prior, 
                last_marg_info.x_0,     key_frame_m.prvag
            );
        }
        else factor_marginalization = new sliding_window::FactorPRVAGMarginalization();

        if(!residual_blocks_.map_matching_pose.empty() && 
            residual_blocks_.map_matching_pose.front().param_index == marg_index)
        {
            sliding_window::FactorPRVAGMapMatchingPose *factor_map_matching_pose = GetResMapMatchingPose(
                residual_blocks_.map_matching_pose.front()
            );
            factor_map_matching_pose->has_fej_ = false;
            factor_marginalization->SetResMapMatchingPose(
                factor_map_matching_pose, 
                std::vector<double *>{key_frame_m.prvag}
            );

            residual_blocks_.map_matching_pose.pop_front();
            delete factor_map_matching_pose;
        }
        if(!residual_blocks_.relative_pose.empty() && 
            residual_blocks_.relative_pose.front().param_index_i == marg_index)
        {
            sliding_window::FactorPRVAGRelativePose *factor_relative_pose = GetResRelativePose(
                residual_blocks_.relative_pose.front()
            );
            factor_relative_pose->has_fej_ = false;
            factor_marginalization->SetResRelativePose(
                factor_relative_pose,
                std::vector<double *>{key_frame_m.prvag, key_frame_r.prvag}
            );

            residual_blocks_.relative_pose.pop_front();
            delete factor_relative_pose;
        }
        if(!residual_blocks_.imu_pre_integration.empty() && 
            residual_blocks_.imu_pre_integration.front().param_index_i == marg_index)
        {
            sliding_window::FactorPRVAGIMUPreIntegration *factor_imu_pre_integration = GetResIMUPreIntegration(
                residual_blocks_.imu_pre_integration.front()
            );
            factor_imu_pre_integration->has_fej_ = false;
            factor_marginalization->SetResIMUPreIntegration(
                factor_imu_pre_integration,
                std::vector<double *>{key_frame_m.prvag, key_frame_r.prvag}
            );

            residual_blocks_.imu_pre_integration.pop_front();
            delete factor_imu_pre_integration;
        }

        // 执行边缘化
        factor_marginalization->Marginalize(key_frame_r.prvag);

        // 缓存本次边缘化结果
        last_marg_info.valid = true;
        last_marg_info.H_prior = factor_marginalization->GetHPrior();
        last_marg_info.b_prior = factor_marginalization->GetbPrior();
        last_marg_info.x_0 = factor_marginalization->GetX0();

        // add marginalization factor into sliding window
        problem.AddResidualBlock(
            factor_marginalization,
            NULL,
            key_frame_r.prvag
        );
    }

（1）构建待边缘化的Hessian矩阵：

先获取窗口中最旧帧（待边缘化）相关的约束；

再获取约束对应的残差及雅可比矩阵。代码中巧妙地运用了运行时多态，用了一个静态Evaluate()函数以及struct ResidualBlockInfo实现了，对三种不同约束（CostFunction）调用其Evaluate()函数计算对应的残差以及雅可比；

再构建：

①先验位姿约束

因为这里计算Hessian矩阵和b矩阵只是为了后续需要，所以不对b矩阵加负号。

②关键帧间相对位姿约束

③帧间imu预积分约束

（2）边缘化****：

构建先验因子：

4.4 特征分解得到边缘化先验因子对应的雅可比矩阵与残差

因为优化器需要的是雅可比和残差，所以需要根据上面计算得到的Hessian矩阵和b矩阵，反推出边缘化先验因子对窗口中剩余的相关状态的雅可比和残差：

边缘化先验因子对状态x的优化影响可以表示为（上述计算b是没有加负号，这里加上）：

边缘化先验因子得到的Hessian矩阵、b矩阵，以及待分解得到的Jacobian矩阵和残差r可以表示如下：

因为是一个对称半正定矩阵，所以可以对其进行特征值分解：

其中，是特征向量组成的矩阵，是对角线上为特征值的对角矩阵。可以将上面的分解拆开：

则雅可比矩阵为：

对于b矩阵：

两边左乘，因为是正交矩阵，所以：

此时，便得到了边缘化先验因子在"边缘化发生的这一瞬间的"处的雅可比和残差（只有与被边缘化掉的有关联）。