Android+RealSense D435i数据录制 VINS离线运行

聪明者戒太察，刚强者戒太暴，温良者戒无断。——（清）金缨《格言联璧·持躬类》

经过之前坚持不懈的探索，已经先后实现了VINS在公开数据集上的运行、PC端+相机VINS在线运行、PC端数据录制VINS离线运行以及ROSbag数据的生成。但是要是想进行室外实际场景的实验，带着电脑进来采集数据实在是一个苦差事，电脑笨重不说，光是凛冽的寒风也让人受不了。因此，本人不断探索，终于实现了在Android移动终端上进行数据录制，之后在电脑端进行一些数据处理，主要是bag数据的加速度计数据和陀螺仪数据合并成一个topic，然后就可以在VINS上进行运行了。

一、Android端数据录制

首先，实名感谢z2309323博主张师兄提供的Android端相机数据录制APP安装包，下载地址：realsense相机Android端安装包。安装之后打开，连接好相机，就可以看到相机的输出了，界面如下图所示。点击红色按钮即可进行数据的录制，录制的数据会保存在realsense文件夹中，在设置一栏中可以选择需要录制的数据、修改数据的频率等。

二、解析提取bag数据

由于Android端录制的数据的topic和VINS需要的相距甚远，尤其是加速度计和陀螺仪的数据还被分开了，所以需要将相关的数据从bag中提取出来，进行相应的处理之后再生成理想的bag数据包。

首先，提取加速度计和陀螺仪的数据到相应的csv文件，请注意修改路径和文件名称。

rostopic echo -b '/home/dong/a.bag' -p /device_0/sensor_2/Accel_0/imu/data > Acce_data.csv && rostopic echo -b '/home/dong/a.bag' -p /device_0/sensor_2/Gyro_0/imu/data > Gyro_data.csv

之后，将提取的加速度计数据和陀螺仪数据合并。由于两种数据频率不同，时间戳也没有同步，因此需要进行一定的处理。本文采取了反距离加权插值的方法，使用Python语言，以加速度计的频率和时间戳为基准对陀螺仪数据进行插值。代码如下，请注意修改路径和文件名称。

# coding=utf-8
import csv


# 使用反距离加权计算权重
def calculate(timeTarget, time1, time2):
    return [float((time2 - timeTarget) / (time2 - time1)), float((timeTarget - time1) / (time2 - time1))]


# 加速度计频率：250赫兹，陀螺仪频率：200赫兹，相机频率：15赫兹，图像分辨率640×480
# 采用将陀螺仪的数据合并至加速度计，也就是使用少的计算多的
if __name__ == '__main__':
    target = '/home/dong/data.csv'
    csv_write = csv.writer(open(target, 'wb'))
    csv_head = ["#timestamp [ns]", "w_RS_S_x [rad s^-1]", "w_RS_S_y [rad s^-1]", "w_RS_S_z [rad s^-1]",
                "a_RS_S_x [m s^-2]", "a_RS_S_y [m s^-2]", "a_RS_S_z [m s^-2]"]
    csv_write.writerow(csv_head)  # 写入文件头
    list_acce = list(csv.reader(open('/home/dong/Acce_data.csv')))
    list_gyro = list(csv.reader(open('/home/dong/Gyro_data.csv')))
    print(len(list_gyro))  # 数据长度
    print(list_acce[1][2])  # 时间
    print(list_gyro[1][17])  # X角速度
    print(list_gyro[1][18])  # Y角速度
    print(list_gyro[1][19])  # Z角速度
    print(list_acce[1][29])  # X线加速度
    print(list_acce[1][30])  # Y线加速度
    print(list_acce[1][31])  # Z线加速度
    a = 1
    b = 2
    tail = 0
    print(len(list_acce))
    for i in range(0, len(list_acce) - 1):
        time = list_acce[i + 1][2]
        print("进入   " + str(i) + "   " + str(a) + "   "