zoufeizhiba的博客

本文详细介绍了VINS（Visual-Inertial Navigation System）中的闭环检测和4自由度优化流程，包括为什么只优化4自由度以及优化的理论推导和代码讲解。

主要包含几个模块：(1) 基础信息赋值(2) VIO位姿初始化(3) 图像处理(4) 特征点信息存储(5) 回环检测相关标志位初始化(6) 计算特征描述子，（computeWindowBRIEFPoint函数中做的），并在关键帧中新检测500个关键点并进行描述子计算（在computeBRIEFPoint函数中实现）

由视觉惯性里程计（VIO）模块实时估计的局部坐标系，原点通常为系统启动时的初始位置。

用J JJ矩阵构造H HH矩阵，并且得到J α J_\alphaJα​和( J α T ) + g 0 ∗ (J^T_\alpha)^+g_0^*(JαT​)+g0∗​，即。这里主要是为以后得到H矩阵做一些前期铺垫工作，比如标记哪些参数块要保留，哪些要边缘化，给一些记录边缘化相关信息的变量赋值，如下所示的变量。边缘化是指将滑窗中旧的状态删除掉，但却保留下旧的状态所带来的约束关系的一个过程，相当于将约束信息转换为优化变量的先验信息。以上代码分为边缘化最老帧和次新帧两种情况，当边缘化最老帧时。

VINS后端非线性优化理论推导及工程实现

纯视觉SFM及视觉IMU对齐

相机IMU外参原理及代码分析

后端图像处理processImg主要流程分析及边缘化mag策略

介绍了imu预积分的推导和运动方程

介绍了VINS的后端主要流程

主要包含imu内参，图像内参及一些配置参数。另外将逆深度参数化在视觉误差中的作用，以及边缘化操作如何形成先验，保持系统优化的稀疏性。从逻辑功能上可以分为六部分：ROS相关初始化、参数读取、estimator中相关参数的设置、topic发布器注册、订阅topic、创建process线程。下一章内容：-----------imu预积分及误差传播方程推导-------------。4）订阅topic并注册回调函数，后端算法逻辑部分就定义在回调函数内部。3)观测先验残差（视觉，imu积分，边缘化）的雅可比推导。

(2) 生成节点句柄，~代表私有命名空间，其可以增加一层命名空间，为编写组件提供便利。定义一个节点函数内代表的节点只有一个，节点句柄可以有多个，它们的区别只在于命名空间不同，表示的都是同一个节点。(3)声明当前节点订阅的话题,并定义对应的回调函数,每收到一条话题数据,ROS调用一次回调函数.vins前端部分回调函数img_callback。(4)声明当前节点发布的话题.发布PointCloud，tracked_img,restart_flag。(1).初始化ROS节点,确定节点的命名空间。

同时启动了feature_tracker、vins_estimator和pose_graph这三个node，并设置配置参数文件euroc_config.yaml，这个yaml文件中包含系统运行的相关配置参数信息。该launch文件对应的配置文件为euroc_config_no_extrinsic.yaml，在该yaml文件中不包含相机和IMU之间的外参，需要slam初始化在线估计。对应的配置文件为vins_rviz_config.rviz，主要包含一些图像，轨迹的可视化参数设置。