VINS-MONO 状态估计器estimator代码解读

主程序入口在estimator_node.cpp的main函数中：

1、ros::init(argc, argv, "vins_estimator")；

2、readParameters(n);读取参数

3、估计器设置参数estimator.setParameter()

包括相机IMU外参tic和ric

4、注册visualization.cpp中创建的发布器registerPub(n)

5、订阅topic执行各回调函数

imu_callback 将imu_msg保存到imu_buf	* 1、将IMU数据imu_msg存入IMU数据缓存队列imu_buf  * 2、完成将img_msg放入imu_buf的操作后唤醒作用于process线程中的获取观测值数据的函数getMeasurements()：读取缓存imu_buf和feature_buf中的观测数据  * 3、predict(imu_msg)通过IMU测量值imu_msg和上一个PVQ递推得到下一时刻的temp_Q,temp_P（使用中值积分）  * 4、pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header)发布里程计信息
feature_callback 将feature_msg放到feature_buf	* 1、将图像特征点数据feature_msg存入图像特征点数据缓存队列feature_buf  * 2、唤醒getMeasurements()读取缓存imu_buf和feature_buf中的观测数据
restart_callback 收到restart时清空feature_buf和imu_buf,restart估计器和时间
relocalization_callback 根据回环检测信息进行重定位

6、创建主线程函数process()，在process()中处理VIO后端，包括IMU预积分、松耦合初始化和local BA

con.wait(lk) 等待数据接收	getMeasurements()成功获取数据后，measurements.size()不为0，继续线程；未被con.notify_one()唤醒时，一直等待，阻塞当前线程
遍历measurements，对imu和图像数据进行组合操作	* 2、对两帧图片之间的IMU数据，进行预积分（测量值累计），得到两帧（！！！累计两帧之间的IMU数据）之间的预积分Estimator::processIMU  *2.1、若图像时间戳晚于或等于IMU数据的时间戳，直接调用Estimator::processIMU计算预积分Rs、Ps、Vs  *2.2、若IMU数据的时间戳晚于图像时间戳，则在上一次IMU测量current_time与当前IMU测量t之间img_t位置进行一次插值，再调用Estimator::processIMU计算预积分
设置重定位用的回环帧	* 3.1、取出最后一个重定位帧  * 3.2、将回环帧对应的点放进match_points中  * 3.3、获取重定位帧的平移向量T（x,y,z）、旋转四元数w,x,y,z、索引值  * 3.4、estimator.setReloFrame设置回环帧
将图像特征点数据img_msg->points存到一个map容器image中	key是feature_id，value是vector>>  (camera_id,[x,y,z,p_u,p_v,velocity_x,velocity_y])
estimator.processImage视觉与IMU的初始化以及非线性优化的紧耦合
每处理完一帧图像特征点数据，都要发布这些话题（里程计、关键位姿、相机位姿、点云、TF关系）pubOdometry。。。
update() VINS系统完成滑动窗口优化后，用优化后的结果，更新里程计数据

getMeasurements()：对imu和图像数据进行对齐并组合

* 1、while(true):直到把imu_buf或者feature_buf中的数据全部取出，才会退出while循环！！！一直读取  * 1.1、对齐标准：IMU最后一个数据的时间要大于第一个图像特征数据的时间，否则需要等待接收IMU数据  * 1.2、对齐标准：IMU第一个数据的时间要小于第一个图像特征数据的时间  * 1.3、每次读取feature_buf中一个数据img_msg，将该帧和上一帧图像特征点数据之间的时间间隔内所有的IMU数据，以及时间戳晚于当前帧图像的第一帧IMU数据放入IMUs  * 1.4、measurements.emplace_back(IMUs, img_msg);   将对应IMU数据IMUs和图像特征img_msg放入measurement

Estimator::processImage

1、addFeatureCheckParallax确定关键帧策略	* 1、把当前帧图像（frame_count）的特征点添加到feature容器中  * 2、若滑窗中只有一帧或者当前帧（第1最新帧）跟踪到的特征点数量小于20，则MARGIN_OLD  * 3、计算第2最新帧和第3最新帧之间跟踪到的特征点的平均视差，若平均视差大于设定的阈值，则把第2最新帧当作关键帧MARGIN_OLD
2、将图像特征数据，时间，临时预积分值存在图像帧类ImageFrame中，并放入all_image_frame
3、相机与IMU外参的在线标定	*3.1、FeatureManager::getCorresponding找出最新两帧之间共视特征点的对应points 在两帧frame_count_l和frame_count_r之间都可观察到的FeaturePerId，找出对应的point[x,y,z,u,x,uv,vv,td]，将对应点放入corres中 *3.2、InitialEXRotation::CalibrationExRotation计算IMU和相机之间的外参  *1、根据对应点计算两帧之间的R矩阵solveRelativeR(corres)  *2、对于滑窗中的图像帧，填写加权A矩阵，包括多个数据Q（0,1）Q（1,2）.。。。Q（frame_count - 1, frame_count），构建最小二乘AX = 0  *3、svd分解得到u\v，取出V的最后一列，即为最小二乘的解，获得旋转外参ric  *4、检查该svd分解得到的解是否合理
4、若solver_flag == INITIAL，表示需要初始化，进行初始化操作	* 4.1、frame_count == WINDOW_SIZE滑动窗口中塞满了才进行初始化 * 4.2、initialStructure()  vins系统初始化 * 4.3、初始化操作成功后，紧耦合优化solveOdometry()，对窗口进行滑动slideWindow()，去除滑出了滑动窗口的特征点removeFailures() solveOdometry()  * 1、对特征点进行三角化求深度（SVD分解）  * 2、Estimator::optimization()滑动窗口紧耦合优化，添加优化变量，添加残差，ceres优化，接着进行边缘化
5、若已经成功初始化，进行正常的VIO紧耦合优化

Estimator::initialStructure():vins系统初始化

1、通过加速度variance确保IMU有足够的excitation
2、global sfm	* 2.1、收集用于sfm的图像特征点数据,将f_manager中所有FeaturePerId存放到sfm_f中，特征点的初始状态state为：未被三角化      * 2.2、Estimator::relativePose在滑动窗口中，寻找与最新帧有足够多数量的特征点对应关系和视差的帧作为选定帧L，然后用5点法恢复相对位姿（L帧与最新帧），如果找不到，则初始化失败，返回；知道有5点法恢复的才继续 Estimator::relativePose 在滑动窗口中，寻找与最新帧有足够多数量的特征点对应关系和视差的帧作为选定帧，然后用5点法MotionEstimator::solveRelativeRT恢复相对位姿 MotionEstimator::solveRelativeRT:5点法恢复位姿  * 1、cv::findFundamentalMat由2D-2D对应点得出E矩阵  * 2、cv::recoverPose通过本质矩阵得到Rt，返回通过手性校验的内点个数，inlier_cnt > 12则5点法恢复成功      * 2.3、GlobalSFM::construct初始化滑动窗口中全部初始帧的相机位姿（相对选定帧L）和特征点空间3D位置，若global SFM failed，则marginalization_flag = MARGIN_OLD; GlobalSFM::construct  * 1、以第L帧为参考，第L帧的姿态设置为一个没有任何旋转的实单位四元数，向量设置为[0, 0, 0]  * 2、滑动窗口中从第l帧相机坐标系到最新帧相机坐标系的旋转和平移，记录在Pose中  * 3、恢复位姿和特征点  * 4、对于sfm_f中没有被三角化的点，进行三角化GlobalSFM::triangulatePoint  * 5、对滑动窗口中的所有帧的位姿和3D特征点使用ceres进行BA优化（固定第L帧的位姿），得到优化后各帧到第L帧的位姿以及3D坐标点sfm_tracked_points 恢复位姿和特征点      * 3.1、根据第l和frame_num - 1帧的R,T，三角化第L帧（参考帧）、frame_num-1当前帧的特征点      * 3.2、先通过pnp计算第i帧（i = L + 1, ..., frame_num - 2）的位姿（每一帧到到l帧的变换矩阵，R_initial，P_initial，保存在Pose中）      * 3.3、再调用triangulateTwoFrames()，与第frame_num - 1帧进行匹配，三角化      * 3.4、遍历（L + 1）帧到第（frame_num - 2）帧，寻找与第L帧的匹配，三角化更多的地图点      * 3.5、从第L - 1帧到第0帧，先通过pnp计算第i帧（i = L - 1, ..., 0）的位姿，再调用triangulateTwoFrames()，与第L帧进行匹配，三角化一些特征点      * 2.4、对于滑窗内的帧，从图像帧坐标转到帧l的IMU坐标系      * 2.5、对于非滑动窗口的所有帧，提供一个初始的R,T，然后solve pnp求解pose；pnp求解成功，并(通过标定的外参）转换为IMU坐标系的位姿
3、camera与IMU对齐visualInitialAlign()

Estimator::visualInitialAlign()：camera与IMU对齐

1、调用VisualIMUAlignment函数，实现陀螺仪的偏置校准，计算速度，重力，尺度	* 1、solveGyroscopeBias估测陀螺仪的Bias，对应论文V-B-1 solveGyroscopeBias  * 1、通过b_k,b_k+1视觉SFM的结果和预积分测量的偏差构建最小二乘AX = b,用LDLT求解delta_bg  * 2、对于滑窗中的图像帧，更新biasBgs[i] += delta_bg;  * 3、更新了bias之后，repropagate[all_image_frame从第一帧开始重新用中值积分递推下一时刻预积分，同时更新雅可比和协防差]  * 2、LinearAlignment求解V 重力向量g和 尺度s LinearAlignment  * 1、初始化所有帧all_image_fram的 速度V[0:n] Gravity Vectorg,尺度s -> 对应论文的V-B-2  * 2、重力修正RefineGravity -> 对应论文的V-B-3  * 3、重力方向跟世界坐标的Z轴对齐
2、获取滑窗中所有图像帧在世界坐标系下的Ps\Rs\Vs，并将其设置为关键帧
3、重新计算所有特征点的逆深度FeatureManager::triangulate
4、bias改变后，repropagate重新计算预积分
5、通过重力旋转到z轴上，得到世界坐标系与图像坐标系c0之间的旋转矩阵rot_diff，所有变量从参考坐标系c0旋转到世界坐标系w

Estimator::optimization()滑动窗口紧耦合优化

step1:添加待优化变量	(para_Pose， 7DOF(x,y,z,qx,qy,qz,qw)、para_SpeedBias,   9Dof(vx,vy,vz,bas_x,bas_y,bas_z,bgs_x,bgs_y,bgs_z)、para_Ex_Pose、para_Td）
step2:添加残差	* 2.1、添加边缘化残差（last_marginalization_info）  * 2.2、添加IMU残差,IMUFactor类的Evaluate中包含残差计算及雅可比计算  * 2.3、添加视觉残差
step3:ceres求解
step4:进行边缘化	* 情况一：marg最老帧，及其看到的路标点和相关联的IMU数据     * 1、先将上次先验残差项传递给marginalization_info,并从中去除需要丢弃的状态量     * 第一步：定义损失函数marginalizationFactor（将上一次先验项的残差（n）的变量维度marginalization_info->keep_block_size传递给mutable_parameter_block_sizes并设置num_residuals_）     * 第二步：定义ResidualBlockInfo     * 第三步：调用addResidualBlockInfo将各个残差以及残差涉及的优化变量添加到marginalization_info     * 2、将第0帧和第一帧间的预积分观测IMUFactor(pre_integrations[1])，添加到marginalization_info     * 第一步：定义代价函数IMUFactor     * 第二步：定义ResidualBlockInfo，优化变量为para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]，都marg掉     * 第三步：调用addResidualBlockInfo将各个残差以及残差涉及的优化变量添加到marginalization_info     * 3、将第一次观测为第0帧的所有路标点对应的视觉观测，添加到marginalization_info中     * 第一步：定义代价函数ProjectionTdFactor（得到velocity_i，velocity_j，针对UNIT_SPHERE_ERROR，计算tangent_base）     * 第二步：定义残差块ResidualBlockInfo（parameter_blocks为vector{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]     * 第三步：向容器factors内添加观测数据ResidualBlockInfo,确定优化变量内存地址及其localSize _parameter_block_size,将待marg的变量parameter_block_idx     * 4、marginalization_info->preMarginalize()计算每个残差对应的雅可比，并更新parameter_block_data  * 对每个观测数据ResidualBlockInfo，调用Evaluate()  * ProjectionTdFactor::Evaluate:计算视觉残差及雅可比  * IMUFactor::Evaluate:计算IMU预积分残差及雅可比     * 5、marginalization_info->marginalize()多线程构造先验舒尔补AX=b的结构，在x0处线性化计算雅可比和残差  * 1、统计待marg变量总localSize-m, 优化变量的总localSize-pos,保留变量的总localSize-n  * 2、计算A，b（根据观测对应的参数块parameter_blocks，填写A，b）  * 3、舒尔补操作得到先验矩阵  * 4、分解得到雅可比和残差值linearized_jacobians、linearized_residuals     * 6、调整参数块在下一次窗口中对应的位置，并移交优化项需要得到的两个变量：last_marginalization_info和last_marginalization_parameter_blocks