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

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

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

一、Android端数据录制

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

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

二、解析提取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哈哈~

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

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

参考:

1、realsense相机Android端安装包

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

3、python 读写csv文件(创建,追加,覆盖)

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

上一篇:Intel Realsense D435 报错 Couldn't resolve requests


下一篇:RealsenseD415/D435深度相机常用资料汇总