VIR-SLAM代码分析1——整体架构

论文名称：VIR-SLAM:visual,inertial,and ranging SLAM for single and multi-robot systems
代码地址：https://github.com/MISTLab/VIR-SLAM.git

系统框架：

（灰色部分是VINS-mono）

代码目录：

ROS节点：

（运行Euroc数据集时模拟UWB数据）

代码概述：

1、benchmark_publisher

benchmark_publisher_node.cpp
这个和VINS中的一致，发布数据集中参考值，作者写了一个 naive benchmark publisher，以帮助可视化ground truth，使用一种naive策略来将VINS与ground truth对齐，只是为了可视化，不适用于学术出版物的定量比较。

2、pg_standalone

（对应论文3.2.2 UWB ranging integration)

pg_standalone.cpp/.h
包含 一系列对UWB数据进行处理的函数，主要功能为：UWB测量数据的高斯平滑处理，UWB和位姿进行时间对齐，估计UWB锚点位置。
getPoseData()
getUWBData()
alignPosesUWB()
smoothRange()
findMatchedRange()
optimization()
initAnchorPos()

pg_standalone_node.cpp
订阅视觉里程计位姿和UWB的值，利用pg_standalone.cpp进行锚点估计与高斯平滑、时间对其等处理。
pose_callback()
uwb_callback()
process()
main()

initial_anchor.h
class intialAnchorFactor

GaussSmoothen.h
Another technique we use to reduce the error in this pre-processing stage is through smoothing. We apply Gaussian smoothing (Witkin1983) to all accepted measurements for the duration of the long window. For the gap in the measurement timeline,we interpolate the points based on the adjacent ends if they are close in time. The smoothed value corresponding to the key frame timestamp is used to create the UWB factors in the long window.
gauss()
gaussKernel()
gaussSmoothen()

test.cpp
用来测试高斯平滑的效果，函数内容与GaussSmoothen.h相同

3、utlitypkg

groundtruthodo.cpp
我们模拟了地面真实数据的UWB测距测量。静态锚点假设在机器人初始化时创建的框架的原点。我们加入高斯白噪声N ( 0、0 )来对我们的UWB传感器的误差进行建模。这个文件接受Euroc数据集的真值信息然后进行处理后模拟成 UWB的信息进行发布，从而得到模拟的UWB数据值。（模拟的UWB发布在/uwb/corrected_range，需要注意的是不同Euroc序列需要对代码进行修改）

4、uwb_pypkg

这个文件中的程序是针对真实环境中实验的UWB传感器进行的处理，对真实UWB传感器得到的值进行指数回归模型进行系统偏差纠正、异常值剔除技术，然后发布到pg_standalone进行高斯平滑等处理。
uwbdriver.py
UWB传感器驱动程序
uwb_node.py
利用训练好的模型进行系统偏差纠正
uwb_outlierRej.py
异常值处理

5、VIR_VINS

基于VINS-mono的结合UWB传感器的估计器，主要改动的文件在uwb_posegraph，vir_estimator中，其余部分和VINS中的一样，属于一个视觉惯性里程计。
camera_model
config
feature_tracker
pose_graph
support_files
以上文件程序与VINS-mono一致。

1、uwb_posegraph

从pose_graph修改而来，主要是关于UWB的位姿图优化的函数，这部分比较重要，后面核心程序解析中会详细介绍。
keyframe.cpp/.h关键帧函数
uwb_factor.h UWB残差与雅可比
uwb_posegraph_node.cpp UWB位姿图节点主函数
uwb_posegraph.cpp/.h UWB位姿图类

2、vir_estimator

状态估计器，包含VIR-SLAM的完整功能：初始化、位姿图优化等等，经过修改的代码是estimator_node.cpp、estimator.cpp/.h、parameters.cpp/.h、uwb_factor.h，后面核心程序会重点介绍。
factor
这个文件夹中新增了UWB_factor.h文件，包含了UWB残差与雅可比。
imu_factor.h：IMU残差、雅可比
intergration_base.h：IMU预积分
marginalization.cpp/.h：边缘化
pose_local_parameterization.cpp/.h：局部参数化
projection_factor.cpp/.h：视觉残差
uwb_factor.h：UWB残差与雅可比（新增加）

initial
与VINS-mono一致
initial_alignment.cpp/.h：视觉和IMU校准(陀螺仪偏置、尺度、重力加速度和速度)
initial_ex_rotation.cpp/.h：相机和IMU外参标定
initial_sfm.cpp/.h：纯视觉SFM、三角化、PNP
solve_5pts.cpp/.h：5点法求基本矩阵得到Rt

utility
与VINS-mono一致
CameraPoseVisualization.cpp/.h：相机位姿可视化
tic_toc.h：记录时间
utility.cpp/.h：各种四元数、矩阵转换
visualization.cpp/.h：各种数据发布

feature_manager.cpp/.h特征点管理，三角化，关键帧等

下面这三个文件相比于VINS-mono进行了修改
estimator_node.cpp ROS 节点函数，回调函数
estimator.cpp/.h紧耦合的VIO状态估计器实现
parameters.cpp/.h 处理前端中需要用到的一些参数