Create draw_utils.py

draw_utils.py

@@ -0,0 +1,18 @@
+import cv2
+
+def draw_trajectory(frames, points, output_path, replay=False):
+    fourcc = cv2.VideoWriter_fourcc(*'mp4v')
+    height, width, _ = frames[0].shape
+    out = cv2.VideoWriter(output_path, fourcc, 20.0, (width, height))
+
+    for idx, frame in enumerate(frames):
+        if replay:
+            cv2.putText(frame, f"Replay Frame {idx}", (10, 30),
+                        cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 255), 2)
+
+        for point in points:
+            cv2.circle(frame, point, 8, (0, 0, 255), -1)
+
+        out.write(frame)
+
+    out.release()