def create_plots(bagfile, output_pdf_file, output_csv_file='results.csv'):
bag = rosbag.Bag(bagfile)
bag_start_time = bag.get_start_time()
has_imu_augmented_graph_localization_state = has_topic(bag, '/gnc/ekf')
has_imu_bias_tester_poses = has_topic(bag, '/imu_bias_tester/pose')
sparse_mapping_poses = poses.Poses('Sparse Mapping',
'/sparse_mapping/pose')
ar_tag_poses = poses.Poses('AR Tag', '/ar_tag/pose')
imu_bias_tester_poses = poses.Poses('Imu Bias Tester',
'/imu_bias_tester/pose')
vec_of_poses = [sparse_mapping_poses, ar_tag_poses, imu_bias_tester_poses]
load_pose_msgs(vec_of_poses, bag, bag_start_time)
graph_localization_states = loc_states.LocStates('Graph Localization',
'/graph_loc/state')
imu_augmented_graph_localization_states = loc_states.LocStates(
'Imu Augmented Graph Localization', '/gnc/ekf')
vec_of_loc_states = [
graph_localization_states, imu_augmented_graph_localization_states
]
load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time)
imu_bias_tester_velocities = velocities.Velocities(
'Imu Bias Tester', '/imu_bias_tester/velocity')
load_velocity_msgs(imu_bias_tester_velocities, bag, bag_start_time)
bag.close()
with PdfPages(output_pdf_file) as pdf:
add_graph_plots(pdf, sparse_mapping_poses, ar_tag_poses,
graph_localization_states,
imu_augmented_graph_localization_states)
if has_imu_bias_tester_poses:
add_imu_bias_tester_poses(pdf, imu_bias_tester_poses,
sparse_mapping_poses)
add_imu_bias_tester_velocities(pdf, imu_bias_tester_velocities)
if has_imu_augmented_graph_localization_state:
add_other_loc_plots(pdf, graph_localization_states,
imu_augmented_graph_localization_states,
sparse_mapping_poses, ar_tag_poses)
else:
add_other_loc_plots(pdf, graph_localization_states,
graph_localization_states)
plot_loc_state_stats(pdf, graph_localization_states,
sparse_mapping_poses, output_csv_file)
plot_loc_state_stats(pdf, imu_augmented_graph_localization_states,
sparse_mapping_poses, output_csv_file,
'imu_augmented_', 0.01)
if has_imu_bias_tester_poses:
plot_loc_state_stats(pdf, imu_bias_tester_poses,
sparse_mapping_poses, output_csv_file,
'imu_bias_tester_', 0.01, False)
def create_plots(
bagfile,
output_pdf_file,
output_csv_file="results.csv",
groundtruth_bagfile=None,
rmse_rel_start_time=0,
rmse_rel_end_time=-1,
):
bag = rosbag.Bag(bagfile)
groundtruth_bag = rosbag.Bag(
groundtruth_bagfile) if groundtruth_bagfile else bag
bag_start_time = bag.get_start_time()
has_imu_augmented_graph_localization_state = has_topic(bag, "/gnc/ekf")
has_imu_bias_tester_poses = has_topic(bag, "/imu_bias_tester/pose")
sparse_mapping_poses = poses.Poses("Sparse Mapping",
"/sparse_mapping/pose")
ar_tag_poses = poses.Poses("AR Tag", "/ar_tag/pose")
imu_bias_tester_poses = poses.Poses("Imu Bias Tester",
"/imu_bias_tester/pose")
vec_of_poses = [ar_tag_poses, imu_bias_tester_poses]
load_pose_msgs(vec_of_poses, bag, bag_start_time)
has_depth_odom = has_topic(bag, "/loc/depth/odom")
depth_odom_relative_poses = poses.Poses("Depth Odom", "/loc/depth/odom")
load_odometry_msgs([depth_odom_relative_poses], bag, bag_start_time)
groundtruth_vec_of_poses = [sparse_mapping_poses]
load_pose_msgs(groundtruth_vec_of_poses, groundtruth_bag, bag_start_time)
graph_localization_states = loc_states.LocStates("Graph Localization",
"/graph_loc/state")
imu_augmented_graph_localization_states = loc_states.LocStates(
"Imu Augmented Graph Localization", "/gnc/ekf")
vec_of_loc_states = [
graph_localization_states,
imu_augmented_graph_localization_states,
]
load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time)
imu_bias_tester_velocities = velocities.Velocities(
"Imu Bias Tester", "/imu_bias_tester/velocity")
load_velocity_msgs(imu_bias_tester_velocities, bag, bag_start_time)
bag.close()
with PdfPages(output_pdf_file) as pdf:
add_graph_plots(
pdf,
sparse_mapping_poses,
ar_tag_poses,
graph_localization_states,
imu_augmented_graph_localization_states,
)
if has_imu_bias_tester_poses:
add_imu_bias_tester_poses(pdf, imu_bias_tester_poses,
sparse_mapping_poses)
add_imu_bias_tester_velocities(pdf, imu_bias_tester_velocities)
if has_imu_augmented_graph_localization_state:
add_other_loc_plots(
pdf,
graph_localization_states,
imu_augmented_graph_localization_states,
sparse_mapping_poses,
ar_tag_poses,
)
else:
add_other_loc_plots(pdf, graph_localization_states,
graph_localization_states)
if has_depth_odom:
depth_odom_poses = utilities.make_absolute_poses_from_relative_poses(
sparse_mapping_poses, depth_odom_relative_poses,
"Depth Odometry")
plot_poses(pdf, depth_odom_poses, sparse_mapping_poses,
ar_tag_poses)
# Note that for absolute time difference tolerance depth images and groudtruth use different sensor data
# and therefore have less similar timestamps. This timestamp difference reduces the accuracy of depth odometry
# groundtruth comparison.
plot_loc_state_stats(
pdf,
depth_odom_poses,
sparse_mapping_poses,
output_csv_file,
"depth_odometry_",
0.01,
False,
rmse_rel_start_time=rmse_rel_start_time,
rmse_rel_end_time=rmse_rel_end_time,
)
plot_covariances(
pdf,
depth_odom_relative_poses.times,
depth_odom_relative_poses.covariances.position,
"Depth Odometry Position",
)
plot_covariances(
pdf,
depth_odom_relative_poses.times,
depth_odom_relative_poses.covariances.orientation,
"Depth Odometry Orientation",
)
plot_loc_state_stats(
pdf,
graph_localization_states,
sparse_mapping_poses,
output_csv_file,
rmse_rel_start_time=rmse_rel_start_time,
rmse_rel_end_time=rmse_rel_end_time,
)
plot_loc_state_stats(
pdf,
imu_augmented_graph_localization_states,
sparse_mapping_poses,
output_csv_file,
"imu_augmented_",
0.01,
rmse_rel_start_time=rmse_rel_start_time,
rmse_rel_end_time=rmse_rel_end_time,
)
if has_imu_bias_tester_poses:
plot_loc_state_stats(
pdf,
imu_bias_tester_poses,
sparse_mapping_poses,
output_csv_file,
"imu_bias_tester_",
0.01,
False,
rmse_rel_start_time=rmse_rel_start_time,
rmse_rel_end_time=rmse_rel_end_time,
)
def create_plots(
bagfile,
output_pdf_file,
output_csv_file="results.csv",
groundtruth_bagfile=None,
rmse_rel_start_time=0,
rmse_rel_end_time=-1,
):
bag = rosbag.Bag(bagfile)
groundtruth_bag = rosbag.Bag(
groundtruth_bagfile) if groundtruth_bagfile else bag
bag_start_time = bag.get_start_time()
has_imu_augmented_graph_localization_state = has_topic(bag, "/gnc/ekf")
has_imu_bias_tester_poses = has_topic(bag, "/imu_bias_tester/pose")
sparse_mapping_poses = poses.Poses("Sparse Mapping",
"/sparse_mapping/pose")
ar_tag_poses = poses.Poses("AR Tag", "/ar_tag/pose")
imu_bias_tester_poses = poses.Poses("Imu Bias Tester",
"/imu_bias_tester/pose")
vec_of_poses = [ar_tag_poses, imu_bias_tester_poses]
load_pose_msgs(vec_of_poses, bag, bag_start_time)
groundtruth_vec_of_poses = [sparse_mapping_poses]
load_pose_msgs(groundtruth_vec_of_poses, groundtruth_bag, bag_start_time)
graph_localization_states = loc_states.LocStates("Graph Localization",
"/graph_loc/state")
imu_augmented_graph_localization_states = loc_states.LocStates(
"Imu Augmented Graph Localization", "/gnc/ekf")
vec_of_loc_states = [
graph_localization_states,
imu_augmented_graph_localization_states,
]
load_loc_state_msgs(vec_of_loc_states, bag, bag_start_time)
imu_bias_tester_velocities = velocities.Velocities(
"Imu Bias Tester", "/imu_bias_tester/velocity")
load_velocity_msgs(imu_bias_tester_velocities, bag, bag_start_time)
bag.close()
with PdfPages(output_pdf_file) as pdf:
add_graph_plots(
pdf,
sparse_mapping_poses,
ar_tag_poses,
graph_localization_states,
imu_augmented_graph_localization_states,
)
if has_imu_bias_tester_poses:
add_imu_bias_tester_poses(pdf, imu_bias_tester_poses,
sparse_mapping_poses)
add_imu_bias_tester_velocities(pdf, imu_bias_tester_velocities)
if has_imu_augmented_graph_localization_state:
add_other_loc_plots(
pdf,
graph_localization_states,
imu_augmented_graph_localization_states,
sparse_mapping_poses,
ar_tag_poses,
)
else:
add_other_loc_plots(pdf, graph_localization_states,
graph_localization_states)
plot_loc_state_stats(
pdf,
graph_localization_states,
sparse_mapping_poses,
output_csv_file,
rmse_rel_start_time=rmse_rel_start_time,
rmse_rel_end_time=rmse_rel_end_time,
)
plot_loc_state_stats(
pdf,
imu_augmented_graph_localization_states,
sparse_mapping_poses,
output_csv_file,
"imu_augmented_",
0.01,
rmse_rel_start_time=rmse_rel_start_time,
rmse_rel_end_time=rmse_rel_end_time,
)
if has_imu_bias_tester_poses:
plot_loc_state_stats(
pdf,
imu_bias_tester_poses,
sparse_mapping_poses,
output_csv_file,
"imu_bias_tester_",
0.01,
False,
rmse_rel_start_time=rmse_rel_start_time,
rmse_rel_end_time=rmse_rel_end_time,
)