1. 源由

之前对VINS-Fusion做了简单了解，详见：《Linux 35.5 + JetPack v5.1.3@VINS-Fusion编译安装》。

为了更好的从模块化视角了解该模块功能，我们需要通过类似灰盒的角度去看模块的数据流。

本章节将从这个角度，借助ROS系统的一些工具以及源代码来进行分析。

2. 思路

模块化分析思路相信大家都不陌生，说起来是很简单，但是在实际工作中的灵活应用作为研发工程师来说是基本功底（压箱底的功底，哈哈）。

目前比较盛行测试分类（个人认为也比较合理）如下所示：

• 黑盒测试
• 灰盒测试
• 白盒测试

通常情况，工程技术人员采用“黑盒”的方式进行工作；系统集成人员采用“灰盒”的方式进行工作；研发人员通过白盒的方式研发摸亏。

因此，根据上述逻辑，我们的初步定位是灰盒级别（可能还是比较浅的灰盒），但是这对于后续系统集成以及深入理解该模块具有重要意义。

3. 仿真分析

借助脚本和ROS系统的强大DEBUG功能，在实际执行以下模拟命令时：

• terminal 1 仿真环境

$ source ./devel/setup.sh
$ roslaunch vins vins_rviz_vins_node.launch

• terminal 2 逻辑算法

$ source ./devel/setup.sh
$ rosrun vins vins_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 

• terminal 3 仿真回放

$ source ./devel/setup.sh
$ rosbag play MH/MH_04_difficult.bag

3.1 rostopic 清单

根据启动脚本vins_rviz_vins_node.launch，具体内容详见附录。

以及rostopic list指令，可以清晰的看到整个环境目前所有的话题。

$ rostopic list
/clicked_point
/clock
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg
/tf
/tf_static
/imu0
/cam0/image_raw
/cam1/image_raw
/leica/position
/vins_cam_switch
/vins_imu_switch
/vins_restart
/feature_tracker/feature
/vins_estimator/camera_pose
/vins_estimator/camera_pose_visual
/vins_estimator/extrinsic
/vins_estimator/image_track
/vins_estimator/image_track/mouse_click
/vins_estimator/imu_propagate
/vins_estimator/key_poses
/vins_estimator/keyframe_point
/vins_estimator/keyframe_pose
/vins_estimator/margin_cloud
/vins_estimator/odometry
/vins_estimator/path
/vins_estimator/point_cloud

3.2 rosbag 输入

仿真数据在回放过程，将会向系统注入以下topic：

• /imu0
• /cam0/image_raw
• /cam1/image_raw
• /leica/position

$ rosbag info MH/MH_04_difficult.bag
path:        MH/MH_04_difficult.bag
version:     2.0
duration:    1:42s (102s)
start:       Jun 25 2014 03:28:47.33 (1403638127.33)
end:         Jun 25 2014 03:30:29.91 (1403638229.91)
size:        1.4 GB
messages:    25855
compression: none [1356/1356 chunks]
types:       geometry_msgs/PointStamped [c63aecb41bfdfd6b7e1fac37c7cbe7bf]
             sensor_msgs/Image          [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu            [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /cam0/image_raw    2033 msgs    : sensor_msgs/Image
             /cam1/image_raw    2032 msgs    : sensor_msgs/Image
             /imu0             20320 msgs    : sensor_msgs/Imu
             /leica/position    1470 msgs    : geometry_msgs/PointStamped

数据格式详见：

$ rosmsg show geometry_msgs/PointStamped
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

daniel@daniel-nvidia:~/ego-planner$ rosmsg show sensor_msgs/Imu
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Quaternion orientation
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance

$ rosmsg show geometry_msgs/PointStamped
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

$ rosmsg show sensor_msgs/Image
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data

4. 代码分析

4.1 启动入口

算法逻辑代码启动过程如下：

rosrun vins vins_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
 └──> vins/vins_node/vins_estimator
     └──> VINS-Fusion/vins_estimator/src/rosNodeTest.cpp (main)

4.2 配置文件

配置文件中含有各类数据，包括输入的topic路径：

VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
 ├──> "/imu0"
 ├──> "/cam0/image_raw"
 ├──> "/cam1/image_raw"
 ├──> "cam0_mei.yaml"
 ├──> "cam1_mei.yaml"
 └──> "~/output/pose_graph/"

4.3 业务数据流

vins/vins_node/vins_estimator
                                            +---------------------------+  
IN.1 "/imu0"                       --->     |   imu_callback     -------|--+
IN.2 "/feature_tracker/feature"    --->     |   feature_callback -------|--|--+
IN.3 "/cam0/image_raw"             --->     |   img0_callback    ----+  |  |  |
IN.4 "/cam1/image_raw"             --->     |   img1_callback    ----+  |  |  |
IN.5 "/vins_restart"               --->     |   restart_callback     |  |  |  |
IN.6 "/vins_imu_switch"            --->     |   imu_switch_callback  |  |  |  |
IN.7 "/vins_cam_switch"            --->     |   cam_switch_callback  |  |  |  |
                                            |   sync_process   <-----+  |  |  |
                                            |       |                   |  |  |
                                            |       +-------------------|--|--|--+
                                            +---------------------------+  |  |  |
                                                                           |  |  |
OUT.1 nav_msgs::Odometry "imu_propagate"     <-----------------------------+  |  |
                                                                              |  |
OUT      <--------------------------------------------------------------------+  |
 ├──> pubOdometry                                                                |
 │    ├──> OUT.2 nav_msgs::Path> "path"                                          |
 │    ├──> OUT.3 nav_msgs::Odometry "odometry"                                   |
 │    └──> "~/output/vio.csv"                                         |
 ├──> pubKeyPoses                                                                |
 │    └──> OUT.6 visualization_msgs::Marker "key_poses"                          |
 ├──> pubCameraPose                                                              |
 │    ├──> OUT.7 nav_msgs::Odometry "camera_pose"                                |
 │    └──> OUT.8 visualization_msgs::MarkerArray "camera_pose_visual"            |
 ├──> pubPointCloud                                                              |
 │    ├──> OUT.4 sensor_msgs::PointCloud "point_cloud"                           |
 │    └──> OUT.5 sensor_msgs::PointCloud "margin_cloud"                          |
 ├──> pubKeyframe                                                                |
 │    ├──> OUT.9 nav_msgs::Odometry "keyframe_pose"                              |
 │    └──> OUT.10 sensor_msgs::PointCloud "keyframe_point"                       |
 └──> pubTF                                                                      |
      └──> OUT.11 nav_msgs::Odometry "extrinsic"                                 |
                                                                                 |
OUT.12 sensor_msgs::Image "image_track"      <-----------------------------------+

注：这里引出一个话题就是关于精度的问题： How to check precision of the VINS-Fusion? #253 ，有兴趣的朋友可以参与讨论。

5. 参考资料

【1】ego-planner开源代码之数据流分析

6 配置文件（整理）

• vins_rviz_vins_node.launch

<launch>
    <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find vins)/../config/vins_rviz_config_lida2003.rviz" />
</launch>

• vins_rviz_config_vins_node.rviz

Panels:
  - Class: rviz/Displays
    Help Height: 0
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /VIOGroup1
        - /GlobalGroup1/CarModel1
      Splitter Ratio: 0.4651159942150116
    Tree Height: 364
  - Class: rviz/Selection
    Name: Selection
  - Class: rviz/Tool Properties
    Expanded:
      - /2D Pose Estimate1
      - /2D Nav Goal1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
  - Class: rviz/Time
    Name: Time
    SyncMode: 0
    SyncSource: track_image
  - Class: rviz/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded: ~
      Splitter Ratio: 0.5
    Tree Height: 363
Preferences:
  PromptSaveOnExit: true
Toolbars:
  toolButtonStyle: 2
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Class: rviz/Axes
      Enabled: true
      Length: 0.5
      Name: Axes
      Radius: 0.05000000074505806
      Reference Frame: <Fixed Frame>
      Show Trail: false
      Value: true
    - Class: rviz/Group
      Displays:
        - Alpha: 1
          Buffer Length: 1
          Class: rviz/Path
          Color: 25; 255; 0
          Enabled: true
          Head Diameter: 0.30000001192092896
          Head Length: 0.20000000298023224
          Length: 0.30000001192092896
          Line Style: Lines
          Line Width: 0.029999999329447746
          Name: VIOPath
          Offset:
            X: 0
            Y: 0
            Z: 0
          Pose Color: 255; 85; 255
          Pose Style: None
          Queue Size: 10
          Radius: 0.029999999329447746
          Shaft Diameter: 0.10000000149011612
          Shaft Length: 0.10000000149011612
          Topic: /vins_estimator/path
          Unreliable: false
          Value: true
        - Class: rviz/MarkerArray
          Enabled: true
          Marker Topic: /vins_estimator/camera_pose_visual
          Name: CameraMarker
          Namespaces:
            {}
          Queue Size: 100
          Value: true
        - Alpha: 1
          Autocompute Intensity Bounds: true
          Autocompute Value Bounds:
            Max Value: 10
            Min Value: -10
            Value: true
          Axis: Z
          Channel Name: intensity
          Class: rviz/PointCloud
          Color: 255; 255; 255
          Color Transformer: Intensity
          Decay Time: 0
          Enabled: true
          Invert Rainbow: false
          Max Color: 255; 255; 255
          Min Color: 0; 0; 0
          Name: PointCloud
          Position Transformer: XYZ
          Queue Size: 10
          Selectable: true
          Size (Pixels): 2
          Size (m): 0.009999999776482582
          Style: Points
          Topic: /vins_estimator/point_cloud
          Unreliable: false
          Use Fixed Frame: true
          Use rainbow: true
          Value: true
        - Alpha: 1
          Autocompute Intensity Bounds: true
          Autocompute Value Bounds:
            Max Value: 10
            Min Value: -10
            Value: true
          Axis: Z
          Channel Name: intensity
          Class: rviz/PointCloud
          Color: 0; 255; 0
          Color Transformer: FlatColor
          Decay Time: 100
          Enabled: true
          Invert Rainbow: false
          Max Color: 255; 255; 255
          Min Color: 0; 0; 0
          Name: HistoryPointCloud
          Position Transformer: XYZ
          Queue Size: 10
          Selectable: true
          Size (Pixels): 1
          Size (m): 0.009999999776482582
          Style: Points
          Topic: /vins_estimator/margin_cloud
          Unreliable: false
          Use Fixed Frame: true
          Use rainbow: true
          Value: true
        - Class: rviz/Image
          Enabled: true
          Image Topic: /vins_estimator/image_track
          Max Value: 1
          Median window: 5
          Min Value: 0
          Name: track_image
          Normalize Range: true
          Queue Size: 2
          Transport Hint: raw
          Unreliable: false
          Value: true
      Enabled: true
      Name: VIOGroup
  Enabled: true
  Global Options:
    Background Color: 0; 0; 0
    Default Light: true
    Fixed Frame: world
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz/MoveCamera
    - Class: rviz/Select
    - Class: rviz/FocusCamera
    - Class: rviz/Measure
    - Class: rviz/SetInitialPose
      Theta std deviation: 0.2617993950843811
      Topic: /initialpose
      X std deviation: 0.5
      Y std deviation: 0.5
    - Class: rviz/SetGoal
      Topic: /move_base_simple/goal
    - Class: rviz/PublishPoint
      Single click: true
      Topic: /clicked_point
  Value: true
  Views:
    Current:
      Class: rviz/ThirdPersonFollower
      Distance: 32.13248825073242
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Field of View: 0.7853981852531433
      Focal Point:
        X: -5.432063579559326
        Y: -4.7521138191223145
        Z: 0.38143399357795715
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.6947961449623108
      Target Frame: world
      Yaw: 3.1406219005584717
    Saved: ~
Window Geometry:
  Displays:
    collapsed: false
  Height: 1016
  Hide Left Dock: false
  Hide Right Dock: true
  QMainWindow State: 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
  Selection:
    collapsed: false
  Time:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: true
  Width: 1848
  X: 72
  Y: 27
  loop_match_Image:
    collapsed: false
  track_image:
    collapsed: false