draw_utils.py

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()