VINS-FUSION代码超详细注释（VIO部分）/VIO入门(4)
文章目录
- 0 前情回顾
- 本次工作
- 1 updateLatestStates() 和slideWindow()
- 2 optimization()
0 前情回顾
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(1)讲到了主程序rosNodeTest.cpp。在程序最后，会进入sync_process线程进行处理。本篇博客接着进行讲解。
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(2)中,讲了sync_process,以及其中的trackImage和processMeasurements,包括processMeasurements中对IMU数据的处理部分.
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(3)中,讲了processImage,其中就包含了初始化,三角化,非线性优化,划窗等等内容.
本文主要对函数optimization() updateLatestStates() slideWindow()进行介绍.
本次工作
我首先一步步的把代码全部注释了，十分的详细，对于C++和OpenCV的一些操作也进行了详细的注释，对于刚入门的同学应该还是有帮助的。之后我将代码开源，并写了相应的博客进行讲解。
开源程序:
https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git
相应博客：
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(1)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(2)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(3)
VINS-FUSION代码超详细注释（VIO部分）/VIO入门(4)
1 updateLatestStates() 和slideWindow()
这两个函数比较简单
// 让此时刻的值都等于上一时刻的值,用来更新状态
void Estimator::updateLatestStates()
// 滑动窗口法
void Estimator::slideWindow()
// 道理很简单,就是把前后元素交换
2 optimization()
这个函数可以说是整个VIO的精华和难点所在!
因为内容太多了,所以直接贴了代码.对于其中某些函数,如果大家有什么问题的话,可以去github上参考我的代码.
// 基于滑动窗口的紧耦合的非线性优化，残差项的构造和求解
void Estimator::optimization()
{
TicToc t_whole, t_prepare;
vector2double();
//------------------ 定义问题 定义本地参数化,并添加优化参数-------------------------------------------------
ceres::Problem problem;// 定义ceres的优化问题
ceres::LossFunction *loss_function;//核函数
//loss_function = NULL;
loss_function = new ceres::HuberLoss(1.0);//HuberLoss当预测偏差小于 δ 时，它采用平方误差,当预测偏差大于 δ 时，采用的线性误差。
//loss_function = new ceres::CauchyLoss(1.0 / FOCAL_LENGTH);
//ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);
for (int i = 0; i < frame_count + 1; i++)
{
// 对于四元数或者旋转矩阵这种使用过参数化表示旋转的方式，它们是不支持广义的加法
// 所以我们在使用ceres对其进行迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个LocalParameterization
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
// AddParameterBlock 向该问题添加具有适当大小和参数化的参数块。
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization); //因为有四元数,所以使用了 local_parameterization
if(USE_IMU)
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);//使用默认的加法
}
// 没使用imu时,将窗口内第一帧的位姿固定
if(!USE_IMU)
// SetParameterBlockConstant 在优化过程中，使指示的参数块保持恒定。设置任何参数块变成一个常量
// 固定第一帧的位姿不变! 这里涉及到论文2中的
problem.SetParameterBlockConstant(para_Pose[0]);
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 && frame_count == WINDOW_SIZE && Vs[0].norm() > 0.2) || openExEstimation)
//Vs[0].norm() > 0.2窗口内第一个速度>2?
{
//ROS_INFO("estimate extinsic param");
openExEstimation = 1;//打开外部估计
}
else//如果不需要估计,则把估计器中的外部参数设为定值
{
//ROS_INFO("fix extinsic param");
problem.SetParameterBlockConstant(para_Ex_Pose[i]);
}
}
problem.AddParameterBlock(para_Td[0], 1);//把时间也作为待优化变量
if (!ESTIMATE_TD || Vs[0].norm() < 0.2)//如果不估计时间就固定
problem.SetParameterBlockConstant(para_Td[0]);
// ------------------------在问题中添加约束,也就是构造残差函数----------------------------------
// 在问题中添加先验信息作为约束
if (last_marginalization_info && last_marginalization_info->valid)
{
// 构造新的marginisation_factor construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
/* 通过提供参数块的向量来添加残差块。
ResidualBlockId AddResidualBlock(
CostFunction* cost_function,//损失函数
LossFunction* loss_function,//核函数
const std::vector& parameter_blocks); */
problem.AddResidualBlock(marginalization_factor, NULL,
last_marginalization_parameter_blocks);
}
// 在问题中添加IMU约束
if(USE_IMU)
{
for (int i = 0; i < frame_count; 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]);
//这里添加的参数包括状态i和状态j
}
}
int f_m_cnt = 0; //每个特征点,观测到它的相机的计数 visual measurement count
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;
// imu_i该特征点第一次被观测到的帧 ,imu_j = imu_i - 1
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)//既,本次不是第一次观测到
{
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);
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]);
/* 相关介绍:
1 只在视觉量测中用了核函数loss_function 用的是huber
2 参数包含了para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]
3 ProjectionTwoFrameOneCamFactor这个重投影并不是很懂 */
}
// 如果是双目的
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);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);
}
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);
problem.AddResidualBlock(f, loss_function, para_Ex_Pose[0], para_Ex_Pose[1], para_Feature[feature_index], para_Td[0]);
}
}
f_m_cnt++;
}
}
ROS_DEBUG("visual measurement count: %d", f_m_cnt);
//printf("prepare for ceres: %f \n", t_prepare.toc());
// ------------------------------------写下来配置优化选项,并进行求解-----------------------------------------
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
//options.num_threads = 2;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;
if (marginalization_flag == MARGIN_OLD)
options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
else
options.max_solver_time_in_seconds = SOLVER_TIME;
TicToc t_solver;
ceres::Solver::Summary summary;//优化信息
ceres::Solve(options, &problem, &summary);
//cout << summary.BriefReport() << endl;
ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
//printf("solver costs: %f \n", t_solver.toc());
double2vector();
//printf("frame_count: %d \n", frame_count);
if(frame_count < WINDOW_SIZE)
return;
// -----------------------------marginalization------------------------------------
TicToc t_whole_marginalization;
//如果需要marg掉最老的一帧
if (marginalization_flag == MARGIN_OLD)
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();//先验信息
vector2double();
if (last_marginalization_info && last_marginalization_info->valid)
{
vector<int> drop_set;//边缘话的优化变量的位置_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掉的优化变量，也就是滑窗内第一个变量,para_Pose[0]和para_SpeedBias[0]
drop_set.push_back(i);
}
// 创建新的marg因子 construct new marginlization_factor
MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
/* 是为了将不同的损失函数_cost_function以及优化变量_parameter_blocks统一起来再一起添加到marginalization_info中
ResidualBlockInfo(ceres::CostFunction *_cost_function,
ceres::LossFunction *_loss_function,
std::vector _parameter_blocks,
std::vector _drop_set) */
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL,
last_marginalization_parameter_blocks,
drop_set);//这一步添加了marg信息
// 将上一步marginalization后的信息作为先验信息
marginalization_info->addResidualBlockInfo(residual_block_info);
}
// 添加IMU的marg信息
// 然后添加第0帧和第1帧之间的IMU预积分值以及第0帧和第1帧相关优化变量
if(USE_IMU)
{
if (pre_integrations[1]->sum_dt < 10.0)
{
IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
// 这一步添加IMU的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});//这里是0,1的原因是0和1是para_Pose[0], para_SpeedBias[0]是需要marg的变量
marginalization_info->addResidualBlockInfo(residual_block_info);
}
}
// 添加视觉的maeg信息
{
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;//这里是从特征点的第一个观察帧开始
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)
{
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});//为0和3的原因是，para_Pose[imu_i]是第一帧的位姿，需要marg掉，而3是para_Feature[feature_index]是和第一帧相关的特征点，需要marg掉
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);
}
}
}
}
}
TicToc t_pre_margin;
// 上面通过调用 addResidualBlockInfo() 已经确定优化变量的数量、存储位置、长度以及待优化变量的数量以及存储位置，
//-------------------------- 下面就需要调用 preMarginalize() 进行预处理
marginalization_info->preMarginalize();
ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());
//------------------------调用 marginalize 正式开始边缘化
TicToc t_margin;
marginalization_info->marginalize();
ROS_DEBUG("marginalization %f ms", t_margin.toc());
//------------------------在optimization的最后会有一部滑窗预移动的操作
// 值得注意的是，这里仅仅是相当于将指针进行了一次移动，指针对应的数据还是旧数据，因此需要结合后面调用的 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];//因此para_Pose这些变量都是双指针变量，因此这一步是指针操作
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)
delete last_marginalization_info;//删除掉上一次的marg相关的内容
last_marginalization_info = marginalization_info;//marg相关内容的递归
last_marginalization_parameter_blocks = parameter_blocks;//优化变量的递归，这里面仅仅是指针
}
else
{
if (last_marginalization_info &&
std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks), para_Pose[WINDOW_SIZE - 1]))
{
MarginalizationInfo *marginalization_info = new MarginalizationInfo();
vector2double();
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++)
{
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_factor
MarginalizationFactor *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);
}
TicToc t_pre_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->preMarginalize();
ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());
TicToc t_margin;
ROS_DEBUG("begin marginalization");
marginalization_info->marginalize();
ROS_DEBUG("end marginalization, %f ms", t_margin.toc());
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];
if(USE_IMU)
addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
}
else
{
addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
if(USE_IMU)
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];
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;
}
}
//printf("whole marginalization costs: %f \n", t_whole_marginalization.toc());
//printf("whole time for ceres: %f \n", t_whole.toc());
}