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

Posted 2022-11-28 猛龙过江ing

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

经过之前坚持不懈的探索，已经先后实现了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) + "   " + str(b) + "   " + str(time) + "  " + str(
            list_gyro[a][2]) + "  " + str(list_gyro[b][2]))

        if (time < list_gyro[a][2]):  # 如果加速度计的时间比陀螺仪最小的时间都小，也有可能是时间戳发生跳动
            print("小于   " + str(i) + "   " + str(a) + "   " + str(b))
            continue
        elif (tail == 1):  # 如果加速度计的时间比陀螺仪最大的时间都大
            print("尾部   " + str(i) + "   " + str(a) + "   " + str(b))
            csv_data = [list_acce[i + 1][2], list_gyro[b][17], list_gyro[b][18], list_gyro[b][19],
                        list_acce[i + 1][29], list_acce[i + 1][30], list_acce[i + 1][31]]  # 使用最早的数据和零来进行计算
            csv_write.writerow(csv_data)
            continue

        while list_gyro[b][2] < time:  # 当当前时间大于2号时间时
            print("大于   " + str(i) + "   " + str(a) + "   " + str(b))
            if b < (len(list_gyro) - 1):  # 如果b后面还有数据就改变索引值
                print("改变   " + str(i) + "   " + str(a) + "   " + str(b))
                a = b
                b += 1
            else:  # 如果没有就用b的数据进行计算
                print("进尾   " + str(i) + "   " + str(a) + "   " + str(b))
                csv_data = [list_acce[i + 1][2], list_gyro[b][17], list_gyro[b][18], list_gyro[b][19],
                            list_acce[i + 1][29], list_acce[i + 1][30], list_acce[i + 1][31]]  # 使用最早的数据和零来进行计算
                csv_write.writerow(csv_data)
                tail = 1
                break

        if (tail == 0):
            print("填充   " + str(i) + "   " + str(a) + "   " + str(b))
            # 只要不是以上两种情况，就是时间位于1号时间和2号时间之间，进行计算
            weight_pre, weight_cur = calculate(float(list_acce[i + 1][2]), float(list_gyro[a][2]),
                                               float(list_gyro[b][2]))
            csv_data = [list_acce[i + 1][2],
                        float(list_gyro[a][17]) * weight_pre + float(list_gyro[b][17]) * weight_cur,
                        float(list_gyro[a][18]) * weight_pre + float(list_gyro[b][18]) * weight_cur,
                        float(list_gyro[a][19]) * weight_pre + float(list_gyro[b][19]) * weight_cur,
                        list_acce[i + 1][29], list_acce[i + 1][30], list_acce[i + 1][31]]
            csv_write.writerow(csv_data)

到这里IMU数据就准备好了，接下来进行视觉数据的处理。视觉数据的处理比较简单，把图片提取出来就可以了，注意要以时间戳来命名。同样，给出Python代码。请注意修改路径和文件名称。

# coding:utf-8
#!/usr/bin/python
 
# Extract images from a bag file.
 
#PKG = 'beginner_tutorials'
import roslib   #roslib.load_manifest(PKG)
import rosbag
import rospy
import decimal
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
 
# Reading bag filename from command line or roslaunch parameter.
#import os
#import sys
 
camera0_path = '/home/dong/Data/cam0/data/'
 
class ImageCreator():
 
    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('/home/dong/a.bag', 'r') as bag:  #要读取的bag文件；
            for topic,msg,t in bag.read_messages():
                if topic == "/device_0/sensor_1/Color_0/image/data": #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print e
                        timestr = msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        timer = 1000000000 * timestr
                        image_name = "%d" % timer + ".png" #图像命名：时间戳.png
                        cv2.imwrite(camera0_path + image_name, cv_image)  #保存；
                
 
if __name__ == '__main__':
 
    #rospy.init_node(PKG)
 
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

三、进行数据组织

在data文件夹下建立cam0和imu0两个文件夹，cam0文件夹中再建立data文件夹，以时间戳命名的图片数据放在data文件夹中，IMU数据即data.csv放置在imu0文件夹中。

四、生成ROS bag数据

这部分内容其实在上一篇博客ASL数据转换为ROS bag数据中已经介绍过了，这里不再详细介绍。不过由于realsense相机的配置文件和EuRoC公开数据集配置文件的topic不同，所以加了一个判断来进行区别。后来一想其实如果不加直接把配置文件里的topic改成一样的也行，而且更方便。加判断可能更科学一些吧，O(∩_∩)O哈哈~，请注意修改路径和文件名。

#!/usr/bin/env python
print
"importing libraries"

import rosbag
import rospy
from sensor_msgs.msg import Image
from sensor_msgs.msg import Imu
from geometry_msgs.msg import PointStamped
# import ImageFile
# import PIL.ImageFile as ImageFile
import time, sys, os
import argparse
import cv2
import numpy as np
import csv

# structure
# dataset/cam0/data/TIMESTAMP.png
# dataset/camN/data/TIMESTAMP.png
# dataset/imu0/data.csv
# dataset/imuN/data.csv
# dataset/leica0/data.csv

# setup the argument list
parser = argparse.ArgumentParser(description='Create a ROS bag using the images and imu data.')
parser.add_argument('--folder', metavar='folder', nargs='?', help='Data folder')
parser.add_argument('--datatype',metavar='datatype',default="standard",help='my or standard')
parser.add_argument('--output_bag', metavar='output_bag', default="output.bag", help='ROS bag file %(default)s')

# print help if no argument is specified
if len(sys.argv) < 2:
    parser.print_help()
    sys.exit(0)

# parse the args
parsed = parser.parse_args()


def getImageFilesFromDir(dir):
    '''Generates a list of files from the directory'''
    image_files = list()
    timestamps = list()
    if os.path.exists(dir):
        for path, names, files in os.walk(dir):
            for f in files:
                if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg']:
                    image_files.append(os.path.join(path, f))
                    timestamps.append(os.path.splitext(f)[0])
                    # sort by timestamp
    sort_list = sorted(zip(timestamps, image_files))
    image_files = [file[1] for file in sort_list]
    return image_files


def getCamFoldersFromDir(dir):
    '''Generates a list of all folders that start with cam e.g. cam0'''
    cam_folders = list()
    if os.path.exists(dir):
        for path, folders, files in os.walk(dir):
            for folder in folders:
                if folder[0:3] == "cam":
                    cam_folders.append(folder)
    return cam_folders


def getImuFoldersFromDir(dir):
    '''Generates a list of all folders that start with imu e.g. cam0'''
    imu_folders = list()
    if os.path.exists(dir):
        for path, folders, files in os.walk(dir):
            for folder in folders:
                if folder[0:3] == "imu":
                    imu_folders.append(folder)
    return imu_folders


def getImuCsvFiles(dir):
    '''Generates a list of all csv files that start with imu'''
    imu_files = list()
    if os.path.exists(dir):
        for path, folders, files in os.walk(dir):
            for file in files:
                if file[0:3] == 'imu' and os.path.splitext(file)[1] == ".csv":
                    imu_files.append(os.path.join(path, file))
    return imu_files


def loadImageToRosMsg(filename):
    image_np = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)

    timestamp_nsecs = os.path.splitext(os.path.basename(filename))[0]
    timestamp = rospy.Time(secs=int(timestamp_nsecs[0:-9]), nsecs=int(timestamp_nsecs[-9:]))

    rosimage = Image()
    rosimage.header.stamp = timestamp
    rosimage.height = image_np.shape[0]
    rosimage.width = image_np.shape[1]
    rosimage.step = rosimage.width  # only with mono8! (step = width * byteperpixel * numChannels)
    rosimage.encoding = "mono8"
    rosimage.data = image_np.tostring()

    return rosimage, timestamp


def createImuMessge(timestamp_int, omega, alpha):
    timestamp_nsecs = str(timestamp_int)
    timestamp = rospy.Time(int(timestamp_nsecs[0:-9]), int(timestamp_nsecs[-9:]))

    rosimu = Imu()
    rosimu.header.stamp = timestamp
    rosimu.angular_velocity.x = float(omega[0])
    rosimu.angular_velocity.y = float(omega[1])
    rosimu.angular_velocity.z = float(omega[2])
    rosimu.linear_acceleration.x = float(alpha[0])
    rosimu.linear_acceleration.y = float(alpha[1])
    rosimu.linear_acceleration.z = float(alpha[2])

    return rosimu, timestamp


# create the bag
bag = rosbag.Bag(parsed.output_bag, 'w')

# write images
camfolders = getCamFoldersFromDir(parsed.folder)
for camfolder in camfolders:
    camdir = parsed.folder + "/0".format(camfolder) + "/data"
    image_files = getImageFilesFromDir(camdir)
    for image_filename in image_files:
        image_msg, timestamp = loadImageToRosMsg(image_filename)
        if parsed.datatype=='my':
            bag.write("/camera/color/image_raw", image_msg, timestamp)
        else:
            bag.write("/0/image_raw".format(camfolder), image_msg, timestamp)


# write imu data
imufolders = getImuFoldersFromDir(parsed.folder)
for imufolder in imufolders:
    imufile = parsed.folder + "/" + imufolder + "/data.csv"
    topic = os.path.splitext(os.path.basename(imufolder))[0]
    with open(imufile, 'rb') as csvfile:
        reader = csv.reader(csvfile, delimiter=',')
        headers = next(reader, None)
        for row in reader:
            imumsg, timestamp = createImuMessge(row[0], row[1:4], row[4:7])
            if parsed.datatype == 'my':
                bag.write("/camera/imu", imumsg, timestamp)
            else:
                bag.write("/0".format(topic), imumsg, timestamp)


# write leica data
# leicafile = parsed.folder + "/leica0/data.csv"
# with open(leicafile, 'rb') as csvfile:
#     reader = csv.reader(csvfile, delimiter=',')
#     headers = next(reader, None)
#     for row in reader:
#         timestamp_nsecs = str(row[0])
#         timestamp = rospy.Time( int(timestamp_nsecs[0:-9]), int(timestamp_nsecs[-9:]) )
#
#         pos = PointStamped()
#         pos.header.stamp = timestamp
#         pos.point.x = float(row[1])
#         pos.point.y = float(row[2])
#         pos.point.z = float(row[3])
#         bag.write("/leica/position", pos, timestamp)

bag.close()

调用方法：

python '/home/dong/Code/zip2bag/zip2bag.py'  --folder /home/dong/Data --datatype my

五、VINS离线运行

数据组织工作到这里就已经全部结束了。之后就可以调用VINS跑起来了，不熟悉操作的小伙伴可以参考博客：RealSense D435i数据录制 VINS离线运行。最后附上一张成功运行的图片，这才叫有图有真相嘛，O(∩_∩)O哈哈~

好了，这篇博客就到这里了。不出意外的话，这应该就是2020年的倒数第二篇了，之后还会发一篇年终总结与新年展望，现在还没开始写，但是肯定能在2021到来之前完成的，O(∩_∩)O哈哈~

参考：

1、realsense相机Android端安装包

2、利用ROS工具从bag包中提取图片和.csv文件

3、python 读写csv文件（创建，追加，覆盖）

4、ASL数据转换为ROS bag数据