AirSim笔记4:利用AirSim录制数据并跑ORB-SLAM

Dec 2,2021   10289 words   37 min

Tags: SLAM

在之前的这篇博客中,我们介绍了基本的数据录制流程,这篇博客中介绍了相关配置参数。有了上面的准备,我们就可以来录一些更“个性化”的数据了,而不是像之前一样,只能用默认值。对于SLAM而言,可以用的数据有很多,如单目RGB影像、双目RGB影像、RGBD影像等。下面分别进行介绍。

1.单目RGB影像录制

对于单目影像录制,我们可以按照如下设置参数:

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Car",
  "ViewMode": "SpringArmChase",
  "SubWindows": [
    {"WindowID": 0, "ImageType": 0, "CameraName": "front_center", "Visible": true},
    {"WindowID": 1, "ImageType": 3, "CameraName": "front_center", "Visible": true},
    {"WindowID": 2, "ImageType": 5, "CameraName": "front_center", "Visible": true}
  ],
  "Recording": {
    "RecordInterval": 0.05,
    "Cameras": [
        { "CameraName": "front_center", "ImageType": 0, "PixelsAsFloat": false, "Compress": true }
    ]
  },
  "CameraDefaults": {
    "CaptureSettings": [
      {
        "ImageType": 0,
        "Width": 752,
        "Height": 480,
        "FOV_Degrees": 90
      }
    ]
  }
}

这里我们可视化了三个子窗口,但实际录制只录制了front_center相机的RGB影像数据,分辨率是752×480,帧率是20(和EuRoC数据集一致),视场角为90°。录制界面如下, 打开录制数据文件夹,如下所示。 可以看到,数据已经存好了,分辨率也就是我们指定的大小。这样,单目的数据就录制完成了。

2.单目深度影像录制

与单目RGB影像类似的,我们依然只需要修改配置文件,如下:

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Car",
  "ViewMode": "SpringArmChase",
  "SubWindows": [
    {"WindowID": 0, "ImageType": 0, "CameraName": "front_center", "Visible": true},
    {"WindowID": 1, "ImageType": 3, "CameraName": "front_center", "Visible": true},
    {"WindowID": 2, "ImageType": 4, "CameraName": "front_center", "Visible": true}
  ],
  "Recording": {
    "RecordInterval": 0.05,
    "Cameras": [
        { "CameraName": "front_center", "ImageType": 3, "PixelsAsFloat": false, "Compress": true }
    ]
  },
  "CameraDefaults": {
    "CaptureSettings": [
      {
        "ImageType": 3,
        "Width": 752,
        "Height": 480,
        "FOV_Degrees": 90
      }
    ]
  }
}

这里我们首先把可视化部分修改了一下,分别显示RGB影像、深度图以及视差图。然后把录制的影像类型由1改成了3,在CaptureSettings属性中也进行了对应修改。录制界面如下。 利用这张图,倒是可以很清晰的说明视差图和深度图之间的关系。直观的感受是深度图中越远的地方越亮,而视差图中越远的地方越暗。所以某种程度上来说,深度图和视差图是一种相反的关系。最终,输出的影像文件如下。 可以看到,影像分辨率是对的,但是内容似乎只有黑白了,有点问题。导致这个问题的主要原因在于,深度图中存在非常大的数值,如好几万,但其实大部分有效值却都在几十、几百的量级。这样一个结果是在“无脑”归一化的时候,那些很小的、有意义的深度就会通通被压缩为0,也就出现了输出影像中的黑白问题。那如何解决这个问题呢?当然是有办法的。那就是设置PixelsAsFloat属性为true,表示以原始点float类型存储深度数据。这样,我们就能得到如下的pfm文件。 这种文件,我们可以用Matlab进行读取,读取并可视化代码如下:

% pfm文件路径
filename_pfm = "test3.pfm";

% 打开文件
fid = fopen(filename_pfm);

% 开始读取
fscanf(fid,'%c',[1,3]);
cols = fscanf(fid,'%f',1);
rows = fscanf(fid,'%f',1);
fscanf(fid,'%f',1);
fscanf(fid,'%c',1);
D = fread(fid,[cols,rows],'single');
fclose(fid);

% 读取的图像可能存在异常值,处理一下
D(D == Inf) = 0;
% 读取的图像方向不正确,所以翻转一下
D = rot90(D);
D = flipud(D);

% 可视化深度图
imshow(D);

可视化的某帧深度图如下所示。 可以看到,无穷远处的深度值为65500。 而较近的地方,深度值为0.02065。这也就是我们刚刚提到的问题。但不管怎么说,保存成pfm文件以后,这些深度信息都被保留下来了,剩下的事情就交给后面数据处理的流程去做了。所以,一个比较靠谱的深度图录制配置应该是下面的样子。

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Car",
  "ViewMode": "SpringArmChase",
  "SubWindows": [
    {"WindowID": 0, "ImageType": 0, "CameraName": "front_center", "Visible": true},
    {"WindowID": 1, "ImageType": 3, "CameraName": "front_center", "Visible": true},
    {"WindowID": 2, "ImageType": 4, "CameraName": "front_center", "Visible": true}
  ],
  "Recording": {
    "RecordInterval": 0.05,
    "Cameras": [
        { "CameraName": "front_center", "ImageType": 3, "PixelsAsFloat": true, "Compress": true }
    ]
  },
  "CameraDefaults": {
    "CaptureSettings": [
      {
        "ImageType": 3,
        "Width": 752,
        "Height": 480,
        "FOV_Degrees": 90
      }
    ]
  }
}

至此,我们便完成了深度图的录制。

3.双目RGB影像录制

对于双目RGB影像,配置应该如下。

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Car",
  "ViewMode": "SpringArmChase",
  "SubWindows": [
    {"WindowID": 0, "ImageType": 0, "CameraName": "front_left", "Visible": true},
    {"WindowID": 1, "ImageType": 0, "CameraName": "front_center", "Visible": true},
    {"WindowID": 2, "ImageType": 0, "CameraName": "front_right", "Visible": true}
  ],
  "Recording": {
    "RecordInterval": 0.05,
    "Cameras": [
        { "CameraName": "front_left", "ImageType": 0, "PixelsAsFloat": false, "Compress": true },
        { "CameraName": "front_right", "ImageType": 0, "PixelsAsFloat": false, "Compress": true }
    ]
  },
  "CameraDefaults": {
    "CaptureSettings": [
      {
        "ImageType": 0,
        "Width": 752,
        "Height": 480,
        "FOV_Degrees": 90
      }
    ]
  }
}

在可视化部分,我们让三个窗口分别显示前置的左、中、右相机的内容。录制部分录制front_leftfront_right相机内容。如前面这篇博客提到的,默认情况下,双目的基线长度为25cm。录制窗口如下。 仔细观察三个子窗口中左边汽车的后视镜,可以看到最左边的相机看到的最多,中间其次,右相机最少。这也是符合我们的常识的。录制好的影像如下图所示。 可以看到,录制的左右相机影像都是放在一个文件夹下的。通过文件名可以区分是左相机还是右相机。当然这里还有一个潜在问题,你会发现,左右相机的文件名并不是对应的。比如,左相机和右相机的第一帧时间戳并非一致。那么如何获得真正对应的时间戳呢?答案是airsim_rec.txt文件。我们打开自动生成的索引文件如下。 可以看到,文件名中的一长串数字是时间戳,但由于单位更小,所以导致左右目影像时间戳不对应。一个解决办法就是根据索引文件的内容来对录制的影像文件重命名,可以通过简单编写脚本实现。重命名完成之后,便可以是正常的双目影像组织形式了(参考EuRoC)。

下面的脚本实现的是根据airsim_rec.txt索引文件对影像文件进行重命名的过程:

# coding=utf-8
import os
import sys

if __name__ == '__main__':
    file_path = sys.argv[1]
    cur_dir = file_path[:file_path.rfind(os.path.sep)] + os.path.sep
    print(cur_dir)

    fin = open(file_path, "r")
    line = fin.readline().strip()
    line = fin.readline().strip()
    while (line):
        parts = line.split("\t")
        timestamp = parts[1]
        names = parts[-1]
        name_list = names.split(";")
        for i in range(len(name_list)):
            name = name_list[i]
            old_name_path = cur_dir + os.path.sep + "images" + os.path.sep + name

            name_prefix = name[:name.rfind("_") + 1]
            new_name_path = cur_dir + os.path.sep + "images" + os.path.sep + name_prefix + timestamp + ".png"

            name_path = file_path.rfind(os.path.sep)
            print(timestamp)

            os.rename(old_name_path, new_name_path)

        line = fin.readline().strip()

下面的脚本则更进一步,将录制的双目影像数据重命名,并分别分开到左右两个文件夹中,最后生成时间戳文件,使得数据组织形式和EuRoC尽可能统一。

# coding=utf-8
import os
import sys
from HaveFun import common
import shutil

if __name__ == '__main__':
    file_path = sys.argv[1]
    cur_dir = file_path[:file_path.rfind(os.path.sep)] + os.path.sep
    print(cur_dir)

    left_target_dir = cur_dir + "images_left" + os.path.sep
    right_target_dir = cur_dir + "images_right" + os.path.sep
    common.isDirExist(left_target_dir)
    common.isDirExist(right_target_dir)

    fout = open(cur_dir + "/timestamps.txt", "w")

    fin = open(file_path, "r")
    line = fin.readline().strip()
    line = fin.readline().strip()
    while (line):
        parts = line.split("\t")
        timestamp = parts[1]
        names = parts[-1]
        name_list = names.split(";")

        print(timestamp)
        fout.write(timestamp + "\n")
        for i in range(len(name_list)):
            name = name_list[i]
            old_name_path = cur_dir + os.path.sep + "images" + os.path.sep + name

            if name.__contains__("left"):
                new_name_path = left_target_dir + os.path.sep + timestamp + ".png"
            elif name.__contains__("right"):
                new_name_path = right_target_dir + os.path.sep + timestamp + ".png"

            shutil.copy2(old_name_path, new_name_path)

        line = fin.readline().strip()
    fin.close()
    fout.close()

当然为了方便使用,上面两个脚本都上传到了Github,点击查看

4.RGBD影像录制

对于RGBD影像录制,说白了就是RGBD影像+深度图。明确了这一点以后,配置文件也就简单了,如下:

{
  "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
  "SettingsVersion": 1.2,
  "SimMode": "Car",
  "ViewMode": "SpringArmChase",
  "SubWindows": [
    {"WindowID": 0, "ImageType": 0, "CameraName": "front_center", "Visible": true},
    {"WindowID": 1, "ImageType": 3, "CameraName": "front_center", "Visible": false},
    {"WindowID": 2, "ImageType": 4, "CameraName": "front_center", "Visible": false}
  ],
  "Recording": {
    "RecordInterval": 0.05,
    "Cameras": [
        { "CameraName": "front_center", "ImageType": 0, "PixelsAsFloat": false, "Compress": true },
        { "CameraName": "front_center", "ImageType": 3, "PixelsAsFloat": true, "Compress": true }
    ]
  },
  "CameraDefaults": {
    "CaptureSettings": [
      {
        "ImageType": 0,
        "Width": 752,
        "Height": 480,
        "FOV_Degrees": 90
      },
      {
        "ImageType": 3,
        "Width": 752,
        "Height": 480,
        "FOV_Degrees": 90
      }
    ]
  }
}

首先可视化部分,我们可视化RGB影像、深度图以及视差图。我们分别录制front_center相机的RGB和深度数据,其中深度数据以pfm格式存储。最后,我们还需要分别指定RGB相机和深度相机的影像分辨率。完成之后即可开始进行录制,如下。 输出影像如下所示。 同理,你也会发现,时间戳不是完全一致的。解决办法和上面类似,通过airsim_rec.txt索引文件对影像重命名即可。如下图所示,可以看到,索引文件中包含了对应关系。

5.录制帧率与时间戳

首先需要说明的事AirSim录制的数据时间戳默认单位为毫秒,比如1638414235497,对应1638414235.497秒。然后想强调一下录制帧率的问题,对于AirSim而言,能录制多少帧率完全取决于你电脑的性能和你要录制的数据量的大小。虽然我们在配置文件中指定了RecordInterval属性,但实际录出来的效果能不能达到预期值是不一定的。这里简单列举一些提升录制性能的方法。从硬件上来说,尽可能用更好的GPU、硬盘的存取速度要够快。从软件上来说,首先,提升性能最有效的办法就是把可视化渲染关闭(ViewMode设为NoDisplay)。如果可以降低录制影像数据的分辨率,也可以提升帧率。还有一种方法就是通过代码控制平台运动,多类型数据分开录制,最后再对处理,但相对会麻烦一些。当然,硬件在一定程度上决定了帧率上限,比如我的电脑录双目数据(可视化开启,设定2×752×480@20fps),实际最多只能到10帧左右,如下图所示。 而如果同样条件下关闭可视化,最多可以到15帧左右,如下图所示。 最后,如果缩小录制影像分辨率,改为2×600×400@20fps,则可以比较稳的到20帧,如下图所示。 另外,如果只录制单目数据,可视化关闭,设定752×480@20fps,实际在我电脑上是可以达到20帧的,如下所示。 所以AirSim能录制多少帧率主要取决于你的硬件性能和数据量多少。

5.跑ORB-SLAM2

既然我们是做SLAM的,拿着AirSim录了数据自然是想着用来跑SLAM。通过上面的脚本,我们已经对数据进行了一定的处理,基本转成了EuRoC的格式。这里我们就以双目ORB-SLAM2为例,展示运行的效果。

6.1 数据与代码准备

对于ORB-SLAM2而言,跑双目需要相机参数配置文件,如下所示。 对于相机的内参,我们认为没有误差,可以根据喜好设置一下。主要就是修改cxcyCamera.widthCamera.heightCamera.bfStereo Rectification,如下。 然后是代码的部分修改。在系统输入层面,接收的时间戳单位都是秒。但EuRoC数据集中时间戳的单位是纳秒(10的-9次方),所以在代码中对时间戳文件中的时间通通除了10的9次方,如下所示。 而AirSim的时间戳单位为毫秒(10的-3次方)。所以我们需要修改一下EuRoC代码中的除数,变成10的3次方。这样运行前的准备工作就完成了。

6.2 运行程序

ORB-SLAM2双目启动有多个参数,包括:字典文件路径、配置文件路径、左影像路径、右影像路径、时间戳文件路径。所以我们依次填入相关内容,点击回车就可以启动程序了。比如在我电脑上的命令是:

./stereo_airsim ../../Vocabulary/ORBvoc.txt AirSim.yaml /root/Datasets/AirSim/seq1/image_left /root/Datasets/AirSim/seq1/image_right /root/Datasets/AirSim/seq1/timestamps.txt 

运行动图如下所示。 可以看到在局部,ORB-SLAM2都可以很好的进行Tracking,但由于累积误差的存在,轨迹存在偏移。但得益于仿真场景较为稳定的光照条件和ORB-SLAM2本身较强的回环能力,当构成回环条件以后,对轨迹进行了比较好的校正。使得最终轨迹和真值比较接近。

6.3 精度评价

我们可以利用前面提到过的evo工具进行精度评价。ORB-SLAM2跑完以后,会生成一个CameraTrajectory.txt的文件,再配合上AirSim输出的airsim_rec.txt真值文件,就可以进行评价了。当然,这里需要注意一些细节问题,比如时间戳的单位问题、位姿的表达顺序问题等。把这些统一了以后,才能丢给evo进行精度评价。

首先对于ORB-SLAM2输出的CameraTrajectory.txt文件,我们在Main()函数中调用的就是SLAM.SaveTrajectoryTUM()。所以保存的数据格式自然就是TUM格式,关于TUM格式,在这篇博客这篇博客都介绍了。简单来说就是:时间戳(秒)+位置X(米)+位置Y(米)+位置Z(米)+四元数X+四元数Y+四元数Z+四元数W,分隔符为一个空格,如下图所示。 而对于AirSim输出的airsim_rec.txt文件,在之前的这篇博客中也提到过了。简单来说就是:时间戳(毫秒)+位置X(米)+位置Y(米)+位置Z(米)+四元数W+四元数X+四元数Y+四元数Z,分割符为一个Tab,如下图所示。 前面说了,evo只能接收相同格式的轨迹文件进行比较。现在CameraTrajectory.txt文件已经是标准的TUM格式了,我们可以不用管。主要处理的就是airsim_rec.txt文件,一是统一时间戳单位,二是调整四元数顺序。我们可以简单写个脚本实现这个功能,如下。

# coding=utf-8
import os
import sys

if __name__ == '__main__':
    file_path = sys.argv[1]
    cur_dir = file_path[:file_path.rfind(os.path.sep)] + os.path.sep

    fout = open(cur_dir + "/groundtruth_TUM.txt", "w")

    fin = open(file_path, "r")
    line = fin.readline().strip()
    line = fin.readline().strip()
    while (line):
        parts = line.split("\t")
        timestamp = float(parts[1]) / 1000.0
        pos_x = parts[2]
        pos_y = parts[3]
        pos_z = parts[4]

        quat_w = parts[5]
        quat_x = parts[6]
        quat_y = parts[7]
        quat_z = parts[8]

        fout.write(str(timestamp) + " " +
                   pos_x + " " +
                   pos_y + " " +
                   pos_z + " " +
                   quat_x + " " +
                   quat_y + " " +
                   quat_z + " " +
                   quat_w + "\n")

        line = fin.readline().strip()

    fin.close()
    fout.close()

源文件同样见Github项目

至此,我们就完成了使用evo进行精度评价前的准备工作。按照之前博客的介绍,打开终端,输入如下命令即可可视化并对比轨迹。

evo_traj tum CameraTrajectory.txt --ref=groundtruth_TUM.txt -p --plot_mode=xyz

如下图所示,可以看到,ORB-SLAM2估计的轨迹总长约为875m,真值轨迹总长约为613m。 下图是轨迹可视化结果。 可以看到,真值轨迹和估计轨迹在坐标上似乎差了一个翻转。但总体而言,轨迹的趋势是相同的。

进一步,我们定量评价估计轨迹的精度,在终端中输入如下命令。

evo_ape tum CameraTrajectory.txt groundtruth_TUM.txt -va --plot --plot_mode xyz --save_results results_ORB.zip

定量结果输出如下。 可以看到总体RMSE在33m左右。对比的轨迹可视化如下图所示。 可以看到,估计轨迹与真实轨迹在整体趋势上是一致的。但是由于整体差了一定的尺度,导致了RMSE较大。而这种尺度差异是和我们一开始在配置文件中设置的Camera.bf有密切关系的。如果设置一个更加精准的bf,尺度上的差异就会小很多,最终的总体误差也会有比较明显的下降。

本文作者原创,未经许可不得转载,谢谢配合

返回顶部