def add_imu_bias_tester_poses(pdf, imu_bias_tester_poses,
sparse_mapping_poses):
colors = ['r', 'b', 'g']
plt.figure()
plot_helpers.plot_positions(sparse_mapping_poses,
colors,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
plot_helpers.plot_positions(imu_bias_tester_poses, colors, linewidth=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.title('Imu Bias Tester vs. Sparse Mapping Position')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# orientations
plt.figure()
plot_helpers.plot_orientations(sparse_mapping_poses,
colors,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
plot_helpers.plot_orientations(imu_bias_tester_poses,
colors,
linewidth=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Orienation (deg)')
plt.title('Imu Bias Tester vs. Sparse Mapping Orientation')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
def add_imu_bias_tester_poses(pdf, imu_bias_tester_poses,
sparse_mapping_poses):
colors = ["r", "b", "g"]
plt.figure()
plot_helpers.plot_positions(
sparse_mapping_poses,
colors,
linestyle="None",
marker="o",
markeredgewidth=0.1,
markersize=1.5,
)
plot_helpers.plot_positions(imu_bias_tester_poses, colors, linewidth=0.5)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
plt.title("Imu Bias Tester vs. Sparse Mapping Position")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# orientations
plt.figure()
plot_helpers.plot_orientations(
sparse_mapping_poses,
colors,
linestyle="None",
marker="o",
markeredgewidth=0.1,
markersize=1.5,
)
plot_helpers.plot_orientations(imu_bias_tester_poses,
colors,
linewidth=0.5)
plt.xlabel("Time (s)")
plt.ylabel("Orienation (deg)")
plt.title("Imu Bias Tester vs. Sparse Mapping Orientation")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
def add_other_vector3d_plots(pdf, imu_augmented_graph_localization_states,
sparse_mapping_poses, ar_tag_poses):
colors = ['r', 'b', 'g']
# Acceleration
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.accelerations,
imu_augmented_graph_localization_states.times, 'Acc.')
plt.xlabel('Time (s)')
plt.ylabel('Acceleration (m/s^2)')
plt.title('Acceleration')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Biases
# Plot Accelerometer Biases on different pages since they can start with quite different
# values, plotting on the same page will lead to a large y axis scale and hide subtle changes.
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.xs,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (X)')
plt.title('Accelerometer Biases (X)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.ys,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (Y)')
plt.title('Accelerometer Biases (Y)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.zs,
'r')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Biases (Z)')
plt.title('Accelerometer Biases (Z)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.xs, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (X)')
plt.title('Gyro Biases (X)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.ys, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (Y)')
plt.title('Gyro Biases (Y)')
pdf.savefig()
plt.close()
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.zs, 'r')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Biases (Z)')
plt.title('Gyro Biases (Z)')
pdf.savefig()
plt.close()
# Angular Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.angular_velocities,
imu_augmented_graph_localization_states.times, 'Ang. Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Angular Velocities')
plt.title('Angular Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.velocities,
imu_augmented_graph_localization_states.times, 'Vel.')
plt.xlabel('Time (s)')
plt.ylabel('Velocities')
plt.title('IMU Augmented Graph Velocities')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Integrated Velocities
plt.figure()
plot_helpers.plot_positions(sparse_mapping_poses,
colors,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
if ar_tag_poses.times:
plot_helpers.plot_positions(ar_tag_poses,
colors,
linestyle='None',
marker='x',
markeredgewidth=0.1,
markersize=1.5)
integrated_imu_augmented_graph_localization_states = utilities.integrate_velocities(
imu_augmented_graph_localization_states)
plot_helpers.plot_positions(
integrated_imu_augmented_graph_localization_states,
colors,
linewidth=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.title(
'Integrated IMU Augmented Graph Velocities vs. Sparse Mapping Position'
)
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Position covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.position_covariances),
'r',
linewidth=0.5,
label='Position Covariance')
plt.title('Position Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Position Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Orientation covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.orientation_covariances),
'r',
linewidth=0.5,
label='Orientation Covariance')
plt.title('Orientation Covariance (Quaternion)')
plt.xlabel('Time (s)')
plt.ylabel('Orientation Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Velocity covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.velocity_covariances),
'r',
linewidth=0.5,
label='Velocity Covariance')
plt.title('Velocity Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Velocity Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Accel Bias covariance
plt.figure()
plt.plot(imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.
accelerometer_bias_covariances),
'r',
linewidth=0.5,
label='Accelerometer Bias Covariance')
plt.title('Accelerometer Bias Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Accelerometer Bias Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Gyro Bias covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.gyro_bias_covariances),
'r',
linewidth=0.5,
label='Gyro Bias Covariance')
plt.title('Gyro Bias Covariance')
plt.xlabel('Time (s)')
plt.ylabel('Gyro Bias Covariance')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
def add_other_vector3d_plots(pdf, imu_augmented_graph_localization_states,
sparse_mapping_poses, ar_tag_poses):
colors = ["r", "b", "g"]
# Acceleration
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.accelerations,
imu_augmented_graph_localization_states.times,
"Acc.",
)
plt.xlabel("Time (s)")
plt.ylabel("Acceleration (m/s^2)")
plt.title("Acceleration")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Biases
# Plot Accelerometer Biases on different pages since they can start with quite different
# values, plotting on the same page will lead to a large y axis scale and hide subtle changes.
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.xs,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Accelerometer Biases (X)")
plt.title("Accelerometer Biases (X)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.ys,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Accelerometer Biases (Y)")
plt.title("Accelerometer Biases (Y)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.accelerometer_biases.zs,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Accelerometer Biases (Z)")
plt.title("Accelerometer Biases (Z)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.xs,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Gyro Biases (X)")
plt.title("Gyro Biases (X)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.ys,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Gyro Biases (Y)")
plt.title("Gyro Biases (Y)")
pdf.savefig()
plt.close()
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
imu_augmented_graph_localization_states.gyro_biases.zs,
"r",
)
plt.xlabel("Time (s)")
plt.ylabel("Gyro Biases (Z)")
plt.title("Gyro Biases (Z)")
pdf.savefig()
plt.close()
# Angular Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.angular_velocities,
imu_augmented_graph_localization_states.times,
"Ang. Vel.",
)
plt.xlabel("Time (s)")
plt.ylabel("Angular Velocities")
plt.title("Angular Velocities")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Velocity
plt.figure()
plot_helpers.plot_vector3ds(
imu_augmented_graph_localization_states.velocities,
imu_augmented_graph_localization_states.times,
"Vel.",
)
plt.xlabel("Time (s)")
plt.ylabel("Velocities")
plt.title("IMU Augmented Graph Velocities")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Integrated Velocities
plt.figure()
plot_helpers.plot_positions(
sparse_mapping_poses,
colors,
linestyle="None",
marker="o",
markeredgewidth=0.1,
markersize=1.5,
)
if ar_tag_poses.times:
plot_helpers.plot_positions(
ar_tag_poses,
colors,
linestyle="None",
marker="x",
markeredgewidth=0.1,
markersize=1.5,
)
integrated_imu_augmented_graph_localization_states = utilities.integrate_velocities(
imu_augmented_graph_localization_states)
plot_helpers.plot_positions(
integrated_imu_augmented_graph_localization_states,
colors,
linewidth=0.5)
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
plt.title(
"Integrated IMU Augmented Graph Velocities vs. Sparse Mapping Position"
)
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Position covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.position_covariances),
"r",
linewidth=0.5,
label="Position Covariance",
)
plt.title("Position Covariance")
plt.xlabel("Time (s)")
plt.ylabel("Position Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Orientation covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(
imu_augmented_graph_localization_states.orientation_covariances),
"r",
linewidth=0.5,
label="Orientation Covariance",
)
plt.title("Orientation Covariance (Quaternion)")
plt.xlabel("Time (s)")
plt.ylabel("Orientation Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Velocity covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.velocity_covariances),
"r",
linewidth=0.5,
label="Velocity Covariance",
)
plt.title("Velocity Covariance")
plt.xlabel("Time (s)")
plt.ylabel("Velocity Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Accel Bias covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.
accelerometer_bias_covariances),
"r",
linewidth=0.5,
label="Accelerometer Bias Covariance",
)
plt.title("Accelerometer Bias Covariance")
plt.xlabel("Time (s)")
plt.ylabel("Accelerometer Bias Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
# Gyro Bias covariance
plt.figure()
plt.plot(
imu_augmented_graph_localization_states.times,
l2_map(imu_augmented_graph_localization_states.gyro_bias_covariances),
"r",
linewidth=0.5,
label="Gyro Bias Covariance",
)
plt.title("Gyro Bias Covariance")
plt.xlabel("Time (s)")
plt.ylabel("Gyro Bias Covariance")
plt.legend(prop={"size": 6})
pdf.savefig()
plt.close()
def add_pose_plots(pdf, sparse_mapping_poses, ar_tag_poses,
graph_localization_poses,
imu_augmented_graph_localization_poses):
colors = ['r', 'b', 'g']
plt.figure()
plot_helpers.plot_positions(sparse_mapping_poses,
colors,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
if ar_tag_poses.times:
plot_helpers.plot_positions(ar_tag_poses,
colors,
linestyle='None',
marker='x',
markeredgewidth=0.1,
markersize=1.5)
plot_helpers.plot_positions(graph_localization_poses,
colors,
linewidth=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.title('Graph vs. Sparse Mapping Position')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# orientations
plt.figure()
plot_helpers.plot_orientations(sparse_mapping_poses,
colors,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
if ar_tag_poses.times:
plot_helpers.plot_orientations(ar_tag_poses,
colors,
linestyle='None',
marker='x',
markeredgewidth=0.1,
markersize=1.5)
plot_helpers.plot_orientations(graph_localization_poses,
colors,
linewidth=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Orienation (deg)')
plt.title('Graph vs. Sparse Mapping Orientation')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# Imu Augmented Loc vs. Loc
plt.figure()
plot_helpers.plot_positions(graph_localization_poses,
colors,
linestyle='None',
marker='o',
markeredgewidth=0.1,
markersize=1.5)
plot_helpers.plot_positions(imu_augmented_graph_localization_poses,
colors,
linewidth=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.title('Graph vs. Imu Augmented Graph Position')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
# orientations
plt.figure()
plot_helpers.plot_orientations(graph_localization_poses,
colors,
marker='o',
markeredgewidth=0.1,
markersize=1.5)
plot_helpers.plot_orientations(imu_augmented_graph_localization_poses,
colors,
linewidth=0.5)
plt.xlabel('Time (s)')
plt.ylabel('Orienation (deg)')
plt.title('Graph vs. Imu Augmented Graph Orientation')
plt.legend(prop={'size': 6})
pdf.savefig()
plt.close()
