本文详细解析了VINS-Mono的后端优化过程，包括VIO残差函数构建、Estimator::Optimization函数中的参数添加、残差块构建、边缘化处理和滑动窗口操作。边缘化处理通过MarginalizationInfo类实现，保证了系统的稳定性和计算效率。滑动窗口操作则根据关键帧和非关键帧的不同处理方式，实现状态变量的更新和优化。
上一篇文章是基于滑动窗的紧耦合后端非线性优化的理论部分，主要分为以下部分：
- VIO残差函数构建
- 视觉约束：视觉重投影误差residual、残差对状态量的Jacobian、协方差
- IMU约束：residual、Jacobian、Covariance
- 基于舒尔补的边缘化
本文将对Estimator::optimization()函数的代码进行详细讲解。
目录
1、添加ceres参数块,待优化变量 AddParameterBlock
1、初始化ceres并将 (p,v,q,ba,bg) 15自由度优化变量加入
1、添加边缘化残差，丢弃帧的约束 MarginalizationFactor
4、添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备
1、将上一次先验残差项MarginalizationFactor传递给marginalization_info
2、将第0帧和第1帧间的预积分观测IMU因子IMUFactor(pre_integrations[1])，添加到marginalization_info中
3、将第一次观测为第0帧的所有路标点对应的视觉观测ProjectionFactor，添加到marginalization_info中
4、preMarginalize()函数：计算每个残差对应的Jacobian，
5、marglinalize()函数：多线程构造先验项舒尔补AX=b的结构，在X0处线性化计算Jacobian和残差
6、调整参数块在下一次窗口中对应的位置（往前移一格），注意这里是指针，后面slideWindow中会赋新值，这里只是提前占座
1、保留次新帧的IMU测量，丢弃该帧的视觉测量，将上一次先验残差项传递给marginalization_info
2、preMarginalize():计算每个残差对应的Jacobian
3、marginalize():多线程构造先验项舒尔补AX=b
5、slideWindowOld()函数，首次在原来最老帧出现的特征点转移到现在现在最老帧
2、MARGIN_SECOND_NEW 边缘化次新帧，但是不删除IMU约束
3、当最新一帧(5)不是关键帧时，用于merge滑窗内最新帧(4)
一、VIO残差函数构建
1、需要优化的状态向量
滑动窗口内IMU状态（PVQ、加速度bias、陀螺仪bias）、IMU到Camera的外参、m+1个3D路标点逆深度。
第一个式子是滑动窗口内所有状态量，n是关键帧数量，m是滑动窗内所有观测到的路标点总数。λi是第i个特征点的第一个观测对应的逆深度.特征点逆深度为了满足高斯系统。
第二个式子xk是在第k帧图像捕获到的IMU状态，包括位置，速度，旋转（PVQ）和加速度偏置，陀螺仪偏置。
第三个式子是相机外参。
注意：xk只与IMU项和Marg有关；特征点深度也只与camera和Marg有关；
维度是15*n+6+m
IMU状态 xk {P、V、Q、Ba、Bg} 是15维（5*3=15）；相机外参 
为 6 维； 特征点逆深度为1维。
2、目标函数
优化函数有三部分：
- 边缘化残差：从滑动窗中去掉的节点和特征点构成的约束，形成一个先验Prior
- IMU残差：相邻两帧IMU产生的residual
- 视觉重投影误差：单个特征点在两帧之间投影形成residual
二、Estimator::Optimization（）函数
整个滑动窗优化在optimization函数中求解。基于滑动窗口紧耦合的非线性优化，残差项的构造和求解
1.添加要优化的变量 (p,v,q,ba,bg) 一共15个自由度，Cam到IMU外参，时钟差
2.添加残差，残差项分为4块 先验残差+IMU残差+视觉残差+闭环检测残差
3.ceres求解Solve（）函数
4.边缘化操作
1、添加ceres参数块,待优化变量 AddParameterBlock
待优化变量分为三部分：IMU状态xk (p,v,q,ba,bg)、Cam到IMU外参、时钟差
1、初始化ceres并将 (p,v,q,ba,bg) 15自由度优化变量加入
创建一个ceres Problem实例, loss_function定义为CauchyLoss.柯西核函数
ceres::Problem problem;
ceres::LossFunction *loss_function;// 残差
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0); // 柯西核函数先添加优化参数量, ceres中参数用ParameterBlock来表示,类似于g2o中的vertex, 这里的参数块有sliding windows中所有帧的para_Pose(7维) 和 para_SpeedBias(9维).
Ps、Rs转变成para_Pose 6自由度7DOF（x,y,z,qx,qy,qz,w），Vs、Bas、Bgs转变成para_SpeedBias 9自由度9DOF(vx,vy,vz,bax,bay,baz,bgx,bgy,bgz)
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);
}2、Cam到IMU的外参估计加入
camera到IMU的外参也添加到估计 para_Ex_Pose
for (int i = 0; i < NUM_OF_CAM; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
if (!ESTIMATE_EXTRINSIC)
{
ROS_DEBUG("fix extinsic param");
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
else
ROS_DEBUG("estimate extinsic param");
}3、滑窗内第一时刻相机到IMU时钟差加入
滑窗内第一个时刻相机到IMU时钟差，保证传感器同步， para_Td
if (ESTIMATE_TD)
{
problem.AddParameterBlock(para_Td[0], 1);
//problem.SetParameterBlockConstant(para_Td[0]);
}4、在ceres中，vevtor转double类型
- 从Ps、Rs、Vs、Bas、Bgs转化为para_Pose（6维，相机位姿）和para_SpeedBias（9维，相机速度、加速度偏置、角速度偏置）
- 从tic和q转化为para_Ex_Pose （6维，Cam到IMU外参）
- 从dep到para_Feature（1维，特征点深度）
- 从td转化为para_Td（1维，标定同步时间）
void Estimator::vector2double()
{
// 1.遍历滑动窗，IMU的15个自由度的优化变量
for (int i = 0; i <= WINDOW_SIZE; i++)
{
// P、R
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();
// V、Ba、Bg
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();
}
// 2.Cam到IMU的外参，6自由度由7个参数表示
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);
// 3、保证传感器同步的时钟差
if (ESTIMATE_TD)
para_Td[0][0] = td;
}2、添加残差 AddResidualBlock
依次加入margin项,IMU项和视觉feature项. 每一项都是一个factor, 这是ceres的使用方法, 创建一个类继承ceres::CostFunction类, 重写Evaluate()函数定义residual的计算形式. 分别对应marginalization_factor.h, imu_factor.h, projection_factor.h中的MarginalizationInfo, IMUFactor, ProjectionFactor三个类。首先会调用addResidualBlockInfo()函数将各个残差以及残差涉及的优化变量添加入上面所述的优化变量中
1、添加边缘化残差，丢弃帧的约束 MarginalizationFactor
if (last_marginalization_info)
{
// construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}2、添加IMU残差 IMUFactor
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
if (pre_integrations[j]->sum_dt > 10.0)
continue;
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
int f_m_cnt = 0;
int feature_index = -1;3、添加视觉残差 ProjectionTdFactor
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;
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());
problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
/*
double **para = new double *[5];
para[0] = para_Pose[imu_i];
para[1] = para_Pose[imu_j];
para[2] = para_Ex_Pose[0];
para[3] = para_Feature[feature_index];
para[4] = para_Td[0];
f_td->check(para);
*/
}
else
{
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
}
f_m_cnt++;
}
}
ROS_DEBUG("visual measurement count: %d", f_m_cnt);
ROS_DEBUG("prepare for ceres: %f", t_prepare.toc());4、添加闭环检测残差，计算滑动窗口中与每一个闭环关键帧的相对位姿，这个相对位置是为后面的图优化准备
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 Pr
最低0.47元/天 解锁文章
扫一扫
4988
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
