Python融合Video+Imu数据，转bag包

github链接（ADVIO）

目录

源代码

合并IMU数据

生成bag包

代码关键点讲解

---

源代码

合并IMU数据

# -*- coding: utf-8 -*-
# Synchronize the accelerometer and gyroscope data  #同步加速度计和陀螺仪数据
# Description:  #描述
#   Create data structure containing IMU and ARKit data.  #创建包含IMU和ARKit数据的数据结构
# Arguments:  #参数
#   -data folder #数据文件夹
#
# Copyright (C) 2018 Santiago Cortes
#
# This software is distributed under the GNU General Public 
# Licence (version 2 or later); please refer to the file 
# Licence.txt, included with the software, for details.

import sys
import pandas as pd
import numpy as np

## Data folder argument
##folder= sys.argv[1]
folder = '../../../'
##for i in range(1,24):
for i in '2':
    #Read data
    path= folder+'/advio-'+str(i).zfill(2)+'/iphone/arkit.csv'
    arkit=pd.read_csv(path,names=list('tabcdefg'))
    path= folder+'/advio-'+str(i).zfill(2)+'/iphone/accelerometer.csv'
    acc=( pd.read_csv(path,names=list('tabc')))
    path= folder+'/advio-'+str(i).zfill(2)+'/iphone/gyro.csv'
    gyro=( pd.read_csv(path,names=list('tabc')))
    g=[]
    a=[]
    t=np.array(list(map(float,gyro[list('t')].values)))
    zer=t*0.0
    # Make imu
    for c in 'abc':
    # Interpolate accelerometer data to match gyroscope timestamps. #插入加速计数据以匹配陀螺仪时间戳
        a.append(np.interp( np.array(list(map(float,gyro[list('t')].values))), np.array(list(map(float,acc[list('t')].values))), np.array(list(map(float,acc[list(c)].values)))))
        g.append(np.array(list(map(float,gyro[list(c)].values))))
        #np.interp(x,xp,fp) 以(x[,fp)做曲线，返回x点的y坐标  秒啊！
    M=np.column_stack((t,zer+34,g[0],g[1],g[2],a[0],a[1],a[2],zer,zer))
    #np.column_stack()将若干矩阵按列合并
    v=[]
    t=np.array(list(map(float,arkit[list('t')].values)))
    t=t-t[0]


    zer=t*0
    #Make arkit data
    for c in 'abcdefg':
        v.append(np.array(list(map(float,arkit[list(c)].values))))
    Mkit=np.column_stack((t,zer+7,np.arange(len(zer)),v[0],v[1],v[2],v[3],v[4],v[5],v[6]))
    full=np.concatenate((M,Mkit))
    #sort to time vector
    full = full[full[:,0].argsort()]
    path= folder+'/advio-'+str(i).zfill(2)+'/iphone/imu-gyro.csv'
    np.savetxt(path, full, delimiter=",",fmt='%.6f')

生成bag包

# -*- coding: utf-8 -*-
# Create ROSBAG with the IMU and uncompressed video data 
#
# Description:
#   Create data structure containing IMU and ARKit data.
#
# Arguments: #参数
#   - Data folder.
#   - sequences to bag #顺序
#
# Copyright (C) 2018 Santiago Cortes
#
# This software is distributed under the GNU General Public 
# Licence (version 2 or later); please refer to the file 
# Licence.txt, included with the software, for details.

# Import system libraries 
import sys, os
import csv
import numpy as np
# Import ros libraries.
import rosbag
from cv_bridge import CvBridge, CvBridgeError
import roslib
##roslib.load_manifest('sensor_msgs')
from sensor_msgs.msg import Image
from sensor_msgs.msg import Imu
#Deal with ros and opencv path error.
##if '/opt/ros/kinetic/lib/python2.7/dist-packages'in sys.path:
##    sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
import rospy
# Import opencv
import cv2

##Read arguments
##Folder
##folder= sys.argv[1]
folder = '../../..'
tst = '2'
# If number argument is added, just build that rosbag. Else, build all.
##try:
##    sys.argv[2]
##except:
##    tst = range(1,24)
##else:#没有异常的时候执行else
##    tst = [int(sys.argv[2])]

print('Creating rosbags for sequences in '+ str(tst))
    
# Delete first frames
start=0.5

# Loop trough all sequences.
for i in iter(tst):
    # Find folder
    dir =folder+'/advio-'+str(i).zfill(2)+'/iphone'
    print(dir)
    
    # Initialize bag, data and video reader.
    bag =rosbag.Bag(dir+'/'+'iphone.bag', 'w')
    index=0
    csvfile=open(dir+'/'+'imu-gyro.csv')
    datareader = csv.reader(csvfile, delimiter=' ', quotechar='|') #???
    
   # Initialize bridge.
    bridge=CvBridge()
    
    # Read video and check for number of frames.
    vidcap = cv2.VideoCapture(dir+'/'+'frames.mov')
    vlength = int(vidcap.get(cv2.CAP_PROP_FRAME_COUNT)) #帧的数目
    
    # For each row in the data matrix
    for row in datareader:
        # Read data row
        sp=row[0].split(",") #row[0]是数据
        
        # If the row correspond to a frame
        if int(float(sp[1]))==7:
            # Read next frame and check.
            success,image = vidcap.read()
            if success and float(sp[0])>start:
                
                
                stamp=float(sp[0])
                Stamp = rospy.rostime.Time.from_sec(stamp)

                # Put image in corect orientation and convert into grayscale.
                gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
                gray_image=cv2.transpose(gray_image)
                gray_image=cv2.flip(gray_image,+1)
                
                # Print progress.
                if index%500==0:
                    print(str(int(np.round(100*float(index)/float(vlength))))+'%')#显示进度
                index=index+1
 
                # Create ros image and put frame in it.
                Img = Image()
                Img=bridge.cv2_to_imgmsg(gray_image,encoding='mono8')
                Img.header.stamp = Stamp
                
                # Put image in rosbag.
                bag.write('/cam0/image_raw', Img, Stamp)
  
        # If the row corresponds to an IMU measurement.#对应imu
        elif int(float(sp[1]))==34 and float(sp[0])>start:
            
            # check time in row and make ros format.
            stamp=float(sp[0])
            Stamp = rospy.rostime.Time.from_sec(stamp)
            
            # Read acceleration and rotational rate.
            gyr=(sp[2:5:1])
            acc=(sp[5:8:1])
            
            # Create imu ros structure and put in the bias corrected values.
            imu=Imu()
            imu.header.stamp=Stamp
            imu.angular_velocity.x=float(gyr[0])+0.0067
            imu.angular_velocity.y=float(gyr[1])-0.0070
            imu.angular_velocity.z=float(gyr[2])+0.0065

            imu.linear_acceleration.x=(float(acc[0])-0.0407)
            imu.linear_acceleration.y=(float(acc[1])+0.0623)
            imu.linear_acceleration.z=(float(acc[2])-0.1017)
            
            # Put Imu measurement in rosbag.
            bag.write('/imu0',imu,Stamp)
    # Finish rosbag
    bag.close() 
print('Process complete')

代码关键点讲解

1、仅仅用ARKit的时间戳作为Video的时间戳

2、以陀螺仪时间戳为基准，从加速度曲线中线性插值得到相同时间戳下的数据，并且对其数据

3、sync-data.py中的[zer+7,zer=t*0,zer,zer]作用为矩阵合并时将列数补齐

4、记得修改folder路径名和数据集序列号

5、传感器内参和外参可以在给出的github链接中找到