3d calibration before tracking

apperception/utils/__init__.py

@@ -14,7 +14,7 @@
 from .ingest_road import ingest_road
 from .join import join
 from .overlay_bboxes import overlay_bboxes
-from .overlay_trajectory import fetch_camera_config, overlay_trajectory
+from .overlay_trajectory import fetch_camera_config, fetch_camera_trajectory, overlay_trajectory
 from .query_to_str import query_to_str
 from .recognize import recognize
 from .reformat_bbox_trajectories import reformat_bbox_trajectories
@@ -40,6 +40,7 @@
     "get_video_roi",
     "get_video_box",
     "fetch_camera_config",
+    "fetch_camera_trajectory",
     "join",
     "transformation",
     "fetch_camera",

apperception/utils/overlay_trajectory.py

@@ -185,28 +185,71 @@ def fetch_camera_config(filename: str, database):
         frameNum,
         fileName,
         cameraHeading,
-        egoHeading
+        egoHeading,
+        timestamp,
+        roadDirection
     FROM Cameras
     WHERE
         fileName = '{filename}'
     ORDER BY cameraId ASC, frameNum ASC;
     """
     result = database.execute(query)[0]
+    print(result)
     camera_config = {
         "cameraId": result[0],
         "egoTranslation": result[1],
         "egoRotation": result[2],
         "cameraTranslation": result[3],
         "cameraRotation": result[4],
         "cameraIntrinsic": result[5],
-        "frameNum": result[5],
+        "frameNum": result[6],
         "fileName": result[7],
         "cameraHeading": result[8],
         "egoHeading": result[9],
+        "timestamp": result[10],
+        "roadDirection": result[11],
     }
     return camera_config
 
 
+def fetch_camera_trajectory(video_name: str, database):
+    query = f"""
+    CREATE OR REPLACE FUNCTION ST_XYZ (g geometry) RETURNS real[] AS $$
+        BEGIN
+            RETURN ARRAY[ST_X(g), ST_Y(g), ST_Z(g)];
+        END;
+    $$ LANGUAGE plpgsql;
+
+    SELECT
+        cameraId,
+        ST_XYZ(egoTranslation),
+        frameNum,
+        timestamp,
+        fileName,
+        cameraHeading,
+        egoHeading
+    FROM Cameras
+    WHERE
+        fileName LIKE '%{video_name}%'
+    ORDER BY cameraId ASC, frameNum ASC;
+    """
+    result = database._execute_query(query)
+    camera_config = []
+    for row in result:
+        camera_config.append(
+            {
+                "cameraId": row[0],
+                "egoTranslation": row[1],
+                "frameNum": row[2],
+                "timestamp": row[3],
+                "fileName": row[4],
+                "cameraHeading": row[5],
+                "egoHeading": row[6],
+            }
+        )
+    return camera_config
+
+
 def fetch_trajectory(itemId: str, time: str, database):
     query = f"""
         CREATE OR REPLACE FUNCTION ST_XYZ (g geometry) RETURNS real[] AS $$

apperception/utils/transformation.py

@@ -1,16 +1,18 @@
-from typing import Any, Dict, Tuple, Union
+from typing import Any, Dict, Tuple
 
 import numpy as np
+import numpy.typing as npt
 from pyquaternion import Quaternion
 
 
 def transformation(
-    copy_centroid_3d: Union[np.ndarray, Tuple[float, float, float]], camera_config: Dict[str, Any]
-) -> np.ndarray:
+    copy_centroid_3d: "npt.NDArray[np.floating] | Tuple[float, float, float]",
+    camera_config: Dict[str, Any],
+) -> "npt.NDArray[np.floating]":
     """
-    TODO: transformation from 3d world coordinate to 2d frame coordinate given the camera config
+    Transformation from 3d world coordinate to 2d frame coordinate given the camera config
     """
-    centroid_3d: np.ndarray = np.copy(copy_centroid_3d)
+    centroid_3d: npt.NDArray[np.floating] = np.copy(copy_centroid_3d)
 
     centroid_3d -= camera_config["egoTranslation"]
     centroid_3d = np.dot(

detection_estimation.ipynb

@@ -0,0 +1,126 @@
+{
+ "cells": [
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "c5c9badc",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import json\n",
+    "import os\n",
+    "import pickle\n",
+    "\n",
+    "from optimized_ingestion.camera_config import camera_config\n",
+    "from optimized_ingestion.payload import Payload\n",
+    "from optimized_ingestion.pipeline import Pipeline\n",
+    "from optimized_ingestion.stages.decode_frame.parallel_decode_frame import ParallelDecodeFrame\n",
+    "from optimized_ingestion.stages.detection_2d.yolo_detection import YoloDetection\n",
+    "from optimized_ingestion.stages.filter_car_facing_sideway import FilterCarFacingSideway\n",
+    "from optimized_ingestion.stages.detection_estimation import DetectionEstimation\n",
+    "from optimized_ingestion.stages.tracking_2d.strongsort import StrongSORT\n",
+    "from optimized_ingestion.stages.tracking_2d.tracking_2d import Tracking2D\n",
+    "from optimized_ingestion.stages.tracking_3d.from_2d_and_road import From2DAndRoad\n",
+    "from optimized_ingestion.stages.tracking_3d.tracking_3d import Tracking3DResult\n",
+    "# from optimized_ingestion.trackers.yolov5_strongsort_osnet_tracker import TrackingResult\n",
+    "from optimized_ingestion.video import Video"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "75cd29a3",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "BOSTON_VIDEOS = [\n",
+    "#     \"scene-0757-CAM_FRONT\",\n",
+    "    # \"scene-0103-CAM_FRONT\",\n",
+    "    # \"scene-0553-CAM_FRONT\",\n",
+    "    # \"scene-0665-CAM_FRONT\",\n",
+    "#     \"scene-0655-CAM_FRONT_RIGHT\",\n",
+    "    \"scene-0655-CAM_BACK_RIGHT\",\n",
+    "]\n",
+    "\n",
+    "NUSCENES_PROCESSED_DATA = \"NUSCENES_PROCESSED_DATA\""
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "436432cc",
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "pipeline = Pipeline()\n",
+    "# pipeline.add_filter(filter=InView(distance=10, segment_type=\"intersection\"))\n",
+    "# pipeline.add_filter(filter=Stopped(min_stopped_frames=2, stopped_threshold=1.0))\n",
+    "pipeline.add_filter(filter=ParallelDecodeFrame())\n",
+    "pipeline.add_filter(filter=YoloDetection())\n",
+    "# pipeline.add_filter(filter=DetectionEstimation())\n",
+    "pipeline.add_filter(filter=StrongSORT())\n",
+    "pipeline.add_filter(filter=From2DAndRoad())\n",
+    "pipeline.add_filter(filter=FilterCarFacingSideway())\n",
+    "\n",
+    "if NUSCENES_PROCESSED_DATA in os.environ:\n",
+    "    DATA_DIR = os.environ[NUSCENES_PROCESSED_DATA]\n",
+    "else:\n",
+    "    DATA_DIR = \"/work/apperception/data/nuScenes/full-dataset-v1.0/Mini\"\n",
+    "with open(os.path.join(DATA_DIR, \"videos/boston-seaport\", \"frames.pickle\"), \"rb\") as f:\n",
+    "    videos = pickle.load(f)\n",
+    "\n",
+    "for name, video in videos.items():\n",
+    "#     if name not in BOSTON_VIDEOS:\n",
+    "#         continue\n",
+    "\n",
+    "    print(name)\n",
+    "    frames = Video(\n",
+    "        os.path.join(DATA_DIR, \"videos/boston-seaport\", video[\"filename\"]),\n",
+    "        [camera_config(*f, 0) for f in video[\"frames\"]],\n",
+    "        video[\"start\"],\n",
+    "    )\n",
+    "\n",
+    "    output = pipeline.run(Payload(frames))\n",
+    "\n",
+    "    benchmark = []\n",
+    "    for stage in pipeline.stages:\n",
+    "        benchmark.append({\n",
+    "            \"stage\": stage.classname(),\n",
+    "            \"runtimes\": stage.runtimes,\n",
+    "        })\n",
+    "\n",
+    "    with open(\"./outputs/benchmark.json\", \"w\") as f3:\n",
+    "        json.dump(benchmark, f3)\n"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "id": "4b1d2293",
+   "metadata": {},
+   "outputs": [],
+   "source": []
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Python (apperception)",
+   "language": "python",
+   "name": "apperception"
+  },
+  "language_info": {
+   "codemirror_mode": {
+    "name": "ipython",
+    "version": 3
+   },
+   "file_extension": ".py",
+   "mimetype": "text/x-python",
+   "name": "python",
+   "nbconvert_exporter": "python",
+   "pygments_lexer": "ipython3",
+   "version": "3.10.6"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 5
+}