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()