标签：D435i 离线 list RealSense bag str timestamp csv data
聪明者戒太察，刚强者戒太暴，温良者戒无断。——（清）金缨《格言联璧·持躬类》
经过之前坚持不懈的探索，已经先后实现了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
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。