在SLAM数据采集中，数据的稳定性对于系统的运行是至关重要的。如果一段数据帧率不够稳定(尤其是有视觉+IMU)的时候，那么可能会给系统带来致命的问题，导致系统运行失败。对于ROS Bag而言，如在这篇博客中提到的，我们可以通过rqt_bag对Bag文件进行可视化，可以看到数据流的帧率是否稳定。但是对于以普通影像文件存储的数据，目前并没有什么比较好的办法。所以我们需要一个检查数据帧率的工具，这也是这篇博客要解决的问题。
1.影像流帧率检查
代码如下。
# coding=utf-8
from HaveFun import common
from matplotlib import pyplot as plt
import sys
import numpy as np
import time
if __name__ == '__main__':
# 用于检查影像流帧率是否稳定
search_dir = sys.argv[1] # 待检查影像所在文件夹
file_type = sys.argv[2] # 影像文件类型
time_unit = sys.argv[3] # 影像时间戳单位
cur_time = time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime())
fout = open("./vision_report_" + cur_time + ".txt", 'w')
print("Loading image data ...")
paths, names, files = common.findFiles(search_dir, file_type)
print("Loaded image data!")
timestamps = []
if time_unit == 's' or time_unit == 'sec':
for i in range(len(names)):
timestamps.append(float(names[i].split(".")[0]))
elif time_unit == 'ms' or time_unit == 'MS':
for i in range(len(names)):
timestamps.append(float(names[i].split(".")[0]) / 1000.0)
elif time_unit == 'us' or time_unit == 'US':
for i in range(len(names)):
timestamps.append(float(names[i].split(".")[0]) / 1000000.0)
elif time_unit == 'ns' or time_unit == 'NS':
for i in range(len(names)):
timestamps.append(float(names[i].split(".")[0]) / 1000000000.0)
elif time_unit == 'ps' or time_unit == 'PS':
for i in range(len(names)):
timestamps.append(float(names[i].split(".")[0]) / 1000000000000.0)
time_intervals = []
for i in range(1, len(timestamps)):
time_intervals.append(timestamps[i] - timestamps[i - 1])
mean_interval = np.mean(time_intervals)
max_interval = max(time_intervals)
min_interval = min(time_intervals)
var_interval = np.var(time_intervals)
mean_fps = 1.0 / mean_interval
max_difference = max_interval - min_interval
index_max = time_intervals.index(max_interval)
index_min = time_intervals.index(min_interval)
timestamp_max = timestamps[index_max]
timestamp_min = timestamps[index_min]
str_line1 = "max interval:" + str(max_interval) + "(sec), index:" + str(index_max) + "/" + str(
len(timestamps)) + ", timestamp:" + str(timestamp_max) + "\n"
str_line2 = "min interval:" + str(min_interval) + "(sec), index:" + str(index_min) + "/" + str(
len(timestamps)) + ", timestamp:" + str(timestamp_min) + "\n"
str_line3 = "mean interval:" + str(mean_interval) + "\n"
str_line4 = "variance interval:" + str(var_interval) + "\n"
str_line5 = "max interval difference:" + str(max_difference) + "\n"
str_line6 = "mean fps:" + str(mean_fps) + "\n"
fout.write(str_line1)
fout.write(str_line2)
fout.write(str_line3)
fout.write(str_line4)
fout.write(str_line5)
fout.write(str_line6)
print("\nStatistics:")
print(str_line1)
print(str_line2)
print(str_line3)
print(str_line4)
print(str_line5)
print(str_line6)
print("\nPlotting ...")
plt.bar(range(len(time_intervals)), time_intervals)
plt.xlabel("Frame number")
plt.ylabel("Time interval")
plt.title("Mean interval:" + str(round(mean_interval, 9)) +
"(sec)\nMean FPS:" + str(round(mean_fps, 1)) +
" Variance:" + str(round(var_interval, 5)))
plt.axhline(mean_interval, color='orange', label='Mean')
plt.legend()
plt.savefig("./vision_report_" + cur_time + ".png", dpi=600)
plt.show()
核心就是通过获取每一帧影像的时间戳然后计算时间差异，进而统计指标，从而判断帧率是否稳定。比如，我们测试EuRoC MH04序列的影像帧率，得到的结果如下。
通过图表和统计数据可以看到，影像流非常稳定。最大和最小时间间隔差异只有10的负7次方数量级。
而再测试一下前些日子我们自己用D435i采集的数据集，结果如下。
可以看到总体还可以，但局部还是有一些“层次不齐”。尤其是最大和最小时间间隔的差异到了0.13秒。这就会给之后的SLAM应用带来一定的隐患。从这也可以看出，数据采集本身不是一个容易的事情。EuRoC之所以能称为世界公认的数据集是有道理的，需要考虑的事情有很多。测试代码上传到了Github项目，点击查看。
2.IMU流帧率检查
IMU数据的存储和影像不太一样。对于影像而言，每一帧都存为一个图片文件。而IMU数据一般都是放在一个文件中，每一行代表一个观测。所以对于IMU数据，也有专门的脚本，如下。
# coding=utf-8
from matplotlib import pyplot as plt
import sys
import numpy as np
import time
def readTimestampsEuRoC(file_path, time_unit):
timestamps = []
fin = open(file_path, "r")
line = fin.readline().strip()
line = fin.readline().strip()
while line:
timestamp = line.split(",")[0]
if time_unit == 's' or time_unit == 'sec':
timestamps.append(float(timestamp))
elif time_unit == 'ms' or time_unit == 'MS':
timestamps.append(float(timestamp) / 1000.0)
elif time_unit == 'us' or time_unit == 'US':
timestamps.append(float(timestamp) / 1000000.0)
elif time_unit == 'ns' or time_unit == 'NS':
timestamps.append(float(timestamp) / 1000000000.0)
elif time_unit == 'ps' or time_unit == 'PS':
timestamps.append(float(timestamp) / 1000000000000.0)
line = fin.readline().strip()
return timestamps
if __name__ == '__main__':
# 用于检查IMU数据流帧率是否稳定
time_file = sys.argv[1] # 待检查IMU数据文件路径
time_unit = sys.argv[2] # IMU观测时间戳单位
print("Loading IMU data ...")
timestamps = readTimestampsEuRoC(time_file, time_unit)
print("Loaded IMU data!")
cur_time = time.strftime("%Y_%m_%d_%H_%M_%S", time.localtime())
fout = open("./imu_report_" + cur_time + ".txt", 'w')
time_intervals = []
for i in range(1, len(timestamps)):
time_intervals.append(timestamps[i] - timestamps[i - 1])
mean_interval = np.mean(time_intervals)
max_interval = max(time_intervals)
min_interval = min(time_intervals)
var_interval = np.var(time_intervals)
mean_fps = 1.0 / mean_interval
max_difference = max_interval - min_interval
index_max = time_intervals.index(max_interval)
index_min = time_intervals.index(min_interval)
timestamp_max = timestamps[index_max]
timestamp_min = timestamps[index_min]
str_line1 = "max interval:" + str(max_interval) + "(sec), index:" + str(index_max) + "/" + str(
len(timestamps)) + ", timestamp:" + str(timestamp_max) + "\n"
str_line2 = "min interval:" + str(min_interval) + "(sec), index:" + str(index_min) + "/" + str(
len(timestamps)) + ", timestamp:" + str(timestamp_min) + "\n"
str_line3 = "mean interval:" + str(mean_interval) + "\n"
str_line4 = "variance interval:" + str(var_interval) + "\n"
str_line5 = "max interval difference:" + str(max_difference) + "\n"
str_line6 = "mean fps:" + str(mean_fps) + "\n"
fout.write(str_line1)
fout.write(str_line2)
fout.write(str_line3)
fout.write(str_line4)
fout.write(str_line5)
fout.write(str_line6)
print("\nStatistics:")
print(str_line1)
print(str_line2)
print(str_line3)
print(str_line4)
print(str_line5)
print(str_line6)
print("\nPlotting ...")
plt.bar(range(len(time_intervals)), time_intervals)
plt.xlabel("Frame number")
plt.ylabel("Time interval")
plt.title("Mean interval:" + str(round(mean_interval, 9)) +
"(sec)\nMean FPS:" + str(round(mean_fps, 1)) +
" Variance:" + str(round(var_interval, 5)))
plt.axhline(mean_interval, color='orange', label='Mean')
plt.legend()
plt.savefig("./imu_report_" + cur_time + ".png", dpi=600)
plt.show()
类似的，我们也对EuRoC MH04的IMU数据进行检查，结果如下。
可以看到，整体依然保持了非常高的稳定性，最大最小间隔的差异仍然在10的-7次方量级。
再对我们采集的IMU数据进行检查，如下。
可以看到，相比于EuRoC，稳定性差了很多。虽然总体还在同一个水平上，但是存在一些个别的粗差。测试代码同样上传到了Github项目，点击查看。
本文作者原创,未经许可不得转载,谢谢配合
