├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── cpp ├── k4a │ ├── CMakeLists.txt │ ├── README.md │ ├── build_windows.sh │ ├── cmake │ │ ├── Findk4a.cmake │ │ └── spectacularAI_k4aPluginConfig.cmake │ └── vio_jsonl.cpp ├── oak │ ├── CMakeLists.txt │ ├── README.md │ ├── vio_jsonl.cpp │ └── vio_replay.cpp ├── offline │ ├── CMakeLists.txt │ ├── README.md │ ├── ffmpeg.hpp │ ├── input.cpp │ ├── input.hpp │ ├── plot_positions.py │ └── vio_jsonl.cpp ├── orbbec │ ├── CMakeLists.txt │ ├── README.md │ ├── build_windows.sh │ └── vio_jsonl.cpp ├── realsense │ ├── CMakeLists.txt │ ├── README.md │ ├── helpers.hpp │ ├── vio_jsonl.cpp │ ├── vio_mapper.cpp │ └── vio_mapper_legacy.cpp └── replay │ ├── CMakeLists.txt │ ├── README.md │ └── replay.cpp └── python ├── mapping ├── README.md ├── record_and_process_oak_d.sh ├── replay_to_instant_ngp.py └── replay_to_nerf.py └── oak ├── README.md ├── april_tag.py ├── depthai_combination.py ├── depthai_combination_install.sh ├── mapping.py ├── mapping_ar.py ├── mapping_ar_renderers ├── mesh.frag ├── mesh.py ├── mesh.vert ├── point_cloud.frag ├── point_cloud.py ├── point_cloud.vert └── util.py ├── mapping_ros.py ├── mapping_visu.py ├── mixed_reality.py ├── mixed_reality_replay.py ├── pen_3d.py ├── ros2 ├── README.md ├── launch │ ├── mapping.py │ └── mapping.rviz ├── package.xml ├── requirements.txt ├── resource │ └── spectacularai_depthai ├── setup.cfg ├── setup.py ├── spectacularai_depthai │ ├── __init__.py │ └── ros2_node.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── vio_hooks.py ├── vio_jsonl.py ├── vio_record.py ├── vio_replay.py └── vio_visu.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | __pycache__ 3 | venv 4 | target/ 5 | data/ 6 | python/oak/models/ 7 | .vscode 8 | python/oak/output/ 9 | cpp/mapping_visu/build/ 10 | cpp/mapping_visu/cmake-build-debug/ 11 | python/oak/ros2/build/ 12 | python/oak/ros2/install/ 13 | python/oak/ros2/log/ 14 | python/mapping/recordings 15 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cpp/offline/lodepng"] 2 | path = cpp/offline/lodepng 3 | url = https://github.com/lvandeve/lodepng 4 | [submodule "cpp/offline/json"] 5 | path = cpp/offline/json 6 | url = https://github.com/nlohmann/json 7 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![SDK install demo](https://spectacularai.github.io/docs/gif/pip-install.gif) 2 | 3 | # Spectacular AI SDK examples 4 | 5 | **Spectacular AI SDK** fuses data from cameras and IMU sensors (accelerometer and gyroscope) 6 | and outputs an accurate 6-degree-of-freedom pose of a device. 7 | This is called Visual-Inertial SLAM (VISLAM) and it can be used in, among other cases, tracking 8 | (autonomous) robots and vehicles, as well as Augmented, Mixed and Virtual Reality. 9 | 10 | The SDK also includes a _Mapping API_ that can be used to access the full SLAM map for 11 | both real-time and offline 3D reconstruction use cases. 12 | 13 | ### Quick links 14 | 15 | #### [SDK documentation](https://spectacularai.github.io/docs/sdk/) 16 | #### [C++ release packages](https://github.com/SpectacularAI/sdk/releases) 17 | #### [Gaussian Splatting & NeRFs](https://spectacularai.github.io/docs/sdk/tools/nerf.html) 18 | 19 | ### List of examples 20 | 21 | * **[C++](https://github.com/SpectacularAI/sdk-examples/tree/main/cpp)** 22 | * **[Python / OAK-D](https://github.com/SpectacularAI/sdk-examples/tree/main/python/oak#spectacular-ai-python-sdk-examples-for-oak-d)** 23 | 24 | See also the parts of the SDK with public source code: 25 | 26 | * [C++ recording tools](https://github.com/SpectacularAI/sdk/tree/main/cpp) 27 | * [Python tools](https://github.com/SpectacularAI/sdk/tree/main/python/cli) 28 | 29 | ## Copyright 30 | 31 | The examples in this repository are licensed under Apache 2.0 (see LICENSE). 32 | 33 | The SDK itself (not included in this repository) is proprietary to Spectacular AI. 34 | The OAK / Depth AI wrapper available in PyPI is free for non-commercial use on x86_64 Windows and Linux platforms. 35 | For commerical licensing options and more SDK variants (ARM binaries & C++ API), 36 | contact us at https://www.spectacularai.com/#contact . 37 | -------------------------------------------------------------------------------- /cpp/k4a/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(MSVC) 2 | # Windows build uses newer features 3 | cmake_minimum_required(VERSION 3.21) 4 | else() 5 | cmake_minimum_required(VERSION 3.3) 6 | endif() 7 | 8 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake") 9 | 10 | project(spectacularAI_k4a_example) 11 | 12 | if(MSVC) 13 | set(CMAKE_CXX_STANDARD 20) 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") 15 | else() 16 | set(CMAKE_CXX_STANDARD 14) 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 18 | endif() 19 | 20 | find_package(Threads REQUIRED) 21 | find_package(spectacularAI_k4aPlugin REQUIRED) 22 | 23 | if(MSVC) # Must be after project() is called 24 | set(CMAKE_CXX_STANDARD 20) 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP /Gy") 26 | # ./cmake/Findk4a.cmake is only tested on Windows and ideally we would rely on system dependency 27 | find_package(k4a MODULE REQUIRED) 28 | else() 29 | set(CMAKE_CXX_STANDARD 14) 30 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 31 | find_package(k4a REQUIRED PATHS "${k4a_DIR}") 32 | SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--exclude-libs=ALL") 33 | endif() 34 | 35 | set(EXAMPLE_LIBS 36 | k4a::k4a 37 | spectacularAI::k4aPlugin) 38 | 39 | # enables searching for dynamic libraries from the relative path ../lib 40 | if(NOT MSVC) 41 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath='$ORIGIN/../lib:$ORIGIN/../lib/3rdparty'") 42 | endif() 43 | add_executable(vio_jsonl vio_jsonl.cpp) 44 | target_link_libraries(vio_jsonl ${EXAMPLE_LIBS}) 45 | 46 | if(MSVC) 47 | add_custom_command(TARGET vio_jsonl POST_BUILD 48 | COMMAND ${CMAKE_COMMAND} -E copy $ $ 49 | COMMAND_EXPAND_LISTS 50 | ) 51 | endif() 52 | -------------------------------------------------------------------------------- /cpp/k4a/README.md: -------------------------------------------------------------------------------- 1 | # Spectacular AI SDK for Azure Kinect DK 2 | 3 | **See https://spectacularai.github.io/docs/sdk/wrappers/k4a.html for instructions.** 4 | -------------------------------------------------------------------------------- /cpp/k4a/build_windows.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -z "$1" ]; then 4 | echo "Usage: $0 " 5 | exit 1 6 | fi 7 | 8 | set -eux 9 | 10 | spectacularAI_k4aPlugin_DIR=$1 11 | 12 | : "${BUILD_TYPE:=Release}" 13 | : "${WINDOWS_SDK_VERSION:=10.0.19041.0}" 14 | : "${WINDOWS_SDK_VERSION:=10.0.19041.0}" 15 | : "${VISUAL_STUDIO_VERSION:=Visual Studio 16 2019}" 16 | 17 | CMAKE_FLAGS=(-G "${VISUAL_STUDIO_VERSION}" -A x64 -DCMAKE_SYSTEM_VERSION=${WINDOWS_SDK_VERSION}) 18 | 19 | ROOT=$(pwd) 20 | TARGET="$ROOT/target" 21 | 22 | K4A_SDK_FOLDER_PATTERN=( /c/Program\ Files/Azure\ Kinect\ SDK* ) # Version number at the end 23 | K4A_SDK_FOLDER=${K4A_SDK_FOLDER_PATTERN[0]} 24 | 25 | echo "Using k4a SDK from directory: ${K4A_SDK_FOLDER}" 26 | 27 | mkdir -p "$TARGET" 28 | cd "$TARGET" 29 | cmake "${CMAKE_FLAGS[@]}" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" \ 30 | -Dk4a_DIR="${K4A_SDK_FOLDER}" \ 31 | -DspectacularAI_k4aPlugin_DIR="$spectacularAI_k4aPlugin_DIR" \ 32 | .. 33 | cmake --build . --config $BUILD_TYPE 34 | 35 | # Copy depth module used by k4a lib manually, CMake doesn't know about it and fixing that seemed impossible 36 | cp "${K4A_SDK_FOLDER}/sdk/windows-desktop/amd64/release/bin/depthengine_2_0.dll" "${TARGET}/Release" 37 | -------------------------------------------------------------------------------- /cpp/k4a/cmake/Findk4a.cmake: -------------------------------------------------------------------------------- 1 | #.rst: 2 | # Findk4a 3 | # ------- 4 | # 5 | # Find Azure Kinect Sensor SDK include dirs, and libraries. 6 | # 7 | # IMPORTED Targets 8 | # ^^^^^^^^^^^^^^^^ 9 | # 10 | # This module defines the :prop_tgt:`IMPORTED` targets: 11 | # 12 | # ``k4a::k4a`` 13 | # Defined if the system has Azure Kinect Sensor SDK. 14 | # 15 | # Result Variables 16 | # ^^^^^^^^^^^^^^^^ 17 | # 18 | # This module sets the following variables: 19 | # 20 | # :: 21 | # 22 | # k4a_FOUND True in case Azure Kinect Sensor SDK is found, otherwise false 23 | # k4a_ROOT Path to the root of found Azure Kinect Sensor SDK installation 24 | # 25 | # Example usage 26 | # ^^^^^^^^^^^^^ 27 | # 28 | # :: 29 | # 30 | # find_package(k4a REQUIRED) 31 | # 32 | # add_executable(foo foo.cc) 33 | # target_link_libraries(foo k4a::k4a) 34 | # 35 | # License 36 | # ^^^^^^^ 37 | # 38 | # Copyright (c) 2019 Tsukasa SUGIURA 39 | # Distributed under the MIT License. 40 | # 41 | # 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: 42 | # The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 43 | # 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. 44 | # 45 | 46 | find_path(k4a_INCLUDE_DIR 47 | NAMES 48 | k4a/k4a.h 49 | HINTS 50 | ${k4a_DIR}/sdk/ 51 | PATHS 52 | "${k4a_PATH_DIR}/sdk/" 53 | PATH_SUFFIXES 54 | include 55 | ) 56 | 57 | find_library(k4a_LIBRARY 58 | NAMES 59 | k4a.lib 60 | HINTS 61 | ${k4a_DIR}/sdk/windows-desktop/amd64/release 62 | PATHS 63 | "${k4a_PATH_DIR}/sdk/windows-desktop/amd64/release" 64 | PATH_SUFFIXES 65 | lib 66 | ) 67 | 68 | include(FindPackageHandleStandardArgs) 69 | find_package_handle_standard_args( 70 | k4a DEFAULT_MSG 71 | k4a_LIBRARY k4a_INCLUDE_DIR 72 | ) 73 | 74 | if(k4a_FOUND) 75 | add_library(k4a::k4a SHARED IMPORTED) 76 | 77 | set_target_properties(k4a::k4a PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${k4a_INCLUDE_DIR}") 78 | set_property(TARGET k4a::k4a APPEND PROPERTY IMPORTED_CONFIGURATIONS "RELEASE") 79 | set_target_properties(k4a::k4a PROPERTIES IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX") 80 | 81 | # TODO: Ugly "hack" to find the DLL. Couldn't find any better way of doing this 82 | string(REPLACE "lib/k4a.lib" "bin/k4a.dll" k4a_RUNTIME_LIBRARY "${k4a_LIBRARY}") 83 | 84 | set_target_properties(k4a::k4a PROPERTIES IMPORTED_IMPLIB "${k4a_LIBRARY}") 85 | set_target_properties(k4a::k4a PROPERTIES IMPORTED_LOCATION "${k4a_RUNTIME_LIBRARY}") 86 | 87 | get_filename_component(K4a_ROOT "${k4a_INCLUDE_DIR}" PATH) 88 | endif() 89 | -------------------------------------------------------------------------------- /cpp/k4a/cmake/spectacularAI_k4aPluginConfig.cmake: -------------------------------------------------------------------------------- 1 | include("${CMAKE_CURRENT_LIST_DIR}/spectacularAI_k4aPluginTargets.cmake") 2 | -------------------------------------------------------------------------------- /cpp/k4a/vio_jsonl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char *argv[]) { 6 | std::vector arguments(argv, argv + argc); 7 | spectacularAI::k4aPlugin::Configuration config; 8 | 9 | // If a folder is given as an argument, record session there 10 | if (argc >= 2) { 11 | config.recordingFolder = argv[1]; 12 | config.recordingOnly = true; 13 | } 14 | 15 | // Create vio pipeline using the config, and then start k4a device and vio. 16 | spectacularAI::k4aPlugin::Pipeline vioPipeline(config); 17 | auto session = vioPipeline.startSession(); 18 | 19 | while (true) { 20 | auto vioOut = session->waitForOutput(); 21 | std::cout << vioOut->asJson() << std::endl; 22 | } 23 | 24 | return EXIT_SUCCESS; 25 | } 26 | -------------------------------------------------------------------------------- /cpp/oak/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(MSVC) 2 | # Windows build uses newer features 3 | cmake_minimum_required(VERSION 3.21) 4 | else() 5 | cmake_minimum_required(VERSION 3.3) 6 | endif() 7 | 8 | project(spectacularAI_depthaiPlugin_example) 9 | 10 | if(MSVC) 11 | set(CMAKE_CXX_STANDARD 20) 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") 13 | else() 14 | set(CMAKE_CXX_STANDARD 14) 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wl,--no-as-needed") 16 | endif() 17 | 18 | set(USE_OPENCV OFF CACHE STRING "Use OpenCV debug visualizations") 19 | 20 | find_package(depthai REQUIRED) 21 | find_package(spectacularAI_depthaiPlugin REQUIRED) 22 | 23 | set(EXAMPLE_LIBS 24 | depthai::core 25 | spectacularAI::depthaiPlugin) 26 | 27 | if(MSVC) 28 | # Depthai-core needs this and cmake can't find it otherwise 29 | find_package(usb-1.0 REQUIRED) 30 | list(APPEND EXAMPLE_LIBS usb-1.0) 31 | endif() 32 | 33 | if (USE_OPENCV) 34 | find_package(OpenCV REQUIRED) 35 | add_definitions(-DEXAMPLE_USE_OPENCV) 36 | list(APPEND EXAMPLE_LIBS "${OpenCV_LIBS}") 37 | list(APPEND EXAMPLE_LIBS depthai::opencv) 38 | endif() 39 | 40 | # enables searching for dynamic libraries from the relative path ../lib 41 | if(NOT MSVC) 42 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath='$ORIGIN/../lib:$ORIGIN/../lib/3rdparty'") 43 | endif() 44 | add_executable(vio_jsonl vio_jsonl.cpp) 45 | target_link_libraries(vio_jsonl ${EXAMPLE_LIBS}) 46 | 47 | add_executable(vio_replay vio_replay.cpp) 48 | target_link_libraries(vio_replay ${EXAMPLE_LIBS}) 49 | 50 | if(MSVC) 51 | add_custom_command(TARGET vio_jsonl POST_BUILD 52 | COMMAND ${CMAKE_COMMAND} -E copy $ $ 53 | COMMAND_EXPAND_LISTS 54 | ) 55 | add_custom_command(TARGET vio_replay POST_BUILD 56 | COMMAND ${CMAKE_COMMAND} -E copy $ $ 57 | COMMAND_EXPAND_LISTS 58 | ) 59 | endif() 60 | -------------------------------------------------------------------------------- /cpp/oak/README.md: -------------------------------------------------------------------------------- 1 | # Spectacular AI C++ SDK for OAK-D 2 | 3 | **See https://spectacularai.github.io/docs/sdk/wrappers/oak.html for instructions and API documentation** 4 | 5 | List of examples: 6 | 7 | * `vio_jsonl.cpp` for minimal tracking and recording 8 | * `vio_replay.cpp` replay API C++ example 9 | -------------------------------------------------------------------------------- /cpp/oak/vio_jsonl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #ifdef EXAMPLE_USE_OPENCV 5 | #include 6 | #endif 7 | 8 | int main(int argc, char** argv) { 9 | // Create Depth AI (OAK-D) pipeline 10 | dai::Pipeline pipeline; 11 | 12 | // Optional configuration 13 | spectacularAI::daiPlugin::Configuration config; 14 | // Example: enable these to support fisheye lenses (SDK 0.16+) 15 | // config.meshRectification = true; 16 | // config.depthScaleCorrection = true; 17 | 18 | // If a folder is given as an argument, record session there 19 | if (argc >= 2) config.recordingFolder = argv[1]; 20 | 21 | spectacularAI::daiPlugin::Pipeline vioPipeline(pipeline, config); 22 | 23 | #ifdef EXAMPLE_USE_OPENCV 24 | // note: must set useFeatureTracker = false or mono cam will not be read 25 | vioPipeline.hooks.monoPrimary = [&](std::shared_ptr img) { 26 | // Note: typically not main thread 27 | cv::imshow("primary mono cam", img->getCvFrame()); 28 | cv::waitKey(1); 29 | }; 30 | #endif 31 | 32 | // Connect to device and start pipeline 33 | dai::Device device(pipeline); 34 | auto vioSession = vioPipeline.startSession(device); 35 | 36 | while (true) { 37 | auto vioOut = vioSession->waitForOutput(); 38 | std::cout << vioOut->asJson() << std::endl; 39 | } 40 | 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /cpp/oak/vio_replay.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #ifdef EXAMPLE_USE_OPENCV 6 | #include 7 | #endif 8 | 9 | int main(int argc, char** argv) { 10 | if (argc < 2) { 11 | std::cout << "Usage: vio_replay path/to/recording" << std::endl; 12 | return 1; 13 | } 14 | 15 | std::string dataFolder = argv[1]; 16 | 17 | spectacularAI::Vio::Builder vioBuilder = spectacularAI::Vio::builder(); 18 | auto replayApi = spectacularAI::Replay::builder(dataFolder, vioBuilder) 19 | .build(); 20 | 21 | replayApi->setExtendedOutputCallback([&](spectacularAI::VioOutputPtr output, spectacularAI::FrameSet frames) { 22 | std::cout << output->asJson() << std::endl; 23 | #ifdef EXAMPLE_USE_OPENCV 24 | for (int i = 0; i < frames.size(); i++) { 25 | // Note: typically not main thread 26 | auto &frame = frames[i]; 27 | if (frame->image) { 28 | cv::Mat img = frame->image->asOpenCV(); 29 | cv::imshow("Video " + std::to_string(i), img); 30 | cv::waitKey(1); 31 | } 32 | } 33 | #else 34 | (void)frames; 35 | #endif 36 | }); 37 | 38 | replayApi->runReplay(); 39 | 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /cpp/offline/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.3) 2 | 3 | project(spectacularAI_offline_example) 4 | 5 | add_executable(vio_jsonl 6 | vio_jsonl.cpp 7 | input.cpp 8 | ) 9 | 10 | find_package(spectacularAI REQUIRED) 11 | add_library(lodepng "lodepng/lodepng.cpp") 12 | add_subdirectory(json) 13 | 14 | target_link_libraries(vio_jsonl PRIVATE spectacularAI::spectacularAI lodepng nlohmann_json::nlohmann_json) 15 | target_include_directories(vio_jsonl PRIVATE ".") 16 | -------------------------------------------------------------------------------- /cpp/offline/README.md: -------------------------------------------------------------------------------- 1 | # Spectacular AI main API example 2 | 3 | This example shows how to run basic stereo VIO using offline data to mimic real-time use case. Functions in the main header `spectacularAI/vio.hpp` are used primarily. 4 | 5 | * Tested platforms: Linux 6 | * Dependencies: CMake, FFmpeg (for video input) 7 | 8 | ## Setup 9 | 10 | * Install the Spectacular AI SDK 11 | * Clone the submodules: `cd cpp/offline/target && git submodule update --init --recursive`. 12 | * Build this example using CMake: 13 | 14 | ``` 15 | mkdir target 16 | cd target 17 | cmake -DspectacularAI_DIR= .. 18 | make 19 | ``` 20 | 21 | The `-DspectacularAI_DIR` option is not needed is you have used `sudo make install` for the SDK. 22 | 23 | ## Usage 24 | 25 | In the target folder, run `./vio_jsonl -i path/to/data -o out.jsonl`, where 26 | 27 | * `-i` specifies the input folder, see details below. If omitted, mock data will be used. 28 | * `-o` specifies output JSONL file. If omitted, prints instead to stdout. 29 | 30 | Input data is read from a given folder with the following hierarchy: 31 | 32 | ``` 33 | ├── calibration.json 34 | ├── vio_config.yaml 35 | ├── data.jsonl 36 | ├── data.mp4 37 | └── data2.mp4 38 | ``` 39 | 40 | when using video files. And if instead using PNG images: 41 | 42 | ``` 43 | ├── calibration.json 44 | ├── vio_config.yaml 45 | ├── data.jsonl 46 | ├── frames1 47 | │   ├── 00000000.png 48 | │   ├── 00000001.png 49 | │   ├── ... 50 | │   └── 00000600.png 51 | └── frames2 52 | ├── 00000000.png 53 | ├── 00000001.png 54 | ├── ... 55 | └── 00000600.png 56 | ``` 57 | 58 | ## Debugging 59 | 60 | The option `-r ` records the session input data and VIO output to the given folder. If the produced video files do not look correct when viewed in a video player, there may be issue with the image data input into the SDK. 61 | 62 | ## Visualization 63 | 64 | To plot the position track from `-o out.jsonl`, you can use `python3 plot_positions.py out.jsonl`. 65 | 66 | ## Copyright 67 | 68 | For the included libraries, see 69 | * [nlohmann/json](https://github.com/nlohmann/json): `json/LICENSE.MIT` 70 | * [lodepng](https://lodev.org/lodepng/): `lodepng/LICENSE` 71 | 72 | For access to the C++ SDK, contact us at . 73 | 74 | Available for multiple OSes and CPU architectures. 75 | -------------------------------------------------------------------------------- /cpp/offline/ffmpeg.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SPECTACULAR_AI_OFFLINE_FFMPEG_HPP 2 | #define SPECTACULAR_AI_OFFLINE_FFMPEG_HPP 3 | 4 | #include 5 | #include 6 | 7 | // Run a shell command and return its stdout (not stderr). 8 | std::string exec(const std::string &cmd) { 9 | std::array buffer; 10 | std::string result; 11 | std::shared_ptr pipe(popen(cmd.c_str(), "r"), pclose); 12 | assert(pipe); 13 | while (!feof(pipe.get())) { 14 | if (fgets(buffer.data(), 128, pipe.get()) != nullptr) 15 | result += buffer.data(); 16 | } 17 | return result; 18 | } 19 | 20 | bool ffprobeResolution(const std::string &videoPath, int &width, int &height) { 21 | std::string cmd = "ffprobe -v error -select_streams v:0 -show_entries stream=width,height -of csv=s=x:p=0 " + videoPath; 22 | std::string resolutionText = exec(cmd + " 2>/dev/null"); 23 | if (sscanf(resolutionText.c_str(), "%dx%d", &width, &height) == 2) { 24 | return true; 25 | } 26 | assert(false); 27 | return false; 28 | } 29 | 30 | struct VideoInput { 31 | private: 32 | FILE *pipe = nullptr; 33 | 34 | public: 35 | int width = 0; 36 | int height = 0; 37 | 38 | VideoInput(const std::string &videoPath) { 39 | bool success = ffprobeResolution(videoPath, width, height); 40 | assert(success && width > 0 && height > 0); 41 | std::stringstream ss; 42 | ss << "ffmpeg -i " << videoPath 43 | << " -f rawvideo -vcodec rawvideo -vsync vfr -pix_fmt gray - 2>/dev/null"; 44 | pipe = popen(ss.str().c_str(), "r"); 45 | assert(pipe); 46 | } 47 | 48 | VideoInput(const VideoInput&) = delete; 49 | 50 | ~VideoInput() { 51 | assert(pipe); 52 | fflush(pipe); 53 | pclose(pipe); 54 | } 55 | 56 | bool read(std::vector &video, int &width, int &height) { 57 | width = this->width; 58 | height = this->height; 59 | assert(pipe); 60 | int n = width * height; 61 | video.resize(n); 62 | int count = std::fread(video.data(), 1, n, pipe); 63 | return count == n; 64 | } 65 | }; 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /cpp/offline/input.cpp: -------------------------------------------------------------------------------- 1 | #include "input.hpp" 2 | 3 | #include "ffmpeg.hpp" 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | namespace { 12 | 13 | const std::string SEPARATOR = "/"; 14 | 15 | using json = nlohmann::json; 16 | 17 | // Format strings in printf style. 18 | template 19 | std::string stringFormat(const std::string &f, Args ... args) { 20 | int n = std::snprintf(nullptr, 0, f.c_str(), args ...) + 1; 21 | assert(n > 0); 22 | auto nn = static_cast(n); 23 | auto buf = std::make_unique(nn); 24 | std::snprintf(buf.get(), nn, f.c_str(), args ...); 25 | return std::string(buf.get(), buf.get() + nn - 1); 26 | } 27 | 28 | std::vector findVideos(const std::string &folderPath, bool &isImageFolder) { 29 | isImageFolder = false; 30 | std::vector videos; 31 | for (size_t cameraInd = 0; cameraInd < 2; ++cameraInd) { 32 | std::string videoPathNoSuffix = folderPath + SEPARATOR + "data"; 33 | if (cameraInd > 0) videoPathNoSuffix += std::to_string(cameraInd + 1); 34 | for (std::string suffix : { "mov", "avi", "mp4", "mkv" }) { 35 | const std::string videoPath = videoPathNoSuffix + "." + suffix; 36 | std::ifstream testFile(videoPath); 37 | if (testFile.is_open()) videos.push_back(videoPath); 38 | } 39 | 40 | const std::string imageFolder = stringFormat("%s/frames%d", folderPath.c_str(), cameraInd + 1); 41 | const std::string firstImage = stringFormat("%s/%08d.png", imageFolder.c_str(), 0); 42 | std::ifstream testFile(firstImage); 43 | if (testFile.is_open()) isImageFolder = true; 44 | } 45 | return videos; 46 | } 47 | 48 | // Read PNG image to buffer. 49 | bool readImage( 50 | const std::string &filePath, 51 | std::vector &data, 52 | std::vector &tmpBuffer, 53 | int &width, 54 | int &height 55 | ) { 56 | std::ifstream file(filePath); 57 | if (!file.is_open()) { 58 | printf("No such file %s\n", filePath.c_str()); 59 | return false; 60 | } 61 | tmpBuffer.clear(); 62 | unsigned w, h; 63 | unsigned error = lodepng::decode(tmpBuffer, w, h, filePath); 64 | if (error) { 65 | printf("Error %s\n", lodepng_error_text(error)); 66 | return false; 67 | } 68 | assert(tmpBuffer.size() == 4 * w * h); 69 | data.resize(w * h); 70 | for (size_t i = 0; i < w * h; ++i) { 71 | // Get green channel from RGBA. Any reasonable gray-scale conversion should work for VIO. 72 | data[i] = tmpBuffer.at(4 * i + 1); 73 | } 74 | width = static_cast(w); 75 | height = static_cast(h); 76 | return true; 77 | } 78 | 79 | class InputJsonl : public Input { 80 | public: 81 | InputJsonl(const std::string &inputFolderPath) : 82 | inputFolderPath(inputFolderPath) 83 | { 84 | imu = std::make_shared(spectacularAI::Vector3d{ 0.0, 0.0, 0.0 }); 85 | jsonlFile.open(inputFolderPath + SEPARATOR + "data.jsonl"); 86 | if (!jsonlFile.is_open()) { 87 | printf("No data.jsonl file found. Does `%s` exist?\n", inputFolderPath.c_str()); 88 | assert(false); 89 | } 90 | videoPaths = findVideos(inputFolderPath, useImageInput); 91 | assert(!videoPaths.empty() || useImageInput); 92 | for (const std::string &videoPath : videoPaths) { 93 | videoInputs.push_back(std::make_unique(videoPath)); 94 | } 95 | } 96 | 97 | std::string getConfig() const final { 98 | std::ifstream configFile(inputFolderPath + SEPARATOR + "vio_config.yaml"); 99 | if (!configFile.is_open()) { 100 | printf("No vio_config.yaml provided, using default config.\n"); 101 | return ""; 102 | } 103 | std::ostringstream oss; 104 | oss << configFile.rdbuf(); 105 | return oss.str(); 106 | } 107 | 108 | std::string getCalibration() const final { 109 | std::ifstream calibrationFile(inputFolderPath + SEPARATOR + "calibration.json"); 110 | // Calibration is always required. 111 | assert(calibrationFile.is_open()); 112 | std::ostringstream oss; 113 | oss << calibrationFile.rdbuf(); 114 | return oss.str(); 115 | } 116 | 117 | bool next(Data &data) final { 118 | if (!std::getline(jsonlFile, line)) return false; 119 | data.video0 = nullptr; 120 | data.video1 = nullptr; 121 | data.accelerometer = nullptr; 122 | data.gyroscope = nullptr; 123 | 124 | json j = json::parse(line, nullptr, false); // stream, callback, allow_exceptions 125 | if (!j.contains("time")) return true; 126 | data.timestamp = j["time"].get(); 127 | 128 | if (j.find("sensor") != j.end()) { 129 | std::array v = j["sensor"]["values"]; 130 | *imu = { .x = v[0], .y = v[1], .z = v[2] }; 131 | const std::string sensorType = j["sensor"]["type"]; 132 | if (sensorType == "gyroscope") { 133 | data.gyroscope = imu; 134 | } 135 | else if (sensorType == "accelerometer") { 136 | data.accelerometer = imu; 137 | } 138 | } 139 | else if (j.find("frames") != j.end()) { 140 | json jFrames = j["frames"]; 141 | size_t cameraCount = jFrames.size(); 142 | assert(cameraCount >= 1); 143 | int number = j["number"].get(); 144 | for (size_t cameraInd = 0; cameraInd < cameraCount; ++cameraInd) { 145 | std::vector &video = cameraInd == 0 ? video0 : video1; 146 | if (useImageInput) { 147 | std::string filePath = stringFormat("%s/frames%zu/%08zu.png", 148 | inputFolderPath.c_str(), cameraInd + 1, number); 149 | bool success = readImage(filePath, video, tmpBuffer, data.width, data.height); 150 | assert(success); 151 | } 152 | else { 153 | bool success = videoInputs.at(cameraInd)->read(video, data.width, data.height); 154 | assert(success); 155 | } 156 | uint8_t *&dataVideo = cameraInd == 0 ? data.video0 : data.video1; 157 | dataVideo = video.data(); 158 | } 159 | } 160 | return true; 161 | } 162 | 163 | private: 164 | std::ifstream jsonlFile; 165 | std::string line; 166 | std::shared_ptr imu; 167 | const std::string inputFolderPath; 168 | std::vector video0, video1, tmpBuffer; 169 | bool useImageInput = false; 170 | std::vector videoPaths; 171 | std::vector> videoInputs; 172 | }; 173 | 174 | class InputMock : public Input { 175 | public: 176 | int n = 0; 177 | const int width; 178 | const int height; 179 | std::vector video0, video1; 180 | std::shared_ptr imu; 181 | 182 | InputMock() : width(640), height(480), 183 | video0(width * height), video1(width * height) 184 | { 185 | imu = std::make_shared(spectacularAI::Vector3d{ 0.0, 0.0, 0.0 }); 186 | printf("Using mock input, VIO may not output anything.\n"); 187 | } 188 | 189 | bool next(Data &data) final { 190 | const size_t ITERATIONS = 100; 191 | data.video0 = video0.data(); 192 | data.video1 = video1.data(); 193 | data.width = width; 194 | data.height = height; 195 | data.accelerometer = imu; 196 | data.gyroscope = imu; 197 | data.timestamp += 0.1; 198 | return n++ < ITERATIONS; 199 | } 200 | 201 | std::string getConfig() const final { 202 | return ""; 203 | } 204 | 205 | std::string getCalibration() const final { 206 | return R"({ "cameras": [ 207 | { "focalLengthX": 1.0, "focalLengthY": 1.0, "model": "pinhole", "imuToCamera": [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]] }, 208 | { "focalLengthX": 1.0, "focalLengthY": 1.0, "model": "pinhole", "imuToCamera": [[1,0,0,1],[0,1,0,0],[0,0,1,0],[0,0,0,1]] } 209 | ] })"; 210 | } 211 | }; 212 | 213 | } // anonymous namespace 214 | 215 | std::unique_ptr Input::buildJsonl(const std::string &inputFolderPath) { 216 | return std::unique_ptr( 217 | new InputJsonl(inputFolderPath)); 218 | } 219 | 220 | std::unique_ptr Input::buildMock() { 221 | return std::unique_ptr( 222 | new InputMock()); 223 | } 224 | -------------------------------------------------------------------------------- /cpp/offline/input.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SPECTACULAR_AI_OFFLINE_INPUT_HPP 2 | #define SPECTACULAR_AI_OFFLINE_INPUT_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | class Input { 10 | public: 11 | struct Data { 12 | double timestamp = 0.0; 13 | uint8_t *video0 = nullptr; 14 | uint8_t *video1 = nullptr; 15 | int width = -1; 16 | int height = -1; 17 | std::shared_ptr accelerometer; 18 | std::shared_ptr gyroscope; 19 | }; 20 | virtual bool next(Data &data) = 0; 21 | virtual std::string getConfig() const = 0; 22 | virtual std::string getCalibration() const = 0; 23 | virtual ~Input() {}; 24 | 25 | static std::unique_ptr buildJsonl(const std::string &inputFolderPath); 26 | static std::unique_ptr buildMock(); 27 | }; 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /cpp/offline/plot_positions.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """Plot position trajectory from JSONL output.""" 4 | 5 | import json 6 | import sys 7 | 8 | import numpy as np 9 | 10 | POSITION = 'position' 11 | 12 | def read_jsonl(fn): 13 | with open(fn) as f: 14 | for l in f: yield(json.loads(l)) 15 | 16 | def read_data(fn): 17 | pos = [] 18 | for o in read_jsonl(fn): 19 | if POSITION not in o: continue 20 | pos.append([o[POSITION][c] for c in 'xyz']) 21 | return np.array(pos) 22 | 23 | if __name__ == '__main__': 24 | import argparse 25 | import matplotlib.pyplot as plt 26 | 27 | p = argparse.ArgumentParser(__doc__) 28 | p.add_argument('jsonl', help='VIO output file') 29 | p.add_argument('-image', help='Save image to this path instead of showing the plot') 30 | args = p.parse_args() 31 | 32 | pos = read_data(args.jsonl) 33 | if pos.size == 0: 34 | print("No data to plot.") 35 | sys.exit() 36 | 37 | plt.plot(pos[:,0], pos[:,1]) 38 | plt.axis('equal') 39 | 40 | if args.image: 41 | plt.savefig(args.image) 42 | else: 43 | plt.show() 44 | -------------------------------------------------------------------------------- /cpp/offline/vio_jsonl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include "input.hpp" 11 | 12 | int main(int argc, char *argv[]) { 13 | std::vector arguments(argv, argv + argc); 14 | std::unique_ptr outputFile; 15 | std::unique_ptr input; 16 | std::string recordingFolder = ""; 17 | for (size_t i = 1; i < arguments.size(); ++i) { 18 | const std::string &argument = arguments.at(i); 19 | if (argument == "-o") { 20 | outputFile = std::make_unique(arguments.at(++i)); 21 | assert(outputFile->is_open()); 22 | } 23 | else if (argument == "-r") recordingFolder = arguments.at(++i); 24 | else if (argument == "-i") input = Input::buildJsonl(arguments.at(++i)); 25 | } 26 | std::ostream &output = outputFile ? *outputFile : std::cout; 27 | if (!input) input = Input::buildMock(); 28 | 29 | std::ostringstream config; 30 | // This option makes the data input wait for VIO to finish to avoid dropping frames. 31 | // It should not be used in real-time scenarios. 32 | config << "blockingReplay: True\n"; 33 | 34 | config << input->getConfig(); 35 | auto builder = spectacularAI::Vio::builder() 36 | .setConfigurationYAML(config.str()) 37 | .setCalibrationJSON(input->getCalibration()); 38 | if (!recordingFolder.empty()) builder.setRecordingFolder(recordingFolder); 39 | std::unique_ptr vio = builder.build(); 40 | 41 | vio->setOutputCallback([&](spectacularAI::VioOutputPtr vioOutput) { 42 | output << vioOutput->asJson().c_str() << std::endl; 43 | }); 44 | 45 | Input::Data data; 46 | while (input->next(data)) { 47 | if (data.video0 && data.video1) { 48 | vio->addFrameStereo(data.timestamp, data.width, data.height, data.video0, data.video1, 49 | spectacularAI::ColorFormat::GRAY); 50 | } 51 | else if (data.video0) { 52 | vio->addFrameMono(data.timestamp, data.width, data.height, data.video0, 53 | spectacularAI::ColorFormat::GRAY); 54 | } 55 | 56 | if (data.accelerometer) { 57 | vio->addAcc(data.timestamp, *data.accelerometer); 58 | } 59 | if (data.gyroscope) { 60 | vio->addGyro(data.timestamp, *data.gyroscope); 61 | } 62 | } 63 | 64 | return 0; 65 | } 66 | -------------------------------------------------------------------------------- /cpp/orbbec/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(MSVC) 2 | # Windows build uses newer features 3 | cmake_minimum_required(VERSION 3.21) 4 | else() 5 | cmake_minimum_required(VERSION 3.3) 6 | endif() 7 | 8 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake") 9 | 10 | project(spectacularAI_orbbec_example) 11 | 12 | if(MSVC) 13 | set(CMAKE_CXX_STANDARD 20) 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") 15 | else() 16 | set(CMAKE_CXX_STANDARD 14) 17 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 18 | endif() 19 | 20 | find_package(Threads REQUIRED) 21 | find_package(spectacularAI_orbbecPlugin REQUIRED) 22 | find_package(OrbbecSDK REQUIRED PATHS "${OrbbecSDK_DIR}") 23 | 24 | if(MSVC) # Must be after project() is called 25 | set(CMAKE_CXX_STANDARD 20) 26 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP /Gy") 27 | else() 28 | set(CMAKE_CXX_STANDARD 14) 29 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 30 | SET(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--exclude-libs=ALL") 31 | endif() 32 | 33 | set(EXAMPLE_LIBS 34 | spectacularAI::orbbecPlugin 35 | OrbbecSDK::OrbbecSDK) 36 | 37 | # enables searching for dynamic libraries from the relative path ../lib 38 | if(NOT MSVC) 39 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath='$ORIGIN/../lib:$ORIGIN/../lib/3rdparty'") 40 | endif() 41 | 42 | # Minimal example 43 | add_executable(vio_jsonl vio_jsonl.cpp) 44 | target_link_libraries(vio_jsonl ${EXAMPLE_LIBS}) 45 | 46 | if(MSVC) 47 | add_custom_command(TARGET vio_jsonl POST_BUILD 48 | COMMAND ${CMAKE_COMMAND} -E copy $ $ 49 | COMMAND_EXPAND_LISTS 50 | ) 51 | 52 | # Explicitly copy live555.dll & ob_usb.dll (not included in TARGET_RUNTIME_DLLS:vio_jsonl) 53 | # NOTE: remove these lines if using older OrbbecSDK without live555.dll and ob_usb.dll 54 | set(LIVE555_DLL_PATH "${OrbbecSDK_LIBS_DIR}/live555.dll") 55 | set(OB_USB_DLL_PATH "${OrbbecSDK_LIBS_DIR}/ob_usb.dll") 56 | add_custom_command(TARGET vio_jsonl POST_BUILD 57 | COMMAND ${CMAKE_COMMAND} -E copy_if_different ${LIVE555_DLL_PATH} $ 58 | COMMAND ${CMAKE_COMMAND} -E copy_if_different ${OB_USB_DLL_PATH} $ 59 | ) 60 | endif() -------------------------------------------------------------------------------- /cpp/orbbec/README.md: -------------------------------------------------------------------------------- 1 | # Spectacular AI SDK for Orbbec 2 | 3 | **See https://spectacularai.github.io/docs/sdk/wrappers/orbbec.html for instructions.** 4 | -------------------------------------------------------------------------------- /cpp/orbbec/build_windows.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ -z "$1" ]; then 4 | echo "Usage: $0 " 5 | exit 1 6 | fi 7 | 8 | if [ -z "$2" ]; then 9 | echo "Usage: $0 " 10 | exit 1 11 | fi 12 | 13 | set -eux 14 | 15 | spectacularAI_orbbecPlugin_DIR=$1 16 | OrbbecSDK_DIR=$2 17 | 18 | : "${BUILD_TYPE:=Release}" 19 | : "${WINDOWS_SDK_VERSION:=10.0.19041.0}" 20 | : "${WINDOWS_SDK_VERSION:=10.0.19041.0}" 21 | : "${VISUAL_STUDIO_VERSION:=Visual Studio 16 2019}" 22 | 23 | CMAKE_FLAGS=(-G "${VISUAL_STUDIO_VERSION}" -A x64 -DCMAKE_SYSTEM_VERSION=${WINDOWS_SDK_VERSION}) 24 | 25 | ROOT=$(pwd) 26 | TARGET="$ROOT/target" 27 | 28 | mkdir -p "$TARGET" 29 | cd "$TARGET" 30 | cmake "${CMAKE_FLAGS[@]}" \ 31 | -DOrbbecSDK_DIR="${OrbbecSDK_DIR}" \ 32 | -DspectacularAI_orbbecPlugin_DIR="$spectacularAI_orbbecPlugin_DIR" \ 33 | .. 34 | cmake --build . --config $BUILD_TYPE 35 | -------------------------------------------------------------------------------- /cpp/orbbec/vio_jsonl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | int main(int argc, char *argv[]) { 7 | std::vector arguments(argv, argv + argc); 8 | ob::Context::setLoggerSeverity(OB_LOG_SEVERITY_OFF); 9 | 10 | // Create OrbbecSDK pipeline (with default device). 11 | ob::Pipeline obPipeline; 12 | 13 | // Create Spectacular AI orbbec plugin configuration (depends on device type). 14 | spectacularAI::orbbecPlugin::Configuration config(obPipeline); 15 | 16 | // If a folder is given as an argument, record session there 17 | if (argc >= 2) { 18 | config.recordingFolder = argv[1]; 19 | config.recordingOnly = true; 20 | } 21 | 22 | // Create VIO pipeline & setup orbbec pipeline 23 | spectacularAI::orbbecPlugin::Pipeline vioPipeline(obPipeline, config); 24 | 25 | // and then start orbbec device and vio. 26 | auto session = vioPipeline.startSession(); 27 | 28 | while (true) { 29 | auto vioOut = session->waitForOutput(); 30 | std::cout << vioOut->asJson() << std::endl; 31 | } 32 | 33 | return EXIT_SUCCESS; 34 | } 35 | -------------------------------------------------------------------------------- /cpp/realsense/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(MSVC) 2 | # Windows build uses newer features 3 | cmake_minimum_required(VERSION 3.21) 4 | else() 5 | cmake_minimum_required(VERSION 3.3) 6 | endif() 7 | 8 | project(spectacularAI_realsense_example) 9 | 10 | if(MSVC) 11 | set(CMAKE_CXX_STANDARD 20) 12 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /MP") 13 | else() 14 | set(CMAKE_CXX_STANDARD 14) 15 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra") 16 | endif() 17 | 18 | find_package(realsense2 REQUIRED) 19 | find_package(spectacularAI_realsensePlugin REQUIRED) 20 | 21 | set(EXAMPLE_LIBS 22 | realsense2::realsense2 23 | spectacularAI::realsensePlugin) 24 | 25 | # enables searching for dynamic libraries from the relative path ../lib 26 | if(NOT MSVC) 27 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-rpath='$ORIGIN/../lib:$ORIGIN/../lib/3rdparty'") 28 | endif() 29 | add_executable(vio_jsonl vio_jsonl.cpp) 30 | target_link_libraries(vio_jsonl ${EXAMPLE_LIBS}) 31 | 32 | if(MSVC) 33 | add_custom_command(TARGET vio_jsonl POST_BUILD 34 | COMMAND ${CMAKE_COMMAND} -E copy $ $ 35 | COMMAND_EXPAND_LISTS 36 | ) 37 | endif() 38 | 39 | set(BUILD_MAPPER ON CACHE STRING "Build VIO mapper example (requires OpenCV)") 40 | if (BUILD_MAPPER) 41 | find_package(Threads REQUIRED) 42 | set(USE_STATIC_OPENCV OFF CACHE STRING "Use OpenCV as statically linked library (internal and unsupported flag, do not use)") 43 | if (USE_STATIC_OPENCV) 44 | find_package(mobile-cv-suite REQUIRED) 45 | set(MAPPER_LIBS mobile-cv-suite::static mobile-cv-suite::imgcodecs Threads::Threads) 46 | else() 47 | find_package(OpenCV REQUIRED) 48 | set(MAPPER_LIBS "${OpenCV_LIBS}" Threads::Threads) 49 | endif() 50 | 51 | add_executable(vio_mapper vio_mapper.cpp) 52 | target_link_libraries(vio_mapper ${EXAMPLE_LIBS} ${MAPPER_LIBS}) 53 | if(MSVC) 54 | add_custom_command(TARGET vio_mapper POST_BUILD 55 | COMMAND ${CMAKE_COMMAND} -E copy $ $ 56 | COMMAND_EXPAND_LISTS 57 | ) 58 | endif() 59 | 60 | add_executable(vio_mapper_legacy vio_mapper_legacy.cpp) 61 | target_link_libraries(vio_mapper_legacy ${EXAMPLE_LIBS} ${MAPPER_LIBS}) 62 | if(MSVC) 63 | add_custom_command(TARGET vio_mapper_legacy POST_BUILD 64 | COMMAND ${CMAKE_COMMAND} -E copy $ $ 65 | COMMAND_EXPAND_LISTS 66 | ) 67 | endif() 68 | endif() 69 | -------------------------------------------------------------------------------- /cpp/realsense/README.md: -------------------------------------------------------------------------------- 1 | # Spectacular AI C++ SDK for RealSense depth cameras 2 | 3 | **See https://spectacularai.github.io/docs/sdk/wrappers/realsense.html for instructions.** 4 | -------------------------------------------------------------------------------- /cpp/realsense/helpers.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifdef _MSC_VER 4 | #include 5 | #endif 6 | 7 | int makeDir(const std::string &dir) { 8 | #ifdef _MSC_VER 9 | return _mkdir(dir.c_str()); 10 | #else 11 | mode_t mode = 0755; 12 | return mkdir(dir.c_str(), mode); 13 | #endif 14 | } 15 | 16 | bool folderExists(const std::string &folder) { 17 | #ifdef _MSC_VER 18 | struct _stat info; 19 | if (_stat(folder.c_str(), &info) != 0) return false; 20 | return (info.st_mode & _S_IFDIR) != 0; 21 | #else 22 | struct stat info; 23 | if (stat(folder.c_str(), &info) != 0) return false; 24 | return (info.st_mode & S_IFDIR) != 0; 25 | #endif 26 | } 27 | 28 | bool createFolders(const std::string &folder) { 29 | int ret = makeDir(folder); 30 | if (ret == 0) return true; 31 | 32 | switch (errno) { 33 | case ENOENT: { 34 | size_t pos = folder.find_last_of('/'); 35 | if (pos == std::string::npos) 36 | #ifdef _MSC_VER 37 | pos = folder.find_last_of('\\'); 38 | if (pos == std::string::npos) 39 | #endif 40 | return false; 41 | if (!createFolders(folder.substr(0, pos))) 42 | return false; 43 | return 0 == makeDir(folder); 44 | } 45 | case EEXIST: 46 | return folderExists(folder); 47 | 48 | default: 49 | return false; 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /cpp/realsense/vio_jsonl.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv) { 6 | spectacularAI::rsPlugin::Configuration config; 7 | 8 | // If a folder is given as an argument, record session there 9 | if (argc >= 2) { 10 | config.recordingFolder = argv[1]; 11 | config.recordingOnly = true; 12 | } 13 | 14 | spectacularAI::rsPlugin::Pipeline vioPipeline(config); 15 | 16 | { 17 | // Find RealSense device 18 | rs2::context rsContext; 19 | rs2::device_list devices = rsContext.query_devices(); 20 | if (devices.size() != 1) { 21 | std::cout << "Connect exactly one RealSense device." << std::endl; 22 | return EXIT_SUCCESS; 23 | } 24 | rs2::device device = devices.front(); 25 | vioPipeline.configureDevice(device); 26 | } 27 | 28 | // Start pipeline 29 | rs2::config rsConfig; 30 | vioPipeline.configureStreams(rsConfig); 31 | auto vioSession = vioPipeline.startSession(rsConfig); 32 | 33 | while (true) { 34 | auto vioOut = vioSession->waitForOutput(); 35 | std::cout << vioOut->asJson() << std::endl; 36 | } 37 | 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /cpp/realsense/vio_mapper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "helpers.hpp" 20 | 21 | namespace { 22 | struct ImageToSave { 23 | std::string fileName; 24 | cv::Mat mat; 25 | }; 26 | 27 | int colorFormatToOpenCVType(spectacularAI::ColorFormat colorFormat) { 28 | switch (colorFormat) { 29 | case spectacularAI::ColorFormat::GRAY: return CV_8UC1; 30 | case spectacularAI::ColorFormat::GRAY16: return CV_16UC1; 31 | case spectacularAI::ColorFormat::RGB: return CV_8UC3; 32 | case spectacularAI::ColorFormat::RGBA: return CV_8UC4; 33 | default: return -1; 34 | } 35 | } 36 | 37 | std::function buildImageWriter( 38 | std::deque &queue, 39 | std::mutex &mutex, 40 | std::atomic &shouldQuit) 41 | { 42 | return [&queue, &mutex, &shouldQuit]() { 43 | while (!shouldQuit) { 44 | std::unique_lock lock(mutex); 45 | constexpr int LOOP_SLEEP_MS = 10; 46 | 47 | if (queue.empty()) { 48 | lock.unlock(); 49 | std::this_thread::sleep_for(std::chrono::milliseconds(LOOP_SLEEP_MS)); 50 | continue; 51 | } 52 | 53 | auto img = queue.front(); 54 | queue.pop_front(); 55 | lock.unlock(); 56 | 57 | // The SDK outputs RGB and OpenCV expects BGR. 58 | cv::Mat bgrMat; 59 | cv::cvtColor(img.mat, bgrMat, cv::COLOR_RGB2BGR); 60 | // If this line crashes, OpenCV probably has been built without PNG support. 61 | cv::imwrite(img.fileName.c_str(), bgrMat); 62 | } 63 | }; 64 | } 65 | 66 | cv::Mat copyImage(std::shared_ptr bitmap) { 67 | int cvType = colorFormatToOpenCVType(bitmap->getColorFormat()); 68 | return cv::Mat( 69 | bitmap->getHeight(), 70 | bitmap->getWidth(), 71 | cvType, 72 | const_cast(bitmap->getDataReadOnly()) 73 | ).clone(); 74 | } 75 | 76 | std::string matrix4ToString(const spectacularAI::Matrix4d &matrix) { 77 | std::stringstream ss; 78 | ss << "["; 79 | for (int i = 0; i < 4; ++i) 80 | for (int j = 0; j < 4; ++j) 81 | ss << matrix[i][j]; 82 | ss << "]"; 83 | return ss.str(); 84 | } 85 | 86 | void serializePosesToFile(std::ofstream& posesFile, std::shared_ptr keyframe) { 87 | auto& frameSet = keyframe->frameSet; 88 | std::stringstream ss; 89 | ss << "{\"frameId\": " << (keyframe->id) << "," 90 | << "\"poses\": {"; 91 | if (frameSet->rgbFrame) { 92 | ss << "\"rgb\": " << matrix4ToString(frameSet->rgbFrame->cameraPose.pose.asMatrix()); 93 | } 94 | if (frameSet->depthFrame) { 95 | if (frameSet->rgbFrame) ss << ","; 96 | ss << "\"depth\": " << matrix4ToString(frameSet->depthFrame->cameraPose.pose.asMatrix()); 97 | } 98 | ss << "}}"; 99 | std::cout << "saving " << ss.str() << std::endl; 100 | posesFile << ss.str() << std::endl; 101 | } 102 | } // namespace 103 | 104 | int main(int argc, char** argv) { 105 | // If a folder is given as an argument, record session there 106 | std::string recordingFolder; 107 | if (argc >= 2) { 108 | recordingFolder = argv[1]; 109 | createFolders(recordingFolder); 110 | } else { 111 | std::cerr 112 | << "Usage: " << argv[0] << " /path/to/recording/folder" << std::endl; 113 | return 1; 114 | } 115 | 116 | // The RS callback thread should not be blocked for long. 117 | // Using worker threads for image encoding and disk I/O 118 | std::atomic shouldQuit(false); 119 | constexpr int N_WORKER_THREADS = 4; 120 | std::mutex queueMutex; 121 | std::deque imageQueue; 122 | std::ofstream posesFile = std::ofstream(recordingFolder + "/poses.jsonl"); 123 | std::set savedFrames; 124 | std::vector fileNameBuf; 125 | fileNameBuf.resize(1000, 0); 126 | 127 | // Create vio pipeline with mapper callback 128 | spectacularAI::rsPlugin::Configuration vioConfig; 129 | spectacularAI::rsPlugin::Pipeline vioPipeline(vioConfig, [&](std::shared_ptr output) { 130 | for (int64_t frameId : output->updatedKeyFrames) { 131 | auto search = output->map->keyFrames.find(frameId); 132 | if (search == output->map->keyFrames.end()) { 133 | continue; // deleted frame 134 | } 135 | 136 | auto& frameSet = search->second->frameSet; 137 | 138 | if (savedFrames.count(frameId) == 0) { 139 | // Only save images once, despide frames pose possibly being updated several times 140 | savedFrames.insert(frameId); 141 | std::lock_guard lock(queueMutex); 142 | char *fileName = fileNameBuf.data(); 143 | // Copy images to ensure they are in memory later for saving 144 | if (frameSet->rgbFrame && frameSet->rgbFrame->image) { 145 | std::snprintf(fileName, fileNameBuf.size(), "%s/rgb_%04ld.png", recordingFolder.c_str(), frameId); 146 | imageQueue.push_back(ImageToSave {fileName, copyImage(frameSet->rgbFrame->image)}); 147 | } 148 | if (frameSet->depthFrame && frameSet->depthFrame->image) { 149 | std::snprintf(fileName, fileNameBuf.size(), "%s/depth_%04ld.png", recordingFolder.c_str(), frameId); 150 | imageQueue.push_back(ImageToSave {fileName, copyImage(frameSet->depthFrame->image)}); 151 | } 152 | // TODO: Save pointclouds as JSON? 153 | } 154 | } 155 | 156 | // Save only final fully optimized poses, might not contain poses for all frames in case they were deleted 157 | if (output->finalMap) { 158 | for (auto it = output->map->keyFrames.begin(); it != output->map->keyFrames.end(); it++) { 159 | serializePosesToFile(posesFile, it->second); 160 | } 161 | } 162 | }); 163 | 164 | { 165 | // Find RealSense device 166 | rs2::context rsContext; 167 | rs2::device_list devices = rsContext.query_devices(); 168 | if (devices.size() != 1) { 169 | std::cout << "Connect exactly one RealSense device." << std::endl; 170 | return EXIT_SUCCESS; 171 | } 172 | rs2::device device = devices.front(); 173 | vioPipeline.configureDevice(device); 174 | } 175 | 176 | // Start pipeline 177 | rs2::config rsConfig; 178 | vioPipeline.configureStreams(rsConfig); 179 | 180 | // VIO works fine with BGR-flipped data too 181 | rsConfig.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_BGR8); 182 | 183 | std::vector imageWriterThreads; 184 | for (int i = 0; i < N_WORKER_THREADS; ++i) { 185 | imageWriterThreads.emplace_back(buildImageWriter(imageQueue, queueMutex, shouldQuit)); 186 | } 187 | 188 | auto vioSession = vioPipeline.startSession(rsConfig); 189 | std::thread inputThread([&]() { 190 | std::cerr << "Press Enter to quit." << std::endl << std::endl; 191 | std::getchar(); 192 | shouldQuit = true; 193 | }); 194 | 195 | while (!shouldQuit) { 196 | auto vioOut = vioSession->waitForOutput(); 197 | } 198 | 199 | vioSession = nullptr; // Ensure Vio is done before we quit 200 | 201 | inputThread.join(); 202 | for (auto &t : imageWriterThreads) t.join(); 203 | std::cerr << "Bye!" << std::endl; 204 | return 0; 205 | } 206 | 207 | -------------------------------------------------------------------------------- /cpp/realsense/vio_mapper_legacy.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "helpers.hpp" 17 | 18 | namespace { 19 | struct ImageToSave { 20 | std::string fileName; 21 | cv::Mat mat; 22 | }; 23 | 24 | std::function buildImageWriter( 25 | std::deque &queue, 26 | std::mutex &mutex, 27 | std::atomic &shouldQuit) 28 | { 29 | return [&queue, &mutex, &shouldQuit]() { 30 | while (!shouldQuit) { 31 | std::unique_lock lock(mutex); 32 | constexpr int LOOP_SLEEP_MS = 10; 33 | 34 | if (queue.empty()) { 35 | lock.unlock(); 36 | std::this_thread::sleep_for(std::chrono::milliseconds(LOOP_SLEEP_MS)); 37 | continue; 38 | } 39 | 40 | auto img = queue.front(); 41 | queue.pop_front(); 42 | lock.unlock(); 43 | // If this line crashes, OpenCV probably has been built without PNG support. 44 | cv::imwrite(img.fileName.c_str(), img.mat); 45 | } 46 | }; 47 | } 48 | } 49 | 50 | int main(int argc, char** argv) { 51 | // If a folder is given as an argument, record session there 52 | std::string recordingFolder; 53 | int keyFrameInterval = 10; 54 | if (argc >= 2) { 55 | recordingFolder = argv[1]; 56 | createFolders(recordingFolder); 57 | if (argc >= 3) { 58 | keyFrameInterval = std::stoi(argv[2]); 59 | } 60 | } else { 61 | std::cerr 62 | << "Usage: " << argv[0] << " /path/to/recording/folder [N]" << std::endl 63 | << "where N is the frame sampling interval, default: " << keyFrameInterval << std::endl; 64 | return 1; 65 | } 66 | 67 | spectacularAI::rsPlugin::Pipeline vioPipeline; 68 | 69 | { 70 | // Find RealSense device 71 | rs2::context rsContext; 72 | rs2::device_list devices = rsContext.query_devices(); 73 | if (devices.size() != 1) { 74 | std::cout << "Connect exactly one RealSense device." << std::endl; 75 | return EXIT_SUCCESS; 76 | } 77 | rs2::device device = devices.front(); 78 | vioPipeline.configureDevice(device); 79 | } 80 | 81 | // Start pipeline 82 | rs2::config rsConfig; 83 | vioPipeline.configureStreams(rsConfig); 84 | 85 | // VIO works fine with BGR-flipped data too 86 | rsConfig.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_BGR8); 87 | 88 | // The RS callback thread should not be blocked for long. 89 | // Using worker threads for image encoding and disk I/O 90 | constexpr int N_WORKER_THREADS = 4; 91 | std::mutex queueMutex; 92 | std::deque imageQueue; 93 | std::atomic shouldQuit(false); 94 | 95 | std::vector imageWriterThreads; 96 | for (int i = 0; i < N_WORKER_THREADS; ++i) { 97 | imageWriterThreads.emplace_back(buildImageWriter(imageQueue, queueMutex, shouldQuit)); 98 | } 99 | 100 | int frameCounter = 0; 101 | std::vector fileNameBuf; 102 | fileNameBuf.resize(1000, 0); 103 | std::shared_ptr vioSession; 104 | 105 | auto callback = [ 106 | &frameCounter, 107 | &fileNameBuf, 108 | &vioSession, 109 | &queueMutex, 110 | &imageQueue, 111 | &shouldQuit, 112 | keyFrameInterval, 113 | recordingFolder 114 | ](const rs2::frame &frame) 115 | { 116 | if (shouldQuit) return; 117 | auto frameset = frame.as(); 118 | if (frameset && frameset.get_profile().stream_type() == RS2_STREAM_DEPTH) { 119 | auto vio = vioSession; // atomic 120 | if (!vio) return; 121 | 122 | if ((frameCounter++ % keyFrameInterval) != 0) return; 123 | int keyFrameNumber = ((frameCounter - 1) / keyFrameInterval) + 1; 124 | 125 | vio->addTrigger(frame.get_timestamp() * 1e-3, keyFrameNumber); 126 | 127 | rs2_stream depthAlignTarget = RS2_STREAM_COLOR; 128 | rs2::align alignDepth(depthAlignTarget); 129 | 130 | // This line can be commented out to disable aligning. 131 | frameset = alignDepth.process(frameset); 132 | 133 | const rs2::video_frame &depth = frameset.get_depth_frame(); 134 | const rs2::video_frame &color = frameset.get_color_frame(); 135 | assert(depth.get_profile().format() == RS2_FORMAT_Z16); 136 | assert(color.get_profile().format() == RS2_FORMAT_BGR8); 137 | 138 | // Display images for testing. 139 | uint8_t *colorData = const_cast((const uint8_t*)color.get_data()); 140 | cv::Mat colorMat(color.get_height(), color.get_width(), CV_8UC3, colorData); 141 | 142 | uint8_t *depthData = const_cast((const uint8_t*)depth.get_data()); 143 | cv::Mat depthMat(depth.get_height(), depth.get_width(), CV_16UC1, depthData); 144 | 145 | char *fileName = fileNameBuf.data(); 146 | std::snprintf(fileName, fileNameBuf.size(), "%s/depth_%04d.png", recordingFolder.c_str(), keyFrameNumber); 147 | cv::imwrite(fileName, depthMat); 148 | 149 | ImageToSave depthImg, colorImg; 150 | depthImg.fileName = fileName; // copy 151 | depthImg.mat = depthMat.clone(); 152 | 153 | std::snprintf(fileName, fileNameBuf.size(), "%s/rgb_%04d.png", recordingFolder.c_str(), keyFrameNumber); 154 | colorImg.fileName = fileName; 155 | colorImg.mat = colorMat.clone(); 156 | 157 | std::lock_guard lock(queueMutex); 158 | imageQueue.push_back(depthImg); 159 | imageQueue.push_back(colorImg); 160 | } 161 | }; 162 | 163 | vioSession = vioPipeline.startSession(rsConfig, callback); 164 | std::ofstream vioOutJsonl(recordingFolder + "/vio.jsonl"); 165 | 166 | std::thread inputThread([&]() { 167 | std::cerr << "Press Enter to quit." << std::endl << std::endl; 168 | std::getchar(); 169 | shouldQuit = true; 170 | }); 171 | 172 | while (!shouldQuit) { 173 | auto vioOut = vioSession->waitForOutput(); 174 | if (vioOut->tag > 0) { 175 | vioOutJsonl << "{\"tag\":" << vioOut->tag << ",\"vio\":" << vioOut->asJson() << "}" << std::endl; 176 | } 177 | } 178 | 179 | inputThread.join(); 180 | for (auto &t : imageWriterThreads) t.join(); 181 | std::cerr << "Bye!" << std::endl; 182 | return 0; 183 | } 184 | -------------------------------------------------------------------------------- /cpp/replay/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.3) 2 | 3 | project(spectacularAI_replay_example) 4 | 5 | add_executable(replay replay.cpp) 6 | 7 | find_package(spectacularAI REQUIRED) 8 | 9 | target_link_libraries(replay PRIVATE spectacularAI::spectacularAI) 10 | target_include_directories(replay PRIVATE ".") 11 | -------------------------------------------------------------------------------- /cpp/replay/README.md: -------------------------------------------------------------------------------- 1 | # Spectacular AI Replay API example 2 | 3 | This example shows how to replay recordings created using Spectacular AI SDK. The header `spectacularAI/replay.hpp` is primarily used. 4 | 5 | * Tested platforms: Linux 6 | * Dependencies: CMake, FFmpeg (for video input) 7 | 8 | ## Setup 9 | 10 | * Install the Spectacular AI SDK 11 | * Build this example using CMake: 12 | 13 | ``` 14 | mkdir target 15 | cd target 16 | cmake -DspectacularAI_DIR= .. 17 | make 18 | ``` 19 | 20 | The `-DspectacularAI_DIR` option is not needed is you have used `sudo make install` for the SDK. 21 | 22 | ## Usage 23 | 24 | In the target folder, run `./replay -i path/to/data -o out.jsonl`, where 25 | 26 | * `-i` specifies the input folder, see details below. 27 | * `-o` specifies output JSONL file. 28 | 29 | See the source code for more options. Input data is read from a given folder with the following hierarchy: 30 | 31 | ``` 32 | ├── calibration.json 33 | ├── vio_config.yaml 34 | ├── data.jsonl 35 | ├── data.mp4 36 | └── data2.mp4 37 | ``` 38 | 39 | when using video files. And if instead using PNG images: 40 | 41 | ``` 42 | ├── calibration.json 43 | ├── vio_config.yaml 44 | ├── data.jsonl 45 | ├── frames1 46 | │   ├── 00000000.png 47 | │   ├── 00000001.png 48 | │   ├── ... 49 | │   └── 00000600.png 50 | └── frames2 51 | ├── 00000000.png 52 | ├── 00000001.png 53 | ├── ... 54 | └── 00000600.png 55 | ``` 56 | 57 | ## Copyright 58 | 59 | For access to the C++ SDK, contact us at . 60 | 61 | Available for multiple OSes and CPU architectures. 62 | -------------------------------------------------------------------------------- /cpp/replay/replay.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | std::string readFileToString(const std::string &filename) { 12 | std::ifstream f(filename); 13 | std::ostringstream oss; 14 | oss << f.rdbuf(); 15 | return oss.str(); 16 | } 17 | 18 | bool fileExists(const std::string &filePath) { 19 | std::ifstream dataFile(filePath); 20 | return dataFile.is_open(); 21 | } 22 | 23 | int main(int argc, char *argv[]) { 24 | std::vector arguments(argv, argv + argc); 25 | std::unique_ptr outputFile; 26 | std::string inputFolder; 27 | std::string userConfigurationYaml; 28 | bool realtime = false; 29 | bool print = false; 30 | for (size_t i = 1; i < arguments.size(); ++i) { 31 | const std::string &argument = arguments.at(i); 32 | if (argument == "-o") { 33 | outputFile = std::make_unique(arguments.at(++i)); 34 | assert(outputFile->is_open()); 35 | } 36 | else if (argument == "-i") inputFolder = arguments.at(++i); 37 | else if (argument == "-c") userConfigurationYaml = readFileToString(arguments.at(++i)); 38 | else if (argument == "--realtime") realtime = true; 39 | else if (argument == "--print") print = true; 40 | } 41 | if (inputFolder.empty()) { 42 | std::cout << "Please specify input directory using `-i`."<< std::endl; 43 | return 0; 44 | } 45 | 46 | // The Replay API builder takes as input the main API builder as a way to share 47 | // configuration methods. 48 | spectacularAI::Vio::Builder vioBuilder = spectacularAI::Vio::builder() 49 | .setConfigurationYAML(userConfigurationYaml); 50 | 51 | std::unique_ptr replay 52 | = spectacularAI::Replay::builder(inputFolder, vioBuilder).build(); 53 | replay->setPlaybackSpeed(realtime ? 1.0 : -1.0); 54 | 55 | replay->setOutputCallback([&](spectacularAI::VioOutputPtr vioOutput) { 56 | if (outputFile) *outputFile << vioOutput->asJson().c_str() << std::endl; 57 | if (print) std::cout << vioOutput->asJson().c_str() << std::endl; 58 | }); 59 | 60 | const auto t0 = std::chrono::steady_clock::now(); 61 | 62 | replay->runReplay(); 63 | 64 | const auto time = std::chrono::duration( 65 | std::chrono::steady_clock::now() - t0).count(); 66 | printf("Replay took %.2fs\n", 1e-3 * time); 67 | 68 | return 0; 69 | } 70 | -------------------------------------------------------------------------------- /python/mapping/README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | ![OAK-D NeRF](https://spectacularai.github.io/docs/gif/oak-d-nerf.gif) 4 | 5 | # Gaussian Splatting & NeRFs 6 | 7 | This page has instructions for post-processing data recorded through the Spectacular AI SDK on supported devices, exporting to Nerfstudio, training NeRFs and 3DGS, and visualizing the process. The Spectacular AI mapping tool (`sai-cli process`) is powered by the Spectacular AI [Mapping API](https://spectacularai.github.io/docs/sdk/mapping.html). 8 | 9 | ## Installation 10 | 11 | These instructions assume you want to train NeRFs or 3DGS using Nerfstudio. For other uses, Nerfstudio and CUDA are not required, but you simply need Python, `pip` and FFmpeg. 12 | 13 | 1. [install Nerfstudio](https://github.com/nerfstudio-project/nerfstudio#1-installation-setup-the-environment) (**Requirement**: a good NVidia GPU + CUDA). 14 | 2. Install FFmpeg. Linux `apt install ffmpeg` (or similar, if using another package manager). Windows: [see here](https://www.editframe.com/guides/how-to-install-and-start-using-ffmpeg-in-under-10-minutes). FFmpeg must be in your `PATH` so that `ffmpeg` works on the command line. 15 | 3. In Nerfstudio's Conda environment, install the Spectacular AI Python library with all recommended dependencies: 16 | 17 | pip install spectacularAI[full] 18 | 19 | ## Recording data 20 | 21 | Choose your device below to see more detailed instructions for creating Spectacular AI recordings (folders or zip files): 22 | 23 |
iPhone (with or without LiDAR)

24 | 25 | 1. Download [Spectacular Rec](https://apps.apple.com/us/app/spectacular-rec/id6473188128) from App Store. 26 | 2. See our [instruction video on YouTube](https://youtu.be/d77u-E96VVw) on how to create recording files and transfer them to your computer. 27 | 28 |

29 | 30 |
Android (with or without ToF)

31 | 32 | 1. Download [Spectacular Rec](https://play.google.com/store/apps/details?id=com.spectacularai.rec) from Play Store. 33 | 2. Use like the iPhone version (tutorial here [here](https://youtu.be/d77u-E96VVw)) 34 | 35 |

36 | 37 |
OAK-D

38 | 39 | 1. Plug in the OAK-D to your laptop (or directly the computer with the heavy GPU) 40 | 2. Run `sai-cli record oak --no_feature_tracker --resolution=800p`. 41 | 42 | If the above settings cause issues, try running `sai-cli record oak` instead. 43 | 44 |

45 | 46 |
RealSense D455/D435i

47 | See the Recording data section under the RealSense wrapper instructions 48 | 49 |

50 | 51 |
Azure Kinect DK

52 | 53 | See the Kinect wrapper page for more information 54 | 55 |

56 | 57 |
Orbbec

58 | 59 | See the Recording data section under the Orbbec wrapper instructions 60 | 61 |

62 | 63 | --- 64 | 65 | With OAK-D or RealSense devices, you can currently expect to be able to map "table-sized" scenes 66 | quite fast and accurately. Move slow while mapping and shoot from different angles to increase quality. 67 | 68 | ## Nerfstudio export and training 69 | 70 | First run our conversion script and then Nerstudio training as 71 | 72 | sai-cli process INPUT_PATH --preview3d --key_frame_distance=0.05 /example/output/path/my-nerf 73 | ns-train nerfacto --data /example/output/path/my-nerf 74 | 75 | Where 76 | 77 | * `INPUT_PATH` is the dataset folder recorded using _Spectacular Rec_ or our other recording tools (the value of `recordingFolder` if using the SDK directly) 78 | * `/example/output/path/my-nerf` (placeholder) is the output folder of this script and the input to Nerfstudio 79 | * `--key_frame_distance` should be set based on the recorded scene size: `0.05` (5cm) is good for small scans and `0.15` for room-sized scans. 80 | * `--preview3d` (optional flag) shows you a 3D preview of the point cloud and estimated trajectory (not the final ones). 81 | 82 | If the processing gets slow, you can also try adding a `--fast` flag to `sai-cli process` to trade off quality for speed. 83 | Without the `--fast` flag, the processing should take around 10 minutes tops. 84 | 85 | ### Gaussian Splatting 86 | 87 | Update Nerfstudio and train as 88 | 89 | ns-train gaussian-splatting --data /example/output/path/my-nerf 90 | 91 | To use the resulting "splats" in other tools, first export as PLY 92 | 93 | ns-export gaussian-splat \ 94 | --load-config outputs/my-nerf/gaussian-splatting/DATE/config.yaml 95 | --output-dir exports/splats 96 | 97 | Then copy the the file `exports/point_cloud.ply`. Examples: 98 | 99 | * Edit in [Super Splat](https://playcanvas.com/super-splat) (splat colors may look wrong here) 100 | * Export to `.splat` or [stand-alone HTML](https://spectacularai.github.io/docs/other/android-3dgs-example-ramen.html) 101 | using [SpectacularAI/point-cloud-tools](https://github.com/SpectacularAI/point-cloud-tools#gaussian-splatting) 102 | * View or embed `.splat` to a web page using [gsplat.js](https://github.com/huggingface/gsplat.js) 103 | 104 | The export process can also be customized by modifying the source code of [`sai-cli process`](https://github.com/SpectacularAI/sdk/blob/main/python/cli/process/process.py) 105 | which can also be used as a standalone Python script. 106 | 107 | ## License note 108 | 109 | Spectacular AI SDK is free to use for non-commercial purposes. [Contact us](https://www.spectacularai.com/#contact) for commercial licensing (e.g., running this in your own cloud service). 110 | Nerfstudio and FFMpeg are used under their own licenses. 111 | -------------------------------------------------------------------------------- /python/mapping/record_and_process_oak_d.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -eux 4 | 5 | : "${OUTPUT_FOLDER:=recordings/$(date "+%Y%m%d-%H%M%S")}" 6 | 7 | mkdir -p "$OUTPUT_FOLDER" 8 | 9 | echo "------------- Press Q to stop recording -------------" 10 | python ../oak/mapping_visu.py --keyFrameCandidateInterval=4 --recordingFolder="$OUTPUT_FOLDER" 11 | python replay_to_nerf.py "$OUTPUT_FOLDER" "$OUTPUT_FOLDER"/nerfstudio --device_preset=oak-d --preview 12 | 13 | echo "Now run this Nerfstudio command: 14 | 15 | ns-train nerfacto --data $OUTPUT_FOLDER/nerfstudio 16 | " 17 | -------------------------------------------------------------------------------- /python/mapping/replay_to_instant_ngp.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # Replay existing session and convert output to format used by instant-ngp 4 | # 5 | # Use output with: https://github.com/NVlabs/instant-ngp 6 | 7 | import argparse 8 | import spectacularAI 9 | import cv2 10 | import json 11 | import os 12 | import shutil 13 | import math 14 | import numpy as np 15 | 16 | 17 | parser = argparse.ArgumentParser() 18 | parser.add_argument("input", help="Path to folder with session to process") 19 | parser.add_argument("output", help="Path to output folder") 20 | parser.add_argument("--scale", help="Scene scale, exponent of 2", type=int, default=128) 21 | parser.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") 22 | args = parser.parse_args() 23 | 24 | # Globals 25 | savedKeyFrames = {} 26 | frameWidth = -1 27 | frameHeight = -1 28 | intrinsics = None 29 | 30 | TRANSFORM_CAM = np.array([ 31 | [1,0,0,0], 32 | [0,-1,0,0], 33 | [0,0,-1,0], 34 | [0,0,0,1], 35 | ]) 36 | 37 | TRANSFORM_WORLD = np.array([ 38 | [0,1,0,0], 39 | [-1,0,0,0], 40 | [0,0,1,0], 41 | [0,0,0,1], 42 | ]) 43 | 44 | 45 | def closestPointBetweenTwoLines(oa, da, ob, db): 46 | normal = np.cross(da, db) 47 | denom = np.linalg.norm(normal)**2 48 | t = ob - oa 49 | ta = np.linalg.det([t, db, normal]) / (denom + 1e-10) 50 | tb = np.linalg.det([t, da, normal]) / (denom + 1e-10) 51 | if ta > 0: ta = 0 52 | if tb > 0: tb = 0 53 | return ((oa + ta * da + ob + tb * db) * 0.5, denom) 54 | 55 | 56 | def resizeToUnitCube(frames): 57 | weight = 0.0 58 | centerPos = np.array([0.0, 0.0, 0.0]) 59 | for f in frames: 60 | mf = f["transform_matrix"][0:3,:] 61 | for g in frames: 62 | mg = g["transform_matrix"][0:3,:] 63 | p, w = closestPointBetweenTwoLines(mf[:,3], mf[:,2], mg[:,3], mg[:,2]) 64 | if w > 0.00001: 65 | centerPos += p * w 66 | weight += w 67 | if weight > 0.0: centerPos /= weight 68 | 69 | scale = 0. 70 | for f in frames: 71 | f["transform_matrix"][0:3,3] -= centerPos 72 | scale += np.linalg.norm(f["transform_matrix"][0:3,3]) 73 | 74 | scale = 4.0 / (scale / len(frames)) 75 | for f in frames: f["transform_matrix"][0:3,3] *= scale 76 | 77 | 78 | def sharpness(path): 79 | img = cv2.imread(path) 80 | img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) 81 | return cv2.Laplacian(img, cv2.CV_64F).var() 82 | 83 | 84 | def onMappingOutput(output): 85 | global savedKeyFrames 86 | global frameWidth 87 | global frameHeight 88 | global intrinsics 89 | 90 | if not output.finalMap: 91 | # New frames, let's save the images to disk 92 | for frameId in output.updatedKeyFrames: 93 | keyFrame = output.map.keyFrames.get(frameId) 94 | if not keyFrame or savedKeyFrames.get(frameId): 95 | continue 96 | savedKeyFrames[frameId] = True 97 | frameSet = keyFrame.frameSet 98 | if not frameSet.rgbFrame or not frameSet.rgbFrame.image: 99 | continue 100 | 101 | if frameWidth < 0: 102 | frameWidth = frameSet.rgbFrame.image.getWidth() 103 | frameHeight = frameSet.rgbFrame.image.getHeight() 104 | 105 | undistortedFrame = frameSet.getUndistortedFrame(frameSet.rgbFrame) 106 | if intrinsics is None: intrinsics = undistortedFrame.cameraPose.camera.getIntrinsicMatrix() 107 | img = undistortedFrame.image.toArray() 108 | bgrImage = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) 109 | 110 | fileName = args.output + "/tmp/frame_" + f'{frameId:05}' + ".png" 111 | cv2.imwrite(fileName, bgrImage) 112 | if args.preview: 113 | cv2.imshow("Frame", bgrImage) 114 | cv2.setWindowTitle("Frame", "Frame #{}".format(frameId)) 115 | cv2.waitKey(1) 116 | else: 117 | # Final optimized poses 118 | frames = [] 119 | index = 0 120 | 121 | up = np.zeros(3) 122 | for frameId in output.map.keyFrames: 123 | keyFrame = output.map.keyFrames.get(frameId) 124 | oldImgName = args.output + "/tmp/frame_" + f'{frameId:05}' + ".png" 125 | newImgName = args.output + "/images/frame_" + f'{index:05}' + ".png" 126 | os.rename(oldImgName, newImgName) 127 | cameraPose = keyFrame.frameSet.rgbFrame.cameraPose 128 | 129 | # Converts Spectacular AI camera to coordinate system used by instant-ngp 130 | cameraToWorld = np.matmul(TRANSFORM_WORLD, np.matmul(cameraPose.getCameraToWorldMatrix(), TRANSFORM_CAM)) 131 | up += cameraToWorld[0:3,1] 132 | frame = { 133 | "file_path": "images/frame_" + f'{index:05}' + ".png", 134 | "sharpness": sharpness(newImgName), 135 | "transform_matrix": cameraToWorld 136 | } 137 | frames.append(frame) 138 | index += 1 139 | 140 | resizeToUnitCube(frames) 141 | 142 | for f in frames: f["transform_matrix"] = f["transform_matrix"].tolist() 143 | 144 | if frameWidth < 0 or frameHeight < 0: raise Exception("Unable get image dimensions, zero images received?") 145 | 146 | fl_x = intrinsics[0][0] 147 | fl_y = intrinsics[1][1] 148 | cx = intrinsics[0][2] 149 | cy = intrinsics[1][2] 150 | angle_x = math.atan(frameWidth / (fl_x * 2)) * 2 151 | angle_y = math.atan(frameHeight / (fl_y * 2)) * 2 152 | 153 | transformationsJson = { 154 | "camera_angle_x": angle_x, 155 | "camera_angle_y": angle_y, 156 | "fl_x": fl_x, 157 | "fl_y": fl_y, 158 | "k1": 0.0, 159 | "k2": 0.0, 160 | "p1": 0.0, 161 | "p2": 0.0, 162 | "cx": cx, 163 | "cy": cy, 164 | "w": frameWidth, 165 | "h": frameHeight, 166 | "aabb_scale": args.scale, 167 | "frames": frames 168 | } 169 | 170 | with open(args.output + "/transformations.json", "w") as outFile: 171 | json.dump(transformationsJson, outFile, indent=2) 172 | 173 | 174 | def main(): 175 | os.makedirs(args.output + "/images", exist_ok=True) 176 | os.makedirs(args.output + "/tmp", exist_ok=True) 177 | 178 | print("Processing") 179 | replay = spectacularAI.Replay(args.input, mapperCallback = onMappingOutput, configuration = { 180 | "globalBABeforeSave": True, # Refine final map poses using bundle adjustment 181 | "maxMapSize": 0, # Unlimited map size 182 | "keyframeDecisionDistanceThreshold": 0.1 # Minimum distance between keyframes 183 | }) 184 | 185 | replay.runReplay() 186 | 187 | shutil.rmtree(args.output + "/tmp") 188 | 189 | print("Done!") 190 | print("") 191 | print("You can now run instant-ngp nerfs using following command:") 192 | print("") 193 | print(" ./build/testbed --mode nerf --scene {}/transformations.json".format(args.output)) 194 | 195 | 196 | if __name__ == '__main__': 197 | main() 198 | -------------------------------------------------------------------------------- /python/mapping/replay_to_nerf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | """ 3 | Post-process data in Spectacular AI format and convert it to input 4 | for NeRF or Gaussian Splatting methods. 5 | """ 6 | 7 | DEPRECATION_NOTE = """ 8 | Note: the replay_to_nerf.py script has been replaced by the sai-cli 9 | tool in Spectacular AI Python package v1.25. Prefer 10 | 11 | sai-cli process [args] 12 | 13 | as a drop-in replacement of 14 | 15 | python replay_to_nerf.py [args] 16 | . 17 | """ 18 | 19 | # The code is still available and usable as a stand-alone script, see: 20 | # https://github.com/SpectacularAI/sdk/blob/main/python/cli/process/process.py 21 | 22 | import_success = False 23 | try: 24 | from spectacularAI.cli.process.process import process, define_args 25 | import_success = True 26 | except ImportError as e: 27 | print(e) 28 | 29 | if not import_success: 30 | msg = """ 31 | 32 | Unable to import new Spectacular AI CLI, please update to SDK version >= 1.25" 33 | """ 34 | raise RuntimeError(msg) 35 | 36 | if __name__ == '__main__': 37 | import argparse 38 | parser = argparse.ArgumentParser( 39 | description=__doc__, 40 | epilog=DEPRECATION_NOTE, 41 | formatter_class=argparse.RawDescriptionHelpFormatter) 42 | define_args(parser) 43 | print(DEPRECATION_NOTE) 44 | process(parser.parse_args()) 45 | -------------------------------------------------------------------------------- /python/oak/README.md: -------------------------------------------------------------------------------- 1 | # Spectacular AI Python SDK examples for OAK-D 2 | 3 | ![SDK install demo](https://spectacularai.github.io/docs/gif/spatial-ai.gif) 4 | 5 | **See https://spectacularai.github.io/docs/sdk/wrappers/oak.html for instructions and API documentation** 6 | 7 | ## Examples 8 | 9 | * **Minimal example**. Prints 6-DoF poses as JSON text: [`python vio_jsonl.py`](vio_jsonl.py) 10 | * **Basic visualization**. Interactive 3D plot / draw in the air with the device: [`python vio_visu.py`](vio_visu.py) 11 | * **3D pen**. Draw in the air: cover the OAK-D color camera to activate the ink. [`python pen_3d.py`](pen_3d.py) 12 | * **3D mapping**. Build and visualize 3D point cloud of the environment in real-time. [`mapping_visu.py`](mapping_visu.py) 13 | * **3D mapping with Augmented Reality**. Show 3D mesh or point cloud on top of camera view, using OpenGL. [`mapping_ar.py`](mapping_ar.py) 14 | * **3D Mapping with ROS Integration**. Runs Spectacular AI VIO and publishes pose information and keyframes over ROS topics. [`mapping_ros.py`](mapping_ros.py) 15 | * **Advanced Spatial AI example**. Spectacular AI VIO + Tiny YOLO object detection. 16 | See [`depthai_combination.py`](depthai_combination.py) for additional dependencies that also need to be installed. 17 | * **Mixed reality**. In less than 130 lines of Python, with the good old OpenGL functions like `glTranslatef` used for rendering. 18 | Also requires `PyOpenGL_accelerate` to be installed, see [`mixed_reality.py`](mixed_reality.py) for details. 19 | * **GNSS-VIO** example, reads external GNSS from standard input [`vio_gnss.py`](vio_gnss.py) (see also [these instructions](https://spectacularai.github.io/docs/pdf/GNSS-VIO_OAK-D_Python.pdf)) 20 | * **April Tag example**: Visualize detected April Tags [`python april_tag.py path/to/tags.json`](april_tag.py). See: https://spectacularai.github.io/docs/pdf/april_tag_instructions.pdf 21 | * **Remote visualization over SSH**. Can be achieved by combining the `vio_jsonl.py` and `vio_visu.py` scripts as follows: 22 | 23 | ssh user@example.org 'python -u /full/path/to/vio_jsonl.py' | python -u vio_visu.py --file=- 24 | 25 | Here `user@example.org` represents a machine (e.g., Raspberry Pi) that is connected to the OAK-D, but is not necessarily attached to a monitor. 26 | The above command can then be executed on a laptop/desktop machine, which then shows the trajectory of the OAK-D remotely (like in [this video](https://youtu.be/mBZ8bszNnwI?t=17)). 27 | -------------------------------------------------------------------------------- /python/oak/april_tag.py: -------------------------------------------------------------------------------- 1 | """ 2 | April Tag example. Visualizes April Tag detections in SLAM keyframes, and April Tag poses using augmented reality. 3 | 4 | For April Tag instructions, see: https://github.com/SpectacularAI/docs/blob/main/pdf/april_tag_instructions.pdf 5 | 6 | Requirements: pip install spectacularAI[full] 7 | """ 8 | 9 | import depthai 10 | import spectacularAI 11 | import cv2 12 | import threading 13 | import time 14 | import numpy as np 15 | from mixed_reality import make_pipelines 16 | 17 | from spectacularAI.cli.visualization.visualizer import Visualizer, VisualizerArgs, CameraMode 18 | 19 | from OpenGL.GL import * # all prefixed with gl so OK to import * 20 | 21 | def parseArgs(): 22 | import argparse 23 | p = argparse.ArgumentParser(__doc__) 24 | p.add_argument('aprilTagPath', help="Path to .json file with AprilTag ids, sizes and poses") 25 | p.add_argument("--dataFolder", help="Instead of running live mapping session, replay session from this folder") 26 | p.add_argument("--useRectification", help="This parameter must be set if the videos inputs are not rectified", action="store_true") 27 | p.add_argument('--cameraInd', help="Which camera to use with Replay mode. Typically 0=left, 1=right, 2=auxiliary/RGB (OAK-D default)", type=int, default=2) 28 | return p.parse_args() 29 | 30 | def drawAprilTag(camera, img, tag): 31 | def projectTagPointToPixel(camera, pTag, tagToCamera): 32 | pCamera = tagToCamera[:3, :3] @ pTag + tagToCamera[:3, 3] 33 | pixel = camera.rayToPixel(spectacularAI.Vector3d(pCamera[0], pCamera[1], pCamera[2])) 34 | if pixel is None: return None 35 | return (int(pixel.x), int(pixel.y)) 36 | 37 | def drawEdges(img, p1, p2, p3, p4, color, thickness): 38 | cv2.line(img, p1, p2, color, thickness) 39 | cv2.line(img, p2, p3, color, thickness) 40 | cv2.line(img, p3, p4, color, thickness) 41 | cv2.line(img, p4, p1, color, thickness) 42 | 43 | # Draw detected April Tag with blue 44 | p1 = (int(tag.corners[0].x), int(tag.corners[0].y)) 45 | p2 = (int(tag.corners[1].x), int(tag.corners[1].y)) 46 | p3 = (int(tag.corners[2].x), int(tag.corners[2].y)) 47 | p4 = (int(tag.corners[3].x), int(tag.corners[3].y)) 48 | drawEdges(img, p1, p2, p3, p4, (0, 0, 255), 2) 49 | 50 | # (Currently) pose and size is known only for April Tags defined in tags.json 51 | if not tag.hasPose: return 52 | 53 | # Project April Tag corners points to image using estimated pose 54 | r = tag.size / 2 55 | tagToCamera = np.linalg.inv(tag.pose.asMatrix()) 56 | p1 = projectTagPointToPixel(camera, (-r, r, 0), tagToCamera) 57 | p2 = projectTagPointToPixel(camera, (r, r, 0), tagToCamera) 58 | p3 = projectTagPointToPixel(camera, (r, -r, 0), tagToCamera) 59 | p4 = projectTagPointToPixel(camera, (-r, -r, 0), tagToCamera) 60 | drawEdges(img, p1, p2, p3, p4, (255, 0, 0), 1) 61 | 62 | # Project AR axis using estimated pose 63 | center = projectTagPointToPixel(camera, (0, 0, 0), tagToCamera) 64 | right = projectTagPointToPixel(camera, (r, 0, 0), tagToCamera) 65 | down = projectTagPointToPixel(camera, (0, r, 0), tagToCamera) 66 | into = projectTagPointToPixel(camera, (0, 0, r), tagToCamera) 67 | 68 | cv2.line(img, center, right, (255, 0, 0), 2) 69 | cv2.line(img, center, down, (0, 255, 0), 2) 70 | cv2.line(img, center, into, (0, 0, 255), 2) 71 | 72 | # Draws April Tags detected in the latest key frame 73 | def onMappingOutput(output): 74 | if not output.map.keyFrames: return # empty map 75 | 76 | # Find latest keyframe 77 | keyFrameId = max(output.map.keyFrames.keys()) 78 | keyFrame = output.map.keyFrames[keyFrameId] 79 | frameSet = keyFrame.frameSet 80 | 81 | def drawAprilTagsInFrame(frame): 82 | if frame is None or frame.image is None: return None 83 | img = cv2.cvtColor(frame.image.toArray(), cv2.COLOR_RGB2BGR) 84 | for marker in frame.visualMarkers: 85 | drawAprilTag(frame.cameraPose.camera, img, marker) 86 | return img 87 | 88 | primary = drawAprilTagsInFrame(frameSet.primaryFrame) 89 | secondary = drawAprilTagsInFrame(frameSet.secondaryFrame) 90 | 91 | if primary is None and secondary is None: return 92 | elif primary is None: img = secondary 93 | elif secondary is None: img = primary 94 | else: img = cv2.hconcat([primary, secondary]) 95 | 96 | cv2.imshow("April Tags", cv2.cvtColor(img, cv2.COLOR_RGB2BGR)) 97 | cv2.setWindowTitle("April Tags", "April Tags detected in key frame #{}".format(keyFrameId)) 98 | cv2.waitKey(1) 99 | 100 | def drawAxis(localToWorld): 101 | AXIS_LINES = ( 102 | (0, 0, 0), (1, 0, 0), # X axis 103 | (0, 0, 0), (0, 1, 0), # Y axis 104 | (0, 0, 0), (0, 0, 1) # Z axis 105 | ) 106 | 107 | glPushMatrix() 108 | glMultMatrixf(localToWorld.transpose()) 109 | glScalef(*([0.1] * 3)) 110 | 111 | glBegin(GL_LINES) 112 | glColor3f(1, 0, 0) 113 | glVertex3fv(AXIS_LINES[0]) 114 | glVertex3fv(AXIS_LINES[1]) 115 | 116 | glColor3f(0, 1, 0) 117 | glVertex3fv(AXIS_LINES[2]) 118 | glVertex3fv(AXIS_LINES[3]) 119 | 120 | glColor3f(0, 0, 1) 121 | glVertex3fv(AXIS_LINES[4]) 122 | glVertex3fv(AXIS_LINES[5]) 123 | glEnd() 124 | 125 | glPopMatrix() 126 | 127 | def loadAprilTagObjs(aprilTagPath): 128 | def readAprilTagPoses(aprilTagPath): 129 | import json 130 | poses = [] 131 | with open(aprilTagPath) as f: 132 | tags = json.load(f) 133 | for tag in tags: 134 | tagToWorld = np.array(tag['tagToWorld']) 135 | poses.append(tagToWorld) 136 | return poses 137 | 138 | poses = readAprilTagPoses(aprilTagPath) 139 | glList = glGenLists(1) 140 | glNewList(glList, GL_COMPILE) 141 | for pose in poses: 142 | drawAxis(pose) 143 | glEndList() 144 | return glList 145 | 146 | def replayOnVioOutput(output, frameSet): 147 | for frame in frameSet: 148 | if not frame.image: continue 149 | if not frame.index == args.cameraInd: continue 150 | img = frame.image.toArray() 151 | width = img.shape[1] 152 | height = img.shape[0] 153 | colorFormat = frame.image.getColorFormat() 154 | cameraPose = frame.cameraPose 155 | visualizer.onVioOutput(cameraPose, img, width, height, colorFormat, output.status) 156 | 157 | if __name__ == '__main__': 158 | args = parseArgs() 159 | 160 | obj = None 161 | def renderObj(): 162 | global obj 163 | if obj is None: 164 | obj = loadAprilTagObjs(args.aprilTagPath) 165 | 166 | glColor3f(1, 0, 1) 167 | glLineWidth(2.0) 168 | glCallList(obj) 169 | 170 | visArgs = VisualizerArgs() 171 | visArgs.cameraMode = CameraMode.AR 172 | visArgs.showPoseTrail = False 173 | visArgs.showKeyFrames = False 174 | visArgs.showGrid = False 175 | visArgs.customRenderCallback = renderObj 176 | visualizer = Visualizer(visArgs) 177 | 178 | if args.dataFolder: 179 | configInternal = { 180 | "aprilTagPath": args.aprilTagPath, 181 | "extendParameterSets" : ["april-tags"] 182 | } 183 | if args.useRectification: 184 | configInternal["useRectification"] = "true" # Undistort images for visualization (assumes undistorted pinhole model) 185 | 186 | replay = spectacularAI.Replay(args.dataFolder, onMappingOutput, configuration=configInternal) 187 | replay.setExtendedOutputCallback(replayOnVioOutput) 188 | replay.startReplay() 189 | visualizer.run() 190 | replay.close() 191 | else: 192 | def captureLoop(): 193 | config = spectacularAI.depthai.Configuration() 194 | config.aprilTagPath = args.aprilTagPath 195 | 196 | pipeline, vioPipeline = make_pipelines(config, onMappingOutput) 197 | with depthai.Device(pipeline) as device, \ 198 | vioPipeline.startSession(device) as vioSession: 199 | 200 | # buffer for frames: show together with the corresponding VIO output 201 | frames = {} 202 | frameNumber = 1 203 | imgQueue = device.getOutputQueue(name="cam_out", maxSize=4, blocking=False) 204 | 205 | while not visualizer.shouldQuit: 206 | if imgQueue.has(): 207 | img = imgQueue.get() 208 | imgTime = img.getTimestampDevice().total_seconds() 209 | frames[frameNumber] = img 210 | vioSession.addTrigger(imgTime, frameNumber) 211 | frameNumber += 1 212 | 213 | elif vioSession.hasOutput(): 214 | output = vioSession.getOutput() 215 | 216 | if output.tag > 0: 217 | import numpy as np 218 | img = frames.get(output.tag) 219 | data = np.flipud(img.getRaw().data) 220 | cameraPose = vioSession.getRgbCameraPose(output) 221 | visualizer.onVioOutput(cameraPose, data, img.getWidth(), img.getHeight(), spectacularAI.ColorFormat.RGB, output.status) 222 | # discard old tags 223 | frames = { tag: v for tag, v in frames.items() if tag > output.tag } 224 | time.sleep(0.01) 225 | 226 | thread = threading.Thread(target=captureLoop) 227 | thread.start() 228 | visualizer.run() 229 | thread.join() 230 | -------------------------------------------------------------------------------- /python/oak/depthai_combination.py: -------------------------------------------------------------------------------- 1 | """ 2 | Spatial AI demo combining Spectacular AI VIO with Tiny YOLO object detection 3 | accelerated on the OAK-D. 4 | 5 | Requirements: 6 | 7 | pip install opencv-python matplotlib 8 | 9 | To download the pre-trained NN model run following shell script (Git Bash recommended on Windows to run it): 10 | 11 | ./depthai_combination_install.sh 12 | 13 | Plug in the OAK-D and run: 14 | 15 | python examples/depthai_combination.py 16 | 17 | """ 18 | import depthai as dai 19 | import time 20 | import cv2 21 | import matplotlib.pyplot as plt 22 | import spectacularAI 23 | import threading 24 | from pathlib import Path 25 | import sys 26 | import numpy as np 27 | 28 | def make_pipelines(nnBlobPath, showRgb): 29 | syncNN = True 30 | 31 | # Create pipeline 32 | pipeline = dai.Pipeline() 33 | vio_pipeline = spectacularAI.depthai.Pipeline(pipeline) 34 | 35 | # Define sources and outputs 36 | camRgb = pipeline.createColorCamera() 37 | spatialDetectionNetwork = pipeline.createYoloSpatialDetectionNetwork() 38 | 39 | if showRgb: 40 | xoutRgb = pipeline.createXLinkOut() 41 | xoutNN = pipeline.createXLinkOut() 42 | xoutBoundingBoxDepthMapping = pipeline.createXLinkOut() 43 | 44 | if showRgb: 45 | xoutRgb.setStreamName("rgb") 46 | xoutNN.setStreamName("detections") 47 | xoutBoundingBoxDepthMapping.setStreamName("boundingBoxDepthMapping") 48 | 49 | # Properties 50 | camRgb.setPreviewSize(416, 416) 51 | camRgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P) 52 | camRgb.setInterleaved(False) 53 | camRgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR) 54 | 55 | spatialDetectionNetwork.setBlobPath(nnBlobPath) 56 | spatialDetectionNetwork.setConfidenceThreshold(0.5) 57 | spatialDetectionNetwork.input.setBlocking(False) 58 | spatialDetectionNetwork.setBoundingBoxScaleFactor(0.5) 59 | spatialDetectionNetwork.setDepthLowerThreshold(100) 60 | spatialDetectionNetwork.setDepthUpperThreshold(5000) 61 | 62 | # Yolo specific parameters 63 | spatialDetectionNetwork.setNumClasses(80) 64 | spatialDetectionNetwork.setCoordinateSize(4) 65 | spatialDetectionNetwork.setAnchors(np.array([10,14, 23,27, 37,58, 81,82, 135,169, 344,319])) 66 | spatialDetectionNetwork.setAnchorMasks({ "side26": np.array([1,2,3]), "side13": np.array([3,4,5]) }) 67 | spatialDetectionNetwork.setIouThreshold(0.5) 68 | 69 | camRgb.preview.link(spatialDetectionNetwork.input) 70 | if showRgb: 71 | if syncNN: 72 | spatialDetectionNetwork.passthrough.link(xoutRgb.input) 73 | else: 74 | camRgb.preview.link(xoutRgb.input) 75 | 76 | spatialDetectionNetwork.out.link(xoutNN.input) 77 | spatialDetectionNetwork.boundingBoxMapping.link(xoutBoundingBoxDepthMapping.input) 78 | 79 | vio_pipeline.stereo.depth.link(spatialDetectionNetwork.inputDepth) 80 | 81 | return pipeline, vio_pipeline 82 | 83 | def make_tracker(): 84 | """ 85 | Simple tracker/smoother/clustring for the YOLO-detected objects. 86 | (The raw YOLO results look quite, well, raw, especially in 3D) 87 | """ 88 | tracked_objects = [] 89 | next_id = 1 90 | 91 | class TrackedObject: 92 | def __init__(self, t, p, l): 93 | self.position = p 94 | self.label = l 95 | self.last_seen = t 96 | self.n_detections = 1 97 | 98 | nonlocal next_id 99 | self.id = next_id 100 | next_id += 1 101 | 102 | def update(self, other): 103 | UPDATE_ALPHA = 0.2 104 | self.last_seen = other.last_seen 105 | self.position = UPDATE_ALPHA * other.position + (1.0 - UPDATE_ALPHA) * self.position 106 | self.n_detections += 1 107 | 108 | def __repr__(self): 109 | return '%s %d' % (self.label, self.id) 110 | 111 | CLUSTERING_DISTANCE_AT_1M = 0.3 112 | 113 | def find_best_match(new_obj, w_to_c_mat): 114 | best = None 115 | best_dist = CLUSTERING_DISTANCE_AT_1M 116 | MIN_DEPTH = 0.5 117 | 118 | local_pos = lambda p: (w_to_c_mat @ np.array(list(p) + [1]))[:3] 119 | 120 | for old in tracked_objects: 121 | if old.label != new_obj.label: continue 122 | 123 | # ignore depth difference in clustering 124 | loc_old = local_pos(old.position) 125 | loc_new = local_pos(new_obj.position) 126 | z = max([MIN_DEPTH, loc_old[2], loc_new[2]]) 127 | dist = np.linalg.norm((loc_old - loc_new)[:2]) / z 128 | 129 | if dist < best_dist: 130 | best_dist = dist 131 | best = old 132 | # if best: print(f'matched with {best} (seen {best.n_detections} time(s))') 133 | return best 134 | 135 | def track(t, detections, view_mat): 136 | SCALE = 0.001 # output is millimeters 137 | MIN_DETECTIONS = 8 138 | DETECTION_WINDOW = 1.0 139 | MAX_UNSEEN_AGE = 8.0 140 | 141 | w_to_c_mat = np.linalg.inv(view_mat) 142 | 143 | for d in detections: 144 | p_local = np.array([ 145 | d.spatialCoordinates.x * SCALE, 146 | -d.spatialCoordinates.y * SCALE, # note: flipped y 147 | d.spatialCoordinates.z * SCALE, 148 | 1 149 | ]) 150 | p_world = (view_mat @ p_local)[:3] 151 | try: 152 | label = LABEL_MAP[d.label] 153 | except: 154 | label = d.label 155 | 156 | # simple O(n^2) 157 | for o in tracked_objects: 158 | if o.label != label: continue 159 | dist = np.linalg.norm(o.position - p_world) 160 | 161 | if label in SELECTED_LABELS: 162 | new_obj = TrackedObject(t, p_world, label) 163 | existing = find_best_match(new_obj, w_to_c_mat) 164 | if existing: 165 | existing.update(new_obj) 166 | else: 167 | tracked_objects.append(new_obj) 168 | 169 | def should_remove(o): 170 | if o.n_detections < MIN_DETECTIONS and o.last_seen < t - DETECTION_WINDOW: return True 171 | if o.last_seen < t - MAX_UNSEEN_AGE: return True 172 | return False 173 | 174 | # remove cruft 175 | i = 0 176 | while i < len(tracked_objects): 177 | if should_remove(tracked_objects[i]): 178 | # print(f'removing ${o}') 179 | del tracked_objects[i] 180 | else: 181 | i += 1 182 | 183 | # print(tracked_objects) 184 | return [o for o in tracked_objects if o.n_detections >= MIN_DETECTIONS] 185 | 186 | return track 187 | 188 | # Tiny yolo v3/4 label texts 189 | LABEL_MAP = [ 190 | "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", 191 | "truck", "boat", "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", 192 | "bird", "cat", "dog", "horse", "sheep", "cow", "elephant", 193 | "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", "tie", 194 | "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", 195 | "baseball glove", "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", 196 | "fork", "knife", "spoon", "bowl", "banana", "apple", "sandwich", 197 | "orange", "broccoli", "carrot", "hot dog", "pizza", "donut", "cake", 198 | "chair", "sofa", "pottedplant", "bed", "diningtable", "toilet", "tvmonitor", 199 | "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", 200 | "toaster", "sink", "refrigerator", "book", "clock", "vase", "scissors", 201 | "teddy bear", "hair drier", "toothbrush" 202 | ] 203 | 204 | SELECTED_LABELS = ['mouse', 'cup', 'dog'] 205 | 206 | def make_camera_wireframe(aspect=640/400., scale=0.05): 207 | # camera "frustum" 208 | corners = [[-1, -1], [1, -1], [1, 1], [-1, 1], [-1, -1]] 209 | cam_wire = [] 210 | for x, y in corners: 211 | cam_wire.append([x*aspect, y, 1]) 212 | for x, y in corners: 213 | cam_wire.append([x*aspect, y, 1]) 214 | cam_wire.append([0, 0, 0]) 215 | return (scale * np.array(cam_wire)).tolist() 216 | 217 | class MatplotlibVisualization: 218 | """ 219 | Interactive / real-time 3D line & point visualization using Matplotlib. 220 | This is quite far from the comfort zone of MPL and not very extensible. 221 | """ 222 | def __init__(self): 223 | from mpl_toolkits.mplot3d import Axes3D 224 | from matplotlib.animation import FuncAnimation 225 | 226 | fig = plt.figure() 227 | ax = Axes3D(fig, auto_add_to_figure=False) 228 | fig.add_axes(ax) 229 | 230 | ax_bounds = (-0.5, 0.5) # meters 231 | ax.set(xlim=ax_bounds, ylim=ax_bounds, zlim=ax_bounds) 232 | ax.view_init(azim=-140) # initial plot orientation 233 | 234 | empty_xyz = lambda: { c: [] for c in 'xyz' } 235 | 236 | vio_data = empty_xyz() 237 | vio_data['plot'] = ax.plot( 238 | xs=[], ys=[], zs=[], 239 | linestyle="-", 240 | marker="", 241 | label='VIO trajectory' 242 | ) 243 | 244 | vio_cam_data = empty_xyz() 245 | vio_cam_data['plot'] = ax.plot( 246 | xs=[], ys=[], zs=[], 247 | linestyle="-", 248 | marker="", 249 | label='current cam pose' 250 | ) 251 | 252 | detection_data = empty_xyz() 253 | detection_data['labels'] = [] 254 | detection_data['plot'] = ax.plot( 255 | xs=[], ys=[], zs=[], 256 | linestyle="", 257 | marker="o", 258 | label=' or '.join(SELECTED_LABELS) 259 | ) 260 | 261 | ax.legend() 262 | ax.set_xlabel("x (m)") 263 | ax.set_ylabel("y (m)") 264 | ax.set_zlabel("z (m)") 265 | 266 | #title = ax.set_title("Spatial AI demo") 267 | def on_close(*args): 268 | self.should_close = True 269 | 270 | fig.canvas.mpl_connect('close_event', on_close) 271 | 272 | self.cam_wire = make_camera_wireframe() 273 | self.vio_data = vio_data 274 | self.vio_cam_data = vio_cam_data 275 | self.detection_data = detection_data 276 | self.should_close = False 277 | 278 | def update_graph(*args): 279 | r = [] 280 | for graph in [self.vio_data, self.vio_cam_data, self.detection_data]: 281 | p = graph['plot'][0] 282 | x, y, z = [np.array(graph[c]) for c in 'xyz'] 283 | p.set_data(x, y) 284 | p.set_3d_properties(z) 285 | r.append(p) 286 | return tuple(r) 287 | 288 | self._anim = FuncAnimation(fig, update_graph, interval=15, blit=True) 289 | 290 | def update_vio(self, vio_out): 291 | if self.should_close: return False 292 | view_mat = vio_out.pose.asMatrix() 293 | 294 | for c in 'xyz': self.vio_cam_data[c] = [] 295 | for vertex in self.cam_wire: 296 | p_local = np.array(vertex + [1]) 297 | p_world = (view_mat @ p_local)[:3] 298 | for i, c in enumerate('xyz'): 299 | self.vio_cam_data[c].append(p_world[i]) 300 | 301 | for c in 'xyz': 302 | self.vio_data[c].append(getattr(vio_out.pose.position, c)) 303 | 304 | return True 305 | 306 | def update_detected_objects(self, tracked_objects): 307 | if self.should_close: return False 308 | 309 | for i in range(3): 310 | self.detection_data['xyz'[i]] = np.array([o.position[i] for o in tracked_objects]) 311 | self.detection_data['labels'] = [o.label for o in tracked_objects] 312 | 313 | return True 314 | 315 | def start_in_parallel_with(self, parallel_thing): 316 | thread = threading.Thread(target = parallel_thing) 317 | thread.start() 318 | plt.show() 319 | thread.join() 320 | 321 | def draw_detections_on_rgb_frame(frame, detections, fps): 322 | # If the frame is available, draw bounding boxes on it and show the frame 323 | height = frame.shape[0] 324 | width = frame.shape[1] 325 | for detection in detections: 326 | # Denormalize bounding box 327 | x1 = int(detection.xmin * width) 328 | x2 = int(detection.xmax * width) 329 | y1 = int(detection.ymin * height) 330 | y2 = int(detection.ymax * height) 331 | try: 332 | label = LABEL_MAP[detection.label] 333 | except: 334 | label = detection.label 335 | if label in SELECTED_LABELS: 336 | color = (0, 255, 0) 337 | cv2.putText(frame, str(label), (x1 + 10, y1 + 20), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) 338 | cv2.putText(frame, "{:.2f}".format(detection.confidence*100), (x1 + 10, y1 + 35), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) 339 | cv2.putText(frame, f"X: {int(detection.spatialCoordinates.x)} mm", (x1 + 10, y1 + 50), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) 340 | cv2.putText(frame, f"Y: {int(detection.spatialCoordinates.y)} mm", (x1 + 10, y1 + 65), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) 341 | cv2.putText(frame, f"Z: {int(detection.spatialCoordinates.z)} mm", (x1 + 10, y1 + 80), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255) 342 | else: 343 | color = (255, 0, 0) 344 | 345 | cv2.rectangle(frame, (x1, y1), (x2, y2), color, cv2.FONT_HERSHEY_SIMPLEX) 346 | 347 | color = (255, 255, 255) 348 | cv2.putText(frame, "NN fps: {:.2f}".format(fps), (2, frame.shape[0] - 4), cv2.FONT_HERSHEY_TRIPLEX, 0.4, color) 349 | 350 | if __name__ == '__main__': 351 | nnBlobPath = 'models/yolo-v4-tiny-tf_openvino_2021.4_6shave.blob' 352 | if len(sys.argv) > 1: 353 | nnBlobPath = sys.argv[1] 354 | 355 | if not Path(nnBlobPath).exists(): 356 | raise FileNotFoundError(f'Could not find {nnBlobPath}"') 357 | 358 | showRgb = True 359 | pipeline, vio_pipeline = make_pipelines(nnBlobPath, showRgb) 360 | 361 | with dai.Device(pipeline) as device: 362 | visu_3d = MatplotlibVisualization() 363 | 364 | def main_loop(): 365 | startTime = time.monotonic() 366 | counter = 0 367 | fps = 0 368 | color = (255, 255, 255) 369 | 370 | vio_session = vio_pipeline.startSession(device) 371 | tracker = make_tracker() 372 | 373 | if showRgb: previewQueue = device.getOutputQueue(name="rgb", maxSize=4, blocking=False) 374 | detectionNNQueue = device.getOutputQueue(name="detections", maxSize=4, blocking=False) 375 | xoutBoundingBoxDepthMappingQueue = device.getOutputQueue(name="boundingBoxDepthMapping", maxSize=4, blocking=False) 376 | 377 | vio_matrix = None 378 | 379 | while True: 380 | if vio_session.hasOutput(): 381 | vio_out = vio_session.getOutput() 382 | vio_matrix = vio_out.pose.asMatrix() 383 | if not visu_3d.update_vio(vio_out): break 384 | elif detectionNNQueue.has(): 385 | if showRgb: 386 | inPreview = previewQueue.get() 387 | frame = inPreview.getCvFrame() 388 | 389 | inDet = detectionNNQueue.get() 390 | 391 | # TODO: depth hook 392 | #depthFrame = depth.getFrame() 393 | #depthFrameColor = cv2.normalize(depthFrame, None, 255, 0, cv2.NORM_INF, cv2.CV_8UC1) 394 | #depthFrameColor = cv2.equalizeHist(depthFrameColor) 395 | #depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_HOT) 396 | 397 | counter+=1 398 | current_time = time.monotonic() 399 | if (current_time - startTime) > 1 : 400 | fps = counter / (current_time - startTime) 401 | counter = 0 402 | startTime = current_time 403 | 404 | detections = inDet.detections 405 | if len(detections) != 0: 406 | boundingBoxMapping = xoutBoundingBoxDepthMappingQueue.get() 407 | roiDatas = boundingBoxMapping.getConfigData() 408 | 409 | if vio_matrix is not None: 410 | detections_world = tracker(current_time, detections, vio_matrix) 411 | visu_3d.update_detected_objects(detections_world) 412 | 413 | if showRgb: 414 | draw_detections_on_rgb_frame(frame, detections, fps) 415 | cv2.imshow("rgb", frame) 416 | 417 | if cv2.waitKey(1) == ord('q'): 418 | break 419 | else: 420 | time.sleep(0.005) 421 | 422 | vio_session.close() 423 | 424 | visu_3d.start_in_parallel_with(main_loop) 425 | -------------------------------------------------------------------------------- /python/oak/depthai_combination_install.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -ex 3 | 4 | git clone https://github.com/luxonis/depthai-python 5 | cd depthai-python/examples 6 | python install_requirements.py -sdai 7 | cp -R models ../.. 8 | cd ../.. 9 | -------------------------------------------------------------------------------- /python/oak/mapping.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import spectacularAI 3 | import cv2 4 | import json 5 | import os 6 | 7 | p = argparse.ArgumentParser(__doc__) 8 | p.add_argument("dataFolder", help="Folder containing the recorded session for mapping", default="data") 9 | p.add_argument("outputFolder", help="Output folder for key frame images and their poses ", default="output") 10 | p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") 11 | args = p.parse_args() 12 | 13 | # KeyFrames for which we've already saved the image 14 | savedKeyFrames = {} 15 | 16 | def saveAsPng(outputFolder, frameId, cameraName, frame): 17 | if not frame or not frame.image: return 18 | fileName = outputFolder + "/" + cameraName + "_" + f'{frameId:05}' + ".png" 19 | cv2.imwrite(fileName, cv2.cvtColor(frame.image.toArray(), cv2.COLOR_RGB2BGR)) 20 | 21 | 22 | def onMappingOutput(output): 23 | if output.finalMap: 24 | # Final optimized poses, let's save them to jsonl file 25 | with open(args.outputFolder + "/poses.jsonl", "w") as outFile: 26 | for frameId in output.map.keyFrames: 27 | keyFrame = output.map.keyFrames.get(frameId) 28 | outputJson = { 29 | "frameId": frameId, 30 | "poses": {} 31 | } 32 | frameSet = keyFrame.frameSet 33 | if frameSet.primaryFrame: outputJson["poses"]["primary"] = frameSet.primaryFrame.cameraPose.getCameraToWorldMatrix().tolist() 34 | if frameSet.secondaryFrame: outputJson["poses"]["secondary"] = frameSet.secondaryFrame.cameraPose.getCameraToWorldMatrix().tolist() 35 | if frameSet.rgbFrame: outputJson["poses"]["rgb"] = frameSet.rgbFrame.cameraPose.getCameraToWorldMatrix().tolist() 36 | if frameSet.depthFrame: outputJson["poses"]["depth"] = frameSet.depthFrame.cameraPose.getCameraToWorldMatrix().tolist() 37 | outFile.write(json.dumps(outputJson) + "\n") 38 | else: 39 | # New frames, let's save the images to disk 40 | for frameId in output.updatedKeyFrames: 41 | keyFrame = output.map.keyFrames.get(frameId) 42 | if not keyFrame or savedKeyFrames.get(frameId): continue 43 | savedKeyFrames[frameId] = True 44 | frameSet = keyFrame.frameSet 45 | saveAsPng(args.outputFolder, frameId, "primary", frameSet.primaryFrame) 46 | saveAsPng(args.outputFolder, frameId, "secondary", frameSet.secondaryFrame) 47 | saveAsPng(args.outputFolder, frameId, "rgb", frameSet.rgbFrame) 48 | saveAsPng(args.outputFolder, frameId, "depth", frameSet.depthFrame) 49 | if args.preview and frameSet.primaryFrame.image: 50 | cv2.imshow("Primary camera", cv2.cvtColor(frameSet.primaryFrame.image.toArray(), cv2.COLOR_RGB2BGR)) 51 | cv2.setWindowTitle("Primary camera", "Primary camera #{}".format(frameId)) 52 | cv2.waitKey(1) 53 | 54 | 55 | os.makedirs(args.outputFolder) 56 | replay = spectacularAI.Replay(args.dataFolder, onMappingOutput) 57 | replay.runReplay() 58 | -------------------------------------------------------------------------------- /python/oak/mapping_ar.py: -------------------------------------------------------------------------------- 1 | """ 2 | Draw Mapping API outputs on video to create an AR visualization. 3 | 4 | Requirements: 5 | pip install pygame PyOpenGL 6 | And optionally to improve performance: 7 | pip install PyOpenGL_accelerate 8 | 9 | For OAK-D live use: 10 | * Plug in OAK-D and run `python3 mapping_ar.py`. 11 | 12 | For OAK-D replays: 13 | * Plug in OAK-D. 14 | * `python vio_record.py --slam --output recording` 15 | * `python mapping_ar.py --dataFolder recording` 16 | 17 | For non-OAK-D replays, you may need to set more options, for example: 18 | * `python mapping_ar.py --dataFolder recording --useRectification --cameraInd 0` 19 | """ 20 | 21 | import os 22 | import subprocess 23 | import time 24 | 25 | import numpy as np 26 | 27 | from OpenGL.GL import * # all prefixed with gl so OK to import * 28 | 29 | os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = "hide" 30 | import pygame 31 | 32 | import spectacularAI 33 | import depthai 34 | 35 | from mixed_reality import init_display, make_pipelines 36 | from mapping_ar_renderers.mesh import MeshRenderer 37 | from mapping_ar_renderers.point_cloud import PointCloudRenderer 38 | from mapping_ar_renderers.util import loadObjToMesh 39 | 40 | class State: 41 | args = None 42 | shouldQuit = False 43 | recordPipe = None 44 | currentMapperOutput = None 45 | lastMapperOutput = None 46 | displayInitialized = False 47 | targetResolution = None 48 | adjustedResolution = None 49 | scale = None 50 | # Must be initialized after pygame. 51 | meshRenderer = None 52 | pointCloudRenderer = None 53 | mesh = None 54 | 55 | def updateRenderer(state, cameraPose, t): 56 | if state.args.pointCloud: 57 | if not state.pointCloudRenderer: return 58 | if not state.currentMapperOutput: return 59 | if state.currentMapperOutput is not state.lastMapperOutput: 60 | state.pointCloudRenderer.setPointCloud(state.currentMapperOutput, t) 61 | state.pointCloudRenderer.setPose(cameraPose, t) 62 | state.pointCloudRenderer.render() 63 | else: 64 | def renderMesh(): 65 | state.meshRenderer.setPose(cameraPose) 66 | state.meshRenderer.render() 67 | 68 | def updateMesh(): 69 | state.meshRenderer.setMesh(state.currentMapperOutput.mesh) 70 | 71 | if not state.meshRenderer: return 72 | if not state.currentMapperOutput: return 73 | if state.mesh: return renderMesh() 74 | 75 | if not state.lastMapperOutput: updateMesh() 76 | elif state.currentMapperOutput.mesh is not state.lastMapperOutput.mesh: updateMesh() 77 | return renderMesh() 78 | 79 | def handleVioOutput(state, cameraPose, t, img, width, height, colorFormat): 80 | if state.shouldQuit: 81 | return 82 | 83 | if not state.displayInitialized: 84 | state.displayInitialized = True 85 | targetWidth = state.targetResolution[0] 86 | targetHeight = state.targetResolution[1] 87 | state.scale = min(targetWidth / width, targetHeight / height) 88 | state.adjustedResolution = [int(state.scale * width), int(state.scale * height)] 89 | init_display(state.adjustedResolution[0], state.adjustedResolution[1]) 90 | state.meshRenderer = MeshRenderer() 91 | state.pointCloudRenderer = PointCloudRenderer(state.args.pointCloudDensity) 92 | if state.mesh: 93 | state.meshRenderer.setMesh(state.mesh) 94 | 95 | for event in pygame.event.get(): 96 | if event.type == pygame.QUIT: 97 | state.shouldQuit = True 98 | if event.type == pygame.KEYDOWN: 99 | if event.key == pygame.K_q: state.shouldQuit = True 100 | if event.key == pygame.K_x: state.args.pointCloud = not state.args.pointCloud 101 | if event.key == pygame.K_m: 102 | if state.meshRenderer: state.meshRenderer.nextMode() 103 | if state.pointCloudRenderer: state.pointCloudRenderer.nextMode() 104 | if state.shouldQuit: 105 | pygame.quit() 106 | return 107 | 108 | glPixelZoom(state.scale, state.scale) 109 | glDrawPixels( 110 | width, 111 | height, 112 | GL_LUMINANCE if colorFormat == spectacularAI.ColorFormat.GRAY else GL_RGB, GL_UNSIGNED_BYTE, 113 | np.frombuffer(img. data, dtype=np.uint8)) 114 | 115 | updateRenderer(state, cameraPose, t) 116 | 117 | if state.currentMapperOutput: 118 | state.lastMapperOutput = state.currentMapperOutput 119 | 120 | if state.args.recordPath: 121 | try: os.makedirs(os.path.dirname(state.args.recordPath)) 122 | except: pass 123 | 124 | if os.name == 'nt': 125 | ffmpegStdErrToNull = "2>NUL" 126 | else: 127 | ffmpegStdErrToNull = "2>/dev/null" 128 | 129 | r = state.adjustedResolution 130 | if state.recordPipe is None: 131 | cmd = "ffmpeg -y -f rawvideo -vcodec rawvideo -pix_fmt rgb24 -s {}x{} -i - -an -pix_fmt yuv420p -c:v libx264 -vf vflip -crf 17 \"{}\" {}".format( 132 | r[0], r[1], state.args.recordPath, ffmpegStdErrToNull) 133 | state.recordPipe = subprocess.Popen( 134 | cmd, stdin=subprocess.PIPE, shell=True) 135 | buffer = glReadPixels(0, 0, r[0], r[1], GL_RGB, GL_UNSIGNED_BYTE) 136 | state.recordPipe.stdin.write(buffer) 137 | 138 | pygame.display.flip() 139 | 140 | def oakdLoop(state, device, vioSession): 141 | img_queue = device.getOutputQueue(name="cam_out", maxSize=4, blocking=False) 142 | # Buffer for frames: show together with the corresponding VIO output 143 | frames = {} 144 | frame_number = 1 145 | 146 | while True: 147 | if img_queue.has(): 148 | img = img_queue.get() 149 | img_time = img.getTimestampDevice().total_seconds() 150 | frames[frame_number] = img 151 | vioSession.addTrigger(img_time, frame_number) 152 | frame_number += 1 153 | elif vioSession.hasOutput(): 154 | vioOutput = vioSession.getOutput() 155 | if vioOutput.tag > 0: 156 | img = frames.get(vioOutput.tag) 157 | cameraPose = vioSession.getRgbCameraPose(vioOutput) 158 | time = vioOutput.pose.time 159 | handleVioOutput(state, cameraPose, time, img.getRaw().data, img.getWidth(), img.getHeight(), spectacularAI.ColorFormat.RGB) 160 | # Discard old tags. 161 | frames = { tag: v for tag, v in frames.items() if tag > vioOutput.tag } 162 | else: 163 | pygame.time.wait(1) 164 | if state.shouldQuit: 165 | return 166 | 167 | def main(args): 168 | print("Control using the keyboard:") 169 | print("* Q: Quit") 170 | print("* X: Change between mesh and point cloud visualizations") 171 | print("* M: Cycle through visualization options.") 172 | print("------\n") 173 | 174 | state = State() 175 | state.args = args 176 | state.targetResolution = [int(s) for s in args.resolution.split("x")] 177 | if args.objLoadPath: 178 | position = [float(s) for s in args.objPosition.split(",")] 179 | state.mesh = loadObjToMesh(args.objLoadPath, position) 180 | 181 | configInternal = { 182 | "stereoPointCloudMaxDepth": str(args.depth), 183 | "recMaxDistance": str(args.depth) 184 | } 185 | 186 | if args.mapLoadPath: 187 | onMappingOutput = None # Appending to existing map is not currently supported. 188 | else: 189 | def onMappingOutput(mapperOutput): 190 | nonlocal state 191 | state.currentMapperOutput = mapperOutput 192 | if mapperOutput.finalMap: 193 | # TODO: call pygame.quit() BUT it has to be called from same thread as init_display(...) 194 | state.shouldQuit = True 195 | 196 | configInternal["computeStereoPointCloud"] = "true" 197 | configInternal["pointCloudNormalsEnabled"] = "true" 198 | configInternal["computeDenseStereoDepth"] = "true" 199 | configInternal["computeDenseStereoDepthKeyFramesOnly"] = "true" 200 | configInternal["recEnabled"] = "true" 201 | configInternal["recCellSize"] = "0.02" 202 | configInternal["useSlam"] = "true" 203 | 204 | def replayOnVioOutput(vioOutput, frameSet): 205 | nonlocal state 206 | for frame in frameSet: 207 | if not frame.image: continue 208 | if not frame.index == args.cameraInd: continue 209 | img = frame.image.toArray() 210 | img = np.ascontiguousarray(np.flipud(img)) # Flip the image upside down for OpenGL. 211 | width = img.shape[1] 212 | height = img.shape[0] 213 | time = vioOutput.pose.time 214 | handleVioOutput(state, frame.cameraPose, time, img, width, height, frame.image.getColorFormat()) 215 | 216 | if args.dataFolder: 217 | if args.useRectification: configInternal["useRectification"] = "true" 218 | if args.mapLoadPath: 219 | configInternal["mapLoadPath"] = args.mapLoadPath 220 | configInternal["extendParameterSets"] = ["relocalization"] 221 | 222 | replay = spectacularAI.Replay(args.dataFolder, onMappingOutput, configuration=configInternal) 223 | replay.setExtendedOutputCallback(replayOnVioOutput) 224 | replay.startReplay() 225 | while not state.shouldQuit: 226 | time.sleep(0.05) 227 | print("Quitting...") 228 | replay.close() 229 | else: 230 | config = spectacularAI.depthai.Configuration() 231 | config.internalParameters = configInternal 232 | if args.noFeatureTracker: config.useFeatureTracker = False 233 | if args.mapLoadPath: 234 | config.mapLoadPath = args.mapLoadPath 235 | config.useSlam = True 236 | 237 | pipeline, vio_pipeline = make_pipelines(config, onMappingOutput) 238 | with depthai.Device(pipeline) as device, \ 239 | vio_pipeline.startSession(device) as vioSession: 240 | if args.irBrightness > 0: 241 | device.setIrLaserDotProjectorBrightness(args.irBrightness) 242 | oakdLoop(state, device, vioSession) 243 | print("Quitting...") 244 | 245 | def parseArgs(): 246 | import argparse 247 | p = argparse.ArgumentParser(__doc__) 248 | # Generic parameters. 249 | p.add_argument("--resolution", help="Window resolution.", default="1920x1080") 250 | p.add_argument("--pointCloud", help="Start in the point cloud mode.", action="store_true") 251 | p.add_argument("--pointCloudDensity", help="Fraction of points to show.", default=0.2, type=float) 252 | p.add_argument("--dataFolder", help="Instead of running live mapping session, replay session from this folder") 253 | p.add_argument("--recordPath", help="Record the window to video file given by path.") 254 | p.add_argument("--depth", help="In meters, the max distance to detect points and construct mesh. Lower values may improve positioning accuracy.", default=4, type=float) 255 | p.add_argument('--objLoadPath', help="Load scene as .obj", default=None) 256 | p.add_argument('--objPosition', help="Set position of .obj mesh", default="0,0,0") 257 | p.add_argument("--mapLoadPath", help="SLAM map path (use in combination with objLoadPath)", default=None) 258 | # OAK-D parameters. 259 | p.add_argument('--irBrightness', help='OAK-D Pro (W) IR laser projector brightness (mA), 0 - 1200. Enabling may improve depth tracking.', type=float, default=0) 260 | p.add_argument('--noFeatureTracker', help="On OAK-D, use stereo images rather than accelerated features + depth.", action="store_true") 261 | # Parameters for non-OAK-D recordings. 262 | p.add_argument("--useRectification", help="--dataFolder option can also be used with some non-OAK-D recordings, but this parameter must be set if the videos inputs are not rectified.", action="store_true") 263 | p.add_argument('--cameraInd', help="Which camera to use. Typically 0=left, 1=right, 2=auxiliary/RGB (OAK-D default)", type=int, default=2) 264 | return p.parse_args() 265 | 266 | if __name__ == '__main__': 267 | main(parseArgs()) 268 | -------------------------------------------------------------------------------- /python/oak/mapping_ar_renderers/mesh.frag: -------------------------------------------------------------------------------- 1 | #version 150 2 | precision mediump float; 3 | 4 | uniform vec4 u_Options; 5 | varying vec3 v_Normal; 6 | varying vec3 v_TexCoord; 7 | varying vec3 v_Position; 8 | varying vec4 v_ScreenNdcHomog; 9 | 10 | void main() { 11 | vec3 objColor = vec3(0.5) + v_Normal*0.5; 12 | float distanceAlpha = exp(-abs(v_ScreenNdcHomog.w) / 400.0); 13 | float meshAlpha = 0.6 * distanceAlpha; 14 | float alpha = v_TexCoord.z * meshAlpha; 15 | 16 | vec2 border; 17 | if (u_Options.y > 0.0) { 18 | // "Fake mesh". 19 | const float GRID_SZ = 4.0; 20 | vec3 relPos = u_Options.y * v_Position / GRID_SZ; 21 | 22 | relPos = relPos - floor(relPos); 23 | if (u_Options.z > 0.0) { 24 | // Z levels only. 25 | border = vec2(relPos.z); 26 | } 27 | else { 28 | vec3 ano = abs(v_Normal); 29 | if (ano.z > max(ano.x, ano.y)) { 30 | border = relPos.xy; 31 | } else if (ano.x > max(ano.y, ano.z)) { 32 | border = relPos.yz; 33 | } else { 34 | border = relPos.xz; 35 | } 36 | } 37 | } 38 | else { 39 | // border = abs(v_TexCoord.xy - 0.5)*2.0; // funny pattern 40 | border = v_TexCoord.xy; // true mesh 41 | } 42 | 43 | float maxBord = 0.015; 44 | float gridAlpha = min(1.0, 2.0 * distanceAlpha); 45 | float relBord = 1.0 - min(border.x, border.y) / maxBord; 46 | if (u_Options.x > 0.0 && relBord > 0.0) { 47 | vec3 gridCol = vec3(0.0, 1.0, 1.0); 48 | float gridAlpha = gridAlpha * sqrt(v_TexCoord.z); 49 | objColor = objColor * (1.0 - gridAlpha) + gridAlpha * gridCol; 50 | alpha = gridAlpha + (1.0 - gridAlpha) * alpha; 51 | } 52 | 53 | gl_FragColor = vec4(objColor, u_Options.w * alpha); 54 | } 55 | -------------------------------------------------------------------------------- /python/oak/mapping_ar_renderers/mesh.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | 3 | import numpy as np 4 | 5 | from OpenGL.GL import * 6 | 7 | from .util import * 8 | 9 | class MeshProgram: 10 | def __init__(self, program): 11 | assert(program) 12 | self.program = program 13 | self.attributeVertex = glGetAttribLocation(self.program, "a_Position") 14 | self.attributeTexCoord = glGetAttribLocation(self.program, "a_TexCoord") 15 | self.attributeNormal = glGetAttribLocation(self.program, "a_Normal") 16 | self.uniformModelViewProjection = glGetUniformLocation(self.program, "u_ModelViewProjection") 17 | self.uniformOptions = glGetUniformLocation(self.program, "u_Options") 18 | 19 | class MeshRenderer: 20 | # Value greater than zero enables at index: 21 | # 0) Show borders. 22 | # 1) Enable "fake mesh". The number sets density of the lines. 23 | # 2) Fake mesh z-levels only. 24 | # 3) Alpha multiplier. 25 | # 4) Show camera view. 26 | # 5) Show mesh. 27 | OPTIONS = [ 28 | [0., 0., 0., 0.75, 1., 1.], 29 | [1., 0., 0., 0.75, 1., 1.], 30 | [1., 10., 0., 1., 1., 1.], 31 | [1., 10., 1., 1., 1., 1.], 32 | [1., 0., 0., 0.75, 0., 1.], 33 | ] 34 | selectedOption = 0 35 | 36 | modelViewProjection = None 37 | meshProgram = None 38 | 39 | # 1d arrays of GLfloat. 40 | vertexData = np.array([]) 41 | normalData = np.array([]) 42 | texCoordData = np.array([]) 43 | 44 | def __init__(self): 45 | assetDir = pathlib.Path(__file__).resolve().parent 46 | meshVert = (assetDir / "mesh.vert").read_text() 47 | meshFrag = (assetDir / "mesh.frag").read_text() 48 | self.meshProgram = MeshProgram(createProgram(meshVert, meshFrag)) 49 | 50 | def __init_tex_coord_data(self, nFaces): 51 | c = 1.0 # Could be used for controlling alpha. 52 | faceTexCoords = np.array([0, 0, c, 1, 0, c, 0, 1, c]) 53 | self.texCoordData = np.tile(faceTexCoords, nFaces) 54 | 55 | def render(self): 56 | if self.modelViewProjection is None: return 57 | if self.vertexData.shape[0] == 0: return 58 | 59 | op = MeshRenderer.OPTIONS[self.selectedOption] 60 | if op[5] == 0.0: return 61 | 62 | glEnable(GL_DEPTH_TEST) 63 | glDepthMask(GL_TRUE) 64 | glClear(GL_DEPTH_BUFFER_BIT) 65 | 66 | glEnable(GL_BLEND) 67 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) 68 | 69 | if op[4] == 0.0: glClear(GL_COLOR_BUFFER_BIT) 70 | 71 | glUseProgram(self.meshProgram.program) 72 | glUniform4fv(self.meshProgram.uniformOptions, 1, np.array(op[:4])) 73 | glUniformMatrix4fv(self.meshProgram.uniformModelViewProjection, 1, GL_FALSE, self.modelViewProjection.transpose()) 74 | 75 | coordsPerVertex = 3 76 | glEnableVertexAttribArray(self.meshProgram.attributeVertex) 77 | glVertexAttribPointer(self.meshProgram.attributeVertex, coordsPerVertex, 78 | GL_FLOAT, GL_FALSE, 0, self.vertexData) 79 | 80 | glEnableVertexAttribArray(self.meshProgram.attributeTexCoord) 81 | coordsPerTex = 3 82 | n = len(self.vertexData) 83 | glVertexAttribPointer(self.meshProgram.attributeTexCoord, coordsPerTex, GL_FLOAT, GL_FALSE, 0, self.texCoordData[:n]) 84 | 85 | glEnableVertexAttribArray(self.meshProgram.attributeNormal) 86 | glVertexAttribPointer(self.meshProgram.attributeNormal, coordsPerVertex, GL_FLOAT, GL_FALSE, 0, 87 | self.normalData) 88 | 89 | nVertices = self.vertexData.shape[0] // coordsPerVertex 90 | glDrawArrays(GL_TRIANGLES, 0, nVertices) 91 | 92 | glDisableVertexAttribArray(self.meshProgram.attributeNormal) 93 | glDisableVertexAttribArray(self.meshProgram.attributeTexCoord) 94 | glDisableVertexAttribArray(self.meshProgram.attributeVertex) 95 | glUseProgram(0) 96 | 97 | glDisable(GL_BLEND) 98 | glDisable(GL_DEPTH_TEST) 99 | 100 | def setMesh(self, mesh): 101 | if mesh is None: return 102 | if not mesh.hasNormals(): return 103 | 104 | vertexPositions = mesh.getPositionData() 105 | vertexInds = mesh.getFaceVertices().flatten() 106 | self.vertexData = vertexPositions[vertexInds, :].flatten() 107 | 108 | normals = mesh.getNormalData() 109 | normalInds = mesh.getFaceNormals().flatten() 110 | self.normalData = normals[normalInds, :].flatten() 111 | 112 | if len(self.texCoordData) < len(self.vertexData): 113 | # Optimization: (in this demo texture coordinates are a constant repeating array) 114 | # Double the tex coordinate array size when the old one is too small. 115 | self.__init_tex_coord_data(2 * len(self.vertexData)) 116 | 117 | def setPose(self, cameraPose): 118 | near, far = 0.01, 100.0 119 | projection = cameraPose.camera.getProjectionMatrixOpenGL(near, far) 120 | modelView = cameraPose.getWorldToCameraMatrix() 121 | self.modelViewProjection = projection @ modelView 122 | 123 | def nextMode(self): 124 | self.selectedOption = (self.selectedOption + 1) % len(MeshRenderer.OPTIONS) 125 | -------------------------------------------------------------------------------- /python/oak/mapping_ar_renderers/mesh.vert: -------------------------------------------------------------------------------- 1 | #version 150 2 | precision mediump float; 3 | 4 | uniform mat4 u_ModelViewProjection; 5 | attribute vec3 a_Position; 6 | attribute vec3 a_Normal; 7 | attribute vec3 a_TexCoord; 8 | varying vec3 v_Normal; 9 | varying vec3 v_TexCoord; 10 | varying vec3 v_Position; 11 | varying vec4 v_ScreenNdcHomog; 12 | 13 | void main() { 14 | v_Normal = a_Normal; 15 | v_TexCoord = a_TexCoord; 16 | v_Position = a_Position.xyz; 17 | v_ScreenNdcHomog = u_ModelViewProjection * vec4(a_Position.xyz, 1.0); 18 | gl_Position = v_ScreenNdcHomog; 19 | } 20 | -------------------------------------------------------------------------------- /python/oak/mapping_ar_renderers/point_cloud.frag: -------------------------------------------------------------------------------- 1 | #version 150 2 | 3 | precision mediump float; 4 | varying vec4 v_Position; 5 | varying vec3 v_Effect; 6 | 7 | void main() { 8 | float d = length(2 * (gl_PointCoord - 0.5)); 9 | float alpha = 1 - d * d; 10 | float c = 0; 11 | 12 | const float TIME_APPEAR = 0.1; 13 | const float TIME_DISAPPEAR = 0.3; 14 | if (v_Effect.z < TIME_APPEAR) { 15 | alpha *= v_Effect.z / TIME_APPEAR; 16 | } 17 | else if (v_Effect.z > TIME_DISAPPEAR) { 18 | c = (v_Effect.z - TIME_DISAPPEAR) / (1 - TIME_DISAPPEAR); 19 | alpha *= (1 - c * c); 20 | } 21 | gl_FragColor = vec4(c, 1, c, alpha); 22 | } 23 | -------------------------------------------------------------------------------- /python/oak/mapping_ar_renderers/point_cloud.py: -------------------------------------------------------------------------------- 1 | import pathlib 2 | import random 3 | 4 | import numpy as np 5 | 6 | from OpenGL.GL import * 7 | 8 | from .util import * 9 | 10 | MAX_AGE_SECONDS = 3 11 | 12 | COORDS_PER_VERTEX = 3 13 | COORDS_PER_EFFECT = 3 14 | 15 | class PointCloud: 16 | vertexData = np.array([]) 17 | effectData = np.array([]) 18 | time = None 19 | 20 | class PointCloudProgram: 21 | def __init__(self, program): 22 | assert(program) 23 | self.program = program 24 | self.attributeVertex = glGetAttribLocation(self.program, "a_Position") 25 | self.attributeEffect = glGetAttribLocation(self.program, "a_Effect") 26 | self.uniformModelView = glGetUniformLocation(self.program, "u_ModelView") 27 | self.uniformProjection = glGetUniformLocation(self.program, "u_Projection") 28 | 29 | class PointCloudRenderer: 30 | # Value greater than zero enables at index: 31 | # 0) Show camera view. 32 | OPTIONS = [ 33 | [1], 34 | [0], 35 | ] 36 | 37 | selectedOption = 0 38 | modelView = None 39 | projection = None 40 | pcProgram = None 41 | density = None 42 | pointClouds = [] 43 | 44 | # 1d arrays of GLfloat. 45 | vertexData = np.array([]) 46 | effectData = np.array([]) 47 | 48 | def __init__(self, density): 49 | self.density = density 50 | 51 | assetDir = pathlib.Path(__file__).resolve().parent 52 | pcVert = (assetDir / "point_cloud.vert").read_text() 53 | pcFrag = (assetDir / "point_cloud.frag").read_text() 54 | self.pcProgram = PointCloudProgram(createProgram(pcVert, pcFrag)) 55 | 56 | def render(self): 57 | if self.modelView is None: return 58 | if self.vertexData.shape[0] == 0: return 59 | 60 | glEnable(GL_BLEND) 61 | glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) 62 | glEnable(GL_PROGRAM_POINT_SIZE) 63 | glEnable(GL_POINT_SPRITE) 64 | 65 | op = PointCloudRenderer.OPTIONS[self.selectedOption] 66 | if op[0] == 0.0: glClear(GL_COLOR_BUFFER_BIT) 67 | 68 | glUseProgram(self.pcProgram.program) 69 | glUniformMatrix4fv(self.pcProgram.uniformModelView, 1, GL_FALSE, self.modelView.transpose()) 70 | glUniformMatrix4fv(self.pcProgram.uniformProjection, 1, GL_FALSE, self.projection.transpose()) 71 | 72 | glEnableVertexAttribArray(self.pcProgram.attributeVertex) 73 | glVertexAttribPointer(self.pcProgram.attributeVertex, COORDS_PER_VERTEX, 74 | GL_FLOAT, GL_FALSE, 0, self.vertexData) 75 | 76 | glEnableVertexAttribArray(self.pcProgram.attributeEffect) 77 | glVertexAttribPointer(self.pcProgram.attributeEffect, COORDS_PER_EFFECT, 78 | GL_FLOAT, GL_FALSE, 0, self.effectData) 79 | 80 | nVertices = self.vertexData.shape[0] // COORDS_PER_VERTEX 81 | glDrawArrays(GL_POINTS, 0, nVertices) 82 | 83 | glDisableVertexAttribArray(self.pcProgram.attributeEffect) 84 | glDisableVertexAttribArray(self.pcProgram.attributeVertex) 85 | glUseProgram(0) 86 | 87 | glDisable(GL_PROGRAM_POINT_SIZE) 88 | glDisable(GL_BLEND) 89 | 90 | def setPointCloud(self, mapperOutput, time): 91 | keyFrameId = max(mapperOutput.map.keyFrames.keys()) 92 | if keyFrameId is None: return 93 | 94 | def affineTransform(M, p): 95 | return M[:3, :3] @ p + M[:3, 3] 96 | 97 | keyFrame = mapperOutput.map.keyFrames[keyFrameId] 98 | if not keyFrame.pointCloud: return 99 | cameraPose = keyFrame.frameSet.primaryFrame.cameraPose 100 | cToW = cameraPose.getCameraToWorldMatrix() 101 | points = keyFrame.pointCloud.getPositionData() 102 | 103 | selected = [] 104 | for i in range(points.shape[0]): 105 | if random.random() < self.density: selected.append(i) 106 | n = len(selected) 107 | 108 | cv = COORDS_PER_VERTEX 109 | pointCloud = PointCloud() 110 | pointCloud.vertexData = np.zeros(cv * n) 111 | pointCloud.effectData = np.zeros(COORDS_PER_EFFECT * n) 112 | pointCloud.time = time 113 | j = 0 114 | for i in selected: 115 | pointCloud.vertexData[(cv * j):(cv * j + cv)] = affineTransform(cToW, points[i, :]) 116 | j += 1 117 | self.pointClouds.append(pointCloud) 118 | 119 | def setPose(self, cameraPose, time): 120 | near, far = 0.01, 100.0 121 | self.projection = cameraPose.camera.getProjectionMatrixOpenGL(near, far) 122 | self.modelView = cameraPose.getWorldToCameraMatrix() 123 | 124 | while len(self.pointClouds) > 0 and time - self.pointClouds[0].time > MAX_AGE_SECONDS: 125 | self.pointClouds.pop(0) 126 | 127 | nv = sum([p.vertexData.shape[0] for p in self.pointClouds]) 128 | ne = sum([p.effectData.shape[0] for p in self.pointClouds]) 129 | self.vertexData = np.zeros(nv) 130 | self.effectData = np.zeros(ne) 131 | iv = 0 132 | ie = 0 133 | TIME_DISAPPEAR = 0.7 134 | for p in self.pointClouds: 135 | self.vertexData[iv:(iv + p.vertexData.shape[0])] = p.vertexData 136 | iv += p.vertexData.shape[0] 137 | 138 | timeRelative = (time - p.time) / MAX_AGE_SECONDS 139 | for j in range(p.effectData.shape[0] // COORDS_PER_EFFECT): 140 | p.effectData[COORDS_PER_EFFECT * j + 2] = timeRelative 141 | if timeRelative > TIME_DISAPPEAR: 142 | p.effectData[COORDS_PER_EFFECT * j + 0] += 0.002 * random.random() 143 | p.effectData[COORDS_PER_EFFECT * j + 1] += 0.006 * random.random() 144 | self.effectData[ie:(ie + p.effectData.shape[0])] = p.effectData 145 | ie += p.effectData.shape[0] 146 | assert(iv == nv) 147 | assert(ie == ne) 148 | 149 | def nextMode(self): 150 | self.selectedOption = (self.selectedOption + 1) % len(PointCloudRenderer.OPTIONS) 151 | -------------------------------------------------------------------------------- /python/oak/mapping_ar_renderers/point_cloud.vert: -------------------------------------------------------------------------------- 1 | #version 150 2 | 3 | uniform mat4 u_ModelView; 4 | uniform mat4 u_Projection; 5 | attribute vec3 a_Position; 6 | attribute vec3 a_Effect; 7 | varying vec4 v_Position; 8 | varying vec3 v_Effect; 9 | 10 | void main() { 11 | v_Position = u_ModelView * vec4(a_Position.xyz, 1.0); 12 | v_Effect = a_Effect; 13 | vec4 shift = vec4(a_Effect.xy, 0, 0); 14 | gl_Position = u_Projection * v_Position + shift; 15 | gl_PointSize = 9.0 / v_Position.z; 16 | } 17 | -------------------------------------------------------------------------------- /python/oak/mapping_ar_renderers/util.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from OpenGL.GL import * 3 | 4 | def loadShader(shaderType, shaderSource): 5 | shader = glCreateShader(shaderType) 6 | assert(shader) 7 | glShaderSource(shader, shaderSource) 8 | glCompileShader(shader) 9 | result = glGetShaderiv(shader, GL_COMPILE_STATUS) 10 | if not result: raise RuntimeError(glGetShaderInfoLog(shader)) 11 | return shader 12 | 13 | def createProgram(vertexSource, fragmentSource): 14 | vertexShader = loadShader(GL_VERTEX_SHADER, vertexSource) 15 | fragmentShader = loadShader(GL_FRAGMENT_SHADER, fragmentSource) 16 | program = glCreateProgram() 17 | assert(program) 18 | glAttachShader(program, vertexShader) 19 | glAttachShader(program, fragmentShader) 20 | glLinkProgram(program) 21 | result = glGetProgramiv(program, GL_LINK_STATUS) 22 | if not result: raise RuntimeError(glGetProgramInfoLog(program)) 23 | return program 24 | 25 | # Class that mocks `spectacularAI::mapping::Mesh`. 26 | class Mesh: 27 | def __init__(self): 28 | self.vertexPositions = np.empty((0, 3), float) 29 | self.normals = np.empty((0, 3), float) 30 | self.faceVertices = np.empty((0, 3), int) 31 | self.faceNormals = np.empty((0, 3), int) 32 | 33 | def hasNormals(self): 34 | return self.normals.shape[0] > 0 35 | 36 | def getPositionData(self): 37 | return self.vertexPositions 38 | 39 | def getNormalData(self): 40 | return self.normals 41 | 42 | def getFaceVertices(self): 43 | return self.faceVertices 44 | 45 | def getFaceNormals(self): 46 | return self.faceNormals 47 | 48 | def loadObjToMesh(objPath, objPosition): 49 | mesh = Mesh() 50 | with open(objPath, 'r') as objFile: 51 | for line in objFile: 52 | if line.startswith('#'): continue 53 | cmd, _, rest = line.partition(' ') 54 | data = rest.split() 55 | if cmd == 'v': 56 | mesh.vertexPositions = np.vstack((mesh.vertexPositions, [float(c) for c in data])) 57 | elif cmd == 'vn': 58 | mesh.normals = np.vstack((mesh.normals, [float(c) for c in data])) 59 | elif cmd == 'f': 60 | v = [] 61 | n = [] 62 | for i, token in enumerate(data): 63 | indices = token.split('/') 64 | assert(int(indices[0]) >= 1) # Negative indices counting from the end are not handled. 65 | v.append(int(indices[0]) - 1) 66 | # Textures not handled. 67 | if len(indices) >= 3: n.append(int(indices[2]) - 1) 68 | 69 | if i < 2: continue 70 | # If i > 2, interpret the polygon as triangle fan and hope for the best. 71 | mesh.faceVertices = np.vstack((mesh.faceVertices, [v[0], v[-2], v[-1]])) 72 | if n: mesh.faceNormals = np.vstack((mesh.faceNormals, [n[0], n[-2], n[-1]])) 73 | 74 | mesh.vertexPositions += objPosition 75 | 76 | return mesh 77 | -------------------------------------------------------------------------------- /python/oak/mapping_ros.py: -------------------------------------------------------------------------------- 1 | """ 2 | !!! For ROS2 see ros2/ folder for a more complete example !!! 3 | 4 | Runs spectacularAI mapping and publishes poses and frames in ROS. 5 | 6 | Make sure to have your ROS environment sourced before running this script. Tested with ROS noetic. 7 | 8 | The SpectacularAI SDK and other dependencies can for example be installed in a virtual environment. 9 | """ 10 | import spectacularAI 11 | import depthai 12 | import rospy 13 | import numpy as np 14 | import time 15 | import os 16 | import tf 17 | from geometry_msgs.msg import PoseStamped, TransformStamped 18 | from scipy.spatial.transform import Rotation 19 | from tf2_msgs.msg import TFMessage 20 | from cv_bridge import CvBridge 21 | from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField 22 | 23 | def to_pose_message(camToWorld): 24 | msg = PoseStamped() 25 | msg.header.stamp = rospy.Time.now() 26 | msg.header.frame_id = "world" 27 | msg.pose.position.x = camToWorld[0, 3] 28 | msg.pose.position.y = camToWorld[1, 3] 29 | msg.pose.position.z = camToWorld[2, 3] 30 | R_CW = Rotation.from_matrix(camToWorld[0:3, 0:3]) 31 | q_cw = R_CW.as_quat() 32 | msg.pose.orientation.x = q_cw[0] 33 | msg.pose.orientation.y = q_cw[1] 34 | msg.pose.orientation.z = q_cw[2] 35 | msg.pose.orientation.w = q_cw[3] 36 | return msg 37 | 38 | def to_tf_message(camToWorld, ts, frame_id): 39 | msg = TFMessage() 40 | msg.transforms = [] 41 | transform = TransformStamped() 42 | transform.header.stamp = ts 43 | transform.header.frame_id = "world" 44 | transform.child_frame_id = frame_id 45 | transform.transform.translation.x = camToWorld[0, 3] 46 | transform.transform.translation.y = camToWorld[1, 3] 47 | transform.transform.translation.z = camToWorld[2, 3] 48 | R_CW = Rotation.from_matrix(camToWorld[0:3, 0:3]) 49 | q_cw = R_CW.as_quat() 50 | transform.transform.rotation.x = q_cw[0] 51 | transform.transform.rotation.y = q_cw[1] 52 | transform.transform.rotation.z = q_cw[2] 53 | transform.transform.rotation.w = q_cw[3] 54 | msg.transforms.append(transform) 55 | return msg 56 | 57 | def to_camera_info_message(camera, frame, ts): 58 | intrinsic = camera.getIntrinsicMatrix() 59 | msg = CameraInfo() 60 | msg.header.stamp = ts 61 | msg.header.frame_id = "rgb_optical" 62 | msg.height = frame.shape[0] 63 | msg.width = frame.shape[1] 64 | msg.distortion_model = "none" 65 | msg.D = [] 66 | msg.K = intrinsic.ravel().tolist() 67 | return msg 68 | 69 | 70 | 71 | class SLAMNode: 72 | def __init__(self): 73 | rospy.init_node("slam_node", anonymous=True) 74 | self.odometry_publisher = rospy.Publisher("/slam/odometry", PoseStamped, queue_size=10) 75 | self.keyframe_publisher = rospy.Publisher("/slam/keyframe", PoseStamped, queue_size=10) 76 | self.rgb_publisher = rospy.Publisher("/slam/rgb", Image, queue_size=10) 77 | self.tf_publisher = rospy.Publisher("/tf", TFMessage, queue_size=10) 78 | self.point_publisher = rospy.Publisher("/slam/pointcloud", PointCloud2, queue_size=10) 79 | self.depth_publisher = rospy.Publisher("/slam/depth", Image, queue_size=10) 80 | self.camera_info_publisher = rospy.Publisher("/slam/camera_info", CameraInfo, queue_size=10) 81 | self.bridge = CvBridge() 82 | self.keyframes = {} 83 | 84 | def has_keyframe(self, frame_id): 85 | return frame_id in self.keyframes 86 | 87 | def newKeyFrame(self, frame_id, keyframe): 88 | now = rospy.Time.now() 89 | self.keyframes[frame_id] = True 90 | camToWorld = keyframe.frameSet.rgbFrame.cameraPose.getCameraToWorldMatrix() 91 | sequence_number = int(frame_id) 92 | msg = to_pose_message(camToWorld) 93 | msg.header.seq = sequence_number 94 | msg.header.stamp = now 95 | self.keyframe_publisher.publish(msg) 96 | 97 | rgb_frame = keyframe.frameSet.getUndistortedFrame(keyframe.frameSet.rgbFrame).image 98 | rgb_frame = rgb_frame.toArray() 99 | rgb_message = self.bridge.cv2_to_imgmsg(rgb_frame, encoding="rgb8") 100 | rgb_message.header.stamp = now 101 | rgb_message.header.frame_id = "rgb_optical" 102 | rgb_message.header.seq = sequence_number 103 | self.rgb_publisher.publish(rgb_message) 104 | tf_message = to_tf_message(camToWorld, now, "rgb_optical") 105 | self.tf_publisher.publish(tf_message) 106 | 107 | camera = keyframe.frameSet.rgbFrame.cameraPose.camera 108 | info_msg = to_camera_info_message(camera, rgb_frame, now) 109 | self.camera_info_publisher.publish(info_msg) 110 | 111 | self.newPointCloud(keyframe) 112 | 113 | depth_frame = keyframe.frameSet.getAlignedDepthFrame(keyframe.frameSet.rgbFrame) 114 | depth = depth_frame.image.toArray() 115 | depth_msg = self.bridge.cv2_to_imgmsg(depth, encoding="mono16") 116 | depth_msg.header.stamp = now 117 | depth_msg.header.frame_id = "rgb_optical" 118 | depth_msg.header.seq = sequence_number 119 | self.depth_publisher.publish(depth_msg) 120 | 121 | 122 | def newOdometryFrame(self, camToWorld): 123 | msg = to_pose_message(camToWorld) 124 | self.odometry_publisher.publish(msg) 125 | 126 | def newPointCloud(self, keyframe): 127 | camToWorld = keyframe.frameSet.rgbFrame.cameraPose.getCameraToWorldMatrix() 128 | positions = keyframe.pointCloud.getPositionData() 129 | pc = np.zeros((positions.shape[0], 6), dtype=np.float32) 130 | p_C = np.vstack((positions.T, np.ones((1, positions.shape[0])))).T 131 | pc[:, :3] = (camToWorld @ p_C[:, :, None])[:, :3, 0] 132 | 133 | msg = PointCloud2() 134 | msg.header.stamp = rospy.Time.now() 135 | msg.header.frame_id = "world" 136 | if keyframe.pointCloud.hasColors(): 137 | pc[:, 3:] = keyframe.pointCloud.getRGB24Data() * (1. / 255.) 138 | msg.point_step = 4 * 6 139 | msg.height = 1 140 | msg.width = pc.shape[0] 141 | msg.row_step = msg.point_step * pc.shape[0] 142 | msg.data = pc.tobytes() 143 | msg.is_bigendian = False 144 | msg.is_dense = False 145 | ros_dtype = PointField.FLOAT32 146 | itemsize = np.dtype(np.float32).itemsize 147 | msg.fields = [PointField(name=n, offset=i*itemsize, datatype=ros_dtype, count=1) for i, n in enumerate('xyzrgb')] 148 | self.point_publisher.publish(msg) 149 | 150 | 151 | def parseArgs(): 152 | import argparse 153 | p = argparse.ArgumentParser(__doc__) 154 | p.add_argument('--ir_dot_brightness', help='OAK-D Pro (W) IR laser projector brightness (mA), 0 - 1200', type=float, default=0) 155 | p.add_argument("--useRectification", help="--dataFolder option can also be used with some non-OAK-D recordings, but this parameter must be set if the videos inputs are not rectified.", action="store_true") 156 | p.add_argument('--map', help='Map file to load', default=None) 157 | p.add_argument('--save-map', help='Map file to save', default=None) 158 | p.add_argument('--color', help="Use RGB camera for tracking", action="store_true") 159 | return p.parse_args() 160 | 161 | if __name__ == '__main__': 162 | args = parseArgs() 163 | 164 | configInternal = { 165 | "computeStereoPointCloud": "true", 166 | "pointCloudNormalsEnabled": "true", 167 | "computeDenseStereoDepth": "true", 168 | } 169 | if args.useRectification: 170 | configInternal["useRectification"] = "true" 171 | 172 | slam_node = SLAMNode() 173 | 174 | def onVioOutput(vioOutput): 175 | cameraPose = vioOutput.getCameraPose(0) 176 | camToWorld = cameraPose.getCameraToWorldMatrix() 177 | slam_node.newOdometryFrame(camToWorld) 178 | 179 | def onMappingOutput(output): 180 | for frame_id in output.updatedKeyFrames: 181 | keyFrame = output.map.keyFrames.get(frame_id) 182 | 183 | # Remove deleted key frames from visualisation 184 | if not keyFrame: 185 | continue 186 | 187 | # Check that point cloud exists 188 | if not keyFrame.pointCloud: continue 189 | 190 | if not slam_node.has_keyframe(frame_id): 191 | slam_node.newKeyFrame(frame_id, keyFrame) 192 | 193 | if output.finalMap: 194 | print("Final map ready!") 195 | 196 | print("Starting OAK-D device") 197 | pipeline = depthai.Pipeline() 198 | config = spectacularAI.depthai.Configuration() 199 | config.internalParameters = configInternal 200 | config.useSlam = True 201 | if args.color: 202 | config.useColor = True 203 | if args.map is not None: 204 | config.mapLoadPath = args.map 205 | if args.save_map is not None: 206 | config.mapSavePath = args.save_map 207 | vioPipeline = spectacularAI.depthai.Pipeline(pipeline, config, onMappingOutput) 208 | 209 | with depthai.Device(pipeline) as device, \ 210 | vioPipeline.startSession(device) as vio_session: 211 | if args.ir_dot_brightness > 0: 212 | device.setIrLaserDotProjectorBrightness(args.ir_dot_brightness) 213 | while not rospy.is_shutdown(): 214 | onVioOutput(vio_session.waitForOutput()) 215 | 216 | 217 | -------------------------------------------------------------------------------- /python/oak/mapping_visu.py: -------------------------------------------------------------------------------- 1 | """ 2 | Visualize 3D point cloud of the environment in real-time, or playback your recordings and view their 3D point cloud. 3 | 4 | Requirements: pip install spectacularAI[full] 5 | """ 6 | 7 | import spectacularAI 8 | import depthai 9 | import threading 10 | 11 | from spectacularAI.cli.visualization.visualizer import Visualizer, VisualizerArgs 12 | 13 | def parseArgs(): 14 | import argparse 15 | p = argparse.ArgumentParser(__doc__) 16 | p.add_argument("--dataFolder", help="Instead of running live mapping session, replay session from this folder") 17 | p.add_argument("--recordingFolder", help="Record live mapping session for replay") 18 | p.add_argument("--resolution", help="Window resolution", default="1280x720") 19 | p.add_argument("--fullScreen", help="Start in full screen mode", action="store_true") 20 | p.add_argument("--recordWindow", help="Window recording filename") 21 | p.add_argument("--voxel", type=float, help="Voxel size (m) for downsampling point clouds") 22 | p.add_argument("--color", help="Filter points without color", action="store_true") 23 | p.add_argument("--useRgb", help="Use OAK-D RGB camera", action="store_true") 24 | p.add_argument('--irDotBrightness', help='OAK-D Pro (W) IR laser projector brightness (mA), 0 - 1200', type=float, default=0) 25 | p.add_argument('--noFeatureTracker', help='Disable on-device feature tracking and depth map', action="store_true") 26 | p.add_argument("--useRectification", help="This parameter must be set if the stereo video inputs are not rectified", action="store_true") 27 | p.add_argument('--keyFrameCandidateInterval', type=int, help='Sets internal parameter keyframeCandidateEveryNthFrame') 28 | return p.parse_args() 29 | 30 | if __name__ == '__main__': 31 | args = parseArgs() 32 | 33 | configInternal = { 34 | "computeStereoPointCloud": "true", 35 | "pointCloudNormalsEnabled": "true", 36 | "computeDenseStereoDepth": "true", 37 | } 38 | 39 | if args.dataFolder and args.useRectification: 40 | configInternal["useRectification"] = "true" 41 | 42 | visArgs = VisualizerArgs() 43 | visArgs.resolution = args.resolution 44 | visArgs.fullScreen = args.fullScreen 45 | visArgs.recordPath = args.recordWindow 46 | visArgs.pointCloudVoxelSize = args.voxel 47 | visArgs.skipPointsWithoutColor = args.color 48 | visArgs.targetFps = 0 if args.dataFolder else 30 49 | visualizer = Visualizer(visArgs) 50 | 51 | def onMappingOutput(mapperOutput): 52 | visualizer.onMappingOutput(mapperOutput) 53 | if mapperOutput.finalMap: print("Final map ready!") 54 | 55 | def onVioOutput(vioOutput): 56 | visualizer.onVioOutput(vioOutput.getCameraPose(0), status=vioOutput.status) 57 | 58 | if args.dataFolder: 59 | print("Starting replay") 60 | replay = spectacularAI.Replay(args.dataFolder, onMappingOutput, configuration=configInternal) 61 | replay.setOutputCallback(onVioOutput) 62 | replay.startReplay() 63 | visualizer.run() 64 | replay.close() 65 | else: 66 | def captureLoop(): 67 | print("Starting OAK-D device") 68 | pipeline = depthai.Pipeline() 69 | config = spectacularAI.depthai.Configuration() 70 | config.useFeatureTracker = not args.noFeatureTracker 71 | config.useColor = args.useRgb 72 | config.internalParameters = configInternal 73 | if args.recordingFolder: config.recordingFolder = args.recordingFolder 74 | if args.keyFrameCandidateInterval: config.keyframeCandidateEveryNthFrame = args.keyFrameCandidateInterval 75 | vioPipeline = spectacularAI.depthai.Pipeline(pipeline, config, onMappingOutput) 76 | 77 | with depthai.Device(pipeline) as device, \ 78 | vioPipeline.startSession(device) as vio_session: 79 | if args.irDotBrightness > 0: 80 | device.setIrLaserDotProjectorBrightness(args.irDotBrightness) 81 | while not visualizer.shouldQuit: 82 | onVioOutput(vio_session.waitForOutput()) 83 | 84 | thread = threading.Thread(target=captureLoop) 85 | thread.start() 86 | visualizer.run() 87 | thread.join() 88 | -------------------------------------------------------------------------------- /python/oak/mixed_reality.py: -------------------------------------------------------------------------------- 1 | """ 2 | Mixed reality example using PyOpenGL. Requirements: 3 | 4 | pip install pygame PyOpenGL PyOpenGL_accelerate 5 | """ 6 | 7 | import depthai 8 | import spectacularAI 9 | import pygame 10 | import time 11 | 12 | from OpenGL.GL import * # all prefixed with gl so OK to import * 13 | 14 | FPS = 24 # set to 30 for smoother frame rate 15 | 16 | def parse_args(): 17 | import argparse 18 | p = argparse.ArgumentParser(__doc__) 19 | p.add_argument("--mapLoadPath", help="SLAM map path", default=None) 20 | p.add_argument('--objLoadPath', help="Load scene as .obj", default=None) 21 | return p.parse_args() 22 | 23 | def make_pipelines(config, onMappingOutput=None): 24 | pipeline = depthai.Pipeline() 25 | vio_pipeline = spectacularAI.depthai.Pipeline(pipeline, config, onMappingOutput) 26 | 27 | # NOTE: this simple method of reading RGB data from the device does not 28 | # scale so well to higher resolutions. Use YUV data with larger resolutions 29 | RGB_OUTPUT_WIDTH = 1024 30 | REF_ASPECT = 1920 / 1080.0 31 | w = RGB_OUTPUT_WIDTH 32 | h = int(round(w / REF_ASPECT)) 33 | 34 | camRgb = pipeline.createColorCamera() 35 | camRgb.setPreviewSize(w, h) 36 | camRgb.setResolution(depthai.ColorCameraProperties.SensorResolution.THE_1080_P) 37 | camRgb.setColorOrder(depthai.ColorCameraProperties.ColorOrder.RGB) 38 | camRgb.setImageOrientation(depthai.CameraImageOrientation.VERTICAL_FLIP) # for OpenGL 39 | camRgb.setFps(FPS) 40 | camRgb.initialControl.setAutoFocusMode(depthai.RawCameraControl.AutoFocusMode.OFF) 41 | camRgb.initialControl.setManualFocus(130) # seems to be about 1m 42 | out_source = camRgb.preview 43 | 44 | xout_camera = pipeline.createXLinkOut() 45 | xout_camera.setStreamName("cam_out") 46 | out_source.link(xout_camera.input) 47 | 48 | return (pipeline, vio_pipeline) 49 | 50 | def init_display(w, h): 51 | from pygame.locals import DOUBLEBUF, OPENGL 52 | pygame.init() 53 | pygame.display.set_mode((w, h), DOUBLEBUF | OPENGL) 54 | 55 | def draw_cube(origin): 56 | CUBE_VERTICES = ( 57 | (1, -1, -1), (1, 1, -1), (-1, 1, -1), (-1, -1, -1), 58 | (1, -1, 1), (1, 1, 1), (-1, -1, 1), (-1, 1, 1) 59 | ) 60 | CUBE_EDGES = ( 61 | (0,1), (0,3), (0,4), (2,1), (2,3), (2,7), 62 | (6,3), (6,4), (6,7), (5,1), (5,4), (5,7) 63 | ) 64 | 65 | glPushMatrix() 66 | # cube world position 67 | glTranslatef(origin[0], origin[1], origin[2]) 68 | glScalef(*([0.1] * 3)) 69 | 70 | glBegin(GL_LINES) 71 | for edge in CUBE_EDGES: 72 | for vertex in edge: 73 | glVertex3fv(CUBE_VERTICES[vertex]) 74 | glEnd() 75 | glPopMatrix() 76 | 77 | def load_and_draw_obj_as_wireframe(in_stream): 78 | vertices = [] 79 | glBegin(GL_LINES) 80 | for line in in_stream: 81 | if line.startswith('#'): continue 82 | cmd, _, rest = line.partition(' ') 83 | data = rest.split() 84 | if cmd == 'v': 85 | vertices.append([float(c) for c in data]) 86 | elif cmd == 'f': 87 | indices = [int(c.split('/')[0]) for c in data] 88 | for i in range(len(indices)): 89 | glVertex3fv(vertices[indices[i] - 1]) 90 | glVertex3fv(vertices[indices[(i + 1) % len(indices)] - 1]) 91 | # skip everything else 92 | glEnd() 93 | 94 | def load_obj(objLoadPath, origin=(0.5, 0, 0)): 95 | gl_list = glGenLists(1) 96 | glNewList(gl_list, GL_COMPILE) 97 | if objLoadPath is None: 98 | draw_cube(origin) 99 | else: 100 | with open(objLoadPath, 'r') as f: 101 | load_and_draw_obj_as_wireframe(f) 102 | glEndList() 103 | return gl_list 104 | 105 | def draw(cam, width, height, data, obj, is_tracking): 106 | # copy image as AR background 107 | glDrawPixels(width, height, GL_RGB, GL_UNSIGNED_BYTE, data) 108 | if not is_tracking: return 109 | 110 | # setup OpenGL camera based on VIO output 111 | near, far = 0.01, 100.0 # clip 112 | glMatrixMode(GL_PROJECTION) 113 | glLoadMatrixf(cam.camera.getProjectionMatrixOpenGL(near, far).transpose()) 114 | glMatrixMode(GL_MODELVIEW) 115 | glLoadMatrixf(cam.getWorldToCameraMatrix().transpose()) 116 | 117 | glClear(GL_DEPTH_BUFFER_BIT) 118 | glColor3f(1, 0, 1) 119 | glLineWidth(2.0) 120 | glCallList(obj) 121 | 122 | def main_loop(args, device, vio_session): 123 | display_initialized = False 124 | img_queue = device.getOutputQueue(name="cam_out", maxSize=4, blocking=False) 125 | 126 | # buffer for frames: show together with the corresponding VIO output 127 | frames = {} 128 | frame_number = 1 129 | obj = None 130 | 131 | while True: 132 | if img_queue.has(): 133 | img = img_queue.get() 134 | img_time = img.getTimestampDevice().total_seconds() 135 | frames[frame_number] = img 136 | vio_session.addTrigger(img_time, frame_number) 137 | frame_number += 1 138 | 139 | elif vio_session.hasOutput(): 140 | out = vio_session.getOutput() 141 | 142 | if out.tag > 0: 143 | img = frames.get(out.tag) 144 | 145 | if not display_initialized: 146 | display_initialized = True 147 | clock = pygame.time.Clock() 148 | init_display(img.getWidth(), img.getHeight()) 149 | obj = load_obj(args.objLoadPath) 150 | 151 | cam = vio_session.getRgbCameraPose(out) 152 | 153 | for event in pygame.event.get(): 154 | if event.type == pygame.QUIT: 155 | pygame.quit() 156 | return 157 | 158 | is_tracking = out.status == spectacularAI.TrackingStatus.TRACKING 159 | draw(cam, img.getWidth(), img.getHeight(), img.getRaw().data, obj, is_tracking) 160 | 161 | pygame.display.flip() 162 | # uncomment for smooth frame rate at higher latency 163 | # clock.tick(FPS) 164 | 165 | # discard old tags 166 | frames = { tag: v for tag, v in frames.items() if tag > out.tag } 167 | else: 168 | pygame.time.wait(1) 169 | 170 | if __name__ == '__main__': 171 | args = parse_args() 172 | 173 | config = spectacularAI.depthai.Configuration() 174 | if args.mapLoadPath is not None: 175 | config.mapLoadPath = args.mapLoadPath 176 | config.useSlam = True 177 | 178 | pipeline, vio_pipeline = make_pipelines(config) 179 | with depthai.Device(pipeline) as device, \ 180 | vio_pipeline.startSession(device) as vio_session: 181 | main_loop(args, device, vio_session) 182 | -------------------------------------------------------------------------------- /python/oak/mixed_reality_replay.py: -------------------------------------------------------------------------------- 1 | """ 2 | Mixed reality example using PyOpenGL. Requirements: spectacularai[full] 3 | 4 | """ 5 | 6 | import spectacularAI 7 | from spectacularAI.cli.visualization.visualizer import Visualizer, VisualizerArgs, CameraMode 8 | from mixed_reality import load_obj 9 | 10 | from OpenGL.GL import * # all prefixed with gl so OK to import * 11 | 12 | def parseArgs(): 13 | import argparse 14 | p = argparse.ArgumentParser(__doc__) 15 | p.add_argument("dataFolder", help="Folder containing the recorded session for replay", default="data") 16 | p.add_argument("--useRectification", help="This parameter must be set if the video inputs are not rectified", action="store_true") 17 | p.add_argument('--cameraInd', help="Which camera to use. Typically 0=left, 1=right, 2=auxiliary/RGB (OAK-D default)", type=int, default=2) 18 | p.add_argument("--mapLoadPath", help="SLAM map path", default=None) 19 | p.add_argument('--objLoadPath', help="Load scene as .obj", default=None) 20 | p.add_argument('--latitude', help="Scene coordinate system geographic origin (WGS84): latitude in degrees", default=None) 21 | p.add_argument('--longitude', help="Scene coordinate system geographic origin (WGS84): longitude in degrees", default=None) 22 | p.add_argument('--altitude', help="Scene coordinate system geographic origin (WGS84): altitude in meters", default=None) 23 | p.add_argument("--recordWindow", help="Window recording filename") 24 | return p.parse_args() 25 | 26 | if __name__ == '__main__': 27 | args = parseArgs() 28 | 29 | configInternal = {} 30 | if args.useRectification: 31 | configInternal["useRectification"] = "true" # Undistort images for visualization (assumes undistorted pinhole model) 32 | 33 | if args.mapLoadPath: 34 | configInternal["mapLoadPath"] = args.mapLoadPath 35 | configInternal["extendParameterSets"] = ["relocalization"] 36 | 37 | obj = None 38 | objPos = None # Position in WGS84 coordinates when GPS fusion is enabled 39 | if args.latitude and args.longitude and args.altitude: 40 | objPos = spectacularAI.WgsCoordinates() 41 | objPos.latitude = float(args.latitude) 42 | objPos.longitude = float(args.longitude) 43 | objPos.altitude = float(args.altitude) 44 | 45 | def renderObj(): 46 | global obj 47 | if obj is None: 48 | obj = load_obj(args.objLoadPath) 49 | 50 | glColor3f(1, 0, 1) 51 | glLineWidth(2.0) 52 | glCallList(obj) 53 | 54 | visArgs = VisualizerArgs() 55 | visArgs.recordPath = args.recordWindow 56 | visArgs.cameraMode = CameraMode.AR 57 | visArgs.showPoseTrail = False 58 | visArgs.showKeyFrames = False 59 | visArgs.showGrid = False 60 | visArgs.customRenderCallback = renderObj 61 | visualizer = Visualizer(visArgs) 62 | 63 | def replayOnVioOutput(output, frameSet): 64 | if output.globalPose and objPos == None: 65 | # If we receive global pose i.e. recording contains GPS coordinates, then 66 | # place object at the first received device coordinates if not provide 67 | # through CLI arguments 68 | objPos = output.globalPose.coordinates 69 | 70 | for frame in frameSet: 71 | if not frame.image: continue 72 | if not frame.index == args.cameraInd: continue 73 | img = frame.image.toArray() 74 | width = img.shape[1] 75 | height = img.shape[0] 76 | colorFormat = frame.image.getColorFormat() 77 | 78 | if output.globalPose: 79 | # Gives camera pose relative to objPos that sits at [0,0,0] in ENU 80 | cameraPose = output.globalPose.getEnuCameraPose(args.cameraInd, objPos) 81 | else: 82 | cameraPose = frame.cameraPose 83 | 84 | visualizer.onVioOutput(cameraPose, img, width, height, colorFormat, output.status) 85 | 86 | replay = spectacularAI.Replay(args.dataFolder, configuration=configInternal) 87 | replay.setExtendedOutputCallback(replayOnVioOutput) 88 | replay.startReplay() 89 | visualizer.run() 90 | replay.close() 91 | -------------------------------------------------------------------------------- /python/oak/pen_3d.py: -------------------------------------------------------------------------------- 1 | """ 2 | Draw in the air: cover the OAK-D color camera to activate the ink. 3 | Requirements: pip install matplotlib opencv-python 4 | """ 5 | import depthai 6 | import time 7 | import matplotlib.pyplot as plt 8 | import spectacularAI 9 | import threading 10 | import numpy as np 11 | 12 | SHOW_CAM = True 13 | if SHOW_CAM: 14 | import cv2 15 | 16 | def make_pipelines(): 17 | pipeline = depthai.Pipeline() 18 | vio_pipeline = spectacularAI.depthai.Pipeline(pipeline) 19 | 20 | RGB_OUTPUT_WIDTH = 200 # very small on purpose 21 | REF_ASPECT = 1920 / 1080.0 22 | w = RGB_OUTPUT_WIDTH 23 | h = int(round(w / REF_ASPECT)) 24 | 25 | camRgb = pipeline.createColorCamera() 26 | camRgb.setPreviewSize(w, h) 27 | camRgb.setResolution(depthai.ColorCameraProperties.SensorResolution.THE_1080_P) 28 | camRgb.setColorOrder(depthai.ColorCameraProperties.ColorOrder.BGR) 29 | camRgb.initialControl.setAutoFocusMode(depthai.RawCameraControl.AutoFocusMode.OFF) 30 | camRgb.initialControl.setManualFocus(0) # seems to be about 1m, not sure about the units 31 | camRgb.initialControl.setManualExposure(10000, 1000) 32 | out_source = camRgb.preview 33 | 34 | xout_camera = pipeline.createXLinkOut() 35 | xout_camera.setStreamName("rgb") 36 | out_source.link(xout_camera.input) 37 | 38 | return pipeline, vio_pipeline 39 | 40 | def make_camera_wireframe(aspect=640/400., scale=0.0025): 41 | # camera "frustum" 42 | corners = [[-1, -1], [1, -1], [1, 1], [-1, 1], [-1, -1]] 43 | cam_wire = [] 44 | for x, y in corners: 45 | cam_wire.append([x*aspect, y, 1]) 46 | for x, y in corners: 47 | cam_wire.append([x*aspect, y, 1]) 48 | cam_wire.append([0, 0, 0]) 49 | return (scale * np.array(cam_wire)).tolist() 50 | 51 | class MatplotlibVisualization: 52 | def __init__(self): 53 | from mpl_toolkits.mplot3d import Axes3D 54 | from matplotlib.animation import FuncAnimation 55 | 56 | self.ink_active = False 57 | self.prev_ink_active = False 58 | 59 | fig = plt.figure() 60 | ax = Axes3D(fig, auto_add_to_figure=False) 61 | fig.add_axes(ax) 62 | 63 | ax_bounds = (-0.5, 0.5) # meters 64 | ax.set(xlim=ax_bounds, ylim=ax_bounds, zlim=ax_bounds) 65 | ax.view_init(azim=-140) # initial plot orientation 66 | 67 | empty_xyz = lambda: { c: [] for c in 'xyz' } 68 | 69 | vio_data = empty_xyz() 70 | vio_data['plot'] = ax.plot( 71 | xs=[], ys=[], zs=[], 72 | linestyle="-", 73 | marker="", 74 | label='VIO trajectory' 75 | ) 76 | 77 | vio_cam_data = empty_xyz() 78 | vio_cam_data['plot'] = ax.plot( 79 | xs=[], ys=[], zs=[], 80 | linestyle="-", 81 | marker="", 82 | label='current cam pose' 83 | ) 84 | 85 | ax.legend() 86 | ax.set_xlabel("x (m)") 87 | ax.set_ylabel("y (m)") 88 | ax.set_zlabel("z (m)") 89 | 90 | def on_close(*args): 91 | self.should_close = True 92 | 93 | fig.canvas.mpl_connect('close_event', on_close) 94 | 95 | self.cam_wire = make_camera_wireframe() 96 | self.vio_data = vio_data 97 | self.vio_cam_data = vio_cam_data 98 | self.should_close = False 99 | 100 | def update_graph(*args): 101 | r = [] 102 | for graph in [self.vio_data, self.vio_cam_data]: 103 | p = graph['plot'][0] 104 | x, y, z = [np.array(graph[c]) for c in 'xyz'] 105 | p.set_data(x, y) 106 | p.set_3d_properties(z) 107 | r.append(p) 108 | return tuple(r) 109 | 110 | self._anim = FuncAnimation(fig, update_graph, interval=15, blit=True) 111 | 112 | def update_vio(self, vio_out): 113 | if self.should_close: return False 114 | view_mat = vio_out.pose.asMatrix() 115 | 116 | for c in 'xyz': self.vio_cam_data[c] = [] 117 | for vertex in self.cam_wire: 118 | p_local = np.array(vertex + [1]) 119 | p_world = (view_mat @ p_local)[:3] 120 | for i, c in enumerate('xyz'): 121 | self.vio_cam_data[c].append(p_world[i]) 122 | 123 | for c in 'xyz': 124 | if self.ink_active: 125 | self.vio_data[c].append(getattr(vio_out.pose.position, c)) 126 | elif not self.prev_ink_active: 127 | # NaN can be used to break lines in matplotlib 128 | self.vio_data[c].append(np.nan) 129 | 130 | self.prev_ink_active = self.ink_active 131 | 132 | return True 133 | 134 | def set_ink_active(self, active): 135 | self.ink_active = active 136 | 137 | def clear(self): 138 | self.ink_active = False 139 | for c in 'xyz': del self.vio_data[c][:] 140 | 141 | def start_in_parallel_with(self, parallel_thing): 142 | thread = threading.Thread(target = parallel_thing) 143 | thread.start() 144 | plt.show() 145 | thread.join() 146 | 147 | if __name__ == '__main__': 148 | pipeline, vio_pipeline = make_pipelines() 149 | 150 | with depthai.Device(pipeline) as device, \ 151 | vio_pipeline.startSession(device) as vio_session: 152 | 153 | visu_3d = MatplotlibVisualization() 154 | 155 | def main_loop(): 156 | rgbQueue = device.getOutputQueue(name="rgb", maxSize=4, blocking=False) 157 | 158 | lightness = 1.0 159 | while True: 160 | if vio_session.hasOutput(): 161 | vio_out = vio_session.getOutput() 162 | if not visu_3d.update_vio(vio_out): break 163 | elif rgbQueue.has(): 164 | rgbFrame = rgbQueue.get() 165 | 166 | lightness = np.max(rgbFrame.getRaw().data) / 255.0 167 | # print(lightness) 168 | 169 | THRESHOLD = 0.6 170 | visu_3d.set_ink_active(lightness < THRESHOLD) 171 | 172 | if SHOW_CAM: 173 | cv2.imshow("rgb", rgbFrame.getCvFrame()) 174 | 175 | cv_key = cv2.waitKey(1) 176 | if cv_key == ord('q'): 177 | break 178 | elif cv_key == ord('c'): # for clear 179 | visu_3d.clear() 180 | else: 181 | time.sleep(0.005) 182 | 183 | visu_3d.start_in_parallel_with(main_loop) 184 | -------------------------------------------------------------------------------- /python/oak/ros2/README.md: -------------------------------------------------------------------------------- 1 | # ROS wrapper 2 | 3 | ## Supported platforms 4 | 5 | * ROS2 Humble on Linux 6 | 7 | ## Dependencies 8 | 9 | ROS2 Hubmle framework: 10 | 11 | * https://docs.ros.org/en/dashing/Installation/Ubuntu-Install-Binary.html 12 | 13 | ## Build and Run 14 | 15 | Make sure to have your ROS environment sourced i.e. `ros2` on command line works and that you have OAK-D device connected 16 | 17 | From this folder, run: 18 | ``` 19 | colcon build 20 | source install/setup.bash 21 | ros2 launch launch/mapping.py 22 | ``` 23 | -------------------------------------------------------------------------------- /python/oak/ros2/launch/mapping.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch_ros.actions import Node 3 | from launch.actions import DeclareLaunchArgument, OpaqueFunction 4 | from launch.substitutions import LaunchConfiguration 5 | from launch.conditions import IfCondition 6 | 7 | 8 | def launch_setup(context, *args, **kwargs): 9 | spectacularai_node = Node( 10 | package='spectacularai_depthai', 11 | executable='ros2_node', 12 | parameters=[ 13 | { 'recordingFolder': LaunchConfiguration("recordingFolder") }, 14 | ], 15 | ) 16 | 17 | rviz_node = Node( 18 | condition=IfCondition(LaunchConfiguration("use_rviz").perform(context)), 19 | package='rviz2', executable='rviz2', output='screen', 20 | arguments=['--display-config', 'launch/mapping.rviz']) 21 | 22 | return [ 23 | spectacularai_node, 24 | rviz_node 25 | ] 26 | 27 | 28 | def generate_launch_description(): 29 | return LaunchDescription( 30 | [ 31 | DeclareLaunchArgument("use_rviz", default_value='True'), 32 | DeclareLaunchArgument("recordingFolder", default_value='') 33 | ] + [ 34 | OpaqueFunction(function=launch_setup) 35 | ] 36 | ) 37 | -------------------------------------------------------------------------------- /python/oak/ros2/launch/mapping.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5029411911964417 10 | Tree Height: 1561 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: PointCloud2 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 0.5 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 1 51 | Axes Length: 0.10000000149011612 52 | Axes Radius: 0.029999999329447746 53 | Class: rviz_default_plugins/Pose 54 | Color: 255; 25; 0 55 | Enabled: true 56 | Head Length: 0.30000001192092896 57 | Head Radius: 0.10000000149011612 58 | Name: Pose 59 | Shaft Length: 1 60 | Shaft Radius: 0.05000000074505806 61 | Shape: Axes 62 | Topic: 63 | Depth: 5 64 | Durability Policy: Volatile 65 | Filter size: 10 66 | History Policy: Keep Last 67 | Reliability Policy: Reliable 68 | Value: /slam/odometry 69 | Value: true 70 | - Alpha: 1 71 | Autocompute Intensity Bounds: true 72 | Autocompute Value Bounds: 73 | Max Value: 10 74 | Min Value: -10 75 | Value: true 76 | Axis: Z 77 | Channel Name: intensity 78 | Class: rviz_default_plugins/PointCloud2 79 | Color: 255; 255; 255 80 | Color Transformer: Intensity 81 | Decay Time: 10 82 | Enabled: true 83 | Invert Rainbow: false 84 | Max Color: 255; 255; 255 85 | Max Intensity: 4096 86 | Min Color: 0; 0; 0 87 | Min Intensity: 0 88 | Name: PointCloud2 89 | Position Transformer: XYZ 90 | Selectable: true 91 | Size (Pixels): 3 92 | Size (m): 0.009999999776482582 93 | Style: Flat Squares 94 | Topic: 95 | Depth: 5 96 | Durability Policy: Volatile 97 | Filter size: 10 98 | History Policy: Keep Last 99 | Reliability Policy: Reliable 100 | Value: /slam/pointcloud 101 | Use Fixed Frame: true 102 | Use rainbow: true 103 | Value: true 104 | - Class: rviz_default_plugins/Image 105 | Enabled: true 106 | Max Value: 1 107 | Median window: 5 108 | Min Value: 0 109 | Name: Image 110 | Normalize Range: true 111 | Topic: 112 | Depth: 5 113 | Durability Policy: Volatile 114 | History Policy: Keep Last 115 | Reliability Policy: Reliable 116 | Value: /slam/left 117 | Value: true 118 | Enabled: true 119 | Global Options: 120 | Background Color: 48; 48; 48 121 | Fixed Frame: world 122 | Frame Rate: 30 123 | Name: root 124 | Tools: 125 | - Class: rviz_default_plugins/Interact 126 | Hide Inactive Objects: true 127 | - Class: rviz_default_plugins/MoveCamera 128 | - Class: rviz_default_plugins/Select 129 | - Class: rviz_default_plugins/FocusCamera 130 | - Class: rviz_default_plugins/Measure 131 | Line color: 128; 128; 0 132 | - Class: rviz_default_plugins/SetInitialPose 133 | Covariance x: 0.25 134 | Covariance y: 0.25 135 | Covariance yaw: 0.06853891909122467 136 | Topic: 137 | Depth: 5 138 | Durability Policy: Volatile 139 | History Policy: Keep Last 140 | Reliability Policy: Reliable 141 | Value: /initialpose 142 | - Class: rviz_default_plugins/SetGoal 143 | Topic: 144 | Depth: 5 145 | Durability Policy: Volatile 146 | History Policy: Keep Last 147 | Reliability Policy: Reliable 148 | Value: /goal_pose 149 | - Class: rviz_default_plugins/PublishPoint 150 | Single click: true 151 | Topic: 152 | Depth: 5 153 | Durability Policy: Volatile 154 | History Policy: Keep Last 155 | Reliability Policy: Reliable 156 | Value: /clicked_point 157 | Transformation: 158 | Current: 159 | Class: rviz_default_plugins/TF 160 | Value: true 161 | Views: 162 | Current: 163 | Class: rviz_default_plugins/Orbit 164 | Distance: 10 165 | Enable Stereo Rendering: 166 | Stereo Eye Separation: 0.05999999865889549 167 | Stereo Focal Distance: 1 168 | Swap Stereo Eyes: false 169 | Value: false 170 | Focal Point: 171 | X: 0 172 | Y: 0 173 | Z: 0 174 | Focal Shape Fixed Size: true 175 | Focal Shape Size: 0.05000000074505806 176 | Invert Z Axis: false 177 | Name: Current View 178 | Near Clip Distance: 0.009999999776482582 179 | Pitch: 0.13479739427566528 180 | Target Frame: 181 | Value: Orbit (rviz) 182 | Yaw: 3.118558645248413 183 | Saved: ~ 184 | Window Geometry: 185 | Displays: 186 | collapsed: false 187 | Height: 2094 188 | Hide Left Dock: false 189 | Hide Right Dock: false 190 | Image: 191 | collapsed: false 192 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000790fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000006a4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000006e7000000e60000002800ffffff000000010000010f00000790fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000790000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000eb40000003efc0100000002fb0000000800540069006d0065010000000000000eb4000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000c430000079000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 193 | Selection: 194 | collapsed: false 195 | Time: 196 | collapsed: false 197 | Tool Properties: 198 | collapsed: false 199 | Views: 200 | collapsed: false 201 | Width: 3764 202 | X: 74 203 | Y: 27 204 | -------------------------------------------------------------------------------- /python/oak/ros2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | spectacularai_depthai 5 | 0.0.1 6 | Spectacular AI plugin for DepthAI 7 | Spectacular AI 8 | All Rights Reserved 9 | 10 | rclpy 11 | sensor_msgs 12 | geometry_msgs 13 | 14 | ament_copyright 15 | ament_flake8 16 | ament_pep257 17 | python3-pytest 18 | 19 | 20 | ament_python 21 | 22 | 23 | -------------------------------------------------------------------------------- /python/oak/ros2/requirements.txt: -------------------------------------------------------------------------------- 1 | depthai>=2.22.0.0 2 | numpy>=1.21.4 3 | spectacularAI>=1.22.0 4 | -------------------------------------------------------------------------------- /python/oak/ros2/resource/spectacularai_depthai: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SpectacularAI/sdk-examples/d6e3730ca13a234a14a36f33c242342501417405/python/oak/ros2/resource/spectacularai_depthai -------------------------------------------------------------------------------- /python/oak/ros2/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/spectacularai_depthai 3 | [install] 4 | install_scripts=$base/lib/spectacularai_depthai 5 | -------------------------------------------------------------------------------- /python/oak/ros2/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'spectacularai_depthai' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.1', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='Spectacular AI', 17 | maintainer_email='firstname.lastname@spectacularai.com', 18 | description='Spectacular AI plugin for DepthAI', 19 | license='All Rights Reserved', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | 'ros2_node = spectacularai_depthai.ros2_node:main' 24 | ], 25 | }, 26 | ) 27 | -------------------------------------------------------------------------------- /python/oak/ros2/spectacularai_depthai/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/SpectacularAI/sdk-examples/d6e3730ca13a234a14a36f33c242342501417405/python/oak/ros2/spectacularai_depthai/__init__.py -------------------------------------------------------------------------------- /python/oak/ros2/spectacularai_depthai/ros2_node.py: -------------------------------------------------------------------------------- 1 | """ 2 | Licensed under the Apache License, Version 2.0 (the "License"); 3 | you may not use this file except in compliance with the License. 4 | You may obtain a copy of the License at 5 | 6 | http://www.apache.org/licenses/LICENSE-2.0 7 | 8 | Unless required by applicable law or agreed to in writing, software 9 | distributed under the License is distributed on an "AS IS" BASIS, 10 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 11 | See the License for the specific language governing permissions and 12 | limitations under the License. 13 | 14 | Spectacular AI ROS2 node that manages the OAK-D device through DepthAI Python API 15 | """ 16 | 17 | import spectacularAI 18 | import depthai 19 | import numpy as np 20 | 21 | from geometry_msgs.msg import PoseStamped, TransformStamped 22 | from tf2_msgs.msg import TFMessage 23 | from cv_bridge import CvBridge 24 | from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField 25 | from builtin_interfaces.msg import Time 26 | 27 | import rclpy 28 | from rclpy.node import Node 29 | 30 | PUBLISHER_QUEUE_SIZE = 10 31 | 32 | def toRosTime(timeInSeconds): 33 | t = Time() 34 | t.sec = int(timeInSeconds) 35 | t.nanosec = int((timeInSeconds % 1) * 1e9) 36 | return t 37 | 38 | 39 | def toPoseMessage(cameraPose, ts): 40 | msg = PoseStamped() 41 | msg.header.stamp = ts 42 | msg.header.frame_id = "world" 43 | msg.pose.position.x = cameraPose.position.x 44 | msg.pose.position.y = cameraPose.position.y 45 | msg.pose.position.z = cameraPose.position.z 46 | msg.pose.orientation.x = cameraPose.orientation.x 47 | msg.pose.orientation.y = cameraPose.orientation.y 48 | msg.pose.orientation.z = cameraPose.orientation.z 49 | msg.pose.orientation.w = cameraPose.orientation.w 50 | return msg 51 | 52 | 53 | def toTfMessage(cameraPose, ts, frame_id): 54 | msg = TFMessage() 55 | msg.transforms = [] 56 | transform = TransformStamped() 57 | transform.header.stamp = ts 58 | transform.header.frame_id = "world" 59 | transform.child_frame_id = frame_id 60 | transform.transform.translation.x = cameraPose.position.x 61 | transform.transform.translation.y = cameraPose.position.y 62 | transform.transform.translation.z = cameraPose.position.z 63 | transform.transform.rotation.x = cameraPose.orientation.x 64 | transform.transform.rotation.y = cameraPose.orientation.y 65 | transform.transform.rotation.z = cameraPose.orientation.z 66 | transform.transform.rotation.w = cameraPose.orientation.w 67 | msg.transforms.append(transform) 68 | return msg 69 | 70 | 71 | def toCameraInfoMessage(camera, frame, ts): 72 | intrinsic = camera.getIntrinsicMatrix() 73 | msg = CameraInfo() 74 | msg.header.stamp = ts 75 | msg.header.frame_id = "left_camera" 76 | msg.height = frame.shape[0] 77 | msg.width = frame.shape[1] 78 | msg.distortion_model = "none" 79 | msg.d = [] 80 | msg.k = intrinsic.ravel().tolist() 81 | return msg 82 | 83 | 84 | class SpectacularAINode(Node): 85 | def __init__(self): 86 | super().__init__("spectacular_ai_node") 87 | self.declare_parameter('recordingFolder', rclpy.Parameter.Type.STRING) 88 | 89 | self.odometry_publisher = self.create_publisher(PoseStamped, "/slam/odometry", PUBLISHER_QUEUE_SIZE) 90 | self.keyframe_publisher = self.create_publisher(PoseStamped, "/slam/keyframe", PUBLISHER_QUEUE_SIZE) 91 | self.left_publisher = self.create_publisher(Image, "/slam/left", PUBLISHER_QUEUE_SIZE) 92 | self.tf_publisher = self.create_publisher(TFMessage, "/tf", PUBLISHER_QUEUE_SIZE) 93 | self.point_publisher = self.create_publisher(PointCloud2, "/slam/pointcloud", PUBLISHER_QUEUE_SIZE) 94 | self.camera_info_publisher = self.create_publisher(CameraInfo, "/slam/camera_info", PUBLISHER_QUEUE_SIZE) 95 | self.bridge = CvBridge() 96 | self.keyframes = {} 97 | self.latestOutputTimestamp = None 98 | 99 | self.pipeline = depthai.Pipeline() 100 | config = spectacularAI.depthai.Configuration() 101 | 102 | recordingFolder = str(self.get_parameter('recordingFolder').value) 103 | if recordingFolder: 104 | self.get_logger().info("Recording: " + recordingFolder) 105 | config.recordingFolder = recordingFolder 106 | config.recordingOnly = True 107 | 108 | config.internalParameters = { 109 | "ffmpegVideoCodec": "libx264 -crf 15 -preset ultrafast", 110 | "computeStereoPointCloud": "true", 111 | "computeDenseStereoDepthKeyFramesOnly": "true" 112 | } 113 | config.useSlam = True 114 | 115 | self.get_logger().info("Starting VIO") # Example of logging. 116 | self.vio_pipeline = spectacularAI.depthai.Pipeline(self.pipeline, config, self.onMappingOutput) 117 | self.device = depthai.Device(self.pipeline) 118 | self.vio_session = self.vio_pipeline.startSession(self.device) 119 | self.timer = self.create_timer(0, self.processOutput) 120 | 121 | 122 | def processOutput(self): 123 | while self.vio_session.hasOutput(): 124 | self.onVioOutput(self.vio_session.getOutput()) 125 | 126 | 127 | def onVioOutput(self, vioOutput): 128 | timestamp = toRosTime(vioOutput.getCameraPose(0).pose.time) 129 | self.latestOutputTimestamp = timestamp 130 | cameraPose = vioOutput.getCameraPose(0).pose 131 | self.odometry_publisher.publish(toPoseMessage(cameraPose, timestamp)) 132 | self.tf_publisher.publish(toTfMessage(cameraPose, timestamp, "left_camera")) 133 | 134 | 135 | def onMappingOutput(self, output): 136 | for frame_id in output.updatedKeyFrames: 137 | keyFrame = output.map.keyFrames.get(frame_id) 138 | if not keyFrame: continue # Deleted keyframe 139 | if not keyFrame.pointCloud: continue 140 | if not self.hasKeyframe(frame_id): 141 | self.newKeyFrame(frame_id, keyFrame) 142 | 143 | 144 | def hasKeyframe(self, frame_id): 145 | return frame_id in self.keyframes 146 | 147 | 148 | def newKeyFrame(self, frame_id, keyframe): 149 | if not self.latestOutputTimestamp: return 150 | timestamp = toRosTime(keyframe.frameSet.primaryFrame.cameraPose.pose.time) 151 | self.keyframes[frame_id] = True 152 | msg = toPoseMessage(keyframe.frameSet.primaryFrame.cameraPose.pose, timestamp) 153 | msg.header.stamp = timestamp 154 | self.keyframe_publisher.publish(msg) 155 | 156 | left_frame_bitmap = keyframe.frameSet.primaryFrame.image.toArray() 157 | left_msg = self.bridge.cv2_to_imgmsg(left_frame_bitmap, encoding="mono8") 158 | left_msg.header.stamp = timestamp 159 | left_msg.header.frame_id = "left_camera" 160 | self.left_publisher.publish(left_msg) 161 | 162 | camera = keyframe.frameSet.primaryFrame.cameraPose.camera 163 | info_msg = toCameraInfoMessage(camera, left_frame_bitmap, timestamp) 164 | self.camera_info_publisher.publish(info_msg) 165 | 166 | self.publishPointCloud(keyframe, timestamp) 167 | 168 | 169 | # NOTE This seems a bit slow. 170 | def publishPointCloud(self, keyframe, timestamp): 171 | camToWorld = keyframe.frameSet.rgbFrame.cameraPose.getCameraToWorldMatrix() 172 | positions = keyframe.pointCloud.getPositionData() 173 | pc = np.zeros((positions.shape[0], 6), dtype=np.float32) 174 | p_C = np.vstack((positions.T, np.ones((1, positions.shape[0])))).T 175 | pc[:, :3] = (camToWorld @ p_C[:, :, None])[:, :3, 0] 176 | 177 | msg = PointCloud2() 178 | msg.header.stamp = timestamp 179 | msg.header.frame_id = "world" 180 | if keyframe.pointCloud.hasColors(): 181 | pc[:, 3:] = keyframe.pointCloud.getRGB24Data() * (1. / 255.) 182 | msg.point_step = 4 * 6 183 | msg.height = 1 184 | msg.width = pc.shape[0] 185 | msg.row_step = msg.point_step * pc.shape[0] 186 | msg.data = pc.tobytes() 187 | msg.is_bigendian = False 188 | msg.is_dense = False 189 | ros_dtype = PointField.FLOAT32 190 | itemsize = np.dtype(np.float32).itemsize 191 | msg.fields = [PointField(name=n, offset=i*itemsize, datatype=ros_dtype, count=1) for i, n in enumerate('xyzrgb')] 192 | self.point_publisher.publish(msg) 193 | 194 | 195 | def main(args=None): 196 | rclpy.init(args=args) 197 | sai_node = SpectacularAINode() 198 | rclpy.spin(sai_node) 199 | sai_node.destroy_node() 200 | rclpy.shutdown() 201 | 202 | 203 | if __name__ == '__main__': 204 | main() 205 | -------------------------------------------------------------------------------- /python/oak/ros2/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /python/oak/ros2/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /python/oak/ros2/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /python/oak/vio_hooks.py: -------------------------------------------------------------------------------- 1 | """ 2 | An example of accessing the raw data feeds from DepthAI that the SDK uses through 3 | the Spectacualar AI hooks API. It's important to note that these are only called 4 | if the particular mode the Spetacular AI SDK is running in uses that data feed 5 | """ 6 | 7 | import depthai 8 | import spectacularAI 9 | import cv2 10 | import numpy as np 11 | 12 | pipeline = depthai.Pipeline() 13 | 14 | config = spectacularAI.depthai.Configuration() 15 | config.useSlam = True 16 | 17 | vio_pipeline = spectacularAI.depthai.Pipeline(pipeline, config) 18 | 19 | featureBuffer = None 20 | 21 | def onImageFactor(name): 22 | def onImage(img): 23 | global featureBuffer 24 | if img.getWidth() <= 0 or img.getHeight() <= 0: 25 | # When SLAM is enabled, monocular frames are only used at 1/6th of normal frame rate, 26 | # rest of the frames are [0,0] in size and must be filtered 27 | return 28 | if type(featureBuffer) is not np.ndarray: featureBuffer = np.zeros((img.getHeight(), img.getWidth(), 1), dtype = "uint8") 29 | cv2.imshow(name, img.getCvFrame()) 30 | if cv2.waitKey(1) == ord("q"): 31 | exit(0) 32 | return onImage 33 | 34 | def onImuData(imuData): 35 | for imuPacket in imuData.packets: 36 | acceleroValues = imuPacket.acceleroMeter 37 | gyroValues = imuPacket.gyroscope 38 | acceleroTs = acceleroValues.getTimestampDevice().total_seconds() * 1000 39 | gyroTs = gyroValues.getTimestampDevice().total_seconds() * 1000 40 | imuF = "{:.06f}" 41 | tsF = "{:.03f}" 42 | print(f"Accelerometer timestamp: {tsF.format(acceleroTs)} ms") 43 | print(f"Accelerometer [m/s^2]: x: {imuF.format(acceleroValues.x)} y: {imuF.format(acceleroValues.y)} z: {imuF.format(acceleroValues.z)}") 44 | print(f"Gyroscope timestamp: {tsF.format(gyroTs)} ms") 45 | print(f"Gyroscope [rad/s]: x: {imuF.format(gyroValues.x)} y: {imuF.format(gyroValues.y)} z: {imuF.format(gyroValues.z)} ") 46 | 47 | def onFeatures(features): 48 | global featureBuffer 49 | if type(featureBuffer) is not np.ndarray: return 50 | featureBuffer[:] = 0 51 | for feature in features.trackedFeatures: 52 | cv2.circle(featureBuffer, (int(feature.position.x), int(feature.position.y)), 2, 255, -1, cv2.LINE_AA, 0) 53 | cv2.imshow("Features", featureBuffer) 54 | if cv2.waitKey(1) == ord("q"): 55 | exit(0) 56 | 57 | vio_pipeline.hooks.imu = onImuData 58 | vio_pipeline.hooks.monoPrimary = onImageFactor("Primary") 59 | vio_pipeline.hooks.monoSecondary = onImageFactor("Secondary") 60 | vio_pipeline.hooks.depth = onImageFactor("Depth") 61 | vio_pipeline.hooks.trackedFeatures = onFeatures 62 | # In default mode the color is not used by the SDK, so this will never get called. 63 | # see mixed_reality.py example how to read the color data in an efficient manner. 64 | vio_pipeline.hooks.color = onImageFactor("Color") 65 | 66 | with depthai.Device(pipeline) as device, \ 67 | vio_pipeline.startSession(device) as vio_session: 68 | 69 | while True: 70 | out = vio_session.waitForOutput() 71 | print(out.asJson()) 72 | -------------------------------------------------------------------------------- /python/oak/vio_jsonl.py: -------------------------------------------------------------------------------- 1 | import depthai 2 | import spectacularAI 3 | import time 4 | 5 | pipeline = depthai.Pipeline() 6 | 7 | vio_pipeline = spectacularAI.depthai.Pipeline(pipeline) 8 | # optional config args: vio_pipeline = spectacularAI.depthai.Pipeline(pipeline, config, useStereo=False) 9 | 10 | with depthai.Device(pipeline) as device, \ 11 | vio_pipeline.startSession(device) as vio_session: 12 | 13 | while True: 14 | out = vio_session.waitForOutput() 15 | print(out.asJson()) 16 | -------------------------------------------------------------------------------- /python/oak/vio_record.py: -------------------------------------------------------------------------------- 1 | """Record data for later playback from OAK devices""" 2 | 3 | DEPRECATION_NOTE = """ 4 | Note: the oak/vio_record.py script has been replaced by the sai-cli 5 | tool in Spectacular AI Python package v1.25. Prefer 6 | 7 | sai-cli record oak [args] 8 | 9 | as a drop-in replacement of 10 | 11 | python vio_record.py [args] 12 | 13 | For more information, type: sai-cli record oak --help 14 | """ 15 | 16 | # The code is still available and usable as a stand-alone script, see: 17 | # https://github.com/SpectacularAI/sdk/blob/main/python/cli/record/oak.py 18 | 19 | import_success = False 20 | try: 21 | from spectacularAI.cli.record.oak import record, define_args 22 | import_success = True 23 | except ImportError as e: 24 | print(e) 25 | 26 | if not import_success: 27 | msg = """ 28 | 29 | Unable to import new Spectacular AI CLI, please update to SDK version >= 1.25" 30 | """ 31 | raise RuntimeError(msg) 32 | 33 | if __name__ == '__main__': 34 | import argparse 35 | parser = argparse.ArgumentParser( 36 | description=__doc__, 37 | epilog=DEPRECATION_NOTE, 38 | formatter_class=argparse.RawDescriptionHelpFormatter) 39 | define_args(parser) 40 | print(DEPRECATION_NOTE) 41 | record(parser.parse_args()) -------------------------------------------------------------------------------- /python/oak/vio_replay.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import spectacularAI 3 | import cv2 4 | 5 | if __name__ == '__main__': 6 | p = argparse.ArgumentParser(__doc__) 7 | p.add_argument("dataFolder", help="Folder containing the recorded session for replay", default="data") 8 | p.add_argument("--preview", help="Show latest primary image as a preview", action="store_true") 9 | args = p.parse_args() 10 | 11 | def onOutput(output, frameSet): 12 | for frame in frameSet: 13 | if args.preview and frame.image: 14 | cv2.imshow("Camera #{}".format(frame.index), cv2.cvtColor(frame.image.toArray(), cv2.COLOR_RGB2BGR)) 15 | cv2.waitKey(1) 16 | print(output.asJson()) 17 | 18 | replay = spectacularAI.Replay(args.dataFolder) 19 | # If frameSet isn't used, it's better to use setOutputCallback(...) which is lighter 20 | # replay.setOutputCallback(onOutput) 21 | replay.setExtendedOutputCallback(onOutput) 22 | replay.runReplay() 23 | -------------------------------------------------------------------------------- /python/oak/vio_visu.py: -------------------------------------------------------------------------------- 1 | """ 2 | Simple VIO result visualizer Python. Reads outputs from the 3 | Spectacular AI OAK-D plugin and plots them in real time. 4 | Plug in the OAK-D to an USB3 port using an USB3 cable before running. 5 | 6 | Can also visualize pre-recorded results using Replay API, or from a JSONL file or from a pipe. 7 | The device does not have to be attached in this case. (See vio_record.py) 8 | """ 9 | import time 10 | import json 11 | import threading 12 | import matplotlib.pyplot as plt 13 | 14 | def live_vio_reader(): 15 | import depthai 16 | import spectacularAI 17 | pipeline = depthai.Pipeline() 18 | vio_pipeline = spectacularAI.depthai.Pipeline(pipeline) 19 | 20 | with depthai.Device(pipeline) as device, \ 21 | vio_pipeline.startSession(device) as vio_session: 22 | 23 | while True: 24 | out = vio_session.waitForOutput() 25 | yield(json.loads(out.asJson())) 26 | 27 | def replay_vio_reader(replay): 28 | outputs = [] 29 | def onOutput(out): 30 | outputs.append(out.asJson()) 31 | 32 | replay.setOutputCallback(onOutput) 33 | replay.startReplay() 34 | 35 | while True: 36 | if outputs: 37 | out = outputs.pop(0) 38 | yield(json.loads(out)) 39 | time.sleep(0.01) 40 | 41 | def file_vio_reader(in_stream): 42 | while True: 43 | line = in_stream.readline() 44 | if not line: break 45 | try: 46 | d = json.loads(line) 47 | if 'position' not in d and 'pose' not in d: continue 48 | yield(d) 49 | except: 50 | # Ignore all lines that aren't valid json 51 | pass 52 | 53 | def make_plotter(): 54 | import numpy as np 55 | from mpl_toolkits.mplot3d import Axes3D 56 | 57 | fig = plt.figure() 58 | ax = Axes3D(fig) 59 | fig.add_axes(ax) 60 | 61 | ax_bounds = (-0.5, 0.5) # meters 62 | ax.set(xlim=ax_bounds, ylim=ax_bounds, zlim=ax_bounds) 63 | ax.view_init(azim=-140) # initial plot orientation 64 | 65 | vio_plot = ax.plot( 66 | xs=[], ys=[], zs=[], 67 | linestyle="-", 68 | marker="" 69 | ) 70 | ax.set_xlabel("x (m)") 71 | ax.set_ylabel("y (m)") 72 | ax.set_zlabel("z (m)") 73 | 74 | title = ax.set_title("VIO trajectory") 75 | 76 | data = { c: [] for c in 'xyz' } 77 | 78 | control = { 'close': False } 79 | fig.canvas.mpl_connect('close_event', lambda _: control.update({'close': True})) 80 | 81 | def update_data(vio_out): 82 | if control['close']: return False 83 | # supports two slightly different JSONL formats 84 | if 'pose' in vio_out: vio_out = vio_out['pose'] 85 | # SDK < 0.12 does not expose the TRACKING status 86 | is_tracking = vio_out.get('status', 'TRACKING') == 'TRACKING' 87 | for c in 'xyz': 88 | val = vio_out['position'][c] 89 | if not is_tracking: val = np.nan 90 | data[c].append(val) 91 | return True 92 | 93 | def update_graph(frames): 94 | x, y, z = [np.array(data[c]) for c in 'xyz'] 95 | vio_plot[0].set_data(x, y) 96 | vio_plot[0].set_3d_properties(z) 97 | return (vio_plot[0],) 98 | 99 | from matplotlib.animation import FuncAnimation 100 | anim = FuncAnimation(fig, update_graph, interval=15, blit=True) 101 | return update_data, anim 102 | 103 | if __name__ == '__main__': 104 | plotter, anim = make_plotter() 105 | import argparse 106 | parser = argparse.ArgumentParser(__doc__) 107 | parser.add_argument("--dataFolder", help="Instead of running live mapping session, replay session from this folder") 108 | parser.add_argument('--file', type=argparse.FileType('r'), 109 | help='Read data from a JSONL file or pipe instead of displaying it live', 110 | default=None) 111 | 112 | args = parser.parse_args() 113 | 114 | def reader_loop(): 115 | replay = None 116 | if args.dataFolder: 117 | import spectacularAI 118 | replay = spectacularAI.Replay(args.dataFolder) 119 | vio_source = replay_vio_reader(replay) 120 | elif args.file: 121 | vio_source = file_vio_reader(args.file) 122 | else: 123 | vio_source = live_vio_reader() 124 | 125 | for vio_out in vio_source: 126 | if not plotter(vio_out): break 127 | if replay: replay.close() 128 | 129 | reader_thread = threading.Thread(target = reader_loop) 130 | reader_thread.start() 131 | plt.show() 132 | reader_thread.join() 133 | 134 | --------------------------------------------------------------------------------