diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000..5ee4642 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,32 @@ +--- +name: Bug report +about: Create a report to help us improve +title: '' +labels: bug +assignees: '' + +--- + +**Describe the bug** +A clear and concise description of what the bug is. + +**To Reproduce** +Steps to reproduce the behavior: +1. Go to '...' +2. Click on '....' +3. Scroll down to '....' +4. See error + +**Expected behavior** +A clear and concise description of what you expected to happen. + +**Screenshots** +If applicable, add screenshots to help explain your problem. + +**Environment information** + - OS: [e.g. Windows 11] + - Python Version: [e.g. Python3.8] + - Python Environment: [e.g. venv, conda, etc.] + +**Additional context** +Add any other context about the problem here. diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..91abb11 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,11 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "pip" # See documentation for possible values + directory: "/" # Location of package manifests + schedule: + interval: "weekly" diff --git a/.gitignore b/.gitignore index dfcfd56..0b7b761 100644 --- a/.gitignore +++ b/.gitignore @@ -348,3 +348,37 @@ MigrationBackup/ # Ionide (cross platform F# VS Code tools) working folder .ionide/ + +# HAMS +log.out +/dist +__pycache__/ +hdf5.dll +log.* +*.out +*.pyc +docs/_build/ +*.mp4 +*.csv +*.hog +*.avi +*.txt +*.pth +*.exe +*.dll +*.yml +*.yaml +*.toml +test_*/ +out/ +data/ +*.jpg +*.png +.ipynb_checkpoints/ + +Dockerfile +docker-compose.* +GPL +binaries +output/ +*output*/ \ No newline at end of file diff --git a/.gitkeep b/.gitkeep new file mode 100644 index 0000000..5b8ab55 --- /dev/null +++ b/.gitkeep @@ -0,0 +1,4 @@ +requirements.txt +docs/requirements.txt +docs/static/ +.github/ \ No newline at end of file diff --git a/CITATION.cff b/CITATION.cff new file mode 100644 index 0000000..2b23687 --- /dev/null +++ b/CITATION.cff @@ -0,0 +1,18 @@ +cff-version: 1.2.0 +message: "If you use this software, please cite it as below." +authors: +- family-names: "Nambi" + given-names: "Akshay" +- family-names: "Mehta" + given-names: "Ishit" +- family-names: "Ghosh" + given-names: "Anurag" +- family-names: "Lingam" + given-names: "Vijay" +- family-names: "Padmanabhan" + given-names: "Venkat" +title: "ALT: Towards Automating Driver License Testing using Smartphones" +version: 0.1.0 +doi: https://doi.org/10.1145/3356250.3360037 +date-released: 2017-12-18 #TODO +url: "https://github.com/microsoft/HAMS" \ No newline at end of file diff --git a/README.md b/README.md index 5cd7cec..5e4e7b7 100644 --- a/README.md +++ b/README.md @@ -1,14 +1,14 @@ -# Project +# HAMS ALT -> This repo has been populated by an initial template to help get you started. Please -> make sure to update the content to build a great experience for community-building. +![LICENSE](https://img.shields.io/github/license/microsoft/HAMS?style=for-the-badge) [![Dashboard](https://img.shields.io/website?down_message=Dashboard%20Offline&style=for-the-badge&up_color=green&up_message=Dashboard&url=https%3A%2F%2Fhams-dashboard.westus3.cloudapp.azure.com%2F)](https://hams-dashboard.westus3.cloudapp.azure.com) [![Documentation](https://img.shields.io/badge/docs-Documentation-blue?style=for-the-badge&logo=appveyor)](https://microsoft.github.io/HAMS) -As the maintainer of this project, please make a few updates: +This project contains the core components of the Automated License Testing(ALT) system from the HAMS group at Microsoft Research, India. -- Improving this README.MD file to provide a great experience -- Updating SUPPORT.MD with content about this project's support experience -- Understanding the security reporting process in SECURITY.MD -- Remove this section from the README +## Installation + +1. Run `pip install git+https://github.com/microsoft/HAMS` to install the latest version +2. Downlaod the binaries from the [HAMS Releases](https://github.com/microsoft/HAMS/releases) +3. Refer to additional requirements and instructions on each of the modules in the `Tutorials` section of the [documentation](https://microsoft.github.io/HAMS). ## Contributing diff --git a/SUPPORT.md b/SUPPORT.md index 291d4d4..5dbad34 100644 --- a/SUPPORT.md +++ b/SUPPORT.md @@ -1,25 +1,16 @@ -# TODO: The maintainer of this repo has not yet edited this file - -**REPO OWNER**: Do you want Customer Service & Support (CSS) support for this product/project? - -- **No CSS support:** Fill out this template with information about how to file issues and get help. -- **Yes CSS support:** Fill out an intake form at [aka.ms/onboardsupport](https://aka.ms/onboardsupport). CSS will work with/help you to determine next steps. -- **Not sure?** Fill out an intake as though the answer were "Yes". CSS will help you decide. - -*Then remove this first heading from this SUPPORT.MD file before publishing your repo.* - # Support +Please use the instructions below to file issues related to this repository. + ## How to file issues and get help -This project uses GitHub Issues to track bugs and feature requests. Please search the existing -issues before filing new issues to avoid duplicates. For new issues, file your bug or -feature request as a new Issue. +This project uses GitHub Issues to track bugs. Please search the existing +issues before filing new issues to avoid duplicates. For new issues, file your bug as a new Issue following the standard templates. + +For help and questions about using this project, please email [Akshay Nambi](https://www.microsoft.com/en-us/research/people/akshayn/) at: [akshayn@microsoft.com](mailto:akshayn@microsoft.com) -For help and questions about using this project, please **REPO MAINTAINER: INSERT INSTRUCTIONS HERE -FOR HOW TO ENGAGE REPO OWNERS OR COMMUNITY FOR HELP. COULD BE A STACK OVERFLOW TAG OR OTHER -CHANNEL. WHERE WILL YOU HELP PEOPLE?**. +> Note: This project is not under active development, so no new feature requests will be added. Furthermore, please expect delay in response because of the same. ## Microsoft Support Policy -Support for this **PROJECT or PRODUCT** is limited to the resources listed above. +Support for this **PROJECT** is limited to the resources listed above. diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 0000000..d4bb2cb --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,20 @@ +# Minimal makefile for Sphinx documentation +# + +# You can set these variables from the command line, and also +# from the environment for the first two. +SPHINXOPTS ?= +SPHINXBUILD ?= sphinx-build +SOURCEDIR = . +BUILDDIR = _build + +# Put it first so that "make" without argument is like "make help". +help: + @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) + +.PHONY: help Makefile + +# Catch-all target: route all unknown targets to Sphinx using the new +# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). +%: Makefile + @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/about.md b/docs/about.md new file mode 100644 index 0000000..5f8bb9d --- /dev/null +++ b/docs/about.md @@ -0,0 +1,23 @@ +# About ALT + +Inadequate driver skills and apathy towards/lack of awareness of safe driving practices are key contributing factors for the lack of road safety. The problem is exacerbated by the fact that the license issuing system is broken in India, with an estimated 59% of licenses issued without a test, making it a significant societal concern. The challenges arise from capacity and cost constraints, and corruption that plagues the driver testing process. While there have been efforts aimed at creating instrumented tracks to automate the license test, these have been stymied by the high cost of the infrastructure (e.g., pole-mounted high-resolution cameras looking down on the tracks) and poor test coverage (e.g., inability to monitor the driver inside the vehicle). + +HAMS-based testing offers a compelling alternative. It is a low-cost and affordable system based on a windshield-mounted smartphone, though for reasons of scalability (i.e., handling a large volume of tests), we can offload computation to an onsite server or to the cloud. The view inside the vehicle also helps expand the test coverage. For instance, the test can verify that the driver taking the test is the same as the one who had registered for it (essential for protecting against impersonation), verify that the driver is wearing their seat belt (an essential safety precaution), and check whether the driver scans their mirrors before effecting a maneuver such as a lane change (an example of multimodal sensing, with inertial sensing and camera-based monitoring being employed in tandem). + +HAMS-based testing allows the entire testing process to be performed without any human intervention. A test report, together with video evidence (to substantiate the test result in case of a dispute), is produced in an automated manner within minutes of the completion of the test. This manner of testing, with the test taken by the driver alone in the vehicle (i.e., no test inspector) has proved to be a boon in the context of the physical distancing norms arising from the COVID-19 pandemic. + +## Deployments + +To roll out HAMS-based driver testing, we first partnered with the Government of Uttarakhand and the Institute of Driving and Traffic Research (IDTR), run by Maruti-Suzuki. Testing is conducted on a track and includes a range of parameters including verification of driver identity, checking of the seat belt, fine-grained trajectory tracking during maneuvers such as negotiating a roundabout and performing parallel parking, and checking on mirror scanning during lane changing. + +**HAMS-based driver license testing @ Dehradun, Uttarakhand:** [HAMS-based license testing went live at Dehradun Regional Transport Office (RTO)](https://news.microsoft.com/en-in/features/microsoft-ai-automates-drivers-license-test-india/), the capital of Uttarakhand state in July 2019. Till date, 10000+ automated tests (as of 15 Feb 2021) have been conducted, with an accuracy of 98%. The objectivity and transparency of the automated testing process has won the praise of not just the RTO staff but also the majority of the candidates, including many that failed the test. The thoroughness of HAMS-based testing is underscored by the fact that now the passing rate is only 54% compared to over 90% with the prior manual testing. + +**Scaling HAMS deployments across India:** The success in Dehradun has spurred interest in HAMS-based automated testing across India and also overseas. RFPs issued by several states have called for capabilities such as continuous driver identification, gaze tracking, and mirror scan monitoring, that were not available before HAMS. HAMS-based testing has been rolled out in IDTR Aurangabad, Bihar, and is in process of being implemented at multiple RTOs across the country. + +````{eval-rst} +.. youtube:: zFuIP5hI4yU +```` + +````{eval-rst} +.. youtube:: XtgpWXM5Hfg +```` \ No newline at end of file diff --git a/docs/conf.py b/docs/conf.py new file mode 100644 index 0000000..6bedbda --- /dev/null +++ b/docs/conf.py @@ -0,0 +1,117 @@ +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +import os +import sys +# sys.path.insert(0, os.path.abspath('~/Microsoft/OLA-Alt/HAMSApp/')) +# sys.path.insert(1, os.path.abspath('../')) +# sys.path.append(os.path.abspath('/Users/balli/Microsoft/')) +# sys.path.append(os.path.abspath('/Users/balli/Microsoft/Ola-ALT/')) +# sys.path.append(os.path.abspath('/Users/balli/Microsoft/Ola-ALT/HAMSApp/')) +# sys.path.append(os.path.abspath('/Users/balli/Microsoft/Ola-ALT/HAMSApp/ALT/')) +sys.path.append('../') +# sys.path.insert(2, os.path.abspath('../../')) +# sys.path.insert(3, os.path.abspath('../../')) + + +# -- Project information ----------------------------------------------------- + +project = 'HAMS' +copyright = '2023, Microsoft Research' +author = 'Anurag Ghosh, Harsh Vijay, Vaibhav Balloli, Jonathan Samuel, Akshay Nambi' + +# The full version, including alpha/beta/rc tags +release = '0.1' + + +# -- General configuration --------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + "sphinx.ext.autodoc", + "sphinx.ext.intersphinx", + "sphinx.ext.viewcode", + "sphinx.ext.doctest", + "sphinx.ext.autosummary", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.mathjax", + "sphinx.ext.napoleon", + "sphinx.ext.autosectionlabel", + "sphinxemoji.sphinxemoji", + "breathe", + "sphinx_markdown_builder", + "sphinx_copybutton", + "jupyter_sphinx", + # "myst_nb", + "myst_parser", + "sphinx_proof", + "sphinx_design", + "sphinxcontrib.video", + "sphinx_togglebutton", + "sphinx_tabs.tabs", + "nbsphinx", + "sphinxcontrib.youtube", +] +autodoc_typehints = "description" +autodoc_class_signature = "separated" +autosummary_generate = True +autodoc_default_options = { + "members": True, + "inherited-members": True, + "show-inheritance": False, +} +autodoc_inherit_docstrings = True +myst_enable_extensions = ["colon_fence"] + +nb_execution_mode = "off" +nbsphinx_allow_errors = True +nbsphinx_execute = "never" + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] + +html_theme_options = { + "repository_url": "https://github.com/microsoft/HAMS", + "use_repository_button": True, + "use_download_button": True, +} + +html_title = "HAMS Docs" +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +# html_theme = "furo" +html_theme = "sphinx_book_theme" + +# removes the .txt suffix +html_sourcelink_suffix = "" + + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["static"] +# source_suffix = ['.rst', '.md'] + +autodoc_mock_imports = ['alabaster', 'amqp', 'anacondaclient', 'anacondaproject', 'anyjson', 'apscheduler', 'asncrypto', 'astroid', 'astropy', 'atomicwrites', 'attrs', 'autobahn', 'automat', 'babel', 'backcall', 'backportsos', 'backportsshutilgetterminalsize', 'bcrypt', 'beautifulsoup', 'billiard', 'bitarray', 'bkcharts', 'bleach', 'bokeh', 'boto', 'bottleneck', 'celery', 'celluloid', 'certifi', 'cffi', 'chardet', 'click', 'cloudpickle', 'clyent', 'cognitiveface', 'colorama', 'comtypes', 'constantly', 'contextlib', 'cryptography', 'cycler', 'cython', 'cytoolz', 'dask', 'decorator', 'defusedxml', 'distributed', 'dlib', 'dnspython', 'docutils', 'entrypoints', 'etxmlfile', 'eventlet', 'fastcache', 'filelock', 'filterpy', 'flask', 'flaskbcrypt', 'flasklogin', 'flaskmarshmallow', 'flasksession', 'flasksqlacodegen', 'flasksqlalchemy', 'flaskwkhtmltopdf', 'gevent', 'greenlet', 'hpy', 'haversine', 'heapdict', 'hexdump', 'hkdf', 'htmllib', 'humanize', 'hyperlink', 'idna', 'imageio', 'imagesize', 'importlibmetadata', 'incremental', 'inflect', 'ipykernel', 'ipython', 'ipythongenutils', 'ipywidgets', 'isort', 'itsdangerous', 'jdcal', 'jedi', 'jinja', 'jsonschema', 'jupyter', 'jupyterclient', 'jupyterconsole', 'jupytercore', 'jupyterlab', 'jupyterlabserver', 'keyring', 'kiwisolver', 'kombu', 'laika', 'lazyobjectproxy', 'libusb', 'llvmlite', 'locket', 'lxml', 'markupsafe', 'marshmallow', 'marshmallowsqlalchemy', 'matplotlib', 'mccabe', 'menuinst', 'mistune', 'mklfft', 'mklrandom', 'monotonic', 'moreitertools', 'mpmath', 'msgpack', 'multipledispatch', 'mysqlconnector', 'nbconvert', 'nbformat', 'networkx', 'nltk', 'nose', 'notebook', 'numba', 'numexpr', 'numpy', 'numpydoc', 'olefile', 'opencvcontribpython', 'openpyxl', 'packaging', 'pandas', 'pandocfilters', 'parso', 'partd', 'pathpy', 'pathlib', 'patsy', 'pdfkit', 'pep', 'pickleshare', 'pillow', 'pip', 'pluggy', 'ply', 'prometheusclient', 'prompttoolkit', 'psutil', 'py', 'pycodestyle', 'pycosat', 'pycparser', 'pycrypto', 'pycurl', 'pyflakes', 'pygments', 'pyhamcrest', 'pylint', 'pymysql', 'pynacl', 'pyodbc', 'pyopenssl', 'pyparsing', 'pyquaternion', 'pyreadline', 'pyrsistent', 'pysocks', 'pytest', 'pytestarraydiff', 'pytestastropy', 'pytestdoctestplus', 'pytestopenfiles', 'pytestremotedata', 'pythondateutil', 'pytz', 'pywavelets', 'pywin', 'pywinctypes', 'pywinpty', 'pyyaml', 'pyzmq', 'qtawesome', 'qtconsole', 'qtpy', 'redis', 'requests', 'rope', 'ruamelyaml', 'scikitimage', 'scikitlearn', 'scipy', 'seaborn', 'sendtrash', 'setuptools', 'shapelypost', 'simplegeneric', 'singledispatch', 'six', 'snowballstemmer', 'sortedcollections', 'sortedcontainers', 'soupsieve', 'sphinx', 'sphinxcontribwebsupport', 'spyder', 'spyderkernels', 'sqlalchemy', 'statsmodels', 'sympy', 'tables', 'tblib', 'terminado', 'testpath', 'toolz', 'tornado', 'tqdm', 'traitlets', 'txaio', 'typedast', 'tzlocal', 'unicodecsv', 'urllib', 'vine', 'wcwidth', 'webencodings', 'werkzeug', 'wfastcgi', 'wheel', 'widgetsnbextension', 'wincertstore', 'wininetpton', 'winunicodeconsole', 'wrapt', 'xlrd', 'xlsxwriter', 'xlwings', 'xlwt', 'zict', 'zipp', 'zopeinterface', 'mysql'] + +autodoc_mock_imports += ['ffmpeg', 'flask_sqlalchemy', 'flask_bcrypt', 'flask_cors', 'flask_session', 'sqlalchemy_utils', 'cv2', 'PIL', 'torch', 'torchvision', 'reporter', 'cognitive_face', 'shapely', 'sklearn', 'moviepy', 'natsort' ,'decord', 'hydra', 'typer', 'omegaconf', 'mapping_cli'] \ No newline at end of file diff --git a/docs/dashboard.md b/docs/dashboard.md new file mode 100644 index 0000000..8e35981 --- /dev/null +++ b/docs/dashboard.md @@ -0,0 +1,7 @@ +# Dashboard + +We have a live dashboard that shows the current deployment of the HAMS ALT systems across India and an aggregate monthly statistics of the tests occuring here. +You can visit the website from [aka.ms/hams-dashboard](https://aka.ms/hams-dashboard) + + +![HAMS ALT Dashboard](static/hams_dashboard.jpeg) \ No newline at end of file diff --git a/docs/index.md b/docs/index.md new file mode 100644 index 0000000..a111ad7 --- /dev/null +++ b/docs/index.md @@ -0,0 +1,43 @@ +# Welcome to HAMS's documentation! + +![LICENSE](https://img.shields.io/github/license/microsoft/HAMS?style=for-the-badge) [![Dashboard](https://img.shields.io/website?down_message=Dashboard%20Offline&style=for-the-badge&up_color=green&up_message=Dashboard&url=https%3A%2F%2Fhams-dashboard.westus3.cloudapp.azure.com%2F)](https://hams-dashboard.westus3.cloudapp.azure.com) [![Documentation](https://img.shields.io/badge/docs-Documentation-blue?style=for-the-badge&logo=appveyor)](https://microsoft.github.io/HAMS) + +Inadequate driver skills and apathy towards/lack of awareness of safe driving practices are key contributing factors for the lack of road safety. The problem is exacerbated by the fact that the license issuing system is broken in India, with an estimated 59% of licenses issued without a test, making it a significant societal concern. The challenges arise from capacity and cost constraints, and corruption that plagues the driver testing process. While there have been efforts aimed at creating instrumented tracks to automate the license test, these have been stymied by the high cost of the infrastructure (e.g., pole-mounted high-resolution cameras looking down on the tracks) and poor test coverage (e.g., inability to monitor the driver inside the vehicle). + +HAMS-based testing offers a compelling alternative. It is a low-cost and affordable system based on a windshield-mounted smartphone, though for reasons of scalability (i.e., handling a large volume of tests), we can offload computation to an onsite server or to the cloud. The view inside the vehicle also helps expand the test coverage. For instance, the test can verify that the driver taking the test is the same as the one who had registered for it (essential for protecting against impersonation), verify that the driver is wearing their seat belt (an essential safety precaution), and check whether the driver scans their mirrors before effecting a maneuver such as a lane change (an example of multimodal sensing, with inertial sensing and camera-based monitoring being employed in tandem). + +To cite this repository, please use the following: + +```bibtex +@inproceedings{nambi2019alt, + title={ALT: towards automating driver license testing using smartphones}, + author={Nambi, Akshay Uttama and Mehta, Ishit and Ghosh, Anurag and Lingam, Vijay and Padmanabhan, Venkata N}, + booktitle={Proceedings of the 17th Conference on Embedded Networked Sensor Systems}, + pages={29--42}, + year={2019} + } +``` + +To use these code, follow the tutorials [here](tutorials/index.md). To know more about our project, you can explore the documentation using the sidebar or from down below: + +````{eval-rst} +.. toctree:: + :maxdepth: 1 + :caption: More about HAMS + + about + dashboard + +.. toctree:: + :maxdepth: 1 + :caption: USAGE + + tutorials/install + tutorials/index + +.. toctree:: + :maxdepth: 2 + :caption: REFERENCE + + modules +```` diff --git a/docs/make.bat b/docs/make.bat new file mode 100644 index 0000000..954237b --- /dev/null +++ b/docs/make.bat @@ -0,0 +1,35 @@ +@ECHO OFF + +pushd %~dp0 + +REM Command file for Sphinx documentation + +if "%SPHINXBUILD%" == "" ( + set SPHINXBUILD=sphinx-build +) +set SOURCEDIR=. +set BUILDDIR=_build + +%SPHINXBUILD% >NUL 2>NUL +if errorlevel 9009 ( + echo. + echo.The 'sphinx-build' command was not found. Make sure you have Sphinx + echo.installed, then set the SPHINXBUILD environment variable to point + echo.to the full path of the 'sphinx-build' executable. Alternatively you + echo.may add the Sphinx directory to PATH. + echo. + echo.If you don't have Sphinx installed, grab it from + echo.https://www.sphinx-doc.org/ + exit /b 1 +) + +if "%1" == "" goto help + +%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% +goto end + +:help +%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% + +:end +popd diff --git a/docs/mapping_cli.rst b/docs/mapping_cli.rst new file mode 100644 index 0000000..d846b34 --- /dev/null +++ b/docs/mapping_cli.rst @@ -0,0 +1,21 @@ +mapping\_cli package +==================== + +Submodules +---------- + +mapping\_cli.main module +------------------------ + +.. automodule:: mapping_cli.main + :members: + :undoc-members: + :show-inheritance: + +Module contents +--------------- + +.. automodule:: mapping_cli + :members: + :undoc-members: + :show-inheritance: diff --git a/docs/mapping_cli/mapping_cli.calibration.rst b/docs/mapping_cli/mapping_cli.calibration.rst new file mode 100644 index 0000000..ce8be71 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.calibration.rst @@ -0,0 +1,6 @@ +mapping\_cli.calibration +======================== + +.. currentmodule:: mapping_cli + +.. autodata:: calibration \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.cli.rst b/docs/mapping_cli/mapping_cli.cli.rst new file mode 100644 index 0000000..206c03a --- /dev/null +++ b/docs/mapping_cli/mapping_cli.cli.rst @@ -0,0 +1,6 @@ +mapping\_cli.cli +================ + +.. currentmodule:: mapping_cli + +.. autodata:: cli \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.config.config.rst b/docs/mapping_cli/mapping_cli.config.config.rst new file mode 100644 index 0000000..29b1781 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.config.config.rst @@ -0,0 +1,6 @@ +mapping\_cli.config.config +========================== + +.. currentmodule:: mapping_cli.config + +.. autodata:: config \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.config.rst b/docs/mapping_cli/mapping_cli.config.rst new file mode 100644 index 0000000..51bddb7 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.config.rst @@ -0,0 +1,23 @@ +mapping\_cli.config +=================== + +.. automodule:: mapping_cli.config + + + + + + + + + + + + + + + + + + + diff --git a/docs/mapping_cli/mapping_cli.halts.rst b/docs/mapping_cli/mapping_cli.halts.rst new file mode 100644 index 0000000..e4c8e47 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.halts.rst @@ -0,0 +1,6 @@ +mapping\_cli.halts +================== + +.. currentmodule:: mapping_cli + +.. autodata:: halts \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.locator.rst b/docs/mapping_cli/mapping_cli.locator.rst new file mode 100644 index 0000000..84de09e --- /dev/null +++ b/docs/mapping_cli/mapping_cli.locator.rst @@ -0,0 +1,6 @@ +mapping\_cli.locator +==================== + +.. currentmodule:: mapping_cli + +.. autodata:: locator \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.main.rst b/docs/mapping_cli/mapping_cli.main.rst new file mode 100644 index 0000000..24d06fb --- /dev/null +++ b/docs/mapping_cli/mapping_cli.main.rst @@ -0,0 +1,6 @@ +mapping\_cli.main +================= + +.. currentmodule:: mapping_cli + +.. autodata:: main \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.face_verification.rst b/docs/mapping_cli/mapping_cli.maneuvers.face_verification.rst new file mode 100644 index 0000000..724b69a --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.face_verification.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.face\_verification +========================================= + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: face_verification \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.forward_eight.rst b/docs/mapping_cli/mapping_cli.maneuvers.forward_eight.rst new file mode 100644 index 0000000..34e7fce --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.forward_eight.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.forward\_eight +===================================== + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: forward_eight \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.gaze.rst b/docs/mapping_cli/mapping_cli.maneuvers.gaze.rst new file mode 100644 index 0000000..9dcf58d --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.gaze.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.gaze +=========================== + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: gaze \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.incline.rst b/docs/mapping_cli/mapping_cli.maneuvers.incline.rst new file mode 100644 index 0000000..6ad3261 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.incline.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.incline +============================== + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: incline \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.maneuver.rst b/docs/mapping_cli/mapping_cli.maneuvers.maneuver.rst new file mode 100644 index 0000000..e471cab --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.maneuver.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.maneuver +=============================== + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: maneuver \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.marker_sequence.rst b/docs/mapping_cli/mapping_cli.maneuvers.marker_sequence.rst new file mode 100644 index 0000000..f59ac73 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.marker_sequence.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.marker\_sequence +======================================= + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: marker_sequence \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.pedestrian.rst b/docs/mapping_cli/mapping_cli.maneuvers.pedestrian.rst new file mode 100644 index 0000000..cefeed3 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.pedestrian.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.pedestrian +================================= + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: pedestrian \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.perp.rst b/docs/mapping_cli/mapping_cli.maneuvers.perp.rst new file mode 100644 index 0000000..9e846a3 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.perp.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.perp +=========================== + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: perp \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.rpp.rst b/docs/mapping_cli/mapping_cli.maneuvers.rpp.rst new file mode 100644 index 0000000..bbfe7dc --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.rpp.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.rpp +========================== + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: rpp \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.rst b/docs/mapping_cli/mapping_cli.maneuvers.rst new file mode 100644 index 0000000..d5ad989 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.rst @@ -0,0 +1,23 @@ +mapping\_cli.maneuvers +====================== + +.. automodule:: mapping_cli.maneuvers + + + + + + + + + + + + + + + + + + + diff --git a/docs/mapping_cli/mapping_cli.maneuvers.seat_belt.rst b/docs/mapping_cli/mapping_cli.maneuvers.seat_belt.rst new file mode 100644 index 0000000..a07a4b8 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.seat_belt.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.seat\_belt +================================= + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: seat_belt \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.maneuvers.traffic.rst b/docs/mapping_cli/mapping_cli.maneuvers.traffic.rst new file mode 100644 index 0000000..b1e0ebb --- /dev/null +++ b/docs/mapping_cli/mapping_cli.maneuvers.traffic.rst @@ -0,0 +1,6 @@ +mapping\_cli.maneuvers.traffic +============================== + +.. currentmodule:: mapping_cli.maneuvers + +.. autodata:: traffic \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.mapper.rst b/docs/mapping_cli/mapping_cli.mapper.rst new file mode 100644 index 0000000..17fdc6f --- /dev/null +++ b/docs/mapping_cli/mapping_cli.mapper.rst @@ -0,0 +1,6 @@ +mapping\_cli.mapper +=================== + +.. currentmodule:: mapping_cli + +.. autodata:: mapper \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.rst b/docs/mapping_cli/mapping_cli.rst new file mode 100644 index 0000000..d3f9854 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.rst @@ -0,0 +1,23 @@ +mapping\_cli +============ + +.. automodule:: mapping_cli + + + + + + + + + + + + + + + + + + + diff --git a/docs/mapping_cli/mapping_cli.segment.rst b/docs/mapping_cli/mapping_cli.segment.rst new file mode 100644 index 0000000..1edc7ec --- /dev/null +++ b/docs/mapping_cli/mapping_cli.segment.rst @@ -0,0 +1,6 @@ +mapping\_cli.segment +==================== + +.. currentmodule:: mapping_cli + +.. autodata:: segment \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.utils.rst b/docs/mapping_cli/mapping_cli.utils.rst new file mode 100644 index 0000000..f117e75 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.utils.rst @@ -0,0 +1,6 @@ +mapping\_cli.utils +================== + +.. currentmodule:: mapping_cli + +.. autodata:: utils \ No newline at end of file diff --git a/docs/mapping_cli/mapping_cli.validation.rst b/docs/mapping_cli/mapping_cli.validation.rst new file mode 100644 index 0000000..431f9e3 --- /dev/null +++ b/docs/mapping_cli/mapping_cli.validation.rst @@ -0,0 +1,6 @@ +mapping\_cli.validation +======================= + +.. currentmodule:: mapping_cli + +.. autodata:: validation \ No newline at end of file diff --git a/docs/modules.rst b/docs/modules.rst new file mode 100644 index 0000000..b14f712 --- /dev/null +++ b/docs/modules.rst @@ -0,0 +1,8 @@ +HAMS API Reference +====================== + +.. autosummary:: + :toctree: mapping_cli + :recursive: + + mapping_cli diff --git a/docs/requirements.txt b/docs/requirements.txt new file mode 100644 index 0000000..f91e79e --- /dev/null +++ b/docs/requirements.txt @@ -0,0 +1,21 @@ +breathe==4.35.0 +furo +git+https://github.com/sphinx-contrib/video +ipywidgets +jupyter-sphinx +myst-nb +myst-parser +nbsphinx +pandoc +sphinx +sphinx-autobuild +sphinx-book-theme +sphinx-copybutton +sphinx-design +sphinx-proof +sphinx-tabs +sphinx-togglebutton +sphinx_markdown_builder +sphinxemoji +sphinxcontrib-youtube +pandoc diff --git a/docs/static/hams_dashboard.jpeg b/docs/static/hams_dashboard.jpeg new file mode 100644 index 0000000..b83fb39 Binary files /dev/null and b/docs/static/hams_dashboard.jpeg differ diff --git a/docs/tutorials/camera_calibration.md b/docs/tutorials/camera_calibration.md new file mode 100644 index 0000000..dc9d328 --- /dev/null +++ b/docs/tutorials/camera_calibration.md @@ -0,0 +1,18 @@ +# Camera Calibration + +Camera calibration is an important first step in using most of `HAMS` modules. To do so, first print the [calibration board present in this file](https://github.com/abakisita/camera_calibration/blob/master/aruco_marker_board.pdf). + +Click a couple of images of the printed board and place them in a folder named `calibration_images`. Additionally, **measure** the `length` of the markers and the `separation` between two adjacent markers `in cm`. + +To run the camera calibration module from the command line, + +```bash +mapping-cli.exe generate-calib +``` + +```{button-link} ./camera_calibration_notebook.ipynb +:color: primary +:shadow: + +Running the camera calibration module on the collected images. +``` \ No newline at end of file diff --git a/docs/tutorials/camera_calibration_notebook.ipynb b/docs/tutorials/camera_calibration_notebook.ipynb new file mode 100644 index 0000000..ccf93cf --- /dev/null +++ b/docs/tutorials/camera_calibration_notebook.ipynb @@ -0,0 +1,74 @@ +{ + "cells": [ + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Running the HAMS Camera Calibration Module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "\n", + "from mapping_cli.calibration import camera_calibration" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "images_path = \"\"\n", + "phone_name = \"\"\n", + "marker_length = 3.5\n", + "marker_separation = 0.5\n", + "\n", + "output_folder = \"output\"" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "os.makedirs(output_folder, exist_ok=True)\n", + "camera_calibration(phone_name, images_path, marker_length, marker_separation, output_folder)" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The calibration file is now saved in the output folder under the name `calib_.yml`" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "hams", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python", + "version": "3.8.16 (default, Jan 17 2023, 22:25:28) [MSC v.1916 64 bit (AMD64)]" + }, + "orig_nbformat": 4, + "vscode": { + "interpreter": { + "hash": "4ecb028c9e4ec612609cae8571d5e3a76bf96bee660effba276681a0b0090bd9" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/docs/tutorials/face_verification.md b/docs/tutorials/face_verification.md new file mode 100644 index 0000000..deda108 --- /dev/null +++ b/docs/tutorials/face_verification.md @@ -0,0 +1,63 @@ +# Face Verification + +Face verification module is used to check if the test taker that is registered is the one driving the test vehicle. + +![Azure Portal Face API](../static/face.png) + +To run this, you'll need Microsoft's [Cognitive Face Python Library](https://pypi.org/project/cognitive-face/) and an API key which you can setup from [here](https://azure.microsoft.com/en-us/products/cognitive-services/face). On your Azure Portal, head on to the Face API resource and enter the `endpoint` in the `base_url` field below and copy the `KEY 1` to the `subscription_key` below(see the screenshot above for reference). + +
+ face_verify.yml(click to open/close) + + ```yaml + base_url: "https://southcentralus.api.cognitive.microsoft.com/face/v1.0" + subscription_key: "" + calib_frame_period: 100 + test_frame_period: 100 + recog_confidence_threshold: 0.75 + video_acceptance_threshold: 0.75 + ``` +
+ +
+ Explanation of the above configuration values(click to open) + + ```{list-table} + :header-rows: 1 + + * - Parameter + - Description + - Example Value + * - base_url + - Base URL for the Cognitive Face API to access + - "https://southcentralus.api.cognitive.microsoft.com/face/v1.0" + * - subscription_key + - API Key from Azure Face API + - KEY + * - calib_frame_period + - Number of seconds of the calibration to read(int) + - 100 + * - test_frame_period + - Number of frames to skip in between successive evaluations(int) + - 100 + * - recog_confidence_threshold + - Threshold for similarity to consider a match(float) + - 0.75 + * - video_acceptance_threshold + - Successful similarity rate to consider face verification a success(float) + - 0.72 + ``` +
+ +Now, run the following command: + +```bash +python main.py --face-verify --front-video front_video.mp4 --calib-video calib_video.mp4 --config face_verify.yml --output-path results/ +``` + +```{button-link} ./face_verification_notebook.ipynb +:color: primary +:shadow: + +Running the seatbelt code on driver-facing video. +``` \ No newline at end of file diff --git a/docs/tutorials/face_verification_notebook.ipynb b/docs/tutorials/face_verification_notebook.ipynb new file mode 100644 index 0000000..44f1d11 --- /dev/null +++ b/docs/tutorials/face_verification_notebook.ipynb @@ -0,0 +1,166 @@ +{ + "cells": [ + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Running HAMS Face Verification module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "from ipywidgets import FileUpload\n", + "from omegaconf import OmegaConf\n", + "\n", + "from mapping_cli.maneuvers.face_verification import FaceVerification\n", + "from mapping_cli.config.config import Config" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from ipywidgets import FileUpload\n", + "from IPython.display import display, Image\n", + "upload = FileUpload(accept='.mp4', multiple=False)\n", + "display(upload)\n" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Upload the calibration video to registered the driver taking the test" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open(\"calibration.mp4\", \"wb\") as f:\n", + " f.write(upload.value[0].content)" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Upload the video of the driver-facing camera to check if it's the registered driver that is taking the test" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open(\"front_video.mp4\", \"wb\") as f:\n", + " f.write(upload.value[0].content)" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Modify the config variables depending on the need. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "config = {\n", + " \n", + "}" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now, save the config to `face_verification.yaml`, create a directory to store the outputs named `output` and run the seabelt module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open('face_verification.yaml', 'w') as f:\n", + " OmegaConf.save(OmegaConf.create(config), f)\n", + "os.makedirs('output', exist_ok=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "face_verification = FaceVerification(inputs={\"fpath\": os.path.abspath('front_video.mp4'), \"calib_video\": os.path.abspath('calibration.mp4')}, config=Config('face_verification.yaml'), out_folder='output')\n", + "_, result, _ = face_verification.run()" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Final Test Results" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "print(f\"Same Driver: {result}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "hams", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.16 (default, Jan 17 2023, 22:25:28) [MSC v.1916 64 bit (AMD64)]" + }, + "orig_nbformat": 4, + "vscode": { + "interpreter": { + "hash": "4ecb028c9e4ec612609cae8571d5e3a76bf96bee660effba276681a0b0090bd9" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/docs/tutorials/gaze.md b/docs/tutorials/gaze.md new file mode 100644 index 0000000..369f435 --- /dev/null +++ b/docs/tutorials/gaze.md @@ -0,0 +1,58 @@ +# Gaze Detection + +Here, you'll first need to setup [OpenFace](https://github.com/TadasBaltrusaitis/OpenFace). In order to setup `OpenFace`, follow the instructions on [their wiki](https://github.com/TadasBaltrusaitis/OpenFace/wiki) depending on your Operating system. + + +
+ seatbelt.yml(click to open/close) + + ```yaml + face_landmark_exe_path: "" + centre_threshold: 10 + left_threshold: 10 + right_threshold: 10 + maneuver: full + ``` +
+ +In the above config file, copy the path to the `FaceLandmarkVidMulti` landmark executable/binary from the installation. + +````{margin} **Note** +```{note} +If `FaceLandmarkVidMulti` already exists in the PATH variable, you don't need to enter the entire path. Just `FaceLandmarkVidMulti`(linux) or `FaceLandmarkVidMulti.exe`(Windows) is sufficient. +``` +```` + +
+ Explanation of the above configuration values(click to open) + + ```{list-table} + :header-rows: 1 + + * - Parameter + - Description + - Example Value + * - face_landmark_exe_path + - Path to the `FaceLandmarkVidMulti` executable + - "D:\\OpenFace_2.2.0_win_x64\\FaceLandmarkVidMulti.exe" + * - centre_threshold + - Number of center looking detections(integer) + - 10 + * - left_threshold + - Number of minimum left gaze detections(int) + - 10 + * - right_threshold + - Number of minimum right gaze detections(int) + - 10 + * - maneuver + - Name of the maneuver for which the gaze detection is being run on + - Traffic + ``` +
+ +```{button-link} ./gaze_notebook.ipynb +:color: primary +:shadow: + +Running the gaze detection code on the driver-facing video. +``` \ No newline at end of file diff --git a/docs/tutorials/gaze_tutorial.ipynb b/docs/tutorials/gaze_tutorial.ipynb new file mode 100644 index 0000000..d6517ec --- /dev/null +++ b/docs/tutorials/gaze_tutorial.ipynb @@ -0,0 +1,191 @@ +{ + "cells": [ + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Running HAMS Gaze Detection module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "from ipywidgets import FileUpload\n", + "from omegaconf import OmegaConf\n", + "\n", + "from mapping_cli.maneuvers.gaze import Gaze\n", + "from mapping_cli.config.config import Config" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from ipywidgets import FileUpload\n", + "from IPython.display import display, Image\n", + "upload = FileUpload(accept='.mp4', multiple=False)\n", + "display(upload)\n" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Upload a driver-facing test video to detect the seatbelt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open(\"front_video.mp4\", \"wb\") as f:\n", + " f.write(upload.value[0].content)" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Upload a driver-facing calibration video to detect the seatbelt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from ipywidgets import FileUpload\n", + "from IPython.display import display, Image\n", + "upload = FileUpload(accept='.mp4', multiple=False)\n", + "display(upload)\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open(\"calib_video.mp4\", \"wb\") as f:\n", + " f.write(upload.value[0].content)" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Modify the config variables depending on the need. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "config = {\n", + " \"device\": \"cpu\",\n", + " \"skip_frames\": 25,\n", + " \"classifier_confidence_threshold\": 0.75,\n", + " \"detection_percentage\": 0.75,\n", + " \"model_path\": [\"models\", \"seatbelt_model.pth\"],\n", + "}" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now, save the config to `gaze.yaml`, create a directory to store the outputs named `output` and run the gaze detection module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open('gaze.yaml', 'w') as f:\n", + " OmegaConf.save(OmegaConf.create(config), f)\n", + "os.makedirs('output', exist_ok=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "gaze = Gaze(inputs={\"fpath\":os.path.abspath('front_video.mp4'), \"calib_video\":os.path.abspath('calib_video.mp4')}, inertial_data=None, config=Config('gaze.yaml'), out_folder='./output')\n", + "decision, stats = gaze.run()" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Vizualize the results" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "display(Video(filename='output/front_gaze.mp4'))" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Statistics" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "print(f\"Statistics: {stats}\")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "hams", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python", + "version": "3.8.16 (default, Jan 17 2023, 22:25:28) [MSC v.1916 64 bit (AMD64)]" + }, + "orig_nbformat": 4, + "vscode": { + "interpreter": { + "hash": "4ecb028c9e4ec612609cae8571d5e3a76bf96bee660effba276681a0b0090bd9" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/docs/tutorials/index.md b/docs/tutorials/index.md new file mode 100644 index 0000000..f804a7c --- /dev/null +++ b/docs/tutorials/index.md @@ -0,0 +1,61 @@ +# Tutorials + +::::{card-carousel} 1 + +:::{card} Camera Calibration +:link: ./camera_calibration +:link-type: doc +:text-align: center + +Running camera calibration +::: + +:::: + +::::{card-carousel} 2 + +:::{card} Map Building +:link: ./map_build +:link-type: doc +:text-align: center + +Running the Map building binary +::: + +:::{card} Trajectory Generation +:link-type: doc +:text-align: center +:link: ./trajectory_generation + +Running the face matching utility on the driver-facing video. +::: + +:::: + +::::{card-carousel} 3 + +:::{card} Segmentation +:link: ./segment +:link-type: doc +:text-align: center + +Segmenting the License Testing video based on the maneuvers. +::: + +:::{card} Face Verification +:link-type: doc +:text-align: center +:link: ./face_verification + +Running the face matching utility on the driver-facing video. +::: + +:::{card} Seatbelt +:link-type: doc +:text-align: center +:link: ./seatbelt + +Detecting seatbelt from video. +::: + +:::: \ No newline at end of file diff --git a/docs/tutorials/install.md b/docs/tutorials/install.md new file mode 100644 index 0000000..2f7ee97 --- /dev/null +++ b/docs/tutorials/install.md @@ -0,0 +1,13 @@ +# Installation + +1. Ensure you're running `Python3 >= 3.8` +2. To use the latest version, you can directly install from the GitHub repository. + ```bash + pip install git+https://github.com/microsoft/HAMS + ``` +3. Run the command below to ensure you've correctly installed the HAMS library. + ```python + import mapping_cli + print(mapping_cli.__version__) + ``` +4. Download the following files in order to run our library based on requirements mentioned in the [tutorials](../tutorials/index.md) \ No newline at end of file diff --git a/docs/tutorials/map_build.md b/docs/tutorials/map_build.md new file mode 100644 index 0000000..a62e3a3 --- /dev/null +++ b/docs/tutorials/map_build.md @@ -0,0 +1,33 @@ +# Map Building + +To setup the track, we first need to build maps of the individual maneuvers. + +First, setup the maneuver with the required aruco markers spread in such a manner that when the test taker performs the maneuver, the camera placed inside the test vehicle +is able to see at least 2 markers at all times. + +Now, collect images of the maneuver that is being tested - for best results, we recommend taking photos at approximately the level of the markers under sufficient sunlight i.e. +not too bright or not too dark. Take photos from different angles(within the maneuver), although the photos should not capture any other markers that are not part of this map. + +## File Requirements + +1. Path to the binaries / executable named `mapper_from_images`(see the `Installation` section from the sidebar to get download instructions) +2. Compile all the images of a map into a single folder - let's call this folder `images` +3. Generate the camera calibration file. To generate one, you can check the `Camera Calibration` module from the `Tutorials` section. Let's call this file `calib.yml` +4. Depending on the aruco markers setup on the map, add the dictionary. Typically, we use `TAG16h5` +5. Marker size: Size of the printed aruco markers + + +In order to directly run this module, execute the following command in your terminal + +```bash +mapping-cli.exe map +``` + +Follow the instructions below to generate and visualize the map + +```{button-link} ./map_building_notebook.ipynb +:color: primary +:shadow: + +Running the map building module on track images. +``` \ No newline at end of file diff --git a/docs/tutorials/map_building_notebook.ipynb b/docs/tutorials/map_building_notebook.ipynb new file mode 100644 index 0000000..37eb081 --- /dev/null +++ b/docs/tutorials/map_building_notebook.ipynb @@ -0,0 +1,123 @@ +{ + "cells": [ + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Running HAMS Map Generation module" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "\n", + "from mapping_cli.mapper import run as mapper" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## File Requirements\n", + "\n", + "1. Path to the binaries / executable named `mapper_from_images`\n", + "2. Compile all the images of a map into a single folder - let's call this folder `images`\n", + "3. Generate the camera calibration file. To generate one, you can check the `Camera Calibration` module from the `Tutorials` section. Let's call this file `calib.yml`\n", + "4. Depending on the aruco markers setup on the map, add the dictionary. Typically, we use `TAG16h5`\n", + "5. Marker size: Size of the printed aruco markers \n", + "\n", + "Let's add these to the variables below. If you've changed the names, please change the variable values in the subsequent cell" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "exec_path = 'mapper_from_images'\n", + "img_folder = \"images\"\n", + "calib_file = \"calib.yml\"\n", + "aruco_dict = \"TAG16h5\"\n", + "marker_size = 29.2\n", + "\n", + "name = \"map_example\"\n", + "output_folder = \"output/\"" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "os.makedirs(output_folder, exist_ok=True)\n", + "mapper(exec_path, img_folder, calib_file, aruco_dict, marker_size, name, output_folder)" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Visualize the Point Cloud Map\n", + "\n", + "We'll use [Open3D](http://www.open3d.org/) to visualize the generated point cloud map" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "%pip install open3d\n", + "import open3d\n", + "from open3d.web_visualizer import draw" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [], + "source": [ + "pcd = open3d.io.read_point_cloud(\"output/map_example.pcd\")\n", + "draw(pcd)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "hams", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.16" + }, + "orig_nbformat": 4, + "vscode": { + "interpreter": { + "hash": "4ecb028c9e4ec612609cae8571d5e3a76bf96bee660effba276681a0b0090bd9" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/docs/tutorials/seatbelt.md b/docs/tutorials/seatbelt.md new file mode 100644 index 0000000..827a3ec --- /dev/null +++ b/docs/tutorials/seatbelt.md @@ -0,0 +1,61 @@ +# Seatbelt Detection + +## Setup + +Given the driver-facing video, the seatbelt detection module uses an object detection pipeline to detect the seatbelt on the driver. + +In order to run the seatbelt module on the front facing video(let's call it `front_video.mp4`), first add a file named `seatbelt.yml` to your current folder. + +
+ seatbelt.yml(click to open/close) + + ```yaml + device: "cpu" + skip_frames: 25 + classifier_confidence_threshold: 0.75 + detection_percentage: 0.75 + model_path: ["models", "seatbelt_model.pth"] + ``` +
+ +
+ Explanation of the above configuration values(click to open) + + ```{list-table} + :header-rows: 1 + + * - Parameter + - Description + - Example Value + * - device + - Hardware for pytorch to run the model inference on + - "cpu" or "cuda:0" + * - skip_frames + - Number of frames to skip(integer) + - 25 + * - classifier_confidence_threshold + - Threshold to classify seatbelt detection(float) + - 0.75 + * - detection_percentage + - Percentage number of detections to consider the test as pass(float) + - 0.75 + * - model_path + - path to the saved model. Format: ['directory', 'file_name'] + - ["models", "seatbelt_model.pth"] + ``` +
+ +Now, run the following command: + +```bash +python main.py --seat-belt front_video.mp4 --config seatbelt.yml --output-path results/ +``` + +The following notebook has a code-walkthrough to run the Seatbelt module and visualize the results: + +```{button-link} ./seatbelt_notebook.ipynb +:color: primary +:shadow: + +Running the seatbelt code on driver-facing video. +``` \ No newline at end of file diff --git a/docs/tutorials/seatbelt_notebook.ipynb b/docs/tutorials/seatbelt_notebook.ipynb new file mode 100644 index 0000000..93e9434 --- /dev/null +++ b/docs/tutorials/seatbelt_notebook.ipynb @@ -0,0 +1,169 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Running HAMS SeatBelt module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "from ipywidgets import FileUpload\n", + "from omegaconf import OmegaConf\n", + "\n", + "from mapping_cli.maneuvers.seat_belt import SeatBelt\n", + "from mapping_cli.config.config import Config" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from ipywidgets import FileUpload\n", + "from IPython.display import display, Image\n", + "upload = FileUpload(accept='.mp4', multiple=False)\n", + "display(upload)\n" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Upload a driver-facing video to detect the seatbelt" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open(\"front_video.mp4\", \"wb\") as f:\n", + " f.write(upload.value[0].content)" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Modify the config variables depending on the need. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "config = {\n", + " \"device\": \"cpu\",\n", + " \"skip_frames\": 25,\n", + " \"classifier_confidence_threshold\": 0.75,\n", + " \"detection_percentage\": 0.75,\n", + " \"model_path\": [\"models\", \"seatbelt_model.pth\"],\n", + "}" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now, save the config to `seatbelt.yaml`, create a directory to store the outputs named `output` and run the seabelt module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open('seatbelt.yaml', 'w') as f:\n", + " OmegaConf.save(OmegaConf.create(config), f)\n", + "os.makedirs('output', exist_ok=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "seatbelt = SeatBelt(inputs={\"fpath\":os.path.abspath('front_video.mp4')}, inertial_data=None, config=Config('seatbelt.yaml'), out_folder='./output')\n", + "_, found_belt, _ = seatbelt.run()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Visualize the Outputs" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "if found_belt:\n", + " display(Image(filename='output/seatbelt_image.jpg'))" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Final Test Results" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import json\n", + "import ast\n", + "with open('output/report.txt', 'r') as f:\n", + " report = json.load(f) \n", + " print(\"Pass: \", ast.literal_eval(report['Seatbelt'])[1])" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.16 (default, Jan 17 2023, 22:25:28) [MSC v.1916 64 bit (AMD64)]" + }, + "vscode": { + "interpreter": { + "hash": "4ecb028c9e4ec612609cae8571d5e3a76bf96bee660effba276681a0b0090bd9" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/docs/tutorials/segment.md b/docs/tutorials/segment.md new file mode 100644 index 0000000..9ef3988 --- /dev/null +++ b/docs/tutorials/segment.md @@ -0,0 +1,10 @@ +# Segmentation + +Segmentation + +```{button-link} ./segment_notebook.ipynb +:color: primary +:shadow: + +Running the segmentation code on track-facing video. +``` \ No newline at end of file diff --git a/docs/tutorials/segment_notebook.ipynb b/docs/tutorials/segment_notebook.ipynb new file mode 100644 index 0000000..5c37b43 --- /dev/null +++ b/docs/tutorials/segment_notebook.ipynb @@ -0,0 +1,140 @@ +{ + "cells": [ + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Running HAMS Video Segmentation module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import os\n", + "from ipywidgets import FileUpload\n", + "from omegaconf import OmegaConf\n", + "\n", + "from mapping_cli.segment import segment\n", + "from mapping_cli.config.config import Config" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from ipywidgets import FileUpload\n", + "from IPython.display import display, Video\n", + "\n", + "upload = FileUpload(accept='.mp4', multiple=False)\n", + "display(upload)\n" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Upload a track-facing video to segment it into its respective maneuvers" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open(\"back_video.mp4\", \"wb\") as f:\n", + " f.write(upload.value[0].content)" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Modify the config variables depending on the need. " + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "config = {}" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Now, save the config to `site.yaml`, create a directory to store the outputs named `output` and run the segmentation module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "with open('seatbelt.yaml', 'w') as f:\n", + " OmegaConf.save(OmegaConf.create(config), f)\n", + "os.makedirs('output', exist_ok=True)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "segment(None, os.path.abspath('back_video.mp4'), 'output', Config('site.yaml'))" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Segmentation Results" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "videos = [path for path in os.listdir('output') if '.mp4' in path]\n", + "for video in videos:\n", + " display(Video(video))" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "hams", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python", + "version": "3.8.16 (default, Jan 17 2023, 22:25:28) [MSC v.1916 64 bit (AMD64)]" + }, + "orig_nbformat": 4, + "vscode": { + "interpreter": { + "hash": "4ecb028c9e4ec612609cae8571d5e3a76bf96bee660effba276681a0b0090bd9" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/docs/tutorials/trajectory_generation.ipynb b/docs/tutorials/trajectory_generation.ipynb new file mode 100644 index 0000000..8303e53 --- /dev/null +++ b/docs/tutorials/trajectory_generation.ipynb @@ -0,0 +1,38 @@ +{ + "cells": [ + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Running HAMS Trajectory Generation module" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "hams", + "language": "python", + "name": "python3" + }, + "language_info": { + "name": "python", + "version": "3.8.16 (default, Jan 17 2023, 22:25:28) [MSC v.1916 64 bit (AMD64)]" + }, + "orig_nbformat": 4, + "vscode": { + "interpreter": { + "hash": "4ecb028c9e4ec612609cae8571d5e3a76bf96bee660effba276681a0b0090bd9" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/docs/tutorials/trajectory_generation.md b/docs/tutorials/trajectory_generation.md new file mode 100644 index 0000000..70eedfd --- /dev/null +++ b/docs/tutorials/trajectory_generation.md @@ -0,0 +1,10 @@ +# Trajectory Generation + +Trajectory generation + +```{button-link} ./trajectory_generation_notebook.ipynb +:color: primary +:shadow: + +Running the trajectory generation module on the user test video. +``` \ No newline at end of file diff --git a/mapping_cli/__init__.py b/mapping_cli/__init__.py new file mode 100644 index 0000000..3dc1f76 --- /dev/null +++ b/mapping_cli/__init__.py @@ -0,0 +1 @@ +__version__ = "0.1.0" diff --git a/mapping_cli/calibration.py b/mapping_cli/calibration.py new file mode 100644 index 0000000..118c74d --- /dev/null +++ b/mapping_cli/calibration.py @@ -0,0 +1,189 @@ +""" +This code assumes that images used for calibration are of the same arUco marker board provided with code i.e. `DICT_6X6_1000` + +Credit: https://github.com/abakisita/camera_calibration +""" + +import logging +import os +from pathlib import Path + +import cv2 +import numpy as np +import yaml +from cv2 import aruco +from tqdm import tqdm + + +def camera_calibration( + phone_model: str, + calib_path: str, + marker_length: str, + marker_separation: str, + output_folder: str, +): + filename = os.path.join(output_folder, "calib_{}.yml".format(phone_model)) + markerLength = float(marker_length) + markerSeparation = float(marker_separation) + yaml_string = "%YAML:1.0 \n\ + --- \n\ + image_width: {} \n\ + image_height: {} \n\ + camera_matrix: !!opencv-matrix \n\ + rows: 3 \n\ + cols: 3 \n\ + dt: d \n\ + data: [ {}, {}, {}, {}, \n\ + {}, {}, {}, {}, {} ] \n\ + distortion_coefficients: !!opencv-matrix \n\ + rows: 1 \n\ + cols: 5 \n\ + dt: d \n\ + data: [ {}, {}, \n\ + {}, {}, \n\ + {} ]" + + # root directory of repo for relative path specification. + root = Path(__file__).parent.absolute() + + # Set this flsg True for calibrating camera and False for validating results real time + calibrate_camera = True + + # Set path to the images + calib_imgs_path = Path(calib_path) + # For validating results, show aruco board to camera. + aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_1000) + + # create arUco board + board = aruco.GridBoard_create(4, 5, markerLength, markerSeparation, aruco_dict) + + """uncomment following block to draw and show the board""" + # img = board.draw((432,540)) + # cv2.imshow("aruco", img) + # cv2.waitKey() + + arucoParams = aruco.DetectorParameters_create() + + if calibrate_camera == True: + img_list = [] + calib_fnms = calib_imgs_path.glob("*.jpg") + logging.info("Using ...") + for idx, fn in enumerate(calib_fnms): + logging.info(f"{idx}, {fn}") + print("Reading: ", r"{}".format(str(os.path.join(calib_imgs_path.parent, fn)))) + img = cv2.imread(r"{}".format(str(os.path.join(calib_imgs_path.parent, fn)))) + assert img is not None + img_list.append(img) + h, w, c = img.shape + logging.info("Shape of the images is : {} , {}, {} ".format(h, w, c)) + logging.info("Calibration images") + + counter, corners_list, id_list = [], [], [] + first = True + for im in tqdm(img_list): + logging.info("In loop") + img_gray = cv2.cvtColor(im, cv2.COLOR_RGB2GRAY) + corners, ids, rejectedImgPoints = aruco.detectMarkers( + img_gray, aruco_dict, parameters=arucoParams + ) + cv2.aruco.drawDetectedMarkers(im, corners, ids) + # cv2.imshow("img", cv2.resize(im, None, fx=0.25,fy=0.25)) + # cv2.waitKey() + if first == True: + corners_list = corners + id_list = ids + first = False + else: + corners_list = np.vstack((corners_list, corners)) + id_list = np.vstack((id_list, ids)) + counter.append(len(ids)) + assert len(id_list) > 0 + print("Found {} unique markers".format(np.unique(id_list))) + + counter = np.array(counter) + logging.info("Calibrating camera .... Please wait...") + # mat = np.zeros((3,3), float) + ret, mtx, dist, rvecs, tvecs = aruco.calibrateCameraAruco( + corners_list, id_list, counter, board, img_gray.shape, None, None + ) + + logging.info( + "Camera matrix is \n", + mtx, + "\n And is stored in calibration.yaml file along with distortion coefficients : \n", + dist, + ) + + mtx = np.asarray(mtx).tolist() + dist = np.asarray(dist).tolist()[0] + logging.info("printing ", dist) + # logging.info("Printing w, h for confirmation : {}".format(w, h)) + yaml_string = yaml_string.format( + w, + h, + mtx[0][0], + mtx[0][1], + mtx[0][2], + mtx[1][0], + mtx[1][1], + mtx[1][2], + mtx[2][0], + mtx[2][1], + mtx[2][2], + dist[0], + dist[1], + dist[2], + dist[3], + dist[4], + ) + + with open(filename, "w") as text_file: + text_file.write(yaml_string) + + else: + camera = cv2.VideoCapture(0) + ret, img = camera.read() + + with open("calibration.yaml") as f: + loadeddict = yaml.load(f) + mtx = loadeddict.get("camera_matrix") + dist = loadeddict.get("dist_coeff") + mtx = np.array(mtx) + dist = np.array(dist) + + ret, img = camera.read() + img_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) + h, w = img_gray.shape[:2] + newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h)) + + pose_r, pose_t = [], [] + while True: + ret, img = camera.read() + img_aruco = img + im_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) + h, w = im_gray.shape[:2] + dst = cv2.undistort(im_gray, mtx, dist, None, newcameramtx) + corners, ids, rejectedImgPoints = aruco.detectMarkers( + dst, aruco_dict, parameters=arucoParams + ) + if corners == None: + logging.info("pass") + else: + ret, rvec, tvec = aruco.estimatePoseBoard( + corners, ids, board, newcameramtx, dist + ) # For a board + logging.info("Rotation ", rvec, "Translation", tvec) + if ret != 0: + img_aruco = aruco.drawDetectedMarkers( + img, corners, ids, (0, 255, 0) + ) + # axis length 100 can be changed according to your requirement + img_aruco = aruco.drawAxis( + img_aruco, newcameramtx, dist, rvec, tvec, 10 + ) + + if cv2.waitKey(0) & 0xFF == ord("q"): + break + # cv2.imshow("World co-ordinate frame axes", img_aruco) + + # cv2.destroyAllWindows() diff --git a/mapping_cli/cli.py b/mapping_cli/cli.py new file mode 100644 index 0000000..2de7db5 --- /dev/null +++ b/mapping_cli/cli.py @@ -0,0 +1,131 @@ +import os + +import typer + +from mapping_cli import mapper +from mapping_cli.config.config import Config +from mapping_cli.maneuvers.face_verification import FaceVerification +from mapping_cli.maneuvers.gaze import Gaze +from mapping_cli.maneuvers.seat_belt import SeatBelt + +app = typer.Typer("HAMS CLI") + + +@app.command() +def map( + mapper_exe_path: str, + images_directory: str, + camera_params_path: str, + dictionary: str, + marker_size: str, + output_path: str, + cwd: str = None, +): + """Command to build a Map using the mapper exe and images + + Args: + mapper_exe_path (str): Mapper exe path. + images_directory (str): Image Directory Path. + camera_params_path (str): Camera config/param yml file path. + dictionary (str): Type of Dictionary. + marker_size (str): Size of the marker. + output_path (str): Output file name. + cwd (str): Working Directry. + """ + mapper.run( + mapper_exe_path, + images_directory, + camera_params_path, + dictionary, + marker_size, + output_path, + cwd, + ) + + +@app.command() +def error(map_file: str, dist_file: str): + """Command to get the error of map generated + + Args: + map_file (str): Map YML File Path + dist_file (str): Dist Text File Path + """ + mapper.distance_error(map_file, dist_file) + + +# @app.command() +# def find(key: str): +# typer.echo(conf.get_config_value(key)) + + +@app.command() +def seat_belt( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = None, + config=".", + config_file_name="seatbelt_config.yaml", +): + assert front_video is not None, typer.echo("Front Video Path is required") + inputs = {"fpath": front_video} + + sb = SeatBelt( + inputs, None, Config(os.path.join(config, config_file_name)), output_path + ) + percentage_detections, wearing_all_the_time, stats = sb.run() + + typer.echo(f"{percentage_detections}, {wearing_all_the_time}, {stats}") + + +@app.command() +def face_verify( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = None, + config=".", + config_file_name="face_verification.yaml", +): + assert front_video is not None, typer.echo("Front Video Path is required") + assert calib_video is not None, typer.echo("Calib Video Path is required") + inputs = { + "fpath": front_video, + "calib_video": calib_video, + } + + face_verify = FaceVerification( + inputs=inputs, + inertial_data=None, + config=Config(os.path.join(config, config_file_name)), + out_folder=output_path, + ) + face_verify.run() + + +@app.command() +def gaze( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config=".", + config_file_name="gaze", +): + assert front_video is not None, typer.echo("Front Video Path is required") + assert calib_video is not None, typer.echo("Calib Video Path is required") + inputs = { + "fpath": front_video, + "calib_video": calib_video, + } + + gaze = Gaze(inputs, None, Config(config_file_name), output_path) + gaze.run() + + +if __name__ == "__main__": + app() diff --git a/mapping_cli/config/__init__.py b/mapping_cli/config/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/mapping_cli/config/config.py b/mapping_cli/config/config.py new file mode 100644 index 0000000..5017253 --- /dev/null +++ b/mapping_cli/config/config.py @@ -0,0 +1,29 @@ +import os + +from hydra import compose, initialize +from omegaconf import DictConfig, OmegaConf + + +class Config: + conf = None + + def __init__(self, path: str): + self.conf = OmegaConf.load(path) + + def get_config_value(self, key: str): + try: + return self.conf[key] + except: + raise KeyError(f"Cannot find the key {key} in config file") + + def __getitem__(self, key): + return self.get_config_value(key) + + def keys(self): + return self.conf.keys() + + def values(self): + return self.conf.values() + + def items(self): + return self.conf.items() diff --git a/mapping_cli/halts.py b/mapping_cli/halts.py new file mode 100644 index 0000000..3a87582 --- /dev/null +++ b/mapping_cli/halts.py @@ -0,0 +1,90 @@ +import argparse +import csv +import itertools +import os + +import matplotlib.pyplot as plt +import numpy as np +from pyquaternion import Quaternion +from scipy.signal import savgol_filter +from sklearn import decomposition + +# from mapping_cli.utils import smooth +# from mapping_cli.utils import smoothen_tr + + +def smooth_halt_signal(x, window_size=31, poly_order=2): + """ """ + print("Len: ", len(x)) + smooth_x = savgol_filter(x, window_size, poly_order) + # for i in range(5): + # smooth_x = savgol_filter(smooth_x, window_size, poly_order) + return smooth_x + + +def debug_halt_visualize( + traj, traj_1d, smooth_traj, velocity, smooth_vel, zero_frames, fname +): + """ """ + f, (ax1, ax2, ax3) = plt.subplots(1, 3) + ax1.plot(traj[:, 0], traj[:, 2], label="Original Camera Path") + ax2.plot(traj_1d, label="1D Camera Path") + ax2.plot(smooth_traj, label="Filtered Trajectory") + ax3.plot(velocity, label="Velocity") + ax3.plot(smooth_vel, label="Smooth Velocity") + ax3.scatter( + [i for i in range(zero_frames.shape[0]) if zero_frames[i]], + [smooth_vel[i] for i in range(zero_frames.shape[0]) if zero_frames[i]], + c="red", + ) + ax2.legend() + ax1.legend() + ax3.legend() + # plt.show() + + save_image_name = os.path.splitext(fname)[0] + "_halts.png" + plt.savefig(save_image_name) + + +def get_halts_worker(tx, ty, tz, vel_delta): + # if len(tx) < 199: + # # pass + # tx, ty, tz = smoothen_trajectory(tx, ty, tz, 21, 49, 2) + # else: + # tx, ty, tz = smoothen_trajectory(tx, ty, tz, 99, 199, 2) + + traj = np.array([x for x in zip(tx, ty, tz)]) + pca = decomposition.PCA(n_components=1) + traj_1d = traj + traj_1d = pca.fit_transform(traj).reshape((-1,)) + + # Smoothen trajectory + try: + smooth_traj = smooth_halt_signal(traj_1d) + except ValueError as e: + print("Trajectory length is too small!\n", e) + + # Get Velocity and smoothen it + velocity = np.abs(smooth_traj[:-vel_delta] - smooth_traj[vel_delta:]) + traj_1d = traj_1d[:-vel_delta] + smooth_traj = smooth_traj[:-vel_delta] + traj = traj[:-vel_delta] + + smooth_vel = smooth_halt_signal(velocity) + + # Get frames with no motion + zero_frames = np.zeros((traj_1d.shape[0]), dtype=bool) + zero_frames[smooth_vel <= 1e-3] = True + zero_frames[traj[:, 0] == 0] = False + + # Compensate for velocity delta + zero_frames = np.array(zero_frames.tolist() + [False] * vel_delta) + + # debug_halt_visualize(traj, traj_1d, smooth_traj, velocity, smooth_vel, zero_frames) + + return zero_frames + + +def get_halts(tx, ty, tz, vel_delta=15): + """ """ + return get_halts_worker(tx, ty, tz, vel_delta) diff --git a/mapping_cli/locator.py b/mapping_cli/locator.py new file mode 100644 index 0000000..8e88d5a --- /dev/null +++ b/mapping_cli/locator.py @@ -0,0 +1,308 @@ +import os +import subprocess +import sys + +import matplotlib +import shapely +import shapely.geometry + +matplotlib.use("Agg") +import glob +import json + +import cv2 +import matplotlib.pyplot as plt +import moviepy.video.io.ImageSequenceClip +import numpy as np +from cv2 import aruco +from natsort import natsorted + +from mapping_cli.utils import (get_marker_coord, read_aruco_traj_file, + smoothen_trajectory, yml_parser) + +DICT_CV2_ARUCO_MAP = { + "TAG16h5": cv2.aruco.DICT_APRILTAG_16h5, + "DICT_4X4_100": cv2.aruco.DICT_4X4_100, +} + + +def generate_trajectory_from_photos( + input_path: str, + maneuver: str, + map_file_path: str, + out_folder: str, + calibration: str, + size_marker: str, + aruco_test_exe: str, + cwd: str, + ignoring_points: str = "", + delete_video: bool = False, + input_extension: str = ".jpg", + framerate: int = 30, + box_plot: bool = True, + annotate: bool = True, +): + out_file = os.path.join(out_folder, "temp.mp4") + + image_files = [ + os.path.join(input_path, img) + for img in os.listdir(input_path) + if img.endswith(input_extension) + ] + image_files = natsorted(image_files) + print(image_files) + clip = moviepy.video.io.ImageSequenceClip.ImageSequenceClip( + image_files, fps=framerate + ) + clip.write_videofile(out_file) + + return get_locations( + out_file, + maneuver, + map_file_path, + out_folder, + calibration, + size_marker, + aruco_test_exe, + cwd, + ignoring_points=ignoring_points, + box_plot=box_plot, + annotate=annotate, + ) + if delete_video: + os.remove(out_file) + + +def parse_marker_set(marker_set_str): + return [int(x.strip()) for x in marker_set_str.split(",")] + + +def black_out(image_dir, out_dir, marker_set_str, marker_dict_str): + images_f = glob.glob(image_dir + "*.jpg") + dict_type = DICT_CV2_ARUCO_MAP[marker_dict_str] + aruco_dict = aruco.Dictionary_get(dict_type) + marker_set = parse_marker_set(marker_set_str) + flatten = lambda l: [item for sublist in l for item in sublist] + for image_f in images_f: + im = cv2.imread(image_f) + marker_info = cv2.aruco.detectMarkers(im, aruco_dict) + markers_in_image = flatten(marker_info[1].tolist()) + blacked_marker_ids = [ + i for i, m in enumerate(markers_in_image) if m not in marker_set + ] + if blacked_marker_ids != []: + cv2.fillPoly( + im, + pts=[marker_info[0][i][0].astype(int) for i in blacked_marker_ids], + color=(0, 0, 0), + ) + cv2.imwrite(out_dir + os.path.split(image_f)[1], im) + + +def get_locations( + input_video: str, + maneuver: str, + map_file: str, + out_folder: str, + calibration: str, + size_marker: str, + aruco_test_exe: str, + cwd: str, + ignoring_points: str = "", + plot: bool = True, + box_plot: bool = True, + smoothen: bool = True, + blacken: bool = False, + return_read: bool = False, + annotate: bool = True, +): + output_traj_path = os.path.join(out_folder, maneuver + "_CameraTrajectory.txt") + + if not os.path.exists(map_file): + map_file = os.path.join(cwd, map_file) + if not os.path.exists(calibration): + calibration = os.path.join(cwd, calibration) + + # aruco_test_exe = r"C:\Users\t-jj\Documents\Projects\electron-hams\data\aruco_binaries\aruco_test_markermap.exe" + sys.path.append(os.path.dirname(aruco_test_exe)) + exec_string = ( + f"{aruco_test_exe} {input_video} {map_file} {calibration} -s {size_marker}" + ) + + print(exec_string, cwd) + + my_env = os.environ.copy() + my_env["PATH"] = out_folder + ";" + cwd + ";" + my_env["PATH"] + + p = subprocess.Popen( + exec_string, + shell=True, + cwd=cwd, + env=my_env, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + stdout, stderr = p.communicate() + # print("Hellloooo traj") + print("out: ", stdout, "\n", "err: ", stderr) + + # output = subprocess.check_output(exec_string, shell=True, cwd=out_folder) + output = stdout.decode("utf-8") + output = ( + output.replace(";\r\n", "") + .replace("]", "") + .replace("[", "") + .replace(",", "") + .replace("\r\n", "\n") + ) + + with open(output_traj_path, "w+") as f: + f.write(output) + f.close() + + plt_path = "" + json_path = "" + + if plot: + if box_plot: + plt_path, json_path = save_box_coords( + map_file, + output_traj_path, + out_folder, + maneuver, + ignoring_points, + smoothen, + annotate, + ) + else: + plt_path, json_path = save_line_coords( + map_file, output_traj_path, out_folder, maneuver, ignoring_points + ) + + if return_read: + return read_aruco_traj_file(output_traj_path, {}) + + return output_traj_path, plt_path, json_path + + +def plot_markers(markers): + markers_array = [] + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + plt.scatter( + marker_coord[0], marker_coord[1], c=[[0, 0, 0]], marker=".", s=100.0 + ) + plt.annotate(str(key), (marker_coord[0], marker_coord[1])) + markers_array.append([marker_coord[0], marker_coord[1]]) + + return markers_array + + +def save_box_coords( + manu_map, + box_trajectory, + out_folder, + manu, + ignoring_points: str = "", + smoothen: bool = False, + annotate: bool = True, +): + # inputs: mapfile, box_trajectory, out_json, manoueuvre + # output: set containing 4 corners of the box to the out_json + if len(ignoring_points) > 0: + ignoring_points_array = ignoring_points.split(",") + ignoring_points_array = list(map(int, ignoring_points_array)) + ignoring_points_dict = {i: True for i in ignoring_points_array} + + else: + ignoring_points_dict = dict() + + t_x = t_y = t_z = f_id = None + if smoothen: + f_id, t_x, t_y, t_z, _ = read_aruco_traj_file( + box_trajectory, ignoring_points_dict + ) + for k in ignoring_points_dict.keys(): + assert k not in f_id + # TODO Check window length + if len(f_id) < 99: + smooth_window = len(f_id) - 1 if len(f_id) % 2 == 0 else len(f_id) - 2 + else: + smooth_window = 99 + # smooth_window = 15 + t_x, t_y, t_z = smoothen_trajectory(t_x, t_y, t_z, smooth_window, 199, 2) + + print(ignoring_points_dict) + + # try: + # t_x, t_y, t_z = smoothen_trajectory(t_x, t_y, t_z, 3, 3, 2) + # except: + # pass + camera_coords = list(zip(t_x, t_z)) + + rect_align = shapely.geometry.MultiPoint( + camera_coords + ).minimum_rotated_rectangle # .envelope #minimum_rotated_rectangle + x, y = rect_align.exterior.coords.xy + rect_align = list(zip(x, y)) + + markers = plot_markers(yml_parser(manu_map)) + + for i in range(len(t_x)): + plt.scatter(t_x[i], t_z[i], color="green") + if annotate: + plt.annotate(str(f_id[i]), (t_x[i], t_z[i])) + for i in range(len(rect_align)): + plt.scatter(rect_align[i][0], rect_align[i][1], color="blue") + plt.gca().invert_yaxis() + + trackId = manu_map.split("/maps/")[0].split("/")[-1] + plt_path = os.path.join(out_folder, f"{manu}.png") + plt.savefig(plt_path) + + points = [list(a) for a in zip(t_x, t_z)] + value = {"box": rect_align, "markers": markers, "points": points} + json_path = os.path.join(out_folder, f"{manu}.json") + with open(json_path, "w") as f: + json.dump(value, f) + + print(rect_align) + + return plt_path, json_path + + +def save_line_coords( + manu_map, line_trajectory, out_folder, manu, ignoring_points: str = "" +): + if len(ignoring_points) > 0: + ignoring_points_array = ignoring_points.split(",") + ignoring_points_array = list(map(int, ignoring_points_array)) + ignoring_points_dict = {i: True for i in ignoring_points_array} + + else: + ignoring_points_dict = dict() + + f_id, t_x, _, t_z, _ = read_aruco_traj_file(line_trajectory, ignoring_points_dict) + line_eq = np.polyfit(t_x, t_z, 1) + + plot_markers(yml_parser(manu_map)) + for i in range(len(t_x)): + plt.scatter(t_x[i], t_z[i], color="green") + plt.annotate(str(f_id[i]), (t_x[i], t_z[i])) + + x_lim = (2, 10) + y_lim = (0, 5) + for x in np.arange(x_lim[0], x_lim[1], 0.1): + y = line_eq[0] * x + line_eq[1] + plt.scatter(x, y, color="blue") + plt.gca().invert_yaxis() + plt_path = os.path.join(out_folder, f"{manu}.png") + plt.savefig(plt_path) + + value = {"line_eq": line_eq.tolist(), "pts": {"tx": t_x, "tz": t_z}} + json_path = os.path.join(out_folder, f"{manu}.json") + with open(json_path, "w") as f: + json.dump(value, f) + + return plt_path, json_path diff --git a/mapping_cli/main.py b/mapping_cli/main.py new file mode 100644 index 0000000..5f3b579 --- /dev/null +++ b/mapping_cli/main.py @@ -0,0 +1,426 @@ +import imp +import os + +import typer + +from mapping_cli import mapper +from mapping_cli.calibration import camera_calibration +from mapping_cli.config.config import Config +from mapping_cli.locator import generate_trajectory_from_photos, get_locations +from mapping_cli.maneuvers.face_verification import FaceVerification +from mapping_cli.maneuvers.forward_eight import ForwardEight +from mapping_cli.maneuvers.gaze import Gaze +from mapping_cli.maneuvers.incline import Incline +from mapping_cli.maneuvers.marker_sequence import MarkerSequence +from mapping_cli.maneuvers.pedestrian import Pedestrian +from mapping_cli.maneuvers.perp import PERP +from mapping_cli.maneuvers.rpp import RPP +from mapping_cli.maneuvers.seat_belt import SeatBelt +from mapping_cli.maneuvers.traffic import Traffic +from mapping_cli.segment import segment as segment_test + +app = typer.Typer(name="HAMS") + + +@app.callback() +def callback(): + """ + HAMS CLI + """ + + +@app.command() +def map( + mapper_exe_path: str, + images_directory: str, + camera_params_path: str, + dictionary: str, + marker_size: str, + output_path: str, + cwd: str = None, +): + """Command to build a Map using the mapper exe and images + + Args: + mapper_exe_path (str): Mapper exe path. + images_directory (str): Image Directory Path. + camera_params_path (str): Camera config/param yml file path. + dictionary (str): Type of Dictionary. + marker_size (str): Size of the marker. + output_path (str): Output file name. + cwd (str): Working Directry. + """ + mapper.run( + mapper_exe_path, + images_directory, + camera_params_path, + dictionary, + marker_size, + output_path, + cwd, + ) + + +@app.command() +def error(map_file: str, dist_file: str): + """Command to get the error of map generated + + Args: + map_file (str): Map YML File Path + dist_file (str): Dist Text File Path + """ + mapper.distance_error(map_file, dist_file) + + +@app.command() +def get_trajectory_from_video( + input_path: str, + maneuver: str, + map_file: str, + out_folder: str, + calibration: str, + size_marker: str, + aruco_test_exe: str, + cwd: str = "", + ignoring_points: str = "", + box_plot: bool = True, +): + return get_locations( + input_path, + maneuver, + map_file, + out_folder, + calibration, + size_marker, + aruco_test_exe, + ignoring_points, + cwd=cwd if len(cwd) > 0 else out_folder, + box_plot=box_plot, + ) + + +@app.command() +def get_trajectory_from_photos( + input_path: str, + maneuver: str, + map_file: str, + out_folder: str, + calibration: str, + size_marker: str, + aruco_test_exe: str, + cwd: str = "", + ignoring_points: str = "", + box_plot: bool = True, +): + return generate_trajectory_from_photos( + input_path, + maneuver, + map_file, + out_folder, + calibration, + size_marker, + aruco_test_exe, + ignoring_points, + cwd=cwd if len(cwd) > 0 else out_folder, + box_plot=box_plot, + ) + + +@app.command() +def generate_calib( + phone_model: str, + calib_path: str, + marker_length: str, + marker_separation: str, + output_folder: str, +): + return camera_calibration( + phone_model, calib_path, marker_length, marker_separation, output_folder + ) + + +@app.command() +def seat_belt( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = None, + config_path="./mapping_cli/config/seatbelt_config.yaml", +): + assert front_video is not None, typer.echo("Front Video Path is required") + inputs = {"fpath": front_video} + + sb = SeatBelt(inputs, None, Config(config_path), output_path) + percentage_detections, wearing_all_the_time, stats = sb.run() + + typer.echo(f"{percentage_detections}, {wearing_all_the_time}, {stats}") + + media = [ + { + "title": "Seatbelt Image", + "path": os.path.join(output_path, "seatbelt_image.jpg"), + }, + ] + + return percentage_detections, wearing_all_the_time, stats, media + + +@app.command() +def face_verify( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = None, + config_path="./mapping_cli/config/face_verification.yaml", +): + assert front_video is not None, typer.echo("Front Video Path is required") + assert calib_video is not None, typer.echo("Calib Video Path is required") + inputs = { + "fpath": front_video, + "calib_video": calib_video, + } + + face_verify = FaceVerification(inputs, None, Config(config_path), output_path) + result = face_verify.run() + + media = [ + {"title": "Front Video", "path": front_video}, + {"title": "Calib Video", "path": calib_video}, + { + "title": "Face Registration", + "path": os.path.join(output_path, "face_registration.png"), + }, + { + "title": "Face Validated", + "path": os.path.join(output_path, "face_validated.png"), + }, + ] + + return result, media + + +@app.command() +def gaze( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/gaze.yaml", +): + assert front_video is not None, typer.echo("Front Video Path is required") + assert calib_video is not None, typer.echo("Calib Video Path is required") + inputs = { + "fpath": front_video, + "calib_video": calib_video, + } + + gaze = Gaze(inputs, None, Config(config_path), output_path) + decision, stats = gaze.run() + + media = [ + {"title": "Front Video", "path": front_video}, + {"title": "Calib Video", "path": calib_video}, + {"title": "Front Gaze", "path": os.path.join(output_path, "front_gaze.mp4"),}, + ] + + return decision, stats, media + + +@app.command() +def rpp( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/rpp.yaml", + cwd: str = "", +): + assert back_video is not None, typer.echo("Back Video Path is required") + try: + inputs = { + "back_video": back_video, + } + inputs["cwd"] = cwd if len(cwd) > 0 else output_path + + rpp = RPP(inputs, None, Config(config_path), output_path) + (decision, stats), media = rpp.run() + return decision, stats, media + except Exception as e: + print(e) + raise e + + +@app.command() +def perp( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/perp.yaml", + cwd: str = "", +): + assert back_video is not None, typer.echo("Back Video Path is required") + try: + inputs = { + "back_video": back_video, + } + inputs["cwd"] = cwd if len(cwd) > 0 else output_path + + perp = PERP(inputs, None, Config(config_path), output_path) + (decision, stats), media = perp.run() + return decision, stats, media + except Exception as e: + print(e) + raise e + + +@app.command() +def incline( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/incline.yaml", + cwd: str = "", +): + assert back_video is not None, typer.echo("Back Video Path is required") + try: + inputs = { + "back_video": back_video, + } + inputs["cwd"] = cwd if len(cwd) > 0 else output_path + + incline = Incline(inputs, None, Config(config_path), output_path) + (stats, decision), (media) = incline.run() + return decision, stats, media + + except Exception as e: + print(e) + raise e + + +@app.command() +def traffic( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/traffic.yaml", + cwd: str = "", +): + assert back_video is not None, typer.echo("Back Video Path is required") + try: + inputs = { + "back_video": back_video, + } + inputs["cwd"] = cwd if len(cwd) > 0 else output_path + + traffic = Traffic(inputs, None, Config(config_path), output_path) + decision, stats, media = traffic.run() + return decision, stats, media + except Exception as e: + print(e) + raise e + + +@app.command() +def pedestrian( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/pedestrian.yaml", + cwd: str = "", +): + assert back_video is not None, typer.echo("Back Video Path is required") + try: + inputs = { + "back_video": back_video, + } + inputs["cwd"] = cwd if len(cwd) > 0 else output_path + + pedestrian = Pedestrian(inputs, None, Config(config_path), output_path) + (decision, stats), (media) = pedestrian.run() + return decision, stats, media + except Exception as e: + print(e) + raise e + + +@app.command() +def marker_sequence( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/marker_sequence.yaml", + cwd: str = "", +): + assert back_video is not None, typer.echo("Back Video Path is required") + try: + inputs = { + "back_video": back_video, + } + inputs["cwd"] = cwd if len(cwd) > 0 else output_path + + marker_sequence = MarkerSequence(inputs, None, Config(config_path), output_path) + (decision, stats), (media) = marker_sequence.run() + return decision, stats, media + except Exception as e: + print(e) + raise e + + +@app.command() +def forward_eight( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/forward_eight.yaml", + cwd: str = "", +): + assert back_video is not None, typer.echo("Back Video Path is required") + try: + inputs = { + "back_video": back_video, + } + inputs["cwd"] = cwd if len(cwd) > 0 else output_path + + forward_eight = ForwardEight(inputs, None, Config(config_path), output_path) + (decision, stats), (media) = forward_eight.run() + return decision, stats, media + except Exception as e: + print(e) + raise e + + +@app.command() +def segment( + front_video: str = None, + back_video: str = None, + calib_video: str = None, + inertial_data: str = None, + output_path: str = "test_output", + config_path="./mapping_cli/config/site.yaml", +): + assert back_video is not None, typer.echo("Back video is required") + inputs = { + "back_video": back_video, + } + segment_paths, segment_warnings = segment_test( + None, back_video, output_path, Config(config_path) + ) + print(segment_paths, segment_warnings) + + return segment_paths, segment_warnings diff --git a/mapping_cli/maneuvers/__init__.py b/mapping_cli/maneuvers/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/mapping_cli/maneuvers/face_verification.py b/mapping_cli/maneuvers/face_verification.py new file mode 100644 index 0000000..7ef435e --- /dev/null +++ b/mapping_cli/maneuvers/face_verification.py @@ -0,0 +1,369 @@ +import logging +import os + +import cognitive_face as CF +import cv2 +from tqdm import tqdm + +from mapping_cli.maneuvers.maneuver import Maneuver + +site_config = None + + +class Person: + def __init__( + self, + vid_file, + base_url, + subscription_key, + calib_frame_period, + test_frame_period, + recog_confidence_threshold, + video_acceptance_threshold, + debug=False, + rep=None, + ): + SUBSCRIPTION_KEY = subscription_key + BASE_URL = base_url + + CF.BaseUrl.set(BASE_URL) + CF.Key.set(SUBSCRIPTION_KEY) + + self.vid_file = vid_file + + self.person_group_id = "test-persons" + self.create_group(self.person_group_id) + self.person_id = None + self.debug = debug + self.calib_frame_period = calib_frame_period + self.test_frame_period = test_frame_period + self.recog_confidence_threshold = recog_confidence_threshold + self.video_acceptance_threshold = video_acceptance_threshold + if vid_file: + self.add_video(vid_file) + + self.rep = rep + + def create_group(self, person_group_id): + exists = False + for p in CF.person_group.lists(): + if p["personGroupId"] == person_group_id: + exists = True + + if not exists: + CF.person_group.create(self.person_group_id) + + def add_face(self, im_file, persist=False): + # Persist if limit exceeded + # Ret val: 0 if success, 1 if no face, 2 if server issue + try: + if not self.person_id: + create_response = CF.person.create(self.person_group_id, "x") + self.person_id = create_response["personId"] + detect_result = CF.face.detect(im_file) + if len(detect_result) < 1: + if self.debug: + logging.info("No face detected!") + return 1, None + # idx = 0 + idx = -1 + min_left = 10000 + for i, r in enumerate(detect_result): + if r["faceRectangle"]["left"] < min_left: + idx = i + min_left = r["faceRectangle"]["left"] + # if > 1, take left most + target_str = "{},{},{},{}".format( + detect_result[idx]["faceRectangle"]["left"], + detect_result[idx]["faceRectangle"]["top"], + detect_result[idx]["faceRectangle"]["width"], + detect_result[idx]["faceRectangle"]["height"], + ) + CF.person.add_face( + im_file, self.person_group_id, self.person_id, target_face=target_str + ) + return 0, detect_result[idx]["faceRectangle"] + except Exception as e: + if self.debug: + logging.info(e) + if persist: + self.add_face(im_file, persist) + else: + return 2, None + return 2, None + + def add_video(self, vid_file): + vc = cv2.VideoCapture(vid_file) + vid_length = int(vc.get(cv2.CAP_PROP_FRAME_COUNT)) + self.reg_face = None + + if self.debug: + pbar = tqdm(total=vid_length) + pbar.set_description("Adding calib video") + + i = 0 + while True and i < vid_length: + ret, frame = vc.read() + if not ret: + return + if i % self.calib_frame_period == 0: + # TODO: Add face detection check before adding a new face + im_file = "temp_{}.jpg".format(i) + + # crop the driver because there could be people at the back who might peep + h, w, c = frame.shape + frame = frame[:, : int(0.7 * w), :] + cv2.imwrite(im_file, frame) + ret, result = self.add_face(im_file, True) + + if result is not None: + cv2.rectangle( + frame, + (result["left"], result["top"]), + ( + result["left"] + result["width"], + result["top"] + result["height"], + ), + (0, 255, 0), + 3, + ) + cv2.imwrite(im_file, frame) + + if ret == 0: + self.reg_face = frame + if os.path.exists(im_file): + os.remove(im_file) + i += 1 + if self.debug: + pbar.update(1) + + if self.debug: + pbar.close() + + def verify_face(self, im_file, persist=False): + """ + Not detected = 2 + Detected & Verified = 1 + Detected but not verified = 0 + """ + + person_id = self.person_id + + try: + detect_result = CF.face.detect(im_file) + + if len(detect_result) < 1: + return 2, -1, None + + idx = -1 + min_left = 10000 + for i, r in enumerate(detect_result): + if r["faceRectangle"]["left"] < min_left: + idx = i + min_left = r["faceRectangle"]["left"] + + face_id = detect_result[idx]["faceId"] + face_rect = detect_result[idx]["faceRectangle"] + + verify_result = CF.face.verify( + face_id, person_group_id=self.person_group_id, person_id=person_id + ) + + if ( + verify_result["isIdentical"] + and float(verify_result["confidence"]) > self.recog_confidence_threshold + ): + return 1, float(verify_result["confidence"]), face_rect + else: + return 0, float(verify_result["confidence"]), face_rect + + except Exception as e: + if self.debug: + logging.info(e) + if persist: + return self.verify_face(im_file, persist) + else: + return 2, -1, None + + def verify_video(self, vid_file, person_id=None): + self.ver_face_true = None + self.ver_face_true_conf = 0 + + self.ver_face_false = None + self.ver_face_false_conf = 1 + + self.video_log = {} + + vc = cv2.VideoCapture(vid_file) + vid_length = int(vc.get(cv2.CAP_PROP_FRAME_COUNT)) + if self.debug: + pbar = tqdm(total=vid_length) + pbar.set_description("Verifying test video") + verify_count = 0 + total_count = 0 + frame_no = 0 + + while True and frame_no < vid_length: + ret, frame = vc.read() + if not ret: + break + if frame_no % self.test_frame_period == 0: + im_file = "V_temp_{}.jpg".format(frame_no) + + # crop the driver because there could be people at the back who might peep + h, w, c = frame.shape + frame = frame[:, : int(0.7 * w), :] + cv2.imwrite(im_file, frame) + + verify_result, verify_confidence, face_rect = self.verify_face( + im_file, False + ) + + if face_rect is not None: + cv2.rectangle( + frame, + (face_rect["left"], face_rect["top"]), + ( + face_rect["left"] + face_rect["width"], + face_rect["top"] + face_rect["height"], + ), + (0, 255, 0), + 3, + ) + cv2.imwrite(im_file, frame) + + self.video_log[frame_no] = [verify_result, verify_confidence] + + if verify_result == 1: + if ( + verify_confidence > self.ver_face_true_conf + ): # get the most confident face + self.ver_face_true = frame + self.ver_face_true_conf = verify_confidence + verify_count += 1 + total_count += 1 + elif verify_result == 0: + if ( + verify_confidence < self.ver_face_false_conf + ): # get the least confident face + self.ver_face_false = frame + self.ver_face_false_conf = verify_confidence + total_count += 1 + + if os.path.exists(im_file): + os.remove(im_file) + frame_no += 1 + if self.debug: + pbar.update(1) + + if self.debug: + pbar.close() + logging.info( + "{} faces verified out of {}".format(verify_count, total_count) + ) + + self.rep.add_report("face_verify_log", self.video_log) + + if verify_count > self.video_acceptance_threshold * total_count: + self.verified = True + return True + else: + self.verified = False + return False + + def save_faces(self, out_folder): + if self.reg_face is not None: + cv2.imwrite( + os.path.join(out_folder, "face_registration.png"), self.reg_face + ) + if self.verified: + cv2.imwrite( + os.path.join(out_folder, "face_validated.png"), self.ver_face_true + ) + elif not self.verified and self.ver_face_false is not None: + cv2.imwrite( + os.path.join(out_folder, "face_validated.png"), self.ver_face_false + ) + + +def verify_two_videos( + vid_file_1, + vid_file_2, + out_folder, + base_url, + subscription_key, + calib_frame_period, + test_frame_period, + recog_confidence_threshold, + video_acceptance_threshold, + debug=True, + rep=None, +): + person = Person( + vid_file_1, + base_url, + subscription_key, + calib_frame_period, + test_frame_period, + recog_confidence_threshold, + video_acceptance_threshold, + debug=debug, + rep=rep, + ) + verified = person.verify_video(vid_file_2) + person.save_faces(out_folder) + return verified + + +def main( + calib_file, + test_file, + out_folder, + base_url, + subscription_key, + calib_frame_period, + test_frame_period, + recog_confidence_threshold, + video_acceptance_threshold, + rep, +): + if verify_two_videos( + calib_file, + test_file, + out_folder, + base_url, + subscription_key, + calib_frame_period, + test_frame_period, + recog_confidence_threshold, + video_acceptance_threshold, + debug=True, + rep=rep, + ): + out_str = "Face Verification: Pass" + if rep: + rep.add_report("face_verify", "Pass") + logging.info(out_str) + return True + else: + out_str = "Face Verification: Fail" + if rep: + rep.add_report("face_verify", "Fail") + logging.info(out_str) + return False + + +class FaceVerification(Maneuver): + def run(self): + return main( + self.inputs["calib_video"], + self.inputs["fpath"], + self.out_folder, + self.config["base_url"], + self.config["subscription_key"], + self.config["calib_frame_period"], + self.config["test_frame_period"], + self.config["recog_confidence_threshold"], + self.config["video_acceptance_threshold"], + rep=self.report, + ) diff --git a/mapping_cli/maneuvers/forward_eight.py b/mapping_cli/maneuvers/forward_eight.py new file mode 100644 index 0000000..ea6e05f --- /dev/null +++ b/mapping_cli/maneuvers/forward_eight.py @@ -0,0 +1,67 @@ +from decord import VideoReader + +from mapping_cli.maneuvers.maneuver import Maneuver +from mapping_cli.utils import detect_marker + + +def get_marker_from_frame(frame, marker_list, marker_dict): + markers = detect_marker(frame, marker_dict) + if len(markers) > 0: + permuted_markers = list( + filter( + lambda marker: marker in marker_list, + list(sorted([int(i) for i in markers])), + ) + ) + else: + return None + return permuted_markers + + +class ForwardEight(Maneuver): + def run(self) -> None: + vid = VideoReader(self.inputs["back_video"]) + frames = range(0, len(vid), self.config["skip_frames"]) + marker_list = self.config["marker_list"] + verification_sequence = self.config["marker_order"] + markers_detected = [] + + for i in frames: + vid.seek_accurate(i) + frame = vid.next().asnumpy() + permuted_markers = get_marker_from_frame( + frame, marker_list, self.config["marker_dict"] + ) + if permuted_markers is not None: + if isinstance(permuted_markers, (list, tuple)): + val_markers = [] + for permuted_marker in permuted_markers: + if permuted_marker not in markers_detected: + val_markers.append(permuted_marker) + + if len(val_markers) > 0: + markers_detected += val_markers + + elif permuted_markers != markers_detected[-1]: + markers_detected += list(permuted_markers) + sequence_verified = False + print("Detected: ", markers_detected) + + verified_sequence = None + + for sequence_verification in verification_sequence: + str_seq = "".join([str(i) for i in sequence_verification]) + iter_seq_verification = iter(str_seq) + str_markers = "".join([str(i) for i in markers_detected]) + res = all( + next((ele for ele in iter_seq_verification if ele == chr), None) + is not None + for chr in list(str_markers) + ) + if res and len(markers_detected) >= len(sequence_verification) - 1: + sequence_verified = True + verified_sequence = sequence_verification + break + + print("Verified: ", sequence_verified) + return (sequence_verified, markers_detected), {} diff --git a/mapping_cli/maneuvers/gaze.py b/mapping_cli/maneuvers/gaze.py new file mode 100644 index 0000000..8807c11 --- /dev/null +++ b/mapping_cli/maneuvers/gaze.py @@ -0,0 +1,401 @@ +import csv +import logging +import math +import os +import time + +import cv2 +import ffmpeg +import numpy as np +import pandas as pd + +from mapping_cli.maneuvers.maneuver import Maneuver +from mapping_cli.utils import euclidean_distance_batch + + +def get_eucledian_distances( + left_gaze_angle_centroid, + right_gaze_angle_centroid, + centre_gaze_angle_centroid, + test_gaze_angle=[0, 0], +): + eu_dist_left = math.sqrt( + (test_gaze_angle[0] - left_gaze_angle_centroid) ** 2 + + (test_gaze_angle[1] - left_gaze_angle_centroid) ** 2 + ) + eu_dist_right = math.sqrt( + (test_gaze_angle[0] - right_gaze_angle_centroid) ** 2 + + (test_gaze_angle[1] - right_gaze_angle_centroid) ** 2 + ) + eu_dist_centre = math.sqrt( + (test_gaze_angle[0] - centre_gaze_angle_centroid) ** 2 + + (test_gaze_angle[1] - centre_gaze_angle_centroid) ** 2 + ) + eu_dist_arr = [eu_dist_left, eu_dist_right, eu_dist_centre] + return eu_dist_arr + + +def start_calib( + calib_vid_path: str, + output_path: str, + name: str, + face_landmark_exe_path: str, + left_gaze_angle_centroid, + right_gaze_angle_centroid, + centre_gaze_angle_centroid, +): + start_time = time.time() + run_openface( + calib_vid_path, output_path, f"{name}_calib.csv", face_landmark_exe_path + ) + logging.info("Finished recording calibration video...") + ( + left_gaze_angle_centroid, + centre_gaze_angle_centroid, + right_gaze_angle_centroid, + ) = kmeans_clustering( + left_gaze_angle_centroid, + right_gaze_angle_centroid, + centre_gaze_angle_centroid, + os.path.join(output_path, f"{name}_calib.csv"), + calib_vid_path, + ) + logging.info("calib_time = " + str(time.time() - start_time)) + return ( + left_gaze_angle_centroid, + centre_gaze_angle_centroid, + right_gaze_angle_centroid, + ) + + +def run_openface( + input_vid_path, saveto_folder, output_filename, face_landmark_exe_path: str +): + logging.info("OpenFace: Processing video...") + call_string = "{} -f {} -out_dir {} -of {}".format( + face_landmark_exe_path, input_vid_path, saveto_folder, output_filename + ) + logging.info(call_string) + # print + # subprocess.Popen(call_string, cwd=working_dir, shell=True) + os.system(call_string) + # logging.info(x) + + +def kmeans_clustering( + left_gaze_angle_centroid, + right_gaze_angle_centroid, + centre_gaze_angle_centroid, + csvfile, + calib_vid, +): + # get array in form [[gaze_x0, gaze_y0], [gaze_x1, gaze_y1]] + video = cv2.VideoCapture(calib_vid) + total_frames = int(video.get(cv2.CAP_PROP_FRAME_COUNT)) + video_frame_width = int(video.get(cv2.CAP_PROP_FRAME_WIDTH)) + f = open(csvfile, "r") + reader = csv.reader(f) + next(reader) + gaze_angles = [] + row_count = 0 + for row in reader: + row_count += 1 + if float(row[299]) > float(video_frame_width / 2): + # print("not_appending:condition1: ", row[0], row[1], row[299]) + continue + if float(row[11]) == 0.0 and float(row[12]) == 0: + continue + if row_count > 1 and not (len(gaze_angles) == 0): + if row[0] == prev_row[0]: + if float(row[299]) > float(prev_row[299]): + # print("not_appending:condition2: ", row[0], row[1], row[299]) + prev_row = row + continue + # print("appending", row[11], row[12]) + gaze_angles.append([float(row[11]), float(row[12])]) + prev_row = row + gaze_angles = np.array(gaze_angles) + # logging.info(gaze_angles.shape, total_frames) + if gaze_angles.shape[0] > (int(total_frames * 3 / 4)): + # if gaze_angles.shape[0] > 0: + dim = gaze_angles.shape[-1] # find the dimensionality of given points + k = 3 + indices = np.random.choice(gaze_angles.shape[0], k, replace=False) + centroids_curr = np.array( + [gaze_angles[i] for i in indices] + ) # randomly select any data points from the input file as current centroids + centroids_old = np.zeros(centroids_curr.shape) + error = euclidean_distance_batch(centroids_curr, centroids_old) + cumulative_error = np.sum([error[i][i] for i in range(k)]) + # Iterate until the error between centroids_old and centroids_curr converges + while not (cumulative_error == 0): + # assign cluster + distance_array = euclidean_distance_batch(gaze_angles, centroids_curr) + cluster_array = np.argmin(distance_array, axis=1) + # find new centroid + centroids_old = centroids_curr + for i in range(k): + cluster_i = np.array( + [ + gaze_angles[j] + for j in range(len(cluster_array)) + if cluster_array[j] == i + ] + ) + centroid_i = np.mean(cluster_i, axis=0) + if i == 0: + temp_centroids_curr = np.array([centroid_i]) + else: + temp_centroids_curr = np.append( + temp_centroids_curr, [centroid_i], axis=0 + ) + centroids_curr = temp_centroids_curr + # find error + error = euclidean_distance_batch(centroids_curr, centroids_old) + cumulative_error = np.sum([error[i][i] for i in range(k)]) + list_centroids_curr = list(centroids_curr) + sorted_coord_centroids = sorted( + list_centroids_curr, key=lambda list_centroids_curr: list_centroids_curr[0] + ) + right_gaze_angle_centroid = ( + sorted_coord_centroids[0][0], + sorted_coord_centroids[0][1], + ) + centre_gaze_angle_centroid = ( + sorted_coord_centroids[1][0], + sorted_coord_centroids[1][1], + ) + left_gaze_angle_centroid = ( + sorted_coord_centroids[2][0], + sorted_coord_centroids[2][1], + ) + print("Changing: ", left_gaze_angle_centroid) + logging.info("\n\n\n\n") + logging.info( + f"{left_gaze_angle_centroid}, {centre_gaze_angle_centroid}, {right_gaze_angle_centroid}" + ) + logging.info("\n\n\n\n") + return ( + left_gaze_angle_centroid, + right_gaze_angle_centroid, + centre_gaze_angle_centroid, + ) + + +def classify_gaze( + vid_path, + gazeangles_csv_path, + gazeclassified_csv_path, + gazeclassified_vid_path, + left_threshold, + right_threshold, + centre_threshold, + maneuver, + output_folder, + rep, +): + # classify gaze by reading from csv generated by run_openface() and overlay this info on the video generated by run_openface() + direction_map = {"0": "left", "1": "right", "2": "centre"} + cap = cv2.VideoCapture(vid_path) + width = int(cap.get(3)) # float + height = int(cap.get(4)) + logging.info(gazeclassified_vid_path) + fourcc = cv2.VideoWriter_fourcc(*"XVID") + out = cv2.VideoWriter(gazeclassified_vid_path, fourcc, 25, (width, height)) + output_csv = open(gazeclassified_csv_path, mode="w") + csv_writer = csv.writer(output_csv) + csv_writer.writerow(["frame_no", "gaze_x", "gaze_y", "classified direction"]) + df = pd.read_csv(gazeangles_csv_path,) + frame_count = 0 + right_count = 0 + left_count = 0 + centre_count = 0 + ret = True + while ret: + ret, frame = cap.read() + if frame is None: + break + found_face = False + if frame_count <= len(df): + frame_count += 1 + # print("Frame count: ", frame_count) + try: + if frame_count == 5: + cv2.imwrite(os.path.join(output_folder, "face.jpg"), frame) + try: + curr_gaze_angle = df.loc[ + df["frame"] == frame_count, + ["gaze_angle_x", "gaze_angle_y", "face_id"], + ] + except: + curr_gaze_angle = df.loc[ + df["frame"] == frame_count, + [" gaze_angle_x", " gaze_angle_y", " face_id"], + ] + # print("Gaze Angle: ", curr_gaze_angle) + curr_gaze_angle = curr_gaze_angle.values.tolist() + # print("Gaze Angle: ", curr_gaze_angle) + for i in curr_gaze_angle: + found_face = True + if not (float(i[0]) == 0.0) and not (float(i[1]) == 0.0): + print("gaze: ", i) + found_face = True + break + curr_gaze_angle = i + except Exception as e: + print("Exception: ", e) + # exit(0) + cv2.putText( + frame, + "No Face Found", + (int(frame.shape[1] / 2), frame.shape[0] - 50), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (255, 255, 255), + 2, + ) + out.write(frame) + continue + if found_face == False: + cv2.putText( + frame, + "No Face Found", + (int(frame.shape[1] / 2), frame.shape[0] - 50), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (255, 255, 255), + 2, + ) + out.write(frame) + continue + else: + curr_gaze_angle[1] = 0.0 # added + eu_dist_arr = get_eucledian_distances( + curr_gaze_angle[0], curr_gaze_angle[1], curr_gaze_angle[2] + ) + looking_dir = direction_map[str(eu_dist_arr.index(min(eu_dist_arr)))] + print( + f"curr_gaze_angle: {curr_gaze_angle} looking direction: {looking_dir}" + ) + cv2.rectangle( + frame, + (10, frame.shape[0] - 100), + (10 + 300, frame.shape[0] - 10), + (0, 0, 0), + -1, + ) + cv2.putText( + frame, + "gaze_x: %.3f" % (curr_gaze_angle[0]), + (15, frame.shape[0] - 60), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (255, 255, 255), + 1, + ) + cv2.putText( + frame, + "gaze_y: %.3f" % (curr_gaze_angle[1]), + (15, frame.shape[0] - 30), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (255, 255, 255), + 1, + ) + cv2.putText( + frame, + "gaze_dir: " + looking_dir, + (int(frame.shape[1] / 2), frame.shape[0] - 50), + cv2.FONT_HERSHEY_SIMPLEX, + 1, + (0, 0, 255), + 2, + ) + if looking_dir == "right": + right_count += 1 + elif looking_dir == "left": + left_count += 1 + elif looking_dir == "centre": + centre_count += 1 + if frame_count == 50: + cv2.imwrite(os.path.join(output_folder, "face.jpg"), frame) + # disp_frame = cv2.resize(frame, (1080, 720)) + # cv2.imwrite('face.jpg', frame) + out.write(frame) + csv_writer.writerow( + [frame_count, curr_gaze_angle[0], curr_gaze_angle[1], looking_dir] + ) + # cv2.imshow("disp", disp_frame) + # if cv2.waitKey(1) & 0xFF == ord('q'): + # break + else: + break + # if rep: + stats = {"right": right_count, "left": left_count, "centre": centre_count} + decision = "Fail" + if ( + right_count > right_threshold + and left_count > left_threshold + and centre_count > centre_threshold + ): + decision = "Pass" + rep.add_report("{}_gaze".format(maneuver), {"stats": stats, "decision": decision}) + with open(gazeclassified_csv_path.replace(".csv", ".txt"), mode="w") as f: + logging.info("\n\n") + logging.info("right_count: " + str(right_count)) + logging.info("left_count: " + str(left_count)) + logging.info("centre_count: " + str()) + logging.info("right_count: " + str(right_count)) + logging.info("left_count: " + str(left_count)) + logging.info("centre_count: " + str(centre_count)) + logging.info("\n\n") + cap.release() + cv2.destroyAllWindows() + # call_string = "ffmpeg -i {} -c:v copy -c:a copy -y {}".format(gazeclassified_vid_path, os.path.splitext(gazeclassified_vid_path)[0] + ".mp4") + ffmpeg.input(gazeclassified_vid_path).output( + os.path.splitext(gazeclassified_vid_path)[0] + ".mp4" + ).run() + # os.system(call_string) + return decision, stats + + +class Gaze(Maneuver): + def run(self) -> None: + left_gaze_angle_centroid = (0.6074, 0.0) + right_gaze_angle_centroid = (-0.0820, 0.0) + centre_gaze_angle_centroid = (0.1472, 0.0) + ( + left_gaze_angle_centroid, + right_gaze_angle_centroid, + centre_gaze_angle_centroid, + ) = start_calib( + self.inputs["calib_video"], + self.out_folder, + "gaze", + self.config["face_landmark_exe_path"], + left_gaze_angle_centroid, + right_gaze_angle_centroid, + centre_gaze_angle_centroid, + ) + print( + f"Left: {left_gaze_angle_centroid} Right: {right_gaze_angle_centroid} Centre: {centre_gaze_angle_centroid}" + ) + run_openface( + self.inputs["fpath"], + self.out_folder, + "front_gaze", + self.config["face_landmark_exe_path"], + ) + decision, stats = classify_gaze( + self.inputs["fpath"], + os.path.join(self.out_folder, "front_gaze.csv"), + os.path.join(self.out_folder, "front_gaze_output.csv"), + os.path.join(self.out_folder, "front_gaze.avi"), + self.config["left_threshold"], + self.config["right_threshold"], + self.config["centre_threshold"], + self.config["maneuver"], + self.out_folder, + self.report, + ) + return decision, stats diff --git a/mapping_cli/maneuvers/incline.py b/mapping_cli/maneuvers/incline.py new file mode 100644 index 0000000..96f0140 --- /dev/null +++ b/mapping_cli/maneuvers/incline.py @@ -0,0 +1,345 @@ +import json +import os + +import matplotlib.patches as mpatches +import matplotlib.pyplot as plt +import numpy as np + +from mapping_cli.halts import get_halts +from mapping_cli.maneuvers.maneuver import Maneuver +from mapping_cli.utils import (aggregate_direction, debug_directions_visualize, + generate_trajectory, get_marker_coord, + majority_vote_smoothen, plot_line, + smoothen_trajectory, yml_parser) + + +def select_traj_pts_by_direction(tx, tz, directions, label): + selected_pts = [] + for x, z, d in zip(tx, tz, directions): + if d == label: # remember, this label is reverse + selected_pts.append([x, z]) + return selected_pts + + +def incline_rollback_logic(tx, tz, directions, max_rollback_allowed_metres, rep): + selected_pts = select_traj_pts_by_direction( + tx, tz, directions, 0 + ) # remember, this label is reverse + max_di = 0 + for p in selected_pts: + for p_d in selected_pts: + di = (p[0] - p_d[0]) ** 2 + (p[1] - p_d[1]) ** 2 + if max_di < di: + max_di = di + incline_rollback_decision = "Fail" + if selected_pts == []: + incline_rollback_decision = "Fail" + elif selected_pts != [] and np.sqrt(max_di) <= max_rollback_allowed_metres: + incline_rollback_decision = "Pass" + print("(incline) Rollback (m): {}".format(np.sqrt(max_di))) + print("(incline) Rollback Decision: {}".format(incline_rollback_decision)) + rep.add_report( + "incline_rollback", + {"value_in_m": np.sqrt(max_di), "decision": incline_rollback_decision}, + ) + return incline_rollback_decision + + +def post_process_direction_vector(stats, directions): + # aggregate seq + uniq_vals = [] + pivot_val = directions[0] + pivot_len = 1 + i = 1 + while i < len(directions): + while i < len(directions) and pivot_val == directions[i]: + pivot_len += 1 + i += 1 + if i < len(directions): + uniq_vals.append([pivot_val, pivot_len]) + pivot_val = directions[i] + pivot_len = 1 + i += 1 + if i == len(directions): + uniq_vals.append([pivot_val, pivot_len]) + break + + # delete from seq + if len(uniq_vals) >= 3: + del_elem = [] + for idx in range(0, len(uniq_vals) - 2): + # R = 0, H = -1, F = 1 + if ( + uniq_vals[idx + 0][0] == 0 + and uniq_vals[idx + 1][0] == -1 + and uniq_vals[idx + 2][0] == 0 + ): # RHR --> RR + stats["R"] -= 1 + stats["H"] -= 1 + uniq_vals[idx][1] += uniq_vals[idx + 1][1] + uniq_vals[idx][1] += uniq_vals[idx + 2][1] + del_elem.append(idx + 1) + del_elem.append(idx + 2) + for idx in del_elem[::-1]: + del uniq_vals[idx] + + # recreate seq + directions = [] + for a, b in uniq_vals: + directions += [a] * b # pythonic? + + return stats, directions + + +def plot_legend(): + red_patch = mpatches.Patch(color="red", label="Reverse") + blue_patch = mpatches.Patch(color="blue", label="Forward") + black_patch = mpatches.Patch(color="black", label="Halt") + plt.legend( + handles=[blue_patch, red_patch, black_patch], + prop={"size": 10}, + loc="upper right", + ) + + +def plot_markers(markers): + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + plt.scatter( + marker_coord[0], marker_coord[1], c=[[0, 0, 0]], marker=".", s=100.0 + ) + + +def plot_limits(x_limits, y_limits): + plt.xlim(x_limits) + plt.ylim(y_limits) + plt.gca().set_aspect("equal", "box") + + +def get_line(line_info_json_f): + line_info = None + with open(line_info_json_f) as f: + line_info = json.load(f) + l_stop = np.polyfit(line_info["pts"]["tx"], line_info["pts"]["tz"], 1) + return l_stop + + +def percentage_of_points_behind_line(selected_pts, l_stop, markers_side): + behind_lines_decision = [] + for x, z in selected_pts: + val_halt = -1 * l_stop[0] * x + z - l_stop[1] + if np.sign(val_halt) != np.sign(markers_side): + behind_lines_decision.append(1) + else: + behind_lines_decision.append(0) + + percentage_obey = 0.0 + for x in behind_lines_decision: + if x == 1: + percentage_obey += 1 + if behind_lines_decision == []: + percentage_obey = 0.0 + else: + percentage_obey /= len(behind_lines_decision) + return behind_lines_decision, percentage_obey + + +def get_side_of_marker_from_line(markers, l_stop): + markers_side = [] + # In Standard form: -m*x + y - c = 0 + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + # placing marker coords into the line + # to determine their side + if ( + -1 * l_stop[0] * marker_coord[0] + marker_coord[1] - l_stop[1] > 0 + ): # Might want to have a looser criterion + markers_side.append(1) + else: + markers_side.append(-1) + if not all(map(lambda x: x == markers_side[0], markers_side)): + raise Exception("All markers for this maneuver should be behind the line!") + else: + return markers_side[0] + + +def plot_traj( + tx, + tz, + directions, + line_json, + x_limits, + y_limits, + rep, + behind_lines_obey_threshold, + markers, + save_image_name=None, +): + if markers is not None: + plot_markers(markers) + + ax = plt.gca() + + line_info_json_f = line_json + l_stop = get_line(line_info_json_f) + line_viz_x_lim = x_limits + + plot_line(plt, l_stop, line_viz_x_lim, "green") + + markers_side = get_side_of_marker_from_line(markers, l_stop) + + selected_pts = select_traj_pts_by_direction(tx, tz, directions, -1) ## halt is -1 + + behind_lines_decision, percentage_obey = percentage_of_points_behind_line( + selected_pts, l_stop, markers_side + ) + + decision = "Fail" + if percentage_obey >= behind_lines_obey_threshold: + decision = "Pass" + rep.add_report( + "{}_behind_line".format("incline"), + {"value": percentage_obey, "decision": decision}, + ) + print( + "({}) Halt Behind Line %: {} Decision: {}".format( + "incline", percentage_obey, decision + ) + ) + + color_f = ["red", "blue", "black"] + size_f = [25, 25, 40] + len_plot = min(min(len(tx), len(tz)), len(directions)) + plt.scatter( + tx[:len_plot], + tz[:len_plot], + c=[color_f[directions[i]] for i in range(0, len_plot)], + s=[size_f[directions[i]] for i in range(0, len_plot)], + marker=".", + ) + + plot_legend() + plot_limits(x_limits, y_limits) + + # if markers is not None: + # rotate_plot(ax, markers, manu) + + plt.savefig(save_image_name, dpi=200) + plt.close() + + +class Incline(Maneuver): + def run(self) -> None: + map_path = self.config.get_config_value("map_file_path") + if not os.path.exists(map_path): + map_path = os.path.join(self.inputs["cwd"], map_path) + + calib_path = self.config["calibration_file_path"] + if not os.path.exists(calib_path): + calib_path = os.path.join(self.inputs["cwd"], calib_path) + + traj = generate_trajectory( + self.inputs["back_video"], + "incline", + map_path, + self.out_folder, + calib_path, + self.config["size_marker"], + self.config["aruco_test_exe"], + self.inputs["cwd"], + ) + + _, tx, ty, tz, camera_matrices = traj + direction, stats = self.get_direction_stats( + traj, + self.config["rev_fwd_halt_segment_min_frame_len"], + self.config["min_frame_len"], + ) + + # smoothen trajectory + tx, ty, tz = smoothen_trajectory(tx, ty, tz, 25, 25, 2) + + # get stats + decision = incline_rollback_logic( + tx, tz, direction, self.config["max_rollback_allowed_metres"], self.report + ) + + # rollback + + line_path = self.config["line_file_path"] + if not os.path.exists(line_path): + line_path = os.path.join(self.inputs["cwd"], line_path) + + markers = yml_parser(map_path) + # plot + plot_traj( + tx, + tz, + direction, + line_path, + self.config["x_limits"], + self.config["y_limits"], + self.report, + self.config["behind_lines_obey_threshold"], + markers, + os.path.join(self.out_folder, "incline.png"), + ) + + return ( + (stats, decision), + ( + { + "back_video": self.inputs["back_video"], + "trajectory": os.path.join(self.out_folder, "incline.png"), + } + ), + ) + + def get_direction_stats( + self, traj, rev_fwd_halt_segment_min_frame_len, min_frame_len + ): + """ """ + f_ids, tx, ty, tz, camera_matrices = traj + directions = [1] + Xs = [] + for i in range(1, len(tx)): + translation = ( + np.array([tx[i], ty[i], tz[i], 1.0]).reshape(4, 1).astype(np.float64) + ) + X = camera_matrices[i - 1][:3, :3].dot( + translation[:3, 0] - camera_matrices[i - 1][:3, -1] + ) + # print(X) + direction = X[2] + + if [tx[i], ty[i], tz[i]] == [tx[i - 1], ty[i - 1], tz[i - 1]]: + directions.append(directions[-1]) + else: + if direction >= 0: + directions.append(1) + else: + directions.append(0) + + directions = directions[1:] + + # Get Forward-Reverse + directions = np.array(directions + [-1]) + + # Get halts + halt_info = get_halts(tx, ty, tz) + directions[halt_info == True] = -1 + directions = directions.tolist() + directions = majority_vote_smoothen( + directions, rev_fwd_halt_segment_min_frame_len + ) + + stats, directions = aggregate_direction(directions, min_frame_len) + stats, _ = aggregate_direction(directions, min_frame_len) + + stats, directions = post_process_direction_vector(stats, directions) + + # debug_directions_visualize(plt, tx, ty, tz, directions) + + return directions, stats diff --git a/mapping_cli/maneuvers/maneuver.py b/mapping_cli/maneuvers/maneuver.py new file mode 100644 index 0000000..05b59db --- /dev/null +++ b/mapping_cli/maneuvers/maneuver.py @@ -0,0 +1,41 @@ +import logging +import os + +from mapping_cli.config.config import Config +from mapping_cli.utils import Report + + +class Maneuver: + def __init__( + self, + inputs=None, + inertial_data=None, + config: Config = None, + out_folder: str = None, + ) -> None: + super().__init__() + self.inputs = inputs + self.inertial_data = inertial_data + self.config = config + self.out_folder = out_folder + + self.report = Report(os.path.join(self.out_folder, "report.txt")) + self.log(f"{self.__class__} Started") + + def run(self) -> None: + raise NotImplementedError + + def save(self): + raise NotImplementedError + + def log(self, *args, **kwargs): + if getattr(self, "logger", None) is not None: + logging.info(*args, **kwargs) + else: + logging.basicConfig( + filename=os.path.join(self.out_folder, f"hams_alt.log"), + filemode="w", + level=logging.INFO, + ) + logging.info(*args, **kwargs) + self.logger = {} diff --git a/mapping_cli/maneuvers/marker_sequence.py b/mapping_cli/maneuvers/marker_sequence.py new file mode 100644 index 0000000..42d9990 --- /dev/null +++ b/mapping_cli/maneuvers/marker_sequence.py @@ -0,0 +1,67 @@ +from decord import VideoReader + +from mapping_cli.maneuvers.maneuver import Maneuver +from mapping_cli.utils import detect_marker + + +def get_marker_from_frame(frame, marker_list, marker_dict): + markers = detect_marker(frame, marker_dict) + if len(markers) > 0: + permuted_markers = list( + filter( + lambda marker: marker in marker_list, + list(sorted([int(i) for i in markers])), + ) + ) + else: + return None + return permuted_markers + + +class MarkerSequence(Maneuver): + def run(self) -> None: + vid = VideoReader(self.inputs["back_video"]) + frames = range(0, len(vid), self.config["skip_frames"]) + marker_list = self.config["marker_list"] + verification_sequence = self.config["marker_order"] + markers_detected = [] + + for i in frames: + vid.seek_accurate(i) + frame = vid.next().asnumpy() + permuted_markers = get_marker_from_frame( + frame, marker_list, self.config["marker_dict"] + ) + if permuted_markers is not None: + if isinstance(permuted_markers, (list, tuple)): + val_markers = [] + for permuted_marker in permuted_markers: + if permuted_marker not in markers_detected: + val_markers.append(permuted_marker) + + if len(val_markers) > 0: + markers_detected += val_markers + + elif permuted_markers != markers_detected[-1]: + markers_detected += list(permuted_markers) + sequence_verified = False + print("Detected: ", markers_detected) + + verified_sequence = None + + for sequence_verification in verification_sequence: + str_seq = "".join([str(i) for i in sequence_verification]) + iter_seq_verification = iter(str_seq) + str_markers = "".join([str(i) for i in markers_detected]) + res = all( + next((ele for ele in iter_seq_verification if ele == chr), None) + is not None + for chr in list(str_markers) + ) + if res and len(markers_detected) >= len(sequence_verification) - 1: + sequence_verified = True + verified_sequence = sequence_verification + break + + print("Verified: ", sequence_verified) + return (sequence_verified, markers_detected), {} diff --git a/mapping_cli/maneuvers/pedestrian.py b/mapping_cli/maneuvers/pedestrian.py new file mode 100644 index 0000000..8071011 --- /dev/null +++ b/mapping_cli/maneuvers/pedestrian.py @@ -0,0 +1,460 @@ +import json +import math +import os + +import matplotlib.patches as mpatches +import matplotlib.pyplot as plt +import numpy as np +import shapely + +from mapping_cli.halts import get_halts +from mapping_cli.maneuvers.maneuver import Maneuver +from mapping_cli.utils import (aggregate_direction, debug_directions_visualize, + generate_trajectory, get_marker_coord, + majority_vote_smoothen, plot_line, + rotate_rectangle, + rotation_matrix_to_euler_angles, + smoothen_trajectory, yml_parser) + + +def get_side_of_marker_from_line(markers, l_stop): + markers_side = [] + # In Standard form: -m*x + y - c = 0 + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + # placing marker coords into the line + # to determine their side + if ( + -1 * l_stop[0] * marker_coord[0] + marker_coord[1] - l_stop[1] > 0 + ): # Might want to have a looser criterion + markers_side.append(1) + else: + markers_side.append(-1) + if not all(map(lambda x: x == markers_side[0], markers_side)): + raise Exception("All markers for this maneuver should be behind the line!") + else: + return markers_side[0] + + +def select_traj_points_by_time(tx, tz, segment_json, fps): + if not os.path.exists(segment_json): + raise Exception("segment_json not part of input arguments!") + with open(segment_json) as f: + seg_vals = json.load(f) + print("Seg vals: ", seg_vals.keys()) + frame_num_pedestrian_light_vid_start_abs = math.ceil( + seg_vals["pedestrian"]["start"][0] + ) + + frame_num_red_light_off_abs = fps + if frame_num_red_light_off_abs == -1: + print("WARNING!!!!! pedestrian Light hook is not implemented") + return list(zip(tx, tz)) + + if ( + frame_num_pedestrian_light_vid_start_abs > frame_num_red_light_off_abs + ): # my video somehow cut afterwards + return [] + + end_frame_red_light_check = ( + frame_num_red_light_off_abs - frame_num_pedestrian_light_vid_start_abs + ) + if end_frame_red_light_check > len(tx): + end_frame_red_light_check = len(tx) + return list(zip(tx[:end_frame_red_light_check], tz[:end_frame_red_light_check])) + + +def percentage_of_points_behind_line(selected_pts, l_stop, markers_side): + behind_lines_decision = [] + for x, z in selected_pts: + val_halt = -1 * l_stop[0] * x + z - l_stop[1] + if np.sign(val_halt) != np.sign(markers_side): + behind_lines_decision.append(1) + else: + behind_lines_decision.append(0) + + percentage_obey = 0.0 + for x in behind_lines_decision: + if x == 1: + percentage_obey += 1 + if behind_lines_decision == []: + percentage_obey = 0.0 + else: + percentage_obey /= len(behind_lines_decision) + return behind_lines_decision, percentage_obey + + +def post_process_direction_vector(stats, directions): + # aggregate seq + uniq_vals = [] + pivot_val = directions[0] + pivot_len = 1 + i = 1 + while i < len(directions): + while i < len(directions) and pivot_val == directions[i]: + pivot_len += 1 + i += 1 + if i < len(directions): + uniq_vals.append([pivot_val, pivot_len]) + pivot_val = directions[i] + pivot_len = 1 + i += 1 + if i == len(directions): + uniq_vals.append([pivot_val, pivot_len]) + break + + # delete from seq + if len(uniq_vals) >= 3: + del_elem = [] + for idx in range(0, len(uniq_vals) - 2): + # R = 0, H = -1, F = 1 + if ( + uniq_vals[idx + 0][0] == 0 + and uniq_vals[idx + 1][0] == -1 + and uniq_vals[idx + 2][0] == 0 + ): # RHR --> RR + stats["R"] -= 1 + stats["H"] -= 1 + uniq_vals[idx][1] += uniq_vals[idx + 1][1] + uniq_vals[idx][1] += uniq_vals[idx + 2][1] + del_elem.append(idx + 1) + del_elem.append(idx + 2) + for idx in del_elem[::-1]: + del uniq_vals[idx] + + # recreate seq + directions = [] + for a, b in uniq_vals: + directions += [a] * b # pythonic? + + return stats, directions + + +def get_line(line_info_json_f): + line_info = None + with open(line_info_json_f) as f: + line_info = json.load(f) + l_stop = np.polyfit(line_info["pts"]["tx"], line_info["pts"]["tz"], 1) + return l_stop + + +def get_maneuver_box(box_path): + val = None + with open(box_path) as f: + d = json.load(f) + val = d["box"] + val = list( + zip(*shapely.geometry.Polygon(val).minimum_rotated_rectangle.exterior.coords.xy) + ) + return np.array(val) + + +def get_car_box(cam_x, cam_y, camera_matrix, manu, car_dims, site_config): + car_top_left = [cam_x - car_dims["x_offset"], cam_y - car_dims["z_offset"]] + car_top_right = [car_top_left[0] + car_dims["width"], car_top_left[1]] + car_bottom_right = [ + car_top_left[0] + car_dims["width"], + car_top_left[1] + car_dims["length"], + ] + car_bottom_left = [car_top_left[0], car_top_left[1] + car_dims["length"]] + + angle = rotation_matrix_to_euler_angles(camera_matrix)[1] + + if site_config.maneuvers_config["viz"][manu]["markers_vertical"]: + angle *= -1.0 + + car_pts = np.array([car_top_left, car_top_right, car_bottom_right, car_bottom_left]) + car_rotated_pts = rotate_rectangle(car_pts, np.array([cam_x, cam_y]), angle) + + return car_rotated_pts + + +def plot_traj( + tx, + tz, + manu, + car_dims, + line_json, + config, + maneuver: Maneuver, + markers=None, + save_image_name=None, + camera_matrices=None, + max_iou_idx=None, + segment_json=None, +): + if markers is not None: + plot_markers(markers) + + ax = plt.gca() + + # poly = mpatches.Polygon(box, alpha=1, fill=False, edgecolor='black') + # ax.add_patch(poly) + + # Plot Car + if max_iou_idx is not None: + position = max_iou_idx + car_pts = get_car_box( + tx[position], + tz[position], + camera_matrices[position][:3, :3], + manu, + car_dims, + ) + car_patch = mpatches.Polygon(car_pts, alpha=1, fill=False, edgecolor="red") + ax.add_patch(car_patch) + + line_info_json_f = line_json + + if config["line_type"] == "stop": + l_stop = get_line(line_info_json_f) + + stop_ped_offset = config["stop_ped_line_dist"] + l_ped = [l_stop[0], l_stop[1] + stop_ped_offset] + elif config["line_type"] == "ped": + l_ped = get_line(line_info_json_f) + stop_line_offset = config["stop_ped_line_dist"] + l_stop = [l_ped[0], l_ped[1] + stop_line_offset] + + line_viz_x_lim = config["xlim"] + + plot_line(plt, l_stop, line_viz_x_lim, "blue") + plot_line(plt, l_ped, line_viz_x_lim, "yellow") + + ## markers are always ahead of stop line, + ## ped line is gonna be ahead of stop line + ## X X * X $ | + ## * $ | + ## * $ | + ## * $ | + ## Legend: Marker : X, StopLine : |, PedLine1 : $, Pedline2 : * + ## Define markers_side_ped such that both PL1 and PL2 are satisfied + ## + ## As we can assume that ped_line will always be ahead of stop_line, + ## every point on ped_line will be sign(stop_line(point)) == sign(stop_line(marker)) + ## + ## Let's find a point on stop_line, that will always be behind ped_line + ## Use that point to find the side and then invert it + markers_side = get_side_of_marker_from_line(markers, l_stop) # say this is 1 + point_on_stop_line = (0, l_stop[0] * 0 + l_stop[1]) + markers_side_ped = -1 * np.sign( + -1 * l_ped[0] * point_on_stop_line[0] + point_on_stop_line[1] - l_ped[1] + ) + + selected_pts = select_traj_points_by_time(tx, tz, segment_json, config["fps"]) + + behind_lines_decision_ped, percentage_obey = percentage_of_points_behind_line( + selected_pts, l_ped, markers_side_ped + ) + behind_lines_decision_stop, percentage_stop = percentage_of_points_behind_line( + selected_pts, l_stop, markers_side + ) + label_decision = [] + for i in range(len(behind_lines_decision_ped)): + if behind_lines_decision_ped[i] == 1 and behind_lines_decision_stop[i] == 1: + label_decision.append(0) + elif behind_lines_decision_ped[i] == 1 and behind_lines_decision_stop[i] == 0: + label_decision.append(1) + elif behind_lines_decision_ped[i] == 0 and behind_lines_decision_stop[i] == 0: + label_decision.append(2) + else: + raise Exception("Error: Ped Line is not ahead of Stop Line in track!") + + obey_decision = "Fail" + if percentage_obey >= config["behind_lines_obey_threshold"]: + obey_decision = "Pass" + + stop_decision = "Fail" + if percentage_stop > config["behind_lines_stop_threshold"]: + stop_decision = "Pass" + + maneuver.report.add_report("pedestrianLight_obey_decision", obey_decision) + maneuver.report.add_report("pedestrianLight_stop_decision", stop_decision) + maneuver.report.add_report( + "pedestrianLight_outcome", + { + "percentage_behind_both_lines": percentage_obey, + "percentage_behind_stop_line": percentage_stop, + }, + ) + + print( + "({}) Halt Behind Stop Line %: {} Decision: {}".format( + manu, percentage_stop, stop_decision + ) + ) + print( + "({}) Halt Behind Ped Line %: {} Decision: {}".format( + manu, percentage_obey, obey_decision + ) + ) + + color_f = ["green", "yellow", "brown"] + size_f = [25, 25, 40] + len_plot = len(selected_pts) + print(len(selected_pts), len(label_decision)) + plt.scatter( + [x[0] for x in selected_pts], + [x[1] for x in selected_pts], + c=[color_f[label_decision[i]] for i in range(0, len_plot)], + s=[size_f[label_decision[i]] for i in range(0, len_plot)], + marker="*", + ) + + plot_legend() + plot_limits(config["xlim"], config["ylim"]) + + # if markers is not None: + # rotate_plot(ax, markers, manu) + + plt.savefig(save_image_name, dpi=200) + plt.close() + return obey_decision and stop_decision + + +def plot_legend(): + red_patch = mpatches.Patch(color="red", label="Reverse") + blue_patch = mpatches.Patch(color="blue", label="Forward") + black_patch = mpatches.Patch(color="black", label="Halt") + plt.legend( + handles=[blue_patch, red_patch, black_patch], + prop={"size": 10}, + loc="upper right", + ) + + +def plot_markers(markers): + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + plt.scatter( + marker_coord[0], marker_coord[1], c=[[0, 0, 0]], marker=".", s=100.0 + ) + + +def plot_limits(x_limits, y_limits): + plt.xlim(x_limits) + plt.ylim(y_limits) + plt.gca().set_aspect("equal", "box") + + +class Pedestrian(Maneuver): + def run(self): + map_path = self.config.get_config_value("map_file_path") + if not os.path.exists(map_path): + map_path = os.path.join(self.inputs["cwd"], map_path) + + calib_path = self.config["calibration_file_path"] + if not os.path.exists(calib_path): + calib_path = os.path.join(self.inputs["cwd"], calib_path) + + try: + traj = generate_trajectory( + self.inputs["back_video"], + self.config.get_config_value("maneuver"), + map_path, + self.out_folder, + calib_path, + self.config["size_marker"], + self.config["aruco_test_exe"], + self.inputs["cwd"], + ) + except Exception as e: + raise e + + _, tx, ty, tz, camera_matrices = traj + tx, ty, tz = smoothen_trajectory(tx, ty, tz, 25, 25, 2) + markers = yml_parser(map_path) + + # get_direction_stats + direction, stats = self.get_direction_stats( + tx, + ty, + tz, + camera_matrices, + self.config["rev_fwd_halt_segment_min_frame_len"], + self.config["min_frame_len"], + ) + + # fwd rev halts + halts = get_halts(tx, ty, tz) + stats["halts"] = halts + + line_path = self.config["line_file_path"] + if not os.path.exists(line_path): + line_path = os.path.join(self.inputs["cwd"], line_path) + + decision = plot_traj( + tx, + tz, + "pedestrian", + self.config["car_dims"], + line_path, + self.config, + self, + markers, + segment_json=os.path.join(self.out_folder, "manu_json_seg_int.json"), + save_image_name=os.path.join(self.out_folder, "pedestrian.png"), + ) + + decision = decision == "Pass" + + return ( + (decision, stats), + { # TODO: Add pass/fail + "trajectory": f"{os.path.join(self.out_folder, 'pedestrian.png')}" + }, + ) + + def get_direction_stats( + self, + tx, + ty, + tz, + camera_matrices, + rev_fwd_halt_segment_min_frame_len, + min_frame_len, + ): + """ """ + directions = [1] + Xs = [] + for i in range(1, len(tx)): + translation = ( + np.array([tx[i], ty[i], tz[i], 1.0]).reshape(4, 1).astype(np.float64) + ) + X = camera_matrices[i - 1][:3, :3].dot( + translation[:3, 0] - camera_matrices[i - 1][:3, -1] + ) + # print(X) + direction = X[2] + + if [tx[i], ty[i], tz[i]] == [tx[i - 1], ty[i - 1], tz[i - 1]]: + directions.append(directions[-1]) + else: + if direction >= 0: + directions.append(1) + else: + directions.append(0) + + directions = directions[1:] + + # Get Forward-Reverse + directions = np.array(directions + [-1]) + + # Get halts + halt_info = get_halts(tx, ty, tz) + directions[halt_info == True] = -1 + directions = directions.tolist() + directions = majority_vote_smoothen( + directions, rev_fwd_halt_segment_min_frame_len + ) + + stats, directions = aggregate_direction(directions, min_frame_len) + stats, _ = aggregate_direction(directions, min_frame_len) + + stats, directions = post_process_direction_vector(stats, directions) + + debug_directions_visualize(plt, tx, ty, tz, directions) + + return directions, stats diff --git a/mapping_cli/maneuvers/perp.py b/mapping_cli/maneuvers/perp.py new file mode 100644 index 0000000..e6b3a26 --- /dev/null +++ b/mapping_cli/maneuvers/perp.py @@ -0,0 +1,558 @@ +import itertools +import json +import os + +import matplotlib.patches as mpatches +import matplotlib.pyplot as plt +import numpy as np +import shapely +from matplotlib.collections import PathCollection +from matplotlib.transforms import Affine2D + +from mapping_cli.halts import get_halts +from mapping_cli.locator import get_locations, read_aruco_traj_file +from mapping_cli.maneuvers.maneuver import Maneuver +from mapping_cli.utils import (get_graph_box, get_marker_coord, + get_plt_rotation_from_markers, + majority_vote_smoothen, rotate_rectangle, + rotation_matrix_to_euler_angles, + smoothen_trajectory, yml_parser) + + +class PERP(Maneuver): + def run(self) -> None: + # 1 Get trajectory + + traj = get_locations( + self.inputs["back_video"], + "perp", + self.config["map_file_path"], + self.out_folder, + self.config["calib_file_path"], + self.config["marker_size"], + self.config["aruco_test_exe"], + cwd=self.inputs["cwd"], + plot=False, + return_read=True, + ) + if traj is False: + raise Exception("Bad trajectory generated at perp. No results") + + _, tx, ty, tz, camera_matrices = traj + if len(traj) < 30: + if len(tx) > 99: + tx, ty, tz = smoothen_trajectory(tx, ty, tz, 199, 299, 1) + else: + try: + tx, ty, tz = smoothen_trajectory( + tx, ty, tz, int(99 / 3), int(199 // 3), 2 + ) + except: + window = len(tx) - 3 if len(tx) % 2 == 0 else len(tx) - 4 + tx, ty, tz = smoothen_trajectory(tx, ty, tz, window, window, 2) + else: + raise Exception("Insufficient trajectory - check segmentation output") + + map_file = self.config["map_file_path"] + if not os.path.exists(map_file): + map_file = os.path.join(self.inputs["cwd"], map_file) + + markers = yml_parser(map_file) + # 2 In box + box_path = self.config["box_file_path"] + if not os.path.exists(box_path): + box_path = os.path.join(self.inputs["cwd"], box_path) + fig, ax = plt.subplots() + is_inside, max_iou, max_iou_idx = is_car_inside( + tx, + ty, + tz, + camera_matrices, + self.config["car_dims"], + markers_vertical=self.config["markers_vertical"], + box_overlap_threshold=self.config["box_overlap_threshold"], + box_path=box_path, + ) + box = get_maneuver_box(box_path) + box_patch = mpatches.Polygon(box, alpha=1, fill=False, edgecolor="red") + ax.add_patch(box_patch) + if max_iou_idx is not None: + print("Adding car patch") + position = max_iou_idx + car_pts = get_car_box( + tx[position], + tz[position], + camera_matrices[position][:3, :3], + self.config["car_dims"], + self.config["is_vertical"], + ) + car_patch = mpatches.Polygon( + car_pts, alpha=1, fill=False, edgecolor="green", label="vehicle" + ) + ax.add_patch(car_patch) + + # plt.savefig('car.png') + # fig.s + _, stats = get_direction_stats( + tx, + ty, + tz, + camera_matrices, + self.config["rev_fwd_halt_segment_min_frame_len"], + self.config["min_frame_len"], + markers, + self.out_folder, + ) + # 3 Standard path check + + plt.clf() + _, std_path_decision = check_standard_path( + tx, + ty, + tz, + camera_matrices, + self.config["car_dims"], + self.config["is_vertical"], + max_iou_idx, + markers, + self.config, + "perp", + self.out_folder, + self.inputs["cwd"], + True, + ) + print( + f"is inside: {is_inside}, stats: {stats}, std path decision: {std_path_decision}" + ) + return ( + (is_inside and std_path_decision, stats), + { + "back_video": self.inputs["back_video"], + "perp_standard_path": f"{os.path.join(self.out_folder, 'perp_trajectory.png')}", + "perp_trajectory_path": f"{os.path.join(self.out_folder, 'standard_path_debug_perp.png')}", + }, + ) + + +def post_process_direction_vector(stats, directions): + # aggregate seq + uniq_vals = [] + pivot_val = directions[0] + pivot_len = 1 + i = 1 + while i < len(directions): + while i < len(directions) and pivot_val == directions[i]: + pivot_len += 1 + i += 1 + if i < len(directions): + uniq_vals.append([pivot_val, pivot_len]) + pivot_val = directions[i] + pivot_len = 1 + i += 1 + if i == len(directions): + uniq_vals.append([pivot_val, pivot_len]) + break + + # delete from seq + if len(uniq_vals) >= 3: + del_elem = [] + for idx in range(0, len(uniq_vals) - 2): + # R = 0, H = -1, F = 1 + if ( + uniq_vals[idx + 0][0] == 0 + and uniq_vals[idx + 1][0] == -1 + and uniq_vals[idx + 2][0] == 0 + ): # RHR --> RR + stats["R"] -= 1 + stats["H"] -= 1 + uniq_vals[idx][1] += uniq_vals[idx + 1][1] + uniq_vals[idx][1] += uniq_vals[idx + 2][1] + del_elem.append(idx + 1) + del_elem.append(idx + 2) + for idx in del_elem[::-1]: + del uniq_vals[idx] + + # recreate seq + directions = [] + for a, b in uniq_vals: + directions += [a] * b # pythonic? + + return stats, directions + + +def get_direction_stats( + tx, + ty, + tz, + camera_matrices, + rev_fwd_halt_segment_min_frame_len, + min_frame_len, + markers, + out_folder, + maneuver=None, + args=None, +): + """ """ + + directions = [1] + Xs = [] + for i in range(1, len(tx)): + translation = ( + np.array([tx[i], ty[i], tz[i], 1.0]).reshape(4, 1).astype(np.float64) + ) + X = camera_matrices[i - 1][:3, :3].dot( + translation[:3, 0] - camera_matrices[i - 1][:3, -1] + ) + # print(X) + direction = X[2] + + if [tx[i], ty[i], tz[i]] == [tx[i - 1], ty[i - 1], tz[i - 1]]: + directions.append(directions[-1]) + else: + if direction >= 0: + directions.append(1) + else: + directions.append(0) + + directions = directions[1:] + + # Get Forward-Reverse + directions = np.array(directions + [-1]) + + # Get halts + halt_info = get_halts(tx, ty, tz) + directions[halt_info == True] = -1 + directions = directions.tolist() + directions = majority_vote_smoothen(directions, rev_fwd_halt_segment_min_frame_len) + + stats, directions = aggregate_direction(directions, min_frame_len) + stats, _ = aggregate_direction(directions, min_frame_len) + + stats, directions = post_process_direction_vector(stats, directions) + + # if args and args.debug: + debug_directions_visualize(tx, ty, tz, directions, markers, out_folder) + + return directions, stats + + +def plot_markers(markers): + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + plt.scatter( + marker_coord[0], marker_coord[1], c=[[0, 0, 0]], marker=".", s=100.0 + ) + plt.annotate(str(key), (marker_coord[0], marker_coord[1])) + + +def plot_legend(): + red_patch = mpatches.Patch(color="red", label="Reverse") + blue_patch = mpatches.Patch(color="blue", label="Forward") + black_patch = mpatches.Patch(color="black", label="Halt") + plt.legend( + handles=[blue_patch, red_patch, black_patch], + prop={"size": 10}, + loc="upper right", + ) + + +def debug_directions_visualize(tx, ty, tz, directions, markers, out_folder): + """ """ + colors = ["red", "blue", "black"] + le = min(min(len(tx) - 1, len(tz) - 1), len(directions) - 1) + plt.scatter( + [tx[i] for i in range(0, le)], + [tz[i] for i in range(0, le)], + c=[colors[directions[i]] for i in range(0, le)], + ) + # plt.show() + plot_markers(markers) + plot_limits() + plot_legend() + plt.savefig(os.path.join(out_folder, "perp_trajectory.png")) + plt.clf() + + +def get_maneuver_box(box_path): + val = None + with open(box_path) as f: + d = json.load(f) + val = d["c"] + val = list( + zip(*shapely.geometry.Polygon(val).minimum_rotated_rectangle.exterior.coords.xy) + ) + return np.array(val) + + +def get_car_box(cam_x, cam_y, camera_matrix, car_dims, markers_vertical): + car_top_left = [cam_x - car_dims["x_offset"], cam_y - car_dims["z_offset"]] + car_top_right = [car_top_left[0] + car_dims["width"], car_top_left[1]] + car_bottom_right = [ + car_top_left[0] + car_dims["width"], + car_top_left[1] + car_dims["length"], + ] + car_bottom_left = [car_top_left[0], car_top_left[1] + car_dims["length"]] + + angle = rotation_matrix_to_euler_angles(camera_matrix)[1] + + if markers_vertical: + angle *= -1.0 + + car_pts = np.array([car_top_left, car_top_right, car_bottom_right, car_bottom_left]) + car_rotated_pts = rotate_rectangle(car_pts, np.array([cam_x, cam_y]), angle) + + return car_rotated_pts + + +def get_car_box_iou(camera_coords, camera_matrix, car_dims, markers_vertical, box_path): + box = get_maneuver_box(box_path) + car_pts = get_car_box( + camera_coords[0], camera_coords[1], camera_matrix, car_dims, markers_vertical + ) + + box_poly = shapely.geometry.Polygon(box.tolist()) + car_poly = shapely.geometry.Polygon(car_pts.tolist()) + + iou = car_poly.intersection(box_poly).area / car_poly.union(box_poly).area + norm_iou = car_poly.area / box_poly.area + + return iou / norm_iou + + +def aggregate_direction(direction_vector, halt_len): + """ + Input: vector containing 'forward', 'reverse' + Output: # of forwards, # of reverse + """ + direction_count = {"F": 0, "R": 0, "H": 0} + # Group consecutive directions + grouped_class = [ + list(l) + for _, l in itertools.groupby(enumerate(direction_vector), key=lambda x: x[1]) + ] + new_direction_vector = [] + for c_idx, c in enumerate(grouped_class): + if c[0][1] == 0: + direction_count["R"] += 1 + new_direction_vector += [0 for i in range(len(c))] + elif c[0][1] == 1: + direction_count["F"] += 1 + new_direction_vector += [1 for i in range(len(c))] + elif c[0][1] == -1: + if len(c) > halt_len: + direction_count["H"] += 1 + new_direction_vector += [-1 for i in range(len(c))] + else: + # Start segment + if c_idx > 0: + prev_direction = grouped_class[c_idx - 1][0][1] + else: + next_direction = grouped_class[c_idx + 1][0][1] + prev_direction = next_direction + + # End segment + if c_idx < len(grouped_class) - 1: + next_direction = grouped_class[c_idx + 1][0][1] + else: + next_direction = prev_direction + + new_direction_vector += [prev_direction for i in range(len(c) // 2)] + new_direction_vector += [next_direction for i in range(len(c) // 2)] + + return direction_count, new_direction_vector + + +def is_car_inside( + tx, + ty, + tz, + camera_matrices, + car_dims, + markers_vertical, + box_overlap_threshold, + box_path, +): + assert len(tx) == len(ty) == len(tz) + max_iou = -1 + max_iou_idx = 0 + counts = 0 + for i in range(len(tx)): + iou = get_car_box_iou( + [tx[i], tz[i]], + camera_matrices[i][:3, :3], + car_dims, + markers_vertical, + box_path, + ) + if iou > box_overlap_threshold: + counts += 1 + if iou > max_iou: + max_iou = iou + max_iou_idx = i + max_iou = float(np.round(max_iou, 2)) + if max_iou > box_overlap_threshold: + return True, max_iou, max_iou_idx + else: + return False, max_iou, max_iou_idx + + +def rotate_plot(ax, config, origin_marker, axis_marker, is_vertical): + rot_deg = get_plt_rotation_from_markers(origin_marker, axis_marker, is_vertical) + if rot_deg == 0.0: + return + r = Affine2D().rotate_deg(rot_deg) + + for x in ax.images + ax.lines + ax.collections + ax.patches: + trans = x.get_transform() + x.set_transform(r + trans) + if isinstance(x, PathCollection): + transoff = x.get_offset_transform() + x._transOffset = r + transoff + + if config["invert_y"]: + ax.invert_yaxis() + if config["invert_x"]: + ax.invert_xaxis() + + +def plot_limits(): + plot_limits = {"xlim": [-20, 20], "ylim": [-20, 20]} + plt.xlim(*plot_limits["xlim"]) + plt.ylim(*plot_limits["ylim"]) + plt.gca().set_aspect("equal", "box") + + +def plot_car(ax, max_iou_idx, tx, ty, tz, camera_matrices, car_dims, is_vertical): + if max_iou_idx is not None: + print("Adding car patch") + position = max_iou_idx + car_pts = get_car_box( + tx[position], + tz[position], + camera_matrices[position][:3, :3], + car_dims, + is_vertical, + ) + car_patch = mpatches.Polygon( + car_pts, alpha=1, fill=False, edgecolor="green", label="vehicle" + ) + ax.add_patch(car_patch) + + +def debug_plot( + tx, + ty, + tz, + camera_matrices, + car_dims, + is_vertical, + max_iou_idx, + manu, + markers, + box, + labels, + out_f, + config, + origin_marker, + axis_marker, +): + plt.scatter(tx[:-1], tz[:-1], marker=".", c=[[1, 0, 0]]) + + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + plt.scatter( + marker_coord[0], marker_coord[1], c=[[0, 0, 0]], marker=".", s=100.0 + ) + + ax = plt.gca() + + for label in labels: + poly = mpatches.Polygon(box[label], alpha=1, fill=False, edgecolor="blue") + ax.add_patch(poly) + + plot_limits() + plot_car(ax, max_iou_idx, tx, ty, tz, camera_matrices, car_dims, is_vertical) + + rotate_plot(ax, config, origin_marker, axis_marker, is_vertical) + plt.savefig(os.path.join(out_f, "standard_path_debug_{}.png".format(manu))) + + +def check_standard_path( + tx, + ty, + tz, + camera_matrices, + car_dims, + is_vertical, + max_iou_idx, + markers, + config, + manu, + out_f, + cwd, + debug=False, +): + plt.cla() + plt.clf() + labels = config["standard_node_visit_order"] + box_path = config["box_file_path"] + if not os.path.exists(box_path): + box_path = os.path.join(cwd, box_path) + boxes = [ + shapely.geometry.Polygon(get_graph_box(box_path, label).tolist()) + for label in labels + ] + values = [] + for i in range(len(tx)): + pos = shapely.geometry.Point([tx[i], tz[i]]) + for j, box in enumerate(boxes): + if box.contains(pos): + # print(labels[j]) + values.append(labels[j]) + break + + if len(values) == 0: + return None, False + manu_order = [values[0]] + for i in range(1, len(values) - 1): + if values[i] != values[i - 1]: + manu_order.append(values[i]) + + origin_marker = markers["aruco_bc_markers"][config["origin_marker"]] + axis_marker = markers["aruco_bc_markers"][config["axis_marker"]] + + with open(box_path) as f: + debug_plot( + tx, + ty, + tz, + camera_matrices, + car_dims, + is_vertical, + max_iou_idx, + manu, + markers, + json.load(f), + labels, + out_f, + config, + origin_marker, + axis_marker, + ) + + # is the subsequence 'BC' in the visit order of the sequence or not? + # C is the box we want to be in, B is the box ahead (from + # where we want the car to reverse) + # Acceptable: + # ABC, ABCB, ACBC, ACBCB etc + # Non Acceptable: + # ABAC etc + manu_order = "".join(manu_order) + substrings = config["acceptable_order_substrings"] + print("Manu order: ", manu_order, " substrings: ", substrings) + for substring in substrings: + if substring in manu_order: + return manu_order, True + return manu_order, False diff --git a/mapping_cli/maneuvers/rpp.py b/mapping_cli/maneuvers/rpp.py new file mode 100644 index 0000000..f96b10e --- /dev/null +++ b/mapping_cli/maneuvers/rpp.py @@ -0,0 +1,463 @@ +import itertools +import json +import os + +import matplotlib.patches as mpatches +import matplotlib.pyplot as plt +import numpy as np +import shapely +from matplotlib.collections import PathCollection +from matplotlib.transforms import Affine2D + +from mapping_cli.halts import get_halts +from mapping_cli.locator import get_locations, read_aruco_traj_file +from mapping_cli.maneuvers.maneuver import Maneuver +from mapping_cli.utils import (get_graph_box, get_marker_coord, + get_plt_rotation_from_markers, + majority_vote_smoothen, rotate_rectangle, + rotation_matrix_to_euler_angles, + smoothen_trajectory, yml_parser) + + +class RPP(Maneuver): + def run(self) -> None: + # 1 Get trajectory + traj = get_locations( + self.inputs["back_video"], + "rpp", + self.config["map_file_path"], + self.out_folder, + self.config["calib_file_path"], + self.config["marker_size"], + self.config["aruco_test_exe"], + cwd=self.inputs["cwd"], + plot=False, + return_read=True, + annotate=False, + ) + if traj is False: + return False + + _, tx, ty, tz, camera_matrices = traj + + if len(tx) > 99: + tx, ty, tz = smoothen_trajectory(tx, ty, tz, 99, 199, 2) + else: + tx, ty, tz = smoothen_trajectory(tx, ty, tz, 33, 199 // 3, 2) + + map_file = self.config["map_file_path"] + if not os.path.exists(map_file): + map_file = os.path.join(self.inputs["cwd"], map_file) + markers = yml_parser(map_file) + # 2 In box + box_path = self.config["box_file_path"] + if not os.path.exists(box_path): + box_path = os.path.join(self.inputs["cwd"], box_path) + is_inside, max_iou, _ = is_car_inside( + tx, + ty, + tz, + camera_matrices, + self.config["car_dims"], + markers_vertical=self.config["markers_vertical"], + box_overlap_threshold=self.config["box_overlap_threshold"], + box_path=box_path, + ) + + _, stats = get_direction_stats( + tx, + ty, + tz, + camera_matrices, + self.config["rev_fwd_halt_segment_min_frame_len"], + self.config["min_frame_len"], + self.out_folder, + ) + # 3 Standard path check + + _, std_path_decision = check_standard_path( + tx, + ty, + tz, + camera_matrices, + markers, + self.config, + "rpp", + self.out_folder, + self.inputs["cwd"], + True, + ) + print( + f"is inside: {is_inside}, stats: {stats}, std path decision: {std_path_decision}" + ) + return ( + (is_inside and std_path_decision, stats), + { + "back_video": self.inputs["back_video"], + "rpp_standard_path": f"{os.path.join(self.out_folder, 'rpp_trajectory.png')}", + "rpp_trajectory_path": f"{os.path.join(self.out_folder, 'standard_path_debug_rpp.png')}", + }, + ) + + +def post_process_direction_vector(stats, directions): + # aggregate seq + uniq_vals = [] + pivot_val = directions[0] + pivot_len = 1 + i = 1 + while i < len(directions): + while i < len(directions) and pivot_val == directions[i]: + pivot_len += 1 + i += 1 + if i < len(directions): + uniq_vals.append([pivot_val, pivot_len]) + pivot_val = directions[i] + pivot_len = 1 + i += 1 + if i == len(directions): + uniq_vals.append([pivot_val, pivot_len]) + break + + # delete from seq + if len(uniq_vals) >= 3: + del_elem = [] + for idx in range(0, len(uniq_vals) - 2): + # R = 0, H = -1, F = 1 + if ( + uniq_vals[idx + 0][0] == 0 + and uniq_vals[idx + 1][0] == -1 + and uniq_vals[idx + 2][0] == 0 + ): # RHR --> RR + stats["R"] -= 1 + stats["H"] -= 1 + uniq_vals[idx][1] += uniq_vals[idx + 1][1] + uniq_vals[idx][1] += uniq_vals[idx + 2][1] + del_elem.append(idx + 1) + del_elem.append(idx + 2) + for idx in del_elem[::-1]: + del uniq_vals[idx] + + # recreate seq + directions = [] + for a, b in uniq_vals: + directions += [a] * b # pythonic? + + return stats, directions + + +def get_direction_stats( + tx, + ty, + tz, + camera_matrices, + rev_fwd_halt_segment_min_frame_len, + min_frame_len, + out_folder, + maneuver=None, + args=None, +): + """ """ + + directions = [1] + Xs = [] + for i in range(1, len(tx)): + translation = ( + np.array([tx[i], ty[i], tz[i], 1.0]).reshape(4, 1).astype(np.float64) + ) + X = camera_matrices[i - 1][:3, :3].dot( + translation[:3, 0] - camera_matrices[i - 1][:3, -1] + ) + # print(X) + direction = X[2] + + if [tx[i], ty[i], tz[i]] == [tx[i - 1], ty[i - 1], tz[i - 1]]: + directions.append(directions[-1]) + else: + if direction >= 0: + directions.append(1) + else: + directions.append(0) + + directions = directions[1:] + + # Get Forward-Reverse + directions = np.array(directions + [-1]) + + # Get halts + halt_info = get_halts(tx, ty, tz) + directions[halt_info == True] = -1 + directions = directions.tolist() + directions = majority_vote_smoothen(directions, rev_fwd_halt_segment_min_frame_len) + + stats, directions = aggregate_direction(directions, min_frame_len) + stats, _ = aggregate_direction(directions, min_frame_len) + + stats, directions = post_process_direction_vector(stats, directions) + + # if args and args.debug: + debug_directions_visualize(tx, ty, tz, directions, out_folder) + + return directions, stats + + +def debug_directions_visualize(tx, ty, tz, directions, out_folder): + """ """ + colors = ["red", "blue", "black"] + le = min(min(len(tx) - 1, len(tz) - 1), len(directions) - 1) + plt.scatter( + [tx[i] for i in range(0, le)], + [tz[i] for i in range(0, le)], + c=[colors[directions[i]] for i in range(0, le)], + ) + plt.show() + plt.savefig(os.path.join(out_folder, "rpp_trajectory.png")) + plt.clf() + + +def get_maneuver_box(box_path): + val = None + with open(box_path) as f: + d = json.load(f) + val = d["c"] + val = list( + zip(*shapely.geometry.Polygon(val).minimum_rotated_rectangle.exterior.coords.xy) + ) + return np.array(val) + + +def get_car_box(cam_x, cam_y, camera_matrix, car_dims, markers_vertical): + car_top_left = [cam_x - car_dims["x_offset"], cam_y - car_dims["z_offset"]] + car_top_right = [car_top_left[0] + car_dims["width"], car_top_left[1]] + car_bottom_right = [ + car_top_left[0] + car_dims["width"], + car_top_left[1] + car_dims["length"], + ] + car_bottom_left = [car_top_left[0], car_top_left[1] + car_dims["length"]] + + angle = rotation_matrix_to_euler_angles(camera_matrix)[1] + + if markers_vertical: + angle *= -1.0 + + car_pts = np.array([car_top_left, car_top_right, car_bottom_right, car_bottom_left]) + car_rotated_pts = rotate_rectangle(car_pts, np.array([cam_x, cam_y]), angle) + + return car_rotated_pts + + +def get_car_box_iou(camera_coords, camera_matrix, car_dims, markers_vertical, box_path): + box = get_maneuver_box(box_path) + car_pts = get_car_box( + camera_coords[0], camera_coords[1], camera_matrix, car_dims, markers_vertical + ) + + box_poly = shapely.geometry.Polygon(box.tolist()) + car_poly = shapely.geometry.Polygon(car_pts.tolist()) + + iou = car_poly.intersection(box_poly).area / car_poly.union(box_poly).area + norm_iou = car_poly.area / box_poly.area + + return iou / norm_iou + + +def aggregate_direction(direction_vector, halt_len): + """ + Input: vector containing 'forward', 'reverse' + Output: # of forwards, # of reverse + """ + direction_count = {"F": 0, "R": 0, "H": 0} + # Group consecutive directions + grouped_class = [ + list(l) + for _, l in itertools.groupby(enumerate(direction_vector), key=lambda x: x[1]) + ] + new_direction_vector = [] + for c_idx, c in enumerate(grouped_class): + if c[0][1] == 0: + direction_count["R"] += 1 + new_direction_vector += [0 for i in range(len(c))] + elif c[0][1] == 1: + direction_count["F"] += 1 + new_direction_vector += [1 for i in range(len(c))] + elif c[0][1] == -1: + if len(c) > halt_len: + direction_count["H"] += 1 + new_direction_vector += [-1 for i in range(len(c))] + else: + # Start segment + if c_idx > 0: + prev_direction = grouped_class[c_idx - 1][0][1] + else: + next_direction = grouped_class[c_idx + 1][0][1] + prev_direction = next_direction + + # End segment + if c_idx < len(grouped_class) - 1: + next_direction = grouped_class[c_idx + 1][0][1] + else: + next_direction = prev_direction + + new_direction_vector += [prev_direction for i in range(len(c) // 2)] + new_direction_vector += [next_direction for i in range(len(c) // 2)] + + return direction_count, new_direction_vector + + +def is_car_inside( + tx, + ty, + tz, + camera_matrices, + car_dims, + markers_vertical, + box_overlap_threshold, + box_path, +): + assert len(tx) == len(ty) == len(tz) + max_iou = -1 + max_iou_idx = 0 + counts = 0 + for i in range(len(tx)): + iou = get_car_box_iou( + [tx[i], tz[i]], + camera_matrices[i][:3, :3], + car_dims, + markers_vertical, + box_path, + ) + if iou > box_overlap_threshold: + counts += 1 + if iou > max_iou: + max_iou = iou + max_iou_idx = i + max_iou = float(np.round(max_iou, 2)) + if max_iou > box_overlap_threshold: + return True, max_iou, max_iou_idx + else: + return False, max_iou, max_iou_idx + + +def rotate_plot(ax, config, origin_marker, axis_marker, is_vertical): + rot_deg = get_plt_rotation_from_markers(origin_marker, axis_marker, is_vertical) + if rot_deg == 0.0: + return + r = Affine2D().rotate_deg(rot_deg) + + for x in ax.images + ax.lines + ax.collections + ax.patches: + trans = x.get_transform() + x.set_transform(r + trans) + if isinstance(x, PathCollection): + transoff = x.get_offset_transform() + x._transOffset = r + transoff + + if config["invert_y"]: + ax.invert_yaxis() + if config["invert_x"]: + ax.invert_xaxis() + + +def plot_limits(): + plot_limits = {"xlim": [-20, 20], "ylim": [-20, 20]} + plt.xlim(*plot_limits["xlim"]) + plt.ylim(*plot_limits["ylim"]) + plt.gca().set_aspect("equal", "box") + + +def debug_plot( + tx, + tz, + manu, + markers, + box, + labels, + out_f, + config, + origin_marker, + axis_marker, + is_vertical, +): + plt.scatter(tx[:-1], tz[:-1], marker=".", c=[[1, 0, 0]]) + + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + plt.scatter( + marker_coord[0], marker_coord[1], c=[[0, 0, 0]], marker=".", s=100.0 + ) + + ax = plt.gca() + + for label in labels: + poly = mpatches.Polygon(box[label], alpha=1, fill=False, edgecolor="blue") + ax.add_patch(poly) + + plot_limits() + + rotate_plot(ax, config, origin_marker, axis_marker, is_vertical) + plt.savefig(os.path.join(out_f, "standard_path_debug_{}.png".format(manu))) + + +def check_standard_path( + tx, ty, tz, camera_matrices, markers, config, manu, out_f, cwd, debug=False +): + # tx, ty, tz = smoothen_trajectory(tx, ty, tz, 99, 199, 2) + assert len(tx) > 0 + plt.cla() + plt.clf() + labels = config["standard_node_visit_order"] + box_path = config["box_file_path"] + if not os.path.exists(box_path): + box_path = os.path.join(cwd, box_path) + boxes = [ + shapely.geometry.Polygon(get_graph_box(box_path, label).tolist()) + for label in labels + ] + values = [] + for i in range(len(tx)): + pos = shapely.geometry.Point([tx[i], tz[i]]) + for j, box in enumerate(boxes): + if box.contains(pos): + # print(labels[j]) + values.append(labels[j]) + break + else: + print("Strange") + + assert len(tx) > 0 + manu_order = [values[0]] + for i in range(1, len(values) - 1): + if values[i] != values[i - 1]: + manu_order.append(values[i]) + + origin_marker = markers["aruco_bc_markers"][config["origin_marker"]] + axis_marker = markers["aruco_bc_markers"][config["axis_marker"]] + + with open(box_path) as f: + debug_plot( + tx, + tz, + manu, + markers, + json.load(f), + labels, + out_f, + config, + origin_marker, + axis_marker, + config["is_vertical"], + ) + + # is the subsequence 'BC' in the visit order of the sequence or not? + # C is the box we want to be in, B is the box ahead (from + # where we want the car to reverse) + # Acceptable: + # ABC, ABCB, ACBC, ACBCB etc + # Non Acceptable: + # ABAC etc + manu_order = "".join(manu_order) + substrings = config["acceptable_order_substrings"] + for substring in substrings: + if substring in manu_order: + return manu_order, True + return manu_order, False diff --git a/mapping_cli/maneuvers/seat_belt.py b/mapping_cli/maneuvers/seat_belt.py new file mode 100644 index 0000000..7970aaf --- /dev/null +++ b/mapping_cli/maneuvers/seat_belt.py @@ -0,0 +1,209 @@ +import os +import subprocess +from distutils.sysconfig import get_python_lib +from pathlib import Path +from typing import Dict + +import cv2 +import torch +import torchvision +from PIL import Image +from torchvision.models.detection.faster_rcnn import FastRCNNPredictor +from torchvision.models.detection.mask_rcnn import MaskRCNNPredictor +from torchvision.transforms import functional as F +from tqdm import tqdm + +from mapping_cli.config.config import Config +from mapping_cli.maneuvers.maneuver import Maneuver + +torchvision.ops.misc.ConvTranspose2d = torch.nn.ConvTranspose2d + +# if os.path.isfile(get_python_lib() + "/plateDetect"): +# BASE_DIR = get_python_lib() + "/plateDetect" +# else: +# BASE_DIR = os.path.dirname(__file__) + + +class SeatBelt(Maneuver): + def save(self): + return super().save() + + def run(self): + out_folder: str = self.out_folder + use_gpu = False + if "gpu" in str(self.config.get_config_value("device")) or "cuda" in str( + self.config.get_config_value("device") + ): + use_gpu = True + + exitcode, new_fpath = frame_dropper(self.inputs["fpath"], out_folder, use_gpu) + if exitcode != 0: + raise Exception(f"Check input video file path! Received code {exitcode}") + + device = torch.device(self.config.get_config_value("device")) + model = get_model_instance_segmentation(2) + BASE_DIR = Path(__file__).parent.parent + model.load_state_dict( + torch.load( + os.path.join(BASE_DIR, *self.config["model_path"]), + map_location=torch.device(self.config.get_config_value("device")), + ) + ) + model.to(device) + model.eval() + + input_file = new_fpath + output_file = os.path.join( + out_folder, os.path.splitext(os.path.split(input_file)[1])[0] + "_out.mp4" + ) + + vid_iter = cv2.VideoCapture(input_file) + W = int(vid_iter.get(cv2.CAP_PROP_FRAME_WIDTH)) + H = int(vid_iter.get(cv2.CAP_PROP_FRAME_HEIGHT)) + vid_length = int(vid_iter.get(cv2.CAP_PROP_FRAME_COUNT)) + + fourcc = cv2.VideoWriter_fourcc(*"mp4v") + skip_frames = self.config.get_config_value("skip_frames") + out_vid_iter = cv2.VideoWriter(output_file, fourcc, 25 / skip_frames, (W, H),) + + where = [] + num_seatbelt = 0 + image_false = None + image_true = None + + pbar = tqdm(total=vid_length) + pbar.set_description("Checking for seatbelt:") + idx = 0 + + while True: + ret, frame = vid_iter.read() + pbar.update(1) + if not ret: + break + + if idx % skip_frames == 0: + image = frame[int(2 * (H / 3)) : H, 0 : int(W / 2)] + image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + image = Image.fromarray(image) + image, target = get_transform()(image, {}) + + outputs = model([image.to(device)]) + boxes = outputs[0]["boxes"].to(torch.device("cpu")).detach().numpy() + confs = outputs[0]["scores"].to(torch.device("cpu")).detach().numpy() + good_boxes = boxes[ + confs + > self.config.get_config_value("classifier_confidence_threshold") + ] + + if len(good_boxes) > 0: + num_seatbelt += 1 + where.append(1) + image_true = frame + else: + where.append(0) + image_false = frame + + for i, box in enumerate(good_boxes): + # I cropped the image, so the box coords are w.r.t. to cropped. Convert to original + box[0] += 0 + box[1] += int(2 * (H / 3)) + box[2] += 0 + box[3] += int(2 * (H / 3)) + cv2.rectangle( + frame, + (int(box[0]), int(box[1])), + (int(box[2]), int(box[3])), + (255, 0, 0), + 2, + ) + out_vid_iter.write(frame) + idx += 1 + out_vid_iter.release() + vid_iter.release() + pbar.close() + stats = {} + stats["vid_length"] = vid_length // skip_frames + stats["num_seatbelt"] = num_seatbelt + yes = 0 + for x in where: + if x == 1: + yes += 1 + if where != []: + percentage_detections = (yes * 1.0) / len(where) + else: + percentage_detections = 0.0 + wearing_all_the_time = True + if percentage_detections < self.config.get_config_value("detection_percentage"): + wearing_all_the_time = False + if wearing_all_the_time and image_true is not None: + cv2.imwrite(os.path.join(out_folder, "seatbelt_image.jpg"), image_true) + elif not wearing_all_the_time and image_false is not None: + cv2.imwrite(os.path.join(out_folder, "seatbelt_image.jpg"), image_false) + + self.report.add_report( + "Seatbelt", f"{percentage_detections}, {wearing_all_the_time}, {stats}" + ) + return percentage_detections, wearing_all_the_time, stats + + +def frame_dropper(fpath: str, out_folder: str, gpu_id: bool = False): + new_fpath = os.path.join(os.path.split(fpath)[0], "seatbelt_temp.mp4") + + ffmpeg_exec = "ffmpeg.exe" if os.name == "nt" else "ffmeg" + + if gpu_id: + call_string = '{} -y -i {} -filter:v "setpts=1/25 * PTS" -an -b:v 4000K -vcodec h264_nvenc -gpu {} {}'.format( + ffmpeg_exec, fpath, int(gpu_id), new_fpath + ) + else: + call_string = '{} -y -i {} -filter:v "setpts=1/25 * PTS" -an -b:v 4000K "{}"'.format( + ffmpeg_exec, fpath, new_fpath + ) + my_env = os.environ.copy() + my_env["PATH"] = out_folder + ";" + my_env["PATH"] + + if os.name == "nt": + exitcode = subprocess.call(call_string, shell=True, cwd=out_folder, env=my_env) + elif os.name == "posix": + exitcode = subprocess.call(call_string, shell=True) + + return exitcode, new_fpath + + +def get_model_instance_segmentation(num_classes): + # load an instance segmentation model pre-trained pre-trained on COCO + model = torchvision.models.detection.maskrcnn_resnet50_fpn(pretrained=False) + # get number of input features for the classifier + in_features = model.roi_heads.box_predictor.cls_score.in_features + # replace the pre-trained head with a new one + model.roi_heads.box_predictor = FastRCNNPredictor(in_features, num_classes) + # now get the number of input features for the mask classifier + in_features_mask = model.roi_heads.mask_predictor.conv5_mask.in_channels + hidden_layer = 256 + # and replace the mask predictor with a new one + model.roi_heads.mask_predictor = MaskRCNNPredictor( + in_features_mask, hidden_layer, num_classes + ) + return model + + +def get_transform(): + transforms = [] + transforms.append(ToTensor()) + return Compose(transforms) + + +class Compose(object): + def __init__(self, transforms): + self.transforms = transforms + + def __call__(self, image, target): + for t in self.transforms: + image, target = t(image, target) + return image, target + + +class ToTensor(object): + def __call__(self, image, target): + image = F.to_tensor(image) + return image, target diff --git a/mapping_cli/maneuvers/traffic.py b/mapping_cli/maneuvers/traffic.py new file mode 100644 index 0000000..44d233e --- /dev/null +++ b/mapping_cli/maneuvers/traffic.py @@ -0,0 +1,460 @@ +import json +import math +import os + +import matplotlib.patches as mpatches +import matplotlib.pyplot as plt +import numpy as np +import shapely + +from mapping_cli.halts import get_halts +from mapping_cli.maneuvers.maneuver import Maneuver +from mapping_cli.utils import (aggregate_direction, debug_directions_visualize, + generate_trajectory, get_marker_coord, + majority_vote_smoothen, plot_line, + rotate_rectangle, + rotation_matrix_to_euler_angles, + smoothen_trajectory, yml_parser) + + +def get_side_of_marker_from_line(markers, l_stop): + markers_side = [] + # In Standard form: -m*x + y - c = 0 + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + # placing marker coords into the line + # to determine their side + if ( + -1 * l_stop[0] * marker_coord[0] + marker_coord[1] - l_stop[1] > 0 + ): # Might want to have a looser criterion + markers_side.append(1) + else: + markers_side.append(-1) + if not all(map(lambda x: x == markers_side[0], markers_side)): + raise Exception("All markers for this maneuver should be behind the line!") + else: + return markers_side[0] + + +def select_traj_points_by_time(tx, tz, segment_json, fps): + if not os.path.exists(segment_json): + raise Exception("segment_json not part of input arguments!") + with open(segment_json) as f: + seg_vals = json.load(f) + print("Seg vals: ", seg_vals.keys()) + frame_num_traffic_light_vid_start_abs = math.ceil(seg_vals["traffic"]["start"][0]) + + frame_num_red_light_off_abs = fps + if frame_num_red_light_off_abs == -1: + print("WARNING!!!!! traffic Light hook is not implemented") + return list(zip(tx, tz)) + + if ( + frame_num_traffic_light_vid_start_abs > frame_num_red_light_off_abs + ): # my video somehow cut afterwards + return [] + + end_frame_red_light_check = ( + frame_num_red_light_off_abs - frame_num_traffic_light_vid_start_abs + ) + if end_frame_red_light_check > len(tx): + end_frame_red_light_check = len(tx) + return list(zip(tx[:end_frame_red_light_check], tz[:end_frame_red_light_check])) + + +def percentage_of_points_behind_line(selected_pts, l_stop, markers_side): + behind_lines_decision = [] + for x, z in selected_pts: + val_halt = -1 * l_stop[0] * x + z - l_stop[1] + if np.sign(val_halt) != np.sign(markers_side): + behind_lines_decision.append(1) + else: + behind_lines_decision.append(0) + + percentage_obey = 0.0 + for x in behind_lines_decision: + if x == 1: + percentage_obey += 1 + if behind_lines_decision == []: + percentage_obey = 0.0 + else: + percentage_obey /= len(behind_lines_decision) + return behind_lines_decision, percentage_obey + + +def post_process_direction_vector(stats, directions): + # aggregate seq + uniq_vals = [] + pivot_val = directions[0] + pivot_len = 1 + i = 1 + while i < len(directions): + while i < len(directions) and pivot_val == directions[i]: + pivot_len += 1 + i += 1 + if i < len(directions): + uniq_vals.append([pivot_val, pivot_len]) + pivot_val = directions[i] + pivot_len = 1 + i += 1 + if i == len(directions): + uniq_vals.append([pivot_val, pivot_len]) + break + + # delete from seq + if len(uniq_vals) >= 3: + del_elem = [] + for idx in range(0, len(uniq_vals) - 2): + # R = 0, H = -1, F = 1 + if ( + uniq_vals[idx + 0][0] == 0 + and uniq_vals[idx + 1][0] == -1 + and uniq_vals[idx + 2][0] == 0 + ): # RHR --> RR + stats["R"] -= 1 + stats["H"] -= 1 + uniq_vals[idx][1] += uniq_vals[idx + 1][1] + uniq_vals[idx][1] += uniq_vals[idx + 2][1] + del_elem.append(idx + 1) + del_elem.append(idx + 2) + for idx in del_elem[::-1]: + del uniq_vals[idx] + + # recreate seq + directions = [] + for a, b in uniq_vals: + directions += [a] * b # pythonic? + + return stats, directions + + +def get_line(line_info_json_f): + line_info = None + with open(line_info_json_f) as f: + line_info = json.load(f) + l_stop = np.polyfit(line_info["pts"]["tx"], line_info["pts"]["tz"], 1) + return l_stop + + +def get_maneuver_box(box_path): + val = None + with open(box_path) as f: + d = json.load(f) + val = d["box"] + val = list( + zip(*shapely.geometry.Polygon(val).minimum_rotated_rectangle.exterior.coords.xy) + ) + return np.array(val) + + +def get_car_box(cam_x, cam_y, camera_matrix, manu, car_dims, site_config): + car_top_left = [cam_x - car_dims["x_offset"], cam_y - car_dims["z_offset"]] + car_top_right = [car_top_left[0] + car_dims["width"], car_top_left[1]] + car_bottom_right = [ + car_top_left[0] + car_dims["width"], + car_top_left[1] + car_dims["length"], + ] + car_bottom_left = [car_top_left[0], car_top_left[1] + car_dims["length"]] + + angle = rotation_matrix_to_euler_angles(camera_matrix)[1] + + if site_config.maneuvers_config["viz"][manu]["markers_vertical"]: + angle *= -1.0 + + car_pts = np.array([car_top_left, car_top_right, car_bottom_right, car_bottom_left]) + car_rotated_pts = rotate_rectangle(car_pts, np.array([cam_x, cam_y]), angle) + + return car_rotated_pts + + +def plot_traj( + tx, + tz, + manu, + car_dims, + line_json, + config, + maneuver: Maneuver, + markers=None, + save_image_name=None, + camera_matrices=None, + max_iou_idx=None, + segment_json=None, +): + if markers is not None: + plot_markers(markers) + + ax = plt.gca() + + # poly = mpatches.Polygon(box, alpha=1, fill=False, edgecolor='black') + # ax.add_patch(poly) + + # Plot Car + if max_iou_idx is not None: + position = max_iou_idx + car_pts = get_car_box( + tx[position], + tz[position], + camera_matrices[position][:3, :3], + manu, + car_dims, + ) + car_patch = mpatches.Polygon(car_pts, alpha=1, fill=False, edgecolor="red") + ax.add_patch(car_patch) + + line_info_json_f = line_json + + if config["line_type"] == "stop": + l_stop = get_line(line_info_json_f) + + stop_ped_offset = config["stop_ped_line_dist"] + l_ped = [l_stop[0], l_stop[1] + stop_ped_offset] + elif config["line_type"] == "ped": + l_ped = get_line(line_info_json_f) + stop_line_offset = config["stop_ped_line_dist"] + l_stop = [l_ped[0], l_ped[1] + stop_line_offset] + + line_viz_x_lim = config["xlim"] + + plot_line(plt, l_stop, line_viz_x_lim, "blue") + plot_line(plt, l_ped, line_viz_x_lim, "yellow") + + ## markers are always ahead of stop line, + ## ped line is gonna be ahead of stop line + ## X X * X $ | + ## * $ | + ## * $ | + ## * $ | + ## Legend: Marker : X, StopLine : |, PedLine1 : $, Pedline2 : * + ## Define markers_side_ped such that both PL1 and PL2 are satisfied + ## + ## As we can assume that ped_line will always be ahead of stop_line, + ## every point on ped_line will be sign(stop_line(point)) == sign(stop_line(marker)) + ## + ## Let's find a point on stop_line, that will always be behind ped_line + ## Use that point to find the side and then invert it + markers_side = get_side_of_marker_from_line(markers, l_stop) # say this is 1 + point_on_stop_line = (0, l_stop[0] * 0 + l_stop[1]) + markers_side_ped = -1 * np.sign( + -1 * l_ped[0] * point_on_stop_line[0] + point_on_stop_line[1] - l_ped[1] + ) + + selected_pts = select_traj_points_by_time(tx, tz, segment_json, config["fps"]) + + behind_lines_decision_ped, percentage_obey = percentage_of_points_behind_line( + selected_pts, l_ped, markers_side_ped + ) + behind_lines_decision_stop, percentage_stop = percentage_of_points_behind_line( + selected_pts, l_stop, markers_side + ) + label_decision = [] + for i in range(len(behind_lines_decision_ped)): + if behind_lines_decision_ped[i] == 1 and behind_lines_decision_stop[i] == 1: + label_decision.append(0) + elif behind_lines_decision_ped[i] == 1 and behind_lines_decision_stop[i] == 0: + label_decision.append(1) + elif behind_lines_decision_ped[i] == 0 and behind_lines_decision_stop[i] == 0: + label_decision.append(2) + else: + raise Exception("Error: Ped Line is not ahead of Stop Line in track!") + + obey_decision = "Fail" + if percentage_obey >= config["behind_lines_obey_threshold"]: + obey_decision = "Pass" + + stop_decision = "Fail" + if percentage_stop > config["behind_lines_stop_threshold"]: + stop_decision = "Pass" + + maneuver.report.add_report("trafficLight_obey_decision", obey_decision) + maneuver.report.add_report("trafficLight_stop_decision", stop_decision) + maneuver.report.add_report( + "trafficLight_outcome", + { + "percentage_behind_both_lines": percentage_obey, + "percentage_behind_stop_line": percentage_stop, + }, + ) + + print( + "({}) Halt Behind Stop Line %: {} Decision: {}".format( + manu, percentage_stop, stop_decision + ) + ) + print( + "({}) Halt Behind Ped Line %: {} Decision: {}".format( + manu, percentage_obey, obey_decision + ) + ) + + color_f = ["green", "yellow", "brown"] + size_f = [25, 25, 40] + len_plot = len(selected_pts) + print(len(selected_pts), len(label_decision)) + plt.scatter( + [x[0] for x in selected_pts], + [x[1] for x in selected_pts], + c=[color_f[label_decision[i]] for i in range(0, len_plot)], + s=[size_f[label_decision[i]] for i in range(0, len_plot)], + marker="*", + ) + + plot_legend() + plot_limits(config["xlim"], config["ylim"]) + + # if markers is not None: + # rotate_plot(ax, markers, manu) + + plt.savefig(save_image_name, dpi=200) + plt.close() + + +def plot_legend(): + red_patch = mpatches.Patch(color="red", label="Reverse") + blue_patch = mpatches.Patch(color="blue", label="Forward") + black_patch = mpatches.Patch(color="black", label="Halt") + plt.legend( + handles=[blue_patch, red_patch, black_patch], + prop={"size": 10}, + loc="upper right", + ) + + +def plot_markers(markers): + for key in markers["aruco_bc_markers"]: + marker_obj = markers["aruco_bc_markers"][key] + marker_coord = get_marker_coord(marker_obj) + plt.scatter( + marker_coord[0], marker_coord[1], c=[[0, 0, 0]], marker=".", s=100.0 + ) + + +def plot_limits(x_limits, y_limits): + plt.xlim(x_limits) + plt.ylim(y_limits) + plt.gca().set_aspect("equal", "box") + + +class Traffic(Maneuver): + def run(self): + map_path = self.config.get_config_value("map_file_path") + if not os.path.exists(map_path): + map_path = os.path.join(self.inputs["cwd"], map_path) + + calib_path = self.config["calibration_file_path"] + if not os.path.exists(calib_path): + calib_path = os.path.join(self.inputs["cwd"], calib_path) + try: + traj = generate_trajectory( + self.inputs["back_video"], + self.config.get_config_value("maneuver"), + map_path, + self.out_folder, + calib_path, + self.config["size_marker"], + self.config["aruco_test_exe"], + self.inputs["cwd"], + ) + except Exception as e: + print("Error generating traffic trajectory: ", e) + raise e + + _, tx, ty, tz, camera_matrices = traj + tx, ty, tz = smoothen_trajectory(tx, ty, tz, 25, 25, 2) + markers = yml_parser(map_path) + + # get_direction_stats + direction, stats = self.get_direction_stats( + tx, + ty, + tz, + camera_matrices, + self.config["rev_fwd_halt_segment_min_frame_len"], + self.config["min_frame_len"], + ) + + # fwd rev halts + halts = get_halts(tx, ty, tz) + + # plot + # plot_traj( + # tx, tz, "traffic", self.config['car_dims'], self.config['line_file_path'], self.config, "traffic", camera_matrices=camera_matrices, max_iou_idx=self.config['max_iou'], segment_json=os.path.join(self.out_folder, f"segment.json") + # ) + + line_path = self.config["line_file_path"] + if not os.path.exists(line_path): + line_path = os.path.join(self.inputs["cwd"], line_path) + + plot_traj( + tx, + tz, + "traffic", + self.config["car_dims"], + line_path, + self.config, + self, + markers, + segment_json=os.path.join(self.out_folder, "manu_json_seg_int.json"), + save_image_name=os.path.join(self.out_folder, "traffic.png"), + ) + + return ( + True, + stats, + { # TODO: Add pass/fail + "trajectory": f"{os.path.join(self.out_folder, 'traffic.png')}" + }, + ) + + def get_direction_stats( + self, + tx, + ty, + tz, + camera_matrices, + rev_fwd_halt_segment_min_frame_len, + min_frame_len, + ): + """ """ + directions = [1] + Xs = [] + for i in range(1, len(tx)): + translation = ( + np.array([tx[i], ty[i], tz[i], 1.0]).reshape(4, 1).astype(np.float64) + ) + X = camera_matrices[i - 1][:3, :3].dot( + translation[:3, 0] - camera_matrices[i - 1][:3, -1] + ) + # print(X) + direction = X[2] + + if [tx[i], ty[i], tz[i]] == [tx[i - 1], ty[i - 1], tz[i - 1]]: + directions.append(directions[-1]) + else: + if direction >= 0: + directions.append(1) + else: + directions.append(0) + + directions = directions[1:] + + # Get Forward-Reverse + directions = np.array(directions + [-1]) + + # Get halts + halt_info = get_halts(tx, ty, tz) + directions[halt_info == True] = -1 + directions = directions.tolist() + directions = majority_vote_smoothen( + directions, rev_fwd_halt_segment_min_frame_len + ) + + stats, directions = aggregate_direction(directions, min_frame_len) + stats, _ = aggregate_direction(directions, min_frame_len) + + stats, directions = post_process_direction_vector(stats, directions) + + debug_directions_visualize(plt, tx, ty, tz, directions) + + return directions, stats diff --git a/mapping_cli/mapper.py b/mapping_cli/mapper.py new file mode 100644 index 0000000..f19ae8e --- /dev/null +++ b/mapping_cli/mapper.py @@ -0,0 +1,211 @@ +import logging +import os +import signal +import subprocess +from threading import Timer +from time import sleep + +import typer +from click import command + +from mapping_cli import utils +from mapping_cli.validation import * + +LOG_FILENAME = "log.out" +logging.basicConfig(filename=LOG_FILENAME, level=logging.DEBUG) + + +def run( + mapper_exe_path: str, + images_directory: str, + camera_params_path: str, + dictionary: str, + marker_size: str, + output_path: str, + cwd: str = None, +): + """Function to build a Map using the mapper exe and images + + Args: + mapper_exe_path (str): Mapper exe path. + images_directory (str): Image Directory Path. + camera_params_path (str): Camera config/param yml file path. + dictionary (str): Type of Dictionary. + marker_size (str): Size of the marker. + output_path (str): Output file name. + """ + try: + check_if_mapper_exe_is_valid(mapper_exe_path) + check_if_images_dir_exists(images_directory) + check_if_camera_config_is_valid(camera_params_path) + + typer.echo("Hey There! Starting to run the mapper!") + + num_of_images = len( + [ + name + for name in os.listdir(images_directory) + if os.path.isfile(os.path.join(images_directory, name)) + ] + ) + typer.echo(f"{num_of_images} num of images") + + def killFunc(): + os.kill(p.pid, signal.CTRL_C_EVENT) + + try: + call_string = '"{}" "{}" "{}" {} {} {}'.format( + mapper_exe_path, + images_directory, + camera_params_path, + marker_size, + dictionary, + output_path, + ) + + if cwd is None: + cwd = os.path.curdir + + my_env = os.environ.copy() + my_env["PATH"] = cwd + ";" + my_env["PATH"] + + p = subprocess.Popen( + call_string, + cwd=cwd, + env=my_env, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + + timer = Timer(num_of_images, p.kill) + + timer.start() + stdout, stderr = p.communicate() + retcode = p.returncode + typer.echo(f"{retcode} {stdout} {stderr} {cwd}") + + if retcode == 0: + decoded_error = stderr.decode(encoding="utf-8") + typer.echo(f"Error! {decoded_error}") + + if "Dictionary::loadFromFile" in decoded_error: + typer.Exit(code=1) + return "Invalid Doctionary" + elif "CameraParameters::readFromXML" in decoded_error: + typer.Exit(code=2) + return "Invalid Camera Calibration File" + + return decoded_error + except Exception as e: + typer.echo(f"Error: {e}") + timer.cancel() + finally: + typer.echo("Cancelling Timer") + timer.cancel() + try: + call_string = "{} {} {} {} {} {}".format( + mapper_exe_path, + images_directory, + camera_params_path, + marker_size, + dictionary, + output_path, + ) + + if cwd is None: + cwd = os.path.curdir + + my_env = os.environ.copy() + my_env["PATH"] = cwd + ";" + my_env["PATH"] + + p = subprocess.Popen( + call_string, + cwd=cwd, + env=my_env, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + + timer = Timer(num_of_images, p.kill) + + timer.start() + stdout, stderr = p.communicate() + retcode = p.returncode + typer.echo(f"{retcode} {stdout} {stderr} {cwd}") + + if retcode == 0: + decoded_error = stderr.decode(encoding="utf-8") + typer.echo(f"Error! {decoded_error}") + + if "Dictionary::loadFromFile" in decoded_error: + typer.Exit(code=1) + return "Invalid Doctionary" + elif "CameraParameters::readFromXML" in decoded_error: + typer.Exit(code=2) + return "Invalid Camera Calibration File" + + return decoded_error + except Exception as e: + typer.echo(f"Error: {e}") + timer.cancel() + finally: + typer.echo("Cancelling Timer") + timer.cancel() + exit(0) + + typer.echo(f"Done! Generated the map and saved as {output_path}") + + except Exception as e: + typer.echo(f"Error : {e}") + + +def distance_error(map_file, dist_file): + try: + aruco_map = utils.yml_parser(map_file)["aruco_bc_markers"] + markers = [] + gt_distances = [] + measured_distances = [] + errors = [] + + with open(dist_file) as f: + for line_idx, line in enumerate(f): + line = line.strip() + content = line.split(" ") + assert ( + len(content) == 3 + ), f"Check your file again, {line_idx} has {len(content)}: {line} wrong formatting of input" + a, corner_a = content[0].split("_") # read the values + b, corner_b = content[1].split("_") + gt = float(content[2]) + markers.append([a + "_" + corner_a, b + "_" + corner_b]) + gt_distances.append(gt) + if int(a) not in aruco_map.keys(): + raise KeyError( + f"Marker {a} in distance file not found in the map. Expected marker list: {aruco_map.keys()}" + ) + if int(b) not in aruco_map.keys(): + raise KeyError( + f"Marker {b} in distance file not found in the map. Expected marker list: {aruco_map.keys()}" + ) + + measured_distance = utils.euclidean_distance( + aruco_map[int(a)][int(corner_a) - 1], + aruco_map[int(b)][int(corner_b) - 1], + ) + measured_distances.append(measured_distance) + err = abs(gt - measured_distance) + errors.append(err) + logging.info( + "Ground Truth: {} Measured Dist: {} Err: {}".format( + gt, measured_distance, err + ) + ) + avg_err = sum(errors) / len(errors) + + typer.echo(f"The Calculated average error is {avg_err}") + return avg_err, None + + except Exception as e: + logging.exception(e) + typer.echo(f"Error : {e}") + return None, str(e) diff --git a/mapping_cli/segment.py b/mapping_cli/segment.py new file mode 100644 index 0000000..8c42215 --- /dev/null +++ b/mapping_cli/segment.py @@ -0,0 +1,347 @@ +"""Video segmentation module +""" + +import json +import os +import time +from typing import Dict + +import cv2 +import numpy as np +from decord import VideoReader + +from mapping_cli.utils import detect_marker, trim_video + + +def get_manu_frame_segments(back_video, out_fldr, configs): + manus_frame_nums = { + x: {"start": [], "end": []} + for x in configs["maneuver_order"] # manu : { start : [], end : [] } + } + marker_list_json = {} + + skip_frames = configs["skip_frames"] + vidcap = cv2.VideoCapture(back_video) + # success, image = vidcap.read() + vid_length = int(vidcap.get(cv2.CAP_PROP_FRAME_COUNT)) + maneuvers_in_order = configs["maneuver_order"] + fps = vidcap.get(cv2.CAP_PROP_FPS) + frame_count = vidcap.get(cv2.CAP_PROP_FRAME_COUNT) + reader = VideoReader(back_video) + + json_manu_f = os.path.join(out_fldr, "manu_json_seg_int.json") + json_marker_f = os.path.join(out_fldr, "manu_json_marker_list.json") + if os.path.exists(json_manu_f): + # with open(json_manu_f) as f: + # manus_frame_nums = json.load(f) + # with open(json_marker_f) as f: + # marker_list_json = json.load(f) + pass + else: + marker_list_json = {} + + count = 0 + idx = 0 + for i in range(len(reader)): + if idx % skip_frames == 0: + image = reader.next().asnumpy() + marker_list = detect_marker(image, configs["marker_type"]) + marker_list_json[idx] = [int(x) for x in marker_list] + for manu in maneuvers_in_order: + if ( + list( + value in marker_list for value in configs[manu]["startMarkers"] + ).count(True) + >= configs[manu]["startMarkersLen"] + ): + manus_frame_nums[manu]["start"].append(float(idx) / fps) + break + + elif ( + list( + value in marker_list for value in configs[manu]["endMarkers"] + ).count(True) + >= configs[manu]["endMarkersLen"] + ): + manus_frame_nums[manu]["end"].append(float(idx) / fps) + break + # success, image = vidcap.read() + idx += 1 + if idx > 1e5: + raise Exception("Too much time") + + with open(json_marker_f, "w") as f: + json.dump(marker_list_json, f) + with open(json_manu_f, "w") as f: + json.dump(manus_frame_nums, f) + + print("Manus: ", manus_frame_nums) + for k, v in manus_frame_nums.items(): + assert v["start"], f"Maneuver {k} has empty start segmentation number" + assert v["end"], f"Maneuver {k} has empty start segmentation number" + + # transform_manus_with_constraints( + # manus_frame_nums, configs, maneuvers_in_order, len(reader), configs["fps"] + # ) + + return manus_frame_nums, vid_length + + +def transform_manus_with_constraints( + manus_frame_nums, manu_info, manus_in_order, vid_length, video_fps +): + try: + outlier_fn_dispatcher = { + "outlier_from_std_dev": outlier_from_std_dev, + "outlier_from_time_threshold": outlier_from_time_threshold, + "pad_time_to_small_seq": pad_time_to_small_seq, + "outlier_from_max_time_difference": outlier_from_max_time_difference, + "ensure_arr_vals_greater_than_prev_manu": ensure_arr_vals_greater_than_prev_manu, + } # Can do eval(fn_string)(args) but unsafe + for manu in manu_info.keys(): + # print(manu) + if manu_info[manu]["outlier_fns_list_start"] is not None: + for fn in manu_info[manu]["outlier_fns_list_start"]: + fn_name = fn[0] + args = fn[1] + if fn_name == "outlier_from_std_dev": + manus_frame_nums[manu]["start"] = outlier_fn_dispatcher[ + fn_name + ](manus_frame_nums[manu]["start"], args["s_threshold"]) + elif fn_name == "outlier_from_time_threshold": + manu_to_test, seg, seg_idx, time_slack = ( + args["manu"], + args["seg_type"], + args["seg_idx"], + args["time_slack"], + ) + manus_frame_nums[manu]["start"] = outlier_fn_dispatcher[ + fn_name + ]( + manus_frame_nums[manu]["start"], + manus_frame_nums[manu_to_test][seg][seg_idx], + time_slack, + False, + video_fps, + ) + elif fn_name == "outlier_from_max_time_difference": + manus_frame_nums[manu]["start"] = outlier_fn_dispatcher[ + fn_name + ]( + manus_frame_nums[manu]["start"], + args["time_slack"], + video_fps, + args["take_first_seg"], + ) + elif fn_name == "ensure_arr_vals_greater_than_prev_manu": + prev_manu, prev_seg, prev_seg_idx = ( + args["prev_manu"], + args["prev_seg_type"], + args["prev_seg_idx"], + ) + try: + prev_thresh = manus_frame_nums[prev_manu][prev_seg][ + prev_seg_idx + ] + except: + prev_thresh = None + manus_frame_nums[manu]["start"] = outlier_fn_dispatcher[ + fn_name + ](manus_frame_nums[manu]["start"], prev_thresh) + elif fn_name == "pad_time_to_small_seq": + manus_frame_nums[manu]["start"] = outlier_fn_dispatcher[ + fn_name + ]( + manus_frame_nums[manu]["start"], + args["min_size"], + args["time_slack"], + False, + video_fps, + None, + ) + if manu_info[manu]["outlier_fns_list_end"] is not None: + for fn in manu_info[manu]["outlier_fns_list_end"]: + fn_name = fn[0] + args = fn[1] + if fn_name == "outlier_from_std_dev": + manus_frame_nums[manu]["end"] = outlier_fn_dispatcher[fn_name]( + manus_frame_nums[manu]["end"], args["s_threshold"] + ) + elif fn_name == "outlier_from_time_threshold": + manu_to_test, seg, seg_idx, time_slack = ( + args["manu"], + args["seg"], + args["seg_idx"], + args["time_slack"], + ) + manus_frame_nums[manu]["end"] = outlier_fn_dispatcher[fn_name]( + manus_frame_nums[manu]["end"], + manus_frame_nums[manu_to_test][seg][seg_idx], + time_slack, + True, + video_fps, + ) + elif fn_name == "outlier_from_max_time_difference": + manus_frame_nums[manu]["end"] = outlier_fn_dispatcher[fn_name]( + manus_frame_nums[manu]["end"], + args["time_slack"], + video_fps, + args["take_first_seg"], + ) + elif fn_name == "ensure_arr_vals_greater_than_prev_manu": + prev_manu, prev_seg, prev_seg_idx = ( + args["prev_manu"], + args["prev_seg_type"], + args["prev_seg_idx"], + ) + try: + prev_thresh = manus_frame_nums[prev_manu][prev_seg][ + prev_seg_idx + ] + except: + prev_thresh = None + manus_frame_nums[manu]["end"] = outlier_fn_dispatcher[fn_name]( + manus_frame_nums[manu]["end"], prev_thresh + ) + elif fn_name == "pad_time_to_small_seq": + manus_frame_nums[manu]["end"] = outlier_fn_dispatcher[fn_name]( + manus_frame_nums[manu]["end"], + args["min_size"], + args["time_slack"], + True, + video_fps, + ) + + for manu in manus_in_order: + if manu_info[manu]["constraint_for_start"] is not None: + constraint = manu_info[manu]["constraint_for_start"] + constraint_manu = constraint["constraint_manu"] + constraint_type = constraint["constraint_type"] + if constraint_type == "hard" or manus_frame_nums[manu]["start"] == []: + if constraint_manu == "vid_start": + value = 0 + else: + seg_type = constraint["seg_type"] + seg_idx = constraint["seg_idx"] + value = manus_frame_nums[constraint_manu][seg_type][seg_idx] + + manus_frame_nums[manu]["start"] = [value] + + if manu_info[manu]["constraint_for_end"] is not None: + constraint = manu_info[manu]["constraint_for_end"] + constraint_manu = constraint["constraint_manu"] + constraint_type = constraint["constraint_type"] + if constraint_type == "hard" or manus_frame_nums[manu]["end"] == []: + if constraint_manu == "vid_end": + value = vid_length - 1 + else: + seg_type = constraint["seg_type"] + seg_idx = constraint["seg_idx"] + value = manus_frame_nums[constraint_manu][seg_type][seg_idx] + + manus_frame_nums[manu]["end"] = [value] + except Exception as e: + print("Segmentation transformation exception: ", e) + + return manus_frame_nums + + +def outlier_from_std_dev(arr, s_threshold): + m = np.median(arr) + s = np.std(arr) + arr = filter(lambda x: x < m + s_threshold * s, arr) + arr = filter(lambda x: x > m - s_threshold * s, arr) + return list(arr) + + +def outlier_from_time_threshold(arr, val, time_skip, after_manu, video_fps): + if after_manu == True: + time_skip = val + time_skip * video_fps + return list(filter(lambda x: x < time_skip, arr)) + else: + time_skip = val - time_skip * video_fps + return list(filter(lambda x: x > time_skip, arr)) + + +def pad_time_to_small_seq(arr_prev, min_size, time_slack, flag, video_fps): + if arr_prev == []: + return [] + if len(arr_prev) <= min_size: + if flag == True: + for i in range(arr_prev[0] + 1, arr_prev[0] + time_slack * video_fps): + arr_prev.append(i) + else: + new_arr = [] + for i in range(arr_prev[0] - time_slack * video_fps, arr_prev[0] - 1): + new_arr = new_arr.append(i) + new_arr.extend(arr_prev) + return new_arr + return arr_prev + + +def ensure_arr_vals_greater_than_prev_manu(arr, thresh): + if thresh is None: + return arr + return list(filter(lambda x: x > thresh, arr)) + + +def outlier_from_max_time_difference(arr, time_slack, video_fps, take_first_seg): + seg_dist = time_slack * video_fps + idx = None + + for i in range(0, len(arr) - 1): + if arr[i + 1] - arr[i] >= seg_dist: + idx = i + break + + if idx is None: + return arr + if take_first_seg: + return arr[: idx + 1] + return arr[idx + 1 :] + + +def segment( + front_video_path, back_video_path: str, output_path: str, configs: Dict +) -> None: + """Segment video into frames""" + # Create output folder if it doesn't exist + t = time.time() + if not os.path.exists(output_path): + os.makedirs(output_path) + + assert ( + "maneuver_order" in configs.keys() + ), f"Missing maneuver_order in configs keys: {configs.keys()}" + + maneuver_order = configs["maneuver_order"] + + maneuver_frame_numbers, _ = get_manu_frame_segments( + back_video_path, output_path, configs + ) + segment_warnings = [] + segment_paths = {} + for i in range(len(maneuver_order)): + if i != len(maneuver_order) - 1: + if ( + maneuver_frame_numbers[maneuver_order[i]]["end"] + > maneuver_frame_numbers[maneuver_order[i + 1]]["start"] + ): + segment_warnings.append( + f"{maneuver_order[i]} crossing {maneuver_order[i+1]} limits" + ) + print( + f"Start;;;;; {maneuver_frame_numbers[maneuver_order[i]]['start']} || {maneuver_frame_numbers[maneuver_order[i]]['end']}" + ) + path = trim_video( + back_video_path, + maneuver_frame_numbers[maneuver_order[i]]["start"][0], + maneuver_frame_numbers[maneuver_order[i]]["end"][-1], + configs["use_gpu"], + output_path, + maneuver_order[i] + ".mp4", + ) + segment_paths[str(maneuver_order[i])] = path + + print("Time: ", time.time() - t) + return segment_paths, segment_warnings diff --git a/mapping_cli/utils.py b/mapping_cli/utils.py new file mode 100644 index 0000000..c9242b9 --- /dev/null +++ b/mapping_cli/utils.py @@ -0,0 +1,536 @@ +import itertools +import json +import logging +import math +import os +import subprocess +from typing import Dict + +import cv2 +import ffmpeg +import filelock +import numpy as np +import scipy +import shapely +import yaml + + +def yml_parser(map_file): + ret = None + with open(map_file, "r") as stream: + stream.readline() # YAML 1.0 bug + try: + ret = yaml.safe_load(stream) + except yaml.YAMLError as exc: + logging.info(exc) + raise exc + ## YAML 1.0 bug + # YAML library handles YAML 1.1 and above, + # to fix one of the bug in the generated YAML + for x in ret["aruco_bc_markers"]: + invalid_key = list(filter(lambda x: x.startswith("id"), x.keys()))[0] + key, value = invalid_key.split(":") + x[key] = value + del x[invalid_key] + marker_dict = {} + for dct in ret["aruco_bc_markers"]: + marker_dict[int(dct["id"])] = dct["corners"] + ret["aruco_bc_markers"] = marker_dict + return ret + + +def euclidean_distance(A, B): + return np.linalg.norm(np.array(A) - np.array(B), 2) + + +def euclidean_distance_batch(A, B): + dist_array = scipy.spatial.distance.cdist(A, B, metric="euclidean") + return dist_array + + +def generate_trajectory( + input_video: str, + maneuver: str, + map_file_path: str, + out_folder: str, + calibration: str, + size_marker: str, + aruco_test_exe: str, + cwd: str, +): + """ + Generate a trajectory from a video and a maneuver + :param input_video: input video path + :param maneuver: maneuver trajectory to be generated + :param map_file_path: map file path + :param out_folder: output folder + :param calibration: calibration + :param size_marker: size of the marker + :param aruco_test_exe: aruco test executable + :return: + """ + print(os.getcwd()) + output_traj_path = os.path.join(out_folder, maneuver + "_trajectory.txt") + exec_string = f"{aruco_test_exe} {input_video} {map_file_path} {calibration} -s {size_marker} {output_traj_path}" + + assert os.path.exists(input_video), f"{input_video} does not exist" + assert os.path.exists(map_file_path), f"{map_file_path} does not exist" + assert os.path.exists(calibration), f"{calibration} does not exist" + # assert os.path.exists(aruco_test_exe), f"{aruco_test_exe} does not exist" + + try: + # output = subprocess.check_output( + # exec_string, shell=True, stderr=subprocess.STDOUT + # ) + + my_env = os.environ.copy() + my_env["PATH"] = out_folder + ";" + cwd + ";" + my_env["PATH"] + + p = subprocess.Popen( + exec_string, + shell=True, + cwd=cwd, + env=my_env, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + ) + stdout, stderr = p.communicate() + + except subprocess.CalledProcessError as e: + raise RuntimeError( + "command '{}' return with error (code {}): {}".format( + e.cmd, e.returncode, e.output + ) + ) + + # popen = subprocess.Popen(exec_string, shell=True, stdout=subprocess.PIPE) + # out, err = popen.communicate() + + # print(exec_string, out, err) + + output = stdout.decode("utf-8") + output = ( + output.replace(";\r\n", "") + .replace("]", "") + .replace("[", "") + .replace(",", "") + .replace("\r\n", "\n") + ) + + with open(output_traj_path, "w+") as f: + f.write(output) + f.close() + + return read_aruco_traj_file(output_traj_path, {}) + + +def get_marker_coord(marker_obj): + return [marker_obj[0][0], marker_obj[0][2]] + + +def aggregate_direction(direction_vector, halt_len): + """ + Input: vector containing 'forward', 'reverse' + Output: # of forwards, # of reverse + """ + direction_count = {"F": 0, "R": 0, "H": 0} + # Group consecutive directions + grouped_class = [ + list(l) + for _, l in itertools.groupby(enumerate(direction_vector), key=lambda x: x[1]) + ] + new_direction_vector = [] + for c_idx, c in enumerate(grouped_class): + if c[0][1] == 0: + direction_count["R"] += 1 + new_direction_vector += [0 for i in range(len(c))] + elif c[0][1] == 1: + direction_count["F"] += 1 + new_direction_vector += [1 for i in range(len(c))] + elif c[0][1] == -1: + if len(c) > halt_len: + direction_count["H"] += 1 + new_direction_vector += [-1 for i in range(len(c))] + else: + # Start segment + if c_idx > 0: + prev_direction = grouped_class[c_idx - 1][0][1] + else: + next_direction = grouped_class[c_idx + 1][0][1] + prev_direction = next_direction + + # End segment + if c_idx < len(grouped_class) - 1: + next_direction = grouped_class[c_idx + 1][0][1] + else: + next_direction = prev_direction + + new_direction_vector += [prev_direction for i in range(len(c) // 2)] + new_direction_vector += [next_direction for i in range(len(c) // 2)] + + return direction_count, new_direction_vector + + +def get_maneuver_box(box_path): + val = None + with open(box_path) as f: + d = json.load(f) + val = d["box"] + val = list( + zip(*shapely.geometry.Polygon(val).minimum_rotated_rectangle.exterior.coords.xy) + ) + return np.array(val) + + +def get_C_coord(rvec, tvec): + """ + Input: rotation vector, translation vector + Returns: Camera coordinates + """ + rvec = cv2.Rodrigues(np.array(rvec))[0] + tvec = np.array(tvec) + return -np.dot(np.transpose(rvec), tvec) + + +def read_aruco_traj_file(filename, ignoring_points_dict): + f_id = [] + t_x = [] + t_y = [] + t_z = [] + camera_matrices = [] + with open(filename, "r") as f: + line = f.readline() + while line: + if not line[0].isdigit(): + line = f.readline() + continue + else: # rvec- rotational vector, tvec - translational vector + row = line.split(" ") + cur_id = int(row[0].strip()) + if not cur_id in ignoring_points_dict: + rvec = [ + float(row[1].strip()), + float(row[2].strip()), + float(row[3].strip()), + ] + tvec = [ + float(row[4].strip()), + float(row[5].strip()), + float(row[6].strip()), + ] + + cvec = get_C_coord(rvec, tvec) + cvec = cvec.reshape(3, 1) + + temp = np.hstack((cv2.Rodrigues(np.array(rvec))[0], cvec)) + camera_matrix = np.vstack((temp, np.array([0.0, 0.0, 0.0, 1.0]))) + + f_id.append(int(row[0].strip())) + camera_matrices.append(camera_matrix) + t_x.append(cvec[0][0]) + t_y.append(cvec[1][0]) + t_z.append(cvec[2][0]) + + line = f.readline() + + # if len(t_x) == 0: + # raise Exception("Trajectory length 0") + + return f_id, t_x, t_y, t_z, camera_matrices + + +def stitch(input_imgs_path: str, input_file_extension: str, output_file_name: str): + ffmpeg.input(os.path.join(input_imgs_path, f"*.{input_file_extension}")).output( + output_file_name + ).run() + return output_file_name + + +def detect_marker(frame, marker_dict): + DICT_CV2_ARUCO_MAP = { + "TAG16h5": cv2.aruco.DICT_APRILTAG_16h5, + "DICT_4X4_100": cv2.aruco.DICT_4X4_100, + } + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + aruco_dict = cv2.aruco.Dictionary_get(DICT_CV2_ARUCO_MAP[marker_dict]) + parameters = cv2.aruco.DetectorParameters_create() + _, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=parameters) + markers = [] + if ids is not None: + for i in ids: + markers.append(i[0]) + return markers + + +def trim_video( + video_path: str, + start_segment, + end_segment, + use_gpu: bool, + out_folder: str, + output_name: str, +): + device_str = "" + if use_gpu: + device_str = " -gpu 0 " + call_string = f"ffmpeg -y -hide_banner -loglevel panic -ss {start_segment} -i {video_path} -t {end_segment-start_segment} -b:v 4000K -vcodec h264_nvenc {device_str}{os.path.join(out_folder, output_name)}" + print("call string: ", call_string) + my_env = os.environ.copy() + my_env["PATH"] = out_folder + ";" + my_env["PATH"] + p = subprocess.Popen(call_string, shell=True, cwd=out_folder, env=my_env) + stdout, stderr = p.communicate() + print(stdout, stderr) + return os.path.join(out_folder, output_name) + + +class Report: + def __init__(self, textfile): + self.textfile = textfile + self.lockfile = textfile + ".lock" + + def open_file(self): + if os.path.exists(self.textfile): + with open(self.textfile, "r") as f: + self.jsonf = json.load(f) + else: + self.jsonf = {} + + def add_report(self, key, val): + with filelock.FileLock(self.lockfile): + self.open_file() + self.jsonf[key] = val + with open(self.textfile, "w") as f: + json.dump(self.jsonf, f, indent=2) + + +def stitch(input_imgs_path: str, input_file_extension: str, output_file_name: str): + ffmpeg.input(os.path.join(input_imgs_path, f"*.{input_file_extension}")).output( + output_file_name + ).run() + return output_file_name + + +def detect_marker(frame, marker_dict): + DICT_CV2_ARUCO_MAP = { + "TAG16h5": cv2.aruco.DICT_APRILTAG_16h5, + "DICT_4X4_100": cv2.aruco.DICT_4X4_100, + } + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + aruco_dict = cv2.aruco.Dictionary_get(DICT_CV2_ARUCO_MAP[marker_dict]) + parameters = cv2.aruco.DetectorParameters_create() + _, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=parameters) + markers = [] + if ids is not None: + for i in ids: + markers.append(i[0]) + return markers + + +def yml_parser(map_file): + ret = None + + with open(map_file, "r") as stream: + stream.readline() # YAML 1.0 bug + try: + ret = yaml.safe_load(stream) + except yaml.YAMLError as exc: + print(exc) + ## YAML 1.0 bug + # YAML library handles YAML 1.1 and above, + # to fix one of the bug in the generated YAML + for x in ret["aruco_bc_markers"]: + invalid_key = list(filter(lambda x: x.startswith("id"), x.keys()))[0] + key, value = invalid_key.split(":") + x[key] = value + del x[invalid_key] + + marker_dict = {} + for dct in ret["aruco_bc_markers"]: + marker_dict[int(dct["id"])] = dct["corners"] + ret["aruco_bc_markers"] = marker_dict + return ret + + +def get_marker_coord(marker_obj): + return [marker_obj[0][0], marker_obj[0][2]] + + +def get_maneuver_box(box_path): + val = None + with open(box_path) as f: + d = json.load(f) + val = d["box"] + val = list( + zip(*shapely.geometry.Polygon(val).minimum_rotated_rectangle.exterior.coords.xy) + ) + return np.array(val) + + +def get_C_coord(rvec, tvec): + """ + Input: rotation vector, translation vector + Returns: Camera coordinates + """ + rvec = cv2.Rodrigues(np.array(rvec))[0] + tvec = np.array(tvec) + return -np.dot(np.transpose(rvec), tvec) + + +# def read_aruco_traj_file(filename): +# f_id = [] +# t_x = [] +# t_y = [] +# t_z = [] +# camera_matrices = [] +# with open(filename, 'r') as f: +# line = f.readline() +# while line: +# if not line[0].isdigit(): +# line = f.readline() +# continue +# else: #rvec- rotational vector, tvec - translational vector +# row = line.split(' ') +# rvec = [float(row[1].strip()), float(row[2].strip()), float(row[3].strip())] +# tvec = [float(row[4].strip()), float(row[5].strip()), float(row[6].strip())] + +# cvec = get_C_coord(rvec, tvec) +# cvec = cvec.reshape(3, 1) + +# temp = np.hstack(( cv2.Rodrigues( np.array(rvec) )[0], cvec)) +# camera_matrix = np.vstack((temp, np.array([0.0, 0.0, 0.0, 1.0]))) + +# f_id.append(int(row[0].strip())) +# camera_matrices.append(camera_matrix) +# t_x.append( cvec[0][0] ) +# t_y.append( cvec[1][0] ) +# t_z.append( cvec[2][0] ) +# line = f.readline() +# return f_id, t_x, t_y, t_z, camera_matrices + + +def majority_vote_smoothen(vector, window): + """ + Input: vector of 1's and 0's + Output: smoothing applied to window + """ + vector_smooth = [] + for i in range(0, len(vector), window): + forw = vector[i : i + window].count(1) + rev = vector[i : i + window].count(0) + halt = vector[i : i + window].count(-1) + x = None + if forw > 2 * rev and forw > halt: + x = 1 + for i in range(window): + vector_smooth.append(1) + elif rev > 2 * forw and rev > halt: + x = 2 + for i in range(window): + vector_smooth.append(0) + else: + x = 3 + for i in range(window): + vector_smooth.append(-1) + # print(forw, rev, halt, x) + return vector_smooth[: len(vector)] + + +def debug_directions_visualize(plt, tx, ty, tz, directions): + """ """ + colors = ["red", "blue", "black"] + le = min(min(len(tx) - 1, len(tz) - 1), len(directions) - 1) + plt.scatter( + [tx[i] for i in range(0, le)], + [tz[i] for i in range(0, le)], + c=[colors[directions[i]] for i in range(0, le)], + ) + plt.show() + + +def rotate_point(x, y, rot_radian): + return [ + x * math.cos(rot_radian) - y * math.sin(rot_radian), + y * math.cos(rot_radian) + x * math.sin(rot_radian), + ] + + +def rotate_rectangle(points, camera_center, rot_radian): + # Displace to origin + o_points = np.zeros(points.shape) + for i in range(4): + o_points[i, :] = points[i, :] - camera_center + # Rotate points + for i in range(4): + o_points[i, 0], o_points[i, 1] = rotate_point( + o_points[i, 0], o_points[i, 1], rot_radian + ) + # Displace again + for i in range(4): + o_points[i, :] = o_points[i, :] + camera_center + return o_points + + +def rotation_matrix_to_euler_angles(R): + assert is_rotation_matrix(R) + sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0]) + singular = sy < 1e-6 + if not singular: + x = np.arctan2(R[2, 1], R[2, 2]) + y = np.arctan2(-R[2, 0], sy) + z = np.arctan2(R[1, 0], R[0, 0]) + else: + x = np.arctan2(-R[1, 2], R[1, 1]) + y = np.arctan2(-R[2, 0], sy) + z = 0 + return np.array([x, y, z]) + + +def is_rotation_matrix(R): + Rt = np.transpose(R) + should_be_identity = np.dot(Rt, R) + I = np.identity(3, dtype=R.dtype) + n = np.linalg.norm(I - should_be_identity) + return n < 1e-6 + + +def plot_line(plt, l_stop, x_lim, color): + for x in np.arange(x_lim[0], x_lim[1], 0.1): + y = l_stop[0] * x + l_stop[1] + plt.scatter(x, y, color=color) + + +def smoothen_trajectory(tx, ty, tz, outlier_window, poly_fit_window, poly_fit_degree): + # Applying median filter + tx = scipy.signal.medfilt(tx, outlier_window) + ty = scipy.signal.medfilt(ty, outlier_window) + tz = scipy.signal.medfilt(tz, outlier_window) + # Applying savitzky golay filter + tx = scipy.signal.savgol_filter(tx, poly_fit_window, poly_fit_degree) + ty = scipy.signal.savgol_filter(ty, poly_fit_window, poly_fit_degree) + tz = scipy.signal.savgol_filter(tz, poly_fit_window, poly_fit_degree) + return tx, ty, tz + + +def get_graph_box(box_path, label): + with open(box_path) as f: + d = json.load(f) + val = d[label] + val = list( + zip(*shapely.geometry.Polygon(val).minimum_rotated_rectangle.exterior.coords.xy) + ) + return np.array(val) + + +def get_plt_rotation_from_markers(origin_marker, axis_marker, is_vertical): + try: + marker_origin = origin_marker + marker_axis = axis_marker + m = (marker_axis[1] - marker_origin[1]) / (marker_axis[0] - marker_origin[0]) + deg = np.rad2deg(np.arctan(m)) + if is_vertical: + deg = 90 - deg + else: + deg = -1 * deg # make anti clockwise + return deg + except: + return 0.0 diff --git a/mapping_cli/validation.py b/mapping_cli/validation.py new file mode 100644 index 0000000..cf93eee --- /dev/null +++ b/mapping_cli/validation.py @@ -0,0 +1,89 @@ +from os.path import exists + + +def check_if_mapper_exe_is_valid(mapper_exe_path: str): + """Validate mapper exe file path and check if file exists. + + Args: + mapper_exe_path (str): Mapper exe path + + Raises: + ValueError: Mapper exe File Path cannot be empty + ValueError: Unsuported Mapper exe File Type. Only supports .exe + FileNotFoundError: Mapper exe File does not exist + """ + + if len(mapper_exe_path) == 0: + raise ValueError("Mapper exe File Path cannot be empty") + + if not mapper_exe_path.endswith(".exe"): + raise ValueError("Unsuported Mapper exe File Type. Only supports .exe") + + if not exists(mapper_exe_path): + raise FileNotFoundError("Mapper exe File does not exist") + + +def check_if_images_dir_exists(images_directory: str): + """Validate output file path. + + Args: + images_directory (str): Images Directory path + + Raises: + ValueError: Images Dir Path cannot be empty + FileNotFoundError: Images Dir Path not found or does not exist + """ + + if len(images_directory) == 0: + raise ValueError("Images Dir Path cannot be empty") + + if not exists(images_directory): + raise FileNotFoundError("Images Dir Path not found or does not exist") + + +def check_if_camera_config_is_valid(camera_params_path: str): + """Validate camera config params path. + + Args: + camera_params_path (str): Camera params path + """ + if len(camera_params_path) == 0: + raise ValueError("Camera Params path cannot be empty") + + if not camera_params_path.endswith(".yml"): + raise ValueError("Camera params file not .yml type") + + if not exists(camera_params_path): + raise FileNotFoundError("Camera params path not found or does not exist") + + +def check_if_map_yml_is_valid(map_yml_path: str): + """Validate map yml path. + + Args: + map_yml_path (str): Map YML File Path + """ + if len(map_yml_path) == 0: + raise ValueError("Map YML File Path cannot be empty") + + if not map_yml_path.endswith(".yml"): + raise ValueError("Map YML File not .yml type") + + if not exists(map_yml_path): + raise FileNotFoundError("Map YML File Path not found or does not exist") + + +def check_if_txt_file_is_valid(txt_path: str): + """Validate txt path. + + Args: + txt_path (str): Txt File Path + """ + if len(txt_path) == 0: + raise ValueError("Txt File Path cannot be empty") + + if not txt_path.endswith(".txt"): + raise ValueError("Txt File not .yml type") + + if not exists(txt_path): + raise FileNotFoundError("Txt File Path not found or does not exist") diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..caba62f --- /dev/null +++ b/requirements.txt @@ -0,0 +1,19 @@ +typer +PyYAML +numpy +opencv-python +torch +torchvision +tqdm +Pillow +argparse +hydra-core +scipy +matplotlib +pandas +cognitive-face +filelock +ffmpeg-python +scikit-learn +pyquaternion +decord \ No newline at end of file diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..2c81c28 --- /dev/null +++ b/setup.py @@ -0,0 +1,28 @@ +from os import path +from setuptools import find_packages, setup +from mapping_cli import __version__ + +with open('requirements.txt') as f: + required = f.read().splitlines() +curdir = path.abspath(path.dirname(__file__)) +with open(path.join(curdir, "README.md"), encoding="utf-8") as f: + long_description = f.read() +setup( + name="hams", + packages=find_packages(), + version=__version__, + license="GPLv3+", + description="HAMS", + long_description=long_description, + long_description_content_type="text/markdown", + author="Vaibhav Balloli, Anurag Ghosh, Harsh Vijay, Jonathan Samuel, Akshay Nambi", + author_email="t-vballoli@microsoft.com", + install_requires=required, + classifiers=[ + "Development Status :: 4 - Beta", + "Intended Audience :: Developers", + "Topic :: Scientific/Engineering :: Artificial Intelligence", + "License :: OSI Approved :: GNU General Public License v3 or later (GPLv3+)", + "Programming Language :: Python :: 3.8", + ] +) \ No newline at end of file