VINS-Mono關鍵知識點總結——邊緣化marginalization理論和代碼詳解

• VINS-Mono關鍵知識點總結——邊緣化marginalization理論和代碼詳解
• • 1. 邊緣化理論
  • • 1.1 為什么要進行邊緣化操作？
    • 1.2 怎樣進行邊緣化呢？
    • 1.3 在實際的邊緣化操作中有什么需要注意的嗎？
  • 2. 代碼剖析
  • • 2.1 優化變量分析
    • 2.1 MarginalizationInfo類分析
    • 2.3 第一步：調用addResidualBlockInfo()
    • 2.4 第二步：調用preMarginalize()
    • 2.5 第三步：調用marginalize()
    • 2.6 第四步：滑窗預移動

VINS-Mono關鍵知識點總結——邊緣化marginalization理論和代碼詳解

個人覺得整個VINS最難理解的部分就是邊緣化（marginalization），除去理論學習，僅僅看代碼前前后后就花了好幾天，也并沒有全部看懂，這里盡可能地對這部分只是詳細總結下

1. 邊緣化理論

邊緣化相關的理論主要是參考高博和賀博的課程以及三篇參考文獻：
《The Humble Gaussian Distribution》
《Exactly Sparse Extended Information Filters for Feature-Based SLAM》
《Consistency Analysis for Sliding-Window Visual Odometry》

1.1 為什么要進行邊緣化操作？

首先我們知道，如果僅僅從前后兩幀圖像來計算相機變換位姿， 其速度快但是精度低，而如果采用全局優化的方法（比如Bundle Adjustment），其精度高但是效率低，因此前輩們引入了滑窗法這樣一個方法，每次對固定數量的幀進行優化操作，這樣既保證了精度又保證了效率。既然是滑窗，在滑動的過程中必然會有新的圖像幀進來以及舊的圖像幀離開，所謂邊緣化就是為了使得離開的圖像幀得到很好的利用。

1.2 怎樣進行邊緣化呢？

我們根據運動模型和觀測模型建立$H$矩陣（高斯牛頓法中的$J{J}^T$）的過程其實就是根據概率圖模型（多元高斯分布）建立各個節點變量間的信息矩陣（協方差矩陣的逆）的過程，而邊緣化則是去掉概率圖中的某一個節點后信息矩陣會發生怎樣的變換的問題。如下圖

其中$$\begin{array}{rl}{x}_2& =v_2\\ x_1& =w_1{x}_2+v_1\\ x_3& =w_3{x}_2+v_3\end{array}$$其中$v_i$相互獨立，且各自服從協方差為${\sigma }_i^2$的高斯分布，可以求得上面概率圖模型的信息矩陣為（求解過程成省略）$$\mathbf{\Lambda }={\mathbf{\Sigma }}^{?1}=?\begin{array}{ccc}\frac{1}{{\sigma }_1^2}& ?\frac{w_1}{{\sigma }_1^2}& 0\\ ?\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}+\frac{w_3^2}{{\sigma }_1^2}& ?\frac{w_3}{{\sigma }_2^2}\\ 0& ?\frac{w_3}{{\sigma }_3^2}& \frac{1}{{\sigma }_3^2}\end{array}?$$邊緣化問題是假如我們將$x_3$去掉之后，概率圖模型的信息矩陣會發生怎樣的變化，如下圖所示：

我們先不加證明的給出結論為：$${\mathbf{\Sigma }}_2^{?1}=\left[\begin{array}{cc}\frac{1}{{\sigma }_1^2}& ?\frac{w_1}{{\sigma }_1^2}\\ ?\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}\end{array}\right]$$下面開始證明，現在假設$a,b$兩個高斯分布的變量之間的協方差矩陣為：$$\mathbf{K}=\left[\begin{array}{cc}A& C^{?}\\ C& D\end{array}\right]$$其中$A=\mathrm{cov}(a,a),D=\mathrm{cov}(b,b),C=\mathrm{cov}(a,b)$，那么其聯合分布可以表示為邊際分布和條件部分的乘積如下：$$P(a,b)=P(a)P(b|a)\propto \exp \left(?\frac{1}{2}{\left[\begin{array}{l}a\\ b\end{array}\right]}^{?}{\left[\begin{array}{cc}A& C^{?}\\ C& D\end{array}\right]}^{?1}\left[\begin{array}{l}a\\ b\end{array}\right]\right)$$然后對高斯分布進行分解可以直接求的邊際分布$P(a)$和條件分布$P(b|a)$的表達形式，如下：$$\begin{array}{l}P(a,b)\\ \propto \exp \left(?\frac{1}{2}{\left[\begin{array}{c}a\\ b\end{array}\right]}^{?}{\left[\begin{array}{cc}A& C^{?}\\ C& D\end{array}\right]}^{?1}{\left[\begin{array}{cc}A& C^{?}\\ C& D\end{array}\right]}^{?1}\left[\begin{array}{c}A\\ b\end{array}\right]\right)\\ \propto \exp \left(?\frac{1}{2}{\left[\begin{array}{c}a\\ b\end{array}\right]}^{?}\left[\begin{array}{cc}I& ?A^{?1}{C}^{?}\\ 0& I\end{array}\right]\left[\begin{array}{cc}{A}^{?1}& 0\\ 0& {\Delta }_{\mathrm{A}}^{?1}\end{array}\right]\left[\begin{array}{cc}I& 0\\ ?C{A}^{?1}& I\end{array}\right]\left[\begin{array}{c}a\\ b\end{array}\right]\right)\\ \propto \exp \left(?\frac{1}{2}\left[a^{?}{\left(b?C{A}^{?1}a\right)}^{?}\right]\left[\begin{array}{cc}{A}^{?1}& 0\\ 0& {\Delta }_{\mathrm{A}}^{?1}\end{array}\right]\left[\begin{array}{c}a\\ b?C{A}^{?1}a\end{array}\right]\right)\\ \propto \exp \left(?\frac{1}{2}\left(a^{?}{A}^{?1}a\right)+{\left(b?C{A}^{?1}a\right)}^{?}{\Delta }_{\mathrm{A}}^{?1}\left(b?C{A}^{?1}a\right)\right)\\ \propto \exp \left(?\frac{1}{2}{a}^{?}{A}^{?1}a\right)\exp \left(?\frac{1}{2}{\left(b?C{A}^{?1}a\right)}^{?}{\Delta }_{\mathrm{A}}^{?1}\left(b?C{A}^{?1}a\right)\right)\end{array}$$其中$$P(a)\propto \exp \left(?\frac{1}{2}{a}^{?}{A}^{?1}a\right)$$$$P(b|a)\propto \exp \left(?\frac{1}{2}{\left(b?C{A}^{?1}a\right)}^{?}{\Delta }_{\mathrm{A}}^{?1}\left(b?C{A}^{?1}a\right)\right)$$則$$P(a)～\mathcal{N}(0,A)\text{\hspace{1em}(1)}$$$$P(b|a)～\mathcal{N}\left(C{A}^{?1}a,{\Delta }_A\right)\text{\hspace{1em}(2)}$$$P(a)$和$P(b|a)$的協方差矩陣分別是，$A$和${\Delta }_A$通過這個結論可以看出，從聯合分布中的協方差矩陣求得邊際概率的協方差矩陣$A$是簡單的，求得條件概率的協方差矩陣${\Delta }_A$會相對復雜，但是呢，我們應該更加關注聯合分布的信息矩陣，因為我們在BA問題中研究的其實是信息矩陣而不是協方差矩陣，假設我們已知上述問題的信息矩陣為：$${\left[\begin{array}{cc}A& C^{?}\\ C& D\end{array}\right]}^{?1}=\left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]$$那么可以求得協方差矩陣各塊和信息矩陣各塊之間的關系為$${\left[\begin{array}{cc}A& C^{?}\\ C& D\end{array}\right]}^{?1}=\left[\begin{array}{cc}{A}^{?1}+A^{?1}{C}^{?}{\Delta }_{\mathrm{A}}^{?1}C{A}^{?1}& ?A^{?1}{C}^{?}{\Delta }_{\mathrm{A}}^{?1}\\ ?{\Delta }_{\mathrm{A}}^{?1}C{A}^{?1}& {\Delta }_{\mathrm{A}}^{?1}\end{array}\right]?\left[\begin{array}{cc}{\Lambda }_{aa}& {\Lambda }_{ab}\\ {\Lambda }_{ba}& {\Lambda }_{bb}\end{array}\right]\text{\hspace{1em}(3)}$$由（1）我們知道$P(b|a)$的協方差矩陣為${\Delta }_A$，那么它的信息矩陣就是${\Delta }_A^{?1}$，由（3）就可以知道其信息矩陣為${\Lambda }_{bb}$，同理，由（2）和（3）我們可以知道$P(a)$的信息矩陣為${\Lambda }_{aa}?{\Lambda }_{ab}{\Lambda }_{bb}^{?1}{\Lambda }_{ba}$，由此我們就知道如何從聯合分布的信息矩陣中求解$P(a)$和$P(b|a)$的信息矩陣了，這一點應用到我們上面最開始的問題中，已知聯合分布的信息矩陣為$${\mathbf{\Sigma }}^{?1}=?\begin{array}{ccc}\frac{1}{{\sigma }_1^2}& ?\frac{w_1}{{\sigma }_1^2}& \theta \\ ?\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_1^2}+\frac{w_3^2}{{\sigma }_1^2}& \frac{w_3}{{\sigma }_3^2}\\ \theta & \frac{w_3}{{\sigma }_3^2}& \frac{1}{{\sigma }_3^2}\end{array}?$$那么，其邊際概率的信息矩陣${\mathbf{\Sigma }}_2^{?1}$為$$\begin{array}{rl}{\mathbf{\Sigma }}_2^{?1}& ={\Lambda }_{aa}?{\Lambda }_{ab}{\Lambda }_{bb}^{?1}{\Lambda }_{ba}\\ & ={\Lambda }_{aa}?\left[\begin{array}{c}0\\ ?\frac{w_3}{{\sigma }_3^2}\end{array}\right]{\sigma }_3^2\left[0?\frac{w_3}{{\sigma }_3^2}\right]\\ & ={\Lambda }_{aa}?\left[\begin{array}{cc}0& 0\\ 0& \frac{w_3}{{\sigma }_3^2}\end{array}\right]\\ & =\left[\begin{array}{cc}\frac{1}{{\sigma }_1^2}& ?\frac{w_1}{{\sigma }_1^2}\\ ?\frac{w_1}{{\sigma }_1^2}& \frac{w_1^2}{{\sigma }_1^2}+\frac{1}{{\sigma }_2^2}\end{array}\right]\end{array}$$這樣就證明了問題的結論，而這一步${\Lambda }_{aa}?{\Lambda }_{ab}{\Lambda }_{bb}^{?1}{\Lambda }_{ba}$我們稱之為求Shur補的操作，我們將其應用到實際的更復雜中就是下面這種情況，下面這幅圖很好地說明了邊緣化的過程：

上圖中，${\xi }_1$、${\xi }_2$、${\xi }_3$、${\xi }_4$、${\xi }_5$、${\xi }_6$分別為六個節點，通過邊緣化操作可以將原稀疏的矩陣變成稠密矩陣，而增加的稠密部分其實就是被邊緣化掉的那個節點傳遞給當前狀態的信息，也就是使得原本獨立的各個變量變得相關。

1.3 在實際的邊緣化操作中有什么需要注意的嗎？

一個比較值得注意的問題是新老信息融合的問題，也就是FEJ算法的使用，如下所示:

承接上面的例子，當我們邊緣話掉變量${\xi }_1$有加入新的變量${\xi }_7$會有如下新老信息融合的情況發生，就${\xi }_2$這個矩陣而言，它的信息矩陣是有兩部分構成的。

新老信息融合的問題在于舊的求解雅克比矩陣的變量線性化點和和新的求解雅克比矩陣的變量線性化點不同，可能會導致信息矩陣的零空間發生變化，使得不客觀的變量變得可觀，從而引入錯誤信息，這個解釋可能會比較抽象，更加具體的解釋可以參看賀博的博客SLAM中的marginalization 和 Schur complement，上述問題的解決辦法呢就是FEJ算法：不同殘差對同一個狀態求雅克比時，線性化點必須一致。這樣就能避免零空間退化而使得不可觀變量變得可觀，具體來說就是計算$r_{27}$ 對 ${\xi }_2$ 的雅克比時，${\xi }_2$ 的線性話點必須和 $r_{12}$對其求導時一致。

2. 代碼剖析

上面理論搞清楚了其實只是第一步，由于VINS-mono優化的變量較多，VINS-mono的邊緣化操作實際上要復雜很多，VINS-mono的邊緣化相關代碼在estimator.cpp的Estimator類的optimization()函數中，該函數先會先進行后端非線性優化然后緊接著就是邊緣化操作，下面就針對這個函數中的邊緣化相關代碼進行剖析。

2.1 優化變量分析

首先我們確定下參與邊緣化操作的變量有哪些，這個可以從vector2double()函數中看出來，因為ceres中變量必須用數組類型，所以需要這樣一個函數進行數據類型轉換，如下：

void Estimator::vector2double() {for (int i = 0; i <= WINDOW_SIZE; i++){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();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();}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);if (ESTIMATE_TD)para_Td[0][0] = td; }

可以看出來，這里面生成的優化變量由：
para_Pose（6維，相機位姿）、
para_SpeedBias（9維，相機速度、加速度偏置、角速度偏置）、
para_Ex_Pose（6維、相機IMU外參）、
para_Feature（1維，特征點深度）、
para_Td（1維，標定同步時間）
五部分組成，在后面進行邊緣化操作時這些優化變量都是當做整體看待。

2.1 MarginalizationInfo類分析

然后，我們先看下和邊緣化類MarginalizationInfo

class MarginalizationInfo {public:~MarginalizationInfo();int localSize(int size) const;int globalSize(int size) const;//添加參差塊相關信息（優化變量，待marg的變量）void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);//計算每個殘差對應的雅克比，并更新parameter_block_datavoid preMarginalize();//pos為所有變量維度，ｍ為需要marg掉的變量，ｎ為需要保留的變量void marginalize();std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);std::vector<ResidualBlockInfo *> factors;//所有觀測項int m, n;//m為要邊緣化的變量個數，n為要保留下來的變量個數std::unordered_map<long, int> parameter_block_size; //<優化變量內存地址,localSize>int sum_block_size;std::unordered_map<long, int> parameter_block_idx; //<優化變量內存地址,在矩陣中的id>std::unordered_map<long, double *> parameter_block_data;//<優化變量內存地址,數據>std::vector<int> keep_block_size; //global sizestd::vector<int> keep_block_idx; //local sizestd::vector<double *> keep_block_data;Eigen::MatrixXd linearized_jacobians;Eigen::VectorXd linearized_residuals;const double eps = 1e-8; };

先說變量，這里有三個unordered_map相關的變量分別是：
parameter_block_size、
parameter_block_idx、
parameter_block_data，
他們的key都同一是long類型的內存地址，而value分別是，各個優化變量的長度，各個優化變量在id以各個優化變量對應的double指針類型的數據。
對應的有三個vector相關的變量分別是：
keep_block_size、
keep_block_idx、
keep_block_data，
他們是進行邊緣化之后保留下來的各個優化變量的長度，各個優化變量在id以各個優化變量對應的double指針類型的數據
還有
linearized_jacobians、
linearized_residuals，
分別指的是邊緣化之后從信息矩陣恢復出來雅克比矩陣和殘差向量

2.3 第一步：調用addResidualBlockInfo()

對于函數我們直接看optimization中的調用會更直觀，首先會調用addResidualBlockInfo()函數將各個殘差以及殘差涉及的優化變量添加入上面所述的優化變量中：
首先添加上一次先驗殘差項：

if (last_marginalization_info) {vector<int> drop_set;for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++)//last_marginalization_parameter_blocks是上一輪留下來的殘差塊{if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||last_marginalization_parameter_blocks[i] == para_SpeedBias[0])//需要marg掉的優化變量，也就是滑窗內第一個變量drop_set.push_back(i);}// construct new marginlization_factorMarginalizationFactor *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預積分值以及第0幀和第1幀相關優化變量

{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});//這里是0,1的原因是0和1是para_Pose[0], para_SpeedBias[0]是需要marg的變量marginalization_info->addResidualBlockInfo(residual_block_info);} }

最后添加第一次觀測滑窗中第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 imu_i = it_per_id.start_frame, imu_j = imu_i - 1;//這里是從特征點的第一個觀察幀開始if (imu_i != 0)//如果第一個觀察幀不是第一幀就不進行考慮，因此后面用來構建marg矩陣的都是和第一幀有共視關系的滑窗幀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});//為0和3的原因是，para_Pose[imu_i]是第一幀的位姿，需要marg掉，而3是para_Feature[feature_index]是和第一幀相關的特征點，需要marg掉marginalization_info->addResidualBlockInfo(residual_block_info);}}} }

上面添加殘差以及優化變量的方式和后端線性優化中添加的方式相似，因為邊緣化類應該就是仿照ceres寫的，我們可以簡單剖析下上面的操作，
第一步定義損失函數，對于先驗殘差就是MarginalizationFactor，對于IMU就是IMUFactor，對于視覺就是ProjectionTdFactor，這三個損失函數的類都是繼承自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）可以計算出各項殘差值residuals，以及殘差對應個優化變量的雅克比矩陣jacobians。

第二步定義ResidualBlockInfo，其構造函數如下

ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set)

這一步是為了將不同的損失函數_cost_function以及優化變量_parameter_blocks統一起來再一起添加到marginalization_info中。變量_loss_function是核函數，在VINS-mono的邊緣化中僅僅視覺殘差有用到couchy核函數，另外會設置需要被邊緣話的優化變量的位置_drop_set，這里對于不同損失函數又會有不同：
對于先驗損失，其待邊緣化優化變量是根據是否等于para_Pose[0]或者para_SpeedBias[0]，也就是說和第一幀相關的優化變量都作為邊緣化的對象。
對于IMU，其輸入的_drop_set是vector{0, 1}，也就是說其待邊緣化變量是para_Pose[0], para_SpeedBias[0]，也是第一政相關的變量都作為邊緣化的對象，這里值得注意的是和后端優化不同，這里只添加了第一幀和第二幀的相關變量作為優化變量，因此邊緣化構造的信息矩陣會比后端優化構造的信息矩陣要小
對于視覺，其輸入的_drop_set是vector{0, 3}，也就是說其待邊緣化變量是para_Pose[imu_i]和para_Feature[feature_index]，從這里可以看出來在VINS-mono的邊緣化操作中會不僅僅會邊緣化第一幀相關的優化變量，還會邊緣化掉以第一幀為起始觀察幀的路標點。

第三步是將定義的residual_block_info添加到marginalization_info中，通過下面這一句

marginalization_info->addResidualBlockInfo(residual_block_info);

然后可以看下addResidualBlockInfo()這個函數的實現如下：

void MarginalizationInfo::addResidualBlockInfo(ResidualBlockInfo *residual_block_info) {factors.emplace_back(residual_block_info);std::vector<double *> &parameter_blocks = residual_block_info->parameter_blocks;//parameter_blocks里面放的是marg相關的變量std::vector<int> parameter_block_sizes = residual_block_info->cost_function->parameter_block_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]];//這個是待邊緣化的變量的idparameter_block_idx[reinterpret_cast<long>(addr)] = 0;//將需要marg的變量的id存入parameter_block_idx} }

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

2.4 第二步：調用preMarginalize()

上面通過調用addResidualBlockInfo()已經確定優化變量的數量、存儲位置、長度以及待優化變量的數量以及存儲位置，下面就需要調用preMarginalize()進行預處理，preMarginalize()實現如下：

void MarginalizationInfo::preMarginalize() {for (auto it : factors)//在前面的addResidualBlockInfo中會將不同的殘差塊加入到factor中{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())//parameter_block_data是整個優化變量的數據{double *data = new double[size];memcpy(data, it->parameter_blocks[i], sizeof(double) * size);//重新開辟一塊內存parameter_block_data[addr] = data;//通過之前的優化變量的數據的地址和新開辟的內存數據進行關聯}}} }

其中 it->Evaluate()這一句里面其實就是調用各個損失函數中的重載函數Evaluate()，這個函數前面有提到過，就是

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

這個函數通過傳入的優化變量值parameters，以及先驗值（對于先驗殘差就是上一時刻的先驗殘差last_marginalization_info，對于IMU就是預計分值pre_integrations[1]，對于視覺就是空間的的像素坐標pts_i, pts_j）可以計算出各項殘差值residuals，以及殘差對應個優化變量的雅克比矩陣jacobians。此外這里會給parameter_block_data賦值，這里引用崔華坤老師寫的《VINS 論文推導及代碼解析》中的例子

parameter_block_sizes中的key值就是上表中的左邊第一列，value值就是上表中的中間一列（localSize）
parameter_block_data中的key值就是上表中的左邊第一列，value值就是上表中的右邊第一列（double*的數據）

2.5 第三步：調用marginalize()

前面兩步已經將數據都準備好了，下面通過調用marginalize()函數就要正式開始進行邊緣化操作了，實現如下：

void MarginalizationInfo::marginalize() {int pos = 0;for (auto &it : parameter_block_idx)//遍歷待marg的優化變量的內存地址{it.second = pos;pos += localSize(parameter_block_size[it.first]);}m = pos;//需要marg掉的變量個數for (const auto &it : parameter_block_size){if (parameter_block_idx.find(it.first) == parameter_block_idx.end())//如果這個變量不是是待marg的優化變量{parameter_block_idx[it.first] = pos;//就將這個待marg的變量id設為pospos += localSize(it.second);//pos加上這個變量的長度}}n = pos - m;//要保留下來的變量個數//通過上面的操作就會將所有的優化變量進行一個偽排序，待marg的優化變量的idx為0，其他的和起所在的位置相關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;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;}//TODOEigen::MatrixXd Amm = 0.5 * (A.block(0, 0, m, m) + A.block(0, 0, m, m).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 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;//這里的A和b應該都是marg過的A和b,大小是發生了變化的//下面就是更新先驗殘差項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(); }

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

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

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

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

2.6 第四步：滑窗預移動

在optimization的最后會有一部滑窗預移動的操作，就是下面這一段代碼

std::unordered_map<long, double *> addr_shift; for (int i = 1; i <= WINDOW_SIZE; i++)//從1開始，因為第一幀的狀態不要了 {//這一步的操作指的是第i的位置存放的的是i-1的內容，這就意味著窗口向前移動了一格addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];//因此para_Pose這些變量都是雙指針變量，因此這一步是指針操作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;//marg相關內容的遞歸 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就是保留下來的滑窗內的所有的優化變量

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

到此邊緣化操作的流程就介紹完了，上面介紹的邊緣化最老幀的情況，邊緣化次新幀的方式類似，在此就不再贅述，如果有什么問題歡迎交流～

此外，對其他SLAM算法感興趣的同學可以看考我的博客SLAM算法總結——經典SLAM算法框架總結

總結

以上是生活随笔為你收集整理的VINS-Mono关键知识点总结——边缘化marginalization理论和代码详解的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。