feat: add ndt_evaluation

localization/ndt_evaluation/.gitignore

@@ -0,0 +1 @@
+*.pyc

localization/ndt_evaluation/README.md

@@ -0,0 +1,61 @@
+# NDT Evaluation
+
+This directory contains tools for evaluating the performance of the NDT localization.
+
+## Step1: Record rosbag
+
+```bash
+ros2 bag record \
+  /localization/kinematic_state \
+  /localization/util/downsample/pointcloud \
+  /sensing/vehicle_velocity_converter/twist_with_covariance \
+  /sensing/imu/imu_data \
+  /tf_static \
+  /sensing/gnss/pose_with_covariance \
+  /initialpose
+```
+
+## Step2: Apply `convert_rosbag_for_ndt_evaluation.py`
+
+- Check whether the necessary topics are available
+- Extract only the necessary topics
+- Change the topic name of the reference trajectory
+
+```bash
+python3 convert_rosbag_for_ndt_evaluation.py /path/to/recorded_rosbag
+```
+
+## Step3: Execute DrivingLogReplayer
+
+Setup [DrivingLogReplayer](https://tier4.github.io/driving_log_replayer/quick_start/installation/) and copy the converted rosbag (input_bad) to `~/driving_log_replayer_data/localization`
+
+```bash
+~/driving_log_replayer_data/localization$ tree -L 2
+.
+├── evaluation_sample
+│   ├── input_bag
+│   └── scenario.yaml
+└── sample
+    ├── input_bag
+    └── scenario.yaml
+
+4 directories, 2 files
+```
+
+Then execute.
+
+```bash
+dlr simulation run -p localization -l "play_rate:=0.5"
+```
+
+```bash
+<< show test result >>
+test case 1 / 2 : use case: evaluation_sample
+--------------------------------------------------
+TestResult: Passed
+Passed: Convergence (Success): 964 / 964 -> 100.00%, Reliability (Success): NVTL Sequential NG Count: 0 (Total Test: 974, Average: 3.07964, StdDev: 0.09657), NDT Availability (Success): NDT available
+test case 2 / 2 : use case: sample
+--------------------------------------------------
+TestResult: Passed
+Passed: Convergence (Success): 294 / 295 -> 99.66%, Reliability (Success): NVTL Sequential NG Count: 0 (Total Test: 295, Average: 2.47750, StdDev: 0.04174), NDT Availability (Success): NDT available
+```

localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py

@@ -0,0 +1,167 @@
+"""Convert rosbag for NDT evaluation.
+
+(1) Check if the rosbag is suitable for ndt evaluation
+(2) Filter topics
+(3) Rename the topic name of the reference kinematic state to /localization/reference_kinematic_state
+"""
+
+import argparse
+import pathlib
+
+from geometry_msgs.msg import PoseWithCovarianceStamped
+import pandas as pd
+from rclpy.serialization import deserialize_message
+from rclpy.serialization import serialize_message
+import rosbag2_py
+from rosidl_runtime_py.utilities import get_message
+
+REQUIRED_FPS_PATTERN_A = {
+    "/localization/kinematic_state": 40,
+    "/localization/util/downsample/pointcloud": 7.5,
+    "/sensing/vehicle_velocity_converter/twist_with_covariance": 20,
+    "/sensing/imu/imu_data": 20,
+    "/tf_static": 0,
+    "/sensing/gnss/pose_with_covariance": -1,  # optional topic
+}
+
+REQUIRED_FPS_PATTERN_B = {
+    "/localization/kinematic_state": 40,
+    "/localization/util/downsample/pointcloud": 7.5,
+    "/localization/twist_estimator/twist_with_covariance": 20,
+}
+
+
+def parse_args():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("rosbag_path", type=pathlib.Path)
+    parser.add_argument(
+        "--reference_topic_name",
+        type=str,
+        default="/localization/kinematic_state",
+        choices=[
+            "/localization/kinematic_state",
+            "/awsim/ground_truth/localization/kinematic_state",
+        ],
+    )
+    parser.add_argument("--add_initialpose", action="store_true")
+    return parser.parse_args()
+
+
+def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: dict) -> bool:
+    """Check if the rosbag is suitable for ndt evaluation."""
+    print(f"{duration=:.1f} sec")
+
+    df = pd.DataFrame(
+        {
+            "topic": list(topic_name_to_msg_list.keys()),
+            "count": [len(msg_list) for msg_list in topic_name_to_msg_list.values()],
+        }
+    )
+    df = df[df["topic"].isin(required_fps.keys())]
+    df["fps"] = df["count"] / duration
+    df["enough_fps"] = df["fps"] > df["topic"].map(required_fps)
+    print(df)
+    return df["enough_fps"].all()
+
+
+if __name__ == "__main__":
+    args = parse_args()
+    rosbag_path = args.rosbag_path
+    reference_topic_name = args.reference_topic_name
+
+    # prepare option
+    serialization_format = "cdr"
+    converter_options = rosbag2_py.ConverterOptions(
+        input_serialization_format=serialization_format,
+        output_serialization_format=serialization_format,
+    )
+
+    # prepare reader
+    reader = rosbag2_py.SequentialReader()
+    storage_options = rosbag2_py.StorageOptions(uri=str(rosbag_path), storage_id="sqlite3")
+    reader.open(storage_options, converter_options)
+
+    # filter topics
+    target_topics = list(REQUIRED_FPS_PATTERN_A.keys()) + list(REQUIRED_FPS_PATTERN_B.keys())
+    target_topics = list(set(target_topics))
+    if reference_topic_name not in target_topics:
+        target_topics.append(reference_topic_name)
+    storage_filter = rosbag2_py.StorageFilter(topics=target_topics)
+    reader.set_filter(storage_filter)
+
+    # get map of topic name and type
+    type_map = {}
+    for topic_type in reader.get_all_topics_and_types():
+        if topic_type.name in target_topics:
+            type_map[topic_type.name] = topic_type.type
+
+    # read rosbag
+    topic_name_to_msg_list = {topic: [] for topic in target_topics}
+    tuple_list = []
+    topic_name_to_rosbag_timestamp = {topic: [] for topic in target_topics}
+    while reader.has_next():
+        (topic_name, data, timestamp_rosbag) = reader.read_next()
+        tuple_list.append((topic_name, data, timestamp_rosbag))
+
+        msg_type = get_message(type_map[topic_name])
+        msg = deserialize_message(data, msg_type)
+        topic_name_to_msg_list[topic_name].append(msg)
+        topic_name_to_rosbag_timestamp[topic_name].append(timestamp_rosbag)
+    duration = (tuple_list[-1][2] - tuple_list[0][2]) * 1e-9
+
+    # check
+    save_topics = list()
+    if check_rosbag(duration, topic_name_to_msg_list, REQUIRED_FPS_PATTERN_A):
+        save_topics = list(REQUIRED_FPS_PATTERN_A.keys())
+        print("OK as pattern A")
+    elif check_rosbag(duration, topic_name_to_msg_list, REQUIRED_FPS_PATTERN_B):
+        save_topics = list(REQUIRED_FPS_PATTERN_B.keys())
+        print("OK as pattern B")
+    else:
+        print("The rosbag is not suitable for ndt evaluation")
+        exit()
+
+    # get first msg from /localization/kinematic_state
+    first_reference = topic_name_to_msg_list[reference_topic_name][0]
+
+    # if there is not gnss topic or selected from args, add initialpose
+    save_initialpose = (
+        len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) == 0
+        or args.add_initialpose
+    )
+    if save_initialpose:
+        print("Add /initialpose")
+        type_map["/initialpose"] = "geometry_msgs/msg/PoseWithCovarianceStamped"
+        save_topics.append("/initialpose")
+        stamp = first_reference.header.stamp
+        msg = PoseWithCovarianceStamped()
+        msg.header = first_reference.header
+        msg.pose = first_reference.pose
+        data = serialize_message(msg)
+        first_pcd = topic_name_to_rosbag_timestamp[reference_topic_name][0]
+        timestamp = first_pcd + int(0.5 * 1e9)
+        tuple_list.append(("/initialpose", data, timestamp))
+
+    # write rosbag
+    rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent
+    filtered_rosbag_path = rosbag_dir / "input_bag"
+    writer = rosbag2_py.SequentialWriter()
+    storage_options = rosbag2_py.StorageOptions(uri=str(filtered_rosbag_path), storage_id="sqlite3")
+    writer.open(storage_options, converter_options)
+    for topic_name, topic_type in type_map.items():
+        if topic_name not in save_topics:
+            continue
+        if topic_name == reference_topic_name:
+            topic_name = "/localization/reference_kinematic_state"
+        topic_info = rosbag2_py.TopicMetadata(
+            name=topic_name, type=topic_type, serialization_format=serialization_format
+        )
+        writer.create_topic(topic_info)
+    for topic_name, data, timestamp_rosbag in tuple_list:
+        if topic_name not in save_topics:
+            continue
+        if topic_name == reference_topic_name:
+            topic_name = "/localization/reference_kinematic_state"
+        writer.write(topic_name, data, timestamp_rosbag)
+
+    print(f"rosbag is saved at {filtered_rosbag_path}")

localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py

@@ -0,0 +1,72 @@
+"""Fix timestamp of a rosbag which is recorded by AWSIM.
+
+When a rosbag is recorded by AWSIM without '--use-sim-time' option,
+the timestamp in the header of each message is simulated time, but the timestamp in the rosbag file is the wall time.
+This script fixes the timestamp in the rosbag file to match the simulated time.
+"""
+
+import argparse
+import pathlib
+
+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)
+    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")
+    converter_options = rosbag2_py.ConverterOptions(
+        input_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))}
+
+    tuple_list = []
+
+    first_timestamp = None
+
+    while reader.has_next():
+        (topic_name, data, timestamp_rosbag) = reader.read_next()
+
+        msg_type = get_message(type_map[topic_name])
+        msg = deserialize_message(data, msg_type)
+        if hasattr(msg, "header"):
+            timestamp_header = int(int(msg.header.stamp.sec) * 1e9 + int(msg.header.stamp.nanosec))
+            if first_timestamp is None:
+                first_timestamp = timestamp_header
+        else:
+            # /tf_static does not have header, so use the first timestamp - 1
+            # (/tf_static should be at the beginning of the rosbag file)
+            timestamp_header = 0 if first_timestamp is None else first_timestamp - 1
+        tuple_list.append((topic_name, data, timestamp_header))
+
+    # write rosbag
+    rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent
+    filtered_rosbag_path = rosbag_dir / "input_bag_sim_time"
+    writer = rosbag2_py.SequentialWriter()
+    storage_options = rosbag2_py.StorageOptions(uri=str(filtered_rosbag_path), storage_id="sqlite3")
+    writer.open(storage_options, converter_options)
+    for topic_name, topic_type in type_map.items():
+        topic_info = rosbag2_py.TopicMetadata(
+            name=topic_name, type=topic_type, serialization_format=serialization_format
+        )
+        writer.create_topic(topic_info)
+    for topic_name, data, timestamp_rosbag in tuple_list:
+        writer.write(topic_name, data, timestamp_rosbag)
+
+    print(f"rosbag is saved at {filtered_rosbag_path}")

localization/ndt_evaluation/interpolate_pose.py

@@ -0,0 +1,64 @@
+import pandas as pd
+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
+    Constraints)
+    * df_pose and target_timestamp must be sorted by timestamp
+    * df_pose must have timestamps with a larger interval than target_timestamp
+      * i.e. df_pose[0] <= target_timestamp[0] and target_timestamp[-1] <= df_pose[-1]
+    * len(df_pose) > len(target_timestamp)
+    出力)
+    * DataFrame with same columns as df_pose and length same as target_timestamp
+    """
+    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": [],
+    }
+    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"]
+        target_time = target_timestamp[target_index]
+
+        # Find a df_index that includes target_time
+        if not (curr_time <= target_time <= next_time):
+            df_index += 1
+            continue
+
+        curr_weight = (next_time - target_time) / (next_time - curr_time)
+        next_weight = 1.0 - curr_weight
+
+        curr_position = df_pose.iloc[df_index][POSITIONS_KEY]
+        next_position = df_pose.iloc[df_index + 1][POSITIONS_KEY]
+        target_position = curr_position * curr_weight + next_position * next_weight
+
+        curr_orientation = df_pose.iloc[df_index][ORIENTATIONS_KEY]
+        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]))
+        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])
+        target_index += 1
+    result_df = pd.DataFrame(data_dict)
+    return result_df