[人工智能]VINS-FUSION 研究日志 （2）主程序入口

技术博客被我写成碎碎念了

注：本系列博客（日志）是围绕小觅官方给的 MYNT-EYE-VINS-FUSION-Samples 代码
小觅的官方代码地址：https://gitee.com/mynt/MYNT-EYE-VINS-FUSION-Samples.git
自己fork的代码地址：https://gitee.com/tsuibeyond/MYNT-EYE-VINS-FUSION-Samples.git
自己fork 的代码将持续的改进与维护

VIO部分的代码都放在 vins_estimator 文件夹中

通过查看 CMakeLists.txt，发现编译生成的可执行文件 vins_d_node, vins_s_node, vins_node 都是基于 src/rosNodeTest.cpp文件，根据名字可以看出 vins_d_node 适用于小觅深度相机，vins_s_node 适用于小觅标准相机，vins_node 适用于Euroc数据

rosNodeTest.cpp 的main入口在最底部，main函数的流程如下：

1. 初始化 ros 相关的内容，设置 logger 的级别
2. 判断参数个数是否合理。argc是命令行总的参数个数，其中第0个参数是程序的全名，以后的参数命令行后面跟的用户输入的参数，从 .launch 文件中可以看出，该程序需要输入配置文件的路径，因此，需要 argc 应该等于2, （注意第一个参数其实就是可执行文件自己的名字）。但是在ROS程序中，.launch 文件已经将 config_file 作为参数放进服务器中了，可执行文件没有其他参数输入，因此在ROS中 argc 为1，且使用 n.getParam 读取配置文件的信息。本部分代码我们删除了 argc > 1 的判断，仅保留了 n.getParam的部分
3. 读取参数 readParameters(config_file) ， 函数定义在 parameters.cpp 中，使用 opencv 提供的 cv::FileStorage 对 yaml 格式的配置文件进行解析读取。首先判断文件是否能打开，这里我们增加了函数返回值，如果能够正常打开，正常进行，否则返回非零错误
   • USE_IMU 是否使用 IMU
   • 如果使用 IMU ，则继续读取加速度计和陀螺仪的噪声参数，还有重力常量
   • 读取 NUM_OF_CAM，判断使用几个相机，这里是两个
   • 读取 左右两个相机的 话题名称：IMAGE0_TOPIC IMAGE1_TOPIC
   • MAX_CNT ： 视觉追踪的最大数目
   • MIN_DIST : 两个特征之间的最小距离
   • F_THRESHOLD ： 随机采样一致性阈值 ransac threshold (pixel)
   • SHOW_TRACK ： 视觉追踪图像显示
   • FLOW_BACK ： 光流法的设置，是否使用 backward 来提高特征追踪的准确度
   • MULTIPLE_THREAD ： 是否使用多线程
   • SOLVER_TIME ： 最大求解时间
   • NUM_ITERATIONS ： 最大迭代次数
   • MIN_PARALLAX ： 用于判别关键帧的最小，其中 FOCAL_LENGTH 是写死的 460 （在 parameters.h中）
   • OUTPUT_FOLDER ： 运算结果保存路径
   • VINS_RESULT_PATH ： 运算结果保存路径中的名称，（OUTPUT_FOLDER + “/vio.txt”）
   • 先打开创建一下，方便后面持续进行 app，注意：std::ios_base::out: 打开文件进行写操作，即写入数据到文件。如果指定路径中并没有包含该文件，会创建一个新的.文件如果指定的路径中包含有该文件，那么打开后，会清空文件中内容，其实就是暗含了std::ios_base::truc
   • ESTIMATE_EXTRINSIC ： 配置对外参信息的先验知识的多少，有0,1,2三个选项。
   • CAM_NAMES ： 搞了一波字符串的操作，获取了 相机校正参数的配置文件，并把文件路径存放在向量 CAM_NAMES 中
   • RIC ：向量，存放左右相机的旋转外参
   • TIC : 向量，存在左右相机的平移外参
   • INIT_DEPTH ： 特征点的初始化深度参数 5.0
   • BIAS_ACC_THRESHOLD ：加速度计常值偏差的阈值 0.1
   • BIAS_GYR_THRESHOLD ：陀螺仪常值偏差的阈值 0.1
   • TD ： IMU 和 相机数据之间的延迟 0
   • ESTIMATE_TD ： 是否在线估计延迟 0，0 表示不估计
   • 额外判断一下，如果不使用 IMU，即USE_IMU == 0，则直接将 TD 和 ESTIMATE_TD 置零
   • ROW和COL ： 图像的长和宽
   • 全部参数读完了，释放掉 fsSettings.release()
   • 返回0 表示一切正常
4. 配置参数，紧接着是 estimator.setParameter(); 函数实现在 estimator.cpp 中
   • 根据 NUM_OF_CAM，分别配置 ric 和 tic。ric 和 tic 是 Estimator 类的成员变量， Matrix3d ric[2]; Vector3d tic[2];
   • f_manager.setRic(ric); 这里仅把 ric 旋转外参传递给 FeatureManager
   • 设置三个投影方差，这里写成了固定值。投影方差在解算VIO时起到权重分配的作用
   • g = G; 把重力向量传递进来
   • featureTracker.readIntrinsicParameter(CAM_NAMES); 把两个相机的内参传递给 featureTracker
   • 创建线程。原来的程序中考虑了多线程的判断，因为如果 linux 的程序，大部分是支持多线程的，所以这里直接把所有多线程的内容全部删除了
5. 判断 EIGEN_DONT_PARALLELIZE 是否使能，这个代码是基于CPU的，所以这个地方我们删掉了
6. registerPub(n); 的实现在 visualization.cpp 中，主要实现了话题的发布：
   • pub_latest_odometry = n.advertise<nav_msgs::Odometry>(“imu_propagate”, 1000);
   • pub_path = n.advertise<nav_msgs::Path>(“path”, 1000);
   • pub_odometry = n.advertise<nav_msgs::Odometry>(“odometry”, 1000);
   • pub_point_cloud = n.advertise<sensor_msgs::PointCloud>(“point_cloud”, 1000);
   • pub_margin_cloud = n.advertise<sensor_msgs::PointCloud>(“margin_cloud”, 1000);
   • pub_key_poses = n.advertise<visualization_msgs::Marker>(“key_poses”, 1000);
   • pub_camera_pose = n.advertise<nav_msgs::Odometry>(“camera_pose”, 1000);
   • pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>(“camera_pose_visual”, 1000);
   • pub_keyframe_pose = n.advertise<nav_msgs::Odometry>(“keyframe_pose”, 1000);
   • pub_keyframe_point = n.advertise<sensor_msgs::PointCloud>(“keyframe_point”, 1000);
   • pub_extrinsic = n.advertise<nav_msgs::Odometry>(“extrinsic”, 1000);
   • 然后是对相机可视化的操作，cameraposevisual.setScale(0.1); cameraposevisual.setLineWidth(0.01); 改变一下这两个参数，可以发现在 rviz 上，相机的模样发生了很大的变化，前者控制相机的大小，后者控制相机线条的粗细。cameraposevisual 属于 CameraPoseVisualization 类的实例，CameraPoseVisualization.h 和 CameraPoseVisualization.cpp 在文件夹 utility 中。
7. 然后订阅传感器的数据，分别是 imu img0 img1
8. 最后开启 一个 sync_process 线程

main 函数的内容如上琐事，下面分别对 imu_callback，img0_callback，img1_callback 和 sync_process 进行解析

imu_callback函数 用来订阅 imu 传感器的数据，内容非常简单，直接从 imu_msg 中获取 加速度测量值（dx,dy,dz） 和 陀螺仪测量值（rx,ry,rz）,并将其放在 Vector3d acc 和 gyr 中，连同时间 t 一起放入 estimator.inputIMU 中
inputIMU 函数中，首先用 mBuf 进行线程加锁，并把数据与时间t配对放进 accBuf 和 gyrBuf 中
accBuf 和 gyrBuf 是队列形式 queue,
fastPredictIMU 简单的在上一时刻的位姿上积分了一次
接着判断 solver_flag == NON_LINEAR？， 当前是否处于非线性求解滑动窗口阶段（还有 初始化阶段）
如果属于非线性求解阶段，则将广告计算得到的位置、姿态、速度和时间一起发布出去。
发布的话题是 “imu_propagate”，也就是上文提到registerPub中的第一个话题

接着是 img0_callback 和 img1_callback ，两个函数均只有三行代码
第一行和第三行是线程锁，中间是 将 img_msg 放入 img0_buf 和 img1_buf 中， 两个都是 队列 queue

restart_callback 这个回调函数好像没啥用，直接删掉了

最后只剩下 sync_process() 函数，sync_process线程 在main 函数中建立，内部是一个 while(1) 无限循环
STEREO 这个变量 是在读取配置文件的时候，当 NUM_OF_CAM == 2 时会被设置为1，程序中其他地方也会使用到这个标志位
另外，这个 if(STEREO == 1) 这个判定其实还有一些其他意义：因为程序对图像数据和IMU数据都使用了队列缓冲机制，但是在实际的应用中，在获取配置文件并读取参数时，程序的的回调函数其实已经开始往缓冲区（队列）里存放数据了，这就会造成明显的延时，如果计算机性能跟不上的话，会一直存在一个明显的滞后感。这一点在数据集的使用上没有明显区别，因为数据集的数据是提前录制好的，但是在使用真实相机时便会显现出来。

由于这个程序就是为双目设计的，所以我删除了 else 分支中的单目内容，并添加了清空队列的操作

else
{
    while (!img0_buf.empty()) img0_buf.pop();
    while (!img0_buf.empty()) img0_buf.pop();
}

此外，也删除了一个冗余的变量 std_msgs::Header header;
并修改了一个对 time 的赋值语句

getImageFromMsg 函数是把 ROS 的图像格式转化为 OpenCV 格式，并转为灰度图。
最终，转化好的图像连同时间传递进 estimator.inputImage 里面

不过 sync_process() 函数里面有个地方没有弄清楚

double time0 = img0_buf.front()->header.stamp.toSec();
double time1 = img1_buf.front()->header.stamp.toSec();

time0 和 time1都是浮点数，后面的程序通过判别 time0 和 time1 的大小来决定执行流程，对于浮点数进行大小比较显得不严谨，为了解决这一问题，改进了 time0 和 time1 的对比方式