diff --git a/.github/workflows/examples.yml b/.github/workflows/examples.yml index d8297e2..e521b7e 100644 --- a/.github/workflows/examples.yml +++ b/.github/workflows/examples.yml @@ -48,12 +48,6 @@ jobs: shell: micromamba-shell {0} run: poetry install --no-interaction --without dev --with test - - name: Install lap (Hack) - shell: micromamba-shell {0} - run: | - pip install --upgrade pip - pip install lap - - name: Check Installed Packages shell: micromamba-shell {0} run: pip list diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 3de384e..55f736d 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -45,12 +45,6 @@ jobs: shell: micromamba-shell {0} run: poetry install --no-interaction --without dev --with test - - name: Install lap (Hack) - shell: micromamba-shell {0} - run: | - pip install --upgrade pip - pip install lap - - name: Check Installed Packages shell: micromamba-shell {0} run: pip list @@ -128,12 +122,6 @@ jobs: shell: micromamba-shell {0} run: poetry install --no-interaction --without dev --with test - - name: Install lap (Hack) - shell: micromamba-shell {0} - run: | - pip install --upgrade pip - pip install lap - - name: Check Installed Packages shell: micromamba-shell {0} run: pip list diff --git a/README.md b/README.md index 579b704..a1bc550 100644 --- a/README.md +++ b/README.md @@ -76,7 +76,6 @@ mamba activate spatialyze # install python dependencies poetry install -pip install lap # a bug in lap/poetry/conda that lap needs to be installed using pip. ``` ## Spatialyze Demo diff --git a/poetry.lock b/poetry.lock index 23223f2..6ec0c53 100644 --- a/poetry.lock +++ b/poetry.lock @@ -797,92 +797,6 @@ files = [ {file = "cycler-0.11.0.tar.gz", hash = "sha256:9c87405839a19696e837b3b818fed3f5f69f16f1eec1a1ad77e043dcea9c772f"}, ] -[[package]] -name = "cython" -version = "3.0.0" -description = "The Cython compiler for writing C extensions in the Python language." -optional = false -python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*" -files = [ - {file = "Cython-3.0.0-cp310-cp310-macosx_10_9_x86_64.whl", hash = "sha256:7c7d728e1a49ad01d41181e3a9ea80b8d14e825f4679e4dd837cbf7bca7998a5"}, - {file = "Cython-3.0.0-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:626a4a6ef4b7ced87c348ea805488e4bd39dad9d0b39659aa9e1040b62bbfedf"}, - {file = "Cython-3.0.0-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:33c900d1ca9f622b969ac7d8fc44bdae140a4a6c7d8819413b51f3ccd0586a09"}, - {file = "Cython-3.0.0-cp310-cp310-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:a65bc50dc1bc2faeafd9425defbdef6a468974f5c4192497ff7f14adccfdcd32"}, - {file = "Cython-3.0.0-cp310-cp310-musllinux_1_1_aarch64.whl", hash = "sha256:3b71b399b10b038b056ad12dce1e317a8aa7a96e99de7e4fa2fa5d1c9415cfb9"}, - {file = "Cython-3.0.0-cp310-cp310-musllinux_1_1_x86_64.whl", hash = "sha256:f42f304c097cc53e9eb5f1a1d150380353d5018a3191f1b77f0de353c762181e"}, - {file = "Cython-3.0.0-cp310-cp310-win32.whl", hash = "sha256:3e234e2549e808d9259fdb23ebcfd145be30c638c65118326ec33a8d29248dc2"}, - {file = "Cython-3.0.0-cp310-cp310-win_amd64.whl", hash = "sha256:829c8333195100448a23863cf64a07e1334fae6a275aefe871458937911531b6"}, - {file = "Cython-3.0.0-cp311-cp311-macosx_10_9_x86_64.whl", hash = "sha256:06db81b1a01858fcc406616f8528e686ffb6cf7c3d78fb83767832bfecea8ad8"}, - {file = "Cython-3.0.0-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:c93634845238645ce7abf63a56b1c5b6248189005c7caff898fd4a0dac1c5e1e"}, - {file = "Cython-3.0.0-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:aa606675c6bd23478b1d174e2a84e3c5a2c660968f97dc455afe0fae198f9d3d"}, - {file = "Cython-3.0.0-cp311-cp311-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:d3355e6f690184f984eeb108b0f5bbc4bcf8b9444f8168933acf79603abf7baf"}, - {file = "Cython-3.0.0-cp311-cp311-musllinux_1_1_aarch64.whl", hash = "sha256:93a34e1ca8afa4b7075b02ed14a7e4969256297029fb1bfd4cbe48f7290dbcff"}, - {file = "Cython-3.0.0-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:bb1165ca9e78823f9ad1efa5b3d83156f868eabd679a615d140a3021bb92cd65"}, - {file = "Cython-3.0.0-cp311-cp311-win32.whl", hash = "sha256:2fadde1da055944f5e1e17625055f54ddd11f451889110278ef30e07bd5e1695"}, - {file = "Cython-3.0.0-cp311-cp311-win_amd64.whl", hash = "sha256:254ed1f03a6c237fa64f0c6e44862058de65bfa2e6a3b48ca3c205492e0653aa"}, - {file = "Cython-3.0.0-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:4e212237b7531759befb92699c452cd65074a78051ae4ee36ff8b237395ecf3d"}, - {file = "Cython-3.0.0-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:9f29307463eba53747b31f71394ed087e3e3e264dcc433e62de1d51f5c0c966c"}, - {file = "Cython-3.0.0-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:53328a8af0806bebbdb48a4191883b11ee9d9dfb084d84f58fa5a8ab58baefc9"}, - {file = "Cython-3.0.0-cp312-cp312-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:5962e70b15e863e72bed6910e8c6ffef77d36cc98e2b31c474378f3b9e49b0e3"}, - {file = "Cython-3.0.0-cp312-cp312-musllinux_1_1_aarch64.whl", hash = "sha256:9e69139f4e60ab14c50767a568612ea64d6907e9c8e0289590a170eb495e005f"}, - {file = "Cython-3.0.0-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:c40bdbcb2286f0aeeb5df9ce53d45da2d2a9b36a16b331cd0809d212d22a8fc7"}, - {file = "Cython-3.0.0-cp312-cp312-win32.whl", hash = "sha256:8abb8915eb2e57fa53d918afe641c05d1bcc6ed1913682ec1f28de71f4e3f398"}, - {file = "Cython-3.0.0-cp312-cp312-win_amd64.whl", hash = "sha256:30a4bd2481e59bd7ab2539f835b78edc19fc455811e476916f56026b93afd28b"}, - {file = "Cython-3.0.0-cp36-cp36m-macosx_10_9_x86_64.whl", hash = "sha256:0e1e4b7e4bfbf22fecfa5b852f0e499c442d4853b7ebd33ae37cdec9826ed5d8"}, - {file = "Cython-3.0.0-cp36-cp36m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:6b00df42cdd1a285a64491ba23de08ab14169d3257c840428d40eb7e8e9979af"}, - {file = "Cython-3.0.0-cp36-cp36m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:650d03ddddc08b051b4659778733f0f173ca7d327415755c05d265a6c1ba02fb"}, - {file = "Cython-3.0.0-cp36-cp36m-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:4965f2ebade17166f21a508d66dd60d2a0b3a3b90abe3f72003baa17ae020dd6"}, - {file = "Cython-3.0.0-cp36-cp36m-musllinux_1_1_aarch64.whl", hash = "sha256:4123c8d03167803df31da6b39de167cb9c04ac0aa4e35d4e5aa9d08ad511b84d"}, - {file = "Cython-3.0.0-cp36-cp36m-musllinux_1_1_x86_64.whl", hash = "sha256:296c53b6c0030cf82987eef163444e8d7631cc139d995f9d58679d9fd1ddbf31"}, - {file = "Cython-3.0.0-cp36-cp36m-win32.whl", hash = "sha256:0d2c1e172f1c81bafcca703093608e10dc16e3e2d24c5644c17606c7fdb1792c"}, - {file = "Cython-3.0.0-cp36-cp36m-win_amd64.whl", hash = "sha256:bc816d8eb3686d6f8d165f4156bac18c1147e1035dc28a76742d0b7fb5b7c032"}, - {file = "Cython-3.0.0-cp37-cp37m-macosx_10_9_x86_64.whl", hash = "sha256:8d86651347bbdbac1aca1824696c5e4c0a3b162946c422edcca2be12a03744d1"}, - {file = "Cython-3.0.0-cp37-cp37m-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:84176bd04ce9f3cc8799b47ec6d1959fa1ea5e71424507df7bbf0b0915bbedef"}, - {file = "Cython-3.0.0-cp37-cp37m-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:35abcf07b8277ec95bbe49a07b5c8760a2d941942ccfe759a94c8d2fe5602e9f"}, - {file = "Cython-3.0.0-cp37-cp37m-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:a44d6b9a29b2bff38bb648577b2fcf6a68cf8b1783eee89c2eb749f69494b98d"}, - {file = "Cython-3.0.0-cp37-cp37m-musllinux_1_1_aarch64.whl", hash = "sha256:4dc6bbe7cf079db37f1ebb9b0f10d0d7f29e293bb8688e92d50b5ea7a91d82f3"}, - {file = "Cython-3.0.0-cp37-cp37m-musllinux_1_1_x86_64.whl", hash = "sha256:e28763e75e380b8be62b02266a7995a781997c97c119efbdccb8fb954bcd7574"}, - {file = "Cython-3.0.0-cp37-cp37m-win32.whl", hash = "sha256:edae615cb4af51d5173e76ba9aea212424d025c57012e9cdf2f131f774c5ba71"}, - {file = "Cython-3.0.0-cp37-cp37m-win_amd64.whl", hash = "sha256:20c604e974832aaf8b7a1f5455ee7274b34df62a35ee095cd7d2ed7e818e6c53"}, - {file = "Cython-3.0.0-cp38-cp38-macosx_10_9_x86_64.whl", hash = "sha256:c85fd2b1cbd9400d60ebe074795bb9a9188752f1612be3b35b0831a24879b91f"}, - {file = "Cython-3.0.0-cp38-cp38-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:090256c687106932339f87f888b95f0d69c617bc9b18801555545b695d29d8ab"}, - {file = "Cython-3.0.0-cp38-cp38-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:cec2a67a0a7d9d4399758c0657ca03e5912e37218859cfbf046242cc532bfb3b"}, - {file = "Cython-3.0.0-cp38-cp38-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:a1cdd01ce45333bc264a218c6e183700d6b998f029233f586a53c9b13455c2d2"}, - {file = "Cython-3.0.0-cp38-cp38-musllinux_1_1_aarch64.whl", hash = "sha256:ecee663d2d50ca939fc5db81f2f8a219c2417b4651ad84254c50a03a9cb1aadd"}, - {file = "Cython-3.0.0-cp38-cp38-musllinux_1_1_x86_64.whl", hash = "sha256:30f10e79393b411af7677c270ea69807acb9fc30205c8ff25561f4deef780ec1"}, - {file = "Cython-3.0.0-cp38-cp38-win32.whl", hash = "sha256:609777d3a7a0a23b225e84d967af4ad2485c8bdfcacef8037cf197e87d431ca0"}, - {file = "Cython-3.0.0-cp38-cp38-win_amd64.whl", hash = "sha256:7f4a6dfd42ae0a45797f50fc4f6add702abf46ab3e7cd61811a6c6a97a40e1a2"}, - {file = "Cython-3.0.0-cp39-cp39-macosx_10_9_x86_64.whl", hash = "sha256:2d8158277c8942c0b20ff4c074fe6a51c5b89e6ac60cef606818de8c92773596"}, - {file = "Cython-3.0.0-cp39-cp39-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:54e34f99b2a8c1e11478541b2822e6408c132b98b6b8f5ed89411e5e906631ea"}, - {file = "Cython-3.0.0-cp39-cp39-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:877d1c8745df59dd2061a0636c602729e9533ba13f13aa73a498f68662e1cbde"}, - {file = "Cython-3.0.0-cp39-cp39-manylinux_2_5_i686.manylinux1_i686.manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:204690be60f0ff32eb70b04f28ef0d1e50ffd7b3f77ba06a7dc2389ee3b848e0"}, - {file = "Cython-3.0.0-cp39-cp39-musllinux_1_1_aarch64.whl", hash = "sha256:06fcb4628ccce2ba5abc8630adbeaf4016f63a359b4c6c3827b2d80e0673981c"}, - {file = "Cython-3.0.0-cp39-cp39-musllinux_1_1_x86_64.whl", hash = "sha256:090e24cfa31c926d0b13d8bb2ef48175acdd061ae1413343c94a2b12a4a4fa6f"}, - {file = "Cython-3.0.0-cp39-cp39-win32.whl", hash = "sha256:4cd00f2158dc00f7f93a92444d0f663eda124c9c29bbbd658964f4e89c357fe8"}, - {file = "Cython-3.0.0-cp39-cp39-win_amd64.whl", hash = "sha256:5b4cc896d49ce2bae8d6a030f9a4c64965b59c38acfbf4617685e17f7fcf1731"}, - {file = "Cython-3.0.0-py2.py3-none-any.whl", hash = "sha256:ff1aef1a03cfe293237c7a86ae9625b0411b2df30c53d1a7f29a8d381f38a1df"}, - {file = "Cython-3.0.0.tar.gz", hash = "sha256:350b18f9673e63101dbbfcf774ee2f57c20ac4636d255741d76ca79016b1bd82"}, -] - -[[package]] -name = "cython_bbox" -version = "0.1.3" -description = "" -optional = false -python-versions = "*" -files = [] -develop = false - -[package.dependencies] -Cython = "*" -numpy = "*" - -[package.source] -type = "git" -url = "https://github.com/apperception-db/cython_bbox.git" -reference = "HEAD" -resolved_reference = "2ae0f4a5cb8a517b089ebc0fd3cd3cf338686eb9" - [[package]] name = "debugpy" version = "1.6.7" @@ -968,16 +882,6 @@ files = [ {file = "docstring_to_markdown-0.12-py3-none-any.whl", hash = "sha256:7df6311a887dccf9e770f51242ec002b19f0591994c4783be49d24cdc1df3737"}, ] -[[package]] -name = "easydict" -version = "1.10" -description = "Access dict values as attributes (works recursively)." -optional = false -python-versions = "*" -files = [ - {file = "easydict-1.10.tar.gz", hash = "sha256:11dcb2c20aaabbfee4c188b4bc143ef6be044b34dbf0ce5a593242c2695a080f"}, -] - [[package]] name = "entrypoints" version = "0.4" @@ -1423,7 +1327,6 @@ optional = false python-versions = ">=2.7, !=3.0.*, !=3.1.*, !=3.2.*, !=3.3.*, !=3.4.*, !=3.5.*, !=3.6.*" files = [ {file = "jsonpointer-2.4-py2.py3-none-any.whl", hash = "sha256:15d51bba20eea3165644553647711d150376234112651b4f1811022aecad7d7a"}, - {file = "jsonpointer-2.4.tar.gz", hash = "sha256:585cee82b70211fa9e6043b7bb89db6e1aa49524340dde8ad6b63206ea689d88"}, ] [[package]] @@ -2611,20 +2514,6 @@ files = [ docs = ["furo (>=2023.7.26)", "proselint (>=0.13)", "sphinx (>=7.1.1)", "sphinx-autodoc-typehints (>=1.24)"] test = ["appdirs (==1.4.4)", "covdefaults (>=2.3)", "pytest (>=7.4)", "pytest-cov (>=4.1)", "pytest-mock (>=3.11.1)"] -[[package]] -name = "plpygis" -version = "0.2.1" -description = "PostGIS Python tools" -optional = false -python-versions = "*" -files = [ - {file = "plpygis-0.2.1-py3-none-any.whl", hash = "sha256:d1504cc08a032aa6585914b84d1c714a91d07c2fc5bce278c122f9457d575f2d"}, - {file = "plpygis-0.2.1.tar.gz", hash = "sha256:62b3ef9ee81293e4f8ca423066cea5940542521cd16c4a08852bf73305da33c8"}, -] - -[package.extras] -shapely-support = ["Shapely (>=1.5.0)"] - [[package]] name = "pluggy" version = "1.2.0" @@ -4469,4 +4358,4 @@ testing = ["big-O", "jaraco.functools", "jaraco.itertools", "more-itertools", "p [metadata] lock-version = "2.0" python-versions = "^3.10" -content-hash = "cb783083c46be72799db2520f24ff68040a84e239a9ecf20736bca2e4753d89c" +content-hash = "b98341fb9bdd070fbd1c041c8bd7c7b4a0fd05053aa3f6f8fb7292c2fcbecd91" diff --git a/pyproject.toml b/pyproject.toml index 6f72d28..4b4d1f6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -8,34 +8,29 @@ authors = ["Your Name "] python = "^3.10" pyquaternion = "^0.9.9" numpy = "^1.22.4" -matplotlib = "^3.5.2" opencv-python = "^4.6.0" pandas = "^1.4.2" bitarray = "^2.6.0" tqdm = "^4.64.1" shapely = "^1.8.5.post1" -plpygis = "^0.2.0" # MobilityDB psycopg2-binary = "^2.9.3" postgis = "^1.0.4" -# python-mobilitydb = "^0.1.2" python-mobilitydb = "^0.1.3" [tool.poetry.group.cv.dependencies] torch = "^1.13.0" torchvision = "^0.14.0" +matplotlib = "^3.5.2" scipy = "^1.4.1" -easydict = "^1.10" gdown = "^4.7.1" -cython-bbox = {git = "https://github.com/apperception-db/cython_bbox.git"} ultralytics = "^8.0.148" [tool.poetry.group.test.dependencies] pytest = "^7.4.0" coverage = "^7.2.7" pytest-cov = "^4.1.0" -notebook = "^6.4.12" [tool.poetry.group.dev.dependencies] types-psycopg2 = "^2.9.16" diff --git a/scripts/import_tables.py b/scripts/import_tables.py index df56c37..70a48f3 100644 --- a/scripts/import_tables.py +++ b/scripts/import_tables.py @@ -75,4 +75,5 @@ def _insert_into_general_bbox(database: "Database", value: tuple, commit=True): database._commit(commit) -import_tables(database, './data/scenic/database') +if __name__ == "__main__": + import_tables(database, './data/scenic/database') diff --git a/spatialyze/utils/F/contained.py b/spatialyze/utils/F/contained.py index f635c35..81ddf67 100644 --- a/spatialyze/utils/F/contained.py +++ b/spatialyze/utils/F/contained.py @@ -9,7 +9,7 @@ @call_node def contained(visitor: "GenSqlVisitor", args: "list[PredicateNode]"): - object, region = args[:2] + object, region = args if isinstance(object, ObjectTableNode): object = object.traj @ camera.time - return f"contained({visitor(object)}, {visitor(region)})" + return f"contained({visitor(object)},{visitor(region)})" diff --git a/spatialyze/video_processor/modules/yolo_tracker b/spatialyze/video_processor/modules/yolo_tracker index b528e70..fd3a839 160000 --- a/spatialyze/video_processor/modules/yolo_tracker +++ b/spatialyze/video_processor/modules/yolo_tracker @@ -1 +1 @@ -Subproject commit b528e706a9ef0e8d526aa11b22da0a54084e7dd9 +Subproject commit fd3a839d0b96d8dc41e9e93aa1ba32426a7cffeb diff --git a/spatialyze/video_processor/stages/detection_estimation/__init__.py b/spatialyze/video_processor/stages/detection_estimation/__init__.py index b40e91b..39e6ca3 100644 --- a/spatialyze/video_processor/stages/detection_estimation/__init__.py +++ b/spatialyze/video_processor/stages/detection_estimation/__init__.py @@ -1,10 +1,8 @@ import logging import time -from typing import Callable, List, Tuple +from typing import Callable, List import postgis -import shapely -import shapely.geometry import torch from bitarray import bitarray from psycopg2 import sql @@ -61,7 +59,6 @@ def _run(self, payload: "Payload"): return keep, {DetectionEstimation.classname(): [[]] * len(keep)} ego_views = get_ego_views(payload) - ego_views = [shapely.wkb.loads(view.to_ewkb(), hex=True) for view in ego_views] skipped_frame_num = [] next_frame_num = 0 @@ -219,10 +216,10 @@ def prune_detection( def generate_sample_plan_once( video: "Video", next_frame_num: "int", - ego_views: "list[shapely.geometry.Polygon]", + ego_views: "list[postgis.Polygon]", all_detection_info: "list[DetectionInfo] | None" = None, fps: "int" = 13, -) -> "Tuple[SamplePlan, None]": +) -> "tuple[SamplePlan, None]": assert all_detection_info is not None next_sample_plan = generate_sample_plan( video, next_frame_num, all_detection_info, ego_views, 50, fps=fps diff --git a/spatialyze/video_processor/stages/detection_estimation/detection_estimation.py b/spatialyze/video_processor/stages/detection_estimation/detection_estimation.py index 4991607..1ea7310 100644 --- a/spatialyze/video_processor/stages/detection_estimation/detection_estimation.py +++ b/spatialyze/video_processor/stages/detection_estimation/detection_estimation.py @@ -16,22 +16,19 @@ """ import datetime -import os -import sys import time from dataclasses import dataclass, field from typing import Any, Tuple -sys.path.append(os.path.abspath(os.path.join(os.getcwd(), os.pardir, os.pardir))) - +import postgis import shapely import shapely.geometry from ...camera_config import CameraConfig from ...types import DetectionId, obj_detection from ...video import Video -from .optimized_segment_mapping import RoadPolygonInfo, get_detection_polygon_mapping from .sample_plan_algorithms import CAR_EXIT_SEGMENT, Action +from .segment_mapping import RoadPolygonInfo, get_detection_polygon_mapping from .utils import ( Float2, Float3, @@ -158,7 +155,7 @@ class SamplePlan: video: "Video" next_frame_num: int all_detection_info: "list[DetectionInfo]" - ego_views: "list[shapely.geometry.Polygon]" + ego_views: "list[postgis.Polygon]" fps: int = 12 metadata: Any = None current_priority: "float | None" = None @@ -304,7 +301,7 @@ def generate_sample_plan( video: "Video", next_frame_num: int, all_detection_info: "list[DetectionInfo]", - ego_views: "list[shapely.geometry.Polygon]", + ego_views: "list[postgis.Polygon]", view_distance: float, fps: int = 12, ): diff --git a/spatialyze/video_processor/stages/detection_estimation/optimized_segment_mapping.py b/spatialyze/video_processor/stages/detection_estimation/optimized_segment_mapping.py deleted file mode 100644 index 369cec0..0000000 --- a/spatialyze/video_processor/stages/detection_estimation/optimized_segment_mapping.py +++ /dev/null @@ -1,329 +0,0 @@ -""" Goal to map the road segment to the frame segment - Now only get the segment of type lane and intersection - except for the segment that contains the ego camera - -Usage example: - from optimization_playground.segment_mapping import map_imgsegment_roadsegment - from spatialyze.utils import fetch_camera_config - - test_config = fetch_camera_config(test_img, database) - mapping = map_imgsegment_roadsegment(test_config) -""" - -import array -import logging -import math -import os -import time -from typing import NamedTuple, Tuple - -import numpy as np -import plpygis -import postgis -import psycopg2.sql as sql -import shapely -import shapely.geometry -import shapely.wkb - -from spatialyze.database import database - -from ...camera_config import CameraConfig -from ...types import DetectionId, obj_detection -from .segment_mapping import ( - RoadSegmentWithHeading, - Segment, - get_fov_lines, - make_road_polygon_with_heading, -) -from .utils import ROAD_TYPES, Float22 - -logger = logging.getLogger(__name__) - -data_path = "/home/yongming/workspace/research/apperception/v1.0-mini/" -input_video_dir = os.path.join(data_path, "sample_videos/") -input_video_name = "CAM_FRONT_n008-2018-08-27.mp4" -input_date = input_video_name.split("_")[-1][:-4] -test_img = "samples/CAM_FRONT/n008-2018-08-01-15-52-19-0400__CAM_FRONT__1533153253912404.jpg" - - -SQL_ROAD_TYPES = ",".join("__RoadType__" + rt + "__" for rt in ROAD_TYPES) - - -MAX_POLYGON_CONTAIN_QUERY = sql.SQL( - f""" -WITH -AvailablePolygon AS ( - SELECT * - FROM SegmentPolygon as p - WHERE - p.location = {{location}} - AND ST_Contains( - p.elementpolygon, - {{ego_translation}}::geometry - ) -), -max_contain AS ( - SELECT MAX(ST_Area(elementpolygon)) max_segment_area - FROM AvailablePolygon -) -SELECT - p.elementid, - p.elementpolygon::geometry, - ARRAY_AGG(s.segmentline)::geometry[], - ARRAY_AGG(s.heading)::real[], - {SQL_ROAD_TYPES} -FROM max_contain, AvailablePolygon AS p - LEFT OUTER JOIN segment AS s USING (elementid) -WHERE ST_Area(p.elementpolygon) = max_contain.max_segment_area -GROUP BY p.elementid, p.elementpolygon, {SQL_ROAD_TYPES}; -""" -) - -USEFUL_TYPES = ["lane", "lanegroup", "intersection"] - - -class RoadPolygonInfo(NamedTuple): - """ - id: unique polygon id - polygon: tuple of (x, y) coordinates - segment_line: list of tuple of (x, y) coordinates - segment_type: road segment type - segment_headings: list of floats - contains_ego: whether the segment contains ego camera - ego_config: ego camfig for the frame we asks info for - fov_lines: field of view lines - """ - - id: str - polygon: "shapely.geometry.Polygon" - segment_lines: "list[shapely.geometry.LineString]" - road_type: str - segment_headings: "list[float]" - contains_ego: bool - ego_config: "CameraConfig" - fov_lines: "tuple[Float22, Float22]" - - -def reformat_return_polygon(segments: "list[RoadSegmentWithHeading]") -> "list[Segment]": - def _(x: "RoadSegmentWithHeading") -> "Segment": - i, polygon, types, lines, headings = x - type = types[-1] - for t in types: - if t in USEFUL_TYPES: - type = t - break - return Segment( - i, - polygon, - type, - lines, - [*map(math.degrees, headings)], - ) - - return list(map(_, segments)) - - -def intersection(fov_line: Tuple[Float22, Float22], segmentpolygon: "shapely.geometry.Polygon"): - """ - return: intersection point: tuple[tuple] - """ - left_fov_line, right_fov_line = fov_line - # left_intersection = line_to_polygon_intersection(segmentpolygon, left_fov_line) - # right_intersection = line_to_polygon_intersection(segmentpolygon, right_fov_line) - # return left_intersection + right_intersection - return [] - - -def is_roadsection(segmenttypes: "list[int]"): - for t, v in zip(ROAD_TYPES, segmenttypes): - if t == "roadsection" and v: - return True - return False - - -def get_largest_polygon_containing_point(ego_config: "CameraConfig"): - point = postgis.Point(*ego_config.ego_translation[:2]) - query = MAX_POLYGON_CONTAIN_QUERY.format( - ego_translation=sql.Literal(point), location=sql.Literal(ego_config.location) - ) - - results = database.execute(query) - if len(results) > 1: - for result in results: - segmenttypes = result[4:] - if not is_roadsection(segmenttypes): - results = [result] - break - assert len(results) == 1, (ROAD_TYPES, [r[4:] for r in results]) - result = results[0] - assert len(result) == 4 + len(ROAD_TYPES), (len(results), len(ROAD_TYPES) + 4) - - output = make_road_polygon_with_heading(result) - - types, line, heading = output[2:5] - assert line is not None - assert types is not None - assert heading is not None - road_polygon = reformat_return_polygon([output])[0] - polygonid, roadpolygon, roadtype, segmentlines, segmentheadings = road_polygon - fov_lines = get_fov_lines(ego_config) - - polygon = shapely.wkb.loads(roadpolygon.to_ewkb(), hex=True) - assert isinstance(polygon, shapely.geometry.Polygon) - return RoadPolygonInfo( - polygonid, polygon, segmentlines, roadtype, segmentheadings, True, ego_config, fov_lines - ) - - -def map_detections_to_segments(detections: "list[obj_detection]", ego_config: "CameraConfig"): - tokens = [*map(lambda x: x.detection_id.obj_order, detections)] - points = [postgis.Point(d.car_loc3d[0], d.car_loc3d[1]) for d in detections] - - location = ego_config.location - - convex_points = np.array([[d.car_loc3d[0], d.car_loc3d[1]] for d in detections]) - - out = sql.SQL( - f""" - WITH - Point AS ( - SELECT * - FROM UNNEST( - {{tokens}}, - {{points}}::geometry(Point)[] - ) AS _point (token, point) - ), - AvailablePolygon AS ( - SELECT * - FROM SegmentPolygon - WHERE location = {{location}} - AND ST_Intersects(SegmentPolygon.elementPolygon, ST_ConvexHull({{convex}}::geometry(MultiPoint))) - AND (SegmentPolygon.__RoadType__intersection__ - OR SegmentPolygon.__RoadType__lane__ - OR SegmentPolygon.__RoadType__lanegroup__ - OR SegmentPolygon.__RoadType__lanesection__) - AND NOT SegmentPolygon.__RoadType__roadsection__ - ), - MinPolygon AS ( - SELECT token, MIN(ST_Area(Polygon.elementPolygon)) as size - FROM Point AS p - JOIN AvailablePolygon AS Polygon - ON ST_Contains(Polygon.elementPolygon, p.point) - GROUP BY token - ), - MinPolygonId AS ( - SELECT token, MIN(elementId) as elementId - FROM Point AS p - JOIN MinPolygon USING (token) - JOIN AvailablePolygon as Polygon - ON ST_Contains(Polygon.elementPolygon, p.point) - AND ST_Area(Polygon.elementPolygon) = MinPolygon.size - GROUP BY token - ) - SELECT - p.token, - AvailablePolygon.elementid, - AvailablePolygon.elementpolygon, - ARRAY_AGG(Segment.segmentline)::geometry[], - ARRAY_AGG(Segment.heading)::real[], - {SQL_ROAD_TYPES} - FROM Point AS p - JOIN MinPolygonId USING (token) - JOIN AvailablePolygon USING (elementId) - JOIN Segment USING (elementId) - GROUP BY - AvailablePolygon.elementid, - p.token, - AvailablePolygon.elementpolygon, - {SQL_ROAD_TYPES}; - """ - ).format( - tokens=sql.Literal(tokens), - points=sql.Literal(points), - convex=sql.Literal(postgis.MultiPoint(map(tuple, convex_points))), - location=sql.Literal(location), - ) - result = database.execute(out) - return result - - -def get_detection_polygon_mapping(detections: "list[obj_detection]", ego_config: "CameraConfig"): - """ - Given a list of detections, return a list of RoadSegmentWithHeading - """ - # start_time = time.time() - times = [] - times.append(time.time()) - results = map_detections_to_segments(detections, ego_config) - times.append(time.time()) - - order_ids, mapped_polygons = [r[0] for r in results], [r[1:] for r in results] - mapped_polygons = [*map(make_road_polygon_with_heading, mapped_polygons)] - times.append(time.time()) - for row in mapped_polygons: - types, line, heading = row[2:5] - assert line is not None - assert types is not None - assert heading is not None - times.append(time.time()) - mapped_polygons = reformat_return_polygon(mapped_polygons) - times.append(time.time()) - if any(p.road_type == "intersection" for p in mapped_polygons): - return {}, times - times.append(time.time()) - fov_lines = get_fov_lines(ego_config) - times.append(time.time()) - - # def not_in_view(point: "Float2"): - # return not in_view(point, ego_config.ego_translation, fov_lines) - - mapped_road_polygon_info: "dict[DetectionId, RoadPolygonInfo]" = {} - for order_id, road_polygon in list(zip(order_ids, mapped_polygons)): - frame_idx = detections[0].detection_id.frame_idx - det_id = DetectionId(frame_idx=frame_idx, obj_order=order_id) - if det_id in mapped_road_polygon_info: - print("skipped") - continue - polygonid, roadpolygon, roadtype, segmentlines, segmentheadings = road_polygon - assert segmentlines is not None - assert segmentheadings is not None - - # assert all(isinstance(line, shapely.geometry.LineString) for line in segmentlines) - - p = plpygis.Geometry(roadpolygon.to_ewkb()) - assert isinstance(p, plpygis.Polygon) - XYs: "Tuple[array.array[float], array.array[float]]" = p.exterior.shapely.xy - assert isinstance(XYs, tuple) - assert isinstance(XYs[0], array.array), type(XYs[0]) - assert isinstance(XYs[1], array.array), type(XYs[1]) - assert isinstance(XYs[0][0], float), type(XYs[0][0]) - assert isinstance(XYs[1][0], float), type(XYs[1][0]) - polygon_points = list(zip(*XYs)) - # roadpolygon = shapely.geometry.Polygon(polygon_points) - - # decoded_road_polygon_points = polygon_points - # if all(map(not_in_view, polygon_points)): - # continue - - # intersection_points = intersection(fov_lines, roadpolygon) - # decoded_road_polygon_points += intersection_points - # keep_road_polygon_points: "list[Float2]" = [] - # for current_road_point in decoded_road_polygon_points: - # if in_view(current_road_point, ego_config.ego_translation, fov_lines): - # keep_road_polygon_points.append(current_road_point) - if len(polygon_points) > 2: - # and shapely.geometry.Polygon(tuple(keep_road_polygon_points)).area > 1): - mapped_road_polygon_info[det_id] = RoadPolygonInfo( - polygonid, - shapely.geometry.Polygon(polygon_points), - segmentlines, - roadtype, - segmentheadings, - False, - ego_config, - fov_lines, - ) - times.append(time.time()) - - # logger.info(f'total mapping time: {time.time() - start_time}') - return mapped_road_polygon_info, times diff --git a/spatialyze/video_processor/stages/detection_estimation/sample_plan_algorithms.py b/spatialyze/video_processor/stages/detection_estimation/sample_plan_algorithms.py index 3668f9c..a5d9cfb 100644 --- a/spatialyze/video_processor/stages/detection_estimation/sample_plan_algorithms.py +++ b/spatialyze/video_processor/stages/detection_estimation/sample_plan_algorithms.py @@ -1,27 +1,8 @@ import datetime -import logging from dataclasses import dataclass, field -from typing import TYPE_CHECKING, List, Literal, Union - -from .utils import ( - OPPOSITE_DIRECTION, - SAME_DIRECTION, - Float2, - Float3, - ego_departure, - meetup, - time_to_exit_current_segment, - time_to_exit_view, - trajectory_3d, -) - -if TYPE_CHECKING: - from ...camera_config import CameraConfig - from .detection_estimation import DetectionInfo - - -logger = logging.getLogger(__name__) +from typing import Literal +from .utils import Float2, Float3 """ Action: @@ -31,14 +12,13 @@ Heuristic to sample """ -EGO_EXIT_SEGMENT = "ego_exit_segment" -CAR_EXIT_SEGMENT = "car_exit_segment" -EXIT_VIEW = "exit_view" -MEET_UP = "meet_up" -EGO_STOP = "ego_stop" -OBJ_BASED_ACTION = [CAR_EXIT_SEGMENT, EXIT_VIEW, MEET_UP] +ActionType = Literal["ego_exit_segment", "car_exit_segment", "exit_view", "meet_up"] -ActionType = Literal["ego_exit_segment", "car_exit_segment", "exit_view", "meet_up", "ego_stop"] +EGO_EXIT_SEGMENT: "ActionType" = "ego_exit_segment" +CAR_EXIT_SEGMENT: "ActionType" = "car_exit_segment" +EXIT_VIEW: "ActionType" = "exit_view" +MEET_UP: "ActionType" = "meet_up" +OBJ_BASED_ACTION: "tuple[ActionType, ActionType]" = (CAR_EXIT_SEGMENT, EXIT_VIEW, MEET_UP) @dataclass @@ -73,167 +53,3 @@ def __str__(self): start loc: {self.start_loc}, end loc: {self.end_loc} estimated time: {self.estimated_time}""" - - -def ego_stop(ego_trajectory: "List[trajectory_3d]", ego_config: "CameraConfig"): - current_time = ego_config.timestamp - ego_loc = ego_config.ego_translation[:2] - _ego_stop, ego_departure_time, ego_departure_loc = ego_departure(ego_trajectory, current_time) - action = None - if _ego_stop: - action = Action( - current_time, ego_departure_time, ego_loc, ego_departure_loc, action_type=EGO_STOP - ) - return _ego_stop, action - - -def ego_exit_current_segment( - detection_info: "DetectionInfo", - ego_trajectory: "List[trajectory_3d]", - ego_config: "CameraConfig", -): - current_time = detection_info.timestamp - ego_loc = ego_config.ego_translation[:2] - exit_time, exit_point = time_to_exit_current_segment( - detection_info, current_time, ego_loc, car_trajectory=ego_trajectory, is_ego=True - ) - exit_action = Action(current_time, exit_time, ego_loc, exit_point, action_type=EGO_EXIT_SEGMENT) - return exit_action - - -def car_exit_current_segment(detection_info: "DetectionInfo"): - """ - Assumption: detected car drives at max speed - """ - current_time = detection_info.timestamp - car_loc = detection_info.car_loc3d - exit_time, exit_point = time_to_exit_current_segment(detection_info, current_time, car_loc) - exit_action = Action( - current_time, - exit_time, - start_loc=car_loc, - end_loc=exit_point, - action_type=CAR_EXIT_SEGMENT, - target_obj_id=detection_info.detection_id, - ) - return exit_action - - -def car_meet_up_with_ego( - detection_info: "DetectionInfo", - ego_trajectory: "List[trajectory_3d]", - ego_config: "CameraConfig", -): - current_time = detection_info.timestamp - car2_loc = detection_info.car_loc3d - car1_heading = ego_config.ego_heading - car2_heading = detection_info.segment_heading - if car2_heading is None: - return None - road_type = detection_info.road_type - car1_trajectory = ego_trajectory - ego_loc = tuple(ego_config.ego_translation) - meet_up_time, meetup_point = meetup( - ego_loc, car2_loc, car1_heading, car2_heading, road_type, current_time, car1_trajectory - ) - if meet_up_time < current_time: - return None - meet_up_action = Action( - current_time, - meet_up_time, - start_loc=car2_loc, - end_loc=meetup_point, - action_type=MEET_UP, - target_obj_id=detection_info.detection_id, - ) - return meet_up_action - - -def car_exit_view( - detection_info: "DetectionInfo", - ego_trajectory: "List[trajectory_3d]", - ego_config: "CameraConfig", - view_distance: float, -): - current_time = detection_info.timestamp - road_type = detection_info.road_type - ego_loc = ego_config.ego_translation - car_loc = detection_info.car_loc3d - car_heading = detection_info.segment_heading - if car_heading is None: - return None - exit_view_point, exit_view_time = time_to_exit_view( - ego_loc, car_loc, car_heading, ego_trajectory, current_time, road_type, view_distance - ) - exit_view_action = Action( - current_time, - exit_view_time, - start_loc=car_loc, - end_loc=exit_view_point, - action_type=EXIT_VIEW, - target_obj_id=detection_info.detection_id, - ) - return exit_view_action - - -def ego_by_pass_car(detection_info: "DetectionInfo") -> "Action": - raise Exception() - - -def combine_sample_actions(sample_plan: "List[Action]"): - best_plan = None - for action in sample_plan: - if best_plan is None and not action.invalid: - best_plan = action - else: - if not action.invalid and action.finish_time < best_plan.finish_time: - best_plan = action - return best_plan - - -def same_direction_sample_action(detection_info: "DetectionInfo", view_distance: float): - ego_trajectory = detection_info.ego_trajectory - ego_config = detection_info.ego_config - _ego_stop, ego_stop_action = ego_stop(ego_trajectory, ego_config) - if _ego_stop: - return ego_stop_action - ego_exit_segment_action = ego_exit_current_segment(detection_info, ego_trajectory, ego_config) - # logger.info(f'ego_exit_segment_action {ego_exit_segment_action}') - car_exit_segment_action = car_exit_current_segment(detection_info) - # logger.info(f'car_exit_segment_action {car_exit_segment_action}') - car_go_beyong_view_action = car_exit_view( - detection_info, ego_trajectory, ego_config, view_distance - ) - # logger.info(f'car_go_beyong_view_action {car_go_beyong_view_action}') - # ego_by_pass_car_action = ego_by_pass_car(detection_info, ego_trajectory, ego_config) - return combine_sample_actions( - [ego_exit_segment_action, car_exit_segment_action, car_go_beyong_view_action] - ) - - -def opposite_direction_sample_action(detection_info: "DetectionInfo", view_distance: float): - ego_trajectory = detection_info.ego_trajectory - ego_config = detection_info.ego_config - _ego_stop, ego_stop_action = ego_stop(ego_trajectory, ego_config) - if _ego_stop: - return ego_stop_action - ego_exit_segment_action = ego_exit_current_segment(detection_info, ego_trajectory, ego_config) - # logger.info(f'ego_exit_segment_action {ego_exit_segment_action}') - car_exit_segment_action = car_exit_current_segment(detection_info) - # logger.info(f'car_exit_segment_action {car_exit_segment_action}') - meet_ego_action = car_meet_up_with_ego(detection_info, ego_trajectory, ego_config) - # logger.info(f'meet_ego_action {meet_ego_action}') - # return car_exit_segment_action - actions = [ego_exit_segment_action, car_exit_segment_action] - if meet_ego_action is not None: - actions.append(meet_ego_action) - return combine_sample_actions(actions) - - -def get_sample_action_alg( - relative_direction: "Union[None, Literal['same_direction', 'opposite_direction']]", -): - if relative_direction == SAME_DIRECTION: - return same_direction_sample_action - elif relative_direction == OPPOSITE_DIRECTION: - return opposite_direction_sample_action diff --git a/spatialyze/video_processor/stages/detection_estimation/segment_mapping.py b/spatialyze/video_processor/stages/detection_estimation/segment_mapping.py index ebc4ff4..ba6d88c 100644 --- a/spatialyze/video_processor/stages/detection_estimation/segment_mapping.py +++ b/spatialyze/video_processor/stages/detection_estimation/segment_mapping.py @@ -13,112 +13,59 @@ import array import logging import math -import os import time -from dataclasses import dataclass from typing import NamedTuple, Tuple import numpy as np -import numpy.typing as npt -import plpygis import postgis -import psycopg2 -import psycopg2.sql -import shapely -import shapely.geometry -import shapely.wkb - -from spatialyze.database import database +import psycopg2.sql as sql +import shapely.geometry as sg +import shapely.wkb as swkb +from ....database import database from ...camera_config import CameraConfig -from .utils import ROAD_TYPES, Float2, Float3, Float22, line_to_polygon_intersection +from ...types import DetectionId, obj_detection +from .utils import ROAD_TYPES, Float22 logger = logging.getLogger(__name__) -data_path = "/home/yongming/workspace/research/apperception/v1.0-mini/" -input_video_dir = os.path.join(data_path, "sample_videos/") -input_video_name = "CAM_FRONT_n008-2018-08-27.mp4" -input_date = input_video_name.split("_")[-1][:-4] -test_img = "samples/CAM_FRONT/n008-2018-08-01-15-52-19-0400__CAM_FRONT__1533153253912404.jpg" - -POLYGON_CONTAIN_QUERY = psycopg2.sql.SQL( - """ -SELECT - p.elementid, - p.elementpolygon, - p.segmenttypes, - ARRAY_AGG(s.segmentline)::geometry[], - ARRAY_AGG(s.heading)::real[], - COUNT(DISTINCT p.elementpolygon), - COUNT(DISTINCT p.segmenttypes) -FROM segmentpolygon AS p - LEFT OUTER JOIN segment AS s USING (elementid) -WHERE ST_Contains( - p.elementpolygon, - {ego_translation}::geometry - ) - AND NOT p.__RoadType__roadsection__ - AND p.location = {location} -GROUP BY p.elementid; -""" -) -POLYGON_DWITHIN_QUERY = psycopg2.sql.SQL( - """ -SELECT - p.elementid, - p.elementpolygon, - p.segmenttypes, - ARRAY_AGG(s.segmentline)::geometry[], - ARRAY_AGG(s.heading)::real[], - COUNT(DISTINCT p.elementpolygon), - COUNT(DISTINCT p.segmenttypes) -FROM segmentpolygon AS p - LEFT OUTER JOIN segment AS s USING (elementid) -WHERE ST_DWithin( - p.elementpolygon, - {start_segment}::geometry, - {view_distance} +SQL_ROAD_TYPES = ",".join("__RoadType__" + rt + "__" for rt in ROAD_TYPES) + + +MAX_POLYGON_CONTAIN_QUERY = sql.SQL( + f""" + WITH + AvailablePolygon AS ( + SELECT * + FROM SegmentPolygon as p + WHERE + p.location = {{location}} + AND ST_Contains( + p.elementpolygon, + {{ego_translation}}::geometry + ) + ), + max_contain AS ( + SELECT MAX(ST_Area(elementpolygon)) max_segment_area + FROM AvailablePolygon ) - AND NOT p.__RoadType__roadsection__ - AND p.location = {location} -GROUP BY p.elementid; -""" + SELECT + p.elementid, + p.elementpolygon::geometry, + ARRAY_AGG(s.segmentline)::geometry[], + ARRAY_AGG(s.heading)::real[], + {SQL_ROAD_TYPES} + FROM max_contain, AvailablePolygon AS p + LEFT OUTER JOIN segment AS s USING (elementid) + WHERE ST_Area(p.elementpolygon) = max_contain.max_segment_area + GROUP BY p.elementid, p.elementpolygon, {SQL_ROAD_TYPES};""" ) +USEFUL_TYPES = ["lane", "lanegroup", "intersection"] -class RoadSegmentWithHeading(NamedTuple): - id: "str" - polygon: "postgis.Polygon" - road_types: "list[str]" - segmentline: "list[shapely.geometry.LineString]" - heading: "list[float]" - - -class Segment(NamedTuple): - id: "str" - polygon: "postgis.Polygon" - road_type: "str" - segmentline: "list[shapely.geometry.LineString]" - heading: "list[float]" - -class AnnotatedSegment(NamedTuple): - id: "str" - polygon: "postgis.Polygon" - road_type: "str" - segmentline: "list[shapely.geometry.LineString]" - heading: "list[float]" - contain: "bool" - - -class SegmentLine(NamedTuple): - line: "shapely.geometry.LineString" - heading: "float" - - -@dataclass -class RoadPolygonInfo: +class RoadPolygonInfo(NamedTuple): """ id: unique polygon id polygon: tuple of (x, y) coordinates @@ -131,152 +78,254 @@ class RoadPolygonInfo: """ id: str - polygon: "shapely.geometry.Polygon" - segment_lines: "list[shapely.geometry.LineString]" + polygon: "sg.Polygon" + segment_lines: "list[sg.LineString]" road_type: str segment_headings: "list[float]" contains_ego: bool ego_config: "CameraConfig" fov_lines: "tuple[Float22, Float22]" - def __post_init__(self): - if len(self.segment_lines) == 0: - return - - start_segment_map: "dict[Float2, tuple[shapely.geometry.LineString, float]]" = {} - ends: "set[Float2]" = set() - for line, heading in zip(self.segment_lines, self.segment_headings): - start = line.coords[0] - end = line.coords[1] - start_segment_map[start] = (line, heading) - ends.add(end) - starts: "list[Float2]" = [start for start in start_segment_map if start not in ends] - assert len(starts) == 1, len(starts) +class RoadSegmentWithHeading(NamedTuple): + id: "str" + polygon: "postgis.Polygon" + road_types: "list[str]" + segmentline: "list[sg.LineString]" + heading: "list[float]" - sorted_lines: "list[shapely.geometry.LineString]" = [] - sorted_headings: "list[float]" = [] - start: "Float2" = starts[0] - while start in start_segment_map: - line, heading = start_segment_map[start] - sorted_lines.append(line) - sorted_headings.append(heading) - start: "Float2" = line.coords[1] +class Segment(NamedTuple): + id: "str" + polygon: "postgis.Polygon" + road_type: "str" + segmentline: "list[sg.LineString]" + heading: "list[float]" - self.segment_lines = sorted_lines - self.segment_headings = sorted_headings +def reformat_return_polygon(segments: "list[RoadSegmentWithHeading]") -> "list[Segment]": + def _(x: "RoadSegmentWithHeading") -> "Segment": + i, polygon, types, lines, headings = x + type = types[-1] + for t in types: + if t in USEFUL_TYPES: + type = t + break + return Segment( + i, + polygon, + type, + lines, + [*map(math.degrees, headings)], + ) -class CameraPolygonMapping(NamedTuple): - cam_segment: "list[npt.NDArray[np.floating]]" - road_polygon_info: "RoadPolygonInfo" + return list(map(_, segments)) -def hex_str_to_linestring(hex: "str"): - return shapely.geometry.LineString(shapely.wkb.loads(bytes.fromhex(hex))) +def intersection(fov_line: Tuple[Float22, Float22], segmentpolygon: "sg.Polygon"): + """ + return: intersection point: tuple[tuple] + """ + left_fov_line, right_fov_line = fov_line + # left_intersection = line_to_polygon_intersection(segmentpolygon, left_fov_line) + # right_intersection = line_to_polygon_intersection(segmentpolygon, right_fov_line) + # return left_intersection + right_intersection + return [] -def make_road_polygon_with_heading(row: "tuple"): - eid, polygon, lines, headings, *types = row - assert len(types) == len(ROAD_TYPES), (types, ROAD_TYPES) - return RoadSegmentWithHeading( - eid, - polygon, - [t for t, v in zip(ROAD_TYPES, types) if v], - [*map(hex_str_to_linestring, lines[1:-1].split(":"))], - headings, - ) +def is_roadsection(segmenttypes: "list[int]"): + for t, v in zip(ROAD_TYPES, segmenttypes): + if t == "roadsection" and v: + return True + return False -def road_polygon_contains(ego_config: "CameraConfig") -> "list[RoadSegmentWithHeading]": +def get_largest_polygon_containing_point(ego_config: "CameraConfig"): point = postgis.Point(*ego_config.ego_translation[:2]) - query = POLYGON_CONTAIN_QUERY.format( - ego_translation=psycopg2.sql.Literal(point), - location=psycopg2.sql.Literal(ego_config.location), + query = MAX_POLYGON_CONTAIN_QUERY.format( + ego_translation=sql.Literal(point), location=sql.Literal(ego_config.location) ) results = database.execute(query) - assert all(r[5:] == (1, 1) for r in results) + if len(results) > 1: + for result in results: + segmenttypes = result[4:] + if not is_roadsection(segmenttypes): + results = [result] + break + assert len(results) == 1, (ROAD_TYPES, [r[4:] for r in results]) + result = results[0] + assert len(result) == 4 + len(ROAD_TYPES), (len(results), len(ROAD_TYPES) + 4) + + output = make_road_polygon_with_heading(result) + + types, line, heading = output[2:5] + assert line is not None + assert types is not None + assert heading is not None + road_polygon = reformat_return_polygon([output])[0] + polygonid, roadpolygon, roadtype, segmentlines, segmentheadings = road_polygon + fov_lines = get_fov_lines(ego_config) - output = [*map(make_road_polygon_with_heading, results)] - assert len(output) > 0 - for row in output: - types, line, heading = row[2:5] - assert line is not None - assert types is not None - assert heading is not None - return output + polygon = swkb.loads(roadpolygon.to_ewkb(), hex=True) + assert isinstance(polygon, sg.Polygon) + return RoadPolygonInfo( + polygonid, polygon, segmentlines, roadtype, segmentheadings, True, ego_config, fov_lines + ) -def find_polygon_dwithin( - start_segment: "AnnotatedSegment", ego_config: "CameraConfig", view_distance: "float | int" = 50 -) -> "list[RoadSegmentWithHeading]": - _, start_segment_polygon, _, _, _, _ = start_segment - query = POLYGON_DWITHIN_QUERY.format( - start_segment=psycopg2.sql.Literal(start_segment_polygon), - view_distance=psycopg2.sql.Literal(view_distance), - location=psycopg2.sql.Literal(ego_config.location), +def map_detections_to_segments(detections: "list[obj_detection]", ego_config: "CameraConfig"): + tokens = [*map(lambda x: x.detection_id.obj_order, detections)] + points = [postgis.Point(d.car_loc3d[0], d.car_loc3d[1]) for d in detections] + + location = ego_config.location + + convex_points = np.array([[d.car_loc3d[0], d.car_loc3d[1]] for d in detections]) + + out = sql.SQL( + f""" + WITH + Point AS ( + SELECT * + FROM UNNEST( + {{tokens}}, + {{points}}::geometry(Point)[] + ) AS _point (token, point) + ), + AvailablePolygon AS ( + SELECT * + FROM SegmentPolygon + WHERE location = {{location}} + AND ST_Intersects(SegmentPolygon.elementPolygon, ST_ConvexHull({{convex}}::geometry(MultiPoint))) + AND (SegmentPolygon.__RoadType__intersection__ + OR SegmentPolygon.__RoadType__lane__ + OR SegmentPolygon.__RoadType__lanegroup__ + OR SegmentPolygon.__RoadType__lanesection__) + AND NOT SegmentPolygon.__RoadType__roadsection__ + ), + MinPolygon AS ( + SELECT token, MIN(ST_Area(Polygon.elementPolygon)) as size + FROM Point AS p + JOIN AvailablePolygon AS Polygon + ON ST_Contains(Polygon.elementPolygon, p.point) + GROUP BY token + ), + MinPolygonId AS ( + SELECT token, MIN(elementId) as elementId + FROM Point AS p + JOIN MinPolygon USING (token) + JOIN AvailablePolygon as Polygon + ON ST_Contains(Polygon.elementPolygon, p.point) + AND ST_Area(Polygon.elementPolygon) = MinPolygon.size + GROUP BY token + ) + SELECT + p.token, + AvailablePolygon.elementid, + AvailablePolygon.elementpolygon, + ARRAY_AGG(Segment.segmentline)::geometry[], + ARRAY_AGG(Segment.heading)::real[], + {SQL_ROAD_TYPES} + FROM Point AS p + JOIN MinPolygonId USING (token) + JOIN AvailablePolygon USING (elementId) + JOIN Segment USING (elementId) + GROUP BY + AvailablePolygon.elementid, + p.token, + AvailablePolygon.elementpolygon, + {SQL_ROAD_TYPES}; + """ + ).format( + tokens=sql.Literal(tokens), + points=sql.Literal(points), + convex=sql.Literal(postgis.MultiPoint(map(tuple, convex_points))), + location=sql.Literal(location), ) + result = database.execute(out) + return result - results = database.execute(query) - assert all(r[5:] == (1, 1) for r in results) - output = [*map(make_road_polygon_with_heading, results)] - for row in output: +def get_detection_polygon_mapping(detections: "list[obj_detection]", ego_config: "CameraConfig"): + """ + Given a list of detections, return a list of RoadSegmentWithHeading + """ + # start_time = time.time() + times = [] + times.append(time.time()) + results = map_detections_to_segments(detections, ego_config) + times.append(time.time()) + + order_ids, mapped_polygons = [r[0] for r in results], [r[1:] for r in results] + mapped_polygons = [*map(make_road_polygon_with_heading, mapped_polygons)] + times.append(time.time()) + for row in mapped_polygons: types, line, heading = row[2:5] assert line is not None assert types is not None assert heading is not None - return output - - -def reformat_return_polygon(segments: "list[RoadSegmentWithHeading]") -> "list[Segment]": - def _(x: "RoadSegmentWithHeading") -> "Segment": - i, polygon, types, lines, headings = x - return Segment( - i, - polygon, - # TODO: fix this hack: all the useful types are 'lane', 'lanegroup', - # 'intersection' which are always at the last position - types[-1], - lines, - [*map(math.degrees, headings)], - ) - - return list(map(_, segments)) - - -def annotate_contain(segments: "list[Segment]", contain: bool = False) -> "list[AnnotatedSegment]": - return [AnnotatedSegment(*s, contain) for s in segments] - - -def construct_search_space( - ego_config: "CameraConfig", view_distance: float = 50.0 -) -> "list[AnnotatedSegment]": - """ - ego_config: current config of a camera - view_distance: in meters, default 50 because scenic standard - return: set(road_polygon) - """ - all_contain_polygons = reformat_return_polygon(road_polygon_contains(ego_config)) - all_contain_polygons = annotate_contain(all_contain_polygons, contain=True) - start_segment = all_contain_polygons[0] + times.append(time.time()) + mapped_polygons = reformat_return_polygon(mapped_polygons) + times.append(time.time()) + if any(p.road_type == "intersection" for p in mapped_polygons): + return {}, times + times.append(time.time()) + fov_lines = get_fov_lines(ego_config) + times.append(time.time()) + + # def not_in_view(point: "Float2"): + # return not in_view(point, ego_config.ego_translation, fov_lines) + + mapped_road_polygon_info: "dict[DetectionId, RoadPolygonInfo]" = {} + for order_id, road_polygon in list(zip(order_ids, mapped_polygons)): + frame_idx = detections[0].detection_id.frame_idx + det_id = DetectionId(frame_idx=frame_idx, obj_order=order_id) + if det_id in mapped_road_polygon_info: + print("skipped") + continue + polygonid, roadpolygon, roadtype, segmentlines, segmentheadings = road_polygon + assert segmentlines is not None + assert segmentheadings is not None - polygons_within_distance = reformat_return_polygon( - find_polygon_dwithin(start_segment, ego_config, view_distance) - ) - polygons_within_distance = annotate_contain(polygons_within_distance, contain=False) + # assert all(isinstance(line, sg.LineString) for line in segmentlines) - ids: "set[str]" = set() - polygons: "list[AnnotatedSegment]" = [] - for p in all_contain_polygons + polygons_within_distance: - if p.id not in ids: - ids.add(p.id) - polygons.append(p) + p = swkb.loads(road_polygon.to_ewkb(), hex=True) + assert isinstance(p, sg.Polygon) + XYs: "Tuple[array.array[float], array.array[float]]" = p.exterior.xy + assert isinstance(XYs, tuple) + assert isinstance(XYs[0], array.array), type(XYs[0]) + assert isinstance(XYs[1], array.array), type(XYs[1]) + assert isinstance(XYs[0][0], float), type(XYs[0][0]) + assert isinstance(XYs[1][0], float), type(XYs[1][0]) + polygon_points = list(zip(*XYs)) + # roadpolygon = sg.Polygon(polygon_points) + + # decoded_road_polygon_points = polygon_points + # if all(map(not_in_view, polygon_points)): + # continue + + # intersection_points = intersection(fov_lines, roadpolygon) + # decoded_road_polygon_points += intersection_points + # keep_road_polygon_points: "list[Float2]" = [] + # for current_road_point in decoded_road_polygon_points: + # if in_view(current_road_point, ego_config.ego_translation, fov_lines): + # keep_road_polygon_points.append(current_road_point) + if len(polygon_points) > 2: + # and sg.Polygon(tuple(keep_road_polygon_points)).area > 1): + mapped_road_polygon_info[det_id] = RoadPolygonInfo( + polygonid, + sg.Polygon(polygon_points), + segmentlines, + roadtype, + segmentheadings, + False, + ego_config, + fov_lines, + ) + times.append(time.time()) - assert any(p.contain for p in polygons) - return polygons + # logger.info(f'total mapping time: {time.time() - start_time}') + return mapped_road_polygon_info, times def get_fov_lines(ego_config: "CameraConfig", ego_fov: float = 70.0) -> "Tuple[Float22, Float22]": @@ -301,254 +350,17 @@ def get_fov_lines(ego_config: "CameraConfig", ego_fov: float = 70.0) -> "Tuple[F return left_fov_line, right_fov_line -def intersection(fov_line: Tuple[Float22, Float22], segmentpolygon: "shapely.geometry.Polygon"): - """ - return: intersection point: tuple[tuple] - """ - left_fov_line, right_fov_line = fov_line - left_intersection = line_to_polygon_intersection(segmentpolygon, left_fov_line) - right_intersection = line_to_polygon_intersection(segmentpolygon, right_fov_line) - return left_intersection + right_intersection +def hex_str_to_linestring(hex: "str"): + return sg.LineString(swkb.loads(bytes.fromhex(hex))) -def in_frame(transformed_point: "npt.NDArray", frame_size: Tuple[int, int]): - return ( - transformed_point[0] > 0 - and transformed_point[0] < frame_size[0] - and transformed_point[1] < frame_size[1] - and transformed_point[1] > 0 +def make_road_polygon_with_heading(row: "tuple"): + eid, polygon, lines, headings, *types = row + assert len(types) == len(ROAD_TYPES), (types, ROAD_TYPES) + return RoadSegmentWithHeading( + eid, + polygon, + [t for t, v in zip(ROAD_TYPES, types) if v], + [*map(hex_str_to_linestring, lines[1:-1].split(":"))], + headings, ) - - -def in_view( - road_point: "Float2", ego_translation: "Float3", fov_lines: Tuple[Float22, Float22] -) -> bool: - """ - return if the road_point is on the left of the left fov line and - on the right of the right fov line - """ - left_fov_line, right_fov_line = fov_lines - Ax, Ay = ego_translation[:2] - Mx, My = road_point - left_fov_line_x, left_fov_line_y = left_fov_line[1] - right_fov_line_x, right_fov_line_y = right_fov_line[1] - return (left_fov_line_x - Ax) * (My - Ay) - (left_fov_line_y - Ay) * (Mx - Ax) <= 0 and ( - right_fov_line_x - Ax - ) * (My - Ay) - (right_fov_line_y - Ay) * (Mx - Ax) >= 0 - - -def world2pixel_factory(config: "CameraConfig"): - def world2pixel(point3d: "Float2") -> "npt.NDArray[np.floating]": - point = np.copy((*point3d, 0)) - - point -= config.camera_translation - point = np.dot(config.camera_rotation.inverse.rotation_matrix, point) - - view = np.array(config.camera_intrinsic) - viewpad = np.eye(4) - viewpad[: view.shape[0], : view.shape[1]] = view - - point = point.reshape((3, 1)) - point = np.concatenate((point, np.ones((1, 1)))) - point = np.dot(viewpad, point) - point = point[:3, :] - - point = point / point[2:3, :].repeat(3, 0).reshape(3, 1) - return point[:2, :] - - return world2pixel - - -def world2pixel_all(points3d: "list[Float2]", config: "CameraConfig"): - n = len(points3d) - points = np.concatenate([np.array(points3d), np.zeros((n, 1))], axis=1) - assert points.shape == (n, 3) - points -= config.camera_translation - assert points.shape == (n, 3) - points = config.camera_rotation.inverse.rotation_matrix @ points.T - assert points.shape == (3, n) - - intrinsic = np.array(config.camera_intrinsic) - assert intrinsic.shape == (3, 3), intrinsic.shape - - points = intrinsic @ points - assert points.shape == (3, n) - - points = points / points[2:3, :] - return points.T[:, :2] - - -def construct_mapping( - decoded_road_polygon_points: "list[Float2]", - frame_size: Tuple[int, int], - fov_lines: Tuple[Float22, Float22], - segmentid: str, - segmentlines: "list[shapely.geometry.LineString]", - segmenttype: str, - segmentheadings: "list[float]", - contains_ego: bool, - ego_config: "CameraConfig", -) -> "CameraPolygonMapping | None": - """ - Given current road segment - determine whether add it to the mapping - - segment that contains the ego - - segment that is larger than 100 pixel x pixel - """ - if segmenttype is None or segmenttype == "None": - return - - ego_translation = ego_config.ego_translation - - # deduced_cam_segment = list(map(worl2pixel_factory(ego_config), decoded_road_segment)) - deduced_cam_polygon_points = world2pixel_all(decoded_road_polygon_points, ego_config) - # assert np.allclose(np.array(deduced_cam_segment).reshape((len(decoded_road_segment), 2)), deduced_cam_segment2) - assert len(deduced_cam_polygon_points) == len(decoded_road_polygon_points) - if contains_ego: - keep_cam_polygon_points = deduced_cam_polygon_points - keep_road_polygon_points = decoded_road_polygon_points - else: - keep_cam_polygon_points: "list[npt.NDArray[np.floating]]" = [] - keep_road_polygon_points: "list[Float2]" = [] - for current_cam_point, current_road_point in zip( - deduced_cam_polygon_points, decoded_road_polygon_points - ): - if in_frame(current_cam_point, frame_size) and in_view( - current_road_point, ego_translation, fov_lines - ): - keep_cam_polygon_points.append(current_cam_point) - keep_road_polygon_points.append(current_road_point) - ret = None - if contains_ego or ( - len(keep_cam_polygon_points) > 2 - and shapely.geometry.Polygon(tuple(keep_cam_polygon_points)).area > 100 - ): - ret = CameraPolygonMapping( - keep_cam_polygon_points, - RoadPolygonInfo( - segmentid, - shapely.geometry.Polygon(keep_road_polygon_points), - segmentlines, - segmenttype, - segmentheadings, - contains_ego, - ego_config, - fov_lines, - ), - ) - return ret - - -def map_imgsegment_roadsegment( - ego_config: "CameraConfig", frame_size: "Tuple[int, int]" = (1600, 900) -) -> "list[CameraPolygonMapping]": - """Construct a mapping from frame segment to road segment - - Given an image, we know that different roads/lanes belong to different - road segment in the road network. We want to find a mapping - from the road/lane/intersection to the real world road segment so that - we know which part of the image belong to which part of the real world - - Return List[namedtuple(cam_segment_mapping)]: each tuple looks like this - (polygon in frame that represents a portion of lane/road/intersection, - roadSegmentInfo) - """ - fov_lines = get_fov_lines(ego_config) - start_time = time.time() - search_space = construct_search_space(ego_config, view_distance=100) - mapping: "dict[str, CameraPolygonMapping]" = dict() - - def not_in_view(point: "Float2"): - return not in_view(point, ego_config.ego_translation, fov_lines) - - for road_polygon in search_space: - polygonid, roadpolygon, roadtype, segmentlines, segmentheadings, contains_ego = road_polygon - assert segmentlines is not None - assert segmentheadings is not None - - assert all(isinstance(line, shapely.geometry.LineString) for line in segmentlines) - - p = plpygis.Geometry(roadpolygon.to_ewkb()) - assert isinstance(p, plpygis.Polygon) - XYs: "Tuple[array.array[float], array.array[float]]" = p.exterior.shapely.xy - assert isinstance(XYs, tuple) - assert isinstance(XYs[0], array.array), type(XYs[0]) - assert isinstance(XYs[1], array.array), type(XYs[1]) - assert isinstance(XYs[0][0], float), type(XYs[0][0]) - assert isinstance(XYs[1][0], float), type(XYs[1][0]) - polygon_points = list(zip(*XYs)) - roadpolygon = shapely.geometry.Polygon(polygon_points) - - decoded_road_polygon_points = polygon_points - if not contains_ego: - if all(map(not_in_view, polygon_points)): - continue - - intersection_points = intersection(fov_lines, roadpolygon) - decoded_road_polygon_points += intersection_points - - current_mapping = construct_mapping( - decoded_road_polygon_points, - frame_size, - fov_lines, - polygonid, - segmentlines, - roadtype, - segmentheadings, - contains_ego, - ego_config, - ) - - if current_mapping is not None: - mapping[polygonid] = current_mapping - - logger.info(f"total mapping time: {time.time() - start_time}") - return [*mapping.values()] - - -# def visualization(test_img_path: str, test_config: Dict[str, Any], mapping: Tuple): -# """ -# visualize the mapping from camera segment to road segment -# for testing only -# """ -# from moviepy.editor import VideoClip -# from moviepy.video.io.bindings import mplfig_to_npimage -# frame = cv2.imread(test_img_path) -# fig, axs = plt.subplots() -# axs.set_aspect('equal', 'datalim') -# x_ego, y_ego = test_config['egoTranslation'][:2] -# axs.plot(x_ego, y_ego, color='green', marker='o', markersize=5) -# colormap = plt.cm.get_cmap('hsv', len(mapping)) -# i = 0 -# fourcc = cv2.VideoWriter_fourcc(*'mp4v') -# display_video = cv2.VideoWriter('in_videw_test_display.avi',fourcc, 1, (1600, 900)) -# for cam_segment, road_segment_info in mapping: -# color = colormap(i) -# xs = [point[0] for point in road_segment_info.segment_polygon.exterior.coords] -# ys = [point[1] for point in road_segment_info.segment_polygon.exterior.coords] -# segmenttype = road_segment_info.segment_type -# axs.fill(xs, ys, alpha=0.5, fc=color, ec='none') -# axs.text(np.mean(np.array(xs)), np.mean(np.array(ys)), -# segmenttype if segmenttype else '') -# current_plt = mplfig_to_npimage(fig) -# i += 1 - -# fov_lines = road_segment_info.fov_lines -# axs.plot([p[0] for p in fov_lines[0]], [p[1] for p in fov_lines[0]], color='red', marker='o', markersize=2) -# axs.plot([p[0] for p in fov_lines[1]], [p[1] for p in fov_lines[1]], color='red', marker='o', markersize=2) - -# display_frame = frame.copy() -# cv2.polylines(display_frame, [np.array(cam_segment, np.int32).reshape((-1, 1, 2))], True, (0, 255, 0), 2) -# display_frame[:current_plt.shape[0], :current_plt.shape[1]] = current_plt -# display_video.write(display_frame) - -# display_video.release() - -# if __name__ == '__main__': -# test_img_path = os.path.join(data_path, test_img) -# test_config = fetch_camera_config( -# test_img, -# database) -# test_config = camera_config(**test_config) -# mapping = map_imgsegment_roadsegment(test_config) -# visualization(test_img_path, test_config, mapping) diff --git a/spatialyze/video_processor/stages/detection_estimation/utils.py b/spatialyze/video_processor/stages/detection_estimation/utils.py index a1311e9..3fb9ad9 100644 --- a/spatialyze/video_processor/stages/detection_estimation/utils.py +++ b/spatialyze/video_processor/stages/detection_estimation/utils.py @@ -1,27 +1,24 @@ import datetime import logging import math -from typing import TYPE_CHECKING, List, NamedTuple, Tuple +from typing import TYPE_CHECKING, List, NamedTuple import numpy as np import numpy.typing as npt +import postgis +import shapely import shapely.geometry - -from spatialyze.database import database +import shapely.wkb if TYPE_CHECKING: from ...camera_config import CameraConfig + from ...types import Float2, Float3, Float22 from .detection_estimation import DetectionInfo - from .optimized_segment_mapping import RoadPolygonInfo - from .segment_mapping import CameraPolygonMapping + from .segment_mapping import RoadPolygonInfo logger = logging.getLogger(__name__) -Float2 = Tuple[float, float] -Float3 = Tuple[float, float, float] -Float22 = Tuple[Float2, Float2] - SAME_DIRECTION = "same_direction" OPPOSITE_DIRECTION = "opposite_direction" @@ -214,10 +211,6 @@ def max_car_speed(road_type): return MAX_CAR_SPEED[road_type] -def min_car_speed(road_type): - return max_car_speed(road_type) / 2 - - ### HELPER FUNCTIONS ### def get_ego_trajectory(video: str, sorted_ego_config: "list[CameraConfig]"): assert sorted_ego_config is not None @@ -245,50 +238,6 @@ def get_ego_avg_speed(ego_trajectory): return sum([speed.speed for speed in point_wise_ego_speed]) / len(point_wise_ego_speed) -def detection_to_img_segment( - car_loc2d: "Float2", - cam_polygon_mapping: "list[CameraPolygonMapping]", -): - """Get the image segment that contains the detected car.""" - maximum_mapping: "CameraPolygonMapping | None" = None - maximum_mapping_area: float = 0.0 - point = shapely.geometry.Point(car_loc2d) - - for mapping in cam_polygon_mapping: - cam_segment, road_segment_info = mapping - p_cam_segment = shapely.geometry.Polygon(cam_segment) - segment_type = road_segment_info.road_type - if p_cam_segment.contains(point) and segment_type in SEGMENT_TO_MAP: - area = p_cam_segment.area - if area > maximum_mapping_area: - maximum_mapping = mapping - maximum_mapping_area = area - - return maximum_mapping - - -def detection_to_nearest_segment( - car_loc3d: "Float3", - cam_polygon_mapping: "list[CameraPolygonMapping]", -): - assert len(cam_polygon_mapping) != 0 - point = shapely.geometry.Point(car_loc3d[:2]) - - min_distance = float("inf") - min_mapping = None - for mapping in cam_polygon_mapping: - _, road_polygon_info = mapping - polygon = road_polygon_info.polygon - - distance = polygon.distance(point) - if distance < min_distance: - min_distance = distance - min_mapping = mapping - - assert min_mapping is not None - return min_mapping - - def get_segment_line(road_segment_info: "RoadPolygonInfo", car_loc3d: "Float3"): """Get the segment line the location is in.""" segment_lines = road_segment_info.segment_lines @@ -314,84 +263,6 @@ def get_segment_line(road_segment_info: "RoadPolygonInfo", car_loc3d: "Float3"): return longest_segment_line, longest_heading -def location_calibration(car_loc3d: "Float3", road_segment_info: "RoadPolygonInfo") -> "Float3": - """Calibrate the 3d location of the car with the road segment - the car lies in. - """ - segment_polygon = road_segment_info.polygon - assert segment_polygon is not None - segment_lines = road_segment_info.segment_lines - if segment_lines is None or len(segment_lines) == 0: - return car_loc3d - # TODO: project onto multiple linestrings and find the closest one - projection = project_point_onto_linestring( - shapely.geometry.Point(car_loc3d[:2]), segment_lines - ).coords - return projection[0], projection[1], car_loc3d[2] - - -def get_largest_polygon_containing_ego(cam_segment_mapping: "List[CameraPolygonMapping]"): - maximum_mapping: "CameraPolygonMapping | None" = None - maximum_mapping_area: float = 0.0 - - for mapping in cam_segment_mapping: - _, road_segment_info = mapping - area = shapely.geometry.Polygon(road_segment_info.polygon).area - if road_segment_info.contains_ego and area > maximum_mapping_area: - maximum_mapping = mapping - maximum_mapping_area = area - - assert maximum_mapping is not None, [c.road_polygon_info.id for c in cam_segment_mapping] - return maximum_mapping - - -def time_to_nearest_frame( - video: str, timestamp: "datetime.datetime" -) -> "Tuple[str, int, datetime.datetime]": - """Return the frame that is closest to the timestamp""" - query = f""" - WITH Cameras_with_diff as ( - SELECT *, abs(extract(epoch from timestamp-\'{timestamp}\')) as diff - FROM Cameras - WHERE fileName LIKE '%{video}%' - ) - SELECT - fileName, - frameNum, - cameras.timestamp - FROM Cameras_with_diff c1 - WHERE c1.diff = (SELECT MIN(c2.diff) from Cameras_with_diff c2) - LIMIT 1 - """ - return database.execute(query)[0] - - -def timestamp_to_nearest_trajectory(trajectory, timestamp): - """Return the trajectory point that is closest to the timestamp""" - return min(trajectory, key=lambda x: abs((x.timestamp - timestamp).total_seconds())) - - -def point_to_nearest_trajectory(point, trajectory): - """Return the trajectory point that is closest to the point""" - return min(trajectory, key=lambda x: compute_distance(x.coordinates, point)) - - -def ego_departure(ego_trajectory: "List[trajectory_3d]", current_time: "datetime.datetime"): - for i in range(len(ego_trajectory)): - point = ego_trajectory[i] - if point.timestamp > current_time: - for j in range(i, len(ego_trajectory)): - if compute_distance(ego_trajectory[j].coordinates, point.coordinates) < 5: - non_stop_point = ego_trajectory[j] - break - if i == j: - return False, point.timestamp, point.coordinates - elif j == len(ego_trajectory) - 1: - return True, ego_trajectory[j].timestamp, ego_trajectory[j].coordinates - return True, non_stop_point.timestamp, non_stop_point.coordinates - return False, ego_trajectory[-1].timestamp, ego_trajectory[-1].coordinates - - def time_to_exit_current_segment( detection_info: "DetectionInfo", current_time, car_loc, car_trajectory=None, is_ego=False ): @@ -454,117 +325,9 @@ def time_to_exit_current_segment( return None, None -def meetup( - car1_loc: "Float3 | shapely.geometry.Point", - car2_loc: "Float3 | shapely.geometry.Point", - car1_heading, - car2_heading, - road_type, - current_time, - car1_trajectory: "List[trajectory_3d] | None" = None, - car2_trajectory: "List[trajectory_3d] | None" = None, - car1_speed=None, - car2_speed=None, -): - """estimate the meetup point as the middle point between car1's loc and car2's loc - - If both trajectories are given, the meetup point is the point where the two trajectories meets - If one trajectory is given, use that trajectory and other car's speed and direction - If none trajectory is given, use both cars' speed, or max speed if no speed is given and cars' direction - - For timestamp, it's an estimation based on the trajectory or speed - Return: (timestamp, meetup_point) - - Assumptions: - car1 and car2 are driving towards each other, not necessarily the opposite direction - There shouldn't be a point they intersect, otherwise it's a collision - If no trajectory, or speed is given, car drives at max speed - TODO: now I've just implemented the case for ego car to meet another detected car - """ - car1_loc = shapely.geometry.Point(car1_loc[:2]) if isinstance(car1_loc, tuple) else car1_loc - assert isinstance(car1_loc, shapely.geometry.Point) - car2_loc = shapely.geometry.Point(car2_loc[:2]) if isinstance(car2_loc, tuple) else car2_loc - assert isinstance(car2_loc, shapely.geometry.Point) - if car1_trajectory is not None and car2_trajectory is None: - car2_speed = max_car_speed(road_type) if car2_speed is None else car2_speed - car2_heading += 90 - car2_vector = ( - car2_loc.x + math.cos(math.radians(car2_heading)), - car2_loc.y + math.sin(math.radians(car2_heading)), - ) - car2_heading_line = (car2_loc, car2_vector) - car1_trajectory_points = [ - point.coordinates for point in car1_trajectory if point.timestamp > current_time - ] - intersection = intersection_between_line_and_trajectory( - car2_heading_line, car1_trajectory_points - ) - if len(intersection) == 1: # i.e. one car drives towards south, the other towards east - # logger.info(f"at intersection 1") - meetup_point = intersection[0] - time1 = point_to_nearest_trajectory(meetup_point, car1_trajectory) - distance2 = compute_distance(car2_loc, meetup_point) - time2 = time_elapse(current_time, distance2 / car2_speed) - return (min(time1, time2), meetup_point) - elif len(intersection) == 0: # i.e. one car drives towards south, the other towards north - # logger.info(f"at intersection 0") - meetup_point = shapely.geometry.Point( - (car1_loc.x + car2_loc.x) / 2, (car1_loc.y + car2_loc.y) / 2 - ) - time1 = point_to_nearest_trajectory(meetup_point, car1_trajectory).timestamp - if time1 < current_time: - time1 = current_time - distance2 = compute_distance(car2_loc, meetup_point) - time2 = time_elapse(current_time, distance2 / car2_speed) - if time2 < current_time: - time2 = current_time - return (min(time1, time2), meetup_point) - - -def catchup_time( - car1_loc, - car2_loc, - road_type=None, - car1_trajectory=None, - car2_trajectory=None, - car1_speed=None, - car2_speed=None, -): - """Return the time that car1 catches up to car2 - - Assumption: - 1. car1 and car2 are driving towards the same direction - 2. car1 drives at max speed, car2 drives at min speed - if no trajectory or speed is given - 3. TODO: assume now ego is the slowest, it won't bypass another car - """ - - -def in_view(car_loc, ego_loc, view_distance): - """At this point, we only care about detect cars. So they are in the frame - in_view means whether the car is recognizable enough - """ - return compute_distance(car_loc, ego_loc) < view_distance - - -def time_to_exit_view( - ego_loc, car_loc, car_heading, ego_trajectory, current_time, road_type, view_distance -): - """Return the time, and location that the car goes beyond ego's view distance - - Assumption: car drives at max speed - """ - ego_speed = get_ego_avg_speed(ego_trajectory) - car_speed = max_car_speed(road_type) - exit_view_time = time_elapse( - current_time, (view_distance - compute_distance(ego_loc, car_loc)) / (car_speed - ego_speed) - ) - return timestamp_to_nearest_trajectory(ego_trajectory, exit_view_time) - - def get_car_exits_view_frame_num( detection_info: "DetectionInfo", - ego_views: "list[shapely.geometry.Polygon]", + ego_views: "list[postgis.Polygon]", max_frame_num: int, fps=20, ): @@ -589,7 +352,7 @@ def car_exits_view_frame_num( car_loc: "Float2", car_heading: "float", road_type: "str", - ego_views: "list[shapely.geometry.Polygon]", + ego_views: "list[postgis.Polygon]", current_frame_num: "int", car_exits_segment_frame_num: "int", fps: "int", @@ -602,30 +365,10 @@ def car_exits_view_frame_num( while current_frame_num + 1 < car_exits_segment_frame_num: next_frame_num = current_frame_num + 1 next_ego_view = ego_views[next_frame_num] + next_ego_view = shapely.wkb.loads(next_ego_view.to_ewkb(), hex=True) duration = (next_frame_num - start_frame_num) / fps next_car_loc = car_move(car_loc, car_heading, car_speed, duration) if not next_ego_view.contains(shapely.geometry.Point(next_car_loc[:2])): return max(current_frame_num, start_frame_num + 1) current_frame_num = next_frame_num return car_exits_segment_frame_num - - -def relative_direction_to_ego(obj_heading: float, ego_heading: float): - """Return the relative direction to ego - Now only support opposite and same direction - TODO: add driving into and driving away from - """ - if obj_heading is None: - return - - relative_heading = abs(obj_heading - ego_heading) % 360 - if ( - math.cos(math.radians(relative_heading)) < 1 - and math.cos(math.radians(relative_heading)) > math.pi / 6 - ): - return SAME_DIRECTION - elif ( - math.cos(math.radians(relative_heading)) > -1 - and math.cos(math.radians(relative_heading)) < -math.pi / 6 - ): - return OPPOSITE_DIRECTION diff --git a/spatialyze/video_processor/stages/segment_trajectory/__init__.py b/spatialyze/video_processor/stages/segment_trajectory/__init__.py index 885b39f..b0e88a3 100644 --- a/spatialyze/video_processor/stages/segment_trajectory/__init__.py +++ b/spatialyze/video_processor/stages/segment_trajectory/__init__.py @@ -1,60 +1,53 @@ -from typing import Any, Dict, Tuple +import datetime +from dataclasses import dataclass +from typing import Any, Dict, NamedTuple, Union -from ...payload import Payload -from ...types import DetectionId -from ..detection_2d.detection_2d import Detection2D -from ..detection_estimation import DetectionEstimation -from ..detection_estimation.detection_estimation import DetectionInfo -from ..detection_estimation.optimized_segment_mapping import RoadPolygonInfo -from ..detection_estimation.segment_mapping import RoadPolygonInfo as RoadPolygonInfo_ -from ..detection_estimation.utils import trajectory_3d +import postgis +import shapely.geometry as sg + +from ...types import DetectionId, Float3 +from ..detection_estimation.segment_mapping import RoadPolygonInfo from ..stage import Stage -from ..tracking_2d.strongsort import StrongSORT -from .construct_segment_trajectory import ( - InvalidSegmentPoint, - PolygonAndId, - SegmentPoint, - ValidSegmentPoint, - calibrate, -) -SegmentTrajectoryMetadatum = Dict[int, SegmentPoint] +class PolygonAndId(NamedTuple): + id: "str" + polygon: "sg.Polygon" -class SegmentTrajectory(Stage[SegmentTrajectoryMetadatum]): - # @cache - def _run(self, payload: "Payload"): - if Detection2D.get(payload) is None: - raise Exception() - metadata: "list[dict[int, SegmentPoint]]" = [dict() for _ in range(len(payload.video))] - obj_trajectories = construct_trajectory(payload) - print("obj_trajectories", len(obj_trajectories)) - for oid, t in obj_trajectories.items(): - print(oid, len(t)) - trajectory_3d = [tt[0] for tt in t] - detection_infos = [tt[1] for tt in t] - frame_indices = [tt[2] for tt in t] +@dataclass +class ValidSegmentPoint: + detection_id: "DetectionId" + car_loc3d: "Float3" + timestamp: "datetime.datetime" + segment_type: "str" + segment_line: "postgis.LineString" + segment_heading: "float" + road_polygon_info: "RoadPolygonInfo | PolygonAndId" + obj_id: "int | None" = None + type: "str | None" = None + next: "SegmentPoint | None" = None + prev: "SegmentPoint | None" = None - calibrated_trajectory = calibrate( - trajectory_3d, detection_infos, frame_indices, payload - ) - for t in calibrated_trajectory: - t.obj_id = oid - calibrated_trajectory.sort(key=lambda t: t.timestamp) +@dataclass +class InvalidSegmentPoint: + detection_id: "DetectionId" + car_loc3d: "Float3" + timestamp: "datetime.datetime" + obj_id: "int | None" = None + type: "str | None" = None + next: "SegmentPoint | None" = None + prev: "SegmentPoint | None" = None - for before, after in zip(calibrated_trajectory[:-1], calibrated_trajectory[1:]): - before.next = after - after.prev = before - for t in calibrated_trajectory: - idx, _ = t.detection_id - assert oid not in metadata[idx] - metadata[idx][oid] = t +SegmentPoint = Union[ValidSegmentPoint, InvalidSegmentPoint] - return None, {self.classname(): metadata} +SegmentTrajectoryMetadatum = Dict[int, SegmentPoint] + + +class SegmentTrajectory(Stage[SegmentTrajectoryMetadatum]): @classmethod def encode_json(cls, o: "Any"): if isinstance(o, ValidSegmentPoint): @@ -81,7 +74,7 @@ def encode_json(cls, o: "Any"): "next": None if o.next is None else tuple(o.next.detection_id), "prev": None if o.prev is None else tuple(o.prev.detection_id), } - if isinstance(o, (RoadPolygonInfo, RoadPolygonInfo_)): + if isinstance(o, RoadPolygonInfo): return { "id": o.id, "polygon": str(o.polygon), @@ -99,37 +92,3 @@ def encode_json(cls, o: "Any"): "id": o.id, "polygon": str(o.polygon), } - - -def construct_trajectory(source: "Payload"): - obj_3d_trajectories: "dict[int, list[Tuple[trajectory_3d, DetectionInfo, int]]]" = {} - trajectories = StrongSORT.get(source) - assert trajectories is not None - - detection_infos = DetectionEstimation.get(source) - assert detection_infos is not None - - detection_info_map: "dict[DetectionId, DetectionInfo]" = {} - for d in detection_infos: - for dd in d: - detection_id = dd.detection_id - assert detection_id not in detection_info_map - detection_info_map[detection_id] = dd - - for frame_idx, frame in enumerate(trajectories): - for obj_id, obj_trajectory in frame.items(): - if obj_id not in obj_3d_trajectories: - obj_3d_trajectories[obj_id] = [] - - detection_id = obj_trajectory.detection_id - assert detection_id in detection_info_map, (detection_id, [*detection_info_map.keys()]) - detection_info = detection_info_map[detection_id] - obj_3d_trajectories[obj_id].append( - ( - trajectory_3d(detection_info.car_loc3d, detection_info.timestamp), - detection_info, - frame_idx, - ) - ) - - return obj_3d_trajectories diff --git a/spatialyze/video_processor/stages/segment_trajectory/construct_segment_trajectory.py b/spatialyze/video_processor/stages/segment_trajectory/construct_segment_trajectory.py deleted file mode 100644 index ea64a20..0000000 --- a/spatialyze/video_processor/stages/segment_trajectory/construct_segment_trajectory.py +++ /dev/null @@ -1,446 +0,0 @@ -import datetime -import math -from dataclasses import dataclass -from typing import List, NamedTuple, Tuple, Union - -import numpy as np -import numpy.typing as npt -import postgis -import psycopg2 -import psycopg2.sql -import shapely.geometry -from plpygis import Geometry -from shapely.geometry import Point -from shapely.ops import nearest_points - -from spatialyze.database import database - -from ...camera_config import CameraConfig -from ...payload import Payload -from ...types import DetectionId, Float3 -from ..detection_estimation.detection_estimation import DetectionInfo, trajectory_3d -from ..detection_estimation.segment_mapping import RoadPolygonInfo -from ..detection_estimation.utils import get_segment_line, project_point_onto_linestring - -test_segment_query = """ -SELECT - segmentpolygon.elementid, - segmentpolygon.elementpolygon, - segment.segmentline, - segmentpolygon.segmenttypes, - segment.heading -FROM segmentpolygon - LEFT OUTER JOIN segment - ON segmentpolygon.elementid = segment.elementid -WHERE segmentpolygon.elementid = '{segment_id}'; -""" - - -segment_closest_query = """ -WITH -AvailablePolygon AS ( - SELECT * - FROM SegmentPolygon - WHERE location = {location} -), -min_distance AS ( -SELECT - MIN(ST_Distance(AvailablePolygon.elementpolygon, {point}::geometry)) distance -FROM AvailablePolygon - LEFT OUTER JOIN segment - ON AvailablePolygon.elementid = segment.elementid -WHERE ST_Distance(AvailablePolygon.elementpolygon, {point}::geometry) > 0 - {heading_filter} -) -SELECT - AvailablePolygon.elementid, - AvailablePolygon.elementpolygon, - segment.segmentline, - AvailablePolygon.segmenttypes, - segment.heading -FROM min_distance, AvailablePolygon -LEFT OUTER JOIN segment - ON AvailablePolygon.elementid = segment.elementid -WHERE ST_Distance(AvailablePolygon.elementpolygon, {point}::geometry) = min_distance.distance; -""" - - -segment_contain_vector_query = """ -WITH -AvailablePolygon AS ( - SELECT * - FROM SegmentPolygon - WHERE location = {location} -), -min_contain AS ( -SELECT - MIN(ST_Area(AvailablePolygon.elementpolygon)) min_segment_area -FROM AvailablePolygon - LEFT OUTER JOIN segment - ON AvailablePolygon.elementid = segment.elementid -WHERE ST_Contains( - AvailablePolygon.elementpolygon, - {point}::geometry - ) - {heading_filter} -) -SELECT - AvailablePolygon.elementid, - AvailablePolygon.elementpolygon, - segment.segmentline, - AvailablePolygon.segmenttypes, - segment.heading -FROM min_contain, AvailablePolygon -LEFT OUTER JOIN segment - ON AvailablePolygon.elementid = segment.elementid -WHERE ST_Area(AvailablePolygon.elementpolygon) = min_contain.min_segment_area; -""" - - -HEADING_FILTER = """ -AND cos(radians( - facingRelative({point_heading}::real, - degrees(segment.heading)::real)) - ) > 0 -""" - - -SegmentQueryResult = Tuple[str, postgis.Polygon, postgis.LineString, List[str], float] - - -class PolygonAndId(NamedTuple): - id: "str" - polygon: "shapely.geometry.Polygon" - - -@dataclass -class ValidSegmentPoint: - detection_id: "DetectionId" - car_loc3d: "Float3" - timestamp: "datetime.datetime" - segment_type: "str" - segment_line: "postgis.LineString" - segment_heading: "float" - road_polygon_info: "RoadPolygonInfo | PolygonAndId" - obj_id: "int | None" = None - type: "str | None" = None - next: "SegmentPoint | None" = None - prev: "SegmentPoint | None" = None - - -@dataclass -class InvalidSegmentPoint: - detection_id: "DetectionId" - car_loc3d: "Float3" - timestamp: "datetime.datetime" - obj_id: "int | None" = None - type: "str | None" = None - next: "SegmentPoint | None" = None - prev: "SegmentPoint | None" = None - - -SegmentPoint = Union[ValidSegmentPoint, InvalidSegmentPoint] - - -def construct_new_road_segment_info(result: "list[SegmentQueryResult]"): - """Construct new road segment info based on query result. - - This Function constructs the new the road segment info - based on the query result that finds the correct road segment that contains - the calibrated trajectory point. - """ - kept_segment = (None, None) - road_segment_info = None - for road_segment in result: - segmentid, segmentpolygon, segmentline, segmenttype, segmentheading = road_segment - segmenttype = (segmenttype[0] if segmenttype is not None else None,) - segmentheading = math.degrees(segmentheading) if segmentheading is not None else None - segmentid = segmentid.split("_")[0] - assert segmentline - segmentline = Geometry(segmentline.to_ewkb()).shapely - if segmentid == kept_segment[0]: - assert road_segment_info is not None - road_segment_info.segment_lines.append(segmentline) - road_segment_info.segment_headings.append(segmentheading) - else: - if kept_segment[0] is not None: - if kept_segment[1] is not None: - continue - segmentpolygon = Geometry(segmentpolygon.to_ewkb()).shapely - road_segment_info = RoadPolygonInfo( - segmentid, - segmentpolygon, - [segmentline], - segmenttype, - [segmentheading], - None, - False, - None, - ) - kept_segment = (segmentid, segmenttype) - assert road_segment_info is not None - return road_segment_info - - -def find_middle_segment( - current_segment: "ValidSegmentPoint", next_segment: "ValidSegmentPoint", payload: "Payload" -): - current_time = current_segment.timestamp - next_time = next_segment.timestamp - - current_segment_polygon = current_segment.road_polygon_info.polygon - next_segment_polygon = next_segment.road_polygon_info.polygon - - assert not current_segment_polygon.intersects(next_segment_polygon), ( - current_segment.road_polygon_info.id, - next_segment.road_polygon_info.id, - ) - - # current_center_point = current_segment.road_segment_info.segment_polygon.centroid - # next_center_point = next_segment.road_segment_info.segment_polygon.centroid - # connected_center = LineString([current_center_point, next_center_point]) - # current_intersection = connected_center.intersection(current_segment_polygon).coords[0] - # next_intersection = connected_center.intersection(next_segment_polygon).coords[0] - p1, p2 = nearest_points(current_segment_polygon, next_segment_polygon) - - middle_idx = (current_segment.detection_id.frame_idx + next_segment.detection_id.frame_idx) // 2 - assert middle_idx != current_segment.detection_id.frame_idx - assert middle_idx != next_segment.detection_id.frame_idx - - middle_camera_config = payload.video._camera_configs[middle_idx] - assert middle_camera_config is not None - - middle_time = middle_camera_config.timestamp - - loc1: "npt.NDArray[np.float64]" = np.array([p1.x, p1.y]) - assert loc1.dtype == np.dtype(np.float64) - loc2: "npt.NDArray[np.float64]" = np.array([p2.x, p2.y]) - assert loc2.dtype == np.dtype(np.float64) - locdiff = loc2 - loc1 - - progress = (middle_time - current_time) / (next_time - current_time) - intersection_center: "list[float]" = (loc1 + locdiff * progress).tolist() - assert isinstance(intersection_center, list) - assert len(intersection_center) == 2 - - locations = set(f.location for f in payload.video._camera_configs) - assert len(locations) == 1, locations - location = [*locations][0] - - assert not current_segment_polygon.contains(Point(intersection_center)) - contain_query = psycopg2.sql.SQL(segment_contain_vector_query).format( - point=psycopg2.sql.Literal(postgis.point.Point(intersection_center)), - heading_filter=psycopg2.sql.SQL("AND True"), - location=psycopg2.sql.Literal(location), - ) - result = database.execute(contain_query) - if not result: - closest_query = psycopg2.sql.SQL(segment_closest_query).format( - point=psycopg2.sql.Literal(postgis.point.Point(intersection_center)), - heading_filter=psycopg2.sql.SQL("AND True"), - location=psycopg2.sql.Literal(location), - ) - result = database.execute(closest_query) - # TODO: function update_current_road_segment_info does not exist - # new_road_segment_info = update_current_road_segment_info(result) - new_road_segment_info = construct_new_road_segment_info(result) - - new_segment_line, new_heading = get_segment_line(new_road_segment_info, intersection_center) - timestamp = current_time + (next_time - current_time) / 2 - middle_segment = ValidSegmentPoint( - DetectionId(middle_idx, None), - (*intersection_center, 0.0), - timestamp, - new_segment_line, - new_heading, - new_road_segment_info, - ) - return middle_segment - - -def binary_search_segment( - current_segment: "ValidSegmentPoint", - next_segment: "ValidSegmentPoint", - payload: "Payload", -) -> "list[ValidSegmentPoint]": - if 0 <= next_segment.detection_id.frame_idx - current_segment.detection_id.frame_idx <= 1: - # same detection or detections on consecutive frame - return [] - elif current_segment.road_polygon_info.id == next_segment.road_polygon_info.id: - # detections in between are in the same polygon - assert current_segment.road_polygon_info == current_segment.road_polygon_info - - if current_segment.segment_line == next_segment.segment_line: - # detections in between are in the same segment line - assert current_segment.segment_heading == next_segment.segment_heading - - loc1: "npt.NDArray[np.float64]" = np.array(current_segment.car_loc3d) - assert loc1.dtype == np.dtype(np.float64) - loc2: "npt.NDArray[np.float64]" = np.array(next_segment.car_loc3d) - assert loc2.dtype == np.dtype(np.float64) - - locdiff = loc2 - loc1 - timediff = next_segment.timestamp - current_segment.timestamp - - def interpolate_segment(args: "Tuple[int, CameraConfig]"): - index, frame = args - - progress = (frame.timestamp - current_segment.timestamp) / timediff - location = loc1 + locdiff * progress - assert location.shape == (3,) - assert location.dtype == np.dtype(np.float64) - - return ValidSegmentPoint( - DetectionId(index, None), - tuple(location), - frame.timestamp, - current_segment.segment_line, - current_segment.segment_heading, - current_segment.road_polygon_info, - ) - - st = current_segment.detection_id.frame_idx + 1 - ed = next_segment.detection_id.frame_idx - configs_with_index = [*enumerate(payload.video._camera_configs)] - return [*map(interpolate_segment, configs_with_index[st:ed])] - else: - # detections in between are in different segment line - # TODO: interpolate all the detections between - # TODO: need to know what are the segment lines in between - return [] - - elif current_segment.road_polygon_info.polygon.intersects( - next_segment.road_polygon_info.polygon - ): - # both polygons are next to each other - # TODO: remove because we still need to fill in approximate locations - # of frames in between - # TODO: interpolate between 2 consecutive segments - # TODO: need to know what are the segment lines in between for both polygon - return [] - else: - middle_segment = find_middle_segment(current_segment, next_segment, payload) - # import code; code.interact(local=vars()) - - before = binary_search_segment(current_segment, middle_segment, payload) - after = binary_search_segment(middle_segment, next_segment, payload) - - return before + [middle_segment] + after - - -def complete_segment_trajectory( - road_segment_trajectory: "list[ValidSegmentPoint]", payload: "Payload" -): - completed_segment_trajectory: "list[ValidSegmentPoint]" = [road_segment_trajectory[0]] - - for current_segment, next_segment in zip( - road_segment_trajectory[:-1], road_segment_trajectory[1:] - ): - completed_segment_trajectory.extend( - binary_search_segment(current_segment, next_segment, payload) - ) - completed_segment_trajectory.append(next_segment) - - return completed_segment_trajectory - - -def calibrate( - trajectory_3d: "list[trajectory_3d]", - detection_infos: "list[DetectionInfo]", - frame_indices: "list[int]", - payload: "Payload", -) -> "list[ValidSegmentPoint]": - """Calibrate the trajectory to the road segments. - - Given a trajectory and the corresponding detection infos, map the trajectory - to the correct road segments. - The returned value is a list of SegmentTrajectoryPoint. - """ - road_segment_trajectory: "list[ValidSegmentPoint]" = [] - for i in range(len(trajectory_3d)): - current_point3d, timestamp = trajectory_3d[i] - current_point = current_point3d[:2] - detection_info = detection_infos[i] - frame_idx = frame_indices[i] - current_road_segment_heading = detection_info.segment_heading - current_segment_line = detection_info.segment_line - current_road_segment_info = detection_info.road_polygon_info - if i != len(trajectory_3d) - 1: - next_point = trajectory_3d[i + 1][0][:2] - if current_road_segment_heading is not None: - current_point_heading = math.atan2( - next_point[1] - current_point[1], next_point[0] - current_point[0] - ) - current_point_heading = math.degrees(current_point_heading) - - relative_heading = ( - abs(current_road_segment_heading + 90 - current_point_heading) % 360 - ) - - if current_road_segment_heading is None or math.cos(math.radians(relative_heading)) > 0: - road_segment_trajectory.append( - ValidSegmentPoint( - detection_info.detection_id, - current_point3d, - timestamp, - current_segment_line, - current_road_segment_heading, - current_road_segment_info, - frame_idx, - ) - ) - continue - - locations = set(f.location for f in payload.video._camera_configs) - assert len(locations) == 1, locations - location = [*locations][0] - - ### project current_point to the segment line of the previous point - ### and then find the segment that contains the projected point - ### however this requires querying for road segment once for each point to be calibrated - if len(road_segment_trajectory) == 0: - heading_filter = psycopg2.sql.SQL(HEADING_FILTER).format( - point_heading=psycopg2.sql.Literal(current_point_heading - 90) - ) - query = psycopg2.sql.SQL(segment_closest_query).format( - point=psycopg2.sql.Literal(postgis.point.Point(current_point)), - heading_filter=heading_filter, - location=psycopg2.sql.Literal(location), - ) - else: - prev_calibrated_point = road_segment_trajectory[-1] - prev_segment_line = prev_calibrated_point.segment_line - prev_segment_heading = prev_calibrated_point.segment_heading - projection = project_point_onto_linestring(Point(current_point), prev_segment_line) - current_point3d = (projection.x, projection.y, 0.0) - heading_filter = psycopg2.sql.SQL(HEADING_FILTER).format( - point_heading=psycopg2.sql.Literal(prev_segment_heading - 90) - ) - query = psycopg2.sql.SQL(segment_contain_vector_query).format( - point=psycopg2.sql.Literal(postgis.point.Point((projection.x, projection.y))), - heading_filter=heading_filter, - location=psycopg2.sql.Literal(location), - ) - result = database.execute(query) - if len(result) == 0: - closest_query = psycopg2.sql.SQL(segment_closest_query).format( - point=psycopg2.sql.Literal(postgis.point.Point(current_point)), - heading_filter=heading_filter, - location=psycopg2.sql.Literal(location), - ) - result = database.execute(closest_query) - assert len(result) > 0 - new_road_segment_info = construct_new_road_segment_info(result) - new_segment_line, new_heading = get_segment_line(current_road_segment_info, current_point3d) - road_segment_trajectory.append( - ValidSegmentPoint( - detection_info.detection_id, - current_point3d, - timestamp, - new_segment_line, - new_heading, - new_road_segment_info, - frame_idx, - ) - ) - return complete_segment_trajectory(road_segment_trajectory, payload) diff --git a/spatialyze/video_processor/stages/segment_trajectory/from_tracking_3d.py b/spatialyze/video_processor/stages/segment_trajectory/from_tracking_3d.py index 2dd07df..5dbc98d 100644 --- a/spatialyze/video_processor/stages/segment_trajectory/from_tracking_3d.py +++ b/spatialyze/video_processor/stages/segment_trajectory/from_tracking_3d.py @@ -4,31 +4,28 @@ import numpy as np import postgis import psycopg2.sql -import shapely -import shapely.geometry +import shapely.geometry as sg import shapely.wkb import torch -from spatialyze.database import database - +from ....database import database from ...payload import Payload from ...types import DetectionId, Float2 from ..detection_3d import Detection3D from ..detection_3d import Metadatum as Detection3DMetadatum from ..tracking.tracking import Metadatum as TrackingMetadatum from ..tracking.tracking import Tracking -from . import SegmentTrajectory, SegmentTrajectoryMetadatum -from .construct_segment_trajectory import ( +from . import ( InvalidSegmentPoint, PolygonAndId, SegmentPoint, + SegmentTrajectory, + SegmentTrajectoryMetadatum, ValidSegmentPoint, ) USEFUL_TYPES = ["lane", "lanegroup", "intersection"] -printed = False - class FromTracking3D(SegmentTrajectory): def __init__(self): @@ -129,7 +126,7 @@ def _run(self, payload: "Payload"): assert did.obj_order == _oid polygon = shapely.wkb.loads(polygon.to_ewkb(), hex=True) - assert isinstance(polygon, shapely.geometry.Polygon) + assert isinstance(polygon, sg.Polygon) type_ = next((t for t in types if t in USEFUL_TYPES), types[-1]) @@ -183,7 +180,7 @@ def valid_segment_point( segmentline: "postgis.LineString", segmentheading: "float", polygonid: "str", - shapely_polygon: "shapely.geometry.Polygon", + shapely_polygon: "sg.Polygon", ): x, y, z = map(float, ((det[6:9] + det[9:12]) / 2).tolist()) return ValidSegmentPoint( diff --git a/tests/interface/utils/F/test_contained.py b/tests/interface/utils/F/test_contained.py index f36a760..d0e880f 100644 --- a/tests/interface/utils/F/test_contained.py +++ b/tests/interface/utils/F/test_contained.py @@ -4,19 +4,9 @@ @pytest.mark.parametrize("fn, sql", [ (contained(1, 2), "contained(1,2)"), + (contained(objects[0], 'intersection'), "contained(valueAtTimestamp(t0.trajCentroids,timestamp),'intersection')"), + (contained(objects[0]@camera.time, 'intersection'), "contained(valueAtTimestamp(t0.trajCentroids,timestamp),'intersection')"), + (contained(objects[0].traj@camera.time, 'intersection'), "contained(valueAtTimestamp(t0.trajCentroids,timestamp),'intersection')"), ]) def test_contained(fn, sql): assert gen(fn) == sql - - -@pytest.mark.parametrize("fn, msg", [ - (contained(1), - "contained is expecting 2 arguments, but received 1"), - (contained(1,2,3), - "contained is expecting 2 arguments, but received 3"), -]) -def test_exception(fn, msg): - with pytest.raises(Exception) as e_info: - gen(fn) - str(e_info.value) == msg -