“ 在優化完成后，滑動窗口前，必須進行邊緣化項的操作”

一、理論部分

VINS 細節系列 - 窗口優化_努力努力努力-CSDN博客

?一個邊緣化的例子

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

以及? 6 個路標點xmk (路標點用 xyz 的參數化), 相機與路標點的邊表示一次觀測, 相鄰相機之間的邊表示 IMU 約束,

相互關系如下:

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

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

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

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

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

這時圖關系則變為:

注意,觀察圖 16 可發現這時的 xm1 、xm2 和 xm3 彼此之間已經產生了新的約束關系,且xp2 也與xm1 產生了新的約束關系。

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

對應的圖關系如下:

我們發現, marg 掉xm1 后,并沒有是 H 矩陣更稠密,這是因為xm1 之前并未與其他pose 有約束關系,即并未被觀察到,

因此如果 marg 掉那些不被其他幀觀測到的路標點,不會顯著地使 H 矩陣變得稠密。而要 marg 掉的路標點中,對于那

些被其他幀觀測到路標點,要么就別設置為 marg, 要么就寧愿丟棄,這就是 OKVIS 中用到的策略

簡單介紹

?1、優化變量：

序號?????? ? ? ? 變量??????????? ? ?? ???? ? ? ? ?? ?? 維度

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

last_marginalization_parameter_blocks ：Xb變量，也是我們要優化的變量；
last_marginalization_info??????????? ? ? ? ? ? ? ? ? ? ? ：Xm與Xb對應的測量Zb，也是先驗殘差（細品一下）

2、思路：
?

邊緣化策略

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

a. 如果次新幀是關鍵幀，則將邊緣化最老幀，及其看到的路標點和IMU數據，轉化為先驗。具體流程為：

1）將上一次先驗殘差項傳遞給marginalization_info

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

3）將第一次觀測為第0幀的所有路標點對應的視覺觀測，添加到marginalization_info中

4）計算每個殘差，對應的Jacobian，并將各參數塊拷貝到統一的內存（parameter_block_data）中

5）多線程構造先驗項舒爾補AX=b的結構，在X0處線性化計算Jacobian和殘差

6）調整參數塊在下一次窗口中對應的位置（往前移一格），注意這里是指針，后面slideWindow中會賦新值，這里只是提前占座

b. 如果次新幀不是關鍵幀，此時具體流程為：

1）保留次新幀的IMU測量，丟棄該幀的視覺測量，將上一次先驗殘差項傳遞給marginalization_info

2）premargin：計算每個殘差，對應的Jacobian，并更新parameter_block_data

3）marginalize：構造先驗項舒爾補AX=b的結構，計算Jacobian和殘差

4）調整參數塊在下一次窗口中對應的位置（去掉次新幀）

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

舉例說明,當第一次 marg 掉最老幀時,parameter_block_size 為所有變量及其對應的
localSize,? parameter_block_data 為對應的數據(double*類型)

?

二、程序部分

2.1? 把下面這個圖搞明白！

?

2.2 看 ?"marginalization_factor.h"? 中類和結構體的定義

?1、class ResidualBlockInfo

類描述：這個類保存了待marg變量（xm）與相關聯變量（xb）之間的一個約束因子關系? -? 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();//調用cost_function的evaluate函數計算殘差 和 雅克比矩陣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	對應ceres中表示約束因子的類
parameter_blocks	該約束因子相關聯的參數塊變量
drop_set	parameter_blocks中待marg變量的索引

2、class MarginalizationInfo

類描述：這個類保存了優化時上一步邊緣化后保留下來的先驗信息

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;//這里將參數塊分為Xm,Xb,Xr,Xm表示被marg掉的參數塊，Xb表示與Xm相連接的參數塊，Xr表示剩余的參數塊//那么m=Xm的localsize之和，n為Xb的localsize之和，pos為（Xm+Xb）localsize之和int m, n; std::unordered_map<long, int> parameter_block_size; //global size 將被marg掉的約束邊相關聯的參數塊，即將被marg掉的參數塊以及與它們直接相連的參數快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掉變量有約束關系的變量的localsize之和，如上圖中xb的localsize
parameter_block_size	<參數塊地址，參數塊的global size>，參數塊包括xm和xb
parameter_block_idx	<參數塊地址，參數塊排序好后的索引>，對參數塊進行排序，xm排在前面，xb排成后面，使用localsize
parameter_block_data	<參數塊地址，參數塊數據>，需要注意的是這里保存的參數塊數據是原始參數塊數據的一個拷貝，不再變化，用于記錄這些參數塊變量在marg時的狀態
keep_block_size	<保留下來的參數塊地址，參數塊的globalsize>
keep_block_idx	<保留下來的參數塊地址，參數塊的索引>，保留下來的參數塊是xb

成員函數信息：

成員函數描述

preMarginalize	得到每次IMU和視覺觀測對應的參數塊，雅克比矩陣，殘差值
marginalize	開啟多線程構建信息矩陣H和b ，同時從H,b中恢復出線性化雅克比和殘差

3、class MarginalizationFactor

類描述：該類是優化時表示上一步邊緣化后保留下來的先驗信息代價因子，變量marginalization_info保存了類似約束測量信息

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 函數里面，當執行完后端優化后，緊接著就會執行邊緣化，兩個過程都需要添加優化變量和殘差，

但區別是邊緣化的變量少一點！主要是構建模塊的過程！

?

一、如果邊緣化 “老幀”

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

即從舊的先驗殘差項 last_marginalization_info 新建一個新的 marginalization_factor，參與這個殘差項的優化變量是：last_marginalization_parameter_blocks 里面的內容：para_Pose、para_SpeedBias、para_Ex_Pose、para_Feature、para_Td 都是按順序排好的，所以要把 要丟棄 的狀態量? para_Pose[0]、para_SpeedBias[0] 對應的序號用 drop_set 記錄下來

// --------------------------------------marginalization-----------------------------------------//添加殘差以及優化變量的方式和后端線性優化中添加的方式相似，因為邊緣化類應該就是仿照ceres寫的TicToc t_whole_marginalization;//一、邊緣化老幀，那就要考慮老幀的情況！ //要邊緣化的參數塊是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗內的第feature_index個點的逆深度)if (marginalization_flag == MARGIN_OLD){MarginalizationInfo *marginalization_info = new MarginalizationInfo();//先驗信息vector2double(); //【1】首先添加上一次先驗殘差項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中是首幀狀態量的序號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); //調用addResidualBlockInfo()函數將各個殘差以及殘差涉及的優化變量添加入優化變量中marginalization_info->addResidualBlockInfo(residual_block_info);}

【2】添加IMU的marg信息到 marginalization_info 中：第0幀和第1幀之間的IMU預積分值以及第0幀和第1幀相關優化變量

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】最后添加第一次觀測滑窗中第0幀的路標點以及其他相關的滑窗中的幀的相關的優化變量 到 marginalization_info 中

{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 < 4)continue;++feature_index;int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;//特征點的起始幀必須是首幀，因此后面用來構建marg矩陣的都是和第一幀有共視關系的滑窗幀，因為 marginalization_flag == MARGIN_OLDif (imu_i != 0)continue;//得到該Feature在起始幀下的歸一化坐標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)//不需要起始觀測幀{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 和視覺觀測(cost_function)對 應的參數塊(parameter_blocks)，雅可比矩陣(jacobians)，殘差值(residuals);

?marginalization_info->preMarginalize();

【4】marginalization_info->marginalize():多線程計整個先驗項的參數塊，雅可比矩陣和 殘差值，其中與代碼對應關系為:

marginalization_info->marginalize();

【5】最后移交了優化項需要得到的兩個變量：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）特征點

理解：

上面三步通過調用 addResidualBlockInfo() 已經確定優化變量的數量、存儲位置、長度以及待優化變量的數量以及存儲位置；

為什么要添加這些量呀？因為在k時刻這些量是我們要邊緣化掉的變量，殘差；但是k+1時刻就是我們的先驗，這不正是我們初衷嘛！

需要注意的是這些都是和第一幀圖像有關的！因為我們邊緣化的是最老幀！三部分的流程都是一樣的，如下：

第一步 定義損失函數，對于先驗殘差就是MarginalizationFactor，對于IMU就是IMUFactor，對于視覺就是ProjectionTdFactor，

這三個損失函數的類都是繼承自ceres的損失函數類ceres::CostFunction，里面都重載了函數; 函數通過傳入的優化變量值 parameters，

以及先驗值（對于先驗殘差就是上一時刻的先驗殘差last_marginalization_info，對于IMU就是預計分值pre_integrations[1]，對于視覺

就是空間的的像素坐標pts_i, pts_j）可以計算出各項殘差值residuals，以及殘差對應個優化變量的雅克比矩陣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);//遍歷本次邊緣化保留下的參數塊：keep_block_sizefor (int i = 0; i < static_cast<int>(marginalization_info->keep_block_size.size()); i++){//得到該參數塊的大小和序號int size = marginalization_info->keep_block_size[i];int idx = marginalization_info->keep_block_idx[i] - m;//x 感覺是邊緣化之前 BA優化后的值 ？Eigen::VectorXd x = Eigen::Map<const Eigen::VectorXd>(parameters[i], size);//x0表示marg時參數塊變量的值(即xb)Eigen::VectorXd x0 = Eigen::Map<const Eigen::VectorXd>(marginalization_info->keep_block_data[i], size);if (size != 7)//speed_bias 9項dx.segment(idx, size) = x - x0;//變量的更新else//位置，姿態項, 姿態不能直接加減了！要變成四元素的 ?{//使用四元數的表達方式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();}}}//計算殘差，理解，泰勒展開求的公式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; }

上面這段代碼是在優化過程中，對先驗約束項的更新，主要是對先驗殘差的更新，雅克比不再發生變化。
代碼中的x0 表示marg xm狀態時 xb變量的值; 優化過程中先驗項的雅克比保持不變，但是殘差需要變化，其變化公式推導如下：

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

ResidualBlockInfo(ceres::CostFunction *_cost_function, ceres::LossFunction *_loss_function, std::vector<double *> _parameter_blocks, std::vector<int> _drop_set) ---------------------------------------------------------------------------------------------------- void ResidualBlockInfo::Evaluate() {//獲取殘差的個數： IMU + 視覺residuals.resize(cost_function->num_residuals());//優化變量參數塊的變量大小：para_Pose、para_SpeedBias、para_Ex_Pose、para_Feature、para_Tdstd::vector<int> block_sizes = cost_function->parameter_block_sizes();//數組外圍的大小，也就是參數塊的個數raw_jacobians = new double *[block_sizes.size()];jacobians.resize(block_sizes.size());//分配每一行的大小，殘差的維數*每個參數塊中參數的個數block_sizes[i]，J矩陣大小的確認！想一下//比如：兩個殘差f1,f2;5個變量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函數計算殘差和雅克比矩陣。cost_function->Evaluate(parameter_blocks.data(), residuals.data(), raw_jacobians);//好像明白了，這個是視覺里面有的Huber核函數；有魯棒核函數的殘差部分，重寫雅克比與殘差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//這一部分公式沒有看懂！{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; }//感覺像根據先驗殘差，推出新的殘差和雅可比公式一樣！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_;} }

這一步是為了將不同的損失函數_cost_function以及優化變量_parameter_blocks統一起來再一起添加到marginalization_info中。

變量_loss_function是核函數，在VINS-mono的邊緣化中僅僅視覺殘差有用到核函數，另外會設置需要被邊緣話的優化變量的位置

_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的邊緣化操作中會不僅僅會邊緣化第一幀相關的優化變量，

還會邊緣化掉以第一幀為起始觀察幀的路標點。

問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：如前面所示，優化變量是按照固定的順序排列的

序號?????? ? ? ? ? ? 變量??????????? ? ?? ???? ? ? ? ?? ?

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

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

答2：看過論文的童鞋應該知道，在優化部分，視覺是加了核函數的，這樣帶來的問題是其對應的殘差和雅可比是變的，這個應該很好理解：

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

經過努力，得一絲頭緒！ 其中核函數如下：

第三步是將定義的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;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++){//這個是待邊緣化的變量的iddouble *addr = parameter_blocks[residual_block_info->drop_set[i]];//將需要marg的變量的id存入parameter_block_idxparameter_block_idx[reinterpret_cast<long>(addr)] = 0;} }

目的：

?將不同損失函數對應的優化變量、邊緣化位置存入到 parameter_block_sizes 和 parameter_block_idx中，這里注意的是執行到這一步，

parameter_block_idx 中僅僅有待邊緣化的優化變量的內存地址的key，而且其對應value全部為0；

【4】調用 preMarginalize() 進行預處理

上面通過調用 addResidualBlockInfo() 已經確定優化變量的數量、存儲位置、長度以及待優化變量的數量以及存儲位置，

下面就需要調用 preMarginalize() 進行預處理， 得到每次 IMU 和視覺觀測 (cost_function) 對 應的參數塊(parameter_blocks)，

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

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

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

【5】調用marginalize()

開啟多線程構建信息矩陣H和b ，同時從H,b中恢復出線性化雅克比和殘差

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

部分程序：

int pos = 0; //pos表示所有的被marg掉的參數塊以及它們的相連接參數塊的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掉參數的相連接參數塊添加道parameter_block_idx中parameter_block_idx[it.first] = pos;pos += localSize(it.second);}}n = pos - m;

這里對 參數塊變量進行排序，待marg的參數塊變量放在前面，其他參數塊變量放在后面，

并將每個參數塊的對應的下標放在parameter_block_idx中； pos = m + n

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();}}//將每個線程構建的A和b加起來for( int i = NUM_THREADS - 1; i >= 0; i--) {pthread_join( tids[i], NULL ); //阻塞等待線程完成A += threadsstruct[i].A;b += threadsstruct[i].b;}

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

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

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

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

void* ThreadsConstructA(void* threadsstruct) {ThreadsStruct* p = ((ThreadsStruct*)threadsstruct);//遍歷該線程分配的所有factors，所有觀測項for (auto it : p->sub_factors){//遍歷該factor中的所有參數塊，五個參數塊，分別計算，理解！for (int i = 0; i < static_cast<int>(it->parameter_blocks.size()); i++){//得到參數塊的大小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)//對于pose來說，是7維的,最后一維為0，這里取左邊6size_i = 6;//只提取local size部分，對于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);//對應對角區域，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{//對應非對角區域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，都是根據公式來寫程序的！p->b.segment(idx_i, size_i) += jacobian_i.transpose() * it->residuals;}}return threadsstruct; }

這部分代碼是用來實現構建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變量，利用的方法是舒爾補

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

代碼中求Amm的逆矩陣時，為了保證數值穩定性，做了Amm=1/2*(Amm+Amm^T)的運算，Amm本身是一個對稱矩陣，

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

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中恢復出雅克比矩陣和殘差

【6】滑窗預移動

//這里僅僅將指針進行了一次移動，指針對應的數據還是舊數據，調用的 slideWindow() 才能實現真正的滑窗移動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];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];//根據地址來得到保留的參數塊vector<double *> parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);if (last_marginalization_info)//Zbdelete last_marginalization_info; //刪除掉上一次的marg相關的內容last_marginalization_info = marginalization_info; //marg相關內容的遞歸last_marginalization_parameter_blocks = parameter_blocks; //優化變量的遞歸，這里面僅僅是指針}

這一部分應該很容易明白；當邊緣化幀的 “新幀”就不說了，把上面的看懂了，估計那個也會了

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

總結

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

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