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

把上一次先驗項中的殘差項(尺寸爲 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);
        }

將滑窗內第 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);
            }
        }

挑選出第一次觀測幀爲第 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);
                    }
                }
            }
        }

marginalization_info->preMarginalize():得到每次 IMU 和視覺觀測(cost_function)對應的參數塊(parameter_blocks)，雅可比矩陣(jacobians)，殘差值(residuals);

 marginalization_info->preMarginalize();

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;

重要的三個結構變量：

MarginalizationFactor *marginalization_factor
*MarginalizationInfo marginalization_info
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;
};

vins 邊緣化優化項

vins-fusion代碼解讀[二] 慣性視覺里程結果與GPS松耦合

VINS_MONO getMeasurements（）同步imu數據以及相機特徵點幀數據代碼邏輯

yolo_v3安裝試運行小記

github使用小記錄

pytorch安裝使用

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結