feat: add covariance information in jsonl (#568)

Signed-off-by: yoshiri <[email protected]>
tier4 · Oct 2, 2024 · 5246956 · 5246956
1 parent 027e025
commit 5246956
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/driving_log_replayer/driving_log_replayer/perception_eval_conversions.py b/driving_log_replayer/driving_log_replayer/perception_eval_conversions.py
@@ -369,10 +369,13 @@ def object_to_covariance_description(obj: ObjectType | None) -> dict:
                 "pose_covariance": [],
                 "twist_covariance": [],
             }
-        # TODO
-        # wait for covariance calculation implementation
-        pose_covariance = []
-        twist_covariance = []
+
+        pose_covariance = (
+            obj.state.pose_covariance.tolist() if obj.state.has_pose_covariance else []
+        )
+        twist_covariance = (
+            obj.state.twist_covariance.tolist() if obj.state.has_twist_covariance else []
+        )
         return {
             "pose_covariance": pose_covariance,
             "twist_covariance": twist_covariance,

diff --git a/driving_log_replayer/scripts/perception_evaluator_node.py b/driving_log_replayer/scripts/perception_evaluator_node.py
@@ -215,6 +215,8 @@ def list_dynamic_object_from_ros_msg(
                 velocity=eval_conversions.velocity_from_ros_msg(
                     perception_object.kinematics.twist_with_covariance.twist.linear,
                 ),
+                pose_covariance=perception_object.kinematics.pose_with_covariance.covariance,
+                twist_covariance=perception_object.kinematics.twist_with_covariance.covariance,
                 semantic_score=most_probable_classification.probability,
                 semantic_label=label,
                 uuid=uuid,