AirSim中运行VIO算法(VINS-Mono)
VINS-Mono在AirSim上跑通
文章目录
- VINS-Mono在AirSim上跑通
- 一 IMU参数配置
- 二 相机参数设置
- 三 AirSim发布数据问题
关于相机、IMU内外参的完整解释，可以参考我的另一篇文章:
一 IMU参数配置
根据文章IMU Kalibr parameters for AirSim 、AirSim仿真IMU内参分析可以得到AirSim中连续时间的IMU随机噪声参数如下：
gyro.arw is the gyroscope_noise_density, and is equal to 8.7266462e-5
gyro_bias_stability_norm is the gyroscope_random_walk, and is equal to 9.9735023e-7
accel.vrw is the accelerometer_noise_density, and is equal to 0.002353596
accel_bias_stability_norm is the accelerometer_random_walk, and is equal to 1.2481827e-5
在VINS-Mono中我们需要的是离散后的噪声参数：
对于高斯白噪声：
σ d = σ 1 Δ t \sigma_d = \sigma \frac{1}{\sqrt{\Delta t}} σd=σΔt1
对于随机游走噪声方差：
σ b d = σ b Δ t \sigma_{bd} = \sigma_b \sqrt{\Delta t} σbd=σbΔt
在采集数据时，测得IMU频率为40~50Hz，这里得采样时间取
Δ t = 0.025 \Delta t = 0.025 Δt=0.025
因此，在Vins-Mono中设置IMU参数如下：
acc_n: 1.4885e-02
gyr_n: 5.5192e-04
acc_w: 1.9736e-06
gyr_w: 1.5769e-07
g_norm: 9.81007
简单的python计算器：
import math
acc_n_c = 0.002353596
acc_w_c = 1.2481827e-5
gyr_n_c = 8.7266462e-5
gyr_w_c = 9.9735023e-7
Delta_t = 0.025
print("acc_n: {:.4e}\ngyr_n: {:.4e}\nacc_w: {:.4e}\ngyr_w: {:.4e}\ng_norm: 9.81007".format(
acc_n_c/math.sqrt(Delta_t), gyr_n_c/math.sqrt(Delta_t), acc_w_c*math.sqrt(Delta_t), gyr_w_c*math.sqrt(Delta_t)))
二 相机参数设置
相机参数包含内参,图像宽高和畸变系数
AirSim中图像默认是没有畸变的,相机的图像大小默认设置为 640 × 480 640\times 480 640×480
因此设置相机参数如下:
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.0
k2: 0.0
p1: 0.0
p2: 0.0
projection_parameters:
fx: 320
fy: 320
cx: 3.20e+02
cy: 2.40e+02
相机与IMU之间的旋转矩阵: from camera frame to imu frame
IMU坐标系:与飞机坐标系一致, FRD；相机系默认为相机镜头前方为z, 右手为x。如下图所示：
对于前视装配的相机，旋转矩阵：
R = M z ( − 90 ) M x ( − 90 ) = [ 0 0 1 1 0 0 0 1 0 ] R = M_z(-90) M_x(-90) = \begin{bmatrix} 0 & 0 & 1\\ 1 & 0 & 0\\ 0 & 1 & 0 \end{bmatrix} R=Mz(−90)Mx(−90)=⎣⎡010001100⎦⎤
如果是下视的相机，首先配置文件settings.json
"Cameras": {
"front_center_custom": {
"CaptureSettings": [
{
"PublishToRos": 1,
"ImageType": 0,
"Width": 800,
"Height": 600,
"FOV_Degrees": 120,
"DepthOfFieldFstop": 2.8,
"DepthOfFieldFocalDistance": 200.0,
"DepthOfFieldFocalRegion": 200.0,
"TargetGamma": 1.5
}
],
"X": 0.50, "Y": 0, "Z": 0.10,
"Pitch": -90, "Roll": 0, "Yaw": 0
}
则下视相机的 R c a m i m u R_{cam}^{imu} Rcamimu为：
R = M z ( − 90 ) = [ 0 − 1 0 1 0 0 0 0 1 ] R = M_z(-90) = \begin{bmatrix} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 1 \end{bmatrix} R=Mz(−90)=⎣⎡010−100001⎦⎤
相机系与imu系的平移 T c a m i m u T_{cam}^{imu} Tcamimu即为相机系的原点在imu系下的坐标。对应airsim中.json文件设置的相机安装位置，这里以上面的.json文件为例：
t = [ 0.5 0.0 0.1 ] t= \begin{bmatrix} 0.5 & 0.0 & 0.1 \end{bmatrix} t=[0.50.00.1]
三 AirSim发布数据问题
增加频率方法参考资料:
Running SLAM system on AirSim · Issue #2369 · microsoft/AirSim (github.com)
修改airsim的ros包实现高频率：xuhao1/airsim_ros_pkgs: An adapter for airsim for SITL or HIL simulation drone app built on DJI-SDK (github.com)
我的另一篇总结bolg：AirSim中获取视觉、惯性数据方法研究