├── .clang-format ├── .flake8 ├── .github └── workflows │ ├── ci-action-ros.yaml │ └── ci-action.yml ├── .gitignore ├── .gitmodules ├── .pre-commit-config.yaml ├── .ruff.toml ├── LICENSE ├── README.md ├── doc ├── Makefile ├── _static │ └── .gitkeep ├── _templates │ └── .gitkeep ├── api │ ├── core.rst │ └── opengv.rst ├── conf.py └── index.rst ├── examples ├── config │ ├── lightglue_config.yaml │ ├── salad_config.yaml │ ├── superpoint_config.yaml │ └── vlc_driver_config.yaml ├── config_example.py ├── lightglue_example.py ├── place_descriptor_debugging.py ├── salad_example.py ├── superpoint_example.py ├── uhumans_example_batch.py ├── vlc_db_example.py └── vlc_server_driver_example.py ├── ouroboros ├── CMakeLists.txt ├── COLCON_IGNORE ├── pyproject.toml ├── src │ ├── ouroboros │ │ ├── __init__.py │ │ ├── pose_recovery.py │ │ ├── utils │ │ │ ├── __init__.py │ │ │ └── plotting_utils.py │ │ ├── vlc_db │ │ │ ├── __init__.py │ │ │ ├── camera.py │ │ │ ├── camera_table.py │ │ │ ├── invertible_vector_store.py │ │ │ ├── spark_image.py │ │ │ ├── spark_loop_closure.py │ │ │ ├── spark_session.py │ │ │ ├── utils.py │ │ │ ├── vlc_db.py │ │ │ ├── vlc_image.py │ │ │ ├── vlc_image_table.py │ │ │ ├── vlc_lc_table.py │ │ │ ├── vlc_pose.py │ │ │ └── vlc_session_table.py │ │ └── vlc_server │ │ │ ├── __init__.py │ │ │ └── vlc_server.py │ ├── ouroboros_gt │ │ ├── __init__.py │ │ ├── gt_descriptors.py │ │ ├── gt_keypoint_detection.py │ │ ├── gt_matches.py │ │ ├── gt_place_recognition.py │ │ └── gt_pose_recovery.py │ ├── ouroboros_keypoints │ │ ├── __init__.py │ │ ├── lightglue_interface.py │ │ └── superpoint_interface.py │ ├── ouroboros_opengv │ │ ├── __init__.py │ │ ├── _bindings.cpp │ │ └── pose_recovery.py │ └── ouroboros_salad │ │ ├── README.md │ │ ├── __init__.py │ │ └── salad_model.py └── tests │ ├── conftest.py │ ├── test_opengv_interface.py │ ├── test_pose_recovery.py │ ├── test_vlc_db.py │ └── test_vlc_pose.py ├── ouroboros_msgs ├── CMakeLists.txt ├── msg │ ├── SparkImageMsg.msg │ ├── VlcImageMetadataMsg.msg │ ├── VlcImageMsg.msg │ └── VlcInfoMsg.msg ├── package.xml └── srv │ └── VlcKeypointQuery.srv ├── ouroboros_ros ├── CMakeLists.txt ├── cfg │ └── PlaceDescriptorDebugging.cfg ├── config │ ├── gt_vlc_server_config.yaml │ ├── vlc_server_config.yaml │ └── vlc_server_node.yaml ├── launch │ ├── place_descriptor_debugging.launch │ └── vlc_server_node.launch ├── nodes │ ├── place_matching_example.py │ ├── vlc_multirobot_server_node.py │ └── vlc_server_node.py ├── package.xml ├── rviz │ └── ouroboros_debug.rviz ├── setup.py ├── src │ └── ouroboros_ros │ │ ├── __init__.py │ │ ├── conversions.py │ │ ├── session_manager.py │ │ ├── utils.py │ │ ├── vlc_multirobot_server_ros.py │ │ ├── vlc_server_ros.py │ │ └── vlc_server_ros_display.py └── tests │ └── test_conversions.py └── resources ├── arch.jpg ├── left_img_0.png ├── left_img_1.png └── right_img_1.png /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | --- 4 | Language: Cpp 5 | DerivePointerAlignment: false 6 | PointerAlignment: Left 7 | ColumnLimit: 120 8 | BinPackArguments: false 9 | BinPackParameters: false 10 | --- 11 | -------------------------------------------------------------------------------- /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 80 3 | extend-select = B950 4 | extend-ignore = E203,E501,E701 5 | per-file-ignores = 6 | *__init__.py:F401,F403,D104 7 | -------------------------------------------------------------------------------- /.github/workflows/ci-action-ros.yaml: -------------------------------------------------------------------------------- 1 | name: Ouroboros-ROS-CI 2 | run-name: Ouroboros-ROS-CI 3 | on: 4 | push: 5 | branches: main 6 | pull_request: 7 | branches: 8 | - main 9 | - develop 10 | jobs: 11 | Ouroboros-ROS-CI: 12 | runs-on: ubuntu-latest 13 | container: ros:noetic-ros-base-focal 14 | steps: 15 | - name: Update git 16 | run: sudo apt update && sudo apt install -y git 17 | - name: Check out repository code 18 | uses: actions/checkout@v4 19 | with: 20 | path: src/ouroboros_repo 21 | submodules: recursive 22 | - name: Dependencies 23 | run: | 24 | sudo apt install -y libeigen3-dev ros-noetic-cv-bridge python3-pip 25 | sudo pip install --upgrade pip 26 | sudo python3 -m pip install catkin_tools empy catkin_pkg 27 | - name: Install Ouroboros 28 | run: pwd && ls && cd src/ouroboros_repo && pwd && pip install ./ouroboros 29 | - name: Install ROS packages with rosdep 30 | shell: bash 31 | run: | 32 | source /opt/ros/noetic/setup.bash 33 | rosdep update 34 | rosdep install --from-paths src --ignore-src -r -s # do a dry-run first 35 | rosdep install --from-paths src --ignore-src -r -y 36 | - name: catkin build 37 | shell: bash 38 | run: | 39 | source /opt/ros/noetic/setup.bash 40 | catkin build -s 41 | - name: Run test script 42 | shell: bash 43 | run: | 44 | source devel/setup.bash 45 | cd src/ouroboros_repo 46 | pytest ouroboros_ros/tests 47 | - run: echo "🍏 This job's status is ${{ job.status }}." 48 | -------------------------------------------------------------------------------- /.github/workflows/ci-action.yml: -------------------------------------------------------------------------------- 1 | name: Ouroboros CI 2 | run-name: Ouroboros CI 3 | on: 4 | push: 5 | branches: main 6 | pull_request: 7 | branches: 8 | - main 9 | - develop 10 | jobs: 11 | Ouroboros-CI: 12 | runs-on: ubuntu-latest 13 | strategy: 14 | matrix: 15 | python-version: ["3.8", "3.13"] 16 | steps: 17 | - name: Check out repository code 18 | uses: actions/checkout@v4 19 | with: 20 | path: ouroboros_repo 21 | submodules: recursive 22 | - name: Set up Python ${{ matrix.python-version }} 23 | uses: actions/setup-python@v5 24 | with: 25 | python-version: ${{ matrix.python-version }} 26 | 27 | - name: Pre-commit 28 | run: pip install pre-commit && cd ${{ github.workspace }}/ouroboros_repo && pre-commit run --all-files 29 | - name: Dependencies 30 | run: | 31 | sudo apt install -y libeigen3-dev git 32 | - name: Install Ouroboros 33 | run: cd ${{ github.workspace }}/ouroboros_repo && pwd && pip install ./ouroboros 34 | - name: Run test script 35 | run: cd ${{ github.workspace }}/ouroboros_repo && pytest ouroboros --ignore=ouroboros/third_party 36 | - run: echo "🍏 This job's status is ${{ job.status }}." 37 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Created by https://www.toptal.com/developers/gitignore/api/python 2 | # Edit at https://www.toptal.com/developers/gitignore?templates=python 3 | 4 | ### Python ### 5 | # Byte-compiled / optimized / DLL files 6 | __pycache__/ 7 | *.py[cod] 8 | *$py.class 9 | 10 | # C extensions 11 | *.so 12 | 13 | # Distribution / packaging 14 | .Python 15 | build/ 16 | develop-eggs/ 17 | dist/ 18 | downloads/ 19 | eggs/ 20 | .eggs/ 21 | lib/ 22 | lib64/ 23 | parts/ 24 | sdist/ 25 | var/ 26 | wheels/ 27 | share/python-wheels/ 28 | *.egg-info/ 29 | .installed.cfg 30 | *.egg 31 | MANIFEST 32 | 33 | # PyInstaller 34 | # Usually these files are written by a python script from a template 35 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 36 | *.manifest 37 | *.spec 38 | 39 | # Installer logs 40 | pip-log.txt 41 | pip-delete-this-directory.txt 42 | 43 | # Unit test / coverage reports 44 | htmlcov/ 45 | .tox/ 46 | .nox/ 47 | .coverage 48 | .coverage.* 49 | .cache 50 | nosetests.xml 51 | coverage.xml 52 | *.cover 53 | *.py,cover 54 | .hypothesis/ 55 | .pytest_cache/ 56 | cover/ 57 | 58 | # Translations 59 | *.mo 60 | *.pot 61 | 62 | # Django stuff: 63 | *.log 64 | local_settings.py 65 | db.sqlite3 66 | db.sqlite3-journal 67 | 68 | # Flask stuff: 69 | instance/ 70 | .webassets-cache 71 | 72 | # Scrapy stuff: 73 | .scrapy 74 | 75 | # Sphinx documentation 76 | docs/_build/ 77 | 78 | # PyBuilder 79 | .pybuilder/ 80 | target/ 81 | 82 | # Jupyter Notebook 83 | .ipynb_checkpoints 84 | 85 | # IPython 86 | profile_default/ 87 | ipython_config.py 88 | 89 | # pyenv 90 | # For a library or package, you might want to ignore these files since the code is 91 | # intended to run in multiple environments; otherwise, check them in: 92 | # .python-version 93 | 94 | # pipenv 95 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 96 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 97 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 98 | # install all needed dependencies. 99 | #Pipfile.lock 100 | 101 | # poetry 102 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 103 | # This is especially recommended for binary packages to ensure reproducibility, and is more 104 | # commonly ignored for libraries. 105 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 106 | #poetry.lock 107 | 108 | # pdm 109 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 110 | #pdm.lock 111 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 112 | # in version control. 113 | # https://pdm.fming.dev/#use-with-ide 114 | .pdm.toml 115 | 116 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 117 | __pypackages__/ 118 | 119 | # Celery stuff 120 | celerybeat-schedule 121 | celerybeat.pid 122 | 123 | # SageMath parsed files 124 | *.sage.py 125 | 126 | # Environments 127 | .env 128 | .venv 129 | env/ 130 | venv/ 131 | ENV/ 132 | env.bak/ 133 | venv.bak/ 134 | 135 | # Spyder project settings 136 | .spyderproject 137 | .spyproject 138 | 139 | # Rope project settings 140 | .ropeproject 141 | 142 | # mkdocs documentation 143 | /site 144 | 145 | # mypy 146 | .mypy_cache/ 147 | .dmypy.json 148 | dmypy.json 149 | 150 | # Pyre type checker 151 | .pyre/ 152 | 153 | # pytype static type analyzer 154 | .pytype/ 155 | 156 | # Cython debug symbols 157 | cython_debug/ 158 | 159 | # PyCharm 160 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 161 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 162 | # and can be added to the global gitignore or merged into this file. For a more nuclear 163 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 164 | #.idea/ 165 | 166 | ### Python Patch ### 167 | # Poetry local configuration file - https://python-poetry.org/docs/configuration/#local-configuration 168 | poetry.toml 169 | 170 | # ruff 171 | .ruff_cache/ 172 | 173 | # LSP config files 174 | pyrightconfig.json 175 | 176 | # End of https://www.toptal.com/developers/gitignore/api/python 177 | 178 | # sphinx specific build info 179 | doc/_build 180 | 181 | *.swp 182 | 183 | examples/output 184 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "third_party/opengv"] 2 | path = ouroboros/third_party/opengv 3 | url = git@github.com:laurentkneip/opengv.git 4 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | # See https://pre-commit.com for more information 3 | # See https://pre-commit.com/hooks.html for more hooks 4 | repos: 5 | - repo: https://github.com/astral-sh/ruff-pre-commit 6 | # Ruff version. 7 | rev: v0.9.2 8 | hooks: 9 | - id: ruff 10 | args: [--select, I, --fix] 11 | # Run the linter. 12 | - id: ruff 13 | # Run the formatter. 14 | - id: ruff-format 15 | -------------------------------------------------------------------------------- /.ruff.toml: -------------------------------------------------------------------------------- 1 | [lint.per-file-ignores] 2 | "__init__.py" = ["F401", "F403"] 3 | 4 | [lint.isort] 5 | split-on-trailing-comma = true 6 | known-local-folder = ["ouroboros*"] 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2025 MIT-SPARK Lab, Massachusetts Institute of Technology. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Spark VLC 2 | 3 | ## Installation 4 | 5 | First install Eigen and other dependencies: 6 | ```bash 7 | apt install libeigen3-dev 8 | ``` 9 | 10 | Then clone this repo: 11 | ```bash 12 | git clone git@github.com:MIT-SPARK/Ouroboros.git --recursive 13 | ``` 14 | 15 | You may want to set up a virtual environment: 16 | ```bash 17 | # this can be whatever invocation of venv or virtual environment construction 18 | # method you like, though if you're running with ROS1 --system-site-packages 19 | # is the easiest way to get access to ROS. 20 | sudo apt install python3-venv 21 | python3 -m venv --system-site-packages path/to/env 22 | source path/to/env/bin/active 23 | pip install --upgrade pip # required on Ubuntu 20.04 24 | ``` 25 | 26 | > **Note**
27 | > If you forget to clone recursively, you can run `git submodule update --init --recursive`, though this is also done automatically during installation. 28 | 29 | Finally, install the library: 30 | ```bash 31 | cd Ouroboros 32 | pip install ./ouroboros 33 | ``` 34 | 35 | Individual components of the VLC pipeline may have extra dependencies which you can install via: 36 | ``` 37 | pip install ./ouroboros[learned] 38 | ``` 39 | 40 | You can run unit tests with 41 | ``` 42 | pytest ouroboros --ignore=ouroboros/third_party 43 | ``` 44 | 45 | See `examples/vlc_server_driver_example.py` for the best example of how to use 46 | the VLC Server abstraction. 47 | 48 | ## ROS Integration 49 | 50 | Ouroboros is integrated with ROS in `ouroboros_ros`, which is a valid 51 | ROS package. Once you have built `ouroboros_ros` in your ROS workspace, you can 52 | run the Ouroboros ROS node with `roslaunch ouroboros_ros 53 | vlc_server_node.launch`. When using the node with you datasets, you will need 54 | to properly remap the following topics: 55 | * `~image_in` 56 | * `~camera_info` 57 | * `~depth_in` 58 | 59 | You will also need to specify a couple of coordinate frames by rosparam: 60 | * `fixed_frame` -- Frame to consider "fixed" for pose hints (mostly for GT debugging or baseline comparisons) 61 | * `hint_body_frame` -- Frame to consider body for pose hints 62 | * `body_frame` -- Robot body frame, the frame that loop closures are computed with respect to 63 | * `camera_frame` 64 | 65 | ## Plugins and Configuration 66 | 67 | Ouroboros models the VLC pipeline as 68 | ``` 69 | Input Image --> Place Recognition --> Keypoint / Descriptor Extraction --> 70 | --> Query-Match Keypoint Association --> Pose Recovery --> Output Looop Closures 71 | ``` 72 | 73 | The recognition, keypoint/descriptor, association, and pose recovery methods 74 | can all be swapped out. Ouroboros uses a plugin-based system that uses the 75 | provided configuration to decide which module to load for each of these parts 76 | of the pipeline. These plugins are also auto-discovered, meaning that it is 77 | possible to extend Ouroboros without needs to add any new code to the Ouroboros 78 | repo itself. 79 | 80 | We will use the Salad place recognition module as an example of how to 81 | implement a custom module. It is implemented 82 | [here](ouroboros/src/ouroboros_salad/salad_model.py). A plugin should be a class (here 83 | `SaladModel`) that takes a configuration struct as an argument in the 84 | constructor. It must implement a function `infer`, which will be called at the 85 | approriate part of the VLC pipeline. For examples of the specific interface 86 | that `infer` must have for each part of the pipeline, check refer to the 87 | "ground truth" example modules [here](ouroboros/src/ouroboros_gt). A plugin should also 88 | have a `load` method to create an instant of the class from a configuration. 89 | 90 | Next, we need to define a configuration (here `SaladModelConfig`) which 91 | inherits from `ouroboros.config.Config`. This should be a dataclass with any 92 | configuration information that you would like to set from a file. It needs to 93 | use the `register_config` decorator to declare that it is a plugin of type 94 | `place_model`, with name `Salad`, and can be constructed into a class with 95 | constructor `SaladModel`. The resulting plugin can be loaded from a config file 96 | such as [vlc\_driver\_config.yaml](examples/config/vlc_driver_config.yaml) (see 97 | `VlcDriverConfig.load` in 98 | [vlc\_server\_driver\_example.py](examples/vlc_server_driver_example.py)). 99 | Plugins are automatically discovered in any top-level imports from packages 100 | that start with `ouroboros_`, so you can use plugins with Ouroboros even 101 | without adding them to the Ouroboros codebase. 102 | 103 | Note that it is possible to have recursive configurations, see e.g. 104 | `VlcDriverConfig` in 105 | [vlc\_server\_driver\_example.py](examples/vlc_server_driver_example.py). 106 | 107 | # Library Building Blocks 108 | 109 | The ROS node implementations are the highest-level and most "out-of-the-box" 110 | solutions that we provide, but they are pretty thin wrappers around the 111 | VlcServer abstraction. There is also a lower-level VLC Database abstraction 112 | that handles the storage and querying of images, embeddings, loop closures, 113 | etc. Currently this database abstraction is implemented in a very naive way, 114 | but the implementation should be able to be improved while maintaining exactly 115 | the same interface. 116 | 117 | 118 | ## VlcServer 119 | 120 | The VlcServer encapsulates the Visual Loop Closure pipeline. First, a sensor 121 | needs to be registered with the VlcServer with 122 | 123 | ```python 124 | session_id = vlc_server.register_camera(robot_id, camera_config, epoch_ns) 125 | ``` 126 | 127 | The `session_id` is a unique identifier for a camera in the current session. 128 | When new frames should be checked and stored for VLC, you can call 129 | `add_and_query_frame`: 130 | 131 | ```python 132 | loop_closure = vlc_server.add_and_query_frame(session_id, image, epoch_ns) 133 | ``` 134 | 135 | For basic use-cases, this is the only function you should need. However, the 136 | VlcServer also provides slightly lower-level functions for more advanced 137 | functionality, such as storing image embeddings without the actual image which 138 | is a common need in distributed VLC systems. 139 | 140 | 141 | ## VlcDb 142 | 143 | `VlcDb` provides an abstraction for storing and querying data related to 144 | sensors, images, and loop closures. Philosophically, `VlcDb` represents a 145 | relational database, although it is currently not implemented with a "real" 146 | database backend. There are four tables: 147 | * Image Table 148 | * Loop Closure Table 149 | * Session Table 150 | * Camera Table 151 | 152 | ### Image Table 153 | 154 | The image table stores the information related to a single image frame. This 155 | includes the RGB(D) image, and any byproducts like embedding vector, keypoints, 156 | descriptors, etc. The Image Table is the most relevant place for making 157 | improvements -- we need to execute vector queries based on the embedding 158 | vectors in this table, and serializing these images to disk is not handled yet. 159 | 160 | `add_image` and `get_image` are the most relevant interfaces. 161 | 162 | ### Loop Closure Table 163 | 164 | The loop closure table stores the history of loop closures that the robot has 165 | detected. This is pretty straightforward and not extensively used right now. 166 | 167 | `get_lc` and `iterate_lcs` are useful if you want to reason over loop closures found so far. 168 | 169 | ### Session Table 170 | 171 | This tracks each session, which is specific to a camera and continuous period of operation. 172 | 173 | See `add_session`. 174 | 175 | ### Camera Table 176 | 177 | This tracks each camera, for example intrinsics and which robot the camera is associated with. 178 | 179 | See `add_camera`. 180 | 181 | ### Queries 182 | 183 | The VlcDb provides an interface to several kinds of queries. The primary 184 | purpose of the query is to find images matching the supplied image embedding 185 | vector. However, there are certain filters that we might need to apply to these 186 | queries. For example, often we want the closest match that was observed more 187 | than N seconds ago. There are also cases where we would like to support batch 188 | queries (e.g. in multisession context) to increase speed. 189 | 190 | The most VLC-specific query is probably `query_embeddings_max_time`, which 191 | queries for the single closest embedding with time at most T. This is a very 192 | common query when running an online VLC module. There is also 193 | `query_embeddings` to find the closest embedding across any time. There are 194 | several other query functions, which allow for batch queries or filtering based 195 | on custom predicate function. Note that the batch queries *might* be faster 196 | than the single versions, although it depends on when we get around to 197 | optimizing the backend. The filter functions with a custom callable should be 198 | expected to be quite a bit slower than versions that don't use a custom 199 | callable. 200 | -------------------------------------------------------------------------------- /doc/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /doc/_static/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/doc/_static/.gitkeep -------------------------------------------------------------------------------- /doc/_templates/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/doc/_templates/.gitkeep -------------------------------------------------------------------------------- /doc/api/core.rst: -------------------------------------------------------------------------------- 1 | API Documentation for Ourobos 2 | ============================= 3 | 4 | .. automodule:: ouroboros 5 | :members: 6 | -------------------------------------------------------------------------------- /doc/api/opengv.rst: -------------------------------------------------------------------------------- 1 | API Documentation for Ourobos-OpenGV 2 | ==================================== 3 | 4 | .. automodule:: ouroboros_opengv 5 | :members: 6 | -------------------------------------------------------------------------------- /doc/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # For the full list of built-in configuration values, see the documentation: 4 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 5 | 6 | # -- Project information ----------------------------------------------------- 7 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information 8 | 9 | project = "Ouroboros" 10 | copyright = "2025, MIT-SPARK Lab, Massachusetts Institute of Technology" 11 | author = "Aaron Ray, Mohammed Ehab, Nathan Hughes, Toya Takahashi, Yun Chang" 12 | 13 | # -- General configuration --------------------------------------------------- 14 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration 15 | 16 | extensions = [ 17 | "sphinx.ext.autodoc", 18 | "sphinx.ext.intersphinx", 19 | "sphinx.ext.napoleon", 20 | ] 21 | 22 | templates_path = ["_templates"] 23 | exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] 24 | 25 | # -- Docstring configuration ------------------------------------------------- 26 | maximum_signature_line_length = 88 27 | autodoc_preserve_defaults = False 28 | autodoc_typehints = "description" 29 | 30 | # -- Options for HTML output ------------------------------------------------- 31 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output 32 | 33 | html_theme = "alabaster" 34 | html_static_path = ["_static"] 35 | html_theme_options = { 36 | "font_size": "12pt", 37 | } 38 | -------------------------------------------------------------------------------- /doc/index.rst: -------------------------------------------------------------------------------- 1 | .. Ouroboros documentation master file, created by 2 | sphinx-quickstart on Wed Jan 29 22:57:36 2025. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to Ouroboros's documentation! 7 | ===================================== 8 | 9 | .. toctree:: 10 | :maxdepth: 2 11 | :caption: Contents: 12 | 13 | api/core 14 | api/opengv 15 | 16 | 17 | Indices and tables 18 | ================== 19 | 20 | * :ref:`genindex` 21 | * :ref:`modindex` 22 | * :ref:`search` 23 | -------------------------------------------------------------------------------- /examples/config/lightglue_config.yaml: -------------------------------------------------------------------------------- 1 | feature_type: superpoint 2 | -------------------------------------------------------------------------------- /examples/config/salad_config.yaml: -------------------------------------------------------------------------------- 1 | embedding_size: 8448 2 | model_source: torchhub 3 | model_variant: serizba/salad 4 | weight_source: dinov2_salad 5 | -------------------------------------------------------------------------------- /examples/config/superpoint_config.yaml: -------------------------------------------------------------------------------- 1 | max_keypoints: 1024 2 | -------------------------------------------------------------------------------- /examples/config/vlc_driver_config.yaml: -------------------------------------------------------------------------------- 1 | camera_config: 2 | type: pinhole_camera 3 | fx: 415.69 4 | fy: 415.69 5 | cx: 360 6 | cy: 240 7 | server_config: 8 | place_method: 9 | type: Salad 10 | embedding_size: 8448 11 | model_source: torchhub 12 | model_variant: serizba/salad 13 | weight_source: dinov2_salad 14 | keypoint_method: 15 | type: SuperPoint 16 | max_keypoints: 1024 17 | #descriptor_method: null 18 | match_method: 19 | type: Lightglue 20 | feature_type: superpoint 21 | pose_method: 22 | type: opengv 23 | scale_recovery: true 24 | use_pnp_for_scale: false 25 | ransac: 26 | inlier_tolerance: 1.0e-6 27 | scale_ransac: 28 | inlier_tolerance: 1.0e-1 29 | lc_frame_lockout_s: 10 30 | place_match_threshold: 0.55 31 | strict_keypoint_evaluation: false 32 | display_method: 33 | type: opencv 34 | display_place_matches: True 35 | save_place_matches: True 36 | save_separate_place_matches: True 37 | display_keypoint_matches: True 38 | save_keypoint_matches: True 39 | display_inlier_keypoint_matches: True 40 | save_inlier_keypoint_matches: True 41 | save_dir: output 42 | -------------------------------------------------------------------------------- /examples/config_example.py: -------------------------------------------------------------------------------- 1 | import os 2 | import tempfile 3 | from dataclasses import dataclass 4 | from typing import Any 5 | 6 | from spark_config import Config, config_field, register_config 7 | 8 | 9 | class ExampleTopLevel: 10 | def __init__(self, config): 11 | self.config = config 12 | self.mouse = self.config.a_virtual_subconfig.create() 13 | 14 | 15 | @register_config("top_level", name="ExampleTopLevel", constructor=ExampleTopLevel) 16 | @dataclass 17 | class ExampleTopLevelConfig(Config): 18 | a_number: int = 0 19 | another_number: int = 0 20 | a_string: str = "a string" 21 | a_virtual_subconfig: Any = config_field("mouse", default="Mouse") 22 | 23 | 24 | class Mouse: 25 | def __init__(self, config): 26 | self.color = config.color 27 | 28 | 29 | @register_config("mouse", name="Mouse", constructor=Mouse) 30 | @dataclass 31 | class MouseConfig(Config): 32 | color: str = "gray" 33 | 34 | 35 | if __name__ == "__main__": 36 | tl_cfg = ExampleTopLevelConfig() 37 | 38 | d = tempfile.gettempdir() 39 | fn = os.path.join(d, "example_config.yaml") 40 | print(f"Saving example config in {fn}") 41 | tl_cfg.save(fn) 42 | 43 | """ 44 | Generates the following yaml file in config/example_config.yaml: 45 | a_number: 0 46 | a_string: a string 47 | a_virtual_subconfig: 48 | color: gray 49 | type: Mouse 50 | another_number: 0 51 | """ 52 | 53 | loaded_cfg = Config.load(ExampleTopLevelConfig, fn) 54 | print("loaded_cfg: ", loaded_cfg) 55 | -------------------------------------------------------------------------------- /examples/lightglue_example.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | 3 | import imageio.v3 as iio 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from lightglue.viz2d import plot_images, plot_matches 7 | 8 | import ouroboros as ob 9 | from ouroboros.utils.plotting_utils import display_kp_match_pair 10 | from ouroboros_keypoints.lightglue_interface import LightglueModel 11 | from ouroboros_keypoints.superpoint_interface import SuperPointModel 12 | 13 | 14 | def resource_dir(): 15 | return pathlib.Path(__file__).absolute().parent.parent / "resources" 16 | 17 | 18 | if __name__ == "__main__": 19 | superpoint_model = SuperPointModel.load("config/superpoint_config.yaml") 20 | lightglue_model = LightglueModel.load("config/lightglue_config.yaml") 21 | 22 | img_l = iio.imread(resource_dir() / "left_img_1.png") 23 | img_l = np.expand_dims(img_l, axis=-1) 24 | img_r = iio.imread(resource_dir() / "right_img_1.png") 25 | img_r = np.expand_dims(img_r, axis=-1) 26 | 27 | keypoints_l, descriptors_l = superpoint_model.infer(ob.SparkImage(rgb=img_l)) 28 | keypoints_r, descriptors_r = superpoint_model.infer(ob.SparkImage(rgb=img_r)) 29 | 30 | vlc_image_l = ob.VlcImage( 31 | None, ob.SparkImage(rgb=img_l), keypoints=keypoints_l, descriptors=descriptors_l 32 | ) 33 | vlc_image_r = ob.VlcImage( 34 | None, ob.SparkImage(rgb=img_r), keypoints=keypoints_r, descriptors=descriptors_r 35 | ) 36 | 37 | m_keypoints_l, m_keypoints_r = lightglue_model.infer(vlc_image_l, vlc_image_r) 38 | 39 | plt.figure() 40 | plt.title("left") 41 | plt.imshow(img_l) 42 | x, y = m_keypoints_l.T 43 | plt.scatter(x, y) 44 | 45 | plt.figure() 46 | plt.title("right") 47 | plt.imshow(img_r) 48 | x, y = m_keypoints_r.T 49 | plt.scatter(x, y) 50 | 51 | plt.figure() 52 | plot_images([img_l, img_r], titles=["left", "right"]) 53 | plot_matches(m_keypoints_l, m_keypoints_r) 54 | 55 | plt.ion() 56 | plt.show() 57 | 58 | display_kp_match_pair(vlc_image_l, vlc_image_r, m_keypoints_l, m_keypoints_r) 59 | -------------------------------------------------------------------------------- /examples/place_descriptor_debugging.py: -------------------------------------------------------------------------------- 1 | from datetime import datetime 2 | 3 | import cv2 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | from spark_dataset_interfaces.rosbag_dataloader import RosbagDataLoader 7 | 8 | import ouroboros as ob 9 | from ouroboros_salad.salad_model import SaladModel 10 | 11 | embedding_model = SaladModel.load("config/salad_config.yaml") 12 | 13 | 14 | def combine_images(left, right): 15 | r = left.shape[0] 16 | c = left.shape[1] * 2 + 10 17 | img_out = np.zeros((r, c, 3)) 18 | img_out[: left.shape[0], : left.shape[1], :] = left 19 | img_out[:, left.shape[1] + 10 :, :] = right 20 | return img_out 21 | 22 | 23 | lc_lockout = 5 # minimal time between two frames in loop closure 24 | 25 | # If s(a,b) is the similarity between image embeddings a and b, if s(a,b) < place_recognition_threshold are not considered putative loop closures 26 | place_recognition_threshold = 0.55 27 | 28 | # Load Data 29 | data_path = "/home/aaron/lxc_datashare/uHumans2_apartment_s1_00h.bag" 30 | rgb_topic = "/tesse/left_cam/rgb/image_raw" 31 | rgb_info_topic = "/tesse/left_cam/camera_info" 32 | body_frame = "base_link_gt" 33 | body_frame = "base_link_gt" 34 | map_frame = "world" 35 | 36 | loader = RosbagDataLoader( 37 | data_path, rgb_topic, rgb_info_topic, body_frame=body_frame, map_frame=map_frame 38 | ) 39 | 40 | images = None # numpy array 41 | poses = None # 7d vector 42 | 43 | vlc_db = ob.VlcDb(8448) 44 | robot_id = 0 45 | session_id = vlc_db.add_session(robot_id) 46 | 47 | uid_to_pose = {} 48 | full_poses = [] 49 | 50 | ### Batch LCD 51 | 52 | # Place embeddings 53 | print("Loading...") 54 | with loader: 55 | print("Loaded!") 56 | for idx, data in enumerate(loader): 57 | image = data.color 58 | pose = data.pose 59 | full_poses.append(pose.matrix()) 60 | 61 | simg = ob.SparkImage(rgb=image) 62 | uid = vlc_db.add_image(session_id, datetime.now(), simg) 63 | embedding = embedding_model.infer(simg) 64 | vlc_db.update_embedding(uid, embedding) 65 | 66 | # To check our estimate vs. GT later 67 | uid_to_pose[uid] = pose.matrix() 68 | 69 | 70 | times = [] 71 | closest = [] 72 | for img in vlc_db.iterate_images(): 73 | ts = img.metadata.epoch_ns 74 | matches = vlc_db.query_embeddings_filter( 75 | img.embedding, 1, lambda m, s: abs(m.metadata.epoch_ns - ts) > lc_lockout * 1e9 76 | ) 77 | times.append(ts / 1e9) 78 | closest.append(matches[0][0]) 79 | if matches[0][0] > place_recognition_threshold: 80 | right = matches[0][1].image.rgb 81 | else: 82 | right = None 83 | img = combine_images(img.image.rgb, right) 84 | cv2.imshow("matches", img.astype(np.uint8)) 85 | cv2.waitKey(30) 86 | 87 | t = [ti - times[0] for ti in times] 88 | plt.ion() 89 | plt.plot(t, closest) 90 | plt.xlabel("Time (s)") 91 | plt.ylabel("Best Descriptor Similarity") 92 | plt.title("Closest Descriptor s.t. Time Constraint") 93 | -------------------------------------------------------------------------------- /examples/salad_example.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | 3 | import imageio.v3 as iio 4 | 5 | import ouroboros as ob 6 | from ouroboros_salad.salad_model import SaladModel 7 | 8 | 9 | def resource_dir(): 10 | return pathlib.Path(__file__).absolute().parent.parent / "resources" 11 | 12 | 13 | if __name__ == "__main__": 14 | model = SaladModel.load("config/salad_config.yaml") 15 | 16 | img_d = iio.imread(resource_dir() / "arch.jpg") 17 | 18 | simg = ob.SparkImage(rgb=img_d) 19 | out = model.infer(simg) 20 | print("Salad model returned: ", out) 21 | -------------------------------------------------------------------------------- /examples/superpoint_example.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | 3 | import imageio.v3 as iio 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | 7 | import ouroboros as ob 8 | from ouroboros_keypoints.superpoint_interface import SuperPointModel 9 | 10 | 11 | def resource_dir(): 12 | return pathlib.Path(__file__).absolute().parent.parent / "resources" 13 | 14 | 15 | if __name__ == "__main__": 16 | model = SuperPointModel.load("superpoint_config.yaml") 17 | 18 | # img_d = iio.imread(resource_dir() / "arch.jpg") 19 | img_d = iio.imread(resource_dir() / "right_img_1.png") 20 | img_d = np.expand_dims(img_d, axis=-1) 21 | 22 | simg = ob.SparkImage(rgb=img_d) 23 | 24 | keypoints, descriptors = model.infer(simg) 25 | 26 | kp = keypoints 27 | desc = descriptors 28 | plt.imshow(img_d) 29 | x, y = kp.T 30 | plt.scatter(x, y) 31 | plt.show() 32 | -------------------------------------------------------------------------------- /examples/uhumans_example_batch.py: -------------------------------------------------------------------------------- 1 | from datetime import datetime 2 | 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | from scipy.spatial.transform import Rotation as R 6 | from spark_dataset_interfaces.rosbag_dataloader import RosbagDataLoader 7 | 8 | import ouroboros as ob 9 | from ouroboros_gt.gt_pose_recovery import get_gt_pose_model 10 | from ouroboros_salad.salad_model import get_salad_model 11 | 12 | 13 | def plot_heading(positions, rpy): 14 | for p, angles in zip(positions, rpy): 15 | y = angles[2] + np.pi / 2 16 | plt.plot([p[0], p[0] + np.cos(y)], [p[1], p[1] + np.sin(y)], color="k") 17 | 18 | 19 | embedding_model = get_salad_model() 20 | 21 | 22 | lc_lockout = 5 # minimal time between two frames in loop closure 23 | gt_lc_max_dist = 5 24 | 25 | # If s(a,b) is the similarity between image embeddings a and b, if s(a,b) < place_recognition_threshold are not considered putative loop closures 26 | place_recognition_threshold = 0.55 27 | 28 | # Load Data 29 | data_path = "/home/aaron/lxc_datashare/uHumans2_apartment_s1_00h.bag" 30 | rgb_topic = "/tesse/left_cam/rgb/image_raw" 31 | rgb_info_topic = "/tesse/left_cam/camera_info" 32 | body_frame = "base_link_gt" 33 | body_frame = "base_link_gt" 34 | map_frame = "world" 35 | 36 | loader = RosbagDataLoader( 37 | data_path, rgb_topic, rgb_info_topic, body_frame=body_frame, map_frame=map_frame 38 | ) 39 | 40 | images = None # numpy array 41 | poses = None # 7d vector 42 | 43 | vlc_db = ob.VlcDb(8448) 44 | robot_id = 0 45 | session_id = vlc_db.add_session(robot_id) 46 | 47 | uid_to_pose = {} 48 | full_poses = [] 49 | 50 | ### Batch LCD 51 | 52 | # Place embeddings 53 | print("Loading...") 54 | with loader: 55 | print("Loaded!") 56 | for idx, data in enumerate(loader): 57 | image = data.color 58 | pose = data.pose 59 | full_poses.append(pose.matrix()) 60 | 61 | if not idx % 2 == 0: 62 | continue 63 | simg = ob.SparkImage(rgb=image) 64 | vlc_pose = ob.VlcPose( 65 | time_ns=data.timestamp, 66 | position=pose.translation, 67 | rotation=pose.rotation.as_quat(), 68 | ) 69 | uid = vlc_db.add_image(session_id, datetime.now(), simg, pose_hint=vlc_pose) 70 | embedding = embedding_model.infer(simg, pose_hint=vlc_pose) 71 | vlc_db.update_embedding(uid, embedding) 72 | 73 | # To check our estimate vs. GT later 74 | uid_to_pose[uid] = pose.matrix() 75 | 76 | 77 | # Query for closest matches 78 | query_embeddings = np.array([image.embedding for image in vlc_db.iterate_images()]) 79 | 80 | 81 | matches = vlc_db.batch_query_embeddings_uuid_filter( 82 | vlc_db.get_image_keys(), 83 | 1, 84 | lambda q, v, s: q.metadata.epoch_ns > v.metadata.epoch_ns + lc_lockout * 1e9, 85 | ) 86 | 87 | putative_loop_closures = [] 88 | for matches_to_frame in matches: 89 | if len(matches_to_frame) == 0: 90 | continue 91 | similarity = matches_to_frame[0][0] 92 | if similarity < place_recognition_threshold: 93 | continue 94 | putative_loop_closures.append((matches_to_frame[0][1], matches_to_frame[0][2])) 95 | 96 | pose_model = get_gt_pose_model() 97 | # Recover poses from matching loop closures 98 | for img_from, img_to in putative_loop_closures: 99 | key_from = img_from.metadata.image_uuid 100 | key_to = img_to.metadata.image_uuid 101 | if img_from.keypoints is None or img_from.descriptors is None: 102 | # TODO: replace with real keypoints and descriptors 103 | # keypoints = generate_keypoints(img_from.image) 104 | # descriptors = generate_descriptors(img_from.image) 105 | keypoints = np.zeros([1, 2]) 106 | pose = uid_to_pose[key_from] 107 | desc = ob.VlcPose( 108 | time_ns=img_from.metadata.epoch_ns, 109 | position=pose[:3, 3], 110 | rotation=R.from_matrix(pose[:3, :3]).as_quat(), 111 | ).to_descriptor() 112 | descriptors = [desc] 113 | # descriptors = [img_from.embedding] 114 | 115 | vlc_db.update_keypoints(key_from, keypoints, descriptors) 116 | img_from = vlc_db.get_image(key_from) 117 | 118 | if img_to.keypoints is None or img_to.descriptors is None: 119 | # TODO: replace with real keypoints and descriptors 120 | # keypoints = generate_keypoints(img_to.image) 121 | # descriptors = generate_descriptors(img_to.image) 122 | keypoints = np.zeros([1, 2]) 123 | pose = uid_to_pose[key_to] 124 | desc = ob.VlcPose( 125 | time_ns=img_to.metadata.epoch_ns, 126 | position=pose[:3, 3], 127 | rotation=R.from_matrix(pose[:3, :3]).as_quat(), 128 | ).to_descriptor() 129 | descriptors = [desc] 130 | # descriptors = [img_to.embedding] 131 | 132 | vlc_db.update_keypoints(key_to, keypoints, descriptors) 133 | img_to = vlc_db.get_image(key_to) 134 | 135 | relative_pose = pose_model.infer(img_from, img_to) 136 | loop_closure = ob.SparkLoopClosure( 137 | from_image_uuid=key_from, 138 | to_image_uuid=key_to, 139 | f_T_t=relative_pose, 140 | quality=1, 141 | ) 142 | vlc_db.add_lc(loop_closure, session_id, creation_time=datetime.now()) 143 | 144 | 145 | # Plot estimated loop closures and ground truth trajectory 146 | positions = np.array([p[:3, 3] for p in full_poses]) 147 | rpy = np.array([R.from_matrix(p[:3, :3]).as_euler("xyz") for p in full_poses]) 148 | 149 | plt.ion() 150 | plt.plot(positions[:, 0], positions[:, 1], color="k") 151 | plt.scatter(positions[:, 0], positions[:, 1], color="k") 152 | plot_heading(positions, rpy) 153 | 154 | 155 | for lc in vlc_db.iterate_lcs(): 156 | w_T_from = uid_to_pose[lc.from_image_uuid] 157 | w_T_to = uid_to_pose[lc.to_image_uuid] 158 | 159 | inferred_pose_to = w_T_from @ lc.f_T_t 160 | 161 | # Plot point for the ground truth poses involved in the loop closure 162 | plt.scatter( 163 | [w_T_from[0, 3], w_T_to[0, 3]], 164 | [w_T_from[1, 3], w_T_to[1, 3]], 165 | color="r", 166 | ) 167 | 168 | plt.plot( 169 | [w_T_from[0, 3], inferred_pose_to[0, 3]], 170 | [w_T_from[1, 3], inferred_pose_to[1, 3]], 171 | color="r", 172 | ) 173 | 174 | plt.show() 175 | -------------------------------------------------------------------------------- /examples/vlc_db_example.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | from datetime import datetime 3 | 4 | import imageio.v3 as iio 5 | import numpy as np 6 | 7 | import ouroboros as ob 8 | 9 | vlc_db = ob.VlcDb(3) 10 | 11 | robot_id = 0 12 | session_id = vlc_db.add_session(robot_id) 13 | 14 | 15 | def load_resource_image(image_name): 16 | resource_path = pathlib.Path(__file__).absolute().parent.parent / "resources" 17 | return iio.imread(resource_path / image_name) 18 | 19 | 20 | def insert_image(db, image): 21 | uid = db.add_image(session_id, datetime.now(), ob.SparkImage(rgb=image)) 22 | 23 | # TODO: expand the example to generate these with a real VLC pipeline 24 | # db.update_embedding() 25 | db.update_keypoints( 26 | uid, np.random.random((30, 2)), descriptors=np.random.random((30, 512)) 27 | ) 28 | 29 | return uid 30 | 31 | 32 | img_a = load_resource_image("left_img_0.png") 33 | img_b = load_resource_image("left_img_1.png") 34 | img_c = load_resource_image("right_img_1.png") 35 | img_d = load_resource_image("arch.jpg") 36 | 37 | a_id = insert_image(vlc_db, img_a) 38 | vlc_db.update_embedding(a_id, np.array([1, 0, 0])) 39 | 40 | b_id = insert_image(vlc_db, img_b) 41 | vlc_db.update_embedding(b_id, np.array([0, 1, 1])) 42 | 43 | c_id = insert_image(vlc_db, img_c) 44 | vlc_db.update_embedding(c_id, np.array([0, 1.1, 1.1])) 45 | 46 | d_id = insert_image(vlc_db, img_d) 47 | vlc_db.update_embedding(d_id, np.array([10, 2, 2])) 48 | 49 | 50 | print("Print: vlc_db.image_table.get_image[a_id]:") 51 | print(vlc_db.get_image(a_id)) 52 | 53 | print("Querying 0,1,1") 54 | imgs, dists = vlc_db.query_embeddings(np.array([0, 1, 1]), 2) 55 | 56 | computed_ts = datetime.now() 57 | loop_closure = ob.SparkLoopClosure( 58 | from_image_uuid=a_id, 59 | to_image_uuid=b_id, 60 | f_T_t=np.eye(4), 61 | quality=1, 62 | ) 63 | 64 | # If you want to add a loop closure where the two poses do not correspond to 65 | # keyframe images in the database (not totally clear why you would want to do 66 | # this), you should just insert the two times into the database as images with 67 | # image=None 68 | 69 | lc_uuid = vlc_db.add_lc(loop_closure, session_id, creation_time=computed_ts) 70 | -------------------------------------------------------------------------------- /examples/vlc_server_driver_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | from dataclasses import dataclass 4 | from datetime import datetime 5 | 6 | import matplotlib.pyplot as plt 7 | import numpy as np 8 | import spark_config as sc 9 | from spark_config import Config, config_field 10 | from spark_dataset_interfaces.rosbag_dataloader import RosbagDataLoader 11 | 12 | import ouroboros as ob 13 | from ouroboros.utils.plotting_utils import plt_fast_pause 14 | 15 | # Must be False in i3 if you want to save consistent-sized overhead plots for animation 16 | LIVE_OVERHEAD_PLOT = True 17 | 18 | 19 | @dataclass 20 | class VlcDriverConfig(Config): 21 | camera_config: ob.PinholeCamera = config_field("camera") 22 | server_config: ob.VlcServerConfig = config_field("vlc_server", default="vlc_server") 23 | 24 | @classmethod 25 | def load(cls, path: str): 26 | return ob.config.Config.load(VlcDriverConfig, path) 27 | 28 | 29 | def update_plot(line, pts, images_to_pose): 30 | if len(images_to_pose) == 0: 31 | return 32 | 33 | positions = [] 34 | for _, v in images_to_pose.items(): 35 | positions.append(v.position) 36 | 37 | positions = np.array(positions) 38 | 39 | line.set_data(positions[:, 0], positions[:, 1]) 40 | pts.set_offsets(positions[:, :2]) 41 | ax = plt.gca() 42 | ax.relim() 43 | ax.autoscale_view() 44 | if LIVE_OVERHEAD_PLOT: 45 | plt.draw_all() 46 | plt_fast_pause(0.05) 47 | 48 | 49 | def get_query_and_est_match_position(lc, image_to_pose): 50 | query_pose = image_to_pose[lc.from_image_uuid] 51 | world_T_query = query_pose.as_matrix() 52 | 53 | match_T_query = lc.f_T_t 54 | 55 | world_T_match = world_T_query @ ob.invert_pose(match_T_query) 56 | 57 | query_position = query_pose.position 58 | est_match_position = world_T_match[:3, 3] 59 | return query_position, est_match_position 60 | 61 | 62 | def plot_lc(query_position, match_position, color): 63 | plt.plot( 64 | [query_position[0], match_position[0]], 65 | [query_position[1], match_position[1]], 66 | color=color, 67 | ) 68 | 69 | 70 | def plot_lc_est_pose(lc, image_to_pose): 71 | query_pose = image_to_pose[lc.from_image_uuid] 72 | world_T_query = query_pose.as_matrix() 73 | 74 | match_T_query = lc.f_T_t 75 | 76 | world_T_match = world_T_query @ ob.invert_pose(match_T_query) 77 | 78 | plt.plot( 79 | [world_T_match[0, 3], world_T_query[0, 3]], 80 | [world_T_match[1, 3], world_T_query[1, 3]], 81 | color="r", 82 | ) 83 | 84 | 85 | def plot_lc_gt_pose(lc, image_to_pose): 86 | query_pos = image_to_pose[lc.from_image_uuid].position 87 | match_pos = image_to_pose[lc.to_image_uuid].position 88 | plt.plot( 89 | [match_pos[0], query_pos[0]], 90 | [match_pos[1], query_pos[1]], 91 | color="r", 92 | ) 93 | 94 | 95 | plugins = sc.discover_plugins() 96 | print("Discovered Plugins: ") 97 | print(plugins) 98 | 99 | data_path = "/home/aaron/lxc_datashare/uHumans2_apartment_s1_00h.bag" 100 | rgb_topic = "/tesse/left_cam/rgb/image_raw" 101 | rgb_info_topic = "/tesse/left_cam/camera_info" 102 | depth_topic = "/tesse/depth_cam/mono/image_raw" 103 | body_frame = "base_link_gt" 104 | body_frame = "base_link_gt" 105 | map_frame = "world" 106 | 107 | loader = RosbagDataLoader( 108 | data_path, 109 | rgb_topic, 110 | rgb_info_topic, 111 | body_frame=body_frame, 112 | map_frame=map_frame, 113 | depth_topic=depth_topic, 114 | ) 115 | 116 | vlc_frame_period_s = 0.5 117 | 118 | images_to_pose = {} 119 | last_vlc_frame_time = None 120 | 121 | driver_config = VlcDriverConfig.load("config/vlc_driver_config.yaml") 122 | 123 | if not os.path.exists("output"): 124 | os.mkdir("output") 125 | log_path = os.path.join("output", datetime.now().strftime("%Y_%m_%d-%I_%M_%S_%p")) 126 | os.mkdir(log_path) 127 | overhead_plot_dir = os.path.join(log_path, "overhead_plots") 128 | os.mkdir(overhead_plot_dir) 129 | 130 | vlc_server = ob.VlcServer( 131 | driver_config.server_config, 132 | robot_id=0, 133 | log_path=log_path, 134 | ) 135 | 136 | 137 | session_id = vlc_server.register_camera(0, driver_config.camera_config, datetime.now()) 138 | 139 | plt.ion() 140 | plt.figure(figsize=(16, 9)) 141 | if LIVE_OVERHEAD_PLOT: 142 | plt.show() 143 | position_line = plt.plot([], [], color="g")[0] 144 | position_points = plt.scatter([], [], color="g") 145 | 146 | uid_to_pose = {} 147 | full_poses = [] 148 | with loader: 149 | for idx, data in enumerate(loader): 150 | image = data.color 151 | depth = data.depth 152 | pose = data.pose 153 | full_poses.append(pose.matrix()) 154 | time = data.timestamp 155 | 156 | if not ( 157 | last_vlc_frame_time is None 158 | or time - last_vlc_frame_time > vlc_frame_period_s * 1e9 159 | ): 160 | continue 161 | last_vlc_frame_time = time 162 | 163 | pose_flat = pose.flatten() 164 | camera_pose = ob.VlcPose(time, pose_flat[:3], pose_flat[3:]) 165 | if camera_pose is None: 166 | print("Cannot get current pose, skipping frame!") 167 | continue 168 | 169 | spark_image = ob.SparkImage(rgb=image, depth=depth) 170 | image_uuid, loop_closures = vlc_server.add_and_query_frame( 171 | session_id, spark_image, time, pose_hint=camera_pose 172 | ) 173 | images_to_pose[image_uuid] = camera_pose 174 | 175 | update_plot(position_line, position_points, images_to_pose) 176 | if loop_closures is not None: 177 | for lc in loop_closures: 178 | query_pos, match_pos = get_query_and_est_match_position( 179 | lc, images_to_pose 180 | ) 181 | true_match_pos = images_to_pose[lc.to_image_uuid].position 182 | if np.linalg.norm(true_match_pos - match_pos) < 1: 183 | plot_lc(query_pos, match_pos, "b") 184 | else: 185 | plot_lc(query_pos, match_pos, "r") 186 | plt.savefig(os.path.join(overhead_plot_dir, f"overhead_{time}.png")) 187 | -------------------------------------------------------------------------------- /ouroboros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.15...3.30) 2 | project(${SKBUILD_PROJECT_NAME} LANGUAGES CXX) 3 | 4 | find_package(Python COMPONENTS Interpreter Development.Module) 5 | find_package(pybind11 REQUIRED CONFIG) 6 | find_package(Eigen3 REQUIRED) 7 | 8 | if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/third_party/opengv/.git) 9 | find_package(Git REQUIRED) 10 | execute_process(COMMAND ${GIT_EXECUTABLE} submodule update --init --recursive 11 | WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) 12 | endif() 13 | 14 | get_target_property(EIGEN_INCLUDE_DIR Eigen3::Eigen 15 | INTERFACE_INCLUDE_DIRECTORIES) 16 | 17 | pybind11_add_module( 18 | _ouroboros_opengv 19 | src/ouroboros_opengv/_bindings.cpp 20 | third_party/opengv/src/absolute_pose/methods.cpp 21 | third_party/opengv/src/absolute_pose/modules/Epnp.cpp 22 | third_party/opengv/src/absolute_pose/modules/gp3p/code.cpp 23 | third_party/opengv/src/absolute_pose/modules/gp3p/init.cpp 24 | third_party/opengv/src/absolute_pose/modules/gp3p/reductors.cpp 25 | third_party/opengv/src/absolute_pose/modules/gp3p/spolynomials.cpp 26 | third_party/opengv/src/absolute_pose/modules/gpnp1/code.cpp 27 | third_party/opengv/src/absolute_pose/modules/gpnp1/init.cpp 28 | third_party/opengv/src/absolute_pose/modules/gpnp1/reductors.cpp 29 | third_party/opengv/src/absolute_pose/modules/gpnp1/spolynomials.cpp 30 | third_party/opengv/src/absolute_pose/modules/gpnp2/code.cpp 31 | third_party/opengv/src/absolute_pose/modules/gpnp2/init.cpp 32 | third_party/opengv/src/absolute_pose/modules/gpnp2/reductors.cpp 33 | third_party/opengv/src/absolute_pose/modules/gpnp2/spolynomials.cpp 34 | third_party/opengv/src/absolute_pose/modules/gpnp3/code.cpp 35 | third_party/opengv/src/absolute_pose/modules/gpnp3/init.cpp 36 | third_party/opengv/src/absolute_pose/modules/gpnp3/reductors.cpp 37 | third_party/opengv/src/absolute_pose/modules/gpnp3/spolynomials.cpp 38 | third_party/opengv/src/absolute_pose/modules/gpnp4/code.cpp 39 | third_party/opengv/src/absolute_pose/modules/gpnp4/init.cpp 40 | third_party/opengv/src/absolute_pose/modules/gpnp4/reductors.cpp 41 | third_party/opengv/src/absolute_pose/modules/gpnp4/spolynomials.cpp 42 | third_party/opengv/src/absolute_pose/modules/gpnp5/code.cpp 43 | third_party/opengv/src/absolute_pose/modules/gpnp5/init.cpp 44 | third_party/opengv/src/absolute_pose/modules/gpnp5/reductors.cpp 45 | third_party/opengv/src/absolute_pose/modules/gpnp5/spolynomials.cpp 46 | third_party/opengv/src/absolute_pose/modules/main.cpp 47 | third_party/opengv/src/absolute_pose/modules/upnp2.cpp 48 | third_party/opengv/src/absolute_pose/modules/upnp4.cpp 49 | third_party/opengv/src/math/Sturm.cpp 50 | third_party/opengv/src/math/arun.cpp 51 | third_party/opengv/src/math/cayley.cpp 52 | third_party/opengv/src/math/gauss_jordan.cpp 53 | third_party/opengv/src/math/quaternion.cpp 54 | third_party/opengv/src/math/roots.cpp 55 | third_party/opengv/src/point_cloud/methods.cpp 56 | third_party/opengv/src/relative_pose/methods.cpp 57 | third_party/opengv/src/relative_pose/modules/eigensolver/modules.cpp 58 | third_party/opengv/src/relative_pose/modules/fivept_kneip/code.cpp 59 | third_party/opengv/src/relative_pose/modules/fivept_kneip/init.cpp 60 | third_party/opengv/src/relative_pose/modules/fivept_kneip/reductors.cpp 61 | third_party/opengv/src/relative_pose/modules/fivept_kneip/spolynomials.cpp 62 | third_party/opengv/src/relative_pose/modules/fivept_nister/modules.cpp 63 | third_party/opengv/src/relative_pose/modules/fivept_stewenius/modules.cpp 64 | third_party/opengv/src/relative_pose/modules/ge/modules.cpp 65 | third_party/opengv/src/relative_pose/modules/main.cpp 66 | third_party/opengv/src/relative_pose/modules/sixpt/modules2.cpp 67 | third_party/opengv/src/sac_problems/absolute_pose/AbsolutePoseSacProblem.cpp 68 | third_party/opengv/src/sac_problems/point_cloud/PointCloudSacProblem.cpp 69 | third_party/opengv/src/sac_problems/relative_pose/CentralRelativePoseSacProblem.cpp 70 | third_party/opengv/src/triangulation/methods.cpp) 71 | target_link_libraries(_ouroboros_opengv PRIVATE Eigen3::Eigen) 72 | target_include_directories( 73 | _ouroboros_opengv PRIVATE third_party/opengv/include 74 | "${EIGEN_INCLUDE_DIR}/unsupported") 75 | 76 | install(TARGETS _ouroboros_opengv DESTINATION ouroboros_opengv) 77 | -------------------------------------------------------------------------------- /ouroboros/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/ouroboros/COLCON_IGNORE -------------------------------------------------------------------------------- /ouroboros/pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["scikit-build-core", "pybind11"] 3 | build-backend = "scikit_build_core.build" 4 | 5 | [project] 6 | name = "ouroboros" 7 | description="Modules and tools for visual loop closure pipelines" 8 | version = "0.0.1" 9 | authors = [ 10 | {name = "Aaron Ray", email = "aaronray@mit.edu"}, 11 | {name = "Nathan Hughes", email = "na26933@mit.edu"}, 12 | {name = "Yun Chang", email = "yunchang@mit.edu"}, 13 | ] 14 | dependencies = [ 15 | "numpy", 16 | "imageio", 17 | "matplotlib", 18 | "opencv-python", 19 | "pyyaml", 20 | "pytest", 21 | "scipy>=1.4.0", 22 | "spark_config @ git+https://github.com/MIT-SPARK/Spark-Config@main", 23 | ] 24 | 25 | [project.optional-dependencies] 26 | learned = [ 27 | "torch", 28 | "torchvision", 29 | "lightning", 30 | "pytorch-metric-learning", 31 | "lightglue @ git+https://github.com/cvg/LightGlue.git@main", 32 | ] 33 | 34 | [tool.scikit-build] 35 | wheel.packages = [ 36 | "src/ouroboros", 37 | "src/ouroboros_gt", 38 | "src/ouroboros_keypoints", 39 | "src/ouroboros_opengv", 40 | "src/ouroboros_salad", 41 | ] 42 | 43 | [tool.pytest] 44 | testpaths = "tests" 45 | addopts = ["--cov-report=term-missing"] 46 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/__init__.py: -------------------------------------------------------------------------------- 1 | from ouroboros.pose_recovery import * 2 | from ouroboros.vlc_db import ( 3 | KeypointSizeException, 4 | PinholeCamera, 5 | SparkImage, 6 | SparkLoopClosure, 7 | SparkLoopClosureMetadata, 8 | SparkSession, 9 | VlcDb, 10 | VlcImage, 11 | VlcImageMetadata, 12 | VlcPose, 13 | invert_pose, 14 | pose_from_quat_trans, 15 | ) 16 | from ouroboros.vlc_server.vlc_server import VlcServer, VlcServerConfig 17 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/pose_recovery.py: -------------------------------------------------------------------------------- 1 | """Module containing interfaces for pose recovery.""" 2 | 3 | import abc 4 | import logging 5 | from dataclasses import dataclass 6 | from typing import List, Optional 7 | 8 | import numpy as np 9 | 10 | from ouroboros.vlc_db.camera import PinholeCamera 11 | from ouroboros.vlc_db.vlc_image import VlcImage 12 | 13 | # TODO(nathan) use actual type alias once we move beyond 3.8 14 | # Matrix3d = np.ndarray[np.float64[3, 3]] 15 | # Matrix4d = np.ndarray[np.float64[4, 4]] 16 | Matrix3d = np.ndarray 17 | Matrix4d = np.ndarray 18 | 19 | 20 | def inverse_camera_matrix(K: Matrix3d) -> Matrix3d: 21 | """ 22 | Get inverse camera matrix. 23 | 24 | Args: 25 | K: Original camera matrix. 26 | 27 | Returns: 28 | Inverted camera matrix that takes pixel space coordinates to unit coordinates. 29 | """ 30 | K_inv = np.eye(3) 31 | K_inv[0, 0] = 1.0 / K[0, 0] 32 | K_inv[1, 1] = 1.0 / K[1, 1] 33 | K_inv[0, 2] = -K[0, 2] / K[0, 0] 34 | K_inv[1, 2] = -K[1, 2] / K[1, 1] 35 | return K_inv 36 | 37 | 38 | def get_bearings(K: Matrix3d, features: np.ndarray, depths: np.ndarray = None): 39 | """ 40 | Get bearings (and optionally points) from undistorted features in pixel space. 41 | 42 | Args: 43 | K: Camera matrix for features. 44 | features: Pixel coordinates in a Nx2 matrix. 45 | depths: Optional depths for pixel features. 46 | 47 | Returns: 48 | Tuple[np.ndarray, Optional[np.ndarray]]: Bearings (and points) in Nx3 matrices. 49 | """ 50 | K_inv = inverse_camera_matrix(K) 51 | versors = np.hstack((features, np.ones((features.shape[0], 1)))) 52 | versors = versors @ K_inv.T 53 | # for broadcasting to be correct needs to be [N, 1] to multiply/divide rowwise 54 | points = None if depths is None else versors * depths[..., np.newaxis] 55 | bearings = versors / np.linalg.norm(versors, axis=1)[..., np.newaxis] 56 | return bearings, points 57 | 58 | 59 | @dataclass 60 | class FeatureGeometry: 61 | """Associated feature geometry.""" 62 | 63 | bearings: np.ndarray 64 | depths: Optional[np.ndarray] = None 65 | points: Optional[np.ndarray] = None 66 | 67 | @property 68 | def is_metric(self): 69 | """Whether or not the features have depth.""" 70 | return self.depths is not None and self.points is not None 71 | 72 | @classmethod 73 | def from_image( 74 | cls, cam: PinholeCamera, img: VlcImage, indices: Optional[np.ndarray] = None 75 | ): 76 | """Get undistorted geometry from keypoints.""" 77 | if img.keypoint_depths is None: 78 | depths = img.get_feature_depths() 79 | else: 80 | depths = img.keypoint_depths 81 | 82 | keypoints = img.keypoints 83 | if indices is not None: 84 | keypoints = keypoints[indices, :] 85 | depths = None if depths is None else depths[indices] 86 | 87 | bearings, points = get_bearings(cam.K, keypoints, depths) 88 | return cls(bearings, depths, points) 89 | 90 | 91 | @dataclass 92 | class PoseRecoveryResult: 93 | """Result for pose recovery.""" 94 | 95 | match_T_query: Optional[Matrix4d] = None 96 | is_metric: bool = False 97 | inliers: Optional[List[int]] = None 98 | 99 | def __bool__(self): 100 | """Return whether or not there is a pose estimate.""" 101 | return self.match_T_query is not None 102 | 103 | @classmethod 104 | def metric(cls, match_T_query: Matrix4d, inliers: Optional[List[int]] = None): 105 | """Construct a metric result.""" 106 | return cls(match_T_query, True, inliers=inliers) 107 | 108 | @classmethod 109 | def nonmetric(cls, match_T_query: Matrix4d, inliers: Optional[List[int]] = None): 110 | """Construct a non-metric result.""" 111 | return cls(match_T_query, False, inliers=inliers) 112 | 113 | 114 | class PoseRecovery(abc.ABC): 115 | """ 116 | Abstract interface for a pose recovery method. 117 | 118 | Implementations must override _recover_pose which takes two sets of 119 | feature geometries and returns a PoseRecoveryResult 120 | """ 121 | 122 | # NOTE(nathan) documented with discussed API from earlier 123 | def recover_pose( 124 | self, 125 | query_camera: PinholeCamera, 126 | query: VlcImage, 127 | match_camera: PinholeCamera, 128 | match: VlcImage, 129 | query_to_match: np.ndarray, 130 | ): 131 | """ 132 | Recover pose from two frames and correspondences. 133 | 134 | Args: 135 | query_camera: Camera corresponding to query image 136 | query: Image and local features for query frame 137 | match_camera: Camera corresponding to match image 138 | match: Image and local features for match frame 139 | query_to_match: Nx2 correspondences between local features 140 | """ 141 | if query.keypoints is None or match.keypoints is None: 142 | logging.error("Keypoints required for pose recovery!") 143 | return None 144 | 145 | # TODO(nathan) undistortion 146 | 147 | query_geometry = FeatureGeometry.from_image( 148 | query_camera, query, query_to_match[:, 0] 149 | ) 150 | match_geometry = FeatureGeometry.from_image( 151 | match_camera, match, query_to_match[:, 1] 152 | ) 153 | return self._recover_pose(query_geometry, match_geometry) 154 | 155 | @abc.abstractmethod 156 | def _recover_pose(self, query: FeatureGeometry, match: FeatureGeometry): 157 | """ 158 | Implement actual pose recovery from feature geometry. 159 | 160 | Args: 161 | query: Feature bearings and associated landmarks if available for query 162 | match: Feature bearings and associated landmarks if available for match 163 | 164 | Returns: 165 | PoseRecoveryResult: match_T_query and associated information 166 | """ 167 | pass # pragma: no cover 168 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/ouroboros/src/ouroboros/utils/__init__.py -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/utils/plotting_utils.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import matplotlib 3 | import matplotlib.pyplot as plt 4 | import numpy as np 5 | 6 | cv2.startWindowThread() 7 | 8 | 9 | def plt_fast_pause(interval): 10 | backend = plt.rcParams["backend"] 11 | if backend in matplotlib.rcsetup.interactive_bk: 12 | figManager = matplotlib._pylab_helpers.Gcf.get_active() 13 | if figManager is not None: 14 | canvas = figManager.canvas 15 | if canvas.figure.stale: 16 | canvas.draw() 17 | canvas.start_event_loop(interval) 18 | 19 | 20 | def create_image_pair(left, right): 21 | r = left.shape[0] 22 | c = left.shape[1] * 2 + 10 23 | img_out = np.zeros((r, c, 3)) 24 | img_out[: left.shape[0], : left.shape[1], :] = left 25 | if right is not None: 26 | img_out[:, left.shape[1] + 10 :, :] = right 27 | 28 | return img_out 29 | 30 | 31 | def display_image_pair(left, right, window="place_matches", show=True): 32 | if right is None: 33 | img = create_image_pair(left.image.rgb, right) 34 | else: 35 | img = create_image_pair(left.image.rgb, right.image.rgb) 36 | cv2.imshow(window, img.astype(np.uint8)) 37 | if show: 38 | cv2.waitKey(10) 39 | 40 | 41 | def save_image_pair(left, right, fn): 42 | if right is None: 43 | img = create_image_pair(left.image.rgb, right) 44 | else: 45 | img = create_image_pair(left.image.rgb, right.image.rgb) 46 | cv2.imwrite(fn, img) 47 | 48 | 49 | def add_kp_matches(img, kp_left, kp_right, color=(0, 255, 0)): 50 | offset = img.shape[1] // 2 51 | for (l_x, l_y), (r_x, r_y) in zip(kp_left, kp_right): 52 | cv2.line(img, (l_x, l_y), (r_x + offset, r_y), color, 1) 53 | 54 | 55 | def create_kp_match_pair(vlc_left, vlc_right, kp_left, kp_right, color=(0, 255, 0)): 56 | img = vlc_left.image.rgb 57 | if img.ndim == 2 or img.shape[2] == 1: 58 | left_color = cv2.cvtColor( 59 | vlc_left.image.rgb.astype(np.uint8), cv2.COLOR_GRAY2RGB 60 | ) 61 | right_color = cv2.cvtColor( 62 | vlc_right.image.rgb.astype(np.uint8), cv2.COLOR_GRAY2RGB 63 | ) 64 | else: 65 | left_color = vlc_left.image.rgb.astype(np.uint8) 66 | right_color = vlc_right.image.rgb.astype(np.uint8) 67 | img = create_image_pair(left_color, right_color) 68 | add_kp_matches(img, kp_left.astype(int), kp_right.astype(int), color=color) 69 | return img 70 | 71 | 72 | def display_kp_match_pair( 73 | vlc_left, vlc_right, kp_left, kp_right, window="kp_matches", show=True 74 | ): 75 | img = create_kp_match_pair(vlc_left, vlc_right, kp_left, kp_right) 76 | cv2.imshow(window, img / 255) 77 | if show: 78 | cv2.waitKey(10) 79 | 80 | 81 | def save_kp_match_pair(vlc_left, vlc_right, kp_left, kp_right, fn): 82 | img = create_kp_match_pair(vlc_left, vlc_right, kp_left, kp_right) 83 | cv2.imwrite(fn, img) # need to divide by 255? 84 | 85 | 86 | def create_inlier_kp_match_pair( 87 | vlc_left, vlc_right, inlier_left, inlier_right, outlier_left, outlier_right 88 | ): 89 | img = create_kp_match_pair( 90 | vlc_left, vlc_right, inlier_left, inlier_right, color=(0, 255, 0) 91 | ) 92 | if len(outlier_left) > 0: 93 | add_kp_matches( 94 | img, outlier_left.astype(int), outlier_right.astype(int), color=(0, 0, 255) 95 | ) 96 | 97 | return img 98 | 99 | 100 | def display_inlier_kp_match_pair( 101 | vlc_left, 102 | vlc_right, 103 | inlier_left, 104 | inlier_right, 105 | outlier_left, 106 | outlier_right, 107 | window="kp_inlier_matches", 108 | show=True, 109 | ): 110 | img = create_inlier_kp_match_pair( 111 | vlc_left, vlc_right, inlier_left, inlier_right, outlier_left, outlier_right 112 | ) 113 | cv2.imshow(window, img / 255) 114 | if show: 115 | cv2.waitKey(10) 116 | 117 | 118 | def save_inlier_kp_match_pair( 119 | vlc_left, vlc_right, inlier_left, inlier_right, outlier_left, outlier_right, fn 120 | ): 121 | img = create_inlier_kp_match_pair( 122 | vlc_left, vlc_right, inlier_left, inlier_right, outlier_left, outlier_right 123 | ) 124 | cv2.imwrite(fn, img) 125 | 126 | 127 | def save_image(fn, vlc_image): 128 | cv2.imwrite(fn, vlc_image.image.rgb) 129 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/__init__.py: -------------------------------------------------------------------------------- 1 | from ouroboros.vlc_db.camera import * 2 | from ouroboros.vlc_db.spark_image import * 3 | from ouroboros.vlc_db.spark_loop_closure import * 4 | from ouroboros.vlc_db.spark_session import * 5 | from ouroboros.vlc_db.vlc_db import * 6 | from ouroboros.vlc_db.vlc_image import * 7 | from ouroboros.vlc_db.vlc_image_table import * 8 | from ouroboros.vlc_db.vlc_lc_table import * 9 | from ouroboros.vlc_db.vlc_pose import * 10 | from ouroboros.vlc_db.vlc_session_table import * 11 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/camera.py: -------------------------------------------------------------------------------- 1 | """Class representing a camera for a particular image.""" 2 | 3 | from dataclasses import dataclass 4 | from typing import Optional 5 | 6 | import numpy as np 7 | from spark_config import Config, register_config 8 | 9 | 10 | @register_config("camera", name="pinhole_camera") 11 | @dataclass 12 | class PinholeCamera(Config): 13 | """Class representing a pinhole camera.""" 14 | 15 | # NOTE(nathan) a unit camera seems like a sane default 16 | fx: float = 1.0 17 | fy: float = 1.0 18 | cx: float = 0.0 19 | cy: float = 0.0 20 | 21 | @property 22 | def K(self): 23 | """Get the (undistorted) camera matrix for the camera.""" 24 | mat = np.eye(3) 25 | mat[0, 0] = self.fx 26 | mat[1, 1] = self.fy 27 | mat[0, 2] = self.cx 28 | mat[1, 2] = self.cy 29 | return mat 30 | 31 | 32 | @dataclass 33 | class VlcCamera: 34 | """Class containing database information about a camera.""" 35 | 36 | session_id: str 37 | camera: PinholeCamera 38 | calibration_epoch_ns: Optional[int] = None 39 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/camera_table.py: -------------------------------------------------------------------------------- 1 | import uuid 2 | from typing import Optional 3 | 4 | from ouroboros.vlc_db.camera import PinholeCamera, VlcCamera 5 | from ouroboros.vlc_db.vlc_image import VlcImageMetadata 6 | 7 | 8 | class CameraTable: 9 | def __init__(self): 10 | self._camera_store = {} 11 | 12 | def add_camera( 13 | self, 14 | session_id: str, 15 | camera: PinholeCamera, 16 | calibration_time: Optional[int] = None, 17 | ) -> str: 18 | camera_uuid = str(uuid.uuid4()) 19 | vlc_camera = VlcCamera( 20 | session_id=session_id, camera=camera, calibration_epoch_ns=calibration_time 21 | ) 22 | self._camera_store[camera_uuid] = vlc_camera 23 | return camera_uuid 24 | 25 | def get_camera(self, image_metadata: VlcImageMetadata) -> Optional[VlcCamera]: 26 | # return camera calibration closest in time to the image timestamp 27 | image_session = image_metadata.session_id 28 | cameras_for_session = [ 29 | (k, v) 30 | for k, v in self._camera_store.items() 31 | if v.session_id == image_session 32 | ] 33 | if not cameras_for_session: 34 | return None 35 | 36 | dts = [ 37 | (abs(image_metadata.epoch_ns - v.calibration_epoch_ns), k) 38 | for k, v in cameras_for_session 39 | ] 40 | return self._camera_store[sorted(dts)[0][1]] 41 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/invertible_vector_store.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class InvertibleVectorStore: 5 | # TODO: Replace with this? https://github.com/nmslib/hnswlib/ 6 | def __init__( 7 | self, 8 | n_dim, 9 | ): 10 | self._vectors = np.empty((0, n_dim)) 11 | self._uuid_to_vector = {} 12 | self._local_idx_to_uuid = {} 13 | 14 | def __contains__(self, key): 15 | return key in self._uuid_to_vector 16 | 17 | def __getitem__(self, key): 18 | if key not in self._uuid_to_vector: 19 | raise IndexError() 20 | return self._uuid_to_vector[key] 21 | 22 | def __setitem__(self, key, newval): 23 | self._uuid_to_vector[key] = newval 24 | self._vectors = np.vstack([self._vectors, newval]) 25 | idx = len(self._vectors) - 1 26 | self._local_idx_to_uuid[idx] = key 27 | 28 | def query(self, embeddings, k=1, similarity_metric="ip"): 29 | if similarity_metric == "ip": 30 | similarities = embeddings @ self._vectors.T 31 | 32 | elif similarity_metric == "cos": 33 | raise NotImplementedError( 34 | "Cosine similarity not yet implemented as similarity metric" 35 | ) 36 | elif callable(similarity_metric): 37 | similarities = np.zeros((len(embeddings), len(self._vectors))) 38 | for ex, e in enumerate(embeddings): 39 | for vx, v in enumerate(self._vectors): 40 | similarities[ex, vx] = similarity_metric(e, v) 41 | 42 | else: 43 | raise NotImplementedError( 44 | f"InvertibleVectorStore does not implement similarity metric {similarity_metric}" 45 | ) 46 | 47 | uuid_list = [] 48 | similarity_list = [] 49 | for row in range(similarities.shape[0]): 50 | if k > 0: 51 | indices = np.squeeze(-similarities[row, :]).argsort()[:k] 52 | else: 53 | indices = np.squeeze(-similarities[row, :]).argsort() 54 | uuid_list.append([self._local_idx_to_uuid[i] for i in indices]) 55 | 56 | if k > 0: 57 | max_similarities = -np.sort(-similarities[row, :])[:k] 58 | else: 59 | max_similarities = -np.sort(-similarities[row, :]) 60 | similarity_list.append(max_similarities) 61 | 62 | return uuid_list, similarity_list 63 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/spark_image.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | 3 | import numpy as np 4 | 5 | 6 | @dataclass 7 | class SparkImage: 8 | rgb: np.ndarray = None 9 | depth: np.ndarray = None 10 | 11 | def __repr__(self): 12 | s = "" 13 | if self.rgb is not None: 14 | shape = self.rgb.shape 15 | s += f"RGB image {shape[0]} x {shape[1]}\n" 16 | if self.depth is not None: 17 | shape = self.depth.shape 18 | s += f"Depth image {shape[0]} x {shape[1]}\n" 19 | 20 | return s 21 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/spark_loop_closure.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | 3 | import numpy as np 4 | 5 | 6 | @dataclass 7 | class SparkLoopClosureMetadata: 8 | lc_uuid: str 9 | session_uuid: str 10 | creation_time: int 11 | 12 | 13 | @dataclass 14 | class SparkLoopClosure: 15 | from_image_uuid: str 16 | to_image_uuid: str 17 | f_T_t: np.ndarray # 4x4 18 | quality: float 19 | is_metric: bool 20 | metadata: SparkLoopClosureMetadata = None 21 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/spark_session.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | 3 | 4 | @dataclass 5 | class SparkSession: 6 | session_uuid: str 7 | start_time: int 8 | name: str 9 | robot_id: int 10 | sensor_id: int 11 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/utils.py: -------------------------------------------------------------------------------- 1 | import datetime 2 | 3 | EPOCH = datetime.datetime.utcfromtimestamp(0) 4 | 5 | 6 | def epoch_ns_from_datetime(dt): 7 | return (dt - EPOCH).total_seconds() * int(1e9) 8 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/vlc_db.py: -------------------------------------------------------------------------------- 1 | from datetime import datetime 2 | from typing import Callable, List, Optional, Tuple, TypeVar, Union 3 | 4 | import numpy as np 5 | 6 | from ouroboros.vlc_db.camera import PinholeCamera, VlcCamera 7 | from ouroboros.vlc_db.camera_table import CameraTable 8 | from ouroboros.vlc_db.spark_image import SparkImage 9 | from ouroboros.vlc_db.spark_loop_closure import SparkLoopClosure 10 | from ouroboros.vlc_db.utils import epoch_ns_from_datetime 11 | from ouroboros.vlc_db.vlc_image import VlcImage, VlcImageMetadata 12 | from ouroboros.vlc_db.vlc_image_table import VlcImageTable 13 | from ouroboros.vlc_db.vlc_lc_table import LcTable 14 | from ouroboros.vlc_db.vlc_pose import VlcPose 15 | from ouroboros.vlc_db.vlc_session_table import SessionTable 16 | 17 | 18 | class KeypointSizeException(BaseException): 19 | pass 20 | 21 | 22 | T = TypeVar("T") 23 | 24 | 25 | class VlcDb: 26 | def __init__(self, image_embedding_dimension): 27 | self._image_table = VlcImageTable(image_embedding_dimension) 28 | self._lc_table = LcTable() 29 | self._session_table = SessionTable() 30 | self._camera_table = CameraTable() 31 | 32 | def add_image( 33 | self, 34 | session_id: str, 35 | image_timestamp: Union[int, datetime], 36 | image: SparkImage, 37 | pose_hint: VlcPose = None, 38 | ) -> str: 39 | """image_timestamp should be a datetime object or integer number of nanoseconds""" 40 | return self._image_table.add_image( 41 | session_id, image_timestamp, image, pose_hint 42 | ) 43 | 44 | def has_image(self, image_uuid: str) -> bool: 45 | return self._image_table.has_image(image_uuid) 46 | 47 | def get_image(self, image_uuid: str) -> VlcImage: 48 | img = self._image_table.get_image(image_uuid) 49 | if img is None: 50 | raise KeyError("Image not in database") 51 | return img 52 | 53 | def get_image_keys(self) -> [str]: 54 | return self._image_table.get_image_keys() 55 | 56 | def iterate_images(self): 57 | for image in self._image_table.iterate_images(): 58 | yield image 59 | 60 | def query_embeddings( 61 | self, 62 | embedding: np.ndarray, 63 | k: int, 64 | similarity_metric: Union[str, callable] = "ip", 65 | ) -> ([VlcImage], [float]): 66 | """Embeddings is a NxD numpy array, where N is the number of queries and D is the descriptor size 67 | Queries for the top k matches. 68 | 69 | Returns the top k closest matches and the match distances 70 | """ 71 | 72 | assert embedding.ndim == 1, "Query embedding must be 1d vector" 73 | 74 | matches, similarities = self.batch_query_embeddings( 75 | np.array([embedding]), k, similarity_metric 76 | ) 77 | return matches[0], similarities[0] 78 | 79 | def query_embeddings_filter( 80 | self, 81 | embedding: np.ndarray, 82 | k: int, 83 | filter_function: Callable[[VlcImage, float], bool], 84 | similarity_metric: Union[str, callable] = "ip", 85 | ): 86 | ret = self.batch_query_embeddings_filter( 87 | np.array([embedding]), 88 | k, 89 | lambda _, img, sim: filter_function(img, sim), 90 | similarity_metric, 91 | ) 92 | 93 | return_matches = [(t[0], t[2]) for t in ret[0]] 94 | return return_matches 95 | 96 | def query_embeddings_max_time( 97 | self, 98 | embedding: np.ndarray, 99 | k: int, 100 | sessions_to_time_filter: [str], 101 | max_time: Union[float, int, datetime], 102 | similarity_metric: Union[str, callable] = "ip", 103 | search_sessions: Optional[List[str]] = None, 104 | ) -> ([VlcImage], [float]): 105 | """Query image embeddings to find the k closest vectors with timestamp older than max_time.""" 106 | 107 | if isinstance(max_time, datetime): 108 | max_time = epoch_ns_from_datetime(max_time) 109 | # NOTE: This is a placeholder implementation. Ideally, we re-implement 110 | # this to be more efficient and not iterate through the full set of 111 | # vectors 112 | 113 | def session_filter(_, vlc_image, similarity): 114 | if search_sessions is None: 115 | return True 116 | return vlc_image.metadata.session_id in search_sessions 117 | 118 | def time_filter(_, vlc_image, similarity): 119 | return ( 120 | vlc_image.metadata.epoch_ns < max_time 121 | or vlc_image.metadata.session_id not in sessions_to_time_filter 122 | ) 123 | 124 | def combined_filter(_, vlc_image, similarity): 125 | return time_filter(_, vlc_image, similarity) and session_filter( 126 | _, vlc_image, similarity 127 | ) 128 | 129 | ret = self.batch_query_embeddings_filter( 130 | np.array([embedding]), k, combined_filter, similarity_metric 131 | ) 132 | matches = [t[2] for t in ret[0]] 133 | similarities = [t[0] for t in ret[0]] 134 | return matches, similarities 135 | 136 | def batch_query_embeddings_uuid_filter( 137 | self, 138 | uuids: [str], 139 | k: int, 140 | filter_function: Callable[[VlcImage, VlcImage, float], bool], 141 | similarity_metric: Union[str, callable] = "ip", 142 | ): 143 | # get image for each uuid and call query_embeddings)filter 144 | 145 | embeddings = np.array([self.get_image(u).embedding for u in uuids]) 146 | images = [self.get_image(u) for u in uuids] 147 | return self.batch_query_embeddings_filter( 148 | embeddings, k, filter_function, similarity_metric, filter_metadata=images 149 | ) 150 | 151 | def batch_query_embeddings( 152 | self, 153 | embeddings: np.ndarray, 154 | k: int, 155 | similarity_metric: Union[str, callable] = "ip", 156 | ) -> ([[VlcImage]], [[float]]): 157 | """Embeddings is a NxD numpy array, where N is the number of queries and D is the descriptor size 158 | Queries for the top k matches. 159 | 160 | Returns the top k closest matches and the match distances 161 | """ 162 | 163 | assert embeddings.ndim == 2, ( 164 | f"Batch query requires an NxD array of query embeddings, not {embeddings.shape}" 165 | ) 166 | 167 | return self._image_table.query_embeddings(embeddings, k, similarity_metric) 168 | 169 | def batch_query_embeddings_filter( 170 | self, 171 | embeddings: np.ndarray, 172 | k: int, 173 | filter_function: Callable[[T, VlcImage, float], bool], 174 | similarity_metric: Union[str, callable] = "ip", 175 | filter_metadata: Optional[Union[T, List[T]]] = None, 176 | ) -> [[Tuple[float, T, VlcImage]]]: 177 | """Query image embeddings to find the k closest vectors that satisfy 178 | the filter function. Note that this query may be much slower than 179 | `query_embeddings_with_max_time` because it requires iterating over all 180 | stored images. 181 | """ 182 | 183 | if isinstance(filter_metadata, list): 184 | assert len(filter_metadata) == len(embeddings) 185 | elif filter_metadata is None: 186 | filter_metadata = [None] * len(embeddings) 187 | elif callable(filter_metadata): 188 | filter_metadata = [filter_metadata] * len(embeddings) 189 | else: 190 | raise Exception("filter function must be a list, callable, or None") 191 | 192 | matches, similarities = self.batch_query_embeddings( 193 | embeddings, -1, similarity_metric=similarity_metric 194 | ) 195 | 196 | filtered_matches_out = [] 197 | for metadata, matches_for_query, similarities_for_query in zip( 198 | filter_metadata, matches, similarities 199 | ): 200 | filtered_matches_for_query = [] 201 | n_matches = 0 202 | for match_image, similarity in zip( 203 | matches_for_query, similarities_for_query 204 | ): 205 | if filter_function(metadata, match_image, similarity): 206 | filtered_matches_for_query.append( 207 | (similarity, metadata, match_image) 208 | ) 209 | n_matches += 1 210 | 211 | if n_matches >= k: 212 | break 213 | 214 | filtered_matches_out.append(filtered_matches_for_query) 215 | 216 | return filtered_matches_out 217 | 218 | def update_embedding(self, image_uuid: str, embedding): 219 | self._image_table.update_embedding(image_uuid, embedding) 220 | return self.get_image(image_uuid) 221 | 222 | def update_keypoints(self, image_uuid: str, keypoints, descriptors=None): 223 | if descriptors is not None: 224 | if len(keypoints) != len(descriptors): 225 | raise KeypointSizeException() 226 | self._image_table.update_keypoints( 227 | image_uuid, keypoints, descriptors=descriptors 228 | ) 229 | return self.get_image(image_uuid) 230 | 231 | def update_keypoint_depths(self, image_uuid: str, keypoint_depths): 232 | self._image_table.update_keypoint_depths(image_uuid, keypoint_depths) 233 | return self.get_image(image_uuid) 234 | 235 | def get_keypoints(self, image_uuid: str): 236 | return self._image_table.get_keypoints(image_uuid) 237 | 238 | def drop_image(self, image_uuid: str): 239 | """This functionality is for marginalization / sparsification of history""" 240 | self._image_table.drop_image(image_uuid) 241 | 242 | def add_session(self, robot_id: int, sensor_id: int = 0, name: str = None) -> str: 243 | return self._session_table.add_session(robot_id, sensor_id, name) 244 | 245 | def insert_session( 246 | self, 247 | session_uuid: str, 248 | start_time: Union[int, datetime], 249 | robot_id: int, 250 | sensor_id: int = 0, 251 | name: str = None, 252 | ): 253 | return self._session_table.insert_session( 254 | session_uuid, start_time, robot_id, sensor_id, name 255 | ) 256 | 257 | def get_session(self, session_uuid: str): 258 | return self._session_table.get_session(session_uuid) 259 | 260 | def add_lc(self, loop_closure: SparkLoopClosure, session_uuid, creation_time=None): 261 | return self._lc_table.add_lc(loop_closure, session_uuid, creation_time) 262 | 263 | def get_lc(self, lc_uuid: str): 264 | return self._lc_table.get_lc(lc_uuid) 265 | 266 | def iterate_lcs(self): 267 | for lc in self._lc_table.iterate_lcs(): 268 | yield lc 269 | 270 | def add_camera( 271 | self, 272 | session_id: str, 273 | camera: PinholeCamera, 274 | calibration_time: Optional[int] = None, 275 | ): 276 | if calibration_time is None: 277 | calibration_time = 0 278 | return self._camera_table.add_camera(session_id, camera, calibration_time) 279 | 280 | def get_camera(self, metadata: VlcImageMetadata) -> Optional[VlcCamera]: 281 | return self._camera_table.get_camera(metadata) 282 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/vlc_image.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | 3 | import numpy as np 4 | 5 | from ouroboros.vlc_db.spark_image import SparkImage 6 | from ouroboros.vlc_db.vlc_pose import VlcPose 7 | 8 | 9 | @dataclass 10 | class VlcImageMetadata: 11 | image_uuid: str 12 | session_id: str 13 | epoch_ns: int 14 | 15 | 16 | @dataclass 17 | class VlcImage: 18 | metadata: VlcImageMetadata 19 | image: SparkImage 20 | embedding: np.ndarray = None 21 | keypoints: np.ndarray = None 22 | keypoint_depths: np.ndarray = None 23 | descriptors: np.ndarray = None 24 | pose_hint: VlcPose = None 25 | 26 | def get_feature_depths(self): 27 | """ 28 | Get depth corresponding to the keypoints for image features. 29 | 30 | Note that this has hard-to-detect artifacts from features at the boundary 31 | of an image. We clip all keypoints to be inside the image with the assumption 32 | that whatever is consuming the depths is robust to small misalignments. 33 | 34 | Args: 35 | data: Image to extract depth from 36 | 37 | Returns: 38 | Optiona[np.ndarray]: Depths for keypoints if possible to extract 39 | """ 40 | if self.keypoints is None: 41 | return None 42 | 43 | if self.image is None or self.image.depth is None: 44 | return None 45 | 46 | # NOTE(nathan) this is ugly, but: 47 | # - To index into the image we need to swap from (u, v) to (row, col) 48 | # - Numpy frustratingly doesn't have a buffered get, so we can't zero 49 | # out-of-bounds elements. This only gets used assuming an 50 | # outlier-robust method, so it should be fine 51 | dims = self.image.depth.shape 52 | limit = (dims[1] - 1, dims[0] - 1) 53 | coords = np.clip(np.round(self.keypoints), a_min=[0, 0], a_max=limit).astype( 54 | np.int64 55 | ) 56 | return self.image.depth[coords[:, 1], coords[:, 0]] 57 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/vlc_image_table.py: -------------------------------------------------------------------------------- 1 | import uuid 2 | from datetime import datetime 3 | from typing import Union 4 | 5 | import numpy as np 6 | 7 | from ouroboros.vlc_db.invertible_vector_store import InvertibleVectorStore 8 | from ouroboros.vlc_db.spark_image import SparkImage 9 | from ouroboros.vlc_db.utils import epoch_ns_from_datetime 10 | from ouroboros.vlc_db.vlc_image import VlcImage, VlcImageMetadata 11 | from ouroboros.vlc_db.vlc_pose import VlcPose 12 | 13 | 14 | class VlcImageTable: 15 | def __init__(self, image_embedding_dimension): 16 | self.metadata_store = {} 17 | self.image_store = {} 18 | self.embedding_store = InvertibleVectorStore(image_embedding_dimension) 19 | self.keypoints_store = {} 20 | self.keypoint_depths_store = {} 21 | self.descriptors_store = {} 22 | self.pose_store = {} 23 | 24 | def has_image(self, image_uuid): 25 | return image_uuid in self.metadata_store 26 | 27 | def get_image(self, image_uuid): 28 | if not self.has_image(image_uuid): 29 | return None 30 | metadata = self.metadata_store[image_uuid] 31 | image = self.image_store[image_uuid] 32 | if image_uuid in self.embedding_store: 33 | embedding = self.embedding_store[image_uuid] 34 | else: 35 | embedding = None 36 | keypoints = self.keypoints_store[image_uuid] 37 | keypoint_depths = self.keypoint_depths_store[image_uuid] 38 | descriptors = self.descriptors_store[image_uuid] 39 | pose = self.pose_store[image_uuid] 40 | 41 | vlc_image = VlcImage( 42 | metadata, image, embedding, keypoints, keypoint_depths, descriptors, pose 43 | ) 44 | return vlc_image 45 | 46 | def get_image_keys(self): 47 | ts_keys = [ 48 | (metadata.epoch_ns, key) for key, metadata in self.metadata_store.items() 49 | ] 50 | return [key for _, key in sorted(ts_keys)] 51 | 52 | def iterate_images(self): 53 | """Iterate through images according to ascending timestamp""" 54 | for key in self.get_image_keys(): 55 | yield self.get_image(key) 56 | 57 | def query_embeddings( 58 | self, 59 | embeddings: np.ndarray, 60 | k: int, 61 | similarity_metric: Union[str, callable] = "ip", 62 | ) -> ([[VlcImage]], [[float]]): 63 | """Embeddings is a NxD numpy array, where N is the number of queries and D is the descriptor size 64 | Queries for the top k matches. 65 | 66 | Returns the top k closest matches and the match distances 67 | """ 68 | 69 | uuid_lists, distance_lists = self.embedding_store.query( 70 | embeddings, k, similarity_metric 71 | ) 72 | images = [] 73 | for uuids, distances in zip(uuid_lists, distance_lists): 74 | images.append([self.get_image(uid) for uid in uuids]) 75 | 76 | return images, distance_lists 77 | 78 | def add_image( 79 | self, 80 | session_id: str, 81 | image_timestamp: Union[int, datetime], 82 | image: SparkImage, 83 | pose_hint: VlcPose = None, 84 | ) -> str: 85 | new_uuid = str(uuid.uuid4()) 86 | 87 | if isinstance(image_timestamp, datetime): 88 | image_timestamp = epoch_ns_from_datetime(image_timestamp) 89 | 90 | metadata = VlcImageMetadata( 91 | image_uuid=new_uuid, 92 | session_id=session_id, 93 | epoch_ns=image_timestamp, 94 | ) 95 | self.metadata_store[new_uuid] = metadata 96 | self.image_store[new_uuid] = image 97 | self.pose_store[new_uuid] = pose_hint 98 | self.keypoints_store[new_uuid] = None 99 | self.keypoint_depths_store[new_uuid] = None 100 | self.descriptors_store[new_uuid] = None 101 | return new_uuid 102 | 103 | def update_embedding(self, image_uuid: str, embedding): 104 | self.embedding_store[image_uuid] = embedding 105 | 106 | def update_keypoints(self, image_uuid: str, keypoints, descriptors=None): 107 | self.keypoints_store[image_uuid] = keypoints 108 | if descriptors is not None: 109 | assert len(keypoints) == len(descriptors) 110 | self.descriptors_store[image_uuid] = descriptors 111 | 112 | def update_keypoint_depths(self, image_uuid: str, keypoint_depths): 113 | self.keypoint_depths_store[image_uuid] = keypoint_depths 114 | 115 | def get_keypoints(self, image_uuid: str): 116 | return self.keypoints_store[image_uuid], self.descriptors_store[image_uuid] 117 | 118 | def get_keypoint_depths(self, image_uuid: str): 119 | return self.keypoint_depths_store[image_uuid] 120 | 121 | def drop_image(self, image_uuid: str): 122 | """This functionality is for marginalization / sparsification of history""" 123 | raise NotImplementedError("Cannot yet drop images from ImageStore") 124 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/vlc_lc_table.py: -------------------------------------------------------------------------------- 1 | import time 2 | import uuid 3 | 4 | from ouroboros.vlc_db.spark_loop_closure import ( 5 | SparkLoopClosure, 6 | SparkLoopClosureMetadata, 7 | ) 8 | 9 | 10 | class LcTable: 11 | def __init__(self): 12 | self._lc_store = {} 13 | 14 | def add_lc(self, loop_closure: SparkLoopClosure, session_uuid, creation_time=None): 15 | lc_uuid = str(uuid.uuid4()) 16 | if creation_time is None: 17 | creation_time = int(time.time() * 1e9) 18 | metadata = SparkLoopClosureMetadata( 19 | lc_uuid=lc_uuid, session_uuid=session_uuid, creation_time=creation_time 20 | ) 21 | loop_closure.metadata = metadata 22 | 23 | self._lc_store[lc_uuid] = loop_closure 24 | return lc_uuid 25 | 26 | def get_lc(self, lc_uuid: str) -> SparkLoopClosure: 27 | return self._lc_store[lc_uuid] 28 | 29 | def iterate_lcs(self): 30 | """Iterate through loop closures according to ascending computed timestamp""" 31 | ts_keys = [ 32 | (lc.metadata.creation_time, key) for key, lc in self._lc_store.items() 33 | ] 34 | for _, uid in sorted(ts_keys): 35 | yield self.get_lc(uid) 36 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/vlc_pose.py: -------------------------------------------------------------------------------- 1 | from dataclasses import dataclass 2 | 3 | import numpy as np 4 | from scipy.spatial.transform import Rotation as R 5 | 6 | 7 | def invert_pose(p): 8 | p_inv = np.zeros((4, 4)) 9 | p_inv[:3, :3] = p[:3, :3].T 10 | p_inv[:3, 3] = -p[:3, :3].T @ p[:3, 3] 11 | p_inv[3, 3] = 1 12 | return p_inv 13 | 14 | 15 | def pose_from_quat_trans(q, t): 16 | pose = np.zeros((4, 4)) 17 | Rmat = R.from_quat(q).as_matrix() 18 | pose[:3, :3] = Rmat 19 | pose[:3, 3] = np.squeeze(t) 20 | pose[3, 3] = 1 21 | return pose 22 | 23 | 24 | @dataclass 25 | class VlcPose: 26 | time_ns: int 27 | position: np.ndarray # x,y,z 28 | rotation: np.ndarray # qx, qy, qz, qw 29 | 30 | def to_descriptor(self): 31 | return np.hstack([[self.time_ns], self.position, self.rotation]) 32 | 33 | @classmethod 34 | def from_descriptor(cls, d): 35 | return cls(time_ns=d[0], position=d[1:4], rotation=d[4:]) 36 | 37 | def as_matrix(self): 38 | return pose_from_quat_trans(self.rotation, self.position) 39 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_db/vlc_session_table.py: -------------------------------------------------------------------------------- 1 | import uuid 2 | from datetime import datetime 3 | from typing import Union 4 | 5 | from ouroboros.vlc_db.spark_session import SparkSession 6 | from ouroboros.vlc_db.utils import epoch_ns_from_datetime 7 | 8 | 9 | class SessionTable: 10 | def __init__(self): 11 | self._session_store = {} 12 | 13 | def add_session(self, robot_id: int, sensor_id: int = 0, name: str = None) -> str: 14 | # Used when the VLC module is in charge of handling session logic 15 | 16 | session_uuid = str(uuid.uuid4()) 17 | self.insert_session(session_uuid, datetime.now(), robot_id, sensor_id, name) 18 | return session_uuid 19 | 20 | def insert_session( 21 | self, 22 | session_uuid: str, 23 | start_time: Union[int, datetime], 24 | robot_id: int, 25 | sensor_id: int = 0, 26 | name: str = None, 27 | ) -> str: 28 | # Used when some external "universal state manager" handles session logic 29 | 30 | if name is None: 31 | name = session_uuid 32 | 33 | if isinstance(start_time, datetime): 34 | start_time = epoch_ns_from_datetime(start_time) 35 | 36 | spark_session = SparkSession( 37 | session_uuid=session_uuid, 38 | start_time=start_time, 39 | name=name, 40 | robot_id=robot_id, 41 | sensor_id=sensor_id, 42 | ) 43 | self._session_store[session_uuid] = spark_session 44 | 45 | def get_session(self, session_uuid: str) -> SparkSession: 46 | return self._session_store[session_uuid] 47 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros/vlc_server/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/ouroboros/src/ouroboros/vlc_server/__init__.py -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_gt/__init__.py: -------------------------------------------------------------------------------- 1 | from ouroboros_gt.gt_descriptors import GtDescriptorModel, GtDescriptorModelConfig 2 | from ouroboros_gt.gt_keypoint_detection import GtKeypointModel, GtKeypointModelConfig 3 | from ouroboros_gt.gt_matches import GtMatchModel, GtMatchModelConfig 4 | from ouroboros_gt.gt_place_recognition import GtPlaceModel, GtPlaceModelConfig 5 | from ouroboros_gt.gt_pose_recovery import GtPoseModel, GtPoseModelConfig 6 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_gt/gt_descriptors.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | import numpy as np 6 | from spark_config import Config, register_config 7 | 8 | import ouroboros as ob 9 | 10 | 11 | class GtDescriptorModel: 12 | def __init__(self, config: GtDescriptorModelConfig): 13 | pass 14 | 15 | def infer(self, image: ob.SparkImage, keypoints: np.ndarray, pose_hint: ob.VlcPose): 16 | if pose_hint is None: 17 | raise Exception("GtDescriptorModel requires setting pose_hint") 18 | return np.array([pose_hint.to_descriptor()]) 19 | 20 | 21 | @register_config("descriptor_model", name="ground_truth", constructor=GtDescriptorModel) 22 | @dataclass 23 | class GtDescriptorModelConfig(Config): 24 | pass 25 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_gt/gt_keypoint_detection.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | import numpy as np 6 | from spark_config import Config, register_config 7 | 8 | import ouroboros as ob 9 | 10 | 11 | class GtKeypointModel: 12 | def __init__(self, config: GtKeypointModelConfig): 13 | pass 14 | 15 | def infer(self, image: ob.SparkImage, pose_hint: ob.VlcPose): 16 | return np.array([[np.nan, np.nan]]), None 17 | 18 | 19 | @register_config("keypoint_model", name="ground_truth", constructor=GtKeypointModel) 20 | @dataclass 21 | class GtKeypointModelConfig(Config): 22 | pass 23 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_gt/gt_matches.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | from spark_config import Config, register_config 6 | 7 | import ouroboros as ob 8 | 9 | 10 | class GtMatchModel: 11 | def __init__(self, config: GtMatchModelConfig): 12 | self.returns_descriptors = True 13 | 14 | def infer( 15 | self, image0: ob.VlcImage, image1: ob.VlcImage, pose_hint: ob.VlcPose = None 16 | ): 17 | return None, None, None 18 | 19 | 20 | @register_config("match_model", name="ground_truth", constructor=GtMatchModel) 21 | @dataclass 22 | class GtMatchModelConfig(Config): 23 | pass 24 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_gt/gt_place_recognition.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | import numpy as np 6 | from spark_config import Config, register_config 7 | 8 | import ouroboros as ob 9 | 10 | 11 | class GtPlaceModel: 12 | def __init__(self, config: GtPlaceModelConfig): 13 | self.embedding_size = config.embedding_size 14 | self.lc_recent_pose_lockout_ns = config.lc_recent_lockout_s * 1e9 15 | self.lc_distance_threshold = config.lc_distance_threshold 16 | 17 | def similarity_metric(self, embedding_query, embedding_stored): 18 | query_pose = ob.VlcPose.from_descriptor(embedding_query) 19 | stored_pose = ob.VlcPose.from_descriptor(embedding_stored) 20 | 21 | if stored_pose.time_ns > query_pose.time_ns - self.lc_recent_pose_lockout_ns: 22 | return -np.inf 23 | 24 | d = np.linalg.norm(query_pose.position - stored_pose.position) 25 | 26 | if d > self.lc_distance_threshold: 27 | return -np.inf 28 | else: 29 | return -d 30 | 31 | def infer(self, image: ob.SparkImage, pose_hint: ob.VlcPose): 32 | if pose_hint is None: 33 | raise Exception("GtPlaceModel requires setting pose_hint") 34 | return pose_hint.to_descriptor() 35 | 36 | 37 | @register_config("place_model", name="ground_truth", constructor=GtPlaceModel) 38 | @dataclass 39 | class GtPlaceModelConfig(Config): 40 | embedding_size: int = 8 41 | lc_recent_lockout_s: float = 20 42 | lc_distance_threshold: float = 5 43 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_gt/gt_pose_recovery.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | import numpy as np 6 | from spark_config import Config, register_config 7 | 8 | import ouroboros as ob 9 | from ouroboros.vlc_db.vlc_pose import invert_pose, pose_from_quat_trans 10 | 11 | 12 | def _recover_pose(query_pose, match_pose): 13 | w_T_query = pose_from_quat_trans(query_pose.rotation, query_pose.position) 14 | w_T_match = pose_from_quat_trans(match_pose.rotation, match_pose.position) 15 | match_T_query = invert_pose(w_T_match) @ w_T_query 16 | return match_T_query 17 | 18 | 19 | class GtPoseModel: 20 | def __init__(self, config: GtPoseModel): 21 | pass 22 | 23 | def recover_pose( 24 | self, 25 | query_camera: ob.PinholeCamera, 26 | query: ob.VlcImage, 27 | match_camera: ob.PinholeCamera, 28 | match: ob.VlcImage, 29 | query_to_match: np.ndarray, 30 | ): 31 | p1 = query.pose_hint 32 | p2 = match.pose_hint 33 | match_T_query = _recover_pose(p1, p2) 34 | return ob.PoseRecoveryResult.metric(match_T_query) 35 | 36 | 37 | @register_config("pose_model", name="ground_truth", constructor=GtPoseModel) 38 | @dataclass 39 | class GtPoseModelConfig(Config): 40 | pass 41 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_keypoints/__init__.py: -------------------------------------------------------------------------------- 1 | from ouroboros_keypoints.lightglue_interface import LightglueModel, LightglueModelConfig 2 | from ouroboros_keypoints.superpoint_interface import ( 3 | SuperPointModel, 4 | SuperPointModelConfig, 5 | ) 6 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_keypoints/lightglue_interface.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | import numpy as np 6 | import torch 7 | from lightglue import LightGlue 8 | from spark_config import Config, register_config 9 | 10 | import ouroboros as ob 11 | 12 | 13 | class LightglueModel: 14 | def __init__(self, config: LightglueModelConfig): 15 | self.model = LightGlue(features=config.feature_type).eval().cuda() 16 | self.returns_descriptors = True 17 | 18 | def infer( 19 | self, image0: ob.VlcImage, image1: ob.VlcImage, pose_hint: ob.VlcPose = None 20 | ): 21 | # Lightglue / Superpoint seem to expect (y,x) keypoints, but ours are storted in (x,y) 22 | kp0 = torch.Tensor( 23 | np.expand_dims(image0.keypoints[:, ::-1].copy(), axis=0) 24 | ).cuda() 25 | desc0 = torch.Tensor(np.expand_dims(image0.descriptors, axis=0)).cuda() 26 | kp1 = torch.Tensor( 27 | np.expand_dims(image1.keypoints[:, ::-1].copy(), axis=0) 28 | ).cuda() 29 | desc1 = torch.Tensor(np.expand_dims(image1.descriptors, axis=0)).cuda() 30 | 31 | with torch.no_grad(): 32 | matches01 = self.model( 33 | { 34 | "image0": {"keypoints": kp0, "descriptors": desc0}, 35 | "image1": {"keypoints": kp1, "descriptors": desc1}, 36 | } 37 | ) 38 | 39 | matches = matches01["matches"][0].cpu().numpy() 40 | 41 | m_kpts0, m_kpts1 = ( 42 | image0.keypoints[matches[:, 0]], 43 | image1.keypoints[matches[:, 1]], 44 | ) 45 | return m_kpts0, m_kpts1, matches 46 | 47 | @classmethod 48 | def load(cls, path): 49 | config = ob.config.Config.load(LightglueModelConfig, path) 50 | return cls(config) 51 | 52 | 53 | @register_config("match_model", name="Lightglue", constructor=LightglueModel) 54 | @dataclass 55 | class LightglueModelConfig(Config): 56 | feature_type: str = "superpoint" 57 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_keypoints/superpoint_interface.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | import numpy as np 6 | import torch 7 | from lightglue import SuperPoint 8 | from spark_config import Config, register_config 9 | 10 | import ouroboros as ob 11 | 12 | 13 | class SuperPointModel: 14 | def __init__(self, config: SuperPointModelConfig): 15 | self.model = SuperPoint(max_num_keypoints=config.max_keypoints).eval().cuda() 16 | self.returns_descriptors = True 17 | 18 | def infer(self, image: ob.SparkImage, pose_hint: ob.VlcPose = None): 19 | img_float = torch.tensor( 20 | np.array([image.rgb.transpose() / 255.0]).astype(np.float32) 21 | ).cuda() 22 | with torch.no_grad(): 23 | ret = self.model({"image": img_float}) 24 | 25 | # Superpoint seems to return (y, x) keypoint coordinates, but our interface assumes keypoints in (x,y) 26 | return ret["keypoints"].cpu().numpy()[0, :, ::-1], ret[ 27 | "descriptors" 28 | ].cpu().numpy()[0] 29 | 30 | @classmethod 31 | def load(cls, path: str): 32 | config = ob.config.Config.load(SuperPointModelConfig, path) 33 | return cls(config) 34 | 35 | 36 | @register_config("keypoint_model", name="SuperPoint", constructor=SuperPointModel) 37 | @dataclass 38 | class SuperPointModelConfig(Config): 39 | max_keypoints: int = 1024 40 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_opengv/__init__.py: -------------------------------------------------------------------------------- 1 | from ouroboros_opengv._ouroboros_opengv import ( 2 | RansacResult, 3 | Solver2d2d, 4 | Solver2d3d, 5 | recover_translation_2d3d, 6 | solve_2d2d, 7 | solve_2d3d, 8 | solve_3d3d, 9 | ) 10 | from ouroboros_opengv.pose_recovery import ( 11 | OpenGVPoseRecovery, 12 | OpenGVPoseRecoveryConfig, 13 | RansacConfig, 14 | ) 15 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_opengv/_bindings.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace py = pybind11; 11 | using namespace py::literals; 12 | 13 | using opengv::absolute_pose::AbsoluteAdapterBase; 14 | using opengv::point_cloud::PointCloudAdapterBase; 15 | using opengv::relative_pose::RelativeAdapterBase; 16 | using opengv::sac::Ransac; 17 | using opengv::sac_problems::point_cloud::PointCloudSacProblem; 18 | 19 | using RelativePoseProblem = opengv::sac_problems::relative_pose::CentralRelativePoseSacProblem; 20 | using AbsolutePoseProblem = opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem; 21 | 22 | using PyVec3Matrix = Eigen::Ref>; 23 | 24 | inline void checkInputs(const PyVec3Matrix& m1, 25 | const PyVec3Matrix& m2, 26 | const std::string& m1_name, 27 | const std::string& m2_name) { 28 | if (m1.cols() == m2.cols()) { 29 | return; 30 | } 31 | 32 | std::stringstream ss; 33 | ss << "Invalid input shapes! Given " << m1_name << ": [" << m1.rows() << ", " << m1.cols() << "] and " << m2_name 34 | << ": [" << m2.rows() << ", " << m2.cols() << "] (columns do not match: " << m1.cols() << " != " << m2.cols() 35 | << ")"; 36 | throw std::invalid_argument(ss.str()); 37 | } 38 | 39 | // NOTE(nathan) we don't expose priors for 2d-2d 40 | struct EigenRelativeAdaptor : public RelativeAdapterBase { 41 | EigenRelativeAdaptor(const PyVec3Matrix _src, const PyVec3Matrix _dest) 42 | : RelativeAdapterBase(Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity()), src(_src), dest(_dest) { 43 | checkInputs(src, dest, "src_bearings", "dest_bearings"); 44 | } 45 | 46 | Eigen::Vector3d getBearingVector1(size_t index) const override { return dest.block<3, 1>(0, index); } 47 | 48 | Eigen::Vector3d getBearingVector2(size_t index) const override { return src.block<3, 1>(0, index); } 49 | 50 | // TODO(nathan) think about weighted correspondences 51 | double getWeight(size_t index) const override { return 1.0; } 52 | 53 | Eigen::Vector3d getCamOffset1(size_t index) const override { return Eigen::Vector3d::Zero(); } 54 | 55 | Eigen::Matrix3d getCamRotation1(size_t index) const override { return Eigen::Matrix3d::Identity(); } 56 | 57 | Eigen::Vector3d getCamOffset2(size_t index) const override { return Eigen::Vector3d::Zero(); } 58 | 59 | Eigen::Matrix3d getCamRotation2(size_t index) const override { return Eigen::Matrix3d::Identity(); } 60 | 61 | size_t getNumberCorrespondences() const override { return dest.cols(); } 62 | 63 | const PyVec3Matrix src; 64 | const PyVec3Matrix dest; 65 | }; 66 | 67 | struct EigenAbsoluteAdaptor : public AbsoluteAdapterBase { 68 | EigenAbsoluteAdaptor(const PyVec3Matrix& _bearings, 69 | const PyVec3Matrix& _points, 70 | const Eigen::Matrix3d& rotation_prior = Eigen::Matrix3d::Identity()) 71 | : AbsoluteAdapterBase(Eigen::Vector3d::Zero(), rotation_prior), bearings(_bearings), points(_points) { 72 | checkInputs(bearings, points, "bearings", "points"); 73 | } 74 | 75 | Eigen::Vector3d getBearingVector(size_t index) const override { return bearings.block<3, 1>(0, index); } 76 | 77 | // TODO(nathan) think about weighted correspondences 78 | double getWeight(size_t index) const override { return 1.0; } 79 | 80 | Eigen::Vector3d getCamOffset(size_t index) const override { return Eigen::Vector3d::Zero(); } 81 | 82 | Eigen::Matrix3d getCamRotation(size_t index) const override { return Eigen::Matrix3d::Identity(); } 83 | 84 | Eigen::Vector3d getPoint(size_t index) const override { return points.block<3, 1>(0, index); } 85 | 86 | size_t getNumberCorrespondences() const override { return bearings.cols(); } 87 | 88 | const PyVec3Matrix bearings; 89 | const PyVec3Matrix points; 90 | }; 91 | 92 | struct EigenPointCloudAdaptor : public PointCloudAdapterBase { 93 | EigenPointCloudAdaptor(const PyVec3Matrix _src, 94 | const PyVec3Matrix _dest, 95 | const Eigen::Matrix3d& rotation_prior = Eigen::Matrix3d::Identity(), 96 | const Eigen::Vector3d& translation_prior = Eigen::Vector3d::Zero()) 97 | : PointCloudAdapterBase(translation_prior, rotation_prior), src(_src), dest(_dest) { 98 | checkInputs(src, dest, "src_points", "dest_points"); 99 | } 100 | 101 | Eigen::Vector3d getPoint1(size_t index) const override { return dest.block<3, 1>(0, index); } 102 | 103 | Eigen::Vector3d getPoint2(size_t index) const override { return src.block<3, 1>(0, index); } 104 | 105 | size_t getNumberCorrespondences() const override { return dest.cols(); } 106 | 107 | // TODO(nathan) think about weighted correspondences 108 | double getWeight(size_t index) const override { return 1.0; } 109 | 110 | const PyVec3Matrix src; 111 | const PyVec3Matrix dest; 112 | }; 113 | 114 | struct RansacResult { 115 | bool valid = false; 116 | Eigen::Matrix4d dest_T_src; 117 | std::vector inliers; 118 | operator bool() const { return valid; } 119 | 120 | RansacResult() = default; 121 | RansacResult(const Ransac& ransac) 122 | : valid(true), dest_T_src(Eigen::Matrix4d::Identity()), inliers(ransac.inliers_.begin(), ransac.inliers_.end()) { 123 | dest_T_src.block<3, 4>(0, 0) = ransac.model_coefficients_; 124 | } 125 | 126 | RansacResult(const Ransac& ransac) 127 | : valid(true), dest_T_src(Eigen::Matrix4d::Identity()), inliers(ransac.inliers_.begin(), ransac.inliers_.end()) { 128 | dest_T_src.block<3, 4>(0, 0) = ransac.model_coefficients_; 129 | } 130 | 131 | RansacResult(const Ransac& ransac) 132 | : valid(true), dest_T_src(Eigen::Matrix4d::Identity()), inliers(ransac.inliers_.begin(), ransac.inliers_.end()) { 133 | dest_T_src.block<3, 4>(0, 0) = ransac.model_coefficients_; 134 | } 135 | }; 136 | 137 | PYBIND11_MODULE(_ouroboros_opengv, module) { 138 | py::options options; 139 | options.disable_function_signatures(); 140 | 141 | module.doc() = R"(Module wrapping opengv two-view geometry solvers.)"; 142 | 143 | py::class_(module, "RansacResult", "Result struct for ransac result.") 144 | .def( 145 | "__bool__", [](const RansacResult& result) -> bool { return result; }, "Return validity of pose.") 146 | .def_readonly("valid", &RansacResult::valid, "Whether or not the pose result is valid.") 147 | .def_readonly("dest_T_src", &RansacResult::dest_T_src, "Transform from the src input to the destination input.") 148 | .def_readonly("inliers", &RansacResult::inliers, "Indicies of the inliers matching the model."); 149 | 150 | py::enum_(module, "Solver2d2d") 151 | .value("STEWENIUS", RelativePoseProblem::Algorithm::STEWENIUS) 152 | .value("NISTER", RelativePoseProblem::Algorithm::NISTER) 153 | .value("SEVENPT", RelativePoseProblem::Algorithm::SEVENPT) 154 | .value("EIGHTPT", RelativePoseProblem::Algorithm::EIGHTPT) 155 | .export_values(); 156 | 157 | // NOTE(nathan) we don't expose EPNP because it spams stdout and P2P because it uses 158 | // known rotation 159 | py::enum_(module, "Solver2d3d") 160 | .value("KNEIP", AbsolutePoseProblem::Algorithm::KNEIP) 161 | .value("GAO", AbsolutePoseProblem::Algorithm::GAO) 162 | .value("GP3P", AbsolutePoseProblem::Algorithm::GP3P) 163 | .export_values(); 164 | 165 | module.def( 166 | "solve_2d2d", 167 | [](const PyVec3Matrix& src, 168 | const PyVec3Matrix& dest, 169 | RelativePoseProblem::Algorithm solver, 170 | size_t max_iterations, 171 | double threshold, 172 | double probability, 173 | size_t min_inliers) -> RansacResult { 174 | EigenRelativeAdaptor adaptor(src, dest); 175 | Ransac ransac; 176 | ransac.max_iterations_ = max_iterations; 177 | ransac.threshold_ = threshold; 178 | ransac.probability_ = probability; 179 | ransac.sac_model_ = std::make_shared(adaptor, solver); 180 | if (src.cols() < ransac.sac_model_->getSampleSize()) { 181 | return {}; 182 | } 183 | 184 | if (!ransac.computeModel() || ransac.inliers_.size() < min_inliers) { 185 | return {}; 186 | } 187 | 188 | return ransac; 189 | }, 190 | R"(Recover two-view pose up-to-scale via feature correspondences. 191 | 192 | Args: 193 | src (numpy.ndarray[numpy.float64[3, n]]): Feature bearings arranged as (3, N) for the "src" input. 194 | dest (numpy.ndarray[numpy.float64[3, n]]): Feature bearings arranged as (3, N) for the "dest" input. 195 | solver (_ouroboros_opengv.Solver2d2d): Minimal solver type to use. 196 | max_iterations (int): Maximum RANSAC iterations. 197 | threshold (float): Inlier error threshold. 198 | probability (float): Likelihood that minimal indices contain at least one inlier. 199 | min_inliers (int): Minimum number of inliers after model selection. 200 | 201 | Returns: 202 | _ouroboros_opengv.RansacResult: Potentially valid dest_T_src and associated inliers)", 203 | "src"_a, 204 | "dest"_a, 205 | "solver"_a = RelativePoseProblem::Algorithm::STEWENIUS, 206 | "max_iterations"_a = 1000, 207 | "threshold"_a = 1.0e-2, 208 | "probability"_a = 0.99, 209 | "min_inliers"_a = 0); 210 | 211 | module.def( 212 | "solve_2d3d", 213 | [](const PyVec3Matrix& bearings, 214 | const PyVec3Matrix& points, 215 | AbsolutePoseProblem::Algorithm solver, 216 | size_t max_iterations, 217 | double threshold, 218 | double probability, 219 | size_t min_inliers) -> RansacResult { 220 | EigenAbsoluteAdaptor adaptor(bearings, points); 221 | Ransac ransac; 222 | ransac.max_iterations_ = max_iterations; 223 | ransac.threshold_ = threshold; 224 | ransac.probability_ = probability; 225 | ransac.sac_model_ = std::make_shared(adaptor, solver); 226 | if (bearings.cols() < ransac.sac_model_->getSampleSize()) { 227 | return {}; 228 | } 229 | 230 | if (!ransac.computeModel() || ransac.inliers_.size() < min_inliers) { 231 | return {}; 232 | } 233 | 234 | return ransac; 235 | }, 236 | R"(Recover two-view pose via PnP. 237 | 238 | Args: 239 | bearings (numpy.ndarray[numpy.float64[3, n]]): Feature bearings arranged as (3, N) for the "src" input. 240 | points (numpy.ndarray[numpy.float64[3, n]]): Corresponding points arranged as (3, N) for the "dest" input. 241 | solver (_ouroboros_opengv.Solver2d3d): PnP minimal solver type to use. 242 | max_iterations (int): Maximum RANSAC iterations. 243 | threshold (float): Inlier error threshold. 244 | probability (float): Likelihood that minimal indices contain at least one inlier. 245 | min_inliers (int): Minimum number of inliers after model selection. 246 | 247 | Returns: 248 | _ouroboros_opengv.RansacResult: Potentially valid dest_T_src and associated inliers)", 249 | "bearings"_a, 250 | "points"_a, 251 | "solver"_a = AbsolutePoseProblem::Algorithm::KNEIP, 252 | "max_iterations"_a = 1000, 253 | "threshold"_a = 1.0e-2, 254 | "probability"_a = 0.99, 255 | "min_inliers"_a = 0); 256 | 257 | module.def( 258 | "recover_translation_2d3d", 259 | [](const PyVec3Matrix& bearings, 260 | const PyVec3Matrix& points, 261 | const Eigen::Matrix3d& dest_R_src, 262 | size_t max_iterations, 263 | double threshold, 264 | double probability, 265 | size_t min_inliers) -> RansacResult { 266 | EigenAbsoluteAdaptor adaptor(bearings, points, dest_R_src); 267 | Ransac ransac; 268 | ransac.max_iterations_ = max_iterations; 269 | ransac.threshold_ = threshold; 270 | ransac.probability_ = probability; 271 | ransac.sac_model_ = std::make_shared(adaptor, AbsolutePoseProblem::Algorithm::TWOPT); 272 | if (bearings.cols() < ransac.sac_model_->getSampleSize()) { 273 | return {}; 274 | } 275 | 276 | if (!ransac.computeModel() || ransac.inliers_.size() < min_inliers) { 277 | return {}; 278 | } 279 | 280 | return ransac; 281 | }, 282 | R"(Recover metric translation from given rotation and bearing-landmark correspondences. 283 | 284 | Args: 285 | bearings (numpy.ndarray[numpy.float64[3, n]]): Feature bearings arranged as (3, N) for the "src" input. 286 | points (numpy.ndarray[numpy.float64[3, n]]): Corresponding points arranged as (3, N) for the "dest" input. 287 | dest_R_src (numpy.ndarray[numpy.float64[3, 3]]): Given rotation. 288 | max_iterations (int): Maximum RANSAC iterations. 289 | threshold (float): Inlier error threshold. 290 | probability (float): Likelihood that minimal indices contain at least one inlier. 291 | min_inliers (int): Minimum number of inliers after model selection. 292 | 293 | Returns: 294 | _ouroboros_opengv.RansacResult: Potentially valid dest_T_src and associated inliers)", 295 | "bearings"_a, 296 | "points"_a, 297 | "dest_R_src"_a, 298 | "max_iterations"_a = 1000, 299 | "threshold"_a = 1.0e-2, 300 | "probability"_a = 0.99, 301 | "min_inliers"_a = 0); 302 | 303 | module.def( 304 | "solve_3d3d", 305 | [](const PyVec3Matrix& src, 306 | const PyVec3Matrix& dest, 307 | size_t max_iterations, 308 | double threshold, 309 | double probability, 310 | size_t min_inliers) -> RansacResult { 311 | EigenPointCloudAdaptor adaptor(src, dest); 312 | Ransac ransac; 313 | ransac.max_iterations_ = max_iterations; 314 | ransac.threshold_ = threshold; 315 | ransac.probability_ = probability; 316 | ransac.sac_model_ = std::make_shared(adaptor); 317 | if (src.cols() < ransac.sac_model_->getSampleSize()) { 318 | return {}; 319 | } 320 | 321 | if (!ransac.computeModel() || ransac.inliers_.size() < min_inliers) { 322 | return {}; 323 | } 324 | 325 | return ransac; 326 | }, 327 | R"(Recover two-view pose via Arun's method. 328 | 329 | Args: 330 | src (numpy.ndarray[numpy.float64[3, n]]): Point cloud for the "src" input. 331 | dest (numpy.ndarray[numpy.float64[3, n]]): Point cloud arranged as (3, N) for the "dest" input. 332 | max_iterations (int): Maximum RANSAC iterations. 333 | threshold (float): Inlier error threshold. 334 | probability (float): Likelihood that minimal indices contain at least one inlier. 335 | min_inliers (int): Minimum number of inliers after model selection. 336 | 337 | Returns: 338 | _ouroboros_opengv.RansacResult: Potentially valid dest_T_src and associated inliers)", 339 | "src"_a, 340 | "dest"_a, 341 | "max_iterations"_a = 1000, 342 | "threshold"_a = 1.0e-2, 343 | "probability"_a = 0.99, 344 | "min_inliers"_a = 0); 345 | } 346 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_opengv/pose_recovery.py: -------------------------------------------------------------------------------- 1 | """Module containing opengv pose recovery implementation.""" 2 | 3 | import logging 4 | from dataclasses import dataclass, field 5 | from typing import Optional 6 | 7 | import numpy as np 8 | from spark_config import Config, ConfigFactory 9 | 10 | from ouroboros.pose_recovery import FeatureGeometry, PoseRecovery, PoseRecoveryResult 11 | from ouroboros.vlc_db.vlc_pose import invert_pose 12 | from ouroboros_opengv._ouroboros_opengv import ( 13 | RansacResult, 14 | Solver2d2d, 15 | recover_translation_2d3d, 16 | solve_2d2d, 17 | solve_3d3d, 18 | ) 19 | 20 | Logger = logging.getLogger(__name__) 21 | Matrix3d = np.ndarray 22 | 23 | 24 | @dataclass 25 | class RansacConfig(Config): 26 | """ 27 | RANSAC parameters used when recovering pose. 28 | 29 | Attributes: 30 | max_iterations: Maximum number of RANSAC iterations to perform 31 | inlier_tolerance: Inlier reprojection tolerance for model-fitting 32 | inlier_probability: Probability of drawing at least one inlier in model indices 33 | min_inliers: Minimum number of inliers 34 | """ 35 | 36 | max_iterations: int = 1000 37 | inlier_tolerance: float = 1.0e-4 38 | inlier_probability: float = 0.99 39 | min_inliers: int = 10 40 | 41 | 42 | @dataclass 43 | class OpenGVPoseRecoveryConfig(Config): 44 | """ 45 | Config for pose recovery. 46 | 47 | Attributes: 48 | solver: 2D-2D initial solver to use [STEWENIUS, NISTER, SEVENPT, EIGHTPT] 49 | ransac: RANSAC parameters for 2D-2D solver 50 | scale_recovery: Attempt to recover translation scale if possible 51 | mask_outliers: Mask rejected correspondences from 2d pose recovery 52 | use_pnp_for_scale: Toggles between P2P and Arun's method for scale recovery 53 | scale_ransac: RANSAC parameters for translation recovery 54 | min_cosine_similarity: Minimum similarity threshold to translation w/o scale 55 | """ 56 | 57 | solver: str = "NISTER" 58 | ransac: RansacConfig = field(default_factory=RansacConfig) 59 | scale_recovery: bool = True 60 | mask_outliers: bool = True 61 | use_pnp_for_scale: bool = True 62 | scale_ransac: RansacConfig = field(default_factory=RansacConfig) 63 | min_cosine_similarity: Optional[float] = None 64 | 65 | @property 66 | def solver_enum(self): 67 | """Get enum from string.""" 68 | if self.solver == "STEWENIUS": 69 | return Solver2d2d.STEWENIUS 70 | elif self.solver == "NISTER": 71 | return Solver2d2d.NISTER 72 | elif self.solver == "SEVENPT": 73 | return Solver2d2d.SEVENPT 74 | elif self.solver == "EIGHTPT": 75 | return Solver2d2d.EIGHTPT 76 | else: 77 | Logger.warning( 78 | "Invalid solver type '{self.solver}', defaulting to STEWENIUS!" 79 | ) 80 | return Solver2d2d.STEWENIUS 81 | 82 | 83 | class OpenGVPoseRecovery(PoseRecovery): 84 | """Class for performing pose recovery.""" 85 | 86 | def __init__(self, config: OpenGVPoseRecoveryConfig): 87 | """Initialize the opengv pose recovery class via a config.""" 88 | self._config = config 89 | 90 | @property 91 | def config(self): 92 | """Get underlying config.""" 93 | return self._config 94 | 95 | @classmethod 96 | def load(cls, path): 97 | """Load opengv pose recovery from file.""" 98 | config = Config.load(OpenGVPoseRecoveryConfig, path) 99 | return cls(config) 100 | 101 | def _recover_translation_3d3d( 102 | self, query: FeatureGeometry, match: FeatureGeometry, result_2d2d: RansacResult 103 | ): 104 | if not query.is_metric or not match.is_metric: 105 | Logger.warning( 106 | "Both inputs must have depth to use Arun's method for translation!" 107 | ) 108 | return None 109 | 110 | match_valid = (match.depths > 0.0) & np.isfinite(match.depths) 111 | query_valid = (query.depths > 0.0) & np.isfinite(query.depths) 112 | valid = match_valid & query_valid 113 | if self._config.mask_outliers: 114 | inlier_mask = np.zeros_like(valid) 115 | inlier_mask[result_2d2d.inliers] = True 116 | valid &= inlier_mask 117 | 118 | # TODO(nathan) adjust tolerance for 3d-3d solver? 119 | return solve_3d3d( 120 | query.points.T, 121 | match.points.T, 122 | max_iterations=self._config.scale_ransac.max_iterations, 123 | threshold=self._config.scale_ransac.inlier_tolerance, 124 | probability=self._config.scale_ransac.inlier_probability, 125 | min_inliers=self._config.scale_ransac.min_inliers, 126 | ) 127 | 128 | def _recover_translation_2d3d( 129 | self, 130 | query: FeatureGeometry, 131 | match: FeatureGeometry, 132 | dest_R_src: Matrix3d, 133 | inliers=np.ndarray, 134 | ): 135 | valid = (match.depths > 0.0) & np.isfinite(match.depths) 136 | if self._config.mask_outliers: 137 | inlier_mask = np.zeros_like(valid) 138 | inlier_mask[inliers] = True 139 | valid &= inlier_mask 140 | 141 | bearings = query.bearings[valid] 142 | points = match.points[valid] 143 | return recover_translation_2d3d( 144 | bearings.T, 145 | points.T, 146 | dest_R_src, 147 | max_iterations=self._config.scale_ransac.max_iterations, 148 | threshold=self._config.scale_ransac.inlier_tolerance, 149 | probability=self._config.scale_ransac.inlier_probability, 150 | min_inliers=self._config.scale_ransac.min_inliers, 151 | ) 152 | 153 | def _check_result(self, pose1, pose2): 154 | if self._config.min_cosine_similarity is None: 155 | return True 156 | 157 | t1 = pose1[:3, 3] / np.linalg.norm(pose1[:3, 3]) 158 | t2 = pose2[:3, 3] / np.linalg.norm(pose2[:3, 3]) 159 | return t1.dot(t2) < self._config.min_cosine_similarity 160 | 161 | def _recover_pose( 162 | self, query: FeatureGeometry, match: FeatureGeometry 163 | ) -> PoseRecoveryResult: 164 | """Recover pose up to scale from 2d correspondences.""" 165 | # order is src (query), dest (match) for dest_T_src (match_T_query) 166 | result = solve_2d2d( 167 | query.bearings.T, 168 | match.bearings.T, 169 | solver=self._config.solver_enum, 170 | max_iterations=self._config.ransac.max_iterations, 171 | threshold=self._config.ransac.inlier_tolerance, 172 | probability=self._config.ransac.inlier_probability, 173 | min_inliers=self._config.ransac.min_inliers, 174 | ) 175 | if not result: 176 | return PoseRecoveryResult() # by default, invalid 177 | 178 | nonmetric = not query.is_metric and not match.is_metric 179 | if not self._config.scale_recovery or nonmetric: 180 | return PoseRecoveryResult.nonmetric(result.dest_T_src, result.inliers) 181 | 182 | if not self._config.use_pnp_for_scale: 183 | metric_result = self._recover_translation_3d3d(query, match, result) 184 | if metric_result is None or not metric_result: 185 | return PoseRecoveryResult.nonmetric(result.dest_T_src, result.inliers) 186 | 187 | if not self._check_result(result.dest_T_src, metric_result.dest_T_src): 188 | return PoseRecoveryResult.nonmetric(result.dest_T_src, result.inliers) 189 | 190 | # NOTE(nathan) we override the recovered 3d-3d translation with the 191 | # more accurate 2d-2d rotation 192 | dest_T_src = metric_result.dest_T_src.copy() 193 | dest_T_src[:3, :3] = result.dest_T_src[:3, :3] 194 | return PoseRecoveryResult.metric(dest_T_src, result.inliers) 195 | 196 | dest_R_src = result.dest_T_src[:3, :3] 197 | need_inverse = False 198 | if not match.is_metric: 199 | # we need to invert the problem to get bearing vs. point order correct 200 | need_inverse = True 201 | metric_result = self._recover_translation_2d3d( 202 | match, query, dest_R_src.T, result.inliers 203 | ) 204 | else: 205 | metric_result = self._recover_translation_2d3d( 206 | query, match, dest_R_src, result.inliers 207 | ) 208 | 209 | if not metric_result: 210 | return PoseRecoveryResult.nonmetric(result.dest_T_src, result.inliers) 211 | 212 | dest_T_src = metric_result.dest_T_src 213 | if need_inverse: 214 | dest_T_src = invert_pose(dest_T_src) 215 | 216 | if not self._check_result(result.dest_T_src, dest_T_src): 217 | return PoseRecoveryResult.nonmetric(result.dest_T_src, result.inliers) 218 | 219 | return PoseRecoveryResult.metric(dest_T_src, result.inliers) 220 | 221 | 222 | ConfigFactory.register( 223 | OpenGVPoseRecoveryConfig, 224 | "pose_model", 225 | name="opengv", 226 | constructor=OpenGVPoseRecovery, 227 | ) 228 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_salad/README.md: -------------------------------------------------------------------------------- 1 | # Ouroboros-Salad 2 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_salad/__init__.py: -------------------------------------------------------------------------------- 1 | from ouroboros_salad.salad_model import SaladModel, SaladModelConfig 2 | -------------------------------------------------------------------------------- /ouroboros/src/ouroboros_salad/salad_model.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | import numpy as np 6 | import torch 7 | import torchvision.transforms as T 8 | from spark_config import Config, register_config 9 | 10 | import ouroboros as ob 11 | 12 | torch.backends.cudnn.benchmark = True 13 | 14 | 15 | class SaladModel: 16 | def __init__(self, config: SaladModelConfig): 17 | self.embedding_size = config.embedding_size 18 | self.similarity_metric = "ip" 19 | 20 | if config.model_source == "torchhub": 21 | model = torch.hub.load(config.model_variant, config.weight_source) 22 | self.model = model.eval().to("cuda") 23 | else: 24 | raise Exception(f"Unknown model source {config.model_source}") 25 | 26 | self.input_transform = get_input_transform((322, 434)) 27 | 28 | def infer(self, image: ob.SparkImage, pose_hint: ob.VlcPose = None): 29 | img_float = torch.tensor((image.rgb.transpose() / 255.0).astype(np.float32)) 30 | with torch.no_grad(): 31 | img = self.input_transform(img_float) 32 | out = self.model(img.unsqueeze(0).to("cuda")).cpu().numpy() 33 | return np.squeeze(out) 34 | 35 | @classmethod 36 | def load(cls, path): 37 | config = ob.config.Config.load(SaladModelConfig, path) 38 | return cls(config) 39 | 40 | 41 | @register_config("place_model", name="Salad", constructor=SaladModel) 42 | @dataclass 43 | class SaladModelConfig(Config): 44 | embedding_size: int = 8448 45 | model_source: str = "torchhub" 46 | model_variant: str = "serizba/salad" 47 | weight_source: str = "dinov2_salad" 48 | 49 | 50 | def get_input_transform(image_size=None): 51 | MEAN = [0.485, 0.456, 0.406] 52 | STD = [0.229, 0.224, 0.225] 53 | if image_size: 54 | return T.Compose( 55 | [ 56 | T.Resize(image_size, interpolation=T.InterpolationMode.BILINEAR), 57 | T.Normalize(mean=MEAN, std=STD), 58 | ] 59 | ) 60 | else: 61 | return T.Compose([T.ToTensor(), T.Normalize(mean=MEAN, std=STD)]) 62 | -------------------------------------------------------------------------------- /ouroboros/tests/conftest.py: -------------------------------------------------------------------------------- 1 | """Fixtures for unit tests.""" 2 | 3 | import pathlib 4 | 5 | import imageio.v3 as iio 6 | import pytest 7 | 8 | 9 | def _resource_dir(): 10 | return pathlib.Path(__file__).absolute().parent.parent.parent / "resources" 11 | 12 | 13 | @pytest.fixture() 14 | def resource_dir() -> pathlib.Path: 15 | """Get a path to the resource directory for tests.""" 16 | return _resource_dir() 17 | 18 | 19 | @pytest.fixture() 20 | def image_factory(): 21 | """Get images from resource directory.""" 22 | resource_path = _resource_dir() 23 | 24 | def factory(name): 25 | """Get image.""" 26 | img_path = resource_path / name 27 | img = iio.imread(img_path) 28 | assert img is not None 29 | return img 30 | 31 | return factory 32 | -------------------------------------------------------------------------------- /ouroboros/tests/test_opengv_interface.py: -------------------------------------------------------------------------------- 1 | """Test opengv solver.""" 2 | 3 | import unittest.mock as mock 4 | 5 | import numpy as np 6 | import pytest 7 | 8 | import ouroboros as ob 9 | import ouroboros_opengv as ogv 10 | 11 | 12 | def _get_test_pose(): 13 | angle = np.pi / 5.0 14 | match_R_query = np.array([np.sin(angle / 2), 0.0, 0.0, np.cos(angle / 2)]) 15 | match_t_query = np.array([0.4, -0.3, 0.1]).reshape((3, 1)) 16 | return ob.pose_from_quat_trans(match_R_query, match_t_query) 17 | 18 | 19 | def _transform_points(bearings, dest_T_src): 20 | return bearings @ dest_T_src[:3, :3].T + dest_T_src[:3, 3, np.newaxis].T 21 | 22 | 23 | def test_nonmetric_solver(): 24 | """Test that two-view geometry is called correctly.""" 25 | rng = np.random.default_rng(0) 26 | 27 | expected = _get_test_pose() # match_T_query 28 | query_features = rng.normal(size=(100, 2)) 29 | query_bearings, _ = ob.get_bearings(np.eye(3), query_features) 30 | match_bearings = _transform_points(query_bearings, expected) 31 | 32 | result = ogv.solve_2d2d( 33 | query_bearings.T, 34 | match_bearings.T, 35 | solver=ogv.Solver2d2d.NISTER, 36 | ).dest_T_src 37 | 38 | t_expected = expected[:3, 3] 39 | t_expected = t_expected / np.linalg.norm(t_expected) 40 | t_result = result[:3, 3] 41 | t_result = t_result / np.linalg.norm(t_result) 42 | assert result[:3, :3] == pytest.approx(expected[:3, :3], abs=1.0e-1) 43 | assert t_result == pytest.approx(t_expected, abs=1.0e-3) 44 | 45 | 46 | def test_pose_recovery_interface(): 47 | """Test that pose recovery interface logic is correct.""" 48 | rng = np.random.default_rng(0) 49 | 50 | match = ob.VlcImage(None, ob.SparkImage()) 51 | match.keypoints = rng.normal(size=(30, 2)) 52 | 53 | query = ob.VlcImage(None, ob.SparkImage()) 54 | query.keypoints = rng.normal(size=(30, 2)) 55 | 56 | indices = np.arange(query.keypoints.shape[0]) 57 | correspondences = np.vstack((indices, indices)).T 58 | 59 | config = ogv.OpenGVPoseRecoveryConfig() 60 | pose_recovery = ogv.OpenGVPoseRecovery(config) 61 | 62 | cam = ob.PinholeCamera() 63 | 64 | empty_image = ob.VlcImage(None, None) 65 | 66 | result = pose_recovery.recover_pose( 67 | cam, empty_image, cam, empty_image, correspondences 68 | ) 69 | assert result is None 70 | 71 | # for coverage to double-check non-metric solver works 72 | result = pose_recovery.recover_pose(cam, query, cam, match, correspondences) 73 | 74 | 75 | def test_pose_recovery(): 76 | """Test that pose recovery is correct.""" 77 | rng = np.random.default_rng(0) 78 | expected = _get_test_pose() # match_T_query 79 | 80 | match = ob.VlcImage(None, ob.SparkImage()) 81 | match.keypoints = rng.normal(size=(100, 2)) 82 | match_depths = rng.uniform(1.5, 2.5, match.keypoints.shape[0]) 83 | _, match_points = ob.get_bearings(np.eye(3), match.keypoints, match_depths) 84 | 85 | query = ob.VlcImage(None, ob.SparkImage()) 86 | query_points = _transform_points(match_points, ob.invert_pose(expected)) 87 | query.keypoints = query_points[:, :2] / query_points[:, 2, np.newaxis] 88 | 89 | indices = np.arange(query_points.shape[0]) 90 | new_indices = np.arange(query.keypoints.shape[0]) 91 | rng.shuffle(new_indices) 92 | query.keypoints = query.keypoints[new_indices, :].copy() 93 | query_depths = query_points[new_indices, 2] 94 | 95 | # needs to be query -> match (so need indices that were used by shuffle for match) 96 | correspondences = np.vstack((indices, new_indices)).T 97 | 98 | config = ogv.OpenGVPoseRecoveryConfig() 99 | pose_recovery = ogv.OpenGVPoseRecovery(config) 100 | 101 | cam = ob.PinholeCamera() 102 | 103 | depths = [query_depths, match_depths] 104 | with mock.patch.object(ob.VlcImage, "get_feature_depths", side_effect=depths): 105 | result = pose_recovery.recover_pose(cam, query, cam, match, correspondences) 106 | assert result 107 | assert result.is_metric 108 | assert result.match_T_query == pytest.approx(expected, abs=1.0e-3) 109 | 110 | # make sure that inverse solve works 111 | depths = [query_depths, None] 112 | with mock.patch.object(ob.VlcImage, "get_feature_depths", side_effect=depths): 113 | result = pose_recovery.recover_pose(cam, query, cam, match, correspondences) 114 | assert result 115 | assert result.is_metric 116 | assert result.match_T_query == pytest.approx(expected, abs=1.0e-3) 117 | 118 | # check non-metric return behavior 119 | depths = [query_depths, np.zeros_like(query_depths)] 120 | with mock.patch.object(ob.VlcImage, "get_feature_depths", side_effect=depths): 121 | result = pose_recovery.recover_pose(cam, query, cam, match, correspondences) 122 | assert result 123 | assert not result.is_metric 124 | 125 | # check 3d-3d solver 126 | config = ogv.OpenGVPoseRecoveryConfig() 127 | config.use_pnp_for_scale = False 128 | pose_recovery = ogv.OpenGVPoseRecovery(config) 129 | 130 | depths = [query_depths, match_depths] 131 | with mock.patch.object(ob.VlcImage, "get_feature_depths", side_effect=depths): 132 | result = pose_recovery.recover_pose(cam, query, cam, match, correspondences) 133 | assert result 134 | assert result.is_metric 135 | assert result.match_T_query == pytest.approx(expected, abs=1.0e-3) 136 | 137 | # 3d-3d solver fails without both depths 138 | depths = [query_depths, None] 139 | with mock.patch.object(ob.VlcImage, "get_feature_depths", side_effect=depths): 140 | result = pose_recovery.recover_pose(cam, query, cam, match, correspondences) 141 | assert result 142 | assert not result.is_metric 143 | -------------------------------------------------------------------------------- /ouroboros/tests/test_pose_recovery.py: -------------------------------------------------------------------------------- 1 | """Test image geometry functions.""" 2 | 3 | import numpy as np 4 | import pytest 5 | 6 | import ouroboros as ob 7 | 8 | 9 | def test_inverse_camera_matrix(): 10 | """Check that explicit inverse is correct.""" 11 | orig = np.array([[500.0, 0.0, 320.0], [0.0, 500.0, 240.0], [0.0, 0.0, 1.0]]) 12 | result = ob.inverse_camera_matrix(orig) 13 | assert result == pytest.approx(np.linalg.inv(orig)) 14 | 15 | 16 | def test_bearings(): 17 | """Check that bearing math is correct.""" 18 | K = np.array([[10.0, 0.0, 5.0], [0.0, 5.0, 2.5], [0.0, 0.0, 1.0]]) 19 | features = np.array([[5.0, 2.5], [15.0, 2.5], [5.0, -2.5]]) 20 | bearings, _ = ob.get_bearings(K, features) 21 | expected = np.array( 22 | [ 23 | [0.0, 0.0, 1.0], 24 | [1.0 / np.sqrt(2), 0.0, 1.0 / np.sqrt(2)], 25 | [0.0, -1.0 / np.sqrt(2), 1.0 / np.sqrt(2)], 26 | ] 27 | ) 28 | assert bearings == pytest.approx(expected) 29 | 30 | 31 | def test_points(): 32 | """Test that point conversion match is correct.""" 33 | K = np.array([[10.0, 0.0, 5.0], [0.0, 5.0, 2.5], [0.0, 0.0, 1.0]]) 34 | features = np.array([[5.0, 2.5], [15.0, 2.5], [5.0, -2.5]]) 35 | depths = np.array([0.9, 2.0, 3.0]) 36 | _, points = ob.get_bearings(K, features, depths) 37 | expected = np.array( 38 | [ 39 | [0.0, 0.0, 0.9], 40 | [2.0, 0.0, 2.0], 41 | [0.0, -3.0, 3.0], 42 | ] 43 | ) 44 | assert points == pytest.approx(expected) 45 | 46 | 47 | def test_feature_geometry(): 48 | """Test feature geometry creation.""" 49 | camera = ob.PinholeCamera(2.0, 2.0, 4.0, 3.0) 50 | img = ob.VlcImage(None, ob.SparkImage()) 51 | img.keypoints = np.array([[4.0, 3.0], [6.0, 3.0], [4.0, 1.0]]) 52 | geometry = ob.FeatureGeometry.from_image(camera, img) 53 | 54 | expected_bearings = np.array( 55 | [ 56 | [0.0, 0.0, 1.0], 57 | [1.0 / np.sqrt(2), 0.0, 1.0 / np.sqrt(2)], 58 | [0.0, -1.0 / np.sqrt(2), 1.0 / np.sqrt(2)], 59 | ] 60 | ) 61 | assert not geometry.is_metric 62 | assert geometry.bearings == pytest.approx(expected_bearings) 63 | 64 | img.image.depth = np.zeros((6, 8)) 65 | img.image.depth[3, 4] = 0.9 66 | img.image.depth[3, 6] = 2.0 67 | img.image.depth[1, 4] = 3.0 68 | geometry = ob.FeatureGeometry.from_image(camera, img) 69 | 70 | expected_depths = np.array([0.9, 2.0, 3.0]) 71 | expected_points = np.array( 72 | [ 73 | [0.0, 0.0, 0.9], 74 | [2.0, 0.0, 2.0], 75 | [0.0, -3.0, 3.0], 76 | ] 77 | ) 78 | assert geometry.is_metric 79 | assert geometry.bearings == pytest.approx(expected_bearings) 80 | assert geometry.depths == pytest.approx(expected_depths) 81 | assert geometry.points == pytest.approx(expected_points) 82 | 83 | geometry = ob.FeatureGeometry.from_image(camera, img, np.array([1])) 84 | 85 | assert geometry.is_metric 86 | assert geometry.bearings == pytest.approx(expected_bearings[np.newaxis, 1]) 87 | assert geometry.depths == pytest.approx(expected_depths[np.newaxis, 1]) 88 | assert geometry.points == pytest.approx(expected_points[np.newaxis, 1]) 89 | 90 | 91 | def test_pose_result_construction(): 92 | """Test that constructor invariants work.""" 93 | invalid = ob.PoseRecoveryResult() 94 | assert not invalid 95 | 96 | valid = ob.PoseRecoveryResult(np.eye(4)) 97 | assert valid 98 | 99 | metric = ob.PoseRecoveryResult.metric(np.eye(4)) 100 | assert metric 101 | assert metric.is_metric 102 | 103 | nonmetric = ob.PoseRecoveryResult.nonmetric(np.eye(4)) 104 | assert nonmetric 105 | assert not nonmetric.is_metric 106 | -------------------------------------------------------------------------------- /ouroboros/tests/test_vlc_db.py: -------------------------------------------------------------------------------- 1 | import time 2 | from datetime import datetime 3 | 4 | import numpy as np 5 | import pytest 6 | 7 | import ouroboros as ob 8 | 9 | 10 | def test_add_image(image_factory): 11 | vlc_db = ob.VlcDb(3) 12 | session_id = vlc_db.add_session(0) 13 | 14 | img_a = image_factory("arch.jpg") 15 | img_b = image_factory("left_img_0.png") 16 | 17 | key_a = vlc_db.add_image(session_id, datetime.now(), ob.SparkImage(rgb=img_a)) 18 | key_b = vlc_db.add_image(session_id, time.time() * 1e9, ob.SparkImage(rgb=img_b)) 19 | 20 | assert key_a != key_b, "Image UUIDs not unique" 21 | 22 | vlc_img_a = vlc_db.get_image(key_a) 23 | assert key_a == vlc_img_a.metadata.image_uuid, "Inconsistent image_uuid" 24 | assert session_id == vlc_img_a.metadata.session_id, "Inconsistent session_id" 25 | 26 | vlc_img_b = vlc_db.get_image(key_b) 27 | 28 | assert vlc_img_b.metadata.epoch_ns > vlc_img_a.metadata.epoch_ns 29 | 30 | with pytest.raises(KeyError): 31 | vlc_db.get_image(0) 32 | 33 | 34 | def test_iterate_image(): 35 | vlc_db = ob.VlcDb(3) 36 | session_id = vlc_db.add_session(0) 37 | for _ in range(10): 38 | t_rand = int(np.random.random() * 1e9) 39 | vlc_db.add_image(session_id, t_rand, None) 40 | 41 | t_last = -np.inf 42 | for img in vlc_db.iterate_images(): 43 | t = img.metadata.epoch_ns 44 | assert t >= t_last, "iterate_images epoch_ns not ascending" 45 | t_last = t 46 | 47 | 48 | def test_get_image_keys(): 49 | vlc_db = ob.VlcDb(3) 50 | session_id = vlc_db.add_session(0) 51 | for _ in range(10): 52 | t_rand = int(np.random.random() * 1e9) 53 | vlc_db.add_image(session_id, t_rand, None) 54 | 55 | t_last = -np.inf 56 | for k in vlc_db.get_image_keys(): 57 | t = vlc_db.get_image(k).metadata.epoch_ns 58 | assert t >= t_last, "get_image_keys epoch_ns not ascending" 59 | t_last = t 60 | 61 | 62 | def test_query_embedding(): 63 | vlc_db = ob.VlcDb(3) 64 | session_id = vlc_db.add_session(0) 65 | 66 | a_id = vlc_db.add_image(session_id, datetime.now(), None) 67 | vlc_db.update_embedding(a_id, np.array([0, 0, 1])) 68 | 69 | b_id = vlc_db.add_image(session_id, datetime.now(), None) 70 | vlc_db.update_embedding(b_id, np.array([0, 1, 0])) 71 | 72 | c_id = vlc_db.add_image(session_id, datetime.now(), None) 73 | vlc_db.update_embedding(c_id, np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2)])) 74 | 75 | d_id = vlc_db.add_image(session_id, datetime.now(), None) 76 | vlc_db.update_embedding(d_id, np.array([0, 0, 1])) 77 | 78 | imgs, sims = vlc_db.query_embeddings(np.array([0, 0.2, 0.8]), 1) 79 | assert len(imgs) == 1, "query_embeddings returned wrong number of closest matches" 80 | assert imgs[0].metadata.image_uuid in [a_id, d_id] 81 | 82 | imgs, sims = vlc_db.query_embeddings(np.array([0, 0.2, 0.8]), 2) 83 | assert len(imgs) == 2, "query_embeddings returned wrong number of matches" 84 | assert set((a_id, d_id)) == set([img.metadata.image_uuid for img in imgs]), ( 85 | "Query did not match k closest" 86 | ) 87 | 88 | imgs, sims = vlc_db.query_embeddings(np.array([0, 0.2, 0.8]), 3) 89 | assert len(imgs) == 3, "query_embeddings returned wrong number of matches" 90 | 91 | assert set((a_id, c_id, d_id)) == set([img.metadata.image_uuid for img in imgs]), ( 92 | "Query did not match k closest" 93 | ) 94 | 95 | 96 | def test_query_embedding_custom_similarity(): 97 | vlc_db = ob.VlcDb(3) 98 | session_id = vlc_db.add_session(0) 99 | 100 | a_id = vlc_db.add_image(session_id, datetime.now(), None) 101 | vlc_db.update_embedding(a_id, np.array([0, 0, 1])) 102 | 103 | b_id = vlc_db.add_image(session_id, datetime.now(), None) 104 | vlc_db.update_embedding(b_id, np.array([0, 2, 0])) 105 | 106 | c_id = vlc_db.add_image(session_id, datetime.now(), None) 107 | vlc_db.update_embedding(c_id, np.array([3, 0, 0])) 108 | 109 | def radius_metric(x, y): 110 | return -abs(np.linalg.norm(x) - np.linalg.norm(y)) 111 | 112 | imgs, sims = vlc_db.query_embeddings( 113 | np.array([2, 0, 0]), 1, similarity_metric=radius_metric 114 | ) 115 | 116 | assert imgs[0].metadata.image_uuid == b_id, "custom similarity matched wrong vector" 117 | 118 | 119 | def test_query_embeddings_max_time(): 120 | vlc_db = ob.VlcDb(3) 121 | session_id = vlc_db.add_session(0) 122 | 123 | a_id = vlc_db.add_image(session_id, 0, None) 124 | vlc_db.update_embedding(a_id, np.array([0, 0, 0.9])) 125 | 126 | b_id = vlc_db.add_image(session_id, 10, None) 127 | vlc_db.update_embedding(b_id, np.array([0, 1, 0])) 128 | 129 | c_id = vlc_db.add_image(session_id, 20, None) 130 | vlc_db.update_embedding(c_id, np.array([0, 0, 1])) 131 | 132 | imgs, sims = vlc_db.query_embeddings_max_time( 133 | np.array([0, 0, 1]), 1, [session_id], 15 134 | ) 135 | 136 | assert imgs[0].metadata.image_uuid == a_id 137 | 138 | imgs, sims = vlc_db.query_embeddings_max_time( 139 | np.array([0, 0, 1]), 2, [session_id], 15 140 | ) 141 | 142 | assert imgs[0].metadata.image_uuid == a_id 143 | assert imgs[1].metadata.image_uuid == b_id 144 | 145 | imgs, sims = vlc_db.query_embeddings_max_time( 146 | np.array([0, 0, 1]), 3, [session_id], 15 147 | ) 148 | 149 | assert len(imgs) == 2 150 | 151 | 152 | def test_batch_query_embeddings(): 153 | vlc_db = ob.VlcDb(3) 154 | session_id = vlc_db.add_session(0) 155 | 156 | a_id = vlc_db.add_image(session_id, datetime.now(), None) 157 | vlc_db.update_embedding(a_id, np.array([0, 0, 1])) 158 | 159 | b_id = vlc_db.add_image(session_id, datetime.now(), None) 160 | vlc_db.update_embedding(b_id, np.array([0, 1, 0])) 161 | 162 | c_id = vlc_db.add_image(session_id, datetime.now(), None) 163 | vlc_db.update_embedding(c_id, np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2)])) 164 | 165 | d_id = vlc_db.add_image(session_id, datetime.now(), None) 166 | vlc_db.update_embedding(d_id, np.array([0, 0, 1])) 167 | 168 | imgs, sims = vlc_db.batch_query_embeddings(np.array([[0, 1, 0], [0, 0.2, 0.2]]), 1) 169 | assert len(imgs) == 2, "number of results different from number of query vectors" 170 | assert len(imgs[0]) == 1, "returned wrong number of matches for vector" 171 | assert imgs[0][0].metadata.image_uuid == b_id, ( 172 | "Did not match correct image embedding" 173 | ) 174 | assert imgs[1][0].metadata.image_uuid == c_id, ( 175 | "Did not match correct image embedding" 176 | ) 177 | 178 | 179 | def test_batch_query_embeddings_uuid_filter(): 180 | vlc_db = ob.VlcDb(3) 181 | session_id = vlc_db.add_session(0) 182 | 183 | a_id = vlc_db.add_image(session_id, datetime.now(), None) 184 | vlc_db.update_embedding(a_id, np.array([0, 0, 1])) 185 | 186 | b_id = vlc_db.add_image(session_id, datetime.now(), None) 187 | vlc_db.update_embedding(b_id, np.array([0, 1, 0])) 188 | 189 | c_id = vlc_db.add_image(session_id, datetime.now(), None) 190 | vlc_db.update_embedding(c_id, np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2)])) 191 | 192 | d_id = vlc_db.add_image(session_id, datetime.now(), None) 193 | vlc_db.update_embedding(d_id, np.array([0, 0, 0.9])) 194 | 195 | matches = vlc_db.batch_query_embeddings_uuid_filter( 196 | [a_id, b_id], 1, lambda q, v, s: True 197 | ) 198 | 199 | assert len(matches) == 2 200 | assert len(matches[0]) == 1 201 | assert len(matches[1]) == 1 202 | 203 | assert matches[0][0][1].metadata.image_uuid == a_id 204 | assert matches[0][0][2].metadata.image_uuid == a_id 205 | 206 | assert matches[1][0][1].metadata.image_uuid == b_id 207 | assert matches[1][0][2].metadata.image_uuid == b_id 208 | 209 | matches = vlc_db.batch_query_embeddings_uuid_filter( 210 | [a_id, b_id], 1, lambda q, v, s: q.metadata.epoch_ns != v.metadata.epoch_ns 211 | ) 212 | 213 | assert matches[0][0][2].metadata.image_uuid == d_id 214 | assert matches[1][0][2].metadata.image_uuid == c_id 215 | 216 | 217 | def test_batch_query_embeddings_filter(): 218 | vlc_db = ob.VlcDb(3) 219 | session_id = vlc_db.add_session(0) 220 | 221 | a_id = vlc_db.add_image(session_id, 0, None) 222 | vlc_db.update_embedding(a_id, np.array([0, 0, 1])) 223 | 224 | b_id = vlc_db.add_image(session_id, 10, None) 225 | vlc_db.update_embedding(b_id, np.array([0, 1, 0])) 226 | 227 | c_id = vlc_db.add_image(session_id, 20, None) 228 | vlc_db.update_embedding(c_id, np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2)])) 229 | 230 | d_id = vlc_db.add_image(session_id, 30, None) 231 | vlc_db.update_embedding(d_id, np.array([0, 0, 0.9])) 232 | 233 | matches = vlc_db.batch_query_embeddings_filter( 234 | np.array([[0, 0, 1], [0, 1, 0]]), 1, lambda q, v, s: True 235 | ) 236 | 237 | assert len(matches) == 2 238 | assert len(matches[0]) == 1 239 | assert len(matches[1]) == 1 240 | 241 | # assert matches[0][0][1].metadata.image_uuid == a_id 242 | assert matches[0][0][2].metadata.image_uuid == a_id 243 | 244 | # assert matches[1][0][1].metadata.image_uuid == b_id 245 | assert matches[1][0][2].metadata.image_uuid == b_id 246 | 247 | metadata = [vlc_db.get_image(a_id), vlc_db.get_image(b_id)] 248 | matches = vlc_db.batch_query_embeddings_filter( 249 | np.array([[0, 0, 1], [0, 1, 0]]), 250 | 1, 251 | lambda q, v, s: q.metadata.epoch_ns != v.metadata.epoch_ns, 252 | filter_metadata=metadata, 253 | ) 254 | 255 | assert matches[0][0][2].metadata.image_uuid == d_id 256 | assert matches[1][0][2].metadata.image_uuid == c_id 257 | 258 | matches = vlc_db.batch_query_embeddings_filter( 259 | np.array([[0, 1, 0]]), 260 | 1, 261 | lambda q, v, s: v.metadata.epoch_ns < 10, 262 | ) 263 | 264 | assert matches[0][0][2].metadata.image_uuid == a_id 265 | 266 | 267 | def test_query_embeddings_filter(): 268 | vlc_db = ob.VlcDb(3) 269 | session_id = vlc_db.add_session(0) 270 | 271 | a_id = vlc_db.add_image(session_id, 0, None) 272 | vlc_db.update_embedding(a_id, np.array([0, 0, 1])) 273 | 274 | b_id = vlc_db.add_image(session_id, 10, None) 275 | vlc_db.update_embedding(b_id, np.array([0, 1, 0])) 276 | 277 | c_id = vlc_db.add_image(session_id, 20, None) 278 | vlc_db.update_embedding(c_id, np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2)])) 279 | 280 | d_id = vlc_db.add_image(session_id, 30, None) 281 | vlc_db.update_embedding(d_id, np.array([0, 0, 0.9])) 282 | 283 | matches = vlc_db.query_embeddings_filter(np.array([0, 0, 1]), 1, lambda v, s: True) 284 | assert len(matches) == 1 285 | assert len(matches[0]) == 2 286 | 287 | assert matches[0][1].metadata.image_uuid == a_id 288 | 289 | matches = vlc_db.query_embeddings_filter( 290 | np.array([0, 0, 1]), 291 | 1, 292 | lambda v, s: v.metadata.epoch_ns != 0, 293 | ) 294 | assert matches[0][1].metadata.image_uuid == d_id 295 | 296 | matches = vlc_db.query_embeddings_filter( 297 | np.array([0, 0, 1]), 298 | 2, 299 | lambda v, s: v.metadata.epoch_ns != 0, 300 | ) 301 | assert matches[0][1].metadata.image_uuid == d_id 302 | assert matches[1][1].metadata.image_uuid == c_id 303 | 304 | 305 | def test_update_keypoints(): 306 | vlc_db = ob.VlcDb(3) 307 | session_id = vlc_db.add_session(0) 308 | 309 | a_id = vlc_db.add_image(session_id, datetime.now(), None) 310 | b_id = vlc_db.add_image(session_id, datetime.now(), None) 311 | 312 | kps = np.random.random((10, 2)) 313 | vlc_db.update_keypoints(a_id, kps) 314 | keypoints, descriptors = vlc_db.get_keypoints(a_id) 315 | assert np.allclose(keypoints, kps) 316 | assert descriptors is None 317 | 318 | descriptors_in = np.random.random(kps.shape) 319 | vlc_db.update_keypoints(b_id, kps, descriptors=descriptors_in) 320 | keypoints, descriptors = vlc_db.get_keypoints(b_id) 321 | assert np.allclose(keypoints, kps) 322 | assert np.allclose(descriptors, descriptors_in) 323 | 324 | 325 | def test_add_session(): 326 | vlc_db = ob.VlcDb(3) 327 | robot_id = 0 328 | sensor_id = 42 329 | session_id = vlc_db.add_session(robot_id, sensor_id, "session_name") 330 | 331 | session = vlc_db.get_session(session_id) 332 | 333 | assert session.robot_id == 0 334 | assert session.name == "session_name" 335 | assert session.sensor_id == 42 336 | 337 | 338 | def test_insert_session(): 339 | vlc_db = ob.VlcDb(3) 340 | session_id = 1234 341 | robot_id = 0 342 | sensor_id = 42 343 | vlc_db.insert_session( 344 | session_id, datetime.now(), robot_id, sensor_id, "session_name" 345 | ) 346 | 347 | session = vlc_db.get_session(session_id) 348 | 349 | assert session.robot_id == 0 350 | assert session.name == "session_name" 351 | assert session.sensor_id == 42 352 | 353 | 354 | def test_add_lc(): 355 | vlc_db = ob.VlcDb(3) 356 | session_id = vlc_db.add_session(0) 357 | 358 | computed_ts = datetime.now() 359 | loop_closure = ob.SparkLoopClosure( 360 | from_image_uuid=0, 361 | to_image_uuid=1, 362 | f_T_t=np.eye(4), 363 | quality=1, 364 | is_metric=True, 365 | ) 366 | 367 | lc_uuid = vlc_db.add_lc(loop_closure, session_id, creation_time=computed_ts) 368 | lc = vlc_db.get_lc(lc_uuid) 369 | assert lc.from_image_uuid == 0 370 | assert lc.to_image_uuid == 1 371 | assert lc.f_T_t.shape == (4, 4) 372 | assert lc.is_metric 373 | assert lc.metadata.lc_uuid == lc_uuid 374 | assert lc.metadata.session_uuid == session_id 375 | 376 | 377 | def test_iterate_lcs(): 378 | vlc_db = ob.VlcDb(3) 379 | session_id = vlc_db.add_session(0) 380 | 381 | computed_ts = datetime.now() 382 | loop_closure = ob.SparkLoopClosure( 383 | from_image_uuid=0, to_image_uuid=1, f_T_t=np.eye(4), quality=1, is_metric=True 384 | ) 385 | lc_uuid_1 = vlc_db.add_lc(loop_closure, session_id, creation_time=computed_ts) 386 | computed_ts_2 = datetime.now() 387 | loop_closure = ob.SparkLoopClosure( 388 | from_image_uuid=3, 389 | to_image_uuid=4, 390 | f_T_t=np.eye(4), 391 | quality=1, 392 | is_metric=False, 393 | ) 394 | lc_uuid_2 = vlc_db.add_lc(loop_closure, session_id, creation_time=computed_ts_2) 395 | 396 | assert set((lc_uuid_1, lc_uuid_2)) == set( 397 | [lc.metadata.lc_uuid for lc in vlc_db.iterate_lcs()] 398 | ) 399 | 400 | 401 | def test_vlc_camera(): 402 | """Test that camera matrix is correct.""" 403 | camera = ob.PinholeCamera(5.0, 10.0, 3.0, 4.0) 404 | expected = np.array([[5.0, 0.0, 3.0], [0.0, 10.0, 4.0], [0.0, 0.0, 1.0]]) 405 | assert camera.K == pytest.approx(expected) 406 | 407 | 408 | def test_get_feature_depths(): 409 | """Test that depth extraction for keypoints works.""" 410 | img = ob.VlcImage(None, ob.SparkImage()) 411 | # no keypoints -> no depths 412 | assert img.get_feature_depths() is None 413 | 414 | img.keypoints = np.array([[2, 1], [3.1, 1.9], [-1, 10], [10, -1]]) 415 | # no depth image -> no depths 416 | assert img.get_feature_depths() is None 417 | 418 | img.image.depth = np.arange(24).reshape((4, 6)) 419 | depths = img.get_feature_depths() 420 | # should be indices (1, 2), (2, 3), (3, 0), (0, 5) 421 | assert depths == pytest.approx(np.array([8, 15, 18, 5])) 422 | -------------------------------------------------------------------------------- /ouroboros/tests/test_vlc_pose.py: -------------------------------------------------------------------------------- 1 | """Test pose utilities.""" 2 | 3 | import numpy as np 4 | import pytest 5 | 6 | import ouroboros as ob 7 | 8 | 9 | def test_pose_from_quat(): 10 | """Test that pose creation from quaternion is sane.""" 11 | T_identity = ob.pose_from_quat_trans(np.array([0, 0, 0, 1.0]), np.zeros((3, 1))) 12 | assert T_identity == pytest.approx(np.eye(4)) 13 | 14 | T_non_identity = ob.pose_from_quat_trans( 15 | np.array([1, 0, 0, 0]), np.array([1, 2, 3]) 16 | ) 17 | expected = np.array([[1, 0, 0, 1], [0, -1, 0, 2], [0, 0, -1, 3], [0, 0, 0, 1]]) 18 | assert T_non_identity == pytest.approx(expected) 19 | 20 | 21 | def test_pose_inverse(): 22 | """Test that pose inversion works.""" 23 | a_T_b = ob.pose_from_quat_trans(np.array([1, 0, 0, 0]), np.array([1, 2, 3])) 24 | b_T_a = ob.invert_pose(a_T_b) 25 | assert a_T_b @ b_T_a == pytest.approx(np.eye(4)) 26 | 27 | 28 | def test_pose_descriptors(): 29 | """Test that pose conversion is sane-ish via roundtrip conversion.""" 30 | desc = np.array([5.0, 1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]) 31 | pose = ob.VlcPose.from_descriptor(desc) 32 | assert desc == pytest.approx(pose.to_descriptor()) 33 | -------------------------------------------------------------------------------- /ouroboros_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(ouroboros_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | geometry_msgs 8 | sensor_msgs 9 | ) 10 | 11 | add_message_files( 12 | DIRECTORY 13 | msg 14 | FILES 15 | SparkImageMsg.msg 16 | VlcImageMetadataMsg.msg 17 | VlcImageMsg.msg 18 | VlcInfoMsg.msg 19 | ) 20 | 21 | add_service_files( 22 | DIRECTORY 23 | srv 24 | FILES 25 | VlcKeypointQuery.srv 26 | ) 27 | 28 | generate_messages(DEPENDENCIES std_msgs geometry_msgs sensor_msgs) 29 | 30 | catkin_package( 31 | CATKIN_DEPENDS 32 | message_runtime 33 | std_msgs 34 | geometry_msgs 35 | sensor_msgs 36 | DEPENDS 37 | INCLUDE_DIRS 38 | LIBRARIES 39 | ) 40 | -------------------------------------------------------------------------------- /ouroboros_msgs/msg/SparkImageMsg.msg: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image rgb 2 | sensor_msgs/Image depth 3 | -------------------------------------------------------------------------------- /ouroboros_msgs/msg/VlcImageMetadataMsg.msg: -------------------------------------------------------------------------------- 1 | string image_uuid 2 | string session_id 3 | uint64 epoch_ns 4 | 5 | -------------------------------------------------------------------------------- /ouroboros_msgs/msg/VlcImageMsg.msg: -------------------------------------------------------------------------------- 1 | ouroboros_msgs/VlcImageMetadataMsg metadata 2 | ouroboros_msgs/SparkImageMsg image 3 | float32[] embedding 4 | sensor_msgs/Image keypoints 5 | float32[] keypoint_depths 6 | sensor_msgs/Image descriptors 7 | bool has_pose_hint 8 | geometry_msgs/PoseStamped pose_hint 9 | 10 | 11 | -------------------------------------------------------------------------------- /ouroboros_msgs/msg/VlcInfoMsg.msg: -------------------------------------------------------------------------------- 1 | uint8 robot_id 2 | sensor_msgs/CameraInfo camera_info 3 | geometry_msgs/PoseStamped body_T_cam 4 | 5 | string embedding_topic 6 | string keypoints_service 7 | -------------------------------------------------------------------------------- /ouroboros_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ouroboros_msgs 3 | 0.0.0 4 | ROS msgs for Ouroboros VLC Server 5 | Aaron Ray 6 | 7 | MIT 8 | 9 | catkin 10 | message_generation 11 | message_runtime 12 | std_msgs 13 | geometry_msgs 14 | sensor_msgs 15 | 16 | -------------------------------------------------------------------------------- /ouroboros_msgs/srv/VlcKeypointQuery.srv: -------------------------------------------------------------------------------- 1 | string image_uuid 2 | --- 3 | ouroboros_msgs/VlcImageMsg vlc_image 4 | -------------------------------------------------------------------------------- /ouroboros_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(ouroboros_ros) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | ouroboros_msgs 7 | dynamic_reconfigure 8 | ) 9 | 10 | catkin_python_setup() 11 | 12 | generate_dynamic_reconfigure_options( 13 | cfg/PlaceDescriptorDebugging.cfg 14 | ) 15 | 16 | catkin_package( 17 | CATKIN_DEPENDS 18 | rospy 19 | ouroboros_msgs 20 | ) 21 | -------------------------------------------------------------------------------- /ouroboros_ros/cfg/PlaceDescriptorDebugging.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "ouroboros_ros" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("lc_lockout", double_t, 0, "Loop Closure Lockout Window", 10, 0, 100) 9 | gen.add("place_match_threshold", double_t, 0, "Place Descriptor Matching Threshold", 0.55, 0, 1) 10 | 11 | exit(gen.generate(PACKAGE, "ouroboros_ros", "PlaceDescriptorDebugging")) 12 | -------------------------------------------------------------------------------- /ouroboros_ros/config/gt_vlc_server_config.yaml: -------------------------------------------------------------------------------- 1 | place_method: 2 | type: ground_truth 3 | lc_distance_threshold: 4 4 | lc_recent_lockout_s: 20 5 | keypoint_method: 6 | type: ground_truth 7 | descriptor_method: 8 | type: ground_truth 9 | match_method: 10 | type: ground_truth 11 | pose_method: 12 | type: ground_truth 13 | lc_frame_lockout_s: 20 14 | place_match_threshold: -4 15 | strict_keypoint_evaluation: false 16 | display_method: 17 | type: ros 18 | display_place_matches: false 19 | display_keypoint_matches: false 20 | display_inlier_keypoint_matches: false 21 | -------------------------------------------------------------------------------- /ouroboros_ros/config/vlc_server_config.yaml: -------------------------------------------------------------------------------- 1 | place_method: 2 | type: Salad 3 | embedding_size: 8448 4 | model_source: torchhub 5 | model_variant: serizba/salad 6 | weight_source: dinov2_salad 7 | keypoint_method: 8 | type: SuperPoint 9 | max_keypoints: 1024 10 | descriptor_method: 11 | type: null 12 | match_method: 13 | type: Lightglue 14 | feature_type: superpoint 15 | pose_method: 16 | type: opengv 17 | scale_recovery: true 18 | use_pnp_for_scale: false 19 | ransac: 20 | inlier_tolerance: 1.0e-6 21 | scale_ransac: 22 | inlier_tolerance: 1.0e-1 23 | 24 | lc_frame_lockout_s: 20 25 | place_match_threshold: 0.65 26 | strict_keypoint_evaluation: false 27 | 28 | display_method: 29 | type: ros 30 | display_place_matches: True 31 | display_keypoint_matches: True 32 | display_inlier_keypoint_matches: True 33 | -------------------------------------------------------------------------------- /ouroboros_ros/config/vlc_server_node.yaml: -------------------------------------------------------------------------------- 1 | show_plots: true 2 | lc_send_delay_s: 15 3 | vlc_frame_period_s: 0.5 4 | robot_id: 0 5 | config_path: $(find ouroboros_ros)/config/vlc_server_config.yaml 6 | 7 | fixed_frame: world 8 | hint_body_frame: ground_truth/husky/base 9 | 10 | body_frame: husky/base 11 | camera_frame: husky/camera 12 | -------------------------------------------------------------------------------- /ouroboros_ros/launch/place_descriptor_debugging.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /ouroboros_ros/launch/vlc_server_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /ouroboros_ros/nodes/place_matching_example.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import cv_bridge 3 | import rospy 4 | from dynamic_reconfigure.server import Server 5 | from sensor_msgs.msg import Image 6 | 7 | from ouroboros import SparkImage, VlcDb 8 | from ouroboros.utils.plotting_utils import display_image_pair 9 | from ouroboros_ros.cfg import PlaceDescriptorDebuggingConfig 10 | from ouroboros_salad.salad_model import get_salad_model 11 | 12 | 13 | class PlaceDescriptorExample: 14 | def __init__(self): 15 | self.lc_lockout = 10 16 | self.place_match_threshold = 0.5 17 | self.reconfigure_server = Server( 18 | PlaceDescriptorDebuggingConfig, self.dynamic_reconfigure_cb 19 | ) 20 | 21 | self.embedding_model = get_salad_model() 22 | 23 | # 1. initialize vlc db 24 | self.vlc_db = VlcDb(8448) 25 | robot_id = 0 26 | self.session_id = self.vlc_db.add_session(robot_id) 27 | 28 | # 2. subscribe to images 29 | rospy.Subscriber("~image_in", Image, self.image_cb) 30 | 31 | def dynamic_reconfigure_cb(self, config, level): 32 | self.lc_lockout = config["lc_lockout"] 33 | self.place_match_threshold = config["place_match_threshold"] 34 | return config 35 | 36 | def image_cb(self, msg): 37 | # call embedding function and insert to db, then display match if it's above threshold 38 | bridge = cv_bridge.CvBridge() 39 | try: 40 | image = bridge.imgmsg_to_cv2(msg, "bgr8") 41 | except cv_bridge.CvBridgeError as e: 42 | print(e) 43 | raise e 44 | 45 | uid = self.vlc_db.add_image( 46 | self.session_id, msg.header.stamp.to_nsec(), SparkImage(rgb=image) 47 | ) 48 | current_image = self.vlc_db.get_image(uid) 49 | embedding = self.embedding_model(image) 50 | 51 | image_matches, similarities = self.vlc_db.query_embeddings_max_time( 52 | embedding, 1, msg.header.stamp.to_nsec() - self.lc_lockout * 1e9 53 | ) 54 | 55 | self.vlc_db.update_embedding(uid, embedding) 56 | 57 | if len(similarities) == 0 or similarities[0] < self.place_match_threshold: 58 | right = None 59 | else: 60 | right = image_matches[0].image.rgb 61 | 62 | left = current_image.image.rgb 63 | 64 | display_image_pair(left, right) 65 | 66 | 67 | if __name__ == "__main__": 68 | rospy.init_node("place_matching_example") 69 | node = PlaceDescriptorExample() 70 | rospy.spin() 71 | -------------------------------------------------------------------------------- /ouroboros_ros/nodes/vlc_multirobot_server_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | 4 | from ouroboros_ros.vlc_multirobot_server_ros import VlcMultirobotServerRos 5 | 6 | if __name__ == "__main__": 7 | rospy.init_node("vlc_server_node") 8 | node = VlcMultirobotServerRos() 9 | node.run() 10 | -------------------------------------------------------------------------------- /ouroboros_ros/nodes/vlc_server_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | 4 | from ouroboros_ros.utils import setup_ros_log_forwarding 5 | from ouroboros_ros.vlc_server_ros import VlcServerRos 6 | 7 | if __name__ == "__main__": 8 | rospy.init_node("vlc_server_node") 9 | setup_ros_log_forwarding() 10 | node = VlcServerRos() 11 | node.run() 12 | -------------------------------------------------------------------------------- /ouroboros_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ouroboros_ros 3 | 0.0.0 4 | ROS interface for Ouroboros VLC Server 5 | Aaron Ray 6 | 7 | MIT 8 | 9 | 10 | catkin 11 | rospy 12 | ouroboros_msgs 13 | python3-matplotlib 14 | python3-scipy 15 | 16 | -------------------------------------------------------------------------------- /ouroboros_ros/rviz/ouroboros_debug.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 65 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 65 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: Image 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/Image 55 | Enabled: true 56 | Image Topic: /vlc_server_node/image_pair_out 57 | Max Value: 1 58 | Median window: 5 59 | Min Value: 0 60 | Name: Image 61 | Normalize Range: true 62 | Queue Size: 2 63 | Transport Hint: raw 64 | Unreliable: false 65 | Value: true 66 | - Class: rviz/Image 67 | Enabled: true 68 | Image Topic: /vlc_server_node/inlier_kp_match_out 69 | Max Value: 1 70 | Median window: 5 71 | Min Value: 0 72 | Name: Image 73 | Normalize Range: true 74 | Queue Size: 2 75 | Transport Hint: raw 76 | Unreliable: false 77 | Value: true 78 | Enabled: true 79 | Global Options: 80 | Background Color: 48; 48; 48 81 | Default Light: true 82 | Fixed Frame: map 83 | Frame Rate: 30 84 | Name: root 85 | Tools: 86 | - Class: rviz/Interact 87 | Hide Inactive Objects: true 88 | - Class: rviz/MoveCamera 89 | - Class: rviz/Select 90 | - Class: rviz/FocusCamera 91 | - Class: rviz/Measure 92 | - Class: rviz/SetInitialPose 93 | Theta std deviation: 0.2617993950843811 94 | Topic: /initialpose 95 | X std deviation: 0.5 96 | Y std deviation: 0.5 97 | - Class: rviz/SetGoal 98 | Topic: /move_base_simple/goal 99 | - Class: rviz/PublishPoint 100 | Single click: true 101 | Topic: /clicked_point 102 | Value: true 103 | Views: 104 | Current: 105 | Class: rviz/Orbit 106 | Distance: 10 107 | Enable Stereo Rendering: 108 | Stereo Eye Separation: 0.05999999865889549 109 | Stereo Focal Distance: 1 110 | Swap Stereo Eyes: false 111 | Value: false 112 | Field of View: 0.7853981852531433 113 | Focal Point: 114 | X: 0 115 | Y: 0 116 | Z: 0 117 | Focal Shape Fixed Size: true 118 | Focal Shape Size: 0.05000000074505806 119 | Invert Z Axis: false 120 | Name: Current View 121 | Near Clip Distance: 0.009999999776482582 122 | Pitch: 0.785398006439209 123 | Target Frame: 124 | Yaw: 0.785398006439209 125 | Saved: ~ 126 | Window Geometry: 127 | Displays: 128 | collapsed: false 129 | Height: 1401 130 | Hide Left Dock: false 131 | Hide Right Dock: false 132 | Image: 133 | collapsed: false 134 | QMainWindow State: 000000ff00000000fd000000040000000000000229000004e7fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000035000000b9000000b900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000000f3000002080000001600fffffffb0000000a0049006d00610067006501000003000000021c0000001600ffffff00000001000000fe000004e7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000035000004e70000009600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003510000003efc0100000002fb0000000800540069006d0065010000000000000351000002d400fffffffb0000000800540069006d0065010000000000000450000000000000000000000020000004e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 135 | Selection: 136 | collapsed: false 137 | Time: 138 | collapsed: false 139 | Tool Properties: 140 | collapsed: false 141 | Views: 142 | collapsed: false 143 | Width: 849 144 | X: 2534 145 | Y: 18 146 | -------------------------------------------------------------------------------- /ouroboros_ros/setup.py: -------------------------------------------------------------------------------- 1 | # ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | from distutils.core import setup 3 | 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=["ouroboros_ros"], 9 | package_dir={"": "src"}, 10 | ) 11 | setup(**setup_args) 12 | -------------------------------------------------------------------------------- /ouroboros_ros/src/ouroboros_ros/__init__.py: -------------------------------------------------------------------------------- 1 | from ouroboros_ros.vlc_server_ros_display import ( 2 | VlcServerRosDisplay, 3 | VlcServerRosDisplayConfig, 4 | ) 5 | -------------------------------------------------------------------------------- /ouroboros_ros/src/ouroboros_ros/conversions.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import rospy 3 | from cv_bridge import CvBridge 4 | from geometry_msgs.msg import PoseStamped 5 | 6 | from ouroboros import SparkImage, VlcImage, VlcImageMetadata, VlcPose 7 | from ouroboros_msgs.msg import SparkImageMsg, VlcImageMetadataMsg, VlcImageMsg 8 | 9 | 10 | def vlc_image_metadata_to_msg(metadata: VlcImageMetadata) -> VlcImageMetadataMsg: 11 | metadata_msg = VlcImageMetadataMsg() 12 | metadata_msg.image_uuid = metadata.image_uuid 13 | metadata_msg.session_id = metadata.session_id 14 | metadata_msg.epoch_ns = metadata.epoch_ns 15 | return metadata_msg 16 | 17 | 18 | def spark_image_to_msg(image: SparkImage) -> SparkImageMsg: 19 | bridge = CvBridge() 20 | image_msg = SparkImageMsg() 21 | image_msg.rgb = bridge.cv2_to_imgmsg(image.rgb, encoding="passthrough") 22 | image_msg.depth = bridge.cv2_to_imgmsg(image.depth, encoding="passthrough") 23 | return image_msg 24 | 25 | 26 | def vlc_pose_to_msg(pose: VlcPose) -> PoseStamped: 27 | pose_msg = PoseStamped() 28 | pose_msg.header.stamp = rospy.Time(nsecs=pose.time_ns) 29 | pose_msg.pose.position.x = pose.position[0] 30 | pose_msg.pose.position.y = pose.position[1] 31 | pose_msg.pose.position.z = pose.position[2] 32 | pose_msg.pose.orientation.x = pose.rotation[0] 33 | pose_msg.pose.orientation.y = pose.rotation[1] 34 | pose_msg.pose.orientation.z = pose.rotation[2] 35 | pose_msg.pose.orientation.w = pose.rotation[3] 36 | return pose_msg 37 | 38 | 39 | def vlc_image_to_msg( 40 | vlc: VlcImage, 41 | *, 42 | convert_image=True, 43 | convert_embedding=True, 44 | convert_keypoints=True, 45 | convert_descriptors=True, 46 | ) -> VlcImageMsg: 47 | bridge = CvBridge() 48 | vlc_msg = VlcImageMsg() 49 | if vlc.image is not None and convert_image: 50 | vlc_msg.image = spark_image_to_msg(vlc.image) 51 | vlc_msg.metadata = vlc_image_metadata_to_msg(vlc.metadata) 52 | if vlc.embedding is not None and convert_embedding: 53 | vlc_msg.embedding = vlc.embedding.tolist() 54 | if vlc.keypoints is not None and convert_keypoints: 55 | vlc_msg.keypoints = bridge.cv2_to_imgmsg(vlc.keypoints, encoding="passthrough") 56 | if vlc.keypoint_depths is not None: 57 | vlc_msg.keypoint_depths = vlc.keypoint_depths.tolist() 58 | if vlc.descriptors is not None and convert_descriptors: 59 | vlc_msg.descriptors = bridge.cv2_to_imgmsg( 60 | vlc.descriptors, encoding="passthrough" 61 | ) 62 | if vlc.pose_hint is not None: 63 | vlc_msg.has_pose_hint = True 64 | vlc_msg.pose_hint = vlc_pose_to_msg(vlc.pose_hint) 65 | return vlc_msg 66 | 67 | 68 | def vlc_image_metadata_from_msg(metadata_msg: VlcImageMetadataMsg) -> VlcImageMetadata: 69 | return VlcImageMetadata( 70 | image_uuid=metadata_msg.image_uuid, 71 | session_id=metadata_msg.session_id, 72 | epoch_ns=metadata_msg.epoch_ns, 73 | ) 74 | 75 | 76 | def spark_image_from_msg(image_msg: SparkImageMsg) -> SparkImage: 77 | bridge = CvBridge() 78 | 79 | if image_msg.rgb.encoding == "": 80 | rgb = None 81 | else: 82 | rgb = bridge.imgmsg_to_cv2(image_msg.rgb, desired_encoding="passthrough") 83 | 84 | if image_msg.depth.encoding == "": 85 | depth = None 86 | else: 87 | depth = bridge.imgmsg_to_cv2(image_msg.depth, desired_encoding="passthrough") 88 | 89 | return SparkImage( 90 | rgb=rgb, 91 | depth=depth, 92 | ) 93 | 94 | 95 | def vlc_pose_from_msg(pose_msg: PoseStamped) -> VlcPose: 96 | pos = pose_msg.pose.position 97 | quat = pose_msg.pose.orientation 98 | return VlcPose( 99 | time_ns=pose_msg.header.stamp.to_nsec(), 100 | position=np.array([pos.x, pos.y, pos.z]), 101 | rotation=np.array([quat.x, quat.y, quat.z, quat.w]), 102 | ) 103 | 104 | 105 | def vlc_image_from_msg(vlc_msg: VlcImageMsg) -> VlcImage: 106 | bridge = CvBridge() 107 | pose_hint = None 108 | if vlc_msg.has_pose_hint: 109 | pose_hint = vlc_pose_from_msg(vlc_msg.pose_hint) 110 | 111 | if len(vlc_msg.embedding) == 0: 112 | embedding = None 113 | else: 114 | embedding = np.array(vlc_msg.embedding) 115 | 116 | if vlc_msg.keypoints.encoding == "": 117 | keypoints = None 118 | else: 119 | keypoints = bridge.imgmsg_to_cv2( 120 | vlc_msg.keypoints, desired_encoding="passthrough" 121 | ) 122 | 123 | if len(vlc_msg.keypoint_depths) == 0: 124 | keypoint_depths = None 125 | else: 126 | keypoint_depths = np.array(vlc_msg.keypoint_depths) 127 | 128 | if vlc_msg.descriptors.encoding == "": 129 | descriptors = None 130 | else: 131 | descriptors = bridge.imgmsg_to_cv2( 132 | vlc_msg.descriptors, desired_encoding="passthrough" 133 | ) 134 | vlc_image = VlcImage( 135 | metadata=vlc_image_metadata_from_msg(vlc_msg.metadata), 136 | image=spark_image_from_msg(vlc_msg.image), 137 | embedding=embedding, 138 | keypoints=keypoints, 139 | keypoint_depths=keypoint_depths, 140 | descriptors=descriptors, 141 | pose_hint=pose_hint, 142 | ) 143 | 144 | return vlc_image 145 | -------------------------------------------------------------------------------- /ouroboros_ros/src/ouroboros_ros/session_manager.py: -------------------------------------------------------------------------------- 1 | # In charge of subscribing to image stream, buffering as necessary, and keeping track of local image index ("session frame id"). Probably also in charge of turning the incoming message into VlcImageMetadata and VlcImage. 2 | -------------------------------------------------------------------------------- /ouroboros_ros/src/ouroboros_ros/utils.py: -------------------------------------------------------------------------------- 1 | import logging 2 | 3 | import numpy as np 4 | import rospy 5 | import tf2_ros 6 | from pose_graph_tools_msgs.msg import PoseGraphEdge 7 | from scipy.spatial.transform import Rotation as R 8 | 9 | import ouroboros as ob 10 | 11 | 12 | def build_robot_lc_message( 13 | key_from_ns, 14 | key_to_ns, 15 | robot_id, 16 | from_T_to, 17 | pose_cov, 18 | body_T_cam, 19 | ): 20 | return build_lc_message( 21 | key_from_ns, 22 | key_to_ns, 23 | robot_id, 24 | robot_id, 25 | from_T_to, 26 | pose_cov, 27 | body_T_cam, 28 | body_T_cam, 29 | ) 30 | 31 | 32 | def build_lc_message( 33 | key_from_ns, 34 | key_to_ns, 35 | robot_from, 36 | robot_to, 37 | from_T_to, 38 | pose_cov, 39 | robot_from_T_cam, 40 | robot_to_T_cam, 41 | ): 42 | bodyfrom_T_bodyto = robot_from_T_cam @ from_T_to @ ob.invert_pose(robot_to_T_cam) 43 | relative_position = bodyfrom_T_bodyto[:3, 3] 44 | relative_orientation = R.from_matrix(bodyfrom_T_bodyto[:3, :3]) 45 | 46 | lc_edge = PoseGraphEdge() 47 | lc_edge.header.stamp = rospy.Time.now() 48 | lc_edge.key_from = key_from_ns 49 | lc_edge.key_to = key_to_ns 50 | lc_edge.robot_from = robot_from 51 | lc_edge.robot_to = robot_to 52 | lc_edge.type = PoseGraphEdge.LOOPCLOSE 53 | lc_edge.pose.position.x = relative_position[0] 54 | lc_edge.pose.position.y = relative_position[1] 55 | lc_edge.pose.position.z = relative_position[2] 56 | q = relative_orientation.as_quat() 57 | lc_edge.pose.orientation.x = q[0] 58 | lc_edge.pose.orientation.y = q[1] 59 | lc_edge.pose.orientation.z = q[2] 60 | lc_edge.pose.orientation.w = q[3] 61 | 62 | lc_edge.covariance = pose_cov.flatten() 63 | return lc_edge 64 | 65 | 66 | def get_tf_as_pose(tf_buffer, fixed_frame, body_frame, time=None, timeout=1.0): 67 | if time is None: 68 | time = rospy.Time() 69 | try: 70 | trans = tf_buffer.lookup_transform( 71 | fixed_frame, body_frame, time, rospy.Duration(timeout) 72 | ) 73 | except ( 74 | tf2_ros.LookupException, 75 | tf2_ros.ConnectivityException, 76 | tf2_ros.ExtrapolationException, 77 | ) as e: 78 | rospy.logwarn( 79 | " Could not transform %s from %s: %s ", fixed_frame, body_frame, str(e) 80 | ) 81 | return 82 | 83 | current_pos = np.array( 84 | [ 85 | trans.transform.translation.x, 86 | trans.transform.translation.y, 87 | trans.transform.translation.z, 88 | ] 89 | ) 90 | 91 | current_rot = np.array( 92 | [ 93 | trans.transform.rotation.x, 94 | trans.transform.rotation.y, 95 | trans.transform.rotation.z, 96 | trans.transform.rotation.w, 97 | ] 98 | ) 99 | 100 | time_ns = trans.header.stamp.to_nsec() 101 | vlc_pose = ob.VlcPose(time_ns=time_ns, position=current_pos, rotation=current_rot) 102 | return vlc_pose 103 | 104 | 105 | def parse_camera_info(info_msg): 106 | K = np.array(info_msg.K).reshape((3, 3)) 107 | fx = K[0, 0] 108 | fy = K[1, 1] 109 | cx = K[0, 2] 110 | cy = K[1, 2] 111 | return ob.PinholeCamera(fx=fx, fy=fy, cx=cx, cy=cy) 112 | 113 | 114 | # adapted from https://gist.github.com/ablakey/4f57dca4ea75ed29c49ff00edf622b38 115 | class RosForwarder(logging.Handler): 116 | """Class to forward logging to ros handler.""" 117 | 118 | level_map = { 119 | logging.DEBUG: rospy.logdebug, 120 | logging.INFO: rospy.loginfo, 121 | logging.WARNING: rospy.logwarn, 122 | logging.ERROR: rospy.logerr, 123 | logging.CRITICAL: rospy.logfatal, 124 | } 125 | 126 | def emit(self, record): 127 | """Send message to ROS.""" 128 | level = record.levelno if record.levelno in self.level_map else logging.CRITICAL 129 | self.level_map[level](f"{record.name}: {record.msg}") 130 | 131 | 132 | def setup_ros_log_forwarding(level=logging.INFO): 133 | """Forward logging to ROS.""" 134 | logger = logging.getLogger("ouroboros") 135 | logger.addHandler(RosForwarder()) 136 | logger.setLevel(level) 137 | -------------------------------------------------------------------------------- /ouroboros_ros/src/ouroboros_ros/vlc_multirobot_server_ros.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from dataclasses import dataclass 3 | 4 | import rospy 5 | from pose_graph_tools_msgs.msg import PoseGraph 6 | from sensor_msgs.msg import CameraInfo 7 | 8 | import ouroboros as ob 9 | from ouroboros_msgs.msg import VlcImageMsg, VlcInfoMsg 10 | from ouroboros_msgs.srv import ( 11 | VlcKeypointQuery, 12 | VlcKeypointQueryResponse, 13 | ) 14 | from ouroboros_ros.conversions import ( 15 | vlc_image_from_msg, 16 | vlc_image_to_msg, 17 | vlc_pose_from_msg, 18 | vlc_pose_to_msg, 19 | ) 20 | from ouroboros_ros.utils import build_lc_message, parse_camera_info 21 | from ouroboros_ros.vlc_server_ros import VlcServerRos 22 | 23 | 24 | @dataclass 25 | class RobotInfo: 26 | session_id: str = None 27 | camera_config: ob.config.Config = None 28 | body_T_cam: ob.VlcPose = None 29 | 30 | @classmethod 31 | def from_info_msg(cls, resp: VlcInfoMsg): 32 | camera_config = parse_camera_info(resp.camera_info) 33 | body_T_cam = vlc_pose_from_msg(resp.body_T_cam) 34 | return cls( 35 | session_id=None, 36 | camera_config=camera_config, 37 | body_T_cam=body_T_cam, 38 | ) 39 | 40 | 41 | class VlcMultirobotServerRos(VlcServerRos): 42 | def __init__(self): 43 | super().__init__() 44 | 45 | self.track_new_uuids = [] 46 | 47 | # Spin up robot info server 48 | self.clients = rospy.get_param("~clients") 49 | 50 | # Publish embeddings 51 | self.embedding_timer = rospy.Timer( 52 | rospy.Duration(5), self.embedding_timer_callback 53 | ) 54 | self.embedding_publisher = rospy.Publisher( 55 | "vlc_embedding", VlcImageMsg, queue_size=10 56 | ) 57 | 58 | # Keypoint request server 59 | self.keypoint_server = rospy.Service( 60 | "vlc_keypoints_request", 61 | VlcKeypointQuery, 62 | self.process_keypoints_request, 63 | ) 64 | 65 | self.embedding_subscribers = [] 66 | self.keypoint_clients = {} 67 | 68 | # Publish info as discovery 69 | self.info_timer = rospy.Timer(rospy.Duration(5), self.info_timer_callback) 70 | self.info_publisher = rospy.Publisher("/vlc_info", VlcInfoMsg) 71 | 72 | # Subscribe to other robots' infos as discovery 73 | self.info_subscriber = rospy.Subscriber( 74 | "/vlc_info", VlcInfoMsg, self.vlc_info_callback 75 | ) 76 | 77 | self.robot_infos = {} 78 | self.uuid_map = {} 79 | self.session_robot_map = {} 80 | 81 | def info_timer_callback(self, event): 82 | # NOTE(Yun) maybe should terminate this? But there's a case where a new server shows up 83 | info_msg = VlcInfoMsg() 84 | info_msg.robot_id = self.robot_id 85 | camera_info = CameraInfo() 86 | camera_info.K = self.camera_config.K.flatten() 87 | info_msg.camera_info = camera_info 88 | info_msg.body_T_cam = vlc_pose_to_msg(self.body_T_cam) 89 | 90 | info_msg.embedding_topic = rospy.resolve_name("vlc_embedding") 91 | info_msg.keypoints_service = rospy.resolve_name("vlc_keypoints_request") 92 | self.info_publisher.publish(info_msg) 93 | 94 | def vlc_info_callback(self, info_msg): 95 | # Note(Yun) alternatively define server(s) in msg 96 | if info_msg.robot_id not in self.clients: 97 | # Not handling this robot 98 | return 99 | 100 | if info_msg.robot_id in self.robot_infos: 101 | # Already initialized 102 | return 103 | 104 | self.robot_infos[info_msg.robot_id] = RobotInfo.from_info_msg(info_msg) 105 | # Assign session_id 106 | self.robot_infos[ 107 | info_msg.robot_id 108 | ].session_id = self.vlc_server.register_camera( 109 | info_msg.robot_id, 110 | self.robot_infos[info_msg.robot_id].camera_config, 111 | rospy.Time.now().to_nsec(), 112 | ) 113 | self.session_robot_map[self.robot_infos[info_msg.robot_id].session_id] = ( 114 | info_msg.robot_id 115 | ) 116 | # Subscribe to embeddings 117 | self.embedding_subscribers.append( 118 | rospy.Subscriber( 119 | info_msg.embedding_topic, 120 | VlcImageMsg, 121 | self.client_embedding_callback, 122 | callback_args=info_msg.robot_id, 123 | ) 124 | ) 125 | # Keypoint request client 126 | self.keypoint_clients[info_msg.robot_id] = rospy.ServiceProxy( 127 | info_msg.keypoints_service, VlcKeypointQuery 128 | ) 129 | 130 | def process_new_frame(self, image, stamp_ns, hint_pose): 131 | # Need a different one due to sometimes needing to request keypoints 132 | new_uuid = self.vlc_server.add_frame( 133 | self.session_id, 134 | image, 135 | stamp_ns, 136 | pose_hint=hint_pose, 137 | ) 138 | self.track_new_uuids.append(new_uuid) 139 | 140 | # Find match using the embeddings. 141 | image_match = self.vlc_server.find_match(new_uuid, stamp_ns) 142 | 143 | if image_match is None: 144 | return new_uuid, None 145 | 146 | match_uuid = image_match.metadata.image_uuid 147 | 148 | interrobot = self.session_id != image_match.metadata.session_id 149 | if image_match.keypoints is None and interrobot: 150 | remapped_match_uuid = self.uuid_map[match_uuid] 151 | # Request keypoint and descriptors for match 152 | robot_id = self.session_robot_map[image_match.metadata.session_id] 153 | response = self.keypoint_clients[robot_id](remapped_match_uuid) 154 | vlc_response = vlc_image_from_msg(response.vlc_image) 155 | self.vlc_server.update_keypoints_decriptors( 156 | match_uuid, vlc_response.keypoints, vlc_response.descriptors 157 | ) 158 | self.vlc_server.update_keypoint_depths( 159 | match_uuid, vlc_response.keypoint_depths 160 | ) 161 | 162 | elif not interrobot: 163 | self.vlc_server.compute_keypoints_descriptors( 164 | match_uuid, compute_depths=True 165 | ) 166 | 167 | # Compute self keypoints and descriptors 168 | self.vlc_server.compute_keypoints_descriptors(new_uuid, compute_depths=True) 169 | 170 | lc_list = self.vlc_server.compute_loop_closure_pose( 171 | self.session_id, new_uuid, image_match.metadata.image_uuid, stamp_ns 172 | ) 173 | 174 | return new_uuid, lc_list 175 | 176 | def embedding_timer_callback(self, event): 177 | while len(self.track_new_uuids) > 0: 178 | new_uuid = self.track_new_uuids.pop(0) 179 | vlc_img_msg = vlc_image_to_msg( 180 | self.vlc_server.get_image(new_uuid), convert_image=False 181 | ) 182 | self.embedding_publisher.publish(vlc_img_msg) 183 | 184 | def client_embedding_callback(self, vlc_img_msg, robot_id): 185 | # Add image 186 | vlc_image = vlc_image_from_msg(vlc_img_msg) 187 | vlc_stamp = vlc_img_msg.metadata.epoch_ns 188 | new_uuid = self.vlc_server.add_embedding_no_image( 189 | self.robot_infos[robot_id].session_id, 190 | vlc_image.embedding, 191 | vlc_stamp, 192 | pose_hint=vlc_image.pose_hint, 193 | ) 194 | self.uuid_map[new_uuid] = vlc_image.metadata.image_uuid 195 | 196 | # Find match 197 | matched_img = self.vlc_server.find_match( 198 | new_uuid, vlc_stamp, search_sessions=[self.session_id] 199 | ) 200 | if matched_img is None: 201 | return 202 | 203 | rospy.logwarn(f"Found match between robots {robot_id} and {self.robot_id}") 204 | 205 | # Request keypoints / descriptors 206 | response = self.keypoint_clients[robot_id](vlc_image.metadata.image_uuid) 207 | vlc_response = vlc_image_from_msg(response.vlc_image) 208 | self.vlc_server.update_keypoints_decriptors( 209 | new_uuid, vlc_response.keypoints, vlc_response.descriptors 210 | ) 211 | self.vlc_server.update_keypoint_depths(new_uuid, vlc_response.keypoint_depths) 212 | 213 | # Detect loop closures 214 | self.vlc_server.compute_keypoints_descriptors( 215 | matched_img.metadata.image_uuid, compute_depths=True 216 | ) 217 | lc_list = self.vlc_server.compute_loop_closure_pose( 218 | self.robot_infos[robot_id].session_id, 219 | new_uuid, 220 | matched_img.metadata.image_uuid, 221 | vlc_stamp, 222 | ) 223 | 224 | if lc_list is None: 225 | return 226 | 227 | rospy.logwarn(f"Found lc between robots {robot_id} and {self.robot_id}") 228 | 229 | pose_cov_mat = self.build_pose_cov_mat() 230 | pg = PoseGraph() 231 | pg.header.stamp = rospy.Time.now() 232 | for lc in lc_list: 233 | if not lc.is_metric: 234 | rospy.logwarn("Skipping non-metric loop closure.") 235 | continue 236 | 237 | from_time_ns, to_time_ns = self.vlc_server.get_lc_times(lc.metadata.lc_uuid) 238 | 239 | lc_edge = build_lc_message( 240 | from_time_ns, 241 | to_time_ns, 242 | robot_id, 243 | self.robot_id, 244 | lc.f_T_t, 245 | pose_cov_mat, 246 | self.robot_infos[robot_id].body_T_cam.as_matrix(), 247 | self.body_T_cam.as_matrix(), 248 | ) 249 | 250 | pg.edges.append(lc_edge) 251 | self.loop_closure_delayed_queue.append( 252 | (rospy.Time.now().to_sec() + self.lc_send_delay_s, pg) 253 | ) 254 | 255 | def process_keypoints_request(self, request): 256 | if not self.vlc_server.has_image(request.image_uuid): 257 | rospy.logwarn(f"Image ID {request.image_uuid} not found!") 258 | return VlcKeypointQueryResponse() 259 | 260 | self.vlc_server.compute_keypoints_descriptors( 261 | request.image_uuid, compute_depths=True 262 | ) 263 | vlc_img = self.vlc_server.get_image(request.image_uuid) 264 | return VlcKeypointQueryResponse( 265 | vlc_image=vlc_image_to_msg( 266 | vlc_img, convert_image=False, convert_embedding=False 267 | ) 268 | ) 269 | -------------------------------------------------------------------------------- /ouroboros_ros/src/ouroboros_ros/vlc_server_ros.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import threading 4 | 5 | import cv_bridge 6 | import matplotlib.pyplot as plt 7 | import message_filters 8 | import numpy as np 9 | import rospy 10 | import spark_config as sc 11 | import tf2_ros 12 | from pose_graph_tools_msgs.msg import PoseGraph 13 | from sensor_msgs.msg import CameraInfo, Image 14 | 15 | import ouroboros as ob 16 | from ouroboros.utils.plotting_utils import plt_fast_pause 17 | from ouroboros_ros.utils import ( 18 | build_robot_lc_message, 19 | get_tf_as_pose, 20 | parse_camera_info, 21 | ) 22 | 23 | 24 | def update_plot(line, pts, images_to_pose): 25 | if len(images_to_pose) == 0: 26 | return 27 | 28 | positions = [] 29 | for _, v in images_to_pose.items(): 30 | positions.append(v.position) 31 | 32 | positions = np.array(positions) 33 | 34 | line.set_data(positions[:, 0], positions[:, 1]) 35 | pts.set_offsets(positions[:, :2]) 36 | ax = plt.gca() 37 | ax.relim() 38 | ax.autoscale_view() 39 | plt.draw_all() 40 | plt_fast_pause(0.05) 41 | 42 | 43 | def plot_lc(query_position, match_position, color): 44 | plt.plot( 45 | [match_position[0], query_position[0]], 46 | [match_position[1], query_position[1]], 47 | color=color, 48 | ) 49 | 50 | 51 | def get_query_and_est_match_position(lc, image_to_pose, body_T_cam): 52 | query_pose = image_to_pose[lc.from_image_uuid] 53 | 54 | world_T_query = query_pose.as_matrix() 55 | query_T_match = body_T_cam @ ob.invert_pose(lc.f_T_t) @ ob.invert_pose(body_T_cam) 56 | world_T_match = world_T_query @ query_T_match 57 | 58 | query_position = query_pose.position 59 | est_match_position = world_T_match[:3, 3] 60 | return query_position, est_match_position 61 | 62 | 63 | class VlcServerRos: 64 | def __init__(self): 65 | self.tf_buffer = tf2_ros.Buffer() 66 | self.listener = tf2_ros.TransformListener(self.tf_buffer) 67 | 68 | self.lc_pub = rospy.Publisher("~loop_closure_output", PoseGraph, queue_size=1) 69 | 70 | self.fixed_frame = rospy.get_param("~fixed_frame") 71 | self.hint_body_frame = rospy.get_param("~hint_body_frame") 72 | self.body_frame = rospy.get_param("~body_frame", "") 73 | self.camera_frame = rospy.get_param("~camera_frame", "") 74 | if self.body_frame == "" and self.camera_frame == "": 75 | rospy.logwarn("Body and camera frames not specified: assuming identity!") 76 | elif self.body_frame == "" or self.camera_frame == "": 77 | frame_str = f"body='{self.body_frame}', camera='{self.camera_frame}'" 78 | raise ValueError(f"Body/camera frames must not be empty, got {frame_str}!") 79 | 80 | self.show_plots = rospy.get_param("~show_plots") 81 | self.vlc_frame_period_s = rospy.get_param("~vlc_frame_period_s") 82 | self.lc_send_delay_s = rospy.get_param("~lc_send_delay_s") 83 | self.robot_id = rospy.get_param("~robot_id") 84 | 85 | plugins = sc.discover_plugins("ouroboros_") 86 | print("Found plugins: ", plugins) 87 | config_path = rospy.get_param("~config_path") 88 | server_config = ob.VlcServerConfig.load(config_path) 89 | self.vlc_server = ob.VlcServer( 90 | server_config, 91 | robot_id=self.robot_id, 92 | ) 93 | 94 | self.camera_config = self.get_camera_config_ros() 95 | print(f"camera config: {self.camera_config}") 96 | self.session_id = self.vlc_server.register_camera( 97 | self.robot_id, self.camera_config, rospy.Time.now().to_nsec() 98 | ) 99 | 100 | self.loop_rate = rospy.Rate(10) 101 | self.images_to_pose = {} 102 | self.image_pose_lock = threading.Lock() 103 | self.last_vlc_frame_time = None 104 | 105 | if self.show_plots: 106 | plt.ion() 107 | plt.figure() 108 | plt.show() 109 | self.position_line = plt.plot([], [], color="g")[0] 110 | self.position_points = plt.scatter([], [], color="g") 111 | 112 | # Hydra takes too long to add agent poses to the backend, so if we send the LC 113 | # immediately it will get rejected. To work around this, we can't send the loop 114 | # closure until several seconds after it is detected 115 | self.loop_closure_delayed_queue = [] 116 | 117 | self.image_sub = message_filters.Subscriber("~image_in", Image) 118 | self.depth_sub = message_filters.Subscriber("~depth_in", Image) 119 | 120 | self.image_depth_sub = message_filters.ApproximateTimeSynchronizer( 121 | [self.image_sub, self.depth_sub], 10, 0.1 122 | ) 123 | self.image_depth_sub.registerCallback(self.image_depth_callback) 124 | 125 | if self.body_frame != self.camera_frame: 126 | self.body_T_cam = self.get_body_T_cam_ros() 127 | else: 128 | self.body_T_cam = ob.VlcPose( 129 | time_ns=0, position=np.array([0, 0, 0]), rotation=np.array([0, 0, 0, 1]) 130 | ) 131 | 132 | def get_camera_config_ros(self): 133 | rate = rospy.Rate(10) 134 | while not rospy.is_shutdown(): 135 | try: 136 | info_msg = rospy.wait_for_message("~camera_info", CameraInfo, timeout=5) 137 | except rospy.ROSException: 138 | rospy.logerr("Timed out waiting for camera info") 139 | rate.sleep() 140 | continue 141 | pinhole = parse_camera_info(info_msg) 142 | return pinhole 143 | return None 144 | 145 | def get_body_T_cam_ros(self, max_retries=10, retry_delay=0.5): 146 | rate = rospy.Rate(10) 147 | while not rospy.is_shutdown(): 148 | try: 149 | return get_tf_as_pose( 150 | self.tf_buffer, self.body_frame, self.camera_frame, rospy.Time.now() 151 | ) 152 | except rospy.ROSException: 153 | rospy.logerr("Failed to get body_T_cam transform") 154 | rate.sleep() 155 | return None 156 | 157 | def process_new_frame(self, image, stamp_ns, hint_pose): 158 | return self.vlc_server.add_and_query_frame( 159 | self.session_id, 160 | image, 161 | stamp_ns, 162 | pose_hint=hint_pose, 163 | ) 164 | 165 | def image_depth_callback(self, img_msg, depth_msg): 166 | if not ( 167 | self.last_vlc_frame_time is None 168 | or (rospy.Time.now() - self.last_vlc_frame_time).to_sec() 169 | > self.vlc_frame_period_s 170 | ): 171 | return 172 | self.last_vlc_frame_time = rospy.Time.now() 173 | 174 | bridge = cv_bridge.CvBridge() 175 | try: 176 | color_image = bridge.imgmsg_to_cv2(img_msg, "bgr8") 177 | except cv_bridge.CvBridgeError as e: 178 | print(e) 179 | raise e 180 | 181 | try: 182 | depth_image = bridge.imgmsg_to_cv2(depth_msg, "passthrough") 183 | except cv_bridge.CvBridgeError as e: 184 | print(e) 185 | raise e 186 | 187 | if depth_msg.encoding == "16UC1": 188 | depth_image = depth_image / 1000 189 | 190 | # An estimate of the current camera pose, which is optionally used to 191 | # inform the place recognition, keypoint detection, keypoint 192 | # descriptor, and pose recovery methods. 193 | hint_pose = get_tf_as_pose( 194 | self.tf_buffer, self.fixed_frame, self.hint_body_frame, img_msg.header.stamp 195 | ) 196 | 197 | if hint_pose is None: 198 | print("Cannot get current pose, skipping frame!") 199 | return 200 | 201 | spark_image = ob.SparkImage(rgb=color_image, depth=depth_image) 202 | image_uuid, loop_closures = self.process_new_frame( 203 | spark_image, img_msg.header.stamp.to_nsec(), hint_pose 204 | ) 205 | 206 | with self.image_pose_lock: 207 | self.images_to_pose[image_uuid] = hint_pose 208 | 209 | if loop_closures is None: 210 | return 211 | 212 | pose_cov_mat = self.build_pose_cov_mat() 213 | pg = PoseGraph() 214 | pg.header.stamp = rospy.Time.now() 215 | for lc in loop_closures: 216 | if self.show_plots: 217 | with self.image_pose_lock: 218 | query_pos, match_pos = get_query_and_est_match_position( 219 | lc, self.images_to_pose, self.body_T_cam.as_matrix() 220 | ) 221 | true_match_pos = self.images_to_pose[lc.to_image_uuid].position 222 | if not lc.is_metric: 223 | plot_lc(query_pos, match_pos, "y") 224 | elif np.linalg.norm(true_match_pos - match_pos) < 1: 225 | plot_lc(query_pos, match_pos, "b") 226 | else: 227 | plot_lc(query_pos, match_pos, "r") 228 | 229 | if not lc.is_metric: 230 | rospy.logwarn("Skipping non-metric loop closure.") 231 | continue 232 | 233 | from_time_ns, to_time_ns = self.vlc_server.get_lc_times(lc.metadata.lc_uuid) 234 | 235 | lc_edge = build_robot_lc_message( 236 | from_time_ns, 237 | to_time_ns, 238 | self.robot_id, 239 | lc.f_T_t, 240 | pose_cov_mat, 241 | self.body_T_cam.as_matrix(), 242 | ) 243 | 244 | pg.edges.append(lc_edge) 245 | self.loop_closure_delayed_queue.append( 246 | (rospy.Time.now().to_sec() + self.lc_send_delay_s, pg) 247 | ) 248 | 249 | def build_pose_cov_mat(self): 250 | pose_cov_mat = np.zeros((6, 6)) 251 | pos_cov = 0.1 252 | pose_cov_mat[0, 0] = pos_cov 253 | pose_cov_mat[1, 1] = pos_cov 254 | pose_cov_mat[2, 2] = pos_cov 255 | rot_cov = 0.001 256 | pose_cov_mat[3, 3] = rot_cov 257 | pose_cov_mat[4, 4] = rot_cov 258 | pose_cov_mat[5, 5] = rot_cov 259 | return pose_cov_mat 260 | 261 | def run(self): 262 | while not rospy.is_shutdown(): 263 | self.step() 264 | self.loop_rate.sleep() 265 | 266 | def step(self): 267 | if self.show_plots: 268 | with self.image_pose_lock: 269 | update_plot( 270 | self.position_line, self.position_points, self.images_to_pose 271 | ) 272 | 273 | # This is a dumb hack because Hydra doesn't deal properly with 274 | # receiving loop closures where the agent nodes haven't been added to 275 | # the backend yet, which occurs a lot when the backend runs slightly 276 | # slowly. You need to wait up to several seconds to send a loop closure 277 | # before it will be accepted by Hydra. 278 | while len(self.loop_closure_delayed_queue) > 0: 279 | send_time, pg = self.loop_closure_delayed_queue[0] 280 | if rospy.Time.now().to_sec() >= send_time: 281 | self.lc_pub.publish(pg) 282 | self.loop_closure_delayed_queue = self.loop_closure_delayed_queue[1:] 283 | else: 284 | break 285 | -------------------------------------------------------------------------------- /ouroboros_ros/src/ouroboros_ros/vlc_server_ros_display.py: -------------------------------------------------------------------------------- 1 | from __future__ import annotations 2 | 3 | from dataclasses import dataclass 4 | 5 | import numpy as np 6 | import rospy 7 | from cv_bridge import CvBridge, CvBridgeError 8 | from sensor_msgs.msg import Image 9 | from spark_config import Config, register_config 10 | 11 | import ouroboros as ob 12 | 13 | 14 | class VlcServerRosDisplay: 15 | def __init__(self, config: VlcServerRosDisplayConfig): 16 | self.config = config 17 | 18 | self.image_pair_pub = rospy.Publisher("~image_pair_out", Image, queue_size=1) 19 | self.kp_match_pub = rospy.Publisher("~kp_match_out", Image, queue_size=1) 20 | self.inlier_kp_match_pub = rospy.Publisher( 21 | "~inlier_kp_match_out", Image, queue_size=1 22 | ) 23 | 24 | self.bridge = CvBridge() 25 | 26 | def setup(self, log_path: str): 27 | pass 28 | 29 | def display_image_pair(self, left: ob.VlcImage, right: ob.VlcImage, time_ns: int): 30 | if not self.config.display_place_matches: 31 | return 32 | 33 | if right is None: 34 | image_pair = ob.utils.plotting_utils.create_image_pair(left.image.rgb, None) 35 | else: 36 | image_pair = ob.utils.plotting_utils.create_image_pair( 37 | left.image.rgb, right.image.rgb 38 | ) 39 | try: 40 | img_msg = self.bridge.cv2_to_imgmsg(image_pair.astype(np.uint8), "bgr8") 41 | except CvBridgeError as e: 42 | rospy.logerr(e) 43 | return 44 | 45 | self.image_pair_pub.publish(img_msg) 46 | 47 | def display_kp_match_pair( 48 | self, left: ob.VlcImage, right: ob.VlcImage, left_kp, right_kp, time_ns: int 49 | ): 50 | if not self.config.display_keypoint_matches: 51 | return 52 | 53 | image_pair = ob.utils.plotting_utils.create_kp_match_pair( 54 | left, right, left_kp, right_kp 55 | ) 56 | 57 | try: 58 | img_msg = self.bridge.cv2_to_imgmsg(image_pair.astype(np.uint8), "bgr8") 59 | except CvBridgeError as e: 60 | rospy.logerr(e) 61 | return 62 | 63 | self.kp_match_pub.publish(img_msg) 64 | 65 | def display_inlier_kp_match_pair( 66 | self, 67 | left: ob.VlcImage, 68 | right: ob.VlcImage, 69 | query_to_match, 70 | inliers, 71 | time_ns: int, 72 | ): 73 | if not self.config.display_inlier_keypoint_matches: 74 | return 75 | inlier_mask = np.zeros(len(query_to_match), dtype=bool) 76 | inlier_mask[inliers] = True 77 | left_inliers = left.keypoints[query_to_match[inlier_mask, 0]] 78 | right_inliers = right.keypoints[query_to_match[inlier_mask, 1]] 79 | left_outliers = left.keypoints[query_to_match[np.logical_not(inlier_mask), 0]] 80 | right_outliers = right.keypoints[query_to_match[np.logical_not(inlier_mask), 1]] 81 | img = ob.utils.plotting_utils.create_inlier_kp_match_pair( 82 | left, right, left_inliers, right_inliers, left_outliers, right_outliers 83 | ) 84 | try: 85 | img_msg = self.bridge.cv2_to_imgmsg(img.astype(np.uint8), "bgr8") 86 | except CvBridgeError as e: 87 | rospy.logerr(e) 88 | return 89 | 90 | self.inlier_kp_match_pub.publish(img_msg) 91 | 92 | 93 | @register_config("vlc_server_display", name="ros", constructor=VlcServerRosDisplay) 94 | @dataclass 95 | class VlcServerRosDisplayConfig(Config): 96 | display_place_matches: bool = True 97 | display_keypoint_matches: bool = True 98 | display_inlier_keypoint_matches: bool = True 99 | -------------------------------------------------------------------------------- /ouroboros_ros/tests/test_conversions.py: -------------------------------------------------------------------------------- 1 | """Test pose utilities.""" 2 | 3 | from datetime import datetime 4 | 5 | import numpy as np 6 | import numpy.testing as npt 7 | 8 | import ouroboros as ob 9 | from ouroboros_ros.conversions import ( 10 | spark_image_from_msg, 11 | spark_image_to_msg, 12 | vlc_image_from_msg, 13 | vlc_image_metadata_from_msg, 14 | vlc_image_metadata_to_msg, 15 | vlc_image_to_msg, 16 | vlc_pose_from_msg, 17 | vlc_pose_to_msg, 18 | ) 19 | 20 | 21 | def test_vlc_metadata_conversion(): 22 | vlc_db = ob.VlcDb(3) 23 | session_id = vlc_db.add_session(0) 24 | img_stamp = datetime.now() 25 | img_uuid = vlc_db.add_image(session_id, img_stamp, ob.SparkImage()) 26 | 27 | metadata = vlc_db.get_image(img_uuid).metadata 28 | 29 | metadata_msg = vlc_image_metadata_to_msg(metadata) 30 | metadata_converted = vlc_image_metadata_from_msg(metadata_msg) 31 | 32 | assert metadata_converted.image_uuid == metadata.image_uuid 33 | assert metadata_converted.session_id == metadata.session_id 34 | assert metadata_converted.epoch_ns == metadata.epoch_ns 35 | 36 | 37 | def test_spark_image_conversion(): 38 | height = 10 39 | width = 10 40 | rgb_image = np.random.randint(0, 256, (height, width, 3), dtype=np.uint8) 41 | depth_image = np.random.uniform(0, 10, (height, width)).astype(np.float32) 42 | 43 | image = ob.SparkImage(rgb=rgb_image, depth=depth_image) 44 | image_msg = spark_image_to_msg(image) 45 | image_converted = spark_image_from_msg(image_msg) 46 | 47 | npt.assert_array_equal(image_converted.rgb, image.rgb) 48 | npt.assert_array_equal(image_converted.depth, image.depth) 49 | 50 | 51 | def test_vlc_pose_conversion(): 52 | pose = ob.VlcPose( 53 | time_ns=100, position=np.array([1, 2, 3]), rotation=np.array([1, 0, 0, 0]) 54 | ) 55 | 56 | geom_msg = vlc_pose_to_msg(pose) 57 | pose_converted = vlc_pose_from_msg(geom_msg) 58 | 59 | assert pose_converted.time_ns == pose.time_ns 60 | npt.assert_array_equal(pose_converted.position, pose.position) 61 | npt.assert_array_equal(pose_converted.rotation, pose.rotation) 62 | 63 | 64 | def test_vlc_image_conversion(): 65 | height = 10 66 | width = 10 67 | rgb_image = np.random.randint(0, 256, (height, width, 3), dtype=np.uint8) 68 | depth_image = np.random.uniform(0, 10, (height, width)).astype(np.float32) 69 | embedding = np.random.uniform(0, 1, (100,)).astype(np.float32) 70 | keypoints = np.random.uniform(0, 10, (10, 3)).astype(np.float32) 71 | descriptors = np.random.uniform(0, 1, (10, 256)).astype(np.float32) 72 | 73 | vlc_db = ob.VlcDb(100) 74 | session_id = vlc_db.add_session(0) 75 | img_stamp = datetime.now() 76 | img_uuid = vlc_db.add_image( 77 | session_id, img_stamp, ob.SparkImage(rgb=rgb_image, depth=depth_image) 78 | ) 79 | vlc_db.update_embedding(img_uuid, embedding) 80 | vlc_db.update_keypoints(img_uuid, keypoints, descriptors) 81 | 82 | vlc_img = vlc_db.get_image(img_uuid) 83 | 84 | vlc_img_msg = vlc_image_to_msg(vlc_img) 85 | vlc_img_converted = vlc_image_from_msg(vlc_img_msg) 86 | assert vlc_img_converted.metadata == vlc_img.metadata 87 | -------------------------------------------------------------------------------- /resources/arch.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/resources/arch.jpg -------------------------------------------------------------------------------- /resources/left_img_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/resources/left_img_0.png -------------------------------------------------------------------------------- /resources/left_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/resources/left_img_1.png -------------------------------------------------------------------------------- /resources/right_img_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MIT-SPARK/Ouroboros/3bbe3337ed10b935378149ca7dc628366d08023b/resources/right_img_1.png --------------------------------------------------------------------------------