implement world and test

spatialyze/geospatial_video.py

@@ -1,2 +1,15 @@
+import json
+
+from .video_processor.camera_config import CameraConfig
+
+
 class GeospatialVideo:
-    pass
+    def __init__(self, video: "str", camera: "list[CameraConfig] | str") -> None:
+        self.video = video
+        if isinstance(camera, str):
+            with open(camera, "r") as f:
+                camera_configs = json.load(f)
+                assert isinstance(camera_configs, list), camera_configs
+                self.camera = [CameraConfig(*c, 0) for c in camera_configs]
+        else:
+            self.camera = camera

spatialyze/road_network.py

@@ -1,5 +1,5 @@
-from spatialyze.database import Database
-from spatialyze.utils.ingest_road import ingest_location
+from .database import Database
+from .utils.ingest_road import ingest_location, add_segment_type, ROAD_TYPES
 
 
 class RoadNetwork:
@@ -9,3 +9,5 @@ def __init__(self, location: "str", road_network_dir: "str"):
 
     def ingest(self, database: "Database"):
         ingest_location(database, self.road_network_dir, self.location)
+        add_segment_type(database, ROAD_TYPES)
+        database._commit()

spatialyze/video_processor/camera_config.py

@@ -6,6 +6,8 @@
 import numpy.typing as npt
 from pyquaternion import Quaternion
 
+from ..data_types import CameraConfig as _CameraConfig
+
 Float3 = Tuple[float, float, float]
 Float4 = Tuple[float, float, float, float]
 Float33 = Tuple[Float3, Float3, Float3]
@@ -25,7 +27,7 @@ def camera_config(
     camera_heading: float,
     ego_heading: float,
     location: str,
-    road_direction: float,
+    road_direction: float = 0,
 ):
     _frame = CameraConfig()
     _frame.camera_id = camera_id
@@ -119,6 +121,13 @@ def __iter__(self):
                 self.road_direction,
             ]
         )
+
+    def to_CameraConfig(self):
+        return _CameraConfig(
+            frame_id=self.frame_id,
+
+        )
+
 
 
 def interpolate(f1: CameraConfig, f2: CameraConfig, timestamp: datetime):

spatialyze/video_processor/stages/in_view/in_view.py

@@ -65,6 +65,7 @@ def __repr__(self) -> str:
     def _run(self, payload: "Payload") -> "tuple[bitarray, None]":
         indices, view_areas = get_views(payload, self.distance)
 
+        keep = bitarray(len(payload.keep))
         if self.predicate is None:
             results = database.execute(
                 sql.SQL(
@@ -84,12 +85,10 @@ def _run(self, payload: "Payload") -> "tuple[bitarray, None]":
                 )
             )
 
-            keep = bitarray(len(payload.keep))
             keep.setall(0)
             for (index,) in results:
                 keep[index] = 1
         elif self.predicate is False:
-            keep = bitarray(len(payload.keep))
             keep.setall(0)
         elif self.predicate is not True:
             exists = sql.SQL(
@@ -120,7 +119,6 @@ def _run(self, payload: "Payload") -> "tuple[bitarray, None]":
                 )
             )
 
-            keep = bitarray(len(payload.keep))
             keep.setall(0)
             for index, *encoded_segment_types in results:
                 segment_types = {
@@ -249,7 +247,7 @@ def visit_BoolOpNode(self, node: BoolOpNode):
             # print('annihilated', visited)
             return lit(annihilator)
         if all(isinstance(e, LiteralNode) for e in visited.exprs):
-            assert len({e.value for e in visited.exprs}) == 1
+            assert len({e.value for e in visited.exprs}) == 1, visited.exprs
             # print('all', visited)
             return lit(visited.exprs[0].value)
         exprs = [e for e in visited.exprs if not isinstance(e, LiteralNode)]

spatialyze/video_processor/stages/tracking_3d/from_tracking_2d_and_road.py

@@ -103,6 +103,7 @@ def _run(self, payload: "Payload"):
 
 def rotate(vectors: "npt.NDArray", rotation: "Quaternion") -> "npt.NDArray":
     """Rotate 3D Vector by rotation quaternion.
+
     Params:
         vectors: (3 x N) 3-vectors each specified as any ordered
             sequence of 3 real numbers corresponding to x, y, and z values.

spatialyze/video_processor/utils/associate_segment_mapping.py

@@ -0,0 +1,11 @@
+from ..stages.segment_trajectory.from_tracking_3d import SegmentTrajectoryMetadatum
+from ..stages.tracking_3d.tracking_3d import Tracking3DResult
+
+
+def associate_segment_mapping(
+    tracking_result: "Tracking3DResult",
+    segment_mapping_meta: "list[SegmentTrajectoryMetadatum] | None",
+):
+    if segment_mapping_meta is None:
+        return None
+    return segment_mapping_meta[tracking_result.frame_idx].get(tracking_result.object_id)

spatialyze/video_processor/utils/format_trajectory.py

@@ -0,0 +1,65 @@
+from ..camera_config import CameraConfig
+from ..stages.segment_trajectory.construct_segment_trajectory import SegmentPoint
+from ..stages.tracking_3d.tracking_3d import Tracking3DResult
+from ..types import Float3
+
+
+def format_trajectory(
+    video_name: "str",
+    obj_id: "str",
+    track: "list[tuple[Tracking3DResult, CameraConfig, SegmentPoint | None]]",
+    base=None,
+):
+    timestamps: "list[str]" = []
+    pairs: "list[Float3]" = []
+    itemHeadings: "list[int]" = []
+    translations: "list[Float3]" = []
+    camera_id = None
+    object_type = None
+    # road_types: "list[str]" = []
+    # roadpolygons: "list[list[Float2]]" = []
+    ### TODO (fge): remove investigation code
+    info_found = []
+    # if obj_id in investigation_ids:
+    #     print(f"obj_id, {obj_id}:", [e[1].filename for e in track])
+    for tracking_result_3d, ego_info, segment_mapping in track:
+        if ego_info:
+            if (
+                "sweeps/CAM_FRONT/n008-2018-08-30-15-16-55-0400__CAM_FRONT__1535657125362404.jpg"
+                in ego_info.filename
+            ):
+                info_found.append(
+                    [
+                        obj_id,
+                        tracking_result_3d.bbox_left,
+                        tracking_result_3d.bbox_top,
+                        tracking_result_3d.bbox_w,
+                        tracking_result_3d.bbox_h,
+                    ]
+                )
+            camera_id = ego_info.camera_id
+            object_type = tracking_result_3d.object_type
+            timestamps.append(ego_info.timestamp)
+            pairs.append(tracking_result_3d.point)
+            if not segment_mapping or (segment_mapping.segment_type == "intersection"):
+                itemHeadings.append(None)
+            else:
+                itemHeadings.append(segment_mapping.segment_heading)
+            translations.append(ego_info.ego_translation)
+            # road_types.append(segment_mapping.road_polygon_info.road_type if base else detection_info.road_type)
+            # roadpolygons.append(None if base else detection_info.road_polygon_info.polygon)
+    if len(timestamps) == 0 or camera_id is None or object_type is None:
+        return None
+    # if obj_id in investigation_ids:
+    #     print(f"pairs for obj {obj_id}:", [(e[0], e[1]) for e in pairs])
+    #     print(f"itemHeadings for obj {obj_id}:", itemHeadings)
+
+    return (
+        video_name + "_obj_" + str(obj_id),
+        camera_id,
+        object_type,
+        timestamps,
+        pairs,
+        itemHeadings,
+        translations,
+    ), info_found

spatialyze/video_processor/utils/get_tracks.py

@@ -0,0 +1,35 @@
+from ..camera_config import CameraConfig
+from ..stages.segment_trajectory.construct_segment_trajectory import SegmentPoint
+from ..stages.segment_trajectory.from_tracking_3d import SegmentTrajectoryMetadatum
+from ..stages.tracking_3d.tracking_3d import Metadatum as Tracking3DMetadatum, Tracking3DResult
+from .associate_segment_mapping import associate_segment_mapping
+
+
+def get_tracks(
+    sortmeta: "list[Tracking3DMetadatum]",
+    ego_meta: "list[CameraConfig]",
+    segment_mapping_meta: "list[SegmentTrajectoryMetadatum] | None",
+    base=None,
+) -> "dict[str, list[tuple[Tracking3DResult, CameraConfig, SegmentPoint | None]]]":
+    trajectories: "dict[str, list[tuple[Tracking3DResult, CameraConfig, SegmentPoint | None]]]" = {}
+    for i in range(len(sortmeta)):
+        frame = sortmeta[i]
+        for obj_id, tracking_result in frame.items():
+            if obj_id not in trajectories:
+                trajectories[obj_id] = []
+            associated_ego_info = ego_meta[i]
+            associated_segment_mapping = associate_segment_mapping(
+                tracking_result, segment_mapping_meta
+            )
+            trajectories[obj_id].append(
+                (tracking_result, associated_ego_info, associated_segment_mapping)
+            )
+
+    for trajectory in trajectories.values():
+        last = len(trajectory) - 1
+        for i, t in enumerate(trajectory):
+            if i > 0:
+                t[0].prev = trajectory[i - 1][0]
+            if i < last:
+                t[0].next = trajectory[i + 1][0]
+    return trajectories

spatialyze/video_processor/utils/infer_heading.py

@@ -0,0 +1,16 @@
+import math
+
+from ..types import Float3
+
+
+def infer_heading(
+    curItemHeading: "int | None", prevPoint: "Float3 | None", current_point: "Float3"
+):
+    if curItemHeading is not None:
+        return math.degrees(curItemHeading)
+    if prevPoint is None:
+        return None
+    x1, y1, z1 = prevPoint
+    x2, y2, z2 = current_point
+    # 0 is north (y-axis) and counter clockwise
+    return math.degrees(math.atan2(y2 - y1, x2 - x1)) - 90

spatialyze/video_processor/utils/insert_trajectory.py

@@ -0,0 +1,61 @@
+from ...database import Database
+from ...utils import join
+from ..types import Float3
+from .infer_heading import infer_heading
+
+
+def insert_trajectory(
+    database: "Database",
+    item_id: str,
+    camera_id: str,
+    object_type: str,
+    postgres_timestamps: "list[str]",
+    pairs: "list[Float3]",
+    itemHeading_list: "list[int]",
+    translation_list: "list[Float3]",
+    # road_types: "list[str]",
+    # roadpolygon_list: "list[list[tuple[float, float]]]"
+):
+    traj_centroids: "list[str]" = []
+    translations: "list[str]" = []
+    itemHeadings: "list[str]" = []
+    prevTimestamp: "str | None" = None
+    prevPoint: "Float3 | None" = None
+    for timestamp, current_point, curItemHeading, current_trans in zip(
+        postgres_timestamps, pairs, itemHeading_list, translation_list
+    ):
+        if prevTimestamp == timestamp:
+            continue
+        prevTimestamp = timestamp
+
+        # Construct trajectory
+        traj_centroids.append(f"POINT Z ({join(current_point, ' ')})@{timestamp}")
+        translations.append(f"POINT Z ({join(current_point, ' ')})@{timestamp}")
+        curItemHeading = infer_heading(curItemHeading, prevPoint, current_point)
+        if curItemHeading is not None:
+            itemHeadings.append(f"{curItemHeading}@{timestamp}")
+        # roadTypes.append(f"{cur_road_type}@{timestamp}")
+        # polygon_point = ', '.join(join(cur_point, ' ') for cur_point in list(
+        #     zip(*cur_roadpolygon.exterior.coords.xy)))
+        # roadPolygons.append(f"Polygon (({polygon_point}))@{timestamp}")
+        prevPoint = current_point
+
+    # Insert the item_trajectory separately
+    item_headings = (
+        f"tfloat 'Interp=Stepwise;{{[{', '.join(itemHeadings)}]}}'" if itemHeadings else "null"
+    )
+    insert_trajectory = f"""
+    INSERT INTO Item_General_Trajectory (itemId, cameraId, objectType, trajCentroids,
+    translations, itemHeadings)
+    VALUES (
+        '{item_id}',
+        '{camera_id}',
+        '{object_type}',
+        tgeompoint '{{[{', '.join(traj_centroids)}]}}',
+        tgeompoint '{{[{', '.join(translations)}]}}',
+        {item_headings}
+    );
+    """
+
+    database.execute(insert_trajectory)
+    database._commit()