VINS-MONO学习

feature_tracker_node

main()

1、读取配置文件，执行readParameters(n)
2、订阅图像消息，执行img_callback()
3、发布消息
4、ros::spin()

img_callback()

1、操作第一帧图像，记录时间
2、检查时间戳是否正常。若不正常，清空状态，发布重启消息
3、根据配置控制发送给后端的频率
4、把ros消息转为opencv类型
5、特征点跟踪提取操作,执行readImage()
6、给新特征点赋予ID，执行updateID()
7、向后端发布特征点数据

readImage()

1、均衡化图像，执行cv::createCLAHE()
2、假如上一帧有特征点
a：进行光流跟踪，执行cv::calcOpticalFlowPyrLK()。
b：根据函数返回的状态位和图像边界剔除outlier
3、假如这帧需要发布,
a：通过对极约束剔除outlier，执行rejectWithF()
b：设置mask，执行setMask()，假如合格特征点不够，利用mask均匀化提取新的特征点，执行cv::goodFeaturesToTrack()
4、更新前后帧信息，去畸变同时计算特征点速度，执行undistortedPoints()

estimator_node

数据类型介绍

Estimator

A: Estimator estimator 全局对象，调用构造函数Estimator::Estimator()
1、f_manager和estimator中的Rs（旋转）地址一致（后面f_manager中的三角化用到）
2、初始化estimator中的变量

main()

1、读取参数，执行readParameters(n)
2、给estimator（相机IMU外参，传感器时间延迟）、f_manager（旋转外参）赋值，执行Estimator::setParameter()
3、订阅IMU消息，执行imu_callback()
4、订阅前端（图像特征点）消息，执行feature_callback()
5、订阅重启消息，执行restart_callback()
6、订阅回环消息，执行relocalization_callback()
7、后端核心处理，执行process()
8、ros::spin()

imu_callback()

1、检测IMU时间戳
2、将消息读入全局变量imu_buf，尝试唤醒数据对齐函数
3、根据imu消息使用中值法计算世界系下imu系的位姿，初始化完成后，发送PVQ消息
注意事项：
a：大括号内局部变量
b：std::lock_guard<>自动上锁，析构解锁

process()

while(true) 一直循环执行
1、条件变量con.wait() ，被唤醒后执行getMeasurements()，返回1，继续往下执行；否则，继续等待
2、遍历图像（多帧图像与对应时段的imu数据）
a：遍历图像对应的imu数据（imu_a（KF1-KF2之间的时刻）、imu_b（大于等于KF2的第一个时刻））。imu_a时：计算当前与上一时刻imu的时间差dt，执行processIMU()；imu_b时：使用最后两个时刻的imu线性插值出对齐到该帧图像时间戳的imu数据，并计算dt，执行processIMU()
b：回环（略过）
c：打包该帧的特征点数据，执行processImage()
d：发送一些消息
3、初始化后，根据imu消息计算世界系下的位姿，发送PVQ消息

processIMU()

1、如果是第一帧，赋值acc_0、gyr_0，为构造预积分类pre_integrations提供初始值
2、假如没有，构造该帧图像预积分类
3、判断是否为第二帧及以后的图像
a：计算预积分，执行pre_integrations.push_back()
b：保存窗口内的传感器数据，
c：利用imu数据更新滑窗中的PVQ
4、用当前时刻imu数据更新上一时刻imu数据acc_0、gyr_0
注意事项：
a：由于预积分是帧间约束，因此第1个预积分量实际上是用不到的
b：tmp_pre_integration在哪里new的？解答：在processImage()，因为第一帧没用，直接存的空指针，并且新new了一个tmp_pre_integration提供给第二帧使用，用作初始化

processImage()

1、 添加特征点信息到f_manager中，并判断次新帧是否为关键帧，执行addFeatureCheckParallax()
2、向初始化容器all_image_frame添加数据（image、imu），使用上一时刻imu数据作为初始值构造下一帧的tmp_pre_integration
3、外参初始化（略过）
4、初始化 || 非线性优化
A.初始化
1、假如条件满足，开始初始化，执行initialStructure()
2、初始化成功，非线性优化求解，执行solveOdometry()，根据边缘化策略滑动窗口，执行slideWindow()，移除无效地图点（没有深度信息），执行removeFailures()；否则，直接滑动窗口
B.非线性优化（类似初始化）

initialStructure()

1、检查IMU数据（未使用）
2、纯视觉SFM，获得b(k)到c(l)旋转、c(k)到c(l)的位移，l为枢纽帧，k为所有帧中任意一帧
a：把所有特征点数据复制到sfm_f容器
b：找枢纽帧，对极几何，执行relativePose()。在窗口中找一帧作枢纽帧l（优先从前往后，视差理论上更大），与窗口中最后一帧进行对极几何，获得两帧之间的旋转与没有尺度的位移。
c：SFM求解，执行construct()。获得窗口内所有帧到枢纽帧的位姿、特征点在枢纽帧相机坐标系下的3D坐标
d：通过上一步获得的空间点3D坐标与对应的2D归一化坐标，执行cv::solvePnP()，求解初始化容器中窗口外的其他帧到枢纽帧l的位姿
3、视觉惯性对齐，执行visualInitialAlign()

visualInitialAlign()

1、求解bgs（陀螺bias）、v_b(k)（b系下的速度）、g（c(l)下的重力向量）、s（尺度），执行VisualIMUAlignment()
2、更新estimator中的Ps（c(k)到c(l)的位移）、Rs（b(k)到c(l)的旋转）
3、三角化特征点，深度是在每个点被观测到的第一帧相机坐标系下的（没有尺度），执行f_manager.triangulate()
4、根据bgs重新计算滑窗内的预积分
5、转换Ps（在c(l)下）、Vs（在c(l)下）
6、根据s恢复空间点深度的尺度
7、建立世界坐标系，z轴对齐真实重力向量重力对齐详细讲解，特别清晰
a：第一次求解R0，为枢纽帧相机坐标系到世界坐标系的旋转矩阵并且c(l) 在w系下yaw=0
b：第二次求解R0，为枢纽帧相机坐标系到世界坐标系的旋转矩阵并且b(0)在w系下yaw=0
c：更新窗口内的PVQ（b系到w系）

w系定义
z轴 与真实重力（估计的重力向量-g=[0 0 9.8]）对齐，指向天
x轴 与第一帧b系的x轴在w系的投影平行
y轴 右手定则
原点 第一帧b系原点

g的正负问题的一些思考
比力方程： f=a-g => a=f+g w系中 z轴向上为正 此时估计的实际g为 [0 0 -9.8]
代码中使用的公式为a=f-g <==> a=f-(-g) 此时估计的为 -g=[0 0 9.8] 朝z轴正方向
因此在枢纽帧c(l)下的重力g y轴向下为正 因此 g_c(l)= [0 -9.8 0]
重力对齐到[0 0 9.8]是因为实际估计的是-g 朝z轴正方向

solveOdometry()

1、根据初始化后的Rs、Ps（已经获得尺度），三角化剩余的特征点，执行triangulate()，
获得初始深度Z 每个点被第一个观测到的帧相机坐标系中的Z
2、非线性优化和边缘化，执行optimization()

optimization()

A.优化求解
1、定义待优化参数块
a：显式声明有旋转的参数块，本地化四元数，变为自由度为3的李代数
b：是否优化外参
c：是否在线估计传感器时间延迟
2、更换初始值容器（para_xxxx），eigen->double，执行vector2double()
3、添加残差块，计算对应facter的残差和雅可比
a：上一次的边缘化约束（每次迭代只更新残差，固定雅可比）
b：imu预积分，相邻KF约束
c：视觉重投影约束
d：回环约束（如果有）
4、优化结果输出，执行double2vector()，固定窗口内第一帧的位移和航向角

B.边缘化（以MARG_OLD为例，保留本次优化中边缘化参数对保留参数的约束）
1、将本次优化结果转为double类型，执行vector2double()
2、找出和这次要边缘化的参数块（以最老帧为起始观察帧的路标点逆深度，最老帧位姿，最老帧速度零偏）有关的约束（预积分约束，重投影约束，以及上一次边缘化约束）
a：上一次边缘化：
上一次的边缘化约束对当前待边缘化的参数块（para_Pose[0]、para_SpeedBias[0]）有约束
找到其在上一次边缘化留下的参数块中的索引
构造自定义解析代价函数，添加自定义残差块到边缘化总类marginalization_info中
b：预积分
KF[0]-KF[1] pre_integrations[1]约束了这相邻两帧
c：重投影约束
找能被最老帧看到的特征点，遍历看到这个特征点的所有KF，通过这个特征点，建立和最老帧的约束
3、收集优化A中对最老帧有约束的所有的残差块，进行预处理，执行preMarginalize()，计算每个残差块的残差和雅可比，备份参数块的数据到parameter_block_data
4、边缘化，执行marginalize()
a：计算参数块（包括边缘化和保留）的大小维度（不是个数），m为总共待边缘化的参数块总大小，n为保留的参数块的总大小
b：构造Ax=b，将每个残差块的残差和雅可比叠加到一块，多线程执行ThreadsConstructA()
c：利用舒尔补，得到一个可以求解保留的参数块的方程（Ax=b），利用下面的公式求解线性化点x0（当前优化解）处雅克比J、残差e
注意事项 程序中，e（残差）=h（观测方程）-z(测量)，所以没负号
/*
Ax = b => JT*J x= JT * e
对A做特征值分解 A = V * S * VT,其中Ｓ是特征值构成的对角矩阵 V是特征列向量构成的矩阵
V的第k个列向量是对应S中第k个特征值的特征向量（eigenvector）
JT * J = (S^(1/2) * VT)T * S^(1/2) * VT = V * S^(1/2)T * S^(1/2) * VT = V * S * VT(对角矩阵的转置等于其本身)
J = S^(1/2) * VT
e = (JT)-1 * b = (V * S^(1/2))-1 * b = S^-(1/2） * VT * b
*/
d：更新边缘化信息，
last_marginalization_info：本次边缘化所有信息
last_marginalization_parameter_blocks：本次边缘化对保留的参数块形成约束，这些参数块在滑窗之后的新地址