1. 邊緣化理論

邊緣化相關(guān)的理論主要是參考高博和賀博的課程以及三篇參考文獻(xiàn)：

• 《The Humble Gaussian Distribution》
• 《Exactly Sparse Extended Information Filters for Feature-Based SLAM》
• 《Consistency Analysis for Sliding-Window Visual Odometry》

1.1 為什么要進(jìn)行邊緣化操作？

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

1.2 怎樣進(jìn)行邊緣化呢？

說明：?的證明在最后；

?

注意：以上的步驟就是為了證明 舒爾補(bǔ)的公式！

?

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

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

?

2. 代碼剖析

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

2.1 優(yōu)化變量分析

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

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

可以看出來，這里面生成的優(yōu)化變量由：

para_Pose（6維，相機(jī)位姿）、

para_SpeedBias（9維，相機(jī)速度、加速度偏置、角速度偏置）、

para_Ex_Pose（6維、相機(jī)IMU外參）、

para_Feature（1維，特征點深度）、

para_Td（1維，標(biāo)定同步時間）

五部分組成，在后面進(jìn)行邊緣化操作時這些優(yōu)化變量都是當(dāng)做整體看待。

2.1 MarginalizationInfo類分析

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

class MarginalizationInfo {public:~MarginalizationInfo();int localSize(int size) const;int globalSize(int size) const;//添加參差塊相關(guān)信息（優(yōu)化變量，待marg的變量）void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);//計算每個殘差對應(yīng)的雅克比，并更新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為要邊緣化的變量個數(shù)，n為要保留下來的變量個數(shù)std::unordered_map<long, int> parameter_block_size; //<優(yōu)化變量內(nèi)存地址,localSize>int sum_block_size;std::unordered_map<long, int> parameter_block_idx; //<優(yōu)化變量內(nèi)存地址,在矩陣中的id>std::unordered_map<long, double *> parameter_block_data;//<優(yōu)化變量內(nèi)存地址,數(shù)據(jù)>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相關(guān)的變量分別是：

• parameter_block_size、
• parameter_block_idx、
• parameter_block_data，

他們的key都同一是long類型的內(nèi)存地址，而value分別是，各個優(yōu)化變量的長度，各個優(yōu)化變量的id以及各個優(yōu)化變量對應(yīng)的double指針類型的數(shù)據(jù)。

對應(yīng)的有三個vector相關(guān)的變量分別是：

• keep_block_size、
• keep_block_idx、
• keep_block_data，

他們是進(jìn)行邊緣化之后保留下來的各個優(yōu)化變量的長度，各個優(yōu)化變量在id以各個優(yōu)化變量對應(yīng)的double指針類型的數(shù)據(jù)
還有

• linearized_jacobians、
• linearized_residuals，

分別指的是邊緣化之后從信息矩陣恢復(fù)出來雅克比矩陣和殘差向量

2.3 第一步：調(diào)用addResidualBlockInfo()

對于函數(shù)我們直接看optimization中的調(diào)用會更直觀，首先會調(diào)用addResidualBlockInfo()函數(shù)將各個殘差以及殘差涉及的優(yōu)化變量添加入上面所述的優(yōu)化變量中：

• 首先添加上一次先驗殘差項：（上一次待邊緣化的參數(shù)與其他參數(shù)的約束關(guān)系）

VINS-Mono 代碼解析六、邊緣化（3）_努力努力努力-CSDN博客

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掉的優(yōu)化變量，也就是滑窗內(nèi)第一個變量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預(yù)積分值以及第0幀和第1幀相關(guān)優(yōu)化變量

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

• 最后添加第一次觀測滑窗中第0幀的路標(biāo)點以及其他相關(guān)的滑窗中的幀的相關(guān)的優(yōu)化變量

{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)//如果第一個觀察幀不是第一幀就不進(jìn)行考慮，因此后面用來構(gòu)建marg矩陣的都是和第一幀有共視關(guān)系的滑窗幀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]},//優(yōu)化變量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]},//優(yōu)化變量vector<int>{0, 3});//為0和3的原因是，para_Pose[imu_i]是第一幀的位姿，需要marg掉，而3是para_Feature[feature_index]是和第一幀相關(guān)的特征點，需要marg掉marginalization_info->addResidualBlockInfo(residual_block_info);}}} }

上面添加殘差以及優(yōu)化變量的方式和后端線性優(yōu)化中添加的方式相似，因為邊緣化類應(yīng)該就是仿照ceres寫的，我們可以簡單剖析下上面的操作；

第一步定義損失函數(shù)，對于先驗殘差就是MarginalizationFactor，對于IMU就是IMUFactor，對于視覺就是ProjectionTdFactor，這三個損失函數(shù)的類都是繼承自ceres的損失函數(shù)類ceres::CostFunction，里面都重載了函數(shù)；

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

這個函數(shù)通過傳入的優(yōu)化變量值parameters，以及先驗值（對于先驗殘差就是上一時刻的先驗殘差last_marginalization_info，對于IMU就是預(yù)計分值pre_integrations[1]，對于視覺就是空間的的像素坐標(biāo)pts_i, pts_j）可以計算出各項殘差值residuals，以及殘差對應(yīng)個優(yōu)化變量的雅克比矩陣jacobians。

第二步定義ResidualBlockInfo，其構(gòu)造函數(shù)如下

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

這一步是為了將不同的損失函數(shù)_cost_function以及優(yōu)化變量_parameter_blocks統(tǒng)一起來再一起添加到marginalization_info中。變量_loss_function是核函數(shù)，在VINS-mono的邊緣化中僅僅視覺殘差有用到couchy核函數(shù)，另外會設(shè)置需要被邊緣話的優(yōu)化變量的位置_drop_set，這里對于不同損失函數(shù)又會有不同：

• 對于先驗損失，其待邊緣化優(yōu)化變量是根據(jù)是否等于para_Pose[0]或者para_SpeedBias[0]，也就是說和第一幀相關(guān)的優(yōu)化變量都作為邊緣化的對象；
• 對于IMU，其輸入的_drop_set是vector{0, 1}，也就是說其待邊緣化變量是para_Pose[0], para_SpeedBias[0]，也是第一政相關(guān)的變量都作為邊緣化的對象，這里值得注意的是和后端優(yōu)化不同，這里只添加了第一幀和第二幀的相關(guān)變量作為優(yōu)化變量，因此邊緣化構(gòu)造的信息矩陣會比后端優(yōu)化構(gòu)造的信息矩陣要小；
• 對于視覺，其輸入的_drop_set是vector{0, 3}，也就是說其待邊緣化變量是para_Pose[imu_i]和para_Feature[feature_index]，從這里可以看出來在VINS-mono的邊緣化操作中會不僅僅會邊緣化第一幀相關(guān)的優(yōu)化變量，還會邊緣化掉以第一幀為起始觀察幀的路標(biāo)點。

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

marginalization_info->addResidualBlockInfo(residual_block_info);

然后可以看下addResidualBlockInfo()這個函數(shù)的實現(xiàn)如下：

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相關(guān)的變量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++)//這里應(yīng)該是優(yōu)化的變量{double *addr = parameter_blocks[i];//指向數(shù)據(jù)的指針int size = parameter_block_sizes[i];//因為僅僅有地址不行，還需要有地址指向的這個數(shù)據(jù)的長度parameter_block_size[reinterpret_cast<long>(addr)] = size;//將指針強(qiáng)轉(zhuǎn)為數(shù)據(jù)的地址}for (int i = 0; i < static_cast<int>(residual_block_info->drop_set.size()); i++)//這里應(yīng)該是待邊緣化的變量{double *addr = parameter_blocks[residual_block_info->drop_set[i]];//這個是待邊緣化的變量的idparameter_block_idx[reinterpret_cast<long>(addr)] = 0;//將需要marg的變量的id存入parameter_block_idx} }

這里其實就是分別將不同損失函數(shù)對應(yīng)的優(yōu)化變量、邊緣化位置存入到parameter_block_sizes和parameter_block_idx中，這里注意的是執(zhí)行到這一步，parameter_block_idx中僅僅有待邊緣化的優(yōu)化變量的內(nèi)存地址的key，而且其對應(yīng)value全部為0；

2.4 第二步：調(diào)用preMarginalize()

上面通過調(diào)用addResidualBlockInfo()已經(jīng)確定優(yōu)化變量的數(shù)量、存儲位置、長度以及待優(yōu)化變量的數(shù)量以及存儲位置，下面就需要調(diào)用preMarginalize()進(jìn)行預(yù)處理，preMarginalize()實現(xiàn)如下：
?

void MarginalizationInfo::preMarginalize() {for (auto it : factors)//在前面的addResidualBlockInfo中會將不同的殘差塊加入到factor中{it->Evaluate();//利用多態(tài)性分別計算所有狀態(tài)變量構(gòu)成的殘差和雅克比矩陣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]);//優(yōu)化變量的地址int size = block_sizes[i];if (parameter_block_data.find(addr) == parameter_block_data.end())//parameter_block_data是整個優(yōu)化變量的數(shù)據(jù){double *data = new double[size];memcpy(data, it->parameter_blocks[i], sizeof(double) * size);//重新開辟一塊內(nèi)存parameter_block_data[addr] = data;//通過之前的優(yōu)化變量的數(shù)據(jù)的地址和新開辟的內(nèi)存數(shù)據(jù)進(jìn)行關(guān)聯(lián)}}} }

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

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

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

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

2.5 第三步：調(diào)用marginalize()

前面兩步已經(jīng)將數(shù)據(jù)都準(zhǔn)備好了，下面通過調(diào)用marginalize()函數(shù)就要正式開始進(jìn)行邊緣化操作了，實現(xiàn)如下：
?

void MarginalizationInfo::marginalize() {int pos = 0;for (auto &it : parameter_block_idx)//遍歷待marg的優(yōu)化變量的內(nèi)存地址{it.second = pos;pos += localSize(parameter_block_size[it.first]);}m = pos;//需要marg掉的變量個數(shù)for (const auto &it : parameter_block_size){if (parameter_block_idx.find(it.first) == parameter_block_idx.end())//如果這個變量不是是待marg的優(yōu)化變量{parameter_block_idx[it.first] = pos;//就將這個待marg的變量id設(shè)為pospos += localSize(it.second);//pos加上這個變量的長度}}n = pos - m;//要保留下來的變量個數(shù)//通過上面的操作就會將所有的優(yōu)化變量進(jìn)行一個偽排序，待marg的優(yōu)化變量的idx為0，其他的和起所在的位置相關(guān)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]));//分別構(gòu)造矩陣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();//舒爾補(bǔ)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應(yīng)該都是marg過的A和b,大小是發(fā)生了變化的//下面就是更新先驗殘差項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數(shù)據(jù)結(jié)構(gòu)沒有即添加，存在即賦值的語法，上面的代碼會先補(bǔ)充parameter_block_idx，前面提到經(jīng)過addResidualBlockInfo()函數(shù)僅僅帶邊緣化的優(yōu)化變量在parameter_block_idx有key值，這里會將保留的優(yōu)化變量的內(nèi)存地址作為key值補(bǔ)充進(jìn)去，并統(tǒng)一他們的value值是其前面已經(jīng)放入parameter_block_idx的優(yōu)化變量的維度之和，同時這里會計算出兩個變量m和n，他們分別是待邊緣化的優(yōu)化變量的維度和以及保留的優(yōu)化變量的維度和。

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

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

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

2.6 第四步：滑窗預(yù)移動

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

std::unordered_map<long, double *> addr_shift; for (int i = 1; i <= WINDOW_SIZE; i++)//從1開始，因為第一幀的狀態(tài)不要了 {//這一步的操作指的是第i的位置存放的的是i-1的內(nèi)容，這就意味著窗口向前移動了一格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相關(guān)的內(nèi)容 last_marginalization_info = marginalization_info;//marg相關(guān)內(nèi)容的遞歸 last_marginalization_parameter_blocks = parameter_blocks;//優(yōu)化變量的遞歸，這里面僅僅是指針

值得注意的是，這里僅僅是相當(dāng)于將指針進(jìn)行了一次移動，指針對應(yīng)的數(shù)據(jù)還是舊數(shù)據(jù)，因此需要結(jié)合后面調(diào)用的slideWindow()函數(shù)才能實現(xiàn)真正的滑窗移動，此外last_marginalization_info就是保留下來的先驗殘差信息，包括保留下來的雅克比linearized_jacobians、殘差linearized_residuals、保留下來的和邊緣化有關(guān)的數(shù)據(jù)長度keep_block_size、順序keep_block_idx以及數(shù)據(jù)keep_block_data。
last_marginalization_info就是保留下來的滑窗內(nèi)的所有的優(yōu)化變量

這里需要明確一個概念就是，邊緣化操作并不會改變優(yōu)化變量的值，而僅僅是改變了優(yōu)化變量之間的關(guān)系，而這個關(guān)系就是通過信息矩陣體現(xiàn)的。
?

備注：?的證明；

?

toy example 2

總結(jié)

以上是生活随笔為你收集整理的VINS-Mono 代码解析六、边缘化（2）理论和代码详解的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。