“ 在優(yōu)化完成后，滑動(dòng)窗口前，必須進(jìn)行邊緣化項(xiàng)的操作”

一、理論部分

VINS 細(xì)節(jié)系列 - 窗口優(yōu)化_努力努力努力-CSDN博客

?一個(gè)邊緣化的例子

下面我們用一個(gè)具體例子來形象說明邊緣化過程及其導(dǎo)致的矩陣稠密現(xiàn)象(fill-in)。設(shè)有 四個(gè)相機(jī)位姿 xpi ,

以及? 6 個(gè)路標(biāo)點(diǎn)xmk (路標(biāo)點(diǎn)用 xyz 的參數(shù)化), 相機(jī)與路標(biāo)點(diǎn)的邊表示一次觀測(cè), 相鄰相機(jī)之間的邊表示 IMU 約束,

相互關(guān)系如下:

下面試圖將 xp1? 給 marg 掉,然后再將 xm1 給 marg 掉, 看看 H 矩陣會(huì)如何變化

圖(12-a) 表示原始的 H 矩陣,注意這里的 左上角為路標(biāo)點(diǎn)相關(guān)部分, 而右下角是 pose 相關(guān)部分

圖(12-b)是把 H 矩陣中跟x p1 相關(guān)的部分移動(dòng)到 H 矩陣左上角

其中各矩陣的維度已在上式標(biāo)明,也就是說圖(12-c)的矩陣大小為 32×32。我們可以看到,圖(c)相對(duì)于圖(b)變得更稠密了,

即 marg 掉一個(gè) pose,會(huì)使得剩余的 H 矩陣有 3 個(gè)地方被 fill-in,如圖(c)中顏色加重區(qū)域。

這時(shí)圖關(guān)系則變?yōu)?

注意,觀察圖 16 可發(fā)現(xiàn)這時(shí)的 xm1 、xm2 和 xm3 彼此之間已經(jīng)產(chǎn)生了新的約束關(guān)系,且xp2 也與xm1 產(chǎn)生了新的約束關(guān)系。

圖(12-d)是 marg 掉xm1 后的 H 矩陣,詳細(xì)如下所示:

對(duì)應(yīng)的圖關(guān)系如下:

我們發(fā)現(xiàn), marg 掉xm1 后,并沒有是 H 矩陣更稠密,這是因?yàn)閤m1 之前并未與其他pose 有約束關(guān)系,即并未被觀察到,

因此如果 marg 掉那些不被其他幀觀測(cè)到的路標(biāo)點(diǎn),不會(huì)顯著地使 H 矩陣變得稠密。而要 marg 掉的路標(biāo)點(diǎn)中,對(duì)于那

些被其他幀觀測(cè)到路標(biāo)點(diǎn),要么就別設(shè)置為 marg, 要么就寧愿丟棄,這就是 OKVIS 中用到的策略

簡(jiǎn)單介紹

?1、優(yōu)化變量：

序號(hào)?????? ? ? ? 變量??????????? ? ?? ???? ? ? ? ?? ?? 維度

?? 0????????? para_Pose???????????????? （6維，相機(jī)位姿）
?? 1????????? para_SpeedBias??? （9維，相機(jī)速度、加速度偏置、角速度偏置）、
?? 2?????? ?? para_Ex_Pose???????? （6維、相機(jī)IMU外參）、
?? 3 ? ? ? ?? para_Feature?????????? （1維，特征點(diǎn)深度）、
?? 4????????? para_Td????????????????????? （1維，標(biāo)定同步時(shí)間）
五部分組成，在后面進(jìn)行邊緣化操作時(shí)這些優(yōu)化變量都是當(dāng)做整體看待

last_marginalization_parameter_blocks ：Xb變量，也是我們要優(yōu)化的變量；
last_marginalization_info??????????? ? ? ? ? ? ? ? ? ? ? ：Xm與Xb對(duì)應(yīng)的測(cè)量Zb，也是先驗(yàn)殘差（細(xì)品一下）

2、思路：
?

邊緣化策略

邊緣化分兩種情況，每種情況有各自的流程

a. 如果次新幀是關(guān)鍵幀，則將邊緣化最老幀，及其看到的路標(biāo)點(diǎn)和IMU數(shù)據(jù)，轉(zhuǎn)化為先驗(yàn)。具體流程為：

1）將上一次先驗(yàn)殘差項(xiàng)傳遞給marginalization_info

2）將第0幀和第1幀間的IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中

3）將第一次觀測(cè)為第0幀的所有路標(biāo)點(diǎn)對(duì)應(yīng)的視覺觀測(cè)，添加到marginalization_info中

4）計(jì)算每個(gè)殘差，對(duì)應(yīng)的Jacobian，并將各參數(shù)塊拷貝到統(tǒng)一的內(nèi)存（parameter_block_data）中

5）多線程構(gòu)造先驗(yàn)項(xiàng)舒爾補(bǔ)AX=b的結(jié)構(gòu)，在X0處線性化計(jì)算Jacobian和殘差

6）調(diào)整參數(shù)塊在下一次窗口中對(duì)應(yīng)的位置（往前移一格），注意這里是指針，后面slideWindow中會(huì)賦新值，這里只是提前占座

b. 如果次新幀不是關(guān)鍵幀，此時(shí)具體流程為：

1）保留次新幀的IMU測(cè)量，丟棄該幀的視覺測(cè)量，將上一次先驗(yàn)殘差項(xiàng)傳遞給marginalization_info

2）premargin：計(jì)算每個(gè)殘差，對(duì)應(yīng)的Jacobian，并更新parameter_block_data

3）marginalize：構(gòu)造先驗(yàn)項(xiàng)舒爾補(bǔ)AX=b的結(jié)構(gòu)，計(jì)算Jacobian和殘差

4）調(diào)整參數(shù)塊在下一次窗口中對(duì)應(yīng)的位置（去掉次新幀）

3、marginalization_factor.cpp 代碼中有幾個(gè)變量需要提前說明:

舉例說明,當(dāng)?shù)谝淮?marg 掉最老幀時(shí),parameter_block_size 為所有變量及其對(duì)應(yīng)的
localSize,? parameter_block_data 為對(duì)應(yīng)的數(shù)據(jù)(double*類型)

?

二、程序部分

2.1? 把下面這個(gè)圖搞明白！

?

2.2 看 ?"marginalization_factor.h"? 中類和結(jié)構(gòu)體的定義

?1、class ResidualBlockInfo

類描述：這個(gè)類保存了待marg變量（xm）與相關(guān)聯(lián)變量（xb）之間的一個(gè)約束因子關(guān)系? -? Zm

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();//調(diào)用cost_function的evaluate函數(shù)計(jì)算殘差 和 雅克比矩陣ceres::CostFunction *cost_function;ceres::LossFunction *loss_function;std::vector<double *> parameter_blocks;std::vector<int> drop_set;double **raw_jacobians;std::vector<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobians;Eigen::VectorXd residuals;int localSize(int size){return size == 7 ? 6 : size;} }; 成員變量描述

cost_function	對(duì)應(yīng)ceres中表示約束因子的類
parameter_blocks	該約束因子相關(guān)聯(lián)的參數(shù)塊變量
drop_set	parameter_blocks中待marg變量的索引

2、class MarginalizationInfo

類描述：這個(gè)類保存了優(yōu)化時(shí)上一步邊緣化后保留下來的先驗(yàn)信息

class MarginalizationInfo {public:~MarginalizationInfo();int localSize(int size) const;int globalSize(int size) const;void addResidualBlockInfo(ResidualBlockInfo *residual_block_info);void preMarginalize();void marginalize();std::vector<double *> getParameterBlocks(std::unordered_map<long, double *> &addr_shift);std::vector<ResidualBlockInfo *> factors;//這里將參數(shù)塊分為Xm,Xb,Xr,Xm表示被marg掉的參數(shù)塊，Xb表示與Xm相連接的參數(shù)塊，Xr表示剩余的參數(shù)塊//那么m=Xm的localsize之和，n為Xb的localsize之和，pos為（Xm+Xb）localsize之和int m, n; std::unordered_map<long, int> parameter_block_size; //global size 將被marg掉的約束邊相關(guān)聯(lián)的參數(shù)塊，即將被marg掉的參數(shù)塊以及與它們直接相連的參數(shù)快int sum_block_size;std::unordered_map<long, int> parameter_block_idx; //local size 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;}; 成員變量描述

factors	xm與xb之間的約束因子，Zb
m	所有將被marg掉變量的localsize之和，如上圖中xm的localsize
n	所有與將被marg掉變量有約束關(guān)系的變量的localsize之和，如上圖中xb的localsize
parameter_block_size	<參數(shù)塊地址，參數(shù)塊的global size>，參數(shù)塊包括xm和xb
parameter_block_idx	<參數(shù)塊地址，參數(shù)塊排序好后的索引>，對(duì)參數(shù)塊進(jìn)行排序，xm排在前面，xb排成后面，使用localsize
parameter_block_data	<參數(shù)塊地址，參數(shù)塊數(shù)據(jù)>，需要注意的是這里保存的參數(shù)塊數(shù)據(jù)是原始參數(shù)塊數(shù)據(jù)的一個(gè)拷貝，不再變化，用于記錄這些參數(shù)塊變量在marg時(shí)的狀態(tài)
keep_block_size	<保留下來的參數(shù)塊地址，參數(shù)塊的globalsize>
keep_block_idx	<保留下來的參數(shù)塊地址，參數(shù)塊的索引>，保留下來的參數(shù)塊是xb

成員函數(shù)信息：

成員函數(shù)描述

preMarginalize	得到每次IMU和視覺觀測(cè)對(duì)應(yīng)的參數(shù)塊，雅克比矩陣，殘差值
marginalize	開啟多線程構(gòu)建信息矩陣H和b ，同時(shí)從H,b中恢復(fù)出線性化雅克比和殘差

3、class MarginalizationFactor

類描述：該類是優(yōu)化時(shí)表示上一步邊緣化后保留下來的先驗(yàn)信息代價(jià)因子，變量marginalization_info保存了類似約束測(cè)量信息

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

2.3 程序講解

邊緣化部分在? optimization 函數(shù)里面，當(dāng)執(zhí)行完后端優(yōu)化后，緊接著就會(huì)執(zhí)行邊緣化，兩個(gè)過程都需要添加優(yōu)化變量和殘差，

但區(qū)別是邊緣化的變量少一點(diǎn)！主要是構(gòu)建模塊的過程！

?

一、如果邊緣化 “老幀”

【1】首先 把上一次先驗(yàn)項(xiàng)中的殘差項(xiàng)(尺寸為 n)傳遞給當(dāng)前先驗(yàn)項(xiàng)，并從中去除需要丟棄 的狀態(tài)量;

即從舊的先驗(yàn)殘差項(xiàng) last_marginalization_info 新建一個(gè)新的 marginalization_factor，參與這個(gè)殘差項(xiàng)的優(yōu)化變量是：last_marginalization_parameter_blocks 里面的內(nèi)容：para_Pose、para_SpeedBias、para_Ex_Pose、para_Feature、para_Td 都是按順序排好的，所以要把 要丟棄 的狀態(tài)量? para_Pose[0]、para_SpeedBias[0] 對(duì)應(yīng)的序號(hào)用 drop_set 記錄下來

// --------------------------------------marginalization-----------------------------------------//添加殘差以及優(yōu)化變量的方式和后端線性優(yōu)化中添加的方式相似，因?yàn)檫吘壔悜?yīng)該就是仿照ceres寫的TicToc t_whole_marginalization;//一、邊緣化老幀，那就要考慮老幀的情況！ //要邊緣化的參數(shù)塊是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗內(nèi)的第feature_index個(gè)點(diǎn)的逆深度)if (marginalization_flag == MARGIN_OLD){MarginalizationInfo *marginalization_info = new MarginalizationInfo();//先驗(yàn)信息vector2double(); //【1】首先添加上一次先驗(yàn)殘差項(xiàng)if (last_marginalization_info && last_marginalization_info->valid){vector<int> drop_set;for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++){//查詢last_marginalization_parameter_blocks中是首幀狀態(tài)量的序號(hào)if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||last_marginalization_parameter_blocks[i] == para_SpeedBias[0])drop_set.push_back(i);}MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,last_marginalization_parameter_blocks,drop_set); //調(diào)用addResidualBlockInfo()函數(shù)將各個(gè)殘差以及殘差涉及的優(yōu)化變量添加入優(yōu)化變量中marginalization_info->addResidualBlockInfo(residual_block_info);}

【2】添加IMU的marg信息到 marginalization_info 中：第0幀和第1幀之間的IMU預(yù)積分值以及第0幀和第1幀相關(guān)優(yōu)化變量

if(USE_IMU){if (pre_integrations[1]->sum_dt < 10.0){IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);//0和1是para_Pose[0], para_SpeedBias[0]，需要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});marginalization_info->addResidualBlockInfo(residual_block_info);}}

【3】最后添加第一次觀測(cè)滑窗中第0幀的路標(biāo)點(diǎn)以及其他相關(guān)的滑窗中的幀的相關(guān)的優(yōu)化變量 到 marginalization_info 中

{int feature_index = -1;//這里是遍歷滑窗所有的特征點(diǎn)for (auto &it_per_id : f_manager.feature){//該特征點(diǎn)被觀測(cè)到的次數(shù)it_per_id.used_num = it_per_id.feature_per_frame.size();if (it_per_id.used_num < 4)continue;++feature_index;int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;//特征點(diǎn)的起始幀必須是首幀，因此后面用來構(gòu)建marg矩陣的都是和第一幀有共視關(guān)系的滑窗幀，因?yàn)?marginalization_flag == MARGIN_OLDif (imu_i != 0)continue;//得到該Feature在起始幀下的歸一化坐標(biāo)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)//不需要起始觀測(cè)幀{Vector3d pts_j = it_per_frame.point;ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(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);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);}if(STEREO && it_per_frame.is_stereo){Vector3d pts_j_right = it_per_frame.pointRight;if(imu_i != imu_j){ProjectionTwoFrameTwoCamFactor *f = new ProjectionTwoFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},vector<int>{0, 4});marginalization_info->addResidualBlockInfo(residual_block_info);}else{ProjectionOneFrameTwoCamFactor *f = new ProjectionOneFrameTwoCamFactor(pts_i, pts_j_right, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocityRight,it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,vector<double *>{para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]},vector<int>{2});marginalization_info->addResidualBlockInfo(residual_block_info);}}}}}

【4】marginalization_info->preMarginalize():得到每次 IMU 和視覺觀測(cè)(cost_function)對(duì) 應(yīng)的參數(shù)塊(parameter_blocks)，雅可比矩陣(jacobians)，殘差值(residuals);

?marginalization_info->preMarginalize();

【4】marginalization_info->marginalize():多線程計(jì)整個(gè)先驗(yàn)項(xiàng)的參數(shù)塊，雅可比矩陣和 殘差值，其中與代碼對(duì)應(yīng)關(guān)系為:

marginalization_info->marginalize();

【5】最后移交了優(yōu)化項(xiàng)需要得到的兩個(gè)變量：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;

重要的三個(gè)結(jié)構(gòu)變量：

• MarginalizationFactor *marginalization_factor
• *MarginalizationInfo marginalization_info
• ResidualBlockInfo *residual_block_info

MarginalizationFactor *marginalization_factor 兩個(gè)作用：（1）構(gòu)建ceres的殘差項(xiàng)，即計(jì)算residual （2）構(gòu)建ResidualBlockInfo *residual_block_info

MarginalizationInfo *marginalization_info 由他可以獲取邊緣化信息，用它來構(gòu)建MarginalizationFactor *marginalization_factor以及得到對(duì)應(yīng)的參數(shù)塊

ResidualBlockInfo *residual_block_info 用它來豐富marginalization_info項(xiàng)的信息

從頭到位都是marginalization_info這個(gè)變量來進(jìn)行統(tǒng)籌安排進(jìn)行邊緣化。

通過marginalization_info->addResidualBlockInfo()來添加約束，有三個(gè)方面的來源：（1）舊的（2）imu預(yù)積分項(xiàng)（3）特征點(diǎn)

理解：

上面三步通過調(diào)用 addResidualBlockInfo() 已經(jīng)確定優(yōu)化變量的數(shù)量、存儲(chǔ)位置、長(zhǎng)度以及待優(yōu)化變量的數(shù)量以及存儲(chǔ)位置；

為什么要添加這些量呀？因?yàn)樵趉時(shí)刻這些量是我們要邊緣化掉的變量，殘差；但是k+1時(shí)刻就是我們的先驗(yàn)，這不正是我們初衷嘛！

需要注意的是這些都是和第一幀圖像有關(guān)的！因?yàn)槲覀冞吘壔氖亲罾蠋?#xff01;三部分的流程都是一樣的，如下：

第一步 定義損失函數(shù)，對(duì)于先驗(yàn)殘差就是MarginalizationFactor，對(duì)于IMU就是IMUFactor，對(duì)于視覺就是ProjectionTdFactor，

這三個(gè)損失函數(shù)的類都是繼承自ceres的損失函數(shù)類ceres::CostFunction，里面都重載了函數(shù); 函數(shù)通過傳入的優(yōu)化變量值 parameters，

以及先驗(yàn)值（對(duì)于先驗(yàn)殘差就是上一時(shí)刻的先驗(yàn)殘差last_marginalization_info，對(duì)于IMU就是預(yù)計(jì)分值pre_integrations[1]，對(duì)于視覺

就是空間的的像素坐標(biāo)pts_i, pts_j）可以計(jì)算出各項(xiàng)殘差值residuals，以及殘差對(duì)應(yīng)個(gè)優(yōu)化變量的雅克比矩陣jacobians；?

bool MarginalizationFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {int n = marginalization_info->n;int m = marginalization_info->m;Eigen::VectorXd dx(n);//遍歷本次邊緣化保留下的參數(shù)塊：keep_block_sizefor (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++){//得到該參數(shù)塊的大小和序號(hào)int size = marginalization_info->keep_block_size[i];int idx = marginalization_info->keep_block_idx[i] - m;//x 感覺是邊緣化之前 BA優(yōu)化后的值 ？Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);//x0表示marg時(shí)參數(shù)塊變量的值(即xb)Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);if (size != 7)//speed_bias 9項(xiàng)dx.segment(idx, size) = x - x0;//變量的更新else//位置，姿態(tài)項(xiàng), 姿態(tài)不能直接加減了！要變成四元素的 ?{//使用四元數(shù)的表達(dá)方式dx.segment<3>(idx + 0) = x.head<3>() - x0.head<3>();dx.segment<3>(idx + 3) = 2.0 * Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();if (!((Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).w() >= 0)){dx.segment<3>(idx + 3) = 2.0 * -Utility::positify(Eigen::Quaterniond(x0(6), x0(3), x0(4), x0(5)).inverse() * Eigen::Quaterniond(x(6), x(3), x(4), x(5))).vec();}}}//計(jì)算殘差，理解，泰勒展開求的公式Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;//僅在邊緣化殘差這一塊，雅克比矩陣要固定？if (jacobians){for (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++){if (jacobians[i]){int size = marginalization_info->keep_block_size[i], local_size = marginalization_info->localSize(size);int idx = marginalization_info->keep_block_idx[i] - m;//Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> jacobian(jacobians[i], n, size);jacobian.setZero();jacobian.leftCols(local_size) = marginalization_info->linearized_jacobians.middleCols(idx, local_size);}}}return true; }

上面這段代碼是在優(yōu)化過程中，對(duì)先驗(yàn)約束項(xiàng)的更新，主要是對(duì)先驗(yàn)殘差的更新，雅克比不再發(fā)生變化。
代碼中的x0 表示marg xm狀態(tài)時(shí) xb變量的值; 優(yōu)化過程中先驗(yàn)項(xiàng)的雅克比保持不變，但是殘差需要變化，其變化公式推導(dǎo)如下：

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

ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set) ---------------------------------------------------------------------------------------------------- void ResidualBlockInfo::Evaluate() {//獲取殘差的個(gè)數(shù)： IMU + 視覺residuals.resize(cost_function->num_residuals());//優(yōu)化變量參數(shù)塊的變量大小：para_Pose、para_SpeedBias、para_Ex_Pose、para_Feature、para_Tdstd::vector<int> block_sizes = cost_function->parameter_block_sizes();//數(shù)組外圍的大小，也就是參數(shù)塊的個(gè)數(shù)raw_jacobians = new double *[block_sizes.size()];jacobians.resize(block_sizes.size());//分配每一行的大小，殘差的維數(shù)*每個(gè)參數(shù)塊中參數(shù)的個(gè)數(shù)block_sizes[i]，J矩陣大小的確認(rèn)！想一下//比如：兩個(gè)殘差f1,f2;5個(gè)變量x1,x2,,,x5, 則J矩陣是2行5列呀for (int i = 0; i < static_cast<int>(block_sizes.size()); i++){jacobians[i].resize(cost_function->num_residuals(), block_sizes[i]);raw_jacobians[i] = jacobians[i].data();}//利用各自殘差的Evaluate函數(shù)計(jì)算殘差和雅克比矩陣。cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);//好像明白了，這個(gè)是視覺里面有的Huber核函數(shù)；有魯棒核函數(shù)的殘差部分，重寫雅克比與殘差if (loss_function){double residual_scaling_, alpha_sq_norm_;double sq_norm, rho[3];sq_norm = residuals.squaredNorm();loss_function->Evaluate(sq_norm, rho);double sqrt_rho1_ = sqrt(rho[1]);if ((sq_norm == 0.0) || (rho[2] <= 0.0)){residual_scaling_ = sqrt_rho1_;alpha_sq_norm_ = 0.0;}else//這一部分公式?jīng)]有看懂！{const double D = 1.0 + 2.0 * sq_norm * rho[2] / rho[1];const double alpha = 1.0 - sqrt(D);residual_scaling_ = sqrt_rho1_ / (1 - alpha); alpha_sq_norm_ = alpha / sq_norm; }//感覺像根據(jù)先驗(yàn)殘差，推出新的殘差和雅可比公式一樣！for (int i = 0; i < static_cast<int>(parameter_blocks.size()); i++){jacobians[i] = sqrt_rho1_ * (jacobians[i] - alpha_sq_norm_ * residuals * (residuals.transpose() * jacobians[i]));}residuals *= residual_scaling_;} }

這一步是為了將不同的損失函數(shù)_cost_function以及優(yōu)化變量_parameter_blocks統(tǒng)一起來再一起添加到marginalization_info中。

變量_loss_function是核函數(shù)，在VINS-mono的邊緣化中僅僅視覺殘差有用到核函數(shù)，另外會(huì)設(shè)置需要被邊緣話的優(yōu)化變量的位置

_drop_set，這里對(duì)于不同損失函數(shù)又會(huì)有不同：對(duì)于先驗(yàn)損失，其待邊緣化優(yōu)化變量是根據(jù)是否等于para_Pose[0]或者

para_SpeedBias[0]，也就是說和第一幀相關(guān)的優(yōu)化變量都作為邊緣化的對(duì)象。對(duì)于IMU，其輸入的_drop_set是 vector{0, 1}，

也就是說其待邊緣化變量是para_Pose[0], para_SpeedBias[0]，也是第一幀相關(guān)的變量都作為邊緣化的對(duì)象，

這里值得注意的是和后端優(yōu)化不同，這里只添加了第一幀和第二幀的相關(guān)變量作為優(yōu)化變量，因此邊緣化構(gòu)造的信息矩陣會(huì)比

后端優(yōu)化構(gòu)造的信息矩陣要小對(duì)于視覺，其輸入的_drop_set是vector{0, 3}，也就是說其待邊緣化變量是para_Pose[imu_i]和

para_Feature[feature_index]，從這里可以看出來在VINS-mono的邊緣化操作中會(huì)不僅僅會(huì)邊緣化第一幀相關(guān)的優(yōu)化變量，

還會(huì)邊緣化掉以第一幀為起始觀察幀的路標(biāo)點(diǎn)。

問1：為什么輸入的_drop_set是 vector{0, 1}，也就是說其待邊緣化變量是para_Pose[0], para_SpeedBias[0]；

輸入的_drop_set是vector{0, 3}，也就是說其待邊緣化變量是para_Pose[imu_i]和para_Feature[feature_index]；

答1：如前面所示，優(yōu)化變量是按照固定的順序排列的

序號(hào)?????? ? ? ? ? ? 變量??????????? ? ?? ???? ? ? ? ?? ?

?? 0???????????? para_Pose?????????????????
?? 1???????????? para_SpeedBias????
?? 2?????? ????? para_Ex_Pose???????
?? 3 ? ? ? ????? para_Feature???????????
?? 4???????????? para_Td??????????????????????

問2：最后一部分為什么要重寫殘差和雅可比公式？公式怎么來的？

答2：看過論文的童鞋應(yīng)該知道，在優(yōu)化部分，視覺是加了核函數(shù)的，這樣帶來的問題是其對(duì)應(yīng)的殘差和雅可比是變的，這個(gè)應(yīng)該很好理解：

比如: f(x) 的雅可比是J， 那么 ρ(f(x))的雅可比肯定變了！ ρ 是核函數(shù)！ 公式我沒有推出來，還請(qǐng)知道的兄弟告訴一下！

經(jīng)過努力，得一絲頭緒！ 其中核函數(shù)如下：

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

marginalization_info->addResidualBlockInfo(residual_block_info);

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

目的：

?將不同損失函數(shù)對(duì)應(yīng)的優(yōu)化變量、邊緣化位置存入到 parameter_block_sizes 和 parameter_block_idx中，這里注意的是執(zhí)行到這一步，

parameter_block_idx 中僅僅有待邊緣化的優(yōu)化變量的內(nèi)存地址的key，而且其對(duì)應(yīng)value全部為0；

【4】調(diào)用 preMarginalize() 進(jìn)行預(yù)處理

上面通過調(diào)用 addResidualBlockInfo() 已經(jīng)確定優(yōu)化變量的數(shù)量、存儲(chǔ)位置、長(zhǎng)度以及待優(yōu)化變量的數(shù)量以及存儲(chǔ)位置，

下面就需要調(diào)用 preMarginalize() 進(jìn)行預(yù)處理， 得到每次 IMU 和視覺觀測(cè) (cost_function) 對(duì) 應(yīng)的參數(shù)塊(parameter_blocks)，

雅可比矩陣(jacobians)，殘差值(residuals);

void MarginalizationInfo::preMarginalize() {//遍歷所有factor，在前面的addResidualBlockInfo中會(huì)將不同的殘差塊加入到factor中。for (auto it : factors){it->Evaluate();//利用多態(tài)性分別計(jì)算所有狀態(tài)變量構(gòu)成的殘差和雅克比矩陣//遍歷所有參數(shù)塊std::vector<int> block_sizes = it->cost_function->parameter_block_sizes();for (int i = 0; i < static_cast<int>(block_sizes.size()); i++){//優(yōu)化變量的地址long addr = reinterpret_cast<long>(it->parameter_blocks[i]);int size = block_sizes[i];//將factor中的參數(shù)塊復(fù)制到parameter_block_data中，parameter_block_data是整個(gè)優(yōu)化變量的數(shù)據(jù)if (parameter_block_data.find(addr) == parameter_block_data.end()){double *data = new double[size]; //會(huì)在析構(gòu)函數(shù)中刪除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()? 這一句里面其實(shí)就是調(diào)用各個(gè)損失函數(shù)中的重載函數(shù)Evaluate()，這個(gè)函數(shù)前面有提到過：

【5】調(diào)用marginalize()

開啟多線程構(gòu)建信息矩陣H和b ，同時(shí)從H,b中恢復(fù)出線性化雅克比和殘差

前面已經(jīng)將數(shù)據(jù)都準(zhǔn)備好了，下面通過調(diào)用marginalize()函數(shù)就要正式開始進(jìn)行邊緣化操作了，實(shí)現(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掉的變量個(gè)數(shù)for (const auto &it : parameter_block_size)//計(jì)算除了邊緣化之外要保留的參數(shù)塊{//如果這個(gè)變量不是是待marg的優(yōu)化變量。if (parameter_block_idx.find(it.first) == parameter_block_idx.end()){parameter_block_idx[it.first] = pos;//就將這個(gè)待marg的變量id設(shè)為pospos += localSize(it.second); //pos加上這個(gè)變量的長(zhǎng)度}}//上面的操作就會(huì)將所有的優(yōu)化變量進(jìn)行一個(gè)偽排序 n = pos - m;//要保留下來的變量個(gè)數(shù)if(m == 0){valid = false;printf("unstable tracking...\n");return;}TicToc t_summing;Eigen::MatrixXd A(pos, pos);//整個(gè)矩陣大小 --- 沒有邊緣化之前的矩陣Eigen::VectorXd b(pos);A.setZero();b.setZero();TicToc t_thread_summing;pthread_t tids[NUM_THREADS];ThreadsStruct threadsstruct[NUM_THREADS];//攜帶每個(gè)線程的輸入輸出信息//為每個(gè)線程均勻分配factor。int i = 0;for (auto it : factors){threadsstruct[i].sub_factors.push_back(it);i++;i = i % NUM_THREADS;}//構(gòu)造4個(gè)線程，并確定線程的主程序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;//分別構(gòu)造矩陣int ret = pthread_create( &tids[i], NULL, ThreadsConstructA ,(void*)&(threadsstruct[i]));if (ret != 0){ROS_WARN("pthread_create error");ROS_BREAK();}}//將每個(gè)線程構(gòu)建的A和b加起來for( int i = NUM_THREADS - 1; i >= 0; i--) {pthread_join( tids[i], NULL ); //阻塞等待線程完成A += threadsstruct[i].A;b += threadsstruct[i].b;}/*代碼中求Amm的逆矩陣時(shí)，為了保證數(shù)值穩(wěn)定性，做了Amm=1/2*(Amm+Amm^T)的運(yùn)算，Amm本身是一個(gè)對(duì)稱矩陣，所以 等式成立。接著對(duì)Amm進(jìn)行了特征值分解,再求逆，更加的快速*/Eigen::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();// 設(shè)x_{m}為要被marg掉的狀態(tài)量，x_{r}是與x_{m}相關(guān)的狀態(tài)量，所以在最后我們要保存的是x_{r}的信息//// | | | | |// | Amm | Amr| m |bmm| |x_{m}|// A = |______|____| b = |__ | A|x_{r}| = b// | Arm | Arr| n |brr|// | | | | |// 使用舒爾補(bǔ):// C = Arr - Arm*Amm^{-1}Amr// d = brr - Arm*Amm^{-1}bmm//舒爾補(bǔ)，上面這段代碼邊緣化掉xm變量，保留xb變量 Eigen::VectorXd bmm = b.segment(0, m);Eigen::MatrixXd Amr = A.block(0, m, m, n);//0，m是開始的位置，m,m是開始位置后的大小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,大小是發(fā)生了變化的//下面就是更新先驗(yàn)殘差項(xiàng)Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> saes2(A);//求更新后 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();//分別指的是邊緣化之后從信息矩陣A和b中恢復(fù)出來雅克比矩陣和殘差向量；//兩者會(huì)作為先驗(yàn)殘差帶入到下一輪的先驗(yàn)殘差的雅克比和殘差的計(jì)算當(dāng)中去linearized_jacobians = S_sqrt.asDiagonal() * saes2.eigenvectors().transpose();linearized_residuals = S_inv_sqrt.asDiagonal() * saes2.eigenvectors().transpose() * b; }

部分程序：

int pos = 0; //pos表示所有的被marg掉的參數(shù)塊以及它們的相連接參數(shù)塊的localsize之和for (auto &it : parameter_block_idx){it.second = pos;pos += localSize(parameter_block_size[it.first]);}m = pos; for (const auto &it : parameter_block_size){if (parameter_block_idx.find(it.first) == parameter_block_idx.end()){//將被marg掉參數(shù)的相連接參數(shù)塊添加道parameter_block_idx中parameter_block_idx[it.first] = pos;pos += localSize(it.second);}}n = pos - m;

這里對(duì) 參數(shù)塊變量進(jìn)行排序，待marg的參數(shù)塊變量放在前面，其他參數(shù)塊變量放在后面，

并將每個(gè)參數(shù)塊的對(duì)應(yīng)的下標(biāo)放在parameter_block_idx中； pos = m + n

TicToc t_thread_summing;pthread_t tids[NUM_THREADS];//攜帶每個(gè)線程的輸入輸出信息ThreadsStruct threadsstruct[NUM_THREADS];int i = 0;//將先驗(yàn)約束因子平均分配到線程中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();}}//將每個(gè)線程構(gòu)建的A和b加起來for( int i = NUM_THREADS - 1; i >= 0; i--) {pthread_join( tids[i], NULL ); //阻塞等待線程完成A += threadsstruct[i].A;b += threadsstruct[i].b;}

這段代碼開啟多線程來構(gòu)建信息矩陣A和殘差b；將所有的先驗(yàn)約束因子平均分配到NUM_THREADS個(gè)線程中，每個(gè)線程分別構(gòu)建一個(gè)A和b；

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

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

子函數(shù)?? void* ThreadsConstructA(void* threadsstruct)?????????????

void* ThreadsConstructA(void* threadsstruct) {ThreadsStruct* p = ((ThreadsStruct*)threadsstruct);//遍歷該線程分配的所有factors，所有觀測(cè)項(xiàng)for (auto it : p->sub_factors){//遍歷該factor中的所有參數(shù)塊，五個(gè)參數(shù)塊，分別計(jì)算，理解！for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++){//得到參數(shù)塊的大小int idx_i = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[i])];int size_i = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[i])];if (size_i == 7)//對(duì)于pose來說，是7維的,最后一維為0，這里取左邊6size_i = 6;//只提取local size部分，對(duì)于pose來說，是7維的,最后一維為0，這里取左邊6維//P.leftCols(cols) = P(:, 1:cols)，取出從1列開始的cols列Eigen::MatrixXd jacobian_i = it->jacobians[i].leftCols(size_i);for (int j = i; j < static_cast<int>(it->parameter_blocks.size()); j++){int idx_j = p->parameter_block_idx[reinterpret_cast<long>(it->parameter_blocks[j])];int size_j = p->parameter_block_size[reinterpret_cast<long>(it->parameter_blocks[j])];if (size_j == 7)size_j = 6;Eigen::MatrixXd jacobian_j = it->jacobians[j].leftCols(size_j);//對(duì)應(yīng)對(duì)角區(qū)域，H*X=b, A代表H矩陣if (i == j)p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;else{//對(duì)應(yīng)非對(duì)角區(qū)域p->A.block(idx_i, idx_j, size_i, size_j) += jacobian_i.transpose() * jacobian_j;p->A.block(idx_j, idx_i, size_j, size_i) = p->A.block(idx_i, idx_j, size_i, size_j).transpose();}}//求取g，Hx=g，都是根據(jù)公式來寫程序的！p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;}}return threadsstruct; }

這部分代碼是用來實(shí)現(xiàn)構(gòu)建A和b的：

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

上面這段代碼邊緣化掉xm變量，保留xb變量，利用的方法是舒爾補(bǔ)

注意：上面這個(gè)等式就是先驗(yàn)信息，程序中又變成了 AX = b 的形式，從A和b中恢復(fù)出雅克比矩陣和殘差，作為下一時(shí)刻的先驗(yàn)信息；

代碼中求Amm的逆矩陣時(shí)，為了保證數(shù)值穩(wěn)定性，做了Amm=1/2*(Amm+Amm^T)的運(yùn)算，Amm本身是一個(gè)對(duì)稱矩陣，

所以等式成立。接著對(duì)Amm進(jìn)行了特征值分解,再求逆，更加的快速。

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;

上面這段代碼是從A和b中恢復(fù)出雅克比矩陣和殘差

【6】滑窗預(yù)移動(dòng)

//這里僅僅將指針進(jìn)行了一次移動(dòng)，指針對(duì)應(yīng)的數(shù)據(jù)還是舊數(shù)據(jù)，調(diào)用的 slideWindow() 才能實(shí)現(xiàn)真正的滑窗移動(dòng)std::unordered_map<long, double *> addr_shift;for (int i = 1; i <= WINDOW_SIZE; i++)//從1開始，因?yàn)榈谝粠臓顟B(tài)不要了{(lán)//這一步的操作指的是第i的位置存放的的是i-1的內(nèi)容，這就意味著窗口向前移動(dòng)了一格addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];if(USE_IMU)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];addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];//根據(jù)地址來得到保留的參數(shù)塊vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);if (last_marginalization_info)//Zbdelete 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)化變量的遞歸，這里面僅僅是指針}

這一部分應(yīng)該很容易明白；當(dāng)邊緣化幀的 “新幀”就不說了，把上面的看懂了，估計(jì)那個(gè)也會(huì)了

/------------------------------------------------------------------程序-----------------------------------------------------------------------/

總結(jié)

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

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