代码框架

/feature_tracker 视觉前端

n.subscribe(IMAGE_TOPIC, 100, img_callback) 订阅图像话题，执行回调函数img_callback

img_callback()

trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)), img_msg->header.stamp.toSec()); //读取图像进行处理

//! void FeatureTracker::readImage(const cv::Mat &_img, double _cur_time)
- Clare->apply(_img, img); //直方图均衡化
- calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, Size(21,21), 3); //计算cur_img到forw_img的光流
- reduceVector() //根据status，剔除跟踪失败的点
- rejectWithF(); //计算F矩阵，使用RANSAC剔除外点
- setMask(); //保证相邻特征点间隔30个像素，设定mask
- goodFeaturesToTrack (forw_img, n_pts, MAX_CNT-forw_pts.size(), 0.01, MIN_DIST, mask); //提取新特征
- addPoints(); //添加新特征
- undistortedPoints(); //对角点图像坐标去畸变，转到归一化平面系上，并计算角点的速度

发布topic

n.advertise\<sensor_msgs::PointCloud>(“feature”,1000); //发布feature，跟踪的特征点，给后端优化用
n.advertise\<sensor_msgs::Bool>(“restart”,1000);
n.advertise\<sensor_msgs::Image>(“feature_img”,1000); //发布feature_img，给RVIZ可视化

/vins_estimator 里程计

1. 订阅IMU，执行回调n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelat());

   • IMU回调，将imu_msgs保存到imu_buf，predict() IMU状态递推并发布［P，Q，V，header］

2. 订阅特征点n.subscribe(“/feature_tracker/feature”, 2000, feature_callback);

   • 将feature_msg放入feature_buf

3. 创建VIO主线程std::thread measurement_process{process};

process() VIO线程

measurements = getMeasurements() //等待并获取观测
estimator.processIMU() //IMU预积分
estimator.processImage() //处理图像特征数据，包括VIO初始化和紧耦合滑窗优化
发布topic到RVIZ
if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)update();  //更新IMU状态 [p, q, v, ba, bg, a, g]

processIMU()

包括两部分：1、计算帧i到j的IMU预积分及误差传递方程；2、计算当前帧j的IMU状态初值。
PS：IMU预积分和原积分的差别在于：预积分是求bi到bj的相对位姿，原积分是根据bi的位姿积分得到bj的位姿。

midPointIntegration() 预积分

1. 计算两个关键帧i、j之间IMU测量的相对变化量，即${\alpha }_{b_i{b}_j},{\beta }_{b_i{b}_j},{\theta }_{b_i{b}_j}$，这里使用中值积分法，即相邻时刻k到k+1（i<k<k+1<j）的积分量用两个时刻的IMU测量a、w的均值计算：$$\begin{array}{rlr}w& =\frac{1}{2}({\hat{w}}^{b_k}+{\hat{w}}^{b_{k+1}})-b_k^g，& 角速度均值\\ q_{b_i{b}_{k+1}}& =q_{b_i{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}w\delta t\end{array}\right]，& i到k+1的相对旋转\\ a& =\frac{1}{2}(q_{b_i{b}_k}({\hat{a}}^{b_k}-b_k^a)+q_{b_i{b}_{k+1}}({\hat{a}}^{b_{k+1}}-b_k^a))，& 加速度均值\\ {\alpha }_{b_i{b}_{k+1}}& ={\alpha }_{b_i{b}_k}+{\beta }_{b_i{b}_k}\delta t+\frac{1}{2}a\delta t^2，& i到k+1的相对平移\\ {\beta }_{b_i{b}_{k+1}}& ={\beta }_{b_i{b}_k}+a\delta t，& i到k+1的相对速度\\ b_{k+1}^a& =b_k^a，& k+1时刻的偏置\\ b_{k+1}^g& =b_k^g& \end{array}$$
2. 计算预积分的误差传递方程：
   $$\left[\begin{array}{c}\delta {\alpha }_{b_{k+1}}\\ \delta {\theta }_{b_{k+1}}\\ \delta {\beta }_{b_{k+1}}\\ \delta b_{k+1}^a\\ \delta b_{k+1}^g\end{array}\right]={\mathbf{F}}_{15\times 15}\left[\begin{array}{c}\delta {\alpha }_{b_k}\\ \delta {\theta }_{b_k}\\ \delta {\beta }_{b_k}\\ \delta b_k^a\\ \delta b_k^g\end{array}\right]+{\mathbf{G}}_{15\times 18}\left[\begin{array}{c}{n}_k^a\\ n_k^g\\ n_{k+1}^a\\ n_{k+1}^g\\ n_{b_k^a}\\ n_{b_k^g}\end{array}\right]$$
   F、G为两个时刻间的协方差传递矩阵。根据计算得到的F、G，更新IMU残差的雅可比:
   $${\mathbf{J}}_{k+1}=\mathbf{F}{\mathbf{J}}_k,{\Sigma }_{k+1}=\mathbf{F}{\Sigma }_k{\mathbf{F}}^T+\mathbf{G}{\Sigma }_n{\mathbf{G}}^T$$

原积分计算PQV初值

从上一帧i开始，中值积分$k\in [i,j)$得到当前帧j的PQV作为优化初值：
$$\begin{array}{rl}w& =\frac{1}{2}({\hat{w}}^{b_k}+{\hat{w}}^{b_{k+1}})-b_k^g\\ q_{w{b}_{k+1}}& =q_{w{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}w\delta t\end{array}\right]\\ a& =\frac{1}{2}(q_{w{b}_k}({\hat{a}}^{b_k}-b_k^a)+q_{w{b}_{k+1}}({\hat{a}}^{b_{k+1}}-b_k^a))-g^w\\ p_{w{b}_{k+1}}& =p_{w{b}_k}+v_{w{b}_k}\delta t+\frac{1}{2}a\delta t^2\\ v_{w{b}_{k+1}}& =v_{w{b}_k}+a\delta t\\ b_{k+1}^a& =b_k^a\\ b_{k+1}^g& =b_k^g\end{array}$$
其中，$p_{w{b}_{k+1}},v_{w{b}_{k+1}},q_{w{b}_{k+1}}$ 即为代码中不断累加的Ps[j], Vs[j], Rs[j]。

processImage()

该函数处理视觉前端发来的图像特征数据，包括初始化及紧耦合滑窗优化。

// 1 根据两帧间的视差决定关键帧
if (f_manager.addFeatureCheckParallax())  // 若跟踪点个数小于20 || 平均视差大于10
marginalization_flag = MARGIN_OLD;
else
marginalization_flag = MARGIN_SECOND_NEW;// 2 如果需要标定外参，则利用旋转约束标定外参旋转
initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric)// 3 判断是否初始化
if (solver_flag == INITIAL) bool result = initialStructure(); // 初始化// 初始化过程包括: // 1.SFM初始化 通过加速度标准差保证IMU有充分运动激励，可以初始化// 2.relativePose() 返回滑动窗口中第一个满足视差的帧，为l帧，以及RT,可以三角化。// 3.sfm.construct() 求解滑动窗内所有图像帧相对于第l帧（参考帧）的位姿和三角化特征点3D坐标。输出所有图像帧相对于参考帧l的姿态四元数Q、平移向量T和特征点坐标，放在sfm_f和sfm_tracked_points内// 4.visualInitialAlign() 视觉惯性初始化if(result)solver_flag = NON_LINEAR; // 初始化成功，先进行一次滑窗优化，得到当前帧和第一帧的位姿solveOdometry(); // 求解里程计，包括三角化特征点和非线性优化slideWindow();   // 滑窗边缘化
elsesolveOdometry(); if (failureDetection()) // 故障检测和重置slideWindow();

visualInitialAlign() 视觉惯性初始化

1、VisualIMUAlignment（）函数计算陀螺仪偏置bg，尺度s，重力加速度g和速度v
2、f_manager.triangulate（）计算特征点深度estimated_depth
3、repropagate（）陀螺仪的偏置bgs改变，重新计算预积分
4、将Ps、Vs、depth进行更新
5、将重力旋转到Z轴，将Ps、Vs、Rs从相机参考坐标系c0旋转到世界坐标系w。

1 VisualIMUAlignment() 视觉惯性对齐

bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d* Bgs, Vector3d &g, VectorXd &x)
{// 利用旋转约束估计陀螺仪偏置 BgssolveGyroscopeBias(all_image_frame, Bgs);// 初始化速度、重力、尺度，及基于重力参数化的重力精化RefineGravity()// 通过构建最小二乘求解if(LinearAlignment(all_image_frame, g, x))return true;else return false;
}

2 得到所有图像帧的位姿Ps、Rs，并将其置为关键帧

for (int i = 0; i <= frame_count; i++)
{Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;Ps[i] = Pi;Rs[i] = Ri;all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
}

3 重新计算特征点的深度depth

VectorXd dep = f_manager.getDepthVector();
for (int i = 0; i < dep.size(); i++)dep[i] = -1;
f_manager.clearDepth(dep);
Vector3d TIC_TMP[NUM_OF_CAM];
for(int i = 0; i < NUM_OF_CAM; i++)TIC_TMP[i].setZero();ric[0] = RIC[0];
f_manager.setRic(ric);
f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));

4 repropagate() 陀螺仪的偏置bgs改变，重新计算预积分

double s = (x.tail<1>())(0);
for (int i = 0; i <= WINDOW_SIZE; i++)
{pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
}

5 将Ps、Vs、depth进行更新

for (int i = frame_count; i >= 0; i--)// 将相机系换成imu系 s*p^c0_bk = s*p^c0_ck - R^c0_bk * p^b_cPs[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);
int kv = -1;
map<double, ImageFrame>::iterator frame_i;
for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++)
{if(frame_i->second.is_key_frame){kv++;Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);}
}
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;it_per_id.estimated_depth *= s;
}

6 所有变量从参考坐标系c0到世界坐标系

//通过将重力旋转到z轴上，得到世界坐标系与摄像机坐标系c0之间的旋转矩阵rot_diff
Matrix3d R0 = Utility::g2R(g);
double yaw = Utility::R2ypr(R0 * Rs[0]).x();
R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
g = R0 * g;Matrix3d rot_diff = R0;
//所有变量从参考坐标系c0旋转到世界坐标系w
for (int i = 0; i <= frame_count; i++)
{Ps[i] = rot_diff * Ps[i];Rs[i] = rot_diff * Rs[i];Vs[i] = rot_diff * Vs[i];
}

solveOdometry() 求解里程计

void Estimator::solveOdometry()
{if (frame_count < WINDOW_SIZE)return;if (solver_flag == NON_LINEAR){TicToc t_tri;f_manager.triangulate(Ps, tic, ric);ROS_DEBUG("triangulation costs %f", t_tri.toc());optimization();}
}

optimization() 非线性优化

1 添加顶点（待优化变量）

IMU状态：
$$\begin{gathered} \chi =[x_n,x_{n+1},...,x_{n+N}]，N为滑窗内关键帧个数 \\ {x}_i=[p_{w{b}_i},q_{w{b}_i},v_{w{b}_i},b_a^{b_i},b_g^{b_i}{]}_{15\times 1} \end{gathered}$$
Cam到IMU外参$[p_c^b,q_c^b]$、时钟差para_Td。

for (int i = 0; i < WINDOW_SIZE + 1; i++)
{ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
}

PS: 这里为什么没有加入特征点的逆深度呢？

2、添加边（残差）

优化的目标函数为
m i n χ { ∣ ∣ r p − H p χ ∣ ∣ 2 + ∑ i ∈ B ρ ( ∣ ∣ r b ( z ^ b i b i + 1 , χ ) ∣ ∣ Σ b i b i + 1 2 ) + ∑ ( l , j ) ∈ C ρ ( ∣ ∣ r c ( z ^ l c j , χ ) ∣ ∣ Σ l c j 2 ) } min_\chi \{||r_p-H_p\chi||^2+\sum_{i\in B}\rho(||r_b(\hat z_{b_ib_{i+1}}, \chi)||^2_{\Sigma_{b_ib_{i+1}}})+\sum_{(l,j)\in C} \rho(||r_c(\hat z^{c_j}_{l},\chi)||^2_{\Sigma^{c_j}_l}) \} minχ​{∣∣rp​−Hp​χ∣∣2+i∈B∑​ρ(∣∣rb​(z^bi​bi+1​​,χ)∣∣Σbi​bi+1​​2​)+(l,j)∈C∑​ρ(∣∣rc​(z^lcj​​,χ)∣∣Σlcj​​2​)}
其中，${\Sigma }_{b_i{b}_{i+1}}$为IMU预积分噪声项的协方差矩阵，${\Sigma }_l^{c_j}$为视觉观测噪声的协方差矩阵。目标函数中的残差包括：边缘化的先验信息、IMU预积分残差、视觉重投影误差。该三项的距离计算都是用马氏距离。

2.1 IMU预积分残差

2.1.1 目标函数

预积分项作为观测，状态量传播作为预测。残差为关键帧i到j的预积分残差，因此残差边IMUFactor的维度为<15, 7, 9, 7, 9>：
$${\left[\begin{array}{c}{r}_p\\ r_q\\ r_v\\ r_{ba}\\ r_{bg}\end{array}\right]}_{15\times 1}=\left[\begin{array}{c}{q}_{b_{i}w}(p_{w{b}_j}-p_{w{b}_i}-v_{w{b}_i}\Delta t+\frac{1}{2}{g}^w\Delta t)-{\hat{\alpha }}_{b_i{b}_j}\\ 2[{\hat{\theta }}_{b_j{b}_i}\otimes (q_{b_{i}w}\otimes q_{w{b}_j}){]}_{xyz}\\ q_{b_{i}w}(v_{w{b}_j}-v_{w{b}_i}+g^w\Delta t)-{\hat{\beta }}_{b_i{b}_j}\\ b_j^a-b_i^a\\ b_j^g-b_i^g\end{array}\right]$$
其中，${\hat{\alpha }}_{b_i{b}_j},{\hat{\beta }}_{b_i{b}_j},{\hat{\theta }}_{b_i{b}_j}$ 为关键帧i到j时间内，仅仅使用含噪声的加速度和角速度观测计算的预积分项。

2.1.2 待优化变量

对于两帧间的IMU残差，待优化变量为：
$$[p_{w{b}_i},q_{w{b}_i}],[v_{w{b}_i},b_i^a,b_i^g],[p_{w{b}_j},q_{w{b}_j}],[v_{w{b}_j},b_j^a,b_j^g]$$

2.1.3 预积分修正

在代码中，pre_integration->evaluate()用于计算预积分残差，需要注意的是，该函数在计算残差前，会先对预积分根据bias的变化进行线性修正（前面预积分时，假设了bias不变）：
$$\begin{gathered} {\hat{\alpha }}_{b_i{b}_j}={\alpha }_{b_i{b}_j}+J_{b_i^a}^{\alpha }\delta b_i^a+J_{b_i^g}^{\alpha }\delta b_i^g \\ {\hat{\beta }}_{b_i{b}_j}={\beta }_{b_i{b}_j}+J_{b_i^a}^{\beta }\delta b_i^a+J_{b_i^g}^{\beta }\delta b_i^g \\ {\hat{\theta }}_{b_i{b}_j}={\theta }_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{b_i^g}^{\theta }\delta b_i^g\end{array}\right] \end{gathered}$$

2.2 视觉重投影误差

2.2.1 重投影残差

考虑第i帧图像首次观测到的地图点l，计算在第j帧图像对地图点l的视觉观测残差。
将l从ci系变换到cj系：ci->bi->w->bj->cj，其齐次坐标变换为：
$$\begin{gathered} f_{c_j}=T_{bc}^{-1}{T}_{w{b}_j}^{-1}{T}_{w{b}_i}{T}_{bc}{f}_{c_i} \\ {f}_{c_i}=[\frac{1}{\lambda }{u}_{ci},\frac{1}{\lambda }{v}_{ci},\frac{1}{\lambda },1{]}^T \\ {f}_{c_j}=[x_{c_j},y_{c_j},z_{c_j},1{]}^T \end{gathered}$$
因此，视觉残差为（归一化平面坐标）：
$$r_c=\left[\begin{array}{c}\frac{x_{c_j}}{z_{c_j}}-u_{c_i}\\ \frac{y_{c_j}}{z_{c_j}}-v_{c_i}\end{array}\right]$$

2.2.2 待优化变量

对于视觉观测误差，待优化变量有关键帧i、j和它们观测到的地图点l，以及相机imu外参：
$$[p_{w{b}_i},q_{w{b}_i}],[p_{w{b}_j},q_{w{b}_j}],[p_c^b,q_c^b],{\lambda }_l$$

2.2.3 球面视觉残差

2.3 边缘化的先验信息

2.3.1 先验残差 $||r_p-H_p\chi |{|}^2$

上一时刻通过schur补构建的先验为:
$$H_p=H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}，b_p=b_{rr}-H_{rm}{H}_{mm}^{-1}{b}_{mm}$$
通过SVD分解可恢复出先验的雅可比和残差：
$$\begin{gathered} H_p=US{U}^T=J^{T}J \\ {J}_p=\sqrt{S}{U}^T \\ {r}_p={\sqrt{S}}^{-1}{U}^T{b}_p \end{gathered}$$

3 求解

$$(H_p+H_b+H_c)d_x=b_p+b_b+b_c$$

ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
if (marginalization_flag == MARGIN_OLD)options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
elseoptions.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

4 边缘化

使用滑动窗口移除位姿时，将关联的约束转化为先验放入优化问题中。为了限制基于优化的VIO计算复杂度，引入边缘化。有选择地从滑动窗口中将IMU状态xK和特征点λ1边缘化，同时将对应于边缘状态的测量值转换为先验。

4.1 边缘化策略：

1. 次新帧如果是关键帧的话，将最老的pose移出Sliding Window，将最旧帧关联的视觉和惯性数据边缘化掉。第一个老关键帧及其测量值被边缘化；Margin_Old作为先验值。
2. 如果次新帧不是关键帧的话，那么就只剔除次新帧的视觉观测，而不剔除它的IMU约束。原因是边缘化次新帧保证了关键帧之间有足够视差去三角化出足够多的地图点，并且保证了IMU预积分的连贯性。

边缘化是通过schur补实现的，假设已知当前优化的增量方程：
$$\left[\begin{array}{cc}{H}_{mm}& H_{mr}\\ H_{rm}& H_{rr}\end{array}\right]\left[\begin{array}{c}\Delta x_m\\ \Delta x_r\end{array}\right]=\left[\begin{array}{c}{b}_{mm}\\ b_{rr}\end{array}\right]$$
其中，$\Delta x_m$为要边缘化的状态量，则可以通过schur补构造边缘化后的$H_p$和$b_p$，作为先验信息：
$$H_p=H_{rr}-H_{rm}{H}_{mm}^{-1}{H}_{mr}，b_p=b_{rr}-H_{rm}{H}_{mm}^{-1}{b}_{mm}$$
在获取了保留变量$x_r$的先验信息后，可通过对$H_p$做SVD分解，从$H_p$中恢复雅可比J和残差r，用于下一轮优化的先验残差计算：
$$\begin{gathered} H_p=US{U}^T=J^{T}J \\ {J}_p=\sqrt{S}{U}^T \\ {r}_p={\sqrt{S}}^{-1}{U}^T{b}_p \end{gathered}$$

4.2 代码实现

接下来，看看VINS-Mono里面的边缘化是如何实现的。首先，会根据marginalization_flag的类型进行判断，要边缘化的关键帧是最老帧还是次新帧。依次分析这两种边缘化的实现有何不同。

4.2.1 marginalization_flag == MARGIN_OLD

1、丢弃第0帧的所有观测，如果有上一次的先验残差last_marginalization_info，则将其传递给新构造的临时对象marginalization_info。

if (last_marginalization_info)
{vector<int> drop_set;for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++){if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||last_marginalization_parameter_blocks[i] == para_SpeedBias[0])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);
}

2、将第0帧（最老帧）到第1帧的IMU预积分残差添加到marginalization_info。

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});marginalization_info->addResidualBlockInfo(residual_block_info);
}

3、将由第0帧首次观测到的所有地图点对应的视觉观测残差添加到marginalization_info。

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});
marginalization_info->addResidualBlockInfo(residual_block_info);

4、计算每个残差的雅可比，对应函数为preMarginalize()里的it->Evaluate()

marginalization_info->preMarginalize();

5、多线程构造先验项schur补$H_p\Delta x_r=b_p$，并且从$H_p$中恢复雅可比linearized_jacobians和残差linearized_residuals，用于下一轮先验残差计算。

marginalization_info->marginalize();

6、调整参数块在下一轮窗口的位置（往前移1格），并更新先验信息

// 调整参数块位置
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;

值得注意的是，这里仅仅是相当于将指针进行了一次移动，指针对应的数据还是旧数据，因此需要结合后面调用的slideWindow()函数才能实现真正的滑窗移动，此外last_marginalization_info就是保留下来的先验残差信息，包括保留下来的雅克比linearized_jacobians、残差linearized_residuals、保留下来的和边缘化有关的数据长度keep_block_size、顺序keep_block_idx以及数据keep_block_data。last_marginalization_parameter_blocks就是保留下来的滑窗内的所有的优化变量。

4.2.1 marginalization_flag == MARGIN_SECOND_NEW

1、保留次新帧的IMU观测，丢弃视觉观测，将上次先验残差传递给marginalization_info

if (last_marginalization_info)
{vector<int> drop_set;for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++){ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1])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);
}

2、计算雅可比

marginalization_info->preMarginalize();

3、构造先验项并恢复雅可比和残差

marginalization_info->marginalize();

4、调整参数块（去掉次新帧），并更新先验

std::unordered_map<long, double *> addr_shift;
for (int i = 0; i <= WINDOW_SIZE; i++)
{if (i == WINDOW_SIZE - 1)continue;else if (i == WINDOW_SIZE){addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];}else{addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];}
}
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;

sildeWindow() 滑动窗口

在构造了先验之后，需要通过对滑动窗口进行调整，有选择地将需要边缘化的关键帧移除，这就是slideWindow()的作用。

假设WINDOW_SIZE=5(代码中为10），则buffer大小为6(WINDOW_SIZE+1)，最新来的帧放到WINDOW_SIZE的位置，记作X，两种操作之后X加入到优化环节中，

1、MARGIN_OLD 边缘化最老帧：一种是倒数第二帧如果是关键帧的话，将最旧的pose移出Sliding Window，将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值被边缘化；Margin_Old作为先验值。

2、MARGIN_SECOND_NEW 边缘化次新帧：如果倒数第二帧不是关键帧的话，那么就只剔除倒数第二帧的视觉观测，而不剔除它的IMU约束。原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点。并且保证了IMU预积分的连贯性。

1 MARGIN_OLD

假设滑动窗内一共有10帧，最新一帧为第11帧图像X

if (marginalization_flag == MARGIN_OLD)

1.1 取出最老帧信息

back_R0 = Rs[0];
back_P0 = Ps[0];

1.2 swap将最老帧移至最后WINDOW_SIZE，其余帧前移

for (int i = 0; i < WINDOW_SIZE; i++)
{Rs[i].swap(Rs[i + 1]);std::swap(pre_integrations[i], pre_integrations[i + 1]);dt_buf[i].swap(dt_buf[i + 1]);linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);Headers[i] = Headers[i + 1];Ps[i].swap(Ps[i + 1]);Vs[i].swap(Vs[i + 1]);Bas[i].swap(Bas[i + 1]);Bgs[i].swap(Bgs[i + 1]);
}

1.3 将最老帧覆盖掉

Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];

1.4 最老帧及之前的所有预积分和图像信息都删除

delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();if (true || solver_flag == INITIAL)
{map<double, ImageFrame>::iterator it_0;it_0 = all_image_frame.find(t_0);delete it_0->second.pre_integration;it_0->second.pre_integration = nullptr;for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it){if (it->second.pre_integration)delete it->second.pre_integration;it->second.pre_integration = NULL;}all_image_frame.erase(all_image_frame.begin(), it_0);all_image_frame.erase(t_0);}

1.5 将最老帧首次观测到的地图点信息都变换到新的最老帧上

slideWindowOld();

2 MAGRIN_SECOND_NEW

边缘化次新帧，为保证IMU预积分的连贯性，保留IMU测量，只丢弃视觉观测。

2.1 保留次新帧的IMU测量，次新帧变为新的最新帧

for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++)
{// 取出最新帧IMU测量double tmp_dt = dt_buf[frame_count][i];Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];// 将最新帧的IMU信息附加到次新帧上，从而保留了次新帧的IMU测量pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);dt_buf[frame_count - 1].push_back(tmp_dt);linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
}
// 用最新帧的位姿信息覆盖次新帧的位姿，此时次新帧变为新的最新帧
Headers[frame_count - 1] = Headers[frame_count];
Ps[frame_count - 1] = Ps[frame_count];
Vs[frame_count - 1] = Vs[frame_count];
Rs[frame_count - 1] = Rs[frame_count];
Bas[frame_count - 1] = Bas[frame_count];
Bgs[frame_count - 1] = Bgs[frame_count];

2.2 移除原最新帧的IMU信息

delete pre_integrations[WINDOW_SIZE];
pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};dt_buf[WINDOW_SIZE].clear();
linear_acceleration_buf[WINDOW_SIZE].clear();
angular_velocity_buf[WINDOW_SIZE].clear();

2.3 丢弃原次新帧的视觉观测

slideWindowNew();

/pose_graph 重定位和闭环

VINS-Mono的闭环部分包括重定位和全局位姿图优化。

// 在构造函数里，启动全局位姿图优化线程，等待optimize_buf加入关键帧
PoseGraph::PoseGraph() { t_optimization = std::thread(&PoseGraph::optimize4DoF, this);
}	

// pose_graph_node的主线程，对当前关键帧进行回环检测
measurement_process = std::thread(process);

1 重定位

重定位部分包括回环检测、回环候选帧与当前帧的特征匹配、紧耦合重定位。

1.1 回环检测

1）VINS-Mono使用DBoW2进行闭环检测，经过时空一致性检查，返回候选帧。
2）为了提升回环检测的鲁棒性，除了VIO在滑窗内提取的gftt角点外，还额外增加了FAST角点，使用BRIEF描述子构建BoW。
3）由于单目VIO的roll和pitch是可观的，因此无需加入旋转不变性。

/// 1、对当前帧构造为关键帧
Vector3d T = Vector3d(pose_msg->pose.pose.position.x,pose_msg->pose.pose.position.y,pose_msg->pose.pose.position.z);
Matrix3d R = Quaterniond(pose_msg->pose.pose.orientation.w,pose_msg->pose.pose.orientation.x,pose_msg->pose.pose.orientation.y,pose_msg->pose.pose.orientation.z).toRotationMatrix();// 1.1 将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧
// 关键帧包括位姿、图像、2D观测、3D观测、时间戳和所属地图序列号等
if((T - last_t).norm() > SKIP_DIS)KeyFrame* keyframe = new KeyFrame(pose_msg->header.stamp.toSec(), frame_index, T, R, image,point_3d, point_2d_uv, point_2d_normal, point_id, sequence);   // 在关键帧构造函数内，分别计算当前帧在滑窗内特征点的BRIEF和新的FAST+BRIEF
KeyFrame::KeyFrame() {computeWindowBRIEFPoint();computeBRIEFPoint();
}// 1.2 往posegraph中添加关键帧，flag_detect_loop=1回环检测
posegraph.addKeyFrame(keyframe, 1);
m_process.unlock();
frame_index++;
last_t = T;

在addKeyFrame中，进行回环检测

/! addKeyFrame()// 1.3 获取当前帧的位姿vio_P_cur、vio_R_cur并更新
// w_r_vio为vio系到world系
cur_kf->getVioPose(vio_P_cur, vio_R_cur);
vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;
vio_R_cur = w_r_vio * vio_R_cur;
cur_kf->updateVioPose(vio_P_cur, vio_R_cur);
cur_kf->index = global_index;if (flag_detect_loop)
{TicToc tmp_t;// 1.4 回环检测，返回回环候选帧的索引loop_index = detectLoop(cur_kf, cur_kf->index);
}

detectLoop查询回环候选帧，找到所有候选帧中最早的候选帧返回

//查询字典数据库，得到与每一帧的相似度评分ret[4]
db.query(keyframe->brief_descriptors, ret, 4, frame_index - 50);
//添加当前关键帧到字典数据库中
db.add(keyframe->brief_descriptors);// ret[0] is the nearest neighbour's score. threshold change with neighour score
bool find_loop = false;
cv::Mat loop_result;if (ret.size() >= 1 &&ret[0].Score > 0.05)for (unsigned int i = 1; i < ret.size(); i++){//评分大于0.015则认为是回环候选帧if (ret[i].Score > 0.015){          find_loop = true;int tmp_index = ret[i].Id;}}//对于50帧以后的关键帧才考虑进行回环
//返回评分大于0.015的最早的关键帧索引min_index
if (find_loop && frame_index > 50)
{int min_index = -1;for (unsigned int i = 0; i < ret.size(); i++){if (min_index == -1 || (ret[i].Id < min_index && ret[i].Score > 0.015))min_index = ret[i].Id;}return min_index;
}
elsereturn -1;

1.2 特征匹配

由于在闭环检测时用BRIEF描述子检测到的匹配关系存在误匹配，因此VINS使用2步几何剔除法剔除外点：
1）2D-2D：使用RANSAC估计F矩阵
2）3D-2D：使用RANSAC求解PnP，基于已知的滑窗内特征点的3D位置和回环候选帧的2D观测，估计回环帧的位姿PnP_T_old, PnP_R_old。

/! addKeyFrame()// 获取回环候选帧
KeyFrame* old_kf = getKeyFrame(loop_index);
// 当前帧与回环候选帧进行描述子匹配
if (cur_kf->findConnection(old_kf))

findConnection()先使用BRIEF进行粗匹配，然后是两步法剔除外点

//关键帧与回环帧进行BRIEF描述子匹配，剔除匹配失败的点
searchByBRIEFDes(matched_2d_old, matched_2d_old_norm, status, old_kf->brief_descriptors, old_kf->keypoints, old_kf->keypoints_norm);
reduceVector(matched_2d_cur, status);
reduceVector(matched_2d_old, status);
reduceVector(matched_2d_cur_norm, status);
reduceVector(matched_2d_old_norm, status);
reduceVector(matched_3d, status);
reduceVector(matched_id, status);//若达到最小回环匹配点数
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{//RANSAC PnP检测去除误匹配的点，并估计回环帧的位姿PnP_T_oldstatus.clear();PnPRANSAC(matched_2d_old_norm, matched_3d, status, PnP_T_old, PnP_R_old);reduceVector(matched_2d_cur, status);reduceVector(matched_2d_old, status);reduceVector(matched_2d_cur_norm, status);reduceVector(matched_2d_old_norm, status);reduceVector(matched_3d, status);reduceVector(matched_id, status);
}
//若达到最小回环匹配点数
if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{// 相对位姿 = 当前帧 - 回环帧relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);relative_q = PnP_R_old.transpose() * origin_vio_R;relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());//相对位姿检验if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0){has_loop = true;loop_index = old_kf->index;loop_info << relative_t.x(), relative_t.y(), relative_t.z(),relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),relative_yaw;}
}

在得到闭环帧及其位姿$P_{old}^{PnP},R_{old}^{PnP}$后，会计算累计漂移，并根据漂移将全局轨迹进行修正。
1、计算当前帧和闭环帧之间的相对位姿relative_t、relative_q

// 相对位移 = vio当前帧 - pnp回环帧
relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old); 
relative_q = PnP_R_old.transpose() * origin_vio_R;
relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());

2、根据相对位姿重新计算当前帧位姿$P_{cur}^w,R_{cur}^w$

//重新计算当前帧位姿w_P_cur、w_R_cur
w_P_cur = w_R_old * relative_t + w_P_old;
w_R_cur = w_R_old * relative_q;

3、计算回环得到的位姿$P_{cur}^w,R_{cur}^w$和VIO位姿$P_{cur}^{VIO},R_{cur}^{VIO}$之间的累积漂移shift_r、shift_t

//回环得到的位姿和VIO位姿之间的累积漂移shift_r、shift_t
double shift_yaw;
Matrix3d shift_r;
Vector3d shift_t; 
shift_yaw = Utility::R2ypr(w_R_cur).x() - Utility::R2ypr(vio_R_cur).x();
shift_r = Utility::ypr2R(Vector3d(shift_yaw, 0, 0));
shift_t = w_P_cur - w_R_cur * vio_R_cur.transpose() * vio_P_cur;

4、将全局位姿根据累积漂移进行修正

if (old_kf->sequence != cur_kf->sequence && sequence_loop[cur_kf->sequence] == 0)
{  w_r_vio = shift_r;w_t_vio = shift_t;vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;vio_R_cur = w_r_vio *  vio_R_cur;cur_kf->updateVioPose(vio_P_cur, vio_R_cur);list<KeyFrame*>::iterator it = keyframelist.begin();for (; it != keyframelist.end(); it++)   {if((*it)->sequence == cur_kf->sequence){Vector3d vio_P_cur;Matrix3d vio_R_cur;(*it)->getVioPose(vio_P_cur, vio_R_cur);vio_P_cur = w_r_vio * vio_P_cur + w_t_vio;vio_R_cur = w_r_vio *  vio_R_cur;(*it)->updateVioPose(vio_P_cur, vio_R_cur);}}sequence_loop[cur_kf->sequence] = 1;
}

上述过程如下图：

1.3 紧耦合重定位

通过在残差项中添加回环项使得VIO的当前滑窗与过去的位姿图对齐，注意在重定位优化过程中，只优化当前滑窗内的状态，而回环帧的位姿（$q_v^w,p_b^w$）固定。

// 在vins_estimator的optimization()里，滑窗优化加上了回环项残差
//添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备
if(relocalization_info)
{//printf("set relocalization factor! \n");ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);int retrive_feature_index = 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 start = it_per_id.start_frame;if(start <= relo_frame_local_index){   while((int)match_points[retrive_feature_index].z() < it_per_id.feature_id){retrive_feature_index++;}if((int)match_points[retrive_feature_index].z() == it_per_id.feature_id){Vector3d pts_j = Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);Vector3d pts_i = it_per_id.feature_per_frame[0].point;ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);retrive_feature_index++;}     }}
}

2 全局位姿图优化

根据重定位的结果对过去的关键帧位姿进行全局优化。

2.1 位姿图中添加关键帧

当一个关键帧检测到回环时会将该帧的索引放入优化队列中。在optimize4DoF()中，从优化队列中取出最近的一帧cur_kf，开启4自由度全局优化。

1、优化变量
为保证全局一致性，找到全局中最早的回环帧，优化从它开始往后的轨迹。即待优化变量为最早回环帧往后（固定该帧）的所有关键帧。

2、残差边：
1）顺序边：对于每个待优化帧，建立其与之前4个关键帧之间的顺序边，即相对位姿残差。假设该顶点为i，其之前有一帧j，则相对位姿测量值（由VIO估计位姿得到）为：
$$\begin{gathered} {\hat{p}}_{ij}^i={\hat{R}}_i^{w-1}({\hat{p}}_j^w-{\hat{p}}_i^w) \\ {\hat{\varphi }}_{ij}={\hat{\varphi }}_j-{\hat{\varphi }}_i \end{gathered}$$
2）回环边：若待优化帧中存在回环，则添加其回环边，其测量值为回环检测中pnp的结果relative_t, relative_yaw。

2.2 4DoF位姿图优化

帧i和j之间的残差为：
$$r_{i,j}(p_i^w,{\varphi }_i^w,p_j^w,{\varphi }_j^w)=\left[\begin{array}{c}R({\hat{\theta }}_i,{\hat{\psi }}_i,{\varphi }_i{)}^{-1}(p_j^w-p_i^w)-{\hat{P}}_{ij}^i\\ {\varphi }_j-{\varphi }_i-{\hat{\varphi }}_{ij}\end{array}\right]$$
通过构建最小二乘：
min ⁡ p , ϕ { ∑ ( i , j ) ∈ S ∣ ∣ r i , j ∣ ∣ 2 + ∑ ( i , j ) ∈ L ρ ( ∣ ∣ r i , j ∣ ∣ 2 ) } \min_{p,\phi} \{\sum_{(i,j)\in S}||r_{i,j}||^2+\sum_{(i,j)\in L}\rho(||r_{i,j}||^2)\} p,ϕmin​{(i,j)∈S∑​∣∣ri,j​∣∣2+(i,j)∈L∑​ρ(∣∣ri,j​∣∣2)}
对pose graph进行优化。

//四自由度位姿图优化
void PoseGraph::optimize4DoF()
{while(true){int cur_index = -1;int first_looped_index = -1;//取出最新一个待优化帧作为当前帧m_optimize_buf.lock();while(!optimize_buf.empty()){cur_index = optimize_buf.front();first_looped_index = earliest_loop_index;optimize_buf.pop();}m_optimize_buf.unlock();if (cur_index != -1){printf("optimize pose graph \n");TicToc tmp_t;m_keyframelist.lock();KeyFrame* cur_kf = getKeyFrame(cur_index);int max_length = cur_index + 1;// w^t_i   w^q_idouble t_array[max_length][3];Quaterniond q_array[max_length];double euler_array[max_length][3];double sequence_array[max_length];ceres::Problem problem;ceres::Solver::Options options;options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;options.max_num_iterations = 5;ceres::Solver::Summary summary;ceres::LossFunction *loss_function;loss_function = new ceres::HuberLoss(0.1);ceres::LocalParameterization* angle_local_parameterization =AngleLocalParameterization::Create();list<KeyFrame*>::iterator it;int i = 0;for (it = keyframelist.begin(); it != keyframelist.end(); it++){//找到第一个回环帧，//回环检测到帧以前的都略过if ((*it)->index < first_looped_index)continue;(*it)->local_index = i;Quaterniond tmp_q;Matrix3d tmp_r;Vector3d tmp_t;// 回环帧的位姿(*it)->getVioPose(tmp_t, tmp_r);tmp_q = tmp_r;t_array[i][0] = tmp_t(0);t_array[i][1] = tmp_t(1);t_array[i][2] = tmp_t(2);q_array[i] = tmp_q;Vector3d euler_angle = Utility::R2ypr(tmp_q.toRotationMatrix());euler_array[i][0] = euler_angle.x();euler_array[i][1] = euler_angle.y();euler_array[i][2] = euler_angle.z();sequence_array[i] = (*it)->sequence;problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);problem.AddParameterBlock(t_array[i], 3);//回环帧参数设为固定if ((*it)->index == first_looped_index || (*it)->sequence == 0){   problem.SetParameterBlockConstant(euler_array[i]);problem.SetParameterBlockConstant(t_array[i]);}//add sequential edge//对于每个i, 只计算它与之前4帧的位置和yaw残差for (int j = 1; j < 5; j++){if (i - j >= 0 && sequence_array[i] == sequence_array[i-j]){Vector3d euler_conncected = Utility::R2ypr(q_array[i-j].toRotationMatrix());Vector3d relative_t(t_array[i][0] - t_array[i-j][0], t_array[i][1] - t_array[i-j][1], t_array[i][2] - t_array[i-j][2]);relative_t = q_array[i-j].inverse() * relative_t;double relative_yaw = euler_array[i][0] - euler_array[i-j][0];ceres::CostFunction* cost_function = FourDOFError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, NULL, euler_array[i-j], t_array[i-j], euler_array[i], t_array[i]);}}//add loop edgeif((*it)->has_loop)// 如果检测到回环{//必须回环检测的帧号大于或者等于当前帧的回环检测匹配帧号assert((*it)->loop_index >= first_looped_index);int connected_index = getKeyFrame((*it)->loop_index)->local_index;Vector3d euler_conncected = Utility::R2ypr(q_array[connected_index].toRotationMatrix());Vector3d relative_t;relative_t = (*it)->getLoopRelativeT();double relative_yaw = (*it)->getLoopRelativeYaw();ceres::CostFunction* cost_function = FourDOFWeightError::Create( relative_t.x(), relative_t.y(), relative_t.z(),relative_yaw, euler_conncected.y(), euler_conncected.z());problem.AddResidualBlock(cost_function, loss_function, euler_array[connected_index], t_array[connected_index], euler_array[i], t_array[i]);}if ((*it)->index == cur_index)break;i++;}m_keyframelist.unlock();ceres::Solve(options, &problem, &summary);m_keyframelist.lock();i = 0;//根据优化后的参数更新参与优化的关键帧的位姿for (it = keyframelist.begin(); it != keyframelist.end(); it++){if ((*it)->index < first_looped_index)continue;Quaterniond tmp_q;tmp_q = Utility::ypr2R(Vector3d(euler_array[i][0], euler_array[i][1], euler_array[i][2]));Vector3d tmp_t = Vector3d(t_array[i][0], t_array[i][1], t_array[i][2]);Matrix3d tmp_r = tmp_q.toRotationMatrix();(*it)-> updatePose(tmp_t, tmp_r);if ((*it)->index == cur_index)break;i++;}//根据当前帧的drift，更新全部关键帧位姿Vector3d cur_t, vio_t;Matrix3d cur_r, vio_r;cur_kf->getPose(cur_t, cur_r);cur_kf->getVioPose(vio_t, vio_r);m_drift.lock();yaw_drift = Utility::R2ypr(cur_r).x() - Utility::R2ypr(vio_r).x();r_drift = Utility::ypr2R(Vector3d(yaw_drift, 0, 0));t_drift = cur_t - r_drift * vio_t;m_drift.unlock();it++;for (; it != keyframelist.end(); it++){Vector3d P;Matrix3d R;(*it)->getVioPose(P, R);P = r_drift * P + t_drift;R = r_drift * R;(*it)->updatePose(P, R);}m_keyframelist.unlock();updatePath();}std::chrono::milliseconds dura(2000);std::this_thread::sleep_for(dura);}
}

参考
一文详解单目VINS论文与代码解读目录
VINS-Mono论文翻译