VINS marginalization源码分析

理论知识：
SLAM中的marginalization 和 Schur complement
深入理解SLAM中的Marginalization
VINS6边缘化
DSO 中的Windowed Optimization

1. 边缘化留下的先验信息有哪些？

//VINS边缘化有两个策略，如果在sliding window中第二近的frame是关键帧则丢弃sliding window中最老的帧、否则丢弃该帧。无论丢弃哪一帧，都需要边缘化。
    if (marginalization_flag == MARGIN_OLD)//丢弃老的一帧。
    {
        MarginalizationInfo *marginalization_info = new MarginalizationInfo();
        vector2double();
 
        if (last_marginalization_info)  //上一次的边缘化信息要留下来
        {
            vector<int> drop_set;
            for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
            {
                if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
                    last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
                    drop_set.push_back(i);
            }
            // construct new marginlization_factor
            MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                           last_marginalization_parameter_blocks,
                                                                           drop_set);
 
            marginalization_info->addResidualBlockInfo(residual_block_info);//添加上一次边缘化的变量
        }
 
        {
            if (pre_integrations[1]->sum_dt < 10.0)   //IMU预积分留下的边缘化信息
            {
                IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL,
                                                                           vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},
                                                                           vector<int>{0, 1});
                marginalization_info->addResidualBlockInfo(residual_block_info);//添加优化变量
            }
        }
 
        {
            int feature_index = -1;
            for (auto &it_per_id : f_manager.feature)  //共视点留下的边缘化信息
            {
                it_per_id.used_num = it_per_id.feature_per_frame.size();
                if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
                    continue;
 
                ++feature_index;
 
                int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
                if (imu_i != 0)  //如果该点没在被边缘化的帧上观察到，则不会留下先验信息
                    continue;
 
                Vector3d pts_i = it_per_id.feature_per_frame[0].point;
 
                for (auto &it_per_frame : it_per_id.feature_per_frame)
                {
                    imu_j++;
                    if (imu_i == imu_j)
                        continue;
 
                    Vector3d pts_j = it_per_frame.point;
                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                    ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
                                                                                   vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
                                                                                   vector<int>{0, 3});
                    marginalization_info->addResidualBlockInfo(residual_block_info);//添加优化变量
                }
            }
        }
 
        TicToc t_pre_margin;
        marginalization_info->preMarginalize();
        ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
        
        TicToc t_margin;
        marginalization_info->marginalize();
        ROS_DEBUG("marginalization %f ms", t_margin.toc());
 
        std::unordered_map<long, double *> addr_shift;
        for (int i = 1; i <= WINDOW_SIZE; i++)
        {
            addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
            addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
        }
        for (int i = 0; i < NUM_OF_CAM; i++)
            addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
 
        vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
 
        if (last_marginalization_info)
            delete last_marginalization_info;
        last_marginalization_info = marginalization_info;
        last_marginalization_parameter_blocks = parameter_blocks;
        
    }
    else
    {
        if (last_marginalization_info &&
            std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
        {
 
            MarginalizationInfo *marginalization_info = new MarginalizationInfo();
            vector2double();
            if (last_marginalization_info)
            {
                vector<int> drop_set;
                for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)
                {
                    ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
                    if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])
                        drop_set.push_back(i);
                }
                // construct new marginlization_factor
                MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
                ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                               last_marginalization_parameter_blocks,
                                                                               drop_set);
 
                marginalization_info->addResidualBlockInfo(residual_block_info);
            }
 
            TicToc t_pre_margin;
            ROS_DEBUG("begin marginalization");
            marginalization_info->preMarginalize();
            ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());
 
            TicToc t_margin;
            ROS_DEBUG("begin marginalization");
            marginalization_info->marginalize();
            ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
            
            std::unordered_map<long, double *> addr_shift;
            for (int i = 0; i <= WINDOW_SIZE; i++)
            {
                if (i == WINDOW_SIZE - 1)
                    continue;
                else if (i == WINDOW_SIZE)
                {
                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
                }
                else
                {
                    addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
                    addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
                }
            }
            for (int i = 0; i < NUM_OF_CAM; i++)
                addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
 
            vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
            if (last_marginalization_info)
                delete last_marginalization_info;
            last_marginalization_info = marginalization_info;
            last_marginalization_parameter_blocks = parameter_blocks;
            
        }
    }
    ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc());
    
    ROS_DEBUG("whole time for ceres: %f", t_whole.toc());
}

从代码中可以看出，某帧(最老帧或者第二新的帧)要被边缘化掉时， 该帧要被丢弃掉， 如果是最老帧，IMU预积分会给与之相邻的帧留下先验信息，该帧的观测会给与之有共视关系的帧留下先验信息，如果某帧既不是与之有IMU预积分约束的相邻帧，也没有共视点，则不会给该帧留下先验信息。如果边缘化最近的第二帧，则只留下上次的边缘化信息，IMU预积分传给下一帧，共视点的先验信息则不会留下。传到marginalization_info里的残差块与optimization时建立的残差块很类似。

2. 在marginalization中加入残差块：

void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{
    factors.emplace_back(residual_block_info);
    std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks; //状态窗口内的所有状态量
    std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();
    // Cost function signature metadata: number of inputs & their sizes,

    for (int i = 0; i < static_cast<int>(residual_block_info->parameter_blocks.size()); i++)
    {
        double *addr = parameter_blocks[i];
        int size = parameter_block_sizes[i];
        parameter_block_size[reinterpret_cast<long>(addr)] = size;  //所有状态参数变量块
    }

    for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)
    {
        double *addr = parameter_blocks[residual_block_info->drop_set[i]];
        parameter_block_idx[reinterpret_cast<long>(addr)] = 0;  //要被marginalize掉的状态参数块
    }
}

将残差块加入MarginalizationInfo， 同时将要丢弃的参数块记录下来。

3. marginalization预处理：
   void MarginalizationInfo::preMarginalize()
   {
   for (auto it : factors)
   { //factors有IMU预积分残差块imu_factor,视觉重投影残差块ProjectionTwoFrameOneCamFactor和marginalization_factor
   it->Evaluate(); //计算残差块的残差及雅克比矩阵
   std::vector block_sizes = it->cost_function->parameter_block_sizes(); //比如imu_factor就涉及到前后两个相机位姿
   for (int i = 0; i < static_cast(block_sizes.size()); i++)
   {
   long addr = reinterpret_cast(it->parameter_blocks[i]);
   int size = block_sizes[i];
   if (parameter_block_data.find(addr) == parameter_block_data.end())
   {//如果当前parameter_block_data里没有it->parameter_blocks,就在parameter_block_data里加入该参数块
   double *data = new double[size];
   memcpy(data, it->parameter_blocks[i], sizeof(double) * size);
   parameter_block_data[addr] = data; //addr是地址,data是地址存放的数据
   }
   }
   }
   }
   对每个残差块计算残差和雅可比矩阵。

4. 进行Marginalization:

void MarginalizationInfo::marginalize()
{
    int pos = 0;
    for (auto &it : parameter_block_idx) //要被marginalize掉的状态量:一个位姿+一堆特征点
    {
        it.second = pos;
        pos += localSize(parameter_block_size[it.first]);
    }

    m = pos;
    for (const auto &it : parameter_block_size)  //要保留的状态量?
    {
        if (parameter_block_idx.find(it.first) == parameter_block_idx.end())//不在被marginalize掉的状态里的状态
        {
            parameter_block_idx[it.first] = pos;                    //此处记录了参数块将来在H,b矩阵中的位置
            pos += localSize(it.second);
        }
    }

    n = pos - m;
    //ROS_INFO("marginalization, pos: %d, m: %d, n: %d, size: %d", pos, m, n, (int)parameter_block_idx.size());
    if(m == 0)
    {
        valid = false;
        printf("unstable tracking...\n");
        return;
    }

    TicToc t_summing;
    Eigen::MatrixXd A(pos, pos);
    Eigen::VectorXd b(pos);
    A.setZero();
    b.setZero();   
    //multi thread
//计算A,b矩阵, 优化问题最后会转化成Ax=b, A=J.transpose()*J,b=-J.transpose()*e
    TicToc t_thread_summing;
    pthread_t tids[NUM_THREADS];
    ThreadsStruct threadsstruct[NUM_THREADS];
    int i = 0;
    for (auto it : factors)
    {
        threadsstruct[i].sub_factors.push_back(it);
        i++;
        i = i % NUM_THREADS;
    }
    for (int i = 0; i < NUM_THREADS; i++)
    {
        TicToc zero_matrix;
        threadsstruct[i].A = Eigen::MatrixXd::Zero(pos,pos);
        threadsstruct[i].b = Eigen::VectorXd::Zero(pos);
        threadsstruct[i].parameter_block_size = parameter_block_size;
        threadsstruct[i].parameter_block_idx = parameter_block_idx;
        int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));
        if (ret != 0)
        {
            ROS_WARN("pthread_create error");
            ROS_BREAK();
        }
    }
    for( int i = NUM_THREADS - 1; i >= 0; i--)  
    {
        pthread_join( tids[i], NULL ); 
        A += threadsstruct[i].A;
        b += threadsstruct[i].b;
    }
    //ROS_DEBUG("thread summing up costs %f ms", t_thread_summing.toc());
    //ROS_INFO("A diff %f , b diff %f ", (A - tmp_A).sum(), (b - tmp_b).sum());


    //TODO
    Eigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).transpose());
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm);

    //ROS_ASSERT_MSG(saes.eigenvalues().minCoeff() >= -1e-4, "min eigenvalue %f", saes.eigenvalues().minCoeff());

    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
    //printf("error1: %f\n", (Amm * Amm_inv - Eigen::MatrixXd::Identity(m, m)).sum());

    Eigen::VectorXd bmm = b.segment(0, m);
    Eigen::MatrixXd Amr = A.block(0, m, m, n);
    Eigen::MatrixXd Arm = A.block(m, 0, n, m);
    Eigen::MatrixXd Arr = A.block(m, m, n, n);//提取起始于m,m,大小为n*n的子矩阵
    Eigen::VectorXd brr = b.segment(m, n);
    A = Arr - Arm * Amm_inv * Amr;
    b = brr - Arm * Amm_inv * bmm;
    //分解A,b得到ceres计算最小二乘问题里需要的雅克比矩阵和残差， ceres优化需求， 否则是没必要这么做的
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);
    Eigen::VectorXd S = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array(), 0));
    Eigen::VectorXd S_inv = Eigen::VectorXd((saes2.eigenvalues().array() > eps).select(saes2.eigenvalues().array().inverse(), 0));

    Eigen::VectorXd S_sqrt = S.cwiseSqrt();
    Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();

    linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
    linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b;   
}

首先根据参数块和要drop掉的参数块大小来确定A、b矩阵的维度，然后根据A、b矩阵的计算方法，在多线程里来计算每个参数块对应处的值，将各个参数块计算的值累加，就得到最后的A、b矩阵， 进行shur补将要丢弃的参数边缘化掉，再反求得到对应的雅克比矩阵和残差。

以上就是vins-mono里marginalization代码解析。 简明地总结一下，每个残差块会涉及到两个参数块，先求每个残差快的残差及其对应参数块的雅可比矩阵，并记住其在H矩阵、b矩阵中的位置， 而后就是根据由雅可比矩阵计算H矩阵的方法往H矩阵里填充即可。H矩阵、b矩阵计算出来进行shur补，然后再反求得出雅克比矩阵和残差，在下一次优化过程中用上该边缘化信息。