SLAMデータ処理ウィジェットメモ
56480 ワード
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ライブラリに定義され、ヘッダファイル
2.EuRoc2TUMTUM ground truth形式:timestamp tx ty tz qy qz qw EUROC ground truth形式:timestamp tx ty tz qw qx qy qz Euroc2Tum.py
3.file2ROSBag ref.
画像ファイルおよびIMUをROSBAgに変換する.Transform2ROS.py
4.各行の末尾のスペースを削除
テストの時、ファイルデータセットに対して厳格な要求があって、またコードを再変更してデータセットの結果を走りたくなくて、直接結果のフォーマットに対して変更を行います.
SpaceDelete.py
5.Matrix Inverse
標定結果に対して行列に対して逆を求める必要がある場合がある.MatricxInverse.cpp
MakeLists.txt
0.はじめに
一部のデータセットの相互処理と最近使用されたいくつかのデータ処理ウィジェットを忘れておく.
1.プログラム実行時間計算
C++のライブラリ関数で、プログラムの実行時間に使用される知識点を計算します.
clock_tデータ型、long型は、一定期間のclocks数、すなわちCPUの運転ユニット時間を記録するために用いられる.
戻りタイプclock_tは,プログラムからclock()関数を呼び出すまでの間のclocksを返す.
これらのライブラリ関数、タイプ、定数は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
#!/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)+'
')
my_output_file.close()
3.file2ROSBag
画像ファイルおよび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('
').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('
').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
')
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 =
" << 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 )