vins 边缘化优化项

参考：
https://www.cnblogs.com/feifanrensheng/p/10532918.html

优化过程的五个步骤：

1 imu直接积分预测新进来的帧的位置
2.三角化整个特征点
3.ceres优化
4.滑动窗口
5.去除深度值为负的特征点

在优化完成后，滑动窗口前，必须要做的一件事情必须进行边缘化项的操作：

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);
    }

从旧的先验残差项last_marginalization_info新建一个新的marginalization_factor
参与这个残差项的优化变量是：last_marginalization_parameter_blocks 里面的内容

当次新帧为关键帧时，MARGIN_OLD，将 marg 掉最老帧，及其看到的路标点和相关联 的 IMU 数据，将其转化为先验信息加到整体的目标函数中:
MarginalizationInfo *marginalization_info = new MarginalizationInfo();

1. 把上一次先验项中的残差项(尺寸为 n)传递给当前先验项，并从中去除需要丢弃 的状态量;

 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);
        }

2. 将滑窗内第 0 帧和第 1 帧间的 IMU 预积分因子(pre_integrations[1])放到 marginalization_info 中，即图 中上半部分中 x0 和 x1 之间的表示 IMU 约束的黄色块;

  {
            if (pre_integrations[1]->sum_dt < 10.0)
            {
                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);
            }
        }

3. 挑选出第一次观测帧为第 0 帧的路标点，将对应的多组视觉观测放到 marginalization_info 中，即图 中上半部分中 x0 所看到的红色五角星的路标点;

{
            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)
                    {
                        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());
                        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]},
                                                                                        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. marginalization_info->preMarginalize():得到每次 IMU 和视觉观测(cost_function)对 应的参数块(parameter_blocks)，雅可比矩阵(jacobians)，残差值(residuals);

 marginalization_info->preMarginalize();

5. marginalization_info->marginalize():多线程计整个先验项的参数块，雅可比矩阵和 残差值，其中与代码对应关系为:
   

marginalization_info->marginalize();

最后移交了优化项需要得到的两个变量：last_marginalization_info和last_marginalization_parameter_blocks

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];
        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;

重要的三个结构变量：

1. MarginalizationFactor *marginalization_factor
2. *MarginalizationInfo marginalization_info
3. ResidualBlockInfo *residual_block_info

MarginalizationFactor *marginalization_factor 两个作用：（1）构建ceres的残差项，即计算residual （2）构建ResidualBlockInfo *residual_block_info

MarginalizationInfo *marginalization_info 由他可以获取边缘化信息，用它来构建MarginalizationFactor *marginalization_factor以及得到对应的参数块

ResidualBlockInfo *residual_block_info 用它来丰富marginalization_info项的信息

从头到位都是marginalization_info这个变量来进行统筹安排进行边缘化。

通过marginalization_info->addResidualBlockInfo()来添加约束，有三个方面的来源：（1）旧的（2）imu预积分项（3）特征点

整个流程：：

通过marginalization_factor，imu_factor， ProjectionTdFactor *f_td，三个残差项，构建出三个residual_block_info，最终添加进marginalization_info里边。
最终通过last_marginalization_info 构建出marginalization_factor，这个factor就是先验的残差项。

现在来看比较重要的这三个结构体：

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; //待marg的优化变量id

    double **raw_jacobians; //Jacobian
    std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians; 
    Eigen::VectorXd residuals;	//残差，IMU:15×1,视觉:2×1

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

class MarginalizationInfo
{
  public:
    ~MarginalizationInfo();
    int localSize(int size) const;
    int globalSize(int size) const;
    
    void addResidualBlockInfo(ResidualBlockInfo *residual_block_info); //计算每个残差对应的Jacobian，并更新parameter_block_data
    void preMarginalize(); //加残差块相关信息(优化变量、待marg的变量)
    void marginalize();//pos为所有变量维度，m为需要marg掉的变量，n为需要保留的变量
    std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);

    std::vector<ResidualBlockInfo *> factors; //所有观测项
    int m, n; 
    //m为要marg掉的变量个数，也就是parameter_block_idx的总localSize，以double为单位，VBias为9，PQ为6
	//n为要保留下的优化变量的变量个数，n=localSize(parameter_block_size) – m


	//<优化变量内存地址，localSize>
    std::unordered_map<long, int> parameter_block_size; //global size
    int sum_block_size;

     
    std::unordered_map<long, int> parameter_block_idx; //local size
   								//<待marg的优化变量内存地址，在 //parameter_block_size中的id,以double为单位>
    std::unordered_map<long, double *> parameter_block_data; //<优化变量内存地址，数据>

    std::vector<int> keep_block_size; //global size
    std::vector<int> keep_block_idx;  //local size
    std::vector<double *> keep_block_data;

    Eigen::MatrixXd linearized_jacobians;
    Eigen::VectorXd linearized_residuals;
    const double eps = 1e-8;

};

class MarginalizationFactor : public ceres::CostFunction
{
  public:
    MarginalizationFactor(MarginalizationInfo* _marginalization_info);
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

    MarginalizationInfo* marginalization_info;
};