SLAM数据处理小程序备忘
SLAM数据处理小程序备忘
- 0.引言
- 1.程序运行时间计算
- 2.EuRoc2TUM
- 3.file2ROSBag
- 4.去除每行头尾的空格
- 5.Matrix Inverse
0.引言
备忘部分数据集相互处理以及近期用到的一些数据处理小程序.
1.程序运行时间计算
C++的库函数中，计算程序运行时间使用的知识点：
- clock_t
clock_t数据类型，long型，用来记录一段时间内的clocks数，即CPU的运行单元时间。
- clock()
返回类型clock_t，返回的是从程序开始，到调用用clock()函数这段时间的clocks。
- CLOCKS_PER_SEC
这些库函数、类型和常量都是定义在ctime库中的，头文件time.h。计算程序的运行时间，只需根据程序的入口点和出口点出计算clocks，再算差就可以。
int main() {
clock_t start, end;
start = clock();
//测试对象
end = clock();
cout<<"Run time: "<<(double)(end - start) / CLOCKS_PER_SEC<<"S"<<endl;
return 0;
}
2.EuRoc2TUM
- TUM ground truth格式: timestamp tx ty tz qx qy qz qw
- EUROC ground truth格式: timestamp tx ty tz qw qx qy qz
Euroc2Tum.py
#!/usr/bin/python
# -*- coding: UTF-8 -*-
import csv
# 输入csv文件名称和输出txt文件名称
csv_file = raw_input('Enter the name of your input csv file: ')
txt_file = raw_input('Enter the name of your output txt file: ')
with open(txt_file, "w") as my_output_file:
with open(csv_file, "r") as my_input_file:
#逐行读取csv存入txt中
for row in csv.reader(my_input_file):
# 前8个数据是:timestamp tx ty tz qw qx qy qz
row = row[0:8]
# 时间戳单位处理
temp1 = row[0][0:10] + '.' + row[0][10:16]
row[0] = temp1
# 互换 qw 和 qx
temp2 = row[4]
row[4] = row[7]
row[7] = temp2
my_output_file.write(" ".join(row)+'\n')
my_output_file.close()
3.file2ROSBag
- ref.
将图片文件以及IMU转换为ROSBag.Transform2ROS.py
#import cv2
import time, sys, os
from ros import rosbag
import roslib
import rospy
roslib.load_manifest('sensor_msgs')
from sensor_msgs.msg import Image,Imu
from geometry_msgs.msg import Vector3
# import ImageFile
from PIL import ImageFile
from PIL import Image as ImagePIL
'''sort image name'''
def CompSortFileNamesNr(f):
g = os.path.splitext(os.path.split(f)[1])[0]
numbertext = ''.join(c for c in g if c.isdigit())
return int(numbertext)
'''get image from dir'''
def ReadImages(dir):
'''Generates a list of files from the directory'''
print( "Searching directory %s" % dir )
all = []
left_files = []
right_files = []
if os.path.exists(dir):
for path, names, files in os.walk(dir + '/cam0/data'):
# for f in files:
for f in sorted(files, key=CompSortFileNamesNr):
if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg']:
if 'left' in f or 'left' in path or '/cam0/data' in f or '/cam0/data' in path :
left_files.append( os.path.join( path, f ) )
elif 'right' in f or 'right' in path or '/cam1/data' in f or '/cam1/data' in path :
right_files.append( os.path.join( path, f ) )
all.append( os.path.join( path, f ) )
for path, names, files in os.walk(dir + '/cam1/data'):
# for f in files:
for f in sorted(files, key=CompSortFileNamesNr):
if os.path.splitext(f)[1] in ['.bmp', '.png', '.jpg']:
if 'left' in f or 'left' in path or '/cam0/data' in f or '/cam0/data' in path :
left_files.append( os.path.join( path, f ) )
elif 'right' in f or 'right' in path or '/cam1/data' in f or '/cam1/data' in path :
right_files.append( os.path.join( path, f ) )
all.append( os.path.join( path, f ) )
#return all, left_files, right_files
return left_files, right_files
def ReadIMU(filename):
'''return IMU data and timestamp of IMU'''
file = open(filename,'r')
all = file.readlines()[1:]
timestamp = []
imu_data = []
for f in all:
line = f.rstrip('\n').split(',')
timestamp.append(line[0])
imu_data.append(line[1:])
return timestamp,imu_data
def CreateStereoBag(args):
'''read image'''
left_imgs, right_imgs = ReadImages(args[0])
'''read time stamps'''
imagetimestamps=[]
file = open(args[1], 'r')
all = file.readlines()[1:] # skip the first line.
for f in all:
imagetimestamps.append(f.rstrip('\n').split(',')[0])
file.close()
'''read imu timestamps and data'''
imutimesteps,imudata = ReadIMU(args[2])
'''Creates a bag file containing stereo image and imu pairs'''
#if not os.path.exists(args[3]):
#os.system(r'touch %s' % args[3])
bag = rosbag.Bag(args[3], 'w')
try:
for i in range(len(imudata)):
imu = Imu()
angular_v = Vector3()
linear_a = Vector3()
angular_v.x = float(imudata[i][0])
angular_v.y = float(imudata[i][1])
angular_v.z = float(imudata[i][2])
linear_a.x = float(imudata[i][3])
linear_a.y = float(imudata[i][4])
linear_a.z = float(imudata[i][5])
#imuStamp = rospy.rostime.Time.from_sec(float( (float(imutimesteps[i]))/1e6 ))
imuStamp = rospy.rostime.Time.from_sec(float(imutimesteps[i]))
imu.header.stamp=imuStamp
imu.angular_velocity = angular_v
imu.linear_acceleration = linear_a
bag.write("/imu0",imu,imuStamp)
for i in range(len(left_imgs)):
print("Adding %s" % left_imgs[i])
fp_left = open( left_imgs[i], "r" )
p_left = ImageFile.Parser()
'''read image size'''
imgpil = ImagePIL.open(left_imgs[0])
width, height = imgpil.size
while 1:
s = fp_left.read(1024)
if not s:
break
p_left.feed(s)
im_left = p_left.close()
fp_right = open( right_imgs[i], "r" )
print("Adding %s" % right_imgs[i])
p_right = ImageFile.Parser()
while 1:
s = fp_right.read(1024)
if not s:
break
p_right.feed(s)
im_right = p_right.close()
# Stamp = roslib.rostime.Time.from_sec(time.time())
# Stamp = rospy.rostime.Time.from_sec(float( (float(imagetimestamps[i]))/1e6 ))
Stamp = rospy.rostime.Time.from_sec(float(imagetimestamps[i]))
# left image
Img_left = Image()
Img_left.header.stamp = Stamp
Img_left.width = width
Img_left.height = height
Img_left.header.frame_id = "camera/left"
Img_left.encoding = "mono8"
Img_left_data = [pix for pixdata in [im_left.getdata()] for pix in pixdata]
Img_left.step = Img_left.width
Img_left.data = Img_left_data
Img_right = Image()
Img_right.header.stamp = Stamp
Img_right.width = width
Img_right.height = height
Img_right.header.frame_id = "camera/right"
Img_right.encoding = "mono8"
Img_right_data = [pix for pixdata in [im_right.getdata()] for pix in pixdata]
Img_right.step = Img_right.width
Img_right.data = Img_right_data
bag.write('/cam0/image_raw', Img_left, Stamp)
bag.write('/cam1/image_raw', Img_right, Stamp)
finally:
bag.close()
if __name__ == "__main__":
if len(sys.argv) == 5:
print(sys.argv)
CreateStereoBag(sys.argv[1:])
else:
print( "Usage: img_file,img_timestamps_file, imu_file, bagname")
print( "Example: python3 img2bag_Stereoimu.py /*/00 data.csv imu.csv img2bag_Stereoimu.bag")
#img_file,img_timestamps_file, imu_file, bagname
4.去除每行头尾的空格
在测试的时候，对文件数据集有严格要求，又不想重新改代码跑数据集结果，就直接对结果格式进行更改。
SpaceDelete.py
#!/usr/bin/python
# -*- coding: UTF-8 -*-
def trimLineSpace(finput, foutput):
fInputHandle = open ( fInput )
fOutputHandle = open (fOutput, 'w')
lines = fInputHandle.readlines()
for line in lines:
fOutputHandle.write(line.strip()+'\r\n')
fInputHandle.close()
fOutputHandle.close()
if __name__ == '__main__':
fInput = raw_input('Enter the name of your input txt file: ')
fOutput = raw_input('Enter the name of your output txt file: ')
trimLineSpace(fInput, fOutput)
5.Matrix Inverse
对标定结果有时候需要对矩阵求逆.
MatricxInverse.cpp
#include matrix_44_cam0_inv;
//Eigen::Matrix matrix_44_cam1_inv;
//cout << "matrix_44_cam0_inv: "<< endl<< setprecision(10)<
//cout << "matrix_44_cam1_inv: "<< endl<< setprecision(10)<
Eigen::Matrix3d rotation_matrix;
rotation_matrix << -9.9951465899298464e-01, 7.5842033363785165e-03,
-3.0214670573904204e-02, 2.9940114644659861e-02,
-3.4023430206013172e-02, -9.9897246995704592e-01,
-8.6044170750674241e-03, -9.9939225835343004e-01,
3.3779845322755464e-02;
Eigen::Vector3d v;
v << 4.4511917113940799e-02, -7.3197096234105752e-02,-4.7972907300764499e-02;
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 虽然称为3d，实质上是4＊4的矩阵
T.rotate ( rotation_matrix ); // 按照rotation_vector进行旋转
T.pretranslate (v); // 把平移向量
cout << "转换矩阵 Transform matrix = \n" << T.matrix() <<endl;
T2Rt();
return 0;
}
MakeLists.txt
cmake_minimum_required( VERSION 2.8 )
project( MatricxInverse )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
include_directories( "/usr/local/include/eigen3" )
add_executable( MatricxInverse MatricxInverse.cpp )