Airsim中运行OpenVINS和VINS_Fusion

1. 简介

本文简介在Airsim中运行OpenVINS和VINS_Fusion.

2. 参考

1. 无人飞行器智能感知技术竞赛 赛事公告
2. 在AirSim中控制无人机平台并采集数据
3. OpenVINS 官网
4. AirSim中运行VIO算法(VINS-Mono)

3. 步骤

3.1 编译

1. 模拟器和Airsim_ros_wrapper下载安装
   参考该工程的README.md文件，本文按照“LINUX 单机开发模式”进行配置。
   （1）模拟器下载（simulator_LINUX.zip）
   在浏览器中输入此网址即可开始下载:
   https://stg-robomasters-hz-q0o2.oss-cn-hangzhou.aliyuncs.com/simulator/simulator_LINUX.zip
   （2）airsim_ros_wrapper下载

git clone https://github.com/RoboMaster/IntelligentUAVChampionshipSimulator.git

2. OpenVINS 下载安装
   参照 OpenVINS的说明文档，安装OpenVINS.

3. VINS_Fusion下载安装
   参照VINS-Fusion的README.md, 安装VINS-Fusion.

4. 键盘控制脚本
   这个博客中提供了一个“无人机控制“的python脚本。复制该脚本的内容，保存为本地文件"airsim_keyboard.py"。

3.2 运行

1. 启动Airsim仿真器 ( simulator_LINUX.zip )

python launcher.py

输入本机IP地址， 127.0.0.1； 启动成功之后，按键“3”， 切到 3.自主飞行-双目 模式。
2. 启动Airsim_ros_wrapper

cd  ${YourPath}/IntelligentUAVChampionshipSimulator/roswrapper
source ros/devel/setup.bash
./simulator.sh  127.0.0.1

用于将收到的Airsim中的数据通过ROS发布出来。
3. 启动OpenVINS

cd ${YourPath}/openvins_ws
source devel/setup.bash
roslaunch ov_msckf  airsim.launch

打开另一个终端，rviz显示openvins运行情况：

rviz  -d  '${YourPath}/openvins_ws/src/open_vins/ov_msckf/launch/display.rviz'

4. 启动VINS-Fusion

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/airsim/airsim_stereo_imu_config.yaml 

5. 启动键盘控制脚本

python airsim_keyboard.py

6. 同时录制bag数据
   录制的rosbag中，包含双目图像和IMU数据，以及无人机的轨迹真值数据。

rosbag  record   /airsim_node/drone_1/front_left/Scene  /airsim_node/drone_1/front_right/Scene   /airsim_node/drone_1/imu/imu    /airsim_node/drone_1/odom_local_ned

7. 录制完rosbag之后，可以参考 “https://github.com/ethz-asl/kalibr/blob/master/aslam_offline_calibration/kalibr/python/kalibr_bagextractor” , 对bag中的数据进行提取。

3.3 运行结果

• 仿真器： 在 simulator_LINUX/Settings/Stereo.json 中，设置 “ViewMode”: “NoDisplay”， 可以提高图像输出帧率。
  
• OpenVINS
  
• VINS-Fusion
  
• 轨迹对齐
  • vio估计的轨迹
    OpenVINS 的 airsim.launch 文件中，配置“ dosave ”为“true”, 可以保存下运行轨迹 “traj_estimate.txt”, 并处理为euroc格式 (t, x, y, z, qw, qx, qy, qz )。
  • airsim的仿真真值
    读取rosbag中的“/airsim_node/drone_1/odom_local_ned” 数据，并保存为euroc格式的轨迹文件，命名为 truth.csv
  • evo轨迹对齐

evo_traj  euroc  traj_estimate.csv   --ref   truth.csv   -a  -p

3.4 相机和IMU参数配置

参考博客， 可以得到工程“simulator_LINUX/Settings/Stereo.json”中对应的相机参数（kalibr_imucam_chain.yaml）为：

%YAML:1.0

cam0:
  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
    - [0.0, 0.0, 1.0, 0.26]
    - [1.0, 0.0, 0.0, -0.0475]
    - [0.0, 1.0, 0.0, 0.0]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.0,0.0,0.0,0.0]
  distortion_model: radtan
  intrinsics: [268.511881977, 268.511881977, 320, 240] #fu, fv, cu, cv # horz fov is 100 degree
  resolution: [640, 480]
  rostopic: /cam0/image_raw
cam1:
  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
    - [0.0, 0.0, 1.0, 0.26]
    - [1.0, 0.0, 0.0, 0.0475]
    - [0.0, 1.0, 0.0, 0.0]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.0,0.0,0.0,0.0]
  distortion_model: radtan
  intrinsics: [268.511881977, 268.511881977, 320, 240] #fu, fv, cu, cv # horz fov is 100 degree
  resolution: [640, 480]
  rostopic: /cam1/image_raw

IMU的噪声参数（kalibr_imu_chain.yaml）这里大致设为：

%YAML:1.0

imu0:
  T_i_b:
    - [1.0, 0.0, 0.0, 0.0]
    - [0.0, 1.0, 0.0, 0.0]
    - [0.0, 0.0, 1.0, 0.0]
    - [0.0, 0.0, 0.0, 1.0]
  accelerometer_noise_density: 2.0000e-3  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
  accelerometer_random_walk: 3.0000e-3    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
  gyroscope_noise_density: 1.6968e-04     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
  gyroscope_random_walk: 1.9393e-05       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
  model: calibrated
  rostopic: /imu0
  time_offset: 0.0
  update_rate: 100.0

airsim.launch 如下：

<launch>

    <!-- what config we are going to run (should match folder name) -->
    <arg name="verbosity"   default="ALL" /> <!-- ALL, DEBUG, INFO, WARNING, ERROR, SILENT -->
    <arg name="config"      default="airsim" /> <!-- euroc_mav, tum_vi, rpng_aruco -->
    <arg name="config_path" default="$(find ov_msckf)/../config/$(arg config)/estimator_config.yaml" />

    <!-- mono or stereo and what ros bag to play -->
    <arg name="max_cameras" default="2" />
    <arg name="use_stereo"  default="true" />
    <arg name="bag_start"   default="0" /> <!-- v1-2: 0, mh1: 40, mh2: 35, mh3: 17.5, mh4-5: 15 -->
    <arg name="bag_rate"    default="1" />
    <arg name="dataset"     default="2022-08-24-10-59-44" /> <!-- V1_01_easy, V1_02_medium, V2_02_medium -->
    <arg name="dobag"       default="false" /> <!-- if we should play back the bag -->
    <arg name="bag"         default="/media/patrick/RPNG\ FLASH\ 3/$(arg config)/$(arg dataset).bag" />

    <!-- saving trajectory path and timing information -->
    <arg name="dosave"      default="true" />
    <arg name="dotime"      default="false" />
    <arg name="path_est"    default="/tmp/traj_estimate.txt" />
    <arg name="path_time"   default="/tmp/traj_timing.txt" />

    <!-- if we should viz the groundtruth -->
    <arg name="dolivetraj"  default="false" />
    <arg name="path_gt"     default="$(find ov_data)/$(arg config)/$(arg dataset).csv" />


    <!-- MASTER NODE! -->
    <node name="ov_msckf" pkg="ov_msckf" type="run_subscribe_msckf" output="screen" clear_params="true" required="true">
        <remap from="/cam0/image_raw" to="/airsim_node/drone_1/front_left/Scene" />
        <remap from="/cam1/image_raw" to="/airsim_node/drone_1/front_right/Scene" />
        <remap from="/imu0" to="/airsim_node/drone_1/imu/imu" />

        <!-- master configuration object -->
        <param name="verbosity"              type="string" value="$(arg verbosity)" />
        <param name="config_path"            type="string" value="$(arg config_path)" />

        <!-- world/filter parameters -->
        <param name="use_stereo"             type="bool"   value="$(arg use_stereo)" />
        <param name="max_cameras"            type="int"    value="$(arg max_cameras)" />

        <!-- timing statistics recording -->
        <param name="record_timing_information"   type="bool"   value="$(arg dotime)" />
        <param name="record_timing_filepath"      type="string" value="$(arg path_time)" />

    </node>

    <!-- play the dataset -->
    <group if="$(arg dobag)">
        <node pkg="rosbag" type="play" name="rosbag" args="-d 1 -r $(arg bag_rate) -s $(arg bag_start) $(arg bag)" required="true"/>
    </group>

    <!-- record the trajectory if enabled -->
    <group if="$(arg dosave)">
        <node name="recorder_estimate" pkg="ov_eval" type="pose_to_file" output="screen" required="true">
            <param name="topic"      type="str" value="/ov_msckf/poseimu" />
            <param name="topic_type" type="str" value="PoseWithCovarianceStamped" />
            <param name="output"     type="str" value="$(arg path_est)" />
        </node>
    </group>

    <!-- path viz of aligned gt -->
    <group if="$(arg dolivetraj)">
        <node name="live_align_trajectory" pkg="ov_eval" type="live_align_trajectory" output="log" clear_params="true">
            <param name="alignment_type" type="str" value="posyaw" />
            <param name="path_gt"        type="str" value="$(arg path_gt)" />
        </node>
    </group>


</launch>