[从零写VIO|第七节]——VINS-Mono代码精简版代码详解——边缘化(内容|代码)

基本流程

1. 判断次新帧是否是关键帧，从而确定边缘化选项

边缘项包括(MARGIN_OLD / MARGIN_SECOND_NEW）

// 判断次新帧是否是关键帧   
    if (f_manager.addFeatureCheckParallax(frame_count, image, td))         
        marginalization_flag = MARGIN_OLD; // 是关键帧，marg掉老帧    
    else        
        marginalization_flag = MARGIN_SECOND_NEW; // 不是关键帧，marg掉前一帧

2. 如果是MARGIN_OLD，就要边缘化掉老帧

边缘化掉最老帧以及其看到的路标点和IMU数据，转化为先验。
对应的代码为：

 if (marginalization_flag == MARGIN_OLD)    
   {        
       vector2double(); // 系统数据类型转换：将向量变成double数组指针的形式
       MargOldFrame();
  ...
  }

3.当次新帧不是关键帧时，边缘化次新帧

保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info

此时，marginalization_flag = MARGIN_SECOND_NEW；

 else    
 {        
     if (Hprior_.rows() > 0)        
     {
            vector2double();
            MargNewFrame();
            ...
     }
 }

4. 详细代码细节

① vector2double()：

此函数将向量转变成数组指针的形式，生成的优化变量包括：
para_Pose（6维，相机位姿）、
para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）、
para_Ex_Pose（6维、相机IMU外参）、
para_Feature（1维，特征点深度）、
para_Td（1维，标定同步时间）
在后面进行边缘化操作时这些优化变量都是当做整体看待。

② MargOldFrame()边缘化老帧操作

1. 构建problem

首先构建problem，依次加入外参数节点，IMU信息的相关节点，视觉节点和约束边，设置先验信息，然后设置需要边缘化掉的节点，进行边缘化操作

void Estimator::MargOldFrame()
{    
    backend::LossFunction *lossfunction;    
    lossfunction = new backend::CauchyLoss(1.0);
    // step1. 构建 problem    
    backend::Problem problem(backend::Problem::ProblemType::SLAM_PROBLEM);    
    vector<shared_ptr<backend::VertexPose>> vertexCams_vec;    
    vector<shared_ptr<backend::VertexSpeedBias>> vertexVB_vec;    
    int pose_dim = 0;

1.1 先把外参数节点加入图优化

这个节点在以后一直会被用到，所以我们把他放在第一个

shared_ptr<backend::VertexPose> vertexExt(new backend::VertexPose());    
{        
    Eigen::VectorXd pose(7);        
    pose << para_Ex_Pose[0][0], para_Ex_Pose[0][1], para_Ex_Pose[0][2],   
         para_Ex_Pose[0][3], para_Ex_Pose[0][4], para_Ex_Pose[0][5], para_Ex_Pose[0][6]; // R,t        
    vertexExt->SetParameters(pose); // 设置外参数值        
    problem.AddVertex(vertexExt);        
    pose_dim += vertexExt->LocalDimension(); // 外参数化维度    
}

1.2 再把滑窗内每一帧的P.Q.V.bias 加入图优化

for (int i = 0; i < WINDOW_SIZE + 1; i++)    
{        
    // 滑动窗口内每一帧的位姿，PQ        
    shared_ptr<backend::VertexPose> vertexCam(new backend::VertexPose());        
    Eigen::VectorXd pose(7);        
    pose << para_Pose[i][0], para_Pose[i][1], para_Pose[i][2], para_Pose[i][3], 
        para_Pose[i][4], para_Pose[i][5], para_Pose[i][6];
                
    vertexCam->SetParameters(pose);        
    vertexCams_vec.push_back(vertexCam);// R,t        
    problem.AddVertex(vertexCam);        
    pose_dim += vertexCam->LocalDimension();
    
   // 滑窗内每一帧的IMU 的速度 加速度、角速度的偏置        
   shared_ptr<backend::VertexSpeedBias> vertexVB(new backend::VertexSpeedBias());        
   Eigen::VectorXd vb(9);       
   vb << para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2],            
       para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5],            
       para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8];
                
   vertexVB->SetParameters(vb);       
   vertexVB_vec.push_back(vertexVB);        
   problem.AddVertex(vertexVB);        
   pose_dim += vertexVB->LocalDimension(); // 两部分看成一个整体  
}

1.3 IMU 初始时刻，从第一帧积起来的第二帧时刻的IMU预积分

 {        
     if (pre_integrations[1]->sum_dt < 10.0)        
     {            
         std::shared_ptr<backend::EdgeImu> imuEdge(new backend::EdgeImu(pre_integrations[1]));            
         std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;             
         edge_vertex.push_back(vertexCams_vec[0]);             
         edge_vertex.push_back(vertexVB_vec[0]);            
         edge_vertex.push_back(vertexCams_vec[1]);            
         edge_vertex.push_back(vertexVB_vec[1]);            
         imuEdge->SetVertex(edge_vertex);            
         problem.AddEdge(imuEdge);        
     }    
} 

1.4 视觉因子的处理
包括获取每个特征的逆深度以及对于与同一个路标点共视的所有帧，计算第一帧(最老帧)与各帧之间的重投影误差、信息矩阵…

{        
    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;
        
        // 特征点的逆深度            
        shared_ptr<backend::VertexInverseDepth> verterxPoint(new backend::VertexInverseDepth());            
        VecX inv_d(1);            
        inv_d << para_Feature[feature_index][0];            
        verterxPoint->SetParameters(inv_d);            
        problem.AddVertex(verterxPoint);
        
        // 遍历所有的观测            
        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;               
            // 对于与同一个路标点共视的所有帧，计算第一帧(最老帧)与各帧之间的重投影误差                
            std::shared_ptr<backend::EdgeReprojection> edge(new backend::EdgeReprojection(pts_i, pts_j));                
            std::vector<std::shared_ptr<backend::Vertex>> edge_vertex;                
            edge_vertex.push_back(verterxPoint); // 逆深度                
            edge_vertex.push_back(vertexCams_vec[imu_i]); // 两帧的PQ                
            edge_vertex.push_back(vertexCams_vec[imu_j]);                
            edge_vertex.push_back(vertexExt); // 外参
            
            edge->SetVertex(edge_vertex);                
            edge->SetInformation(project_sqrt_info_.transpose() * project_sqrt_info_);
            edge->SetLossFunction(lossfunction);                
            problem.AddEdge(edge);            
        }        
    }    
}

1.5 先验

 {        
     // 已经有 Prior 了        
     if (Hprior_.rows() > 0)        
     {            
         problem.SetHessianPrior(Hprior_); // 告诉这个 problem            
         problem.SetbPrior(bprior_);            
         problem.SetErrPrior(errprior_);            
         problem.SetJtPrior(Jprior_inv_);            
         problem.ExtendHessiansPriorSize(15); // 但是这个 prior 还是之前的维度，需要扩展下装新的pose        
     }        
     else        
     {            
         Hprior_ = MatXX(pose_dim, pose_dim);            
         Hprior_.setZero();            
         bprior_ = VecX(pose_dim);            
         bprior_.setZero();            
         problem.SetHessianPrior(Hprior_); // 告诉这个 problem            
         problem.SetbPrior(bprior_);        
     }    
 }

2. 传入要边缘化掉的最老帧信息,进行边缘化

 std::vector<std::shared_ptr<backend::Vertex>> marg_vertex;
     
 // 2 传入要边缘化掉的最老帧信息,进行边缘化     
 marg_vertex.push_back(vertexCams_vec[0]); // //这里的0即为最老帧对应的索引 // PQ     
 marg_vertex.push_back(vertexVB_vec[0]); // V,bias 
    
 problem.Marginalize(marg_vertex, pose_dim);// 边缘化处理    

$边缘化处理！！！重点！！！$

传入要边缘化的顶点和所有状态向量的维度和。

bool Problem::Marginalize(const std::vector<std::shared_ptr<Vertex> > margVertexs, int pose_dim);// 边缘化处理    

3. 先验信息更新

Hprior_ = problem.GetHessianPrior();    
bprior_ = problem.GetbPrior();    
errprior_ = problem.GetErrPrior();    
Jprior_inv_ = problem.GetJtPrior();

③ MargNewFrame()边缘化次新帧操作

1. 构建problem

没有加入视觉约束边，以及IMU预积分

2.传入要边缘化的次新帧信息，进行边缘化

    std::vector<std::shared_ptr<backend::Vertex>> marg_vertex;      
    // 把窗口倒数第二个帧 marg 掉    
    marg_vertex.push_back(vertexCams_vec[WINDOW_SIZE - 1]);//注意这里的索引，次新帧    
    marg_vertex.push_back(vertexVB_vec[WINDOW_SIZE - 1]);    
    
    problem.Marginalize(marg_vertex, pose_dim);

3. 先验信息更新

同上，略。

边缘化Marginalize()代码解析

step1. SetOrdering()

重新计算所有要优化变量的总维度，若是SLAM问题则需要分别统计pose和landmark的维数。

void Problem::SetOrdering() {

    // 每次重新计数
    ordering_poses_ = 0;
    ordering_generic_ = 0;
    ordering_landmarks_ = 0;

    // Note:: verticies_ 是 map 类型的, 顺序是按照 id 号排序的
    for (auto vertex: verticies_) {
        ordering_generic_ += vertex.second->LocalDimension();  // 所有的优化变量总维数

        if (problemType_ == ProblemType::SLAM_PROBLEM)    // 如果是 slam 问题，还要分别统计 pose 和 landmark 的维数，后面会对他们进行排序
        {
            AddOrderingSLAM(vertex.second);
        }

    }

    if (problemType_ == ProblemType::SLAM_PROBLEM) {
        // 这里要把 landmark 的 ordering 加上 pose 的数量，就保持了 landmark 在后,而 pose 在前
        ulong all_pose_dimension = ordering_poses_;
        for (auto landmarkVertex : idx_landmark_vertices_) {
            landmarkVertex.second->SetOrderingId(
                landmarkVertex.second->OrderingId() + all_pose_dimension
            );
        }
    }

//    CHECK_EQ(CheckOrdering(), true);
}

step2. GetConnectedEdges(margVertexs[0])

找到所有与最老帧相关联的边，包括与路标点(共视关系)和IMU(预积分)的。

std::vector<shared_ptr<Edge>> marg_edges = GetConnectedEdges(margVertexs[0]);

详细代码为：

vector<shared_ptr<Edge>> Problem::GetConnectedEdges(std::shared_ptr<Vertex> vertex) {
    vector<shared_ptr<Edge>> edges;
    auto range = vertexToEdge_.equal_range(vertex->Id());
    for (auto iter = range.first; iter != range.second; ++iter) {

        // 并且这个edge还需要存在，而不是已经被remove了
        if (edges_.find(iter->second->Id()) == edges_.end())
            continue;

        edges.emplace_back(iter->second);
    }
    return edges;
}

range.first是与该顶点第一个关联的边，range.second是最后一个关联的边？？

【补充】equal_range二分查找算法

step3. 重新设置路标点的顺序

对于marg_edges所关联的特征，重新设定其顺序：pose_dim + marg_landmark_size，这里的pose_dim是增加了所有节点后的维度。

   std::unordered_map<int, shared_ptr<Vertex>> margLandmark;
    // 构建 Hessian 的时候 pose 的顺序不变，landmark的顺序要重新设定
    int marg_landmark_size = 0;
//    std::cout << "\n marg edge 1st id: "<< marg_edges.front()->Id() << " end id: "<Id()<
    for (size_t i = 0; i < marg_edges.size(); ++i) {
//        std::cout << "marg edge id: "<< marg_edges[i]->Id() <
        auto verticies = marg_edges[i]->Verticies(); // 与要边缘化边相关联的顶点
        for (auto iter : verticies) {
            // 是与最老边关联(共视)的路标点？？对路标点重新排序--放在pose后面
            if (IsLandmarkVertex(iter) && margLandmark.find(iter->Id()) == margLandmark.end()) { // end()指向map的最后一个元素的
                iter->SetOrderingId(pose_dim + marg_landmark_size);
                margLandmark.insert(make_pair(iter->Id(), iter));
                marg_landmark_size += iter->LocalDimension();
            }
        }
    }

step4. 构建H矩阵和b矩阵

    int cols = pose_dim + marg_landmark_size;    
    /// 构建误差 H 矩阵 H = H_marg + H_pp_prior    
    MatXX H_marg(MatXX::Zero(cols, cols));    
    VecX b_marg(VecX::Zero(cols));

接下来，对于每个要边缘化的边，计算其残差和雅克比矩阵，然后叠加所有节点对之间的$J_i^{T}W{J}_j$构造H和b。

    int ii = 0;
    for (auto edge: marg_edges) {
        edge->ComputeResidual(); // 计算要边缘化边的残差
        edge->ComputeJacobians(); // 计算要边缘化边的雅可比
        auto jacobians = edge->Jacobians();
        auto verticies = edge->Verticies();
        ii++;

        assert(jacobians.size() == verticies.size());
        for (size_t i = 0; i < verticies.size(); ++i) {
            auto v_i = verticies[i];
            auto jacobian_i = jacobians[i];
            ulong index_i = v_i->OrderingId(); // 索引
            ulong dim_i = v_i->LocalDimension(); // 维度
            
            // 求解鲁棒核函数中带权重的信息矩阵
            double drho;
            MatXX robustInfo(edge->Information().rows(),edge->Information().cols()); // 定义
            edge->RobustInfo(drho,robustInfo); // 输出robustInfo 信息矩阵 

            for (size_t j = i; j < verticies.size(); ++j) {
                auto v_j = verticies[j];
                auto jacobian_j = jacobians[j];
                ulong index_j = v_j->OrderingId();
                ulong dim_j = v_j->LocalDimension();

                MatXX hessian = jacobian_i.transpose() * robustInfo * jacobian_j; 

                assert(hessian.rows() == v_i->LocalDimension() && hessian.cols() == v_j->LocalDimension());
                // 所有的信息矩阵叠加起来
                H_marg.block(index_i, index_j, dim_i, dim_j) += hessian;
                if (j != i) {
                    // 对称的下三角
                    H_marg.block(index_j, index_i, dim_j, dim_i) += hessian.transpose(); 
                }
            }
            b_marg.segment(index_i, dim_i) -= drho * jacobian_i.transpose() * edge->Information() * edge->Residual();
        }

    }       

【备注】相关带鲁棒核函数的信息矩阵和H、b矩阵的知识点以及代码解析，可以看我的另一篇博客单目BA求解代码解析中的【补充】鲁棒核函数的H矩阵以及b部分。

step5. 针对所关联的视觉特征的边缘化

1. 对H、b进行分块，为舒尔补操作做准备。
   构造出的形式如下：
   $$\left[\begin{array}{cc}{H}_{pp}& H_{pm}\\ H_{mp}& H_{mm}\end{array}\right]\left[\begin{array}{c}\delta x_p\\ \delta x_m\end{array}\right]=\left[\begin{array}{c}{b}_{pp}\\ b_{mm}\end{array}\right]$$
   

   int reserve_size = pose_dim;    
   if (marg_landmark_size > 0) 
   {        
       int marg_size = marg_landmark_size;        
       MatXX Hmm = H_marg.block(reserve_size, reserve_size, marg_size, marg_size);        
       MatXX Hpm = H_marg.block(0, reserve_size, reserve_size, marg_size);        
       MatXX Hmp = H_marg.block(reserve_size, 0, marg_size, reserve_size);
               
       VecX bpp = b_marg.segment(0, reserve_size);        
       VecX bmm = b_marg.segment(reserve_size, marg_size);

2. 舒尔补操作–针对所关联的视觉特征的边缘化
   边缘化视觉特征(最老帧与共视路标点的关联边)：
   
   相关代码如下：

        // Hmm 是对角线矩阵，它的求逆可以直接为对角线块分别求逆，如果是逆深度，对角线块为1维的，则直接为对角线的倒数，这里可以加速
        MatXX Hmm_inv(MatXX::Zero(marg_size, marg_size));
        // TODO:: use openMP
        for (auto iter: margLandmark) {
            int idx = iter.second->OrderingId() - reserve_size;
            int size = iter.second->LocalDimension();
            Hmm_inv.block(idx, idx, size, size) = Hmm.block(idx, idx, size, size).inverse();
        }

        MatXX tempH = Hpm * Hmm_inv;
        MatXX Hpp = H_marg.block(0, 0, reserve_size, reserve_size) - tempH * Hmp; // 公式(5)等式左边的系数矩阵
        bpp = bpp - tempH * bmm; // 公式(5)等式右边部分
        H_marg = Hpp;
        b_marg = bpp;

step6. 针对帧和speedbias进行边缘化
在边缘化掉视觉信息特征的H_marg、b_marg上继续进行对帧和speedbias的边缘化。

1. 需要把将要边缘化的节点移动到H_marg的右下角

图示：

$\left[\begin{array}{cc}{H}_{pp}& H_{pm}\\ H_{mp}& H_{mm}\end{array}\right]->H_{pp}=H_{pp}-H_{pm}\ast H_{mm}^{-1}\ast H_{mp}->H_{pp}、b_{pp}->移动要边缘化的节点到右下角$

最终矩阵形式为：
A_mm表示要边缘化掉的所有帧和speedbias信息。

相关代码：

    /// marg frame and speedbias
    int marg_dim = 0;

    // index 大的先移动
    for (int k = margVertexs.size() -1 ; k >= 0; --k)
    {

        int idx = margVertexs[k]->OrderingId();
        int dim = margVertexs[k]->LocalDimension();
//        std::cout << k << " "<
        marg_dim += dim;
        // move the marg pose to the Hmm bottown right
        // 将 row i 移动矩阵最下面
        Eigen::MatrixXd temp_rows = H_marg.block(idx, 0, dim, reserve_size);
        Eigen::MatrixXd temp_botRows = H_marg.block(idx + dim, 0, reserve_size - idx - dim, reserve_size);
        H_marg.block(idx, 0, reserve_size - idx - dim, reserve_size) = temp_botRows;
        H_marg.block(reserve_size - dim, 0, dim, reserve_size) = temp_rows;

        // 将 col i 移动矩阵最右边
        Eigen::MatrixXd temp_cols = H_marg.block(0, idx, reserve_size, dim);
        Eigen::MatrixXd temp_rightCols = H_marg.block(0, idx + dim, reserve_size, reserve_size - idx - dim);
        H_marg.block(0, idx, reserve_size, reserve_size - idx - dim) = temp_rightCols;
        H_marg.block(0, reserve_size - dim, reserve_size, dim) = temp_cols;

        Eigen::VectorXd temp_b = b_marg.segment(idx, dim);
        Eigen::VectorXd temp_btail = b_marg.segment(idx + dim, reserve_size - idx - dim);
        b_marg.segment(idx, reserve_size - idx - dim) = temp_btail;
        b_marg.segment(reserve_size - dim, dim) = temp_b;
    }

2. 舒尔补操作，最后产生的就是先验

    double eps = 1e-8;
    int m2 = marg_dim;
    int n2 = reserve_size - marg_dim;   // marg pose
    Eigen::MatrixXd Amm = 0.5 * (H_marg.block(n2, n2, m2, m2) + H_marg.block(n2, n2, m2, m2).transpose()); // 保证数值求解是对称矩阵

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes(Amm); // 求解矩阵的特征值和特征向量
    Eigen::MatrixXd Amm_inv = saes.eigenvectors() * Eigen::VectorXd( // 奇异值分解？
            (saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() *
                              saes.eigenvectors().transpose();

    Eigen::VectorXd bmm2 = b_marg.segment(n2, m2);
    Eigen::MatrixXd Arm = H_marg.block(0, n2, n2, m2);
    Eigen::MatrixXd Amr = H_marg.block(n2, 0, m2, n2);
    Eigen::MatrixXd Arr = H_marg.block(0, 0, n2, n2);
    Eigen::VectorXd brr = b_marg.segment(0, n2);
    Eigen::MatrixXd tempB = Arm * Amm_inv;
    H_prior_ = Arr - tempB * Amr;
    b_prior_ = brr - tempB * bmm2;

step7. 为利用ceres进行优化，更新先验残差项
相关知识点可以查看这篇博客——VINS-MONO边缘化策略。摘自这篇参考博客，
边缘化步骤总结如下：

第一步为前面对视觉特征、相关帧和speedbias的边缘化，得到$H_0^{\ast }$,也就是代码中的H_prior_、b_prior_

① 求解$(J^T{)}^{+}$得到残差e，重新获得$H$


相关代码如下：

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(H_prior_); // 求解矩阵的特征值和特征向量
    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(); // S^(1/2)
    Eigen::VectorXd S_inv_sqrt = S_inv.cwiseSqrt();
    Jt_prior_inv_ = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose(); // (J^T)^+
    err_prior_ = -Jt_prior_inv_ * b_prior_; // e0

    MatXX J = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();
    H_prior_ = J.transpose() * J; // H
    MatXX tmp_h = MatXX( (H_prior_.array().abs() > 1e-9).select(H_prior_.array(),0) );
    H_prior_ = tmp_h; // H 先验矩阵

step7. 移除相关的边缘化的节点和边

    for (size_t k = 0; k < margVertexs.size(); ++k) {
        RemoveVertex(margVertexs[k]); // 移除该顶点对应的边
    }

    for (auto landmarkVertex: margLandmark) {
        RemoveVertex(landmarkVertex.second);
    }

RemoveVertex():

bool Problem::RemoveVertex(std::shared_ptr<Vertex> vertex) {
    //check if the vertex is in map_verticies_
    if (verticies_.find(vertex->Id()) == verticies_.end()) {
        // LOG(WARNING) << "The vertex " << vertex->Id() << " is not in the problem!" << endl;
        return false;
    }

    // 这里要 remove 该顶点对应的 edge.
    vector<shared_ptr<Edge>> remove_edges = GetConnectedEdges(vertex);
    for (size_t i = 0; i < remove_edges.size(); i++) {
        RemoveEdge(remove_edges[i]);
    }

    if (IsPoseVertex(vertex))
        idx_pose_vertices_.erase(vertex->Id());
    else
        idx_landmark_vertices_.erase(vertex->Id());

    vertex->SetOrderingId(-1);      // used to debug
    verticies_.erase(vertex->Id());
    vertexToEdge_.erase(vertex->Id());

    return true;
}

bool Problem::RemoveEdge(std::shared_ptr<Edge> edge) {
    //check if the edge is in map_edges_
    if (edges_.find(edge->Id()) == edges_.end()) {
        // LOG(WARNING) << "The edge " << edge->Id() << " is not in the problem!" << endl;
        return false;
    }

    edges_.erase(edge->Id());
    return true;
}

【注意】边缘化求解出的H_prior_、b_prior_是作为后面优化求解的先验矩阵。先验、未涉及边缘化的信息再加上新的测量信息，组成新的信息矩阵。

5.滑窗预移动

调整参数块在下一次窗口中对应的位置，注意，这里只是相当于将指针进行了一次移动，指针对应的数据还是旧数据，因此需要结合后面调用的slideWindow()函数才能实现真正的滑窗移动。

5.1 若边缘化的是老帧

边缘化老帧后，调整参数块在下一次窗口中对应的位置，整体往前移一格(删除最老帧)，注意这里只是指针，后面slideWindow中会赋新值，这里只是提前占座。

相关代码：

 std::unordered_map<long, double *> addr_shift; // prior 中对应的保留下来的参数地址
        // 从1开始，因为第一帧的状态要剔除
        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]; //  外参保持不变
        if (ESTIMATE_TD)
        {
            addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
        }

5.2 若边缘化的是次新帧

调整参数块在下一次窗口中对应的位置（去掉次新帧）

相关代码：

            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];
            if (ESTIMATE_TD)
            {
                addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];
            }

【备注】continue：跳过本次循环体中余下尚未执行的语句,立即进行下一次的循环条件判定,可以理解为仅结束本次循环 -----hhhhh小儿科了~~~

6.实现滑窗的更新

相关解析可见我的另一篇博客——thd_BackEnd线程(内容|代码)滑窗更新部分。

至此，边缘化内容全部完成！！！撒花★,°:.☆(￣▽￣)/$:.°★ 。