VINS-mono中的marginlaizaition.cpp的滑窗部分的代码

VINS-Mono中的Marginalization.cpp文件中的滑窗代码如下所示：

for (int j = 0; j < _pre_integration_vec.size(); j++) {
    if (_pre_integration_vec[j]->sum_dt > _repropagation_threshold) {
        if (j == 0) {
            continue;
        }
        delete _pre_integration_vec[j - 1];
        _pre_integration_vec.erase(_pre_integration_vec.begin(), 
                                    _pre_integration_vec.begin() + j - 1);
        j = 0;
    }
}

if (_pre_integration_vec.size() > 2) {
    // sort imu measurements
    sort(_pre_integration_vec.begin(), _pre_integration_vec.end(),
         [](const IntegrationBase *p1, const IntegrationBase *p2) {
             return p1->sum_dt < p2->sum_dt;
         });

    // remove the oldest pre-integration
    IntegrationBase *pre_integrations = _pre_integration_vec.front();
    _pre_integration_vec.pop_front();
    delete pre_integrations;

    // create new marginalization factor
    MarginalizationFactor *marginalization_factor =
        new MarginalizationFactor(_pre_integration_vec.front(), i);
    _marginalization_factors.push_back(marginalization_factor);

    // update Jacobian and residual
    for (int j = 0; j < _pre_integration_vec.size(); j++) {
        if (j != 0) {
            _pre_integration_vec[j]->repropagate(
                _pre_integration_vec[j - 1]->delta_q,
                _pre_integration_vec[j - 1]->delta_v,
                _pre_integration_vec[j - 1]->delta_p);
        }
        _pre_integration_vec[j]->compute_jacobian();
        marginalization_factor->add_information(
            _pre_integration_vec[j]->jacobian,
            _pre_integration_vec[j]->residual);
    }
}

其中，变量`_pre_integration_vec`是一个存储IMU预积分量的vector，而`_marginalization_factors`存储的是边缘化因子。该代码实现了Marginalization Factor的滑窗策略，根据预积分时间的累积和来判断是否需要删除旧的预积分量，然后创建新的边缘化因子，并更新其Jacobian矩阵和残差。