GVINS代码学习3

估计器estimator的学习：

estimator.h头文件实现了名为estimator的类，包含了对输入数据的处理、状态估计、优化、特征点管理等一系列步骤，以及辅助函数和变量，用于实现算法的各个功能模块

estimator_node.cpp文件

main()

1 初始化读取参数，执行readParameters(n)

2 创建Estimator对象，给对象赋值，并执行设置参数setParameter()

3 注册了发布者,用于发布算法的输出结果registerPub（n）

4 初始化下一个时间脉冲信息，GNSS本地时间差，最新的GNSS时间等变量

5 根据是否启用GNSS功能，选择订阅不同的话题

6 订阅IMU信息，执行imu_callback()

7 订阅前端（图像特征点）信息，执行feature_callback（）

8 订阅重启信息，执行restart_callback()

9 如果启用了GNSS，则会订阅GNSS相关的话题，如GNSS星历,执行gnss_ephem_callback();GLONASS星历，执行gnss_glo_ephem_callback();GNSS测量值，执行gnss_meas_callback();GNSS电离层信息，执行gnss_iono_params_callback()

10 如果启用了本地在线同步功能，还会订阅GNSS时间脉冲，执行gnss_tp_info_callback（）；本地触发信息，执行local_trigger_info_callback();

如果没有启用，则会执行预设的GNSS本地时间差

11 创建两个线程，执行process()函数处理数据，以及执行ros::spin()函数处理ROS信息

imu_callback()

1 获取IMU消息的时间戳，并与上一次接收到的IMU消息的时间戳进行比较

2 更新时间为当前IMU信息的时间戳

3 在互斥锁的保护下进行预测操作，执行predict(imu_msg)

4 如果估计器的状态为非线性，则发布最新的PVQ里程计信息，执行pubLastestOdometry()

process()

while(true) 一直循环执行
1、创建存储传感器数据的：measurements是一个存储IMU和图像数据的容器，每个元素是一个IMU数据和图像数据的pair；imu_msg是存储IMU数据的变量；img_msg是存储图像数据的变量；gnss_msg是存储GNSS数据的变量。

2 、创建互斥锁m_buf，并使用std::unique_lock对其进行上锁。然后，调用con.wait函数，该函数会等待条件getMeasurements(imu_msg, img_msg, gnss_msg)返回true。条件函数getMeasurements获取传感器数据，并将其存储到相应的变量中。一旦满足条件，wait函数将会返回，释放互斥锁

2、对于每个IMU数据，获取时间戳并与图像数据的时间戳进行比较。

a.如果IMU数据的时间早于等于图像数据的时间，则将该IMU数据用于估计器的processIMU（）函数进行处理。

b.否则，根据时间戳进行插值处理，并将插值后的IMU数据传递给processIMU函数
3、如果启用了GNSS功能且存在GNSS数据，则将GNSS数据传递给估计器的processGNSS（）函数进行处理。

4、根据图像数据中的特征点信息，将其按照特征ID和相机ID进行存储，并调用估计器的processImage（）函数进行处理。

5、使用计时器t_s计算整个处理过程的耗时，并打印出估计器的统计信息

6、根据估计器的结果得到世界坐标系下的位姿，发布里程计信息、关键帧位姿信息、相机位姿信息、点云信息、TF变换信息和关键帧信息。

7、如果估计器的求解器标志为非线性，调用update函数进行状态更新

8、释放已锁定的互斥量，以允许其他线程继续操作。

9、进入下一次循环，处理下一组传感器数据。

processIMU()

1、判断是否是第一帧，若是则赋值acc_0、gyr_0，为构造预积分类pre_integrations提供初始值
2、假如没有，则创建一个新的IntegrationBase对象，并将acc_0、gyr_0、Bas[frame_count]和Bgs[frame_count]构造该帧图像预积分类
3、对于非第一帧的情况
 将IMU数据加入到当前预积分对象中，并将其加入到tmp_pre_integration中，将时间戳，线性加速度和角速度存储在相应的缓存中
4、根据公式进行状态估计

a.通过旋转矩阵将加速度和角速度从传感器坐标变换到世界坐标系

b.使用Utility::deltaQ函数将角速度转换为四元数增量，并将其转换为旋转矩阵R，更新Rs[j]值

c.根据公式计算出未校正加速度un_acc_0和un_acc_1，以及校正后的加速度un_acc

d.更新相机位置Ps[j]和速度Vs[j]

5、用当前时刻imu数据更新上一时刻imu数据acc_0、gyr_0,以便在处理下一帧IMU数据时使用
注意事项：
a：由于预积分是帧间约束，因此第1个预积分量实际上是用不到的
b：tmp_pre_integration在哪里new的？解答：在processImage()，因为第一帧没用，直接存的空指针，并且新new了一个tmp_pre_integration提供给第二帧使用，用作初始化

processImage()

打印新图像数据到达，并输出特征点数量

1、 添加特征点信息到f_manager中，并判断次新帧是否为关键帧，执行addFeatureCheckParallax()来添加特征点并检查视差
2、将图像数据和时间戳存储在ImageFrame对象中，将之前预积分对象tmp_pre_integration赋值给imageframe.pre_integration。将该ImageFrame对象插入到all_image_frame容器中，并更新tmp_pre_integration为新的预积分对象
3、外参初始化：如果ESTIMATE_EXTRINSIC为2，表示需要校准外部参数，非第一帧时，调用CalibrationExRotation（）函数进行外部旋转矫正。校准成功，将值设为1
4、初始化 || 非线性优化
A.初始化
1、假如条件满足，开始初始化，执行initialStructure()
2、初始化成功，非线性优化求解，执行solveOdometry()，根据边缘化策略滑动窗口，执行slideWindow()，移除无效地图点（没有深度信息），执行removeFailures()；否则直接滑动窗口
B.非线性优化

1、执行solveOdometry()视觉里程计的求解

2、进行故障检测failureDetection()

a.检测到故障，则执行清空状态、设置参数等操作，并返回

b.未检测到故障，执行滑动窗口、移除失败特征点等操作,更新关键位姿向量和最新的旋转矩阵和位置向量

initialStructure()

1、检查IMU数据，遍历所有的图像帧，得到加速度计观测值，通过累计计算，得到了平均加速度值aver_g，计算每个加速度计值与平均值之间的差异，累加得到方法var,计算方法的均方根作为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的位姿

枢纽帧是一种特殊类型的关键帧，它在纯视觉SFM中起到关键的作用。枢纽帧通常被选择为能够从不同角度观察到场景中大部分特征点的帧，以便于有效地确定特征点的三维位置，并用于初始化场景的结构和相机的位姿,作为SFM过程的起点。因此，枢纽帧在纯视觉SFM中被视为关键的帧。
3、视觉惯性对齐，执行visualInitialAlign()对视觉和IMU之间的对齐进行优化

visualInitialAlign()

1、调用VisualIMUAlignment()函数进行视觉和IMU对齐，并将所有图像帧、陀螺仪偏置和重力向量作为参数传入
2、对齐失败则返回；对齐成功则遍历所有图像帧，更新estimator中的Ps（c(k)到c(l)的位移）、Rs（b(k)到c(l)的旋转）
3、三角化特征点，深度是在每个点被观测到的第一帧相机坐标系下的（没有尺度），执行f_manager.triangulate()
4、考虑缩放因子s，重新积分预积分器pre_integration计算滑动窗口内的预积分
5、更新每一帧中的位置，通过缩放因子s对平移向量进行缩放，并根据旋转矩阵调整位置，转换Ps（在c(l)下）、Vs（在c(l)下）
6、初始化关键帧索引kv为-1，遍历所有图像帧，如果图像帧是关键帧，则增加关键帧索引kv，并将x中对应的旋转向量赋值给Vs[kv]，表示该关键帧的速度
7、对于特征点管理器中的每个特征点，根据s恢复空间点深度的尺度。
a：第一次求解R0，根据重力向量g和初始旋转矩阵Rs[0]计算初始旋转矩阵R0，为枢纽帧相机坐标系到世界坐标系的旋转矩阵并且c(l) 在w系下yaw=0
b：第二次求解R0，计算初始旋转矩阵与第一帧旋转矩阵Rs[0]之间的旋转差异rot_diff，更新Ps、Rs和Vs数组中每一帧的位置和速度，通过旋转差异rot_diff进行旋转变换，输出 更新后的重力向量g和初始旋转矩阵Rs[0]的欧拉角表示，为枢纽帧相机坐标系到世界坐标系的旋转矩阵并且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、两个判断，首先判断是否当前帧数量小于窗口大小，小于则返回不计算。

2、如果估计器的求解器是非线性，则执行：

调用了f_manager.triangulate()函数来进行三角化计算。该函数接收相机位姿Ps、相机旋转角度ric和时间戳tic，并输出三维特征点的坐标
3、非线性优化和边缘化，执行optimization()函数，使用Levenberg-Marquardt算法对视觉惯性里程计系统进行优化，以最小化重投影误差

4、判断是否启用了GNSS定位，如果启用了GNSS定位（GNSS_ENABLE），则进行GNSS和视觉里程计的融合，以提高定位的精度和鲁棒性。如果GNSS准备好（gnss_ready），则更新GNSS统计信息（updateGNSSStatistics()）
 

待续……