forked from FrancescoGradi/RealSense_T265_evaluator
-
Notifications
You must be signed in to change notification settings - Fork 0
/
saveTrajectory.py
executable file
·42 lines (32 loc) · 1.56 KB
/
saveTrajectory.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
import pyrealsense2 as rs
import numpy as np
import time
def saveTrajectory(filename='trajectory'):
pipe = rs.pipeline()
cfg = rs.config()
cfg.enable_stream(rs.stream.pose)
pipe.start(cfg)
trajectory = []
start = time.time()
try:
while True:
frames = pipe.wait_for_frames()
pose = frames.get_pose_frame()
if pose:
data = pose.get_pose_data()
trajectory.append([data.translation.x, data.translation.y, data.translation.z,
data.velocity.x, data.velocity.y, data.velocity.z,
data.acceleration.x, data.acceleration.y, data.acceleration.z,
data.rotation.x, data.rotation.y, data.rotation.z, data.rotation.w,
data.angular_velocity.x, data.angular_velocity.y, data.angular_velocity.z,
data.angular_acceleration.x, data.angular_acceleration.y, data.angular_acceleration.z,
data.tracker_confidence, data.mapper_confidence])
finally:
print("Saving...")
print("Total frames: #{}".format(pose.frame_number))
end = time.time()
elapsed_time = end - start
print("Time elapsed: " + str(elapsed_time))
np.savetxt("../data/{}_{}_{}.csv".format(filename, str(time.strftime("%H_%M_%S", time.gmtime(elapsed_time))),
str(time.time())), np.asarray(trajectory), delimiter=",")
pipe.stop()