【VINS论文笔记】系列之状态估计器流程
标题：VINS-Mono代码解读——状态估计器流程 estimator 写在初始化和非线性优化前
作者：Manii
来源：https://blog.csdn.net/qq_41839222/category_9286052.html
排版：点云PCL
本文仅做学术分享，已获得作者授权转载，未经允许请勿二次转载！欢迎各位加入免费知识星球，获取PDF文档，欢迎转发朋友圈，分享快乐。
希望有更多的小伙伴能够加入我们，一起开启论文阅读，相互分享的微信群。参与和分享的方式：dianyunpcl@163.com
前言
本文主要介绍VINS的状态估计器模块（estimator），主要在代码中/vins_estimator节点的相关部分实现。
这个模块可以说是VINS的最核心模块，从论文的内容上来说，里面的内容包括了VINS的估计器初始化、基于滑动窗口的非线性优化实现紧耦合. 该模块的代码放在文件夹vins_estimator中，可以看到，除了上述内容外，还包括有外参标定、可视化等其他功能的实现，内容实在是太多了！所以本文主要是对vins_estimator文件夹内每个文件的代码功能进行简单整理，并从estimator_node.cpp开始，对状态估计器的具体流程进行代码解读，初始化以及紧耦合的理论知识和具体实现将放在以后进行详细说明。
其中论文中对于关键帧的选择（论文IV A部分）：
两个关键帧选择标准：
1、与上一个关键帧的平均视差。如果在当前帧和最新关键帧之间跟踪的特征点的平均视差超出某个特定阈值，则将该帧视为新的关键帧。
2、跟踪质量。如果跟踪的特征数量低于某一阈值，则将此帧视为新的关键帧。这个标准是为了避免跟踪特征完全丢失。
具体在bool FeatureManager::addFeatureCheckParallax()中实现。
流程图
代码实现
vins_estimator文件夹
factor：
imu_factor.h：IMU残差、雅可比
intergration_base.h：IMU预积分
marginalization.cpp/.h：边缘化
pose_local_parameterization.cpp/.h：局部参数化
projection_factor.cpp/.h：视觉残差
initial：
initial_alignment.cpp/.h：视觉和IMU校准(陀螺仪偏置、尺度、重力加速度和速度)
initial_ex_rotation.cpp/.h：相机和IMU外参标定
initial_sfm.cpp/.h：纯视觉SFM、三角化、PNP
solve_5pts.cpp/.h：5点法求基本矩阵得到Rt
utility：
CameraPoseVisualization.cpp/.h：相机位姿可视化
tic_toc.h：记录时间
utility.cpp/.h：各种四元数、矩阵转换
visualization.cpp/.h：各种数据发布
estimator.cpp/.h：紧耦合的VIO状态估计器实现
estimator_node.cpp：ROS 节点函数，回调函数
feature_manager.cpp/.h：特征点管理，三角化，关键帧等
parameters.cpp/.h：读取参数
输入输出
输入：
1、IMU的角速度和线加速度，即订阅了IMU发布的topic：IMU_TOPIC='/imu0'
2、图像追踪的特征点，即订阅了feature_trackers模块发布的topic：“/feature_tracker/feature'
3、复位信号，即订阅了feature_trackers模块发布的topic：“/feature_tracker/restart'
4、重定位的匹配点，即订阅了pose_graph模块发布的topic：“/pose_graph/match_points'
输出：
1、在线程void process()中给RVIZ发送里程计信息PQV、关键点三维坐标、相机位姿、点云信息、IMU到相机的外参、重定位位姿等
2、在回调函数
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)发布最新的由IMU直接递推得到的PQV
estimator_node.cpp
程序入口 int main(int argc, char **argv)
1、ROS初始化、设置句柄
2、读取参数，设置状态估计器参数
3、发布用于RVIZ显示的Topic，本模块具体发布的内容详见输入输出
4、订阅IMU、feature、restart、match_points的topic，执行各自回调函数
5、创建VIO主线程
4个回调函数
这里需要注意的一点是：节点estimator，以及创建了一个process，必须考虑多线程安全问题：
1、队列imu_buf、feature_buf、relo_buf是被多线程共享的，因而在回调函数将相应的msg放入buf或进行pop时，需要设置互斥锁m_buf，在操作前lock()，操作后unlock()。其他互斥锁同理。
2、在feature_callback和imu_callback中还设置了条件锁，在完成将msg放入buf的操作后唤醒作用于process线程中的获取观测值数据的函数。
3、在imu_callback中还通过lock_guard的方式构造互斥锁m_state，它能在构造时加锁，析构时解锁。
主线程 process()
通过while (true)不断循环，主要功能包括等待并获取measurements，计算dt，然后执行以下函数：
stimator.processIMU()进行IMU预积分
estimator.setReloFrame()设置重定位帧
estimator.processImage()处理图像帧：初始化，紧耦合的非线性优化
其中measurements的数据格式可以表示为：(IMUs, img_msg)s s表示容器（vector）
1、 等待上面两个接收数据完成就会被唤醒，在执行getMeasurements()提取measurements时互斥锁m_buf会锁住，此时无法接收数据。
getMeasurements()的作用是对imu和图像数据进行对齐并组合，之后会具体分析
2、对measurements中的每一个measurement (IMUs,IMG)组合进行操作
for (auto &measurement : measurements)
2.1、对于measurement中的每一个imu_msg，计算dt并执行processIMU()。
processIMU()实现了IMU的预积分，通过中值积分得到当前PQV作为优化初值
2.2、在relo_buf中取出最后一个重定位帧，拿出其中的信息并执行setReloFrame()
2.3、建立每个特征点的(camera_id,[x,y,z,u,v,vx,vy])s的map，索引为feature_id
2.4、处理图像，这里实现了视觉与IMU的初始化以及非线性优化的紧耦合
2.5、向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系，这部分在之前输入输出已经介绍了
2.6、更新IMU参数[P,Q,V,ba,bg,a,g]，注意线程安全
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> getMeasurements()
该函数的主要功能是对imu和图像数据进行对齐并组合，返回的是(IMUs, img_msg)s，即图像帧所对应的所有IMU数据，并将其放入一个容器vector中。
IMU和图像帧的对应关系在新版的代码中有变化：对图像帧j，每次取完imu_buf中所有时间戳小于它的imu_msg，以及第一个时间戳大于图像帧时间戳的imu_msg （这里还需要加上同步时间存在的延迟td）。
因此在新代码中，每个大于图像帧时间戳的第一个imu_msg是被两个图像帧共用的，而产生的差别在processIMU()前进行了对应的处理。
estimator.cpp/.h
构建了一个estimator类，这次我们主要讨论流程问题，因而暂时只分析一下processImage()
void Estimator::processImage()
1、addFeatureCheckParallax()添加之前检测到的特征点到feature容器list中，计算每一个点跟踪的次数，以及它的视差并通过检测两帧之间的视差决定是否作为关键帧。
param[in] frame_count 窗口内帧的个数
param[in] image 某帧所有特征点的[camera_id,[x,y,z,u,v,vx,vy]]构成的map,索引为feature_id
param[in] td 相机和IMU同步校准得到的时间差
2、 将图像数据、时间、临时预积分值存到图像帧类中
3、更新临时预积分初始值
4、判断是否需要进行外参标定
5、solver_flag==INITIAL 进行初始化
5.1、确保有足够的frame参与初始化，有外参，且当前帧时间戳大于初始化时间戳+0.1秒
5.2、执行视觉惯性联合初始化
5.3、初始化成功则进行一次非线性优化，不成功则进行滑窗操作
6、solver_flag==NON_LINEAR进行非线性优化
6.1、执行非线性优化具体函数solveOdometry()
6.2、检测系统运行是否失败，若失败则重置估计器
6.3、执行窗口滑动函数slideWindow();
6.4、去除估计失败的点并发布关键点位置
void update()
这个函数在非线性优化时才会在process()中被调用
1、从估计器中得到滑动窗口中最后一个图像帧的imu更新项[P,Q,V,ba,bg,a,g]
2、对imu_buf中剩余的imu_msg进行PVQ递推
（因为imu的频率比图像频率要高很多，在getMeasurements(）将图像和imu时间对齐后，imu_buf中还会存在imu数据）
queue<sensor_msgs::ImuConstPtr> tmp_imu_buf = imu_buf; for (sensor_msgs::ImuConstPtr tmp_imu_msg; !tmp_imu_buf.empty(); tmp_imu_buf.pop()) predict(tmp_imu_buf.front());资源
三维点云论文及相关应用分享
【点云论文速读】基于激光雷达的里程计及3D点云地图中的定位方法
3D-MiniNet: 从点云中学习2D表示以实现快速有效的3D LIDAR语义分割（2020）
PCL中outofcore模块---基于核外八叉树的大规模点云的显示
更多文章可查看：点云学习历史文章大汇总
SLAM及AR相关分享