VINS-mono代码笔记

feature_tracker_node.cpp:
一、通过roslaunch文件的参数服务器获得配置参数
二、获得相机的内参
三、订阅图像，img_callback：
1、第一帧图像只记录时间戳
2、与之前时间戳比较一下，判断是否要发布当前帧，避免高频率发送（PUB_THIS_FRAME），不管发不发布，都要做光流跟踪，这样跟踪间隔时间才短，跟踪才稳定
3、根据是否多目相机，把图像分割出来
4、根据曝光配置来决定是否对图像直方图均衡化
5、prev_img（上上帧）、cur_img（上帧）、 forw_img（当前帧）都有图后才进行光流追踪
6、如果上一帧有特征点，则对上一帧和当前帧进行光流追踪，在当前帧找到上一帧对应的特征点（https://blog.csdn.net/ergevv/article/details/137981026?spm=1001.2014.3001.5502）
7、剔除新特征点超出当前帧位置的所有对应数据
8、记录被跟踪的次数track_cnt，对仍然存在的特征点数量加1
9、通过对级约束来剔除异常点：
（1）将特征点从像素坐标转到归一化坐标，先将像素投影到归一化相机坐标系（未去畸变），
x_image = fx * x/z + cx（fx为焦距*缩放率，cx为宽度/2）。基于相机的畸变程度离中心越远，畸变越大的原理，将畸变点代入畸变模型计算出该点的畸变程度，然后畸变点A再减去该值，得到新畸变点，通过新畸变点得到畸变程度，再由畸变点A再减去该值，如此迭代几轮后完成去畸变，因为迭代过程，畸变程度越来越小，点会越来越接近真值。
（2）将归一化坐标点转到设定好的图像坐标系，这样好处就是无论是什么相机坐标系都可以得到一样的图像坐标系，这样进行对极约束得到基础矩阵时，内外点的判断可以使用同一个参数
（3）将当前帧和上一帧去畸变后，利用cv::findFundamentalMat进行对极约束得到基础矩阵，将是外点的特征点去除。（对极约束原理：P、p1、p2、O1、O2五点共面。cv::findFundamentalMat输入归一化或者图像坐标都行，因为基础矩阵包含K，不同的坐标系基础矩阵值不一样而已）
10、特征点均匀化：如果是鱼眼相机，去除边缘的点，保留特征点被跟踪数量较多的点，其旁边的特征点去除
11、如果目前剩余的特征点少于设定的阈值，则对当前帧提取角点，补充数量到forw_pts，id为-1，跟踪次数为1。这也是一开始特征点的由来。
12、帧间图像和特征点传递
13、去除当前帧的畸变，放到cur_un_pts，id+归一化坐标放到cur_un_pts_map
14、对id不为-1的特征点，计算其当前帧与上一帧的去畸变后归一化坐标下的速度pts_velocity
15、更新特征点id，每个特征点id都是不一样的，每次加1
16、发布数据：将归一化坐标、速度、id、像素坐标发送到estimator_node.cpp（"/feature_tracker/feature"）
17、可视化数据

estimator_node.cpp
feature_callback：
1、接收feature_tracker_node.cpp计算得到的特征点数据（"/feature_tracker/feature"）
2、将特征点数据放入feature_buf
3、唤醒process检查数据

imu_callback：
1、接收imu数据
2、将imu数据放入imu_buf
3、唤醒process检查数据

relocalization_callback：
1、
2、

线程process：
1、等待被唤醒，检查数据是否合适，保证特征点前后皆有imu，返回数据个数需要不为0（estimator.td这是预测的时延）
（1）feature_buf和imu_buf不为空
（2）imu_buf最后一个值时间要大于特征点第一个值
（3）imu_buf第一个值时间要小于特征点第一个值
2、将小于特征点时间的所有imu和一个时间大于等于特征点放入IMUs
3、将IMUs和特征点打包到measurements
4、遍历IMUs，第一次进去，frame_count为0，只是定义一个预积分器，所以frame_count为0时没有用。滑窗里的每一帧都建了一个预积分器，初始化各个变量，记录了一开始的角速度和线加速度，方便中值积分：
（1）midPointIntegration函数计算：
1）将上一次线加速度转到相机坐标，un_acc_0 = delta_q * (_acc_0 - linearized_ba);
2）计算角速度中值，un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
3）积分角速度，delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);这里使用了近似计算，假设角速度接近0，从而将sinx转为x
4）将当前线加速度转到相机坐标，un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
5）计算线加速度中值，un_acc = 0.5 * (un_acc_0 + un_acc_1);
6）积分位移，result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
7）积分速度，result_delta_v = delta_v + un_acc * _dt;
8）零偏不变，result_linearized_ba = linearized_ba; result_linearized_bg = linearized_bg;       
9）X_k+1 = F * X_k + V * noise (noise为一开始定义，后续不更新)，计算转移矩阵F，以及噪声矩阵V，
10）计算残差对变量的导数，即雅可比矩阵，jacobian = F * jacobian;jacobian初始值为单位矩阵，因为一开始时，各变量只与自身有关
11）预测估计协方差矩阵，covariance = F * covariance * F.transpose() + V * noise * V.transpose(); 这个矩阵的逆用于后续作为残差的信息矩阵
（2）更新预积分值与累计时间
（3）tmp_pre_integration记录每一帧之间的预积分，而不是只记录关键帧之间的预积分，另一个积份就是只记录关键帧之间的
（4）记录每一次预积分的值Rs、Ps、Vs
5、取出回环帧处理
6、channels数据顺序：id、像素坐标x、y、去畸变的归一化坐标下的速度x、y，point为去畸变的归一化相机坐标系，提取出特征点数据到image
7、特征点feature添加（feature是f_manager的变量）：
（1）如果是新特征点，则新创建一个特征点id，这里的feature_id是id，frame_count就是该特征点在滑窗中的当前位置，作为这个特征点的起始位置。在feature_per_frame记录特征点信息，坐标、速度
（2）如果已经存在，则直接在feature_per_frame后叠加特征点信息
8、关键帧判断：
（1）前两帧为关键帧
（2）特征点数量少于20为关键帧
（3）计算归一化坐标系下，对应特征点在相邻帧下的平均位移（视差），需要大于设定的阈值
9、将特征点、时间、和预积分得到的帧间约束记录到all_image_frame
10、tmp_pre_integration清零
11、外参初始化：
（1）ESTIMATE_EXTRINSIC为2时，没有先验外参，需要自己计算；为1时，表示有可信的外参，但是仍需要优化，为0时则是精确的，不需要优化。
（2）获取当前帧和上一帧的所有归一化坐标对
（3）使用cv::findFundamentalMat计算基础矩阵，因为是归一化坐标，K为单位矩阵，因此这个求的也是本质矩阵E = t * R
（4）根据本质矩阵求解R:https://www.cnblogs.com/weihao-ysgs/p/epipolar-constriant-nature.html
（5）使用预积分和R求解外参：https://blog.csdn.net/ergevv/article/details/139201880?spm=1001.2014.3001.5501
（6）求解一次后，外参已经具有可信度，将ESTIMATE_EXTRINSIC设为1
12、初始化：
（1）一开始solver_flag 为 INITIAL
（2）累积关键帧，如果不够WINDOW_SIZE帧，则frame_count++，直到关键帧足够，则开始初始化。
（3）计算相邻帧的线加速度方差，若方差较小，则代表运动状态变化较小，不宜初始化
（4）遍历特征点，将各帧下的归一化坐标放到一起再放入sfm_f
（5）从滑窗的第一帧开始计算与最后一帧的共视特征点，需要大于20个。计算对应特征点在不同帧之间坐标的平均距离（视差），需要大于设定阈值。然后根据两帧匹配对求解E矩阵，再调用recoverPose通过E矩阵与R、t的关系求解R和带尺度的t（非真实的），记录枢纽帧为l
（6）以枢纽帧为世界系原点，则滑窗最后一帧位姿为（5）所求的R和t
（7）通过sfm求出每一帧的位姿，通过三角化（求深度）、PnP（求位姿）计算出所有特征点的坐标，再通过BA优化
（8）零偏求解：利用预积分和视觉求得的相对位姿对零偏约束列出方程，使用Cholesky求解
（9）惯性对齐：求解imu坐标系下速度、世界坐标下g、s
（10）使用（9）求得的g作为方向向量，模为设定好的固定值。类似第（9）步再优化g
（11）将（7）求得每一帧的位姿用来更新关键帧位姿
（12）将特征点深度estimated_depth设为-1，设置之前计算出来的旋转外参
（13）使用三角化计算特征点在被观测的第一帧带尺度深度
（14）使用计算的零偏重新积分预积分（这里不使用雅可比修正，第一次积分误差较大）
（15）目前原点是在枢纽帧下，将所有数据转到第0帧关键帧下（在优化过程中，因为重力的参与，pitch和roll已经被修正，但是yaw不可观，求出重力的时候，只需要更新yaw即可）
13、
（1）设solver_flag 为NON_LINEAR

后端优化：
1、预积分约束：之前计算的相对变换和预积分之间的残差
2、重投影约束：将第i帧下的特征归一化的坐标投影到第j帧得到归一化坐标与光流追踪得到的归一化坐标的差作为残差

边缘化：
1、求出第0帧和第一帧的雅可比矩阵和残差，约束关系和后端优化是一致的

边缘化作用，去除了旧帧之后得到新的约束关系，然后重新求解得到J^T*J在后端优化时仍然使用

回环检测：重投影约束

改进：
1、使用因子图代替滑窗边缘化