def read_trajectory(fname: str,
format_spec: str,
min_time: float = -1.0) -> Trajectory:
times = []
poses = []
scol = 0
if format_spec[0].isdigit():
scol = int(format_spec[0])
format_spec = format_spec[1:]
time_scale = 1.0
if format_spec[0] == 'n':
time_scale = 1.0e-9
format_spec = format_spec[1:]
with open(fname, 'r') as file:
reader = csv.reader(file)
# Skip header
next(reader)
for line in reader:
t = float(line[scol + 0]) * time_scale
if t < 0:
continue
if min_time > 0 and t < min_time:
continue
pose = SE3.from_list(line[scol + 1:], format_spec)
times.append(t)
poses.append(pose)
return Trajectory(poses, times)
frame_artist = frameArtist(ax, frame, style)
return frame_artist
def plotArtificialHorizon(attitude: SO3, ax: Axes3D = None):
if ax is None:
ax = plt.gca()
horizon_artist = ArtificialHorizonArtist(ax, attitude)
return horizon_artist
if __name__ == "__main__":
pose = SE3.identity()
pose = SE3.from_list([1, 2, 3], "x")
pose2 = SE3.from_list([4, 5, 6], "x")
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
artist = plotFrame(pose)
plt.pause(1)
artist.set_pose_data(pose2)
plt.show()
plt.cla()
yaw_tf = SO3.from_list(np.array([0, 0, 0.5]), 'r')
att = SO3.from_list(np.array([0.1, 0, 0.]), 'r')
att2 = SO3.from_list(np.array([0.2, 0, 0]), 'r')
Q = mat[0:3,0:3]
t = mat[0:3,3:4]
frame_axes = [[],[],[]]
for a in range(3):
for i in range(3):
frame_axes[a] += [t[a,0], t[a,0]+Q[a,i], np.nan]
# result = ax.plot(frame_axes[0], frame_axes[1], frame_axes[2])
colors = ['r','g','b']
colors = [c+style for c in colors]
result = []
for a in range(3):
temp = ax.plot([t[0,0], t[0,0]+Q[0,a]], [t[1,0], t[1,0]+Q[1,a]], [t[2,0], t[2,0]+Q[2,a]], colors[a])
result.append(temp)
temp = ax.plot(t[0,0], t[1,0], t[2,0], 'ko')
result.append(temp)
return result
if __name__ == "__main__":
pose = SE3.identity()
pose = SE3.from_list([1,2,3], "x")
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plotFrame(pose)
plt.show()
reader = csv.reader(f, delimiter=args.delim)
# Ignore the header
for _ in range(args.srow):
next(reader)
# Read the poses and animate trajectory
poses = []
counter = -1
for line in reader:
counter += 1
if args.skip != 0 and counter % args.skip != 0:
continue
pose = SE3.from_list(line[args.scol:], args.fspec)
poses.append(pose)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
trail = np.hstack([pose.x().x() for pose in poses])
trail_line, = ax.plot(trail[0, :], trail[1, :], trail[2, :], 'm')
ax.set_box_aspect(
np.max(trail, axis=1) - np.min(trail, axis=1) + np.ones(3) * 2)
trail_line.set_data_3d([], [], [])
frame_artist = plotting.plotFrame(pose, '-', ax)
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
type=int,
default=1,
help="The column where the poses start in the file. Default 1.")
parser.add_argument("--header",
type=int,
default=1,
help="The number of header rows in the file. Default 1.")
args = parser.parse_args()
P_list = [
0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
-0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949
]
frame_offset = SE3.from_list(P_list, 'P')
output_fname = args.original[:-4] + "_offset.csv"
with open(args.original, 'r') as original_file, open(output_fname,
'w') as output_file:
reader = csv.reader(original_file)
writer = csv.writer(output_file)
# Copy the header
for _ in range(args.header):
line = next(reader)
writer.writerow(line)
# Offset and write the poses
for row in reader:
original_pose = SE3.from_list(row[args.col:], args.fspec)