Clean up duplicate ingest

apperception/utils/ingest_road.py

@@ -157,10 +157,13 @@ def create_polygon_table(database: "Database", polygons, drop=True):
         "CREATE INDEX IF NOT EXISTS segPoly_idx ON SegmentPolygon USING GiST(elementPolygon);"
     )
 
+    ids = set([p["id"].split("_")[0] for p in polygons if len(p["id"].split("_")) == 1])
+
     values = []
     for poly in polygons:
         i = poly["id"]
         if len(i.split("_")) != 1:
+            assert i.split("_")[0] in ids
             continue
         values.append(
             f"""(
@@ -186,10 +189,15 @@ def create_segment_table(database: "Database", segments, drop=True):
     database.update(CREATE_SEGMENT_SQL)
     database.update("CREATE INDEX IF NOT EXISTS element_idx ON Segment(elementId);")
 
+    ids = set(
+        [s["polygonId"].split("_")[0] for s in segments if len(s["polygonId"].split("_")) == 1]
+    )
+
     values = []
     for seg in segments:
         i = seg["polygonId"]
         if len(i.split("_")) != 1:
+            assert i.split("_")[0] in ids
             continue
         values.append(
             f"""(

optimized_ingestion/stages/detection_estimation/__init__.py

@@ -5,6 +5,7 @@
 from tqdm import tqdm
 from typing import Callable, List, Tuple
 
+from ...cache import cache
 from ...camera_config import CameraConfig
 from ...payload import Payload
 from ...types import DetectionId
@@ -32,6 +33,7 @@ def __init__(self, predicate: "Callable[[DetectionInfo], bool]" = lambda _: True
         self.predicate = predicate
         super(DetectionEstimation, self).__init__()
 
+    # @cache
     def _run(self, payload: "Payload"):
         if Detection2D.get(payload) is None:
             raise Exception()
@@ -67,15 +69,15 @@ def _run(self, payload: "Payload"):
             start_detection_time = time.time()
             det, _ = dets[i]
             all_detection_info = construct_estimated_all_detection_info(det, current_ego_config, ego_trajectory, i)
-            all_detection_info, det = prune_detection(all_detection_info, det, self.predicate)
-            # assert len(all_detection_info) == len(det), (len(all_detection_info), len(det))
+            all_detection_info_pruned, det = prune_detection(all_detection_info, det, self.predicate)
+            assert len(all_detection_info_pruned) == len(det), (len(all_detection_info_pruned), len(det))
             total_detection_time += time.time() - start_detection_time
-            if len(all_detection_info) == 0:
+            if len(all_detection_info_pruned) == 0:
                 skipped_frame_num.append(i)
                 metadata.append([])
                 continue
             start_generate_sample_plan = time.time()
-            next_sample_plan, _ = generate_sample_plan_once(payload.video, current_ego_config, next_frame_num, all_detection_info=all_detection_info)
+            next_sample_plan, _ = generate_sample_plan_once(payload.video, current_ego_config, next_frame_num, all_detection_info=all_detection_info_pruned)
             total_sample_plan_time += time.time() - start_generate_sample_plan
             next_action_type = next_sample_plan.get_action_type()
             if next_action_type not in action_type_counts:

optimized_ingestion/stages/detection_estimation/segment_mapping.py

@@ -416,7 +416,7 @@ def not_in_view(point: "Float2"):
         assert segmentlines is not None
         assert segmentheadings is not None
 
-        assert all(isinstance(l, shapely.geometry.LineString) for l in segmentlines)
+        assert all(isinstance(line, shapely.geometry.LineString) for line in segmentlines)
 
         p = plpygis.Geometry(roadpolygon.to_ewkb())
         assert isinstance(p, plpygis.Polygon)

optimized_ingestion/stages/segment_trajectory/from_tracking_3d.py

@@ -0,0 +1,156 @@
+from apperception.database import database
+
+import numpy as np
+import psycopg2.sql
+from typing import Tuple
+
+from ...payload import Payload
+from ..tracking_3d import tracking_3d
+from ..tracking_3d.from_2d_and_road import From2DAndRoad
+from . import SegmentTrajectory
+
+
+class FromTracking3D(SegmentTrajectory):
+    def _run(self, payload: "Payload"):
+
+        t3d: "list[tracking_3d.Metadatum] | None" = From2DAndRoad.get(payload)
+        assert t3d is not None
+
+        object_mapping: "dict[int, list[tracking_3d.Tracking3DResult]]" = dict()
+        for frame in t3d:
+            for oid, t in frame.items():
+                if oid not in object_mapping:
+                    object_mapping[oid] = []
+
+                object_mapping[oid].append(t)
+
+        points: "list[Tuple[tracking_3d.Tracking3DResult, Tuple[float, float] | None]]" = []
+        for traj in object_mapping.values():
+            traj.sort(key=lambda t: t.timestamp)
+
+            if len(traj) <= 1:
+                points.extend((t, None) for t in traj)
+                continue
+
+            points.append((traj[0], _get_direction_2d(traj[0], traj[1])))
+            points.append((traj[-1], _get_direction_2d(traj[-2], traj[-1])))
+
+            for prev, curr, next in zip(traj[:-2], traj[1:-1], traj[2:]):
+                points.append((curr, _get_direction_2d(prev, next)))
+
+        return super()._run(payload)
+
+
+def _get_direction_2d(p1: "tracking_3d.Tracking3DResult", p2: "tracking_3d.Tracking3DResult") -> "Tuple[float, float]":
+    diff = (p2.point - p1.point)[:2]
+    udiff = diff / np.linalg.norm(diff)
+    return tuple(udiff)
+
+
+def map_points_and_directions_to_segment(annotations: "list[Tuple[tracking_3d.Tracking3DResult, Tuple[float, float] | None]]"):
+    # tokens = [a.detection_id for a, _ in annotations]
+    frame_indices = [a.detection_id.frame_idx for a, _ in annotations]
+    object_indices = [a.detection_id.obj_order for a, _ in annotations]
+    txs = [a.point[0] for a, _ in annotations]
+    tys = [a.point[1] for a, _ in annotations]
+    tzs = [a.point[2] for a, _ in annotations]
+    dxs = [d and d[0] for _, d in annotations]
+    dys = [d and d[1] for _, d in annotations]
+
+    _point = psycopg2.sql.SQL("UNNEST({fields}) AS _point (fid, oid, tx, ty, dx, dy)").format(
+        fields=psycopg2.sql.SQL(',').join(map(psycopg2.sql.Literal, [frame_indices, object_indices, txs, tys, dxs, dys]))
+    )
+
+    out = psycopg2.sql.SQL("""
+    DROP FUNCTION IF EXISTS _angle(double precision);
+    CREATE OR REPLACE FUNCTION _angle(a double precision) RETURNS double precision AS
+    $BODY$
+    BEGIN
+        RETURN ((a::decimal % 360) + 360) % 360;
+    END
+    $BODY$
+    LANGUAGE 'plpgsql';
+
+    WITH
+    Point AS (SELECT * FROM {_point}),
+    _SegmentWithDirection AS (
+        SELECT
+            *,
+            ST_X(endPoint) - ST_X(startPoint) AS _x,
+            ST_Y(endPoint) - ST_Y(startPoint) AS _y
+        FROM Segment
+    ),
+    SegmentWithDirection AS (
+        SELECT
+            *,
+            (_x / SQRT(POWER(_x, 2) + POWER(_y, 2))) AS dx,
+            (_y / SQRT(POWER(_x, 2) + POWER(_y, 2))) AS dy
+        FROM _SegmentWithDirection
+        WHERE
+            _x <> 0 OR _y <> 0
+    ),
+    MinPolygon AS (
+        SELECT token, MIN(ST_Area(Polygon.elementPolygon)) as size
+        FROM Point AS p
+        JOIN SegmentPolygon AS Polygon
+            ON ST_Contains(Polygon.elementPolygon, ST_Point(p.tx, p.ty))
+            AND ARRAY ['intersection', 'lane', 'lanegroup', 'lanesection'] && Polygon.segmenttypes
+        GROUP BY token
+    ),
+    MinPolygonId AS (
+        SELECT token, MIN(elementId) as elementId
+        FROM Point AS p
+        JOIN MinPolygon USING (token)
+        JOIN SegmentPolygon as Polygon
+            ON ST_Contains(Polygon.elementPolygon, ST_Point(p.tx, p.ty))
+            AND ST_Area(Polygon.elementPolygon) = MinPolygon.size
+            AND ARRAY ['intersection', 'lane', 'lanegroup', 'lanesection'] && Polygon.segmenttypes
+        GROUP BY token
+    ),
+    PointPolygonSegment AS (
+        SELECT
+            *,
+            ST_Distance(ST_Point(tx, ty), ST_MakeLine(startPoint, endPoint)) AS distance,
+            CASE
+                WHEN p.dx IS NULL THEN 0
+                WHEN p.dy IS NULL THEN 0
+                ELSE _angle(ACOS((p.dx * sd.dx) + (p.dy * sd.dy)) * 180 / PI())
+            END AS anglediff
+        FROM Point AS p
+        JOIN MinPolygonId USING (token)
+        JOIN SegmentPolygon USING (elementId)
+        JOIN SegmentWithDirection AS sd USING (elementId)
+        WHERE
+            'intersection' = Any(SegmentPolygon.segmenttypes)
+            OR
+            p.dx IS NULL
+            OR
+            p.dy IS NULL
+            OR
+            _angle(ACOS((p.dx * sd.dx) + (p.dy * sd.dy)) * 180 / PI()) < 90
+            OR
+            _angle(ACOS((p.dx * sd.dx) + (p.dy * sd.dy)) * 180 / PI()) > 270
+    ),
+    MinDis as (
+        SELECT token, MIN(distance) as mindistance
+        FROM PointPolygonSegment
+        GROUP BY token
+    ),
+    MinDisMinAngle as (
+        SELECT token, MIN(LEAST(pps.anglediff, 360-pps.anglediff)) as minangle
+        FROM PointPolygonSegment AS pps
+        JOIN MinDis USING (token)
+        WHERE pps.distance = MinDis.mindistance
+        GROUP BY token
+    )
+
+    SELECT token, elementid, segmentid
+    FROM PointPolygonSegment
+    JOIN MinDis USING (token)
+    JOIN MinDisMinAngle USING (token)
+    WHERE PointPolygonSegment.distance = MinDis.mindistance
+        AND PointPolygonSegment.anglediff = MinDisMinAngle.minangle
+    """).format(_point=_point)
+
+    result = database.execute(out)
+    return result

optimized_ingestion/stages/tracking_2d/strongsort.py

@@ -23,6 +23,12 @@
 
 
 class StrongSORT(Tracking2D):
+    def __init__(self, cache: "bool" = True) -> None:
+        super().__init__()
+        self.cache: "bool" = cache
+        # self.ss_benchmarks: "list[list[list[float]]]" = []
+
+    # @cache
     def _run(self, payload: "Payload"):
         detections = Detection2D.get(payload)
         assert detections is not None
@@ -36,27 +42,37 @@ def _run(self, payload: "Payload"):
         assert isinstance(strongsort, _StrongSORT)
         curr_frame, prev_frame = None, None
         with torch.no_grad():
+            # ss_benchmark: "list[list[float]]" = []
             if hasattr(strongsort, 'model'):
                 if hasattr(strongsort.model, 'warmup'):
                     strongsort.model.warmup()
 
             assert len(detections) == len(images)
             for idx, ((det, names), im0s) in tqdm(enumerate(zip(detections, images)), total=len(images)):
+                # for idx, ((det, names), im0s) in enumerate(zip(detections, images)):
                 if not payload.keep[idx] or len(det) == 0:
                     metadata.append({})
                     strongsort.increment_ages()
                     prev_frame = im0s.copy()
                     continue
+
+                # frame_benchmark: "list[float]" = []
+                # frame_benchmark.append(time.time())
                 im0 = im0s.copy()
                 curr_frame = im0
+                # frame_benchmark.append(time.time())
 
                 if hasattr(strongsort, 'tracker') and hasattr(strongsort.tracker, 'camera_update'):
                     if prev_frame is not None and curr_frame is not None:
-                        strongsort.tracker.camera_update(prev_frame, curr_frame, cache=True)
+                        strongsort.tracker.camera_update(prev_frame, curr_frame, cache=self.cache)
+                # frame_benchmark.append(time.time())
 
                 confs = det[:, 4]
                 output_ = strongsort.update(det.cpu(), im0)
 
+                # frame_benchmark.extend(_t)
+                # frame_benchmark.append(time.time())
+
                 if len(output_) > 0:
                     labels: "Dict[int, Tracking2DResult]" = {}
                     for i, (output, conf) in enumerate(zip(output_, confs)):
@@ -84,8 +100,22 @@ def _run(self, payload: "Payload"):
                     metadata.append(labels)
                 else:
                     metadata.append({})
+
+                # frame_benchmark.append(time.time())
                 prev_frame = curr_frame
 
+            #     ss = [
+            #         t2 - t1
+            #         for t1, t2
+            #         in zip(frame_benchmark[:-1], frame_benchmark[1:])
+            #     ]
+            #     # print('[', ', '.join(map(str, ss)), '],')
+            #     ss_benchmark.append(ss)
+
+            # # for s in ss_benchmark:
+            # #     print(s)
+            # self.ss_benchmarks.append(ss_benchmark)
+
         for trajectory in trajectories.values():
             for before, after in zip(trajectory[:-1], trajectory[1:]):
                 before.next = after

optimized_ingestion/types.py

@@ -5,6 +5,9 @@ class DetectionId(NamedTuple):
     frame_idx: int
     obj_order: int
 
+    def __repr__(self) -> str:
+        return f"(f={self.frame_idx} o={self.obj_order})"
+
 
 class obj_detection(NamedTuple):
     detection_id: DetectionId

optimized_ingestion/video_compressed.py

@@ -0,0 +1,21 @@
+from .camera_config import CameraConfig
+from .video import Video
+
+
+class VideoCompressed(Video):
+    videofile: "str"
+
+    def __init__(
+        self,
+        videofile: str,
+        camera_configs: "list[CameraConfig]"
+    ):
+        super().__init__(
+            videofile,
+            camera_configs,
+            camera_configs[0].timestamp
+        )
+
+    @property
+    def interpolated_frames(self):
+        return self._camera_configs

optimized_ingestion/video_skipped.py

@@ -1,5 +1,3 @@
-from datetime import datetime
-
 from .camera_config import CameraConfig, has_config, interpolate
 from .video import Video
 
@@ -9,14 +7,14 @@ class VideoSkipped(Video):
 
     def __init__(
         self,
-        videofile: "str",
-        camera_configs: "list[CameraConfig]",
+        videofile: str,
+        camera_configs: "list[CameraConfig]"
     ):
-        self.videofile = videofile
-        self._camera_configs: "list[CameraConfig]" = camera_configs
-        self._start: "datetime" = camera_configs[0].timestamp
-        self._length: "int | None" = None
-        self._fps: "float | None" = None
+        super().__init__(
+            videofile,
+            camera_configs,
+            camera_configs[0].timestamp
+        )
 
     @property
     def interpolated_frames(self):