當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

VINS-Mono 代码解析六、边缘化（2）理论和代码详解

發布時間：2023/12/20 编程问答 75 豆豆

生活随笔收集整理的這篇文章主要介紹了 VINS-Mono 代码解析六、边缘化（2）理论和代码详解小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

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 怎樣進行邊緣化呢？

說明：?的證明在最后；

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

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

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

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()函數將各個殘差以及殘差涉及的優化變量添加入上面所述的優化變量中：

首先添加上一次先驗殘差項：（上一次待邊緣化的參數與其他參數的約束關系）

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掉的優化變量，也就是滑窗內第一個變量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;

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就是保留下來的滑窗內的所有的優化變量

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

備注：?的證明；

toy example 2

總結

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

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

上一篇： linux DSA 开发（一）
下一篇： R语言两个矩阵（两组）数据的相关性分析