AirSim相机、IMU内外参分析(VIO、vSLAM)

智能之欣

已于 2022-03-16 17:50:23 修改

阅读量3k

点赞数 9

分类专栏： SLAM 环境配置文章标签： c++ 算法自动驾驶人工智能

于 2022-03-16 17:37:36 首次发布

本文链接：https://blog.csdn.net/ZDPZN/article/details/123531686

版权

SLAM 同时被 2 个专栏收录

4 篇文章 0 订阅

订阅专栏

环境配置

4 篇文章 1 订阅

订阅专栏

作者: 朱贞欣，公ZH：SLAM学习er

文章目录

0 引入

假设你想通过AirSim获取仿真数据运行VIO或者生成点云等等，现在你可以获得图像、IMU以及真值，但是你又不知道这些传感器之间的外参如何定义？相机的内参又是多少？AirSim的接口坐标系定义又是如何？那么这篇文章将会尝试对AirSim做一个剖析。

总结一下我们要解决的问题：

API获取参考真值的坐标系定义
$T_{wb}$ (body与world转换)、 $T_{wi}$ (imu系与world系转换)的定义及计算
IMU的噪声参数如何获取
相机内参 $K$ 如何获得
相机外参 $T_{ic}$ 如何获得

1 世界坐标系

本节参考资料：Coordinate System

根据参考资料，可以知道：

所有的AirSim API使用的都是NED坐标系统
所有的单位均为SI单位制
无人机的起始点坐标作为NED坐标系的原点，即无人机起始位置的坐标为 $(0, 0, 0)$ 。

AirSim中获取无人机绝对位置或姿态的接口有：

simGetGroundTruthEnvironment(vehicle_name='')

该函数返回class airsim.types.EnvironmentState，包含信息：

成员	解释
air_density	大气密度
air_pressure	大气压
geo_point	Palyer Start的起始地理位置
gravity	重力
position	无人机位置(NED)
temperature	温度

我们输出一个测试一下输出结果

>>> print(client.simGetGroundTruthEnvironment())
<EnvironmentState> {   'air_density': 1.210699200630188,
    'air_pressure': 99866.2421875,
    'geo_point': <GeoPoint> {   'altitude': 122.14649963378906,
    'latitude': 47.64148596630509,
    'longitude': -122.140165},
    'gravity': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 9.806650161743164},
    'position': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': -0.14649587869644165},
    'temperature': 287.3560485839844}

这里需要关注一下重力，其z方向为9.8，即重力是在NED坐标系的表示

simGetGroundTruthKinematics(vehicle_name='')

该函数返回airsim.types.KinematicsState，包含信息：

成员
angular_acceleration 角加速度
angular_velocity 角速度
linear_acceleration 加速度
linear_velocity 速度
orientation 姿态四元数
position 位置
其中，角速度、角加速度是在体坐标系描述的，其余的为NED系描述(参考)，这里NED我们后面也会叫作 $w o r l d$ 系，即世界系。

最后还要说明的一点是，这里kinematics输出的位置信息和simGetGroundTruthEnvironment()接口的位置信息不一定完全相同，kinematics的结果是通过动力学物理引擎计算得到的，具体可以参考AirSim论文

同样，我们输出一下该API获得数据：
```
>>> print(client.simGetGroundTruthKinematics())
<KinematicsState> {   'angular_acceleration': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.0},
    'angular_velocity': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.0},
    'linear_acceleration': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.0},
    'linear_velocity': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.0},
    'orientation': <Quaternionr> {   'w_val': 0.9238795042037964,
    'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.38268348574638367},
    'position': <Vector3r> {   'x_val': -0.0055317687802016735,
    'y_val': 0.005531685426831245,
    'z_val': 4.842146396636963}}
```

成员
angular_acceleration	角加速度
angular_velocity	角速度
linear_acceleration	加速度
linear_velocity	速度
orientation	姿态四元数
position	位置

2 IMU

2.1 IMU数据生成

AirSim中关于IMU数据生成的代码地址：IMU数据生成. 下面对代码进行简单解释。

首先通过getGroundTruth()获得了当前无人机的参考真值数据，包含动力学等(ground_truth.kinematics)，output为要得到的IMU数据。

void updateOutput()
{
    Output output;
    const GroundTruth& ground_truth = getGroundTruth();

接着看下一段代码：

ground_truth.kinematics->twist.angular是无人机的角速度,描述在本体系下

output.angular_velocity = ground_truth.kinematics->twist.angular;//将无人机的角速度赋值给output.angular_velocity
output.linear_acceleration = ground_truth.kinematics->accelerations.linear - ground_truth.environment->getState().gravity;//考虑重力，生成加速度数据(描述在世界坐标系w下的)
output.orientation = ground_truth.kinematics->pose.orientation;// 得到无人机在世界系下的姿态

//acceleration is in world frame so transform to body frame
output.linear_acceleration = VectorMath::transformToBodyFrame(output.linear_acceleration, 
ground_truth.kinematics->pose.orientation, true);// 将加速度转换为体坐标系下，这里使用表明Rwb后直接作为IMU数据，而没有Rbi，表明i系与b系为同一系

关于transformToBodyFrame函数，功能是将另一系下的向量转换到体系下，有两个函数重载，其代码如下：

static Vector3T transformToBodyFrame(const Vector3T& v_world, const QuaternionT& q_world, bool assume_unit_quat = true)// 仅考虑旋转
{
    return rotateVectorReverse(v_world, q_world, assume_unit_quat);
}

static Vector3T transformToBodyFrame(const Vector3T& v_world, const Pose& body_world, bool assume_unit_quat = true)// 考虑平移和旋转
{
    //translate
    Vector3T translated = v_world - body_world.position;
    //rotate
    return transformToBodyFrame(translated, body_world.orientation, assume_unit_quat);
}

上述代码中的rotateVectorReverse()返回经过 $q^*$ (共轭四元数)旋转后的向量，用旋转矩阵描述就是传入向量 $v$ 和旋转 $R$ ,返回 $R^{T}v$

static Vector3T rotateVectorReverse(const Vector3T& v, const QuaternionT& q, bool assume_unit_quat)
{
    //QuaternionT vq(0, v.x(), v.y(), v.z());
    //QuaternionT qi = assume_unit_quat ? q.conjugate() : q.inverse();如果是单位四元数就取共轭，否，就求逆；这里的四元数是基于Eigen库实现的typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaterniond;
    //return (qi * vq * q).vec();

    if (!assume_unit_quat)
    return q.inverse()._transformVector(v);// Q._transformVector(v)：返回经过Q旋转后的v
    else
    return q.conjugate()._transformVector(v);// Q._transformVector(v)：返回经过Q旋转后的v
}

这里解释一下为什么将 $w$ 系下的加速度测量值转换到体系 $b$ 时使用的是姿态的逆：这里无人机的姿态pose.orientation可以表述为无人机机体姿态在 $w$ 系中表示，也可以描述为两个坐标系间的转换： $b o d y$ 系到 $w$ 系旋转。

AirSim中使用四元数描述，为了便于理解，可以假设其等效的旋转矩阵为 $R$ ，那么这个 $R$ 实际上就是 $R_{wb}$ ，即表征 $b$ 到 $w$ 的旋转。因此，对于 $w$ 系下的一个向量 $v^{w}$ ，要想得到其在 $b$ 系下的坐标，就需要通过 $R_{wb}$ 的逆得到：
$v^b = R_{wb}^T v^w$
最后添加噪声：

//add noise
addNoise(output.linear_acceleration, output.angular_velocity);
// TODO: Add noise in orientation?

output.time_stamp = clock()->nowNanos();

setOutput(output);
}

从上面的分析中，我们也可以得到:

AirSim中body系与imu系重合，因为IMU测量值直接使用了描述在体系下的角速度和转换到体系下的加速度(减去了重力)
ground_truth.kinematics->pose.orientation表征的实际涵义相当于旋转矩阵 $R_{wb}$

至此我们虽然得到了 $i m u$ 系与 $b o d y$ 系重合这一信息，但是还是不知道坐标系的三轴方向是如何定义的，好在AirSim官网中给出了定义：

body系参考资料，直接贴出来，如下：

The body frame follows the Front Left Up (FLU) convention, and right-handedness.
Frame Convention:
- X axis is along the Front direction of the quadrotor.Clockwise rotation about this axis defines a positive roll angle.Hence, rolling with a positive angle is equivalent to translating in the right direction, w.r.t. our FLU body frame.
- Y axis is along the Left direction of the quadrotor.Clockwise rotation about this axis defines a positive pitch angle.Hence, pitching with a positive angle is equivalent to translating in the front direction, w.r.t. our FLU body frame.
- Z axis is along the Up direction.Clockwise rotation about this axis defines a positive yaw angle.Hence, yawing with a positive angle is equivalent to rotated towards the left direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane.

可以看到，这里关于体系的定义：x轴指向旋翼机的前方，y轴指向左侧，z轴向上。然而，这个定义貌似有点问题，正确的机体系定义应该是：x轴指向旋翼机的前方，y轴指向右侧，z轴向下，如下图：

在这里插入图片描述

最后，我们使用输出一下IMU数据和Kinematics：

>>> print(client.simGetGroundTruthKinematics())
<KinematicsState> {   'angular_acceleration': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.0},
    'angular_velocity': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.0},
    'linear_acceleration': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.0},
    'linear_velocity': <Vector3r> {   'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.0},
    'orientation': <Quaternionr> {   'w_val': 0.9238795042037964,
    'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.38268348574638367},
    'position': <Vector3r> {   'x_val': -0.0055317687802016735,
    'y_val': 0.005531685426831245,
    'z_val': 4.842146396636963}}
>>> print(client.getImuData())
<ImuData> {   'angular_velocity': <Vector3r> {   'x_val': -0.0013163003604859114,
    'y_val': -0.0075134402140975,
    'z_val': -0.003700035624206066},
    'linear_acceleration': <Vector3r> {   'x_val': 0.014119932428002357,
    'y_val': 0.10340915620326996,
    'z_val': -9.82824420928955},
    'orientation': <Quaternionr> {   'w_val': 0.9238795042037964,
    'x_val': 0.0,
    'y_val': 0.0,
    'z_val': 0.38268348574638367},
    'time_stamp': 1647009947410646016}

在使用IMU数据时，我们通常按下述公式来得到世界系下的加速度： $a^w=R_{b}^{w}a^{b}+g^w$

因此，在仿真中已知 $a^{w},g^{w},R_{b}^{w}$ ，要反演IMU加速度测量值时就应用下式：
$a^{b}=R^b_{w}(a^w-g^w)$
此式与2.1节中的代码完全对应。进一步，在AirSim中，当无人机水平静止放置时(假设机头朝北)，如下图：

在这里插入图片描述

$a^w=[0,0,0]^T \\ g^w=[0,0,9.8]^T \\ R_{w}^{b}= \begin{bmatrix} 1 & 0 & 0\\ 0 & 1 & 0\\ 0 & 0 & 1 \end{bmatrix}$
带入计算得到： $a^b=[0,0,-9.8]^T$ ，该结果与上面输出的结果一致：

'linear_acceleration': <Vector3r> {   'x_val': 0.014119932428002357,
    'y_val': 0.10340915620326996,
    'z_val': -9.82824420928955},

2.2 关于IMU噪声

参考资料:

IMU Kalibr parameters for AirSim
AirSim仿真IMU内参分析
AirSim仿真IMU内参分析_hanmoge

可以得到AirSim中连续时间的IMU随机噪声参数如下：

gyro.arw 是陀螺高斯白噪声 $\sigma_{g}$ ，值为8.7266462e-5
gyro_bias_stability_norm是陀螺随机游走 $\sigma_{bg}$ ，值为9.9735023e-7
accel.vrw是加速度计白噪声 $\sigma_{a}$ ，值为0.002353596
accel_bias_stability_norm 是加速度计游走 $\sigma_{bg}$ ，值为1.2481827e-5

上述均是连续情况下的值，实际工程中常常使用离散的值，即考虑IMU的采样频率。对于高斯白噪声，离散的计算公式：
$\sigma_d = \sigma \frac{1}{\sqrt{\Delta t}}$
对于随机游走噪声的离散计算公式：
$\sigma_{bd} = \sigma_b \sqrt{\Delta t}$
在VINS-Mono中我们需要的是离散后的噪声参数，以我在笔记本上测得AirSim发布IMU频率为40~50Hz为例，这里采样时间应取：
$\Delta t = 0.025$

编写一个简单的python计算器，指定一下Delta_t即可：

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)))

根据输出结果，在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

3 相机

3.1 相机外参

在AirSim中，相机的坐标系定义为：x轴为相机右侧，y轴为相机下方，z轴相机镜头前方

在这里插入图片描述

以相机水平安装为例，其镜头朝向机体的正前方，对应的settings.json如下，其中RPY表示绕体坐标系的各轴旋转对应的角度(deg)后得到相机系，XYZ表示相机系的原点在 $b o d y$ 系下的坐标。

"Cameras": {
    "front_center_custom": {
        "CaptureSettings": [
            {
                "PublishToRos": 1,
                "ImageType": 0,
                "Width": 800,
                "Height": 600,
                "FOV_Degrees": 90
            }
        ],
        "X": 0.50, "Y": 0, "Z": 0.10,
        "Pitch": 0, "Roll": 0, "Yaw": 0
    }
}

在这里插入图片描述

结合 $b o d y$ 的定义以及下图，可以得到其对应的相机外参：
$R_{c}^{i}= \begin{bmatrix} 0 & 0 & 1 \\ 1 & 0 & 0\\ 0 & 1 & 0 \end{bmatrix} \\ t_c^i=[0.5, 0, 0.1]^T$

3.2 内参

参考资料：

Camera Calibration Values · Issue #2396 · microsoft/AirSim (github.com)

Camera intrinsic parameters corresponding to the Scene image? · Issue #269 · microsoft/AirSim (github.com)

AirSim中相机内参计算公式参考其ROS wrapper中的代码

sensor_msgs::CameraInfo AirsimROSWrapper::generate_cam_info(const std::string& camera_name,
                                                            const CameraSetting& camera_setting,
                                                            const CaptureSetting& capture_setting) const
{
    sensor_msgs::CameraInfo cam_info_msg;
    cam_info_msg.header.frame_id = camera_name + "/" + image_type_int_to_string_map_.at(capture_setting.image_type) + "_optical";
    cam_info_msg.height = capture_setting.height;
    cam_info_msg.width = capture_setting.width;
    float f_x = (capture_setting.width / 2.0) / tan(math_common::deg2rad(capture_setting.fov_degrees / 2.0));
    // todo focal length in Y direction should be same as X it seems. this can change in future a scene capture component which exactly correponds to a cine camera
    // float f_y = (capture_setting.height / 2.0) / tan(math_common::deg2rad(fov_degrees / 2.0));
    cam_info_msg.K = {f_x, 0.0, capture_setting.width / 2.0, 
                        0.0, f_x, capture_setting.height / 2.0, 
                        0.0, 0.0, 1.0};
    cam_info_msg.P = {f_x, 0.0, capture_setting.width / 2.0, 0.0,
                        0.0, f_x, capture_setting.height / 2.0, 0.0, 
                        0.0, 0.0, 1.0, 0.0};
    return cam_info_msg;
}

总结起来如下：
$f_x=f_y= \frac{width/2}{\tan (fov/2)} \\ K=\begin{bmatrix} f_x & 0 & \frac{width}{2}\\ 0 & f_y & \frac{height}{2}\\ 0 & 0 & 1 \end{bmatrix}$
关于畸变参数K1, K2, K3, P1, P2 默认为0，可以通过Camera APIs设置。

如果您觉得文章写的不错，欢迎关注：

B乎：智能之欣，公Z号：SLAM学习er

智能之欣

关注

9
点赞
踩
34

收藏

觉得还不错? 一键收藏
0
评论
AirSim相机、IMU内外参分析(VIO、vSLAM)

假设你想通过AirSim获取仿真数据运行VIO或者生成点云等等，现在你可以获得图像、IMU以及真值，但是你又不知道这些传感器之间的外参如何定义？相机的内参又是多少？AirSim的接口坐标系定义又是如何？那么这篇文章将会尝试对AirSim做一个剖析。
复制链接

扫一扫