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)