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) + " " + 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哈哈~
参考：
标签：D435i,离线,list,RealSense,bag,str,timestamp,csv,data 来源： https://blog.csdn.net/weixin_38141453/article/details/110880281