第一個式子是滑動窗口內所有狀態量，n是關鍵幀數量，m是滑動窗內所有觀測到的路標點總數。λi是第i個特徵點的第一個觀測對應的逆深度.特徵點逆深度爲了滿足高斯系統。

第二個式子xk是在第k幀圖像捕獲到的IMU狀態，包括位置，速度，旋轉（PVQ）和加速度偏置，陀螺儀偏置。

第三個式子是相機外參。

注意：xk只與IMU項和Marg有關；特徵點深度也只與camera和Marg有關；

維度是15*n+6+m

IMU狀態 xk {P、V、Q、Ba、Bg} 是15維（5*3=15）；相機外參爲 6 維；特徵點逆深度爲1維。

2、目標函數

優化函數有三部分：

邊緣化殘差：從滑動窗中去掉的節點和特徵點構成的約束，形成一個先驗Prior

IMU殘差：相鄰兩幀IMU產生的residual

視覺重投影誤差：單個特徵點在兩幀之間投影形成residual

二、Estimator::Optimization（）函數

整個滑動窗優化在optimization函數中求解。基於滑動窗口緊耦合的非線性優化，殘差項的構造和求解

1.添加要優化的變量 (p,v,q,ba,bg) 一共15個自由度，Cam到IMU外參，時鐘差

2.添加殘差，殘差項分爲4塊先驗殘差+IMU殘差+視覺殘差+閉環檢測殘差

3.ceres求解Solve（）函數

4.邊緣化操作

1、添加ceres參數塊,待優化變量 AddParameterBlock

待優化變量分爲三部分：IMU狀態xk (p,v,q,ba,bg)、Cam到IMU外參、時鐘差

1、初始化ceres並將 (p,v,q,ba,bg) 15自由度優化變量加入

創建一個ceres Problem實例, loss_function定義爲CauchyLoss.柯西核函數

    ceres::Problem problem;
    ceres::LossFunction *loss_function;// 殘差
    //loss_function = new ceres::HuberLoss(1.0);
    loss_function = new ceres::CauchyLoss(1.0); // 柯西核函數

先添加優化參數量, ceres中參數用ParameterBlock來表示,類似於g2o中的vertex, 這裏的參數塊有sliding windows中所有幀的para_Pose(7維) 和 para_SpeedBias(9維).

Ps、Rs轉變成para_Pose 6自由度7DOF（x,y,z,qx,qy,qz,w），Vs、Bas、Bgs轉變成para_SpeedBias 9自由度9DOF(vx,vy,vz,bax,bay,baz,bgx,bgy,bgz)

for (int i = 0; i < WINDOW_SIZE + 1; i++) // 遍歷滑動窗數量
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
        problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
    }

2、Cam到IMU的外參估計加入

camera到IMU的外參也添加到估計 para_Ex_Pose

for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
        if (!ESTIMATE_EXTRINSIC)
        {
            ROS_DEBUG("fix extinsic param");
            problem.SetParameterBlockConstant(para_Ex_Pose[i]);
        }
        else
            ROS_DEBUG("estimate extinsic param");
    }

3、滑窗內第一時刻相機到IMU時鐘差加入

滑窗內第一個時刻相機到IMU時鐘差，保證傳感器同步， para_Td

if (ESTIMATE_TD)
    {
        problem.AddParameterBlock(para_Td[0], 1);
        //problem.SetParameterBlockConstant(para_Td[0]);
    }

4、在ceres中，vevtor轉double類型

從Ps、Rs、Vs、Bas、Bgs轉化爲para_Pose（6維，相機位姿）和para_SpeedBias（9維，相機速度、加速度偏置、角速度偏置）

從tic和q轉化爲para_Ex_Pose （6維，Cam到IMU外參）

從dep到para_Feature（1維，特徵點深度）

從td轉化爲para_Td（1維，標定同步時間）

void Estimator::vector2double()
{
    // 1.遍歷滑動窗，IMU的15個自由度的優化變量
    for (int i = 0; i <= WINDOW_SIZE; i++)
    {
        // P、R
        para_Pose[i][0] = Ps[i].x();
        para_Pose[i][1] = Ps[i].y();
        para_Pose[i][2] = Ps[i].z();
        Quaterniond q{Rs[i]};
        para_Pose[i][3] = q.x();
        para_Pose[i][4] = q.y();
        para_Pose[i][5] = q.z();
        para_Pose[i][6] = q.w();
        // V、Ba、Bg
        para_SpeedBias[i][0] = Vs[i].x();
        para_SpeedBias[i][1] = Vs[i].y();
        para_SpeedBias[i][2] = Vs[i].z();

        para_SpeedBias[i][3] = Bas[i].x();
        para_SpeedBias[i][4] = Bas[i].y();
        para_SpeedBias[i][5] = Bas[i].z();

        para_SpeedBias[i][6] = Bgs[i].x();
        para_SpeedBias[i][7] = Bgs[i].y();
        para_SpeedBias[i][8] = Bgs[i].z();
    }
    // 2.Cam到IMU的外參，6自由度由7個參數表示
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        para_Ex_Pose[i][0] = tic[i].x();
        para_Ex_Pose[i][1] = tic[i].y();
        para_Ex_Pose[i][2] = tic[i].z();
        Quaterniond q{ric[i]};
        para_Ex_Pose[i][3] = q.x();
        para_Ex_Pose[i][4] = q.y();
        para_Ex_Pose[i][5] = q.z();
        para_Ex_Pose[i][6] = q.w();
    }
    
    VectorXd dep = f_manager.getDepthVector();
    for (int i = 0; i < f_manager.getFeatureCount(); i++)
        para_Feature[i][0] = dep(i);
        
    // 3、保證傳感器同步的時鐘差
    if (ESTIMATE_TD)
        para_Td[0][0] = td;
}

2、添加殘差 AddResidualBlock

依次加入margin項,IMU項和視覺feature項. 每一項都是一個factor, 這是ceres的使用方法, 創建一個類繼承ceres::CostFunction類, 重寫Evaluate()函數定義residual的計算形式. 分別對應marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三個類。首先會調用addResidualBlockInfo()函數將各個殘差以及殘差涉及的優化變量添加入上面所述的優化變量中

1、添加邊緣化殘差，丟棄幀的約束 MarginalizationFactor

    if (last_marginalization_info)
    {
        // construct new marginlization_factor
        MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
        problem.AddResidualBlock(marginalization_factor, NULL,
                                 last_marginalization_parameter_blocks);
    }

2、添加IMU殘差 IMUFactor

    for (int i = 0; i < WINDOW_SIZE; i++)
    {
        int j = i + 1;
        if (pre_integrations[j]->sum_dt > 10.0)
            continue;
        IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
    }
    int f_m_cnt = 0;
    int feature_index = -1;

3、添加視覺殘差 ProjectionTdFactor

    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;
        
        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;
            if (ESTIMATE_TD)
            {
                    ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                                     it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                                                     it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
                    problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
                    /*
                    double **para = new double *[5];
                    para[0] = para_Pose[imu_i];
                    para[1] = para_Pose[imu_j];
                    para[2] = para_Ex_Pose[0];
                    para[3] = para_Feature[feature_index];
                    para[4] = para_Td[0];
                    f_td->check(para);
                    */
            }
            else
            {
                ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
            }
            f_m_cnt++;
        }
    }

    ROS_DEBUG("visual measurement count: %d", f_m_cnt);
    ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());

4、添加閉環檢測殘差，計算滑動窗口中與每一個閉環關鍵幀的相對位姿，這個相對位置是爲後面的圖優化準備

    if(relocalization_info)
    {
        //printf("set relocalization factor! \n");
        ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
        problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);
        int retrive_feature_index = 0;
        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 start = it_per_id.start_frame;
            if(start <= relo_frame_local_index)
            {   
                while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id)
                {
                    retrive_feature_index++;
                }
                if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id)
                {
                    Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
                    Vector3d pts_i = it_per_id.feature_per_frame[0].point;
                    
                    ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                    problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);
                    retrive_feature_index++;
                }     
            }
        }

    }

3、ceres求解Solver

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    //options.num_threads = 2;
    options.trust_region_strategy_type = ceres::DOGLEG;
    options.max_num_iterations = NUM_ITERATIONS;
    //options.use_explicit_schur_complement = true;
    //options.minimizer_progress_to_stdout = true;
    //options.use_nonmonotonic_steps = true;
    if (marginalization_flag == MARGIN_OLD)
        options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
    else
        options.max_solver_time_in_seconds = SOLVER_TIME;
    TicToc t_solver;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    //cout << summary.BriefReport() << endl;
    ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
    ROS_DEBUG("solver costs: %f", t_solver.toc());

4、邊緣化處理

移除位姿時將關聯的約束轉化爲先驗放入優化問題中。爲了限制基於優化的VIO計算複雜度，引入邊緣化。有選擇地從滑動窗口中將IMU狀態xK和特徵點λ1邊緣化，同時將對應於邊緣狀態的測量值轉換爲先驗。

分爲兩種情況，

1、MARGIN_OLD 邊緣化最老幀：一種是倒數第二幀如果是關鍵幀的話，將最舊的pose移出Sliding Window，將最舊幀關聯的視覺和慣性數據邊緣化掉。把第一個老關鍵幀及其測量值被邊緣化；Margin_Old作爲先驗值。

2、MARGIN_SECOND_NEW 邊緣化次新幀：如果倒數第二幀不是關鍵幀的話，那麼就只剔除倒數第二幀的視覺觀測，而不剔除它的IMU約束。原因是邊緣化保證關鍵幀之間有足夠視差而能夠三角化足夠多的地圖點。並且保證了IMU預積分的連貫性。

前言：MarginalizationInfo類

邊緣化Marg類在vins_estimator/src/factor/marginalization_factor.h中

class MarginalizationInfo
{
  public:
    ~MarginalizationInfo();

    int localSize(int size) const;
    int globalSize(int size) const;
    // 添加殘差塊相關信息
    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);
    // 計算每個殘差對應的雅克比，並更新 parameter_block_data
    void preMarginalize();
    // 多線程構造先驗項舒爾補AX=b的結構，計算Jacobian和殘差
    void marginalize();
    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);

    std::vector<ResidualBlockInfo *> factors;//所有觀測項
    int m, n;//m爲要邊緣化的變量個數，n爲要保留下來的變量個數
    int sum_block_size;
    // 1
    std::unordered_map<long, int> parameter_block_size; //<優化變量內存地址,localSize>
    std::unordered_map<long, int> parameter_block_idx; //<待邊緣化的優化變量內存地址,在parameter_block_size中的id>
    std::unordered_map<long, double *> parameter_block_data;//<優化變量內存地址,數據>
    // 2
    std::vector<int> keep_block_size; //global size
    std::vector<int> keep_block_idx;  //local size
    std::vector<double *> keep_block_data;
    // 3
    Eigen::MatrixXd linearized_jacobians;
    Eigen::VectorXd linearized_residuals;
    const double eps = 1e-8;

};

主要類的成員內參爲：

1、三個undorded_map相關變量分別爲：

parameter_block_size、
parameter_block_idx、
parameter_block_data

他們的key都是同一long類型的優化變量的內存地址，value分別是優化變量的長度、id、以及優化變量對應的double指針

2、三個vector相關變量

keep_block_size、
keep_block_idx、
keep_block_data，

他們是進行邊緣化之後保留下來的各個優化變量的長度，各個優化變量在id以各個優化變量對應的double指針類型的數據

3、兩個Eigen變量

linearized_jacobians、
linearized_residuals，

分別指的是邊緣化之後從信息矩陣H恢復出來雅克比矩陣和殘差向量

4.1、邊緣化最老幀

如果次新幀是關鍵幀，將邊緣化最老幀，及其看到的路標點和IMU數據，將其轉化爲先驗。

if (marginalization_flag == MARGIN_OLD)
    {
        MarginalizationInfo *marginalization_info = new MarginalizationInfo();
        vector2double();

後面主要操作分爲：

1、添加三個殘差marginalization_info->addResidualBlockInfo（）：邊緣化殘差MarginalizationFactor、預積分殘差IMUFactor、視覺重投影誤差ProjectionTdFactor，三個損失函數的類都繼承自ceres的損失函數類ceres::CostFunction。

2、計算每個殘差對應的Jacobian，上述三個類各自對應的::Evaluate（）函數

3、主函數：構造Ax=b， marginalization_info->marginalize();

4、調整參數塊在下一次窗口中對應的位置（往前移一格），注意這裏是指針，後面slideWindow中會賦新值，這裏只是提前佔座

下面1-3操作類似，基本流程是：

1、定義三個損失函數。

三個損失函數的類都繼承自ceres的損失函數類ceres::CostFunction。裏面都重載了函數

virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

輸入：待優化變量parameters；以及先驗值（對於先驗殘差就是上一時刻的先驗殘差last_marginalization_info，對於IMU就是預計分值pre_integrations[1]，對於視覺就是空間的的像素座標pts_i, pts_j）

輸出：各項殘差值residual以及殘差對應各優化變量的Jacobian。

2、定義殘差ResidualBlockInfo類。

這一步是爲了將不同的損失函數_cost_function以及優化變量_parameter_blocks統一起來再一起添加到marginalization_info中。

struct ResidualBlockInfo
{
    ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)
        : cost_function(_cost_function), loss_function(_loss_function), parameter_blocks(_parameter_blocks), drop_set(_drop_set) {}

    void Evaluate();

    ceres::CostFunction *cost_function;
    ceres::LossFunction *loss_function;
    std::vector<double *> parameter_blocks;//優化變量數據
    std::vector<int> drop_set;//待邊緣化的優化變量id

    double **raw_jacobians;
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;
    Eigen::VectorXd residuals;//殘差 IMU:15X1 視覺2X1

    int localSize(int size)
    {
        return size == 7 ? 6 : size;
    }
};

3、將殘差添加到marginalization_info中。

marginalization_info->addResidualBlockInfo(residual_block_info);

添加殘差塊相關信息（優化變量，）

//添加殘差塊相關信息（優化變量，待邊緣化變量）
void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info)
{
    factors.emplace_back(residual_block_info);
     // merg相關變量
    std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;
    std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_sizes();
    // 遍歷待優化變量parameter_blocks
    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;// 將指針強轉爲數據的地址
    }
    // 待邊緣化的變量drop_set
    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]];//待邊緣化變量的ID
        parameter_block_idx[reinterpret_cast<long>(addr)] = 0;//將需要marg的變量的id存入parameter_block_idx
    }
}

分別將不同損失函數對應的優化變量、邊緣化位置存入到parameter_block_sizes和parameter_block_idx中，這裏注意的是執行到這一步，parameter_block_idx中僅僅有待邊緣化的優化變量的內存地址的key，而且其對應value全部爲0。

至此，確定優化變量的數量、存儲位置、長度以及待優化變量的數量以及存儲位置

1、將上一次先驗殘差項MarginalizationFactor傳遞給marginalization_info

//1、將上一次先驗殘差項傳遞給marginalization_info,並去除要丟棄的狀態量
        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);
            }
            // 1.1 定義損失函數 marginlization_factor
            MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
            // 1.2 定義殘差
            ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
                                                                           last_marginalization_parameter_blocks,
                                                                           drop_set);
            // 1.3 將各個殘差以及殘差涉及的優化變量添加到 marginalization_info
            marginalization_info->addResidualBlockInfo(residual_block_info);
        }

2、將第0幀和第1幀間的預積分觀測IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中

//2、將第0幀和第1幀間的預積分觀測IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中
        {
            if (pre_integrations[1]->sum_dt < 10.0)
            {
                // 2.1 定義損失函數IMUfactor
                IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
                // 2.2 定義殘差，優化變量爲para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]，都marg掉
                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});
                // 2.3將各個殘差和優化變量添加到 marginalization_info
                marginalization_info->addResidualBlockInfo(residual_block_info);
            }
        }

3、將第一次觀測爲第0幀的所有路標點對應的視覺觀測ProjectionFactor，添加到marginalization_info中

//3、將第一次觀測爲第0幀的所有路標點對應的視覺觀測，添加到marginalization_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;
                    if (ESTIMATE_TD)
                    {
                        // 3.1定義代價函數
                        ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                                          it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                        // 3.2定義殘差塊                                                  it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
                        ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,
                                                                                        vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},
                        // 3.3 將各個殘差和優化變量添加到 marginalization_info                                                               vector<int>{0, 3});
                        marginalization_info->addResidualBlockInfo(residual_block_info);
                    }
                    else
                    {
                        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);
                    }
                }
            }
        }

4、preMarginalize()函數：計算每個殘差對應的Jacobian，

//4、計算每個殘差對應的Jacobian，並將各參數塊拷貝到統一的內存（parameter_block_data）中
        marginalization_info->preMarginalize();
        ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());

void MarginalizationInfo::preMarginalize()
{
    // 在前面的addResidualBlockInfo中會將不同的殘差塊加入到factor中
    for (auto it : factors)
    {
        // 分別計算所有狀態變量構成的殘差 和雅克比矩陣
        it->Evaluate();

        std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();
        for (int i = 0; i < static_cast<int>(block_sizes.size()); i++)
        {
            // 優化變量的地址
            long addr = reinterpret_cast<long>(it->parameter_blocks[i]);
            int size = block_sizes[i];
            if (parameter_block_data.find(addr) == parameter_block_data.end())
            {
                double *data = new double[size];
                // 重新開闢一塊內存
                memcpy(data, it->parameter_blocks[i], sizeof(double) * size);
                // 通過之前的優化變量的數據的地址和新開闢的內存數據進行關聯
                parameter_block_data[addr] = data;
            }
        }
    }
}

主要就是調用各個損失函數中的重載函數Evaluate（）函數。

此外，這裏會給parameter_block_data賦值

5、marglinalize()函數：多線程構造先驗項舒爾補AX=b的結構，在X0處線性化計算Jacobian和殘差

//5、多線程構造先驗項舒爾補AX=b的結構，在X0處線性化計算Jacobian和殘差
        marginalization_info->marginalize();
        ROS_DEBUG("marginalization %f ms", t_margin.toc());

pos爲所有變量維度，ｍ爲需要marg掉的變量，ｎ爲需要保留的變量

//多線程構造先驗項舒爾補AX=b的結構，計算Jacobian和殘差
// pos爲所有變量維度，ｍ爲需要marg掉的變量，ｎ爲需要保留的變量
void MarginalizationInfo::marginalize()
{
    // 1.將所有的優化變量進行一個僞排序，待marg的優化變量的idx爲0，其他的和起所在的位置相關
    int pos = 0;
    // 遍歷待marg的優化變量的內存地址
    for (auto &it : parameter_block_idx)
    {
        it.second = pos;
        pos += localSize(parameter_block_size[it.first]);
    }

    m = pos;// m是需要marg掉的變量個數

    // 遍歷所有待優化變量
    for (const auto &it : parameter_block_size)
    {
        if (parameter_block_idx.find(it.first) == parameter_block_idx.end())
        {
            parameter_block_idx[it.first] = pos;
            pos += localSize(it.second);
        }
    }

    n = pos - m;// n是要保留下來的變量個數

    //ROS_DEBUG("marginalization, pos: %d, m: %d, n: %d, size: %d", pos, m, n, (int)parameter_block_idx.size());
    // 2.通過多線程快速構造各個殘差對應的各個優化變量的信息矩陣
    TicToc t_summing;
    Eigen::MatrixXd A(pos, pos);// 整個矩陣A的大小
    Eigen::VectorXd b(pos);
    A.setZero();
    b.setZero();

    TicToc t_thread_summing;
    pthread_t tids[NUM_THREADS];
    ThreadsStruct threadsstruct[NUM_THREADS];
    int i = 0;
    // 將各個殘差塊的Jacoian矩陣分配到各線程中
    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());
    // 3.通過shur補操作進行邊緣化
    //舒爾補
    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);
    Eigen::VectorXd brr = b.segment(m, n);
    A = Arr - Arm * Amm_inv * Amr;
    b = brr - Arm * Amm_inv * bmm;
    // 更新先驗殘差項
    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;
}

第一步，秉承這map數據結構沒有即添加，存在即賦值的語法，上面的代碼會先補充parameter_block_idx，前面提到經過addResidualBlockInfo()函數僅僅帶邊緣化的優化變量在parameter_block_idx有key值，這裏會將保留的優化變量的內存地址作爲key值補充進去，並統一他們的value值是其前面已經放入parameter_block_idx的優化變量的維度之和，同時這裏會計算出兩個變量m和n，他們分別是待邊緣化的優化變量的維度和以及保留的優化變量的維度和。

第二步，函數會通過多線程快速構造各個殘差對應的各個優化變量的信息矩陣（雅克比和殘差前面都已經求出來了），然後在加起來，如下圖所示：

因爲這裏構造信息矩陣時採用的正是parameter_block_idx作爲構造順序，因此，就會自然而然地將待邊緣化的變量構造在矩陣的左上方。

第三步，函數會通過shur補操作進行邊緣化，然後再從邊緣化後的信息矩陣中恢復出來雅克比矩陣linearized_jacobians和殘差linearized_residuals，這兩者會作爲先驗殘差帶入到下一輪的先驗殘差的雅克比和殘差的計算當中去。

這裏參考：https://blog.csdn.net/weixin_44580210/article/details/95748091

6、調整參數塊在下一次窗口中對應的位置（往前移一格），注意這裏是指針，後面slideWindow中會賦新值，這裏只是提前佔座


        //6.調整參數塊在下一次窗口中對應的位置（往前移一格），注意這裏是指針，後面slideWindow中會賦新值，這裏只是提前佔座
        std::unordered_map<long, double *> addr_shift;
        for (int i = 1; i <= WINDOW_SIZE; i++)// 從1開始，因爲第1幀狀態不要
        {
            //第i的位置存放的的是i-1的內容，這就意味着窗口向前移動了一格
            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];
        }
        vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);

        if (last_marginalization_info)
            delete last_marginalization_info;// 刪除掉上一次marg相關內容
        last_marginalization_info = marginalization_info;// 更新
        last_marginalization_parameter_blocks = parameter_blocks;
        
    }

值得注意的是，這裏僅僅是相當於將指針進行了一次移動，指針對應的數據還是舊數據，因此需要結合後面調用的slideWindow()函數才能實現真正的滑窗移動，此外last_marginalization_info就是保留下來的先驗殘差信息，包括保留下來的雅克比linearized_jacobians、殘差linearized_residuals、保留下來的和邊緣化有關的數據長度keep_block_size、順序keep_block_idx以及數據keep_block_data。last_marginalization_info就是保留下來的滑窗內的所有的優化變量

這裏需要明確一個概念就是，邊緣化操作並不會改變優化變量的值，而僅僅是改變了優化變量之間的關係，而這個關係就是通過信息矩陣體現的。

4.2、邊緣化次新幀

1、保留次新幀的IMU測量，丟棄該幀的視覺測量，將上一次先驗殘差項傳遞給marginalization_info

//1.保留次新幀的IMU測量，丟棄該幀的視覺測量，將上一次先驗殘差項傳遞給marginalization_info
            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);
            }

2、preMarginalize():計算每個殘差對應的Jacobian

 TicToc t_pre_margin;
            ROS_DEBUG("begin marginalization");
            marginalization_info->preMarginalize();
            ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());

3、marginalize():多線程構造先驗項舒爾補AX=b

//3、marginalize
            TicToc t_margin;
            ROS_DEBUG("begin marginalization");
            marginalization_info->marginalize();
            ROS_DEBUG("end marginalization, %f ms", t_margin.toc());

4、調整參數塊在下一次窗口中對應的位置（去掉次新幀）

            //4.調整參數塊在下一次窗口中對應的位置（去掉次新幀）
            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];
            }
            
            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;
            
        }
    }

三、滑動窗 slideWindow()

假設WINDOW_SIZE=5(代碼中爲10），則buffer大小爲6(WINDOW_SIZE+1)，最新來的幀放到WINDOW_SIZE的位置，記作X，兩種操作之後X加入到優化環節中，

1、MARGIN_OLD 邊緣化最老幀：一種是倒數第二幀如果是關鍵幀的話，將最舊的pose移出Sliding Window，將最舊幀關聯的視覺和慣性數據邊緣化掉。把第一個老關鍵幀及其測量值被邊緣化；Margin_Old作爲先驗值。

2、MARGIN_SECOND_NEW 邊緣化次新幀：如果倒數第二幀不是關鍵幀的話，那麼就只剔除倒數第二幀的視覺觀測，而不剔除它的IMU約束。原因是邊緣化保證關鍵幀之間有足夠視差而能夠三角化足夠多的地圖點。並且保證了IMU預積分的連貫性。

1、 MARGIN_OLD 邊緣化最老幀

假設滑動窗內一共有10幀，最新一幀爲第11幀圖像X

if (marginalization_flag == MARGIN_OLD)

1、保存最老幀信息到back_R0,back_P0

back_R0 = Rs[0];
back_P0 = Ps[0];

2、滑窗內0-5所有信息前移

包含IMU狀態量（PVQ、Ba、Bg）、IMU預積分量、線速度、角速度、dt、時間戳

for (int i = 0; i < WINDOW_SIZE; i++)// 遍歷所有
            {
                Rs[i].swap(Rs[i + 1]);

                std::swap(pre_integrations[i], pre_integrations[i + 1]);

                dt_buf[i].swap(dt_buf[i + 1]);
                linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
                angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);

                Headers[i] = Headers[i + 1];
                Ps[i].swap(Ps[i + 1]);
                Vs[i].swap(Vs[i + 1]);
                Bas[i].swap(Bas[i + 1]);
                Bgs[i].swap(Bgs[i + 1]);
            }

3、第5幀情況更新

 // 3.1、第10幀信息給了11幀（第10、11幀相同都是新來的）
            Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
            Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
            Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
            Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
            Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
            Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];
            // 3.2、新實例化一個IMU預積分給到第11幀
            delete pre_integrations[WINDOW_SIZE];
            pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
            // 3.3、清空第11幀的三個buf
            dt_buf[WINDOW_SIZE].clear();
            linear_acceleration_buf[WINDOW_SIZE].clear();
            angular_velocity_buf[WINDOW_SIZE].clear();

4、最老幀之前預積分和圖像都刪除

if (true || solver_flag == INITIAL)
            {
                double t_0 = Headers[0].stamp.toSec();
                map<double, ImageFrame>::iterator it_0;
                it_0 = all_image_frame.find(t_0);// 最老幀

                delete it_0->second.pre_integration;// 刪除預積分信息
                all_image_frame.erase(all_image_frame.begin(), it_0);// 最老幀和之前圖像幀都刪除
            }

5、slideWindowOld()函數，首次在原來最老幀出現的特徵點轉移到現在現在最老幀

void Estimator::slideWindowOld()
{
    // 1、統計一共多少次merg滑窗第一幀情況
    sum_of_back++;

    bool shift_depth = solver_flag == NON_LINEAR ? true : false;
    if (shift_depth)// 非線性
    {
        Matrix3d R0, R1;
        Vector3d P0, P1;
        //back_R0、back_P0爲窗口中最老幀的位姿
        //Rs[0]、Ps[0]爲滑動窗口後第0幀的位姿，即原來的第1幀
        R0 = back_R0 * ric[0];// 滑窗原來第0幀
        R1 = Rs[0] * ric[0];// 滑窗原來第1幀（現在最老的第0幀）
        P0 = back_P0 + back_R0 * tic[0];
        P1 = Ps[0] + Rs[0] * tic[0];
        // 首次在原來最老幀出現的特徵點轉移到現在現在最老幀
        f_manager.removeBackShiftDepth(R0, P0, R1, P1);
    }
    else
    // 當最新一幀是關鍵幀，merg滑窗內最老幀
        f_manager.removeBack();
}

2、MARGIN_SECOND_NEW 邊緣化次新幀，但是不刪除IMU約束

WINDOW_SIZE=5，則buffer大小爲6(WINDOW_SIZE+1)

1、第5幀圖像信息複製到第4幀上

if (frame_count == WINDOW_SIZE)
        {
            for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
            {
                // 1. 
                // 取出最新一幀的信息
                double tmp_dt = dt_buf[frame_count][i];
                Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
                Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];
                // 第5幀信息複製到第4幀上
                dt_buf[frame_count - 1].push_back(tmp_dt);
                linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
                angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);

                // 留着IMU約束，當前幀和前一幀的IMU預積分轉化爲當前幀和前二幀
                pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);
            }
            
            // 第5幀IMU信息複製到第4幀上
            Headers[frame_count - 1] = Headers[frame_count];
            Ps[frame_count - 1] = Ps[frame_count];
            Vs[frame_count - 1] = Vs[frame_count];
            Rs[frame_count - 1] = Rs[frame_count];
            Bas[frame_count - 1] = Bas[frame_count];
            Bgs[frame_count - 1] = Bgs[frame_count];

2、更新第5幀

// 新實例化一個IMU預積分給到第5幀
            delete pre_integrations[WINDOW_SIZE];
            pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};
            // 清空第5幀的三個buf
            dt_buf[WINDOW_SIZE].clear();
            linear_acceleration_buf[WINDOW_SIZE].clear();
            angular_velocity_buf[WINDOW_SIZE].clear();

3、當最新一幀(5)不是關鍵幀時，用於merge滑窗內最新幀(4)

void Estimator::slideWindowNew()
{
    sum_of_front++;
    f_manager.removeFront(frame_count);
}

VINS-Mono 代碼詳細解讀——基於滑動窗口的緊耦合後端非線性優化 optimization()+滑動窗slideWindow()

一、VIO殘差函數構建

1、需要優化的狀態向量