diff --git a/localization/ndt_evaluation/interpolate_pose.py b/localization/ndt_evaluation/interpolate_pose.py index 007f29e9..6cddeff6 100644 --- a/localization/ndt_evaluation/interpolate_pose.py +++ b/localization/ndt_evaluation/interpolate_pose.py @@ -1,9 +1,10 @@ import pandas as pd -from scipy.spatial.transform import Rotation, Slerp +from scipy.spatial.transform import Rotation +from scipy.spatial.transform import Slerp def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.DataFrame: - """ Interpolate each pose in df_pose to match the timestamp in target_timestamp + """Interpolate each pose in df_pose to match the timestamp in target_timestamp Constraints) * df_pose and target_timestamp must be sorted by timestamp * df_pose must have timestamps with a larger interval than target_timestamp @@ -12,23 +13,23 @@ def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.D 出力) * DataFrame with same columns as df_pose and length same as target_timestamp """ - POSITIONS_KEY = ['x', 'y', 'z'] - ORIENTATIONS_KEY = ['qw', 'qx', 'qy', 'qz'] + POSITIONS_KEY = ["x", "y", "z"] + ORIENTATIONS_KEY = ["qw", "qx", "qy", "qz"] target_index = 0 df_index = 0 data_dict = { - 'x': [], - 'y': [], - 'z': [], - 'qx': [], - 'qy': [], - 'qz': [], - 'qw': [], - 'timestamp': [], + "x": [], + "y": [], + "z": [], + "qx": [], + "qy": [], + "qz": [], + "qw": [], + "timestamp": [], } while df_index < len(df_pose) - 1 and target_index < len(target_timestamp): - curr_time = df_pose.iloc[df_index]['timestamp'] - next_time = df_pose.iloc[df_index + 1]['timestamp'] + curr_time = df_pose.iloc[df_index]["timestamp"] + next_time = df_pose.iloc[df_index + 1]["timestamp"] target_time = target_timestamp[target_index] # Find a df_index that includes target_time @@ -47,18 +48,17 @@ def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.D next_orientation = df_pose.iloc[df_index + 1][ORIENTATIONS_KEY] curr_r = Rotation.from_quat(curr_orientation) next_r = Rotation.from_quat(next_orientation) - slerp = Slerp([curr_time, next_time], - Rotation.concatenate([curr_r, next_r])) + slerp = Slerp([curr_time, next_time], Rotation.concatenate([curr_r, next_r])) target_orientation = slerp([target_time]).as_quat()[0] - data_dict['timestamp'].append(target_timestamp[target_index]) - data_dict['x'].append(target_position[0]) - data_dict['y'].append(target_position[1]) - data_dict['z'].append(target_position[2]) - data_dict['qw'].append(target_orientation[0]) - data_dict['qx'].append(target_orientation[1]) - data_dict['qy'].append(target_orientation[2]) - data_dict['qz'].append(target_orientation[3]) + data_dict["timestamp"].append(target_timestamp[target_index]) + data_dict["x"].append(target_position[0]) + data_dict["y"].append(target_position[1]) + data_dict["z"].append(target_position[2]) + data_dict["qw"].append(target_orientation[0]) + data_dict["qx"].append(target_orientation[1]) + data_dict["qy"].append(target_orientation[2]) + data_dict["qz"].append(target_orientation[3]) target_index += 1 result_df = pd.DataFrame(data_dict) return result_df diff --git a/localization/ndt_evaluation/plot_exe_time_ms.py b/localization/ndt_evaluation/plot_exe_time_ms.py index 505d1a0a..4c77898e 100644 --- a/localization/ndt_evaluation/plot_exe_time_ms.py +++ b/localization/ndt_evaluation/plot_exe_time_ms.py @@ -1,35 +1,35 @@ -import rosbag2_py -from rclpy.serialization import deserialize_message -from rosidl_runtime_py.utilities import get_message import argparse -import matplotlib.pyplot as plt import pathlib -import pandas as pd + from interpolate_pose import interpolate_pose +import matplotlib.pyplot as plt +import pandas as pd +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message def parse_args(): parser = argparse.ArgumentParser() - parser.add_argument('rosbag_path', type=pathlib.Path) + parser.add_argument("rosbag_path", type=pathlib.Path) return parser.parse_args() if __name__ == "__main__": args = parse_args() rosbag_path = args.rosbag_path - serialization_format = 'cdr' - storage_options = rosbag2_py.StorageOptions( - uri=str(rosbag_path), storage_id='sqlite3') + serialization_format = "cdr" + storage_options = rosbag2_py.StorageOptions(uri=str(rosbag_path), storage_id="sqlite3") converter_options = rosbag2_py.ConverterOptions( input_serialization_format=serialization_format, - output_serialization_format=serialization_format) + output_serialization_format=serialization_format, + ) reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) topic_types = reader.get_all_topics_and_types() - type_map = { - topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} target_topics = [ "/localization/pose_estimator/exe_time_ms", @@ -46,26 +46,28 @@ def parse_args(): msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) if topic == "/localization/pose_estimator/exe_time_ms": - timestamp_header = int(msg.stamp.sec) + \ - int(msg.stamp.nanosec) * 1e-9 - ndt_exe_time_ms.append({ - 'timestamp': timestamp_header, - 'value': msg.data, - }) + timestamp_header = int(msg.stamp.sec) + int(msg.stamp.nanosec) * 1e-9 + ndt_exe_time_ms.append( + { + "timestamp": timestamp_header, + "value": msg.data, + } + ) elif topic == "/localization/pose_estimator/pose": - timestamp_header = int(msg.header.stamp.sec) + \ - int(msg.header.stamp.nanosec) * 1e-9 + timestamp_header = int(msg.header.stamp.sec) + int(msg.header.stamp.nanosec) * 1e-9 pose = msg.pose - ndt_pose_list.append({ - 'timestamp': timestamp_header, - 'x': pose.position.x, - 'y': pose.position.y, - 'z': pose.position.z, - 'qw': pose.orientation.w, - 'qx': pose.orientation.x, - 'qy': pose.orientation.y, - 'qz': pose.orientation.z, - }) + ndt_pose_list.append( + { + "timestamp": timestamp_header, + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "qw": pose.orientation.w, + "qx": pose.orientation.x, + "qy": pose.orientation.y, + "qz": pose.orientation.z, + } + ) else: assert False, f"Unknown topic: {topic}" @@ -75,34 +77,34 @@ def parse_args(): df_ndt_exe_time_ms = pd.DataFrame(ndt_exe_time_ms) df_ndt_pose = pd.DataFrame(ndt_pose_list) - df_ndt_pose = interpolate_pose( - df_ndt_pose, df_ndt_exe_time_ms['timestamp']) + df_ndt_pose = interpolate_pose(df_ndt_pose, df_ndt_exe_time_ms["timestamp"]) print(f"{len(df_ndt_exe_time_ms)=}") print(f"{len(df_ndt_pose)=}") # rosbag path may be the path to the db3 file, or it may be the path to the directory containing it - save_dir = (rosbag_path.parent if rosbag_path.is_dir() - else rosbag_path.parent.parent) / "exe_time_ms" + save_dir = ( + rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent + ) / "exe_time_ms" save_dir.mkdir(exist_ok=True) # plot exe_time_ms - plt.plot(df_ndt_exe_time_ms['timestamp'], - df_ndt_exe_time_ms['value']) + plt.plot(df_ndt_exe_time_ms["timestamp"], df_ndt_exe_time_ms["value"]) plt.xlabel("timestamp") plt.ylabel("exe_time [ms]") save_path = save_dir / "exe_time_ms.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close() # plot exe_time_ms on trajectory of ndt_pose - plt.scatter(df_ndt_pose['x'], df_ndt_pose['y'], - c=df_ndt_exe_time_ms['value'].values, cmap='viridis') - plt.colorbar(label='pose_instability_diff_pose[m]') + plt.scatter( + df_ndt_pose["x"], df_ndt_pose["y"], c=df_ndt_exe_time_ms["value"].values, cmap="viridis" + ) + plt.colorbar(label="pose_instability_diff_pose[m]") plt.xlabel("x[m]") plt.ylabel("y[m]") - plt.axis('equal') + plt.axis("equal") save_path = save_dir / "exe_time_ms_on_ndt_pose.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close() diff --git a/localization/ndt_evaluation/plot_pose_instability.py b/localization/ndt_evaluation/plot_pose_instability.py index 6dfc10e4..66d11639 100644 --- a/localization/ndt_evaluation/plot_pose_instability.py +++ b/localization/ndt_evaluation/plot_pose_instability.py @@ -1,36 +1,36 @@ -import rosbag2_py -from rclpy.serialization import deserialize_message -from rosidl_runtime_py.utilities import get_message import argparse +import pathlib + +from interpolate_pose import interpolate_pose import matplotlib.pyplot as plt import numpy as np -import pathlib import pandas as pd -from interpolate_pose import interpolate_pose +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message def parse_args(): parser = argparse.ArgumentParser() - parser.add_argument('rosbag_path', type=pathlib.Path) + parser.add_argument("rosbag_path", type=pathlib.Path) return parser.parse_args() if __name__ == "__main__": args = parse_args() rosbag_path = args.rosbag_path - serialization_format = 'cdr' - storage_options = rosbag2_py.StorageOptions( - uri=str(rosbag_path), storage_id='sqlite3') + serialization_format = "cdr" + storage_options = rosbag2_py.StorageOptions(uri=str(rosbag_path), storage_id="sqlite3") converter_options = rosbag2_py.ConverterOptions( input_serialization_format=serialization_format, - output_serialization_format=serialization_format) + output_serialization_format=serialization_format, + ) reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) topic_types = reader.get_all_topics_and_types() - type_map = { - topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} target_topics = [ "/localization/pose_twist_fusion_filter/pose_instability_detector/debug/diff_pose", @@ -46,44 +46,48 @@ def parse_args(): (topic, data, timestamp_rosbag) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) - timestamp_header = int(msg.header.stamp.sec) + \ - int(msg.header.stamp.nanosec) * 1e-9 - if topic == "/localization/pose_twist_fusion_filter/pose_instability_detector/debug/diff_pose": + timestamp_header = int(msg.header.stamp.sec) + int(msg.header.stamp.nanosec) * 1e-9 + if ( + topic + == "/localization/pose_twist_fusion_filter/pose_instability_detector/debug/diff_pose" + ): pose = msg.pose - pose_instability_diff_pose_list.append({ - 'timestamp': timestamp_header, - 'x': pose.position.x, - 'y': pose.position.y, - 'z': pose.position.z, - 'qw': pose.orientation.w, - 'qx': pose.orientation.x, - 'qy': pose.orientation.y, - 'qz': pose.orientation.z, - }) + pose_instability_diff_pose_list.append( + { + "timestamp": timestamp_header, + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "qw": pose.orientation.w, + "qx": pose.orientation.x, + "qy": pose.orientation.y, + "qz": pose.orientation.z, + } + ) elif topic == "/localization/pose_twist_fusion_filter/kinematic_state": pose = msg.pose.pose - ekf_pose_list.append({ - 'timestamp': timestamp_header, - 'x': pose.position.x, - 'y': pose.position.y, - 'z': pose.position.z, - 'qw': pose.orientation.w, - 'qx': pose.orientation.x, - 'qy': pose.orientation.y, - 'qz': pose.orientation.z, - }) + ekf_pose_list.append( + { + "timestamp": timestamp_header, + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "qw": pose.orientation.w, + "qx": pose.orientation.x, + "qy": pose.orientation.y, + "qz": pose.orientation.z, + } + ) else: assert False, f"Unknown topic: {topic}" print(f"{len(pose_instability_diff_pose_list)=}") print(f"{len(ekf_pose_list)=}") - df_pose_instability_diff_pose = pd.DataFrame( - pose_instability_diff_pose_list) + df_pose_instability_diff_pose = pd.DataFrame(pose_instability_diff_pose_list) df_ekf_pose = pd.DataFrame(ekf_pose_list) - df_ekf_pose = interpolate_pose( - df_ekf_pose, df_pose_instability_diff_pose['timestamp']) + df_ekf_pose = interpolate_pose(df_ekf_pose, df_pose_instability_diff_pose["timestamp"]) print(f"{len(df_pose_instability_diff_pose)=}") print(f"{len(df_ekf_pose)=}") @@ -91,31 +95,33 @@ def parse_args(): save_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent # plot pose_instability_diff_pose - plt.plot(df_pose_instability_diff_pose['timestamp'], - df_pose_instability_diff_pose['x'], label='x') - plt.plot(df_pose_instability_diff_pose['timestamp'], - df_pose_instability_diff_pose['y'], label='y') - plt.plot(df_pose_instability_diff_pose['timestamp'], - df_pose_instability_diff_pose['z'], label='z') + plt.plot( + df_pose_instability_diff_pose["timestamp"], df_pose_instability_diff_pose["x"], label="x" + ) + plt.plot( + df_pose_instability_diff_pose["timestamp"], df_pose_instability_diff_pose["y"], label="y" + ) + plt.plot( + df_pose_instability_diff_pose["timestamp"], df_pose_instability_diff_pose["z"], label="z" + ) plt.xlabel("timestamp") plt.ylabel("pose_instability_diff_pose[m]") plt.legend() save_path = save_dir / "pose_instability_diff_pose.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close() # plot norm of pose_instability_diff_pose on trajectory of ekf_pose - diff_pose = df_pose_instability_diff_pose[['x', 'y', 'z']].values + diff_pose = df_pose_instability_diff_pose[["x", "y", "z"]].values diff_pose_norm = np.linalg.norm(diff_pose, axis=1) - plt.scatter(df_ekf_pose['x'], df_ekf_pose['y'], - c=diff_pose_norm, cmap='viridis') - plt.colorbar(label='pose_instability_diff_pose[m]') + plt.scatter(df_ekf_pose["x"], df_ekf_pose["y"], c=diff_pose_norm, cmap="viridis") + plt.colorbar(label="pose_instability_diff_pose[m]") plt.xlabel("x[m]") plt.ylabel("y[m]") - plt.axis('equal') + plt.axis("equal") save_path = save_dir / "pose_instability_diff_pose_on_ekf_pose.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close()