diff --git a/LICENSES/BSD-3-Clause-Rectus.txt b/LICENSES/BSD-3-Clause-Rectus.txt new file mode 100644 index 000000000..d08aa70e9 --- /dev/null +++ b/LICENSES/BSD-3-Clause-Rectus.txt @@ -0,0 +1,28 @@ + +Copyright (c) 2026, Rectus +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this +list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/README-monado.md b/README-monado.md new file mode 100644 index 000000000..a23335933 --- /dev/null +++ b/README-monado.md @@ -0,0 +1,278 @@ +# Monado - XR Runtime (XRT) + + + +> * Main homepage and documentation: +> * Promotional homepage: +> * Maintained at +> * Latest API documentation: +> * Continuously-updated changelog of the default branch: +> + +Monado is an open source XR runtime delivering immersive experiences such as VR +and AR on mobile, PC/desktop, and any other device +(because gosh darn people +come up with a lot of weird hardware). +Monado aims to be a complete and conforming implementation +of the OpenXR API made by Khronos. +The project is primarily developed on GNU/Linux, but also runs on Android and Windows. +"Monado" has no specific meaning and is just a name. + +## Monado source tree + +* `src/xrt/include` - headers that define the internal interfaces of Monado. +* `src/xrt/compositor` - code for doing distortion and driving the display hardware of a device. +* `src/xrt/auxiliary` - utilities and other larger components. +* `src/xrt/drivers` - hardware drivers. +* `src/xrt/state_trackers/oxr` - OpenXR API implementation. +* `src/xrt/targets` - glue code and build logic to produce final binaries. +* `src/external` - a small collection of external code and headers. + +## Getting Started + +Dependencies include: + +* [CMake][] 3.13 or newer (Note Ubuntu 18.04 only has 3.10) +* Python 3.6 or newer +* Vulkan headers and loader - Fedora package `vulkan-loader-devel` +* OpenGL headers +* Eigen3 - Debian/Ubuntu package `libeigen3-dev` +* glslangValidator - Debian/Ubuntu package `glslang-tools`, Fedora package `glslang`. +* libusb +* libudev - Debian/Ubuntu package `libudev-dev`, Fedora package `systemd-devel` +* Video 4 Linux - Debian/Ubuntu package `libv4l-dev`. + +Optional (but recommended) dependencies: + +* libxcb and xcb-xrandr development packages +* [OpenHMD][] 0.3.0 or newer (found using pkg-config) + +Truly optional dependencies, useful for some drivers, app support, etc.: + +* Doxygen - Debian/Ubuntu package `doxygen` and `graphviz` +* Wayland development packages +* Xlib development packages +* libhidapi - Debian/Ubuntu package `libhidapi-dev` +* OpenCV +* libuvc - Debian/Ubuntu package `libuvc-dev` +* libjpeg +* libbluetooth - Debian/Ubuntu package `libbluetooth-dev` +* libsdl - Debian/Ubuntu package `libsdl2-dev` + +Experimental Windows support requires the Vulkan SDK and also needs or works +best with the following vcpkg packages installed: + +* pthreads eigen3 libusb hidapi zlib doxygen + +If you have a recent [vcpkg](https://vcpkg.io) installed and use the appropriate +CMake toolchain file, the vcpkg manifest in the Monado repository will instruct +vcpkg to locally install the dependencies automatically. The Vulkan SDK +installer should set the `VULKAN_SDK` Windows environment variable to point +at the installation location (for example, `C:/VulkanSDK/1.3.250.1`), though +make sure you open a new terminal (or open the CMake GUI) *after* doing that +install to make sure it is available. + +Monado has been tested on these distributions, but is expected to work on almost +any modern distribution. + +* Ubuntu 24.04, 22.04, 20.04, (18.04 may not be fully supported) +* Debian 11 `bookworm`, 10 `buster` + * Up-to-date package lists can be found in our CI config file, + `.gitlab-ci.yml` +* Archlinux + +These distributions include recent-enough versions of all the +software to use direct mode, +without using any external, third-party, or backported +package sources. + +See also [Status of DRM Leases][drm-lease] +for more details on specific packages, versions, and commits. + +Due to the lack of a OpenGL extension: GL_EXT_memory_object_fd on Intel's +OpenGL driver, only the AMD +radeonsi driver and the proprietary NVIDIA driver will work for OpenGL OpenXR +clients. This is due to a requirement of the Compositor. Support status of the +extension can be found on the [mesamatrix website][mesamatrix-ext]. + +### CMake + +Build process is similar to other CMake builds, +so something like the following will build it. + +Go into the source directory, create a build directory, +and change into it. + +```bash +mkdir build +cd build +``` + +Then, invoke [CMake to generate a project][cmake-generate]. +Feel free to change the build type or generator ("Ninja" is fast and parallel) as you see fit. + +```bash +cmake .. -DCMAKE_BUILD_TYPE=Debug -G "Unix Makefiles" +``` + +If you plan to install the runtime, +append something like `-DCMAKE_INSTALL_PREFIX=~/.local` +to specify the root of the install directory. +(The default install prefix is `/usr/local`.) + +To build, [the generic CMake build commands][cmake-build] below will work on all systems, +though you can manually invoke your build tool (`make`, `ninja`, etc.) if you prefer. +The first command builds the runtime and docs, +and the second, which is optional, installs the runtime under `${CMAKE_INSTALL_PREFIX}`. + +```bash +cmake --build . +cmake --build . --target install +``` + +Alternately, if using Make, the following will build the runtime and docs, then install. +Replace `make` with `ninja` if you used the Ninja generator. + +```bash +make +make install +``` + +Documentation can be browsed by opening `doc/html/index.html` in the build directory in a web browser. + +## Getting started using OpenXR with Monado + +This implements the [OpenXR][] API, +so to do anything with it, you'll need an application +that uses OpenXR, along with the OpenXR loader. +The OpenXR loader is a glue library that connects OpenXR applications to OpenXR runtimes such as Monado +It determines which runtime to use by looking for the config file `active_runtime.json` (either a symlink to +or a copy of a runtime manifest) in the usual XDG config paths +and processes environment variables such as `XR_RUNTIME_JSON=/usr/share/openxr/0/openxr_monado.json`. +It can also insert OpenXR API Layers without the application or the runtime having to do it. + +You can use the `hello_xr` sample provided with the OpenXR SDK. + +The OpenXR loader can be pointed to a runtime json file in a nonstandard location with the environment variable `XR_RUNTIME_JSON`. Example: + +```bash +XR_RUNTIME_JSON=~/monado/build/openxr_monado-dev.json ./openxr-example +``` + +For ease of development Monado creates a runtime manifest file in its build directory using an absolute path to the +Monado runtime in the build directory called `openxr_monado-dev.json`. Pointing `XR_RUNTIME_JSON` to this +file allows using Monado after building, without installing. + +Note that the loader can always find and load the runtime +if the path to the runtime library given in the json manifest is an absolute path, +but if a relative path like `libopenxr_monado.so.0` is given, +then `LD_LIBRARY_PATH` must include the directory that contains `libopenxr_monado.so.0`. +The absolute path in `openxr_monado-dev.json` takes care of this for you. + +Distribution packages for monado may provide the "active runtime" file `/etc/xdg/openxr/1/active_runtime.json`. +In this case the loader will automatically use Monado when starting an OpenXR application. This global configuration +can be overridden on a per user basis by creating `~/.config/openxr/1/active_runtime.json`. + +## Direct mode + +On AMD and Intel GPUs our direct mode code requires a connected HMD to have +the `non-desktop` xrandr property set to 1. +Only the most common HMDs have the needed quirks added to the linux kernel. + +If you know that your HMD lacks the quirk you can run this command **before** or +after connecting the HMD and it will have it. Where `HDMI-A-0` is the xrandr +output name where you plug the HMD in. + +```bash +xrandr --output HDMI-A-0 --prop --set non-desktop 1 +``` + +You can verify that it stuck with the command. + +```bash +xrandr --prop +``` + +## Running Vulkan Validation + +To run Monado with Vulkan validation the loader's layer functionality can be used. +``` +VK_INSTANCE_LAYERS=VK_LAYER_KHRONOS_validation ./build/src/xrt/targets/service/monado-service +``` +The same can be done when launching a Vulkan client. + +If you want a backtrace to be produced at validation errors, create a `vk_layer_settings.txt` +file with the following content: +``` +khronos_validation.debug_action = VK_DBG_LAYER_ACTION_LOG_MSG,VK_DBG_LAYER_ACTION_BREAK +khronos_validation.report_flags = error,warn +khronos_validation.log_filename = stdout +``` + +## Coding style and formatting + +[clang-format][] is used, +and a `.clang-format` config file is present in the repo +to allow your editor to use them. + +To manually apply clang-format to every non-external source file in the tree, +run this command in the source dir with a `sh`-compatible shell +(Git for Windows git-bash should be OK): + +```bash +scripts/format-project.sh +``` + +You can optionally put something like `CLANG_FORMAT=clang-format-7` before that command +if your clang-format binary isn't named `clang-format`. +**Note that you'll typically prefer** to use something like `git clang-format` +to re-format only your changes, in case version differences in tools result in overall format changes. +The CI "style" job currently runs on Debian Bullseye, so it has clang-format-11. + +[OpenHMD]: http://openhmd.net +[drm-lease]: https://haagch.frickel.club/#!drmlease%2Emd +[OpenXR]: https://khronos.org/openxr +[clang-format]: https://releases.llvm.org/7.0.0/tools/clang/docs/ClangFormat.html +[cmake-build]: https://cmake.org/cmake/help/v3.12/manual/cmake.1.html#build-tool-mode +[cmake-generate]: https://cmake.org/cmake/help/v3.12/manual/cmake.1.html +[CMake]: https://cmake.org +[mesamatrix-ext]: https://mesamatrix.net/#Version_ExtensionsthatarenotpartofanyOpenGLorOpenGLESversion + +## Contributing, Code of Conduct + +See `CONTRIBUTING.md` for details of contribution guidelines. GitLab Issues and +Merge Requests are the preferred way to discuss problems, suggest enhancements, +or submit changes for review. **In case of a security issue**, you should choose +the "confidential" option when using the GitLab issues page. For highest +security, you can send encrypted email (using GPG/OpenPGP) to Rylie Pavlik at + and using the associated key from +. + +Please note that this project is released with a Contributor Code of Conduct. +By participating in this project you agree to abide by its terms. + +We follow the standard freedesktop.org code of conduct, +available at , +which is based on the [Contributor Covenant](https://www.contributor-covenant.org). + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported by contacting: + +* First-line project contacts: + * Rylie Pavlik + * Frederic Plourde + * Jakob Bornecrantz +* freedesktop.org contacts: see most recent list at + +Code of Conduct section excerpt adapted from the +[Contributor Covenant](https://www.contributor-covenant.org), version 1.4.1, +available at +, and +from the freedesktop.org-specific version of that code, available at +, used under CC-BY-4.0. diff --git a/README.md b/README.md index a23335933..9867d687f 100644 --- a/README.md +++ b/README.md @@ -1,278 +1,50 @@ -# Monado - XR Runtime (XRT) +# Rift S on Linux - +## Status -> * Main homepage and documentation: -> * Promotional homepage: -> * Maintained at -> * Latest API documentation: -> * Continuously-updated changelog of the default branch: -> +Early project. It already works and is usable in SteamVR with the headset and both controllers. Good changes will keep landing here. -Monado is an open source XR runtime delivering immersive experiences such as VR -and AR on mobile, PC/desktop, and any other device -(because gosh darn people -come up with a lot of weird hardware). -Monado aims to be a complete and conforming implementation -of the OpenXR API made by Khronos. -The project is primarily developed on GNU/Linux, but also runs on Android and Windows. -"Monado" has no specific meaning and is just a name. +Known rough edges: -## Monado source tree +- Controller behavior when the controllers leave the camera view is still rough. +- The boundary does not persist reliably across restarts. -* `src/xrt/include` - headers that define the internal interfaces of Monado. -* `src/xrt/compositor` - code for doing distortion and driving the display hardware of a device. -* `src/xrt/auxiliary` - utilities and other larger components. -* `src/xrt/drivers` - hardware drivers. -* `src/xrt/state_trackers/oxr` - OpenXR API implementation. -* `src/xrt/targets` - glue code and build logic to produce final binaries. -* `src/external` - a small collection of external code and headers. +## Build and install -## Getting Started +Head tracking needs Basalt. Clone Basalt, apply the patches in `patches/basalt/` in order, then build and install it by its own readme so that `libbasalt.so` is on the library path. If it is somewhere else, set `VIT_SYSTEM_LIBRARY_PATH`. -Dependencies include: - -* [CMake][] 3.13 or newer (Note Ubuntu 18.04 only has 3.10) -* Python 3.6 or newer -* Vulkan headers and loader - Fedora package `vulkan-loader-devel` -* OpenGL headers -* Eigen3 - Debian/Ubuntu package `libeigen3-dev` -* glslangValidator - Debian/Ubuntu package `glslang-tools`, Fedora package `glslang`. -* libusb -* libudev - Debian/Ubuntu package `libudev-dev`, Fedora package `systemd-devel` -* Video 4 Linux - Debian/Ubuntu package `libv4l-dev`. - -Optional (but recommended) dependencies: - -* libxcb and xcb-xrandr development packages -* [OpenHMD][] 0.3.0 or newer (found using pkg-config) - -Truly optional dependencies, useful for some drivers, app support, etc.: - -* Doxygen - Debian/Ubuntu package `doxygen` and `graphviz` -* Wayland development packages -* Xlib development packages -* libhidapi - Debian/Ubuntu package `libhidapi-dev` -* OpenCV -* libuvc - Debian/Ubuntu package `libuvc-dev` -* libjpeg -* libbluetooth - Debian/Ubuntu package `libbluetooth-dev` -* libsdl - Debian/Ubuntu package `libsdl2-dev` - -Experimental Windows support requires the Vulkan SDK and also needs or works -best with the following vcpkg packages installed: - -* pthreads eigen3 libusb hidapi zlib doxygen - -If you have a recent [vcpkg](https://vcpkg.io) installed and use the appropriate -CMake toolchain file, the vcpkg manifest in the Monado repository will instruct -vcpkg to locally install the dependencies automatically. The Vulkan SDK -installer should set the `VULKAN_SDK` Windows environment variable to point -at the installation location (for example, `C:/VulkanSDK/1.3.250.1`), though -make sure you open a new terminal (or open the CMake GUI) *after* doing that -install to make sure it is available. - -Monado has been tested on these distributions, but is expected to work on almost -any modern distribution. - -* Ubuntu 24.04, 22.04, 20.04, (18.04 may not be fully supported) -* Debian 11 `bookworm`, 10 `buster` - * Up-to-date package lists can be found in our CI config file, - `.gitlab-ci.yml` -* Archlinux - -These distributions include recent-enough versions of all the -software to use direct mode, -without using any external, third-party, or backported -package sources. - -See also [Status of DRM Leases][drm-lease] -for more details on specific packages, versions, and commits. - -Due to the lack of a OpenGL extension: GL_EXT_memory_object_fd on Intel's -OpenGL driver, only the AMD -radeonsi driver and the proprietary NVIDIA driver will work for OpenGL OpenXR -clients. This is due to a requirement of the Compositor. Support status of the -extension can be found on the [mesamatrix website][mesamatrix-ext]. - -### CMake - -Build process is similar to other CMake builds, -so something like the following will build it. - -Go into the source directory, create a build directory, -and change into it. - -```bash -mkdir build -cd build +``` +git clone https://gitlab.freedesktop.org/mateosss/basalt.git +cd basalt +git submodule update --init --recursive +for p in /path/to/this/repo/patches/basalt/*.patch; do patch -p1 < "$p"; done ``` -Then, invoke [CMake to generate a project][cmake-generate]. -Feel free to change the build type or generator ("Ninja" is fast and parallel) as you see fit. +The patches were made against Basalt main at df6e970. -```bash -cmake .. -DCMAKE_BUILD_TYPE=Debug -G "Unix Makefiles" +Then build Monado. OpenCV, Vulkan and Eigen are required on top of Monado's usual dependencies. + +``` +cmake -B build -G Ninja -DCMAKE_BUILD_TYPE=RelWithDebInfo \ + -DXRT_BUILD_DRIVER_RIFT_S=ON -DXRT_FEATURE_SLAM=ON -DXRT_FEATURE_STEAMVR_PLUGIN=ON +ninja -C build +sudo ninja -C build install ``` -If you plan to install the runtime, -append something like `-DCMAKE_INSTALL_PREFIX=~/.local` -to specify the root of the install directory. -(The default install prefix is `/usr/local`.) +The install puts the SteamVR driver in `/usr/local/share/steamvr-monado`. Register it with SteamVR and start SteamVR with the headset plugged in. -To build, [the generic CMake build commands][cmake-build] below will work on all systems, -though you can manually invoke your build tool (`make`, `ninja`, etc.) if you prefer. -The first command builds the runtime and docs, -and the second, which is optional, installs the runtime under `${CMAKE_INSTALL_PREFIX}`. - -```bash -cmake --build . -cmake --build . --target install +``` +~/.local/share/Steam/steamapps/common/SteamVR/bin/vrpathreg.sh adddriver /usr/local/share/steamvr-monado ``` -Alternately, if using Make, the following will build the runtime and docs, then install. -Replace `make` with `ninja` if you used the Ninja generator. +Unit tests build with the default `BUILD_TESTING=ON` and run with `ctest --test-dir build`. -```bash -make -make install -``` +## License -Documentation can be browsed by opening `doc/html/index.html` in the build directory in a web browser. +Boost Software License 1.0, the same as Monado. See `LICENSE`. Third-party files keep their own licenses in `LICENSES/`. -## Getting started using OpenXR with Monado - -This implements the [OpenXR][] API, -so to do anything with it, you'll need an application -that uses OpenXR, along with the OpenXR loader. -The OpenXR loader is a glue library that connects OpenXR applications to OpenXR runtimes such as Monado -It determines which runtime to use by looking for the config file `active_runtime.json` (either a symlink to -or a copy of a runtime manifest) in the usual XDG config paths -and processes environment variables such as `XR_RUNTIME_JSON=/usr/share/openxr/0/openxr_monado.json`. -It can also insert OpenXR API Layers without the application or the runtime having to do it. - -You can use the `hello_xr` sample provided with the OpenXR SDK. - -The OpenXR loader can be pointed to a runtime json file in a nonstandard location with the environment variable `XR_RUNTIME_JSON`. Example: - -```bash -XR_RUNTIME_JSON=~/monado/build/openxr_monado-dev.json ./openxr-example -``` - -For ease of development Monado creates a runtime manifest file in its build directory using an absolute path to the -Monado runtime in the build directory called `openxr_monado-dev.json`. Pointing `XR_RUNTIME_JSON` to this -file allows using Monado after building, without installing. - -Note that the loader can always find and load the runtime -if the path to the runtime library given in the json manifest is an absolute path, -but if a relative path like `libopenxr_monado.so.0` is given, -then `LD_LIBRARY_PATH` must include the directory that contains `libopenxr_monado.so.0`. -The absolute path in `openxr_monado-dev.json` takes care of this for you. - -Distribution packages for monado may provide the "active runtime" file `/etc/xdg/openxr/1/active_runtime.json`. -In this case the loader will automatically use Monado when starting an OpenXR application. This global configuration -can be overridden on a per user basis by creating `~/.config/openxr/1/active_runtime.json`. - -## Direct mode - -On AMD and Intel GPUs our direct mode code requires a connected HMD to have -the `non-desktop` xrandr property set to 1. -Only the most common HMDs have the needed quirks added to the linux kernel. - -If you know that your HMD lacks the quirk you can run this command **before** or -after connecting the HMD and it will have it. Where `HDMI-A-0` is the xrandr -output name where you plug the HMD in. - -```bash -xrandr --output HDMI-A-0 --prop --set non-desktop 1 -``` - -You can verify that it stuck with the command. - -```bash -xrandr --prop -``` - -## Running Vulkan Validation - -To run Monado with Vulkan validation the loader's layer functionality can be used. -``` -VK_INSTANCE_LAYERS=VK_LAYER_KHRONOS_validation ./build/src/xrt/targets/service/monado-service -``` -The same can be done when launching a Vulkan client. - -If you want a backtrace to be produced at validation errors, create a `vk_layer_settings.txt` -file with the following content: -``` -khronos_validation.debug_action = VK_DBG_LAYER_ACTION_LOG_MSG,VK_DBG_LAYER_ACTION_BREAK -khronos_validation.report_flags = error,warn -khronos_validation.log_filename = stdout -``` - -## Coding style and formatting - -[clang-format][] is used, -and a `.clang-format` config file is present in the repo -to allow your editor to use them. - -To manually apply clang-format to every non-external source file in the tree, -run this command in the source dir with a `sh`-compatible shell -(Git for Windows git-bash should be OK): - -```bash -scripts/format-project.sh -``` - -You can optionally put something like `CLANG_FORMAT=clang-format-7` before that command -if your clang-format binary isn't named `clang-format`. -**Note that you'll typically prefer** to use something like `git clang-format` -to re-format only your changes, in case version differences in tools result in overall format changes. -The CI "style" job currently runs on Debian Bullseye, so it has clang-format-11. - -[OpenHMD]: http://openhmd.net -[drm-lease]: https://haagch.frickel.club/#!drmlease%2Emd -[OpenXR]: https://khronos.org/openxr -[clang-format]: https://releases.llvm.org/7.0.0/tools/clang/docs/ClangFormat.html -[cmake-build]: https://cmake.org/cmake/help/v3.12/manual/cmake.1.html#build-tool-mode -[cmake-generate]: https://cmake.org/cmake/help/v3.12/manual/cmake.1.html -[CMake]: https://cmake.org -[mesamatrix-ext]: https://mesamatrix.net/#Version_ExtensionsthatarenotpartofanyOpenGLorOpenGLESversion - -## Contributing, Code of Conduct - -See `CONTRIBUTING.md` for details of contribution guidelines. GitLab Issues and -Merge Requests are the preferred way to discuss problems, suggest enhancements, -or submit changes for review. **In case of a security issue**, you should choose -the "confidential" option when using the GitLab issues page. For highest -security, you can send encrypted email (using GPG/OpenPGP) to Rylie Pavlik at - and using the associated key from -. - -Please note that this project is released with a Contributor Code of Conduct. -By participating in this project you agree to abide by its terms. - -We follow the standard freedesktop.org code of conduct, -available at , -which is based on the [Contributor Covenant](https://www.contributor-covenant.org). - -Instances of abusive, harassing, or otherwise unacceptable behavior may be -reported by contacting: - -* First-line project contacts: - * Rylie Pavlik - * Frederic Plourde - * Jakob Bornecrantz -* freedesktop.org contacts: see most recent list at - -Code of Conduct section excerpt adapted from the -[Contributor Covenant](https://www.contributor-covenant.org), version 1.4.1, -available at -, and -from the freedesktop.org-specific version of that code, available at -, used under CC-BY-4.0. +Contact lbgos at contact@lbgos.dev. diff --git a/patches/basalt/0001-keep-imu-frequency-per-tracker.patch b/patches/basalt/0001-keep-imu-frequency-per-tracker.patch new file mode 100644 index 000000000..a8fd4c7cf --- /dev/null +++ b/patches/basalt/0001-keep-imu-frequency-per-tracker.patch @@ -0,0 +1,27 @@ +--- a/src/vit/vit_tracker.cpp ++++ b/src/vit/vit_tracker.cpp +@@ -127,6 +127,7 @@ + vector cam; + } calib_data_ready; + uint32_t cam_count; ++ double imu_calibration_frequency = -1; + + Calibration calib; + VioConfig vio_config; +@@ -311,13 +312,13 @@ + int i = imu_calib.imu_index; + ASSERT(i == 0, "More than one IMU unsupported (%d)", i); + +- static double frequency = -1; ++ double &frequency = imu_calibration_frequency; + if (frequency == -1) { + frequency = imu_calib.frequency; + calib.imu_update_rate = frequency; + } else { +- ASSERT(frequency == calib.imu_update_rate, "Unsupported mix of IMU frequencies %lf != %lf", frequency, +- calib.imu_update_rate); ++ ASSERT(frequency == imu_calib.frequency, "Unsupported mix of IMU frequencies %lf != %lf", frequency, ++ imu_calib.frequency); + } + + // Accelerometer calibration diff --git a/patches/basalt/0002-limit-workers-with-driver-calibration.patch b/patches/basalt/0002-limit-workers-with-driver-calibration.patch new file mode 100644 index 000000000..e319232a1 --- /dev/null +++ b/patches/basalt/0002-limit-workers-with-driver-calibration.patch @@ -0,0 +1,12 @@ +--- a/src/vit/vit_tracker.cpp ++++ b/src/vit/vit_tracker.cpp +@@ -170,6 +170,9 @@ + calib_data_ready.cam.resize(cam_count, false); + + if (config->file == nullptr) { ++ // Driver-provided calibration still uses the default one-thread CPU budget. ++ tbb_global_control = ++ std::make_unique(tbb::global_control::max_allowed_parallelism, num_threads); + // For the pipeline to work now, the user will need to use add_cam/imu_calibration + return; + } diff --git a/patches/basalt/0003-safe-recreation-and-realtime-worker-budget.patch b/patches/basalt/0003-safe-recreation-and-realtime-worker-budget.patch new file mode 100644 index 000000000..9fe78adac --- /dev/null +++ b/patches/basalt/0003-safe-recreation-and-realtime-worker-budget.patch @@ -0,0 +1,193 @@ +--- a/src/vit/vit_tracker.cpp ++++ b/src/vit/vit_tracker.cpp +@@ -55,6 +55,7 @@ + #include + + #include ++#include + #include + #include + #include +@@ -119,7 +120,7 @@ + bool print_queue = false; + bool use_double = false; + bool deterministic = false; +- int num_threads = 1; ++ int num_threads = 4; + + // VIO members + struct { +@@ -170,7 +171,14 @@ + calib_data_ready.cam.resize(cam_count, false); + + if (config->file == nullptr) { +- // Driver-provided calibration still uses the default one-thread CPU budget. ++ // Bound the pool even without a unified config. A single thread can leave ++ // both five-camera pipeline stages behind; benchmarks may select 1..4 for comparison. ++ if (const char *value = std::getenv("BASALT_MAX_THREADS")) { ++ char *end = nullptr; ++ const long requested = std::strtol(value, &end, 10); ++ if (end != value && *end == '\0' && requested >= 1 && requested <= 4) num_threads = int(requested); ++ } ++ cout << "Basalt max parallelism=" << num_threads << "\n"; + tbb_global_control = + std::make_unique(tbb::global_control::max_allowed_parallelism, num_threads); + // For the pipeline to work now, the user will need to use add_cam/imu_calibration +@@ -421,16 +429,37 @@ + + void stop() { + running = false; ++ // Discard queued images. The in-flight image finishes before the sentinel. ++ OpticalFlowInput::Ptr image; ++ while (image_data_queue->try_pop(image)) {} + image_data_queue->push(nullptr); ++ ImuData::Ptr imu; ++ while (imu_data_queue->try_pop(imu)) {} ++ while (opt_flow_ptr->input_imu_queue.try_pop(imu)) {} + imu_data_queue->push(nullptr); + opt_flow_ptr->input_imu_queue.push(nullptr); ++ ++ // VIO can exit on its IMU sentinel before flow finishes. Once it has exited, ++ // drain its bounded vision queue so the remaining producer cannot block. ++ std::atomic flow_joined{false}; ++ thread drain_vision([&] { ++ while (!flow_joined.load()) { ++ if (vio->finished.load()) { ++ OpticalFlowResult::Ptr result; ++ while (vio->vision_data_queue.try_pop(result)) {} ++ } ++ std::this_thread::sleep_for(std::chrono::milliseconds(1)); ++ } ++ }); ++ // Join while dynamic types and all connected queues still exist. ++ opt_flow_ptr->maybe_join(); ++ flow_joined = true; ++ drain_vision.join(); ++ vio->maybe_join(); + + if (print_queue) queues_printer_thread.join(); + if (!deterministic) state_consumer_thread.join(); + if (show_gui) ui.stop(); +- +- // TODO: There is a segfault when closing monado without starting the stream +- // happens in a lambda from keypoint_vio.cpp and ends at line c1alib_bias.hpp:112 + } + + void finalize() const { +@@ -439,6 +468,16 @@ + } + + void reset() { vio->scheduleResetState(); } ++ ++ // VIT submissions must not block restart when another input stream stalls. ++ // Keep bounded queues current by discarding their oldest pending sample. ++ template ++ static void push_latest(tbb::concurrent_bounded_queue &queue, const T &sample) { ++ while (!queue.try_push(sample)) { ++ T discarded; ++ queue.try_pop(discarded); ++ } ++ } + + void push_imu_sample(const vit::ImuSample *s) { + // concurrent_bounded_queue expects Erasable and Allocator named +@@ -448,8 +487,8 @@ + data->t_ns = s->timestamp; + data->accel = {s->ax, s->ay, s->az}; + data->gyro = {s->wx, s->wy, s->wz}; +- imu_data_queue->push(data); +- opt_flow_ptr->input_imu_queue.push(data); ++ push_latest(*imu_data_queue, data); ++ push_latest(opt_flow_ptr->input_imu_queue, data); + } + + void push_frame(const vit::ImgSample *s) { +@@ -498,7 +537,7 @@ + if (i == cam_count - 1) { + partial_frame->addTime("frames_read"); + partial_frame->addTime("frames_pushed"); +- image_data_queue->push(partial_frame); ++ push_latest(*image_data_queue, partial_frame); + if (show_gui) ui.update_last_image(partial_frame); + if (deterministic) pop_state(); + } +@@ -609,7 +648,6 @@ + } + + vit::Result Tracker::stop() { +- impl_->finalize(); + impl_->stop(); + return vit::Result::VIT_SUCCESS; + } +@@ -718,3 +756,8 @@ + + return VIT_SUCCESS; + } ++ ++// Optional Monado capability. This build joins flow/VIO before destroying their ++// connected queues and accepts shutdown sentinels while waiting for IMU. ++extern "C" __attribute__((visibility("default"))) bool ++vit_tracker_supports_safe_recreation() { return true; } +--- a/include/basalt/optical_flow/optical_flow.h ++++ b/include/basalt/optical_flow/optical_flow.h +@@ -128,7 +128,11 @@ + // patch_coord = PatchT::pattern2.template cast(); + depth_guess = config.optical_flow_matching_default_depth; + } +- virtual ~OpticalFlowBase() { processing_thread->join(); } ++ virtual ~OpticalFlowBase() { maybe_join(); } ++ ++ void maybe_join() { ++ if (processing_thread && processing_thread->joinable()) processing_thread->join(); ++ } + + virtual void processingLoop() = 0; + +--- a/src/vi_estimator/sqrt_keypoint_vio.cpp ++++ b/src/vi_estimator/sqrt_keypoint_vio.cpp +@@ -60,6 +60,7 @@ + const VioConfig& config_) + : take_kf(true), + frames_after_kf(0), ++ take_ltkf(false), + g(g_.cast()), + initialized(false), + schedule_reset(false), +@@ -145,6 +146,7 @@ + lmdb.clear(); + + take_kf = true; ++ take_ltkf = false; + frames_after_kf = 0; + kf_ids.clear(); + ltkfs.clear(); +@@ -271,6 +273,9 @@ + // std::cout << "Skipping IMU data.." << std::endl; + } + ++ // A shutdown sentinel abandons this frame instead of dereferencing it. ++ if (!data) break; ++ + Vec3 vel_w_i_init; + vel_w_i_init.setZero(); + +@@ -318,6 +323,9 @@ + data->accel = calib.calib_accel_bias.getCalibrated(data->accel); + data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro); + } ++ ++ // A shutdown sentinel abandons this frame instead of dereferencing it. ++ if (!data) break; + + while (data->t_ns <= curr_frame->t_ns) { + meas->integrate(*data, accel_cov, gyro_cov); +@@ -326,6 +334,9 @@ + data->accel = calib.calib_accel_bias.getCalibrated(data->accel); + data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro); + } ++ ++ // A shutdown sentinel abandons this frame instead of dereferencing it. ++ if (!data) break; + + if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) { + if (!data.get()) break; diff --git a/scripts/install-rifts-passthrough-overlay.py b/scripts/install-rifts-passthrough-overlay.py new file mode 100755 index 000000000..d67a571d8 --- /dev/null +++ b/scripts/install-rifts-passthrough-overlay.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +# Copyright 2026, lbgos +# SPDX-License-Identifier: BSL-1.0 +"""Install and enable the optional passthrough overlay in the user's systemd session.""" +import argparse +from pathlib import Path +import shutil +import subprocess +import sys + + +def quote(value): + return '"' + str(value).replace('\\', '\\\\').replace('"', '\\"').replace('%', '%%') + '"' + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--python', type=Path, default=Path(sys.executable), help='Python interpreter with openvr installed') + args = parser.parse_args() + python = args.python.expanduser().absolute() + subprocess.run([str(python), '-c', 'import openvr'], check=True) + target = Path.home() / '.local/lib/rifts-passthrough' + target.mkdir(parents=True, exist_ok=True) + source = Path(__file__).resolve().parent + for name in ('rifts-passthrough-overlay.py', 'rifts_passthrough.py'): + shutil.copy2(source / name, target / name) + units = Path.home() / '.config/systemd/user' + units.mkdir(parents=True, exist_ok=True) + unit = f'''[Unit] +Description=Rift S passthrough overlay + +[Service] +ExecStart={quote(python)} -u {quote(target / 'rifts-passthrough-overlay.py')} --start-hidden +Restart=on-failure +RestartSec=5 +NoNewPrivileges=true + +[Install] +WantedBy=default.target +''' + (units / 'rifts-passthrough-overlay.service').write_text(unit) + subprocess.run(['systemctl', '--user', 'daemon-reload'], check=True) + subprocess.run(['systemctl', '--user', 'enable', '--now', 'rifts-passthrough-overlay.service'], check=True) + print('Overlay installed and enabled; starts hidden. Native Room View is preferred.') + + +if __name__ == '__main__': + main() diff --git a/scripts/rift-s-controller-capture.sh b/scripts/rift-s-controller-capture.sh new file mode 100755 index 000000000..e07c3151a --- /dev/null +++ b/scripts/rift-s-controller-capture.sh @@ -0,0 +1,62 @@ +#!/usr/bin/env bash +# Copyright 2026, lbgos +# SPDX-License-Identifier: BSL-1.0 +set -euo pipefail +# Run with SteamVR stopped and the headset ready. Needs the xr-probe OpenXR pose logger; set XR_PROBE to its path. +# Usage: rift-s-controller-capture.sh OUTPUT [SECONDS=80] [ENV=VALUE ...] +# RIFTS_LOCK_OWNER names the hardware lock holder (default rift-s-capture). +# Keep unworn runs to SECONDS <= 20, for a total service lifetime <= 30 s. +root=$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd) +out=$(realpath -m "${1:?output}") +seconds=${2:-80} +shift "$(( $# > 1 ? 2 : 1 ))" +if pgrep -x monado-service >/dev/null || pgrep -x vrserver >/dev/null; then + echo 'Hardware busy: monado-service or vrserver is running' >&2 + exit 1 +fi +owner=${RIFTS_LOCK_OWNER:-rift-s-capture} +# A lock already written under our own name is accepted, so a caller can queue for it. +if [[ $(cat /tmp/rifts-hw.lock 2>/dev/null) != "$owner" ]]; then + (set -o noclobber; printf '%s\n' "$owner" > /tmp/rifts-hw.lock) || exit 1 +fi +pid='' +cleanup() { + if [[ -n "$pid" ]]; then + kill -INT "$pid" 2>/dev/null || true + wait "$pid" 2>/dev/null || true + fi + rm -f /tmp/rifts-hw.lock +} +trap cleanup EXIT +mkdir -p "$out/led" +start=$(date --iso-8601=seconds) +printf "%s\n" "$start" > "$out/started-at.txt" +export XDG_RUNTIME_DIR=/run/user/$(id -u) +export WAYLAND_DISPLAY=${WAYLAND_DISPLAY:-wayland-0} +export DBUS_SESSION_BUS_ADDRESS=unix:path=$XDG_RUNTIME_DIR/bus +export XRT_COMPOSITOR_FORCE_WAYLAND_DIRECT=1 +export SLAM_SUBMIT_FROM_START=true +export VIT_SYSTEM_LIBRARY_PATH=/usr/local/lib/libbasalt.so +export XRT_NO_STDIN=true +export RIFT_S_LOG=trace CONSTELLATION_LOG=trace SLAM_LOG=info +export RIFT_S_CONTROLLER_DUMP_DIR="$out/led" +export XR_RUNTIME_JSON="$root/build/openxr_monado-dev.json" +env "$@" timeout -s INT "$((seconds+10))" "$root/build/src/xrt/targets/service/monado-service" > "$out/monado.log" 2>&1 & +pid=$! +sleep 8 +if ! grep -q 'Controller IMU device=2 ' "$out/monado.log" || + ! grep -q 'Controller IMU device=3 ' "$out/monado.log"; then + echo 'Both controllers must stream IMU. Stopping; wake them before the next capture.' >&2 + exit 1 +fi +printf "Pose capture starts now: %s seconds\n" "$seconds" +"${XR_PROBE:?set XR_PROBE to the xr-probe binary}" --seconds "$seconds" > "$out/poses.jsonl" 2> "$out/probe.log" +set +e +wait "$pid" +rc=$? +set -e +pid='' +printf '%s\n' "$rc" > "$out/monado.exit" +[[ $rc == 124 || $rc == 0 ]] +sudo -n journalctl -k --since "$start" --no-pager -o short-monotonic > "$out/kernel.log" 2> "$out/kernel.error" || true +printf "%s\n" "$out" diff --git a/scripts/rifts-passthrough-overlay.py b/scripts/rifts-passthrough-overlay.py new file mode 100755 index 000000000..68a82df0d --- /dev/null +++ b/scripts/rifts-passthrough-overlay.py @@ -0,0 +1,123 @@ +#!/usr/bin/env python3 +# Copyright 2026, lbgos +# SPDX-License-Identifier: BSL-1.0 +"""Optional head-relative stereo overlay; native Room View is preferred.""" +import argparse +import ctypes +import json +import signal +import statistics +import time + +import openvr +from rifts_passthrough import Stream + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--distance", type=float, default=1.0) + parser.add_argument("--width", type=float, default=2.0) + parser.add_argument("--start-hidden", action="store_true") + parser.add_argument("--test-frames", type=int, default=0) + parser.add_argument("--timeout", type=float, default=0) + args = parser.parse_args() + if args.distance <= 0 or args.width <= 0: + parser.error("distance and width must be positive") + stream = Stream() + initialized = False + handle = None + visible = not args.start_hidden + running = True + delivered = 0 + previous = 0 + toggles = stream.toggles + arrivals, ages, upload_ms = [], [], [] + + def toggle(*_): + nonlocal visible + visible = not visible + + def stop(*_): + nonlocal running + running = False + + signal.signal(signal.SIGUSR1, toggle) + signal.signal(signal.SIGTERM, stop) + signal.signal(signal.SIGINT, stop) + start = time.monotonic() + try: + openvr.init(openvr.VRApplication_Overlay) + initialized = True + overlay = openvr.VROverlay() + handle = overlay.createOverlay("rifts_passthrough", "Rift S Passthrough") + overlay.setOverlayFlag(handle, openvr.VROverlayFlags_SideBySide_Parallel, True) + overlay.setOverlayWidthInMeters(handle, args.width) + # The packed texture is twice as wide as one eye. Keep the quad square. + overlay.setOverlayTexelAspect(handle, 0.5) + overlay.setOverlaySortOrder(handle, 1000) + matrix = openvr.HmdMatrix34_t() + for i in range(3): + matrix.m[i][i] = 1 + matrix.m[2][3] = -args.distance + overlay.setOverlayTransformTrackedDeviceRelative(handle, openvr.k_unTrackedDeviceIndex_Hmd, matrix) + vr_system = openvr.VRSystem() + event = openvr.VREvent_t() + shown = False + buffer = (ctypes.c_uint8 * stream.size)() + print("Ready: right Oculus button or left menu double-click", flush=True) + while running: + while vr_system.pollNextEvent(event): + if event.eventType == openvr.VREvent_Quit: + running = False + current = stream.toggles + if (current - toggles) & 1: + toggle() + toggles = current + stream.activate(visible) + frame = stream.frame(previous) if visible else None + if frame: + previous, timestamp, pixels, age = frame + # A stable private copy remains unchanged throughout SetOverlayRaw. + ctypes.memmove(buffer, pixels, stream.size) + before = time.monotonic() + overlay.setOverlayRaw(handle, ctypes.cast(buffer, ctypes.c_void_p), stream.width, stream.height, 4) + upload_ms.append((time.monotonic() - before) * 1000) + arrivals.append(time.monotonic()) + ages.append(age) + if len(arrivals) > 600: + arrivals.pop(0) + ages.pop(0) + upload_ms.pop(0) + delivered += 1 + if not shown: + overlay.showOverlay(handle) + shown = True + elif shown and (not visible or time.monotonic() - arrivals[-1] > 0.25): + overlay.hideOverlay(handle) + shown = False + if args.test_frames and delivered >= args.test_frames: + break + if args.timeout and time.monotonic() - start >= args.timeout: + break + time.sleep(0.005 if visible else 0.025) + finally: + stream.close() + try: + if handle is not None: + overlay.hideOverlay(handle) + overlay.destroyOverlay(handle) + finally: + if initialized: + openvr.shutdown() + intervals = [(b - a) * 1000 for a, b in zip(arrivals, arrivals[1:])] + print(json.dumps({"submitted_frames": delivered, "last_sequence": previous, + "median_interval_ms": statistics.median(intervals) if intervals else None, + "max_interval_ms": max(intervals) if intervals else None, + "median_capture_age_ms": statistics.median(ages) if ages else None, + "median_upload_ms": statistics.median(upload_ms) if upload_ms else None}), flush=True) + if args.test_frames and delivered < args.test_frames: + raise SystemExit("Camera did not deliver the requested frames") + + +if __name__ == "__main__": + main() diff --git a/scripts/rifts-passthrough-probe.py b/scripts/rifts-passthrough-probe.py new file mode 100755 index 000000000..6eaed25f7 --- /dev/null +++ b/scripts/rifts-passthrough-probe.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 +# Copyright 2026, lbgos +# SPDX-License-Identifier: BSL-1.0 +"""Check advancing, non-black camera export frames and capture-to-reader age.""" +import argparse +import hashlib +import json +import statistics +import time +from rifts_passthrough import Stream + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--timeout", type=float, default=5.0) + parser.add_argument("--min-frames", type=int, default=5) + args = parser.parse_args() + stream = Stream() + frames, ages, hashes, sequences = [], [], set(), [] + black = 0 + previous = 0 + try: + end = time.monotonic() + args.timeout + while time.monotonic() < end: + stream.activate(True) + frame = stream.frame(previous) + if frame: + previous, timestamp, pixels, age = frame + frames.append(time.monotonic()) + ages.append(age) + sequences.append(previous) + hashes.add(hashlib.sha256(pixels).digest()) + black += not any(pixels[c::4].strip(b"\0") for c in range(3)) + time.sleep(0.002) + finally: + stream.close() + intervals = [(b - a) * 1000 for a, b in zip(frames, frames[1:])] + success = len(frames) >= args.min_frames and black < len(frames) + print(json.dumps({"width": stream.width, "height": stream.height, "advancing_frames": len(frames), + "first_sequence": sequences[0] if sequences else None, + "last_sequence": previous, "distinct_images": len(hashes), "black_frames": black, + "median_delivery_hz": 1000 / statistics.median(intervals) if intervals else None, + "median_capture_age_ms": statistics.median(ages) if ages else None, + "max_capture_age_ms": max(ages) if ages else None, "success": success}, indent=2)) + if not success: + raise SystemExit(1) + + +if __name__ == "__main__": + main() diff --git a/scripts/rifts_passthrough.py b/scripts/rifts_passthrough.py new file mode 100644 index 000000000..143b0f7e5 --- /dev/null +++ b/scripts/rifts_passthrough.py @@ -0,0 +1,58 @@ +# Copyright 2026, lbgos +# SPDX-License-Identifier: BSL-1.0 +"""Versioned camera export reader. One consumer owns the activation flag.""" +import fcntl +import mmap +import os +import struct +import time + +PATH = "/dev/shm/rifts_passthrough" +HEADER = struct.Struct("= HEADER.size + self.size): + raise ValueError("Invalid camera export dimensions") + except BaseException: + if hasattr(self, "mm"): + self.mm.close() + os.close(self.fd) + raise + + def activate(self, visible): + # Lease expires if the consumer crashes, so rectification cannot stay on. + heartbeat = int(time.monotonic() * 1000) & 0xffffffff if visible else 0 + struct.pack_into(" size: + raise SystemExit("Unexpected camera buffer layout") + stride = max(1, header.nWidth * header.nHeight // 1000) * channels + black += not any(pixels[offset + c] for offset in range(0, size - channels + 1, stride) + for c in range(3)) + except openvr.error_code.TrackedCameraError as error: + last_error = type(error).__name__ + time.sleep(0.01) + intervals = [b - a for a, b in zip(arrivals, arrivals[1:])] + print(json.dumps({"width": width, "height": height, "bytes": size, + "unique_frames": len(seen), "black_frames": black, + "median_delivery_hz": 1 / statistics.median(intervals) if intervals else None, + "last_error": last_error})) + if len(seen) < 2: + raise SystemExit("No advancing video stream; inspect vrserver camera logs") +finally: + if handle is not None: + camera.releaseVideoStreamingService(handle) + openvr.shutdown() diff --git a/src/external/openvr_includes/openvr_driver.h b/src/external/openvr_includes/openvr_driver.h index 703243fb1..b3f1ceb98 100644 --- a/src/external/openvr_includes/openvr_driver.h +++ b/src/external/openvr_includes/openvr_driver.h @@ -387,6 +387,7 @@ enum ETrackedDeviceProperty Prop_ManufacturerSerialNumber_String = 1049, Prop_ComputedSerialNumber_String = 1050, Prop_EstimatedDeviceFirstUseTime_Int32 = 1051, + Prop_AllowCameraToggle_Bool = 1055, // SteamVR camera toggle control // Properties that are unique to TrackedDeviceClass_HMD Prop_ReportsTimeSinceVSync_Bool = 2000, @@ -2078,6 +2079,7 @@ enum ECameraVideoStreamFormat CVS_FORMAT_YUYV16 = 5, // 16 bits per pixel CVS_FORMAT_BAYER16BG = 6, // 16 bits per pixel, 10-bit BG-format Bayer, see https://docs.opencv.org/3.1.0/de/d25/imgproc_color_conversions.html CVS_FORMAT_MJPEG = 7, // variable-sized MJPEG Open DML format, see https://www.loc.gov/preservation/digital/formats/fdd/fdd000063.shtml + CVS_FORMAT_RGBX32 = 8, // Full-sized pixels, 4BPP, LSB = RED CVS_MAX_FORMATS }; diff --git a/src/xrt/auxiliary/bindings/steamvr_profiles.py b/src/xrt/auxiliary/bindings/steamvr_profiles.py index f54a303f9..ddd516247 100644 --- a/src/xrt/auxiliary/bindings/steamvr_profiles.py +++ b/src/xrt/auxiliary/bindings/steamvr_profiles.py @@ -82,14 +82,15 @@ def main(): for idx, component in enumerate(p.components): subpath_name = component.steamvr_path - input_source[subpath_name] = { + # Optional extension components must not replace the base source type. + source = input_source.setdefault(subpath_name, { "type": component.subpath_type, "binding_image_point": [0, 0], # TODO "order": idx - } + }) for req in get_required_components(component.subpath_type): - input_source[subpath_name][req] = req in component.components_for_subpath + source[req] = source.get(req, False) or req in component.components_for_subpath j = { "json_id": "input_profile", @@ -110,6 +111,26 @@ def main(): } + # Monado owns the compositor binding, including native room setup. + if p.name == "/interaction_profiles/oculus/touch_controller": + j["controller_type"] = "monado_oculus_touch" + j["compatibility_mode_controller_type"] = "oculus_touch" + j["legacy_binding"] = "{oculus}/input/legacy_bindings_touch.json" + # The driver exposes Touch's menu button as the system button. + j["input_source"]["/input/system"] = j["input_source"].pop("/input/menu") + # SteamVR resolves these poses through the Touch render model. The + # compositor's Pointer action binds /pose/tip, not OpenXR /pose/aim. + for pose in ("raw", "tip", "handgrip", "openxr_aim", "openxr_grip"): + input_source["/pose/" + pose] = { + "type": "pose", + "binding_image_point": [0, 0], + "order": len(input_source), + } + j["default_bindings"] = [{ + "app_key": "openvr.component.vrcompositor", + "binding_url": "monado_touch_compositor.json", + }] + f = open_file(args, fname) # print("Creating SteamVR input profile", f.name) diff --git a/src/xrt/auxiliary/math/m_clock_tracking.c b/src/xrt/auxiliary/math/m_clock_tracking.c index 73fe5a44a..3df999bbc 100644 --- a/src/xrt/auxiliary/math/m_clock_tracking.c +++ b/src/xrt/auxiliary/math/m_clock_tracking.c @@ -176,7 +176,10 @@ m_clock_windowed_skew_tracker_push(struct m_clock_windowed_skew_tracker *t, t->current_window_pos = (t->current_window_pos + 1) % t->max_window_samples; /* Update the moving average skew */ - size_t w = t->current_window_samples; + /* The skew is negative whenever the remote clock runs ahead of the local one. Keep the + * window size signed: with a size_t, a negative skew is multiplied and divided as an unsigned + * 64-bit value and the average lands near INT64_MAX. */ + time_duration_ns w = (time_duration_ns)t->current_window_samples; t->current_skew = (t->current_min_skew + t->current_skew * (w - 1)) / w; t->have_skew_estimate = true; } diff --git a/src/xrt/auxiliary/math/m_vec3.h b/src/xrt/auxiliary/math/m_vec3.h index 4fefeffe4..d805f9b59 100644 --- a/src/xrt/auxiliary/math/m_vec3.h +++ b/src/xrt/auxiliary/math/m_vec3.h @@ -119,7 +119,9 @@ m_vec3_angle(struct xrt_vec3 l, struct xrt_vec3 r) } lengths = sqrtf(lengths); - return acosf(dot / lengths); + // Nearly parallel vectors can round outside acos's domain. + float cosine = dot / lengths; + return acosf(cosine > 1 ? 1 : cosine < -1 ? -1 : cosine); } static inline struct xrt_vec3 diff --git a/src/xrt/auxiliary/tracking/t_camera_models.h b/src/xrt/auxiliary/tracking/t_camera_models.h index 951fbe713..7ade3b562 100644 --- a/src/xrt/auxiliary/tracking/t_camera_models.h +++ b/src/xrt/auxiliary/tracking/t_camera_models.h @@ -134,7 +134,8 @@ static inline float kb4_solve_theta(const struct t_camera_model_params *dist, const float *r_theta, float *d_func_d_theta) { float theta = *r_theta; - for (int i = 4; i > 0; i--) { + // Wide-angle pixels need more than four Newton steps to recover their bearing. + for (int i = 12; i > 0; i--) { float theta2 = theta * theta; float func = dist->fisheye.k4 * theta2; @@ -157,7 +158,10 @@ kb4_solve_theta(const struct t_camera_model_params *dist, const float *r_theta, *d_func_d_theta += 1.0f; // Iteration of Newton method - theta += ((*r_theta) - func) / (*d_func_d_theta); + float step = ((*r_theta) - func) / (*d_func_d_theta); + theta += step; + if (fabsf(step) < 1e-7f) + break; } return theta; diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp index 7add1f6f9..57c8b9fa8 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp @@ -40,12 +40,15 @@ #include #include +#include +#include #include #include #include #include #include #include +#include #include #include @@ -83,9 +86,13 @@ DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_INFO) DEBUG_GET_ONCE_OPTION(vit_system_library_path, "VIT_SYSTEM_LIBRARY_PATH", PREFERRED_VIT_SYSTEM_LIBRARY) DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", nullptr) DEBUG_GET_ONCE_BOOL_OPTION(slam_ui, "SLAM_UI", false) +// Unknown backends remain opt-in; safety-patched backends advertise recreation support. +DEBUG_GET_ONCE_OPTION(slam_backend_recreate_override, "SLAM_BACKEND_RECREATE", nullptr) +DEBUG_GET_ONCE_BOOL_OPTION(slam_backend_recreate, "SLAM_BACKEND_RECREATE", false) DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false) DEBUG_GET_ONCE_NUM_OPTION(slam_openvr_groundtruth_device, "SLAM_OPENVR_GROUNDTRUTH_DEVICE", 0) DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_DEAD_RECKONING)) +DEBUG_GET_ONCE_OPTION(slam_record_path, "SLAM_RECORD_PATH", nullptr) DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false) DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/") DEBUG_GET_ONCE_BOOL_OPTION(slam_timing_stat, "SLAM_TIMING_STAT", true) @@ -233,6 +240,11 @@ struct TimingWriter : public CSVWriter TimingWriter(const string &dir, const string &fn, bool e, const vector &cn) : CSVWriter(dir, fn, e, cn) {} + void + set_columns(const vector &columns) + { + column_names = columns; + } }; //! Writes feature information specific to a particular estimated pose @@ -260,6 +272,16 @@ struct FeaturesWriter : public CSVWriter */ struct TrackerSlam { + // Backend replacement excludes users; camera and IMU submission may run concurrently. + std::shared_mutex backend_mutex; + // Protect pose history and prediction/filter state without holding it during queue pushes. + std::mutex state_mutex; + vit_config system_config{}; + t_slam_calibration calibration{}; + bool use_driver_calibration = false; + bool started = false; + bool backend_recreate = false; + struct xrt_tracked_slam base = {}; struct xrt_frame_node node = {}; //!< Will be called on destruction struct t_vit_bundle vit; //!< VIT system function pointers @@ -390,6 +412,9 @@ struct TrackerSlam u_var_curves fcs_ui; //!< Display of `fcs` in UI bool enabled = false; //!< Whether the features extension is enabled + //! Features tracked over all cameras in the latest pose, -1 when unknown. + int last_total = -1; // Protected with last_total_ts by state_mutex. + timepoint_ns last_total_ts = 0; struct u_var_button enable_btn; //!< Toggle extension } features; @@ -423,15 +448,19 @@ timing_ui_setup(TrackerSlam &t) static const char *msg[2] = {"[OFF] Enable timing", "[ON] Disable timing"}; u_var_button_cb cb = [](void *t_ptr) { TrackerSlam *t = (TrackerSlam *)t_ptr; + std::unique_lock backend_lock(t->backend_mutex); + std::lock_guard lock(t->state_mutex); + if (t->tracker == nullptr) + return; u_var_button &btn = t->timing.enable_btn; bool e = !t->timing.enabled; - snprintf(btn.label, sizeof(btn.label), "%s", msg[e]); vit_result_t vres = t->vit.tracker_enable_extension(t->tracker, VIT_TRACKER_EXTENSION_POSE_TIMING, e); if (vres != VIT_SUCCESS) { U_LOG_IFL_E(t->log_level, "Failed to set tracker timing extension"); return; } t->timing.enabled = e; + snprintf(btn.label, sizeof(btn.label), "%s", msg[e]); }; t.timing.enable_btn.cb = cb; t.timing.enable_btn.disabled = !t.exts.has_pose_timing; @@ -543,15 +572,19 @@ features_ui_setup(TrackerSlam &t) static const char *msg[2] = {"[OFF] Enable features info", "[ON] Disable features info"}; u_var_button_cb cb = [](void *t_ptr) { TrackerSlam *t = (TrackerSlam *)t_ptr; + std::unique_lock backend_lock(t->backend_mutex); + std::lock_guard lock(t->state_mutex); + if (t->tracker == nullptr) + return; u_var_button &btn = t->features.enable_btn; bool e = !t->features.enabled; - snprintf(btn.label, sizeof(btn.label), "%s", msg[e]); vit_result_t vres = t->vit.tracker_enable_extension(t->tracker, VIT_TRACKER_EXTENSION_POSE_FEATURES, e); if (vres != VIT_SUCCESS) { U_LOG_IFL_E(t->log_level, "Failed to set tracker features extension"); return; } t->features.enabled = e; + snprintf(btn.label, sizeof(btn.label), "%s", msg[e]); }; t.features.enable_btn.cb = cb; t.features.enable_btn.disabled = !t.exts.has_pose_features; @@ -773,8 +806,8 @@ flush_poses(TrackerSlam &t) // Last relation xrt_space_relation lr = XRT_SPACE_RELATION_ZERO; - int64_t lts; - t.slam_rels.get_latest(<s, &lr); + int64_t lts = nts; + bool have_previous = t.slam_rels.get_latest(<s, &lr); xrt_quat lrot = lr.pose.orientation; double dt = time_ns_to_s(nts - lts); @@ -787,7 +820,9 @@ flush_poses(TrackerSlam &t) rel.relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; rel.pose = {nrot, npos}; rel.linear_velocity = nvel; - math_quat_finite_difference(&lrot, &nrot, dt, &rel.angular_velocity); + if (have_previous && dt > 0) { + math_quat_finite_difference(&lrot, &nrot, dt, &rel.angular_velocity); + } // Push to relationship history unless we are debugging prediction if (t.dbg_pred_counter % t.dbg_pred_every == 0) { @@ -806,6 +841,14 @@ flush_poses(TrackerSlam &t) if (t.features.enabled) { vector feat_count = features_ui_push(t, pose, nts); t.slam_features_writer->push({nts, feat_count}); + if (!feat_count.empty()) { + int total = 0; + for (int c : feat_count) { + total += c; + } + t.features.last_total = total; + t.features.last_total_ts = nts; + } } t.vit.pose_destroy(pose); @@ -972,8 +1015,9 @@ setup_ui(TrackerSlam &t) u_var_button_cb reset_state_cb = [](void *t_ptr) { TrackerSlam &t = *(TrackerSlam *)t_ptr; - vit_result_t vres = t.vit.tracker_reset(t.tracker); - if (vres != VIT_SUCCESS) { + std::unique_lock backend_lock(t.backend_mutex); + std::lock_guard lock(t.state_mutex); + if (t.started && t.vit.tracker_reset(t.tracker) != VIT_SUCCESS) { SLAM_WARN("Failed to reset VIT tracker"); } }; @@ -1084,6 +1128,10 @@ add_camera_calibration(const TrackerSlam &t, const t_slam_camera_calibration *ca for (size_t i = 0; i < ARRAY_SIZE(params.transform); ++i) params.transform[i] = T.v[i]; + SLAM_DEBUG("DIAG VIT cam%u %ux%u fx=%f fy=%f cx=%f cy=%f hz=%f", cam_index, params.width, + params.height, params.fx, params.fy, params.cx, params.cy, params.frequency); + for (size_t i = 0; i < 16; i++) SLAM_DEBUG("DIAG VIT cam%u T[%zu]=%.9g", cam_index, i, params.transform[i]); + for (size_t i = 0; i < params.distortion_count; i++) SLAM_DEBUG("DIAG VIT cam%u d[%zu]=%.9g", cam_index, i, params.distortion[i]); vit_result_t vres = t.vit.tracker_add_camera_calibration(t.tracker, ¶ms); if (vres != VIT_SUCCESS) { SLAM_ERROR("Failed to add camera calibration for camera %u", cam_index); @@ -1149,6 +1197,146 @@ using namespace xrt::auxiliary::tracking::slam; * */ +extern "C" int +t_slam_restart(struct xrt_tracked_slam *xts) +{ + auto &t = *container_of(xts, TrackerSlam, base); + if (!t.backend_recreate) { + SLAM_WARN("SLAM_BACKEND_RECREATE is disabled; enable only with a restart-compatible backend"); + return -1; + } + std::unique_lock backend_lock(t.backend_mutex); + std::lock_guard lock(t.state_mutex); + if (t.started && t.vit.tracker_stop(t.tracker) != VIT_SUCCESS) { + SLAM_ERROR("Failed to stop VIT session for restart"); + return -1; + } + t.started = false; + if (t.tracker != nullptr) { + t.vit.tracker_destroy(t.tracker); + t.tracker = nullptr; + } + + /* VIT reset is asynchronous in Basalt and leaves its input queues and + * optical flow alive. Recreate the backend so no pre-gap state can escape. */ + t.slam_rels.clear(); + t.last_rel = XRT_SPACE_RELATION_ZERO; + t.last_ts = 0; + t.last_imu_ts = INT64_MIN; + std::fill(t.last_cam_ts.begin(), t.last_cam_ts.end(), INT64_MIN); + t.dbg_pred_counter = 0; + t.features.last_total = -1; + { + unique_lock lock(t.last_hand_masks_mutex); + t.last_hand_masks = {}; + } + { + unique_lock lock(t.last_controller_masks_mutex); + t.last_controller_masks = {}; + } + os_mutex_lock(&t.lock_ff); + m_ff_vec3_f32_free(&t.gyro_ff); + m_ff_vec3_f32_free(&t.accel_ff); + m_ff_vec3_f32_alloc(&t.gyro_ff, 1000); + m_ff_vec3_f32_alloc(&t.accel_ff, 1000); + os_mutex_unlock(&t.lock_ff); + m_ff_vec3_f32_free(&t.filter.pos_ff); + m_ff_vec3_f32_free(&t.filter.rot_ff); + m_ff_vec3_f32_alloc(&t.filter.pos_ff, 1000); + m_ff_vec3_f32_alloc(&t.filter.rot_ff, 1000); + t.filter.last = XRT_SPACE_RELATION_ZERO; + t.filter.target = XRT_SPACE_RELATION_ZERO; + m_filter_euro_vec3_init(&t.filter.pos_oe, t.filter.min_cutoff, t.filter.min_dcutoff, t.filter.beta); + m_filter_euro_quat_init(&t.filter.rot_oe, t.filter.min_cutoff, t.filter.min_dcutoff, t.filter.beta); + + if (t.vit.tracker_create(&t.system_config, &t.tracker) != VIT_SUCCESS) { + SLAM_ERROR("Failed to recreate VIT tracker"); + return -1; + } + if (t.use_driver_calibration) { + send_calibration(t, t.calibration); + } + if (t.timing.enabled) { + t.vit.tracker_enable_extension(t.tracker, VIT_TRACKER_EXTENSION_POSE_TIMING, true); + } + if (t.features.enabled) { + t.vit.tracker_enable_extension(t.tracker, VIT_TRACKER_EXTENSION_POSE_FEATURES, true); + } + if (t.vit.tracker_start(t.tracker) != VIT_SUCCESS) { + SLAM_ERROR("Failed to start replacement VIT tracker"); + return -1; + } + t.started = true; + SLAM_INFO("VIT session restarted; cleared input queues and prediction history"); + return 0; +} + +extern "C" int +t_slam_reset(struct xrt_tracked_slam *xts) +{ + auto &t = *container_of(xts, TrackerSlam, base); + std::unique_lock backend_lock(t.backend_mutex); + std::lock_guard lock(t.state_mutex); + if (!t.started || t.vit.tracker_reset(t.tracker) != VIT_SUCCESS) { + SLAM_WARN("Failed to reset VIT tracker"); + return -1; + } + + // Forget the poses of the previous map so prediction and filters start from the new one. + t.slam_rels.clear(); + t.last_rel = XRT_SPACE_RELATION_ZERO; + t.last_ts = 0; + t.filter.last = XRT_SPACE_RELATION_ZERO; + t.filter.target = XRT_SPACE_RELATION_ZERO; + m_filter_euro_vec3_init(&t.filter.pos_oe, t.filter.min_cutoff, t.filter.min_dcutoff, t.filter.beta); + m_filter_euro_quat_init(&t.filter.rot_oe, t.filter.min_cutoff, t.filter.min_dcutoff, t.filter.beta); + t.features.last_total = -1; + + SLAM_INFO("VIT tracker reset; cleared prediction history"); + return 0; +} + +extern "C" bool +t_slam_get_feature_count(struct xrt_tracked_slam *xts, int *out_count, timepoint_ns *out_ts) +{ + auto &t = *container_of(xts, TrackerSlam, base); + std::unique_lock lock(t.state_mutex, std::try_to_lock); + if (!lock.owns_lock()) + return false; + int count = t.features.last_total; + if (count < 0) { + return false; + } + *out_count = count; + *out_ts = t.features.last_total_ts; + return true; +} + +extern "C" bool +t_slam_get_latest_sample(struct xrt_tracked_slam *xts, + timepoint_ns when_ns, + timepoint_ns *out_ts, + struct xrt_space_relation *out_relation, + struct xrt_space_relation *out_query_relation) +{ + auto &t = *container_of(xts, TrackerSlam, base); + *out_relation = XRT_SPACE_RELATION_ZERO; + *out_query_relation = XRT_SPACE_RELATION_ZERO; + std::shared_lock backend_lock(t.backend_mutex, std::try_to_lock); + if (!backend_lock.owns_lock()) { + return false; + } + std::lock_guard lock(t.state_mutex); + if (!t.started) { + return false; + } + flush_poses(t); + // Keep the configured IMU prediction for display poses. The guard consumes only + // the source sample below, so historical camera queries cannot change its cadence. + predict_pose(t, when_ns, out_query_relation); + return t.slam_rels.get_latest(out_ts, out_relation); +} + //! Get a filtered prediction from the SLAM tracked poses. extern "C" void t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, struct xrt_space_relation *out_relation) @@ -1157,6 +1345,17 @@ t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, stru auto &t = *container_of(xts, TrackerSlam, base); + *out_relation = XRT_SPACE_RELATION_ZERO; + // Report no pose rather than stall the caller while the backend is being replaced. + std::shared_lock backend_lock(t.backend_mutex, std::try_to_lock); + if (!backend_lock.owns_lock()) { + return; + } + std::lock_guard lock(t.state_mutex); + if (!t.started) { + return; + } + //! @todo This should not be cached, the same timestamp can be requested at a //! later time on the frame for a better prediction. if (when_ns == t.last_ts) { @@ -1227,19 +1426,33 @@ t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) auto &t = *container_of(sink, TrackerSlam, imu_sink); + std::shared_lock backend_lock(t.backend_mutex, std::try_to_lock); + if (!backend_lock.owns_lock() || !t.started) { + return; + } + timepoint_ns ts = s->timestamp_ns; xrt_vec3_f64 a = s->accel_m_s2; xrt_vec3_f64 w = s->gyro_rad_secs; timepoint_ns now = (timepoint_ns)os_monotonic_get_ns(); SLAM_TRACE("[%ld] imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", now, ts, a.x, a.y, a.z, w.x, w.y, w.z); - // Check monotonically increasing timestamps - if (ts <= t.last_imu_ts) { - SLAM_WARN("Sample (%" PRId64 ") is older than last (%" PRId64 ") by %" PRId64 " ns", ts, t.last_imu_ts, - t.last_imu_ts - ts); - return; + { + std::lock_guard lock(t.state_mutex); + // Check monotonically increasing timestamps. + if (ts <= t.last_imu_ts) { + SLAM_WARN("Sample (%" PRId64 ") is older than last (%" PRId64 ") by %" PRId64 " ns", ts, + t.last_imu_ts, t.last_imu_ts - ts); + return; + } + t.last_imu_ts = ts; + struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z}; + struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z}; + os_mutex_lock(&t.lock_ff); + m_ff_vec3_f32_push(t.gyro_ff, &gyro, ts); + m_ff_vec3_f32_push(t.accel_ff, &accel, ts); + os_mutex_unlock(&t.lock_ff); } - t.last_imu_ts = ts; //! @todo There are many conversions like these between xrt and //! slam_tracker.hpp types. Implement a casting mechanism to avoid copies. @@ -1257,13 +1470,6 @@ t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) } xrt_sink_push_imu(t.euroc_recorder->imu, s); - - struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z}; - struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z}; - os_mutex_lock(&t.lock_ff); - m_ff_vec3_f32_push(t.gyro_ff, &gyro, ts); - m_ff_vec3_f32_push(t.accel_ff, &accel, ts); - os_mutex_unlock(&t.lock_ff); } //! Push the frame to the external SLAM system @@ -1272,6 +1478,11 @@ receive_frame(TrackerSlam &t, struct xrt_frame *frame, uint32_t cam_index) { XRT_TRACE_MARKER(); + std::shared_lock backend_lock(t.backend_mutex); + if (!t.started) { + return; + } + SLAM_DASSERT_(frame->timestamp < INT64_MAX); // Return early if we don't submit @@ -1279,21 +1490,21 @@ receive_frame(TrackerSlam &t, struct xrt_frame *frame, uint32_t cam_index) return; } - if (cam_index == t.cam_count - 1) { - flush_poses(t); // Useful to flush SLAM poses when no openxr app is open - } - - SLAM_DASSERT(t.last_cam_ts[0] != INT64_MIN || cam_index == 0, "First frame was not a cam0 frame"); - - // Check monotonically increasing timestamps - timepoint_ns &last_ts = t.last_cam_ts[cam_index]; timepoint_ns ts = (int64_t)frame->timestamp; - SLAM_TRACE("[%" PRId64 "] cam%d frame t=%" PRId64, os_monotonic_get_ns(), cam_index, ts); - if (last_ts >= ts) { - SLAM_WARN("Frame (%" PRId64 ") is older than last (%" PRId64 ") by %" PRId64 " ns", ts, last_ts, - last_ts - ts); + { + std::lock_guard lock(t.state_mutex); + if (cam_index == t.cam_count - 1) { + flush_poses(t); // Flush even when no app requests poses. + } + SLAM_DASSERT(t.last_cam_ts[0] != INT64_MIN || cam_index == 0, "First frame was not a cam0 frame"); + timepoint_ns &last_ts = t.last_cam_ts[cam_index]; + SLAM_TRACE("[%" PRId64 "] cam%d frame t=%" PRId64, os_monotonic_get_ns(), cam_index, ts); + if (last_ts >= ts) { + SLAM_WARN("Frame (%" PRId64 ") is older than last (%" PRId64 ") by %" PRId64 " ns", ts, last_ts, + last_ts - ts); + } + last_ts = ts; } - last_ts = ts; // Construct and send the image sample vit_img_sample sample = {}; @@ -1382,12 +1593,18 @@ t_slam_node_break_apart(struct xrt_frame_node *node) t_openvr_tracker_stop(t.ovr_tracker); } + std::unique_lock backend_lock(t.backend_mutex); + std::lock_guard lock(t.state_mutex); + if (!t.started) { + return; + } vit_result_t vres = t.vit.tracker_stop(t.tracker); if (vres != VIT_SUCCESS) { SLAM_ERROR("Failed to stop VIT tracker"); return; } + t.started = false; SLAM_DEBUG("SLAM tracker dismantled"); } @@ -1397,6 +1614,7 @@ t_slam_node_destroy(struct xrt_frame_node *node) auto t_ptr = container_of(node, TrackerSlam, node); auto &t = *t_ptr; // Needed by SLAM_DEBUG SLAM_DEBUG("Destroying SLAM tracker"); + u_var_remove_root(t_ptr); if (t.ovr_tracker != NULL) { t_openvr_tracker_destroy(t.ovr_tracker); } @@ -1406,7 +1624,6 @@ t_slam_node_destroy(struct xrt_frame_node *node) delete t.slam_traj_writer; delete t.pred_traj_writer; delete t.filt_traj_writer; - u_var_remove_root(t_ptr); for (size_t i = 0; i < t.ui_sink.size(); i++) { u_sink_debug_destroy(&t.ui_sink[i]); } @@ -1416,8 +1633,15 @@ t_slam_node_destroy(struct xrt_frame_node *node) m_ff_vec3_f32_free(&t.filter.pos_ff); m_ff_vec3_f32_free(&t.filter.rot_ff); - t_ptr->vit.tracker_destroy(t_ptr->tracker); - t_vit_bundle_unload(&t_ptr->vit); + { + std::unique_lock backend_lock(t.backend_mutex); + std::lock_guard lock(t.state_mutex); + if (t.tracker != nullptr) { + t.vit.tracker_destroy(t.tracker); + t.tracker = nullptr; + } + t_vit_bundle_unload(&t.vit); + } delete t_ptr; } @@ -1426,12 +1650,14 @@ extern "C" int t_slam_start(struct xrt_tracked_slam *xts) { auto &t = *container_of(xts, TrackerSlam, base); + std::unique_lock backend_lock(t.backend_mutex); vit_result_t vres = t.vit.tracker_start(t.tracker); if (vres != VIT_SUCCESS) { SLAM_ERROR("Failed to start VIT tracker"); return -1; } + t.started = true; SLAM_DEBUG("SLAM tracker started"); return 0; } @@ -1480,6 +1706,12 @@ t_slam_create(struct xrt_frame_context *xfctx, return -1; } + t.backend_recreate = debug_get_option_slam_backend_recreate_override() != nullptr + ? debug_get_bool_option_slam_backend_recreate() + : t.vit.tracker_supports_safe_recreation && t.vit.tracker_supports_safe_recreation(); + SLAM_INFO("Backend recreation enabled=%d advertised_safe=%d", t.backend_recreate, + t.vit.tracker_supports_safe_recreation != nullptr); + // Check the user has provided a SLAM_CONFIG file const char *config_file = config->slam_config; bool some_calib = config->slam_calib != nullptr; @@ -1506,6 +1738,12 @@ t_slam_create(struct xrt_frame_context *xfctx, } t.base.get_tracked_pose = t_slam_get_tracked_pose; + t.base.restart = t_slam_restart; + t.system_config = system_config; + t.use_driver_calibration = !config_file; + if (t.use_driver_calibration) { + t.calibration = *config->slam_calib; + } if (!config_file) { SLAM_INFO("Using calibration from driver and default pipeline settings"); @@ -1541,7 +1779,8 @@ t_slam_create(struct xrt_frame_context *xfctx, xrt_frame_context_add(xfctx, &t.node); - t.euroc_recorder = euroc_recorder_create(xfctx, NULL, t.cam_count, false); + t.euroc_recorder = euroc_recorder_create(xfctx, debug_get_option_slam_record_path(), t.cam_count, + debug_get_option_slam_record_path() != nullptr); t.last_imu_ts = INT64_MIN; t.last_cam_ts = vector(t.cam_count, INT64_MIN); @@ -1565,6 +1804,20 @@ t_slam_create(struct xrt_frame_context *xfctx, t.filt_traj_writer = new TrajectoryWriter(dir, "filtering.csv", write_csvs); setup_ui(t); + t.slam_times_writer->set_columns(t.timing.columns); + if (config->timing_stat && t.exts.has_pose_timing && + t.vit.tracker_enable_extension(t.tracker, VIT_TRACKER_EXTENSION_POSE_TIMING, true) == VIT_SUCCESS) { + t.timing.enabled = true; + snprintf(t.timing.enable_btn.label, sizeof(t.timing.enable_btn.label), "%s", "[ON] Disable timing"); + } + + // Feature counts let drivers notice cameras that see nothing (headset face down or covered). + if (config->features_stat && t.exts.has_pose_features && + t.vit.tracker_enable_extension(t.tracker, VIT_TRACKER_EXTENSION_POSE_FEATURES, true) == VIT_SUCCESS) { + t.features.enabled = true; + snprintf(t.features.enable_btn.label, sizeof(t.features.enable_btn.label), "%s", + "[ON] Disable features info"); + } // Setup OpenVR groundtruth tracker if (config->openvr_groundtruth_device > 0) { diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h index 1dcc871e5..9fcb38c8e 100644 --- a/src/xrt/auxiliary/tracking/t_tracking.h +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -699,6 +699,28 @@ t_slam_create(struct xrt_frame_context *xfctx, int t_slam_start(struct xrt_tracked_slam *xts); +/*! + * Re-initialise the SLAM system and drop the poses of the previous map. + * + * @public @memberof xrt_tracked_slam + */ +int +t_slam_reset(struct xrt_tracked_slam *xts); + +/*! + * Features tracked over all cameras in the latest SLAM pose. Returns false while unknown (the + * backend has no feature extension, or no pose since the last reset). + * + * @public @memberof xrt_tracked_slam + */ +bool +t_slam_get_feature_count(struct xrt_tracked_slam *xts, int *out_count, timepoint_ns *out_ts); + +//! Read source and prediction under one backend/history lock. Guard only the source timestamp. +bool +t_slam_get_latest_sample(struct xrt_tracked_slam *xts, timepoint_ns when_ns, timepoint_ns *out_ts, + struct xrt_space_relation *out_relation, struct xrt_space_relation *out_query_relation); + /* * * Camera calibration diff --git a/src/xrt/auxiliary/tracking/t_vit_loader.c b/src/xrt/auxiliary/tracking/t_vit_loader.c index 44cf65c47..127def2c9 100644 --- a/src/xrt/auxiliary/tracking/t_vit_loader.c +++ b/src/xrt/auxiliary/tracking/t_vit_loader.c @@ -92,6 +92,12 @@ t_vit_bundle_load(struct t_vit_bundle *vit, const char *path) GET_PROC(pose_get_features); #undef GET_PROC + // Older backends remain opt-in. Missing optional symbols are not load errors. + dlerror(); + *(void **)&vit->tracker_supports_safe_recreation = dlsym(vit->handle, "vit_tracker_supports_safe_recreation"); + if (dlerror() != NULL) + vit->tracker_supports_safe_recreation = NULL; + return true; } diff --git a/src/xrt/auxiliary/tracking/t_vit_loader.h b/src/xrt/auxiliary/tracking/t_vit_loader.h index 2789d009e..87b118e67 100644 --- a/src/xrt/auxiliary/tracking/t_vit_loader.h +++ b/src/xrt/auxiliary/tracking/t_vit_loader.h @@ -33,6 +33,8 @@ struct t_vit_bundle } version; PFN_vit_api_get_version api_get_version; + // Optional private capability: all workers/queue users are joined by stop. + bool (*tracker_supports_safe_recreation)(void); PFN_vit_tracker_create tracker_create; PFN_vit_tracker_destroy tracker_destroy; PFN_vit_tracker_has_image_format tracker_has_image_format; diff --git a/src/xrt/compositor/main/comp_renderer.c b/src/xrt/compositor/main/comp_renderer.c index f6c403015..107c3f1fc 100644 --- a/src/xrt/compositor/main/comp_renderer.c +++ b/src/xrt/compositor/main/comp_renderer.c @@ -1017,6 +1017,42 @@ dispatch_compute(struct comp_renderer *r, * */ +/* Mesh topology and FOV are fixed. Refresh CPU UVs and the existing VBO in place. */ +static bool +renderer_update_distortion(struct comp_renderer *r) +{ + struct comp_compositor *c = r->c; + if (c->xdev->update_distortion == NULL || !c->xdev->update_distortion(c->xdev)) { + return true; + } + uint64_t start = os_monotonic_get_ns(); + renderer_wait_for_last_fence(r); + struct xrt_hmd_parts *hmd = c->xdev->hmd; + float *vertices = hmd->distortion.mesh.vertices; + uint32_t stride = hmd->distortion.mesh.stride / sizeof(float); + uint32_t per_view = hmd->distortion.mesh.vertex_count / hmd->view_count; + uint64_t hash = 14695981039346656037ull; + for (uint32_t i = 0; i < hmd->distortion.mesh.vertex_count; i++) { + float *v = &vertices[i * stride]; + if (xrt_device_compute_distortion(c->xdev, i / per_view, (v[0] + 1) * 0.5f, (v[1] + 1) * 0.5f, + (struct xrt_uv_triplet *)&v[2]) != XRT_SUCCESS) { + return false; + } + } + const unsigned char *bytes = (const unsigned char *)vertices; + size_t size = hmd->distortion.mesh.vertex_count * hmd->distortion.mesh.stride; + for (size_t i = 0; i < size; i++) { + hash = (hash ^ bytes[i]) * 1099511628211ull; + } + if (render_buffer_write(&c->base.vk, &c->nr.mesh.vbo, vertices, size) != VK_SUCCESS || + !render_distortion_images_update(&c->nr, c->xdev)) { + return false; + } + COMP_INFO(c, "Live distortion mesh hash=%016" PRIx64 " update_ms=%.3f", hash, + (os_monotonic_get_ns() - start) / 1000000.0); + return true; +} + XRT_CHECK_RESULT xrt_result_t comp_renderer_draw(struct comp_renderer *r) { @@ -1058,6 +1094,10 @@ comp_renderer_draw(struct comp_renderer *r) comp_target_update_timings(ct); + if (!renderer_update_distortion(r)) { + return XRT_ERROR_VULKAN; + } + // Hardcoded for now. const uint32_t view_count = c->nr.view_count; enum comp_target_fov_source fov_source = COMP_TARGET_FOV_SOURCE_DISTORTION; diff --git a/src/xrt/compositor/render/render_distortion.c b/src/xrt/compositor/render/render_distortion.c index 179252f62..53045a4ed 100644 --- a/src/xrt/compositor/render/render_distortion.c +++ b/src/xrt/compositor/render/render_distortion.c @@ -288,10 +288,10 @@ render_distortion_buffer_init(struct render_resources *r, struct xrt_device *xdev, bool pre_rotate) { - struct render_buffer bufs[RENDER_DISTORTION_IMAGES_SIZE]; - VkDeviceMemory device_memories[RENDER_DISTORTION_IMAGES_SIZE]; - VkImage images[RENDER_DISTORTION_IMAGES_SIZE]; - VkImageView image_views[RENDER_DISTORTION_IMAGES_SIZE]; + struct render_buffer bufs[RENDER_DISTORTION_IMAGES_SIZE] = {0}; + VkDeviceMemory device_memories[RENDER_DISTORTION_IMAGES_SIZE] = {0}; + VkImage images[RENDER_DISTORTION_IMAGES_SIZE] = {0}; + VkImageView image_views[RENDER_DISTORTION_IMAGES_SIZE] = {0}; VkCommandBuffer upload_buffer = VK_NULL_HANDLE; VkResult ret; @@ -417,3 +417,40 @@ render_distortion_images_ensure(struct render_resources *r, return true; } + +/* The caller has waited for rendering to finish. Reuse images/descriptors and + * upload new UVs; no Vulkan pipeline, swapchain or session rebuild is needed. */ +bool +render_distortion_images_update(struct render_resources *r, struct xrt_device *xdev) +{ + struct vk_bundle *vk = r->vk; + struct render_buffer bufs[RENDER_DISTORTION_IMAGES_SIZE] = {0}; + VkCommandBuffer cmd = VK_NULL_HANDLE; + VkResult ret = VK_SUCCESS; + for (uint32_t i = 0; i < r->view_count; i++) { + ret = create_and_fill_in_distortion_buffer_for_view(vk, xdev, &bufs[i], &bufs[r->view_count + i], + &bufs[2 * r->view_count + i], i, + r->distortion.pre_rotated); + if (ret != VK_SUCCESS) { + goto cleanup; + } + } + struct vk_cmd_pool *pool = &r->distortion_pool; + vk_cmd_pool_lock(pool); + ret = vk_cmd_pool_create_and_begin_cmd_buffer_locked(vk, pool, 0, &cmd); + if (ret == VK_SUCCESS) { + VkExtent2D extent = {RENDER_DISTORTION_IMAGE_DIMENSIONS, RENDER_DISTORTION_IMAGE_DIMENSIONS}; + for (uint32_t i = 0; i < RENDER_DISTORTION_IMAGES_COUNT(r); i++) { + /* UNDEFINED discards the completed image contents; handles remain stable. */ + queue_upload_for_first_level_and_layer_locked(vk, cmd, bufs[i].buffer, r->distortion.images[i], + extent); + } + ret = vk_cmd_pool_end_submit_wait_and_free_cmd_buffer_locked(vk, pool, cmd); + } + vk_cmd_pool_unlock(pool); +cleanup: + for (uint32_t i = 0; i < RENDER_DISTORTION_IMAGES_COUNT(r); i++) { + render_buffer_fini(vk, &bufs[i]); + } + return ret == VK_SUCCESS; +} diff --git a/src/xrt/compositor/render/render_interface.h b/src/xrt/compositor/render/render_interface.h index 8a198707f..43c658fda 100644 --- a/src/xrt/compositor/render/render_interface.h +++ b/src/xrt/compositor/render/render_interface.h @@ -1431,6 +1431,10 @@ render_compute_clear(struct render_compute *render, */ +/* Refresh existing UV textures after the caller waits for GPU readers. */ +bool +render_distortion_images_update(struct render_resources *r, struct xrt_device *xdev); + #ifdef __cplusplus } #endif diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index c7908308d..f51e2809a 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -348,14 +348,27 @@ if(XRT_BUILD_DRIVER_RIFT_S) rift_s/rift_s_camera.h rift_s/rift_s_controller.c rift_s/rift_s_controller.h + rift_s/rift_s_position_filter.c + rift_s/rift_s_position_filter.h + rift_s/rift_s_slam_guard.c + rift_s/rift_s_slam_guard.h rift_s/rift_s_firmware.c rift_s/rift_s_firmware.h rift_s/rift_s_hmd.c rift_s/rift_s_hmd.h + rift_s/rift_s_model_check.cpp + rift_s/rift_s_model_check.h + rift_s/rift_s_passthrough.c + rift_s/rift_s_passthrough.h + rift_s/rift_s_optics.c + rift_s/rift_s_optics.h rift_s/rift_s_protocol.c rift_s/rift_s_protocol.h rift_s/rift_s_radio.c rift_s/rift_s_radio.h + rift_s/rift_s_world_anchor.cpp + rift_s/rift_s_world_anchor.h + rift_s/rift_s_world_control.h rift_s/rift_s_tracker.c rift_s/rift_s_tracker.h rift_s/rift_s_util.cpp @@ -374,6 +387,8 @@ if(XRT_BUILD_DRIVER_RIFT_S) xrt-external-tinyceres constellation_tracker ) + target_include_directories(drv_rift_s PRIVATE ${OpenCV_INCLUDE_DIRS}) + target_link_libraries(drv_rift_s PRIVATE ${OpenCV_LIBRARIES}) if(XRT_BUILD_DRIVER_HANDTRACKING) target_link_libraries(drv_rift_s PRIVATE drv_ht) endif() diff --git a/src/xrt/drivers/rift_s/rift_s.c b/src/xrt/drivers/rift_s/rift_s.c index e7130eca1..0e3986fd8 100644 --- a/src/xrt/drivers/rift_s/rift_s.c +++ b/src/xrt/drivers/rift_s/rift_s.c @@ -34,6 +34,7 @@ #include "os/os_time.h" +#include "util/u_debug.h" #include "util/u_device.h" #include "util/u_distortion_mesh.h" #include "util/u_trace_marker.h" @@ -50,6 +51,8 @@ #include "rift_s_controller.h" #include "rift_s_camera.h" +DEBUG_GET_ONCE_BOOL_OPTION(steamvr_bench, "MONADO_STEAMVR_BENCH", false) + static void * rift_s_run_thread(void *ptr); static void @@ -145,6 +148,17 @@ read_hmd_config(struct os_hid_device *hid_hmd, struct rift_s_hmd_config *config) return ret; } + /* Optional and informational: the optics use the Oculus runtime lens centres, not these */ + char *lens_json = NULL; + int lens_json_len = 0; + if (rift_s_read_firmware_block(hid_hmd, RIFT_S_FIRMWARE_BLOCK_LENS_CALIB, &lens_json, &lens_json_len) >= 0) { + config->lens_centers_valid = rift_s_parse_lens_centers(lens_json, config->lens_center_px) == 0; + free(lens_json); + } + if (!config->lens_centers_valid) { + RIFT_S_DEBUG("No lens calibration block"); + } + return 0; } @@ -219,6 +233,11 @@ rift_s_system_create(struct xrt_prober *xp, goto cleanup; } + // Bench sessions must power the panel even if no new proximity packet arrives. + if (debug_get_bool_option_steamvr_bench()) { + rift_s_hmd_set_proximity(sys->hmd, true); + } + // Allow time for enumeration of available displays by host system, so the compositor can select among them. RIFT_S_INFO( "Sleeping until the HMD display is powered up so, the available displays " @@ -542,6 +561,12 @@ handle_packets(struct rift_s_system *sys) /* System state packet. Enable the screen if the prox sensor is * triggered. */ bool prox_sensor = (buf[1] == 0) ? false : true; + if (prox_sensor != sys->physical_proximity || now - sys->last_proximity_log_ns >= 5000000000ULL) { + RIFT_S_INFO("HMD physical proximity=%d bench=%d", prox_sensor, + debug_get_bool_option_steamvr_bench()); + sys->physical_proximity = prox_sensor; + sys->last_proximity_log_ns = now; + } os_mutex_lock(&sys->dev_mutex); if (sys->hmd != NULL) { rift_s_hmd_set_proximity(sys->hmd, prox_sensor); diff --git a/src/xrt/drivers/rift_s/rift_s.h b/src/xrt/drivers/rift_s/rift_s.h index 00a545fdd..2ec8529ab 100644 --- a/src/xrt/drivers/rift_s/rift_s.h +++ b/src/xrt/drivers/rift_s/rift_s.h @@ -62,6 +62,11 @@ struct rift_s_hmd_config struct rift_s_imu_config_info_t imu_config_info; struct rift_s_imu_calibration imu_calibration; + + /* Lens optical centres from firmware block 0x12, in native panel pixels + * (2560x1440, left eye first). Only valid if lens_centers_valid. */ + bool lens_centers_valid; + struct xrt_vec2 lens_center_px[2]; }; /* Structure to track online devices and type */ @@ -80,6 +85,9 @@ struct rift_s_system struct os_thread_helper oth; struct os_hid_device *handles[3]; uint64_t last_keep_alive; + // Raw proximity diagnostics must stay independent of the bench override. + uint64_t last_proximity_log_ns; + bool physical_proximity; /* state tracking for tracked devices on our radio link */ int num_active_tracked_devices; diff --git a/src/xrt/drivers/rift_s/rift_s_camera.c b/src/xrt/drivers/rift_s/rift_s_camera.c index 674f0100e..3253478ee 100644 --- a/src/xrt/drivers/rift_s/rift_s_camera.c +++ b/src/xrt/drivers/rift_s/rift_s_camera.c @@ -16,12 +16,13 @@ * @ingroup drv_rift_s */ #include +#include #include "rift_s.h" #include "rift_s_camera.h" +#include "rift_s_camera_metadata.h" #include "os/os_threading.h" -#include "xrt/xrt_byte_order.h" #include "xrt/xrt_defines.h" #include "xrt/xrt_frame.h" @@ -46,6 +47,9 @@ //! Specifies whether the user wants to enable autoexposure from the start. DEBUG_GET_ONCE_BOOL_OPTION(rift_s_autoexposure, "RIFT_S_AUTOEXPOSURE", true) +// Save up to ten raw LED frames, one per second, to an existing directory. +DEBUG_GET_ONCE_OPTION(rift_s_controller_dump_dir, "RIFT_S_CONTROLLER_DUMP_DIR", NULL) + struct rift_s_camera { struct os_mutex lock; @@ -55,6 +59,18 @@ struct rift_s_camera struct rift_s_camera_calibration_block *camera_calibration; uint64_t last_frame_ts_ns; + uint64_t last_diag_ns[2]; + uint64_t last_arrival_ns[2]; + uint64_t max_arrival_gap_ns[2]; + bool have_frame_counter; + uint16_t last_frame_counter; + struct + { + uint64_t accepted[2]; // controller, SLAM + uint64_t bad_layout, bad_magic, bad_crc, bad_timestamp; + uint64_t missing_frames, counter_discontinuities, bad_crop, bad_guard; + } metadata_stats; + uint32_t controller_dump_frames; struct xrt_frame_sink in_sink; // Receive raw frames and split them @@ -78,27 +94,6 @@ struct rift_s_camera_finder struct xrt_frame_context *xfctx; }; -union rift_s_frame_data { - struct - { - uint8_t frame_type; // 0x06 or 0x86 (controller or SLAM exposure) - __le16 magic_abcd; // 0xabcd - __le16 frame_ctr; // Increments every exposure - __le32 const1; // QHWH - uint8_t pad1[7]; // all zeroes padding to 16 bytes - __le64 frame_ts; // microseconds - __le32 frame_ctr2; // Another frame counter, but only increments on alternate frames @ 30Hz - __le16 slam_exposure[5]; // One 16-bit per camera. Exposure duration? - uint8_t pad2[2]; // zero padding - uint8_t slam_gain[5]; // One byte per camera. 0x40 or 0xf0 depending on frame type - uint8_t pad3; // zero padding - __le16 unknown1; // changes every frame. No clear pattern - __le16 magic_face; // 0xface - - } __attribute__((packed)) data; - uint8_t raw[50]; -}; - static void update_expgain(struct rift_s_camera *cam, struct xrt_frame *xf); @@ -269,56 +264,55 @@ void rift_s_camera_destroy(struct rift_s_camera *cam) { u_var_remove_root(cam); + u_autoexpgain_destroy(&cam->aeg); os_mutex_destroy(&cam->lock); free(cam); } static bool -parse_frame_data(const struct xrt_frame *xf, union rift_s_frame_data *row_data) +crop_in_bounds(const struct xrt_frame *xf, struct xrt_rect roi) { - /* Parse out the bits encoded as 8x8 blocks in the top rows */ - unsigned int x, out_x; - - if (xf->width != 50 * 8 * 8 || xf->height < 8) + if (roi.offset.w < 0 || roi.offset.h < 0 || roi.extent.w <= 0 || roi.extent.h <= 0) { return false; + } + uint32_t x = roi.offset.w, y = roi.offset.h, w = roi.extent.w, h = roi.extent.h; + if (x > xf->width || y > xf->height || w > xf->width - x || h > xf->height - y || xf->stride < xf->width || + xf->size < x + w || xf->stride == 0) { + return false; + } + return (size_t)y + h - 1 <= (xf->size - x - w) / xf->stride; +} - uint8_t *pix = &xf->data[xf->width * 4]; - - int bit = 7; - for (x = 4, out_x = 0; x < xf->width; x += 8) { - uint8_t val = 0; - if (pix[x] > 128) - val = 1 << bit; - - if (bit == 7) { - row_data->raw[out_x] = val; - } else { - row_data->raw[out_x] |= val; +// Neighboring black rows are transport guards, not LED segmentation thresholds. +static bool +crop_guards_valid(const struct xrt_frame *xf, struct xrt_rect roi) +{ + uint32_t rows[2] = {roi.offset.h - 1, roi.offset.h + roi.extent.h}; + for (int i = 0; i < 2; i++) { + if (rows[i] >= xf->height) { + continue; } - if (bit > 0) - bit--; - else { - bit = 7; - out_x++; + struct xrt_rect guard = {.offset = {roi.offset.w, rows[i]}, .extent = {roi.extent.w, 1}}; + if (!crop_in_bounds(xf, guard)) { + return false; + } + const uint8_t *row = xf->data + rows[i] * xf->stride + roi.offset.w; + for (int x = 0; x < roi.extent.w; x++) { + if (row[x] > 20) { + return false; + } } } - - /* Check magic numbers */ - if (__le16_to_cpu(row_data->data.magic_abcd) != 0xabcd) - return false; - if (__le16_to_cpu(row_data->data.magic_face) != 0xface) - return false; - return true; } static int -get_y_offset(struct rift_s_camera *cam, enum rift_s_camera_id cam_id, union rift_s_frame_data *row_data) +get_y_offset(struct rift_s_camera *cam, enum rift_s_camera_id cam_id, struct rift_s_frame_metadata *row_data) { /* There's a magic formula for computing the vertical offset of each camera view * based on exposure, due to some internals of the headset. This formula extracted * through trial and error */ - int exposure = __le16_to_cpu(row_data->data.slam_exposure[cam_id]); + int exposure = row_data->exposure[cam_id]; int y_offset = (exposure + 275) / 38; if (y_offset > 375) { @@ -334,7 +328,7 @@ static struct xrt_frame * rift_s_camera_extract_frame(struct rift_s_camera *cam, enum rift_s_camera_id cam_id, struct xrt_frame *full_frame, - union rift_s_frame_data *row_data) + struct rift_s_frame_metadata *row_data) { struct rift_s_camera_calibration *calib = &cam->camera_calibration->cameras[cam_id]; struct xrt_rect roi = calib->roi; @@ -342,12 +336,53 @@ rift_s_camera_extract_frame(struct rift_s_camera *cam, roi.offset.h = get_y_offset(cam, cam_id, row_data); struct xrt_frame *xf_crop = NULL; - + if (!crop_in_bounds(full_frame, roi)) { + cam->metadata_stats.bad_crop++; + RIFT_S_DEBUG("DIAG camcrop camera=%u rejected=bounds", cam_id); + return NULL; + } + if (!crop_guards_valid(full_frame, roi)) { + cam->metadata_stats.bad_guard++; + // The empirical SLAM crop formula has no raw-frame fixture yet. Count the guard + // failure without suppressing SLAM until its neighboring rows are verified. + RIFT_S_DEBUG("DIAG camcrop camera=%u guard=nonblack y=%d count=%" PRIu64, cam_id, roi.offset.h, + cam->metadata_stats.bad_guard); + } u_frame_create_roi(full_frame, roi, &xf_crop); return xf_crop; } +static void +dump_controller_frame(struct rift_s_camera *cam, struct xrt_frame *xf, uint64_t hw_ns) +{ + const char *dir = debug_get_option_rift_s_controller_dump_dir(); + if (dir == NULL || cam->controller_dump_frames >= 300) { + return; + } + if (cam->controller_dump_frames++ % 30 != 0) { + return; + } + + char path[1024]; + int len = snprintf(path, sizeof(path), "%s/led-%" PRIu64 ".pgm", dir, hw_ns); + if (len < 0 || (size_t)len >= sizeof(path)) { + return; + } + FILE *file = fopen(path, "wb"); + if (file == NULL) { + RIFT_S_WARN("Unable to write controller frame to %s", path); + return; + } + bool ok = fprintf(file, "P5\n%u %u\n255\n", xf->width, xf->height) > 0; + for (uint32_t y = 0; ok && y < xf->height; y++) { + ok = fwrite(xf->data + y * xf->stride, 1, xf->width, file) == xf->width; + } + if (fclose(file) != 0 || !ok) { + RIFT_S_WARN("Incomplete controller frame at %s", path); + } +} + static void receive_cam_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) { @@ -371,43 +406,101 @@ receive_cam_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) release_xf = true; } - // Dump mid-row of the 8 pix data line - union rift_s_frame_data row_data; - - if (!parse_frame_data(xf, &row_data)) { - RIFT_S_TRACE("Invalid frame top-row data. Skipping"); - return; + struct rift_s_frame_metadata row_data; + enum rift_s_metadata_status status = + rift_s_metadata_extract(xf->data, xf->width, xf->height, xf->stride, xf->size, &row_data); + if (status != RIFT_S_METADATA_OK) { + if (status == RIFT_S_METADATA_BAD_LAYOUT) + cam->metadata_stats.bad_layout++; + if (status == RIFT_S_METADATA_BAD_MAGIC) + cam->metadata_stats.bad_magic++; + if (status == RIFT_S_METADATA_BAD_CRC) + cam->metadata_stats.bad_crc++; + RIFT_S_DEBUG("DIAG cammeta rejected=%s layout=%" PRIu64 " magic=%" PRIu64 " crc=%" PRIu64, + status == RIFT_S_METADATA_BAD_LAYOUT ? "layout" + : status == RIFT_S_METADATA_BAD_MAGIC ? "magic" + : "crc", + cam->metadata_stats.bad_layout, cam->metadata_stats.bad_magic, + cam->metadata_stats.bad_crc); + goto out; + } + if (row_data.frame_ts_us > UINT64_MAX / OS_NS_PER_USEC) { + cam->metadata_stats.bad_timestamp++; + RIFT_S_DEBUG("DIAG cammeta rejected=timestamp_overflow ctr=%u", row_data.frame_ctr); + goto out; + } + uint64_t frame_ts_ns = row_data.frame_ts_us * OS_NS_PER_USEC; + if (frame_ts_ns == 0 || frame_ts_ns <= cam->last_frame_ts_ns) { + cam->metadata_stats.bad_timestamp++; + RIFT_S_DEBUG("DIAG cammeta rejected=timestamp ctr=%u hw_us=%" PRIu64, row_data.frame_ctr, + row_data.frame_ts_us); + goto out; + } + cam->last_frame_ts_ns = frame_ts_ns; + if (cam->have_frame_counter) { + uint16_t missing; + if (rift_s_metadata_counter_delta(cam->last_frame_counter, row_data.frame_ctr, &missing)) { + cam->metadata_stats.missing_frames += missing; + } else { + cam->metadata_stats.counter_discontinuities++; + } + } + cam->last_frame_counter = row_data.frame_ctr; + cam->have_frame_counter = true; + unsigned int kind = (row_data.frame_type & 0x80) != 0; + cam->metadata_stats.accepted[kind]++; + uint64_t now_ns = os_monotonic_get_ns(); + if (cam->last_arrival_ns[kind] != 0) { + cam->max_arrival_gap_ns[kind] = MAX(cam->max_arrival_gap_ns[kind], now_ns - cam->last_arrival_ns[kind]); + } + cam->last_arrival_ns[kind] = now_ns; + if (now_ns - cam->last_diag_ns[kind] >= 5 * (uint64_t)U_TIME_1S_IN_NS) { + cam->last_diag_ns[kind] = now_ns; + RIFT_S_INFO("Camera timing kind=%s arrival_ns=%" PRIu64 " hw_us=%" PRIu64 + " max_arrival_gap_ms=%.3f missing=%" PRIu64 " discontinuities=%" PRIu64 + " exposure=%u,%u,%u,%u,%u gain16=%u,%u,%u,%u,%u", + kind ? "slam" : "controller", now_ns, row_data.frame_ts_us, + (double)cam->max_arrival_gap_ns[kind] / U_TIME_1MS_IN_NS, + cam->metadata_stats.missing_frames, cam->metadata_stats.counter_discontinuities, + row_data.exposure[0], row_data.exposure[1], row_data.exposure[2], row_data.exposure[3], row_data.exposure[4], + row_data.gain[0], row_data.gain[1], row_data.gain[2], row_data.gain[3], row_data.gain[4]); + cam->max_arrival_gap_ns[kind] = 0; } - RIFT_S_DEBUG("frame ctr %u ts %" PRIu64 - " µS pair ctr %u " - "exposure[0] %u gain[0] %u unk %u", - (uint16_t)__le16_to_cpu(row_data.data.frame_ctr), (uint64_t)__le64_to_cpu(row_data.data.frame_ts), - (uint32_t)__le32_to_cpu(row_data.data.frame_ctr2), - (uint16_t)__le16_to_cpu(row_data.data.slam_exposure[0]), row_data.data.slam_gain[0], - (uint16_t)__le16_to_cpu(row_data.data.unknown1)); - - // rift_s_hexdump_buffer("Row data", row_data.raw, sizeof(row_data.row)); - uint64_t frame_ts_ns = (uint64_t)__le64_to_cpu(row_data.data.frame_ts) * OS_NS_PER_USEC; - if (frame_ts_ns == cam->last_frame_ts_ns) { - RIFT_S_WARN("Camera frame TS didn't advance. Probably the headset crashed"); - return; + RIFT_S_DEBUG("DIAG cammeta kind=%s ctr=%u pair=%u hw_us=%" PRIu64 " controller=%" PRIu64 " slam=%" PRIu64 + " missing=%" PRIu64 " discontinuities=%" PRIu64, + kind ? "slam" : "controller", row_data.frame_ctr, row_data.frame_ctr2, row_data.frame_ts_us, + cam->metadata_stats.accepted[0], cam->metadata_stats.accepted[1], + cam->metadata_stats.missing_frames, cam->metadata_stats.counter_discontinuities); + for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) { + RIFT_S_TRACE("DIAG cammeta camera=%d kind=%s ctr=%u exposure=%u gain=%f", i, + kind ? "slam" : "controller", row_data.frame_ctr, row_data.exposure[i], + row_data.gain[i] / 16.0f); } - // If the top left pixel is > 128, send as SLAM frame else controller - if (row_data.data.frame_type & 0x80) { + // Route by metadata kind, independent of arrival order or pixel brightness. + if (kind) { int y_offset = get_y_offset(cam, 0, &row_data); struct xrt_rect roi = {.offset = {0, y_offset}, .extent = {.w = xf->width, .h = 480}}; struct xrt_frame *xf_crop = NULL; - u_frame_create_roi(xf, roi, &xf_crop); - u_sink_debug_push_frame(&cam->debug_sinks[0], xf_crop); + if (crop_in_bounds(xf, roi)) { + u_frame_create_roi(xf, roi, &xf_crop); + u_sink_debug_push_frame(&cam->debug_sinks[0], xf_crop); + } xrt_frame_reference(&xf_crop, NULL); /* Extract camera frames and push to the tracker */ struct xrt_frame *frames[RIFT_S_CAMERA_COUNT] = {0}; + bool valid = true; for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) { frames[i] = rift_s_camera_extract_frame(cam, CAM_IDX_TO_ID[i], xf, &row_data); + valid &= frames[i] != NULL; + } + if (!valid) { + for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) + xrt_frame_reference(&frames[i], NULL); + goto out; } /* Update the exposure for all cameras based on the auto exposure for the left camera view */ @@ -420,15 +513,29 @@ receive_cam_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) xrt_frame_reference(&frames[i], NULL); } } else { + dump_controller_frame(cam, xf, frame_ts_ns); struct xrt_rect roi = {.offset = {0, 40}, .extent = {.w = xf->width, .h = 480}}; struct xrt_frame *xf_crop = NULL; + if (!crop_in_bounds(xf, roi)) { + cam->metadata_stats.bad_crop++; + RIFT_S_DEBUG("DIAG camcrop kind=controller rejected=bounds count=%" PRIu64, + cam->metadata_stats.bad_crop); + goto out; + } + if (!crop_guards_valid(xf, roi)) { + cam->metadata_stats.bad_guard++; + RIFT_S_DEBUG("DIAG camcrop kind=controller rejected=guard count=%" PRIu64, + cam->metadata_stats.bad_guard); + goto out; + } u_frame_create_roi(xf, roi, &xf_crop); u_sink_debug_push_frame(&cam->debug_sinks[1], xf_crop); rift_s_tracker_push_controller_frameset(cam->tracker, frame_ts_ns, xf_crop); xrt_frame_reference(&xf_crop, NULL); } +out: if (release_xf) xrt_frame_reference(&xf, NULL); } diff --git a/src/xrt/drivers/rift_s/rift_s_camera_metadata.h b/src/xrt/drivers/rift_s/rift_s_camera_metadata.h new file mode 100644 index 000000000..db87febca --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_camera_metadata.h @@ -0,0 +1,104 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include +#include +#include + +#define RIFT_S_METADATA_SIZE 50 +#define RIFT_S_METADATA_WIDTH (RIFT_S_METADATA_SIZE * 8 * 8) + +struct rift_s_frame_metadata +{ + uint8_t frame_type; + uint16_t frame_ctr; + uint64_t frame_ts_us; + uint32_t frame_ctr2; + uint16_t exposure[5]; + uint8_t gain[5]; +}; + +enum rift_s_metadata_status +{ + RIFT_S_METADATA_OK, + RIFT_S_METADATA_BAD_LAYOUT, + RIFT_S_METADATA_BAD_MAGIC, + RIFT_S_METADATA_BAD_CRC, +}; + +static inline uint16_t +rift_s_metadata_crc(const uint8_t *data, size_t len) +{ + uint16_t crc = 0xffff; + for (size_t i = 0; i < len; i++) { + crc ^= (uint16_t)data[i] << 8; + for (int bit = 0; bit < 8; bit++) { + crc = (uint16_t)((crc << 1) ^ ((crc & 0x8000) ? 0x1021 : 0)); + } + } + return crc; +} + +static inline uint64_t +rift_s_metadata_le(const uint8_t *data, size_t len) +{ + uint64_t result = 0; + for (size_t i = 0; i < len; i++) { + result |= (uint64_t)data[i] << (8 * i); + } + return result; +} + +static inline enum rift_s_metadata_status +rift_s_metadata_decode(const uint8_t raw[RIFT_S_METADATA_SIZE], struct rift_s_frame_metadata *out) +{ + if (rift_s_metadata_le(raw + 1, 2) != 0xabcd || rift_s_metadata_le(raw + 48, 2) != 0xface) { + return RIFT_S_METADATA_BAD_MAGIC; + } + if (rift_s_metadata_crc(raw + 16, 30) != rift_s_metadata_le(raw + 46, 2)) { + return RIFT_S_METADATA_BAD_CRC; + } + + out->frame_type = raw[0]; + out->frame_ctr = (uint16_t)rift_s_metadata_le(raw + 3, 2); + out->frame_ts_us = rift_s_metadata_le(raw + 16, 8); + out->frame_ctr2 = (uint32_t)rift_s_metadata_le(raw + 24, 4); + for (int i = 0; i < 5; i++) { + out->exposure[i] = (uint16_t)rift_s_metadata_le(raw + 28 + 2 * i, 2); + out->gain[i] = raw[40 + i]; + } + return RIFT_S_METADATA_OK; +} + +// Metadata bits occupy 8x8 blocks. Sample their centers using the transport stride. +static inline enum rift_s_metadata_status +rift_s_metadata_extract( + const uint8_t *data, uint32_t width, uint32_t height, size_t stride, size_t size, struct rift_s_frame_metadata *out) +{ + if (data == NULL || width != RIFT_S_METADATA_WIDTH || height < 8 || stride < width || stride > SIZE_MAX / 4 || + size < width || 4 * stride > size - width) { + return RIFT_S_METADATA_BAD_LAYOUT; + } + uint8_t raw[RIFT_S_METADATA_SIZE] = {0}; + const uint8_t *row = data + 4 * stride; + for (size_t i = 0; i < RIFT_S_METADATA_SIZE * 8; i++) { + if (row[4 + 8 * i] > 128) { + raw[i / 8] |= 1u << (7 - i % 8); + } + } + return rift_s_metadata_decode(raw, out); +} + +// Unsigned subtraction accepts 65535 -> 0. Half-range jumps indicate a reset/backwards counter. +static inline bool +rift_s_metadata_counter_delta(uint16_t previous, uint16_t current, uint16_t *missing) +{ + uint16_t delta = (uint16_t)(current - previous); + *missing = 0; + if (delta == 0 || delta >= 0x8000) { + return false; + } + *missing = delta - 1; + return true; +} diff --git a/src/xrt/drivers/rift_s/rift_s_controller.c b/src/xrt/drivers/rift_s/rift_s_controller.c index bb8949d2f..cfb2f885f 100644 --- a/src/xrt/drivers/rift_s/rift_s_controller.c +++ b/src/xrt/drivers/rift_s/rift_s_controller.c @@ -22,11 +22,14 @@ #include #include "math/m_api.h" +#include "math/m_clock_tracking.h" +#include "math/m_predict.h" #include "math/m_space.h" #include "math/m_vec3.h" #include "os/os_hid.h" +#include "util/u_debug.h" #include "util/u_device.h" #include "util/u_trace_marker.h" #include "util/u_var.h" @@ -36,6 +39,16 @@ #include "rift_s_radio.h" #include "rift_s_protocol.h" #include "rift_s_controller.h" +#include "rift_s_model_check.h" +#include "rift_s_util.h" + +/* LED period written to both controllers. Monado streams controller exposures at 30 Hz + * (every second frame of the 60 Hz camera stream), so the Windows rule period = 1000000 / rate + * gives 33333 us. 0 only reads and logs the register. */ +// Position filter process noise overrides for field tuning; <= 0 keeps the defaults. +DEBUG_GET_ONCE_FLOAT_OPTION(rift_s_position_accel_noise, "RIFT_S_POSITION_ACCEL_NOISE", 0) +DEBUG_GET_ONCE_FLOAT_OPTION(rift_s_position_bias_walk, "RIFT_S_POSITION_BIAS_WALK", 0) +DEBUG_GET_ONCE_NUM_OPTION(rift_s_controller_led_period_us, "RIFT_S_CONTROLLER_LED_PERIOD_US", 1000000 / 30) /* Set to 1 to print controller states continuously */ #define DUMP_CONTROLLER_STATE 0 @@ -150,6 +163,762 @@ print_controller_state(struct rift_s_controller *ctrl) } #endif +static bool +finite_vec3(const struct xrt_vec3 *v) +{ + return isfinite(v->x) && isfinite(v->y) && isfinite(v->z); +} + +static bool +valid_quat(const struct xrt_quat *q) +{ + float norm = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w; + return isfinite(norm) && norm > 1e-6f; +} + +static const char * +hand_alias(const struct rift_s_controller *ctrl) +{ + return ctrl->device_type == RIFT_S_DEVICE_LEFT_CONTROLLER ? "left" : "right"; +} + +static struct rift_s_attitude_sample * +attitude_sample(struct rift_s_controller *ctrl, unsigned int index) +{ + return &ctrl->attitude_history[(ctrl->attitude_history_start + index) % RIFT_S_ATTITUDE_HISTORY_CAPACITY]; +} + +/* Forget the device-to-host clock mapping and everything captured against it. The next sample + * starts a new mapping. Caller holds the mutex. */ +static void +reset_clock_mapping(struct rift_s_controller *ctrl) +{ + if (ctrl->imu_clock != NULL) { + m_clock_windowed_skew_tracker_destroy(ctrl->imu_clock); + ctrl->imu_clock = NULL; + } + ctrl->imu_to_host_ns = 0; + ctrl->attitude_history_count = ctrl->attitude_history_start = 0; + ctrl->clock_stable_samples = 0; + ctrl->optical_heading_valid = false; + // Retained IMU times no longer match the optical capture times: hold the position. + rift_s_position_filter_freeze(&ctrl->position_filter); +} + +bool +rift_s_controller_record_attitude(struct rift_s_controller *ctrl, + timepoint_ns device_capture_ns, + timepoint_ns arrival_ns) +{ + if (!valid_quat(&ctrl->fusion.rot) || !finite_vec3(&ctrl->fusion.last.gyro)) { + ctrl->diagnostics.reject_nonfinite++; + return false; + } + if (device_capture_ns <= 0 || arrival_ns <= 0 || + (ctrl->attitude_history_count != 0 && + device_capture_ns <= attitude_sample(ctrl, ctrl->attitude_history_count - 1)->device_capture_ns)) { + ctrl->diagnostics.reject_clock++; + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_HISTORY_ORDER]++; + return false; + } + /* Transport delay is milliseconds. A sample a second or more off the mapping means the device + * clock itself jumped: its 32-bit microsecond counter wraps every 71.6 minutes, so a controller + * that sleeps longer loses whole wraps. The minimum-skew window would follow such a step only + * by 1/64 per sample, resetting the history on each of about 660 samples (r18 wake log). + * Start a new mapping at once. */ + if (ctrl->imu_clock != NULL && ctrl->imu_to_host_ns != 0) { + time_duration_ns error = arrival_ns - device_capture_ns - ctrl->imu_to_host_ns; + if (error > U_TIME_1S_IN_NS || error < -U_TIME_1S_IN_NS) { + RIFT_S_INFO("Controller IMU hand=%s clock_reanchor=1 mapping_error_ms=%.3f", hand_alias(ctrl), + (double)error / U_TIME_1MS_IN_NS); + reset_clock_mapping(ctrl); + ctrl->diagnostics.clock_resyncs++; + } + } + /* Radio reports arrive in bursts. A mean-delay filter follows every burst, moving capture + * times by milliseconds and occasionally past the discontinuity threshold below. Like the + * CV1 Touch driver, track the minimum transport delay over a window instead: it is stable + * under jitter and closest to the true capture time. */ + if (ctrl->imu_clock == NULL) { + ctrl->imu_clock = m_clock_windowed_skew_tracker_alloc(64); + if (ctrl->imu_clock == NULL) { + return false; + } + } + m_clock_windowed_skew_tracker_push(ctrl->imu_clock, arrival_ns, device_capture_ns); + timepoint_ns capture_ns; + if (!m_clock_windowed_skew_tracker_to_local(ctrl->imu_clock, device_capture_ns, &capture_ns)) { + ctrl->diagnostics.reject_clock++; + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_UNMAPPED]++; + return false; + } + time_duration_ns old_offset = ctrl->imu_to_host_ns; + ctrl->imu_to_host_ns = capture_ns - device_capture_ns; + time_duration_ns change = ctrl->imu_to_host_ns - old_offset; + if (old_offset != 0 && (change > 2 * U_TIME_1MS_IN_NS || change < -2 * U_TIME_1MS_IN_NS)) { + // A real discontinuity (the estimator restarted): earlier capture times no longer line up. + RIFT_S_INFO("Controller IMU hand=%s clock_discontinuity_ms=%.3f history_reset=1", hand_alias(ctrl), + (double)change / U_TIME_1MS_IN_NS); + ctrl->attitude_history_count = ctrl->clock_stable_samples = 0; + ctrl->optical_heading_valid = false; + // Retained IMU times no longer match the optical capture times: hold the position. + rift_s_position_filter_freeze(&ctrl->position_filter); + ctrl->diagnostics.reject_clock++; + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY]++; + } + if (change <= U_TIME_HALF_MS_IN_NS && change >= -U_TIME_HALF_MS_IN_NS) { + if (ctrl->clock_stable_samples < 32) { + ctrl->clock_stable_samples++; + } + } else { + ctrl->clock_stable_samples = 0; + } + if (capture_ns <= 0 || (ctrl->attitude_history_count != 0 && + capture_ns <= attitude_sample(ctrl, ctrl->attitude_history_count - 1)->capture_ns)) { + ctrl->diagnostics.reject_clock++; + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_MAPPED_ORDER]++; + return false; + } + if (ctrl->attitude_history_count == RIFT_S_ATTITUDE_HISTORY_CAPACITY) { + ctrl->attitude_history_start = (ctrl->attitude_history_start + 1) % RIFT_S_ATTITUDE_HISTORY_CAPACITY; + ctrl->attitude_history_count--; + } + struct rift_s_attitude_sample *sample = attitude_sample(ctrl, ctrl->attitude_history_count++); + *sample = (struct rift_s_attitude_sample){ + .device_capture_ns = device_capture_ns, + .capture_ns = capture_ns, + .arrival_ns = arrival_ns, + .orientation = ctrl->fusion.rot, + .gyro = ctrl->fusion.last.gyro, + .tilt_valid = ctrl->gravity_initialized && ctrl->clock_stable_samples >= 32, + }; + math_quat_normalize(&sample->orientation); + return true; +} + +bool +rift_s_controller_get_attitude(struct rift_s_controller *ctrl, + timepoint_ns capture_ns, + struct rift_s_attitude_sample *out) +{ + enum rift_s_attitude_rejection reject = RIFT_S_ATTITUDE_EMPTY; + if (capture_ns <= 0) { + reject = RIFT_S_ATTITUDE_INVALID_TIME; + goto rejected; + } + if (ctrl->attitude_history_count == 0) { + goto rejected; + } + struct rift_s_attitude_sample *first = attitude_sample(ctrl, 0); + struct rift_s_attitude_sample *last = attitude_sample(ctrl, ctrl->attitude_history_count - 1); + /* The camera thread can outrun the next 500 Hz radio report. A bounded gyro + * prediction bridges that scheduling gap; it never bridges a lost radio burst. */ + if (capture_ns > last->capture_ns && capture_ns - last->capture_ns <= 20 * U_TIME_1MS_IN_NS && + last->tilt_valid && ctrl->attitude_history_count >= 2) { + struct rift_s_attitude_sample *previous = attitude_sample(ctrl, ctrl->attitude_history_count - 2); + if (previous->tilt_valid && last->capture_ns - previous->capture_ns <= 10 * U_TIME_1MS_IN_NS) { + *out = *last; + out->capture_ns = capture_ns; + out->device_capture_ns += capture_ns - last->capture_ns; + math_quat_integrate_velocity(&last->orientation, &last->gyro, + (float)(capture_ns - last->capture_ns) / U_TIME_1S_IN_NS, + &out->orientation); + math_quat_normalize(&out->orientation); + return true; + } + } + if (capture_ns < first->capture_ns || capture_ns > last->capture_ns) { + reject = capture_ns < first->capture_ns ? RIFT_S_ATTITUDE_TOO_OLD : RIFT_S_ATTITUDE_TOO_NEW; + goto rejected; + } + for (unsigned int i = 0; i < ctrl->attitude_history_count; i++) { + struct rift_s_attitude_sample *right = attitude_sample(ctrl, i); + if (capture_ns > right->capture_ns) { + continue; + } + if (capture_ns == right->capture_ns) { + *out = *right; + if (right->tilt_valid) return true; + reject = RIFT_S_ATTITUDE_TILT_UNREADY; + goto rejected; + } + struct rift_s_attitude_sample *left = attitude_sample(ctrl, i - 1); + time_duration_ns dt = right->capture_ns - left->capture_ns; + // Never interpolate over dropped radio packets or a startup tilt reset. + if (!left->tilt_valid || !right->tilt_valid || dt > 10 * U_TIME_1MS_IN_NS) { + reject = !left->tilt_valid || !right->tilt_valid ? RIFT_S_ATTITUDE_TILT_UNREADY + : RIFT_S_ATTITUDE_RADIO_GAP; + goto rejected; + } + float t = (float)(capture_ns - left->capture_ns) / (float)dt; + *out = *left; + out->capture_ns = capture_ns; + out->device_capture_ns += capture_ns - left->capture_ns; + math_quat_slerp(&left->orientation, &right->orientation, t, &out->orientation); + out->gyro = m_vec3_add(m_vec3_mul_scalar(left->gyro, 1 - t), m_vec3_mul_scalar(right->gyro, t)); + return true; + } +rejected: + ctrl->diagnostics.attitude_rejected[reject]++; + ctrl->diagnostics.last_attitude_reject = reject; + ctrl->diagnostics.last_attitude_reject_ns = capture_ns; + return false; +} + +static const char *const attitude_rejection_names[] = { + "empty", "invalid_time", "before_history", "after_history", "tilt_unready", "radio_gap", +}; + +// Called with ctrl->mutex held. Keep failed capture queries visible without per-frame logging. +static void +log_tracking_health(struct rift_s_controller *ctrl, timepoint_ns now, bool transition) +{ + struct rift_s_fusion_diagnostics *d = &ctrl->diagnostics; + if (!transition && now - d->last_health_log_ns < 5 * (time_duration_ns)U_TIME_1S_IN_NS) return; + d->last_health_log_ns = now; + timepoint_ns oldest = ctrl->attitude_history_count ? attitude_sample(ctrl, 0)->capture_ns : 0; + timepoint_ns newest = ctrl->attitude_history_count + ? attitude_sample(ctrl, ctrl->attitude_history_count - 1)->capture_ns : 0; + RIFT_S_INFO("Controller health hand=%s transition=%d source=%s imu_age_ms=%.1f optical_age_ms=%.1f" + " calibrated=%d configured=%d gravity=%d accel_offset=%d accel_mps2=%.3f bias_valid=%d bias_mps2=%.3f" + " optical_tilt_updates=%u clock_stable=%u offset_ns=%" PRIi64 + " history=%u oldest_ns=%" PRIi64 " newest_ns=%" PRIi64 + " history_reject=%s rejected_capture_ns=%" PRIi64 + " attitude_empty=%" PRIu64 " attitude_invalid=%" PRIu64 " attitude_old=%" PRIu64 + " attitude_new=%" PRIu64 " attitude_tilt=%" PRIu64 " attitude_gap=%" PRIu64 + " optical_result=%s processing_ns=%" PRIi64 " accepted=%" PRIu64 + " reject_stale=%" PRIu64 " reject_nonfinite=%" PRIu64 " reject_clock=%" PRIu64 + " clock_resyncs=%u history_resets=%" PRIu64, + hand_alias(ctrl), transition, ctrl->position_source ? ctrl->position_source : "none", + ctrl->last_imu_local_time_ns ? (double)(now - ctrl->last_imu_local_time_ns) / U_TIME_1MS_IN_NS : -1, + ctrl->last_fix_ns ? (double)(now - ctrl->last_fix_ns) / U_TIME_1MS_IN_NS : -1, + ctrl->have_calibration, ctrl->have_config, ctrl->gravity_initialized, ctrl->accel_offset, + m_vec3_len(ctrl->accel), ctrl->attitude_bias.valid, m_vec3_len(ctrl->attitude_bias.bias), + ctrl->optical_tilt_updates, ctrl->clock_stable_samples, ctrl->imu_to_host_ns, + ctrl->attitude_history_count, oldest, newest, + d->last_attitude_reject_ns ? attitude_rejection_names[d->last_attitude_reject] : "none", + d->last_attitude_reject_ns, d->attitude_rejected[0], d->attitude_rejected[1], + d->attitude_rejected[2], d->attitude_rejected[3], d->attitude_rejected[4], d->attitude_rejected[5], + d->last_optical_result ? d->last_optical_result : "none", d->last_optical_processing_ns, + d->optical_accepted, d->reject_stale, d->reject_nonfinite, d->reject_clock, d->clock_resyncs, + d->reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY]); +} + +bool +rift_s_controller_has_heading_prior(struct rift_s_controller *ctrl, timepoint_ns capture_ns) +{ + struct rift_s_attitude_sample sample; + return ctrl->optical_heading_valid && ctrl->last_tracked_pose_ts > 0 && + capture_ns >= ctrl->last_tracked_pose_ts && + capture_ns - ctrl->last_tracked_pose_ts <= 500 * U_TIME_1MS_IN_NS && + capture_ns >= ctrl->last_optical_yaw_ts && + capture_ns - ctrl->last_optical_yaw_ts <= RIFT_S_HEADING_PRIOR_MAX_AGE_NS && + rift_s_controller_get_attitude(ctrl, capture_ns, &sample); +} + +bool +rift_s_rest_stats_push(struct rift_s_rest_stats *s, + timepoint_ns timestamp_ns, + const struct xrt_vec3 *accel, + const struct xrt_vec3 *gyro) +{ + float length = m_vec3_len(*accel); + bool usable = isfinite(length) && length > 7.0f && length < 12.5f && m_vec3_len(*gyro) < 0.1f; + bool continues = usable && s->samples != 0 && timestamp_ns - s->last_ns <= 10 * U_TIME_1MS_IN_NS; + if (continues) { + struct xrt_vec3 mean = m_vec3_mul_scalar(s->sum, 1.0f / s->samples); + continues = m_vec3_len(m_vec3_sub(*accel, mean)) < 0.3f; + } + s->last_ns = timestamp_ns; + if (!continues) { + s->samples = 0; + s->sum = (struct xrt_vec3){0}; + s->start_ns = timestamp_ns; + } + if (!usable) { + return false; + } + s->sum = m_vec3_add(s->sum, *accel); + s->samples++; + if (timestamp_ns - s->start_ns < 250 * U_TIME_1MS_IN_NS) { + return false; + } + struct xrt_vec3 mean = m_vec3_mul_scalar(s->sum, 1.0f / s->samples); + s->last_mean = mean; + float g = m_vec3_len(mean); + math_vec3_normalize(&mean); + if (s->windows == 0 || g < s->min_mps2) { + s->min_mps2 = g; + s->min_dir = mean; + } + if (s->windows == 0 || g > s->max_mps2) { + s->max_mps2 = g; + s->max_dir = mean; + } + s->windows++; + s->samples = 0; + s->sum = (struct xrt_vec3){0}; + s->start_ns = timestamp_ns; + return true; +} + +static float +accel_bias_rms(const struct rift_s_accel_bias *b, const struct xrt_vec3 *bias) +{ + float sum = 0; + for (unsigned int i = 0; i < b->count; i++) { + float r = m_vec3_len(m_vec3_sub(b->means[i], *bias)) - (float)MATH_GRAVITY_M_S2; + sum += r * r; + } + return b->count ? sqrtf(sum / b->count) : 0; +} + +void +rift_s_accel_bias_add_window(struct rift_s_accel_bias *b, const struct xrt_vec3 *mean) +{ + float length = m_vec3_len(*mean); + if (!isfinite(length) || length < 1.0f) { + return; + } + struct xrt_vec3 dir = m_vec3_mul_scalar(*mean, 1.0f / length); + // Keep the newest window per direction; when full, replace the window nearest in direction so + // the stored set keeps its spread. + unsigned int slot = b->count; + float nearest = -2; + for (unsigned int i = 0; i < b->count; i++) { + float c = m_vec3_dot(dir, m_vec3_normalize(b->means[i])); + if (c > nearest) { + nearest = c; + slot = i; + } + } + if (b->count < RIFT_S_ACCEL_BIAS_WINDOWS && nearest < cosf(DEG_TO_RAD(15.0f))) { + slot = b->count++; + } + b->means[slot] = *mean; + + float spread = 1; + for (unsigned int i = 0; i < b->count; i++) { + for (unsigned int j = i + 1; j < b->count; j++) { + spread = + fminf(spread, m_vec3_dot(m_vec3_normalize(b->means[i]), m_vec3_normalize(b->means[j]))); + } + } + b->spread_deg = RAD_TO_DEG(acosf(fmaxf(-1.0f, fminf(1.0f, spread)))); + + /* Gauss-Newton on sum((|m - b| - g) / 0.05)^2 + |b / 1.0|^2: window means are good to a few + * hundredths of m/s^2, and a bias beyond 1 m/s^2 must be observed to be believed. */ + const double inv_window_var = 1.0 / (0.05 * 0.05), inv_prior_var = 1.0; + double x[3] = {0, 0, 0}; + for (int iteration = 0; iteration < 10; iteration++) { + double h[3][3] = {{inv_prior_var, 0, 0}, {0, inv_prior_var, 0}, {0, 0, inv_prior_var}}; + double g[3] = {inv_prior_var * x[0], inv_prior_var * x[1], inv_prior_var * x[2]}; + for (unsigned int i = 0; i < b->count; i++) { + double d[3] = {b->means[i].x - x[0], b->means[i].y - x[1], b->means[i].z - x[2]}; + double n = sqrt(d[0] * d[0] + d[1] * d[1] + d[2] * d[2]); + if (n < 1e-6) { + continue; + } + double r = n - MATH_GRAVITY_M_S2; + double j[3] = {-d[0] / n, -d[1] / n, -d[2] / n}; + for (int k = 0; k < 3; k++) { + g[k] += inv_window_var * j[k] * r; + for (int l = 0; l < 3; l++) { + h[k][l] += inv_window_var * j[k] * j[l]; + } + } + } + struct xrt_matrix_3x3 m = {{(float)h[0][0], (float)h[0][1], (float)h[0][2], (float)h[1][0], + (float)h[1][1], (float)h[1][2], (float)h[2][0], (float)h[2][1], + (float)h[2][2]}}; + struct xrt_matrix_3x3 inverse; + math_matrix_3x3_inverse(&m, &inverse); + struct xrt_vec3 gradient = {(float)g[0], (float)g[1], (float)g[2]}, step; + math_matrix_3x3_transform_vec3(&inverse, &gradient, &step); + if (!finite_vec3(&step)) { + return; + } + x[0] -= step.x; + x[1] -= step.y; + x[2] -= step.z; + if (m_vec3_len(step) < 1e-4f) { + break; + } + } + struct xrt_vec3 bias = {(float)x[0], (float)x[1], (float)x[2]}, zero = {0}; + b->rms_before_mps2 = accel_bias_rms(b, &zero); + b->rms_after_mps2 = accel_bias_rms(b, &bias); + // Three directions at least 45 degrees apart, a fit to within 0.1 m/s^2, a plausible size. + b->valid = b->count >= 3 && b->spread_deg >= 45 && b->rms_after_mps2 < 0.1f && m_vec3_len(bias) < 3.0f; + if (b->valid) { + b->bias = bias; + } +} + +// Startup gravity magnitude error above which the accelerometer is treated as offset. +#define RIFT_S_ACCEL_OFFSET_MPS2 0.4f +// Share of the optical tilt difference applied per reliable optical fix while the accelerometer +// is offset: about one second to level, while smoothing the 2-3 degree tilt noise of single fits. +#define RIFT_S_OPTICAL_TILT_FRACTION 0.05f +// Optical levelling counts as settled after this many fixes with a smoothed residual below this. +#define RIFT_S_OPTICAL_TILT_SETTLED_UPDATES 30 +#define RIFT_S_OPTICAL_TILT_SETTLED_DEG 3.0f +#define RIFT_S_OPTICAL_TILT_MAX_AGE_NS (300 * U_TIME_1MS_IN_NS) +// Attitude bias samples must agree to this RMS, and the result must be plausible. +#define RIFT_S_ATTITUDE_BIAS_MAX_RMS_MPS2 0.6f +#define RIFT_S_ATTITUDE_BIAS_MAX_MPS2 3.0f + +void +rift_s_attitude_bias_add(struct rift_s_attitude_bias *b, + const struct xrt_vec3 *rest_mean, + const struct xrt_quat *attitude) +{ + // Gravity's reaction in the IMU frame: world up rotated by the inverse attitude. + struct xrt_quat inverse; + math_quat_invert(attitude, &inverse); + const struct xrt_vec3 up = {0, (float)MATH_GRAVITY_M_S2, 0}; + struct xrt_vec3 expected; + math_quat_rotate_vec3(&inverse, &up, &expected); + b->samples[b->count % RIFT_S_ATTITUDE_BIAS_SAMPLES] = m_vec3_sub(*rest_mean, expected); + b->count++; + unsigned int n = b->count < RIFT_S_ATTITUDE_BIAS_SAMPLES ? b->count : RIFT_S_ATTITUDE_BIAS_SAMPLES; + struct xrt_vec3 mean = {0}; + for (unsigned int i = 0; i < n; i++) { + mean = m_vec3_add(mean, m_vec3_mul_scalar(b->samples[i], 1.0f / n)); + } + float sq = 0; + for (unsigned int i = 0; i < n; i++) { + struct xrt_vec3 d = m_vec3_sub(b->samples[i], mean); + sq += m_vec3_dot(d, d) / n; + } + b->bias = mean; + b->rms_mps2 = sqrtf(sq); + b->valid = n >= RIFT_S_ATTITUDE_BIAS_MIN_SAMPLES && b->rms_mps2 < RIFT_S_ATTITUDE_BIAS_MAX_RMS_MPS2 && + m_vec3_len(mean) < RIFT_S_ATTITUDE_BIAS_MAX_MPS2; +} + +float +rift_s_optical_tilt_correction(const struct xrt_quat *imu, + const struct xrt_quat *optical, + float fraction, + struct xrt_quat *out_correction) +{ + // The device's up direction according to the optical fit, seen through the IMU attitude. + const struct xrt_vec3 up = {0, 1, 0}; + struct xrt_quat optical_inverse; + math_quat_invert(optical, &optical_inverse); + struct xrt_vec3 device_up, v; + math_quat_rotate_vec3(&optical_inverse, &up, &device_up); + math_quat_rotate_vec3(imu, &device_up, &v); + // Rotate v towards up about a horizontal axis: tilt only, heading unchanged. + struct xrt_vec3 axis; + math_vec3_cross(&v, &up, &axis); + float sin_angle = m_vec3_len(axis); + float angle = atan2f(sin_angle, m_vec3_dot(v, up)); + *out_correction = (struct xrt_quat)XRT_QUAT_IDENTITY; + if (sin_angle > 1e-6f) { + axis = m_vec3_mul_scalar(axis, 1.0f / sin_angle); + math_quat_from_angle_vector(fraction * angle, &axis, out_correction); + } + return angle; +} + +static void +set_fusion_gravity(struct rift_s_controller *ctrl, bool enabled) +{ + if (enabled) { + ctrl->fusion.flags |= M_IMU_3DOF_USE_GRAVITY_DUR_20MS; + } else { + ctrl->fusion.flags &= ~M_IMU_3DOF_USE_GRAVITY_DUR_20MS; + // Drop any tilt correction already measured from the offset reading. + ctrl->fusion.grav.error_angle = 0; + } +} + +/* A radio gap has no measured angular velocity. Preserve the last finite attitude, + * but rebuild capture-time support and require a fresh optical heading. */ +static void +reset_fusion_interval(struct rift_s_controller *ctrl, const struct xrt_quat *orientation) +{ + m_imu_3dof_reset(&ctrl->fusion); + ctrl->fusion.rot = *orientation; + ctrl->pose.orientation = *orientation; + ctrl->attitude_history_count = ctrl->attitude_history_start = 0; + ctrl->clock_stable_samples = 0; + ctrl->optical_heading_valid = false; + ctrl->world_accel_valid = false; + ctrl->gravity_samples = 0; + ctrl->gravity_accel_sum = ctrl->gravity_gyro_sum = (struct xrt_vec3){0}; + ctrl->gravity_moving = false; + ctrl->rest = (struct rift_s_rest_stats){0}; + rift_s_position_filter_freeze(&ctrl->position_filter); +} + +static bool +bounded_imu_vector(const struct xrt_vec3 *v, double max_length) +{ + return finite_vec3(v) && + (double)v->x * v->x + (double)v->y * v->y + (double)v->z * v->z <= max_length * max_length; +} + +void +rift_s_controller_update_fusion(struct rift_s_controller *ctrl, timepoint_ns timestamp_ns) +{ + // These limits exceed normal hand motion and avoid overflow from corrupt calibration. + if (!bounded_imu_vector(&ctrl->accel, 2000.0) || !bounded_imu_vector(&ctrl->gyro, 100.0)) { + ctrl->diagnostics.reject_nonfinite++; + ctrl->world_accel_valid = false; + return; + } + if (!valid_quat(&ctrl->fusion.rot)) { + const struct xrt_quat identity = {0, 0, 0, 1}; + reset_fusion_interval(ctrl, &identity); + ctrl->gravity_initialized = false; + } + if (ctrl->fusion.last.timestamp_ns != 0 && (timestamp_ns < (timepoint_ns)ctrl->fusion.last.timestamp_ns || + timestamp_ns - ctrl->fusion.last.timestamp_ns > U_TIME_1S_IN_NS)) { + struct xrt_quat orientation = ctrl->fusion.rot; + RIFT_S_INFO("Controller IMU hand=%s fusion_gap_ms=%.1f; discarding missing motion", hand_alias(ctrl), + (double)(timestamp_ns - ctrl->fusion.last.timestamp_ns) / U_TIME_1MS_IN_NS); + reset_fusion_interval(ctrl, &orientation); + } + if (!ctrl->gravity_initialized) { + set_fusion_gravity(ctrl, false); + /* Average specific force in the gyro-integrated frame. Averaging sensor axes + * while the hand rotates cancels gravity; requiring 250 ms without motion + * prevents acquisition altogether. Outliers have bounded influence. */ + float length = m_vec3_len(ctrl->accel); + if (timestamp_ns - ctrl->fusion.last.timestamp_ns > 50 * U_TIME_1MS_IN_NS) { + ctrl->gravity_samples = 0; + ctrl->gravity_accel_sum = ctrl->gravity_gyro_sum = (struct xrt_vec3){0}; + ctrl->gravity_moving = false; + } + if (length > 7.5f && length < 12.0f) { + struct xrt_vec3 world; + math_quat_rotate_vec3(&ctrl->fusion.rot, &ctrl->accel, &world); + if (ctrl->gravity_samples == 0) { + ctrl->gravity_start_ns = timestamp_ns; + } else { + struct xrt_vec3 mean = m_vec3_mul_scalar(ctrl->gravity_accel_sum, 1.f / ctrl->gravity_samples); + struct xrt_vec3 residual = m_vec3_sub(world, mean); + float error = m_vec3_len(residual); + ctrl->gravity_moving |= error > .3f; + if (error > 1.5f) + world = m_vec3_add(mean, m_vec3_mul_scalar(residual, 1.5f / error)); + } + ctrl->gravity_moving |= m_vec3_len(ctrl->gyro) >= .1f; + ctrl->gravity_accel_sum = m_vec3_add(ctrl->gravity_accel_sum, world); + ctrl->gravity_gyro_sum = m_vec3_add(ctrl->gravity_gyro_sum, ctrl->gyro); + ctrl->gravity_samples++; + if (ctrl->gravity_samples >= 64 && timestamp_ns - ctrl->gravity_start_ns >= 250 * U_TIME_1MS_IN_NS) { + struct xrt_vec3 mean = m_vec3_mul_scalar(ctrl->gravity_accel_sum, 1.f / ctrl->gravity_samples); + ctrl->gravity_accel_scale = MATH_GRAVITY_M_S2 / m_vec3_len(mean); + ctrl->accel_offset = ctrl->gravity_moving || + fabsf(m_vec3_len(mean) - (float)MATH_GRAVITY_M_S2) > RIFT_S_ACCEL_OFFSET_MPS2; + math_vec3_normalize(&mean); + const struct xrt_vec3 up = {0, 1, 0}; + struct xrt_quat correction; + math_quat_from_vec_a_to_vec_b(&mean, &up, &correction); + math_quat_rotate(&correction, &ctrl->fusion.rot, &ctrl->fusion.rot); + math_quat_normalize(&ctrl->fusion.rot); + for (unsigned i = 0; i < ctrl->attitude_history_count; i++) { + struct rift_s_attitude_sample *sample = attitude_sample(ctrl, i); + math_quat_rotate(&correction, &sample->orientation, &sample->orientation); + math_quat_normalize(&sample->orientation); + sample->tilt_valid = ctrl->clock_stable_samples >= 32; + } + set_fusion_gravity(ctrl, !ctrl->accel_offset); + ctrl->fusion.grav.error_angle = 0; + ctrl->fusion.grav.level_timestamp_ns = timestamp_ns; + if (!ctrl->gravity_moving) + ctrl->fusion.gyro_bias.value = m_vec3_mul_scalar(ctrl->gravity_gyro_sum, 1.f / ctrl->gravity_samples); + ctrl->gravity_initialized = true; + RIFT_S_INFO("Controller gravity initialized hand=%s samples=%u moving=%d accel_mps2=%.3f", + hand_alias(ctrl), ctrl->gravity_samples, ctrl->gravity_moving, + MATH_GRAVITY_M_S2 / ctrl->gravity_accel_scale); + } + } + } + if (rift_s_rest_stats_push(&ctrl->rest, timestamp_ns, &ctrl->accel, &ctrl->gyro)) { + bool was_valid = ctrl->accel_bias.valid; + rift_s_accel_bias_add_window(&ctrl->accel_bias, &ctrl->rest.last_mean); + // With the attitude levelled by optical tilt, each window gives the whole offset. + bool settled = ctrl->accel_offset && + ctrl->optical_tilt_updates >= RIFT_S_OPTICAL_TILT_SETTLED_UPDATES && + ctrl->optical_tilt_residual_deg < RIFT_S_OPTICAL_TILT_SETTLED_DEG && + ctrl->last_imu_capture_ns - ctrl->last_optical_tilt_ns <= RIFT_S_OPTICAL_TILT_MAX_AGE_NS; + if (settled) { + rift_s_attitude_bias_add(&ctrl->attitude_bias, &ctrl->rest.last_mean, &ctrl->fusion.rot); + } + if (ctrl->accel_bias.valid && + (!was_valid || timestamp_ns - ctrl->last_bias_log_ns >= 10 * (timepoint_ns)U_TIME_1S_IN_NS)) { + ctrl->last_bias_log_ns = timestamp_ns; + const struct rift_s_accel_bias *b = &ctrl->accel_bias; + RIFT_S_DEBUG( + "Controller accel bias hand=%s bias_mps2=%.3f,%.3f,%.3f windows=%u spread_deg=%.0f" + " rest_rms_mps2=%.3f->%.3f", + hand_alias(ctrl), b->bias.x, b->bias.y, b->bias.z, b->count, b->spread_deg, + b->rms_before_mps2, b->rms_after_mps2); + } + } + /* Keep the acceleration gate active during motion. With an estimated bias, the corrected + * reading is gravity at rest in every orientation. Before that, the startup average + * estimates a scale; normalizing every packet would hide acceleration. */ + const struct xrt_vec3 *bias = ctrl->accel_bias.valid ? &ctrl->accel_bias.bias + : ctrl->attitude_bias.valid ? &ctrl->attitude_bias.bias + : NULL; + if (ctrl->accel_offset && bias != NULL) { + // The offset is known: the corrected reading is gravity again, level to it. + ctrl->accel_offset = false; + set_fusion_gravity(ctrl, true); + RIFT_S_DEBUG( + "Controller accel hand=%s state=corrected source=%s bias_mps2=%.3f,%.3f,%.3f" + " attitude_samples=%u attitude_rms_mps2=%.3f", + hand_alias(ctrl), ctrl->accel_bias.valid ? "rest_spread" : "optical_attitude", bias->x, bias->y, + bias->z, ctrl->attitude_bias.count, ctrl->attitude_bias.rms_mps2); + } + struct xrt_vec3 accel = + bias != NULL ? m_vec3_sub(ctrl->accel, *bias) + : m_vec3_mul_scalar(ctrl->accel, ctrl->gravity_initialized ? ctrl->gravity_accel_scale : 1.0f); + struct xrt_quat previous = ctrl->fusion.rot; + m_imu_3dof_update(&ctrl->fusion, timestamp_ns, &accel, &ctrl->gyro); + if (!valid_quat(&ctrl->fusion.rot)) { + ctrl->diagnostics.reject_nonfinite++; + reset_fusion_interval(ctrl, &previous); + return; + } + ctrl->pose.orientation = ctrl->fusion.rot; + + // Specific force in the world frame minus gravity drives the position filter. + ctrl->world_accel_valid = ctrl->gravity_initialized && valid_quat(&ctrl->fusion.rot); + if (ctrl->world_accel_valid) { + math_quat_rotate_vec3(&ctrl->fusion.rot, &accel, &ctrl->world_accel); + ctrl->world_accel.y -= MATH_GRAVITY_M_S2; + } +} + +// Stop waiting for the IMU descriptor and run with the factory gyro calibration. A descriptor +// that arrives later is still applied. Caller holds the mutex. +static void +imu_descriptor_give_up(struct rift_s_controller *ctrl, const char *reason) +{ + if (ctrl->imu_descriptor_done) + return; + ctrl->imu_descriptor_done = true; + RIFT_S_WARN("No controller IMU descriptor hand=%s device=%016" PRIx64 " reason=%s; keeping factory calibration", + hand_alias(ctrl), ctrl->device_id, reason); +} + +static void +reset_position_filter(struct rift_s_controller *ctrl) +{ + rift_s_position_filter_reset(&ctrl->position_filter); + ctrl->reacquire_offset = (struct xrt_vec3){0}; + ctrl->reacquire_started_ns = 0; + float accel_noise = debug_get_float_option_rift_s_position_accel_noise(); + float bias_walk = debug_get_float_option_rift_s_position_bias_walk(); + if (accel_noise > 0) { + ctrl->position_filter.accel_noise_density = accel_noise; + } + if (bias_walk > 0) { + ctrl->position_filter.bias_random_walk = bias_walk; + } +} + +// Fixes are captured every 33 ms and delivered 40-60 ms later: without one for 120 ms (two +// missed exposures plus delivery latency) the filter coasts on the IMU alone. +#define RIFT_S_COAST_AFTER_NS (120 * U_TIME_1MS_IN_NS) +// Innovation that re-initializes the filter instead of fusing: the 0.15 m broad association +// gate the Windows tracker uses by default. +#define RIFT_S_POSITION_RESET_DISTANCE_M 0.15f +// Longest render-time extrapolation of aim/grip. +#define RIFT_S_MAX_PREDICTION_NS (100 * U_TIME_1MS_IN_NS) +// Resume the optical filter immediately, but remove the held-pose display offset over 100 ms. +#define RIFT_S_REACQUIRE_BLEND_NS (100 * U_TIME_1MS_IN_NS) + +enum position_tracking +{ + POSITION_NONE, // never fixed: no position estimate + POSITION_TRACKED, // fresh optical fix + POSITION_COASTING, // IMU-only, within the position duration of the loss ladder + POSITION_LOST, // held at the last estimate, waiting for an optical reset +}; + +static enum position_tracking +position_tracking_at(const struct rift_s_controller *ctrl, timepoint_ns t, bool *velocity_tracked) +{ + *velocity_tracked = false; + if (!ctrl->position_filter.initialized) { + return POSITION_NONE; + } + if (ctrl->position_filter.frozen) { + return POSITION_LOST; + } + time_duration_ns position_duration, velocity_duration; + rift_s_tracking_loss_durations(ctrl->track_vision_updates, ctrl->track_reliable_updates, &position_duration, + &velocity_duration); + time_duration_ns age = t > ctrl->last_fix_ns ? t - ctrl->last_fix_ns : 0; + if (position_duration > RIFT_S_POSITION_FILTER_BRIDGE_NS) + position_duration = RIFT_S_POSITION_FILTER_BRIDGE_NS; + if (velocity_duration > position_duration) + velocity_duration = position_duration; + *velocity_tracked = age <= velocity_duration; + if (age > position_duration) { + return POSITION_LOST; + } + return age > RIFT_S_COAST_AFTER_NS ? POSITION_COASTING : POSITION_TRACKED; +} + +// Apply the loss ladder at IMU time: coast, then hold the position once it is lost. +static void +update_position_tracking(struct rift_s_controller *ctrl, timepoint_ns capture_ns) +{ + bool velocity_tracked; + enum position_tracking state = position_tracking_at(ctrl, capture_ns, &velocity_tracked); + struct rift_s_position_filter *f = &ctrl->position_filter; + f->coasting = state == POSITION_COASTING; + if (state == POSITION_LOST && !f->frozen) { + rift_s_position_filter_freeze(f); + RIFT_S_DEBUG("Controller position hand=%s state=lost age_ms=%.1f vision_updates=%u reliable_updates=%u" + " held=%.3f,%.3f,%.3f", + hand_alias(ctrl), (double)(capture_ns - ctrl->last_fix_ns) / U_TIME_1MS_IN_NS, + ctrl->track_vision_updates, ctrl->track_reliable_updates, f->state.x[0][0], + f->state.x[1][0], f->state.x[2][0]); + ctrl->track_vision_updates = ctrl->track_reliable_updates = 0; + } +} + +bool +rift_s_imu_clock_needs_resync(uint32_t backwards_us, uint32_t backwards_run) +{ + // A late resend steps back a few ms once. A run of rejects, or two in a row more than a + // second back, means the accepted anchor was wrong (e.g. a stale timestamp after a host + // reset without a headset power cycle) and every later sample would be rejected. + return backwards_run >= RIFT_S_IMU_RESYNC_RUN || + (backwards_run >= 2 && backwards_us > RIFT_S_IMU_RESYNC_BACKWARDS_US); +} + +/* Re-anchor the 32-bit device clock on @p imu_timestamp. The 64-bit device time continues from + * the last accepted sample by the host arrival gap, so fusion time keeps moving forward; the + * device-to-host mapping and everything captured against it are rebuilt as after a clock + * discontinuity. Caller holds the mutex. */ +static void +resync_imu_clock(struct rift_s_controller *ctrl, uint32_t imu_timestamp, timepoint_ns local_ts) +{ + struct rift_s_fusion_diagnostics *d = &ctrl->diagnostics; + time_duration_ns gap = local_ts - ctrl->last_imu_local_time_ns; + if (gap < (time_duration_ns)OS_NS_PER_USEC) { + gap = OS_NS_PER_USEC; + } + RIFT_S_INFO("Controller IMU hand=%s clock_resync=1 backwards_us=%u run=%u previous_us=%u new_us=%u", + hand_alias(ctrl), ctrl->imu_timestamp32 - imu_timestamp, d->backwards_run, ctrl->imu_timestamp32, + imu_timestamp); + ctrl->last_imu_device_time_ns += gap; + d->backwards_run = 0; + d->clock_resyncs++; + reset_clock_mapping(ctrl); +} + static void handle_imu_update(struct rift_s_controller *ctrl, timepoint_ns local_ts, @@ -167,21 +936,56 @@ handle_imu_update(struct rift_s_controller *ctrl, /* Sometimes we see 1-2 repeated IMU updates from a controller, * that must be ignored or else time jumps wildly */ if (dt == 0 || dt > 2147483648) { - RIFT_S_TRACE("Controller %" PRIx64 " - ignoring repeated IMU update", ctrl->device_id); - return; + struct rift_s_fusion_diagnostics *d = &ctrl->diagnostics; + if (dt == 0) { + d->reject_duplicate++; + } else { + d->reject_clock++; + d->reject_clock_by[RIFT_S_CLOCK_BACKWARDS]++; + d->backwards_run++; + /* An old sample arriving late steps back by a few ms after normal steps; a + * corrupted timestamp shows as one large accepted step followed by a run of + * backwards samples. */ + if (local_ts - d->last_clock_log_ns >= U_TIME_1S_IN_NS) { + d->last_clock_log_ns = local_ts; + RIFT_S_DEBUG( + "Controller IMU hand=%s reject=clock_backwards backwards_us=%u run=%u" + " last_accepted_step_us=%u arrival_since_accepted_ms=%.1f", + hand_alias(ctrl), (uint32_t)(0u - dt), d->backwards_run, d->last_step_us, + (double)(local_ts - ctrl->last_imu_local_time_ns) / U_TIME_1MS_IN_NS); + } + } + if (dt != 0 && rift_s_imu_clock_needs_resync(0u - dt, ctrl->diagnostics.backwards_run)) { + resync_imu_clock(ctrl, imu_timestamp, local_ts); + dt = 0; + } else { + RIFT_S_TRACE("Controller IMU hand=%s reject=%s capture_us=%u arrival_ns=%" PRIi64, + hand_alias(ctrl), dt == 0 ? "duplicate" : "clock", imu_timestamp, + local_ts); + return; + } + } else { + ctrl->diagnostics.last_step_us = dt; + ctrl->diagnostics.backwards_run = 0; + ctrl->last_imu_device_time_ns += (timepoint_ns)dt * OS_NS_PER_USEC; } - - ctrl->last_imu_device_time_ns += (timepoint_ns)dt * OS_NS_PER_USEC; } else { ctrl->last_imu_device_time_ns = (timepoint_ns)imu_timestamp * OS_NS_PER_USEC; ctrl->imu_time_valid = true; } + bool log_imu = local_ts / U_TIME_1S_IN_NS != ctrl->last_imu_local_time_ns / U_TIME_1S_IN_NS; ctrl->imu_timestamp32 = imu_timestamp; ctrl->last_imu_local_time_ns = local_ts; if (!ctrl->have_calibration || !ctrl->have_config) return; /* We need to finish reading the calibration or config blocks first */ + if (!ctrl->imu_descriptor_done) { + if (local_ts < ctrl->imu_descriptor_deadline_ns) + return; /* Wait briefly for the IMU descriptor: it selects the gyro correction */ + imu_descriptor_give_up(ctrl, "timeout"); + } + const float gyro_scale = ctrl->config.gyro_scale; const float accel_scale = MATH_GRAVITY_M_S2 * ctrl->config.accel_scale; @@ -195,15 +999,43 @@ handle_imu_update(struct rift_s_controller *ctrl, accel.y = accel_scale * raw_accel[1]; accel.z = accel_scale * raw_accel[2]; - /* Apply correction offsets first, then rectify */ - accel = m_vec3_sub(accel, ctrl->calibration.accel.offset); - gyro = m_vec3_sub(gyro, ctrl->calibration.gyro.offset); + /* Factory TrackedObject matrices map sensor samples into the LED model frame. + * Their biases are in sensor units and are subtracted before the row-major matrix. */ + accel = m_vec3_sub(accel, ctrl->calibration.accel_calibration.offset); + gyro = m_vec3_sub(gyro, ctrl->gyro_calibration.offset); - math_matrix_3x3_transform_vec3(&ctrl->calibration.accel.rectification, &accel, &ctrl->accel); - math_matrix_3x3_transform_vec3(&ctrl->calibration.gyro.rectification, &gyro, &ctrl->gyro); + math_matrix_3x3_transform_vec3(&ctrl->calibration.accel_calibration.matrix, &accel, &ctrl->accel); + math_matrix_3x3_transform_vec3(&ctrl->gyro_calibration.matrix, &gyro, &ctrl->gyro); - m_imu_3dof_update(&ctrl->fusion, ctrl->last_imu_device_time_ns, &ctrl->accel, &ctrl->gyro); - ctrl->pose.orientation = ctrl->fusion.rot; + if (!finite_vec3(&ctrl->accel) || !finite_vec3(&ctrl->gyro)) { + ctrl->diagnostics.reject_nonfinite++; + return; + } + rift_s_controller_update_fusion(ctrl, ctrl->last_imu_device_time_ns); + if (ctrl->fusion.last.timestamp_ns != (uint64_t)ctrl->last_imu_device_time_ns) + return; // Rejected samples must not acquire capture-time history. + if (rift_s_controller_record_attitude(ctrl, ctrl->last_imu_device_time_ns, local_ts)) { + timepoint_ns capture_ns = attitude_sample(ctrl, ctrl->attitude_history_count - 1)->capture_ns; + ctrl->last_imu_capture_ns = capture_ns; + if (ctrl->world_accel_valid) { + update_position_tracking(ctrl, capture_ns); + rift_s_position_filter_imu(&ctrl->position_filter, capture_ns, &ctrl->world_accel); + } + } + if (log_imu) { + RIFT_S_DEBUG("Controller IMU sample hand=%s capture_us=%u raw_accel=%d,%d,%d raw_gyro=%d,%d,%d" + " accel_mps2=%.6f,%.6f,%.6f gyro_rps=%.6f,%.6f,%.6f", + hand_alias(ctrl), imu_timestamp, raw_accel[0], raw_accel[1], raw_accel[2], raw_gyro[0], + raw_gyro[1], raw_gyro[2], ctrl->accel.x, ctrl->accel.y, ctrl->accel.z, ctrl->gyro.x, + ctrl->gyro.y, ctrl->gyro.z); + RIFT_S_TRACE("Controller IMU device=%d ts=%u local=%" PRIu64 " mapped_ns=%" PRIi64 " offset_ns=%" PRIi64 + " history=%u dt_us=%u accel=%f,%f,%f gyro=%f,%f,%f fusion=%f,%f,%f,%f", + ctrl->base.device_type, imu_timestamp, local_ts, + ctrl->last_imu_device_time_ns + ctrl->imu_to_host_ns, ctrl->imu_to_host_ns, + ctrl->attitude_history_count, dt, ctrl->accel.x, ctrl->accel.y, ctrl->accel.z, + ctrl->gyro.x, ctrl->gyro.y, ctrl->gyro.z, ctrl->fusion.rot.x, ctrl->fusion.rot.y, + ctrl->fusion.rot.z, ctrl->fusion.rot.w); + } #if 0 RIFT_S_DEBUG("%" PRIx64 " dt %u device time %u ns %" PRIu64 @@ -344,9 +1176,146 @@ rift_s_controller_handle_report(struct rift_s_controller *ctrl, return true; } -#define READ_LE16(b) (b)[0] | ((b)[1]) << 8 -#define READ_LE32(b) (b)[0] | ((b)[1]) << 8 | ((b)[2]) << 16 | ((b)[3]) << 24 -#define READ_LEFLOAT32(b) (*(float *)(b)); +struct controller_imu_descriptor_read +{ + struct rift_s_controller *ctrl; + struct rift_s_system *sys; + const char *hand; + uint64_t device_id; +}; + +static void +ctrl_imu_descriptor_cb(bool success, uint8_t *response_bytes, int response_bytes_len, void *cb_data) +{ + struct controller_imu_descriptor_read *read = cb_data; + if (!success) { + // Shutdown can cancel this request after the controller itself was freed. + RIFT_S_DEBUG("Cancelled controller IMU descriptor read hand=%s device=%016" PRIx64, read->hand, + read->device_id); + free(read); + return; + } + os_mutex_lock(&read->sys->dev_mutex); + if (read->sys->controllers[0] != read->ctrl && read->sys->controllers[1] != read->ctrl) { + os_mutex_unlock(&read->sys->dev_mutex); + free(read); + return; + } + struct rift_s_controller *ctrl = read->ctrl; + os_mutex_lock(&ctrl->mutex); + if (read->device_id == ctrl->device_id) { + if (response_bytes_len >= 0 && rift_s_decode_controller_imu_descriptor(response_bytes, (size_t)response_bytes_len, + ctrl->imu_descriptor)) { + // Applies even after the wait timed out; the correction is derived from factory data. + ctrl->imu_descriptor_done = true; + ctrl->gyro_calibration = rift_s_controller_gyro_calibration_for_imu( + &ctrl->calibration.gyro_calibration, ctrl->imu_descriptor); + RIFT_S_INFO("Controller IMU hand=%s device=%016" PRIx64 " descriptor=%s gyro_correction=%s", + hand_alias(ctrl), ctrl->device_id, ctrl->imu_descriptor, + strcmp(ctrl->imu_descriptor, "LSM6DSL") == 0 ? "0.8714285714285714" : "none"); + } else { + imu_descriptor_give_up(ctrl, "bad reply"); + } + } + os_mutex_unlock(&ctrl->mutex); + os_mutex_unlock(&read->sys->dev_mutex); + free(read); +} + +enum controller_irled_step +{ + IRLED_READ, + IRLED_WRITE, + IRLED_VERIFY, +}; + +struct controller_irled_op +{ + struct rift_s_system *sys; + const char *hand; + uint64_t device_id; + enum controller_irled_step step; + rift_s_controller_irled_config cfg; +}; + +static void +ctrl_irled_cb(bool success, uint8_t *response_bytes, int response_bytes_len, void *cb_data); + +static void +queue_irled_op(struct controller_irled_op *op) +{ + uint8_t req[12]; + size_t len; + uint8_t report = RIFT_S_RADIO_REPORT_READ; + if (op->step == IRLED_WRITE) { + len = rift_s_encode_controller_irled_write(&op->cfg, req); + report = RIFT_S_RADIO_REPORT_WRITE; + } else { + len = rift_s_encode_controller_irled_read(req); + } + rift_s_radio_queue_report(rift_s_system_radio(op->sys), report, op->device_id, req, (int)len, ctrl_irled_cb, + op); +} + +/* Same sequence as the Windows driver: read register 0x28, keep the LED on-time, set the + * period to the controller exposure period, then read it back. Called from the radio thread. */ +static void +ctrl_irled_cb(bool success, uint8_t *response_bytes, int response_bytes_len, void *cb_data) +{ + struct controller_irled_op *op = cb_data; + if (!success) { + RIFT_S_DEBUG("Cancelled controller IRLED request hand=%s device=%016" PRIx64, op->hand, op->device_id); + free(op); + return; + } + size_t size = response_bytes_len > 0 ? (size_t)response_bytes_len : 0; + char hex[3 * 16 + 1] = {0}; + for (size_t i = 0; i < size && i < 16; i++) + snprintf(hex + 3 * i, 4, "%02x ", response_bytes[i]); + + rift_s_controller_irled_config cfg; + switch (op->step) { + case IRLED_READ: { + bool ok = rift_s_decode_controller_irled_config(response_bytes, size, &cfg); + RIFT_S_INFO("Controller IRLED hand=%s device=%016" PRIx64 " read=%s period_us=%u ontime_us=%u reply=%s", + op->hand, op->device_id, ok ? "ok" : "failed", ok ? cfg.period_us : 0, + ok ? cfg.ontime_us : 0, hex); + uint32_t period = (uint32_t)debug_get_num_option_rift_s_controller_led_period_us(); + if (period == 0) { + free(op); + return; + } + op->cfg.period_us = period; + // Windows keeps the controller's on-time and falls back to 19 us when the read fails. + op->cfg.ontime_us = ok ? cfg.ontime_us : RIFT_S_CONTROLLER_IRLED_DEFAULT_ONTIME_US; + op->step = IRLED_WRITE; + queue_irled_op(op); + return; + } + case IRLED_WRITE: { + uint32_t status = 0; + bool ok = rift_s_controller_write_succeeded(response_bytes, size, &status); + RIFT_S_INFO("Controller IRLED hand=%s device=%016" PRIx64 + " write=%s period_us=%u ontime_us=%u status=0x%x reply=%s", + op->hand, op->device_id, ok ? "ok" : "failed", op->cfg.period_us, op->cfg.ontime_us, status, + hex); + op->step = IRLED_VERIFY; + queue_irled_op(op); + return; + } + case IRLED_VERIFY: { + bool ok = rift_s_decode_controller_irled_config(response_bytes, size, &cfg); + bool match = ok && cfg.period_us == op->cfg.period_us && cfg.ontime_us == op->cfg.ontime_us; + RIFT_S_INFO("Controller IRLED hand=%s device=%016" PRIx64 + " verify=%s period_us=%u ontime_us=%u reply=%s", + op->hand, op->device_id, match ? "ok" : ok ? "mismatch" : "failed", ok ? cfg.period_us : 0, + ok ? cfg.ontime_us : 0, hex); + free(op); + return; + } + } + free(op); +} static void ctrl_config_cb(bool success, uint8_t *response_bytes, int response_bytes_len, struct rift_s_controller *ctrl) @@ -355,54 +1324,114 @@ ctrl_config_cb(bool success, uint8_t *response_bytes, int response_bytes_len, st RIFT_S_WARN("Failed to read controller config"); return; } - + os_mutex_lock(&ctrl->mutex); ctrl->reading_config = false; - - if (response_bytes_len < 5) { - RIFT_S_WARN("Failed to read controller config - short result"); + if (response_bytes_len < 0 || + !rift_s_decode_controller_config(response_bytes, (size_t)response_bytes_len, &ctrl->config)) { + RIFT_S_WARN("Failed to read controller config hand=%s", hand_alias(ctrl)); + os_mutex_unlock(&ctrl->mutex); return; } - - /* Response 0u32 0x10 00 7d a0 0f f4 01 f4 01 00 00 80 3a ff ff f9 3d - * 0x7d00 = 32000 0x0fa0 = 4000 0x01f4 = 500 0x01f4 = 500 - * 0x3a800000 = 0.9765625e-03 = 1/1024 - * 0x3df9ffff = 0.1220703 = 1/8192 - */ - - response_bytes_len = response_bytes[4]; - if (response_bytes_len < 16) { - char buf[16384] = ""; - int bufsize = sizeof(buf) - 2; - int printed = 0; - - printed += rift_s_snprintf_hexdump_buffer(buf + printed, bufsize - printed, "Controller Config", - response_bytes, response_bytes_len); - - RIFT_S_ERROR("Failed to read controller config block - only got %d bytes\n%s", response_bytes_len, buf); - return; - } - response_bytes += 5; - - ctrl->config.accel_limit = READ_LE16(response_bytes + 0); - ctrl->config.gyro_limit = READ_LE16(response_bytes + 2); - ctrl->config.accel_hz = READ_LE16(response_bytes + 4); - ctrl->config.gyro_hz = READ_LE16(response_bytes + 6); - ctrl->config.accel_scale = READ_LEFLOAT32(response_bytes + 8); - ctrl->config.gyro_scale = READ_LEFLOAT32(response_bytes + 12); - ctrl->have_config = true; + if (!ctrl->imu_descriptor_requested) { + struct controller_imu_descriptor_read *read = U_TYPED_CALLOC(struct controller_imu_descriptor_read); + ctrl->imu_descriptor_requested = true; + ctrl->imu_descriptor_deadline_ns = os_monotonic_get_ns() + 2 * U_TIME_1S_IN_NS; + if (read != NULL) { + read->ctrl = ctrl; + read->sys = ctrl->sys; + read->hand = hand_alias(ctrl); + read->device_id = ctrl->device_id; + // The Windows driver uses the 0x32 reply length, then copies 16 descriptor bytes. + const uint8_t descriptor_req[] = {0x31, response_bytes[4], 0xe8, 0x03}; + rift_s_radio_queue_command(rift_s_system_radio(ctrl->sys), ctrl->device_id, descriptor_req, + sizeof(descriptor_req), ctrl_imu_descriptor_cb, read); + } else { + imu_descriptor_give_up(ctrl, "alloc failure"); + } + } + + if (!ctrl->irled_requested) { + struct controller_irled_op *op = U_TYPED_CALLOC(struct controller_irled_op); + ctrl->irled_requested = true; + if (op != NULL) { + op->sys = ctrl->sys; + op->hand = hand_alias(ctrl); + op->device_id = ctrl->device_id; + op->step = IRLED_READ; + queue_irled_op(op); + } + } RIFT_S_INFO("Read config for controller 0x%16" PRIx64 " type %08x. " "limit/scale/hz Accel %u %f %u Gyro %u %f %u", ctrl->device_id, ctrl->device_type, ctrl->config.accel_limit, ctrl->config.accel_scale, ctrl->config.accel_hz, ctrl->config.gyro_limit, ctrl->config.gyro_scale, ctrl->config.gyro_hz); + os_mutex_unlock(&ctrl->mutex); +} + +// One-time firmware model summary per hand, for the left/right audit. Caller holds the mutex. +static void +log_model_check(struct rift_s_controller *ctrl) +{ + if (rift_s_log_level > U_LOGGING_INFO) { + return; + } + struct rift_s_controller_model_report r; + rift_s_controller_model_check(&ctrl->calibration, &r); + RIFT_S_INFO( + "Controller model hand=%s leds=%u lensing_models=%u normal_len=%.3f..%.3f outward=%u/%u" + " centroid=%.4f,%.4f,%.4f imu=%.4f,%.4f,%.4f imu_to_centroid_m=%.4f angles=%.0f..%.0f,%.0f..%.0f" + " acc_m det=%.4f rot_deg=%.2f sv=%.4f..%.4f gyro_m det=%.4f rot_deg=%.2f sv=%.4f..%.4f" + " acc_gyro_axes_deg=%.2f AccCalibration values=%u det=%.4f rot_deg=%.2f sv=%.4f..%.4f" + " offset=%.4f vs_acc_m_deg=%.2f GyroCalibration values=%u det=%.4f rot_deg=%.2f sv=%.4f..%.4f" + " offset=%.4f vs_gyro_m_deg=%.2f", + hand_alias(ctrl), r.num_leds, r.num_lensing_models, r.normal_length_min, r.normal_length_max, + r.outward_normals, r.num_leds, r.centroid.x, r.centroid.y, r.centroid.z, r.imu_position.x, r.imu_position.y, + r.imu_position.z, r.imu_to_centroid_m, r.angle_x_min, r.angle_x_max, r.angle_y_min, r.angle_y_max, + r.accel_rectification.determinant, r.accel_rectification.rotation_deg, r.accel_rectification.singular_min, + r.accel_rectification.singular_max, r.gyro_rectification.determinant, r.gyro_rectification.rotation_deg, + r.gyro_rectification.singular_min, r.gyro_rectification.singular_max, r.accel_gyro_axes_deg, + r.accel_calibration_values, r.accel_calibration.determinant, r.accel_calibration.rotation_deg, + r.accel_calibration.singular_min, r.accel_calibration.singular_max, r.accel_calibration_offset_norm, + r.accel_calibration_vs_rectification_deg, r.gyro_calibration_values, r.gyro_calibration.determinant, + r.gyro_calibration.rotation_deg, r.gyro_calibration.singular_min, r.gyro_calibration.singular_max, + r.gyro_calibration_offset_norm, r.gyro_calibration_vs_rectification_deg); + // The raw numbers, so the frames can be compared offline without another capture. + const struct rift_s_controller_imu_calibration *c = &ctrl->calibration; + const float *a = c->accel.rectification.v, *g = c->gyro.rectification.v; + const float *ta = c->accel_calibration.matrix.v, *tg = c->gyro_calibration.matrix.v; + RIFT_S_DEBUG( + "Controller IMU calibration hand=%s acc_m=%g,%g,%g;%g,%g,%g;%g,%g,%g acc_b=%g,%g,%g" + " gyro_m=%g,%g,%g;%g,%g,%g;%g,%g,%g gyro_b=%g,%g,%g AccCalibration=%g,%g,%g;%g,%g,%g;%g,%g,%g" + " bias=%g,%g,%g GyroCalibration=%g,%g,%g;%g,%g,%g;%g,%g,%g bias=%g,%g,%g", + hand_alias(ctrl), a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8], c->accel.offset.x, + c->accel.offset.y, c->accel.offset.z, g[0], g[1], g[2], g[3], g[4], g[5], g[6], g[7], g[8], + c->gyro.offset.x, c->gyro.offset.y, c->gyro.offset.z, ta[0], ta[1], ta[2], ta[3], ta[4], ta[5], ta[6], + ta[7], ta[8], c->accel_calibration.offset.x, c->accel_calibration.offset.y, c->accel_calibration.offset.z, + tg[0], tg[1], tg[2], tg[3], tg[4], tg[5], tg[6], tg[7], tg[8], c->gyro_calibration.offset.x, + c->gyro_calibration.offset.y, c->gyro_calibration.offset.z); + if (r.accel_rectification.reflection || r.gyro_rectification.reflection) { + RIFT_S_WARN("Controller %s IMU rectification contains a reflection: IMU handedness differs from the" + " LED model frame", + hand_alias(ctrl)); + } + if (r.accel_calibration_values != 12 || r.gyro_calibration_values != 12) { + RIFT_S_DEBUG("Controller model hand=%s suspicious=tracked_object_layout values=%u/%u (expected 12)", + hand_alias(ctrl), r.accel_calibration_values, r.gyro_calibration_values); + } + if (r.accel_gyro_axes_deg > 5) { + RIFT_S_DEBUG("Controller model hand=%s suspicious=imu_axes accel_gyro_deg=%.1f", hand_alias(ctrl), + r.accel_gyro_axes_deg); + } } static void ctrl_json_cb(bool success, uint8_t *response_bytes, int response_bytes_len, struct rift_s_controller *ctrl) { if (!success) { + // Only radio shutdown completes commands unsuccessfully; the controller may be gone. RIFT_S_DEBUG("Failed to read controller calibration block"); return; } @@ -410,17 +1439,53 @@ ctrl_json_cb(bool success, uint8_t *response_bytes, int response_bytes_len, stru os_mutex_lock(&ctrl->mutex); ctrl->reading_calibration = false; - RIFT_S_TRACE("Got Controller calibration:\n%s", response_bytes); + RIFT_S_TRACE("Got Controller calibration device=%d:\n%s", ctrl->base.device_type, response_bytes); - if (rift_s_controller_parse_imu_calibration((char *)response_bytes, &ctrl->calibration) == 0) { + bool parsed = rift_s_controller_parse_imu_calibration((char *)response_bytes, &ctrl->calibration) == 0; + if (parsed) { + ctrl->gyro_calibration = rift_s_controller_gyro_calibration_for_imu( + &ctrl->calibration.gyro_calibration, ctrl->imu_descriptor); ctrl->P_device_imu.position = ctrl->calibration.imu_position; math_pose_invert(&ctrl->P_device_imu, &ctrl->P_imu_device); ctrl->have_calibration = true; + log_model_check(ctrl); } else { - RIFT_S_ERROR("Failed to parse controller configuration for controller 0x%16" PRIx64 "\n", - ctrl->device_id); + // A parse failure (including an unexpected FlsVersion) retries after a delay and is + // counted, so a hand stuck without a model shows up in the summary. + ctrl->calibration_retry_ns = os_monotonic_get_ns() + U_TIME_1S_IN_NS; + ctrl->diagnostics.calibration_parse_failures++; + RIFT_S_ERROR("Failed to parse controller calibration hand=%s failures=%" PRIu64, hand_alias(ctrl), + ctrl->diagnostics.calibration_parse_failures); } os_mutex_unlock(&ctrl->mutex); + + // Calibration callbacks arrive one at a time on the radio thread, so taking the other + // controller's mutex after releasing ours cannot deadlock. + struct rift_s_controller *left = ctrl->sys ? ctrl->sys->controllers[0] : NULL; + struct rift_s_controller *right = ctrl->sys ? ctrl->sys->controllers[1] : NULL; + if (parsed && left != NULL && right != NULL && rift_s_log_level <= U_LOGGING_DEBUG) { + os_mutex_lock(&left->mutex); + os_mutex_lock(&right->mutex); + if (left->have_calibration && right->have_calibration) { + struct rift_s_controller_hand_comparison cmp; + rift_s_controller_compare_hands(&left->calibration, &right->calibration, &cmp); + RIFT_S_DEBUG("Controller model hands best=%s led_rms_m identical=%.4f mirror_x=%.4f mirror_y=%.4f" + " mirror_z=%.4f normal_deg identical=%.1f mirror_x=%.1f mirror_y=%.1f mirror_z=%.1f" + " imu_m identical=%.4f mirror_x=%.4f mirror_y=%.4f mirror_z=%.4f", + rift_s_hand_relation_name(cmp.best), cmp.position_rms_m[0], cmp.position_rms_m[1], + cmp.position_rms_m[2], cmp.position_rms_m[3], cmp.normal_mean_deg[0], + cmp.normal_mean_deg[1], cmp.normal_mean_deg[2], cmp.normal_mean_deg[3], + cmp.imu_difference_m[0], cmp.imu_difference_m[1], cmp.imu_difference_m[2], + cmp.imu_difference_m[3]); + if (cmp.best != RIFT_S_HANDS_MIRROR_X) { + RIFT_S_DEBUG("Controller model suspicious=hands_not_mirror_x best=%s: check the right-hand" + " model and IMU frame handling", + rift_s_hand_relation_name(cmp.best)); + } + } + os_mutex_unlock(&right->mutex); + os_mutex_unlock(&left->mutex); + } } static void @@ -512,13 +1577,40 @@ rift_s_controller_update_inputs(struct xrt_device *xdev) return XRT_SUCCESS; } -static void +static bool rift_s_controller_get_fusion_pose(struct rift_s_controller *ctrl, enum xrt_input_name name, int64_t at_timestamp_ns, struct xrt_space_relation *out_relation) { out_relation->pose = ctrl->pose; + struct xrt_vec3 gyro = ctrl->fusion.last.gyro; + if (name == XRT_INPUT_GENERIC_TRACKER_POSE) { + struct rift_s_attitude_sample sample; + if (!rift_s_controller_get_attitude(ctrl, at_timestamp_ns, &sample)) { + ctrl->diagnostics.reject_history++; + timepoint_ns now = os_monotonic_get_ns(); + if (now - ctrl->diagnostics.last_history_log_ns >= U_TIME_1S_IN_NS) { + ctrl->diagnostics.last_history_log_ns = now; + RIFT_S_INFO("Controller capture hand=%s reject=%s capture_ns=%" PRIi64 + " processing_ns=%" PRIi64 " latency_ms=%.1f newest_gap_ms=%.3f", hand_alias(ctrl), + attitude_rejection_names[ctrl->diagnostics.last_attitude_reject], + at_timestamp_ns, now, (double)(now - at_timestamp_ns) / U_TIME_1MS_IN_NS, + ctrl->attitude_history_count == 0 + ? -1.0 + : (double)(at_timestamp_ns - + attitude_sample(ctrl, ctrl->attitude_history_count - 1)->capture_ns) / + U_TIME_1MS_IN_NS); + } + RIFT_S_TRACE("Controller fusion hand=%s reject=history capture_ns=%" PRIi64 + " processing_ns=%" PRIu64, + ctrl->device_type == RIFT_S_DEVICE_LEFT_CONTROLLER ? "left" : "right", + at_timestamp_ns, os_monotonic_get_ns()); + return false; + } + out_relation->pose.orientation = sample.orientation; + gyro = sample.gyro; + } out_relation->linear_velocity.x = 0.0f; out_relation->linear_velocity.y = 0.0f; out_relation->linear_velocity.z = 0.0f; @@ -529,14 +1621,82 @@ rift_s_controller_get_fusion_pose(struct rift_s_controller *ctrl, * device orientation is enough to get it into the right space, angular * velocity is a derivative so needs a special rotation. */ - math_quat_rotate_derivative(&ctrl->pose.orientation, &ctrl->fusion.last.gyro, &out_relation->angular_velocity); + math_quat_rotate_derivative(&out_relation->pose.orientation, &gyro, &out_relation->angular_velocity); - out_relation->relation_flags = (enum xrt_space_relation_flags)( - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | - XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); + out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); + return true; } -static xrt_result_t +// Translation smoothing is only for presentation, never the capture-time association prior. +static bool +apply_reacquire_offset(const struct rift_s_controller *ctrl, timepoint_ns t, struct xrt_vec3 *position) +{ + if (ctrl->reacquire_started_ns == 0 || t >= ctrl->reacquire_started_ns + RIFT_S_REACQUIRE_BLEND_NS) { + return false; + } + double u = + t > ctrl->reacquire_started_ns ? (double)(t - ctrl->reacquire_started_ns) / RIFT_S_REACQUIRE_BLEND_NS : 0; + float weight = (float)(1 - u * u * (3 - 2 * u)); + *position = m_vec3_add(*position, m_vec3_mul_scalar(ctrl->reacquire_offset, weight)); + return true; +} + +// Aim/grip: gyro orientation and the unchanged IMU/optical position filter. After the bounded +// bridge, hold a valid estimated position indefinitely with optical confidence cleared. +static bool +predict_aim_grip(struct rift_s_controller *ctrl, int64_t at_timestamp_ns, struct xrt_space_relation *rel) +{ + time_duration_ns ahead = at_timestamp_ns - ctrl->last_imu_capture_ns; + if (ctrl->last_imu_capture_ns > 0 && ahead > 0) { + if (ahead > RIFT_S_MAX_PREDICTION_NS) { + ahead = RIFT_S_MAX_PREDICTION_NS; + } + struct xrt_space_relation predicted; + m_predict_relation(rel, (double)ahead / U_TIME_1S_IN_NS, &predicted); + rel->pose.orientation = predicted.pose.orientation; + } + + bool velocity_tracked; + enum position_tracking state = position_tracking_at(ctrl, at_timestamp_ns, &velocity_tracked); + const char *source = "unacquired"; + if (state != POSITION_NONE) { + struct xrt_vec3 velocity; + rift_s_position_filter_predict(&ctrl->position_filter, at_timestamp_ns, RIFT_S_MAX_PREDICTION_NS, + &rel->pose.position, &velocity); + rel->relation_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT; + if (state != POSITION_LOST) { + rel->relation_flags |= XRT_SPACE_RELATION_POSITION_TRACKED_BIT; + } + if (state != POSITION_LOST && velocity_tracked) { + rel->linear_velocity = velocity; + rel->relation_flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT; + } + if (apply_reacquire_offset(ctrl, at_timestamp_ns, &rel->pose.position)) { + // SteamVR must not extrapolate the filter velocity across this presentation correction. + rel->linear_velocity = (struct xrt_vec3){0}; + rel->relation_flags &= ~XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT; + } + source = state == POSITION_TRACKED ? "optical" : state == POSITION_COASTING ? "coast" : "held"; + } + if (source != ctrl->position_source) { + bool fallback = state == POSITION_NONE; + ctrl->diagnostics.fallback_entries += fallback && !ctrl->fallback_active; + ctrl->fallback_active = fallback; + RIFT_S_DEBUG("Controller hand=%s position_source=%s query_ns=%" PRIi64 " last_fix_ns=%" PRIi64 + " vision_updates=%u reliable_updates=%u", + hand_alias(ctrl), source, at_timestamp_ns, ctrl->last_fix_ns, ctrl->track_vision_updates, + ctrl->track_reliable_updates); + ctrl->position_source = source; + log_tracking_health(ctrl, os_monotonic_get_ns(), true); + } else { + log_tracking_health(ctrl, os_monotonic_get_ns(), false); + } + return state != POSITION_NONE; +} + +xrt_result_t rift_s_controller_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, int64_t at_timestamp_ns, @@ -553,26 +1713,56 @@ rift_s_controller_get_tracked_pose(struct xrt_device *xdev, struct xrt_relation_chain xrc = {0}; os_mutex_lock(&ctrl->mutex); - if (name == XRT_INPUT_TOUCH_AIM_POSE) { + // Constellation needs the bounded gyro-compensated gravity bootstrap. Aim/grip + // can expose gyro orientation during its first 250 ms. + if (name == XRT_INPUT_GENERIC_TRACKER_POSE && !ctrl->gravity_initialized) { + *out_relation = (struct xrt_space_relation)XRT_SPACE_RELATION_ZERO; + os_mutex_unlock(&ctrl->mutex); + return XRT_ERROR_POSE_NOT_ACTIVE; + } + + // Innermost first: published pose -> hand/model -> IMU -> world. + // Constellation queries only the IMU pose and must bypass these offsets. + if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { + const struct xrt_pose *offset = + name == XRT_INPUT_TOUCH_AIM_POSE ? &ctrl->P_device_aim : &ctrl->P_device_grip; + m_relation_chain_push_pose(&xrc, offset); m_relation_chain_push_pose(&xrc, &ctrl->P_imu_device); - } else if (name == XRT_INPUT_TOUCH_GRIP_POSE) { - m_relation_chain_push_pose(&xrc, &ctrl->P_imu_device); - m_relation_chain_push_pose(&xrc, &ctrl->P_aim_grip); } /* Apply the fusion rotation */ struct xrt_space_relation *rel = m_relation_chain_reserve(&xrc); - rift_s_controller_get_fusion_pose(ctrl, name, at_timestamp_ns, rel); - if (ctrl->last_tracked_pose_ts != 0) { - rel->pose.position = ctrl->last_tracked_pose.position; - rel->relation_flags |= (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_POSITION_VALID_BIT | - XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + if (!rift_s_controller_get_fusion_pose(ctrl, name, at_timestamp_ns, rel)) { + *out_relation = (struct xrt_space_relation)XRT_SPACE_RELATION_ZERO; + os_mutex_unlock(&ctrl->mutex); + return XRT_ERROR_POSE_NOT_ACTIVE; } + bool have_position = true; + if (name == XRT_INPUT_GENERIC_TRACKER_POSE) { + // Constellation prior: the last optical position, unchanged by the IMU filter. + if (ctrl->last_tracked_pose_ts != 0 && at_timestamp_ns >= ctrl->last_tracked_pose_ts) { + rel->pose.position = ctrl->last_tracked_pose.position; + rel->relation_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT; + if (at_timestamp_ns <= ctrl->last_tracked_pose_ts + 500 * U_TIME_1MS_IN_NS) { + rel->relation_flags |= XRT_SPACE_RELATION_POSITION_TRACKED_BIT; + } + } + } else { + have_position = predict_aim_grip(ctrl, at_timestamp_ns, rel); + } + bool position_valid = name == XRT_INPUT_GENERIC_TRACKER_POSE || + (rel->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; os_mutex_unlock(&ctrl->mutex); m_relation_chain_resolve(&xrc, out_relation); + if (!have_position || !position_valid) { + out_relation->relation_flags &= + ~(XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | + XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); + } + return XRT_SUCCESS; } @@ -600,13 +1790,16 @@ rift_s_controller_destroy(struct xrt_device *xdev) u_var_remove_root(ctrl); m_imu_3dof_close(&ctrl->fusion); + if (ctrl->imu_clock != NULL) { + m_clock_windowed_skew_tracker_destroy(ctrl->imu_clock); + } os_mutex_destroy(&ctrl->mutex); u_device_free(&ctrl->base); } -static bool +bool rift_s_controller_get_led_model(struct xrt_device *xdev, struct t_constellation_led_model *led_model) { struct rift_s_controller *ctrl = (struct rift_s_controller *)(xdev); @@ -618,23 +1811,25 @@ rift_s_controller_get_led_model(struct xrt_device *xdev, struct t_constellation_ } os_mutex_unlock(&ctrl->mutex); - t_constellation_led_model_init((int)ctrl->base.device_type, NULL, led_model, ctrl->calibration.num_leds, - 0); + t_constellation_led_model_init((int)ctrl->base.device_type, NULL, led_model, ctrl->calibration.num_leds, 0); - // Note: This LED model is in OpenXR coordinates with - // XYZ = Right/Down/Forward. Flip to OpenCV for the constellation tracker - // after moving to imu-relative position + // A Rift S ring can expose only five LEDs on the table or at oblique angles. + led_model->min_acquisition_leds = 5; + + // Firmware LEDs are device-relative in OpenXR coordinates. Move them into + // the IMU frame used by GENERIC_TRACKER_POSE, then flip to OpenCV. for (int i = 0; i < ctrl->calibration.num_leds; i++) { struct t_constellation_led *led = led_model->leds + i; struct xrt_vec3 pos, dir; - math_pose_transform_point(&ctrl->P_device_imu, &ctrl->calibration.leds[i].pos, &pos); - math_quat_rotate_vec3(&ctrl->P_device_imu.orientation, &ctrl->calibration.leds[i].dir, &dir); + math_pose_transform_point(&ctrl->P_imu_device, &ctrl->calibration.leds[i].pos, &pos); + math_quat_rotate_vec3(&ctrl->P_imu_device.orientation, &ctrl->calibration.leds[i].dir, &dir); led->id = i; led->pos.x = pos.x; led->pos.y = -pos.y; led->pos.z = -pos.z; + math_vec3_normalize(&dir); led->dir.x = dir.x; led->dir.y = -dir.y; led->dir.z = -dir.z; @@ -646,62 +1841,336 @@ rift_s_controller_get_led_model(struct xrt_device *xdev, struct t_constellation_ } static void -rift_s_controller_push_observed_pose(struct xrt_device *xdev, timepoint_ns frame_mono_ns, const struct xrt_pose *pose) +log_fusion(struct rift_s_controller *ctrl, + timepoint_ns capture_ns, + timepoint_ns processing_ns, + const char *result, + const struct t_constellation_pose_observation *obs) +{ + ctrl->diagnostics.last_optical_result = result; + ctrl->diagnostics.last_optical_processing_ns = processing_ns; + const float *c = ctrl->last_position_covariance; + RIFT_S_TRACE("Controller fusion hand=%s capture_ns=%" PRIi64 " processing_ns=%" PRIi64 + " result=%s hypothesis=%s inliers=%u unique=%u yaw_observed=%d accepted=%" PRIu64 + " reliable=%" PRIu64 " position_only=%" PRIu64 + " position_var_m2=%g,%g,%g" + " velocity_covariance=unavailable reject_stale=%" PRIu64 " reject_nonfinite=%" PRIu64 + " reject_clock=%" PRIu64 " reject_duplicate=%" PRIu64 " reject_history=%" PRIu64, + hand_alias(ctrl), capture_ns, processing_ns, result, + obs && obs->hypothesis ? obs->hypothesis : "unknown", obs ? obs->inliers : 0, + obs ? obs->distinct_leds : 0, obs ? obs->orientation_observed : 0, + ctrl->diagnostics.optical_accepted, ctrl->diagnostics.optical_reliable, + ctrl->diagnostics.optical_position_only, c[0], c[4], c[8], ctrl->diagnostics.reject_stale, + ctrl->diagnostics.reject_nonfinite, ctrl->diagnostics.reject_clock, + ctrl->diagnostics.reject_duplicate, ctrl->diagnostics.reject_history); + if (rift_s_log_level <= U_LOGGING_DEBUG && + processing_ns - ctrl->diagnostics.last_summary_ns >= 5 * (timepoint_ns)U_TIME_1S_IN_NS) { + ctrl->diagnostics.last_summary_ns = processing_ns; + const struct rift_s_position_filter *pf = &ctrl->position_filter; + RIFT_S_DEBUG("Controller position summary hand=%s source=%s updates=%" PRIu64 " resets=%" PRIu64 + " gated=%" PRIu64 " rejected=%" PRIu64 " stale=%" PRIu64 + " innovation_rms_mm=%.2f (n=%u)" + " velocity_mps=%.3f,%.3f,%.3f accel_bias_mps2=%.3f,%.3f,%.3f position_std_mm=%.2f", + hand_alias(ctrl), ctrl->position_source ? ctrl->position_source : "none", pf->updates, + pf->resets, pf->gated, pf->rejected, pf->stale, + pf->innovation_count ? 1000.0 * sqrt(pf->innovation_sq_sum / pf->innovation_count) : 0.0, + pf->innovation_count, pf->state.x[0][1], pf->state.x[1][1], pf->state.x[2][1], + pf->state.x[0][2], pf->state.x[1][2], pf->state.x[2][2], + 1000.0 * sqrt(fmax(pf->state.P[0][0][0] + pf->state.P[1][0][0] + + pf->state.P[2][0][0], + 0))); + ctrl->position_filter.innovation_sq_sum = 0; + ctrl->position_filter.innovation_count = 0; + RIFT_S_DEBUG( + "Controller fusion summary hand=%s accepted=%" PRIu64 " reliable=%" PRIu64 " position_only=%" PRIu64 + " fallback_entries=%" PRIu64 " calibration_parse_failures=%" PRIu64 + " heading_prior=%d position_std_m=%.4f velocity_covariance=unavailable reject_stale=%" PRIu64 + " reject_nonfinite=%" PRIu64 " reject_clock=%" PRIu64 " (backwards=%" PRIu64 + " history_order=%" PRIu64 " unmapped=%" PRIu64 " discontinuity=%" PRIu64 " mapped_order=%" PRIu64 + " resyncs=%u" + ") reject_duplicate=%" PRIu64 " reject_history=%" PRIu64 + " rest_windows=%u rest_mps2=%.3f..%.3f rest_min_dir=%.2f,%.2f,%.2f" + " rest_max_dir=%.2f,%.2f,%.2f accel_bias=%s%.3f,%.3f,%.3f bias_windows=%u bias_spread_deg=%.0f" + " bias_rms_mps2=%.3f->%.3f accel_offset=%d optical_tilt_updates=%u optical_tilt_residual_deg=%.1f" + " attitude_bias=%s%.3f,%.3f,%.3f attitude_samples=%u attitude_rms_mps2=%.3f", + hand_alias(ctrl), ctrl->diagnostics.optical_accepted, ctrl->diagnostics.optical_reliable, + ctrl->diagnostics.optical_position_only, ctrl->diagnostics.fallback_entries, + ctrl->diagnostics.calibration_parse_failures, ctrl->optical_heading_valid, + sqrtf(fmaxf(c[0] + c[4] + c[8], 0)), ctrl->diagnostics.reject_stale, + ctrl->diagnostics.reject_nonfinite, ctrl->diagnostics.reject_clock, + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_BACKWARDS], + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_HISTORY_ORDER], + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_UNMAPPED], + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY], + ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_MAPPED_ORDER], ctrl->diagnostics.clock_resyncs, + ctrl->diagnostics.reject_duplicate, ctrl->diagnostics.reject_history, ctrl->rest.windows, + ctrl->rest.min_mps2, ctrl->rest.max_mps2, ctrl->rest.min_dir.x, ctrl->rest.min_dir.y, + ctrl->rest.min_dir.z, ctrl->rest.max_dir.x, ctrl->rest.max_dir.y, ctrl->rest.max_dir.z, + ctrl->accel_bias.valid ? "" : "pending:", ctrl->accel_bias.bias.x, ctrl->accel_bias.bias.y, + ctrl->accel_bias.bias.z, ctrl->accel_bias.count, ctrl->accel_bias.spread_deg, + ctrl->accel_bias.rms_before_mps2, ctrl->accel_bias.rms_after_mps2, ctrl->accel_offset, + ctrl->optical_tilt_updates, ctrl->optical_tilt_residual_deg, + ctrl->attitude_bias.valid ? "" : "pending:", ctrl->attitude_bias.bias.x, ctrl->attitude_bias.bias.y, + ctrl->attitude_bias.bias.z, ctrl->attitude_bias.count, ctrl->attitude_bias.rms_mps2); + } +} + +// Per-axis variance of an optical position: the fit covariance diagonal, else 3 mm. +static struct xrt_vec3 +optical_position_variance(const float covariance[9]) +{ + float v[3]; + for (int i = 0; i < 3; i++) { + float c = covariance[4 * i]; + v[i] = isfinite(c) && c > 0 ? c : 3e-3f * 3e-3f; + v[i] = fminf(fmaxf(v[i], 1e-3f * 1e-3f), 0.05f * 0.05f); + } + return (struct xrt_vec3){v[0], v[1], v[2]}; +} + +static enum rift_s_position_fix_result +fuse_optical_position(struct rift_s_controller *ctrl, + const struct t_constellation_pose_observation *obs, + timepoint_ns processing_ns) +{ + struct xrt_vec3 variance = optical_position_variance(obs->position_covariance); + bool was_held = ctrl->position_filter.initialized && ctrl->position_filter.frozen; + struct xrt_vec3 held_position = {0}, velocity; + if (was_held) { + rift_s_position_filter_predict(&ctrl->position_filter, processing_ns, RIFT_S_MAX_PREDICTION_NS, + &held_position, &velocity); + apply_reacquire_offset(ctrl, processing_ns, &held_position); + } + bool was_lost = !ctrl->position_filter.initialized || ctrl->position_filter.frozen; + enum rift_s_position_fix_result result = + rift_s_position_filter_fix(&ctrl->position_filter, obs->capture_ns, &obs->pose.position, &variance, + RIFT_S_POSITION_RESET_DISTANCE_M); + if (result == RIFT_S_POSITION_FIX_STALE) { + return result; + } + if (result == RIFT_S_POSITION_FIX_PENDING) { + // Not part of a track until confirmed: the held or tracked estimate stays. + if (ctrl->position_filter.candidate_count == 1 && + obs->capture_ns - ctrl->last_candidate_log_ns >= U_TIME_1S_IN_NS) { + ctrl->last_candidate_log_ns = obs->capture_ns; + RIFT_S_DEBUG("Controller position hand=%s state=candidate capture_ns=%" PRIi64 + " position=%.3f,%.3f,%.3f held=%.3f,%.3f,%.3f rejected=%" PRIu64, + hand_alias(ctrl), obs->capture_ns, obs->pose.position.x, obs->pose.position.y, + obs->pose.position.z, ctrl->position_filter.state.x[0][0], + ctrl->position_filter.state.x[1][0], ctrl->position_filter.state.x[2][0], + ctrl->position_filter.rejected); + } + return result; + } + if (result == RIFT_S_POSITION_FIX_RESET) { + if (was_held) { + struct xrt_vec3 resumed_position; + rift_s_position_filter_predict(&ctrl->position_filter, processing_ns, RIFT_S_MAX_PREDICTION_NS, + &resumed_position, &velocity); + ctrl->reacquire_offset = m_vec3_sub(held_position, resumed_position); + ctrl->reacquire_started_ns = processing_ns; + } + // A new track: the loss ladder counts from here. + ctrl->track_vision_updates = ctrl->track_reliable_updates = 0; + RIFT_S_DEBUG("Controller position hand=%s state=reset reason=%s capture_ns=%" PRIi64 + " position=%.3f,%.3f,%.3f resets=%" PRIu64 " gated=%" PRIu64, + hand_alias(ctrl), was_lost ? "acquire" : "innovation", obs->capture_ns, + obs->pose.position.x, obs->pose.position.y, obs->pose.position.z, + ctrl->position_filter.resets, ctrl->position_filter.gated); + } + ctrl->track_vision_updates++; + if (obs->orientation_observed && obs->distinct_leds >= 3) { + ctrl->track_reliable_updates++; + } + if (obs->capture_ns > ctrl->last_fix_ns) { + ctrl->last_fix_ns = obs->capture_ns; + } + return result; +} + +/* Refine startup and accelerometer tilt with a geometry-verified optical fit. Like optical yaw, + * the world-frame correction is applied to the current + * fusion state and the retained capture attitudes. Caller holds the mutex. */ +static void +apply_optical_tilt(struct rift_s_controller *ctrl, timepoint_ns capture_ns, const struct xrt_quat *optical) +{ + struct rift_s_attitude_sample at_capture; + if (!rift_s_controller_get_attitude(ctrl, capture_ns, &at_capture)) { + return; + } + struct xrt_quat correction; + float tilt = + rift_s_optical_tilt_correction(&at_capture.orientation, optical, RIFT_S_OPTICAL_TILT_FRACTION, &correction); + math_quat_rotate(&correction, &ctrl->fusion.rot, &ctrl->fusion.rot); + math_quat_normalize(&ctrl->fusion.rot); + for (unsigned int i = 0; i < ctrl->attitude_history_count; i++) { + struct rift_s_attitude_sample *sample = attitude_sample(ctrl, i); + math_quat_rotate(&correction, &sample->orientation, &sample->orientation); + math_quat_normalize(&sample->orientation); + } + ctrl->pose.orientation = ctrl->fusion.rot; + float tilt_deg = RAD_TO_DEG(tilt); + ctrl->optical_tilt_residual_deg = + ctrl->optical_tilt_updates == 0 ? tilt_deg : 0.9f * ctrl->optical_tilt_residual_deg + 0.1f * tilt_deg; + ctrl->optical_tilt_updates++; + ctrl->last_optical_tilt_ns = capture_ns; +} + +void +rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs) { struct rift_s_controller *ctrl = (struct rift_s_controller *)(xdev); + timepoint_ns processing_ns = os_monotonic_get_ns(); + timepoint_ns frame_mono_ns = obs->capture_ns; + const struct xrt_pose *pose = &obs->pose; os_mutex_lock(&ctrl->mutex); - - ctrl->last_tracked_pose_ts = frame_mono_ns; - ctrl->last_tracked_pose = *pose; - - if (ctrl->update_yaw_from_optical) { - // Apply 5% of observed orientation yaw to 3dof fusion - // FIXME: Do better - struct xrt_quat delta; - math_quat_unrotate(&ctrl->fusion.rot, &pose->orientation, &delta); - delta.x = delta.z = 0.0; // We only want Yaw - - if (fabs(delta.y) > sin(DEG_TO_RAD(5)) / 2) { - delta.y = sin(0.10 * asinf(delta.y)); // 10% correction - math_quat_normalize(&delta); - - struct xrt_quat prev = ctrl->fusion.rot; - math_quat_rotate(&ctrl->fusion.rot, &delta, &ctrl->fusion.rot); - - if (rift_s_log_level <= U_LOGGING_DEBUG) { - struct xrt_quat post_delta; - math_quat_unrotate(&ctrl->fusion.rot, &pose->orientation, &post_delta); - post_delta.x = post_delta.z = 0.0; // We only want Yaw - post_delta.y = 0.10 * post_delta.y; // 5% - math_quat_normalize(&post_delta); - - RIFT_S_DEBUG( - "Applying delta yaw rotation of %f degrees delta quat %f,%f,%f,%f from " - "%f,%f,%f,%f to " - "%f,%f,%f,%f. delta after correction: %f,%f,%f,%f", - RAD_TO_DEG(2 * asinf(delta.y)), delta.x, delta.y, delta.z, delta.w, prev.x, prev.y, - prev.z, prev.w, ctrl->fusion.rot.x, ctrl->fusion.rot.y, ctrl->fusion.rot.z, - ctrl->fusion.rot.w, post_delta.x, post_delta.y, post_delta.z, post_delta.w); - } - } else if (fabs(delta.y) > sin(DEG_TO_RAD(0.25)) / 2) { - math_quat_normalize(&delta); - - RIFT_S_DEBUG("Applying full yaw correction of %f degrees. delta quat %f,%f,%f,%f", - RAD_TO_DEG(2 * asinf(delta.y)), delta.x, delta.y, delta.z, delta.w); - math_quat_rotate(&ctrl->fusion.rot, &delta, &ctrl->fusion.rot); + const char *reject = NULL; + struct rift_s_attitude_sample historical; + if (frame_mono_ns <= 0 || frame_mono_ns <= ctrl->last_tracked_pose_ts) { + ctrl->diagnostics.reject_stale++; + reject = "reject_stale"; + } else if (!finite_vec3(&pose->position) || !valid_quat(&pose->orientation)) { + ctrl->diagnostics.reject_nonfinite++; + reject = "reject_nonfinite"; + } else if (!rift_s_controller_get_attitude(ctrl, frame_mono_ns, &historical)) { + ctrl->diagnostics.reject_history++; + reject = "reject_history"; + } + if (reject != NULL) { + log_fusion(ctrl, frame_mono_ns, processing_ns, reject, obs); + os_mutex_unlock(&ctrl->mutex); + return; + } + // Candidates must pass position confirmation before changing the IMU heading or + // the association prior. Otherwise a rejected fit can steer the next search. + enum rift_s_position_fix_result position_result = fuse_optical_position(ctrl, obs, processing_ns); + if (position_result == RIFT_S_POSITION_FIX_PENDING || position_result == RIFT_S_POSITION_FIX_STALE) { + log_fusion(ctrl, frame_mono_ns, processing_ns, "reject_position", obs); + os_mutex_unlock(&ctrl->mutex); + return; + } + struct xrt_quat optical = pose->orientation; + math_quat_normalize(&optical); + // A position-only fit returns the IMU prior as its orientation. Treating that echo as an + // optical yaw measurement would keep a drifting heading "verified" indefinitely. + if (ctrl->update_yaw_from_optical && obs->orientation_observed) { + struct xrt_quat corrected = historical.orientation; + float yaw = rift_s_apply_optical_yaw(&corrected, &optical); + struct xrt_quat correction = {0, sinf(yaw / 2), 0, cosf(yaw / 2)}; + // A world-yaw change commutes with gravity correction. Apply it to the + // capture state, retained IMU states and current fusion consistently. + math_quat_rotate(&correction, &ctrl->fusion.rot, &ctrl->fusion.rot); + math_quat_normalize(&ctrl->fusion.rot); + for (unsigned int i = 0; i < ctrl->attitude_history_count; i++) { + struct rift_s_attitude_sample *sample = attitude_sample(ctrl, i); + math_quat_rotate(&correction, &sample->orientation, &sample->orientation); + math_quat_normalize(&sample->orientation); + } + ctrl->pose.orientation = ctrl->fusion.rot; + struct xrt_quat inverse, residual; + math_quat_invert(&corrected, &inverse); + math_quat_rotate(&optical, &inverse, &residual); + ctrl->optical_heading_valid = fabsf(residual.y) + fabsf(residual.w) >= 1e-6f && + 2 * atan2f(fabsf(residual.y), fabsf(residual.w)) < DEG_TO_RAD(5.0f); + ctrl->last_optical_yaw_ts = frame_mono_ns; + RIFT_S_TRACE("Optical hand=%s capture_ns=%" PRIi64 " yaw_correction_deg=%f heading_prior=%d", + hand_alias(ctrl), frame_mono_ns, RAD_TO_DEG(yaw), ctrl->optical_heading_valid); + } else if (!obs->orientation_observed) { + ctrl->diagnostics.optical_position_only++; + } + if (obs->orientation_observed && obs->distinct_leds >= 3) { + ctrl->diagnostics.optical_reliable++; + if (historical.tilt_valid) { + apply_optical_tilt(ctrl, frame_mono_ns, &optical); } } - // Update pose position for the debug UI + ctrl->last_tracked_pose_ts = frame_mono_ns; + ctrl->last_tracked_pose = *pose; ctrl->pose.position = pose->position; + memcpy(ctrl->last_position_covariance, obs->position_covariance, sizeof(ctrl->last_position_covariance)); + ctrl->diagnostics.optical_accepted++; + log_fusion(ctrl, frame_mono_ns, processing_ns, "accepted", obs); os_mutex_unlock(&ctrl->mutex); } +void +rift_s_controller_push_observed_pose(struct xrt_device *xdev, timepoint_ns frame_mono_ns, const struct xrt_pose *pose) +{ + // Legacy full-pose path: the caller vouches for the orientation. + struct t_constellation_pose_observation obs = { + .capture_ns = frame_mono_ns, + .pose = *pose, + .orientation_observed = true, + .information_rank = 4, + .yaw_variance = NAN, + .hypothesis = "legacy", + }; + for (unsigned i = 0; i < 9; i++) { + obs.position_covariance[i] = NAN; + } + rift_s_controller_push_pose_observation(xdev, &obs); +} + +static bool +has_heading_prior(struct xrt_device *xdev, timepoint_ns capture_ns) +{ + struct rift_s_controller *ctrl = (struct rift_s_controller *)xdev; + os_mutex_lock(&ctrl->mutex); + bool valid = rift_s_controller_has_heading_prior(ctrl, capture_ns); + os_mutex_unlock(&ctrl->mutex); + return valid; +} + static struct t_constellation_tracked_device_callbacks tracking_callbacks = { .get_led_model = rift_s_controller_get_led_model, .notify_frame_received = NULL, .push_observed_pose = rift_s_controller_push_observed_pose, + .has_heading_prior = has_heading_prior, + .push_pose_observation = rift_s_controller_push_pose_observation, }; +static struct xrt_pose +pose_about_x(float deg, float x, float y, float z) +{ + struct xrt_pose p = XRT_POSE_IDENTITY; + const struct xrt_vec3 axis = {1, 0, 0}; + math_quat_from_angle_vector(DEG_TO_RAD(deg), &axis, &p.orientation); + p.position = (struct xrt_vec3){x, y, z}; + return p; +} + +void +rift_s_controller_model_frames(bool left, struct xrt_pose *out_model_device, struct xrt_pose *out_model_grip) +{ + const float sx = left ? 1.0f : -1.0f; + // Oculus publishes the firmware Device frame as HandPose with the default identity hand + // correction. Its SteamVR driver supplies this HandPoseFromModel transform. Normalize + // the rounded vendor quaternion before using it in XRT's rigid-pose math. + struct xrt_pose device_model = { + .orientation = {0.3371f, 0.0f, 0.0f, 0.94147f}, + .position = {-0.007f * sx, 0.036124f, -0.037617f}, + }; + math_quat_normalize(&device_model.orientation); + math_pose_invert(&device_model, out_model_device); + // OpenXR grip of the render model: in the handle, 10 cm behind the model origin. + *out_model_grip = pose_about_x(20.6f, 0.007f * sx, -0.00182941f, 0.1019482f); +} + +void +rift_s_controller_pose_frames(bool left, struct xrt_pose *device_aim, struct xrt_pose *device_grip) +{ + // The render-model frame is X right, Y up, Z back: the LED ring is at the front (Z 0.026 to + // 0.05 m) and the handle runs back along +Z to 0.14 m. Aim relative to grip is the Windows + // LCON offset: grip = aim * (+60 degrees about X, (0, -0.03, 0.095) m). + const float sx = left ? 1.0f : -1.0f; + struct xrt_pose model_device, model_grip, device_model; + rift_s_controller_model_frames(left, &model_device, &model_grip); + math_pose_invert(&model_device, &device_model); + const struct xrt_pose model_aim = pose_about_x(-39.4f, 0.007f * sx, -0.03894766f, 0.00949694f); + math_pose_transform(&device_model, &model_grip, device_grip); + math_pose_transform(&device_model, &model_aim, device_aim); +} + struct rift_s_controller * rift_s_controller_create(struct rift_s_system *sys, enum xrt_device_type device_type) { @@ -719,11 +2188,8 @@ rift_s_controller_create(struct rift_s_system *sys, enum xrt_device_type device_ os_mutex_init(&ctrl->mutex); - /* Default grip pose up by 40° degrees around the X axis and back about 10cm in Z */ - struct xrt_vec3 translation = {0.0, 0, 0.1}; - struct xrt_vec3 axis = {1.0, 0, 0}; - math_quat_from_angle_vector(DEG_TO_RAD(40), &axis, &ctrl->P_aim_grip.orientation); - ctrl->P_aim_grip.position = translation; + rift_s_controller_pose_frames(device_type == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER, &ctrl->P_device_aim, + &ctrl->P_device_grip); u_device_populate_function_pointers(&ctrl->base, rift_s_controller_get_tracked_pose, rift_s_controller_destroy); ctrl->base.update_inputs = rift_s_controller_update_inputs; @@ -785,7 +2251,8 @@ rift_s_controller_create(struct rift_s_system *sys, enum xrt_device_type device_ u_var_add_gui_header(ctrl, NULL, "Tracking"); u_var_add_pose(ctrl, &ctrl->pose, "Tracked Pose"); - u_var_add_pose(ctrl, &ctrl->P_aim_grip, "Grip pose offset"); + u_var_add_pose(ctrl, &ctrl->P_device_aim, "Aim pose in device frame"); + u_var_add_pose(ctrl, &ctrl->P_device_grip, "Grip pose in device frame"); u_var_add_gui_header(ctrl, NULL, "3DoF Tracking"); m_imu_3dof_add_vars(&ctrl->fusion, ctrl, ""); @@ -816,6 +2283,7 @@ rift_s_controller_create(struct rift_s_system *sys, enum xrt_device_type device_ struct rift_s_tracker *tracker = rift_s_system_get_tracker(sys); ctrl->update_yaw_from_optical = true; + reset_position_filter(ctrl); ctrl->tracking_connection = rift_s_tracker_add_controller(tracker, &ctrl->base, &tracking_callbacks); return ctrl; } @@ -824,14 +2292,45 @@ void rift_s_controller_update_configuration(struct rift_s_controller *ctrl, uint64_t device_id) { rift_s_radio_state *radio = rift_s_system_radio(ctrl->sys); + os_mutex_lock(&ctrl->mutex); if (ctrl->device_id != device_id) { ctrl->device_id = device_id; snprintf(ctrl->base.serial, XRT_DEVICE_NAME_LEN, "%016" PRIx64, device_id); // If the device ID changed somehow, re-read the JSON blocks ctrl->have_config = ctrl->have_calibration = false; + ctrl->imu_descriptor_requested = ctrl->imu_descriptor_done = false; + ctrl->irled_requested = false; + reset_position_filter(ctrl); + ctrl->track_vision_updates = ctrl->track_reliable_updates = 0; + ctrl->last_fix_ns = 0; + ctrl->imu_descriptor[0] = '\0'; + ctrl->gravity_initialized = false; + ctrl->gravity_moving = false; + // A bias belongs to one physical accelerometer. + ctrl->accel_bias = (struct rift_s_accel_bias){0}; + ctrl->attitude_bias = (struct rift_s_attitude_bias){0}; + ctrl->accel_offset = false; + ctrl->optical_tilt_updates = 0; + ctrl->optical_tilt_residual_deg = 0; + set_fusion_gravity(ctrl, true); + ctrl->imu_time_valid = false; + ctrl->imu_to_host_ns = 0; + if (ctrl->imu_clock != NULL) { + // Re-allocated on the next sample; reset() would keep a stale window position. + m_clock_windowed_skew_tracker_destroy(ctrl->imu_clock); + ctrl->imu_clock = NULL; + } + ctrl->clock_stable_samples = 0; + ctrl->attitude_history_count = ctrl->attitude_history_start = 0; + ctrl->optical_heading_valid = false; + ctrl->last_tracked_pose_ts = 0; + ctrl->gravity_samples = 0; + ctrl->gravity_accel_sum = ctrl->gravity_gyro_sum = (struct xrt_vec3){0}; + m_imu_3dof_reset(&ctrl->fusion); } + timepoint_ns now = os_monotonic_get_ns(); if (!ctrl->have_config && !ctrl->reading_config) { const uint8_t config_req[] = {0x32, 0x20, 0xe8, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; rift_s_radio_queue_command(radio, ctrl->device_id, config_req, sizeof(config_req), @@ -839,8 +2338,9 @@ rift_s_controller_update_configuration(struct rift_s_controller *ctrl, uint64_t ctrl->reading_config = true; } - if (!ctrl->have_calibration && !ctrl->reading_calibration) { + if (!ctrl->have_calibration && !ctrl->reading_calibration && now >= ctrl->calibration_retry_ns) { rift_s_radio_get_json_block(radio, ctrl->device_id, (rift_s_radio_completion_fn)ctrl_json_cb, ctrl); ctrl->reading_calibration = true; } + os_mutex_unlock(&ctrl->mutex); } diff --git a/src/xrt/drivers/rift_s/rift_s_controller.h b/src/xrt/drivers/rift_s/rift_s_controller.h index e7e56654c..68874d47d 100644 --- a/src/xrt/drivers/rift_s/rift_s_controller.h +++ b/src/xrt/drivers/rift_s/rift_s_controller.h @@ -22,19 +22,150 @@ #include "xrt/xrt_device.h" #include "rift_s.h" +#include "rift_s_position_filter.h" #define MAX_LOG_SIZE 1024 -typedef struct -{ - uint16_t accel_limit; - uint16_t gyro_limit; - uint16_t accel_hz; - uint16_t gyro_hz; +// About two seconds at the observed 500 Hz report rate. Not a runtime constant. +#define RIFT_S_ATTITUDE_HISTORY_CAPACITY 1024 - float accel_scale; - float gyro_scale; -} rift_s_controller_config; +struct rift_s_attitude_sample +{ + timepoint_ns device_capture_ns; + timepoint_ns capture_ns; + timepoint_ns arrival_ns; + struct xrt_quat orientation; + struct xrt_vec3 gyro; + bool tilt_valid; +}; + +// Monado policy: a heading prior needs an optical yaw measurement no older than this. +#define RIFT_S_HEADING_PRIOR_MAX_AGE_NS (2 * U_TIME_1S_IN_NS) + +//! Why an IMU sample's time was rejected; the parts of reject_clock. +enum rift_s_clock_reject +{ + //! The 32-bit device timestamp went backwards. + RIFT_S_CLOCK_BACKWARDS, + //! Device capture time not after the last attitude history sample. + RIFT_S_CLOCK_HISTORY_ORDER, + //! The skew tracker could not map device time to host time. + RIFT_S_CLOCK_UNMAPPED, + //! The mapping jumped by more than 2 ms; the attitude history restarts. + RIFT_S_CLOCK_DISCONTINUITY, + //! The mapped host capture time was not after the previous one. + RIFT_S_CLOCK_MAPPED_ORDER, + RIFT_S_CLOCK_REJECT_COUNT, +}; + +enum rift_s_attitude_rejection +{ + RIFT_S_ATTITUDE_EMPTY, + RIFT_S_ATTITUDE_INVALID_TIME, + RIFT_S_ATTITUDE_TOO_OLD, + RIFT_S_ATTITUDE_TOO_NEW, + RIFT_S_ATTITUDE_TILT_UNREADY, + RIFT_S_ATTITUDE_RADIO_GAP, + RIFT_S_ATTITUDE_REJECT_COUNT, +}; + +struct rift_s_fusion_diagnostics +{ + //! Optical observations that updated position. + uint64_t optical_accepted; + //! Accepted observations whose fit measured yaw from at least three distinct LEDs. + uint64_t optical_reliable; + //! Accepted observations whose orientation was the IMU prior (position-only update). + uint64_t optical_position_only; + //! Entries into the unacquired position state. + uint64_t fallback_entries; + //! Calibration JSON blocks that failed to parse (bad transfer or unexpected FlsVersion). + uint64_t calibration_parse_failures; + timepoint_ns last_summary_ns; + uint64_t reject_stale; + uint64_t reject_nonfinite; + //! Device timestamp went backwards, or the host clock mapping was discontinuous. + uint64_t reject_clock; + uint64_t reject_clock_by[RIFT_S_CLOCK_REJECT_COUNT]; + //! Device time step of the last accepted IMU sample, and consecutive backwards samples. + uint32_t last_step_us; + uint32_t backwards_run; + //! Device clock re-anchored after a run of backwards samples. + uint32_t clock_resyncs; + timepoint_ns last_clock_log_ns; + //! Repeated IMU reports with the same device timestamp (radio retransmits); harmless. + uint64_t reject_duplicate; + uint64_t reject_history; + uint64_t attitude_rejected[RIFT_S_ATTITUDE_REJECT_COUNT]; + enum rift_s_attitude_rejection last_attitude_reject; + timepoint_ns last_attitude_reject_ns; + timepoint_ns last_history_log_ns; + timepoint_ns last_health_log_ns; + const char *last_optical_result; + timepoint_ns last_optical_processing_ns; + +}; + +/*! + * Resting accelerometer magnitude across the session. A calibrated accelerometer reads 9.81 m/s^2 + * at rest in every orientation; a magnitude that changes with orientation means an uncorrected + * bias or per-axis scale, which also tilts the IMU attitude by up to a few degrees. + */ +struct rift_s_rest_stats +{ + // Current stationary window. + struct xrt_vec3 sum; + unsigned int samples; + timepoint_ns start_ns, last_ns; + // Completed 250 ms windows. + unsigned int windows; + float min_mps2, max_mps2; + //! Gravity direction in the IMU frame at the smallest and largest reading. + struct xrt_vec3 min_dir, max_dir; + //! Mean acceleration of the window completed last. + struct xrt_vec3 last_mean; +}; + +#define RIFT_S_ACCEL_BIAS_WINDOWS 16 + +/*! + * Accelerometer bias from rest windows in different orientations. At rest the corrected reading + * must have the magnitude of gravity whatever the orientation, so windows spread over enough + * directions determine the bias along them. A weak prior keeps unobserved axes at zero. A + * measured residual bias of 2 m/s^2 can tilt the IMU attitude by up to 12 degrees depending on + * orientation. + */ +struct rift_s_accel_bias +{ + //! Rest-window means, at most one per 15 degree cone of directions. + struct xrt_vec3 means[RIFT_S_ACCEL_BIAS_WINDOWS]; + unsigned int count; + struct xrt_vec3 bias; + //! Enough spread and a consistent fit: the bias is applied. + bool valid; + //! RMS of |mean - bias| - g over the stored windows, before and after the correction. + float rms_before_mps2, rms_after_mps2; + //! Largest angle between two stored window directions. + float spread_deg; +}; + +/*! + * Accelerometer offset from rest windows at a known attitude: the window mean minus gravity + * rotated into the IMU frame gives the full offset from one window, without the spread of + * orientations rift_s_accel_bias needs. The attitude is levelled by optical tilt. + */ +#define RIFT_S_ATTITUDE_BIAS_SAMPLES 32 +#define RIFT_S_ATTITUDE_BIAS_MIN_SAMPLES 8 + +struct rift_s_attitude_bias +{ + struct xrt_vec3 samples[RIFT_S_ATTITUDE_BIAS_SAMPLES]; + unsigned int count; + struct xrt_vec3 bias; + //! RMS distance of the samples from their mean, m/s^2. + float rms_mps2; + bool valid; +}; struct rift_s_controller { @@ -59,6 +190,29 @@ struct rift_s_controller struct xrt_pose last_tracked_pose; //! debug boolean - enable yaw updates bool update_yaw_from_optical; + bool optical_heading_valid; + //! Capture time of the last observation whose fit measured yaw. + timepoint_ns last_optical_yaw_ts; + //! Position covariance (m^2, row-major world) of the last accepted optical fit. + float last_position_covariance[9]; + //! Aim/grip position: IMU acceleration integrated at IMU rate, corrected by optical fixes. + struct rift_s_position_filter position_filter; + //! Presentation-only translation correction after held position reacquires optics. + struct xrt_vec3 reacquire_offset; + timepoint_ns reacquire_started_ns; + //! Successful and reliable optical fixes in the current track (inputs of the loss ladder). + uint32_t track_vision_updates, track_reliable_updates; + //! Capture time of the newest optical fix given to position_filter. + timepoint_ns last_fix_ns; + //! Host capture time and world-frame acceleration (gravity removed) of the newest IMU sample. + timepoint_ns last_imu_capture_ns; + struct xrt_vec3 world_accel; + bool world_accel_valid; + //! Last reported aim/grip position source, for logging transitions. + const char *position_source; + //! No optical position has been acquired yet. + bool fallback_active; + struct rift_s_fusion_diagnostics diagnostics; /* Debug logs */ /* 0x04 = new log line @@ -73,6 +227,14 @@ struct rift_s_controller uint32_t imu_timestamp32; timepoint_ns last_imu_device_time_ns; timepoint_ns last_imu_local_time_ns; + //! Device-to-host IMU clock estimate (minimum transport delay); allocated on the first sample. + struct m_clock_windowed_skew_tracker *imu_clock; + //! Current offset from imu_clock, kept to detect discontinuities. + time_duration_ns imu_to_host_ns; + unsigned int clock_stable_samples; + struct rift_s_attitude_sample attitude_history[RIFT_S_ATTITUDE_HISTORY_CAPACITY]; + unsigned int attitude_history_start; + unsigned int attitude_history_count; uint16_t imu_unknown_varying2; int16_t raw_accel[3]; @@ -83,13 +245,41 @@ struct rift_s_controller struct xrt_vec3 mag; struct m_imu_3dof fusion; + // Gyro-compensated startup gravity, before constellation uses the tilt prior. + bool gravity_initialized; + bool gravity_moving; + struct rift_s_rest_stats rest; + struct rift_s_accel_bias accel_bias; + timepoint_ns last_bias_log_ns; + /*! + * The stationary startup reading was not gravity's magnitude: the accelerometer carries an + * offset that tilts its gravity direction by up to ~12 degrees depending on orientation. + * Until a bias is known, the fusion does not level to it; optical tilt levels instead. + */ + bool accel_offset; + struct rift_s_attitude_bias attitude_bias; + //! Reliable optical tilt corrections while accel_offset, their latest capture time and a + //! smoothed residual tilt in degrees. + uint32_t optical_tilt_updates; + timepoint_ns last_optical_tilt_ns; + float optical_tilt_residual_deg; + timepoint_ns gravity_start_ns; + unsigned int gravity_samples; + struct xrt_vec3 gravity_accel_sum; + struct xrt_vec3 gravity_gyro_sum; + float gravity_accel_scale; + //! Offset for IMU pose from device (from json calibration) struct xrt_pose P_device_imu; //! Offset for device pose from imu struct xrt_pose P_imu_device; - //! Offset for grip pose - struct xrt_pose P_aim_grip; + //! Published OpenXR aim and grip in the firmware LED-model frame. + struct xrt_pose P_device_aim; + struct xrt_pose P_device_grip; + + //! Rate limit for the unconfirmed-fix debug line. + timepoint_ns last_candidate_log_ns; /* Controls / buttons state */ timepoint_ns last_controls_local_time_ns; @@ -116,16 +306,115 @@ struct rift_s_controller bool reading_config; bool have_config; + //! Earliest time to re-request the calibration block after a parse failure. + timepoint_ns calibration_retry_ns; rift_s_controller_config config; + //! Attempt 0x31 once per physical controller, with unchanged gyro calibration on failure. + bool imu_descriptor_requested; + //! The IR LED timing (register 0x28) was queued for this physical controller. + bool irled_requested; + //! The IMU may start: the descriptor was read, or waiting for it gave up. + bool imu_descriptor_done; + //! IMU samples wait for the descriptor until this time, then use the factory gyro calibration. + timepoint_ns imu_descriptor_deadline_ns; + char imu_descriptor[RIFT_S_CONTROLLER_IMU_DESCRIPTOR_SIZE + 1]; + struct rift_s_tracked_imu_calibration gyro_calibration; bool reading_calibration; bool have_calibration; struct rift_s_controller_imu_calibration calibration; }; +#ifdef __cplusplus +extern "C" { +#endif + struct rift_s_controller * rift_s_controller_create(struct rift_s_system *sys, enum xrt_device_type device_type); +/*! + * Rift S Touch frames in the SteamVR render-model frame (oculus_rifts_controller_left/right): + * the firmware LED-model ("device") frame and the OpenXR grip. + */ +void +rift_s_controller_model_frames(bool left, struct xrt_pose *out_model_device, struct xrt_pose *out_model_grip); + +//! OpenXR aim and grip in the firmware LED-model frame. +void +rift_s_controller_pose_frames(bool left, struct xrt_pose *device_aim, struct xrt_pose *device_grip); + +// Bootstrap tilt from gyro-compensated acceleration; estimate gyro bias only at rest. +void +rift_s_controller_update_fusion(struct rift_s_controller *ctrl, timepoint_ns timestamp_ns); + +// Record the current fused attitude with both clock readings; caller holds the mutex. +bool +rift_s_controller_record_attitude(struct rift_s_controller *ctrl, + timepoint_ns device_capture_ns, + timepoint_ns arrival_ns); + +//! Feed one rectified, unscaled accelerometer sample and the gyro rate. True when a window completed. +bool +rift_s_rest_stats_push(struct rift_s_rest_stats *s, + timepoint_ns timestamp_ns, + const struct xrt_vec3 *accel, + const struct xrt_vec3 *gyro); + +//! Add one completed rest window and re-estimate the bias. +void +rift_s_accel_bias_add_window(struct rift_s_accel_bias *b, const struct xrt_vec3 *mean); + +//! Consecutive backwards IMU samples that re-anchor the device clock. +#define RIFT_S_IMU_RESYNC_RUN 16 +//! A backwards step this large re-anchors after two samples in a row. +#define RIFT_S_IMU_RESYNC_BACKWARDS_US 1000000u + +//! Whether a backwards IMU timestamp means the device clock anchor is wrong. +bool +rift_s_imu_clock_needs_resync(uint32_t backwards_us, uint32_t backwards_run); + +//! Add a rest-window mean taken at a known IMU @p attitude (IMU to world). +void +rift_s_attitude_bias_add(struct rift_s_attitude_bias *b, + const struct xrt_vec3 *rest_mean, + const struct xrt_quat *attitude); + +/*! + * World-frame rotation that moves @p imu's tilt a @p fraction of the way to @p optical's, leaving + * heading alone. Returns the full tilt difference in radians. + */ +float +rift_s_optical_tilt_correction(const struct xrt_quat *imu, + const struct xrt_quat *optical, + float fraction, + struct xrt_quat *out_correction); + +// Interpolate a capture-time attitude, with up to 20 ms of gyro prediction for radio scheduling. +bool +rift_s_controller_get_attitude(struct rift_s_controller *ctrl, + timepoint_ns capture_ns, + struct rift_s_attitude_sample *out); + +// A heading prior requires recent optical yaw agreement as well as capture-time tilt. +bool +rift_s_controller_has_heading_prior(struct rift_s_controller *ctrl, timepoint_ns capture_ns); + +void +rift_s_controller_push_observed_pose(struct xrt_device *xdev, timepoint_ns frame_mono_ns, const struct xrt_pose *pose); +// Build the constellation LED model in the OpenCV-flipped IMU frame, firmware order as LED id. +bool +rift_s_controller_get_led_model(struct xrt_device *xdev, struct t_constellation_led_model *led_model); +// Accept an optical fit. Yaw is corrected only when the fit itself observed it. +void +rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs); + +// Controller pose callback. Optical loss preserves IMU orientation and estimates hand position. +xrt_result_t +rift_s_controller_get_tracked_pose(struct xrt_device *xdev, + enum xrt_input_name name, + int64_t at_timestamp_ns, + struct xrt_space_relation *out_relation); + void rift_s_controller_update_configuration(struct rift_s_controller *ctrl, uint64_t device_id); bool @@ -133,4 +422,8 @@ rift_s_controller_handle_report(struct rift_s_controller *ctrl, timepoint_ns local_ts, rift_s_controller_report_t *report); +#ifdef __cplusplus +} +#endif + #endif diff --git a/src/xrt/drivers/rift_s/rift_s_firmware.c b/src/xrt/drivers/rift_s/rift_s_firmware.c index 1b5a43dbe..ecc47cea4 100644 --- a/src/xrt/drivers/rift_s/rift_s_firmware.c +++ b/src/xrt/drivers/rift_s/rift_s_firmware.c @@ -18,6 +18,7 @@ /* Oculus Rift S Driver - firmware JSON parsing functions */ #include #include +#include #include "util/u_json.h" #include "util/u_misc.h" @@ -32,6 +33,43 @@ #define JSON_MATRIX_3X3_ARRAY(a, b, c) u_json_get_float_array(u_json_get(a, b), c.v, 9) #define JSON_MATRIX_4x4_ARRAY(a, b, c) u_json_get_float_array(u_json_get(a, b), c.v, 16) +struct rift_s_tracked_imu_calibration +rift_s_controller_gyro_calibration_for_imu(const struct rift_s_tracked_imu_calibration *factory, + const char *descriptor) +{ + struct rift_s_tracked_imu_calibration gyro = *factory; + if (strcmp(descriptor, "LSM6DSL") == 0) { + // Windows multiplies in double precision, then stores each coefficient as float. + const double factor = 0.8714285714285714; + for (unsigned i = 0; i < 9; i++) + gyro.matrix.v[i] = (float)(gyro.matrix.v[i] * factor); + gyro.offset.x = (float)(gyro.offset.x * factor); + gyro.offset.y = (float)(gyro.offset.y * factor); + gyro.offset.z = (float)(gyro.offset.z * factor); + } + return gyro; +} + +static bool +parse_tracked_imu_calibration(const cJSON *obj, const char *name, struct rift_s_tracked_imu_calibration *out) +{ + float values[12]; + const cJSON *array = u_json_get(obj, name); + if (!cJSON_IsArray(array) || cJSON_GetArraySize(array) != 12) + return false; + size_t count = u_json_get_float_array(array, values, 12); + if (count != 12) + return false; + for (unsigned i = 0; i < 12; i++) { + if (!isfinite(values[i])) + return false; + } + *out = (struct rift_s_tracked_imu_calibration){.num_values = (unsigned)count}; + memcpy(out->matrix.v, values, sizeof(out->matrix.v)); + out->offset = (struct xrt_vec3){values[9], values[10], values[11]}; + return true; +} + int rift_s_parse_proximity_threshold(char *json_string, int *proximity_threshold) { @@ -54,6 +92,28 @@ fail: return -1; } +/* Parse LensOpticalCenterPixelCoord from the lens calibration block (0x12). */ +int +rift_s_parse_lens_centers(char *json_string, struct xrt_vec2 centers[2]) +{ + cJSON *json_root = cJSON_Parse(json_string); + const cJSON *coords = cJSON_IsObject(json_root) ? u_json_get(json_root, "LensOpticalCenterPixelCoord") : NULL; + const char *names[2] = {"Left", "Right"}; + + for (int eye = 0; eye < 2; eye++) { + float xy[2]; + if (coords == NULL || u_json_get_float_array(u_json_get(coords, names[eye]), xy, 2) != 2) { + RIFT_S_WARN("Unrecognised Rift S lens calibration JSON data."); + cJSON_Delete(json_root); + return -1; + } + centers[eye] = (struct xrt_vec2){xy[0], xy[1]}; + } + + cJSON_Delete(json_root); + return 0; +} + static bool check_file_format_version(cJSON *json_root, float expected_version, float *version_number) { @@ -312,7 +372,12 @@ json_read_lensing_model(const cJSON *lensing_model, struct rift_s_lensing_model return false; } - model->num_points = cJSON_GetArrayItem(array, 0)->valueint; + const cJSON *count = cJSON_GetArrayItem(array, 0); + if (!cJSON_IsNumber(count) || count->valuedouble < 0 || count->valuedouble > 4 || + count->valuedouble != count->valueint) { + return false; + } + model->num_points = count->valueint; for (int j = 0; j < 4; j++) { const cJSON *item = cJSON_GetArrayItem(array, j + 1); @@ -355,10 +420,10 @@ rift_s_controller_parse_imu_calibration(char *json_string, struct rift_s_control if (!JSON_VEC3(obj, "ImuPosition", &c->imu_position)) goto fail; - if (!JSON_MATRIX_4x4_ARRAY(obj, "AccCalibration", c->accel_calibration)) + if (!parse_tracked_imu_calibration(obj, "AccCalibration", &c->accel_calibration)) goto fail; - if (!JSON_MATRIX_4x4_ARRAY(obj, "GyroCalibration", c->gyro_calibration)) + if (!parse_tracked_imu_calibration(obj, "GyroCalibration", &c->gyro_calibration)) goto fail; /* LED positions */ @@ -367,8 +432,15 @@ rift_s_controller_parse_imu_calibration(char *json_string, struct rift_s_control goto fail; } - c->num_leds = cJSON_GetArraySize(leds); + int num_leds = cJSON_GetArraySize(leds); + if (num_leds <= 0 || num_leds > UINT8_MAX) { + goto fail; + } + c->num_leds = num_leds; c->leds = calloc(c->num_leds, sizeof(struct rift_s_led)); + if (c->leds == NULL) { + goto fail; + } i = 0; cJSON_ArrayForEach(item, leds) { @@ -384,12 +456,19 @@ rift_s_controller_parse_imu_calibration(char *json_string, struct rift_s_control } c->num_lensing_models = cJSON_GetArraySize(leds); + if (c->num_lensing_models <= 0) { + goto fail; + } c->lensing_models = calloc(c->num_lensing_models, sizeof(struct rift_s_lensing_model)); + if (c->lensing_models == NULL) { + goto fail; + } i = 0; cJSON_ArrayForEach(item, leds) { if (!json_read_lensing_model(leds, c->lensing_models + i, i)) goto fail; + i++; } if (!JSON_MATRIX_3X3_ARRAY(json_root, "gyro_m", c->gyro.rectification) || diff --git a/src/xrt/drivers/rift_s/rift_s_firmware.h b/src/xrt/drivers/rift_s/rift_s_firmware.h index b7a36a6c4..bf65c7b3f 100644 --- a/src/xrt/drivers/rift_s/rift_s_firmware.h +++ b/src/xrt/drivers/rift_s/rift_s_firmware.h @@ -109,8 +109,20 @@ struct rift_s_lensing_model float points[4]; }; +/* Factory TrackedObject AccCalibration/GyroCalibration: a row-major 3x3 matrix (0..8) + * and sensor-space bias (9..11), applied as matrix * (sample - bias). + * Acceleration is in m/s^2 and angular velocity in rad/s before this transform. */ +struct rift_s_tracked_imu_calibration +{ + struct xrt_matrix_3x3 matrix; + struct xrt_vec3 offset; + //! Numbers in the firmware array; the parser requires exactly 12. + unsigned num_values; +}; + struct rift_s_controller_imu_calibration { + //! Legacy top-level calibration retained for comparison with TrackedObject. struct { struct xrt_matrix_3x3 rectification; @@ -128,12 +140,7 @@ struct rift_s_controller_imu_calibration uint8_t num_leds; struct rift_s_led *leds; - /* For some reason we have a separate calibration - * 4x4 matrix on top of the separate rectification - * and offset for gyro and accel - */ - struct xrt_matrix_4x4 gyro_calibration; - struct xrt_matrix_4x4 accel_calibration; + struct rift_s_tracked_imu_calibration gyro_calibration, accel_calibration; /* Lensing models */ int num_lensing_models; @@ -143,11 +150,18 @@ struct rift_s_controller_imu_calibration int rift_s_parse_proximity_threshold(char *json, int *proximity_threshold); int +rift_s_parse_lens_centers(char *json, struct xrt_vec2 centers[2]); +int rift_s_parse_imu_calibration(char *json, struct rift_s_imu_calibration *c); int rift_s_parse_camera_calibration_block(char *json, struct rift_s_camera_calibration_block *c); int rift_s_controller_parse_imu_calibration(char *json, struct rift_s_controller_imu_calibration *c); +/* Derive an effective gyro calibration from the unchanged factory values. + * The Windows tracker scales both matrix and bias for exactly "LSM6DSL". */ +struct rift_s_tracked_imu_calibration +rift_s_controller_gyro_calibration_for_imu(const struct rift_s_tracked_imu_calibration *factory, + const char *descriptor); void rift_s_controller_free_imu_calibration(struct rift_s_controller_imu_calibration *c); diff --git a/src/xrt/drivers/rift_s/rift_s_hmd.c b/src/xrt/drivers/rift_s/rift_s_hmd.c index edfad4632..c60ef7fea 100644 --- a/src/xrt/drivers/rift_s/rift_s_hmd.c +++ b/src/xrt/drivers/rift_s/rift_s_hmd.c @@ -26,13 +26,17 @@ #include #include #include +#include +#include #include "math/m_api.h" +#include "util/u_debug.h" #include "math/m_vec3.h" #include "os/os_time.h" #include "util/u_device.h" +#include "util/u_distortion_mesh.h" #include "util/u_trace_marker.h" #include "util/u_var.h" @@ -41,6 +45,8 @@ #include "rift_s_hmd.h" +DEBUG_GET_ONCE_BOOL_OPTION(steamvr_bench, "MONADO_STEAMVR_BENCH", false) + static xrt_result_t rift_s_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, @@ -125,8 +131,8 @@ rift_s_hmd_handle_report(struct rift_s_hmd *hmd, timepoint_ns local_ts, rift_s_h accel = m_vec3_sub(raw_accel, imu_calibration->accel.offset_at_0C); gyro = m_vec3_sub(raw_gyro, imu_calibration->gyro.offset); - math_matrix_3x3_transform_vec3(&imu_calibration->accel.rectification, &raw_accel, &accel); - math_matrix_3x3_transform_vec3(&imu_calibration->gyro.rectification, &raw_gyro, &gyro); + math_matrix_3x3_transform_vec3(&imu_calibration->accel.rectification, &accel, &accel); + math_matrix_3x3_transform_vec3(&imu_calibration->gyro.rectification, &gyro, &gyro); /* FIXME: This doesn't seem to produce the right numbers, but it's OK - we don't use it anyway */ hmd->temperature = temperature_scale * (s->temperature - temperature_offset) + 25; @@ -153,7 +159,7 @@ static xrt_result_t rift_s_compute_distortion(struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *result) { struct rift_s_hmd *hmd = (struct rift_s_hmd *)(xdev); - u_compute_distortion_panotools(&hmd->distortion_vals[view], u, v, result); + rift_s_optics_compute(&hmd->optics[view], u, v, result); return XRT_SUCCESS; } @@ -173,6 +179,69 @@ dump_fw_block(struct os_hid_device *handle, uint8_t block_id) { } #endif +/* Parse a positive projection-tangent scale from the environment; 1.0 if unset or invalid. */ +DEBUG_GET_ONCE_NUM_OPTION(rift_s_ipd_mm, "RIFT_S_IPD_MM", 63.5) + +static float +parse_fov_scale(const char *name) +{ + const char *text = getenv(name); + if (text == NULL) { + return 1.0f; + } + char *end; + const float parsed = strtof(text, &end); + const bool consumed = end != text; + while (isspace((unsigned char)*end)) { + end++; + } + struct rift_s_optics check; + if (consumed && *end == '\0' && rift_s_optics_init(&check, 0, 0.0635f, parsed, parsed)) { + return parsed; + } + RIFT_S_WARN("Invalid %s; using 1.0", name); + return 1.0f; +} + +/* Re-read the IPD file at most twice a second; ignores missing or out-of-range values. */ +static void +rift_s_poll_ipd(struct rift_s_hmd *hmd) +{ + int64_t now = os_monotonic_get_ns(); + if (now < hmd->ipd_next_poll_ns || hmd->ipd_path[0] == '\0') { + return; + } + hmd->ipd_next_poll_ns = now + 500 * 1000000ll; + FILE *f = fopen(hmd->ipd_path, "r"); + if (f == NULL) { + return; + } + float mm = 0; + if (fscanf(f, "%f", &mm) == 1 && mm >= 50.0f && mm <= 80.0f && fabsf(mm / 1000.0f - hmd->ipd_m) > 1e-5f) { + hmd->ipd_m = mm / 1000.0f; + RIFT_S_INFO("Rift S IPD set to %.1f mm", mm); + } + fclose(f); +} + +static xrt_result_t +rift_s_get_view_poses(struct xrt_device *xdev, + const struct xrt_vec3 *default_eye_relation, + int64_t at_timestamp_ns, + enum xrt_view_type view_type, + uint32_t view_count, + struct xrt_space_relation *out_head_relation, + struct xrt_fov *out_fovs, + struct xrt_pose *out_poses) +{ + struct rift_s_hmd *hmd = (struct rift_s_hmd *)xdev; + rift_s_poll_ipd(hmd); + struct xrt_vec3 eye_relation = *default_eye_relation; + eye_relation.x = hmd->ipd_m; + return u_device_get_view_poses(xdev, &eye_relation, at_timestamp_ns, view_type, view_count, out_head_relation, + out_fovs, out_poses); +} + static void rift_s_hmd_destroy(struct xrt_device *xdev) { @@ -218,12 +287,22 @@ rift_s_hmd_create(struct rift_s_system *sys, const unsigned char *hmd_serial_no, hmd->base.update_inputs = u_device_noop_update_inputs; hmd->base.get_tracked_pose = rift_s_get_tracked_pose; - hmd->base.get_view_poses = u_device_get_view_poses; + hmd->base.get_view_poses = rift_s_get_view_poses; + hmd->ipd_m = debug_get_num_option_rift_s_ipd_mm() / 1000.0f; + if (hmd->ipd_m < 0.05f || hmd->ipd_m > 0.08f) { + hmd->ipd_m = 0.0635f; + } + snprintf(hmd->ipd_path, sizeof(hmd->ipd_path), "%s/rift-s-ipd.conf", getenv("HOME")); + rift_s_poll_ipd(hmd); + RIFT_S_INFO("Rift S IPD %.1f mm", hmd->ipd_m * 1000.0f); hmd->base.destroy = rift_s_hmd_destroy; hmd->base.name = XRT_DEVICE_GENERIC_HMD; hmd->base.device_type = XRT_DEVICE_TYPE_HMD; hmd->tracker = rift_s_system_get_tracker(sys); + hmd->base.camera = hmd->tracker->base.camera; + hmd->base.supported.orientation_tracking = true; + hmd->base.supported.position_tracking = hmd->tracker->tracking.slam_enabled; // Print name. snprintf(hmd->base.str, XRT_DEVICE_NAME_LEN, "Oculus Rift S"); @@ -248,9 +327,7 @@ rift_s_hmd_create(struct rift_s_system *sys, const unsigned char *hmd_serial_no, dump_fw_block(hid_hmd, 0x12); #endif - // Set up display details - // FIXME: These are all wrong and should be derived from HMD reports - // refresh rate + // Native display layout and 80 Hz refresh rate. hmd->base.hmd->screens[0].nominal_frame_interval_ns = time_s_to_ns(1.0f / 80.0f); /* In the Rift S, there's one panel that is rotated @@ -280,45 +357,24 @@ rift_s_hmd_create(struct rift_s_system *sys, const unsigned char *hmd_serial_no, hmd->base.hmd->views[0].viewport.y_pixels = 0; hmd->base.hmd->views[1].viewport.y_pixels = view_h; - /* FIXME: Incorrection distortion taken from the Rift CV1 for now */ - const double display_w_meters = 0.149760f / 2.0; // Per-eye width - const double display_h_meters = 0.093600f; - const double lens_sep = 0.074f; - const double hFOV = DEG_TO_RAD(105.0); - - // center of projection - const double hCOP = lens_sep / 2.0; - const double vCOP = display_h_meters / 2.0; - - struct u_panotools_values distortion_vals = { - .distortion_k = {0.819f, -0.241f, 0.324f, 0.098f, 0.0}, - .aberration_k = {0.9952420f, 1.0f, 1.0008074f}, - .scale = display_w_meters - - lens_sep / 2.0, // Assume distortion is across the larger distance from lens center to edge - .lens_center = {display_w_meters - hCOP, vCOP}, - .viewport_size = {display_w_meters, display_h_meters}, - }; - - if ( - /* right eye */ - !math_compute_fovs(display_w_meters, hCOP, hFOV, display_h_meters, vCOP, 0.0, - &hmd->base.hmd->distortion.fov[1]) || - /* - * left eye - same as right eye, except the horizontal center of projection is moved in the opposite - * direction now - */ - !math_compute_fovs(display_w_meters, display_w_meters - hCOP, hFOV, display_h_meters, vCOP, 0.0, - &hmd->base.hmd->distortion.fov[0])) { - // If those failed, it means our math was impossible. - RIFT_S_ERROR("Failed to setup basic device info"); - goto cleanup; + // RIFT_S_FOV_SCALE scales all projection tangents, RIFT_S_FOV_SCALE_Y only the vertical ones. + const float fov_scale = parse_fov_scale("RIFT_S_FOV_SCALE"); + const float fov_scale_y = parse_fov_scale("RIFT_S_FOV_SCALE_Y"); + for (uint8_t eye = 0; eye < 2; eye++) { + // The lens centre follows the IPD at creation; the mesh is built once. + if (!rift_s_optics_init(&hmd->optics[eye], eye, hmd->ipd_m, fov_scale, fov_scale_y)) { + RIFT_S_ERROR("Failed to initialise Rift S optics"); + goto cleanup; + } + hmd->base.hmd->distortion.fov[eye] = hmd->optics[eye].fov; + const struct xrt_fov *fov = &hmd->optics[eye].fov; + RIFT_S_INFO("Rift S eye %u FOV tan: left=%.6f right=%.6f up=%.6f down=%.6f; scale=%.6f, vertical scale=%.6f", + eye, tanf(fov->angle_left), tanf(fov->angle_right), tanf(fov->angle_up), tanf(fov->angle_down), + fov_scale, fov_scale_y); + RIFT_S_INFO("Rift S eye %u lens centre (%.2f, %.2f) px at IPD %.1f mm", eye, + hmd->optics[eye].lens_center_px.x, hmd->optics[eye].lens_center_px.y, hmd->ipd_m * 1000.0f); } - hmd->distortion_vals[0] = distortion_vals; - // Move the lens center for the right view - distortion_vals.lens_center.x = hCOP; - hmd->distortion_vals[1] = distortion_vals; - hmd->base.hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE; hmd->base.hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE; hmd->base.compute_distortion = rift_s_compute_distortion; @@ -349,6 +405,11 @@ cleanup: void rift_s_hmd_set_proximity(struct rift_s_hmd *hmd, bool prox_sensor) { + if (debug_get_bool_option_steamvr_bench()) { + prox_sensor = true; + } + rift_s_tracker_set_standby(hmd->tracker, !prox_sensor); + /* Enable the screen if the prox sensor is triggered, or turn it off otherwise. */ if (prox_sensor != hmd->display_on) { struct os_hid_device *hid_hmd = rift_s_system_hid_handle(hmd->sys); diff --git a/src/xrt/drivers/rift_s/rift_s_hmd.h b/src/xrt/drivers/rift_s/rift_s_hmd.h index 5dbeb8c6c..8a7322ca9 100644 --- a/src/xrt/drivers/rift_s/rift_s_hmd.h +++ b/src/xrt/drivers/rift_s/rift_s_hmd.h @@ -16,7 +16,7 @@ #pragma once #include "math/m_imu_3dof.h" -#include "util/u_distortion_mesh.h" +#include "rift_s_optics.h" #include "xrt/xrt_defines.h" #include "xrt/xrt_device.h" @@ -45,8 +45,14 @@ struct rift_s_hmd float temperature; bool display_on; - /* Temporary distortion values for mesh calc */ - struct u_panotools_values distortion_vals[2]; + /* Per-eye projection and lens model. */ + struct rift_s_optics optics[2]; + + /* Eye separation in metres. Rift S lenses are fixed, IPD is a rendering setting. + * Set by RIFT_S_IPD_MM, live-updated from ~/rift-s-ipd.conf (millimetres). */ + float ipd_m; + char ipd_path[4096]; + int64_t ipd_next_poll_ns; }; struct rift_s_hmd * diff --git a/src/xrt/drivers/rift_s/rift_s_model_check.cpp b/src/xrt/drivers/rift_s/rift_s_model_check.cpp new file mode 100644 index 000000000..31e6c33f4 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_model_check.cpp @@ -0,0 +1,207 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Consistency checks for Rift S controller firmware models. + * @ingroup drv_rift_s + */ +#include "rift_s_model_check.h" + +#include +#include +#include + +#include +#include +#include + +namespace { +using V3 = Eigen::Vector3f; +using M3 = Eigen::Matrix3f; + +V3 +vec(const xrt_vec3 &v) +{ + return V3(v.x, v.y, v.z); +} + +float +degrees(float radians) +{ + return radians * 180.0f / float(M_PI); +} + +rift_s_matrix_summary +summarise(const M3 &m) +{ + rift_s_matrix_summary s{}; + s.determinant = m.determinant(); + s.reflection = s.determinant < 0; + Eigen::JacobiSVD svd(m, Eigen::ComputeFullU | Eigen::ComputeFullV); + s.singular_min = svd.singularValues().minCoeff(); + s.singular_max = svd.singularValues().maxCoeff(); + M3 d = M3::Identity(); + d(2, 2) = (svd.matrixU() * svd.matrixV().transpose()).determinant() < 0 ? -1 : 1; + M3 r = svd.matrixU() * d * svd.matrixV().transpose(); + s.rotation_deg = degrees(std::acos(std::clamp((r.trace() - 1) / 2, -1.0f, 1.0f))); + return s; +} + +M3 +nearest_rotation(const M3 &m) +{ + Eigen::JacobiSVD svd(m, Eigen::ComputeFullU | Eigen::ComputeFullV); + M3 d = M3::Identity(); + d(2, 2) = (svd.matrixU() * svd.matrixV().transpose()).determinant() < 0 ? -1 : 1; + return svd.matrixU() * d * svd.matrixV().transpose(); +} + +/* Firmware matrices are stored row-major, as parsed. */ +M3 +matrix3(const xrt_matrix_3x3 &m) +{ + M3 out; + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + out(r, c) = m.v[3 * r + c]; + return out; +} + +float +rotation_between_deg(const M3 &a, const M3 &b) +{ + M3 between = nearest_rotation(a).transpose() * nearest_rotation(b); + return degrees(std::acos(std::clamp((between.trace() - 1) / 2, -1.0f, 1.0f))); +} + +V3 +relate(const V3 &v, rift_s_hand_relation relation) +{ + V3 out = v; + if (relation != RIFT_S_HANDS_IDENTICAL) + out(relation - RIFT_S_HANDS_MIRROR_X) = -out(relation - RIFT_S_HANDS_MIRROR_X); + return out; +} +} // namespace + +extern "C" void +rift_s_controller_model_check(const struct rift_s_controller_imu_calibration *c, + struct rift_s_controller_model_report *out) +{ + *out = {}; + if (c == nullptr || c->leds == nullptr || c->num_leds == 0) + return; + out->num_leds = c->num_leds; + out->num_lensing_models = c->num_lensing_models; + // The parser reads "Point%d" for 0..n-1, so a gap fails parsing instead of renumbering. + out->ids_sequential = true; + V3 centroid = V3::Zero(); + for (unsigned i = 0; i < c->num_leds; i++) + centroid += vec(c->leds[i].pos); + centroid /= float(c->num_leds); + out->centroid = {centroid.x(), centroid.y(), centroid.z()}; + out->normal_length_min = std::numeric_limits::infinity(); + out->angle_x_min = out->angle_y_min = std::numeric_limits::infinity(); + out->angle_x_max = out->angle_y_max = -std::numeric_limits::infinity(); + for (unsigned i = 0; i < c->num_leds; i++) { + const rift_s_led &led = c->leds[i]; + float length = vec(led.dir).norm(); + out->normal_length_min = std::min(out->normal_length_min, length); + out->normal_length_max = std::max(out->normal_length_max, length); + out->outward_normals += vec(led.dir).dot(vec(led.pos) - centroid) > 0; + out->angle_x_min = std::min(out->angle_x_min, led.angles.x); + out->angle_x_max = std::max(out->angle_x_max, led.angles.x); + out->angle_y_min = std::min(out->angle_y_min, led.angles.y); + out->angle_y_max = std::max(out->angle_y_max, led.angles.y); + } + out->imu_position = c->imu_position; + out->imu_to_centroid_m = (vec(c->imu_position) - centroid).norm(); + M3 accel = matrix3(c->accel.rectification), gyro = matrix3(c->gyro.rectification); + out->accel_rectification = summarise(accel); + out->gyro_rectification = summarise(gyro); + out->accel_gyro_axes_deg = rotation_between_deg(accel, gyro); + M3 accel_tracked = matrix3(c->accel_calibration.matrix), gyro_tracked = matrix3(c->gyro_calibration.matrix); + out->accel_calibration = summarise(accel_tracked); + out->gyro_calibration = summarise(gyro_tracked); + out->accel_calibration_offset_norm = vec(c->accel_calibration.offset).norm(); + out->gyro_calibration_offset_norm = vec(c->gyro_calibration.offset).norm(); + out->accel_calibration_values = c->accel_calibration.num_values; + out->gyro_calibration_values = c->gyro_calibration.num_values; + out->accel_calibration_vs_rectification_deg = rotation_between_deg(accel, accel_tracked); + out->gyro_calibration_vs_rectification_deg = rotation_between_deg(gyro, gyro_tracked); +} + +extern "C" void +rift_s_controller_gravity_check(const struct rift_s_controller_imu_calibration *c, + const struct xrt_vec3 *calibrated_mean, + struct rift_s_controller_gravity_report *out) +{ + *out = {}; + if (c == nullptr || calibrated_mean == nullptr || c->accel_calibration.num_values != 12) + return; + M3 factory = matrix3(c->accel_calibration.matrix); + if (std::abs(factory.determinant()) < 1e-6f) + return; + // Recover sensor units from the factory-calibrated sample, then compare the legacy fields. + V3 tracked = vec(*calibrated_mean); + V3 sample = factory.inverse() * tracked + vec(c->accel_calibration.offset); + V3 rectified = matrix3(c->accel.rectification) * (sample - vec(c->accel.offset)); + out->rectified_mps2 = rectified.norm(); + out->tracked_mps2 = tracked.norm(); + if (out->rectified_mps2 <= 0 || out->tracked_mps2 <= 0) + return; + float dot = rectified.normalized().dot(tracked.normalized()); + out->direction_deg = degrees(std::acos(std::clamp(dot, -1.0f, 1.0f))); + out->valid = true; +} + +extern "C" void +rift_s_controller_compare_hands(const struct rift_s_controller_imu_calibration *left, + const struct rift_s_controller_imu_calibration *right, + struct rift_s_controller_hand_comparison *out) +{ + *out = {}; + if (left == nullptr || right == nullptr || left->leds == nullptr || right->leds == nullptr || + left->num_leds == 0 || right->num_leds == 0) + return; + float best = std::numeric_limits::infinity(); + for (int r = 0; r < RIFT_S_HANDS_RELATION_COUNT; r++) { + auto relation = rift_s_hand_relation(r); + double squared = 0, normal = 0; + for (unsigned i = 0; i < right->num_leds; i++) { + V3 p = relate(vec(right->leds[i].pos), relation); + V3 n = relate(vec(right->leds[i].dir), relation).normalized(); + float nearest = std::numeric_limits::infinity(); + unsigned match = 0; + for (unsigned j = 0; j < left->num_leds; j++) { + float d = (vec(left->leds[j].pos) - p).norm(); + if (d < nearest) { + nearest = d; + match = j; + } + } + squared += double(nearest) * nearest; + float dot = std::clamp(n.dot(vec(left->leds[match].dir).normalized()), -1.0f, 1.0f); + normal += degrees(std::acos(dot)); + } + out->position_rms_m[r] = float(std::sqrt(squared / right->num_leds)); + out->normal_mean_deg[r] = float(normal / right->num_leds); + out->imu_difference_m[r] = (relate(vec(right->imu_position), relation) - vec(left->imu_position)).norm(); + if (out->position_rms_m[r] < best) { + best = out->position_rms_m[r]; + out->best = relation; + } + } +} + +extern "C" const char * +rift_s_hand_relation_name(enum rift_s_hand_relation relation) +{ + switch (relation) { + case RIFT_S_HANDS_IDENTICAL: return "identical"; + case RIFT_S_HANDS_MIRROR_X: return "mirror_x"; + case RIFT_S_HANDS_MIRROR_Y: return "mirror_y"; + case RIFT_S_HANDS_MIRROR_Z: return "mirror_z"; + default: return "unknown"; + } +} diff --git a/src/xrt/drivers/rift_s/rift_s_model_check.h b/src/xrt/drivers/rift_s/rift_s_model_check.h new file mode 100644 index 000000000..c5c993ea5 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_model_check.h @@ -0,0 +1,117 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Consistency checks for Rift S controller firmware models. + * + * These checks do not change tracking. They summarise per-device firmware data so left/right + * identity, normals, IMU placement, IMU axes and mirroring can be compared on real hardware. + * + * @ingroup drv_rift_s + */ +#pragma once + +#include "rift_s_firmware.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//! Rotation, reflection and scale summary of a calibration matrix. +struct rift_s_matrix_summary +{ + float determinant; + //! Angle of the nearest proper rotation (polar decomposition), degrees. + float rotation_deg; + //! Singular values: away from 1 means scale or non-orthogonality. + float singular_min, singular_max; + //! The matrix contains a reflection (determinant < 0). + bool reflection; +}; + +struct rift_s_controller_model_report +{ + unsigned num_leds; + unsigned num_lensing_models; + //! LED ids are the PointN indices: always 0..num_leds-1 when parsing succeeded. + bool ids_sequential; + float normal_length_min, normal_length_max; + //! Normals pointing away from the LED centroid (dot(dir, pos - centroid) > 0). + unsigned outward_normals; + struct xrt_vec3 centroid; + struct xrt_vec3 imu_position; + float imu_to_centroid_m; + //! Range of the two per-LED angle fields (85/80 so far); Monado uses one global limit. + float angle_x_min, angle_x_max, angle_y_min, angle_y_max; + struct rift_s_matrix_summary accel_rectification; + struct rift_s_matrix_summary gyro_rectification; + //! Angle between the rotation parts of the accelerometer and gyro rectifications, degrees. + float accel_gyro_axes_deg; + //! TrackedObject AccCalibration/GyroCalibration 3x3 part: parsed, not applied by Monado. + struct rift_s_matrix_summary accel_calibration; + struct rift_s_matrix_summary gyro_calibration; + float accel_calibration_offset_norm, gyro_calibration_offset_norm; + //! Numbers in each firmware array (the 3x3 + offset layout needs 12). + unsigned accel_calibration_values, gyro_calibration_values; + //! Angle between the rotation parts of the TrackedObject matrix and the rectification, degrees. + float accel_calibration_vs_rectification_deg, gyro_calibration_vs_rectification_deg; +}; + +enum rift_s_hand_relation +{ + RIFT_S_HANDS_IDENTICAL, + RIFT_S_HANDS_MIRROR_X, + RIFT_S_HANDS_MIRROR_Y, + RIFT_S_HANDS_MIRROR_Z, + RIFT_S_HANDS_RELATION_COUNT, +}; + +struct rift_s_controller_hand_comparison +{ + //! Nearest-neighbour RMS (metres) from right LEDs to left LEDs under each relation. + float position_rms_m[RIFT_S_HANDS_RELATION_COUNT]; + //! Mean angle between matched normals under each relation, degrees. + float normal_mean_deg[RIFT_S_HANDS_RELATION_COUNT]; + //! IMU position difference under each relation, metres. + float imu_difference_m[RIFT_S_HANDS_RELATION_COUNT]; + enum rift_s_hand_relation best; +}; + +/*! + * The resting accelerometer under both firmware calibrations. Monado uses the top-level + * rectification and offset; the TrackedObject calibration is the alternative. On a stationary + * controller the right one reads 9.81 m/s^2, and the direction difference is the tilt error the + * other would introduce. + */ +struct rift_s_controller_gravity_report +{ + //! |g| under acc_m/acc_b (the driver's calibration), m/s^2. + float rectified_mps2; + //! |g| under the TrackedObject AccCalibration, m/s^2. + float tracked_mps2; + //! Angle between the two gravity directions in the IMU frame, degrees. + float direction_deg; + bool valid; +}; + +//! @p calibrated_mean is the mean resting accelerometer after factory TrackedObject calibration. +void +rift_s_controller_gravity_check(const struct rift_s_controller_imu_calibration *calibration, + const struct xrt_vec3 *calibrated_mean, + struct rift_s_controller_gravity_report *out); + +void +rift_s_controller_model_check(const struct rift_s_controller_imu_calibration *calibration, + struct rift_s_controller_model_report *out); + +void +rift_s_controller_compare_hands(const struct rift_s_controller_imu_calibration *left, + const struct rift_s_controller_imu_calibration *right, + struct rift_s_controller_hand_comparison *out); + +const char * +rift_s_hand_relation_name(enum rift_s_hand_relation relation); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/rift_s/rift_s_optics.c b/src/xrt/drivers/rift_s/rift_s_optics.c new file mode 100644 index 000000000..ebf69acee --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_optics.c @@ -0,0 +1,149 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 + +#include "rift_s_optics.h" + +#include + +#define VIEW_W 1280.0 +#define VIEW_H 1440.0 + +/* Oculus runtime 1.82.0, hmdq 2.1.5, Rift S native 80 Hz, IPD 63 mm: + * https://risa2000.github.io/hmdgdb/hmd_cfgs/OculusRiftS_Native_R80.html + * Left eye FOV tangents; the right eye is mirrored. + */ +static const double left_extent = 0.965689; +static const double right_extent = 1.035530; +static const double up_extent = 1.000000; +static const double down_extent = 1.150368; + +/* Fitted to the runtime's distortion mesh cache (Meta Horizon runtime 1.208, firmware 2.2), identical for both + * eyes: panel pixel radius = k0*t + k1*t^3 + k2*t^5 + k3*t^7 + k4*t^9 for tangent radius t. Max fit error is + * below 0.45 px inside the visible FOV. Monotonic on [0, TAN_MAX]. + */ +#define TAN_MAX 1.7 +static const double channel_k[3][5] = { + {828.9789, -281.8288, 92.5256, -18.9440, 1.7376}, // R + {823.6001, -288.6719, 104.1311, -25.0075, 2.8234}, // G + {820.1631, -289.5132, 107.2478, -26.7999, 3.1539}, // B +}; + +/* Left eye lens centre x in the per-eye view for IPDs 53.5, 55.5, ... 73.5 mm. The runtime shifts the lens + * centre with IPD ("eye shift swim"); y is the same for all of them. The right eye is the mirror image. + * Firmware block 0x12 centres differ from these by ~2 px and are not what the runtime renders with. + */ +#define CENTER_IPD_MIN_MM 53.5 +#define CENTER_IPD_STEP_MM 2.0 +#define CENTER_COUNT 11 +static const double left_center_x[CENTER_COUNT] = { + 605.495, 605.366, 605.671, 606.293, 607.116, 608.039, 608.962, 609.785, 610.407, 610.712, 610.583, +}; +#define CENTER_Y 719.992 + +static double +radius_px(const double *k, double t) +{ + const double t2 = t * t; + return t * (k[0] + t2 * (k[1] + t2 * (k[2] + t2 * (k[3] + t2 * k[4])))); +} + +static double +radius_px_slope(const double *k, double t) +{ + const double t2 = t * t; + return k[0] + t2 * (3 * k[1] + t2 * (5 * k[2] + t2 * (7 * k[3] + t2 * 9 * k[4]))); +} + +/* Inverts radius_px on [0, TAN_MAX] with a bracketed Newton iteration. Beyond the range it continues linearly; + * those points are outside the FOV anyway. + */ +static double +tangent_radius(const double *k, double r) +{ + const double r_max = radius_px(k, TAN_MAX); + if (r >= r_max) { + return TAN_MAX + (r - r_max) / radius_px_slope(k, TAN_MAX); + } + double lo = 0, hi = TAN_MAX; + double t = r / k[0]; + for (int i = 0; i < 50; i++) { + if (!(t > lo && t < hi)) { + t = 0.5 * (lo + hi); + } + const double err = radius_px(k, t) - r; + if (fabs(err) < 1e-9) { + break; + } + if (err > 0) { + hi = t; + } else { + lo = t; + } + t -= err / radius_px_slope(k, t); + } + return t; +} + +/* Piecewise-linear lookup, clamped to the table's IPD range. */ +static double +lens_center_x(double ipd_mm) +{ + double pos = (ipd_mm - CENTER_IPD_MIN_MM) / CENTER_IPD_STEP_MM; + pos = fmin(fmax(pos, 0.0), CENTER_COUNT - 1); + const int i = pos < CENTER_COUNT - 1 ? (int)pos : CENTER_COUNT - 2; + return left_center_x[i] + (pos - i) * (left_center_x[i + 1] - left_center_x[i]); +} + +bool +rift_s_optics_init(struct rift_s_optics *optics, uint32_t eye, float ipd_m, float fov_scale, float fov_scale_y) +{ + if (eye > 1 || !isfinite(ipd_m) || !isfinite(fov_scale) || fov_scale <= 0 || !isfinite(fov_scale_y) || + fov_scale_y <= 0) { + return false; + } + const float left = (eye == 0 ? left_extent : right_extent) * fov_scale; + const float right = (eye == 0 ? right_extent : left_extent) * fov_scale; + const float up = up_extent * fov_scale * fov_scale_y; + const float down = down_extent * fov_scale * fov_scale_y; + const struct xrt_fov fov = { + .angle_left = -atanf(left), + .angle_right = atanf(right), + .angle_up = atanf(up), + .angle_down = -atanf(down), + }; + /* Reject scales that overflow or round a projection angle to pi/2. */ + if (!(tanf(fov.angle_left) < 0 && tanf(fov.angle_right) > 0 && tanf(fov.angle_up) > 0 && + tanf(fov.angle_down) < 0)) { + return false; + } + const double cx = lens_center_x(ipd_m * 1000.0); + *optics = (struct rift_s_optics){ + .fov = fov, + .lens_center_px = {eye == 0 ? cx : VIEW_W - cx, CENTER_Y}, + .tan_left = -tanf(fov.angle_left), + .tan_right = tanf(fov.angle_right), + .tan_up = tanf(fov.angle_up), + .tan_down = -tanf(fov.angle_down), + }; + return true; +} + +void +rift_s_optics_compute(const struct rift_s_optics *optics, float u, float v, struct xrt_uv_triplet *result) +{ + /* UVs are in the upright per-eye view, top-left origin and Y down. + * driver_monado/compositor already apply the physical panel rotation. + * Out-of-FOV samples are normal near the corners and are not clamped. + */ + const double dx = u * VIEW_W - optics->lens_center_px.x; + const double dy = v * VIEW_H - optics->lens_center_px.y; + const double r = hypot(dx, dy); + const double width = (double)optics->tan_left + optics->tan_right; + const double height = (double)optics->tan_up + optics->tan_down; + struct xrt_vec2 *rgb[3] = {&result->r, &result->g, &result->b}; + for (int i = 0; i < 3; i++) { + const double scale = r > 0 ? tangent_radius(channel_k[i], r) / r : 0.0; + rgb[i]->x = (float)((dx * scale + optics->tan_left) / width); + rgb[i]->y = (float)((dy * scale + optics->tan_up) / height); + } +} diff --git a/src/xrt/drivers/rift_s/rift_s_optics.h b/src/xrt/drivers/rift_s/rift_s_optics.h new file mode 100644 index 000000000..5fe985aa8 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_optics.h @@ -0,0 +1,37 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include "xrt/xrt_defines.h" +#include "xrt/xrt_device.h" + +/*! + * Rift S lens model fitted to the Oculus runtime's own distortion mesh. + * + * Each colour channel is purely radial around the lens centre: + * panel pixel radius = odd polynomial of the tangent-space radius. + */ +struct rift_s_optics +{ + struct xrt_fov fov; + //! Lens centre in the upright 1280x1440 per-eye view, top-left origin, Y down. + struct xrt_vec2 lens_center_px; + //! Positive tangents of the published fov, used to map tangent space to UV. + float tan_left, tan_right, tan_up, tan_down; +}; + +/*! + * @param eye 0 left, 1 right + * @param ipd_m Configured eye separation in metres; shifts the lens centre like the Oculus runtime does + * @param fov_scale Scales all projection tangents (changes the fov only, not the lens) + * @param fov_scale_y Additionally scales the vertical tangents + */ +bool +rift_s_optics_init(struct rift_s_optics *optics, uint32_t eye, float ipd_m, float fov_scale, float fov_scale_y); + +/*! + * @param u,v Position in the upright per-eye view, top-left origin, Y down, in 0..1 + * @param result Per-channel UV into the eye's rendered image + */ +void +rift_s_optics_compute(const struct rift_s_optics *optics, float u, float v, struct xrt_uv_triplet *result); diff --git a/src/xrt/drivers/rift_s/rift_s_passthrough.c b/src/xrt/drivers/rift_s/rift_s_passthrough.c new file mode 100644 index 000000000..a7bc38587 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_passthrough.c @@ -0,0 +1,263 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "rift_s_passthrough.h" +#include "os/os_threading.h" +#include "os/os_time.h" +#include "util/u_frame.h" +#include "util/u_logging.h" +#include +#include +#include + +#define VIEW_SIZE 320 +#define MAX_AGE_NS (250 * U_TIME_1MS_IN_NS) + +struct rift_s_passthrough +{ + struct xrt_camera base; + struct os_mutex mutex; + struct os_mutex convert_mutex; + bool active; + struct xrt_frame *raw[2]; + struct xrt_frame *converted; + uint64_t rejected_pairs; + uint64_t last_diag_ns; + struct xrt_vec2 map[2][VIEW_SIZE * VIEW_SIZE]; +}; + +static bool +valid_payload(const struct xrt_frame *frame) +{ + return frame && frame->data && frame->format == XRT_FORMAT_L8 && frame->width >= 2 && + frame->height >= 2 && frame->stride >= frame->width && frame->size >= frame->width && + frame->height - 1 <= (frame->size - frame->width) / frame->stride; +} + +struct sensor_pixels +{ + uint8_t min, max; + uint64_t sum; +}; + +static bool +has_pixels(const struct xrt_frame *frame) +{ + for (uint32_t y = 0; y < frame->height; ++y) + for (uint32_t x = 0; x < frame->width; ++x) + if (frame->data[y * frame->stride + x] != 0) + return true; + return false; +} + +static struct sensor_pixels +measure_pixels(const struct xrt_frame *frame) +{ + struct sensor_pixels pixels = {.min = UINT8_MAX}; + // Inspect image rows, not the transport padding between sensor crops. + for (uint32_t y = 0; y < frame->height; ++y) { + const uint8_t *row = frame->data + y * frame->stride; + for (uint32_t x = 0; x < frame->width; ++x) { + uint8_t value = row[x]; + pixels.min = value < pixels.min ? value : pixels.min; + pixels.max = value > pixels.max ? value : pixels.max; + pixels.sum += value; + } + } + return pixels; +} + +bool +rift_s_passthrough_project(const struct rift_s_camera_calibration *c, float x, float y, struct xrt_vec2 *pixel) +{ + /* Firmware DeviceFromCamera maps native +Y down/+Z forward camera + * rays into the +Y up/-Z forward headset. Include sensor roll/pitch. */ + const float *m = c->device_from_camera.v; + double cx = m[0] * x - m[1] * y - m[2]; + double cy = m[4] * x - m[5] * y - m[6]; + double cz = m[8] * x - m[9] * y - m[10]; + if (cz <= 0 || !isfinite(cx) || !isfinite(cy) || !isfinite(cz)) + return false; + cx /= cz; + cy /= cz; + double radius = hypot(cx, cy); + double theta = atan(radius); + double theta2 = theta * theta; + double scale = radius > 1e-8 ? theta / radius : 1; + double u = cx * scale, v = cy * scale; + double radial = c->distortion.k[5]; + for (int i = 4; i >= 0; --i) + radial = radial * theta2 + c->distortion.k[i]; + radial = radial * theta2 + 1; + double p1 = c->distortion.p1, p2 = c->distortion.p2; + pixel->x = c->projection.fx * (u * radial + 2 * p1 * u * v + p2 * (theta2 + 2 * u * u)) + c->projection.cx; + pixel->y = c->projection.fy * (v * radial + 2 * p2 * u * v + p1 * (theta2 + 2 * v * v)) + c->projection.cy; + return isfinite(pixel->x) && isfinite(pixel->y); +} + +static void +set_active(struct xrt_camera *camera, bool active) +{ + struct rift_s_passthrough *p = (struct rift_s_passthrough *)camera; + os_mutex_lock(&p->mutex); + p->active = active; + if (!active) + for (int i = 0; i < 2; ++i) + xrt_frame_reference(&p->raw[i], NULL); + os_mutex_unlock(&p->mutex); +} + +void +rift_s_passthrough_push(struct xrt_camera *camera, struct xrt_frame *left, struct xrt_frame *right) +{ + if (!camera) + return; + struct rift_s_passthrough *p = (struct rift_s_passthrough *)camera; + /* This handoff runs on camera ingestion, which also delivers controller + * exposures. A slow export consumer must lose a video frame, not block it. */ + if (os_mutex_trylock(&p->mutex) != 0) + return; + if (p->active) { + bool valid = valid_payload(left) && valid_payload(right) && left->width == right->width && + left->height == right->height && left->timestamp == right->timestamp && + left->source_sequence == right->source_sequence; + bool nonempty[2] = {false}; + if (valid) { + nonempty[0] = has_pixels(left); + nonempty[1] = has_pixels(right); + // Missing sensor payloads can have valid packed-frame metadata. A dark + // stereo scene is valid; a unilateral all-zero payload is not a pair. + valid = nonempty[0] == nonempty[1]; + } + if (!valid) { + ++p->rejected_pairs; + if ((p->rejected_pairs & (p->rejected_pairs - 1)) == 0) + U_LOG_W("Passthrough rejected stereo pair count=%" PRIu64 " left_nonzero=%d right_nonzero=%d", + p->rejected_pairs, nonempty[0], nonempty[1]); + // Preserve only the previous complete pair. get_frame still expires it + // at its original capture timestamp, never at the rejected timestamp. + os_mutex_unlock(&p->mutex); + return; + } + uint64_t now = os_monotonic_get_ns(); + if (now - p->last_diag_ns >= 5ULL * U_TIME_1S_IN_NS) { + p->last_diag_ns = now; + struct sensor_pixels pixels[2] = {measure_pixels(left), measure_pixels(right)}; + double count = (double)left->width * left->height; + u_log(__FILE__, __LINE__, __func__, U_LOGGING_INFO, + "Passthrough stereo sequence=%" PRIu64 " timestamp=%" PRIu64 + " left=%u,%u,%.3f right=%u,%u,%.3f rejected=%" PRIu64, + left->source_sequence, left->timestamp, pixels[0].min, pixels[0].max, + pixels[0].sum / count, pixels[1].min, pixels[1].max, pixels[1].sum / count, + p->rejected_pairs); + } + xrt_frame_reference(&p->raw[0], left); + xrt_frame_reference(&p->raw[1], right); + } + os_mutex_unlock(&p->mutex); +} + +static bool +get_frame(struct xrt_camera *camera, struct xrt_frame **out) +{ + struct rift_s_passthrough *p = (struct rift_s_passthrough *)camera; + struct xrt_frame *raw[2] = {0}; + os_mutex_lock(&p->convert_mutex); + os_mutex_lock(&p->mutex); + int64_t now = os_monotonic_get_ns(); + bool available = p->active && p->raw[0] && p->raw[1] && now >= p->raw[0]->timestamp && + now - p->raw[0]->timestamp <= MAX_AGE_NS; + if (available) + for (int i = 0; i < 2; ++i) + xrt_frame_reference(&raw[i], p->raw[i]); + os_mutex_unlock(&p->mutex); + if (!available) { + os_mutex_unlock(&p->convert_mutex); + return false; + } + if (!p->converted || p->converted->timestamp != raw[0]->timestamp) { + struct xrt_frame *frame = NULL; + u_frame_create_one_off(XRT_FORMAT_R8G8B8, VIEW_SIZE * 2, VIEW_SIZE, &frame); + frame->timestamp = raw[0]->timestamp; + frame->source_sequence = raw[0]->source_sequence; + frame->stereo_format = XRT_STEREO_FORMAT_SBS; + for (int eye = 0; eye < 2; ++eye) { + struct xrt_frame *src = raw[eye]; + for (uint32_t y = 0; y < VIEW_SIZE; ++y) + for (uint32_t x = 0; x < VIEW_SIZE; ++x) { + struct xrt_vec2 uv = p->map[eye][y * VIEW_SIZE + x]; + uint8_t value = 0; + if (uv.x >= 0 && uv.y >= 0 && uv.x < src->width - 1 && uv.y < src->height - 1) { + uint32_t ix = (uint32_t)uv.x, iy = (uint32_t)uv.y; + float dx = uv.x - ix, dy = uv.y - iy; + const uint8_t *a = src->data + iy * src->stride + ix; + value = + (uint8_t)((a[0] * (1 - dx) + a[1] * dx) * (1 - dy) + + (a[src->stride] * (1 - dx) + a[src->stride + 1] * dx) * + dy); + } + uint8_t *dst = frame->data + y * frame->stride + (eye * VIEW_SIZE + x) * 3; + dst[0] = dst[1] = dst[2] = value; + } + } + xrt_frame_reference(&p->converted, frame); + xrt_frame_reference(&frame, NULL); + } + xrt_frame_reference(out, p->converted); + for (int i = 0; i < 2; ++i) + xrt_frame_reference(&raw[i], NULL); + os_mutex_unlock(&p->convert_mutex); + return true; +} + +struct xrt_camera * +rift_s_passthrough_create(const struct rift_s_camera_calibration_block *calibration) +{ + struct rift_s_passthrough *p = calloc(1, sizeof(*p)); + if (!p) + return NULL; + if (os_mutex_init(&p->mutex) != 0) { + free(p); + return NULL; + } + if (os_mutex_init(&p->convert_mutex) != 0) { + os_mutex_destroy(&p->mutex); + free(p); + return NULL; + } + p->base.view_width = p->base.view_height = VIEW_SIZE; + // Both front sensors cover this head-aligned square after firmware rectification. + // Use the same pinhole focal for the resampling map and the exported intrinsics. + float focal = VIEW_SIZE / (2.0f * tanf(55.0f * (float)M_PI / 180.0f)); + p->base.focal_length = (struct xrt_vec2){focal, focal}; + p->base.center = (struct xrt_vec2){(VIEW_SIZE - 1) / 2.0f, (VIEW_SIZE - 1) / 2.0f}; + p->base.set_active = set_active; + p->base.get_frame = get_frame; + for (int eye = 0; eye < 2; ++eye) { + const struct rift_s_camera_calibration *c = &calibration->cameras[CAM_IDX_TO_ID[eye]]; + p->base.head_from_camera[eye] = (struct xrt_pose)XRT_POSE_IDENTITY; + p->base.head_from_camera[eye].position = (struct xrt_vec3){ + c->device_from_camera.v[12], c->device_from_camera.v[13], c->device_from_camera.v[14]}; + for (uint32_t y = 0; y < VIEW_SIZE; ++y) + for (uint32_t x = 0; x < VIEW_SIZE; ++x) { + struct xrt_vec2 *uv = &p->map[eye][y * VIEW_SIZE + x]; + float rx = (x - p->base.center.x) / p->base.focal_length.x; + float ry = (y - p->base.center.y) / p->base.focal_length.y; + if (!rift_s_passthrough_project(c, rx, ry, uv)) + *uv = (struct xrt_vec2){-1, -1}; + } + } + return &p->base; +} + +void +rift_s_passthrough_destroy(struct xrt_camera *camera) +{ + if (!camera) + return; + struct rift_s_passthrough *p = (struct rift_s_passthrough *)camera; + set_active(camera, false); + xrt_frame_reference(&p->converted, NULL); + os_mutex_destroy(&p->convert_mutex); + os_mutex_destroy(&p->mutex); + free(p); +} diff --git a/src/xrt/drivers/rift_s/rift_s_passthrough.h b/src/xrt/drivers/rift_s/rift_s_passthrough.h new file mode 100644 index 000000000..a127f79e0 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_passthrough.h @@ -0,0 +1,23 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once +#include "xrt/xrt_camera.h" +#include "rift_s_firmware.h" +#ifdef __cplusplus +extern "C" { +#endif +struct xrt_camera * +rift_s_passthrough_create(const struct rift_s_camera_calibration_block *calibration); +void +rift_s_passthrough_push(struct xrt_camera *camera, struct xrt_frame *left, struct xrt_frame *right); +void +rift_s_passthrough_destroy(struct xrt_camera *camera); +/* Project an upright, head-aligned pinhole ray into a native calibrated camera. */ +bool +rift_s_passthrough_project(const struct rift_s_camera_calibration *calibration, + float x, + float y, + struct xrt_vec2 *pixel); +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/rift_s/rift_s_position_filter.c b/src/xrt/drivers/rift_s/rift_s_position_filter.c new file mode 100644 index 000000000..4cbce421f --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_position_filter.c @@ -0,0 +1,334 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Controller position/velocity filter driven by IMU acceleration and corrected by optical fixes. + * + * Three independent per-axis Kalman filters with state [position, velocity, acceleration bias] + * and the world-frame IMU acceleration as control input. Optical fixes arrive tens of + * milliseconds after their capture time; they are fused at capture time and the retained IMU + * inputs are replayed, as the Windows tracker does for delayed vision. + * @ingroup drv_rift_s + */ +#include "rift_s_position_filter.h" + +#include +#include + +//! Velocity decay time constant while coasting without optical fixes. +#define COAST_VELOCITY_TAU_S 0.5 +#define INITIAL_VELOCITY_VARIANCE (0.5 * 0.5) +#define INITIAL_BIAS_VARIANCE (0.5 * 0.5) +//! Longest single integration step; larger gaps are dropped radio packets. +#define MAX_STEP_S 0.05 + +static struct rift_s_position_filter_input * +input_at(struct rift_s_position_filter *f, unsigned int i) +{ + return &f->history[(f->history_start + i) % RIFT_S_POSITION_FILTER_HISTORY]; +} + +static void +propagate(const struct rift_s_position_filter *f, + struct rift_s_position_filter_state *s, + const struct xrt_vec3 *accel, + timepoint_ns t) +{ + const bool coasting = f->coasting; + double dt = (double)(t - s->t) / U_TIME_1S_IN_NS; + s->t = t; + if (dt <= 0) { + return; + } + if (dt > MAX_STEP_S) { + dt = MAX_STEP_S; + } + if (coasting && f->track_updates < RIFT_S_POSITION_FILTER_MATURE_UPDATES) { + // A young track's velocity comes from a handful of fixes, possibly a wrong one: hold. + for (int axis = 0; axis < 3; axis++) { + s->x[axis][1] = 0; + } + return; + } + const double a[3] = {accel->x, accel->y, accel->z}; + const double d = coasting ? exp(-dt / COAST_VELOCITY_TAU_S) : 1.0; + const double F[3][3] = {{1, dt, -0.5 * dt * dt}, {0, d, -d * dt}, {0, 0, 1}}; + for (int axis = 0; axis < 3; axis++) { + double *x = s->x[axis]; + double u = a[axis] - x[2]; + x[0] += x[1] * dt + 0.5 * u * dt * dt; + x[1] = d * (x[1] + u * dt); + + double FP[3][3] = {{0}}; + double (*P)[3] = s->P[axis]; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + for (int k = 0; k < 3; k++) + FP[i][j] += F[i][k] * P[k][j]; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) { + double v = 0; + for (int k = 0; k < 3; k++) + v += FP[i][k] * F[j][k]; + P[i][j] = v; + } + P[0][0] += f->accel_noise_density * dt * dt * dt / 3; + P[0][1] += f->accel_noise_density * dt * dt / 2; + P[1][0] += f->accel_noise_density * dt * dt / 2; + P[1][1] += f->accel_noise_density * dt; + P[2][2] += f->bias_random_walk * dt; + } +} + +static void +init_state(struct rift_s_position_filter_state *s, timepoint_ns t, const struct xrt_vec3 *z, const struct xrt_vec3 *r) +{ + const double zz[3] = {z->x, z->y, z->z}, rr[3] = {r->x, r->y, r->z}; + s->t = t; + memset(s->P, 0, sizeof(s->P)); + for (int axis = 0; axis < 3; axis++) { + s->x[axis][0] = zz[axis]; + s->x[axis][1] = 0; + s->x[axis][2] = 0; + s->P[axis][0][0] = rr[axis]; + s->P[axis][1][1] = INITIAL_VELOCITY_VARIANCE; + s->P[axis][2][2] = INITIAL_BIAS_VARIANCE; + } +} + +static void +update(struct rift_s_position_filter_state *s, const struct xrt_vec3 *z, const struct xrt_vec3 *r) +{ + const double zz[3] = {z->x, z->y, z->z}, rr[3] = {r->x, r->y, r->z}; + for (int axis = 0; axis < 3; axis++) { + double *x = s->x[axis]; + double (*P)[3] = s->P[axis]; + double S = P[0][0] + rr[axis]; + double K[3] = {P[0][0] / S, P[1][0] / S, P[2][0] / S}; + double innovation = zz[axis] - x[0]; + for (int i = 0; i < 3; i++) + x[i] += K[i] * innovation; + double row0[3] = {P[0][0], P[0][1], P[0][2]}; + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + P[i][j] -= K[i] * row0[j]; + for (int i = 0; i < 3; i++) + for (int j = i + 1; j < 3; j++) + P[i][j] = P[j][i] = 0.5 * (P[i][j] + P[j][i]); + } +} + +void +rift_s_position_filter_reset(struct rift_s_position_filter *f) +{ + memset(f, 0, sizeof(*f)); + f->accel_noise_density = RIFT_S_POSITION_FILTER_ACCEL_NOISE_DENSITY; + f->bias_random_walk = RIFT_S_POSITION_FILTER_BIAS_RANDOM_WALK; +} + +void +rift_s_position_filter_imu(struct rift_s_position_filter *f, timepoint_ns t, const struct xrt_vec3 *accel_world) +{ + if (!f->initialized || t <= f->state.t) { + f->last_accel = *accel_world; + return; + } + if (f->frozen) { + f->state.t = t; + } else { + propagate(f, &f->state, accel_world, t); + } + f->last_accel = *accel_world; + if (f->initialized && t - f->last_fix_ns > 120 * U_TIME_1MS_IN_NS) { + // No measurement can correct acceleration/tilt error while occluded. Bound the + // displacement from the last optical fix, including before coasting was noticed. + double delta[3] = {f->state.x[0][0] - f->last_fix_position.x, f->state.x[1][0] - f->last_fix_position.y, + f->state.x[2][0] - f->last_fix_position.z}; + double distance = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (distance > 0.1) { + const double base[3] = {f->last_fix_position.x, f->last_fix_position.y, f->last_fix_position.z}; + for (int axis = 0; axis < 3; ++axis) { + f->state.x[axis][0] = base[axis] + delta[axis] * 0.1 / distance; + f->state.x[axis][1] = 0; + } + } + if (t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS) + rift_s_position_filter_freeze(f); + } + if (f->history_count == RIFT_S_POSITION_FILTER_HISTORY) { + f->history_start = (f->history_start + 1) % RIFT_S_POSITION_FILTER_HISTORY; + f->history_count--; + } + struct rift_s_position_filter_input *in = input_at(f, f->history_count++); + in->t = t; + in->accel = *accel_world; + in->after = f->state; +} + +// Continue from @p s (at or before the IMU input @p first) through the retained inputs. +static void +replay(struct rift_s_position_filter *f, struct rift_s_position_filter_state s, unsigned int first) +{ + for (unsigned int i = first; i < f->history_count; i++) { + struct rift_s_position_filter_input *in = input_at(f, i); + propagate(f, &s, &in->accel, in->t); + in->after = s; + } + f->state = s; +} + +// Count @p position towards replacing the estimate; true once enough consecutive candidates agree. +static bool +confirm_candidate(struct rift_s_position_filter *f, + timepoint_ns capture, + const struct xrt_vec3 *position, + float reset_distance_m) +{ + bool consistent = false; + if (f->candidate_count > 0 && capture > f->candidate_t && + capture - f->candidate_t <= RIFT_S_POSITION_FILTER_CONFIRM_GAP_NS) { + struct xrt_vec3 d = {position->x - f->candidate_position.x, position->y - f->candidate_position.y, + position->z - f->candidate_position.z}; + consistent = sqrtf(d.x * d.x + d.y * d.y + d.z * d.z) <= reset_distance_m; + } + f->candidate_count = consistent ? f->candidate_count + 1 : 1; + f->candidate_position = *position; + f->candidate_t = capture; + return f->candidate_count >= RIFT_S_POSITION_FILTER_CONFIRM_FIXES; +} + +enum rift_s_position_fix_result +rift_s_position_filter_fix(struct rift_s_position_filter *f, + timepoint_ns capture, + const struct xrt_vec3 *position, + const struct xrt_vec3 *variance, + float reset_distance_m) +{ + // First retained input after the capture time. + unsigned int next = 0; + while (next < f->history_count && input_at(f, next)->t <= capture) { + next++; + } + // A fix needs the state at or before its capture time; the oldest retained input only has + // the state after it. + bool reset = !f->initialized || f->frozen; + bool stale = f->history_count != 0 ? next == 0 : capture < f->state.t; + if (f->initialized && stale) { + f->stale++; + return RIFT_S_POSITION_FIX_STALE; + } + + struct rift_s_position_filter_state s; + if (!reset) { + s = next > 0 ? input_at(f, next - 1)->after : f->state; + const struct xrt_vec3 *accel = next < f->history_count ? &input_at(f, next)->accel : &f->last_accel; + propagate(f, &s, accel, capture); + struct xrt_vec3 innovation = {(float)(position->x - s.x[0][0]), (float)(position->y - s.x[1][0]), + (float)(position->z - s.x[2][0])}; + float distance = + sqrtf(innovation.x * innovation.x + innovation.y * innovation.y + innovation.z * innovation.z); + if (!(distance <= reset_distance_m)) { + f->gated++; + reset = true; + } else { + f->innovation_sq_sum += (double)distance * distance; + f->innovation_count++; + } + } + if (reset && f->initialized && !confirm_candidate(f, capture, position, reset_distance_m)) { + f->rejected++; + return RIFT_S_POSITION_FIX_PENDING; + } + f->candidate_count = 0; + if (reset) { + init_state(&s, capture, position, variance); + f->resets++; + f->track_updates = 1; + } else { + update(&s, position, variance); + f->updates++; + f->track_updates++; + } + f->initialized = true; + f->last_fix_ns = capture; + f->last_fix_position = *position; + f->frozen = false; + f->coasting = false; + // Inputs before the capture keep their states; later ones are replayed from the fused state. + replay(f, s, next); + return reset ? RIFT_S_POSITION_FIX_RESET : RIFT_S_POSITION_FIX_UPDATED; +} + +void +rift_s_position_filter_freeze(struct rift_s_position_filter *f) +{ + if (!f->initialized || f->frozen) { + return; + } + f->frozen = true; + for (int axis = 0; axis < 3; axis++) { + f->state.x[axis][1] = 0; + } +} + +void +rift_s_position_filter_predict(const struct rift_s_position_filter *f, + timepoint_ns t, + time_duration_ns max_horizon_ns, + struct xrt_vec3 *out_position, + struct xrt_vec3 *out_velocity) +{ + time_duration_ns ahead = t - f->state.t; + if (ahead < 0 || f->frozen || (f->initialized && t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS)) { + ahead = 0; + } else if (ahead > max_horizon_ns) { + ahead = max_horizon_ns; + } + double dt = (double)ahead / U_TIME_1S_IN_NS; + const struct rift_s_position_filter_state *s = &f->state; + out_velocity->x = (float)s->x[0][1]; + out_velocity->y = (float)s->x[1][1]; + out_velocity->z = (float)s->x[2][1]; + out_position->x = (float)(s->x[0][0] + s->x[0][1] * dt); + out_position->y = (float)(s->x[1][0] + s->x[1][1] * dt); + out_position->z = (float)(s->x[2][0] + s->x[2][1] * dt); + if (f->initialized && t - f->last_fix_ns > 120 * U_TIME_1MS_IN_NS) { + // Queries can run beyond the last IMU sample, including during a radio gap. + double delta[3] = {out_position->x - f->last_fix_position.x, out_position->y - f->last_fix_position.y, + out_position->z - f->last_fix_position.z}; + double distance = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (distance > 0.1) { + out_position->x = (float)(f->last_fix_position.x + delta[0] * 0.1 / distance); + out_position->y = (float)(f->last_fix_position.y + delta[1] * 0.1 / distance); + out_position->z = (float)(f->last_fix_position.z + delta[2] * 0.1 / distance); + *out_velocity = (struct xrt_vec3){0}; + } + if (t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS) { + *out_velocity = (struct xrt_vec3){0}; + } + } +} + +void +rift_s_tracking_loss_durations(uint32_t vision_updates, + uint32_t reliable_updates, + time_duration_ns *out_position, + time_duration_ns *out_velocity) +{ + double position_s = 0, velocity_s = 0; + if (reliable_updates > 20) { + position_s = 3.0; + velocity_s = 5.0; + } else if (vision_updates > 10) { + position_s = 2.0; + velocity_s = 4.0; + } else if (vision_updates > 2) { + position_s = 0.5; + velocity_s = 0.5; + } else if (vision_updates >= 1) { + position_s = 0.25; + velocity_s = 0.25; + } + *out_position = (time_duration_ns)(position_s * U_TIME_1S_IN_NS); + *out_velocity = (time_duration_ns)(velocity_s * U_TIME_1S_IN_NS); +} diff --git a/src/xrt/drivers/rift_s/rift_s_position_filter.h b/src/xrt/drivers/rift_s/rift_s_position_filter.h new file mode 100644 index 000000000..eb956d641 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_position_filter.h @@ -0,0 +1,156 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Controller position/velocity filter driven by IMU acceleration and corrected by optical fixes. + * @ingroup drv_rift_s + */ +#pragma once + +#include "xrt/xrt_defines.h" +#include "util/u_time.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/*! + * Defaults chosen on the synthetic cases in tests_rift_s_position_filter (noise at rest, delayed + * fixes during a hand swing, slowly varying acceleration error); no Meta values are used. + * Continuous white acceleration noise density, (m/s^2)^2 per Hz, and bias random walk, + * (m/s^2)^2 per second. + */ +#define RIFT_S_POSITION_FILTER_ACCEL_NOISE_DENSITY (0.1 * 0.1) +#define RIFT_S_POSITION_FILTER_BIAS_RANDOM_WALK (0.004) + +/*! + * Mutually consistent fixes needed before a fix that disagrees with the estimate (a gated + * innovation, or any fix while the position is held after loss) replaces it. A single wrong + * constellation fit, e.g. a partial ring seen behind the back, must not move the hand. + */ +#define RIFT_S_POSITION_FILTER_CONFIRM_FIXES 3 +//! Longest gap between consecutive candidate fixes of one confirmation. +#define RIFT_S_POSITION_FILTER_CONFIRM_GAP_NS (250 * U_TIME_1MS_IN_NS) +/*! + * Fixes before a track's velocity is trusted for coasting; younger tracks hold their position + * when fixes stop. Matches the "more than 10 updates" step of the loss ladder. + */ +#define RIFT_S_POSITION_FILTER_MATURE_UPDATES 11 + +/*! + * Longest optical gap the IMU bridges before the position is frozen and reported lost: the + * 0.5 s step of the loss ladder. Fixes arrive every 33 ms, so in view only real occlusion + * reaches it; displacement while bridging stays bounded to 10 cm from the last fix. + */ +#define RIFT_S_POSITION_FILTER_BRIDGE_NS (500 * U_TIME_1MS_IN_NS) + +//! IMU inputs retained for replaying after a delayed optical fix: 1 s at 500 Hz. +#define RIFT_S_POSITION_FILTER_HISTORY 512 + +/*! + * Per axis state: world position, velocity and a world-frame acceleration bias, which absorbs + * accelerometer bias and residual tilt error while optical fixes arrive. + */ +struct rift_s_position_filter_state +{ + timepoint_ns t; + double x[3][3]; //!< [axis][p, v, b] + double P[3][3][3]; //!< [axis] 3x3 covariance +}; + +struct rift_s_position_filter_input +{ + timepoint_ns t; + struct xrt_vec3 accel; //!< World-frame acceleration, gravity removed. + struct rift_s_position_filter_state after; //!< State after integrating up to t. +}; + +struct rift_s_position_filter +{ + bool initialized; + //! Velocity damping and no bias update: no optical fix for longer than the coast threshold. + bool coasting; + //! Position frozen and velocity zero: tracking was lost; waits for an optical reset. + bool frozen; + //! Process noise; set by reset(), may be overridden afterwards. + double accel_noise_density, bias_random_walk; + struct rift_s_position_filter_state state; + struct xrt_vec3 last_accel; + struct xrt_vec3 last_fix_position; + timepoint_ns last_fix_ns; + + struct rift_s_position_filter_input history[RIFT_S_POSITION_FILTER_HISTORY]; + unsigned int history_start, history_count; + + //! Fixes fused since the last (re)initialization. + uint32_t track_updates; + //! Candidate replacement position awaiting confirmation. + uint32_t candidate_count; + struct xrt_vec3 candidate_position; + timepoint_ns candidate_t; + + uint64_t resets, updates, stale, gated, rejected; + //! Squared innovation norm of fused fixes, m^2, since the caller last cleared it. + double innovation_sq_sum; + uint32_t innovation_count; +}; + +//! Result of applying one optical position fix. +enum rift_s_position_fix_result +{ + RIFT_S_POSITION_FIX_RESET, //!< (Re)initialized from the fix. + RIFT_S_POSITION_FIX_UPDATED, //!< Fused at capture time and replayed. + RIFT_S_POSITION_FIX_STALE, //!< Older than the retained IMU history. + RIFT_S_POSITION_FIX_PENDING, //!< Disagrees with the estimate; held until confirmed. +}; + +void +rift_s_position_filter_reset(struct rift_s_position_filter *f); + +//! Integrate one IMU sample, @p accel_world in m/s^2 with gravity removed, at host time @p t. +void +rift_s_position_filter_imu(struct rift_s_position_filter *f, timepoint_ns t, const struct xrt_vec3 *accel_world); + +/*! + * Fuse an optical position at its capture time. @p variance is the per-axis measurement variance + * in m^2. The first fix initializes the filter. An innovation beyond @p reset_distance_m, or any fix + * while frozen, is a candidate: it re-initializes position and velocity (the Windows "6DoF reset") + * only once RIFT_S_POSITION_FILTER_CONFIRM_FIXES consecutive candidates agree within + * @p reset_distance_m of each other; until then the estimate is kept and PENDING returned. + */ +enum rift_s_position_fix_result +rift_s_position_filter_fix(struct rift_s_position_filter *f, + timepoint_ns capture, + const struct xrt_vec3 *position, + const struct xrt_vec3 *variance, + float reset_distance_m); + +//! Stop integrating: hold the current position with zero velocity until the next reset. +void +rift_s_position_filter_freeze(struct rift_s_position_filter *f); + +/*! + * Position and velocity at @p t. Times after the last IMU sample extrapolate with constant + * velocity, at most @p max_horizon_ns; earlier times return the current state. + */ +void +rift_s_position_filter_predict(const struct rift_s_position_filter *f, + timepoint_ns t, + time_duration_ns max_horizon_ns, + struct xrt_vec3 *out_position, + struct xrt_vec3 *out_velocity); + +/*! + * Windows controller loss ladder (its default configuration): how long position and velocity + * stay tracked after the last optical update, by the number of successful and reliable optical + * updates in the current track. + */ +void +rift_s_tracking_loss_durations(uint32_t vision_updates, + uint32_t reliable_updates, + time_duration_ns *out_position, + time_duration_ns *out_velocity); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/rift_s/rift_s_protocol.c b/src/xrt/drivers/rift_s/rift_s_protocol.c index 079559ea0..c33d42216 100644 --- a/src/xrt/drivers/rift_s/rift_s_protocol.c +++ b/src/xrt/drivers/rift_s/rift_s_protocol.c @@ -25,6 +25,7 @@ #include #include #include +#include #include "os/os_hid.h" #include "os/os_time.h" @@ -34,6 +35,116 @@ #include "rift_s.h" #include "rift_s_protocol.h" +static bool +controller_register_response_valid(const uint8_t *response, size_t size) +{ + return response != NULL && size >= 21 && response[0] == 0 && response[1] == 0 && response[2] == 0 && + response[3] == 0 && response[4] >= 16 && response[4] <= 32 && size >= 5u + response[4]; +} + +bool +rift_s_decode_controller_config(const uint8_t *response, size_t size, rift_s_controller_config *out) +{ + if (!controller_register_response_valid(response, size)) + return false; + const uint8_t *data = response + 5; + rift_s_controller_config config = {0}; + config.accel_limit = (uint16_t)data[0] | (uint16_t)data[1] << 8; + config.gyro_limit = (uint16_t)data[2] | (uint16_t)data[3] << 8; + config.accel_hz = (uint16_t)data[4] | (uint16_t)data[5] << 8; + config.gyro_hz = (uint16_t)data[6] | (uint16_t)data[7] << 8; + uint32_t accel_bits = (uint32_t)data[8] | (uint32_t)data[9] << 8 | (uint32_t)data[10] << 16 | + (uint32_t)data[11] << 24; + uint32_t gyro_bits = (uint32_t)data[12] | (uint32_t)data[13] << 8 | (uint32_t)data[14] << 16 | + (uint32_t)data[15] << 24; + memcpy(&config.accel_scale, &accel_bits, sizeof(accel_bits)); + memcpy(&config.gyro_scale, &gyro_bits, sizeof(gyro_bits)); + if (!isfinite(config.accel_scale) || config.accel_scale <= 0 || !isfinite(config.gyro_scale) || + config.gyro_scale <= 0) + return false; + *out = config; + return true; +} + +bool +rift_s_decode_controller_imu_descriptor(const uint8_t *response, + size_t size, + char out[RIFT_S_CONTROLLER_IMU_DESCRIPTOR_SIZE + 1]) +{ + if (!controller_register_response_valid(response, size) || response[4] != 16) + return false; + const uint8_t *data = response + 5; + size_t len = 0; + while (len < RIFT_S_CONTROLLER_IMU_DESCRIPTOR_SIZE && data[len] != 0) { + if (data[len] < 0x20 || data[len] > 0x7e) + return false; + len++; + } + if (len == 0) + return false; + memcpy(out, data, len); + out[len] = '\0'; + return true; +} + +static uint32_t +read_le32(const uint8_t *p) +{ + return (uint32_t)p[0] | (uint32_t)p[1] << 8 | (uint32_t)p[2] << 16 | (uint32_t)p[3] << 24; +} + +static void +write_le32(uint8_t *p, uint32_t v) +{ + p[0] = v & 0xff; + p[1] = (v >> 8) & 0xff; + p[2] = (v >> 16) & 0xff; + p[3] = (v >> 24) & 0xff; +} + +size_t +rift_s_encode_controller_irled_read(uint8_t out[4]) +{ + // The register read passes a zero payload length, as the Windows driver does. + out[0] = RIFT_S_CONTROLLER_REG_IRLED; + out[1] = 0; + out[2] = RIFT_S_RADIO_TIMEOUT_MS & 0xff; + out[3] = RIFT_S_RADIO_TIMEOUT_MS >> 8; + return 4; +} + +size_t +rift_s_encode_controller_irled_write(const rift_s_controller_irled_config *cfg, uint8_t out[12]) +{ + out[0] = RIFT_S_CONTROLLER_REG_IRLED; + out[1] = 8; + out[2] = RIFT_S_RADIO_TIMEOUT_MS & 0xff; + out[3] = RIFT_S_RADIO_TIMEOUT_MS >> 8; + write_le32(out + 4, cfg->period_us); + write_le32(out + 8, cfg->ontime_us); + return 12; +} + +bool +rift_s_decode_controller_irled_config(const uint8_t *response, size_t size, rift_s_controller_irled_config *out) +{ + // Status 0, then a length of at least 8 and that many bytes. + if (response == NULL || size < 13 || read_le32(response) != 0 || response[4] < 8 || size < 5u + response[4]) + return false; + out->period_us = read_le32(response + 5); + out->ontime_us = read_le32(response + 9); + return true; +} + +bool +rift_s_controller_write_succeeded(const uint8_t *response, size_t size, uint32_t *status_out) +{ + if (response == NULL || size < 4) + return false; + *status_out = read_le32(response); + return *status_out == 0; +} + /* FIXME: The code in this file is not portable to big-endian as-is - it needs endian swaps */ bool rift_s_parse_hmd_report(rift_s_hmd_report_t *report, const unsigned char *buf, int size) diff --git a/src/xrt/drivers/rift_s/rift_s_protocol.h b/src/xrt/drivers/rift_s/rift_s_protocol.h index 4fdda6484..82c9ddeea 100644 --- a/src/xrt/drivers/rift_s/rift_s_protocol.h +++ b/src/xrt/drivers/rift_s/rift_s_protocol.h @@ -45,6 +45,55 @@ #define RIFT_S_FINGER_STICK_WEAK 0x40 #define RIFT_S_FINGER_TRIGGER_WEAK 0x80 +#define RIFT_S_CONTROLLER_IMU_DESCRIPTOR_SIZE 16 + +typedef struct +{ + uint16_t accel_limit; + uint16_t gyro_limit; + uint16_t accel_hz; + uint16_t gyro_hz; + float accel_scale; + float gyro_scale; +} rift_s_controller_config; + +/* Radio callback bytes: LE32 status, u8 payload length, then register data. + * 0x32 is four LE16 limits/rates and two LE float scales; 0x31 is a 16-byte ASCII name. */ +bool +rift_s_decode_controller_config(const uint8_t *response, size_t size, rift_s_controller_config *out); +bool +rift_s_decode_controller_imu_descriptor(const uint8_t *response, + size_t size, + char out[RIFT_S_CONTROLLER_IMU_DESCRIPTOR_SIZE + 1]); + +/* Controller IR LED timing, register 0x28: LE32 period_us, LE32 ontime_us. + * The Windows driver reads it, keeps ontime_us (19 if the read fails), sets + * period_us = 1000000 / controller exposure rate (25 or 30 Hz) and writes it back. + * Windows logs "updated ontime_us/period_us to 19/40000" for both hands. */ +#define RIFT_S_CONTROLLER_REG_IRLED 0x28 +#define RIFT_S_CONTROLLER_IRLED_DEFAULT_ONTIME_US 19 +#define RIFT_S_RADIO_REPORT_READ 0x12 +#define RIFT_S_RADIO_REPORT_WRITE 0x13 +#define RIFT_S_RADIO_TIMEOUT_MS 1000 + +typedef struct +{ + uint32_t period_us; + uint32_t ontime_us; +} rift_s_controller_irled_config; + +/* Request bytes after the device id for report 0x12 (read) or 0x13 (write), as the Windows + * driver builds them: register, payload length, LE16 timeout ms, payload. */ +size_t +rift_s_encode_controller_irled_read(uint8_t out[4]); +size_t +rift_s_encode_controller_irled_write(const rift_s_controller_irled_config *cfg, uint8_t out[12]); +bool +rift_s_decode_controller_irled_config(const uint8_t *response, size_t size, rift_s_controller_irled_config *out); +//! A write reply only carries the LE32 status, 0 on success. +bool +rift_s_controller_write_succeeded(const uint8_t *response, size_t size, uint32_t *status_out); + typedef enum { RIFT_S_CTRL_MASK08 = 0x08, /* Unknown. Vals seen 0x28, 0x0a, 0x32, 0x46, 0x00... */ diff --git a/src/xrt/drivers/rift_s/rift_s_radio.c b/src/xrt/drivers/rift_s/rift_s_radio.c index 9b31cf889..457496129 100644 --- a/src/xrt/drivers/rift_s/rift_s_radio.c +++ b/src/xrt/drivers/rift_s/rift_s_radio.c @@ -33,7 +33,7 @@ struct rift_s_radio_command rift_s_radio_command *prev; rift_s_radio_command *next; - /* Request packet data */ + /* Request packet data; read_command.cmd is the HID report id */ rift_s_hmd_radio_command_t read_command; /* Completion callback */ @@ -64,7 +64,6 @@ rift_s_radio_update(rift_s_radio_state *state, struct os_hid_device *hid) rift_s_radio_command *cmd = state->pending_commands; rift_s_hmd_radio_command_t *pkt = &cmd->read_command; - pkt->cmd = 0x12; os_hid_set_feature(hid, (uint8_t *)pkt, sizeof(*pkt)); // rift_s_hexdump_buffer ("ControllerFWSend", (unsigned char *)(pkt), sizeof(*pkt)); state->command_result_pending = true; @@ -150,11 +149,24 @@ rift_s_radio_queue_command(rift_s_radio_state *state, const int cmd_bytes_len, rift_s_radio_completion_fn cb, void *cb_data) +{ + rift_s_radio_queue_report(state, RIFT_S_RADIO_REPORT_READ, device_id, cmd_bytes, cmd_bytes_len, cb, cb_data); +} + +void +rift_s_radio_queue_report(rift_s_radio_state *state, + uint8_t report_id, + const uint64_t device_id, + const uint8_t *cmd_bytes, + const int cmd_bytes_len, + rift_s_radio_completion_fn cb, + void *cb_data) { rift_s_radio_command *cmd = U_TYPED_CALLOC(rift_s_radio_command); assert(cmd_bytes_len <= (int)sizeof(cmd->read_command.cmd_bytes)); + cmd->read_command.cmd = report_id; cmd->read_command.device_id = device_id; memcpy(cmd->read_command.cmd_bytes, cmd_bytes, cmd_bytes_len); cmd->cb = cb; diff --git a/src/xrt/drivers/rift_s/rift_s_radio.h b/src/xrt/drivers/rift_s/rift_s_radio.h index a16cebfc1..1dae9359b 100644 --- a/src/xrt/drivers/rift_s/rift_s_radio.h +++ b/src/xrt/drivers/rift_s/rift_s_radio.h @@ -60,6 +60,15 @@ rift_s_radio_queue_command(rift_s_radio_state *state, const int cmd_bytes_len, rift_s_radio_completion_fn cb, void *cb_data); +//! Like rift_s_radio_queue_command, but sent with an explicit report id (0x13 writes a register). +void +rift_s_radio_queue_report(rift_s_radio_state *state, + uint8_t report_id, + const uint64_t device_id, + const uint8_t *cmd_bytes, + const int cmd_bytes_len, + rift_s_radio_completion_fn cb, + void *cb_data); void rift_s_radio_get_json_block(rift_s_radio_state *state, const uint64_t device_id, diff --git a/src/xrt/drivers/rift_s/rift_s_slam_guard.c b/src/xrt/drivers/rift_s/rift_s_slam_guard.c new file mode 100644 index 000000000..65009b9c7 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_slam_guard.c @@ -0,0 +1,463 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Rejects diverged SLAM poses and supplies the fallback HMD pose. + * + * Validate source samples, separately from historical and predicted queries. A lost pose holds + * position and follows IMU attitude. Ordinary loss retains the map alignment; a backend reset + * invalidates it. A saved world requires visual relocalization, never a held-head realignment. + * @ingroup drv_rift_s + */ +#include "rift_s_slam_guard.h" + +#include "math/m_api.h" +#include "math/m_vec3.h" + +#include +#include + +static const struct xrt_quat identity_quat = {0, 0, 0, 1}; + +static bool +pose_is_finite(const struct xrt_pose *p) +{ + return isfinite(p->position.x) && isfinite(p->position.y) && isfinite(p->position.z) && + isfinite(p->orientation.x) && isfinite(p->orientation.y) && isfinite(p->orientation.z) && + isfinite(p->orientation.w); +} + +bool +rift_s_slam_guard_pose_usable(const struct xrt_pose *p, const struct xrt_quat *fusion) +{ + if (!pose_is_finite(p)) return false; + float norm = p->orientation.x * p->orientation.x + p->orientation.y * p->orientation.y + + p->orientation.z * p->orientation.z + p->orientation.w * p->orientation.w; + if (!(norm > 0.9f && norm < 1.1f)) return false; + // Compare gravity in the body frame: world yaw is arbitrary, physical head tilt is not. + struct xrt_vec3 up = {0, 1, 0}, slam_up, imu_up; + struct xrt_quat inverse; + math_quat_invert(&p->orientation, &inverse); + math_quat_rotate_vec3(&inverse, &up, &slam_up); + math_quat_invert(fusion, &inverse); + math_quat_rotate_vec3(&inverse, &up, &imu_up); + return m_vec3_dot(slam_up, imu_up) >= 0.70710678f; +} + +void +rift_s_slam_guard_set_world(struct rift_s_slam_guard *g, const struct xrt_pose *world_from_slam) +{ + g->boundary_confirmed = true; + g->world_transition = g->world_valid && g->have_good && g->state == RIFT_S_SLAM_GUARD_TRACKING; + if (g->world_transition) { + g->transition_from = g->align; + g->transition_to = *world_from_slam; + g->transition_start_ns = g->good_ns; + } else { + g->align = *world_from_slam; + } + g->world_valid = true; +} + +static void +apply_align(const struct xrt_pose *align, const struct xrt_pose *raw, struct xrt_pose *out) +{ + struct xrt_pose result; + math_quat_rotate(&align->orientation, &raw->orientation, &result.orientation); + math_quat_normalize(&result.orientation); + math_quat_rotate_vec3(&align->orientation, &raw->position, &result.position); + result.position = m_vec3_add(result.position, align->position); + *out = result; +} + +//! Checks a step from @p from to @p to against the jump and speed limits. +static enum rift_s_slam_guard_reason +check_step(const struct xrt_vec3 *from, int64_t from_ns, const struct xrt_vec3 *to, int64_t to_ns, float *out_step) +{ + float step = m_vec3_len(m_vec3_sub(*to, *from)); + double dt_s = fabs((double)(to_ns - from_ns)) / 1e9; + *out_step = step; + if (step > RIFT_S_SLAM_GUARD_MAX_STEP_M) { + return RIFT_S_SLAM_GUARD_JUMP; + } + if (step > RIFT_S_SLAM_GUARD_STEP_TOLERANCE_M + RIFT_S_SLAM_GUARD_MAX_SPEED_MPS * dt_s) { + return RIFT_S_SLAM_GUARD_SPEED; + } + return RIFT_S_SLAM_GUARD_OK; +} + +//! Height outside the room (below the nominal floor or far above it). +static bool +outside_room(const struct xrt_vec3 *p, const struct xrt_vec3 *no_pose_position) +{ + float floor_y = no_pose_position->y - RIFT_S_SLAM_GUARD_EYE_HEIGHT_M; + return p->y < floor_y - RIFT_S_SLAM_GUARD_BELOW_FLOOR_M || p->y > floor_y + RIFT_S_SLAM_GUARD_ABOVE_FLOOR_M; +} + +bool +rift_s_slam_guard_query(const struct rift_s_slam_guard *g, const struct xrt_pose *raw, int64_t query_ns, + const struct xrt_quat *fusion, const struct xrt_vec3 *no_pose_position, + struct xrt_pose *out) +{ + if (g->state != RIFT_S_SLAM_GUARD_TRACKING || !g->world_valid || + (g->require_relocalization && !g->boundary_confirmed) || + !rift_s_slam_guard_pose_usable(raw, fusion)) return false; + struct xrt_pose aligned; + apply_align(&g->align, raw, &aligned); + float step; + if (m_vec3_len(aligned.position) > RIFT_S_SLAM_GUARD_MAX_DISTANCE_M || + outside_room(&aligned.position, no_pose_position) || + check_step(&g->good.position, g->good_ns, &aligned.position, query_ns, &step) != RIFT_S_SLAM_GUARD_OK) + return false; + *out = aligned; + return true; +} + +static bool +reset_due(const struct rift_s_slam_guard *g, int64_t ts_ns) +{ + if (g->reset_pending) { + return false; + } + return !g->have_reset || (ts_ns - g->last_reset_ns) >= RIFT_S_SLAM_GUARD_RESET_INTERVAL_NS; +} + +void +rift_s_slam_guard_init(struct rift_s_slam_guard *g) +{ + memset(g, 0, sizeof(*g)); + g->state = RIFT_S_SLAM_GUARD_ACQUIRING; + g->good.orientation = identity_quat; + g->align.orientation = identity_quat; + g->fallback_yaw = identity_quat; + g->stable_last.orientation = identity_quat; +} + +struct xrt_vec3 +rift_s_slam_guard_no_pose_position(float origin_offset_y) +{ + struct xrt_vec3 p = {0, RIFT_S_SLAM_GUARD_EYE_HEIGHT_M - origin_offset_y, 0}; + return p; +} + +void +rift_s_slam_guard_yaw_of(const struct xrt_quat *q, struct xrt_quat *out_yaw) +{ + struct xrt_quat yaw = {0, q->y, 0, q->w}; + float len = sqrtf(yaw.y * yaw.y + yaw.w * yaw.w); + if (!(len > 1e-6f)) { + *out_yaw = identity_quat; + return; + } + yaw.y /= len; + yaw.w /= len; + *out_yaw = yaw; +} + +void +rift_s_slam_guard_fallback_pose(const struct rift_s_slam_guard *g, + const struct xrt_quat *fusion_orientation, + const struct xrt_vec3 *no_pose_position, + struct xrt_pose *out_pose) +{ + math_quat_rotate(&g->fallback_yaw, fusion_orientation, &out_pose->orientation); + float norm = out_pose->orientation.x * out_pose->orientation.x + out_pose->orientation.y * out_pose->orientation.y + + out_pose->orientation.z * out_pose->orientation.z + out_pose->orientation.w * out_pose->orientation.w; + if (isfinite(norm) && norm > 0.5f) math_quat_normalize(&out_pose->orientation); + else out_pose->orientation = g->have_good ? g->good.orientation : identity_quat; + out_pose->position = g->have_good ? g->good.position : *no_pose_position; +} + +void +rift_s_slam_guard_backend_started(struct rift_s_slam_guard *g, int64_t ts_ns) +{ + g->reset_pending = false; + g->invalid_since_ns = 0; + g->have_reset = true; + g->last_reset_ns = ts_ns; + g->stable_samples = 0; +} + +bool +rift_s_slam_guard_take_reset(struct rift_s_slam_guard *g, int64_t ts_ns) +{ + if (!g->reset_pending) { + return false; + } + g->reset_pending = false; + g->have_reset = true; + g->last_reset_ns = ts_ns; + g->resets++; + g->world_valid = false; + g->boundary_confirmed = false; + g->world_transition = false; + // Samples queued before the reset belong to the diverged run. + g->stable_samples = 0; + return true; +} + +static void +enter_diverged(struct rift_s_slam_guard *g, + int64_t ts_ns, + const struct xrt_quat *fusion_orientation, + bool diverged, + float distance, + float step) +{ + g->state = RIFT_S_SLAM_GUARD_DIVERGED; + g->stable_samples = 0; + + // Continue the 3DoF orientation in the heading SLAM last showed. + struct xrt_quat inv_fusion; + struct xrt_quat delta; + math_quat_invert(fusion_orientation, &inv_fusion); + math_quat_rotate(&g->good.orientation, &inv_fusion, &delta); + rift_s_slam_guard_yaw_of(&delta, &g->fallback_yaw); + + if (diverged) { + g->divergences++; + g->last_reject_distance_m = distance; + g->last_reject_step_m = step; + if (reset_due(g, ts_ns)) { + g->reset_pending = true; + } + } +} + +void +rift_s_slam_guard_suspend(struct rift_s_slam_guard *g, const struct xrt_quat *fusion_orientation) +{ + if (g->state == RIFT_S_SLAM_GUARD_TRACKING) { + enter_diverged(g, 0, fusion_orientation, false, 0.0f, 0.0f); + } +} + +//! Feeds a raw sample into the stability run used while acquiring or diverged. +static enum rift_s_slam_guard_reason +settle(struct rift_s_slam_guard *g, bool slam_valid, const struct xrt_pose *raw, int64_t ts_ns) +{ + if (!slam_valid) { + g->stable_samples = 0; + if (g->have_good && ts_ns - g->good_ns > RIFT_S_SLAM_GUARD_RESET_INTERVAL_NS && reset_due(g, ts_ns)) + g->reset_pending = true; + return RIFT_S_SLAM_GUARD_NO_POSE; + } + + enum rift_s_slam_guard_reason reason = RIFT_S_SLAM_GUARD_OK; + float step = 0.0f; + if (!pose_is_finite(raw)) { + reason = RIFT_S_SLAM_GUARD_NOT_FINITE; + } else if (m_vec3_len(raw->position) > RIFT_S_SLAM_GUARD_MAX_DISTANCE_M) { + reason = RIFT_S_SLAM_GUARD_TOO_FAR; + } else if (g->stable_samples > 0) { + reason = check_step(&g->stable_last.position, g->stable_last_ns, &raw->position, ts_ns, &step); + } + + bool far_off = reason == RIFT_S_SLAM_GUARD_NOT_FINITE || reason == RIFT_S_SLAM_GUARD_TOO_FAR; + bool still_diverging = far_off || (reason != RIFT_S_SLAM_GUARD_OK && g->state == RIFT_S_SLAM_GUARD_DIVERGED); + if (still_diverging && reset_due(g, ts_ns)) { + // Retry the re-initialisation at the rate limit. + g->reset_pending = true; + } + if (far_off) { + g->stable_samples = 0; + return reason; + } + + if (reason != RIFT_S_SLAM_GUARD_OK || g->stable_samples == 0) { + // A jump starts a new run here: a re-initialised SLAM snaps to its new origin. + g->stable_samples = 0; + g->stable_start_ns = ts_ns; + } + g->stable_samples++; + g->stable_last = *raw; + g->stable_last_ns = ts_ns; + + if (reason != RIFT_S_SLAM_GUARD_OK) { + return reason; + } + if (g->stable_samples < RIFT_S_SLAM_GUARD_STABLE_SAMPLES || + (ts_ns - g->stable_start_ns) < RIFT_S_SLAM_GUARD_STABLE_NS) { + return RIFT_S_SLAM_GUARD_SETTLING; + } + return RIFT_S_SLAM_GUARD_OK; +} + +bool +rift_s_slam_guard_update(struct rift_s_slam_guard *g, + bool slam_valid, + const struct xrt_pose *slam_pose, + int64_t ts_ns, + const struct xrt_quat *fusion_orientation, + const struct xrt_vec3 *no_pose_position, + struct xrt_pose *out_pose, + enum rift_s_slam_guard_reason *out_reason) +{ + if (g->require_relocalization && !g->boundary_confirmed && g->world_valid) { + // A failed/cancelled setup can restore saved bounds while the fresh frame is still + // active. Revoke that frame before either source or predicted poses are published. + g->world_valid = false; + g->world_transition = false; + if (g->state == RIFT_S_SLAM_GUARD_TRACKING) + enter_diverged(g, ts_ns, fusion_orientation, false, 0, 0); + } + bool usable = slam_valid && rift_s_slam_guard_pose_usable(slam_pose, fusion_orientation); + if (usable) { + g->invalid_since_ns = 0; + } else { + if (g->invalid_since_ns == 0) + g->invalid_since_ns = ts_ns; + if (ts_ns - g->invalid_since_ns >= RIFT_S_SLAM_GUARD_RESET_INTERVAL_NS && reset_due(g, ts_ns)) + g->reset_pending = true; + } + if (slam_valid && pose_is_finite(slam_pose) && !usable) { + *out_reason = RIFT_S_SLAM_GUARD_TILT; + if (g->state == RIFT_S_SLAM_GUARD_TRACKING) + enter_diverged(g, ts_ns, fusion_orientation, true, 0, 0); + g->stable_samples = 0; + rift_s_slam_guard_fallback_pose(g, fusion_orientation, no_pose_position, out_pose); + return false; + } + if (g->state == RIFT_S_SLAM_GUARD_TRACKING) { + // Duplicate/out-of-order reads cannot change the source anchor or trigger a reset. + if (slam_valid && ts_ns <= g->good_ns) { + *out_pose = g->good; + *out_reason = RIFT_S_SLAM_GUARD_OK; + return true; + } + if (!slam_valid || !pose_is_finite(slam_pose)) { + *out_reason = slam_valid ? RIFT_S_SLAM_GUARD_NOT_FINITE : RIFT_S_SLAM_GUARD_NO_POSE; + // A lost SLAM pose is not a divergence; a non-finite one is. + enter_diverged(g, ts_ns, fusion_orientation, slam_valid, NAN, NAN); + rift_s_slam_guard_fallback_pose(g, fusion_orientation, no_pose_position, out_pose); + return false; + } + + if (g->world_transition) { + // Move the published frame over two seconds. Reframe history too, so this deliberate + // correction cannot masquerade as a physical head jump in the source guard. + float t = fminf(1.0f, fmaxf(0.0f, (float)(ts_ns - g->transition_start_ns) / 2e9f)); + struct xrt_pose previous = g->align, inverse, raw_good; + math_pose_invert(&previous, &inverse); + math_pose_transform(&inverse, &g->good, &raw_good); + math_pose_interpolate(&g->transition_from, &g->transition_to, t, &g->align); + apply_align(&g->align, &raw_good, &g->good); + g->world_transition = t < 1.0f; + } + struct xrt_pose aligned; + apply_align(&g->align, slam_pose, &aligned); + float distance = m_vec3_len(aligned.position); + float step = 0.0f; + enum rift_s_slam_guard_reason reason = + check_step(&g->good.position, g->good_ns, &aligned.position, ts_ns, &step); + if (distance > RIFT_S_SLAM_GUARD_MAX_DISTANCE_M) { + reason = RIFT_S_SLAM_GUARD_TOO_FAR; + } else if (outside_room(&aligned.position, no_pose_position)) { + reason = RIFT_S_SLAM_GUARD_HEIGHT; + } + + if (reason == RIFT_S_SLAM_GUARD_OK) { + g->good = aligned; + g->good_ns = ts_ns; + *out_pose = aligned; + *out_reason = RIFT_S_SLAM_GUARD_OK; + return true; + } + + *out_reason = reason; + enter_diverged(g, ts_ns, fusion_orientation, true, distance, step); + rift_s_slam_guard_fallback_pose(g, fusion_orientation, no_pose_position, out_pose); + return false; + } + + // Acquiring or diverged: publish the fallback until SLAM has been consistent for a while. + struct xrt_pose fallback; + rift_s_slam_guard_fallback_pose(g, fusion_orientation, no_pose_position, &fallback); + + if (g->stable_samples > 0 && ts_ns <= g->stable_last_ns) { + *out_reason = RIFT_S_SLAM_GUARD_SETTLING; + *out_pose = fallback; + return false; + } + enum rift_s_slam_guard_reason reason = settle(g, slam_valid, slam_pose, ts_ns); + if (reason != RIFT_S_SLAM_GUARD_OK) { + *out_reason = reason; + *out_pose = fallback; + return false; + } + + if (!g->world_valid) { + if (g->require_relocalization && !g->boundary_confirmed) { + // SteamVR's saved standing origin belongs to the persisted map, not this VIO + // run's arbitrary yaw/translation. Only a visual match can join those frames. + *out_reason = RIFT_S_SLAM_GUARD_RELOCALIZING; + *out_pose = fallback; + return false; + } + // An explicitly new room can start a fresh frame before it has saved landmarks. + struct xrt_quat inv_raw, delta; + math_quat_invert(&slam_pose->orientation, &inv_raw); + math_quat_rotate(&fallback.orientation, &inv_raw, &delta); + rift_s_slam_guard_yaw_of(&delta, &g->align.orientation); + struct xrt_vec3 rotated; + math_quat_rotate_vec3(&g->align.orientation, &slam_pose->position, &rotated); + g->align.position = m_vec3_sub(fallback.position, rotated); + g->world_valid = true; + } + if (g->state == RIFT_S_SLAM_GUARD_DIVERGED) { + g->recoveries++; + } + + g->state = RIFT_S_SLAM_GUARD_TRACKING; + g->stable_samples = 0; + apply_align(&g->align, slam_pose, &g->good); + g->good_ns = ts_ns; + g->have_good = true; + *out_pose = g->good; + *out_reason = RIFT_S_SLAM_GUARD_OK; + return true; +} + +const char * +rift_s_slam_guard_reason_str(enum rift_s_slam_guard_reason reason) +{ + switch (reason) { + case RIFT_S_SLAM_GUARD_OK: return "ok"; + case RIFT_S_SLAM_GUARD_NO_POSE: return "no_pose"; + case RIFT_S_SLAM_GUARD_NOT_FINITE: return "not_finite"; + case RIFT_S_SLAM_GUARD_TOO_FAR: return "too_far"; + case RIFT_S_SLAM_GUARD_HEIGHT: return "height"; + case RIFT_S_SLAM_GUARD_JUMP: return "jump"; + case RIFT_S_SLAM_GUARD_SPEED: return "speed"; + case RIFT_S_SLAM_GUARD_SETTLING: return "settling"; + case RIFT_S_SLAM_GUARD_TILT: return "gravity_mismatch"; + case RIFT_S_SLAM_GUARD_RELOCALIZING: return "relocalizing"; + } + return "unknown"; +} + +enum rift_s_slam_feature_event +rift_s_slam_feature_gate_update(struct rift_s_slam_feature_gate *gate, bool known, int count, int64_t ts_ns) +{ + if (!known || ts_ns <= gate->last_ts_ns) { + return RIFT_S_SLAM_FEATURES_NO_CHANGE; + } + gate->last_ts_ns = ts_ns; + gate->last_count = count; + + bool low = count < RIFT_S_SLAM_FEATURES_MIN; + if (low != gate->lost) { + if (!gate->crossing) { + gate->crossing = true; + gate->crossing_since_ns = ts_ns; + } + if (ts_ns - gate->crossing_since_ns >= RIFT_S_SLAM_FEATURES_DWELL_NS) { + gate->lost = low; + gate->crossing = false; + return low ? RIFT_S_SLAM_FEATURES_LOST : RIFT_S_SLAM_FEATURES_RETURNED; + } + } else { + gate->crossing = false; + } + return RIFT_S_SLAM_FEATURES_NO_CHANGE; +} diff --git a/src/xrt/drivers/rift_s/rift_s_slam_guard.h b/src/xrt/drivers/rift_s/rift_s_slam_guard.h new file mode 100644 index 000000000..c9c9a222f --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_slam_guard.h @@ -0,0 +1,226 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Rejects diverged SLAM poses and supplies the fallback HMD pose. + * @ingroup drv_rift_s + */ + +#pragma once + +#include "xrt/xrt_defines.h" + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//! SLAM positions farther than this from the tracking origin are treated as divergence. +#define RIFT_S_SLAM_GUARD_MAX_DISTANCE_M 5.0f +//! A position step larger than this between two SLAM samples is treated as divergence. +#define RIFT_S_SLAM_GUARD_MAX_STEP_M 0.5f +//! A head speed above this between two SLAM samples is treated as divergence. +#define RIFT_S_SLAM_GUARD_MAX_SPEED_MPS 5.0f +//! Step allowed on top of the speed limit, for samples queried very close in time. +#define RIFT_S_SLAM_GUARD_STEP_TOLERANCE_M 0.01f +//! Consecutive consistent SLAM samples needed before SLAM is trusted (again). +#define RIFT_S_SLAM_GUARD_STABLE_SAMPLES 30 +//! Time over which those samples must stay consistent. +#define RIFT_S_SLAM_GUARD_STABLE_NS (1000LL * 1000LL * 1000LL) +//! Minimum interval between two SLAM re-initialisation requests. +#define RIFT_S_SLAM_GUARD_RESET_INTERVAL_NS (2000LL * 1000LL * 1000LL) +//! Eye height above the floor reported before SLAM ever gave a good position. +#define RIFT_S_SLAM_GUARD_EYE_HEIGHT_M 1.6f +//! A head this far below the (nominal) floor is underground: divergence. +#define RIFT_S_SLAM_GUARD_BELOW_FLOOR_M 0.3f +//! A head this high above the (nominal) floor is divergence. +#define RIFT_S_SLAM_GUARD_ABOVE_FLOOR_M 2.5f + +enum rift_s_slam_guard_state +{ + //! No trusted SLAM pose yet since start. + RIFT_S_SLAM_GUARD_ACQUIRING, + //! SLAM poses are published. + RIFT_S_SLAM_GUARD_TRACKING, + //! SLAM diverged or stopped; 3DoF with the held position until SLAM is consistent again. + RIFT_S_SLAM_GUARD_DIVERGED, +}; + +struct rift_s_slam_guard +{ + //! Saved world alignment is usable only until the backend changes its map. + bool world_valid; + //! Saved-boundary confirmation is independent of temporary SLAM tracking. + bool boundary_confirmed; + bool world_transition; + struct xrt_pose transition_from, transition_to; + int64_t transition_start_ns; + //! A saved standing frame requires visual confirmation before publishing position tracking. + bool require_relocalization; + enum rift_s_slam_guard_state state; + + //! Last accepted source SLAM pose in the world frame and its source timestamp. + bool have_good; + struct xrt_pose good; + int64_t good_ns; + + //! Maps raw SLAM poses into the published frame (yaw and translation only). + struct xrt_pose align; + + //! Yaw applied to the 3DoF orientation while falling back, so it continues the SLAM heading. + struct xrt_quat fallback_yaw; + + //! Stability run of raw SLAM samples while acquiring or diverged. + uint32_t stable_samples; + int64_t stable_start_ns; + struct xrt_pose stable_last; + int64_t stable_last_ns; + + //! Re-initialisation bookkeeping. + bool reset_pending; + bool have_reset; + int64_t last_reset_ns; + //! Invalid output must also recover before a first trusted pose exists. + int64_t invalid_since_ns; + + //! Diagnostics. + uint32_t divergences; + uint32_t resets; + uint32_t recoveries; + float last_reject_distance_m; + float last_reject_step_m; +}; + +//! Why a SLAM sample was not published. +enum rift_s_slam_guard_reason +{ + RIFT_S_SLAM_GUARD_OK, + RIFT_S_SLAM_GUARD_NO_POSE, + RIFT_S_SLAM_GUARD_NOT_FINITE, + RIFT_S_SLAM_GUARD_TOO_FAR, + RIFT_S_SLAM_GUARD_HEIGHT, + RIFT_S_SLAM_GUARD_JUMP, + RIFT_S_SLAM_GUARD_SPEED, + RIFT_S_SLAM_GUARD_SETTLING, + RIFT_S_SLAM_GUARD_TILT, + RIFT_S_SLAM_GUARD_RELOCALIZING, +}; + +//! Finite, unit-length orientation whose gravity direction agrees with capture-time IMU fusion. +bool +rift_s_slam_guard_pose_usable(const struct xrt_pose *pose, const struct xrt_quat *fusion); + +//! Validate a predicted/historical query without changing source history or world alignment. +bool +rift_s_slam_guard_query(const struct rift_s_slam_guard *g, const struct xrt_pose *raw, int64_t query_ns, + const struct xrt_quat *fusion, const struct xrt_vec3 *no_pose_position, + struct xrt_pose *out); + +//! Install a visually recovered world transform. No head-height or held-position adjustment. +void +rift_s_slam_guard_set_world(struct rift_s_slam_guard *g, const struct xrt_pose *world_from_slam); + +void +rift_s_slam_guard_init(struct rift_s_slam_guard *g); + +/*! + * Feed one SLAM sample and get the IMU pose to publish. + * + * @param g Guard state. + * @param slam_valid Whether the SLAM tracker returned a pose with a valid position. + * @param slam_pose SLAM IMU pose, already converted to the Monado frame. + * @param ts_ns Query time of the sample. + * @param fusion_orientation Current 3DoF orientation. + * @param no_pose_position Position to report while no SLAM position was ever trusted. It is at eye + * height, so it also gives the nominal floor for the height bound. + * @param[out] out_pose Pose to publish. + * @param[out] out_reason Why SLAM was not used, or OK. + * @return true when @p out_pose comes from SLAM (position tracked). + */ +bool +rift_s_slam_guard_update(struct rift_s_slam_guard *g, + bool slam_valid, + const struct xrt_pose *slam_pose, + int64_t ts_ns, + const struct xrt_quat *fusion_orientation, + const struct xrt_vec3 *no_pose_position, + struct xrt_pose *out_pose, + enum rift_s_slam_guard_reason *out_reason); + +/*! + * The pose to publish without SLAM: the 3DoF orientation, continued in the SLAM heading, at the + * last good position (or @p no_pose_position). + */ +void +rift_s_slam_guard_fallback_pose(const struct rift_s_slam_guard *g, + const struct xrt_quat *fusion_orientation, + const struct xrt_vec3 *no_pose_position, + struct xrt_pose *out_pose); + +/*! + * Stop publishing SLAM without counting a divergence (3DoF chosen by the user). SLAM is aligned onto + * the shown pose again once it is consistent. + */ +void +rift_s_slam_guard_suspend(struct rift_s_slam_guard *g, const struct xrt_quat *fusion_orientation); + +//! Start the replacement backend's recovery interval without changing the held world pose. +void +rift_s_slam_guard_backend_started(struct rift_s_slam_guard *g, int64_t ts_ns); + +/*! + * Returns true once per required SLAM re-initialisation, and records it as done at @p ts_ns. + */ +bool +rift_s_slam_guard_take_reset(struct rift_s_slam_guard *g, int64_t ts_ns); + +/*! + * Position, in the tracker frame, that puts the head at eye height above the floor when the tracking + * origin is already offset by @p origin_offset_y. + */ +struct xrt_vec3 +rift_s_slam_guard_no_pose_position(float origin_offset_y); + +//! The yaw-only part (about +Y) of a rotation. +void +rift_s_slam_guard_yaw_of(const struct xrt_quat *q, struct xrt_quat *out_yaw); + +const char * +rift_s_slam_guard_reason_str(enum rift_s_slam_guard_reason reason); + +//! Fewer features than this over all SLAM cameras means the cameras see nothing usable. +#define RIFT_S_SLAM_FEATURES_MIN 20 +//! How long the count must stay on one side of the limit before the state changes. +#define RIFT_S_SLAM_FEATURES_DWELL_NS (500LL * 1000LL * 1000LL) + +//! Tracks whether the SLAM cameras see enough features (not face down, covered or dark). +struct rift_s_slam_feature_gate +{ + bool lost; + bool crossing; + int64_t crossing_since_ns; + int64_t last_ts_ns; + int last_count; +}; + +enum rift_s_slam_feature_event +{ + RIFT_S_SLAM_FEATURES_NO_CHANGE, + //! Features gone: hold the head pose. + RIFT_S_SLAM_FEATURES_LOST, + //! Features back: re-initialise SLAM and realign. + RIFT_S_SLAM_FEATURES_RETURNED, +}; + +/*! + * Feed the latest feature count. @p known false (no count yet) changes nothing. Samples with a + * timestamp not newer than the last one are ignored. + */ +enum rift_s_slam_feature_event +rift_s_slam_feature_gate_update(struct rift_s_slam_feature_gate *gate, bool known, int count, int64_t ts_ns); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.c b/src/xrt/drivers/rift_s/rift_s_tracker.c index 11c832232..194fabd40 100644 --- a/src/xrt/drivers/rift_s/rift_s_tracker.c +++ b/src/xrt/drivers/rift_s/rift_s_tracker.c @@ -20,6 +20,7 @@ #include #include #include +#include #include #include "math/m_api.h" @@ -34,6 +35,8 @@ #include "util/u_sink.h" #include "util/u_trace_marker.h" #include "util/u_var.h" +#include "util/u_file.h" +#include "rift_s_world_control.h" #include "xrt/xrt_config_build.h" #include "xrt/xrt_config_drivers.h" @@ -48,6 +51,7 @@ #include "rift_s_interface.h" #include "rift_s_util.h" #include "rift_s_tracker.h" +#include "rift_s_passthrough.h" #ifdef XRT_FEATURE_SLAM static const bool slam_supported = true; @@ -71,6 +75,9 @@ DEBUG_GET_ONCE_BOOL_OPTION(rift_s_handtracking, "RIFT_S_HANDTRACKING", true) DEBUG_GET_ONCE_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", NULL) #endif +static void * +rift_s_world_thread(void *ptr); + static xrt_result_t rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev, enum xrt_input_name name, @@ -94,6 +101,7 @@ rift_s_tracker_switch_method_cb(void *t_ptr) os_mutex_lock(&t->mutex); m_imu_3dof_reset(&t->fusion.i3dof); t->fusion.i3dof.rot = t->pose.orientation; + rift_s_slam_guard_suspend(&t->slam_guard, &t->fusion.i3dof.rot); os_mutex_unlock(&t->mutex); } } @@ -172,6 +180,12 @@ rift_s_fill_slam_cameras_calibration(struct rift_s_tracker *t, struct rift_s_hmd enum rift_s_camera_id cam_id = CAM_IDX_TO_ID[i]; struct rift_s_camera_calibration *cam = &camera_calibration->cameras[cam_id]; + RIFT_S_DEBUG("DIAG FW cam%d id%d %ux%u fx=%.9g fy=%.9g cx=%.9g cy=%.9g", i, cam_id, + cam->roi.extent.w, cam->roi.extent.h, cam->projection.fx, cam->projection.fy, + cam->projection.cx, cam->projection.cy); + for (int j = 0; j < 16; j++) RIFT_S_DEBUG("DIAG FW cam%d device_from_cam[%d]=%.9g", i, j, cam->device_from_camera.v[j]); + for (int j = 0; j < 6; j++) RIFT_S_DEBUG("DIAG FW cam%d k[%d]=%.9g", i, j, cam->distortion.k[j]); + RIFT_S_DEBUG("DIAG FW cam%d p1=%.9g p2=%.9g", i, cam->distortion.p1, cam->distortion.p2); struct xrt_pose device_from_cam; math_pose_from_isometry(&cam->device_from_camera, &device_from_cam); @@ -197,6 +211,13 @@ rift_s_fill_slam_cameras_calibration(struct rift_s_tracker *t, struct rift_s_hmd static void rift_s_fill_slam_calibration(struct rift_s_tracker *t, struct rift_s_hmd_config *hmd_config) { + const struct rift_s_imu_calibration *c = &hmd_config->imu_calibration; + RIFT_S_DEBUG("DIAG FW accel offset %.9g %.9g %.9g tempcoeff %.9g %.9g %.9g gyro offset %.9g %.9g %.9g", + c->accel.offset_at_0C.x, c->accel.offset_at_0C.y, c->accel.offset_at_0C.z, + c->accel.temp_coeff.x, c->accel.temp_coeff.y, c->accel.temp_coeff.z, + c->gyro.offset.x, c->gyro.offset.y, c->gyro.offset.z); + for (int i = 0; i < 9; i++) RIFT_S_DEBUG("DIAG FW rect[%d] accel=%.9g gyro=%.9g", i, c->accel.rectification.v[i], c->gyro.rectification.v[i]); + for (int i = 0; i < 16; i++) RIFT_S_DEBUG("DIAG FW device_from_imu[%d]=%.9g", i, c->device_from_imu.v[i]); rift_s_fill_slam_imu_calibration(t, hmd_config); rift_s_fill_slam_cameras_calibration(t, hmd_config); } @@ -313,6 +334,12 @@ rift_s_fill_constellation_calibration(struct rift_s_tracker *t, struct rift_s_hm struct rift_s_camera_calibration *cam = &camera_calibration->cameras[cam_id]; /* Compute the cam from IMU transform for each cam */ + RIFT_S_DEBUG("DIAG FW cam%d id%d %ux%u fx=%.9g fy=%.9g cx=%.9g cy=%.9g", i, cam_id, + cam->roi.extent.w, cam->roi.extent.h, cam->projection.fx, cam->projection.fy, + cam->projection.cx, cam->projection.cy); + for (int j = 0; j < 16; j++) RIFT_S_DEBUG("DIAG FW cam%d device_from_cam[%d]=%.9g", i, j, cam->device_from_camera.v[j]); + for (int j = 0; j < 6; j++) RIFT_S_DEBUG("DIAG FW cam%d k[%d]=%.9g", i, j, cam->distortion.k[j]); + RIFT_S_DEBUG("DIAG FW cam%d p1=%.9g p2=%.9g", i, cam->distortion.p1, cam->distortion.p2); struct xrt_pose device_from_cam; math_pose_from_isometry(&cam->device_from_camera, &device_from_cam); @@ -333,8 +360,14 @@ rift_s_fill_constellation_calibration(struct rift_s_tracker *t, struct rift_s_hm }; out->cams[i] = (struct t_constellation_camera){ .P_imu_cam = P_imu_camcv, + .slam_tracking_index = i, .roi = roi, .calibration = rift_s_get_cam_calib(&hmd_config->camera_calibration, cam_id), + .fisheye62_valid = true, + .fisheye62_radial = {cam->distortion.k[0], cam->distortion.k[1], cam->distortion.k[2], + cam->distortion.k[3], cam->distortion.k[4], cam->distortion.k[5]}, + .fisheye62_p1 = cam->distortion.p1, + .fisheye62_p2 = cam->distortion.p2, .blob_min_threshold = BLOB_PIXEL_THRESHOLD, .blob_detect_threshold = BLOB_THRESHOLD_MIN}; @@ -408,10 +441,20 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin, int ret = os_mutex_init(&t->mutex); if (ret != 0) { RIFT_S_ERROR("Failed to init mutex!"); - rift_s_tracker_destroy(t); + free(t); return NULL; } + ret = os_mutex_init(&t->slam_mutex); + if (ret != 0) { + os_mutex_destroy(&t->mutex); + free(t); + return NULL; + } + rift_s_slam_guard_init(&t->slam_guard); + t->slam_guard.require_relocalization = false; // Set from the saved map below; no map means a fresh world. + t->world_command_id = os_monotonic_get_ns(); + // Compute IMU and camera device poses for get_tracked_pose relations math_pose_from_isometry(&hmd_config->imu_calibration.device_from_imu, &t->device_from_imu); @@ -453,6 +496,7 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin, // Initialize 3DoF tracker m_imu_3dof_init(&t->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + m_relation_history_create(&t->fusion.history); t->pose.orientation.w = 1.0f; // All other values set to zero by U_DEVICE_ALLOCATE (which calls U_CALLOC) @@ -516,8 +560,44 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin, entry_sinks = (struct xrt_slam_sinks){0}; } + t->base.camera = rift_s_passthrough_create(&hmd_config->camera_calibration); t->slam_sinks = entry_sinks; t->handtracker = hand_device; + struct rift_s_world_anchor_config world_config = {.camera_count = RIFT_S_CAMERA_COUNT, + .require_boundary = true}; + struct xrt_pose imu_from_device; + math_pose_invert(&t->device_from_imu, &imu_from_device); + for (int eye = 0; eye < RIFT_S_CAMERA_COUNT; eye++) { + world_config.camera[eye] = hmd_config->camera_calibration.cameras[CAM_IDX_TO_ID[eye]]; + struct xrt_pose device_from_camera; + math_pose_from_isometry(&world_config.camera[eye].device_from_camera, &device_from_camera); + math_pose_transform(&imu_from_device, &device_from_camera, &world_config.imu_from_camera[eye]); + } + char map_path[1024]; + const char *bench = getenv("MONADO_STEAMVR_BENCH"); + const char *bench_map = getenv("RIFT_S_WORLD_BENCH_MAP"); + if (bench && strcmp(bench, "1") == 0 && bench_map && bench_map[0] == '/') { + // Offline test maps use their capture's SLAM gauge, independent of the live boundary. + world_config.require_boundary = false; + t->world_anchor = rift_s_world_anchor_create(&world_config, bench_map); + RIFT_S_INFO("World bench map override active"); + } else if (u_file_get_path_in_config_dir("rift-s-world.bin", map_path, sizeof(map_path)) > 0) { + t->world_anchor = rift_s_world_anchor_create(&world_config, map_path); + } + t->world_map_available = rift_s_world_anchor_has_map(t->world_anchor); + t->slam_guard.require_relocalization = + t->world_map_available || rift_s_world_anchor_has_boundary(t->world_anchor); + if (t->world_anchor && os_thread_helper_init(&t->world_thread) == 0) { + t->world_thread_initialized = true; + if (os_thread_helper_start(&t->world_thread, rift_s_world_thread, t) != 0) { + os_thread_helper_destroy(&t->world_thread); + t->world_thread_initialized = false; + } + } + RIFT_S_INFO("World worker started=%d map_available=%d require_relocalization=%d", t->world_thread_initialized, + t->world_map_available, t->slam_guard.require_relocalization); + if (t->slam_guard.require_relocalization && !t->world_map_available) + RIFT_S_WARN("Saved boundary has no compatible visual map; room setup is required to replace its frame"); return t; } @@ -525,9 +605,17 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin, void rift_s_tracker_destroy(struct rift_s_tracker *t) { + if (t->world_thread_initialized) + os_thread_helper_destroy(&t->world_thread); + for (int eye = 0; eye < RIFT_S_CAMERA_COUNT; eye++) + xrt_frame_reference(&t->world_frames[eye], NULL); + rift_s_world_anchor_destroy(t->world_anchor); + rift_s_passthrough_destroy(t->base.camera); t_stereo_camera_calibration_reference(&t->stereo_calib, NULL); m_imu_3dof_close(&t->fusion.i3dof); + m_relation_history_destroy(&t->fusion.history); + os_mutex_destroy(&t->slam_mutex); os_mutex_destroy(&t->mutex); } @@ -582,6 +670,47 @@ clock_hw2mono_get(struct rift_s_tracker *t, uint64_t device_ts, timepoint_ns *ou *out = t->hw2mono + device_ts; } +/* Three missing 30 Hz SLAM frames are enough to stop submitting IMU before + * Basalt's bounded IMU queue fills. Caller holds mutex. */ +#define RIFT_S_CAMERA_TIMEOUT_NS (100 * U_TIME_1MS_IN_NS) + +static void +pause_slam_locked(struct rift_s_tracker *t) +{ + if (t->slam_paused) { + return; + } + t->slam_paused = true; + t->slam_restart_pending = t->tracking.slam_enabled && t->last_camera_arrival_ns != 0; + // SLAM is realigned onto the held pose once it is consistent again. + rift_s_slam_guard_suspend(&t->slam_guard, &t->pose.orientation); + RIFT_S_INFO("Head tracking paused (%s); holding last pose", t->standby ? "standby" : "camera timeout"); +} + +static void +check_camera_timeout_locked(struct rift_s_tracker *t, timepoint_ns now_ns) +{ + if (t->standby || + (t->last_camera_arrival_ns != 0 && now_ns - t->last_camera_arrival_ns > RIFT_S_CAMERA_TIMEOUT_NS)) { + pause_slam_locked(t); + } +} + +void +rift_s_tracker_set_standby(struct rift_s_tracker *t, bool standby) +{ + os_mutex_lock(&t->mutex); + if (standby != t->standby) { + t->standby = standby; + if (standby) { + pause_slam_locked(t); + } else { + RIFT_S_INFO("Headset awake; waiting for fresh camera frames"); + } + } + os_mutex_unlock(&t->mutex); +} + void rift_s_tracker_imu_update(struct rift_s_tracker *t, uint64_t device_timestamp_ns, @@ -590,6 +719,8 @@ rift_s_tracker_imu_update(struct rift_s_tracker *t, { os_mutex_lock(&t->mutex); + check_camera_timeout_locked(t, os_monotonic_get_ns()); + /* Ignore packets before we're ready and clock is stable */ if (!t->ready_for_data || !t->have_hw2mono || t->last_frame_time == 0) { os_mutex_unlock(&t->mutex); @@ -613,15 +744,36 @@ rift_s_tracker_imu_update(struct rift_s_tracker *t, (double)(device_timestamp_ns - t->fusion.last_imu_timestamp_ns) / 1000000000.0, local_timestamp_ns, (double)(local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns) / 1000000000.0, t->hw2mono); + if (local_timestamp_ns / U_TIME_1S_IN_NS != t->fusion.last_imu_local_timestamp_ns / U_TIME_1S_IN_NS) { + RIFT_S_TRACE("HMD IMU accel=%f,%f,%f fusion=%f,%f,%f,%f", accel->x, accel->y, accel->z, + t->fusion.i3dof.rot.x, t->fusion.i3dof.rot.y, t->fusion.i3dof.rot.z, + t->fusion.i3dof.rot.w); + } + + uint64_t sample_generation = t->slam_generation; t->fusion.last_angular_velocity = *gyro; t->fusion.last_imu_timestamp_ns = device_timestamp_ns; t->fusion.last_imu_local_timestamp_ns = local_timestamp_ns; t->pose.orientation = t->fusion.i3dof.rot; + struct xrt_space_relation fusion_relation = XRT_SPACE_RELATION_ZERO; + fusion_relation.pose.orientation = t->pose.orientation; + math_quat_rotate_derivative(&t->pose.orientation, gyro, &fusion_relation.angular_velocity); + fusion_relation.relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT; + if (t->fusion.history) m_relation_history_push(t->fusion.history, &fusion_relation, local_timestamp_ns); os_mutex_unlock(&t->mutex); - if (t->slam_sinks.imu) { + // A delayed old-generation IMU must never enter the replacement backend. Do not + // stall HID ingestion while the camera thread is replacing a session. + if (os_mutex_trylock(&t->slam_mutex) != 0) + return; + os_mutex_lock(&t->mutex); + bool submit = !t->standby && !t->slam_paused && sample_generation == t->slam_generation; + os_mutex_unlock(&t->mutex); + if (submit && t->slam_sinks.imu) { /* Push IMU sample to the SLAM tracker */ struct xrt_vec3_f64 accel64 = {accel->x, accel->y, accel->z}; struct xrt_vec3_f64 gyro64 = {gyro->x, gyro->y, gyro->z}; @@ -630,6 +782,7 @@ rift_s_tracker_imu_update(struct rift_s_tracker *t, xrt_sink_push_imu(t->slam_sinks.imu, &sample); } + os_mutex_unlock(&t->slam_mutex); } #define UPPER_32BITS(x) ((x) & 0xffffffff00000000ULL) @@ -663,40 +816,103 @@ rift_s_tracker_push_slam_frames(struct rift_s_tracker *t, uint64_t frame_ts_ns, struct xrt_frame *frames[RIFT_S_CAMERA_COUNT]) { - + os_mutex_lock(&t->slam_mutex); os_mutex_lock(&t->mutex); - /* Ignore packets before we're ready */ - if (!t->ready_for_data) { + /* Ignore packets before we're ready, and drop any frames before we have IMU */ + if (!t->ready_for_data || !t->have_hw2mono) { os_mutex_unlock(&t->mutex); + os_mutex_unlock(&t->slam_mutex); return; } - if (!t->have_hw2mono) { - /* Drop any frames before we have IMU */ - os_mutex_unlock(&t->mutex); - return; - } - - + timepoint_ns now_ns = os_monotonic_get_ns(); timepoint_ns frame_mono_ns = raw_frame_ts_to_mono_ts(t, frame_ts_ns); if (frame_mono_ns < t->last_frame_time) { RIFT_S_WARN("Camera frame time went backward by %" PRId64 " ns", frame_mono_ns - t->last_frame_time); os_mutex_unlock(&t->mutex); + os_mutex_unlock(&t->slam_mutex); return; } - RIFT_S_TRACE("SLAM frame timestamp %" PRIu64 " local %" PRIu64, frame_ts_ns, frame_mono_ns); - t->last_frame_time = frame_mono_ns; + + /* Queued pre-standby frames must not wake tracking. */ + bool fresh = + now_ns - frame_mono_ns <= RIFT_S_CAMERA_TIMEOUT_NS && frame_mono_ns - now_ns <= RIFT_S_CAMERA_TIMEOUT_NS; + bool submit = fresh && !t->standby; + bool restart = submit && t->tracking.slam_enabled && t->slam_restart_pending; + if (restart) { + t->slam_generation++; + t->slam_min_sample_ns = frame_mono_ns; + t->slam_guard.world_valid = false; + t->slam_guard.boundary_confirmed = false; + t->slam_guard.world_transition = false; + } os_mutex_unlock(&t->mutex); for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) { - if (t->slam_sinks.cams[i]) { - frames[i]->timestamp = frame_mono_ns; - xrt_sink_push_frame(t->slam_sinks.cams[i], frames[i]); + frames[i]->timestamp = frame_mono_ns; + } + rift_s_passthrough_push(t->base.camera, frames[0], frames[1]); + if (fresh && t->world_thread_initialized && frame_mono_ns - t->world_last_queued_ns >= 200 * U_TIME_1MS_IN_NS) { + os_thread_helper_lock(&t->world_thread); + for (int eye = 0; eye < RIFT_S_CAMERA_COUNT; eye++) + xrt_frame_reference(&t->world_frames[eye], frames[eye]); + t->world_last_queued_ns = frame_mono_ns; + os_thread_helper_unlock(&t->world_thread); + } + + if (restart) { + /* Safety-patched Basalt advertises recreation support. Replacement clears flow, + * estimator and queues; unknown backends keep the estimator-reset fallback. */ + int ret = -1; + const char *how = "none"; + timepoint_ns restart_begin_ns = os_monotonic_get_ns(); +#ifdef XRT_FEATURE_SLAM + if (t->tracking.slam->restart != NULL && t->tracking.slam->restart(t->tracking.slam) == 0) { + ret = 0; + how = "recreate"; + } else if (t_slam_reset(t->tracking.slam) == 0) { + ret = 0; + how = "reset"; + } +#endif + os_mutex_lock(&t->mutex); + t->slam_restart_pending = false; + /* Samples from before the restart belong to the old session. */ + t->slam_guard.stable_samples = 0; + if (ret == 0) + rift_s_slam_guard_backend_started(&t->slam_guard, os_monotonic_get_ns()); + t->feature_gate = (struct rift_s_slam_feature_gate){0}; + os_mutex_unlock(&t->mutex); + if (ret == 0) { + RIFT_S_INFO("SLAM session restarted method=%s elapsed_ms=%.3f; waiting for consistent poses", + how, (os_monotonic_get_ns() - restart_begin_ns) / 1e6); + } else { + RIFT_S_WARN("Unable to restart SLAM; the guard keeps holding until its poses are consistent"); } } + + if (submit) { + os_mutex_lock(&t->mutex); + /* Proximity may change while the backend is restarting. */ + submit = !t->standby; + if (submit) { + t->last_camera_arrival_ns = os_monotonic_get_ns(); + t->slam_paused = false; + } + os_mutex_unlock(&t->mutex); + } + + if (submit) { + for (int i = 0; i < RIFT_S_CAMERA_COUNT; i++) { + if (t->slam_sinks.cams[i]) { + xrt_sink_push_frame(t->slam_sinks.cams[i], frames[i]); + } + } + } + os_mutex_unlock(&t->slam_mutex); } struct t_constellation_tracked_device_connection * @@ -728,9 +944,17 @@ rift_s_tracker_push_controller_frameset(struct rift_s_tracker *t, uint64_t frame } timepoint_ns frame_mono_ns = raw_frame_ts_to_mono_ts(t, frame_ts_ns); + bool usable = !t->standby && !t->slam_paused && + (timepoint_ns)os_monotonic_get_ns() - frame_mono_ns <= RIFT_S_CAMERA_TIMEOUT_NS; os_mutex_unlock(&t->mutex); + if (!usable) { + return; + } if (t->controller_sink) { + RIFT_S_TRACE("Controller frame seq=%" PRIu64 " hw=%" PRIu64 " mono=%" PRIu64 " age_ms=%.3f", + frameset->source_sequence, frame_ts_ns, frame_mono_ns, + (double)((int64_t)os_monotonic_get_ns() - frame_mono_ns) / U_TIME_1MS_IN_NS); frameset->timestamp = frame_mono_ns; xrt_sink_push_frame(t->controller_sink, frameset); } @@ -771,46 +995,447 @@ rift_s_tracker_get_tracked_pose(struct rift_s_tracker *t, struct xrt_relation_chain xrc = {0}; if (pose == RIFT_S_TRACKER_POSE_DEVICE) { - m_relation_chain_push_pose(&xrc, &t->device_from_imu); + m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &t->device_from_imu); } else if (pose == RIFT_S_TRACKER_POSE_LEFT_CAMERA) { - m_relation_chain_push_pose(&xrc, &t->left_cam_from_imu); + m_relation_chain_push_inverted_pose_if_not_identity(&xrc, &t->left_cam_from_imu); } - if (t->tracking.slam_enabled && t->slam_over_3dof) { - struct xrt_space_relation imu_relation = XRT_SPACE_RELATION_ZERO; + // Off-head (proximity) or without camera frames, SLAM input is stopped and the head pose is held. + os_mutex_lock(&t->mutex); + check_camera_timeout_locked(t, os_monotonic_get_ns()); + bool query_slam = t->tracking.slam_enabled && t->slam_over_3dof; + bool paused = t->tracking.slam_enabled && (t->standby || t->slam_paused || t->slam_restart_pending); + uint64_t sample_generation = t->slam_generation; + os_mutex_unlock(&t->mutex); - // Get the IMU pose from the SLAM tracker - xrt_tracked_slam_get_tracked_pose(t->tracking.slam, at_timestamp_ns, &imu_relation); + // Basalt can diverge (tens of metres to ~1e7 m away within seconds, seen with the headset left idle). + // The guard publishes SLAM only while it stays near the origin and moves like a head; otherwise it + // falls back to the 3DoF orientation at the last good position and asks for a SLAM re-initialisation. + struct xrt_space_relation slam_relation = XRT_SPACE_RELATION_ZERO; + struct xrt_space_relation query_relation = XRT_SPACE_RELATION_ZERO; + timepoint_ns sample_ts = 0; + bool slam_valid = false; + bool features_known = false; + int feature_count = 0; + timepoint_ns feature_ts = 0; + if (query_slam && !paused) { #ifdef XRT_FEATURE_SLAM + slam_valid = t_slam_get_latest_sample(t->tracking.slam, at_timestamp_ns, &sample_ts, + &slam_relation, &query_relation) && + (slam_relation.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; + timepoint_ns now_ns = os_monotonic_get_ns(); + slam_valid = slam_valid && sample_ts <= now_ns && now_ns - sample_ts < 250 * U_TIME_1MS_IN_NS; // !todo Correct pose depending on the VIT system in use, this should be done in the system itself. // For now, assume that we are using Basalt. - rift_s_tracker_correct_pose_from_basalt(&imu_relation.pose); + rift_s_tracker_correct_pose_from_basalt(&slam_relation.pose); + rift_s_tracker_correct_pose_from_basalt(&query_relation.pose); + struct xrt_quat basalt_to_xr = {0.70710678, 0, 0, -0.70710678}; + math_quat_rotate_vec3(&basalt_to_xr, &slam_relation.linear_velocity, &slam_relation.linear_velocity); + math_quat_rotate_vec3(&basalt_to_xr, &slam_relation.angular_velocity, &slam_relation.angular_velocity); + math_quat_rotate_vec3(&basalt_to_xr, &query_relation.linear_velocity, &query_relation.linear_velocity); + math_quat_rotate_vec3(&basalt_to_xr, &query_relation.angular_velocity, &query_relation.angular_velocity); + features_known = t_slam_get_feature_count(t->tracking.slam, &feature_count, &feature_ts); #endif - imu_relation.relation_flags = (enum xrt_space_relation_flags)( - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | - XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); - - m_relation_chain_push_relation(&xrc, &imu_relation); - } else { - struct xrt_space_relation imu_relation = XRT_SPACE_RELATION_ZERO; - - os_mutex_lock(&t->mutex); - // TODO: Estimate pose at timestamp at_timestamp_ns - math_quat_normalize(&t->pose.orientation); - imu_relation.pose = t->pose; - imu_relation.angular_velocity = t->fusion.last_angular_velocity; - imu_relation.relation_flags = (enum xrt_space_relation_flags)( - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | - XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); - - m_relation_chain_push_relation(&xrc, &imu_relation); - - os_mutex_unlock(&t->mutex); } + // Without any good SLAM position, put the head at eye height above the floor. The tracking origin + // may already carry that height as its offset (the Rift S builder sets 1.6 m). + struct xrt_vec3 no_pose_position = rift_s_slam_guard_no_pose_position( + t->base.tracking_origin != NULL ? t->base.tracking_origin->initial_offset.position.y : 0.0f); + + struct xrt_space_relation imu_relation = XRT_SPACE_RELATION_ZERO; + const enum xrt_space_relation_flags held_flags = (enum xrt_space_relation_flags)( + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT); + + os_mutex_lock(&t->mutex); + math_quat_normalize(&t->pose.orientation); + paused = paused || t->standby || t->slam_paused || t->slam_restart_pending || + sample_generation != t->slam_generation; + slam_valid = slam_valid && sample_ts >= t->slam_min_sample_ns; + struct xrt_quat sample_fusion = t->pose.orientation; + struct xrt_quat query_fusion = t->pose.orientation; + if (t->fusion.history) { + struct xrt_space_relation queried; + if (m_relation_history_get(t->fusion.history, at_timestamp_ns, &queried) != M_RELATION_HISTORY_RESULT_INVALID) + query_fusion = queried.pose.orientation; + if (m_relation_history_get(t->fusion.history, sample_ts, &queried) != M_RELATION_HISTORY_RESULT_INVALID) + sample_fusion = queried.pose.orientation; + } + struct rift_s_slam_guard *g = &t->slam_guard; + uint32_t divergences = g->divergences; + uint32_t recoveries = g->recoveries; + enum rift_s_slam_guard_state previous_state = g->state; + uint32_t resets = g->resets; + int64_t previous_good_ns = g->good_ns; + enum rift_s_slam_guard_reason reason = RIFT_S_SLAM_GUARD_OK; + + enum rift_s_slam_feature_event feature_event = RIFT_S_SLAM_FEATURES_NO_CHANGE; + if (query_slam && !paused) { + feature_event = rift_s_slam_feature_gate_update(&t->feature_gate, features_known, feature_count, + (int64_t)feature_ts); + if (feature_event == RIFT_S_SLAM_FEATURES_LOST) { + rift_s_slam_guard_suspend(g, &t->pose.orientation); + } else if (feature_event == RIFT_S_SLAM_FEATURES_RETURNED) { + t->slam_restart_pending = true; + } + } + bool hold = paused || (query_slam && t->feature_gate.lost); + if (query_slam && !paused && t->feature_gate.lost) { + // Keep the health clock running through a feature hold without accepting new world poses. + if (slam_valid && rift_s_slam_guard_pose_usable(&slam_relation.pose, &sample_fusion)) { + g->invalid_since_ns = 0; + } else { + struct xrt_pose ignored; + rift_s_slam_guard_update(g, slam_valid, &slam_relation.pose, + slam_valid ? sample_ts : os_monotonic_get_ns(), &sample_fusion, + &no_pose_position, &ignored, &reason); + if (rift_s_slam_guard_take_reset(g, os_monotonic_get_ns())) + t->slam_restart_pending = true; + } + } + + if (hold) { + // Freeze the head where it was last shown: untracked, no motion. + if (!t->have_published_imu_pose) { + rift_s_slam_guard_fallback_pose(g, &t->pose.orientation, &no_pose_position, + &t->published_imu_pose); + t->have_published_imu_pose = true; + } + imu_relation.pose = t->published_imu_pose; + imu_relation.relation_flags = held_flags; + } else { + bool use_slam = false; + if (query_slam) { + use_slam = rift_s_slam_guard_update(g, slam_valid, &slam_relation.pose, + slam_valid ? sample_ts : os_monotonic_get_ns(), &sample_fusion, + &no_pose_position, &imu_relation.pose, &reason); + if (rift_s_slam_guard_take_reset(g, os_monotonic_get_ns())) { + t->slam_restart_pending = true; + } + } else { + // TODO: Estimate pose at timestamp at_timestamp_ns + rift_s_slam_guard_fallback_pose(g, &t->pose.orientation, &no_pose_position, &imu_relation.pose); + } + + if (use_slam && rift_s_slam_guard_query(g, &query_relation.pose, at_timestamp_ns, &query_fusion, + &no_pose_position, &imu_relation.pose)) { + math_quat_rotate_vec3(&g->align.orientation, &query_relation.linear_velocity, + &imu_relation.linear_velocity); + math_quat_rotate_vec3(&g->align.orientation, &query_relation.angular_velocity, + &imu_relation.angular_velocity); + imu_relation.relation_flags = + (enum xrt_space_relation_flags)(held_flags | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + } else { + rift_s_slam_guard_fallback_pose(g, &query_fusion, &no_pose_position, &imu_relation.pose); + imu_relation.angular_velocity = t->fusion.last_angular_velocity; + imu_relation.relation_flags = + (enum xrt_space_relation_flags)(held_flags | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + } + if (pose == RIFT_S_TRACKER_POSE_DEVICE || !t->have_published_imu_pose) { + t->published_imu_pose = imu_relation.pose; + t->have_published_imu_pose = true; + } + } + + if (at_timestamp_ns < (uint64_t)t->slam_min_sample_ns) + imu_relation.relation_flags = held_flags; + + if (feature_event == RIFT_S_SLAM_FEATURES_LOST) { + RIFT_S_WARN("SLAM cameras see %d features (< %d); holding head pose", feature_count, + RIFT_S_SLAM_FEATURES_MIN); + } else if (feature_event == RIFT_S_SLAM_FEATURES_RETURNED) { + RIFT_S_INFO("SLAM cameras see %d features again; re-initialising SLAM", feature_count); + } + if (g->divergences != divergences) { + RIFT_S_WARN( + "SLAM diverged reason=%s distance_m=%.2f step_m=%.2f slam=(%.2f,%.2f,%.2f) " + "held=(%.2f,%.2f,%.2f) query_ns=%lld previous_ns=%lld delta_ms=%.3f pose_kind=%d divergences=%u: 3DoF until SLAM is consistent again", + rift_s_slam_guard_reason_str(reason), g->last_reject_distance_m, g->last_reject_step_m, + slam_relation.pose.position.x, slam_relation.pose.position.y, slam_relation.pose.position.z, + imu_relation.pose.position.x, imu_relation.pose.position.y, imu_relation.pose.position.z, + (long long)at_timestamp_ns, (long long)previous_good_ns, + ((int64_t)at_timestamp_ns - previous_good_ns) / 1e6, pose, g->divergences); + } + if (g->recoveries != recoveries) { + RIFT_S_INFO("SLAM recovered position=(%.2f,%.2f,%.2f) recoveries=%u resets=%u", + imu_relation.pose.position.x, imu_relation.pose.position.y, imu_relation.pose.position.z, + g->recoveries, g->resets); + } + if (previous_state != g->state || recoveries != g->recoveries || resets != g->resets) { + float origin_y = t->base.tracking_origin ? t->base.tracking_origin->initial_offset.position.y : 0; + RIFT_S_INFO("SLAM alignment state=%d->%d query_ns=%lld origin_y=%.6f nominal_floor_y=%.6f align_p=(%.6f,%.6f,%.6f) align_q=(%.6f,%.6f,%.6f,%.6f) raw_p=(%.6f,%.6f,%.6f) published_p=(%.6f,%.6f,%.6f) recoveries=%u resets=%u", + previous_state, g->state, (long long)at_timestamp_ns, origin_y, + no_pose_position.y - RIFT_S_SLAM_GUARD_EYE_HEIGHT_M, + g->align.position.x, g->align.position.y, g->align.position.z, + g->align.orientation.x, g->align.orientation.y, g->align.orientation.z, g->align.orientation.w, + slam_relation.pose.position.x, slam_relation.pose.position.y, slam_relation.pose.position.z, + imu_relation.pose.position.x, imu_relation.pose.position.y, imu_relation.pose.position.z, + g->recoveries, g->resets); + } + timepoint_ns pose_log_now = os_monotonic_get_ns(); + if (pose == RIFT_S_TRACKER_POSE_DEVICE && + pose_log_now - t->last_published_pose_log_ns >= 5LL * U_TIME_1S_IN_NS) { + t->last_published_pose_log_ns = pose_log_now; + RIFT_S_INFO("HMD pose flags=0x%x paused=%d slam_valid=%d sample_age_ms=%.1f guard=%d " + "boundary_confirmed=%d position=(%.3f,%.3f,%.3f)", + imu_relation.relation_flags, hold, slam_valid, + sample_ts ? (pose_log_now - sample_ts) / 1e6 : -1.0, g->state, + g->boundary_confirmed, imu_relation.pose.position.x, + imu_relation.pose.position.y, imu_relation.pose.position.z); + } + os_mutex_unlock(&t->mutex); + + m_relation_chain_push_relation(&xrc, &imu_relation); m_relation_chain_resolve(&xrc, out_relation); } +// Run feature matching off the camera/pose threads. A single latest-frame pair bounds backlog. +static float +world_rotation_difference(const struct xrt_quat *a, const struct xrt_quat *b) +{ + float dot = fabsf(a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w); + return 2 * acosf(fminf(1, dot)); +} + +static void +world_write_status(struct rift_s_tracker *t) +{ + os_mutex_lock(&t->mutex); + const char *state = t->world_storage_failed ? "save_failed" + : t->world_setup ? "setup" + : (t->slam_guard.boundary_confirmed && !t->slam_guard.world_transition) ? "localized" + : t->world_map_available ? "relocalizing" + : "unanchored"; + os_mutex_unlock(&t->mutex); + static timepoint_ns last_write_ns; + static const char *last_state; + timepoint_ns now_ns = os_monotonic_get_ns(); + if (state == last_state && now_ns - last_write_ns < 500 * U_TIME_1MS_IN_NS) return; + last_write_ns = now_ns; + last_state = state; + char path[1024], temp[1050]; + rift_s_world_control_path("status", path, sizeof(path)); + snprintf(temp, sizeof(temp), "%s.tmp", path); + FILE *file = fopen(temp, "w"); + if (!file) return; + fprintf(file, "%lld %s\n", (long long)os_monotonic_get_ns(), state); + if (fclose(file) == 0) rename(temp, path); +} + +static void +world_read_command(struct rift_s_tracker *t) +{ + char path[1024], command[16] = {0}; + rift_s_world_control_path("command", path, sizeof(path)); + FILE *file = fopen(path, "r"); + if (!file) + return; + unsigned long long id = 0; + int count = fscanf(file, "%llu %15s", &id, command); + fclose(file); + if (count != 2 || id <= t->world_command_id) + return; + t->world_command_id = id; + if (strcmp(command, "begin") == 0) { + rift_s_world_anchor_clear(t->world_anchor); + os_mutex_lock(&t->mutex); + t->world_setup = true; + t->slam_guard.boundary_confirmed = false; + t->slam_guard.require_relocalization = false; + t->world_confirmations = 0; + os_mutex_unlock(&t->mutex); + RIFT_S_INFO("New room setup may establish a world; saved anchors retained until Accept"); + } else if (strcmp(command, "save") == 0) { + os_mutex_lock(&t->mutex); + bool setup = t->world_setup; + os_mutex_unlock(&t->mutex); + if (!setup) { + RIFT_S_WARN("Ignoring world save without a room setup begin"); + return; + } + bool saved = rift_s_world_anchor_save(t->world_anchor); + bool requires_relocalization = + rift_s_world_anchor_has_map(t->world_anchor) || rift_s_world_anchor_has_boundary(t->world_anchor); + os_mutex_lock(&t->mutex); + t->world_storage_failed = !saved; + t->slam_guard.boundary_confirmed = saved && rift_s_world_anchor_has_map(t->world_anchor); + t->world_setup = false; + t->world_map_available = rift_s_world_anchor_has_map(t->world_anchor); + t->slam_guard.require_relocalization = requires_relocalization; + if (!saved || !t->world_map_available) + t->slam_guard.boundary_confirmed = false; + t->slam_guard.world_transition = false; + os_mutex_unlock(&t->mutex); + RIFT_S_INFO("Boundary world anchors saved=%d map_available=%d", saved, t->world_map_available); + } else if (strcmp(command, "cancel") == 0) { + rift_s_world_anchor_reload(t->world_anchor); + bool requires_relocalization = + rift_s_world_anchor_has_map(t->world_anchor) || rift_s_world_anchor_has_boundary(t->world_anchor); + os_mutex_lock(&t->mutex); + if (t->world_setup) { + t->slam_guard.world_valid = false; + t->slam_guard.boundary_confirmed = false; + t->slam_guard.world_transition = false; + rift_s_slam_guard_suspend(&t->slam_guard, &t->pose.orientation); + } + t->world_setup = false; + t->world_map_available = rift_s_world_anchor_has_map(t->world_anchor); + t->slam_guard.require_relocalization = requires_relocalization; + t->world_confirmations = 0; + os_mutex_unlock(&t->mutex); + } +} + +static void * +rift_s_world_thread(void *ptr) +{ + struct rift_s_tracker *t = ptr; + timepoint_ns last_attempt_log_ns = 0; + uint64_t attempts = 0, usable = 0; + os_thread_helper_lock(&t->world_thread); + while (os_thread_helper_is_running_locked(&t->world_thread)) { + struct xrt_frame *frames[RIFT_S_CAMERA_COUNT] = {0}; + for (int eye = 0; eye < RIFT_S_CAMERA_COUNT; eye++) { + frames[eye] = t->world_frames[eye]; + t->world_frames[eye] = NULL; + } + os_thread_helper_unlock(&t->world_thread); + world_read_command(t); + if (frames[0] && frames[1] && t->tracking.slam_enabled) { + timepoint_ns capture_ns = frames[0]->timestamp; + struct xrt_space_relation source = XRT_SPACE_RELATION_ZERO, capture = XRT_SPACE_RELATION_ZERO; + timepoint_ns source_ns = 0; + os_mutex_lock(&t->mutex); + uint64_t generation = t->slam_generation; + os_mutex_unlock(&t->mutex); + bool valid = false; +#ifdef XRT_FEATURE_SLAM + // Wait briefly for this exposure's SLAM sample, rather than triangulate against a prediction. + for (int attempt = 0; attempt < 12; attempt++) { + valid = t_slam_get_latest_sample(t->tracking.slam, capture_ns, &source_ns, &source, + &capture); + if (valid && source_ns >= capture_ns) + break; + os_nanosleep(10 * U_TIME_1MS_IN_NS); + } + rift_s_tracker_correct_pose_from_basalt(&capture.pose); +#endif + os_mutex_lock(&t->mutex); + struct xrt_quat fusion = t->pose.orientation; + struct xrt_space_relation fusion_capture; + if (m_relation_history_get(t->fusion.history, capture_ns, &fusion_capture) != + M_RELATION_HISTORY_RESULT_INVALID) + fusion = fusion_capture.pose.orientation; + valid = valid && source_ns >= capture_ns && source_ns - capture_ns <= 150 * U_TIME_1MS_IN_NS && + (capture.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) && + capture_ns >= t->slam_min_sample_ns && generation == t->slam_generation && + !t->standby && !t->slam_paused && !t->slam_restart_pending && + rift_s_slam_guard_pose_usable(&capture.pose, &fusion); + bool standby = t->standby; + struct xrt_pose world_capture; + struct xrt_vec3 no_pose_position = rift_s_slam_guard_no_pose_position( + t->base.tracking_origin ? t->base.tracking_origin->initial_offset.position.y : 0.0f); + // Recording must pass the same displacement/gravity checks as a displayed pose. + bool record = valid && (t->world_setup || t->slam_guard.boundary_confirmed) && + !t->slam_guard.world_transition && + rift_s_slam_guard_query(&t->slam_guard, &capture.pose, capture_ns, &fusion, + &no_pose_position, &world_capture); + os_mutex_unlock(&t->mutex); + rift_s_world_anchor_record_rig_frames(t->world_anchor, (const struct xrt_frame *const *)frames, + RIFT_S_CAMERA_COUNT, valid ? &capture.pose : NULL); + struct rift_s_world_anchor_result result = {0}; + bool processed = (record || rift_s_world_anchor_has_map(t->world_anchor)) && + rift_s_world_anchor_process_rig_gravity( + t->world_anchor, (const struct xrt_frame *const *)frames, + RIFT_S_CAMERA_COUNT, record ? &world_capture : NULL, + valid ? &capture.pose.orientation : NULL, &result); + attempts++; + usable += valid; + if (capture_ns - last_attempt_log_ns >= U_TIME_1S_IN_NS) { + last_attempt_log_ns = capture_ns; + RIFT_S_INFO("World attempt=%" PRIu64 " usable=%" PRIu64 + " pose_valid=%d sample_gap_ms=%.1f" + " standby=%d keyframes=%u features=%u/%u/%u/%u/%u matches=%u/%u/%u/%u/%u " + "inliers=%u/%u/%u/%u/%u" + " points_by_camera=%u/%u/%u/%u/%u support=%u stereo=%u triangulated=%u " + "error_px=%.3f record=%d matched=%d confirmations=%u rejection=%s", + attempts, usable, valid, + source_ns > 0 ? (double)(source_ns - capture_ns) / U_TIME_1MS_IN_NS : -1.0, + standby, result.keyframes, result.features[0], result.features[1], + result.features[2], result.features[3], result.features[4], + result.matches[0], result.matches[1], result.matches[2], result.matches[3], + result.matches[4], result.inliers[0], result.inliers[1], result.inliers[2], + result.inliers[3], result.inliers[4], result.camera_points[0], + result.camera_points[1], result.camera_points[2], result.camera_points[3], + result.camera_points[4], result.supporting_cameras, result.stereo_matches, + result.triangulated, result.max_reprojection_px, record, result.relocalized, + t->world_confirmations, result.rejection ? result.rejection : "none"); + } + if (processed && result.recorded) { + os_mutex_lock(&t->mutex); + bool setup = t->world_setup; + t->world_map_available = true; + os_mutex_unlock(&t->mutex); + if (!setup) { + bool saved = rift_s_world_anchor_save(t->world_anchor); + os_mutex_lock(&t->mutex); + t->world_storage_failed = !saved; + os_mutex_unlock(&t->mutex); + } + RIFT_S_INFO("World keyframe recorded count=%u", result.keyframes); + } + if (processed && result.relocalized && valid && !record) { + struct xrt_pose inverse, candidate; + math_pose_invert(&capture.pose, &inverse); + math_pose_transform(&result.world_from_imu, &inverse, &candidate); + struct xrt_vec3 up = {0, 1, 0}, world_up; + math_quat_rotate_vec3(&candidate.orientation, &up, &world_up); + os_mutex_lock(&t->mutex); + if (generation == t->slam_generation && !t->standby && !t->slam_restart_pending && + world_up.y > 0.9961947f) { + rift_s_slam_guard_yaw_of(&candidate.orientation, &candidate.orientation); + struct xrt_vec3 rotated; + math_quat_rotate_vec3(&candidate.orientation, &capture.pose.position, &rotated); + candidate.position = m_vec3_sub(result.world_from_imu.position, rotated); + bool consistent = + t->world_confirmations > 0 && t->world_candidate_generation == generation && + capture_ns > t->world_candidate_ns && + capture_ns - t->world_candidate_ns <= 3LL * U_TIME_1S_IN_NS && + m_vec3_len(m_vec3_sub(candidate.position, t->world_candidate.position)) < + 0.04f && + world_rotation_difference(&candidate.orientation, + &t->world_candidate.orientation) < DEG_TO_RAD(3); + t->world_confirmations = consistent ? t->world_confirmations + 1 : 1; + t->world_candidate = candidate; + t->world_candidate_generation = generation; + t->world_candidate_ns = capture_ns; + if (t->world_confirmations >= 3 && !t->slam_guard.boundary_confirmed) { + rift_s_slam_guard_set_world(&t->slam_guard, &candidate); + RIFT_S_INFO( + "World relocalized inliers=%u/%u error_px=%.3f " + "transform=(%.4f,%.4f,%.4f)", + result.inliers[0], result.inliers[1], result.max_reprojection_px, + candidate.position.x, candidate.position.y, candidate.position.z); + } + } + os_mutex_unlock(&t->mutex); + } else if (!record && (!valid || capture_ns - t->world_candidate_ns > 3LL * U_TIME_1S_IN_NS)) { + os_mutex_lock(&t->mutex); + t->world_confirmations = 0; + os_mutex_unlock(&t->mutex); + } + } + for (int eye = 0; eye < RIFT_S_CAMERA_COUNT; eye++) + xrt_frame_reference(&frames[eye], NULL); + world_write_status(t); + os_nanosleep(20 * U_TIME_1MS_IN_NS); + os_thread_helper_lock(&t->world_thread); + } + os_thread_helper_unlock(&t->world_thread); + return NULL; +} + void rift_s_tracker_start(struct rift_s_tracker *t) { diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.h b/src/xrt/drivers/rift_s/rift_s_tracker.h index fb644c987..af52811a1 100644 --- a/src/xrt/drivers/rift_s/rift_s_tracker.h +++ b/src/xrt/drivers/rift_s/rift_s_tracker.h @@ -16,6 +16,7 @@ #pragma once #include "math/m_imu_3dof.h" +#include "math/m_relation_history.h" #include "os/os_threading.h" #include "util/u_var.h" #include "xrt/xrt_defines.h" @@ -25,6 +26,8 @@ #include "tracking/t_constellation_tracking.h" #include "rift_s_firmware.h" +#include "rift_s_slam_guard.h" +#include "rift_s_world_anchor.h" /* Oculus Rift S HMD Tracking */ #ifndef RIFT_S_TRACKER_H @@ -46,6 +49,42 @@ struct rift_s_tracker //! Protects shared access to 3dof and pose storage struct os_mutex mutex; + //! Serializes camera submission and SLAM session replacement. + struct os_mutex slam_mutex; + //! Latest-frame queue; feature matching never blocks USB, SLAM or controller ingestion. + struct os_thread_helper world_thread; + bool world_thread_initialized; + struct rift_s_world_anchor *world_anchor; + struct xrt_frame *world_frames[RIFT_S_CAMERA_COUNT]; + timepoint_ns world_last_queued_ns; + bool world_setup; + bool world_map_available; + bool world_storage_failed; + uint64_t world_command_id; + uint32_t world_confirmations; + struct xrt_pose world_candidate; + uint64_t world_candidate_generation; + timepoint_ns world_candidate_ns; + + //! Camera arrival watchdog uses the host clock, not predicted pose time. + timepoint_ns last_camera_arrival_ns; + //! Proximity sensor reports the headset off-head. + bool standby; + //! SLAM input stopped (standby or camera timeout). + bool slam_paused; + //! Re-initialise SLAM before submitting the next fresh camera frame. + bool slam_restart_pending; + uint64_t slam_generation; + timepoint_ns slam_min_sample_ns; + + //! Whether the SLAM cameras see enough features. Under @ref mutex. + struct rift_s_slam_feature_gate feature_gate; + + //! Last published IMU pose, frozen while the head pose is held. + bool have_published_imu_pose; + timepoint_ns last_published_pose_log_ns; + struct xrt_pose published_imu_pose; + //! Don't process IMU / video until started bool ready_for_data; @@ -53,6 +92,7 @@ struct rift_s_tracker { //! Main fusion calculator. struct m_imu_3dof i3dof; + struct m_relation_history *history; //! The last angular velocity from the IMU, for prediction. struct xrt_vec3 last_angular_velocity; @@ -101,6 +141,9 @@ struct rift_s_tracker //! Last tracked pose struct xrt_pose pose; + //! Rejects diverged SLAM poses and holds the position while falling back to 3DoF. Under @ref mutex. + struct rift_s_slam_guard slam_guard; + /* Stereo calibration for the front 2 cameras */ struct t_stereo_camera_calibration *stereo_calib; struct t_slam_calibration slam_calib; @@ -128,6 +171,10 @@ struct rift_s_tracker } gui; }; +#ifdef __cplusplus +extern "C" { +#endif + struct rift_s_tracker * rift_s_tracker_create(struct xrt_tracking_origin *origin, struct xrt_frame_context *xfctx, @@ -135,6 +182,8 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin, void rift_s_tracker_start(struct rift_s_tracker *t); void +rift_s_tracker_set_standby(struct rift_s_tracker *t, bool standby); +void rift_s_tracker_destroy(struct rift_s_tracker *t); void rift_s_tracker_add_debug_ui(struct rift_s_tracker *t, void *root); @@ -175,4 +224,8 @@ rift_s_tracker_get_tracked_pose(struct rift_s_tracker *t, uint64_t at_timestamp_ns, struct xrt_space_relation *out_relation); +#ifdef __cplusplus +} +#endif + #endif diff --git a/src/xrt/drivers/rift_s/rift_s_util.cpp b/src/xrt/drivers/rift_s/rift_s_util.cpp index 1cdd5a4c8..ec714eff0 100644 --- a/src/xrt/drivers/rift_s/rift_s_util.cpp +++ b/src/xrt/drivers/rift_s/rift_s_util.cpp @@ -214,7 +214,7 @@ struct DistortParamKB4CostFunctor struct t_camera_calibration rift_s_get_cam_calib(struct rift_s_camera_calibration_block *camera_calibration, enum rift_s_camera_id cam_id) { - struct t_camera_calibration tcc; + struct t_camera_calibration tcc{}; struct rift_s_camera_calibration *rift_s_cam = &camera_calibration->cameras[cam_id]; tcc.image_size_pixels.h = rift_s_cam->roi.extent.h; @@ -341,3 +341,31 @@ rift_s_create_stereo_camera_calib_rotated(struct rift_s_camera_calibration_block return calib; } + +float +rift_s_apply_optical_yaw(struct xrt_quat *imu_orientation, const struct xrt_quat *optical_orientation) +{ + struct xrt_quat inverse, delta; + math_quat_invert(imu_orientation, &inverse); + math_quat_rotate(optical_orientation, &inverse, &delta); + + // Project the world-space difference onto Y. q and -q must take the same shortest path. + if (delta.w < 0) { + delta.y = -delta.y; + delta.w = -delta.w; + } + if (fabsf(delta.y) + fabsf(delta.w) < 1e-6f) { + return 0; + } + float yaw = 2 * atan2f(delta.y, delta.w); + if (fabsf(yaw) <= DEG_TO_RAD(0.25f)) { + return 0; + } + if (fabsf(yaw) > DEG_TO_RAD(5.0f)) { + yaw *= 0.10f; + } + struct xrt_quat correction = {0, sinf(yaw / 2), 0, cosf(yaw / 2)}; + math_quat_rotate(&correction, imu_orientation, imu_orientation); + math_quat_normalize(imu_orientation); + return yaw; +} diff --git a/src/xrt/drivers/rift_s/rift_s_util.h b/src/xrt/drivers/rift_s/rift_s_util.h index 3f3b0f836..7cc2ee495 100644 --- a/src/xrt/drivers/rift_s/rift_s_util.h +++ b/src/xrt/drivers/rift_s/rift_s_util.h @@ -20,6 +20,11 @@ extern "C" { #endif +// Apply optical yaw around world Y, preserving the IMU's gravity alignment. +// Returns the applied angle in radians. Large discrepancies are corrected by 10%. +float +rift_s_apply_optical_yaw(struct xrt_quat *imu_orientation, const struct xrt_quat *optical_orientation); + struct t_camera_calibration rift_s_get_cam_calib(struct rift_s_camera_calibration_block *camera_calibration, enum rift_s_camera_id cam_id); diff --git a/src/xrt/drivers/rift_s/rift_s_world_anchor.cpp b/src/xrt/drivers/rift_s/rift_s_world_anchor.cpp new file mode 100644 index 000000000..a444c9abc --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_world_anchor.cpp @@ -0,0 +1,1490 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "rift_s_world_anchor.h" +#include "util/u_json.h" +#include "util/u_logging.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { +constexpr uint32_t MAX_KEYFRAMES = 64, MAX_POINTS = 1000, MIN_POINTS = 40; +constexpr double MAX_ERROR_PX = 2.5; +struct Transform +{ + cv::Matx33d r = cv::Matx33d::eye(); + cv::Vec3d t = {0, 0, 0}; + cv::Vec3d + apply(const cv::Vec3d &p) const + { + return r * p + t; + } + Transform + inverse() const + { + return {r.t(), -(r.t() * t)}; + } + Transform + operator*(const Transform &b) const + { + return {r * b.r, r * b.t + t}; + } +}; +bool +finite_pose(const xrt_pose &p) +{ + const auto &q = p.orientation; + double n = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; + return std::isfinite(n) && std::abs(n - 1) < 0.01 && std::isfinite(p.position.x) && + std::isfinite(p.position.y) && std::isfinite(p.position.z); +} +Transform +transform(const xrt_pose &p) +{ + const auto &q = p.orientation; + double x = q.x, y = q.y, z = q.z, w = q.w; + return {{1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w), 2 * (x * y + z * w), + 1 - 2 * (x * x + z * z), 2 * (y * z - x * w), 2 * (x * z - y * w), 2 * (y * z + x * w), + 1 - 2 * (x * x + y * y)}, + {p.position.x, p.position.y, p.position.z}}; +} +xrt_pose +pose(const Transform &t) +{ + cv::Vec3d rv; + cv::Rodrigues(t.r, rv); + double angle = cv::norm(rv), scale = angle > 1e-12 ? std::sin(angle / 2) / angle : 0.5; + return {{float(rv[0] * scale), float(rv[1] * scale), float(rv[2] * scale), float(std::cos(angle / 2))}, + {float(t.t[0]), float(t.t[1]), float(t.t[2])}}; +} +double +angle_between(const Transform &a, const Transform &b) +{ + cv::Vec3d rv; + cv::Rodrigues(a.r.t() * b.r, rv); + return cv::norm(rv); +} +unsigned +camera_count(const rift_s_world_anchor_config &c) +{ + return c.camera_count ? c.camera_count : 2; +} +bool +calibration_valid(const rift_s_world_anchor_config &c) +{ + if (camera_count(c) < 2 || camera_count(c) > RIFT_S_WORLD_MAX_CAMERAS) + return false; + uint64_t pixels = 0; + for (unsigned eye = 0; eye < camera_count(c); ++eye) { + const auto &cam = c.camera[eye]; + if (cam.roi.extent.w < 64 || cam.roi.extent.w > 2048 || cam.roi.extent.h < 64 || + cam.roi.extent.h > 2048 || !finite_pose(c.imu_from_camera[eye]) || + !std::isfinite(cam.projection.fx) || !std::isfinite(cam.projection.fy) || cam.projection.fx < 10 || + cam.projection.fy < 10 || !std::isfinite(cam.projection.cx) || !std::isfinite(cam.projection.cy) || + !std::isfinite(cam.distortion.p1) || !std::isfinite(cam.distortion.p2)) + return false; + for (float k : cam.distortion.k) + if (!std::isfinite(k)) + return false; + pixels += uint64_t(cam.roi.extent.w) * uint64_t(cam.roi.extent.h); + } + // Bound retained ray/warp caches to the native five-camera crop budget. + if (pixels > uint64_t(RIFT_S_WORLD_MAX_CAMERAS) * 640 * 480) + return false; + double baseline = cv::norm(transform(c.imu_from_camera[0]).t - transform(c.imu_from_camera[1]).t); + return camera_count(c) > 2 || (baseline > 0.02 && baseline < 0.5); +} +cv::Vec2d +distort(const rift_s_camera_calibration &c, cv::Vec2d angular) +{ + double t2 = angular.dot(angular), radial = c.distortion.k[5]; + for (int i = 4; i >= 0; --i) + radial = radial * t2 + c.distortion.k[i]; + angular *= radial * t2 + 1; + const cv::Vec2d tangent{c.distortion.p1, c.distortion.p2}; + return angular * (1 + 2 * angular.dot(tangent)) + tangent * angular.dot(angular); +} +bool +project(const rift_s_camera_calibration &c, cv::Vec3d p, cv::Point2d &out) +{ + if (p[2] <= 0.01 || !std::isfinite(cv::norm(p))) + return false; + double r = std::hypot(p[0], p[1]), scale = r > 1e-12 ? std::atan2(r, p[2]) / r : 1 / p[2]; + auto pixel = distort(c, {p[0] * scale, p[1] * scale}); + out = {c.projection.fx * pixel[0] + c.projection.cx, c.projection.fy * pixel[1] + c.projection.cy}; + return std::isfinite(out.x) && std::isfinite(out.y); +} +bool +unproject(const rift_s_camera_calibration &c, cv::Point2d pixel, cv::Vec3d &ray) +{ + if (!std::isfinite(pixel.x) || !std::isfinite(pixel.y) || c.projection.fx <= 0 || c.projection.fy <= 0) + return false; + cv::Vec2d target{(pixel.x - c.projection.cx) / c.projection.fx, (pixel.y - c.projection.cy) / c.projection.fy}; + // Invert the radial+tangential polynomial on the angular plane, then tan(theta). + cv::Vec2d uv = target; + if (cv::norm(uv) > 1.2) + uv *= 1.2 / cv::norm(uv); + for (int i = 0; i < 30; ++i) { + cv::Vec2d residual = distort(c, uv) - target; + if (cv::norm(residual) < 1e-10) + break; + constexpr double eps = 1e-6; + cv::Vec2d dx = (distort(c, uv + cv::Vec2d{eps, 0}) - distort(c, uv - cv::Vec2d{eps, 0})) / (2 * eps); + cv::Vec2d dy = (distort(c, uv + cv::Vec2d{0, eps}) - distort(c, uv - cv::Vec2d{0, eps})) / (2 * eps); + cv::Matx22d j{dx[0], dy[0], dx[1], dy[1]}; + if (std::abs(cv::determinant(j)) < 1e-8) + return false; + cv::Vec2d step = j.inv() * residual; + if (cv::norm(step) > 0.25) + step *= 0.25 / cv::norm(step); + uv -= step; + if (!std::isfinite(cv::norm(uv))) + return false; + } + double theta = cv::norm(uv); + if (theta > 1.55 || cv::norm(distort(c, uv) - target) > 1e-6) + return false; + double s = theta > 1e-12 ? std::sin(theta) / theta : 1; + ray = {uv[0] * s, uv[1] * s, std::cos(theta)}; + cv::Point2d check; + return project(c, ray, check) && cv::norm(check - pixel) < 0.001; +} +bool +spread(const std::vector &pixels, const rift_s_camera_calibration &c) +{ + unsigned bins = 0; + double minx = 1e9, miny = 1e9, maxx = -1e9, maxy = -1e9; + for (auto p : pixels) { + int x = std::clamp(int(4 * p.x / c.roi.extent.w), 0, 3), + y = std::clamp(int(3 * p.y / c.roi.extent.h), 0, 2); + bins |= 1u << (4 * y + x); + minx = std::min(minx, p.x); + maxx = std::max(maxx, p.x); + miny = std::min(miny, p.y); + maxy = std::max(maxy, p.y); + } + unsigned occupied = 0; + for (; bins; bins >>= 1) + occupied += bins & 1; + return occupied >= 4 && maxx - minx > 0.25 * c.roi.extent.w && maxy - miny > 0.2 * c.roi.extent.h; +} + +// PnP supplies the robust seed. All subsequent fitting is on the four parameters +// allowed by gravity, using the calibrated fisheye residual rather than perspective pixels. +bool +refine_gravity(const rift_s_world_anchor_config &c, + unsigned camera, + const xrt_quat &attitude, + const std::vector &world, + const std::vector &pixels, + Transform &cam_from_world) +{ + Transform raw = transform({attitude, {0, 0, 0}}); + Transform imu_from_camera = transform(c.imu_from_camera[camera]); + Transform seed = cam_from_world.inverse() * imu_from_camera.inverse(); + cv::Matx33d delta = seed.r * raw.r.t(); + double yaw = std::atan2(delta(0, 2) - delta(2, 0), delta(0, 0) + delta(2, 2)); + cv::Vec4d parameters{yaw, seed.t[0], seed.t[1], seed.t[2]}; + auto camera_pose = [&](const cv::Vec4d &p) { + double s = std::sin(p[0]), co = std::cos(p[0]); + Transform imu{{co, 0, s, 0, 1, 0, -s, 0, co}, {p[1], p[2], p[3]}}; + imu.r = imu.r * raw.r; + return (imu * imu_from_camera).inverse(); + }; + for (unsigned iteration = 0; iteration < 15; ++iteration) { + cv::Matx44d normal = cv::Matx44d::zeros(); + cv::Vec4d gradient{0, 0, 0, 0}; + Transform current = camera_pose(parameters); + unsigned used = 0; + for (size_t i = 0; i < world.size(); ++i) { + cv::Vec3d point{world[i].x, world[i].y, world[i].z}; + cv::Point2d projected; + if (!project(c.camera[camera], current.apply(point), projected)) + continue; + cv::Vec2d residual{projected.x - pixels[i].x, projected.y - pixels[i].y}; + cv::Matx jacobian; + bool finite = true; + for (unsigned axis = 0; axis < 4; ++axis) { + cv::Vec4d perturbed = parameters; + perturbed[axis] += 1e-5; + cv::Point2d next; + if (!project(c.camera[camera], camera_pose(perturbed).apply(point), next)) { + finite = false; + break; + } + jacobian(0, axis) = (next.x - projected.x) / 1e-5; + jacobian(1, axis) = (next.y - projected.y) / 1e-5; + } + if (!finite) + continue; + double weight = std::min(1.0, MAX_ERROR_PX / std::max(cv::norm(residual), 1e-9)); + normal += weight * (jacobian.t() * jacobian); + gradient += weight * (jacobian.t() * residual); + ++used; + } + if (used < 30) + return false; + cv::Vec4d step; + if (!cv::solve(normal, gradient, step, cv::DECOMP_CHOLESKY) || !std::isfinite(cv::norm(step))) + return false; + double scale = std::max({1.0, std::abs(step[0]) / 0.05, + cv::norm(cv::Vec3d{step[1], step[2], step[3]}) / 0.2}); + parameters -= step / scale; + if (cv::norm(step) < 1e-7) + break; + } + cam_from_world = camera_pose(parameters); + return std::isfinite(cv::norm(cam_from_world.t)); +} +struct Point +{ + cv::Vec3d world; + std::array descriptors[2]; + uint8_t cameras[2] = {0, 1}; +}; +struct Keyframe +{ + Transform world_from_imu; + std::vector points; + int64_t last_refresh_ns = 0; +}; +struct Features +{ + std::vector keys; + cv::Mat descriptors; +}; +struct StereoWarp +{ + cv::Mat x, y; + bool initialized = false; +}; +void +build_warp(StereoWarp &warp, const rift_s_world_anchor_config &c, cv::Mat &rays) +{ + warp.initialized = true; + Transform cameras[2] = {transform(c.imu_from_camera[0]), transform(c.imu_from_camera[1])}; + if (angle_between(cameras[0], cameras[1]) < 3 * M_PI / 180) + return; + cv::Matx33d target_from_source = cameras[1].r.t() * cameras[0].r; + if (rays.empty()) { + rays.create(c.camera[0].roi.extent.h, c.camera[0].roi.extent.w, CV_64FC3); + for (int y = 0; y < rays.rows; ++y) + for (int x = 0; x < rays.cols; ++x) { + cv::Vec3d ray{0, 0, 0}; + unproject(c.camera[0], {double(x), double(y)}, ray); + rays.at(y, x) = ray; + } + } + warp.x.create(c.camera[0].roi.extent.h, c.camera[0].roi.extent.w, CV_32F); + warp.y.create(warp.x.size(), CV_32F); + for (int y = 0; y < warp.x.rows; ++y) + for (int x = 0; x < warp.x.cols; ++x) { + cv::Point2d pixel; + bool valid = project(c.camera[1], target_from_source * rays.at(y, x), pixel); + warp.x.at(y, x) = valid ? float(pixel.x) : -1; + warp.y.at(y, x) = valid ? float(pixel.y) : -1; + } +} +// Match patches at subpixel precision before triangulating. Independent ORB detections +// discard most stereo correspondences in a room and leave noisy integer-pixel depths. +// Backward tracking, descriptor agreement and calibrated reprojection validate each pair. +std::vector +stereo_matches(Features (&features)[2], + const cv::Mat (&images)[2], + const rift_s_world_anchor_config &config, + cv::ORB &orb, + const StereoWarp *warp = nullptr) +{ + Transform camera[2] = {transform(config.imu_from_camera[0]), transform(config.imu_from_camera[1])}; + cv::Mat target; + bool rectified = warp && !warp->x.empty(); + if (rectified) + cv::remap(images[1], target, warp->x, warp->y, cv::INTER_LINEAR, cv::BORDER_CONSTANT); + else + target = images[1]; + std::vector left, right, backward; + for (const auto &key : features[0].keys) { + left.push_back(key.pt); + cv::Vec3d ray; + cv::Point2d guess = key.pt; + if (unproject(config.camera[0], key.pt, ray)) { + cv::Vec3d p = camera[1].inverse().apply(camera[0].apply(ray * 2.0)); + if (rectified) + p = camera[0].r.t() * camera[1].r * p; + project(config.camera[rectified ? 0 : 1], p, guess); + } + right.push_back(guess); + } + if (left.empty()) + return {}; + std::vector forward_ok, backward_ok; + std::vector forward_error, backward_error; + cv::calcOpticalFlowPyrLK(images[0], target, left, right, forward_ok, forward_error, {21, 21}, 4, + {cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01}, + cv::OPTFLOW_USE_INITIAL_FLOW); + backward = left; + cv::calcOpticalFlowPyrLK(target, images[0], right, backward, backward_ok, backward_error, {21, 21}, 4, + {cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 30, 0.01}, + cv::OPTFLOW_USE_INITIAL_FLOW); + std::vector pyramid(orb.getNLevels()); + pyramid[0] = images[1]; + for (int level = 1; level < orb.getNLevels(); ++level) { + double scale = std::pow(orb.getScaleFactor(), level); + cv::resize(images[1], pyramid[level], + {cvRound(images[1].cols / scale), cvRound(images[1].rows / scale)}, 0, 0, + cv::INTER_LINEAR_EXACT); + } + Features tracked; + for (size_t i = 0; i < left.size(); ++i) { + if (!forward_ok[i] || !backward_ok[i] || forward_error[i] > 20 || + cv::norm(backward[i] - left[i]) > 0.75) + continue; + cv::KeyPoint key = features[0].keys[i]; + key.pt = right[i]; + if (rectified) { + cv::Vec3d ray; + cv::Point2d native; + if (!unproject(config.camera[0], right[i], ray) || + !project(config.camera[1], camera[1].r.t() * camera[0].r * ray, native)) + continue; + key.pt = native; + } + // ORB compute preserves a supplied orientation. Recompute it in the right + // patch rather than copy the left camera's intensity-centroid angle. + double scale = std::pow(orb.getScaleFactor(), key.octave); + const auto &level = pyramid[key.octave]; + int x = cvRound(key.pt.x / scale), y = cvRound(key.pt.y / scale); + if (x < 16 || y < 16 || x + 16 >= level.cols || y + 16 >= level.rows) + continue; + double mx = 0, my = 0; + for (int dy = -15; dy <= 15; ++dy) { + int radius = cvRound(std::sqrt(225 - dy * dy)); + for (int dx = -radius; dx <= radius; ++dx) { + double intensity = level.at(y + dy, x + dx); + mx += dx * intensity; + my += dy * intensity; + } + } + key.angle = float(std::atan2(my, mx) * 180 / M_PI); + if (key.angle < 0) + key.angle += 360; + key.class_id = int(i); + tracked.keys.push_back(key); + } + orb.compute(images[1], tracked.keys, tracked.descriptors); + std::vector out; + for (size_t i = 0; i < tracked.keys.size(); ++i) { + int source = tracked.keys[i].class_id; + int distance = 0; + for (unsigned word = 0; word < 4; ++word) { + uint64_t a, b; + std::memcpy(&a, features[0].descriptors.ptr(source) + word * 8, 8); + std::memcpy(&b, tracked.descriptors.ptr(int(i)) + word * 8, 8); + distance += __builtin_popcountll(a ^ b); + } + if (distance < 72) + out.emplace_back(source, int(i), float(distance)); + } + features[1] = std::move(tracked); + return out; +} + +// Explicit scalar encoding avoids padding-dependent camera hashes and caps map allocation before reads. +struct Bytes +{ + std::vector data; + size_t offset = 0; + bool ok = true; + void + put32(uint32_t v) + { + for (int i = 0; i < 4; ++i) + data.push_back(uint8_t(v >> (8 * i))); + } + uint32_t + get32() + { + if (offset + 4 > data.size()) { + ok = false; + return 0; + } + uint32_t v = 0; + for (int i = 0; i < 4; ++i) + v |= uint32_t(data[offset++]) << (8 * i); + return v; + } + void + put64(uint64_t v) + { + put32(uint32_t(v)); + put32(uint32_t(v >> 32)); + } + uint64_t + get64() + { + uint64_t a = get32(), b = get32(); + return a | (b << 32); + } + void + putfloat(float f) + { + uint32_t v; + std::memcpy(&v, &f, 4); + put32(v); + } + float + getfloat() + { + uint32_t v = get32(); + float f; + std::memcpy(&f, &v, 4); + return f; + } +}; +uint64_t +hash(const uint8_t *data, size_t size) +{ + uint64_t h = 14695981039346656037ULL; + for (size_t i = 0; i < size; ++i) { + h ^= data[i]; + h *= 1099511628211ULL; + } + return h; +} +bool +boundary_number(const cJSON *value, Bytes &out) +{ + if (!cJSON_IsNumber(value) || !std::isfinite(value->valuedouble) || std::abs(value->valuedouble) > 10000) + return false; + float v = float(value->valuedouble); + out.putfloat(v == 0 ? 0 : v); + return true; +} +bool +boundary_vector(const cJSON *value, unsigned count, Bytes &out) +{ + if (!cJSON_IsArray(value) || cJSON_GetArraySize(value) != int(count)) + return false; + for (unsigned i = 0; i < count; ++i) + if (!boundary_number(cJSON_GetArrayItem(value, int(i)), out)) + return false; + return true; +} +bool +boundary_hash(const rift_s_world_anchor_config &c, uint64_t &out, uint64_t *legacy = nullptr) +{ + out = 0; + if (!c.require_boundary) + return true; + if (!c.boundary_path[0]) + return false; + FILE *file = std::fopen(c.boundary_path, "rb"); + if (!file) + return false; + struct stat st{}; + bool ok = fstat(fileno(file), &st) == 0 && S_ISREG(st.st_mode) && st.st_size > 0 && st.st_size <= 1024 * 1024; + std::vector bytes(ok ? size_t(st.st_size) : 0); + if (ok) + ok = std::fread(bytes.data(), 1, bytes.size(), file) == bytes.size(); + std::fclose(file); + if (!ok) + return false; + if (legacy) + *legacy = hash(bytes.data(), bytes.size()); + bytes.push_back(0); + std::unique_ptr json(cJSON_Parse(reinterpret_cast(bytes.data())), + cJSON_Delete); + const auto *id = cJSON_GetObjectItemCaseSensitive(json.get(), "jsonid"); + if (!cJSON_IsString(id) || std::strcmp(id->valuestring, "chaperone_info") != 0) + return false; + const auto *universes = cJSON_GetObjectItemCaseSensitive(json.get(), "universes"); + const cJSON *room = nullptr; + // The SteamVR driver publishes universe 2. Unrelated universes and serialization metadata + // cannot change the binding between this world's landmarks and its standing frame. + for (int i = 0; i < cJSON_GetArraySize(universes); ++i) { + const auto *u = cJSON_GetArrayItem(universes, i); + const auto *universe = cJSON_GetObjectItemCaseSensitive(u, "universeID"); + if (cJSON_IsString(universe) && std::strcmp(universe->valuestring, "2") == 0) { + if (room) + return false; + room = u; + } + } + if (!room) + return false; + Bytes semantic; + semantic.put32(1); // Semantic boundary format. + semantic.put64(2); + const auto *standing = cJSON_GetObjectItemCaseSensitive(room, "standing"); + if (!boundary_vector(cJSON_GetObjectItemCaseSensitive(standing, "translation"), 3, semantic) || + !boundary_number(cJSON_GetObjectItemCaseSensitive(standing, "yaw"), semantic) || + !boundary_vector(cJSON_GetObjectItemCaseSensitive(room, "play_area"), 2, semantic)) + return false; + const auto *bounds = cJSON_GetObjectItemCaseSensitive(room, "collision_bounds"); + int count = cJSON_GetArraySize(bounds); + if (!cJSON_IsArray(bounds) || count <= 0 || count > 2048) + return false; + semantic.put32(uint32_t(count)); + for (int i = 0; i < count; ++i) { + const auto *quad = cJSON_GetArrayItem(bounds, i); + if (!cJSON_IsArray(quad) || cJSON_GetArraySize(quad) != 4) + return false; + for (int j = 0; j < 4; ++j) + if (!boundary_vector(cJSON_GetArrayItem(quad, j), 3, semantic)) + return false; + } + out = hash(semantic.data.data(), semantic.data.size()); + return true; +} +void +resolve_boundary_path(rift_s_world_anchor_config &c) +{ + if (!c.require_boundary || c.boundary_path[0]) + return; + const char *home = std::getenv("HOME"), *config = std::getenv("XDG_CONFIG_HOME"); + if (!home) + return; + std::string registry = + (config ? std::string(config) : std::string(home) + "/.config") + "/openvr/openvrpaths.vrpath"; + FILE *file = std::fopen(registry.c_str(), "rb"); + if (!file) + return; + char bytes[8192]{}; + size_t count = std::fread(bytes, 1, sizeof(bytes) - 1, file); + bool complete = std::feof(file); + std::fclose(file); + if (!complete || !count) + return; + cJSON *json = cJSON_Parse(bytes); + const cJSON *entry = cJSON_GetArrayItem(cJSON_GetObjectItemCaseSensitive(json, "config"), 0); + if (cJSON_IsString(entry) && entry->valuestring && entry->valuestring[0]) { + std::string path = std::string(entry->valuestring) + "/chaperone_info.vrchap"; + if (path.size() < sizeof(c.boundary_path)) + std::memcpy(c.boundary_path, path.c_str(), path.size() + 1); + } + cJSON_Delete(json); +} +uint64_t +calibration_hash(const rift_s_world_anchor_config &c, unsigned count = 0) +{ + Bytes b; + for (unsigned e = 0; e < (count ? count : camera_count(c)); ++e) { + const auto &cam = c.camera[e]; + b.put32(cam.roi.extent.w); + b.put32(cam.roi.extent.h); + b.putfloat(cam.projection.fx); + b.putfloat(cam.projection.fy); + b.putfloat(cam.projection.cx); + b.putfloat(cam.projection.cy); + for (float k : cam.distortion.k) + b.putfloat(k); + b.putfloat(cam.distortion.p1); + b.putfloat(cam.distortion.p2); + const auto &p = c.imu_from_camera[e]; + b.putfloat(p.orientation.x); + b.putfloat(p.orientation.y); + b.putfloat(p.orientation.z); + b.putfloat(p.orientation.w); + b.putfloat(p.position.x); + b.putfloat(p.position.y); + b.putfloat(p.position.z); + } + return hash(b.data.data(), b.data.size()); +} +} // namespace + +struct rift_s_world_anchor +{ + rift_s_world_anchor_config config; + std::string path; + uint64_t bound_boundary = 0; + bool boundary_bound = false; + bool storage_rejected = false; + std::vector frames; + bool index_dirty = true; + std::vector indexed_world; + cv::Mat indexed_descriptors[2]; + cv::Ptr index[2]; + StereoWarp warps[RIFT_S_WORLD_MAX_CAMERAS][RIFT_S_WORLD_MAX_CAMERAS]; + cv::Mat camera_rays[RIFT_S_WORLD_MAX_CAMERAS]; + // Trusted capture reference for temporal depth in room directions without rig overlap. + bool reference_valid = false; + Transform reference_pose; + int64_t reference_ns = 0; + cv::Mat reference_images[RIFT_S_WORLD_MAX_CAMERAS]; + Features reference_features[RIFT_S_WORLD_MAX_CAMERAS]; + std::string record_dir; + unsigned recorded_pairs = 0; + cv::Ptr orb = cv::ORB::create(1400, 1.2f, 8, 19, 0, 2, cv::ORB::HARRIS_SCORE, 31, 12); +}; + +bool +rift_s_world_anchor_project(const rift_s_camera_calibration *c, const xrt_vec3 *p, xrt_vec2 *out) +{ + if (!c || !p || !out) + return false; + cv::Point2d q; + if (!project(*c, {p->x, p->y, p->z}, q)) + return false; + *out = {float(q.x), float(q.y)}; + return true; +} +bool +rift_s_world_anchor_unproject(const rift_s_camera_calibration *c, const xrt_vec2 *p, xrt_vec3 *out) +{ + if (!c || !p || !out) + return false; + cv::Vec3d q; + if (!unproject(*c, {p->x, p->y}, q)) + return false; + *out = {float(q[0]), float(q[1]), float(q[2])}; + return true; +} +bool +rift_s_world_anchor_solve(const rift_s_world_anchor_config *config, + const rift_s_world_anchor_observation *obs[2], + const uint32_t counts[2], + rift_s_world_anchor_result *result) +{ + return rift_s_world_anchor_solve_rig(config, obs, counts, 2, result); +} +bool +rift_s_world_anchor_solve_rig(const rift_s_world_anchor_config *config, + const rift_s_world_anchor_observation *const obs[], + const uint32_t counts[], + unsigned count, + rift_s_world_anchor_result *result) +{ + return rift_s_world_anchor_solve_rig_gravity(config, obs, counts, count, nullptr, result); +} +bool +rift_s_world_anchor_solve_rig_gravity(const rift_s_world_anchor_config *config, + const rift_s_world_anchor_observation *const obs[], + const uint32_t counts[], + unsigned count, + const xrt_quat *attitude, + rift_s_world_anchor_result *result) +{ + if (!config || !obs || !counts || !result) + return false; + *result = {}; + if (!calibration_valid(*config) || count < 2 || count > camera_count(*config)) + return false; + if (attitude && !finite_pose({*attitude, {0, 0, 0}})) + return false; + try { + Transform estimates[RIFT_S_WORLD_MAX_CAMERAS]; + bool usable[RIFT_S_WORLD_MAX_CAMERAS]{}; + result->rejection = "matches"; + result->matches[0] = counts[0]; + result->matches[1] = counts[1]; + for (unsigned eye = 0; eye < count; ++eye) { + result->matches[eye] = counts[eye]; + if (!obs[eye] || counts[eye] < MIN_POINTS) + continue; + if (counts[eye] > 4096) + return false; + usable[eye] = [&]() -> bool { + const auto &c = config->camera[eye]; + std::vector world; + std::vector normalized, pixels; + for (uint32_t i = 0; i < counts[eye]; ++i) { + const auto &o = obs[eye][i]; + cv::Vec3d ray; + if (!unproject(c, {o.pixel.x, o.pixel.y}, ray) || + !std::isfinite(o.world_point.x) || !std::isfinite(o.world_point.y) || + !std::isfinite(o.world_point.z)) + return false; + world.emplace_back(o.world_point.x, o.world_point.y, o.world_point.z); + normalized.emplace_back(ray[0] / ray[2], ray[1] / ray[2]); + pixels.emplace_back(o.pixel.x, o.pixel.y); + } + cv::Vec3d rv, tv; + cv::Mat inliers; + double threshold = MAX_ERROR_PX / std::max(c.projection.fx, c.projection.fy); + result->rejection = "pnp"; + bool solved = + cv::solvePnPRansac(world, normalized, cv::Matx33d::eye(), cv::noArray(), rv, tv, + false, 150, float(threshold), 0.999, inliers, cv::SOLVEPNP_EPNP); + result->inliers[eye] = uint32_t(inliers.rows); + result->rejection = "inliers"; + if (!solved || inliers.rows < 30 || inliers.rows < 0.55 * counts[eye]) + return false; + std::vector iw; + std::vector in, ip; + for (int j = 0; j < inliers.rows; ++j) { + int i = inliers.at(j); + iw.push_back(world[i]); + in.push_back(normalized[i]); + ip.push_back(pixels[i]); + } + Transform cam_from_world; + // RANSAC runs in the perspective plane, whose scale varies across a fisheye + // image. Rescore in native pixels and refit the remaining support rather + // than reject a whole room match for one perspective-plane inlier. + for (unsigned pass = 0; pass < 4; ++pass) { + cv::Rodrigues(rv, cam_from_world.r); + cam_from_world.t = tv; + if (attitude) { + if (!refine_gravity(*config, eye, *attitude, iw, ip, cam_from_world)) + return false; + cv::Rodrigues(cam_from_world.r, rv); + tv = cam_from_world.t; + } else { + cv::solvePnPRefineLM(iw, in, cv::Matx33d::eye(), cv::noArray(), rv, tv); + cv::Rodrigues(rv, cam_from_world.r); + cam_from_world.t = tv; + } + if (!std::isfinite(cv::norm(tv)) || !std::isfinite(cv::norm(rv))) + return false; + std::vector kept_world; + std::vector kept_normalized, kept_pixels; + float max_error = 0; + for (size_t i = 0; i < iw.size(); ++i) { + cv::Point2d projected; + if (!project(c, cam_from_world.apply({iw[i].x, iw[i].y, iw[i].z}), + projected)) + continue; + double error = cv::norm(projected - ip[i]); + if (error > MAX_ERROR_PX) + continue; + kept_world.push_back(iw[i]); + kept_normalized.push_back(in[i]); + kept_pixels.push_back(ip[i]); + max_error = std::max(max_error, float(error)); + } + result->inliers[eye] = uint32_t(kept_world.size()); + result->rejection = "native_inliers"; + if (kept_world.size() < 30 || kept_world.size() < 0.55 * counts[eye]) + return false; + result->rejection = "coverage"; + if (!spread(kept_pixels, c)) + return false; + if (kept_world.size() == iw.size()) { + result->max_reprojection_px = + std::max(result->max_reprojection_px, max_error); + break; + } + result->rejection = "refinement"; + if (pass == 3) + return false; + iw = std::move(kept_world); + in = std::move(kept_normalized); + ip = std::move(kept_pixels); + } + estimates[eye] = + cam_from_world.inverse() * transform(config->imu_from_camera[eye]).inverse(); + return true; + }(); + } + unsigned best = count, support = 0; + for (unsigned i = 0; i < count; ++i) { + if (!usable[i]) + continue; + unsigned agreeing = 0; + for (unsigned j = 0; j < count; ++j) + if (usable[j] && cv::norm(estimates[i].t - estimates[j].t) <= 0.06 && + angle_between(estimates[i], estimates[j]) <= 3 * M_PI / 180) + ++agreeing; + if (agreeing > support || + (agreeing == support && best < count && result->inliers[i] > result->inliers[best])) { + best = i; + support = agreeing; + } + } + if (support < 2) { + if (support) + result->rejection = "camera_disagreement"; + return false; + } + result->rejection = "camera_disagreement"; + // Two independent confident clusters would leave the room identity ambiguous. + for (unsigned i = 0; i < count; ++i) + for (unsigned j = i + 1; j < count; ++j) { + if (!usable[i] || !usable[j]) + continue; + if (cv::norm(estimates[i].t - estimates[j].t) <= 0.06 && + angle_between(estimates[i], estimates[j]) <= 3 * M_PI / 180 && + (cv::norm(estimates[i].t - estimates[best].t) > 0.12 || + angle_between(estimates[i], estimates[best]) > 6 * M_PI / 180)) + return false; + } + result->supporting_cameras = support; + result->world_from_imu = pose(estimates[best]); + result->rejection = nullptr; + result->relocalized = finite_pose(result->world_from_imu); + return result->relocalized; + } catch (const cv::Exception &) { + return false; + } +} + +namespace { +void +put_pose(Bytes &b, const Transform &t) +{ + xrt_pose p = pose(t); + b.putfloat(p.orientation.x); + b.putfloat(p.orientation.y); + b.putfloat(p.orientation.z); + b.putfloat(p.orientation.w); + b.putfloat(p.position.x); + b.putfloat(p.position.y); + b.putfloat(p.position.z); +} +xrt_pose +get_pose(Bytes &b) +{ + xrt_pose p; + p.orientation.x = b.getfloat(); + p.orientation.y = b.getfloat(); + p.orientation.z = b.getfloat(); + p.orientation.w = b.getfloat(); + p.position.x = b.getfloat(); + p.position.y = b.getfloat(); + p.position.z = b.getfloat(); + return p; +} +bool +load(rift_s_world_anchor &a) +{ + FILE *file = std::fopen(a.path.c_str(), "rb"); + if (!file) { + U_LOG_I("World map absent errno=%d", errno); + return false; + } + struct stat st{}; + if (fstat(fileno(file), &st) != 0 || !S_ISREG(st.st_mode) || st.st_size < 28 || st.st_size > 8 * 1024 * 1024) { + std::fclose(file); + return false; + } + Bytes b; + b.data.resize(size_t(st.st_size)); + bool ok = std::fread(b.data.data(), 1, b.data.size(), file) == b.data.size(); + std::fclose(file); + if (!ok) + return false; + uint32_t magic = b.get32(), version = b.get32(); + if (magic != 0x41575352 || (version != 2 && version != 3 && version != 4)) { + U_LOG_I("World map rejected: format/version"); + return false; + } + if (b.get64() != calibration_hash(a.config, version == 2 ? 2 : 0)) { + U_LOG_I("World map rejected: calibration mismatch"); + return false; + } + uint64_t current_boundary = 0, legacy_boundary = 0; + uint64_t saved_boundary = b.get64(); + if (!boundary_hash(a.config, current_boundary, &legacy_boundary) || + saved_boundary != (version < 4 ? legacy_boundary : current_boundary)) { + U_LOG_I("World map rejected: committed boundary mismatch"); + return false; + } + uint32_t frames = b.get32(); + if (!frames || frames > MAX_KEYFRAMES) + return false; + std::vector loaded; + for (uint32_t i = 0; i < frames; ++i) { + xrt_pose p = get_pose(b); + if (!b.ok || !finite_pose(p)) + return false; + Keyframe k{transform(p), {}}; + uint32_t count = b.get32(); + if (count < MIN_POINTS || count > MAX_POINTS) + return false; + for (uint32_t j = 0; j < count; ++j) { + Point point; + for (int d = 0; d < 3; ++d) + point.world[d] = b.getfloat(); + if (!std::isfinite(cv::norm(point.world)) || cv::norm(point.world - k.world_from_imu.t) > 10 || + b.offset + 64 > b.data.size()) + return false; + for (auto &descriptor : point.descriptors) + for (auto &v : descriptor) + v = b.data[b.offset++]; + if (version >= 3) { + uint32_t pair = b.get32(); + point.cameras[0] = pair & 0xff; + point.cameras[1] = (pair >> 8) & 0xff; + if (point.cameras[0] >= camera_count(a.config) || + point.cameras[1] >= camera_count(a.config)) + return false; + } + k.points.push_back(point); + } + loaded.push_back(std::move(k)); + } + size_t end = b.offset; + uint64_t checksum = b.get64(); + if (!b.ok || b.offset != b.data.size() || checksum != hash(b.data.data(), end)) + return false; + a.frames = std::move(loaded); + a.bound_boundary = current_boundary; + a.boundary_bound = true; + a.index_dirty = true; + U_LOG_I("World map loaded keyframes=%zu bytes=%zu", a.frames.size(), b.data.size()); + return true; +} +} // namespace +rift_s_world_anchor * +rift_s_world_anchor_create(const rift_s_world_anchor_config *config, const char *path) +{ + if (!config || !calibration_valid(*config)) + return nullptr; + try { + auto a = std::make_unique(); + a->config = *config; + resolve_boundary_path(a->config); + a->path = path ? path : ""; + const char *record = std::getenv("RIFT_S_WORLD_RECORD_DIR"), + *bench = std::getenv("MONADO_STEAMVR_BENCH"); + if (record && record[0] && bench && (std::strcmp(bench, "1") == 0 || std::strcmp(bench, "true") == 0)) { + if (mkdir(record, 0700) == 0 || errno == EEXIST) { + a->record_dir = record; + FILE *file = std::fopen((a->record_dir + "/calibration.bin").c_str(), "wb"); + if (file) { + std::fwrite(&a->config, sizeof(a->config), 1, file); + std::fclose(file); + } + } + } + if (!a->path.empty()) + a->storage_rejected = !load(*a) && access(a->path.c_str(), F_OK) == 0; + return a.release(); + } catch (const std::exception &) { + return nullptr; + } +} +void +rift_s_world_anchor_record_frames(rift_s_world_anchor *a, + const xrt_frame *left, + const xrt_frame *right, + const xrt_pose *raw) +{ + const xrt_frame *frames[2] = {left, right}; + rift_s_world_anchor_record_rig_frames(a, frames, 2, raw); +} +void +rift_s_world_anchor_record_rig_frames(rift_s_world_anchor *a, + const xrt_frame *const frames[], + unsigned count, + const xrt_pose *raw) +{ + if (!a || a->record_dir.empty() || a->recorded_pairs >= 600 || !frames || count < 2 || + count > RIFT_S_WORLD_MAX_CAMERAS || !frames[0]) + return; + for (unsigned eye = 0; eye < count; ++eye) { + const auto *f = frames[eye]; + if (!f || f->timestamp != frames[0]->timestamp || !f->data || f->format != XRT_FORMAT_L8 || + f->stride < f->width || !f->width || !f->height || f->size < (f->height - 1) * f->stride + f->width) + return; + } + unsigned index = a->recorded_pairs++; + for (unsigned eye = 0; eye < count; ++eye) { + char name[64]; + std::snprintf(name, sizeof(name), "/%04u-%u.pgm", index, eye); + FILE *file = std::fopen((a->record_dir + name).c_str(), "wb"); + if (!file) + return; + const auto *f = frames[eye]; + std::fprintf(file, "P5\n%u %u\n255\n", f->width, f->height); + for (uint32_t row = 0; row < f->height; ++row) + std::fwrite(f->data + row * f->stride, 1, f->width, file); + std::fclose(file); + } + FILE *file = std::fopen((a->record_dir + "/poses.csv").c_str(), "a"); + if (!file) + return; + xrt_pose p = raw ? *raw : xrt_pose{{0, 0, 0, 1}, {0, 0, 0}}; + std::fprintf(file, "%u,%lld,%d,%.9g,%.9g,%.9g,%.9g,%.9g,%.9g,%.9g\n", index, (long long)frames[0]->timestamp, + raw != nullptr, p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w, p.position.x, + p.position.y, p.position.z); + std::fclose(file); +} + +void +rift_s_world_anchor_destroy(rift_s_world_anchor *a) +{ + delete a; +} +bool +rift_s_world_anchor_has_map(const rift_s_world_anchor *a) +{ + return a && !a->frames.empty(); +} +bool +rift_s_world_anchor_has_boundary(const rift_s_world_anchor *a) +{ + uint64_t binding; + return a && a->config.require_boundary && boundary_hash(a->config, binding); +} +void +rift_s_world_anchor_clear(rift_s_world_anchor *a) +{ + if (!a) + return; + a->frames.clear(); + a->boundary_bound = false; + a->storage_rejected = false; + a->reference_valid = false; + a->index_dirty = true; +} +bool +rift_s_world_anchor_reload(rift_s_world_anchor *a) +{ + if (!a) + return false; + a->frames.clear(); + a->boundary_bound = false; + a->reference_valid = false; + a->index_dirty = true; + bool loaded = !a->path.empty() && load(*a); + a->storage_rejected = !loaded && !a->path.empty() && access(a->path.c_str(), F_OK) == 0; + return loaded; +} +bool +rift_s_world_anchor_save(rift_s_world_anchor *a) +{ + if (!a || a->path.empty()) + return false; + if (a->storage_rejected) { + U_LOG_W("World map save rejected: existing map was not loaded; explicit room setup is required"); + return false; + } + auto parent_slash = a->path.find_last_of('/'); + if (parent_slash != std::string::npos) { + auto parent = a->path.substr(0, parent_slash); + if (mkdir(parent.c_str(), 0700) != 0 && errno != EEXIST) + return false; + } + uint64_t committed_boundary; + if (!boundary_hash(a->config, committed_boundary)) + return false; + if (a->boundary_bound && a->bound_boundary != committed_boundary) { + U_LOG_W("World map save rejected: standing frame or boundary changed without room setup"); + return false; + } + Bytes b; + b.put32(0x41575352); + b.put32(4); + b.put64(calibration_hash(a->config)); + b.put64(committed_boundary); + b.put32(uint32_t(a->frames.size())); + for (const auto &k : a->frames) { + put_pose(b, k.world_from_imu); + b.put32(uint32_t(k.points.size())); + for (const auto &p : k.points) { + for (double v : p.world.val) + b.putfloat(float(v)); + for (const auto &d : p.descriptors) + b.data.insert(b.data.end(), d.begin(), d.end()); + b.put32(p.cameras[0] | (uint32_t(p.cameras[1]) << 8)); + } + } + b.put64(hash(b.data.data(), b.data.size())); + std::string temporary = a->path + ".tmp-XXXXXX"; + std::vector name(temporary.begin(), temporary.end()); + name.push_back(0); + int fd = mkstemp(name.data()); + if (fd < 0) + return false; + bool ok = fchmod(fd, 0600) == 0; + size_t offset = 0; + while (ok && offset < b.data.size()) { + ssize_t n = write(fd, b.data.data() + offset, b.data.size() - offset); + if (n <= 0) + ok = false; + else + offset += size_t(n); + } + if (ok && fsync(fd) != 0) + ok = false; + if (close(fd) != 0) + ok = false; + uint64_t latest_boundary; + if (ok && (!boundary_hash(a->config, latest_boundary) || latest_boundary != committed_boundary)) + ok = false; + if (ok && rename(name.data(), a->path.c_str()) != 0) + ok = false; + if (!ok) + unlink(name.data()); + if (ok) { + auto slash = a->path.find_last_of('/'); + std::string dir = slash == std::string::npos ? "." : a->path.substr(0, slash); + int d = open(dir.c_str(), O_RDONLY | O_DIRECTORY); + if (d < 0) + ok = false; + else { + if (fsync(d) != 0) + ok = false; + close(d); + } + } + if (ok) { + a->bound_boundary = committed_boundary; + a->boundary_bound = true; + } + return ok; +} +namespace { +void +build_index(rift_s_world_anchor &a) +{ + if (!a.index_dirty) + return; + a.indexed_world.clear(); + for (const auto &k : a.frames) + for (const auto &point : k.points) + a.indexed_world.push_back(point.world); + for (unsigned bank = 0; bank < 2; ++bank) { + a.indexed_descriptors[bank].create(int(a.indexed_world.size()), 32, CV_8U); + unsigned row = 0; + for (const auto &k : a.frames) + for (const auto &point : k.points) + std::memcpy(a.indexed_descriptors[bank].ptr(int(row++)), point.descriptors[bank].data(), + 32); + if (a.indexed_world.size() > 2000) + a.index[bank] = + cv::makePtr(cv::makePtr(12, 20, 2)); + else + a.index[bank] = cv::makePtr(cv::NORM_HAMMING); + a.index[bank]->add(std::vector{a.indexed_descriptors[bank]}); + a.index[bank]->train(); + } + a.index_dirty = false; +} +std::vector +room_matches(rift_s_world_anchor &a, const Features &f) +{ + std::vector candidates; + if (f.descriptors.rows < 2 || a.indexed_world.size() < 4) + return {}; + for (unsigned bank = 0; bank < 2; ++bank) { + std::vector> nearest; + a.index[bank]->knnMatch(f.descriptors, nearest, 4); + for (const auto &list : nearest) { + if (list.empty() || list[0].distance >= 64) + continue; + const auto &best = list[0]; + float second = 256; + // Repeated observations of one physical landmark are not competing identities. + for (size_t i = 1; i < list.size(); ++i) + if (cv::norm(a.indexed_world[list[i].trainIdx] - a.indexed_world[best.trainIdx]) > + 0.03) { + second = list[i].distance; + break; + } + if (best.distance < 0.75f * second) + candidates.push_back(best); + } + } + std::sort(candidates.begin(), candidates.end(), + [](const auto &a, const auto &b) { return a.distance < b.distance; }); + std::vector used_feature(f.keys.size()), used_point(a.indexed_world.size()); + std::vector out; + for (const auto &m : candidates) { + if (used_feature[m.queryIdx] || used_point[m.trainIdx]) + continue; + used_feature[m.queryIdx] = used_point[m.trainIdx] = true; + const auto &w = a.indexed_world[m.trainIdx]; + const auto &p = f.keys[m.queryIdx].pt; + out.push_back({{float(w[0]), float(w[1]), float(w[2])}, {p.x, p.y}}); + } + return out; +} +} // namespace +bool +rift_s_world_anchor_process(rift_s_world_anchor *a, + const xrt_frame *left, + const xrt_frame *right, + const xrt_pose *trusted, + rift_s_world_anchor_result *result) +{ + const xrt_frame *frames[2] = {left, right}; + return rift_s_world_anchor_process_rig(a, frames, 2, trusted, result); +} +bool +rift_s_world_anchor_process_rig(rift_s_world_anchor *a, + const xrt_frame *const frames[], + unsigned count, + const xrt_pose *trusted, + rift_s_world_anchor_result *result) +{ + return rift_s_world_anchor_process_rig_gravity(a, frames, count, trusted, nullptr, result); +} +bool +rift_s_world_anchor_process_rig_gravity(rift_s_world_anchor *a, + const xrt_frame *const frames[], + unsigned count, + const xrt_pose *trusted, + const xrt_quat *attitude, + rift_s_world_anchor_result *result) +{ + if (!a || !frames || !result || count < 2 || count > camera_count(a->config)) + return false; + *result = {}; + result->keyframes = uint32_t(a->frames.size()); + if (!frames[0] || frames[0]->timestamp <= 0) + return false; + for (unsigned e = 0; e < count; ++e) { + const auto *f = frames[e]; + const auto &c = a->config.camera[e]; + if (!f || f->timestamp != frames[0]->timestamp || f->format != XRT_FORMAT_L8 || !f->data || + f->width != uint32_t(c.roi.extent.w) || f->height != uint32_t(c.roi.extent.h) || + f->stride < f->width || f->size < (f->height - 1) * f->stride + f->width) { + result->rejection = "frame"; + return false; + } + } + try { + size_t replace = a->frames.size(); + if (trusted) { + if (!finite_pose(*trusted)) + return false; + Transform world_from_imu = transform(*trusted); + for (size_t i = 0; i < a->frames.size(); ++i) { + auto &k = a->frames[i]; + if (cv::norm(world_from_imu.t - k.world_from_imu.t) < 0.2 && + angle_between(world_from_imu, k.world_from_imu) < 15 * M_PI / 180) { + if (k.points.size() >= 3 * MAX_POINTS / 4 || + (k.last_refresh_ns > 0 && frames[0]->timestamp >= k.last_refresh_ns && + frames[0]->timestamp - k.last_refresh_ns < 2000000000LL)) + return false; + k.last_refresh_ns = frames[0]->timestamp; + replace = i; + break; + } + } + // Evict the most redundant viewpoint rather than erase a unique room direction. + if (replace == a->frames.size() && a->frames.size() >= MAX_KEYFRAMES) { + double redundancy = 1e9; + for (size_t i = 0; i < a->frames.size(); ++i) { + double nearest = 1e9; + for (size_t j = 0; j < a->frames.size(); ++j) + if (i != j) + nearest = std::min( + nearest, cv::norm(a->frames[i].world_from_imu.t - + a->frames[j].world_from_imu.t) + + angle_between(a->frames[i].world_from_imu, + a->frames[j].world_from_imu)); + if (nearest < redundancy) { + redundancy = nearest; + replace = i; + } + } + } + } + Features features[RIFT_S_WORLD_MAX_CAMERAS]; + cv::Mat images[RIFT_S_WORLD_MAX_CAMERAS]; + for (unsigned e = 0; e < count; ++e) { + images[e] = cv::Mat(int(frames[e]->height), int(frames[e]->width), CV_8UC1, frames[e]->data, + frames[e]->stride); + a->orb->detectAndCompute(images[e], cv::noArray(), features[e].keys, features[e].descriptors); + result->features[e] = uint32_t(features[e].descriptors.rows); + } + if (trusted) { + Transform world_from_imu = transform(*trusted); + if (attitude && !a->frames.empty()) { + build_index(*a); + std::vector matched[RIFT_S_WORLD_MAX_CAMERAS]; + const rift_s_world_anchor_observation *obs[RIFT_S_WORLD_MAX_CAMERAS]{}; + uint32_t counts[RIFT_S_WORLD_MAX_CAMERAS]{}; + unsigned overlap = 0; + for (unsigned e = 0; e < count; ++e) { + matched[e] = room_matches(*a, features[e]); + obs[e] = matched[e].data(); + counts[e] = uint32_t(matched[e].size()); + overlap += counts[e] >= MIN_POINTS; + } + // A novel room direction can extend the map. Overlapping views must agree + // with its existing geometry before automatic updates can replace landmarks. + if (overlap >= 2) { + rift_s_world_anchor_result checked{}; + if (!rift_s_world_anchor_solve_rig_gravity(&a->config, obs, counts, count, + attitude, &checked) || + cv::norm(transform(checked.world_from_imu).t - world_from_imu.t) > 0.08 || + angle_between(transform(checked.world_from_imu), world_from_imu) > 5 * M_PI / 180) { + result->rejection = "map_update_disagreement"; + return false; + } + } + } + Keyframe keyframe{world_from_imu, {}, frames[0]->timestamp}; + unsigned bins[RIFT_S_WORLD_MAX_CAMERAS][12]{}, source_points[RIFT_S_WORLD_MAX_CAMERAS]{}; + std::vector used[RIFT_S_WORLD_MAX_CAMERAS]; + std::vector coverage[RIFT_S_WORLD_MAX_CAMERAS]; + for (unsigned e = 0; e < count; ++e) + used[e].resize(features[e].keys.size()); + // Front stereo, side/front overlaps, and the top camera supply room directions. + std::vector> pairs = {{0, 1}, {2, 0}, {3, 1}, {4, 0}, {4, 1}, + {2, 4}, {3, 4}, {0, 2}, {1, 3}, {2, 3}}; + double movement = a->reference_valid ? cv::norm(world_from_imu.t - a->reference_pose.t) : 0; + bool temporal = a->reference_valid && frames[0]->timestamp > a->reference_ns && + frames[0]->timestamp - a->reference_ns <= 2000000000LL && movement >= 0.05 && + movement <= 0.65 && + angle_between(world_from_imu, a->reference_pose) < 45 * M_PI / 180; + if (temporal) + for (unsigned e = 0; e < count; ++e) + pairs.push_back({e, e}); + for (const auto &pair : pairs) { + unsigned first = pair[0], second = pair[1]; + bool previous_capture = first == second; + if (first >= count || second >= count || + features[first].descriptors.rows < int(MIN_POINTS) || + features[second].descriptors.rows < int(MIN_POINTS)) + continue; + rift_s_world_anchor_config stereo_config = a->config; + stereo_config.camera_count = 2; + stereo_config.camera[0] = a->config.camera[first]; + stereo_config.camera[1] = a->config.camera[second]; + stereo_config.imu_from_camera[0] = a->config.imu_from_camera[first]; + stereo_config.imu_from_camera[1] = a->config.imu_from_camera[second]; + if (previous_capture) + stereo_config.imu_from_camera[1] = + pose(world_from_imu.inverse() * a->reference_pose * + transform(a->config.imu_from_camera[second])); + Transform cameras[2] = {transform(stereo_config.imu_from_camera[0]), + transform(stereo_config.imu_from_camera[1])}; + if (cv::norm(cameras[1].t - cameras[0].t) < 0.025) + continue; + StereoWarp temporal_warp; + auto &warp = previous_capture ? temporal_warp : a->warps[first][second]; + if (!warp.initialized) + build_warp(warp, stereo_config, a->camera_rays[first]); + Features pair_features[2] = {features[first], previous_capture + ? a->reference_features[second] + : features[second]}; + cv::Mat pair_images[2] = {images[first], previous_capture ? a->reference_images[second] + : images[second]}; + auto stereo = stereo_matches(pair_features, pair_images, stereo_config, *a->orb, &warp); + result->stereo_matches += uint32_t(stereo.size()); + for (const auto &m : stereo) { + cv::Point2d pixels[2] = {pair_features[0].keys[m.queryIdx].pt, + pair_features[1].keys[m.trainIdx].pt}; + int bin = std::clamp(int(4 * pixels[0].x / frames[first]->width), 0, 3) + + 4 * std::clamp(int(3 * pixels[0].y / frames[first]->height), 0, 2); + if (used[first][m.queryIdx] || bins[first][bin] >= MAX_POINTS / 24 || + source_points[first] >= MAX_POINTS / (count == 2 ? 1 : count)) + continue; + cv::Vec3d rays[2]; + if (!unproject(stereo_config.camera[0], pixels[0], rays[0]) || + !unproject(stereo_config.camera[1], pixels[1], rays[1])) + continue; + cv::Vec3d d0 = cameras[0].r * rays[0], d1 = cameras[1].r * rays[1], + baseline = cameras[1].t - cameras[0].t; + double dot = d0.dot(d1), denom = 1 - dot * dot; + if (denom < 0.000225) + continue; + double depth0 = (d0.dot(baseline) - dot * d1.dot(baseline)) / denom; + double depth1 = (dot * d0.dot(baseline) - d1.dot(baseline)) / denom; + if (depth0 < 0.25 || depth0 > 8 || depth1 < 0.25 || depth1 > 8) + continue; + cv::Vec3d point = + (cameras[0].t + depth0 * d0 + cameras[1].t + depth1 * d1) * 0.5; + bool good = true; + for (unsigned eye = 0; eye < 2; ++eye) { + cv::Point2d projected; + if (!project(stereo_config.camera[eye], + cameras[eye].inverse().apply(point), projected) || + cv::norm(projected - pixels[eye]) > 1.0) + good = false; + } + if (!good) + continue; + Point p; + p.world = world_from_imu.apply(point); + p.cameras[0] = first; + p.cameras[1] = second; + std::memcpy(p.descriptors[0].data(), + pair_features[0].descriptors.ptr(m.queryIdx), 32); + std::memcpy(p.descriptors[1].data(), + pair_features[1].descriptors.ptr(m.trainIdx), 32); + keyframe.points.push_back(p); + used[first][m.queryIdx] = true; + ++source_points[first]; + coverage[first].push_back(pixels[0]); + if (!previous_capture) + coverage[second].push_back(pixels[1]); + ++bins[first][bin]; + ++result->camera_points[first]; + if (!previous_capture) + ++result->camera_points[second]; + if (keyframe.points.size() >= MAX_POINTS) + break; + } + if (keyframe.points.size() >= MAX_POINTS) + break; + } + // Keep enough baseline for temporal depth, but never bridge a stale pose interval. + if (!a->reference_valid || movement >= 0.08 || + frames[0]->timestamp - a->reference_ns > 2000000000LL || + angle_between(world_from_imu, a->reference_pose) > 20 * M_PI / 180) { + a->reference_valid = true; + a->reference_pose = world_from_imu; + a->reference_ns = frames[0]->timestamp; + for (unsigned e = 0; e < count; ++e) { + a->reference_images[e] = images[e].clone(); + a->reference_features[e] = features[e]; + } + } + result->triangulated = uint32_t(keyframe.points.size()); + unsigned supported = 0; + for (unsigned e = 0; e < count; ++e) + supported += + result->camera_points[e] >= MIN_POINTS && spread(coverage[e], a->config.camera[e]); + if (keyframe.points.size() < MIN_POINTS || supported < 2) { + result->rejection = "triangulation"; + return false; + } + if (replace < a->frames.size()) { + if (cv::norm(a->frames[replace].world_from_imu.t - world_from_imu.t) < 0.2 && + angle_between(a->frames[replace].world_from_imu, world_from_imu) < + 15 * M_PI / 180 && + a->frames[replace].points.size() < MAX_POINTS && + keyframe.points.size() < a->frames[replace].points.size() + 25) + return false; + a->frames[replace] = std::move(keyframe); + } else + a->frames.push_back(std::move(keyframe)); + a->index_dirty = true; + result->recorded = true; + result->keyframes = uint32_t(a->frames.size()); + return true; + } + if (a->frames.empty()) + return false; + build_index(*a); + std::vector observations[RIFT_S_WORLD_MAX_CAMERAS]; + const rift_s_world_anchor_observation *obs[RIFT_S_WORLD_MAX_CAMERAS]{}; + uint32_t counts[RIFT_S_WORLD_MAX_CAMERAS]{}; + for (unsigned e = 0; e < count; ++e) { + observations[e] = room_matches(*a, features[e]); + obs[e] = observations[e].data(); + counts[e] = observations[e].size(); + } + rift_s_world_anchor_result solved{}; + bool ok = rift_s_world_anchor_solve_rig_gravity(&a->config, obs, counts, count, attitude, &solved); + for (unsigned e = 0; e < count; ++e) + solved.features[e] = result->features[e]; + solved.keyframes = result->keyframes; + *result = solved; + return ok; + } catch (const cv::Exception &) { + result->rejection = "opencv"; + return false; + } +} diff --git a/src/xrt/drivers/rift_s/rift_s_world_anchor.h b/src/xrt/drivers/rift_s/rift_s_world_anchor.h new file mode 100644 index 000000000..41cd3e5c1 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_world_anchor.h @@ -0,0 +1,126 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once +#include "rift_s_firmware.h" +#include "xrt/xrt_frame.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct rift_s_world_anchor; +#define RIFT_S_WORLD_MAX_CAMERAS 5 +struct rift_s_world_anchor_config +{ + // Raw sensor calibration in tracker camera order. Native camera axes are +Y down, +Z forward. + struct rift_s_camera_calibration camera[RIFT_S_WORLD_MAX_CAMERAS]; + struct xrt_pose imu_from_camera[RIFT_S_WORLD_MAX_CAMERAS]; + uint32_t camera_count; // Zero selects the legacy two-camera rig. + // Bind a persisted map to the committed boundary. Empty path resolves Steam's registry. + bool require_boundary; + char boundary_path[1024]; +}; +struct rift_s_world_anchor_observation +{ + struct xrt_vec3 world_point; + struct xrt_vec2 pixel; +}; +struct rift_s_world_anchor_result +{ + struct xrt_pose world_from_imu; + uint32_t features[RIFT_S_WORLD_MAX_CAMERAS], matches[RIFT_S_WORLD_MAX_CAMERAS], + inliers[RIFT_S_WORLD_MAX_CAMERAS], camera_points[RIFT_S_WORLD_MAX_CAMERAS], keyframes; + uint32_t supporting_cameras; + uint32_t stereo_matches, triangulated; + float max_reprojection_px; + const char *rejection; // Static diagnostic string, NULL on success. + bool recorded, relocalized; +}; + +// Opt-in bench recorder. Writes at most 600 synchronized camera sets and capture-time SLAM poses. +// RIFT_S_WORLD_RECORD_DIR is honored only with MONADO_STEAMVR_BENCH enabled. +void +rift_s_world_anchor_record_frames(struct rift_s_world_anchor *anchor, + const struct xrt_frame *left, + const struct xrt_frame *right, + const struct xrt_pose *raw_from_imu); + +void +rift_s_world_anchor_record_rig_frames(struct rift_s_world_anchor *anchor, + const struct xrt_frame *const frames[], + uint32_t camera_count, + const struct xrt_pose *raw_from_imu); +bool +rift_s_world_anchor_process_rig(struct rift_s_world_anchor *anchor, + const struct xrt_frame *const frames[], + uint32_t camera_count, + const struct xrt_pose *trusted_world_from_imu, + struct rift_s_world_anchor_result *result); +// Recovery fixes roll/pitch to the exposure-time VIO attitude and solves only yaw/translation. +bool +rift_s_world_anchor_process_rig_gravity(struct rift_s_world_anchor *anchor, + const struct xrt_frame *const frames[], + uint32_t camera_count, + const struct xrt_pose *trusted_world_from_imu, + const struct xrt_quat *raw_attitude, + struct rift_s_world_anchor_result *result); + +// Worker-owned: none of these functions are thread safe. No frame references are retained. +// An absent/invalid file creates an empty map, never a nominal world alignment. +struct rift_s_world_anchor * +rift_s_world_anchor_create(const struct rift_s_world_anchor_config *config, const char *path); +void +rift_s_world_anchor_destroy(struct rift_s_world_anchor *anchor); +bool +rift_s_world_anchor_has_map(const struct rift_s_world_anchor *anchor); +// A committed standing frame must not be paired with a fresh, arbitrary VIO frame, even if +// its visual map is missing or rejected. Only room setup may establish a replacement frame. +bool +rift_s_world_anchor_has_boundary(const struct rift_s_world_anchor *anchor); +// Clear an in-memory draft, without changing the saved map. Reload restores it after Cancel. +void +rift_s_world_anchor_clear(struct rift_s_world_anchor *anchor); +// Save also accepts an empty draft, atomically invalidating the old world after a new calibration. +bool +rift_s_world_anchor_save(struct rift_s_world_anchor *anchor); +bool +rift_s_world_anchor_reload(struct rift_s_world_anchor *anchor); +// Non-null trusted_world_from_imu records stereo keyframes. NULL only attempts recovery. +// Pose must correspond to the exposure timestamp, not the latest predicted head pose. +bool +rift_s_world_anchor_process(struct rift_s_world_anchor *anchor, + const struct xrt_frame *left, + const struct xrt_frame *right, + const struct xrt_pose *trusted_world_from_imu, + struct rift_s_world_anchor_result *result); + +// Geometry shared by recording/recovery and focused calibration tests. +bool +rift_s_world_anchor_project(const struct rift_s_camera_calibration *camera, + const struct xrt_vec3 *native_point, + struct xrt_vec2 *pixel); +bool +rift_s_world_anchor_unproject(const struct rift_s_camera_calibration *camera, + const struct xrt_vec2 *pixel, + struct xrt_vec3 *native_ray); +bool +rift_s_world_anchor_solve(const struct rift_s_world_anchor_config *config, + const struct rift_s_world_anchor_observation *observations[2], + const uint32_t counts[2], + struct rift_s_world_anchor_result *result); +bool +rift_s_world_anchor_solve_rig(const struct rift_s_world_anchor_config *config, + const struct rift_s_world_anchor_observation *const observations[], + const uint32_t counts[], + uint32_t camera_count, + struct rift_s_world_anchor_result *result); +bool +rift_s_world_anchor_solve_rig_gravity(const struct rift_s_world_anchor_config *config, + const struct rift_s_world_anchor_observation *const observations[], + const uint32_t counts[], + uint32_t camera_count, + const struct xrt_quat *raw_attitude, + struct rift_s_world_anchor_result *result); +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/rift_s/rift_s_world_control.h b/src/xrt/drivers/rift_s/rift_s_world_control.h new file mode 100644 index 000000000..45ea2e99c --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_world_control.h @@ -0,0 +1,15 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once +#include +#include +#include + +// Same-user native setup guard and tracker exchange only command IDs/status. Maps live in config. +static inline void +rift_s_world_control_path(const char *name, char *out, size_t size) +{ + const char *runtime = getenv("XDG_RUNTIME_DIR"); + if (runtime && runtime[0]) snprintf(out, size, "%s/monado-rifts-world-%s", runtime, name); + else snprintf(out, size, "/run/user/%u/monado-rifts-world-%s", (unsigned)getuid(), name); +} diff --git a/src/xrt/include/tracking/t_constellation_tracking.h b/src/xrt/include/tracking/t_constellation_tracking.h index 4319b6ef3..c6ec881db 100644 --- a/src/xrt/include/tracking/t_constellation_tracking.h +++ b/src/xrt/include/tracking/t_constellation_tracking.h @@ -46,6 +46,9 @@ struct t_constellation_camera struct xrt_rect roi; //! Intrinsics and distortion parameters struct t_camera_calibration calibration; + //! Native Fisheye62 coefficients, when a KB4 approximation would discard calibration. + bool fisheye62_valid; + float fisheye62_radial[6], fisheye62_p1, fisheye62_p2; //! Minimum blob brightness threshold uint8_t min_threshold; //! Minimum blob brightness threshold for pixel inclusion @@ -70,12 +73,42 @@ t_constellation_tracker_create(struct xrt_frame_context *xfctx, struct xrt_frame_sink **out_sink, struct xrt_device_masks_sink *controller_mask_sink); +/*! + * One verified optical measurement of a tracked device at image capture time. + * + * The pose is the device pose in the OpenXR world frame, as passed to push_observed_pose. + * Covariances describe the optical fit only; there is no velocity estimate yet. + */ +struct t_constellation_pose_observation +{ + timepoint_ns capture_ns; + struct xrt_pose pose; + //! The optical fit itself constrained yaw. Otherwise orientation is the IMU prior echoed back. + bool orientation_observed; + //! Accepted observations (repeated camera views counted) and distinct physical LEDs. + uint32_t inliers; + uint32_t distinct_leds; + //! Information rank of the optical fit: 3 (translation) or 4 (translation and yaw). + uint32_t information_rank; + //! Row-major world position covariance in m^2. + float position_covariance[9]; + //! Yaw variance in rad^2, INFINITY when not observed. + float yaw_variance; + //! Object-space RMS cost in metres and the hypothesis that produced the pose. + float cost_m; + const char *hypothesis; +}; + struct t_constellation_tracked_device_callbacks { bool (*get_led_model)(struct xrt_device *xdev, struct t_constellation_led_model *led_model); void (*notify_frame_received)(struct xrt_device *xdev, uint64_t frame_mono_ns, uint64_t frame_sequence); void (*push_observed_pose)(struct xrt_device *xdev, timepoint_ns frame_mono_ns, const struct xrt_pose *pose); void (*push_brightness_update)(struct xrt_device *xdev, uint8_t average_brightness); + //! True only when optical heading and capture-time attitude support a rotation prior. + bool (*has_heading_prior)(struct xrt_device *xdev, timepoint_ns capture_ns); + //! Optional: preferred over push_observed_pose when set, carries fit quality and yaw observability. + void (*push_pose_observation)(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs); }; struct t_constellation_tracked_device_connection * diff --git a/src/xrt/include/tracking/t_led_models.h b/src/xrt/include/tracking/t_led_models.h index 7466d1566..a81566fe6 100644 --- a/src/xrt/include/tracking/t_led_models.h +++ b/src/xrt/include/tracking/t_led_models.h @@ -15,6 +15,10 @@ #include "xrt/xrt_device.h" +#ifdef __cplusplus +extern "C" { +#endif + /* This is the angle that Rift CV1 LEDs are visible at. Let's see if * it works for other controller types... */ #define LED_ANGLE 82 @@ -49,6 +53,9 @@ struct t_constellation_led_model struct t_constellation_led *leds; uint8_t num_leds; + // Minimum for gravity-constrained acquisition. Zero keeps the seven-LED default. + uint8_t min_acquisition_leds; + // bounding box for the device itself, not it's LEDs struct t_constellation_bounding_point *bounding_points; uint8_t num_bounding_points; @@ -101,3 +108,7 @@ struct t_constellation_search_model * t_constellation_search_model_new(struct t_constellation_led_model *led_model); void t_constellation_search_model_free(struct t_constellation_search_model *model); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/include/xrt/xrt_camera.h b/src/xrt/include/xrt/xrt_camera.h new file mode 100644 index 000000000..4f2a764d1 --- /dev/null +++ b/src/xrt/include/xrt/xrt_camera.h @@ -0,0 +1,17 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once +#include "xrt/xrt_defines.h" +#include "xrt/xrt_frame.h" + +/* Optional local-device camera provider. Frames are rectified RGB8, left/right + * side by side, with monotonic capture timestamps. Owned by the device; callers + * release returned frame references. No IPC transport is implied. */ +struct xrt_camera +{ + uint32_t view_width, view_height; + struct xrt_vec2 focal_length, center; + struct xrt_pose head_from_camera[2]; + void (*set_active)(struct xrt_camera *camera, bool active); + bool (*get_frame)(struct xrt_camera *camera, struct xrt_frame **out_frame); +}; diff --git a/src/xrt/include/xrt/xrt_device.h b/src/xrt/include/xrt/xrt_device.h index 72dea4be9..bbc7812f8 100644 --- a/src/xrt/include/xrt/xrt_device.h +++ b/src/xrt/include/xrt/xrt_device.h @@ -610,6 +610,15 @@ struct xrt_device struct xrt_fov *out_fovs, struct xrt_pose *out_poses); + /* Optional: called only by the compositor render thread, before recording a frame. + * Return true when UVs changed. Projection FOV/topology must remain unchanged. + * Other consumers may load configuration at device creation instead. */ + bool (*update_distortion)(struct xrt_device *xdev); + + //! Optional tracking-camera video, owned by this device. + struct xrt_camera *camera; + + /** * Compute the distortion at a single point. * @@ -624,6 +633,7 @@ struct xrt_device * @param v vertical texture coordinate * @param[out] out_result corresponding u,v pairs for all three color channels. */ + xrt_result_t (*compute_distortion)( struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *out_result); diff --git a/src/xrt/include/xrt/xrt_tracking.h b/src/xrt/include/xrt/xrt_tracking.h index b648ffa5f..7f3e6047c 100644 --- a/src/xrt/include/xrt/xrt_tracking.h +++ b/src/xrt/include/xrt/xrt_tracking.h @@ -297,6 +297,13 @@ struct xrt_tracked_psvr */ struct xrt_tracked_slam { + /*! + * Discard the current tracking session, including queued sensor data and + * predicted poses. Sinks remain valid. Returns zero on success. + * Optional: drivers must hold an untracked pose if restart is unavailable. + */ + int (*restart)(struct xrt_tracked_slam *); + /*! * Called by the owning @ref xrt_device to get the last estimated pose * of the SLAM tracker. diff --git a/src/xrt/state_trackers/steamvr_drv/CMakeLists.txt b/src/xrt/state_trackers/steamvr_drv/CMakeLists.txt index 9e766f583..0a0f35f1f 100644 --- a/src/xrt/state_trackers/steamvr_drv/CMakeLists.txt +++ b/src/xrt/state_trackers/steamvr_drv/CMakeLists.txt @@ -3,7 +3,7 @@ add_subdirectory(steamvr_bindings) -add_library(st_ovrd STATIC ovrd_driver.cpp ovrd_interface.h) +add_library(st_ovrd STATIC ovrd_driver.cpp ovrd_interface.h ovrd_pose.hpp ovrd_camera.cpp ovrd_camera.hpp rifts_passthrough_shm.h) target_include_directories(st_ovrd INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) target_link_libraries( @@ -15,3 +15,20 @@ target_link_libraries( aux_generated_bindings_helpers ovrd_generated_bindings ) + +# Linux SteamVR's sparse SDF staging images need initialized storage on NVIDIA. +if(XRT_HAVE_VULKAN AND CMAKE_SYSTEM_NAME STREQUAL "Linux" AND XRT_BUILD_DRIVER_RIFT_S AND CMAKE_SIZEOF_VOID_P EQUAL 8) + add_library(monado_roomsetup_vulkan SHARED ovrd_roomsetup_vulkan.cpp) + target_include_directories(monado_roomsetup_vulkan PRIVATE ${Vulkan_INCLUDE_DIR}) + target_link_options(monado_roomsetup_vulkan PRIVATE "-Wl,-Bsymbolic") + set_target_properties(monado_roomsetup_vulkan PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/steamvr-monado/bin/linux64") + set(ROOMSETUP_LAYER_LIBRARY "${CMAKE_INSTALL_PREFIX}/share/steamvr-monado/bin/linux64/libmonado_roomsetup_vulkan.so") + configure_file(monado_roomsetup_vulkan.json.in ${CMAKE_CURRENT_BINARY_DIR}/monado_roomsetup_vulkan.json @ONLY) + install(FILES ${CMAKE_CURRENT_BINARY_DIR}/monado_roomsetup_vulkan.json DESTINATION ${CMAKE_INSTALL_DATADIR}/vulkan/implicit_layer.d) +endif() + +if(CMAKE_SYSTEM_NAME STREQUAL "Linux" AND XRT_BUILD_DRIVER_RIFT_S AND CMAKE_SIZEOF_VOID_P EQUAL 8) + add_executable(monado-roomsetup-guard ovrd_roomsetup_guard.cpp) + target_link_libraries(monado-roomsetup-guard PRIVATE xrt-external-openvr aux_util ${CMAKE_DL_LIBS}) + set_target_properties(monado-roomsetup-guard PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/steamvr-monado/bin/linux64") +endif() diff --git a/src/xrt/state_trackers/steamvr_drv/monado_roomsetup_vulkan.json.in b/src/xrt/state_trackers/steamvr_drv/monado_roomsetup_vulkan.json.in new file mode 100644 index 000000000..8c54135d6 --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/monado_roomsetup_vulkan.json.in @@ -0,0 +1,13 @@ +{ + "file_format_version": "1.2.0", + "layer": { + "name": "VK_LAYER_MONADO_roomsetup", + "type": "GLOBAL", + "library_path": "@ROOMSETUP_LAYER_LIBRARY@", + "library_arch": "64", + "api_version": "1.3.0", + "implementation_version": "1", + "description": "Monado SteamVR room setup SDF storage compatibility", + "disable_environment": {"MONADO_DISABLE_ROOMSETUP_COMPAT": "1"} + } +} diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_camera.cpp b/src/xrt/state_trackers/steamvr_drv/ovrd_camera.cpp new file mode 100644 index 000000000..69dea71e7 --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_camera.cpp @@ -0,0 +1,573 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "ovrd_camera.hpp" +#include "math/m_api.h" +#include "math/m_space.h" +#include "os/os_time.h" +#include "util/u_debug.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +DEBUG_GET_ONCE_BOOL_OPTION(steamvr_bench, "MONADO_STEAMVR_BENCH", false) +DEBUG_GET_ONCE_BOOL_OPTION(bench_camera, "MONADO_STEAMVR_BENCH_CAMERA", false) + +std::atomic MonadoCamera::s_instance{nullptr}; + +MonadoCamera::MonadoCamera(xrt_device *device, vr::IVRBlockQueue *queue, vr::IVRPaths *paths) + : device(device), camera(device->camera), queue(queue), paths(paths) +{ + s_instance.store(this); + shm_fd = open(RIFTS_PASSTHROUGH_SHM_PATH, O_CREAT | O_RDWR | O_NOFOLLOW | O_CLOEXEC, 0600); + if (shm_fd >= 0) { + // Hold the export ownership lock for the camera's lifetime. Consumers can + // distinguish this producer from a stale export left by an earlier session. + struct flock owner{}; + owner.l_type = F_WRLCK; + owner.l_whence = SEEK_SET; + if (fcntl(shm_fd, F_SETLK, &owner) < 0) { + close(shm_fd); + shm_fd = -1; + } + } + if (shm_fd >= 0) { + fchmod(shm_fd, 0600); + if (ftruncate(shm_fd, sizeof(struct rifts_passthrough_shm)) == 0) { + shm = static_cast( + mmap(nullptr, sizeof(struct rifts_passthrough_shm), PROT_READ | PROT_WRITE, MAP_SHARED, + shm_fd, 0)); + if (shm == MAP_FAILED) { + shm = nullptr; + } else { + memset(shm, 0, sizeof(*shm)); + shm->magic = RIFTS_PASSTHROUGH_MAGIC; + shm->version = RIFTS_PASSTHROUGH_VERSION; + shm->width = camera ? camera->view_width * 2 : RIFTS_PASSTHROUGH_WIDTH; + shm->height = camera ? camera->view_height : RIFTS_PASSTHROUGH_HEIGHT; + shm->stride = shm->width * 4; + shm->format = 0; // RGBA32 + shm->client_active = 0; + shm->toggle_count = 0; + shm->sequence = 0; + shm->timestamp_ns = 0; + } + } + } + if (queue && paths && !initialize_queue()) { + if (queue_handle) + queue->Destroy(queue_handle); + queue_handle = 0; + } + worker_running = true; + worker = std::thread(&MonadoCamera::run, this); +} + +MonadoCamera::~MonadoCamera() +{ + s_instance.store(nullptr); + StopVideoStream(); + worker_running = false; + if (worker.joinable()) + worker.join(); + if (queue_handle) + queue->Destroy(queue_handle); + if (shm) { + shm->client_active = 0; + munmap(shm, sizeof(struct rifts_passthrough_shm)); + shm = nullptr; + } + if (shm_fd >= 0) { + close(shm_fd); + shm_fd = -1; + } +} + +void +MonadoCamera::NotifyButton(enum xrt_input_name name, bool pressed) +{ + MonadoCamera *self = s_instance.load(); + if (!self || !self->shm || (name != XRT_INPUT_TOUCH_SYSTEM_CLICK && name != XRT_INPUT_TOUCH_MENU_CLICK)) + return; + // RunFrame reports the held state repeatedly. Toggle only on rising edges. + bool &previous = name == XRT_INPUT_TOUCH_SYSTEM_CLICK ? self->system_pressed : self->menu_pressed; + if (pressed == previous) + return; + previous = pressed; + if (!pressed) + return; + + int64_t now = os_monotonic_get_ns(); + + bool toggle = false; + if (name == XRT_INPUT_TOUCH_SYSTEM_CLICK) { + toggle = true; + } else if (name == XRT_INPUT_TOUCH_MENU_CLICK) { + if (self->last_menu_press_ns != 0 && now - self->last_menu_press_ns < 400 * 1000 * 1000) { + toggle = true; + self->last_menu_press_ns = 0; + } else { + self->last_menu_press_ns = now; + } + } + + if (toggle) { + __atomic_fetch_add(&self->shm->toggle_count, 1, __ATOMIC_SEQ_CST); + } +} + +bool +MonadoCamera::GetCameraFrameDimensions(vr::ECameraVideoStreamFormat format, uint32_t *width, uint32_t *height) +{ + if (format != vr::CVS_FORMAT_RGBX32 || !width || !height) + return false; + *width = camera->view_width * 2; + *height = camera->view_height; + return true; +} + +bool +MonadoCamera::GetCameraFrameBufferingRequirements(int *count, uint32_t *size) +{ + if (!count || !size) + return false; + *count = 3; + *size = bytes(); + return true; +} + +bool +MonadoCamera::SetCameraFrameBuffering(int count, void **data, uint32_t size) +{ + std::lock_guard lock(mutex); + if (active || count < 2 || count > 32 || !data || size < bytes()) + return false; + for (const auto &b : buffers) + if (b.acquired) + return false; + for (int i = 0; i < count; ++i) + if (!data[i]) + return false; + buffers.clear(); + storage.clear(); + buffers.resize(count); + for (int i = 0; i < count; ++i) + buffers[i].data = data[i]; + return true; +} + +bool +MonadoCamera::SetCameraVideoStreamFormat(vr::ECameraVideoStreamFormat format) +{ + return format == vr::CVS_FORMAT_RGBX32; +} + +bool +MonadoCamera::StartVideoStream() +{ + std::lock_guard callback_lock(callback_mutex); + std::lock_guard lock(mutex); + if (active) + return true; + for (const auto &b : buffers) + if (b.acquired) + return false; + if (buffers.empty()) { + storage.resize(3, std::vector(bytes())); + buffers.resize(3); + for (size_t i = 0; i < buffers.size(); ++i) + buffers[i].data = storage[i].data(); + } + for (auto &b : buffers) + b.ready = false; + start_ns = os_monotonic_get_ns(); + ++stream_generation; + frame_sequence = 0; + native_sequence = 0; + paused = false; + active = true; + return true; +} + +void +MonadoCamera::StopVideoStream() +{ + // Serialize completion with publication and callbacks, but never wait for get_frame. + std::lock_guard callback_lock(callback_mutex); + std::lock_guard lock(mutex); + active = false; + ++stream_generation; + for (auto &b : buffers) + b.ready = false; +} + +bool +MonadoCamera::IsVideoStreamActive(bool *is_paused, float *elapsed) +{ + std::lock_guard lock(mutex); + if (is_paused) + *is_paused = paused; + if (elapsed) + *elapsed = active ? (os_monotonic_get_ns() - start_ns) / 1e9f : 0; + return active; +} + +bool +MonadoCamera::PauseVideoStream() +{ + std::lock_guard callback_lock(callback_mutex); + std::lock_guard lock(mutex); + paused = true; + return active; +} + +bool +MonadoCamera::ResumeVideoStream() +{ + std::lock_guard callback_lock(callback_mutex); + std::lock_guard lock(mutex); + if (!active) + return false; + paused = false; + return true; +} + +const vr::CameraVideoStreamFrame_t * +MonadoCamera::GetVideoStreamFrame() +{ + std::lock_guard lock(mutex); + if (!active || paused) + return nullptr; + Buffer *latest = nullptr; + uint64_t now = os_monotonic_get_ns(); + for (auto &b : buffers) { + if (b.ready && !b.acquired && now >= b.frame.m_nFrameCaptureTicks_ServerAbsolute && + now - b.frame.m_nFrameCaptureTicks_ServerAbsolute <= 250 * U_TIME_1MS_IN_NS && + (!latest || b.frame.m_nFrameSequence > latest->frame.m_nFrameSequence)) + latest = &b; + } + if (!latest) + return nullptr; + for (auto &b : buffers) + if (!b.acquired) + b.ready = false; + latest->acquired = true; + return &latest->frame; +} + +void +MonadoCamera::ReleaseVideoStreamFrame(const vr::CameraVideoStreamFrame_t *frame) +{ + std::lock_guard lock(mutex); + for (auto &b : buffers) + if (&b.frame == frame) + b.acquired = false; +} + +bool +MonadoCamera::SetCameraVideoSinkCallback(vr::ICameraVideoSinkCallback *sink) +{ + std::lock_guard lock(callback_mutex); + callback = sink; + return true; +} + +void +MonadoCamera::run() +{ + // Export must not inherit a tracking thread's realtime policy or priority. + pthread_setname_np(pthread_self(), "rift-s-export"); + sched_param scheduling{}; + pthread_setschedparam(pthread_self(), SCHED_OTHER, &scheduling); + setpriority(PRIO_PROCESS, static_cast(syscall(SYS_gettid)), 10); + int64_t last_timestamp = 0; + std::vector rgba(bytes()); + while (worker_running) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + bool stream_active; + uint64_t generation; + int64_t stream_start_ns; + { + std::lock_guard lock(mutex); + stream_active = active && !paused; + generation = stream_generation; + stream_start_ns = start_ns; + } + uint32_t heartbeat = shm ? __atomic_load_n(&shm->client_active, __ATOMIC_ACQUIRE) : 0; + uint32_t now_ms = os_monotonic_get_ns() / U_TIME_1MS_IN_NS; + const bool export_active = heartbeat != 0 && static_cast(now_ms - heartbeat) < 1500; + bool want_frames = stream_active || export_active; + if (!want_frames || !camera) { + if (camera) + camera->set_active(camera, false); + std::this_thread::sleep_for(std::chrono::milliseconds(8)); + continue; + } + + camera->set_active(camera, true); + xrt_frame *frame = nullptr; + if (!camera->get_frame(camera, &frame)) + continue; + if (frame->format != XRT_FORMAT_R8G8B8 || frame->width != camera->view_width * 2 || + frame->height != camera->view_height || frame->stride < frame->width * 3 || + frame->size < frame->stride * frame->height) { + xrt_frame_reference(&frame, nullptr); + continue; + } + if (frame->timestamp <= last_timestamp) { + xrt_frame_reference(&frame, nullptr); + continue; + } + + // Convert once for both the compositor queue and the overlay export. + for (uint32_t y = 0; y < frame->height; ++y) { + const uint8_t *src = frame->data + y * frame->stride; + uint8_t *dst = rgba.data() + y * frame->width * 4; + for (uint32_t x = 0; x < frame->width; ++x) { + memcpy(dst + x * 4, src + x * 3, 3); + dst[x * 4 + 3] = 255; + } + } + if (debug_get_bool_option_steamvr_bench() && debug_get_bool_option_bench_camera()) { + // Distinct diagnostic cameras identify eye selection and packed UVs. + for (uint32_t y = 0; y < frame->height; ++y) { + for (uint32_t x = 0; x < frame->width; ++x) { + bool right = x >= camera->view_width; + bool grid = x % 40 < 2 || y % 40 < 2; + auto *pixel = rgba.data() + (y * frame->width + x) * 4; + pixel[0] = right ? (grid ? 255 : 0) : 255; + pixel[1] = grid ? 255 : 0; + pixel[2] = right ? 255 : (grid ? 255 : 0); + } + } + } + if (export_active) { + // Odd while writing; readers accept only equal, even sequence values. + __atomic_store_n(&shm->sequence, ++export_sequence, __ATOMIC_SEQ_CST); + memcpy(shm->pixels, rgba.data(), rgba.size()); + shm->timestamp_ns = frame->timestamp; + __atomic_store_n(&shm->sequence, ++export_sequence, __ATOMIC_RELEASE); + } + // A frame fetched outside the stream lock may belong to a stopped generation. + if (stream_active && frame->timestamp >= stream_start_ns) { + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + xrt_device_get_tracked_pose(device, XRT_INPUT_GENERIC_HEAD_POSE, frame->timestamp, &rel); + xrt_relation_chain chain{}; + m_relation_chain_push_relation(&chain, &rel); + m_relation_chain_push_pose_if_not_identity(&chain, &device->tracking_origin->initial_offset); + m_relation_chain_resolve(&chain, &rel); + bool delivered = false; + std::lock_guard callback_lock(callback_mutex); + { + std::lock_guard lock(mutex); + if (!active || paused || generation != stream_generation) { + xrt_frame_reference(&frame, nullptr); + continue; + } + if (queue_handle) { + double interval = + last_timestamp ? (frame->timestamp - last_timestamp) / 1e9 : 1.0 / 30; + publish_queue(rgba.data(), frame->timestamp, native_sequence++, interval); + } + for (size_t i = 0; i < buffers.size(); ++i) { + auto &b = buffers[i]; + if (b.acquired) + continue; + memcpy(b.data, rgba.data(), rgba.size()); + b.frame = {}; + b.frame.m_nStreamFormat = vr::CVS_FORMAT_RGBX32; + b.frame.m_nWidth = frame->width; + b.frame.m_nHeight = frame->height; + b.frame.m_nImageDataSize = bytes(); + b.frame.m_nFrameSequence = frame_sequence++; + b.frame.m_nBufferIndex = i; + b.frame.m_nBufferCount = buffers.size(); + b.frame.m_flFrameElapsedTime = (frame->timestamp - start_ns) / 1e9; + b.frame.m_flFrameDeliveryRate = 30; + b.frame.m_flFrameCaptureTime_DriverAbsolute = frame->timestamp / 1e9; + b.frame.m_flFrameCaptureTime_ServerRelative = + (static_cast(frame->timestamp) - + static_cast(os_monotonic_get_ns())) / + 1e9; + b.frame.m_nFrameCaptureTicks_ServerAbsolute = frame->timestamp; + xrt_matrix_4x4 matrix; + math_matrix_4x4_isometry_from_pose(&rel.pose, &matrix); + for (int row = 0; row < 3; ++row) + for (int col = 0; col < 4; ++col) + b.frame.m_RawTrackedDevicePose.mDeviceToAbsoluteTracking + .m[row][col] = matrix.v[col * 4 + row]; + auto required = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT; + b.frame.m_RawTrackedDevicePose.bPoseIsValid = + (rel.relation_flags & required) == required; + b.frame.m_RawTrackedDevicePose.bDeviceIsConnected = true; + b.frame.m_RawTrackedDevicePose.eTrackingResult = + b.frame.m_RawTrackedDevicePose.bPoseIsValid + ? vr::TrackingResult_Running_OK + : vr::TrackingResult_Running_OutOfRange; + b.frame.m_pImageData = reinterpret_cast(b.data); + b.ready = true; + delivered = true; + break; + } + } + if (delivered && callback) + callback->OnCameraVideoSinkCallback(); + } + + last_timestamp = frame->timestamp; + xrt_frame_reference(&frame, nullptr); + } + + if (camera) + camera->set_active(camera, false); +} + +bool +MonadoCamera::GetCameraDistortion(uint32_t eye, float u, float v, float *out_u, float *out_v) +{ + if (eye >= 2 || !out_u || !out_v) + return false; + *out_u = u; + *out_v = v; + return true; +} + +bool +MonadoCamera::GetCameraProjection( + uint32_t eye, vr::EVRTrackedCameraFrameType type, float near, float far, vr::HmdMatrix44_t *out) +{ + if (eye >= 2 || type < 0 || type >= vr::MAX_CAMERA_FRAME_TYPES || !std::isfinite(near) || !std::isfinite(far) || + near <= 0 || far <= near || !out) + return false; + *out = {}; + // SteamVR 2D selects the raw stereo half before projection. Each eye + // then uses the same camera-local region of a packed-sized texture. + float width = camera->view_width * 2.0f; + float center_x = camera->center.x; + out->m[0][0] = 2 * camera->focal_length.x / width; + out->m[1][1] = 2 * camera->focal_length.y / camera->view_height; + out->m[0][2] = 1 - 2 * center_x / width; + out->m[1][2] = 2 * camera->center.y / camera->view_height - 1; + // OpenVR camera projections use a zero-to-one depth range. + out->m[2][2] = -double(far) / (double(far) - near); + out->m[2][3] = -double(far) * near / (double(far) - near); + if (!std::isfinite(out->m[2][2]) || !std::isfinite(out->m[2][3])) { + *out = {}; + return false; + } + out->m[3][2] = -1; + return true; +} + +bool +MonadoCamera::GetCameraCompatibilityMode(vr::ECameraCompatibilityMode *out) +{ + if (!out) + return false; + *out = vr::CAMERA_COMPAT_MODE_BULK_DEFAULT; + return true; +} + +bool +MonadoCamera::GetCameraFrameBounds( + vr::EVRTrackedCameraFrameType type, uint32_t *left, uint32_t *top, uint32_t *width, uint32_t *height) +{ + if (type < 0 || type >= vr::MAX_CAMERA_FRAME_TYPES || !left || !top || !width || !height) + return false; + *left = *top = 0; + *width = camera->view_width * 2; + *height = camera->view_height; + return true; +} + +bool +MonadoCamera::GetCameraIntrinsics(uint32_t eye, + vr::EVRTrackedCameraFrameType type, + vr::HmdVector2_t *focal, + vr::HmdVector2_t *center, + vr::EVRDistortionFunctionType *distortion, + double *coefficients) +{ + if (eye >= 2 || type < 0 || type >= vr::MAX_CAMERA_FRAME_TYPES || !focal || !center || !distortion || + !coefficients) + return false; + *focal = {{camera->focal_length.x, camera->focal_length.y}}; + *center = {{camera->center.x, camera->center.y}}; + *distortion = vr::VRDistortionFunctionType_None; + std::fill_n(coefficients, vr::k_unMaxDistortionFunctionParameters, 0); + return true; +} + +// Queue storage and synchronization belong to SteamVR. Metadata is typed path +// data in its 512-byte header, not a CameraVideoStreamFrame_t memory dump. +bool +MonadoCamera::initialize_queue() +{ + vr::PropertyContainerHandle_t created = 0; + if (queue->Create(&created, "/lighthouse/camera/raw_frames", bytes(), 512, 4, 0) != 0) + return false; + queue_handle = created; + const char *names[] = {"/format", "/width", "/height"}; + int32_t values[] = {vr::CVS_FORMAT_RGBX32, static_cast(camera->view_width * 2), + static_cast(camera->view_height)}; + for (int i = 0; i < 3; ++i) { + vr::PathWrite_t write{}; + if (paths->StringToHandle(&write.ulPath, names[i]) != vr::TrackedProp_Success) + return false; + write.writeType = vr::PropertyWrite_Set; + write.pvBuffer = &values[i]; + write.unBufferSize = sizeof(values[i]); + write.unTag = vr::k_unInt32PropertyTag; + if (paths->WritePathBatch(queue_handle, &write, 1) != vr::TrackedProp_Success || + write.eError != vr::TrackedProp_Success) + return false; + } + const char *metadata[] = {"/frame_size", "/frame_sequence", "/frame_time_monotonic", + "/server_time_ticks", "/delivery_rate", "/elapsed_time"}; + for (int i = 0; i < 6; ++i) + if (paths->StringToHandle(&metadata_paths[i], metadata[i]) != vr::TrackedProp_Success) + return false; + return true; +} + +void +MonadoCamera::publish_queue(const uint8_t *pixels, int64_t timestamp, uint64_t sequence, double interval) +{ + vr::PropertyContainerHandle_t block = 0; + void *data = nullptr; + if (queue->AcquireWriteOnlyBlock(queue_handle, &block, &data) != 0) + return; + if (!data) { + queue->ReleaseWriteOnlyBlock(queue_handle, block); + return; + } + memcpy(data, pixels, bytes()); + int32_t size = bytes(); + double capture = timestamp / 1e9; + uint64_t ticks = timestamp; + double elapsed = (timestamp - start_ns) / 1e9; + void *values[] = {&size, &sequence, &capture, &ticks, &interval, &elapsed}; + uint32_t sizes[] = {sizeof(size), sizeof(sequence), sizeof(capture), + sizeof(ticks), sizeof(interval), sizeof(elapsed)}; + vr::PropertyTypeTag_t tags[] = {vr::k_unInt32PropertyTag, vr::k_unUint64PropertyTag, + vr::k_unDoublePropertyTag, vr::k_unUint64PropertyTag, + vr::k_unDoublePropertyTag, vr::k_unDoublePropertyTag}; + vr::PathWrite_t writes[6]{}; + for (int i = 0; i < 6; ++i) { + writes[i].ulPath = metadata_paths[i]; + writes[i].writeType = vr::PropertyWrite_Set; + writes[i].pvBuffer = values[i]; + writes[i].unBufferSize = sizes[i]; + writes[i].unTag = tags[i]; + } + paths->WritePathBatch(block, writes, 6); + queue->ReleaseWriteOnlyBlock(queue_handle, block); +} diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_camera.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_camera.hpp new file mode 100644 index 000000000..048376ba5 --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_camera.hpp @@ -0,0 +1,132 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once +#include "openvr_driver.h" +#include "ovrd_camera_queue.hpp" +#include "rifts_passthrough_shm.h" +#include "xrt/xrt_device.h" +#include "xrt/xrt_camera.h" +#include "xrt/xrt_tracking.h" +#include +#include +#include +#include + +class MonadoCamera final : public vr::IVRCameraComponent +{ +public: + explicit MonadoCamera(xrt_device *device, vr::IVRBlockQueue *queue = nullptr, vr::IVRPaths *paths = nullptr); + bool + native_ready() const + { + return queue_handle != 0; + } + ~MonadoCamera(); + bool + GetCameraFrameDimensions(vr::ECameraVideoStreamFormat, uint32_t *, uint32_t *) override; + bool + GetCameraFrameBufferingRequirements(int *, uint32_t *) override; + bool + SetCameraFrameBuffering(int, void **, uint32_t) override; + bool + SetCameraVideoStreamFormat(vr::ECameraVideoStreamFormat format) override; + vr::ECameraVideoStreamFormat + GetCameraVideoStreamFormat() override + { + return vr::CVS_FORMAT_RGBX32; + } + bool + StartVideoStream() override; + void + StopVideoStream() override; + bool + IsVideoStreamActive(bool *, float *) override; + const vr::CameraVideoStreamFrame_t * + GetVideoStreamFrame() override; + void + ReleaseVideoStreamFrame(const vr::CameraVideoStreamFrame_t *) override; + // Exposure belongs to tracking. Accept its existing automatic mode only. + bool + SetAutoExposure(bool enable) override + { + return enable; + } + bool + PauseVideoStream() override; + bool + ResumeVideoStream() override; + bool + GetCameraDistortion(uint32_t, float, float, float *, float *) override; + bool + GetCameraProjection(uint32_t, vr::EVRTrackedCameraFrameType, float, float, vr::HmdMatrix44_t *) override; + bool + SetFrameRate(int isp, int sensor) override + { + return isp == 30 && (sensor == 30 || sensor == 60); + } + bool + SetCameraVideoSinkCallback(vr::ICameraVideoSinkCallback *) override; + bool + GetCameraCompatibilityMode(vr::ECameraCompatibilityMode *) override; + bool + SetCameraCompatibilityMode(vr::ECameraCompatibilityMode mode) override + { + return mode == vr::CAMERA_COMPAT_MODE_BULK_DEFAULT; + } + bool + GetCameraFrameBounds(vr::EVRTrackedCameraFrameType, uint32_t *, uint32_t *, uint32_t *, uint32_t *) override; + bool + GetCameraIntrinsics(uint32_t, + vr::EVRTrackedCameraFrameType, + vr::HmdVector2_t *, + vr::HmdVector2_t *, + vr::EVRDistortionFunctionType *, + double *) override; + + static void + NotifyButton(enum xrt_input_name name, bool pressed); + +private: + struct Buffer + { + vr::CameraVideoStreamFrame_t frame{}; + void *data = nullptr; + bool ready = false, acquired = false; + }; + xrt_device *device; + xrt_camera *camera; + std::mutex mutex; + std::recursive_mutex callback_mutex; + vr::ICameraVideoSinkCallback *callback = nullptr; + std::vector buffers; + std::vector> storage; + bool active = false, paused = false; // Protected by mutex. + std::thread worker; + int64_t start_ns = 0; + uint64_t stream_generation = 0, native_sequence = 0; + uint32_t frame_sequence = 0; + + int shm_fd = -1; + struct rifts_passthrough_shm *shm = nullptr; + std::atomic worker_running{false}; + uint64_t export_sequence = 0; + int64_t last_menu_press_ns = 0; + bool system_pressed = false, menu_pressed = false; + vr::IVRBlockQueue *queue; + vr::IVRPaths *paths; + vr::PropertyContainerHandle_t queue_handle = 0; + vr::PathHandle_t metadata_paths[6]{}; + bool + initialize_queue(); + void + publish_queue(const uint8_t *pixels, int64_t timestamp, uint64_t sequence, double interval); + static std::atomic s_instance; + + void + run(); + uint32_t + bytes() const + { + return camera->view_width * 2 * camera->view_height * 4; + } +}; diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_camera_queue.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_camera_queue.hpp new file mode 100644 index 000000000..0fe39b275 --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_camera_queue.hpp @@ -0,0 +1,162 @@ +/* +Copyright (c) 2026, Rectus +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this +list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +// Public OpenVR C API declarations adapted by Rectus, openvr_camera_sim. +// https://github.com/Rectus/openvr_camera_sim/blob/4415906bfa4a97a3ae97acaa6b6025560dd140f8/vr_blockqueue.h +// Converted to C++ from the OpenVR C API. + +#pragma once + +#ifndef _OPENVR_BLOCKQUEUE +#define _OPENVR_BLOCKQUEUE + +#include "openvr_driver.h" + +namespace vr { +typedef uint64_t PathHandle_t; + +struct PathWrite_t +{ + PathHandle_t ulPath; + enum EPropertyWriteType writeType; + enum ETrackedPropertyError eSetError; + void *pvBuffer; + uint32_t unBufferSize; + PropertyTypeTag_t unTag; + enum ETrackedPropertyError eError; + const char *pszPath; +}; + +struct PathRead_t +{ + PathHandle_t ulPath; + void *pvBuffer; + uint32_t unBufferSize; + PropertyTypeTag_t unTag; + uint32_t unRequiredBufferSize; + enum ETrackedPropertyError eError; + const char *pszPath; +}; + +typedef enum EBlockQueueError +{ + EBlockQueueError_BlockQueueError_None = 0, + EBlockQueueError_BlockQueueError_QueueAlreadyExists = 1, + EBlockQueueError_BlockQueueError_QueueNotFound = 2, + EBlockQueueError_BlockQueueError_BlockNotAvailable = 3, + EBlockQueueError_BlockQueueError_InvalidHandle = 4, + EBlockQueueError_BlockQueueError_InvalidParam = 5, + EBlockQueueError_BlockQueueError_ParamMismatch = 6, + EBlockQueueError_BlockQueueError_InternalError = 7, + EBlockQueueError_BlockQueueError_AlreadyInitialized = 8, + EBlockQueueError_BlockQueueError_OperationIsServerOnly = 9, + EBlockQueueError_BlockQueueError_TooManyConnections = 10, +} EBlockQueueError; + +typedef enum EBlockQueueReadType +{ + EBlockQueueReadType_BlockQueueRead_Latest = 0, + EBlockQueueReadType_BlockQueueRead_New = 1, + EBlockQueueReadType_BlockQueueRead_Next = 2, +} EBlockQueueReadType; + +typedef enum EBlockQueueCreationFlag +{ + EBlockQueueCreationFlag_BlockQueueFlag_OwnerIsReader = 1, +} EBlockQueueCreationFlag; + +class IVRPaths +{ +public: + virtual ETrackedPropertyError + ReadPathBatch(PropertyContainerHandle_t ulRootHandle, PathRead_t *pBatch, uint32_t unBatchEntryCount) = 0; + virtual ETrackedPropertyError + WritePathBatch(PropertyContainerHandle_t ulRootHandle, PathWrite_t *pBatch, uint32_t unBatchEntryCount) = 0; + virtual ETrackedPropertyError + StringToHandle(PathHandle_t *pHandle, const char *pchPath) = 0; + virtual ETrackedPropertyError + HandleToString(PathHandle_t pHandle, + const char *pchBuffer, + uint32_t unBufferSize, + uint32_t *punBufferSizeUsed) = 0; +}; + +class IVRBlockQueue +{ +public: + virtual EBlockQueueError + Create(PropertyContainerHandle_t *pulQueueHandle, + const char *pchPath, + uint32_t unBlockDataSize, + uint32_t unBlockHeaderSize, + uint32_t unBlockCount, + uint32_t unFlags) = 0; + + virtual EBlockQueueError + Connect(PropertyContainerHandle_t *pulQueueHandle, const char *pchPath) = 0; + + virtual EBlockQueueError + Destroy(PropertyContainerHandle_t ulQueueHandle) = 0; + + virtual EBlockQueueError + AcquireWriteOnlyBlock(PropertyContainerHandle_t ulQueueHandle, + PropertyContainerHandle_t *pulBlockHandle, + void **ppvBuffer) = 0; + + virtual EBlockQueueError + ReleaseWriteOnlyBlock(PropertyContainerHandle_t ulQueueHandle, PropertyContainerHandle_t ulBlockHandle) = 0; + + virtual EBlockQueueError + WaitAndAcquireReadOnlyBlock(PropertyContainerHandle_t ulQueueHandle, + PropertyContainerHandle_t *pulBlockHandle, + void **ppvBuffer, + EBlockQueueReadType eReadType, + uint32_t unTimeoutMs) = 0; + + virtual EBlockQueueError + AcquireReadOnlyBlock(PropertyContainerHandle_t ulQueueHandle, + PropertyContainerHandle_t *pulBlockHandle, + void **ppvBuffer, + EBlockQueueReadType eReadType) = 0; + + virtual EBlockQueueError + ReleaseReadOnlyBlock(PropertyContainerHandle_t ulQueueHandle, PropertyContainerHandle_t ulBlockHandle) = 0; + + virtual EBlockQueueError + QueueHasReader(PropertyContainerHandle_t ulQueueHandle, bool *pbHasReaders) = 0; +}; + + +inline constexpr const char *IVRPaths_Version = "IVRPaths_001"; +inline constexpr const char *IVRBlockQueue_Version = "IVRBlockQueue_005"; + + +} // namespace vr + +#endif diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_display.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_display.hpp new file mode 100644 index 000000000..6b7ec34fe --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_display.hpp @@ -0,0 +1,57 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include "xrt/xrt_device.h" + +#include +#include + +// Match the upright eye FOV to the green-channel pixel density at the panel centre. +// Physical panel dimensions and rotation describe the output, not the source texture. +inline void +ovrd_get_recommended_render_target_size(xrt_device *xdev, float scale, uint32_t *width, uint32_t *height) +{ + double max_width = 0, max_height = 0; + for (uint32_t eye = 0; eye < xdev->hmd->view_count; eye++) { + const auto &display = xdev->hmd->views[eye].display; + double view_width = display.w_pixels, view_height = display.h_pixels; + const auto &fov = xdev->hmd->distortion.fov[eye]; + const double tan_width = std::tan(fov.angle_right) - std::tan(fov.angle_left); + const double tan_height = std::tan(fov.angle_up) - std::tan(fov.angle_down); + if (xdev->compute_distortion != nullptr && view_width > 0 && view_height > 0) { + xrt_uv_triplet left{}, right{}, up{}, down{}; + const float du = 0.5f / view_width, dv = 0.5f / view_height; + if (xdev->compute_distortion(xdev, eye, 0.5f - du, 0.5f, &left) == XRT_SUCCESS && + xdev->compute_distortion(xdev, eye, 0.5f + du, 0.5f, &right) == XRT_SUCCESS && + xdev->compute_distortion(xdev, eye, 0.5f, 0.5f - dv, &up) == XRT_SUCCESS && + xdev->compute_distortion(xdev, eye, 0.5f, 0.5f + dv, &down) == XRT_SUCCESS) { + // Each pair spans one upright panel pixel. UV differences become tangent differences. + const double dx = std::hypot((right.g.x - left.g.x) * tan_width, + (right.g.y - left.g.y) * tan_height); + const double dy = std::hypot((down.g.x - up.g.x) * tan_width, + (down.g.y - up.g.y) * tan_height); + const double density = 1.0 / std::min(dx, dy); + if (std::isfinite(density) && dx > 0 && dy > 0 && tan_width > 0 && tan_height > 0) { + view_width = tan_width * density; + view_height = tan_height * density; + } + } + } + max_width = std::max(max_width, view_width); + max_height = std::max(max_height, view_height); + } + *width = (uint32_t)std::ceil(max_width * scale); + *height = (uint32_t)std::ceil(max_height * scale); +} + +// OpenVR top/bottom are the minimum/maximum Y-up tangents, despite their names. +// Verified against SteamVR's projection matrix and a Rift S headset A/B test. +inline void +ovrd_get_projection_raw(const xrt_fov &fov, float *left, float *right, float *top, float *bottom) +{ + *left = std::tan(fov.angle_left); + *right = std::tan(fov.angle_right); + *top = std::tan(fov.angle_down); + *bottom = std::tan(fov.angle_up); +} diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp b/src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp index 61e2d4a5f..fbaa2eef4 100644 --- a/src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp @@ -15,9 +15,18 @@ #include #include #include +#include +#include +#include +#include #include "math/m_api.h" #include "ovrd_log.hpp" +#include "ovrd_pose.hpp" +#include "ovrd_display.hpp" +#include "ovrd_camera.hpp" +#include "ovrd_roomsetup_guard_process.hpp" +#include #include "openvr_driver.h" extern "C" { @@ -52,12 +61,79 @@ extern "C" { //! controllers, but input mapping may be incomplete or not ideal. DEBUG_GET_ONCE_BOOL_OPTION(emulate_index_controller, "STEAMVR_EMULATE_INDEX_CONTROLLER", false) -DEBUG_GET_ONCE_NUM_OPTION(scale_percentage, "XRT_COMPOSITOR_SCALE_PERCENTAGE", 140) +DEBUG_GET_ONCE_BOOL_OPTION(steamvr_bench, "MONADO_STEAMVR_BENCH", false) + +DEBUG_GET_ONCE_BOOL_OPTION(native_room_setup, "STEAMVR_NATIVE_ROOM_SETUP", true) +DEBUG_GET_ONCE_BOOL_OPTION(native_room_setup_vrlink, "STEAMVR_NATIVE_ROOM_SETUP_VRLINK", false) + +// The recommended size already matches the panel-centre pixel density. SteamVR adds its own GPU-based +// supersampling on top, so the upstream 140 % default for Monado's compositor would apply twice. +DEBUG_GET_ONCE_NUM_OPTION(scale_percentage, "XRT_COMPOSITOR_SCALE_PERCENTAGE", 100) #define MODELNUM_LEN (XRT_DEVICE_NAME_LEN + 9) // "[Monado] " #define OPENVR_BONE_COUNT 31 +static bool +is_rift_s_touch(const struct xrt_device *xdev) +{ + static const char prefix[] = "Oculus Rift S"; + return xdev->name == XRT_DEVICE_TOUCH_CONTROLLER && strncmp(xdev->str, prefix, sizeof(prefix) - 1) == 0; +} + +// DebugRequest bench commands expire so a stopped script cannot hold a trigger. +// Commands: bench x y z pitch trigger; bench6 x y z pitch yaw roll trigger. +// Angles are radians. "bench off" returns to tracking. +struct BenchInput +{ + std::mutex mutex; + xrt_pose pose = XRT_POSE_IDENTITY; + float trigger = 0; + int64_t until_ns = 0; + + void request(const char *request, char *response, uint32_t size) + { + if (size) response[0] = 0; + if (!debug_get_bool_option_steamvr_bench()) return; + std::lock_guard lock(mutex); + if (strcmp(request, "bench off") == 0) { + until_ns = 0; + if (size) snprintf(response, size, "ok"); + return; + } + float x, y, z, pitch, yaw = 0, roll = 0, value; + char extra; + bool parsed = sscanf(request, "bench6 %f %f %f %f %f %f %f %c", &x, &y, &z, &pitch, &yaw, &roll, &value, &extra) == 7; + if (!parsed) parsed = sscanf(request, "bench %f %f %f %f %f %c", &x, &y, &z, &pitch, &value, &extra) == 5; + if (!parsed || !std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) || + !std::isfinite(pitch) || !std::isfinite(yaw) || !std::isfinite(roll) || !std::isfinite(value)) return; + pose = XRT_POSE_IDENTITY; + pose.position = {x, y, z}; + const xrt_vec3 angles{pitch, yaw, roll}; + math_quat_from_euler_angles(&angles, &pose.orientation); + trigger = std::clamp(value, 0.0f, 1.0f); + until_ns = os_monotonic_get_ns() + 500 * U_TIME_1MS_IN_NS; + if (size) snprintf(response, size, "ok"); + } + + bool sample(xrt_pose &out, float &value) + { + if (!debug_get_bool_option_steamvr_bench()) return false; + std::lock_guard lock(mutex); + if (os_monotonic_get_ns() >= until_ns) return false; + out = pose; + value = trigger; + return true; + } +}; + +static void bench_relation(xrt_space_relation &relation, const xrt_pose &pose) +{ + relation = XRT_SPACE_RELATION_ZERO; + relation.pose = pose; + relation.relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; +} + // Debug define(s), always off. #undef DUMP_POSE #undef DUMP_POSE_CONTROLLERS @@ -76,14 +152,14 @@ struct MonadoInputComponent struct SteamVRDriverControl { - const char *steamvr_control_path; + std::string steamvr_control_path; vr::VRInputComponentHandle_t control_handle; }; struct SteamVRDriverControlInput : SteamVRDriverControl { enum xrt_input_type monado_input_type; - enum xrt_input_name monado_input_name; + struct xrt_input *monado_input; struct MonadoInputComponent component; }; @@ -94,56 +170,6 @@ struct SteamVRDriverControlOutput : SteamVRDriverControl enum xrt_output_name monado_output_name; }; -static void -copy_vec3(struct xrt_vec3 *from, double *to) -{ - to[0] = from->x; - to[1] = from->y; - to[2] = from->z; -} - -static void -copy_quat(struct xrt_quat *from, vr::HmdQuaternion_t *to) -{ - to->x = from->x; - to->y = from->y; - to->z = from->z; - to->w = from->w; -} - -static void -apply_pose(struct xrt_space_relation *rel, vr::DriverPose_t *m_pose) -{ - if ((rel->relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) != 0) { - copy_quat(&rel->pose.orientation, &m_pose->qRotation); - } else { - m_pose->result = vr::TrackingResult_Running_OutOfRange; - m_pose->poseIsValid = false; - } - - if ((rel->relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0) { - copy_vec3(&rel->pose.position, m_pose->vecPosition); - } else { - } - - if ((rel->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0) { - // linear velocity in world space - copy_vec3(&rel->linear_velocity, m_pose->vecVelocity); - } - - if ((rel->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0) { - // angular velocity reported by monado in world space, - // expected by steamvr to be in "controller space" - struct xrt_quat orientation_inv; - math_quat_invert(&rel->pose.orientation, &orientation_inv); - - struct xrt_vec3 vel; - math_quat_rotate_derivative(&orientation_inv, &rel->angular_velocity, &vel); - - copy_vec3(&vel, m_pose->vecAngularVelocity); - } -} - #define OPENVR_BONE_COUNT 31 // these are currently only used for the root and wrist transforms, but are kept here as they are useful for debugging. @@ -368,8 +394,11 @@ hand_joint_set_to_bone_transform(struct xrt_hand_joint_set hand_joint_set, class CDeviceDriver_Monado_Controller : public vr::ITrackedDeviceServerDriver { public: - CDeviceDriver_Monado_Controller(struct xrt_instance *xinst, struct xrt_device *xdev, enum xrt_hand hand) - : m_xdev(xdev), m_hand(hand) + CDeviceDriver_Monado_Controller(struct xrt_instance *xinst, + struct xrt_device *xdev, + enum xrt_hand hand, + struct xrt_device *xhmd = NULL) + : m_xdev(xdev), m_xhmd(xhmd), m_hand(hand) { ovrd_log("Creating Controller %s\n", xdev->str); @@ -403,6 +432,16 @@ public: } break; case XRT_DEVICE_TOUCH_CONTROLLER: + if (is_rift_s_touch(xdev)) { + if (hand == XRT_HAND_LEFT) { + m_render_model = "oculus_rifts_controller_left"; + } + if (hand == XRT_HAND_RIGHT) { + m_render_model = "oculus_rifts_controller_right"; + } + m_grip_to_model = rift_s_touch_grip_to_model(hand); + break; + } if (hand == XRT_HAND_LEFT) { m_render_model = "oculus_cv1_controller_left"; } @@ -444,39 +483,55 @@ public: { enum xrt_input_type monado_input_type = XRT_GET_INPUT_TYPE(monado_input_name); - SteamVRDriverControlInput in; + SteamVRDriverControlInput in{}; in.monado_input_type = monado_input_type; in.steamvr_control_path = steamvr_control_path; - in.monado_input_name = monado_input_name; + in.monado_input = NULL; + for (uint32_t i = 0; i < m_xdev->input_count; i++) { + if (m_xdev->inputs[i].name == monado_input_name) { + in.monado_input = &m_xdev->inputs[i]; + break; + } + } + if (in.monado_input == NULL) { + ovrd_log("Skipping unsupported input %s on %s\n", steamvr_control_path, m_xdev->str); + return; + } if (component != NULL) { in.component = *component; } else { in.component.has_component = false; } + vr::EVRInputError err = vr::VRInputError_None; if (monado_input_type == XRT_INPUT_TYPE_BOOLEAN) { - vr::VRDriverInput()->CreateBooleanComponent(m_ulPropertyContainer, steamvr_control_path, + err = vr::VRDriverInput()->CreateBooleanComponent(m_ulPropertyContainer, steamvr_control_path, &in.control_handle); } else if (monado_input_type == XRT_INPUT_TYPE_VEC1_MINUS_ONE_TO_ONE) { - vr::VRDriverInput()->CreateScalarComponent(m_ulPropertyContainer, steamvr_control_path, + err = vr::VRDriverInput()->CreateScalarComponent(m_ulPropertyContainer, steamvr_control_path, &in.control_handle, vr::VRScalarType_Absolute, vr::VRScalarUnits_NormalizedTwoSided); } else if (monado_input_type == XRT_INPUT_TYPE_VEC1_ZERO_TO_ONE) { - vr::VRDriverInput()->CreateScalarComponent(m_ulPropertyContainer, steamvr_control_path, + err = vr::VRDriverInput()->CreateScalarComponent(m_ulPropertyContainer, steamvr_control_path, &in.control_handle, vr::VRScalarType_Absolute, vr::VRScalarUnits_NormalizedOneSided); } else if (monado_input_type == XRT_INPUT_TYPE_VEC2_MINUS_ONE_TO_ONE) { // 2D values are added as 2 1D values // usually those are [-1,1] - vr::VRDriverInput()->CreateScalarComponent(m_ulPropertyContainer, steamvr_control_path, + err = vr::VRDriverInput()->CreateScalarComponent(m_ulPropertyContainer, steamvr_control_path, &in.control_handle, vr::VRScalarType_Absolute, vr::VRScalarUnits_NormalizedTwoSided); } + if (err != vr::VRInputError_None) { + ovrd_log("Failed to create input %s on %s: %d\n", steamvr_control_path, m_xdev->str, err); + return; + } + m_input_controls.push_back(in); ovrd_log("Added input %s\n", steamvr_control_path); } @@ -493,7 +548,7 @@ public: out.steamvr_control_path = steamvr_control_path; out.monado_output_name = monado_output_name; - vr::VRDriverInput()->CreateHapticComponent(m_ulPropertyContainer, out.steamvr_control_path, + vr::VRDriverInput()->CreateHapticComponent(m_ulPropertyContainer, out.steamvr_control_path.c_str(), &out.control_handle); m_output_controls.push_back(out); @@ -507,11 +562,11 @@ public: { enum xrt_input_type monado_input_type = XRT_GET_INPUT_TYPE(monado_input_name); - SteamVRDriverControlInput in; + SteamVRDriverControlInput in{}; in.monado_input_type = monado_input_type; in.steamvr_control_path = steamvr_control_path; - in.monado_input_name = monado_input_name; + in.monado_input = NULL; in.component.has_component = false; vr::EVRInputError err = vr::VRDriverInput()->CreateSkeletonComponent( @@ -772,6 +827,14 @@ public: enum xrt_input_name monado_input_name = b->input; const char *steamvr_path = b->steamvr_path; + // SteamVR's oculus_touch bindings toggle the dashboard from the left /input/system and leave the + // right one (Oculus button) unbound. The Touch left menu button is the equivalent, so publish it + // as system. Without this nothing can open the dashboard. + if (m_xdev->name == XRT_DEVICE_TOUCH_CONTROLLER && m_hand == XRT_HAND_LEFT && + strcmp(steamvr_path, "/input/menu/click") == 0) { + steamvr_path = "/input/system/click"; + } + enum xrt_input_type monado_input_type = XRT_GET_INPUT_TYPE(monado_input_name); switch (monado_input_type) { @@ -809,6 +872,10 @@ public: for (size_t i = 0; i < p->binding_count; i++) { struct binding_template *b = &p->bindings[i]; + const char *hand_path = m_hand == XRT_HAND_LEFT ? "/user/hand/left" : "/user/hand/right"; + if (strcmp(b->subaction_path, hand_path) != 0) { + continue; + } if (b->input != 0) { AddMonadoInput(b); @@ -894,12 +961,14 @@ public: } m_input_profile = std::string("{monado}/input/") + std::string(p->steamvr_input_profile_path); - m_controller_type = p->steamvr_controller_type; + m_controller_type = m_xdev->name == XRT_DEVICE_TOUCH_CONTROLLER ? "monado_oculus_touch" + : p->steamvr_controller_type; } ovrd_log("Using input profile %s\n", m_input_profile.c_str()); ovrd_log("Using render model%s\n", m_render_model); vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, vr::Prop_InputProfilePath_String, m_input_profile.c_str()); + vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, vr::Prop_ControllerType_String, m_controller_type); vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, vr::Prop_RenderModelName_String, m_render_model); vr::VRProperties()->SetStringProperty(m_ulPropertyContainer, vr::Prop_ModelNumber_String, m_xdev->str); @@ -951,8 +1020,7 @@ public: void DebugRequest(const char *pchRequest, char *pchResponseBuffer, uint32_t unResponseBufferSize) { - if (unResponseBufferSize >= 1) - pchResponseBuffer[0] = 0; + m_bench.request(pchRequest, pchResponseBuffer, unResponseBufferSize); } vr::DriverPose_t @@ -1002,12 +1070,42 @@ public: struct xrt_pose *offset = &m_xdev->tracking_origin->initial_offset; - struct xrt_relation_chain chain = {}; - m_relation_chain_push_relation(&chain, &rel); - m_relation_chain_push_pose_if_not_identity(&chain, offset); - m_relation_chain_resolve(&chain, &rel); + if (is_rift_s_touch(m_xdev)) { + transform_rift_s_touch_relation(&rel, &m_grip_to_model, offset); + } else { + struct xrt_relation_chain chain = {}; + m_relation_chain_push_pose_if_not_identity(&chain, &m_grip_to_model); + m_relation_chain_push_relation(&chain, &rel); + m_relation_chain_push_pose_if_not_identity(&chain, offset); + m_relation_chain_resolve(&chain, &rel); + } - apply_pose(&rel, &m_pose); + xrt_pose bench_pose; + float bench_trigger; + if (is_rift_s_touch(m_xdev) && m_bench.sample(bench_pose, bench_trigger)) + bench_relation(rel, bench_pose); + + if (is_rift_s_touch(m_xdev)) + apply_rift_s_touch_pose(&rel, &m_pose); + else + apply_pose(&rel, &m_pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, + m_xdev->name == XRT_DEVICE_TOUCH_CONTROLLER); + if ((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0) { + m_had_position = true; + } else if (is_rift_s_touch(m_xdev) && !m_had_position && m_xhmd != NULL) { + // Never fixed: SteamVR may still draw the invalid pose, at the tracking origin under + // the floor. Keep it at a resting hand position below the head instead. + struct xrt_space_relation head; + xrt_device_get_tracked_pose(m_xhmd, XRT_INPUT_GENERIC_HEAD_POSE, now_ns, &head); + struct xrt_relation_chain head_chain = {}; + m_relation_chain_push_relation(&head_chain, &head); + m_relation_chain_push_pose_if_not_identity(&head_chain, offset); + m_relation_chain_resolve(&head_chain, &head); + if ((head.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0) { + struct xrt_vec3 hand = nominal_hand_position(head.pose, m_hand); + copy_vec3(&hand, m_pose.vecPosition); + } + } #ifdef DUMP_POSE_CONTROLLERS ovrd_log("get controller %d pose %f %f %f %f, %f %f %f\n", m_unObjectId, m_pose.qRotation.x, @@ -1028,33 +1126,23 @@ public: RunFrame() { m_xdev->update_inputs(m_xdev); - + xrt_pose bench_pose; + float bench_trigger = 0; + bool synthetic = is_rift_s_touch(m_xdev) && m_bench.sample(bench_pose, bench_trigger); for (const auto &in : m_input_controls) { // ovrd_log("Update %d: %s\n", i, // m_controls[i].steamvr_control_path); - enum xrt_input_name binding_name = in.monado_input_name; - - struct xrt_input *input = NULL; - for (uint32_t ii = 0; ii < m_xdev->input_count; ii++) { - if (m_xdev->inputs[ii].name == binding_name) { - input = &m_xdev->inputs[ii]; - break; - } - } - - if (input == NULL) { - ovrd_log("Input for %s not found!\n", in.steamvr_control_path); - continue; - } + struct xrt_input *input = in.monado_input; vr::VRInputComponentHandle_t handle = in.control_handle; if (in.monado_input_type == XRT_INPUT_TYPE_BOOLEAN) { - bool state = input->value.boolean; + bool state = synthetic ? false : input->value.boolean; vr::VRDriverInput()->UpdateBooleanComponent(handle, state, 0); + MonadoCamera::NotifyButton(input->name, state); // ovrd_log("Update %s: %d\n", // m_controls[i].steamvr_control_path, state); // U_LOG_D("Update %s: %d", @@ -1074,6 +1162,8 @@ public: value = input->value.vec1.x; } + if (synthetic) + value = in.steamvr_control_path == "/input/trigger/value" ? bench_trigger : 0; vr::VRDriverInput()->UpdateScalarComponent(handle, value, 0); // ovrd_log("Update %s: %f\n", // m_controls[i].steamvr_control_path, @@ -1166,6 +1256,7 @@ public: struct xrt_device *m_xdev; + BenchInput m_bench; vr::DriverPose_t m_pose; vr::TrackedDeviceIndex_t m_unObjectId; vr::PropertyContainerHandle_t m_ulPropertyContainer; @@ -1184,6 +1275,11 @@ private: const char *m_controller_type = NULL; const char *m_render_model = NULL; + //! Render-model origin in the grip frame; identity where the model origin is the grip. + struct xrt_pose m_grip_to_model = XRT_POSE_IDENTITY; + //! Head, for the resting position of a controller that has never had a position fix. + struct xrt_device *m_xhmd = NULL; + bool m_had_position = false; enum xrt_hand m_hand; bool m_handed_controller; @@ -1247,6 +1343,8 @@ public: virtual void *GetComponent(const char *pchComponentNameAndVersion); virtual void DebugRequest(const char *pchRequest, char *pchResponseBuffer, uint32_t unResponseBufferSize); virtual vr::DriverPose_t GetPose(); + void UpdateRoomSetupGuard(); + void UpdateDisplayFrequency(); // IVRDisplayComponent virtual void GetWindowBounds(int32_t *pnX, int32_t *pnY, uint32_t *pnWidth, uint32_t *pnHeight); @@ -1261,6 +1359,12 @@ public: private: struct xrt_device *m_xdev = NULL; + BenchInput m_bench; + std::unique_ptr m_camera; + vr::VRInputComponentHandle_t m_benchProximity = 0; + RoomSetupGuardProcess m_roomSetupGuard; + bool m_nativeSetupRequested = false; + bool m_nativeSetupEnabled = false; // clang-format off @@ -1269,6 +1373,7 @@ private: float m_flSecondsFromVsyncToPhotons = -1; float m_flDisplayFrequency = -1; + uint64_t m_nextFrequencyCheckNs = 0; float m_flIPD = -1; struct xrt_fov m_fovs[2]; @@ -1300,6 +1405,39 @@ create_translation_rotation_matrix(struct xrt_pose *pose, struct vr::HmdMatrix34 res->m[2][3] = t.z; } +void +CDeviceDriver_Monado::UpdateDisplayFrequency() +{ + // SteamVR's Vulkan renderer falls back to 90 Hz even after selecting the Rift S 80 Hz mode. + // The Rift S has one display mode; do not override dynamically selected modes on other HMDs. + if (strcmp(m_xdev->str, "Oculus Rift S") != 0) return; + uint64_t now = os_monotonic_get_ns(); + if (now < m_nextFrequencyCheckNs) return; + m_nextFrequencyCheckNs = now + 1000000000ULL; + vr::ETrackedPropertyError error; + auto properties = vr::VRProperties(); + float reported = properties->GetFloatProperty(m_ulPropertyContainer, vr::Prop_DisplayFrequency_Float, &error); + if (error == vr::TrackedProp_Success && (!std::isfinite(reported) || std::abs(reported - m_flDisplayFrequency) > 0.01f)) { + properties->SetFloatProperty(m_ulPropertyContainer, vr::Prop_DisplayFrequency_Float, m_flDisplayFrequency); + ovrd_log("Restored Rift S display frequency %.3f Hz after compositor reported %.3f Hz\n", + m_flDisplayFrequency, reported); + } +} + +void +CDeviceDriver_Monado::UpdateRoomSetupGuard() +{ + bool enabled = m_nativeSetupRequested && m_roomSetupGuard.ready(); + if (enabled == m_nativeSetupEnabled) return; + m_nativeSetupEnabled = enabled; + auto properties = vr::VRProperties(); + properties->SetBoolProperty(m_ulPropertyContainer, static_cast(11003), enabled); + properties->SetBoolProperty(m_ulPropertyContainer, static_cast(11004), + enabled && debug_get_bool_option_native_room_setup_vrlink()); + vr::VRSettings()->SetBool("steamvr", "inHMDRoomSetupDebug", enabled); + ovrd_log("Native room setup guard ready=%d\n", enabled); +} + void CDeviceDriver_Monado::PoseUpdateThreadFunction() { @@ -1308,6 +1446,8 @@ CDeviceDriver_Monado::PoseUpdateThreadFunction() while (m_poseUpdating) { //! @todo figure out the best pose update rate std::this_thread::sleep_for(std::chrono::milliseconds(1)); + if (m_benchProximity) + vr::VRDriverInput()->UpdateBooleanComponent(m_benchProximity, true, 0); vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_trackedDeviceIndex, GetPose(), sizeof(vr::DriverPose_t)); } @@ -1335,11 +1475,84 @@ CDeviceDriver_Monado::Activate(vr::TrackedDeviceIndex_t unObjectId) vr::VRProperties()->SetFloatProperty(m_ulPropertyContainer, vr::Prop_DisplayFrequency_Float, m_flDisplayFrequency); vr::VRProperties()->SetFloatProperty( m_ulPropertyContainer, vr::Prop_SecondsFromVsyncToPhotons_Float, m_flSecondsFromVsyncToPhotons); - // return a constant that's not 0 (invalid) or 1 (reserved for Oculus) + if (debug_get_bool_option_steamvr_bench()) { + vr::VRProperties()->SetBoolProperty(m_ulPropertyContainer, vr::Prop_ContainsProximitySensor_Bool, true); + auto proximity_error = vr::VRDriverInput()->CreateBooleanComponent(m_ulPropertyContainer, "/input/proximity", &m_benchProximity); + ovrd_log("Bench proximity component: error=%d\n", proximity_error); + ovrd_log("MONADO_STEAMVR_BENCH enabled: synthetic fallback and awake HMD\n"); + } + // return a constant that's not 0 (invalid) or 1 (reserved for Oculus) vr::VRProperties()->SetUint64Property( m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, 2); + if (m_xdev->camera) { + vr::EVRInitError queue_error = vr::VRInitError_None; + auto queue = static_cast(vr::VRDriverContext()->GetGenericInterface(vr::IVRBlockQueue_Version, &queue_error)); + auto paths = static_cast(vr::VRDriverContext()->GetGenericInterface(vr::IVRPaths_Version, &queue_error)); + m_camera = std::make_unique(m_xdev, queue, paths); + ovrd_log("Rift S native camera queue: %s\n", m_camera->native_ready() ? "ready" : "unavailable"); + auto properties = vr::VRProperties(); + properties->SetBoolProperty(m_ulPropertyContainer, vr::Prop_HasCamera_Bool, true); + properties->SetBoolProperty(m_ulPropertyContainer, vr::Prop_AllowCameraToggle_Bool, true); + if (m_camera->native_ready()) { + // SteamVR's legacy Room View gate treats these as compatibility + // revisions. They do not describe the Rift S firmware. + properties->SetUint64Property(m_ulPropertyContainer, vr::Prop_FPGAVersion_Uint64, 260); + properties->SetUint64Property(m_ulPropertyContainer, vr::Prop_FirmwareVersion_Uint64, 1447390112); + properties->SetUint64Property(m_ulPropertyContainer, vr::Prop_CameraFirmwareVersion_Uint64, 8590196809ULL); + } + // SteamVR 2.17's local setup entry gate requires its experimental flag. + // Keep the VRLink request property off unless explicitly testing that path. + vr::EVRSettingsError settings_error = vr::VRSettingsError_None; + bool enable_setup = vr::VRSettings()->GetBool("driver_monado", "enableNativeRoomSetup", &settings_error); + if (settings_error != vr::VRSettingsError_None) enable_setup = true; + const bool native_setup = m_camera->native_ready() && enable_setup && debug_get_bool_option_native_room_setup(); + const bool vrlink = native_setup && debug_get_bool_option_native_room_setup_vrlink(); + constexpr auto supports_setup = static_cast(11003); + constexpr auto supports_vrlink_requests = static_cast(11004); + m_nativeSetupRequested = native_setup; + if (native_setup && !m_roomSetupGuard.start()) ovrd_log("Native room setup guard could not start\n"); + properties->SetBoolProperty(m_ulPropertyContainer, supports_setup, false); + properties->SetBoolProperty(m_ulPropertyContainer, supports_vrlink_requests, false); + vr::EVRSettingsError flag_error = vr::VRSettingsError_None; + vr::VRSettings()->SetBool("steamvr", "inHMDRoomSetupDebug", false, &flag_error); + ovrd_log("Native room setup requested=%d: VRLink requests=%d, settings error=%d\n", native_setup, vrlink, flag_error); + if (native_setup) { + // The setup scene override still requires Room View to be enabled. + // Use 2D reprojection: this camera supplies rectified stereo, not depth. + auto settings = vr::VRSettings(); + settings->SetBool("camera", "enableCamera", true); + settings->SetBool("camera", "enableConstructRoomView", true); + settings->SetInt32("camera", "roomView", 1); + settings->SetInt32("camera", "roomViewStyle", 4); // Opaque video. + ovrd_log("Native room setup camera: enabled 2D opaque Room View\n"); + } + properties->SetBoolProperty(m_ulPropertyContainer, vr::Prop_HasCameraComponent_Bool, true); + properties->SetInt32Property(m_ulPropertyContainer, vr::Prop_NumCameras_Int32, 2); + properties->SetBoolProperty(m_ulPropertyContainer, vr::Prop_SupportsRoomViewDepthProjection_Bool, false); + properties->SetInt32Property(m_ulPropertyContainer, vr::Prop_CameraFrameLayout_Int32, + vr::EVRTrackedCameraFrameLayout_Stereo | vr::EVRTrackedCameraFrameLayout_HorizontalLayout); + properties->SetInt32Property(m_ulPropertyContainer, vr::Prop_CameraStreamFormat_Int32, vr::CVS_FORMAT_RGBX32); + // The exported images are already rectified pinhole views. Public raw + // camera clients need these properties as well as component intrinsics. + int32_t distortion[2] = {vr::VRDistortionFunctionType_None, vr::VRDistortionFunctionType_None}; + double coefficients[2][vr::k_unMaxDistortionFunctionParameters]{}; + vr::HmdVector4_t white_balance[2] = {{{1, 1, 1, 1}}, {{1, 1, 1, 1}}}; + properties->SetProperty(m_ulPropertyContainer, vr::Prop_CameraDistortionFunction_Int32_Array, + distortion, sizeof(distortion), vr::k_unInt32PropertyTag); + properties->SetProperty(m_ulPropertyContainer, vr::Prop_CameraDistortionCoefficients_Float_Array, + coefficients, sizeof(coefficients), vr::k_unFloatPropertyTag); + properties->SetProperty(m_ulPropertyContainer, vr::Prop_CameraWhiteBalance_Vector4_Array, + white_balance, sizeof(white_balance), vr::k_unHmdVector4PropertyTag); + vr::HmdMatrix34_t transforms[2]; + for (int i = 0; i < 2; ++i) create_translation_rotation_matrix(&m_xdev->camera->head_from_camera[i], &transforms[i]); + properties->SetProperty(m_ulPropertyContainer, vr::Prop_CameraToHeadTransform_Matrix34, + &transforms[0], sizeof(transforms[0]), vr::k_unHmdMatrix34PropertyTag); + properties->SetProperty(m_ulPropertyContainer, vr::Prop_CameraToHeadTransforms_Matrix34_Array, + transforms, sizeof(transforms), vr::k_unHmdMatrix34PropertyTag); + } + // avoid "not fullscreen" warnings from vrmonitor - //vr::VRProperties()->SetBoolProperty(m_ulPropertyContainer, vr::Prop_IsOnDesktop_Bool, false); + vr::VRProperties()->SetBoolProperty(m_ulPropertyContainer, vr::Prop_IsOnDesktop_Bool, IsDisplayOnDesktop()); // clang-format on @@ -1365,6 +1578,8 @@ CDeviceDriver_Monado::Activate(vr::TrackedDeviceIndex_t unObjectId) void CDeviceDriver_Monado::Deactivate() { + if (m_camera) + m_camera->StopVideoStream(); m_poseUpdating = false; m_poseUpdateThread->join(); ovrd_log("Deactivate\n"); @@ -1379,6 +1594,8 @@ CDeviceDriver_Monado::EnterStandby() void * CDeviceDriver_Monado::GetComponent(const char *pchComponentNameAndVersion) { + if (m_camera && strcmp(pchComponentNameAndVersion, vr::IVRCameraComponent_Version) == 0) + return m_camera.get(); // clang-format off if (strcmp(pchComponentNameAndVersion, vr::IVRDisplayComponent_Version) == 0) { return (vr::IVRDisplayComponent *)this; @@ -1391,7 +1608,7 @@ CDeviceDriver_Monado::GetComponent(const char *pchComponentNameAndVersion) void CDeviceDriver_Monado::DebugRequest(const char *pchRequest, char *pchResponseBuffer, uint32_t unResponseBufferSize) { - //! @todo + m_bench.request(pchRequest, pchResponseBuffer, unResponseBufferSize); } static constexpr inline vr::HmdQuaternion_t @@ -1420,6 +1637,16 @@ CDeviceDriver_Monado::GetPose() m_relation_chain_push_pose_if_not_identity(&chain, offset); m_relation_chain_resolve(&chain, &rel); + if (debug_get_bool_option_steamvr_bench()) { + xrt_pose bench_pose = XRT_POSE_IDENTITY; + bench_pose.position.y = 1.6f; + float trigger; + const auto tracked = XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT; + if (m_bench.sample(bench_pose, trigger) || (rel.relation_flags & tracked) != tracked) + bench_relation(rel, bench_pose); + } + vr::DriverPose_t t = { // monado predicts pose "now", see xrt_device_get_tracked_pose .poseTimeOffset = 0, @@ -1445,7 +1672,12 @@ CDeviceDriver_Monado::GetPose() .shouldApplyHeadModel = !m_xdev->supported.position_tracking, .deviceIsConnected = true, }; - apply_pose(&rel, &t); + // A loss of positional tracking still has a finite held position. Dropping it sent Y=0 to games. + apply_pose(&rel, &t, XRT_SPACE_RELATION_POSITION_VALID_BIT); + if (!(rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT)) { + t.result = vr::TrackingResult_Running_OutOfRange; + t.poseIsValid = false; + } #ifdef DUMP_POSE ovrd_log("get hmd pose %f %f %f %f, %f %f %f\n", t.qRotation.x, t.qRotation.y, t.qRotation.z, t.qRotation.w, @@ -1496,10 +1728,7 @@ CDeviceDriver_Monado::GetRecommendedRenderTargetSize(uint32_t *pnWidth, uint32_t int scale = debug_get_num_option_scale_percentage(); float fscale = (float)scale / 100.f; - - - *pnWidth = m_xdev->hmd->screens[0].w_pixels * fscale; - *pnHeight = m_xdev->hmd->screens[0].h_pixels * fscale; + ovrd_get_recommended_render_target_size(m_xdev, fscale, pnWidth, pnHeight); ovrd_log("Render Target Size: %dx%d (%fx)\n", *pnWidth, *pnHeight, fscale); } @@ -1520,10 +1749,7 @@ CDeviceDriver_Monado::GetEyeOutputViewport( void CDeviceDriver_Monado::GetProjectionRaw(vr::EVREye eEye, float *pfLeft, float *pfRight, float *pfTop, float *pfBottom) { - *pfLeft = tanf(m_xdev->hmd->distortion.fov[eEye].angle_left); - *pfRight = tanf(m_xdev->hmd->distortion.fov[eEye].angle_right); - *pfTop = tanf(-m_xdev->hmd->distortion.fov[eEye].angle_up); - *pfBottom = tanf(-m_xdev->hmd->distortion.fov[eEye].angle_down); + ovrd_get_projection_raw(m_xdev->hmd->distortion.fov[eEye], pfLeft, pfRight, pfTop, pfBottom); ovrd_log("Projection Raw: L%f R%f T%f B%f\n", *pfLeft, *pfRight, *pfTop, *pfBottom); } @@ -1592,7 +1818,7 @@ public: virtual void Cleanup(); virtual const char *const * GetInterfaceVersions() { return vr::k_InterfaceVersions; } virtual void RunFrame(); - virtual bool ShouldBlockStandbyMode() { return false; } + virtual bool ShouldBlockStandbyMode() { return debug_get_bool_option_steamvr_bench(); } virtual void EnterStandby() {} virtual void LeaveStandby() {} virtual void HandleHapticEvent(vr::VREvent_t *event); @@ -1605,6 +1831,8 @@ private: struct xrt_space_overseer *m_xso = NULL; struct xrt_device *m_xhmd = NULL; + bool m_restoreBenchPause = false; + bool m_savedBenchPause = true; CDeviceDriver_Monado *m_MonadoDeviceDriver = NULL; CDeviceDriver_Monado_Controller *m_left = NULL; CDeviceDriver_Monado_Controller *m_right = NULL; @@ -1622,6 +1850,12 @@ CServerDriver_Monado::Init(vr::IVRDriverContext *pDriverContext) ovrd_log_init(vr::VRDriverLog()); ovrd_log("Initializing Monado driver\n"); + if (debug_get_bool_option_steamvr_bench()) { + vr::EVRSettingsError error = vr::VRSettingsError_None; + m_savedBenchPause = vr::VRSettings()->GetBool("power", "pauseCompositorOnStandby", &error); + m_restoreBenchPause = error == vr::VRSettingsError_None; + vr::VRSettings()->SetBool("power", "pauseCompositorOnStandby", false); + } //! @todo instance initialization is difficult to replicate @@ -1671,11 +1905,11 @@ CServerDriver_Monado::Init(vr::IVRDriverContext *pDriverContext) u_builder_setup_tracking_origins(m_xhmd, nullptr, left_xdev, right_xdev, nullptr, &offset); if (left_xdev) { - m_left = new CDeviceDriver_Monado_Controller(m_xinst, left_xdev, XRT_HAND_LEFT); + m_left = new CDeviceDriver_Monado_Controller(m_xinst, left_xdev, XRT_HAND_LEFT, m_xhmd); ovrd_log("Added left Controller: %s\n", left_xdev->str); } if (right_xdev) { - m_right = new CDeviceDriver_Monado_Controller(m_xinst, right_xdev, XRT_HAND_RIGHT); + m_right = new CDeviceDriver_Monado_Controller(m_xinst, right_xdev, XRT_HAND_RIGHT, m_xhmd); ovrd_log("Added right Controller: %s\n", right_xdev->str); } @@ -1685,6 +1919,10 @@ CServerDriver_Monado::Init(vr::IVRDriverContext *pDriverContext) void CServerDriver_Monado::Cleanup() { + if (m_restoreBenchPause) { + vr::VRSettings()->SetBool("power", "pauseCompositorOnStandby", m_savedBenchPause); + m_restoreBenchPause = false; + } if (m_MonadoDeviceDriver != NULL) { delete m_MonadoDeviceDriver; m_MonadoDeviceDriver = NULL; @@ -1742,13 +1980,17 @@ CServerDriver_Monado::HandleHapticEvent(vr::VREvent_t *event) SteamVRDriverControlOutput *control = &controller->m_output_controls.at(0); enum xrt_output_name name = control->monado_output_name; - ovrd_log("Haptic vibration %s, %d\n", control->steamvr_control_path, name); + ovrd_log("Haptic vibration %s, %d\n", control->steamvr_control_path.c_str(), name); controller->m_xdev->set_output(controller->m_xdev, name, &out); } void CServerDriver_Monado::RunFrame() { + if (m_MonadoDeviceDriver) { + m_MonadoDeviceDriver->UpdateRoomSetupGuard(); + m_MonadoDeviceDriver->UpdateDisplayFrequency(); + } if (m_left) { m_left->RunFrame(); } @@ -1762,7 +2004,8 @@ CServerDriver_Monado::RunFrame() switch (event.eventType) { case vr::VREvent_Input_HapticVibration: HandleHapticEvent(&event); break; case vr::VREvent_PropertyChanged: - // ovrd_log("Property changed\n"); + if (m_MonadoDeviceDriver && event.data.property.prop == vr::Prop_DisplayFrequency_Float) + m_MonadoDeviceDriver->UpdateDisplayFrequency(); break; case vr::VREvent_TrackedDeviceActivated: ovrd_log("Device activated %d\n", event.trackedDeviceIndex); diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp new file mode 100644 index 000000000..a7dcb095e --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp @@ -0,0 +1,180 @@ +// Copyright 2020,2024 Collabora, Ltd. +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include "math/m_api.h" +#include "math/m_space.h" +#include "openvr_driver.h" +#include + +inline void +copy_vec3(struct xrt_vec3 *from, double *to) +{ + to[0] = from->x; + to[1] = from->y; + to[2] = from->z; +} + +inline void +copy_quat(struct xrt_quat *from, vr::HmdQuaternion_t *to) +{ + to->x = from->x; + to->y = from->y; + to->z = from->z; + to->w = from->w; +} + +inline void +copy_pose_velocities(struct xrt_space_relation *rel, vr::DriverPose_t *m_pose) +{ + // Controller GetPose reuses DriverPose_t. SteamVR has no velocity validity bits, + // so an invalid Monado velocity must not leave the previous frame's prediction active. + for (auto &velocity : m_pose->vecVelocity) + velocity = 0; + for (auto &velocity : m_pose->vecAngularVelocity) + velocity = 0; + if ((rel->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0 && + std::isfinite(rel->linear_velocity.x) && std::isfinite(rel->linear_velocity.y) && + std::isfinite(rel->linear_velocity.z)) { + // linear velocity in world space + copy_vec3(&rel->linear_velocity, m_pose->vecVelocity); + } + + if ((rel->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0 && + std::isfinite(rel->angular_velocity.x) && std::isfinite(rel->angular_velocity.y) && + std::isfinite(rel->angular_velocity.z)) { + // angular velocity reported by monado in world space, + // expected by steamvr to be in "controller space" + struct xrt_quat orientation_inv; + math_quat_invert(&rel->pose.orientation, &orientation_inv); + + struct xrt_vec3 vel; + math_quat_rotate_derivative(&orientation_inv, &rel->angular_velocity, &vel); + + if (std::isfinite(vel.x) && std::isfinite(vel.y) && std::isfinite(vel.z)) + copy_vec3(&vel, m_pose->vecAngularVelocity); + } +} + +inline bool +finite_pose(const struct xrt_pose &p) +{ + float norm = p.orientation.x * p.orientation.x + p.orientation.y * p.orientation.y + + p.orientation.z * p.orientation.z + p.orientation.w * p.orientation.w; + return std::isfinite(norm) && norm >= 0.9f && norm <= 1.1f && std::isfinite(p.position.x) && + std::isfinite(p.position.y) && std::isfinite(p.position.z); +} + +inline void +apply_pose(struct xrt_space_relation *rel, + vr::DriverPose_t *m_pose, + enum xrt_space_relation_flags position_flag = XRT_SPACE_RELATION_POSITION_TRACKED_BIT, + bool require_position = false) +{ + if (!finite_pose(rel->pose)) { + for (auto &velocity : m_pose->vecVelocity) + velocity = 0; + for (auto &velocity : m_pose->vecAngularVelocity) + velocity = 0; + m_pose->result = vr::TrackingResult_Running_OutOfRange; + m_pose->poseIsValid = false; + return; + } + if ((rel->relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) != 0) { + copy_quat(&rel->pose.orientation, &m_pose->qRotation); + } else { + m_pose->result = vr::TrackingResult_Running_OutOfRange; + m_pose->poseIsValid = false; + } + if (require_position && (rel->relation_flags & position_flag) == 0) { + m_pose->result = vr::TrackingResult_Running_OutOfRange; + m_pose->poseIsValid = false; + } + if ((rel->relation_flags & position_flag) != 0) + copy_vec3(&rel->pose.position, m_pose->vecPosition); + copy_pose_velocities(rel, m_pose); +} + +inline void +transform_rift_s_touch_relation(struct xrt_space_relation *rel, + const struct xrt_pose *grip_to_model, + const struct xrt_pose *origin) +{ + const auto position_flags = rel->relation_flags & + (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + // m_relation_chain promotes orientation-only relations to positioned 3DoF devices and + // otherwise discards their estimated coordinates. Transform the estimate, then restore + // the controller's own confidence instead of publishing that promotion as a tracked fix. + if ((rel->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0) + rel->relation_flags = static_cast( + rel->relation_flags | XRT_SPACE_RELATION_POSITION_VALID_BIT); + struct xrt_relation_chain chain = {}; + m_relation_chain_push_pose_if_not_identity(&chain, grip_to_model); + m_relation_chain_push_relation(&chain, rel); + m_relation_chain_push_pose_if_not_identity(&chain, origin); + m_relation_chain_resolve(&chain, rel); + rel->relation_flags = static_cast( + (rel->relation_flags & ~(XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT)) | + position_flags); +} + +// Oculus' controller adapter admits orientation-valid poses, copies their estimated +// position even without PositionValid, and reports that case as Running_OutOfRange. +inline void +apply_rift_s_touch_pose(struct xrt_space_relation *rel, vr::DriverPose_t *m_pose) +{ + bool connected = m_pose->deviceIsConnected; + *m_pose = {}; + m_pose->deviceIsConnected = connected; + bool position_valid = (rel->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; + m_pose->willDriftInYaw = !position_valid; + m_pose->result = vr::TrackingResult_Uninitialized; + if ((rel->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) == 0) + return; + if (!finite_pose(rel->pose)) { + m_pose->result = vr::TrackingResult_Running_OutOfRange; + return; + } + m_pose->poseIsValid = true; + m_pose->result = position_valid ? vr::TrackingResult_Running_OK : vr::TrackingResult_Running_OutOfRange; + copy_quat(&rel->pose.orientation, &m_pose->qRotation); + copy_vec3(&rel->pose.position, m_pose->vecPosition); + copy_pose_velocities(rel, m_pose); +} + +/*! + * SteamVR draws a render model with its origin at the device pose, and its OpenXR runtime takes + * grip from the model's openxr_grip component. Monado reports grip, so the device pose is moved + * from grip to the model origin. Rift S Touch (oculus_rifts_controller_*): grip is +20.6 degrees + * about X at (+-0.007, -0.00183, 0.10195) m in the model, in the handle 10 cm behind the origin. + */ +inline struct xrt_pose +rift_s_touch_grip_to_model(enum xrt_hand hand) +{ + struct xrt_pose model_grip = XRT_POSE_IDENTITY; + const struct xrt_vec3 axis = {1, 0, 0}; + math_quat_from_angle_vector((float)DEG_TO_RAD(20.6), &axis, &model_grip.orientation); + model_grip.position = {hand == XRT_HAND_LEFT ? 0.007f : -0.007f, -0.00182941f, 0.1019482f}; + struct xrt_pose grip_model; + math_pose_invert(&model_grip, &grip_model); + return grip_model; +} + +/*! + * Resting hand position for a controller that has never had a position fix: 0.2 m to the side, + * 0.5 m below and 0.3 m in front of the head, following its heading only. The pose stays + * invalid; this only keeps SteamVR from drawing it at the tracking origin under the floor. + */ +inline struct xrt_vec3 +nominal_hand_position(const struct xrt_pose &head, enum xrt_hand hand) +{ + struct xrt_vec3 forward; + const struct xrt_vec3 minus_z = {0, 0, -1}; + math_quat_rotate_vec3(&head.orientation, &minus_z, &forward); + float length = std::hypot(forward.x, forward.z); + float fx = length > 1e-3f ? forward.x / length : 0, fz = length > 1e-3f ? forward.z / length : -1; + // Right is forward x up. + float side = hand == XRT_HAND_LEFT ? -0.2f : 0.2f; + return {head.position.x + 0.3f * fx - side * fz, head.position.y - 0.5f, head.position.z + 0.3f * fz + side * fx}; +} diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp new file mode 100644 index 000000000..90314e2be --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp @@ -0,0 +1,110 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once +#include "openvr.h" +#include "ovrd_roomsetup_frame.hpp" +#include +#include +#include + +namespace roomsetup { +// OpenVR mailbox and path interfaces use the same client connection as +// IVRSystem. +class Mailbox +{ +public: + virtual int + Register(const char *name, uint64_t *handle) = 0; + virtual int + Unregister(uint64_t handle) = 0; + virtual int + Send(uint64_t handle, const char *destination, const char *json) = 0; + virtual int + Read(uint64_t handle, char *buffer, uint32_t bytes, uint32_t *required) = 0; +}; +struct PathRead +{ + uint64_t path; + void *buffer; + uint32_t size, tag, required; + int error; + const char *name; +}; +class Paths +{ +public: + virtual int + Read(uint64_t root, PathRead *batch, uint32_t count) = 0; +}; + +inline std::optional +saved_floor(const vr::HmdMatrix34_t &standing, const vr::HmdMatrix34_t &setup) +{ + float raw_floor[3]{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + raw_floor[i] -= standing.m[j][i] * standing.m[j][3]; + float y = setup.m[1][3]; + for (int i = 0; i < 3; ++i) + y += setup.m[1][i] * raw_floor[i]; + return std::isfinite(y) ? std::optional{y} : std::nullopt; +} + +class SavedFloor +{ + Mailbox *mailbox; + Paths *paths; + uint64_t handle = 0; + SharedFrame frame{true}; + +public: + SavedFloor(Mailbox *messages, Paths *properties) : mailbox(messages), paths(properties) + { + if (mailbox && mailbox->Register("monado/roomsetup-floor", &handle) != 0) + handle = 0; + } + ~SavedFloor() + { + if (handle) + mailbox->Unregister(handle); + } + bool + transforms(vr::HmdMatrix34_t &standing, vr::HmdMatrix34_t &setup) + { + if (!paths) + return false; + PathRead read[] = {{0, &standing, sizeof(standing), 0, 0, 0, "/chaperone/standing/transform"}, + {0, &setup, sizeof(setup), 0, 0, 0, "/chaperone/room_setup/transform"}}; + return paths->Read(0x600000003, read, 2) == 0 && !read[0].error && !read[1].error && + read[0].tag == 20 && read[1].tag == 20; + } + void + update_frame(bool active) + { + frame.write(active); + } + bool + publish(bool initialize_manual = false) + { + if (!handle || !paths) + return false; + vr::HmdMatrix34_t standing{}, setup{}; + if (!transforms(standing, setup)) + return false; + auto y = saved_floor(standing, setup); + if (!y) + return false; + char message[128]; + snprintf(message, sizeof(message), "{\"type\":\"floor_detection_result\",\"pos\":%.9g}", *y); + bool sent = mailbox->Send(handle, "vrcompositor_systemlayer", message) == 0; + // The native manual step starts at tracking-origin Y=0. Seed it from the + // saved plane once; the first stick/laser adjustment selects Manual again. + if (sent && initialize_manual) + sent = mailbox->Send(handle, "vrcompositor_systemlayer", + "{\"type\":\"reset_playspace_floorheight\",\"value_" + "source\":2}") == 0; + fprintf(stderr, "monado-roomsetup-floor: saved_floor_setup_y=%.6f sent=%d\n", *y, sent); + return sent; + } +}; +} // namespace roomsetup diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_frame.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_frame.hpp new file mode 100644 index 000000000..2cb805ede --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_frame.hpp @@ -0,0 +1,182 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace roomsetup { +using Matrix = std::array; +inline Matrix +identity() +{ + return {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; +} +inline Matrix +multiply(const Matrix &a, const Matrix &b) +{ + Matrix out{}; + for (int i = 0; i < 4; ++i) + for (int j = 0; j < 4; ++j) + for (int k = 0; k < 4; ++k) + out[4 * i + j] += a[4 * i + k] * b[4 * k + j]; + return out; +} +inline Matrix +inverse_rigid(const Matrix &a) +{ + auto out = identity(); + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) + out[4 * i + j] = a[4 * j + i]; + for (int j = 0; j < 3; ++j) + out[4 * i + 3] -= out[4 * i + j] * a[4 * j + 3]; + } + return out; +} +inline uint64_t +monotonic_ns() +{ + timespec now{}; + clock_gettime(CLOCK_MONOTONIC, &now); + return uint64_t(now.tv_sec) * 1000000000 + now.tv_nsec; +} +struct FrameState +{ + uint64_t sequence, timestamp; + uint32_t active, reserved; +}; +class SharedFrame +{ + FrameState *state = nullptr; + std::string path = "/monado-roomsetup-frame-" + std::to_string(getuid()); + bool writer; + +public: + explicit SharedFrame(bool write) : writer(write) {} + ~SharedFrame() + { + if (state) + munmap(state, sizeof(FrameState)); + if (writer && state) + shm_unlink(path.c_str()); + } + bool + open() + { + if (state) + return true; + int fd = shm_open(path.c_str(), (writer ? O_CREAT | O_RDWR : O_RDONLY) | O_CLOEXEC, 0600); + if (fd < 0) + return false; + if (writer && ftruncate(fd, sizeof(FrameState)) != 0) { + close(fd); + return false; + } + struct stat info{}; + if (fstat(fd, &info) != 0 || info.st_size != sizeof(FrameState) || info.st_uid != getuid()) { + close(fd); + return false; + } + void *mapped = + mmap(nullptr, sizeof(FrameState), writer ? PROT_READ | PROT_WRITE : PROT_READ, MAP_SHARED, fd, 0); + close(fd); + if (mapped == MAP_FAILED) + return false; + state = static_cast(mapped); + return true; + } + void + write(bool active) + { + if (!writer || !open()) + return; + auto sequence = __atomic_load_n(&state->sequence, __ATOMIC_RELAXED); + __atomic_store_n(&state->sequence, sequence | 1, __ATOMIC_SEQ_CST); + state->timestamp = monotonic_ns(); + state->active = active; + __atomic_store_n(&state->sequence, (sequence | 1) + 1, __ATOMIC_RELEASE); + } + bool + read() + { + if (writer || !open()) + return false; + auto sequence = __atomic_load_n(&state->sequence, __ATOMIC_ACQUIRE); + if (sequence & 1) + return false; + FrameState value; + memcpy(&value, state, sizeof(value)); + if (sequence != __atomic_load_n(&state->sequence, __ATOMIC_ACQUIRE) || !value.active || + monotonic_ns() - value.timestamp > 200000000) + return false; + return true; + } +}; + +// RGB supplies no measured depth. In every Room View mode, retain capture-to-eye +// rotation but remove translation against the assumed dome and floor surface. +// The same eye ray then samples the same camera pixel at every mesh depth. +inline bool +correct_camera(void *bytes, size_t size) +{ + if (size != 400) + return false; + std::array values; + memcpy(values.data(), bytes, sizeof(values)); + if (!std::all_of(values.begin(), values.end(), [](float v) { return std::isfinite(v); })) + return false; + auto model = identity(); + for (int i = 0; i < 16; ++i) + if (std::abs(values[i] - model[i]) > 1e-5f) + return false; + // Exclude UI uniforms and the depth path, which has a nonidentity model. + if (std::abs(values[46]) < .5f || std::abs(values[47]) > .01f) + return false; + Matrix view, camera; + memcpy(view.data(), values.data() + 16, sizeof(view)); + memcpy(camera.data(), values.data() + 48, sizeof(camera)); + for (int i = 12; i < 16; ++i) + if (std::abs(view[i] - model[i]) > 1e-5f) + return false; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + float dot = 0; + for (int k = 0; k < 3; ++k) + dot += view[4 * i + k] * view[4 * j + k]; + if (std::abs(dot - (i == j ? 1.f : 0.f)) > .001f) + return false; + } + } + float dot = 0, norm = 0; + for (int i = 0; i < 3; ++i) { + dot += camera[8 + i] * camera[12 + i]; + norm += camera[12 + i] * camera[12 + i]; + } + if (!std::isfinite(norm) || norm < .25f) + return false; + float depth_scale = dot / norm; + if (!std::isfinite(depth_scale)) + return false; + for (int i = 0; i < 3; ++i) + if (std::abs(camera[8 + i] - depth_scale * camera[12 + i]) > .001f) + return false; + auto relative = multiply(camera, inverse_rigid(view)); + // Preserve the projection's near/far depth term while dropping translation. + relative[11] -= depth_scale * relative[15]; + relative[3] = relative[7] = relative[15] = 0; + auto fixed = multiply(relative, view); + if (!std::all_of(fixed.begin(), fixed.end(), [](float v) { return std::isfinite(v); })) + return false; + memcpy(static_cast(bytes) + 192, fixed.data(), sizeof(fixed)); + return true; +} +} // namespace roomsetup diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp new file mode 100644 index 000000000..36638c426 --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp @@ -0,0 +1,414 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +// Keep the pre-setup calibration if SteamVR's compositor dies during native drawing. +#include "openvr.h" +#include "ovrd_roomsetup_floor.hpp" +#include "../../drivers/rift_s/rift_s_world_control.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fs = std::filesystem; + +static std::string read_file(const fs::path &path) +{ + std::ifstream stream(path, std::ios::binary); + if (!stream) return {}; + stream.seekg(0, std::ios::end); + auto size = stream.tellg(); + if (size <= 0 || size > 4 * 1024 * 1024) return {}; + std::string bytes(static_cast(size), '\0'); + stream.seekg(0); + stream.read(bytes.data(), bytes.size()); + return stream ? bytes : std::string{}; +} + +static fs::path calibration_path(fs::path &log) +{ + const char *home = getenv("HOME"); + if (!home) return {}; + const char *config = getenv("XDG_CONFIG_HOME"); + fs::path registry = (config ? fs::path(config) : fs::path(home) / ".config") / "openvr/openvrpaths.vrpath"; + auto bytes = read_file(registry); + cJSON *json = cJSON_Parse(bytes.c_str()); + if (!json) return {}; + auto *entry = cJSON_GetArrayItem(cJSON_GetObjectItemCaseSensitive(json, "config"), 0); + fs::path path; + if (cJSON_IsString(entry) && entry->valuestring) path = fs::path(entry->valuestring) / "chaperone_info.vrchap"; + auto *log_entry = cJSON_GetArrayItem(cJSON_GetObjectItemCaseSensitive(json, "log"), 0); + if (cJSON_IsString(log_entry) && log_entry->valuestring) log = fs::path(log_entry->valuestring) / "vrcompositor.txt"; + cJSON_Delete(json); + return path; +} + +// Room setup is the only action that may establish a new physical world. +static void world_command(const char *command) +{ + char name[1024]; + rift_s_world_control_path("command", name, sizeof(name)); + fs::path path(name), temp = path.string() + ".tmp"; + std::ofstream stream(temp, std::ios::trunc); + if (!stream) return; + auto ns = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(); + stream << ns << " " << command << "\n"; + stream.close(); + if (stream) { std::error_code error; fs::rename(temp, path, error); } +} + +static std::string world_status() +{ + char path[1024]; + rift_s_world_control_path("status", path, sizeof(path)); + std::ifstream stream(path); + int64_t stamp = 0; + std::string status; + stream >> stamp >> status; + auto ns = std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(); + if (!stream || stamp > ns || ns - stamp > 2000000000LL) return {}; + return status; +} + +// Retain the delivered state only after OpenVR accepts it, so a transient failure retries. +static bool update_world_notification(vr::IVRNotifications ¬ifications, + vr::VROverlayHandle_t overlay, + const std::string &warning, + std::string &delivered, + vr::VRNotificationId &id) +{ + if (warning == delivered) return true; + if (id) { + auto error = notifications.RemoveNotification(id); + if (error != vr::VRNotificationError_OK && error != vr::VRNotificationError_InvalidNotificationId) + return false; + id = 0; + delivered.clear(); + } + if (!warning.empty()) { + vr::VRNotificationId created = 0; + if (notifications.CreateNotification(overlay, 0, vr::EVRNotificationType_Persistent, + warning.c_str(), vr::EVRNotificationStyle_Application, nullptr, &created) != + vr::VRNotificationError_OK) return false; + id = created; + } + delivered = warning; + return true; +} + +static int compositor_fd() +{ + std::error_code error; + for (const auto &entry : fs::directory_iterator("/proc", error)) { + auto name = entry.path().filename().string(); + if (name.empty() || name.find_first_not_of("0123456789") != std::string::npos) continue; + std::ifstream comm(entry.path() / "comm"); + std::string process; + std::getline(comm, process); + if (process != "vrcompositor") continue; + // Ignore another user's compositor and retain this process generation, not its reusable PID. + struct stat state{}; + if (stat(entry.path().c_str(), &state) != 0 || state.st_uid != getuid()) continue; + return static_cast(syscall(SYS_pidfd_open, std::stoi(name), 0)); + } + return -1; +} + +static bool exited(int fd) +{ + if (fd < 0) return true; + pollfd process{fd, POLLIN, 0}; + return poll(&process, 1, 0) > 0; +} + +// Read only new compositor diagnostics for this setup attempt. The assertion is +// visible before the process finishes crash handling and its pidfd becomes ready. +class OverflowLog +{ + fs::path path; + uint64_t cursor = 0; + std::string suffix; +public: + explicit OverflowLog(fs::path value) : path(std::move(value)) {} + void begin() + { + std::error_code error; + cursor = fs::file_size(path, error); + if (error) cursor = 0; + suffix.clear(); + } + bool overflowed() + { + std::error_code error; + uint64_t size = fs::file_size(path, error); + if (error) return false; + if (size < cursor) { cursor = 0; suffix.clear(); } + if (size == cursor) return false; + std::ifstream stream(path, std::ios::binary); + stream.seekg(cursor); + std::string bytes(static_cast(std::min(size - cursor, 65536)), '\0'); + stream.read(bytes.data(), bytes.size()); + bytes.resize(static_cast(stream.gcount())); + cursor += bytes.size(); + suffix += bytes; + bool found = suffix.find("Vulkan transfer ring buffer overflow") != std::string::npos; + if (suffix.size() > 128) suffix.erase(0, suffix.size() - 128); + return found; + } +}; + +struct Snapshot +{ + std::string file; + std::vector exported; +}; + +static std::optional snapshot(vr::IVRChaperoneSetup &setup, const fs::path &path) +{ + Snapshot saved{read_file(path), {}}; + uint32_t size = 0; + setup.ExportLiveToBuffer(nullptr, &size); + if (saved.file.empty() || size == 0 || size > 4 * 1024 * 1024) return std::nullopt; + saved.exported.resize(size + 1, 0); + if (!setup.ExportLiveToBuffer(saved.exported.data(), &size)) return std::nullopt; + return saved; +} + +static bool restore(vr::IVRChaperoneSetup &setup, const fs::path &path, const Snapshot &saved) +{ + setup.RevertWorkingCopy(); + if (!setup.ImportFromBufferToWorking(saved.exported.data(), 0) || + !setup.CommitWorkingCopy(vr::EChaperoneConfigFile_Live)) return false; + // Commit updates the server's live calibration. Restore the complete file too, + // including other universes and formatting that OpenVR's export does not retain. + auto temporary = path; + temporary += ".monado-guard-" + std::to_string(getpid()); + { + std::ofstream stream(temporary, std::ios::binary | std::ios::trunc); + stream.write(saved.file.data(), saved.file.size()); + stream.flush(); + if (!stream) return false; + } + std::error_code error; + fs::rename(temporary, path, error); + if (error) { fs::remove(temporary, error); return false; } + setup.ReloadFromDisk(vr::EChaperoneConfigFile_Live); + return read_file(path) == saved.file; +} + +// Capture public coordinate state on transitions without changing calibration. +static void log_coordinates(vr::IVRSystem &system, vr::IVRChaperoneSetup &setup, uint32_t event, uint32_t step) +{ + auto raw_to_standing = system.GetRawZeroPoseToStandingAbsoluteTrackingPose(); + auto seated_to_standing = system.GetSeatedZeroPoseToStandingAbsoluteTrackingPose(); + vr::HmdMatrix34_t standing_to_raw{}; + bool working = setup.GetWorkingStandingZeroPoseToRawTrackingPose(&standing_to_raw); + vr::TrackedDevicePose_t head{}; + system.GetDeviceToAbsoluteTrackingPose(vr::TrackingUniverseRawAndUncalibrated, 0, &head, 1); + auto universe = system.GetUint64TrackedDeviceProperty(0, vr::Prop_CurrentUniverseId_Uint64); + auto ticks = std::chrono::duration_cast(std::chrono::steady_clock::now().time_since_epoch()).count(); + fprintf(stderr, "monado-coordinates: event=%u step=%u monotonic_ns=%lld universe=%llu head_valid=%d tracking=%d raw_head=(%.6f,%.6f,%.6f) floor_raw_y=%.6f working=%d\n", + event, step, (long long)ticks, (unsigned long long)universe, head.bPoseIsValid, head.eTrackingResult, + head.mDeviceToAbsoluteTracking.m[0][3], head.mDeviceToAbsoluteTracking.m[1][3], + head.mDeviceToAbsoluteTracking.m[2][3], standing_to_raw.m[1][3], working); + auto log_matrix = [](const char *name, const vr::HmdMatrix34_t &m) { + fprintf(stderr, "monado-coordinates: %s=[%.6f %.6f %.6f %.6f; %.6f %.6f %.6f %.6f; %.6f %.6f %.6f %.6f]\n", + name, m.m[0][0], m.m[0][1], m.m[0][2], m.m[0][3], m.m[1][0], m.m[1][1], m.m[1][2], m.m[1][3], + m.m[2][0], m.m[2][1], m.m[2][2], m.m[2][3]); + }; + log_matrix("raw_to_standing", raw_to_standing); + log_matrix("seated_to_standing", seated_to_standing); + if (working) log_matrix("working_standing_to_raw", standing_to_raw); +} + +int main(int argc, char **argv) +{ + if (argc != 4) return 1; + int parent = static_cast(syscall(SYS_pidfd_open, std::atoi(argv[2]), 0)); + int ready = std::atoi(argv[3]); + void *library = dlopen(argv[1], RTLD_NOW | RTLD_LOCAL); + if (!library || parent < 0) return 1; + auto init = reinterpret_cast(dlsym(library, "VR_InitInternal2")); + auto interface = reinterpret_cast(dlsym(library, "VR_GetGenericInterface")); + auto shutdown = reinterpret_cast(dlsym(library, "VR_ShutdownInternal")); + if (!init || !interface || !shutdown) return 1; + vr::EVRInitError error = vr::VRInitError_Unknown; + for (unsigned attempt = 0; attempt < 100 && !exited(parent); ++attempt) { + init(&error, vr::VRApplication_Background, nullptr); + if (error == vr::VRInitError_None) break; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + if (error != vr::VRInitError_None) return 1; + auto *system = static_cast(interface(vr::IVRSystem_Version, &error)); + auto *setup = static_cast(interface(vr::IVRChaperoneSetup_Version, &error)); + auto *overlays = static_cast(interface(vr::IVROverlay_Version, &error)); + auto *notifications = static_cast(interface(vr::IVRNotifications_Version, &error)); + fs::path log_path; + fs::path path = calibration_path(log_path); + OverflowLog diagnostics(log_path); + if (!system || !setup || path.empty() || !snapshot(*setup, path)) { shutdown(); return 1; } + vr::VROverlayHandle_t overlay = vr::k_ulOverlayHandleInvalid; + if (overlays) overlays->CreateOverlay("monado.roomsetup.guard", "Monado playspace setup", &overlay); + if (write(ready, "1", 1) != 1) { shutdown(); return 1; } + fprintf(stderr, "monado-roomsetup-guard: ready\n"); + std::optional saved; + int compositor = -1; + uint32_t step = 0; + bool failed = false, failure_handled = false, native_commit = false; + bool overflow_detected = false; + auto commit_deadline = std::chrono::steady_clock::time_point{}; + auto exit_deadline = std::chrono::steady_clock::time_point{}; + auto compositor_failed = [&] { + overflow_detected |= diagnostics.overflowed(); + return overflow_detected || exited(compositor); + }; + auto drawing = [](uint32_t value) { return value == 5 || value == 7; }; + auto repair = [&] { + failure_handled = true; + // Hide setup until a fresh SteamVR session, while retaining late-Accept protection. + if (write(ready, "0", 1) != 1) return false; + bool ok = saved && restore(*setup, path, *saved); + fprintf(stderr, "monado-roomsetup-guard: previous calibration restore=%s\n", ok ? "ok" : "FAILED"); + if (notifications && overlay != vr::k_ulOverlayHandleInvalid) { + vr::VRNotificationId id; + notifications->CreateNotification(overlay, 0, vr::EVRNotificationType_Persistent, + ok ? "Playspace setup failed. Previous boundary restored. Restart SteamVR before drawing again." + : "Playspace setup failed. Calibration restore failed. Stop using VR and restore your saved boundary.", + vr::EVRNotificationStyle_Application, nullptr, &id); + } + return ok; + }; + auto *mailbox = static_cast(interface("IVRMailbox_001", &error)); + auto *paths = static_cast(interface("IVRPaths_001", &error)); + auto floor = std::make_unique(mailbox, paths); + floor->publish(); + log_coordinates(*system, *setup, 0, step); + bool floor_pending = false; + auto floor_deadline = std::chrono::steady_clock::time_point{}; + bool running = true; + bool world_setup = false; + auto world_cancel_deadline = std::chrono::steady_clock::time_point{}; + std::string world_warning; + vr::VRNotificationId world_notification = 0; + auto next_notification_attempt = std::chrono::steady_clock::time_point{}; + while (running && !exited(parent)) { + // Test before consuming Accept's events, including a compositor that exits immediately before Accept. + bool recent_exit = std::chrono::steady_clock::now() <= exit_deadline; + if ((drawing(step) || recent_exit) && saved && !failure_handled && + compositor_failed()) { + failed = true; + fprintf(stderr, "monado-roomsetup-guard: native setup failure=%s\n", + overflow_detected ? "transfer_ring_overflow" : "compositor_exit"); + if (!repair()) break; + } + vr::VREvent_t event{}; + while (system->PollNextEvent(&event, sizeof(event))) { + if (event.eventType == 809) { + // SteamVR 2.17's native setup step event is reserved in the public header. + uint32_t next = 0; + memcpy(&next, &event.data, sizeof(next)); + if (drawing(next) && !drawing(step) && !failed) { + saved = snapshot(*setup, path); + diagnostics.begin(); + overflow_detected = false; + exit_deadline = {}; + if (compositor >= 0) close(compositor); + compositor = compositor_fd(); + if (!saved || exited(compositor)) { running = false; break; } + fprintf(stderr, "monado-roomsetup-guard: snapshot step=%u\n", next); + } + if (next == 0 && drawing(step)) { + native_commit = true; + exit_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(5); + commit_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2); + if (saved && compositor_failed()) failed = true; + } + if ((next == 2 || next == 3 || drawing(next)) && !world_setup && !failed) { + world_setup = true; + world_cancel_deadline = {}; + world_command("begin"); + } + if (next == 0 && world_setup) + world_cancel_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(2); + step = next; + // Native Manual mode retains stick adjustments even when a result arrives. + if (next == 2 || next == 3 || next == 5 || next == 7) { + floor_pending = true; + floor_deadline = std::chrono::steady_clock::now() + std::chrono::seconds(1); + } + } + if (event.eventType == vr::VREvent_ChaperoneRoomSetupFinished && native_commit) { + // Cancel has the same step-zero event, but no immediate commit. + // A later unrelated calibration edit must not inherit that attempt. + bool immediate = std::chrono::steady_clock::now() <= commit_deadline; + native_commit = false; // Our restore also emits this event. + if (immediate && saved && compositor_failed()) failed = true; + if (immediate && failed && !repair()) { running = false; break; } + if (immediate && world_setup && !failed) { + world_command("save"); + world_setup = false; + world_cancel_deadline = {}; + } + } + if (event.eventType == 809 || event.eventType == vr::VREvent_ChaperoneUniverseHasChanged || + event.eventType == vr::VREvent_ChaperoneDataHasChanged || event.eventType == vr::VREvent_ChaperoneRoomSetupFinished || + event.eventType == vr::VREvent_StandingZeroPoseReset || event.eventType == vr::VREvent_SeatedZeroPoseReset || + event.eventType == vr::VREvent_RoomViewShown || event.eventType == vr::VREvent_RoomViewHidden || + event.eventType == vr::VREvent_DashboardActivated || event.eventType == vr::VREvent_DashboardDeactivated) + log_coordinates(*system, *setup, event.eventType, step); + if (event.eventType == vr::VREvent_Quit) running = false; + } + if (world_setup && world_cancel_deadline != std::chrono::steady_clock::time_point{} && + std::chrono::steady_clock::now() >= world_cancel_deadline) { + world_command("cancel"); + world_setup = false; + world_cancel_deadline = {}; + } + auto status = world_status(); + std::string warning; + if (status == "save_failed") + warning = "Visual boundary map could not be saved. Check Monado config write access before restarting VR."; + else if (status == "unanchored") + warning = "Boundary has no visual anchor. Complete room setup once to save its physical location."; + else if (status == "relocalizing") + warning = "Boundary not confirmed, look around. Temporary tracking is active."; + auto now = std::chrono::steady_clock::now(); + if (warning != world_warning && notifications && overlays && now >= next_notification_attempt) { + next_notification_attempt = now + std::chrono::seconds(1); + if (overlay == vr::k_ulOverlayHandleInvalid) + overlays->CreateOverlay("monado.roomsetup.guard", "Monado playspace setup", &overlay); + if (overlay != vr::k_ulOverlayHandleInvalid) + update_world_notification(*notifications, overlay, warning, world_warning, world_notification); + } + if (floor_pending) { + if (step == 0 || floor->publish(step == 2) || std::chrono::steady_clock::now() > floor_deadline) + floor_pending = false; + } + floor->update_frame(step == 2 || step == 3 || step == 4 || step == 5 || step == 7); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } + if (world_setup) world_command("cancel"); + if (world_notification && notifications) notifications->RemoveNotification(world_notification); + if (overlays && overlay != vr::k_ulOverlayHandleInvalid) overlays->DestroyOverlay(overlay); + if (compositor >= 0) close(compositor); + close(parent); + close(ready); + floor.reset(); + shutdown(); + dlclose(library); + return running ? 1 : 0; +} diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard_process.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard_process.hpp new file mode 100644 index 000000000..a5ed82713 --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard_process.hpp @@ -0,0 +1,83 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once +#ifdef __linux__ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +extern char **environ; +static void roomsetup_guard_module() {} + +// Native setup is advertised only after the session-owned guard acknowledges readiness. +class RoomSetupGuardProcess +{ + pid_t pid = -1; + int acknowledgement = -1; + bool acknowledged = false; +public: + ~RoomSetupGuardProcess() + { + if (acknowledgement >= 0) close(acknowledgement); + if (pid > 0 && waitpid(pid, nullptr, WNOHANG) == 0) { + kill(pid, SIGTERM); + waitpid(pid, nullptr, 0); + } + } + bool start() + { + Dl_info module{}; + char server[4096]; + ssize_t length = readlink("/proc/self/exe", server, sizeof(server) - 1); + if (length <= 0 || !dladdr(reinterpret_cast(roomsetup_guard_module), &module)) return false; + server[length] = 0; + auto directory = std::filesystem::path(module.dli_fname).parent_path(); + std::string executable = (directory / "monado-roomsetup-guard").string(); + std::string library = (std::filesystem::path(server).parent_path() / "libopenvr_api.so").string(); + std::string parent = std::to_string(getpid()); + int descriptors[2]; + if (pipe2(descriptors, O_CLOEXEC | O_NONBLOCK) != 0) return false; + posix_spawn_file_actions_t actions; + posix_spawn_file_actions_init(&actions); + constexpr int child_acknowledgement = 198; + posix_spawn_file_actions_adddup2(&actions, descriptors[1], child_acknowledgement); + char destination[] = "198"; + char *arguments[] = {executable.data(), library.data(), parent.data(), destination, nullptr}; + int error = posix_spawn(&pid, executable.c_str(), &actions, nullptr, arguments, environ); + posix_spawn_file_actions_destroy(&actions); + close(descriptors[1]); + if (error) { fprintf(stderr, "monado-roomsetup-guard: spawn %s: %s\n", executable.c_str(), strerror(error)); close(descriptors[0]); pid = -1; return false; } + acknowledgement = descriptors[0]; + return true; + } + bool ready() + { + if (pid <= 0) return false; + if (waitpid(pid, nullptr, WNOHANG) == pid) { pid = -1; acknowledged = false; } + if (acknowledgement >= 0) { + char value = 0; + ssize_t count = read(acknowledgement, &value, 1); + if (count == 1) acknowledged = pid > 0 && value == '1'; + if (count == 0) { + acknowledged = false; + close(acknowledgement); + acknowledgement = -1; + } + } + return acknowledged; + } +}; +#else +class RoomSetupGuardProcess +{ +public: + bool start() { return false; } + bool ready() { return false; } +}; +#endif diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_vulkan.cpp b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_vulkan.cpp new file mode 100644 index 000000000..15e052ae0 --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_vulkan.cpp @@ -0,0 +1,1089 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +// SteamVR 2.17's sparse room-setup SDF upload needs defined storage, including +// before its first stroke. Its playspace meshes also upload 32-bit indices but +// bind them as 16-bit. Recognize those uploads without inspecting shader code. +#include +#include +#include "rifts_passthrough_shm.h" +#include "ovrd_roomsetup_frame.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { +struct Instance +{ + VkInstance handle; + PFN_vkGetInstanceProcAddr gipa; + bool enabled; +}; +struct Image +{ + VkExtent3D extent; + bool empty; + bool linear; + bool initialized = false; + VkDeviceMemory memory = VK_NULL_HANDLE; + VkDeviceSize offset = 0; + VkSubresourceLayout layout{}; +}; +struct IndexUpload +{ + uint32_t count; + uint32_t maximum; +}; +struct Buffer +{ + VkDeviceSize size; + VkBufferUsageFlags usage; + VkDeviceMemory memory = VK_NULL_HANDLE; + VkDeviceSize memory_offset = 0; + std::optional indices = std::nullopt; + uint64_t id = 0; + uint64_t fixes = 0; +}; +struct Mapping +{ + void *data; + VkDeviceSize offset; + VkDeviceSize size; +}; +// SteamVR 2.17.10 Linux x86-64 stores the mapped pointer, buffer, memory, +// and used bytes consecutively. The Vulkan objects provide the capacity. +// Only this exact executable build is supported; no executable bytes change. +struct RingFields +{ + uintptr_t mapped; + VkBuffer buffer; + VkDeviceMemory memory; + uint64_t used; +}; +static_assert(sizeof(RingFields) == 32 && offsetof(RingFields, used) == 24); +constexpr uint64_t ring_capacity = 256ULL * 1024 * 1024; +constexpr uint64_t ring_margin = 64ULL * 1024 * 1024; + +bool local_read(uintptr_t address, void *out, size_t bytes) +{ + iovec local{out, bytes}, remote{reinterpret_cast(address), bytes}; + return syscall(SYS_process_vm_readv, getpid(), &local, 1, &remote, 1, 0) == static_cast(bytes); +} +bool local_write(uintptr_t address, uint64_t value) +{ + iovec local{&value, sizeof(value)}, remote{reinterpret_cast(address), sizeof(value)}; + return syscall(SYS_process_vm_writev, getpid(), &local, 1, &remote, 1, 0) == static_cast(sizeof(value)); +} + +bool supported_ring_build() +{ + // GNU build IDs identify the supported SteamVR executable without a code signature. + constexpr std::array supported{ + 0xf5, 0xc5, 0x60, 0xdc, 0xaa, 0x16, 0xd7, 0xb2, 0x62, 0x5e, + 0xce, 0x68, 0x4b, 0x02, 0x39, 0x65, 0x05, 0xe0, 0x76, 0x8f}; + int fd = open("/proc/self/exe", O_RDONLY | O_CLOEXEC); + if (fd < 0) return false; + Elf64_Ehdr header{}; + bool valid = pread(fd, &header, sizeof(header), 0) == sizeof(header) && + memcmp(header.e_ident, ELFMAG, SELFMAG) == 0 && header.e_ident[EI_CLASS] == ELFCLASS64 && + header.e_ident[EI_DATA] == ELFDATA2LSB && header.e_machine == EM_X86_64 && + header.e_phentsize == sizeof(Elf64_Phdr) && header.e_phnum < 128; + bool matched = false; + for (uint16_t i = 0; valid && i < header.e_phnum; ++i) { + Elf64_Phdr segment{}; + if (header.e_phoff > INT64_MAX - uint64_t(i) * sizeof(segment) || + pread(fd, &segment, sizeof(segment), header.e_phoff + uint64_t(i) * sizeof(segment)) != sizeof(segment)) break; + if (segment.p_type != PT_NOTE || segment.p_filesz > 65536 || segment.p_offset > INT64_MAX) continue; + std::vector bytes(segment.p_filesz); + if (pread(fd, bytes.data(), bytes.size(), segment.p_offset) != static_cast(bytes.size())) continue; + for (size_t offset = 0; offset + sizeof(Elf64_Nhdr) <= bytes.size();) { + Elf64_Nhdr note{}; + memcpy(¬e, bytes.data() + offset, sizeof(note)); + offset += sizeof(note); + uint64_t name_size = (uint64_t(note.n_namesz) + 3) & ~uint64_t{3}; + uint64_t data_size = (uint64_t(note.n_descsz) + 3) & ~uint64_t{3}; + if (name_size > bytes.size() - offset || data_size > bytes.size() - offset - name_size) break; + if (note.n_type == NT_GNU_BUILD_ID && note.n_namesz == 4 && note.n_descsz == supported.size() && + memcmp(bytes.data() + offset, "GNU", 4) == 0 && + memcmp(bytes.data() + offset + name_size, supported.data(), supported.size()) == 0) matched = true; + offset += name_size + data_size; + } + } + close(fd); + return matched; +} + +struct RingCommand +{ + bool submitted = false; + bool ended = false; +}; +struct RingRecovery +{ + bool disabled = false; + uintptr_t slot = 0; + RingFields identity{}; + VkQueue queue = VK_NULL_HANDLE; + pid_t owner = 0; + uint64_t copied_end = 0; + uint64_t resets = 0; + std::unordered_map commands; + + void disable(const char *reason) + { + if (!disabled) fprintf(stderr, "monado-ring: disabled: %s\n", reason); + disabled = true; + } + bool matches(const RingFields &value, const Buffer &buffer, const Mapping &map) const + { + return value.mapped == reinterpret_cast(map.data) && value.buffer == identity.buffer && + value.memory == identity.memory && value.used <= ring_capacity && + buffer.size == ring_capacity && buffer.usage == VK_BUFFER_USAGE_TRANSFER_SRC_BIT && + buffer.memory == identity.memory && buffer.memory_offset == 0 && map.offset == 0 && + (map.size == VK_WHOLE_SIZE || map.size >= ring_capacity); + } + bool pending() const + { + return std::any_of(commands.begin(), commands.end(), [](const auto &entry) { return !entry.second.submitted; }); + } + bool same_thread(bool establish = false) + { + auto current = static_cast(syscall(SYS_gettid)); + // Startup records uploads on the initializing thread and hands its command + // buffer to Render. Establish ownership only after that first submission. + if (owner && owner != current) { disable("upload author changed after first submit"); return false; } + if (establish) owner = current; + return true; + } + void copy(VkCommandBuffer command, uint64_t end) + { + if (disabled || !same_thread()) return; + auto entry = commands.try_emplace(command).first; + if (entry->second.submitted || entry->second.ended) { disable("copy outside a fresh recording"); return; } + if (end != UINT64_MAX && end > ring_capacity) { disable("copy outside the ring"); return; } + copied_end = end; + } + bool submitted(VkQueue target, const std::vector &batch) + { + bool touched = false; + for (auto command : batch) { + auto found = commands.find(command); + if (found == commands.end()) continue; + if (found->second.submitted || !found->second.ended) { disable("upload resubmitted or still recording"); return false; } + found->second.submitted = true; + touched = true; + } + if (!touched || disabled) return false; + if (queue && queue != target) { disable("transfer queue changed"); return false; } + queue = target; + return same_thread(true); + } + bool ready(uint64_t used) const + { + return !disabled && queue && !pending() && used == copied_end && used >= ring_capacity - ring_margin && used <= ring_capacity; + } +}; + +struct Draw +{ + VkPipeline pipeline = VK_NULL_HANDLE; + VkBuffer vertex = VK_NULL_HANDLE; + VkBuffer index = VK_NULL_HANDLE; + VkDeviceSize vertex_offset = 0; + VkDeviceSize index_offset = 0; + VkIndexType index_type = VK_INDEX_TYPE_UINT16; +}; +struct Device +{ + PFN_vkGetDeviceProcAddr gdpa; + VkDevice handle; + bool enabled; + std::unordered_map images{}; + std::unordered_map buffers{}; + std::unordered_map strides{}; + std::unordered_map draws{}; + std::unordered_map mappings{}; + bool index_fix = true; + bool sdf_fix = true; + bool ring_allowed = false; + std::unordered_map rings{}; + std::unordered_map formats{}; + uint64_t next_buffer_id = 0; +}; +std::mutex mutex; +std::unordered_map instances; +std::unordered_map> devices; + +void *key(const void *handle) +{ + return *static_cast(handle); +} + +bool monado_compositor() +{ + char executable[4096]; + ssize_t length = readlink("/proc/self/exe", executable, sizeof(executable) - 1); + if (length <= 0) return false; + executable[length] = '\0'; + const char *name = strrchr(executable, '/'); + if (!name || strcmp(name + 1, "vrcompositor") != 0) return false; + int fd = open(RIFTS_PASSTHROUGH_SHM_PATH, O_RDONLY | O_NOFOLLOW | O_CLOEXEC); + if (fd < 0) return false; + uint32_t header[4]{}; + ssize_t count = pread(fd, header, sizeof(header), 0); + struct flock owner{}; + owner.l_type = F_RDLCK; + owner.l_whence = SEEK_SET; + bool live = fcntl(fd, F_GETLK, &owner) == 0 && owner.l_type == F_WRLCK && owner.l_pid > 0; + close(fd); + return live && count == sizeof(header) && header[0] == RIFTS_PASSTHROUGH_MAGIC && + header[1] == RIFTS_PASSTHROUGH_VERSION && header[2] == 640 && header[3] == 320; +} + +std::shared_ptr device_for(const void *handle) +{ + std::lock_guard guard(mutex); + auto found = devices.find(key(handle)); + return found == devices.end() ? nullptr : found->second; +} + +template T next(const std::shared_ptr &device, const char *name) +{ + return reinterpret_cast(device->gdpa(device->handle, name)); +} + +void identify_ring(const std::shared_ptr &d, VkDeviceMemory memory, void **out) +{ + if (!d->ring_allowed) return; + for (const auto &[buffer, state] : d->buffers) { + if (state.memory != memory || state.size != ring_capacity || state.usage != VK_BUFFER_USAGE_TRANSFER_SRC_BIT) continue; + if (d->rings.count(buffer)) { d->rings.at(buffer).disable("ring remapped"); continue; } + RingRecovery recovery{}; + recovery.slot = reinterpret_cast(out); + recovery.identity.buffer = buffer; + recovery.identity.memory = memory; + RingFields fields{}; + if (local_read(recovery.slot, &fields, sizeof(fields)) && recovery.matches(fields, state, d->mappings.at(memory))) { + recovery.identity = fields; + fprintf(stderr, "monado-ring: allocator validated, capacity=%llu\n", (unsigned long long)ring_capacity); + } else recovery.disable("allocator layout or mapping mismatch"); + d->rings.emplace(buffer, std::move(recovery)); + } +} +std::optional validate_ring(const std::shared_ptr &d, RingRecovery &ring) +{ + if (ring.disabled) return std::nullopt; + auto buffer = d->buffers.find(ring.identity.buffer); + auto mapping = d->mappings.find(ring.identity.memory); + RingFields fields{}; + if (buffer == d->buffers.end() || mapping == d->mappings.end() || + !local_read(ring.slot, &fields, sizeof(fields)) || !ring.matches(fields, buffer->second, mapping->second)) { + ring.disable("allocator identity changed"); + return std::nullopt; + } + return fields; +} +void ring_copy(const std::shared_ptr &d, VkCommandBuffer command, VkBuffer buffer, uint64_t end) +{ + auto ring = d->rings.find(buffer); + if (ring == d->rings.end() || !validate_ring(d, ring->second)) return; + ring->second.copy(command, end); +} +void recover_rings(const std::shared_ptr &d, VkQueue queue, const std::vector &commands) +{ + for (auto &[buffer, ring] : d->rings) { + if (ring.disabled || !ring.submitted(queue, commands)) continue; + auto before = validate_ring(d, ring); + if (!before || !ring.ready(before->used)) continue; + auto wait = next(d, "vkQueueWaitIdle"); + if (!wait) { ring.disable("transfer queue wait unavailable"); continue; } + auto start = std::chrono::steady_clock::now(); + auto result = wait(queue); + auto elapsed = std::chrono::duration_cast(std::chrono::steady_clock::now() - start).count(); + auto after = validate_ring(d, ring); + // Waiting drains submitted reads. It cannot drain a still-recording upload. + // A cursor change during the wait means an unobserved CPU reservation exists. + if (result != VK_SUCCESS || !after || after->used != before->used || !ring.ready(after->used)) { + ring.disable("queue drain failed or reservation changed during drain"); + continue; + } + if (!local_write(ring.slot + offsetof(RingFields, used), 0)) { ring.disable("cursor write failed"); continue; } + ring.copied_end = 0; + fprintf(stderr, "monado-ring: reset=%llu used=%llu stall_us=%lld\n", (unsigned long long)++ring.resets, + (unsigned long long)before->used, (long long)elapsed); + } +} + +bool sdf_image(const VkImageCreateInfo &info) +{ + return info.imageType == VK_IMAGE_TYPE_2D && info.format == VK_FORMAT_R8_UNORM && + info.extent.depth == 1 && info.mipLevels == 1 && info.arrayLayers == 1 && + info.samples == VK_SAMPLE_COUNT_1_BIT && info.flags == 0 && + ((info.tiling == VK_IMAGE_TILING_LINEAR && + info.usage == (VK_IMAGE_USAGE_TRANSFER_SRC_BIT | VK_IMAGE_USAGE_TRANSFER_DST_BIT)) || + (info.tiling == VK_IMAGE_TILING_OPTIMAL && + info.usage == (VK_IMAGE_USAGE_TRANSFER_SRC_BIT | VK_IMAGE_USAGE_TRANSFER_DST_BIT | + VK_IMAGE_USAGE_SAMPLED_BIT))); +} +// These are the API-visible layouts of the playspace SDF and colored outline. +// Normal compositor/UI meshes and primitive-restart pipelines are excluded. +uint32_t playspace_stride(const VkGraphicsPipelineCreateInfo &info) +{ + auto *assembly = info.pInputAssemblyState; + auto *vertex = info.pVertexInputState; + if (!assembly || assembly->topology != VK_PRIMITIVE_TOPOLOGY_TRIANGLE_LIST || + assembly->primitiveRestartEnable || !vertex || vertex->vertexBindingDescriptionCount != 1 || + vertex->vertexAttributeDescriptionCount != 2) return 0; + const auto &binding = vertex->pVertexBindingDescriptions[0]; + if (binding.binding != 0 || binding.inputRate != VK_VERTEX_INPUT_RATE_VERTEX) return 0; + std::array attributes{}; + for (uint32_t i = 0; i < 2; ++i) { + const auto &attribute = vertex->pVertexAttributeDescriptions[i]; + if (attribute.location > 1 || attribute.binding != 0) return 0; + attributes[attribute.location] = attribute; + } + if (vertex->pVertexAttributeDescriptions[0].location == vertex->pVertexAttributeDescriptions[1].location || + attributes[0].offset != 0) return 0; + if (binding.stride == 16 && attributes[0].format == VK_FORMAT_R32G32_SFLOAT && + attributes[1].format == VK_FORMAT_R32G32_SFLOAT && attributes[1].offset == 8) return 16; + if (binding.stride == 28 && attributes[0].format == VK_FORMAT_R32G32B32_SFLOAT && + attributes[1].format == VK_FORMAT_R32G32B32A32_SFLOAT && attributes[1].offset == 12) return 28; + return 0; +} + +std::optional index_upload(VkDeviceSize size, const void *data) +{ + // Inspect bounded uploads, including copies from a mapped staging ring. + // Require complete 32-bit triangles, + // zero upper halves throughout, and at least one nondegenerate triangle. + if (size < 12 || size > 4 * 1024 * 1024 || size % 12) return std::nullopt; + uint32_t maximum = 0; + bool triangle = false; + auto *bytes = static_cast(data); + for (VkDeviceSize offset = 0; offset < size; offset += 12) { + std::array indices; + memcpy(indices.data(), bytes + offset, 12); + for (auto index : indices) { + if (index > std::numeric_limits::max()) return std::nullopt; + maximum = std::max(maximum, index); + } + triangle |= indices[0] != indices[1] && indices[0] != indices[2] && indices[1] != indices[2]; + } + if (!triangle) return std::nullopt; + return IndexUpload{static_cast(size / 4), maximum}; +} + +// SteamVR 2.17 direct mode starts its frame timing at 90 Hz and adopts a display mode's refresh rate +// only when that mode matches the desired rate better than an earlier matching mode. A panel with a +// single mode (Rift S: 1440x2560@80) never gets its rate adopted, so the compositor paces 90 Hz frames +// on an 80 Hz panel. Listing a 1 mHz slower alias of the same mode first makes the real mode the better +// match. Both entries carry the same VkDisplayModeKHR, so either selection drives the same mode. +VkResult alias_single_display_mode(PFN_vkGetDisplayModePropertiesKHR get, VkPhysicalDevice physical, + VkDisplayKHR display, uint32_t *count, VkDisplayModePropertiesKHR *out) +{ + uint32_t available = 0; + VkDisplayModePropertiesKHR mode{}; + if (get(physical, display, &available, nullptr) != VK_SUCCESS || available != 1 || + get(physical, display, &available, &mode) != VK_SUCCESS || available != 1 || mode.parameters.refreshRate <= 1) + return get(physical, display, count, out); + std::array modes{mode, mode}; + modes[0].parameters.refreshRate -= 1; + if (!out) { + *count = modes.size(); + return VK_SUCCESS; + } + *count = std::min(*count, modes.size()); + std::copy_n(modes.begin(), *count, out); + return *count < modes.size() ? VK_INCOMPLETE : VK_SUCCESS; +} +} // namespace + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkCreateImage(VkDevice handle, const VkImageCreateInfo *info, const VkAllocationCallbacks *allocator, VkImage *out) +{ + auto device = device_for(handle); + VkImageCreateInfo adjusted = *info; + bool tracked = device->enabled && sdf_image(adjusted); + bool empty = adjusted.extent.width == 0 || adjusted.extent.height == 0; + if (device->sdf_fix && tracked && empty) { + adjusted.extent.width = std::max(1u, adjusted.extent.width); + adjusted.extent.height = std::max(1u, adjusted.extent.height); + } + auto result = next(device, "vkCreateImage")(handle, &adjusted, allocator, out); + if (result == VK_SUCCESS && device->ring_allowed) { + std::lock_guard guard(mutex); + device->formats[*out] = adjusted.format; + } + if (result == VK_SUCCESS && tracked) { + std::lock_guard guard(mutex); + device->images.emplace(*out, Image{adjusted.extent, empty, adjusted.tiling == VK_IMAGE_TILING_LINEAR}); + fprintf(stderr, "monado-roomsetup: R8 %ux%u -> %ux%u, tiling=%u\n", info->extent.width, + info->extent.height, adjusted.extent.width, adjusted.extent.height, adjusted.tiling); + } + return result; +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkDestroyImage(VkDevice handle, VkImage image, const VkAllocationCallbacks *allocator) +{ + auto device = device_for(handle); + { + std::lock_guard guard(mutex); + device->images.erase(image); + device->formats.erase(image); + } + next(device, "vkDestroyImage")(handle, image, allocator); +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkBindImageMemory(VkDevice handle, VkImage image, VkDeviceMemory memory, VkDeviceSize offset) +{ + auto device = device_for(handle); + auto result = next(device, "vkBindImageMemory")(handle, image, memory, offset); + if (result == VK_SUCCESS) { + std::lock_guard guard(mutex); + auto found = device->images.find(image); + if (found != device->images.end() && found->second.linear) { + auto &state = found->second; + state.memory = memory; + state.offset = offset; + VkImageSubresource subresource{VK_IMAGE_ASPECT_COLOR_BIT, 0, 0}; + next(device, "vkGetImageSubresourceLayout")( + handle, image, &subresource, &state.layout); + } + } + return result; +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkMapMemory(VkDevice handle, VkDeviceMemory memory, VkDeviceSize offset, VkDeviceSize size, + VkMemoryMapFlags flags, void **out) +{ + auto device = device_for(handle); + auto result = next(device, "vkMapMemory")(handle, memory, offset, size, flags, out); + if (result != VK_SUCCESS) return result; + std::lock_guard guard(mutex); + device->mappings[memory] = Mapping{*out, offset, size}; + identify_ring(device, memory, out); + for (auto &[buffer, state] : device->buffers) { + if (state.memory == memory) state.indices.reset(); + } + for (auto &[image, state] : device->images) { + if (!device->sdf_fix || !state.linear || state.initialized || state.memory != memory) continue; + VkDeviceSize start = state.offset + state.layout.offset; + VkDeviceSize bytes = (state.extent.height - 1) * state.layout.rowPitch + state.extent.width; + if (start < offset || (size != VK_WHOLE_SIZE && (start - offset > size || bytes > size - (start - offset)))) + continue; + auto *pixels = static_cast(*out) + start - offset; + // Positive distance saturates to 255. Keep existing strokes on later maps. + for (uint32_t y = 0; y < state.extent.height; ++y) + memset(pixels + y * state.layout.rowPitch, 255, state.extent.width); + state.initialized = true; + fprintf(stderr, "monado-roomsetup: initialized R8 %ux%u, row pitch=%llu\n", + state.extent.width, state.extent.height, static_cast(state.layout.rowPitch)); + } + return result; +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdCopyImage(VkCommandBuffer command, VkImage source, VkImageLayout source_layout, + VkImage destination, VkImageLayout destination_layout, uint32_t count, const VkImageCopy *regions) +{ + auto device = device_for(command); + std::vector adjusted; + { + std::lock_guard guard(mutex); + auto src = device->images.find(source); + auto dst = device->images.find(destination); + bool source_empty = src != device->images.end() && src->second.empty; + bool destination_empty = dst != device->images.end() && dst->second.empty; + if (device->sdf_fix && (source_empty || destination_empty)) { + for (uint32_t i = 0; i < count; ++i) { + auto region = regions[i]; + // Copy the initialized sentinel only between two empty images. + // An empty previous grid must not overwrite a new drawing. + if (!source_empty || !destination_empty || region.srcOffset.x || region.srcOffset.y || + region.dstOffset.x || region.dstOffset.y) continue; + region.extent.width = std::min(src->second.extent.width, dst->second.extent.width); + region.extent.height = std::min(src->second.extent.height, dst->second.extent.height); + adjusted.push_back(region); + } + count = adjusted.size(); + regions = adjusted.data(); + } + } + if (count) next(device, "vkCmdCopyImage")( + command, source, source_layout, destination, destination_layout, count, regions); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdCopyBufferToImage(VkCommandBuffer command, VkBuffer source, VkImage destination, VkImageLayout layout, + uint32_t count, const VkBufferImageCopy *regions) +{ + auto device = device_for(command); + { + std::lock_guard guard(mutex); + if (device->rings.count(source)) { + auto format = device->formats.find(destination); + for (uint32_t i = 0; i < count; ++i) { + const auto ®ion = regions[i]; + uint64_t end = UINT64_MAX; + // Exact last-reservation coverage is established for the unchanged RGBA camera. + if (format != device->formats.end() && (format->second == VK_FORMAT_R8G8B8A8_UNORM || + format->second == VK_FORMAT_R8G8B8A8_SRGB) && !region.bufferRowLength && !region.bufferImageHeight && + region.imageExtent.depth == 1 && region.imageSubresource.layerCount == 1 && + region.imageExtent.width <= ring_capacity / 4 && region.imageExtent.height && + region.imageExtent.width * 4ULL <= ring_capacity / region.imageExtent.height) { + uint64_t bytes = uint64_t(region.imageExtent.width) * region.imageExtent.height * 4; + if (region.bufferOffset <= ring_capacity - bytes) end = region.bufferOffset + bytes; + } + ring_copy(device, command, source, end); + } + } + } + next(device, "vkCmdCopyBufferToImage")(command, source, destination, layout, count, regions); +} +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkEndCommandBuffer(VkCommandBuffer command) +{ + auto device = device_for(command); + auto result = next(device, "vkEndCommandBuffer")(command); + if (result == VK_SUCCESS) { + std::lock_guard guard(mutex); + for (auto &[buffer, ring] : device->rings) { + auto found = ring.commands.find(command); + if (found != ring.commands.end()) found->second.ended = true; + } + } + return result; +} +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkQueueSubmit(VkQueue queue, uint32_t count, const VkSubmitInfo *infos, VkFence fence) +{ + auto device = device_for(queue); + // Binary waits reference signal operations already submitted. A nonzero + // timeline wait could instead depend on future CPU work, so leave it alone. + std::vector commands; + bool future_wait = false; + for (uint32_t i = 0; i < count; ++i) { + for (auto *extension = static_cast(infos[i].pNext); extension; extension = extension->pNext) { + if (extension->sType != VK_STRUCTURE_TYPE_TIMELINE_SEMAPHORE_SUBMIT_INFO) continue; + const auto &timeline = *reinterpret_cast(extension); + for (uint32_t j = 0; j < timeline.waitSemaphoreValueCount; ++j) + future_wait |= timeline.pWaitSemaphoreValues[j] != 0; + } + for (uint32_t j = 0; j < infos[i].commandBufferCount; ++j) commands.push_back(infos[i].pCommandBuffers[j]); + } + auto result = next(device, "vkQueueSubmit")(queue, count, infos, fence); + if (result == VK_SUCCESS && device->ring_allowed) { + std::lock_guard guard(mutex); + if (!future_wait) recover_rings(device, queue, commands); + else for (auto &[buffer, ring] : device->rings) { + bool touched = std::any_of(commands.begin(), commands.end(), [&ring](auto cmd) { return ring.commands.count(cmd); }); + if (touched) ring.disable("upload has a nonzero timeline wait"); + } + } + return result; +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkCreateBuffer(VkDevice handle, const VkBufferCreateInfo *info, const VkAllocationCallbacks *allocator, VkBuffer *out) +{ + auto device = device_for(handle); + auto result = next(device, "vkCreateBuffer")(handle, info, allocator, out); + if (result == VK_SUCCESS && (info->usage & (VK_BUFFER_USAGE_VERTEX_BUFFER_BIT | VK_BUFFER_USAGE_INDEX_BUFFER_BIT | VK_BUFFER_USAGE_TRANSFER_SRC_BIT | VK_BUFFER_USAGE_UNIFORM_BUFFER_BIT))) { + std::lock_guard guard(mutex); + auto [entry, inserted] = device->buffers.emplace(*out, Buffer{info->size, info->usage}); + if (inserted) entry->second.id = ++device->next_buffer_id; + } + return result; +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkDestroyBuffer(VkDevice handle, VkBuffer buffer, const VkAllocationCallbacks *allocator) +{ + auto device = device_for(handle); + { + std::lock_guard guard(mutex); + device->buffers.erase(buffer); + auto ring = device->rings.find(buffer); + if (ring != device->rings.end()) ring->second.disable("ring destroyed"); + } + next(device, "vkDestroyBuffer")(handle, buffer, allocator); +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkBindBufferMemory(VkDevice handle, VkBuffer buffer, VkDeviceMemory memory, VkDeviceSize offset) +{ + auto device = device_for(handle); + auto result = next(device, "vkBindBufferMemory")(handle, buffer, memory, offset); + if (result == VK_SUCCESS) { + std::lock_guard guard(mutex); + auto found = device->buffers.find(buffer); + if (found != device->buffers.end()) { found->second.memory = memory; found->second.memory_offset = offset; found->second.indices.reset(); } + } + return result; +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkBindBufferMemory2(VkDevice handle, uint32_t count, const VkBindBufferMemoryInfo *infos) +{ + auto device = device_for(handle); + auto result = next(device, "vkBindBufferMemory2")(handle, count, infos); + if (result == VK_SUCCESS) { + std::lock_guard guard(mutex); + for (uint32_t i = 0; i < count; ++i) { + auto found = device->buffers.find(infos[i].buffer); + if (found != device->buffers.end()) { found->second.memory = infos[i].memory; found->second.memory_offset = infos[i].memoryOffset; found->second.indices.reset(); } + } + } + return result; +} + +static bool correct_camera_uniform(void *data, size_t size) +{ + if (size != 400) return false; + std::array before; + memcpy(before.data(), data, size); + bool corrected = roomsetup::correct_camera(data, size); + static uint64_t last_trace = 0; + auto now = roomsetup::monotonic_ns(); + if (getenv("MONADO_STEAMVR_BENCH") && now - last_trace > 500000000) { + last_trace = now; + fprintf(stderr, "monado-roomview-matrices: ns=%llu corrected=%d view=", (unsigned long long)now, corrected); + for (int j = 16; j < 32; ++j) fprintf(stderr, " %.9g", before[j]); + fprintf(stderr, " camera_before="); + for (int j = 48; j < 64; ++j) fprintf(stderr, " %.9g", before[j]); + std::array after; + memcpy(after.data(), data, size); + fprintf(stderr, " camera_after="); + for (int j = 48; j < 64; ++j) fprintf(stderr, " %.9g", after[j]); + fprintf(stderr, "\n"); + } + return corrected; +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdUpdateBuffer(VkCommandBuffer command, VkBuffer buffer, VkDeviceSize offset, VkDeviceSize size, const void *data) +{ + auto device = device_for(command); + std::array camera; + { + std::lock_guard guard(mutex); + auto found = device->buffers.find(buffer); + if (found != device->buffers.end() && (found->second.usage & VK_BUFFER_USAGE_UNIFORM_BUFFER_BIT) && + size == camera.size() && offset <= found->second.size && size <= found->second.size - offset) { + memcpy(camera.data(), data, camera.size()); + if (correct_camera_uniform(camera.data(), camera.size())) { + data = camera.data(); + ++found->second.fixes; + } + } + if (found != device->buffers.end() && (found->second.usage & VK_BUFFER_USAGE_INDEX_BUFFER_BIT)) { + // A validated prefix is sufficient when the draw stays within it. + // Updates elsewhere invalidate the inferred format. + found->second.indices = offset == 0 && size <= found->second.size ? index_upload(size, data) : std::nullopt; + } + } + next(device, "vkCmdUpdateBuffer")(command, buffer, offset, size, data); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkUnmapMemory(VkDevice handle, VkDeviceMemory memory) +{ + auto device = device_for(handle); + { + std::lock_guard guard(mutex); + device->mappings.erase(memory); + for (auto &[buffer, ring] : device->rings) + if (ring.identity.memory == memory) ring.disable("ring memory released"); + } + next(device, "vkUnmapMemory")(handle, memory); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkFreeMemory(VkDevice handle, VkDeviceMemory memory, const VkAllocationCallbacks *allocator) +{ + auto device = device_for(handle); + { + std::lock_guard guard(mutex); + device->mappings.erase(memory); + for (auto &[buffer, ring] : device->rings) + if (ring.identity.memory == memory) ring.disable("ring memory released"); + } + next(device, "vkFreeMemory")(handle, memory, allocator); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdCopyBuffer(VkCommandBuffer command, VkBuffer source, VkBuffer destination, uint32_t count, const VkBufferCopy *regions) +{ + auto device = device_for(command); + { + std::lock_guard guard(mutex); + for (uint32_t i = 0; i < count; ++i) { + const auto ®ion = regions[i]; + uint64_t end = region.srcOffset <= ring_capacity && region.size <= ring_capacity - region.srcOffset + ? region.srcOffset + region.size : UINT64_MAX; + ring_copy(device, command, source, end); + } + auto dst = device->buffers.find(destination); + if (dst != device->buffers.end()) { + dst->second.indices.reset(); + if ((dst->second.usage & VK_BUFFER_USAGE_UNIFORM_BUFFER_BIT) && count == 1 && regions[0].size == 400 && + regions[0].dstOffset <= dst->second.size && 400 <= dst->second.size - regions[0].dstOffset) { + auto src=device->buffers.find(source); + if (src != device->buffers.end() && regions[0].srcOffset <= src->second.size && + 400 <= src->second.size - regions[0].srcOffset && + src->second.memory_offset <= UINT64_MAX - regions[0].srcOffset) { + auto map=device->mappings.find(src->second.memory); + auto start=src->second.memory_offset+regions[0].srcOffset; + if (map!=device->mappings.end() && start>=map->second.offset && + (map->second.size==VK_WHOLE_SIZE || (start-map->second.offset<=map->second.size && + 400<=map->second.size-(start-map->second.offset)))) { + auto *data=static_cast(map->second.data)+start-map->second.offset; + bool corrected = correct_camera_uniform(data, 400); + if (corrected) { + ++dst->second.fixes; + if ((dst->second.fixes & (dst->second.fixes-1))==0) + fprintf(stderr,"monado-roomview: rotation-only hits=%llu\n", + (unsigned long long)dst->second.fixes); + } + } + } + } + + auto src = device->buffers.find(source); + if ((dst->second.usage & VK_BUFFER_USAGE_INDEX_BUFFER_BIT) && src != device->buffers.end() && + count == 1 && regions[0].dstOffset == 0 && regions[0].size <= dst->second.size && + regions[0].srcOffset <= src->second.size && regions[0].size <= src->second.size - regions[0].srcOffset) { + const auto &buffer = src->second; + auto mapped = device->mappings.find(buffer.memory); + if (mapped != device->mappings.end() && buffer.memory_offset <= UINT64_MAX - regions[0].srcOffset) { + const auto &map = mapped->second; + VkDeviceSize start = buffer.memory_offset + regions[0].srcOffset; + if (start >= map.offset && (map.size == VK_WHOLE_SIZE || + (start - map.offset <= map.size && regions[0].size <= map.size - (start - map.offset)))) { + auto *data = static_cast(map.data) + start - map.offset; + dst->second.indices = index_upload(regions[0].size, data); + } + } + } + } + } + next(device, "vkCmdCopyBuffer")(command, source, destination, count, regions); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdFillBuffer(VkCommandBuffer command, VkBuffer buffer, VkDeviceSize offset, VkDeviceSize size, uint32_t data) +{ + auto device = device_for(command); + { + std::lock_guard guard(mutex); + auto found = device->buffers.find(buffer); + if (found != device->buffers.end()) found->second.indices.reset(); + } + next(device, "vkCmdFillBuffer")(command, buffer, offset, size, data); +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkCreateGraphicsPipelines(VkDevice handle, VkPipelineCache cache, uint32_t count, + const VkGraphicsPipelineCreateInfo *infos, const VkAllocationCallbacks *allocator, VkPipeline *out) +{ + auto device = device_for(handle); + auto result = next(device, "vkCreateGraphicsPipelines")(handle, cache, count, infos, allocator, out); + std::lock_guard guard(mutex); + for (uint32_t i = 0; i < count; ++i) { + if (out[i] != VK_NULL_HANDLE) device->strides[out[i]] = playspace_stride(infos[i]); + } + return result; +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkDestroyPipeline(VkDevice handle, VkPipeline pipeline, const VkAllocationCallbacks *allocator) +{ + auto device = device_for(handle); + { std::lock_guard guard(mutex); device->strides.erase(pipeline); } + next(device, "vkDestroyPipeline")(handle, pipeline, allocator); +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkBeginCommandBuffer(VkCommandBuffer command, const VkCommandBufferBeginInfo *info) +{ + auto device = device_for(command); + auto result = next(device, "vkBeginCommandBuffer")(command, info); + if (result == VK_SUCCESS) { + std::lock_guard guard(mutex); + device->draws[command] = {}; + for (auto &[buffer, ring] : device->rings) { + auto entry = ring.commands.find(command); + if (entry != ring.commands.end() && !entry->second.submitted) ring.disable("unsubmitted upload discarded"); + ring.commands.erase(command); + } + } + return result; +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkFreeCommandBuffers(VkDevice handle, VkCommandPool pool, uint32_t count, const VkCommandBuffer *commands) +{ + auto device = device_for(handle); + { + std::lock_guard guard(mutex); + for (uint32_t i = 0; i < count; ++i) { + device->draws.erase(commands[i]); + for (auto &[buffer, ring] : device->rings) { + auto found = ring.commands.find(commands[i]); + if (found != ring.commands.end() && !found->second.submitted) ring.disable("unsubmitted upload freed"); + ring.commands.erase(commands[i]); + } + } + } + next(device, "vkFreeCommandBuffers")(handle, pool, count, commands); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdBindPipeline(VkCommandBuffer command, VkPipelineBindPoint point, VkPipeline pipeline) +{ + auto device = device_for(command); + if (point == VK_PIPELINE_BIND_POINT_GRAPHICS) { + std::lock_guard guard(mutex); + device->draws[command].pipeline = pipeline; + } + next(device, "vkCmdBindPipeline")(command, point, pipeline); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdBindVertexBuffers(VkCommandBuffer command, uint32_t first, uint32_t count, const VkBuffer *buffers, const VkDeviceSize *offsets) +{ + auto device = device_for(command); + if (first == 0 && count) { + std::lock_guard guard(mutex); + auto &draw = device->draws[command]; + draw.vertex = buffers[0]; draw.vertex_offset = offsets[0]; + } + next(device, "vkCmdBindVertexBuffers")(command, first, count, buffers, offsets); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdBindIndexBuffer(VkCommandBuffer command, VkBuffer buffer, VkDeviceSize offset, VkIndexType type) +{ + auto device = device_for(command); + { + std::lock_guard guard(mutex); + auto &draw = device->draws[command]; + draw.index = buffer; draw.index_offset = offset; draw.index_type = type; + } + next(device, "vkCmdBindIndexBuffer")(command, buffer, offset, type); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL +vkCmdDrawIndexed(VkCommandBuffer command, uint32_t count, uint32_t instances, uint32_t first, int32_t base, uint32_t first_instance) +{ + auto device = device_for(command); + Draw draw; + bool repair = false; + { + std::lock_guard guard(mutex); + draw = device->draws[command]; + auto index = device->buffers.find(draw.index); + auto vertex = device->buffers.find(draw.vertex); + auto pipeline = device->strides.find(draw.pipeline); + if (device->index_fix && !device->images.empty() && index != device->buffers.end() && index->second.indices && + vertex != device->buffers.end() && pipeline != device->strides.end() && pipeline->second && + draw.index_type == VK_INDEX_TYPE_UINT16 && draw.index_offset == 0 && draw.vertex_offset == 0 && + first == 0 && base == 0 && count && count % 3 == 0) { + const auto &upload = *index->second.indices; + repair = count <= upload.count && vertex->second.size >= (upload.maximum + 1ULL) * pipeline->second; + if (repair) { + auto &buffer = index->second; + ++buffer.fixes; + if ((buffer.fixes & (buffer.fixes - 1)) == 0) + fprintf(stderr, "monado-roomsetup: index-fix buffer=%llu vertex=%llu hits=%llu stride=%u index_bytes=%llu vertex_bytes=%llu usage=%u count=%u upload_count=%u maximum=%u\n", + (unsigned long long)buffer.id, (unsigned long long)vertex->second.id, + (unsigned long long)buffer.fixes, pipeline->second, (unsigned long long)buffer.size, + (unsigned long long)vertex->second.size, buffer.usage, count, upload.count, upload.maximum); + } + + } + } + auto bind = next(device, "vkCmdBindIndexBuffer"); + if (repair) bind(command, draw.index, draw.index_offset, VK_INDEX_TYPE_UINT32); + next(device, "vkCmdDrawIndexed")(command, count, instances, first, base, first_instance); + // Keep the caller's binding valid for subsequent unrelated draws. + if (repair) bind(command, draw.index, draw.index_offset, draw.index_type); +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkWaitForPresentKHR(VkDevice handle, VkSwapchainKHR swapchain, uint64_t present_id, uint64_t timeout_ns) +{ + auto device = device_for(handle); + auto wait = next(device, "vkWaitForPresentKHR"); + auto start = std::chrono::steady_clock::now(); + auto result = wait(handle, swapchain, present_id, timeout_ns); + if (result != VK_SUCCESS) { + static std::atomic failures{0}; + auto count = ++failures; + // Bound logging during a fault while retaining its frequency and exact VkResult. + if (count <= 8 || (count & (count - 1)) == 0) { + auto elapsed = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start).count(); + fprintf(stderr, "monado-present: wait result=%d present_id=%llu timeout_ns=%llu elapsed_us=%lld failures=%llu\n", + result, (unsigned long long)present_id, (unsigned long long)timeout_ns, + (long long)elapsed, (unsigned long long)count); + } + } + return result; +} + +extern "C" VKAPI_ATTR void VKAPI_CALL vkDestroyDevice(VkDevice handle, const VkAllocationCallbacks *allocator) +{ + auto device = device_for(handle); + auto destroy = next(device, "vkDestroyDevice"); + { + std::lock_guard guard(mutex); + devices.erase(key(handle)); + } + destroy(handle, allocator); +} + +extern "C" VKAPI_ATTR void VKAPI_CALL vkDestroyInstance(VkInstance handle, const VkAllocationCallbacks *allocator) +{ + Instance instance; + { + std::lock_guard guard(mutex); + instance = instances.at(key(handle)); + instances.erase(key(handle)); + } + reinterpret_cast(instance.gipa(handle, "vkDestroyInstance"))(handle, allocator); +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkGetDisplayModePropertiesKHR(VkPhysicalDevice physical, VkDisplayKHR display, uint32_t *count, + VkDisplayModePropertiesKHR *out) +{ + Instance instance; + { + std::lock_guard guard(mutex); + instance = instances.at(key(physical)); + } + auto get = reinterpret_cast( + instance.gipa(instance.handle, "vkGetDisplayModePropertiesKHR")); + return alias_single_display_mode(get, physical, display, count, out); +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkCreateInstance(const VkInstanceCreateInfo *info, const VkAllocationCallbacks *allocator, VkInstance *out) +{ + auto *chain = reinterpret_cast(const_cast(info->pNext)); + while (chain && (chain->sType != VK_STRUCTURE_TYPE_LOADER_INSTANCE_CREATE_INFO || chain->function != VK_LAYER_LINK_INFO)) + chain = reinterpret_cast(const_cast(chain->pNext)); + if (!chain) return VK_ERROR_INITIALIZATION_FAILED; + auto gipa = chain->u.pLayerInfo->pfnNextGetInstanceProcAddr; + auto create = reinterpret_cast(gipa(VK_NULL_HANDLE, "vkCreateInstance")); + chain->u.pLayerInfo = chain->u.pLayerInfo->pNext; + auto result = create(info, allocator, out); + if (result == VK_SUCCESS) { + bool enabled = monado_compositor(); + std::lock_guard guard(mutex); + instances.emplace(key(*out), Instance{*out, gipa, enabled}); + if (enabled) fprintf(stderr, "monado-roomsetup: Vulkan SDF compatibility enabled\n"); + } + return result; +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL +vkCreateDevice(VkPhysicalDevice physical, const VkDeviceCreateInfo *info, const VkAllocationCallbacks *allocator, VkDevice *out) +{ + Instance instance; + { + std::lock_guard guard(mutex); + instance = instances.at(key(physical)); + } + auto *chain = reinterpret_cast(const_cast(info->pNext)); + while (chain && (chain->sType != VK_STRUCTURE_TYPE_LOADER_DEVICE_CREATE_INFO || chain->function != VK_LAYER_LINK_INFO)) + chain = reinterpret_cast(const_cast(chain->pNext)); + if (!chain) return VK_ERROR_INITIALIZATION_FAILED; + auto gipa = chain->u.pLayerInfo->pfnNextGetInstanceProcAddr; + auto gdpa = chain->u.pLayerInfo->pfnNextGetDeviceProcAddr; + auto create = reinterpret_cast(gipa(instance.handle, "vkCreateDevice")); + chain->u.pLayerInfo = chain->u.pLayerInfo->pNext; + auto result = create(physical, info, allocator, out); + if (result == VK_SUCCESS) { + std::lock_guard guard(mutex); + auto device = std::make_shared(Device{gdpa, *out, instance.enabled, {}}); + device->index_fix = !getenv("MONADO_DISABLE_ROOMSETUP_INDEX_FIX"); + device->sdf_fix = !getenv("MONADO_DISABLE_ROOMSETUP_SDF_FIX"); + device->ring_allowed = instance.enabled && !getenv("MONADO_DISABLE_TRANSFER_RING_RECOVERY") && supported_ring_build(); + static std::atomic_flag reported = ATOMIC_FLAG_INIT; + if (instance.enabled && !device->ring_allowed && !reported.test_and_set()) + fprintf(stderr, "monado-ring: disabled: unknown SteamVR build or recovery disabled\n"); + devices.emplace(key(*out), device); + } + return result; +} + +extern "C" VKAPI_ATTR PFN_vkVoidFunction VKAPI_CALL vkGetDeviceProcAddr(VkDevice handle, const char *name) +{ + auto device = device_for(handle); +#define MATCH(n) if (strcmp(name, #n) == 0) return reinterpret_cast(n) + MATCH(vkGetDeviceProcAddr); MATCH(vkDestroyDevice); + if (device->enabled) { + if (strcmp(name, "vkWaitForPresentKHR") == 0 && device->gdpa(handle, name)) + return reinterpret_cast(vkWaitForPresentKHR); + MATCH(vkCreateImage); MATCH(vkDestroyImage); MATCH(vkBindImageMemory); MATCH(vkMapMemory); MATCH(vkCmdCopyImage); + MATCH(vkQueueSubmit); MATCH(vkEndCommandBuffer); MATCH(vkCmdCopyBufferToImage); + MATCH(vkCreateBuffer); MATCH(vkDestroyBuffer); MATCH(vkBindBufferMemory); MATCH(vkBindBufferMemory2); + MATCH(vkCmdUpdateBuffer); MATCH(vkCmdCopyBuffer); MATCH(vkCmdFillBuffer); MATCH(vkUnmapMemory); MATCH(vkFreeMemory); + MATCH(vkCreateGraphicsPipelines); MATCH(vkDestroyPipeline); MATCH(vkBeginCommandBuffer); MATCH(vkFreeCommandBuffers); + MATCH(vkCmdBindPipeline); MATCH(vkCmdBindVertexBuffers); MATCH(vkCmdBindIndexBuffer); MATCH(vkCmdDrawIndexed); + } + return device->gdpa(handle, name); +} + +extern "C" VKAPI_ATTR PFN_vkVoidFunction VKAPI_CALL vkGetInstanceProcAddr(VkInstance handle, const char *name) +{ + MATCH(vkGetInstanceProcAddr); MATCH(vkCreateInstance); MATCH(vkCreateDevice); MATCH(vkDestroyInstance); + MATCH(vkGetDeviceProcAddr); MATCH(vkDestroyDevice); + if (!handle) return nullptr; + Instance instance; + { + std::lock_guard guard(mutex); + instance = instances.at(key(handle)); + } + if (instance.enabled) { + if (strcmp(name, "vkWaitForPresentKHR") == 0 && instance.gipa(handle, name)) + return reinterpret_cast(vkWaitForPresentKHR); + if (strcmp(name, "vkGetDisplayModePropertiesKHR") == 0 && instance.gipa(handle, name)) + return reinterpret_cast(vkGetDisplayModePropertiesKHR); + MATCH(vkCreateImage); MATCH(vkDestroyImage); MATCH(vkBindImageMemory); MATCH(vkMapMemory); MATCH(vkCmdCopyImage); + MATCH(vkQueueSubmit); MATCH(vkEndCommandBuffer); MATCH(vkCmdCopyBufferToImage); + MATCH(vkCreateBuffer); MATCH(vkDestroyBuffer); MATCH(vkBindBufferMemory); MATCH(vkBindBufferMemory2); + MATCH(vkCmdUpdateBuffer); MATCH(vkCmdCopyBuffer); MATCH(vkCmdFillBuffer); MATCH(vkUnmapMemory); MATCH(vkFreeMemory); + MATCH(vkCreateGraphicsPipelines); MATCH(vkDestroyPipeline); MATCH(vkBeginCommandBuffer); MATCH(vkFreeCommandBuffers); + MATCH(vkCmdBindPipeline); MATCH(vkCmdBindVertexBuffers); MATCH(vkCmdBindIndexBuffer); MATCH(vkCmdDrawIndexed); + } + return instance.gipa(handle, name); +#undef MATCH +} + +extern "C" VKAPI_ATTR VkResult VKAPI_CALL vkNegotiateLoaderLayerInterfaceVersion(VkNegotiateLayerInterface *info) +{ + info->loaderLayerInterfaceVersion = std::min(info->loaderLayerInterfaceVersion, 2u); + info->pfnGetInstanceProcAddr = vkGetInstanceProcAddr; + info->pfnGetDeviceProcAddr = vkGetDeviceProcAddr; + info->pfnGetPhysicalDeviceProcAddr = nullptr; + return VK_SUCCESS; +} diff --git a/src/xrt/state_trackers/steamvr_drv/rifts_passthrough_shm.h b/src/xrt/state_trackers/steamvr_drv/rifts_passthrough_shm.h new file mode 100644 index 000000000..431f2cb2a --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/rifts_passthrough_shm.h @@ -0,0 +1,36 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include + +#ifndef RIFTS_PASSTHROUGH_SHM_PATH +#define RIFTS_PASSTHROUGH_SHM_PATH "/dev/shm/rifts_passthrough" +#endif +#define RIFTS_PASSTHROUGH_MAGIC 0x54535052 /* "RPST" */ +#define RIFTS_PASSTHROUGH_VERSION 2 +#define RIFTS_PASSTHROUGH_WIDTH 640 +#define RIFTS_PASSTHROUGH_HEIGHT 320 + +#ifdef __cplusplus +extern "C" { +#endif + +struct rifts_passthrough_shm +{ + uint32_t magic; /* RIFTS_PASSTHROUGH_MAGIC */ + uint32_t version; /* RIFTS_PASSTHROUGH_VERSION */ + uint32_t width; /* 640 */ + uint32_t height; /* 320 */ + uint32_t stride; /* 640 * 4 = 2560 */ + uint32_t format; /* 0 = RGBA32 */ + uint32_t client_active; /* Consumer monotonic-ms heartbeat; 0 = idle */ + uint32_t toggle_count; /* Incremented on controller button toggle */ + uint64_t sequence; /* Seqlock: odd during write, even when complete */ + uint64_t timestamp_ns; /* Monotonic capture timestamp */ + uint8_t pixels[RIFTS_PASSTHROUGH_WIDTH * RIFTS_PASSTHROUGH_HEIGHT * 4]; +}; + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/state_trackers/steamvr_drv/steamvr_bindings/ovrd_bindings.py b/src/xrt/state_trackers/steamvr_drv/steamvr_bindings/ovrd_bindings.py index 3a816ff66..28b1a369b 100755 --- a/src/xrt/state_trackers/steamvr_drv/steamvr_bindings/ovrd_bindings.py +++ b/src/xrt/state_trackers/steamvr_drv/steamvr_bindings/ovrd_bindings.py @@ -28,7 +28,7 @@ def ovrd_generate_bindings_c(file, b): hw_name = str(profile.name.split("/")[-1]) vendor_name = str(profile.name.split("/")[-2]) fname = vendor_name + "_" + hw_name + "_profile.json" - controller_type = "monado_" + vendor_name + "_" + hw_name + controller_type = profile.steamvr_controller_type or "monado_" + vendor_name + "_" + hw_name binding_count = len(profile.components) wl(f, @@ -47,7 +47,7 @@ def ovrd_generate_bindings_c(file, b): # @todo Doesn't handle pose yet. steamvr_path = component.steamvr_path - if component.component_name in ["click", "touch", "force", "value", "proximity"]: + if component.component_name in ["click", "touch", "force", "value", "proximity", "proximity_fb"]: steamvr_path += "/" + component.component_name wl(f, diff --git a/src/xrt/targets/common/target_builder_rift_s.c b/src/xrt/targets/common/target_builder_rift_s.c index 3b5dfed1d..588cd6920 100644 --- a/src/xrt/targets/common/target_builder_rift_s.c +++ b/src/xrt/targets/common/target_builder_rift_s.c @@ -44,7 +44,7 @@ enum u_logging_level rift_s_log_level; * */ -DEBUG_GET_ONCE_LOG_OPTION(rift_s_log, "RIFT_S_LOG", U_LOGGING_WARN) +DEBUG_GET_ONCE_LOG_OPTION(rift_s_log, "RIFT_S_LOG", U_LOGGING_INFO) #ifdef XRT_BUILD_DRIVER_HANDTRACKING DEBUG_GET_ONCE_BOOL_OPTION(rift_s_hand_tracking_as_controller, "RIFT_S_HAND_TRACKING_AS_CONTROLLERS", false) @@ -166,7 +166,8 @@ rift_s_open_system_impl(struct xrt_builder *xb, /* We'll give everyone a shared tracking origin using the system allocated one */ origin->type = XRT_TRACKING_TYPE_OTHER; origin->initial_offset.orientation.w = 1.0f; - origin->initial_offset.position.y = 1.6; + // The SLAM guard already supplies nominal eye height in this tracking frame. + origin->initial_offset.position.y = 0.0f; snprintf(origin->name, XRT_TRACKING_NAME_LEN, "%s", "Oculus Rift S SLAM Tracking"); // Create and add to list. diff --git a/src/xrt/targets/steamvr_drv/CMakeLists.txt b/src/xrt/targets/steamvr_drv/CMakeLists.txt index 52d6f5c77..b2ae4615a 100644 --- a/src/xrt/targets/steamvr_drv/CMakeLists.txt +++ b/src/xrt/targets/steamvr_drv/CMakeLists.txt @@ -10,7 +10,8 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${INPUT_PROFILES_INPUT_DIR}/steamvr_profiles.py ${INPUT_PROFILES_INPUT_DIR}/bindings.json "${INPUT_PROFILES_OUTPUT_DIR}" "-s" - DEPENDS ${INPUT_PROFILES_INPUT_DIR}/bindings.py ${INPUT_PROFILES_INPUT_DIR}/bindings.json + DEPENDS ${INPUT_PROFILES_INPUT_DIR}/steamvr_profiles.py + ${INPUT_PROFILES_INPUT_DIR}/bindings.py ${INPUT_PROFILES_INPUT_DIR}/bindings.json COMMENT "Generating SteamVR input profiles to ${INPUT_PROFILES_OUTPUT_DIR}" ) add_custom_target( @@ -36,6 +37,17 @@ target_link_libraries( # meta data that the steamvr plugin needs in the base directory of the steamvr plugin file(COPY driver.vrdrivermanifest DESTINATION ${PROJECT_BINARY_DIR}/steamvr-monado) file(COPY resources DESTINATION ${PROJECT_BINARY_DIR}/steamvr-monado) +configure_file( + resources/input/monado_touch_compositor.json + ${INPUT_PROFILES_OUTPUT_DIR}/monado_touch_compositor.json + COPYONLY + ) + +configure_file( + resources/settings/default.vrsettings + ${PROJECT_BINARY_DIR}/steamvr-monado/resources/settings/default.vrsettings + COPYONLY + ) # determine the output directory for the steamvr plugin if(WIN32) @@ -69,3 +81,8 @@ install( DIRECTORY "${PROJECT_BINARY_DIR}/steamvr-monado" DESTINATION "${CMAKE_INSTALL_PREFIX}/share" ) + +# The directory install treats files as data; install the guard as an executable last. +if(TARGET monado-roomsetup-guard) + install(TARGETS monado-roomsetup-guard RUNTIME DESTINATION ${CMAKE_INSTALL_DATADIR}/steamvr-monado/bin/linux64) +endif() diff --git a/src/xrt/targets/steamvr_drv/resources/input/monado_touch_compositor.json b/src/xrt/targets/steamvr_drv/resources/input/monado_touch_compositor.json new file mode 100644 index 000000000..abc3b0fe6 --- /dev/null +++ b/src/xrt/targets/steamvr_drv/resources/input/monado_touch_compositor.json @@ -0,0 +1,279 @@ +{ + "action_manifest_version": 0, + "app_key": "openvr.component.vrcompositor", + "controller_type": "monado_oculus_touch", + "category": "steamvr_input", + "name": "Monado Oculus Touch compositor", + "description": "Dashboard and native playspace setup for Oculus Touch.", + "bindings": { + "/actions/system": { + "sources": [ + { + "path": "/user/hand/left/input/system", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/system/in/ToggleDashboard" + } + } + } + ] + }, + "/actions/lasermouse": { + "sources": [ + { + "path": "/user/hand/left/input/trigger", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse/in/LeftClick" + } + }, + "parameters": { + "click_activate_threshold": "0.65", + "click_deactivate_threshold": "0.6" + } + }, + { + "path": "/user/hand/left/input/trigger", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse/in/LockMousePosition" + } + }, + "parameters": { + "click_activate_threshold": "0.1", + "click_deactivate_threshold": "0.05", + "haptic_amplitude": "0" + } + }, + { + "path": "/user/hand/left/input/x", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse/in/Home" + } + } + }, + { + "path": "/user/hand/left/input/y", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse/in/Back" + } + } + }, + { + "path": "/user/hand/left/input/joystick", + "mode": "joystick", + "inputs": { + "position": { + "output": "/actions/lasermouse/in/TrackpadValue" + }, + "touch": { + "output": "/actions/lasermouse/in/TrackpadTouch" + }, + "click": { + "output": "/actions/lasermouse/in/RightClick" + } + } + }, + { + "path": "/user/hand/right/input/trigger", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse/in/LeftClick" + } + }, + "parameters": { + "click_activate_threshold": "0.65", + "click_deactivate_threshold": "0.6" + } + }, + { + "path": "/user/hand/right/input/trigger", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse/in/LockMousePosition" + } + }, + "parameters": { + "click_activate_threshold": "0.1", + "click_deactivate_threshold": "0.05", + "haptic_amplitude": "0" + } + }, + { + "path": "/user/hand/right/input/a", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse/in/Home" + } + } + }, + { + "path": "/user/hand/right/input/b", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse/in/Back" + } + } + }, + { + "path": "/user/hand/right/input/joystick", + "mode": "joystick", + "inputs": { + "position": { + "output": "/actions/lasermouse/in/TrackpadValue" + }, + "touch": { + "output": "/actions/lasermouse/in/TrackpadTouch" + }, + "click": { + "output": "/actions/lasermouse/in/RightClick" + } + } + } + ], + "poses": [ + { + "path": "/user/hand/left/pose/tip", + "output": "/actions/lasermouse/in/Pointer" + }, + { + "path": "/user/hand/right/pose/tip", + "output": "/actions/lasermouse/in/Pointer" + } + ], + "haptics": [ + { + "path": "/user/hand/left/output/haptic", + "output": "/actions/lasermouse/out/Haptic" + }, + { + "path": "/user/hand/right/output/haptic", + "output": "/actions/lasermouse/out/Haptic" + } + ] + }, + "/actions/roomsetup": { + "sources": [ + { + "path": "/user/hand/left/input/x", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/roomsetup/in/Accept" + } + } + }, + { + "path": "/user/hand/left/input/y", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/roomsetup/in/Back" + } + } + }, + { + "path": "/user/hand/left/input/joystick", + "mode": "joystick", + "inputs": { + "position": { + "output": "/actions/roomsetup/in/FloorFineAdjust" + } + } + }, + { + "path": "/user/hand/right/input/a", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/roomsetup/in/Accept" + } + } + }, + { + "path": "/user/hand/right/input/b", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/roomsetup/in/Back" + } + } + }, + { + "path": "/user/hand/right/input/joystick", + "mode": "joystick", + "inputs": { + "position": { + "output": "/actions/roomsetup/in/FloorFineAdjust" + } + } + } + ] + }, + "/actions/scroll_smooth": { + "sources": [ + { + "path": "/user/hand/left/input/joystick", + "mode": "joystick", + "inputs": { + "position": { + "output": "/actions/scroll_smooth/in/Scroll" + } + } + }, + { + "path": "/user/hand/right/input/joystick", + "mode": "joystick", + "inputs": { + "position": { + "output": "/actions/scroll_smooth/in/Scroll" + } + } + } + ] + }, + "/actions/lasermouse_secondary": { + "sources": [ + { + "path": "/user/hand/left/input/trigger", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse_secondary/in/SwitchLaserHand" + } + }, + "parameters": { + "click_activate_threshold": "0.05", + "click_deactivate_threshold": "0.02", + "haptic_amplitude": "0" + } + }, + { + "path": "/user/hand/right/input/trigger", + "mode": "button", + "inputs": { + "click": { + "output": "/actions/lasermouse_secondary/in/SwitchLaserHand" + } + }, + "parameters": { + "click_activate_threshold": "0.05", + "click_deactivate_threshold": "0.02", + "haptic_amplitude": "0" + } + } + ] + } + } +} diff --git a/src/xrt/targets/steamvr_drv/resources/settings/default.vrsettings b/src/xrt/targets/steamvr_drv/resources/settings/default.vrsettings index 039cf3fa6..2ec389e1d 100644 --- a/src/xrt/targets/steamvr_drv/resources/settings/default.vrsettings +++ b/src/xrt/targets/steamvr_drv/resources/settings/default.vrsettings @@ -1,5 +1,6 @@ { "driver_monado" : { - "enable" : true + "enable" : true, + "enableNativeRoomSetup" : true } } diff --git a/src/xrt/tracking/constellation/CMakeLists.txt b/src/xrt/tracking/constellation/CMakeLists.txt index 1acb00efd..0bd073e1f 100644 --- a/src/xrt/tracking/constellation/CMakeLists.txt +++ b/src/xrt/tracking/constellation/CMakeLists.txt @@ -9,6 +9,8 @@ add_library( internal/blobwatch.h internal/correspondence_search.c internal/correspondence_search.h + internal/constrained_pose.cpp + internal/constrained_pose.h internal/debug_draw.cpp internal/debug_draw.h internal/lambdatwist/cubic.h @@ -31,6 +33,7 @@ target_link_libraries( constellation_tracker PRIVATE xrt-interfaces + Eigen3::Eigen aux_os aux_util aux_math diff --git a/src/xrt/tracking/constellation/internal/blobwatch.c b/src/xrt/tracking/constellation/internal/blobwatch.c index 79837dcd6..eb392ae7d 100644 --- a/src/xrt/tracking/constellation/internal/blobwatch.c +++ b/src/xrt/tracking/constellation/internal/blobwatch.c @@ -272,16 +272,14 @@ process_scanline(uint8_t *line, struct xrt_frame *frame, blobservation *ob) { - struct extent *le_end = prev_el->extents; - struct extent *le = prev_el->extents; + struct extent *le = prev_el ? prev_el->extents : NULL; + struct extent *le_end = prev_el ? le + prev_el->num : NULL; struct extent *extent = el->extents; int num_extents = MAX_EXTENTS_PER_LINE; float center; uint32_t x; int e = 0; - if (prev_el) - le_end += prev_el->num; for (x = 0; x < frame->width; x++) { int start, end; @@ -292,6 +290,7 @@ process_scanline(uint8_t *line, if (line[x] <= bw->pixel_threshold) continue; + max_pixel = line[x]; start = x++; /* Loop until pixel value falls below threshold */ @@ -316,7 +315,7 @@ process_scanline(uint8_t *line, * bottom of finished blobs. Store them into an array. */ while (le < le_end && le->end < center) { - extent_to_blobs(bw, ob, le, y, frame); + extent_to_blobs(bw, ob, le, y - 1, frame); le++; } @@ -357,7 +356,7 @@ process_scanline(uint8_t *line, * extents in the previous line are finished blobs. Store them. */ while (le < le_end) { - extent_to_blobs(bw, ob, le, y, frame); + extent_to_blobs(bw, ob, le, y - 1, frame); le++; } } @@ -389,8 +388,8 @@ process_frame(blobwatch *bw, blobservation *ob, struct xrt_frame *frame) process_scanline(line, bw, 0, &el1, NULL, frame, ob); for (uint32_t y = 1; y < frame->height; y++) { - process_scanline(line, bw, y, y & 1 ? &el2 : &el1, y & 1 ? &el1 : &el2, frame, ob); line += frame->stride; + process_scanline(line, bw, y, y & 1 ? &el2 : &el1, y & 1 ? &el1 : &el2, frame, ob); } } diff --git a/src/xrt/tracking/constellation/internal/camera_model.h b/src/xrt/tracking/constellation/internal/camera_model.h index e02bedcac..7a054d877 100644 --- a/src/xrt/tracking/constellation/internal/camera_model.h +++ b/src/xrt/tracking/constellation/internal/camera_model.h @@ -16,4 +16,102 @@ struct camera_model /* Distortion / projection parameters */ struct t_camera_model_params calib; + bool fisheye62_valid; + float fisheye62_radial[6], fisheye62_p1, fisheye62_p2; }; + +/* Native six-radial/two-tangential fisheye model. Work in the angular plane so + * inversion stays finite at the wide sensor edges, unlike view-plane x/z. */ +static inline void +camera_model_fisheye62(const struct camera_model *camera, double x, double y, double *u, double *v, double jacobian[4]) +{ + double r2 = x * x + y * y, radial = 1, derivative = 0, power = 1; + for (unsigned i = 0; i < 6; ++i) { + derivative += (i + 1) * camera->fisheye62_radial[i] * power; + power *= r2; + radial += camera->fisheye62_radial[i] * power; + } + // The two tangential coefficients act on X/Y after radial distortion. + double p1 = camera->fisheye62_p1, p2 = camera->fisheye62_p2; + double rx = x * radial, ry = y * radial, rr2 = rx * rx + ry * ry; + double tangent = 2 * (p1 * rx + p2 * ry); + *u = rx * (1 + tangent) + p1 * rr2; + *v = ry * (1 + tangent) + p2 * rr2; + if (jacobian) { + double rxx = radial + 2 * x * x * derivative; + double rxy = 2 * x * y * derivative; + double ryy = radial + 2 * y * y * derivative; + double txx = 1 + 6 * p1 * rx + 2 * p2 * ry; + double txy = 2 * p1 * ry + 2 * p2 * rx; + double tyy = 1 + 2 * p1 * rx + 6 * p2 * ry; + // Row-major J_tangential * J_radial is generally not symmetric. + jacobian[0] = txx * rxx + txy * rxy; + jacobian[1] = txx * rxy + txy * ryy; + jacobian[2] = txy * rxx + tyy * rxy; + jacobian[3] = txy * rxy + tyy * ryy; + } +} + +static inline bool +camera_model_project(const struct camera_model *camera, float x, float y, float z, float *u, float *v) +{ + *u = *v = NAN; + if (!camera->fisheye62_valid) + return t_camera_models_project(&camera->calib, x, y, z, u, v) && isfinite(*u) && isfinite(*v); + if (!isfinite(x) || !isfinite(y) || !isfinite(z) || z <= 0) + return false; + double r = hypot(x, y), scale = r > 1e-12 ? atan2(r, z) / r : 1 / z; + double a, b; + camera_model_fisheye62(camera, x * scale, y * scale, &a, &b, NULL); + *u = camera->calib.fx * a + camera->calib.cx; + *v = camera->calib.fy * b + camera->calib.cy; + return isfinite(*u) && isfinite(*v); +} + +static inline bool +camera_model_unproject(const struct camera_model *camera, float u, float v, float *x, float *y, float *z) +{ + *x = *y = *z = NAN; + if (!isfinite(u) || !isfinite(v) || !(camera->calib.fx > 0) || !(camera->calib.fy > 0)) + return false; + if (!camera->fisheye62_valid) + return t_camera_models_unproject(&camera->calib, u, v, x, y, z) && isfinite(*x) && isfinite(*y) && + isfinite(*z); + double target_x = (u - camera->calib.cx) / camera->calib.fx; + double target_y = (v - camera->calib.cy) / camera->calib.fy; + double a = target_x, b = target_y; + for (unsigned iteration = 0; iteration < 20; ++iteration) { + double px, py, j[4]; + camera_model_fisheye62(camera, a, b, &px, &py, j); + double ex = px - target_x, ey = py - target_y; + if (hypot(ex, ey) < 1e-9) + break; + double det = j[0] * j[3] - j[1] * j[2]; + if (!isfinite(det) || det <= 1e-12) + return false; + a -= (j[3] * ex - j[1] * ey) / det; + b -= (j[0] * ey - j[2] * ex) / det; + } + double px, py; + camera_model_fisheye62(camera, a, b, &px, &py, NULL); + double theta = hypot(a, b); + if (!isfinite(theta) || theta >= M_PI || hypot(px - target_x, py - target_y) > 1e-7) + return false; + double scale = theta > 1e-12 ? sin(theta) / theta : 1; + *x = a * scale; + *y = b * scale; + *z = cos(theta); + return true; +} + +static inline bool +camera_model_undistort(const struct camera_model *camera, float u, float v, float *x, float *y) +{ + float xp, yp, z; + *x = *y = NAN; + if (!camera_model_unproject(camera, u, v, &xp, &yp, &z) || z <= 1e-6f) + return false; + *x = xp / z; + *y = yp / z; + return isfinite(*x) && isfinite(*y); +} diff --git a/src/xrt/tracking/constellation/internal/constrained_pose.cpp b/src/xrt/tracking/constellation/internal/constrained_pose.cpp new file mode 100644 index 000000000..596d93923 --- /dev/null +++ b/src/xrt/tracking/constellation/internal/constrained_pose.cpp @@ -0,0 +1,1614 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "constrained_pose.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { +using V3 = Eigen::Vector3d; +using M3 = Eigen::Matrix3d; +using Q = Eigen::Quaterniond; +using M4 = Eigen::Matrix4d; +using M6 = Eigen::Matrix; +using V6 = Eigen::Matrix; +using Clock = std::chrono::steady_clock; +constexpr double pi = 3.14159265358979323846; + +V3 +vec(const xrt_vec3 &v) +{ + return V3(v.x, v.y, v.z); +} +Q +quat(const xrt_quat &q) +{ + return Q(q.w, q.x, q.y, q.z).normalized(); +} +xrt_vec3 +xvec(const V3 &v) +{ + return {float(v.x()), float(v.y()), float(v.z())}; +} +xrt_pose +pose(const Q &q, const V3 &t) +{ + return {{float(q.x()), float(q.y()), float(q.z()), float(q.w())}, xvec(t)}; +} +struct Pose +{ + Q q; + V3 t; +}; +Pose +from_pose(const xrt_pose &p) +{ + return {quat(p.orientation), vec(p.position)}; +} +Pose +compose(const Pose &a, const Pose &b) +{ + return {a.q * b.q, a.q * b.t + a.t}; +} +bool +valid_pose(const xrt_pose &p) +{ + const Q q(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z); + return q.coeffs().allFinite() && std::abs(q.norm() - 1.0) < 0.01 && vec(p.position).allFinite(); +} +enum constellation_constrained_solver_status +valid_pair(const xrt_vec3 bearings[2], double min_angle, V3 &a, V3 &b) +{ + a = vec(bearings[0]); + b = vec(bearings[1]); + if (!a.allFinite() || !b.allFinite() || a.norm() < 1e-9 || b.norm() < 1e-9 || !std::isfinite(min_angle)) + return CONSTELLATION_SOLVER_INVALID_INPUT; + a.normalize(); + b.normalize(); + if (a.dot(b) < std::cos(min_angle) && a.dot(b) > -std::cos(min_angle)) + return CONSTELLATION_SOLVER_OK; + return CONSTELLATION_SOLVER_DEGENERATE_BEARINGS; +} +/* Least-squares translation from perpendicular ray projectors; rejects rank loss and LEDs behind the camera. */ +bool +translation(const V3 p[2], const V3 u[2], const Q &q, V3 &out) +{ + M3 m = M3::Zero(); + V3 b = V3::Zero(); + for (unsigned i = 0; i < 2; i++) { + M3 a = M3::Identity() - u[i] * u[i].transpose(); + m += a; + b -= a * (q * p[i]); + } + Eigen::ColPivHouseholderQR solve(m); + solve.setThreshold(1e-8); + if (solve.rank() != 3) + return false; + out = solve.solve(b); + return out.allFinite() && (q * p[0] + out).dot(u[0]) > 0 && (q * p[1] + out).dot(u[1]) > 0 && + (q * p[0] + out).z() > 0 && (q * p[1] + out).z() > 0; +} +void +set_status(enum constellation_constrained_solver_status *out, enum constellation_constrained_solver_status value) +{ + if (out) + *out = value; +} + +struct Ray +{ + V3 u; + unsigned blob_index; +}; +struct View +{ + const constellation_constrained_view *input; + Pose world_cam, cam_world; + std::vector rays; +}; +struct Match +{ + unsigned view, ray, led; + double squared; +}; +struct Candidate +{ + Pose p; + constellation_constrained_origin origin; + unsigned sample_size; + unsigned candidates = 0, visible = 0, distinct = 0; + double sum = 0, cost = 0; + std::vector matches; + M4 covariance = M4::Zero(); + unsigned rank = 0; + //! Whether yaw about gravity is a free parameter of the optical fit. + bool yaw_free = true; + //! World gravity in the model frame of the hypothesis as generated: the reference tilt. + V3 model_gravity; + double association_gap = std::numeric_limits::infinity(); + std::array camera_visible{}, camera_candidates{}, + camera_inliers{}; + Candidate(Pose pose, constellation_constrained_origin source, unsigned samples) + : p(std::move(pose)), origin(source), sample_size(samples), + yaw_free(source == CONSTELLATION_CONSTRAINED_GRAVITY || source == CONSTELLATION_CONSTRAINED_P3P), + model_gravity(p.q.conjugate() * V3::UnitY()) + {} +}; +struct Context +{ + t_constellation_led_model *model; + const constellation_constrained_config &cfg; + std::vector views; + constellation_constrained_result &stats; + Clock::time_point deadline; + //! No trusted position prior: the cold acquisition policy applies. + bool cold = false; + //! Trusted prior position that bounds every hypothesis (see maximum_prior_distance_m). + bool gated = false; + V3 prior_position = V3::Zero(); + bool heading_gated = false; + Q prior_rotation = Q::Identity(); +}; + +bool +outside_prior_gate(Context &ctx, const Candidate &c) +{ + if (ctx.gated && (c.p.t - ctx.prior_position).norm() > ctx.cfg.maximum_prior_distance_m) { + ctx.stats.prior_distance_rejections++; + return true; + } + if (ctx.heading_gated && c.p.q.angularDistance(ctx.prior_rotation) > ctx.cfg.maximum_prior_rotation_rad) { + ctx.stats.prior_rotation_rejections++; + return true; + } + return false; +} + +bool +prepare(Context &ctx, const constellation_constrained_view *views, unsigned count) +{ + if (!ctx.model || !ctx.model->leds || ctx.model->num_leds < 2 || !views || count == 0 || + count > CONSTELLATION_CONSTRAINED_MAX_VIEWS || !(ctx.cfg.score_gate_m > 0) || + !(ctx.cfg.association_gate_m >= ctx.cfg.score_gate_m) || !(ctx.cfg.ray_noise_std_m > 0) || + !(ctx.cfg.minimum_bearing_separation_rad > 0) || ctx.cfg.max_pair_trials == 0 || + !(ctx.cfg.minimum_bearing_separation_rad < pi / 2) || + !(ctx.cfg.maximum_depth_m > ctx.cfg.minimum_depth_m) || ctx.cfg.minimum_depth_m < 0 || + !(ctx.cfg.maximum_position_std_m > 0) || !(ctx.cfg.maximum_yaw_std_rad > 0) || + !(ctx.cfg.ambiguity_chi_square > 0) || ctx.cfg.time_budget_us == 0) + return false; + for (double value : + {ctx.cfg.score_gate_m, ctx.cfg.association_gate_m, ctx.cfg.ray_noise_std_m, + ctx.cfg.minimum_bearing_separation_rad, ctx.cfg.minimum_depth_m, ctx.cfg.maximum_depth_m, + ctx.cfg.maximum_position_std_m, ctx.cfg.maximum_yaw_std_rad, ctx.cfg.ambiguity_chi_square}) { + if (!std::isfinite(value)) + return false; + } + std::array identities{}; + for (unsigned l = 0; l < ctx.model->num_leds; l++) { + const auto &led = ctx.model->leds[l]; + if (!vec(led.pos).allFinite() || !vec(led.dir).allFinite() || vec(led.dir).norm() < 1e-9 || + identities[led.id]) + return false; + identities[led.id] = true; + } + for (unsigned k = 0; k < count; k++) { + const auto &v = views[k]; + if (!v.calib || !v.observation || !valid_pose(v.P_world_cam) || !valid_pose(v.P_cam_world) || + v.observation->num_blobs < 0 || v.observation->num_blobs > MAX_BLOBS_PER_FRAME) + return false; + for (const auto &previous : ctx.views) + if (previous.input->camera_index == v.camera_index) + return false; + View prepared{&v, from_pose(v.P_world_cam), from_pose(v.P_cam_world), {}}; + Pose identity = compose(prepared.cam_world, prepared.world_cam); + if (identity.t.norm() > 1e-4 || identity.q.angularDistance(Q::Identity()) > 1e-4) + return false; + for (int b = 0; b < v.observation->num_blobs; b++) { + if (v.excluded_blobs[b]) + continue; + const blob &blob = v.observation->blobs[b]; + float x, y, z; + if (!std::isfinite(blob.x) || !std::isfinite(blob.y) || + !camera_model_unproject(v.calib, blob.x, blob.y, &x, &y, &z)) + continue; + V3 u(x, y, z); + if (!u.allFinite() || u.norm() < 1e-9 || u.z() <= 0) + continue; + prepared.rays.push_back({u.normalized(), unsigned(b)}); + } + ctx.views.push_back(std::move(prepared)); + } + /* Stable physical camera order; input permutation must not affect truncation or tie breaks. */ + std::sort(ctx.views.begin(), ctx.views.end(), + [](const View &a, const View &b) { return a.input->camera_index < b.input->camera_index; }); + ctx.stats.num_camera_diagnostics = ctx.views.size(); + for (unsigned k = 0; k < ctx.views.size(); k++) { + auto &d = ctx.stats.camera_diagnostics[k]; + d.camera_index = ctx.views[k].input->camera_index; + d.detected_blobs = ctx.views[k].input->observation->num_blobs; + d.eligible_blobs = ctx.views[k].rays.size(); + } + return true; +} + +bool +visible(Context &ctx, const Pose &p_cam, unsigned led, V3 &x) +{ + const auto &l = ctx.model->leds[led]; + x = p_cam.q * vec(l.pos) + p_cam.t; + if (!x.allFinite() || !(x.z() > ctx.cfg.minimum_depth_m) || x.z() > ctx.cfg.maximum_depth_m) { + ctx.stats.depth_rejections++; + return false; + } + V3 normal = p_cam.q * vec(l.dir).normalized(); + if (normal.dot(-x.normalized()) < std::cos(LED_ANGLE * pi / 180)) { + ctx.stats.invisible_rejections++; + return false; + } + if (ctx.model->check_led_visibility) { + V3 origin = p_cam.q.conjugate() * -p_cam.t; + if (!ctx.model->check_led_visibility(ctx.model, led, xvec(origin))) + return false; + } + return true; +} + +bool +score(Context &ctx, Candidate &candidate) +{ + candidate.matches.clear(); + candidate.visible = candidate.candidates = candidate.distinct = 0; + candidate.sum = 0; + candidate.camera_visible.fill(0); + candidate.camera_candidates.fill(0); + candidate.camera_inliers.fill(0); + candidate.association_gap = std::numeric_limits::infinity(); + const double broad2 = ctx.cfg.association_gate_m * ctx.cfg.association_gate_m; + const double gate2 = ctx.cfg.score_gate_m * ctx.cfg.score_gate_m; + std::array distinct{}; + for (unsigned k = 0; k < ctx.views.size(); k++) { + const auto &v = ctx.views[k]; + Pose pc = compose(v.cam_world, candidate.p); + std::vector edges; + for (unsigned l = 0; l < ctx.model->num_leds; l++) { + V3 x; + if (!visible(ctx, pc, l, x)) + continue; + /* Projection is only an in-frame visibility check, never a pixel residual gate. */ + float px, py; + if (!camera_model_project(v.input->calib, x.x(), x.y(), x.z(), &px, &py) || px < 0 || + py < 0 || px >= v.input->calib->width || py >= v.input->calib->height) + continue; + candidate.visible++; + candidate.camera_visible[k]++; + for (unsigned b = 0; b < v.rays.size(); b++) { + double depth = x.dot(v.rays[b].u); + if (!(depth > 0)) + continue; + double e = std::max(0.0, x.squaredNorm() - depth * depth); + if (e <= broad2) + edges.push_back({k, b, l, e}); + } + } + std::sort(edges.begin(), edges.end(), [](const Match &a, const Match &b) { + if (a.squared != b.squared) + return a.squared < b.squared; + if (a.led != b.led) + return a.led < b.led; + return a.ray < b.ray; + }); + std::array used_blobs{}; + std::array used_leds{}; + for (const auto &e : edges) { + if (used_blobs[e.ray] || used_leds[e.led]) + continue; + used_blobs[e.ray] = used_leds[e.led] = true; + candidate.candidates++; + candidate.camera_candidates[k]++; + candidate.sum += std::min(e.squared, gate2); + if (e.squared <= gate2) { + candidate.matches.push_back(e); + candidate.camera_inliers[k]++; + if (!distinct[e.led]) { + distinct[e.led] = true; + candidate.distinct++; + } + } + } + /* Competing LED assignments at a fixed pose are also hypotheses. Test unassigned LEDs and + * pair swaps without inventing permanent identities from temporal blob labels. */ + for (const auto &m : candidate.matches) { + if (m.view != k) + continue; + for (const auto &e : edges) { + if (e.ray != m.ray || e.led == m.led || e.squared > gate2) + continue; + double difference = e.squared - m.squared; + if (used_leds[e.led]) { + bool swapped = false; + for (const auto &other : candidate.matches) { + if (other.view != k || other.led != e.led) + continue; + for (const auto &back : edges) + if (back.ray == other.ray && back.led == m.led && + back.squared <= gate2) { + difference += back.squared - other.squared; + swapped = true; + break; + } + } + if (!swapped) + continue; + } + candidate.association_gap = + std::min(candidate.association_gap, + difference / (ctx.cfg.ray_noise_std_m * ctx.cfg.ray_noise_std_m)); + } + } + } + std::sort(candidate.matches.begin(), candidate.matches.end(), [](const Match &a, const Match &b) { + if (a.view != b.view) + return a.view < b.view; + if (a.ray != b.ray) + return a.ray < b.ray; + return a.led < b.led; + }); + if (candidate.matches.size() <= candidate.sample_size || candidate.candidates <= candidate.sample_size) { + ctx.stats.insufficient_hypotheses++; + return false; + } + candidate.cost = std::sqrt(candidate.sum / (candidate.candidates - candidate.sample_size)); + return candidate.cost <= ctx.cfg.score_gate_m; +} + +unsigned +parameters(const Candidate &c) +{ + /* Heading and retained-pose hypotheses leave yaw to the IMU prior unless the optical fit + * independently constrains it (see refine). */ + return c.yaw_free ? 4 : 3; +} + +//! Where the hypothesis' reference gravity now points in the world; +Y when the tilt is unchanged. +V3 +tilted_gravity(const Candidate &c) +{ + return (c.p.q * c.model_gravity).normalized(); +} + +//! Angle between the refined tilt and the tilt the hypothesis was generated with. +double +tilt_change(const Candidate &c) +{ + return std::acos(std::clamp(tilted_gravity(c).y(), -1.0, 1.0)); +} + +/* Too few distinct LEDs hardly observe tilt, and freeing it there mostly lets the other hand's + * mirrored ring fit a small subset of this one. */ +bool +tilt_free(const Context &ctx, const Candidate &c) +{ + return ctx.cfg.tilt_prior_std_rad > 0 && ctx.cfg.maximum_tilt_correction_rad > 0 && + c.distinct >= ctx.cfg.minimum_tilt_distinct_leds; +} + +/* Parameters: translation, yaw about world gravity, and tilt about world X and Z. Tilt starts at the + * IMU's (or P3P's) value and is held there by a prior of tilt_prior_std_rad, expressed as a + * pseudo-observation with the ray noise so it weighs against the object-space residuals. */ +void +normal_equations(Context &ctx, const Candidate &c, M6 &information, V6 &gradient, double &sse) +{ + information.setZero(); + gradient.setZero(); + sse = 0; + for (const auto &m : c.matches) { + const auto &v = ctx.views[m.view]; + const V3 &u = v.rays[m.ray].u; + V3 rp = c.p.q * vec(ctx.model->leds[m.led].pos); + V3 x = v.cam_world.q * (rp + c.p.t) + v.cam_world.t; + M3 a = M3::Identity() - u * u.transpose(); + Eigen::Matrix j; + j.leftCols<3>() = v.cam_world.q.toRotationMatrix(); + j.col(3) = v.cam_world.q * V3::UnitY().cross(rp); + j.col(4) = v.cam_world.q * V3::UnitX().cross(rp); + j.col(5) = v.cam_world.q * V3::UnitZ().cross(rp); + if (parameters(c) == 3) + j.col(3).setZero(); + j = a * j; + V3 residual = a * x; + information += j.transpose() * j; + gradient += j.transpose() * residual; + sse += residual.squaredNorm(); + } + if (!tilt_free(ctx, c)) { + information(4, 4) = information(5, 5) = 1; + information.block<4, 2>(0, 4).setZero(); + information.block<2, 4>(4, 0).setZero(); + gradient.tail<2>().setZero(); + return; + } + /* Rotating by small angles (ax, az) about world X and Z moves the reference gravity g by + * (-az * g.y, ., ax * g.y): the prior residual is g's horizontal part. */ + double w = std::pow(ctx.cfg.ray_noise_std_m / ctx.cfg.tilt_prior_std_rad, 2); + V3 g = tilted_gravity(c); + information(4, 4) += w; + information(5, 5) += w; + gradient(4) += w * g.z(); + gradient(5) -= w * g.x(); +} + +//! Rotate a refined pose back onto the tilt bound when an update leaves it. +void +bound_tilt(const Context &ctx, Candidate &c) +{ + double angle = tilt_change(c); + double excess = angle - ctx.cfg.maximum_tilt_correction_rad; + if (!(excess > 0)) + return; + V3 axis = tilted_gravity(c).cross(V3::UnitY()); + if (axis.norm() < 1e-12) + return; + c.p.q = (Q(Eigen::AngleAxisd(excess, axis.normalized())) * c.p.q).normalized(); +} + +/* Gauss-Newton over translation, yaw about world gravity when free, and the bounded tilt. The + * covariance uses the proposed ray noise floor, or the fit residual when larger. */ +bool +refine_with(Context &ctx, Candidate &c, bool yaw) +{ + c.yaw_free = yaw; + /* Not bounded by the search deadline: at most 24 retained candidates of 6 iterations each. + * Stopping here would leave candidates with the same identities at slightly different + * unrefined poses, which then look like competitors. */ + for (unsigned iteration = 0; iteration < 6; iteration++) { + M6 information; + V6 gradient; + double sse; + normal_equations(ctx, c, information, gradient, sse); + if (parameters(c) == 3) + information(3, 3) = 1; + Eigen::ColPivHouseholderQR solve(information); + solve.setThreshold(1e-8); + if (solve.rank() != 6) + return false; + V6 change = -solve.solve(gradient); + if (!change.allFinite() || change.head<3>().norm() > 0.1 || std::abs(change(3)) > 0.4 || + change.tail<2>().norm() > 0.4) + break; + Candidate updated = c; + updated.p.t += change.head<3>(); + updated.p.q = + (Q(Eigen::AngleAxisd(change(4), V3::UnitX())) * Q(Eigen::AngleAxisd(change(5), V3::UnitZ())) * + Q(Eigen::AngleAxisd(change(3), V3::UnitY())) * c.p.q) + .normalized(); + bound_tilt(ctx, updated); + if (!score(ctx, updated) || updated.matches.size() < c.matches.size() || updated.cost > c.cost + 1e-9) + break; + c = std::move(updated); + if (change.norm() < 1e-7) + break; + } + M6 info; + V6 gradient; + double sse; + normal_equations(ctx, c, info, gradient, sse); + unsigned dims = parameters(c); + if (dims == 3) + info(3, 3) = 1; + Eigen::ColPivHouseholderQR solve(info); + solve.setThreshold(1e-8); + c.rank = unsigned(solve.rank()) - 2 - (dims == 3 ? 1 : 0); + if (c.rank != dims) + return false; + double variance = std::max(ctx.cfg.ray_noise_std_m * ctx.cfg.ray_noise_std_m, + sse / std::max(1.0, double(2 * c.matches.size()) - dims)); + // Tilt is marginalized: its remaining freedom widens the translation and yaw covariance. + M6 covariance = variance * solve.solve(M6::Identity()); + c.covariance = covariance.topLeftCorner<4, 4>(); + c.covariance = ((c.covariance + c.covariance.transpose()) * 0.5).eval(); + if (dims == 3) + c.covariance(3, 3) = std::numeric_limits::infinity(); + return c.covariance.topLeftCorner<3, 3>().allFinite(); +} + +bool +refine(Context &ctx, Candidate &c) +{ + if (c.origin == CONSTELLATION_CONSTRAINED_GRAVITY || c.origin == CONSTELLATION_CONSTRAINED_P3P) + return refine_with(ctx, c, true); + /* A known-heading hypothesis may still let the optical fit measure yaw. Only claim it with + * redundant observations and a bounded yaw variance; otherwise keep yaw from the prior. */ + if (c.matches.size() >= 3) { + Candidate trial = c; + if (refine_with(ctx, trial, true) && std::sqrt(trial.covariance(3, 3)) <= ctx.cfg.maximum_yaw_std_rad) { + c = std::move(trial); + return true; + } + } + return refine_with(ctx, c, false); +} + +bool +better(const Candidate &a, const Candidate &b) +{ + if (a.matches.size() != b.matches.size()) + return a.matches.size() > b.matches.size(); + if (a.cost != b.cost) + return a.cost < b.cost; + return a.sample_size < b.sample_size; +} +bool +same_assignments(const Candidate &a, const Candidate &b) +{ + if (a.matches.size() != b.matches.size()) + return false; + for (unsigned i = 0; i < a.matches.size(); i++) { + const auto &x = a.matches[i], &y = b.matches[i]; + if (x.view != y.view || x.ray != y.ray || x.led != y.led) + return false; + } + return true; +} +bool +same(const Candidate &a, const Candidate &b) +{ + return same_assignments(a, b) && (a.p.t - b.p.t).norm() < 1e-4 && a.p.q.angularDistance(b.p.q) < 1e-3; +} +/* The same identities at nearby poses are one solution reached from different seeds, not an + * identity ambiguity. Poorly conditioned fits (distant rings, few LEDs) converge to slightly + * different poses; the covariance and the uncertainty gate already account for that spread. */ +bool +same_solution(const Context &ctx, const Candidate &a, const Candidate &b) +{ + return same_assignments(a, b) && (a.p.t - b.p.t).norm() <= ctx.cfg.maximum_position_std_m && + a.p.q.angularDistance(b.p.q) <= ctx.cfg.maximum_yaw_std_rad; +} +void +retain(std::vector &candidates, Candidate c) +{ + for (auto &old : candidates) + if (same(c, old)) { + if (better(c, old)) + old = std::move(c); + return; + } + candidates.push_back(std::move(c)); + std::sort(candidates.begin(), candidates.end(), better); + if (candidates.size() > 24) + candidates.erase(candidates.begin() + 24, candidates.end()); +} + +/* Unmatched rays that pass within the conflict gate of an LED this pose predicts visible: the + * hypothesis says an LED is there yet assigns that blob to nothing. Rays within two LED radii of + * an accepted LED are fragments of that blob, not independent evidence. */ +unsigned +unexplained(Context &ctx, const Candidate &c) +{ + const double conflict = ctx.cfg.conflict_gate_m; + unsigned count = 0; + for (unsigned k = 0; k < ctx.views.size(); k++) { + const auto &v = ctx.views[k]; + Pose pc = compose(v.cam_world, c.p); + std::vector predicted, accepted; + std::vector fragment; + for (unsigned l = 0; l < ctx.model->num_leds; l++) { + V3 x; + if (!visible(ctx, pc, l, x)) + continue; + predicted.push_back(x); + } + std::vector matched(v.rays.size(), false); + for (const auto &m : c.matches) { + if (m.view != k) + continue; + matched[m.ray] = true; + const auto &led = ctx.model->leds[m.led]; + accepted.push_back(pc.q * vec(led.pos) + pc.t); + fragment.push_back(2 * led.radius_mm * 1e-3); + } + auto near = [](const V3 &x, const V3 &u, double gate) { + double d = x.dot(u); + return d > 0 && x.squaredNorm() - d * d <= gate * gate; + }; + for (unsigned b = 0; b < v.rays.size(); b++) { + if (matched[b]) + continue; + const V3 &u = v.rays[b].u; + bool is_fragment = false; + for (unsigned i = 0; i < accepted.size() && !is_fragment; i++) + is_fragment = near(accepted[i], u, fragment[i]); + if (is_fragment) + continue; + for (const auto &x : predicted) + if (near(x, u, conflict)) { + count++; + break; + } + } + } + return count; +} + +/* Angular residual chi-square of the accepted observations and its Wilson-Hilferty quantile. */ +void +fit_test(Context &ctx, const Candidate &c, double &chi_square, double &limit) +{ + chi_square = 0; + for (const auto &m : c.matches) { + const auto &v = ctx.views[m.view]; + V3 x = v.cam_world.q * (c.p.q * vec(ctx.model->leds[m.led].pos) + c.p.t) + v.cam_world.t; + double depth = std::max(x.dot(v.rays[m.ray].u), 1e-6); + double angle = std::sqrt(m.squared) / depth / ctx.cfg.bearing_noise_std_rad; + chi_square += angle * angle; + } + // The tilt prior's pseudo-observations pay for the two tilt parameters. + if (tilt_free(ctx, c)) + chi_square += std::pow(tilt_change(c) / ctx.cfg.tilt_prior_std_rad, 2); + double dof = std::max(1.0, 2.0 * c.matches.size() - parameters(c)); + double h = 2 / (9 * dof); + limit = dof * std::pow(1 - h + ctx.cfg.cold_fit_quantile_z * std::sqrt(h), 3); +} + +bool +finish(Context &ctx, std::vector &candidates) +{ + if (candidates.empty()) { + ctx.stats.rejection = ctx.stats.hypotheses ? CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS + : CONSTELLATION_CONSTRAINED_NO_PAIR; + return false; + } + std::vector fitted; + bool refined = false; + for (auto &c : candidates) { + if (!refine(ctx, c)) + continue; + refined = true; + if (!outside_prior_gate(ctx, c)) + retain(fitted, std::move(c)); + } + if (fitted.empty()) { + // Refined fits that all drifted away from a trusted prior are not near it at all. + ctx.stats.rejection = + refined ? CONSTELLATION_CONSTRAINED_NO_PAIR : CONSTELLATION_CONSTRAINED_RANK_DEFICIENT; + return false; + } + Candidate &best = fitted.front(); + ctx.stats.unexplained_blobs = unexplained(ctx, best); + bool fit_rejected = false; + if (ctx.cfg.bearing_noise_std_rad > 0) { + fit_test(ctx, best, ctx.stats.fit_chi_square, ctx.stats.fit_chi_square_limit); + fit_rejected = ctx.stats.fit_chi_square > ctx.stats.fit_chi_square_limit; + } + unsigned minimum_inliers = ctx.cold ? ctx.cfg.minimum_cold_inliers : ctx.cfg.minimum_tracking_inliers; + unsigned minimum_distinct = + ctx.cold ? ctx.cfg.minimum_cold_distinct_leds : ctx.cfg.minimum_tracking_distinct_leds; + bool cold_count = best.matches.size() < minimum_inliers || best.distinct < minimum_distinct; + bool cold_rejected = fit_rejected || ctx.stats.unexplained_blobs > ctx.cfg.maximum_cold_unexplained_blobs; + ctx.stats.P_world_model = pose(best.p.q, best.p.t); + ctx.stats.origin = best.origin; + ctx.stats.cost_m = best.cost; + ctx.stats.inliers = best.matches.size(); + ctx.stats.distinct_leds = best.distinct; + ctx.stats.candidate_observations = best.candidates; + ctx.stats.projected_visible_leds = best.visible; + ctx.stats.information_rank = best.rank; + ctx.stats.vision_determines_yaw = parameters(best) == 4; + ctx.stats.tilt_correction_rad = tilt_change(best); + for (unsigned r = 0; r < 4; r++) + for (unsigned c = 0; c < 4; c++) + ctx.stats.covariance[4 * r + c] = best.covariance(r, c); + ctx.stats.num_assignments = best.matches.size(); + for (unsigned k = 0; k < ctx.views.size(); k++) { + auto &d = ctx.stats.camera_diagnostics[k]; + d.projected_visible_leds = best.camera_visible[k]; + d.candidate_observations = best.camera_candidates[k]; + d.inliers = best.camera_inliers[k]; + } + for (unsigned i = 0; i < best.matches.size(); i++) { + const auto &m = best.matches[i]; + const auto &v = ctx.views[m.view]; + ctx.stats.assignments[i] = {v.input->camera_index, v.rays[m.ray].blob_index, m.led, + ctx.model->leds[m.led].id, std::sqrt(m.squared)}; + } + /* Consistency first: a count rejection then means the fit itself was sound, which + * arbitration relies on when weighing rejected hypotheses. */ + if (cold_rejected) { + ctx.stats.rejection = CONSTELLATION_CONSTRAINED_INCONSISTENT; + return false; + } + if (cold_count) { + ctx.stats.rejection = CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS; + return false; + } + Eigen::SelfAdjointEigenSolver position(best.covariance.topLeftCorner<3, 3>()); + if (std::sqrt(position.eigenvalues().maxCoeff()) > ctx.cfg.maximum_position_std_m || + (parameters(best) == 4 && std::sqrt(best.covariance(3, 3)) > ctx.cfg.maximum_yaw_std_rad)) { + ctx.stats.rejection = CONSTELLATION_CONSTRAINED_UNCERTAIN; + return false; + } + ctx.stats.second_best_cost_m = std::numeric_limits::infinity(); + ctx.stats.ambiguity_likelihood_gap = best.association_gap; + if (std::isfinite(best.association_gap)) { + ctx.stats.second_best_cost_m = std::sqrt( + std::max(0.0, best.sum + best.association_gap * ctx.cfg.ray_noise_std_m * ctx.cfg.ray_noise_std_m) / + (best.candidates - best.sample_size)); + if (best.association_gap <= ctx.cfg.ambiguity_chi_square) { + ctx.stats.rejection = CONSTELLATION_CONSTRAINED_AMBIGUOUS; + return false; + } + } + for (unsigned i = 1; i < fitted.size(); i++) { + const auto &other = fitted[i]; + if (same_solution(ctx, best, other)) + continue; + /* A competitor must explain all the same observations. Extra observations are evidence, not missing-LED + * penalties. */ + bool covers = true; + for (const auto &b : best.matches) { + bool found = false; + for (const auto &o : other.matches) + if (b.view == o.view && b.ray == o.ray) { + found = true; + break; + } + if (!found) { + covers = false; + break; + } + } + if (!covers) + continue; + double best_sse = 0, other_sse = 0; + for (const auto &b : best.matches) { + best_sse += b.squared; + for (const auto &o : other.matches) + if (b.view == o.view && b.ray == o.ray) { + other_sse += o.squared; + break; + } + } + double gap = (other_sse - best_sse) / (ctx.cfg.ray_noise_std_m * ctx.cfg.ray_noise_std_m); + if (gap < ctx.stats.ambiguity_likelihood_gap) { + ctx.stats.ambiguity_likelihood_gap = gap; + ctx.stats.second_best_cost_m = other.cost; + } + if (gap <= ctx.cfg.ambiguity_chi_square) { + ctx.stats.rejection = CONSTELLATION_CONSTRAINED_AMBIGUOUS; + return false; + } + } + ctx.stats.rejection = CONSTELLATION_CONSTRAINED_ACCEPTED; + return true; +} + +/* One minimal sample: two LED/blob correspondences in the same calibrated view. */ +struct PairSample +{ + unsigned view; + unsigned la, lb, ba, bb; +}; + +/* Generate gravity-constrained and, if available, heading-constrained hypotheses from one pair, + * require the generating pair to survive visibility and the tight gate, then score jointly. */ +void +try_pair(Context &ctx, const PairSample &s, const Pose &prior, bool heading, std::vector &candidates) +{ + const auto &v = ctx.views[s.view]; + const auto &cfg = ctx.cfg; + xrt_vec3 p[2] = {ctx.model->leds[s.la].pos, ctx.model->leds[s.lb].pos}; + xrt_vec3 u[2] = {xvec(v.rays[s.ba].u), xvec(v.rays[s.bb].u)}; + Q qc = v.cam_world.q * prior.q; + xrt_quat tilt = pose(qc, V3::Zero()).orientation; + struct Root + { + xrt_pose p; + constellation_constrained_origin origin; + }; + std::array roots; + unsigned count = 0; + + xrt_pose gravity_roots[2]; + xrt_vec3 g = xvec(v.cam_world.q * V3::UnitY()); + enum constellation_constrained_solver_status status; + unsigned n = constellation_constrained_gravity_two_point(p, u, &tilt, &g, cfg.minimum_bearing_separation_rad, + gravity_roots, &status); + switch (status) { + case CONSTELLATION_SOLVER_OK: break; + case CONSTELLATION_SOLVER_UNRESOLVED_YAW: ctx.stats.unresolved_yaw_pairs++; break; + case CONSTELLATION_SOLVER_NO_ROOT: ctx.stats.no_root_pairs++; break; + case CONSTELLATION_SOLVER_NONPOSITIVE_DEPTH: ctx.stats.depth_pairs++; break; + default: ctx.stats.degenerate_pairs++; break; + } + for (unsigned i = 0; i < n; i++) + roots[count++] = {gravity_roots[i], CONSTELLATION_CONSTRAINED_GRAVITY}; + ctx.stats.gravity_hypotheses += n; + if (heading) { + xrt_pose heading_root; + if (constellation_constrained_heading_two_point(p, u, &tilt, cfg.minimum_bearing_separation_rad, + &heading_root, nullptr)) { + roots[count++] = {heading_root, CONSTELLATION_CONSTRAINED_HEADING}; + ctx.stats.heading_hypotheses++; + } + } + for (unsigned i = 0; i < count; i++) { + ctx.stats.hypotheses++; + Pose cam_model = from_pose(roots[i].p); + bool seeded = true; + for (const auto &pair : {std::pair(s.la, s.ba), {s.lb, s.bb}}) { + V3 x; + if (!visible(ctx, cam_model, pair.first, x)) { + seeded = false; + break; + } + double d = x.dot(v.rays[pair.second].u); + if (x.squaredNorm() - d * d > cfg.score_gate_m * cfg.score_gate_m) { + seeded = false; + break; + } + } + if (!seeded) { + ctx.stats.seed_rejections++; + continue; + } + Candidate c{compose(v.world_cam, cam_model), roots[i].origin, 2}; + if (!outside_prior_gate(ctx, c) && score(ctx, c)) + retain(candidates, std::move(c)); + } +} + +/* Pairs of candidate edges near a trusted prediction, tightest edges first. Temporal blob labels + * are never consulted; edges come from the broad physical association gate. */ +std::vector +prior_pairs(Context &ctx, const Pose &prior) +{ + struct Edge + { + unsigned led, ray; + double squared; + }; + struct Scored + { + PairSample s; + double key; + }; + std::vector scored; + const double broad2 = ctx.cfg.association_gate_m * ctx.cfg.association_gate_m; + for (unsigned k = 0; k < ctx.views.size(); k++) { + const auto &v = ctx.views[k]; + Pose pc = compose(v.cam_world, prior); + std::vector edges; + for (unsigned l = 0; l < ctx.model->num_leds; l++) { + V3 x = pc.q * vec(ctx.model->leds[l].pos) + pc.t; + if (!(x.z() > 0)) + continue; + for (unsigned b = 0; b < v.rays.size(); b++) { + double d = x.dot(v.rays[b].u); + double e = std::max(0.0, x.squaredNorm() - d * d); + if (d > 0 && e <= broad2) + edges.push_back({l, b, e}); + } + } + for (unsigned i = 0; i < edges.size(); i++) + for (unsigned j = i + 1; j < edges.size(); j++) { + const auto &a = edges[i], &b = edges[j]; + if (a.led == b.led || a.ray == b.ray) + continue; + scored.push_back({{k, a.led, b.led, a.ray, b.ray}, std::sqrt(a.squared) + std::sqrt(b.squared)}); + } + } + std::stable_sort(scored.begin(), scored.end(), [](const Scored &a, const Scored &b) { return a.key < b.key; }); + std::vector out; + out.reserve(scored.size()); + for (const auto &s : scored) + out.push_back(s.s); + return out; +} + +} // namespace + +void +constellation_constrained_default_config(constellation_constrained_config *c) +{ + /* Score/association gates and the angular separation follow the stock Windows defaults. + * Everything else is Monado policy chosen with the synthetic + * partial-ring tests in tests_constellation_constrained: object-space ray noise 1 mm, + * position std 3 cm, yaw std 20 degrees, ambiguity gap chi-square 9, at least 4 observations + * of 3 distinct LEDs (cold and tracking), at most one unmatched blob within 8 mm (twice the + * score gate) of a predicted visible LED, bearing noise 0.0026 rad (about 0.5 px at the Rift S + * focal length of ~190 px) tested at the 99.9% chi-square quantile, a tilt adjustment of + * 3 degrees std bounded at 6 degrees on fits of at least 4 distinct LEDs, and the trial/time + * caps. */ + *c = {}; + c->score_gate_m = 0.004; + c->association_gate_m = 0.15; + c->maximum_prior_distance_m = 0.15; + c->maximum_prior_rotation_rad = 30 * pi / 180; + c->tilt_prior_std_rad = 3 * pi / 180; + c->maximum_tilt_correction_rad = 6 * pi / 180; + c->minimum_tilt_distinct_leds = 4; + c->minimum_bearing_separation_rad = 0.1 * pi / 180; + c->minimum_depth_m = 0.0; + c->maximum_depth_m = 1.75; + c->ray_noise_std_m = 0.001; + c->maximum_position_std_m = 0.03; + c->maximum_yaw_std_rad = 0.35; + c->ambiguity_chi_square = 9.0; + c->minimum_cold_inliers = 4; + c->minimum_cold_distinct_leds = 3; + c->minimum_tracking_inliers = 4; + c->minimum_tracking_distinct_leds = 3; + c->maximum_cold_unexplained_blobs = 1; + c->conflict_gate_m = 0.008; + c->bearing_noise_std_rad = 0.0026; + c->cold_fit_quantile_z = 3.09; + c->max_pair_trials = 20000; + c->max_prior_pair_trials = 1000; + c->time_budget_us = 25000; + c->seed = 0; +} + +const char * +constellation_constrained_origin_name(enum constellation_constrained_origin origin) +{ + switch (origin) { + case CONSTELLATION_CONSTRAINED_GRAVITY: return "gravity_p2p"; + case CONSTELLATION_CONSTRAINED_HEADING: return "heading_translation"; + case CONSTELLATION_CONSTRAINED_PRIOR: return "retained_prior"; + case CONSTELLATION_CONSTRAINED_P3P: return "p3p"; + } + return "unknown"; +} + +const char * +constellation_constrained_rejection_name(enum constellation_constrained_rejection rejection) +{ + switch (rejection) { + case CONSTELLATION_CONSTRAINED_ACCEPTED: return "none"; + case CONSTELLATION_CONSTRAINED_INVALID_INPUT: return "invalid_input"; + case CONSTELLATION_CONSTRAINED_NO_PAIR: return "no_pair"; + case CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS: return "verification_count"; + case CONSTELLATION_CONSTRAINED_RANK_DEFICIENT: return "rank"; + case CONSTELLATION_CONSTRAINED_UNCERTAIN: return "uncertainty"; + case CONSTELLATION_CONSTRAINED_AMBIGUOUS: return "ambiguity"; + case CONSTELLATION_CONSTRAINED_SHARED_BLOB: return "shared_blob"; + case CONSTELLATION_CONSTRAINED_INCONSISTENT: return "inconsistent"; + } + return "unknown"; +} + +const char * +constellation_constrained_solver_status_name(enum constellation_constrained_solver_status status) +{ + switch (status) { + case CONSTELLATION_SOLVER_OK: return "ok"; + case CONSTELLATION_SOLVER_INVALID_INPUT: return "invalid_input"; + case CONSTELLATION_SOLVER_DEGENERATE_BEARINGS: return "degenerate_bearings"; + case CONSTELLATION_SOLVER_DEGENERATE_MODEL: return "degenerate_model"; + case CONSTELLATION_SOLVER_UNRESOLVED_YAW: return "unresolved_yaw"; + case CONSTELLATION_SOLVER_NO_ROOT: return "no_root"; + case CONSTELLATION_SOLVER_NONPOSITIVE_DEPTH: return "nonpositive_depth"; + } + return "unknown"; +} + +bool +constellation_constrained_heading_two_point(const xrt_vec3 points[2], + const xrt_vec3 bearings[2], + const xrt_quat *orientation, + double min_angle, + xrt_pose *out, + enum constellation_constrained_solver_status *status) +{ + set_status(status, CONSTELLATION_SOLVER_INVALID_INPUT); + V3 a, b; + if (!orientation || !out || !points || !bearings) + return false; + enum constellation_constrained_solver_status pair = valid_pair(bearings, min_angle, a, b); + if (pair != CONSTELLATION_SOLVER_OK) { + set_status(status, pair); + return false; + } + Q q = quat(*orientation); + V3 p[2] = {vec(points[0]), vec(points[1])}; + if (!q.coeffs().allFinite() || !p[0].allFinite() || !p[1].allFinite()) + return false; + if ((p[1] - p[0]).norm() < 1e-6) { + set_status(status, CONSTELLATION_SOLVER_DEGENERATE_MODEL); + return false; + } + V3 t; + const V3 u[2] = {a, b}; + if (!translation(p, u, q, t)) { + set_status(status, CONSTELLATION_SOLVER_NONPOSITIVE_DEPTH); + return false; + } + *out = pose(q, t); + set_status(status, CONSTELLATION_SOLVER_OK); + return true; +} + +unsigned +constellation_constrained_gravity_two_point(const xrt_vec3 points[2], + const xrt_vec3 bearings[2], + const xrt_quat *tilt, + const xrt_vec3 *gravity, + double min_angle, + xrt_pose out[2], + enum constellation_constrained_solver_status *status) +{ + set_status(status, CONSTELLATION_SOLVER_INVALID_INPUT); + V3 a, b; + if (!tilt || !gravity || !out || !points || !bearings) + return 0; + enum constellation_constrained_solver_status pair = valid_pair(bearings, min_angle, a, b); + if (pair != CONSTELLATION_SOLVER_OK) { + set_status(status, pair); + return 0; + } + V3 g = vec(*gravity); + if (!g.allFinite() || g.norm() < 1e-9) + return 0; + g.normalize(); + Q q0 = quat(*tilt); + V3 p[2] = {vec(points[0]), vec(points[1])}; + V3 d = q0 * (p[1] - p[0]); + if (!d.allFinite() || !q0.coeffs().allFinite()) + return 0; + if (d.norm() < 1e-6) { + set_status(status, CONSTELLATION_SOLVER_DEGENERATE_MODEL); + return 0; + } + /* R(yaw)*d must lie in the plane of the two bearings: A cos + B sin + C = 0. */ + V3 n = a.cross(b).normalized(), parallel = g * d.dot(g), perpendicular = d - parallel; + double A = n.dot(perpendicular), B = n.dot(g.cross(perpendicular)), C = n.dot(parallel); + double amplitude = std::hypot(A, B); + /* Relative to the model baseline: below this the pair does not observe heading. */ + const double eps = 1e-6 * d.norm(); + if (amplitude < eps) { + /* Never turn an arbitrary heading into tracked orientation. */ + set_status(status, std::abs(C) < eps ? CONSTELLATION_SOLVER_UNRESOLVED_YAW : CONSTELLATION_SOLVER_NO_ROOT); + return 0; + } + if (std::abs(C) > amplitude * (1 + 1e-12)) { + set_status(status, CONSTELLATION_SOLVER_NO_ROOT); + return 0; + } + double phase = std::atan2(B, A), offset = std::acos(std::clamp(-C / amplitude, -1.0, 1.0)); + const V3 u[2] = {a, b}; + unsigned count = 0; + for (double yaw : {phase + offset, phase - offset}) { + Q q = Q(Eigen::AngleAxisd(yaw, g)) * q0; + V3 t; + if (!translation(p, u, q, t)) + continue; + if (count && q.angularDistance(quat(out[0].orientation)) < 1e-7) + continue; + out[count++] = pose(q, t); + } + set_status(status, count ? CONSTELLATION_SOLVER_OK : CONSTELLATION_SOLVER_NONPOSITIVE_DEPTH); + return count; +} + +bool +constellation_constrained_search(t_constellation_led_model *model, + const constellation_constrained_view *views, + unsigned count, + const xrt_pose *prior, + bool heading, + bool trusted, + const constellation_constrained_config *cfg, + constellation_constrained_result *out) +{ + if (!out) + return false; + *out = {}; + out->rejection = CONSTELLATION_CONSTRAINED_INVALID_INPUT; + if (!cfg || !prior || !valid_pose(*prior)) + return false; + Context ctx{model, *cfg, {}, *out, Clock::now() + std::chrono::microseconds(cfg->time_budget_us)}; + if (!prepare(ctx, views, count)) + return false; + Pose prior_pose = from_pose(*prior); + ctx.cold = !trusted; + ctx.gated = trusted && cfg->maximum_prior_distance_m > 0; + ctx.prior_position = prior_pose.t; + ctx.heading_gated = trusted && heading && cfg->maximum_prior_rotation_rad > 0; + ctx.prior_rotation = prior_pose.q; + std::vector candidates; + if (trusted && heading) { + /* The retained prior is a one-point hypothesis; it still needs independent observations. */ + Candidate prior_candidate{prior_pose, CONSTELLATION_CONSTRAINED_PRIOR, 1}; + out->hypotheses++; + if (score(ctx, prior_candidate)) + retain(candidates, prior_candidate); + } + + bool complete = true, prior_truncated = false; + if (trusted && heading) { + /* A reliable heading makes the prior's predicted LED positions meaningful, so the + * tightest predicted edges go first. With a ring in view the list easily exceeds the trial + * cap and the fast-path budget (thousands of pairs); requiring it exhausted rejected almost + * every tracked frame (r18). Stop at the cap and judge the candidates found: the + * continuing identity is tried first, and identity jumps still meet the ambiguity test, + * the prior gates and the track's IMU rotation check. */ + std::vector pairs = prior_pairs(ctx, prior_pose); + for (const auto &s : pairs) { + if (out->pair_trials >= cfg->max_prior_pair_trials || Clock::now() >= ctx.deadline) { + prior_truncated = true; + break; + } + out->pair_trials++; + try_pair(ctx, s, prior_pose, heading, candidates); + } + } + if (Clock::now() < ctx.deadline) { + /* Ordered model pairs that a single camera can see together, nearest first. This + * ordering does not depend on the prior's yaw. With a trusted prior only blobs whose + * rays pass near the prior position take part, and the prior gate bounds the result. */ + complete = true; + struct LedPair + { + unsigned a, b; + double distance; + }; + std::vector led_pairs; + double model_radius = 0; + const double covisible = std::cos(2 * LED_ANGLE * pi / 180); + for (unsigned a = 0; a < model->num_leds; a++) { + model_radius = std::max(model_radius, vec(model->leds[a].pos).norm()); + for (unsigned b = 0; b < model->num_leds; b++) { + if (a == b) + continue; + const auto &la = model->leds[a], &lb = model->leds[b]; + if (vec(la.dir).normalized().dot(vec(lb.dir).normalized()) < covisible) + continue; + led_pairs.push_back({a, b, (vec(la.pos) - vec(lb.pos)).norm()}); + } + } + std::stable_sort(led_pairs.begin(), led_pairs.end(), + [](const LedPair &x, const LedPair &y) { return x.distance < y.distance; }); + /* Unordered blob pairs per view, nearest first: image neighbours usually are model neighbours. */ + struct BlobPair + { + unsigned a, b; + double angle; + }; + const double near2 = std::pow(cfg->maximum_prior_distance_m + model_radius, 2); + std::vector> blob_pairs(ctx.views.size()); + size_t longest = 0; + for (unsigned k = 0; k < ctx.views.size(); k++) { + const auto &v = ctx.views[k]; + const auto &rays = v.rays; + std::vector near(rays.size(), true); + if (ctx.gated) { + V3 x = v.cam_world.q * prior_pose.t + v.cam_world.t; + for (unsigned a = 0; a < rays.size(); a++) { + double d = x.dot(rays[a].u); + near[a] = d > 0 && x.squaredNorm() - d * d <= near2; + } + } + for (unsigned a = 0; a < rays.size(); a++) + for (unsigned b = a + 1; b < rays.size(); b++) + if (near[a] && near[b]) + blob_pairs[k].push_back( + {a, b, std::acos(std::clamp(rays[a].u.dot(rays[b].u), -1.0, 1.0))}); + std::stable_sort(blob_pairs[k].begin(), blob_pairs[k].end(), + [](const BlobPair &x, const BlobPair &y) { return x.angle < y.angle; }); + longest = std::max(longest, blob_pairs[k].size()); + } + /* Visit (blob pair rank + model pair rank) diagonals, interleaving cameras, so the trial cap + * and time budget are spent on adjacent assignments first in every view. */ + const unsigned cap = out->pair_trials + cfg->max_pair_trials; + if (!led_pairs.empty() && longest) { + const size_t diagonals = longest + led_pairs.size() - 1; + for (size_t sum = 0; sum < diagonals && complete; sum++) { + for (unsigned k = 0; k < ctx.views.size() && complete; k++) { + const auto &bp = blob_pairs[k]; + size_t first = sum >= led_pairs.size() ? sum - led_pairs.size() + 1 : 0; + for (size_t i = first; i <= sum && i < bp.size(); i++) { + if (out->pair_trials >= cap || Clock::now() >= ctx.deadline) { + complete = false; + break; + } + const auto &lp = led_pairs[sum - i]; + out->pair_trials++; + try_pair(ctx, {k, lp.a, lp.b, bp[i].a, bp[i].b}, prior_pose, heading, + candidates); + } + } + } + } + } else { + complete = false; + } + out->budget_exhausted = !complete || prior_truncated; + return finish(ctx, candidates); +} + +bool +constellation_constrained_verify_pose(t_constellation_led_model *model, + const constellation_constrained_view *views, + unsigned count, + const xrt_pose *p, + constellation_constrained_origin origin, + const constellation_constrained_config *cfg, + constellation_constrained_result *out) +{ + if (!out) + return false; + *out = {}; + out->rejection = CONSTELLATION_CONSTRAINED_INVALID_INPUT; + if (!cfg || !p || !valid_pose(*p)) + return false; + Context ctx{model, *cfg, {}, *out, Clock::now() + std::chrono::microseconds(cfg->time_budget_us)}; + if (!prepare(ctx, views, count)) + return false; + /* A hypothesis built from k correspondences explains them by construction: require more. */ + unsigned sample_size = origin == CONSTELLATION_CONSTRAINED_PRIOR ? 1u + : origin == CONSTELLATION_CONSTRAINED_P3P ? 3u + : 2u; + Candidate c{from_pose(*p), origin, sample_size}; + // P3P is the cold-acquisition fallback, so the same cold verification policy applies. + ctx.cold = origin == CONSTELLATION_CONSTRAINED_P3P; + out->hypotheses = 1; + std::vector candidates; + if (score(ctx, c)) + candidates.push_back(std::move(c)); + return finish(ctx, candidates); +} + +bool +constellation_constrained_results_share_blob(const constellation_constrained_result *a, + const constellation_constrained_result *b) +{ + if (!a || !b) + return false; + for (unsigned i = 0; i < a->num_assignments; i++) + for (unsigned j = 0; j < b->num_assignments; j++) { + const auto &x = a->assignments[i], &y = b->assignments[j]; + if (x.camera_index == y.camera_index && x.blob_index == y.blob_index) + return true; + } + return false; +} + +static bool +clearly_stronger(const constellation_constrained_result &mine, + const constellation_constrained_result &other, + unsigned margin) +{ + return mine.inliers >= other.inliers + margin && mine.distinct_leds >= other.distinct_leds; +} + +static bool +competitive_rejection(const constellation_constrained_result &r) +{ + return r.num_assignments > 0 && (r.rejection == CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS || + r.rejection == CONSTELLATION_CONSTRAINED_AMBIGUOUS || + r.rejection == CONSTELLATION_CONSTRAINED_UNCERTAIN); +} + +void +constellation_constrained_arbitrate(const constellation_constrained_result *const *results, + const bool *published, + bool *candidates, + enum constellation_constrained_claim *blocked, + unsigned count, + unsigned margin) +{ + constellation_constrained_arbitrate_with_owners(results, published, candidates, blocked, nullptr, count, margin); +} +void +constellation_constrained_arbitrate_with_owners(const constellation_constrained_result *const *results, + const bool *published, + bool *candidates, + enum constellation_constrained_claim *blocked, + unsigned *blockers, + unsigned count, + unsigned margin) +{ + if (!results || !published || !candidates || !blocked) + return; + for (unsigned i = 0; i < count; i++) { + blocked[i] = CONSTELLATION_CLAIM_NONE; + if (blockers) + blockers[i] = count; + } + /* Decide every candidate against the original claims before clearing any of them, so the + * outcome does not depend on device order. */ + for (unsigned a = 0; a < count; a++) { + if (!candidates[a] || !results[a]) + continue; + const auto &mine = *results[a]; + for (unsigned b = 0; b < count; b++) { + if (b == a || !results[b] || + !constellation_constrained_results_share_blob(results[a], results[b])) + continue; + const auto &other = *results[b]; + if (published[b]) { + blocked[a] = CONSTELLATION_CLAIM_PUBLISHED; + if (blockers) + blockers[a] = b; + break; // No stronger reason exists. + } else if (candidates[b]) { + if (!clearly_stronger(mine, other, margin)) { + blocked[a] = CONSTELLATION_CLAIM_CANDIDATE; + if (blockers) + blockers[a] = b; + } + } else if (blocked[a] == CONSTELLATION_CLAIM_NONE && competitive_rejection(other) && + other.inliers >= mine.inliers) { + blocked[a] = CONSTELLATION_CLAIM_REJECTED; + if (blockers) + blockers[a] = b; + } + } + } + for (unsigned i = 0; i < count; i++) + if (blocked[i] != CONSTELLATION_CLAIM_NONE) + candidates[i] = false; +} + +void +constellation_constrained_default_motion_limits(constellation_motion_limits *limits) +{ + // Proposed values: fast hand motion stays below about 3 m/s; 2 cm covers fit noise. + *limits = {3.0, 0.02, 10 * pi / 180, 300000000}; +} + +bool +constellation_constrained_motion_consistent(const constellation_track_point *prev, + const constellation_track_point *next, + const constellation_motion_limits *limits, + double *position_excess_m, + double *rotation_error_rad) +{ + double excess = std::numeric_limits::infinity(), rotation = pi; + bool ok = false; + if (prev && next && limits && valid_pose(prev->P_world_model) && valid_pose(next->P_world_model) && + next->capture_ns > prev->capture_ns && next->capture_ns - prev->capture_ns <= limits->max_gap_ns) { + double dt = (next->capture_ns - prev->capture_ns) * 1e-9; + double moved = (vec(next->P_world_model.position) - vec(prev->P_world_model.position)).norm(); + excess = moved - (limits->position_slack_m + limits->max_speed_mps * dt); + // Body-frame deltas: a constant offset between the optical and IMU worlds cancels. + Q optical = quat(prev->P_world_model.orientation).conjugate() * quat(next->P_world_model.orientation); + Q imu = quat(prev->imu_world).conjugate() * quat(next->imu_world); + rotation = optical.angularDistance(imu); + ok = excess <= 0 && rotation <= limits->max_rotation_error_rad; + } + if (position_excess_m) + *position_excess_m = excess; + if (rotation_error_rad) + *rotation_error_rad = rotation; + return ok; +} + +void +constellation_track_init(constellation_track *track) +{ + *track = {}; + track->state = CONSTELLATION_TRACK_LOST; + track->max_jumps = 3; + constellation_constrained_default_motion_limits(&track->limits); +} + +void +constellation_track_reset(constellation_track *track) +{ + track->state = CONSTELLATION_TRACK_LOST; + track->consecutive_jumps = 0; + track->have_published = false; +} + +bool +constellation_track_expire(constellation_track *track, int64_t capture_ns) +{ + if (track->state == CONSTELLATION_TRACK_LOST || capture_ns - track->ref.capture_ns <= track->limits.max_gap_ns) + return false; + track->state = CONSTELLATION_TRACK_LOST; + track->consecutive_jumps = 0; + track->have_published = false; + return true; +} + +enum constellation_track_verdict +constellation_track_update(constellation_track *track, + const constellation_track_point *next_in, + bool orientation_observed, + const xrt_quat *ref_imu_world, + double *position_excess_m, + double *rotation_error_rad) +{ + double excess = std::numeric_limits::infinity(), rotation = pi; + constellation_track_point next = *next_in; + if (!orientation_observed) + next.P_world_model.orientation = next.imu_world; + enum constellation_track_verdict verdict = CONSTELLATION_TRACK_PUBLISH; + if (track->state != CONSTELLATION_TRACK_LOST && next.capture_ns <= track->ref.capture_ns) { + verdict = CONSTELLATION_TRACK_HOLD_STALE; + } else if (track->state == CONSTELLATION_TRACK_LOST) { + track->state = CONSTELLATION_TRACK_CONFIRMING; + verdict = CONSTELLATION_TRACK_HOLD_CONFIRMING; + } else { + bool consistent = false; + if (ref_imu_world) { + constellation_track_point ref = track->ref; + ref.imu_world = *ref_imu_world; + // An optical/IMU heading offset only cancels when both endpoints observe yaw. + // A source change has no measured optical rotation to compare; position still gates it. + if (!track->ref_orientation_observed || !orientation_observed) { + ref.P_world_model.orientation = ref.imu_world; + next.P_world_model.orientation = next.imu_world; + } + consistent = constellation_constrained_motion_consistent(&ref, &next, &track->limits, &excess, + &rotation); + if (orientation_observed) + next.P_world_model.orientation = next_in->P_world_model.orientation; + } + if (track->state == CONSTELLATION_TRACK_CONFIRMING) { + if (track->have_published && next.capture_ns > track->published.capture_ns && + next.capture_ns - track->published.capture_ns <= track->limits.max_gap_ns) { + double dt = (next.capture_ns - track->published.capture_ns) * 1e-9; + double distance = + (vec(next.P_world_model.position) - vec(track->published.P_world_model.position)) + .norm(); + consistent = consistent && distance <= track->limits.position_slack_m + + track->limits.max_speed_mps * dt; + } + if (consistent) + track->state = CONSTELLATION_TRACK_TRACKING; + else + verdict = CONSTELLATION_TRACK_HOLD_UNCONFIRMED; + } else if (consistent) { + track->consecutive_jumps = 0; + } else if (++track->consecutive_jumps >= track->max_jumps) { + // Either the track or the new evidence is wrong; publish neither until confirmed. + track->state = CONSTELLATION_TRACK_CONFIRMING; + track->consecutive_jumps = 0; + verdict = CONSTELLATION_TRACK_HOLD_RESTART; + } else { + verdict = CONSTELLATION_TRACK_HOLD_JUMP; + } + } + if (verdict != CONSTELLATION_TRACK_HOLD_STALE && verdict != CONSTELLATION_TRACK_HOLD_JUMP) { + track->ref = next; + track->ref_orientation_observed = orientation_observed; + } + if (verdict == CONSTELLATION_TRACK_PUBLISH) { + track->published = next; + track->have_published = true; + } + if (position_excess_m) + *position_excess_m = excess; + if (rotation_error_rad) + *rotation_error_rad = rotation; + return verdict; +} + +const char * +constellation_track_state_name(enum constellation_track_state state) +{ + switch (state) { + case CONSTELLATION_TRACK_LOST: return "lost"; + case CONSTELLATION_TRACK_CONFIRMING: return "confirming"; + case CONSTELLATION_TRACK_TRACKING: return "tracking"; + default: return "unknown"; + } +} + +const char * +constellation_track_verdict_name(enum constellation_track_verdict verdict) +{ + switch (verdict) { + case CONSTELLATION_TRACK_PUBLISH: return "publish"; + case CONSTELLATION_TRACK_HOLD_CONFIRMING: return "confirming"; + case CONSTELLATION_TRACK_HOLD_UNCONFIRMED: return "unconfirmed"; + case CONSTELLATION_TRACK_HOLD_JUMP: return "jump"; + case CONSTELLATION_TRACK_HOLD_RESTART: return "jump_restart"; + case CONSTELLATION_TRACK_HOLD_STALE: return "stale_exposure"; + } + return "unknown"; +} + +bool +constellation_constrained_contests_published(const constellation_constrained_result *candidate, + const constellation_constrained_result *published, + unsigned margin) +{ + return candidate && published && constellation_constrained_results_share_blob(candidate, published) && + clearly_stronger(*candidate, *published, margin); +} + +const char * +constellation_constrained_claim_name(enum constellation_constrained_claim claim) +{ + switch (claim) { + case CONSTELLATION_CLAIM_NONE: return "none"; + case CONSTELLATION_CLAIM_PUBLISHED: return "published"; + case CONSTELLATION_CLAIM_CANDIDATE: return "candidate"; + case CONSTELLATION_CLAIM_REJECTED: return "rejected"; + } + return "unknown"; +} + +bool +constellation_constrained_select_results(const constellation_constrained_result *results, + unsigned count, + const constellation_constrained_config *cfg, + constellation_constrained_result *out) +{ + if (!out) + return false; + const constellation_constrained_result *best = nullptr; + for (unsigned i = 0; results && i < count; i++) { + const auto &r = results[i]; + if (r.rejection != CONSTELLATION_CONSTRAINED_ACCEPTED) + continue; + if (!best || r.inliers > best->inliers || (r.inliers == best->inliers && r.cost_m < best->cost_m)) + best = &r; + } + if (!best || !cfg || !(cfg->ray_noise_std_m > 0)) { + *out = {}; + out->rejection = CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS; + return false; + } + /* Copy first so out may alias one of the candidate records. */ + const constellation_constrained_result selected = *best; + bool ambiguous = false; + double smallest_gap = selected.ambiguity_likelihood_gap; + double second_cost = selected.second_best_cost_m; + for (unsigned i = 0; i < count; i++) { + const auto &other = results[i]; + if (&other == best || other.rejection != CONSTELLATION_CONSTRAINED_ACCEPTED) + continue; + bool covers = true, same_assignments = selected.num_assignments == other.num_assignments; + double best_sse = 0, other_sse = 0; + for (unsigned b = 0; b < selected.num_assignments; b++) { + const auto &x = selected.assignments[b]; + bool found = false; + for (unsigned j = 0; j < other.num_assignments; j++) { + const auto &y = other.assignments[j]; + if (x.camera_index == y.camera_index && x.blob_index == y.blob_index) { + found = true; + same_assignments &= x.led_index == y.led_index; + best_sse += x.residual_m * x.residual_m; + other_sse += y.residual_m * y.residual_m; + break; + } + } + if (!found) { + covers = false; + break; + } + } + if (!covers) + continue; + if (same_assignments && + (vec(selected.P_world_model.position) - vec(other.P_world_model.position)).norm() < 1e-4 && + quat(selected.P_world_model.orientation).angularDistance(quat(other.P_world_model.orientation)) < + 1e-3) + continue; + double gap = (other_sse - best_sse) / (cfg->ray_noise_std_m * cfg->ray_noise_std_m); + if (gap < smallest_gap) { + smallest_gap = gap; + second_cost = other.cost_m; + } + if (gap <= cfg->ambiguity_chi_square) + ambiguous = true; + } + *out = selected; + out->ambiguity_likelihood_gap = smallest_gap; + out->second_best_cost_m = second_cost; + if (ambiguous) + out->rejection = CONSTELLATION_CONSTRAINED_AMBIGUOUS; + return !ambiguous; +} diff --git a/src/xrt/tracking/constellation/internal/constrained_pose.h b/src/xrt/tracking/constellation/internal/constrained_pose.h new file mode 100644 index 000000000..d4555e3dc --- /dev/null +++ b/src/xrt/tracking/constellation/internal/constrained_pose.h @@ -0,0 +1,424 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include "blobwatch.h" +#include "camera_model.h" +#include "tracking/t_led_models.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define CONSTELLATION_CONSTRAINED_MAX_VIEWS 5 +#define CONSTELLATION_CONSTRAINED_MAX_ASSIGNMENTS (CONSTELLATION_CONSTRAINED_MAX_VIEWS * MAX_BLOBS_PER_FRAME) + +/* Both transforms use the camera's optical (+Z forward) coordinates. */ +struct constellation_constrained_view +{ + unsigned camera_index; + struct xrt_pose P_world_cam; + struct xrt_pose P_cam_world; + const struct camera_model *calib; + const struct blobservation *observation; + //! Same-exposure published claims to omit from acquisition; temporal LED labels stay advisory. + bool excluded_blobs[MAX_BLOBS_PER_FRAME]; +}; + +enum constellation_constrained_origin +{ + CONSTELLATION_CONSTRAINED_GRAVITY, + CONSTELLATION_CONSTRAINED_HEADING, + CONSTELLATION_CONSTRAINED_PRIOR, + CONSTELLATION_CONSTRAINED_P3P, +}; + +enum constellation_constrained_rejection +{ + CONSTELLATION_CONSTRAINED_ACCEPTED, + CONSTELLATION_CONSTRAINED_INVALID_INPUT, + CONSTELLATION_CONSTRAINED_NO_PAIR, + CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS, + CONSTELLATION_CONSTRAINED_RANK_DEFICIENT, + CONSTELLATION_CONSTRAINED_UNCERTAIN, + CONSTELLATION_CONSTRAINED_AMBIGUOUS, + CONSTELLATION_CONSTRAINED_SHARED_BLOB, + //! Cold fit fails the angular goodness-of-fit test or leaves blobs unexplained inside its footprint. + CONSTELLATION_CONSTRAINED_INCONSISTENT, +}; + +//! Outcome of one minimal two-point solve, so degenerate geometry is never silently resolved. +enum constellation_constrained_solver_status +{ + CONSTELLATION_SOLVER_OK, + CONSTELLATION_SOLVER_INVALID_INPUT, + //! Bearings closer than the minimum separation (or anti-parallel). + CONSTELLATION_SOLVER_DEGENERATE_BEARINGS, + //! The two model points coincide. + CONSTELLATION_SOLVER_DEGENERATE_MODEL, + //! The pair carries no heading information: every yaw explains it equally. + CONSTELLATION_SOLVER_UNRESOLVED_YAW, + //! No real yaw root exists for this assignment. + CONSTELLATION_SOLVER_NO_ROOT, + //! Roots exist but put an LED behind the camera or leave translation rank deficient. + CONSTELLATION_SOLVER_NONPOSITIVE_DEPTH, +}; + +struct constellation_constrained_config +{ + //! Object-space score gate in metres (stock default, not an active setting). + double score_gate_m; + //! Broad object-space association gate in metres for candidate edges. + double association_gate_m; + /* With a trusted prior, hypotheses whose model origin lands farther than this from the prior + * position are discarded. The prior only selects candidate edges; without this bound a + * hypothesis anywhere in space could win on inlier count and either be published as a wrong + * identity or make the correct one look ambiguous. */ + double maximum_prior_distance_m; + //! Bound association rotation only while a confirmed track has a verified heading. + double maximum_prior_rotation_rad; + /* Refinement may change the hypothesis' tilt (from the IMU, or from P3P) under a Gaussian + * prior of this standard deviation, and never by more than the maximum: the accelerometer and + * the attitude history at the exposure time are off by a few degrees, which otherwise fails + * the goodness-of-fit test on well-observed rings. Zero disables the adjustment, and fits of + * fewer distinct LEDs than the minimum keep the prior tilt. */ + double tilt_prior_std_rad; + double maximum_tilt_correction_rad; + unsigned minimum_tilt_distinct_leds; + double minimum_bearing_separation_rad; + double minimum_depth_m; + double maximum_depth_m; + /* Proposed noise/uncertainty policy, not device calibration. */ + double ray_noise_std_m; + double maximum_position_std_m; + double maximum_yaw_std_rad; + //! Minimum likelihood gap (chi-square units of ray_noise_std_m) to the best competing assignment. + double ambiguity_chi_square; + /* Monado verification policy, beyond the stock scorer count (inliers > sample size): + * accepted observations required when acquiring without a trusted position prior, and the + * distinct physical LEDs among them. Repeated views of one LED do not fix its identity. */ + unsigned minimum_cold_inliers; + unsigned minimum_cold_distinct_leds; + /* The same counts for searches around a trusted prior. The stock minimum (inliers > sample + * size, so 2 for the retained prior) let one stale prior keep re-verifying itself on two blobs + * in worn use; a published pose now needs this much evidence on every path. */ + unsigned minimum_tracking_inliers; + unsigned minimum_tracking_distinct_leds; + /* Unmatched blobs within conflict_gate_m (object space) of an LED the hypothesis predicts + * visible, ignoring fragments next to an accepted LED. A wrong fit typically puts predicted + * LEDs next to the ring's own blobs without explaining them. Applies on every path. */ + unsigned maximum_cold_unexplained_blobs; + double conflict_gate_m; + /* Goodness of fit on every path: angular residuals divided by this centroid bearing noise + * must pass a chi-square test at the given standard-normal quantile (Wilson-Hilferty). The noise + * is a proposed value pending measurement from replayed frames; zero disables the test. */ + double bearing_noise_std_rad; + double cold_fit_quantile_z; + //! Cap on minimal-sample pair trials without a trusted position prior. + unsigned max_pair_trials; + //! Cap on pair trials drawn from edges near a trusted position prior. + unsigned max_prior_pair_trials; + unsigned time_budget_us; + //! Unused by the deterministic enumeration; kept so callers can record the exposure identity. + uint64_t seed; +}; + +struct constellation_constrained_assignment +{ + unsigned camera_index; + unsigned blob_index; + unsigned led_index; + uint8_t led_id; + double residual_m; +}; + +struct constellation_constrained_result +{ + struct xrt_pose P_world_model; + enum constellation_constrained_origin origin; + enum constellation_constrained_rejection rejection; + unsigned candidate_observations; + unsigned inliers; + unsigned distinct_leds; + unsigned projected_visible_leds; + unsigned unexplained_blobs; + //! Chi-square of angular residuals and its threshold for the selected hypothesis. + double fit_chi_square; + double fit_chi_square_limit; + unsigned pair_trials; + unsigned hypotheses; + unsigned gravity_hypotheses; + unsigned heading_hypotheses; + unsigned degenerate_pairs; + unsigned unresolved_yaw_pairs; + unsigned no_root_pairs; + unsigned depth_pairs; + unsigned seed_rejections; + unsigned insufficient_hypotheses; + unsigned invisible_rejections; + unsigned depth_rejections; + //! Trusted-prior hypotheses discarded for landing outside maximum_prior_distance_m. + unsigned prior_distance_rejections; + unsigned prior_rotation_rejections; + unsigned num_camera_diagnostics; + struct + { + unsigned camera_index; + unsigned detected_blobs; + unsigned eligible_blobs; + unsigned projected_visible_leds; + unsigned candidate_observations; + unsigned inliers; + } camera_diagnostics[CONSTELLATION_CONSTRAINED_MAX_VIEWS]; + bool budget_exhausted; + /* Cost is clipped object-space RMS in metres, normalized by candidates - sample size. */ + double cost_m; + double second_best_cost_m; + double ambiguity_likelihood_gap; + unsigned information_rank; + /* Row-major [world translation, yaw about world +Y]; absent yaw has infinite variance. */ + double covariance[16]; + //! True only when the optical fit itself constrains yaw within maximum_yaw_std_rad. + bool vision_determines_yaw; + //! How far refinement moved the tilt from the hypothesis' prior tilt. + double tilt_correction_rad; + unsigned num_assignments; + struct constellation_constrained_assignment assignments[CONSTELLATION_CONSTRAINED_MAX_ASSIGNMENTS]; +}; + +void +constellation_constrained_default_config(struct constellation_constrained_config *config); + +const char * +constellation_constrained_origin_name(enum constellation_constrained_origin origin); + +const char * +constellation_constrained_rejection_name(enum constellation_constrained_rejection rejection); + +const char * +constellation_constrained_solver_status_name(enum constellation_constrained_solver_status status); + +/* Enumerate every real same-camera yaw/depth root for a gravity-aligned tilt. Returns the + * number of poses (model to camera). Zero roots always come with a non-OK status. */ +unsigned +constellation_constrained_gravity_two_point(const struct xrt_vec3 points[2], + const struct xrt_vec3 bearings[2], + const struct xrt_quat *tilt_model_to_cam, + const struct xrt_vec3 *gravity_cam, + double minimum_bearing_separation_rad, + struct xrt_pose out_poses[2], + enum constellation_constrained_solver_status *status); + +/* Translation from two bearings with a known model-to-camera rotation. */ +bool +constellation_constrained_heading_two_point(const struct xrt_vec3 points[2], + const struct xrt_vec3 bearings[2], + const struct xrt_quat *model_to_cam, + double minimum_bearing_separation_rad, + struct xrt_pose *out_pose, + enum constellation_constrained_solver_status *status); + +/* Low-count acquisition. Gravity-constrained two-point hypotheses are always generated from + * the tilt of prior_world. With reliable_heading, known-orientation translation hypotheses are + * added. With trusted_position, pairs come from edges near the prior and, if heading is also + * reliable, the prior pose itself is scored as a one-point hypothesis. */ +bool +constellation_constrained_search(struct t_constellation_led_model *model, + const struct constellation_constrained_view *views, + unsigned num_views, + const struct xrt_pose *prior_world, + bool reliable_heading, + bool trusted_position, + const struct constellation_constrained_config *config, + struct constellation_constrained_result *result); + +/* Jointly verify/refine an existing hypothesis. Its sample size follows origin: + * prior 1, gravity/heading 2, P3P 3. P3P hypotheses are cold acquisitions and also get the + * cold verification policy. */ +bool +constellation_constrained_verify_pose(struct t_constellation_led_model *model, + const struct constellation_constrained_view *views, + unsigned num_views, + const struct xrt_pose *pose_world, + enum constellation_constrained_origin origin, + const struct constellation_constrained_config *config, + struct constellation_constrained_result *result); + +bool +constellation_constrained_results_share_blob(const struct constellation_constrained_result *a, + const struct constellation_constrained_result *b); + +//! What kind of claim on a shared blob blocked a candidate. +enum constellation_constrained_claim +{ + CONSTELLATION_CLAIM_NONE, + //! Another device already published a pose using the blob in this exposure. + CONSTELLATION_CLAIM_PUBLISHED, + //! Another candidate explains the blob and neither has clearly more support. + CONSTELLATION_CLAIM_CANDIDATE, + //! Another device's rejected hypothesis passed the consistency test and explains at least as + //! many observations. + CONSTELLATION_CLAIM_REJECTED, +}; + +/* Cross-device ownership. A candidate whose observations share a blob with another device's + * claim loses (candidates[i] cleared, blocked[i] set) when the claim is a published pose; when it + * is another candidate that does not have clearly less support (minimum_support_margin more + * inliers and no fewer distinct LEDs wins); or when it is a competitive rejected hypothesis, that + * is one rejected only for count, ambiguity or uncertainty with at least as many inliers. + * Hypotheses rejected as inconsistent or without a pair are not evidence of ownership. + * Published results are never revoked. */ +void +constellation_constrained_arbitrate(const struct constellation_constrained_result *const *results, + const bool *published, + bool *candidates, + enum constellation_constrained_claim *blocked, + unsigned count, + unsigned minimum_support_margin); + +// Optional blocker indices name the claim actually responsible for rejection; count means none. +void +constellation_constrained_arbitrate_with_owners(const struct constellation_constrained_result *const *results, + const bool *published, + bool *candidates, + enum constellation_constrained_claim *blocked, + unsigned *blockers, + unsigned count, + unsigned minimum_support_margin); + +const char * +constellation_constrained_claim_name(enum constellation_constrained_claim claim); + +/* True when a candidate blocked by a published pose (CONSTELLATION_CLAIM_PUBLISHED) shares a blob + * with it and has clearly more support by the same margin rule. The published pose stays + * published, but its track should end: a lock on the other controller's ring otherwise keeps + * that controller's blobs forever. */ +bool +constellation_constrained_contests_published(const struct constellation_constrained_result *candidate, + const struct constellation_constrained_result *published, + unsigned minimum_support_margin); + +/* One accepted optical pose with the IMU attitude at the same capture time, both in world. Read + * both points' IMU attitudes from the same, current attitude history: a world-yaw correction + * applied in between then cancels instead of looking like an optical jump. */ +struct constellation_track_point +{ + struct xrt_pose P_world_model; + struct xrt_quat imu_world; + int64_t capture_ns; +}; + +/* Monado policy for publishing a pose after an earlier one. A hand can move fast, but an optical + * identity error moves the estimate by about one LED spacing or flips it onto the other ring + * without the controller's gyro seeing the rotation. */ +struct constellation_motion_limits +{ + double max_speed_mps; + double position_slack_m; + /* Bound on the difference between the optical and the IMU rotation over the same interval, + * both expressed in the controller's own frame. The IMU's world yaw may be far from the + * optical one (the driver corrects it gradually); a world-frame comparison would then see + * every wrist rotation about a non-vertical axis as an error. */ + double max_rotation_error_rad; + //! Older reference poses do not constrain the new one. + int64_t max_gap_ns; +}; + +void +constellation_constrained_default_motion_limits(struct constellation_motion_limits *limits); + +/* True when next is a plausible continuation of prev. Outputs the position change beyond the + * allowed motion (<= 0 when inside) and the body-frame rotation disagreement between optics and + * IMU. */ +bool +constellation_constrained_motion_consistent(const struct constellation_track_point *prev, + const struct constellation_track_point *next, + const struct constellation_motion_limits *limits, + double *position_excess_m, + double *rotation_error_rad); + +/*! + * Publication state of one device. A pose is published only when it continues an earlier one: a + * cold acquisition waits for a consistent second result, and while tracking every result must be a + * plausible continuation of the last published pose. Missing a pose is preferred over publishing a + * wrong one. This is Monado policy. + */ +enum constellation_track_state +{ + CONSTELLATION_TRACK_LOST, + //! One unpublished result is waiting for a consistent second one. + CONSTELLATION_TRACK_CONFIRMING, + CONSTELLATION_TRACK_TRACKING, + CONSTELLATION_TRACK_STATE_COUNT, +}; + +enum constellation_track_verdict +{ + CONSTELLATION_TRACK_PUBLISH, + //! First result after loss; held until a consistent second result confirms it. + CONSTELLATION_TRACK_HOLD_CONFIRMING, + //! Inconsistent with the held result; it replaces that result as the one to confirm. + CONSTELLATION_TRACK_HOLD_UNCONFIRMED, + //! Inconsistent with the published track; the track is kept. + CONSTELLATION_TRACK_HOLD_JUMP, + //! Repeatedly inconsistent: the track ended and this result is held for confirmation. + CONSTELLATION_TRACK_HOLD_RESTART, + //! Not newer than the reference. + CONSTELLATION_TRACK_HOLD_STALE, +}; + +struct constellation_track +{ + enum constellation_track_state state; + //! Last published pose while tracking, or the result awaiting confirmation. + struct constellation_track_point ref; + //! Last published location remains a depth/position constraint during re-confirmation. + struct constellation_track_point published; + bool have_published; + //! The reference orientation came from optics rather than echoing the IMU prior. + bool ref_orientation_observed; + unsigned consecutive_jumps; + //! Consecutive jumps that end a track. + unsigned max_jumps; + struct constellation_motion_limits limits; +}; + +void +constellation_track_init(struct constellation_track *track); + +//! End the track now; the next verified result must confirm itself again. +void +constellation_track_reset(struct constellation_track *track); + +//! End a track whose reference is too old to vouch for anything. Returns true when it ended. +bool +constellation_track_expire(struct constellation_track *track, int64_t capture_ns); + +/* Decide whether a verified result may be published and advance the state. next->imu_world and + * ref_imu_world (the IMU attitude at track->ref.capture_ns) must be read from the same, current + * attitude history; ref_imu_world may be NULL when unavailable, which makes the result + * discontinuous. A result whose orientation was not observed is compared with the IMU's own. */ +enum constellation_track_verdict +constellation_track_update(struct constellation_track *track, + const struct constellation_track_point *next, + bool orientation_observed, + const struct xrt_quat *ref_imu_world, + double *position_excess_m, + double *rotation_error_rad); + +const char * +constellation_track_state_name(enum constellation_track_state state); + +const char * +constellation_track_verdict_name(enum constellation_track_verdict verdict); + +/* Select jointly verified P3P candidates with the same discrete-assignment ambiguity test as acquisition. */ +bool +constellation_constrained_select_results(const struct constellation_constrained_result *results, + unsigned count, + const struct constellation_constrained_config *config, + struct constellation_constrained_result *out); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/tracking/constellation/internal/correspondence_search.c b/src/xrt/tracking/constellation/internal/correspondence_search.c index b99af8217..da32a98fa 100644 --- a/src/xrt/tracking/constellation/internal/correspondence_search.c +++ b/src/xrt/tracking/constellation/internal/correspondence_search.c @@ -106,12 +106,14 @@ correspondence_search_new(struct camera_model *camera_calib) static int compare_blobs_distance(const void *elem1, const void *elem2, void *arg); -static void +static bool undistort_blob_points(struct blob *blobs, int num_blobs, struct xrt_vec2 *out_points, struct camera_model *calib) { for (int i = 0; i < num_blobs; i++) { - t_camera_models_undistort(&calib->calib, blobs[i].x, blobs[i].y, &out_points[i].x, &out_points[i].y); + if (!camera_model_undistort(calib, blobs[i].x, blobs[i].y, &out_points[i].x, &out_points[i].y)) + return false; } + return true; } #if CHECK_ALL_PROJECTIONS @@ -123,7 +125,7 @@ project_led_point(struct xrt_vec3 *led_pos, { struct xrt_vec3 tmp; math_pose_transform_point(pose, led_pos, &tmp); - t_camera_models_project(&calib->calib, tmp.x, tmp.y, tmp.z, &out_point->x, &out_point->y); + camera_model_project(calib, tmp.x, tmp.y, tmp.z, &out_point->x, &out_point->y); } #endif @@ -144,7 +146,10 @@ correspondence_search_set_blobs(struct correspondence_search *cs, struct blob *b cs->blobs = blobs; /* Undistort points so we can project / match properly */ - undistort_blob_points(blobs, num_blobs, undistorted_points, cs->calib); + if (!undistort_blob_points(blobs, num_blobs, undistorted_points, cs->calib)) { + cs->num_points = 0; + return; + } #if DUMP_BLOBS printf("Building blobs search array\n"); @@ -276,27 +281,16 @@ correspondence_search_project_pose(struct correspondence_search *cs, /* See if we need to make a gravity vector alignment check */ if (mi->search_flags & CS_FLAG_MATCH_GRAVITY) { - struct xrt_quat pose_gravity_swing, pose_gravity_twist; - - math_quat_decompose_swing_twist(&pose->orientation, &mi->gravity_vector, &pose_gravity_swing, - &pose_gravity_twist); - - // Calculate the difference between the amount of gravity swing, ignoring axis - float pose_angle = fabs(acosf(pose_gravity_swing.w)) - fabs(acosf(mi->gravity_swing.w)); - if (pose_angle > mi->gravity_tolerance_rad) { - DEBUG( - "model %d failed pose match - orientation was not within tolerance (error %f deg > %f " - "deg)\n" - "gravity vec %f %f %f pose %f %f %f %f swing %f %f %f %f prior swing %f %f %f %f\n", - mi->id, RAD_TO_DEG(pose_angle), RAD_TO_DEG(mi->gravity_tolerance_rad), mi->gravity_vector.x, - mi->gravity_vector.y, mi->gravity_vector.z, pose->orientation.x, pose->orientation.y, - pose->orientation.z, pose->orientation.w, pose_gravity_swing.x, pose_gravity_swing.y, - pose_gravity_swing.z, pose_gravity_swing.w, mi->gravity_swing.x, mi->gravity_swing.y, - mi->gravity_swing.z, mi->gravity_swing.w); + float pose_angle = pose_metrics_gravity_error(pose, &mi->pose_prior, &mi->gravity_vector); + if (!isfinite(pose_angle) || pose_angle > mi->gravity_tolerance_rad) { + cs->num_gravity_rejects++; + DEBUG("model %d failed gravity match (error %f deg > %f deg)\n", mi->id, RAD_TO_DEG(pose_angle), + RAD_TO_DEG(mi->gravity_tolerance_rad)); return false; } } + cs->num_metric_checks++; struct pose_metrics score; /* Check how many LEDs have matching blobs in this pose, @@ -311,8 +305,24 @@ correspondence_search_project_pose(struct correspondence_search *cs, pose_metrics_evaluate_pose(&score, pose, cs->blobs, cs->num_points, leds, cs->calib, NULL); } + /* A gravity prior constrains tilt even before the first positional lock. Some + * controller rings expose fewer than seven LEDs, so use their requested + * acquisition minimum only with a tight, mostly complete geometric match. + * This remains a weak match: keep searching for a stronger candidate. */ + if ((mi->search_flags & CS_FLAG_MATCH_GRAVITY) && pose_metrics_can_acquire_with_gravity(&score, leds)) { + score.match_flags |= POSE_MATCH_GOOD; + } + if ((mi->search_flags & CS_FLAG_JOINT_P3P) && cs->pose_candidate_cb != NULL) { + // The rig verifier requires a fourth observation and checks ambiguity itself. + // A single view's acquisition count must not hide a valid split-camera root. + cs->pose_candidate_cb(cs->pose_candidate_userdata, pose, &score); + } + /* If this pose is any good, test it further */ if (POSE_HAS_FLAGS(&score, POSE_MATCH_GOOD)) { + if (cs->pose_candidate_cb != NULL && !(mi->search_flags & CS_FLAG_JOINT_P3P)) { + cs->pose_candidate_cb(cs->pose_candidate_userdata, pose, &score); + } if (pose_metrics_score_is_better_pose(&mi->best_score, &score)) { mi->best_score = score; mi->best_pose = *pose; @@ -413,6 +423,15 @@ quat_from_rotation_matrix(struct xrt_quat *me, double R[9]) } } +static bool +search_budget_expired(struct correspondence_search *cs) +{ + if (cs->search_deadline_ns != 0 && (uint64_t)os_monotonic_get_ns() >= cs->search_deadline_ns) { + cs->budget_exhausted = true; + } + return cs->budget_exhausted; +} + static void check_led_against_model_subset(struct correspondence_search *cs, struct cs_model_info *mi, @@ -420,6 +439,10 @@ check_led_against_model_subset(struct correspondence_search *cs, struct t_constellation_led *model_leds[4], int depth) { + if (search_budget_expired(cs)) { + return; + } + struct t_constellation_search_model *model = mi->model; double x[3][3]; struct xrt_vec3 *xcheck; @@ -438,7 +461,6 @@ check_led_against_model_subset(struct correspondence_search *cs, x[i][1] = model_leds[i]->pos.y; x[i][2] = model_leds[i]->pos.z; } - xcheck = &model_leds[3]->pos; /* FIXME: It would be better if this spat out quaternions, * then we wouldn't need to convert below */ @@ -536,8 +558,13 @@ check_led_against_model_subset(struct correspondence_search *cs, if (checks_failed) { continue; } + if (mi->search_flags & CS_FLAG_JOINT_P3P) { + correspondence_search_project_pose(cs, model, &pose, mi, depth); + continue; + } /* check against the 4th point to check the proposed P3P solution */ + xcheck = &model_leds[3]->pos; math_quat_rotate_vec3(&pose.orientation, xcheck, &checkpos); math_vec3_accum(&pose.position, &checkpos); math_vec3_scalar_mul(1.0 / checkpos.z, &checkpos); @@ -600,6 +627,10 @@ select_k_blobs_from_n(struct correspondence_search *cs, int n, int depth) { + if (search_budget_expired(cs)) { + return; + } + if (k == 1) { output_list[0] = candidate_list[0]; check_led_against_model_subset(cs, mi, result_list, model_leds, depth); @@ -645,14 +676,15 @@ check_leds_against_anchor(struct correspondence_search *cs, struct t_constellation_led **model_leds, struct cs_image_point *anchor) { - struct cs_image_point *work_list[MAX_BLOB_SEARCH_DEPTH + 1]; + struct cs_image_point *work_list[MAX_BLOB_SEARCH_DEPTH + 1] = {0}; int max_blob_search_depth = MIN(anchor->num_neighbours, mi->max_blob_depth); + int neighbours = (mi->search_flags & CS_FLAG_JOINT_P3P) ? 2 : 3; - if (max_blob_search_depth < 3) + if (max_blob_search_depth < neighbours) return; // Not enough blobs to compare against work_list[0] = anchor; - select_k_blobs_from_n(cs, mi, model_leds, work_list, work_list + 1, anchor->neighbours, 3, + select_k_blobs_from_n(cs, mi, model_leds, work_list, work_list + 1, anchor->neighbours, neighbours, max_blob_search_depth, 1); } @@ -663,6 +695,10 @@ check_led_match(struct correspondence_search *cs, struct t_constellation_led **model_leds, int depth) { + if (search_budget_expired(cs)) { + return; + } + int b; mi->led_depth = depth; @@ -686,6 +722,10 @@ select_k_leds_from_n(struct correspondence_search *cs, int n, int depth) { + if (search_budget_expired(cs)) { + return; + } + if (k == 1) { struct t_constellation_led *swap_list[4]; @@ -744,14 +784,15 @@ generate_led_match_candidates(struct correspondence_search *cs, struct cs_model_info *mi, struct t_constellation_search_led_candidate *c) { - struct t_constellation_led *work_list[MAX_LED_SEARCH_DEPTH + 1]; + struct t_constellation_led *work_list[MAX_LED_SEARCH_DEPTH + 1] = {0}; int max_search_depth = MIN(mi->max_led_depth, c->num_neighbours) - mi->min_led_depth + 1; - if (max_search_depth < 3) + int neighbours = (mi->search_flags & CS_FLAG_JOINT_P3P) ? 2 : 3; + if (max_search_depth < neighbours) return; // Not enough LEDs to compare against work_list[0] = c->led; - select_k_leds_from_n(cs, mi, work_list, work_list + 1, c->neighbours + mi->min_led_depth - 1, 3, + select_k_leds_from_n(cs, mi, work_list, work_list + 1, c->neighbours + mi->min_led_depth - 1, neighbours, max_search_depth, mi->min_led_depth); } @@ -766,7 +807,9 @@ search_pose_for_model(struct correspondence_search *cs, struct cs_model_info *mi mi->match_flags = 0; /* Clear stats */ - cs->num_trials = cs->num_pose_checks = 0; + cs->budget_exhausted = false; + cs->search_deadline_ns = cs->max_search_ns ? os_monotonic_get_ns() + cs->max_search_ns : 0; + cs->num_trials = cs->num_pose_checks = cs->num_gravity_rejects = cs->num_metric_checks = 0; /* Configure search params from the flags */ if (mi->search_flags & CS_FLAG_SHALLOW_SEARCH) { @@ -796,13 +839,15 @@ search_pose_for_model(struct correspondence_search *cs, struct cs_model_info *mi int out_index = 0, in_index; uint16_t led_id = anchor->blob->led_id; - if ((mi->search_flags & CS_FLAG_MATCH_ALL_BLOBS) || led_id == LED_INVALID_ID || - LED_OBJECT_ID(led_id) == mi->id) { + if ((!cs->excluded_blobs || !cs->excluded_blobs[b]) && + ((mi->search_flags & CS_FLAG_MATCH_ALL_BLOBS) || led_id == LED_INVALID_ID || + LED_OBJECT_ID(led_id) == mi->id)) { for (in_index = 0; in_index < cs->num_points && out_index < MAX_BLOB_SEARCH_DEPTH; in_index++) { struct cs_image_point *p = all_neighbours[in_index]; /* Don't include the blob in its own neighbours */ - if (anchor->blob == p->blob) + if (anchor->blob == p->blob || + (cs->excluded_blobs && cs->excluded_blobs[p->blob - cs->blobs])) continue; led_id = p->blob->led_id; @@ -828,7 +873,7 @@ search_pose_for_model(struct correspondence_search *cs, struct cs_model_info *mi /* Start correspondence search for this model */ /* At this stage, each image point has a list of the nearest neighbours filtered for this model */ - for (l = 0; l < model->num_points; l++) { + for (l = 0; l < model->num_points && !search_budget_expired(cs); l++) { struct t_constellation_search_led_candidate *c = model->points[l]; mi->led_index = l; @@ -862,7 +907,7 @@ correspondence_search_find_one_pose(struct correspondence_search *cs, if ((search_flags & (CS_FLAG_SHALLOW_SEARCH | CS_FLAG_DEEP_SEARCH)) == 0) search_flags |= CS_FLAG_SHALLOW_SEARCH | CS_FLAG_DEEP_SEARCH; - struct cs_model_info mi; + struct cs_model_info mi = {0}; mi.id = model->id; mi.model = model; @@ -879,17 +924,13 @@ correspondence_search_find_one_pose(struct correspondence_search *cs, } if (search_flags & CS_FLAG_MATCH_GRAVITY) { - struct xrt_quat pose_gravity_twist; - - /* We need a pose prior to extract the gravity swing to match */ - assert((search_flags & CS_FLAG_HAVE_POSE_PRIOR) != 0); + /* Cold acquisition knows tilt before position. Keep the supplied orientation + * without enabling the positional/heading scoring of HAVE_POSE_PRIOR. */ assert(gravity_vector != NULL); - + mi.pose_prior.orientation = pose->orientation; mi.gravity_vector = *gravity_vector; mi.gravity_tolerance_rad = gravity_tolerance_rad; - math_quat_decompose_swing_twist(&pose->orientation, gravity_vector, &mi.gravity_swing, - &pose_gravity_twist); } if (search_pose_for_model(cs, &mi) && (mi.match_flags & POSE_MATCH_GOOD)) { @@ -909,7 +950,6 @@ correspondence_search_find_one_pose(struct correspondence_search *cs, return true; } - *pose = mi.best_pose; *score = mi.best_score; return false; } diff --git a/src/xrt/tracking/constellation/internal/correspondence_search.h b/src/xrt/tracking/constellation/internal/correspondence_search.h index b599e5a67..a11003afb 100644 --- a/src/xrt/tracking/constellation/internal/correspondence_search.h +++ b/src/xrt/tracking/constellation/internal/correspondence_search.h @@ -17,6 +17,10 @@ #include "camera_model.h" #include "pose_metrics.h" +#ifdef __cplusplus +extern "C" { +#endif + #define MAX_BLOB_SEARCH_DEPTH 5 enum correspondence_search_flags @@ -29,7 +33,8 @@ enum correspondence_search_flags CS_FLAG_MATCH_ALL_BLOBS = 0x8, /* Allow matching against all blobs, not just unlabelled ones or for the current device */ CS_FLAG_HAVE_POSE_PRIOR = 0x10, /* If the input obj_cam_pose contains a valid prior */ - CS_FLAG_MATCH_GRAVITY = 0x20, /* Use the provided gravity vector to check pose verticality */ + CS_FLAG_MATCH_GRAVITY = 0x20, /* Use pose orientation and gravity even without a position prior. */ + CS_FLAG_JOINT_P3P = 0x40, /* Callback verifies three-point roots across the complete camera rig. */ }; struct cs_image_point @@ -76,7 +81,6 @@ struct cs_model_info /* Used when CS_FLAG_MATCH_GRAVITY is set */ struct xrt_vec3 gravity_vector; - struct xrt_quat gravity_swing; float gravity_tolerance_rad; }; @@ -85,9 +89,22 @@ struct correspondence_search int num_points; struct cs_image_point *points; struct blob *blobs; /* Original blobs structs [num_points] */ + //! Optional same-exposure ownership mask, indexed like blobs. Null searches all observations. + const bool *excluded_blobs; + + //! Optional elapsed-time budget per search, zero preserves the caller default. + uint64_t max_search_ns; + uint64_t search_deadline_ns; + bool budget_exhausted; + + //! Called for each geometry-verified candidate, before selecting a per-view winner. + void (*pose_candidate_cb)(void *userdata, const struct xrt_pose *pose, const struct pose_metrics *score); + void *pose_candidate_userdata; unsigned int num_trials; unsigned int num_pose_checks; + unsigned int num_gravity_rejects; + unsigned int num_metric_checks; struct camera_model *calib; @@ -118,3 +135,7 @@ correspondence_search_have_pose(struct correspondence_search *cs, int model_id, struct xrt_pose *pose, struct pose_metrics *score); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/tracking/constellation/internal/debug_draw.cpp b/src/xrt/tracking/constellation/internal/debug_draw.cpp index 21c530f33..7491de92a 100644 --- a/src/xrt/tracking/constellation/internal/debug_draw.cpp +++ b/src/xrt/tracking/constellation/internal/debug_draw.cpp @@ -427,9 +427,9 @@ debug_draw_blobs_leds(struct xrt_frame *rgb_out, // Draw cross at pose center struct xrt_vec2 pose_center; - t_camera_models_project(&calib->calib, P_cam_obj_prior.position.x, P_cam_obj_prior.position.y, - P_cam_obj_prior.position.z, &pose_center.x, &pose_center.y); - draw_rgb_marker(dest, width, out_stride, height, pose_center.x, pose_center.y, 5, 5, c); + if (camera_model_project(calib, P_cam_obj_prior.position.x, P_cam_obj_prior.position.y, + P_cam_obj_prior.position.z, &pose_center.x, &pose_center.y)) + draw_rgb_marker(dest, width, out_stride, height, pose_center.x, pose_center.y, 5, 5, c); pose_metrics_match_pose_to_blobs(&P_cam_obj_prior, NULL, 0, dev_state->led_model, calib, &blob_match_info); @@ -448,9 +448,9 @@ debug_draw_blobs_leds(struct xrt_frame *rgb_out, // Draw cross at pose center struct xrt_vec2 pose_center; - t_camera_models_project(&calib->calib, P_cam_obj.position.x, P_cam_obj.position.y, - P_cam_obj.position.z, &pose_center.x, &pose_center.y); - draw_rgb_marker(dest, width, out_stride, height, pose_center.x, pose_center.y, 5, 5, c); + if (camera_model_project(calib, P_cam_obj.position.x, P_cam_obj.position.y, P_cam_obj.position.z, + &pose_center.x, &pose_center.y)) + draw_rgb_marker(dest, width, out_stride, height, pose_center.x, pose_center.y, 5, 5, c); struct pose_metrics_blob_match_info blob_match_info; diff --git a/src/xrt/tracking/constellation/internal/pose_metrics.c b/src/xrt/tracking/constellation/internal/pose_metrics.c index 35909effb..5960907f2 100644 --- a/src/xrt/tracking/constellation/internal/pose_metrics.c +++ b/src/xrt/tracking/constellation/internal/pose_metrics.c @@ -144,7 +144,11 @@ project_led_points(struct t_constellation_led_model *led_model, for (int i = 0; i < led_model->num_leds; i++) { struct xrt_vec3 *tmp = out_positions + i; math_pose_transform_point(pose, &led_model->leds[i].pos, tmp); - if (!t_camera_models_project(&calib->calib, tmp->x, tmp->y, tmp->z, &out_points[i].x, &out_points[i].y)) + if (tmp->z <= 0) { + out_points[i] = (struct xrt_vec2){NAN, NAN}; + continue; // Visibility below skips LEDs behind the camera. + } + if (!camera_model_project(calib, tmp->x, tmp->y, tmp->z, &out_points[i].x, &out_points[i].y)) return false; } return true; @@ -165,6 +169,7 @@ get_visible_leds_and_bounds(struct xrt_pose *pose, struct t_constellation_led *leds = led_model->leds; const int num_leds = led_model->num_leds; + *bounds = (struct pose_rect){0, 0, 0, 0}; /* Project LEDs into the distorted image space */ if (!project_led_points(led_model, calib, pose, led_out_positions, led_out_points)) { *num_visible_leds = 0; @@ -268,7 +273,11 @@ project_bounding_points(struct t_constellation_led_model *led_model, for (int i = 0; i < led_model->num_bounding_points; i++) { struct xrt_vec3 *tmp = out_positions + i; math_pose_transform_point(P_cam_obj, &led_model->bounding_points[i].pos, tmp); - if (!t_camera_models_project(&calib->calib, tmp->x, tmp->y, tmp->z, &out_points[i].x, + if (tmp->z <= 0) { + out_points[i] = (struct xrt_vec2){NAN, NAN}; + continue; + } + if (!camera_model_project(calib, tmp->x, tmp->y, tmp->z, &out_points[i].x, &out_points[i].y)) { return false; } @@ -372,10 +381,16 @@ pose_metrics_match_pose_to_blobs(struct xrt_pose *pose, continue; } + struct pose_metrics_visible_led_info *led_info = match_info->visible_leds + match_led_index; + /* A split blob or reflection cannot count as another LED correspondence. */ + if (led_info->matched_blob != NULL) { + match_info->unmatched_blobs++; + continue; + } + match_info->reprojection_error += sqerror; match_info->matched_blobs++; - struct pose_metrics_visible_led_info *led_info = match_info->visible_leds + match_led_index; led_info->matched_blob = b; if (b->led_id != LED_INVALID_ID) { @@ -538,3 +553,31 @@ pose_metrics_score_is_better_pose(struct pose_metrics *old_score, struct pose_me return false; } + +float +pose_metrics_gravity_error(const struct xrt_pose *pose, + const struct xrt_pose *prior, + const struct xrt_vec3 *camera_gravity) +{ + struct xrt_quat inverse, prior_inverse; + struct xrt_vec3 gravity, prior_gravity; + math_quat_invert(&pose->orientation, &inverse); + math_quat_invert(&prior->orientation, &prior_inverse); + math_quat_rotate_vec3(&inverse, camera_gravity, &gravity); + math_quat_rotate_vec3(&prior_inverse, camera_gravity, &prior_gravity); + + // Compare directions in model space, preserving tilt axis and ignoring camera-space yaw. + float dot = m_vec3_dot(gravity, prior_gravity); + return isfinite(dot) ? acosf(fmaxf(-1.0f, fminf(1.0f, dot))) : INFINITY; +} + +/* The caller must independently validate the IMU gravity prior. */ +bool +pose_metrics_can_acquire_with_gravity(const struct pose_metrics *score, + const struct t_constellation_led_model *led_model) +{ + const int min_leds = led_model->min_acquisition_leds; + return min_leds >= 5 && min_leds < 7 && score->matched_blobs >= min_leds && + POSE_HAS_FLAGS(score, POSE_MATCH_LED_IDS) && score->reprojection_error / score->matched_blobs < 1.5 && + score->unmatched_blobs <= 1 && 5 * score->matched_blobs >= 4 * score->visible_leds; +} diff --git a/src/xrt/tracking/constellation/internal/pose_metrics.h b/src/xrt/tracking/constellation/internal/pose_metrics.h index abd80eb6e..6dc5daa88 100644 --- a/src/xrt/tracking/constellation/internal/pose_metrics.h +++ b/src/xrt/tracking/constellation/internal/pose_metrics.h @@ -89,6 +89,12 @@ struct pose_metrics_blob_match_info struct pose_rect bounds; }; +// Angular disagreement of gravity in model space. Camera-space yaw is unconstrained. +float +pose_metrics_gravity_error(const struct xrt_pose *pose, + const struct xrt_pose *prior, + const struct xrt_vec3 *camera_gravity); + void pose_metrics_get_device_bounds(struct xrt_pose *P_cam_obj, struct t_constellation_led_model *led_model, @@ -127,6 +133,11 @@ pose_metrics_evaluate_pose_with_prior(struct pose_metrics *score, struct camera_model *calib, struct pose_rect *out_bounds); +// Additional acquisition path for models with fewer visible LEDs. Caller must validate gravity. +bool +pose_metrics_can_acquire_with_gravity(const struct pose_metrics *score, + const struct t_constellation_led_model *led_model); + bool pose_metrics_score_is_better_pose(struct pose_metrics *old_score, struct pose_metrics *new_score); diff --git a/src/xrt/tracking/constellation/internal/ransac_pnp.cpp b/src/xrt/tracking/constellation/internal/ransac_pnp.cpp index 3377ef2b3..77026e28e 100644 --- a/src/xrt/tracking/constellation/internal/ransac_pnp.cpp +++ b/src/xrt/tracking/constellation/internal/ransac_pnp.cpp @@ -28,6 +28,7 @@ #endif #include +#include using namespace std; @@ -49,15 +50,17 @@ quat_to_3x3(cv::Mat &mat, struct xrt_quat *me) mat.at(2, 2) = 1 - 2 * me->x * me->x - 2 * me->y * me->y; } -static void +static bool undistort_blob_points(std::vector in_points, std::vector &out_points, struct camera_model *calib) { for (size_t i = 0; i < in_points.size(); i++) { - t_camera_models_undistort(&calib->calib, in_points[i].x, in_points[i].y, &out_points[i].x, - &out_points[i].y); + if (!camera_model_undistort(calib, in_points[i].x, in_points[i].y, &out_points[i].x, + &out_points[i].y)) + return false; } + return true; } bool @@ -69,6 +72,9 @@ ransac_pnp_pose(struct xrt_pose *pose, int *num_leds_out, int *num_inliers) { + if (num_inliers) { + *num_inliers = 0; + } int i, j; int num_leds = 0; uint64_t taken = 0; @@ -142,25 +148,47 @@ ransac_pnp_pose(struct xrt_pose *pose, // we undistort the image points manually before passing them to the PnpRansac solver // and we give the solver identity camera + null distortion matrices - undistort_blob_points(list_points2d, list_points2d_undistorted, calib); + if (!undistort_blob_points(list_points2d, list_points2d_undistorted, calib)) + return false; /* 3 pixel reprojection threshold */ float reprojectionError = 3.0 / calib->calib.fx; - cv::solvePnPRansac(list_points3d, list_points2d_undistorted, dummyK, dummyD, rvec, tvec, false, iterationsCount, - reprojectionError, confidence, inliers, flags); + bool solved = false; + try { + solved = cv::solvePnPRansac(list_points3d, list_points2d_undistorted, dummyK, dummyD, rvec, tvec, false, + iterationsCount, reprojectionError, confidence, inliers, flags); + } catch (const cv::Exception &error) { + U_LOG_D("PnP rejected correspondences: %s", error.what()); + return false; + } + if (!solved || inliers.rows < 4 || !cv::checkRange(rvec) || !cv::checkRange(tvec)) { + return false; + } + + cv::Rodrigues(rvec, R); + for (int row = 0; row < inliers.rows; row++) { + const cv::Point3f &point = list_points3d[inliers.at(row)]; + double depth = R.at(2, 0) * point.x + R.at(2, 1) * point.y + + R.at(2, 2) * point.z + tvec.at(2); + if (!(depth > 0.0)) { + return false; + } + } if (num_inliers) *num_inliers = inliers.rows; struct xrt_vec3 v; double angle = sqrt(rvec.dot(rvec)); - double inorm = 1.0f / angle; - - v.x = rvec.at(0) * inorm; - v.y = rvec.at(1) * inorm; - v.z = rvec.at(2) * inorm; - math_quat_from_angle_vector(angle, &v, &pose->orientation); + if (angle < 1e-8) { + pose->orientation = {0, 0, 0, 1}; + } else { + v.x = rvec.at(0) / angle; + v.y = rvec.at(1) / angle; + v.z = rvec.at(2) / angle; + math_quat_from_angle_vector(angle, &v, &pose->orientation); + } pose->position.x = tvec.at(0); pose->position.y = tvec.at(1); pose->position.z = tvec.at(2); diff --git a/src/xrt/tracking/constellation/internal/sample.c b/src/xrt/tracking/constellation/internal/sample.c index 6555b51d3..22d893c27 100644 --- a/src/xrt/tracking/constellation/internal/sample.c +++ b/src/xrt/tracking/constellation/internal/sample.c @@ -9,6 +9,11 @@ */ #include "sample.h" +#include "math/m_api.h" +#include "math/m_vec3.h" + +#include + struct constellation_tracking_sample * constellation_tracking_sample_new(void) { @@ -16,6 +21,28 @@ constellation_tracking_sample_new(void) return sample; } +bool +constellation_tracking_sample_set_hmd_pose(struct constellation_tracking_sample *sample, + xrt_result_t result, + const struct xrt_space_relation *relation) +{ + sample->have_hmd_pose = false; + const enum xrt_space_relation_flags required = + XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT; + if (result != XRT_SUCCESS || (relation->relation_flags & required) != required) + return false; + const struct xrt_pose *p = &relation->pose; + float norm = p->orientation.x * p->orientation.x + p->orientation.y * p->orientation.y + + p->orientation.z * p->orientation.z + p->orientation.w * p->orientation.w; + if (!isfinite(p->position.x) || !isfinite(p->position.y) || !isfinite(p->position.z) || + !isfinite(norm) || norm < 0.9f || norm > 1.1f) + return false; + sample->P_xrworld_hmd = *p; + sample->have_hmd_pose = true; + return true; +} + void constellation_tracking_sample_free(struct constellation_tracking_sample *sample) { @@ -33,3 +60,45 @@ constellation_tracking_sample_free(struct constellation_tracking_sample *sample) free(sample); } + +unsigned +constellation_tracking_sample_exclude_published(const struct constellation_tracking_sample *sample, + unsigned device_index, + struct constellation_constrained_view *views, + unsigned num_views) +{ + unsigned excluded = 0; + for (unsigned d = 0; d < sample->n_devices; d++) { + const struct tracking_sample_device_state *owner = &sample->devices[d]; + if (d == device_index || !owner->found_device_pose) { + continue; + } + for (unsigned a = 0; a < owner->joint_result.num_assignments; a++) { + const struct constellation_constrained_assignment *claim = &owner->joint_result.assignments[a]; + for (unsigned v = 0; v < num_views; v++) { + if (views[v].camera_index != claim->camera_index || !views[v].observation || + claim->blob_index >= (unsigned)views[v].observation->num_blobs || + claim->blob_index >= MAX_BLOBS_PER_FRAME || views[v].excluded_blobs[claim->blob_index]) { + continue; + } + views[v].excluded_blobs[claim->blob_index] = true; + excluded++; + } + } + } + return excluded; +} + +void +constellation_head_relative_position(const struct xrt_pose *P_xrworld_hmd, + const struct xrt_vec3 *position, + struct xrt_vec3 *out) +{ + const struct xrt_vec3 forward = {0, 0, -1}, up = {0, 1, 0}; + struct xrt_vec3 head_forward; + math_quat_rotate_vec3(&P_xrworld_hmd->orientation, &forward, &head_forward); + struct xrt_quat inverse_heading; + math_quat_from_angle_vector(-atan2f(-head_forward.x, -head_forward.z), &up, &inverse_heading); + struct xrt_vec3 offset = m_vec3_sub(*position, P_xrworld_hmd->position); + math_quat_rotate_vec3(&inverse_heading, &offset, out); +} diff --git a/src/xrt/tracking/constellation/internal/sample.h b/src/xrt/tracking/constellation/internal/sample.h index 9e2204e74..19b0154bf 100644 --- a/src/xrt/tracking/constellation/internal/sample.h +++ b/src/xrt/tracking/constellation/internal/sample.h @@ -13,9 +13,11 @@ #include "xrt/xrt_defines.h" #include "xrt/xrt_frame.h" +#include "xrt/xrt_results.h" #include "blobwatch.h" #include "pose_metrics.h" +#include "constrained_pose.h" #ifdef __cplusplus extern "C" { @@ -37,6 +39,8 @@ struct tracking_sample_device_state struct xrt_vec3 prior_pos_error; struct xrt_vec3 prior_rot_error; float gravity_error_rad; /* Gravity vector uncertainty in radians 0..M_PI */ + bool trusted_position; + bool reliable_heading; /* Last observed pose, in world space */ bool have_last_seen_pose; @@ -46,6 +50,7 @@ struct tracking_sample_device_state int found_pose_view_id; /* Set to the camera ID where the device was found */ struct xrt_pose final_pose; /* Global pose that was detected */ + struct constellation_constrained_result joint_result; struct pose_metrics score; struct pose_metrics_blob_match_info blob_match_info; }; @@ -79,6 +84,10 @@ struct constellation_tracking_sample struct tracking_sample_frame views[CONSTELLATION_MAX_CAMERAS]; uint8_t n_views; + /* HMD pose at capture time in the OpenXR world, for logging positions relative to the head */ + struct xrt_pose P_xrworld_hmd; + bool have_hmd_pose; + bool need_long_analysis; bool long_analysis_found_new_blobs; @@ -89,6 +98,28 @@ constellation_tracking_sample_new(void); void constellation_tracking_sample_free(struct constellation_tracking_sample *sample); +// Reject held or failed HMD queries before constructing world-space camera views. +bool +constellation_tracking_sample_set_hmd_pose(struct constellation_tracking_sample *sample, + xrt_result_t result, + const struct xrt_space_relation *relation); + +/* Mark the other devices' published assignments in this exposure. Original blob indices are + * preserved for joint verification and arbitration; inherited temporal labels are ignored. */ +unsigned +constellation_tracking_sample_exclude_published(const struct constellation_tracking_sample *sample, + unsigned device_index, + struct constellation_constrained_view *views, + unsigned num_views); + +/* A world position relative to the head, in the head's heading frame (OpenXR axes): x right, y up, + * -z forward. Head pitch and roll are removed, so "forward" and "below" mean the same thing + * wherever the user looks. */ +void +constellation_head_relative_position(const struct xrt_pose *P_xrworld_hmd, + const struct xrt_vec3 *position, + struct xrt_vec3 *out); + #ifdef __cplusplus } #endif diff --git a/src/xrt/tracking/constellation/t_constellation_tracking.c b/src/xrt/tracking/constellation/t_constellation_tracking.c index 65e88d8c1..af47f97b8 100644 --- a/src/xrt/tracking/constellation/t_constellation_tracking.c +++ b/src/xrt/tracking/constellation/t_constellation_tracking.c @@ -8,6 +8,10 @@ */ #include #include +#include +#ifdef __linux__ +#include +#endif #include "os/os_threading.h" @@ -24,11 +28,20 @@ #include "internal/blobwatch.h" #include "internal/camera_model.h" #include "internal/correspondence_search.h" +#include "internal/constrained_pose.h" #include "internal/debug_draw.h" #include "internal/ransac_pnp.h" #include "internal/sample.h" DEBUG_GET_ONCE_LOG_OPTION(ct_log, "CONSTELLATION_LOG", U_LOGGING_INFO) +DEBUG_GET_ONCE_FLOAT_OPTION(ct_score_gate, "CONSTELLATION_SCORE_GATE_M", 0.004) +DEBUG_GET_ONCE_FLOAT_OPTION(ct_association_gate, "CONSTELLATION_ASSOCIATION_GATE_M", 0.15) +DEBUG_GET_ONCE_FLOAT_OPTION(ct_ray_noise, "CONSTELLATION_RAY_NOISE_M", 0.001) +DEBUG_GET_ONCE_FLOAT_OPTION(ct_max_depth, "CONSTELLATION_MAX_DEPTH_M", 1.75) +DEBUG_GET_ONCE_FLOAT_OPTION(ct_bearing_noise, "CONSTELLATION_BEARING_NOISE_RAD", 0.0026) +DEBUG_GET_ONCE_NUM_OPTION(ct_cold_inliers, "CONSTELLATION_COLD_MIN_INLIERS", 4) +// Standard deviation of the bounded IMU tilt adjustment in degrees; 0 keeps the IMU tilt. +DEBUG_GET_ONCE_FLOAT_OPTION(ct_tilt_prior, "CONSTELLATION_TILT_PRIOR_DEG", 3.0) #define MIN_ROT_ERROR DEG_TO_RAD(30) #define MIN_POS_ERROR 0.10 @@ -78,9 +91,76 @@ struct t_constellation_tracked_device_connection struct t_constellation_tracker *tracker; //! Parent tracker instance }; +enum ct_stage +{ + CT_STAGE_FAST, + CT_STAGE_COLD, + CT_STAGE_P3P, + CT_STAGE_COUNT, +}; + +#define CT_REJECTION_COUNT (CONSTELLATION_CONSTRAINED_INCONSISTENT + 1) +#define CT_ORIGIN_COUNT (CONSTELLATION_CONSTRAINED_P3P + 1) + +//! Per-device acquisition counters, reported periodically at DEBUG. +struct constellation_device_diagnostics +{ + uint64_t exposures; + //! No capture-time attitude from the device: the prior is unavailable. + uint64_t no_attitude; + //! No blobs in any camera at all. + uint64_t no_blobs; + //! Exposures where the position prior was not trusted, so the fast path could not run. + uint64_t untrusted_prior; + uint64_t attempts[CT_STAGE_COUNT]; + uint64_t accepted[CT_STAGE_COUNT]; + uint64_t rejected[CT_STAGE_COUNT][CT_REJECTION_COUNT]; + uint64_t accepted_origin[CT_ORIGIN_COUNT]; + uint64_t published; + uint64_t vision_yaw; + uint64_t shared_blob; + //! shared_blob split by the kind of claim that won the blob. + uint64_t shared_by[CONSTELLATION_CLAIM_REJECTED + 1]; + uint64_t stale; + //! Verified results not published because they did not continue the track, by origin. + uint64_t jumps[CT_ORIGIN_COUNT]; + //! Results held for confirmation, and holds replaced by an inconsistent next result. + uint64_t confirming; + uint64_t unconfirmed; + //! Tracks confirmed, and tracks ended by a timeout or repeated jumps. + uint64_t confirmed; + uint64_t lost; + //! No IMU attitude for the continuity check. + uint64_t track_no_attitude; + //! Tracks ended because another device's clearly stronger candidate claimed their blobs. + uint64_t contested; + //! Best hypothesis inliers per attempt, accepted or not: 0, 1-3, 4-7, 8+. + uint64_t best_inliers[4]; + //! Published poses each camera contributed inliers to. + uint64_t published_by_camera[XRT_TRACKING_MAX_SLAM_CAMS]; + uint64_t last_pose_log_ns; + uint64_t last_shared_log_ns; + uint64_t last_reject_log_ns; + uint64_t last_summary_ns; + const char *last_failure; + uint64_t last_failure_capture_ns; + uint64_t last_failure_processing_ns; + uint64_t published_processing_ns; + uint64_t missing_model; + uint64_t publication_timeouts; + + //! Head-relative position of the last published pose, reported when its track ends. + struct xrt_vec3 last_published_head; + bool have_last_published_head; +}; + struct constellation_tracker_device { struct t_constellation_tracked_device_connection *connection; + //! Stable private alias for logs ("left", "right"); never a serial number. + const char *alias; + char name[64]; + struct constellation_device_diagnostics diagnostics; bool have_led_model; struct t_constellation_led_model led_model; @@ -92,6 +172,11 @@ struct constellation_tracker_device int last_matched_blobs; int last_matched_cam; struct xrt_pose last_matched_cam_pose; // Camera-relative pose + + //! Publication state, world in OpenCV coordinates. Protected by tracked_device_lock. + struct constellation_track track; + uint64_t jumps_since_log; + uint64_t last_jump_log_ns; }; struct constellation_tracker_camera_state @@ -148,6 +233,9 @@ struct t_constellation_tracker /* Debug */ enum u_logging_level log_level; + bool fast_thread_named; + uint64_t long_samples_replaced; + uint64_t last_queue_log_ns; bool debug_draw_normalise; bool debug_draw_blob_tint; bool debug_draw_blob_circles; @@ -165,6 +253,14 @@ struct t_constellation_tracker uint64_t last_blob_analysis_ms; uint64_t last_long_analysis_ms; + //! Per-camera blob statistics for the periodic DEBUG summary, reset after each report. + uint64_t camera_exposures[XRT_TRACKING_MAX_SLAM_CAMS]; + uint64_t camera_blobs[XRT_TRACKING_MAX_SLAM_CAMS]; + uint64_t camera_dark_blobs[XRT_TRACKING_MAX_SLAM_CAMS]; + uint64_t camera_blob_area[XRT_TRACKING_MAX_SLAM_CAMS]; + uint64_t camera_blob_brightness[XRT_TRACKING_MAX_SLAM_CAMS]; + uint64_t last_camera_summary_ns; + struct u_var_button full_search_button; bool do_full_search; @@ -192,18 +288,6 @@ constellation_tracked_device_connection_notify_frame(struct t_constellation_trac os_mutex_unlock(&ctdc->lock); } -static void -constellation_tracked_device_connection_notify_pose(struct t_constellation_tracked_device_connection *ctdc, - timepoint_ns frame_mono_ns, - const struct xrt_pose *pose) -{ - os_mutex_lock(&ctdc->lock); - if (!ctdc->disconnected && ctdc->cb->push_observed_pose) { - ctdc->cb->push_observed_pose(ctdc->xdev, frame_mono_ns, pose); - } - os_mutex_unlock(&ctdc->lock); -} - static void constellation_tracked_device_connection_notify_brightness_update(struct t_constellation_tracked_device_connection *ctdc, uint8_t average_brightness) @@ -240,8 +324,9 @@ constellation_tracked_device_connection_get_tracked_pose(struct t_constellation_ os_mutex_lock(&ctdc->lock); if (!ctdc->disconnected) { struct xrt_device *xdev = ctdc->xdev; - xrt_device_get_tracked_pose(xdev, XRT_INPUT_GENERIC_TRACKER_POSE, timestamp_ns, xsr); - ret = true; + xrt_result_t result = xrt_device_get_tracked_pose(xdev, XRT_INPUT_GENERIC_TRACKER_POSE, timestamp_ns, xsr); + ret = result == XRT_SUCCESS && + (xsr->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); } os_mutex_unlock(&ctdc->lock); @@ -273,353 +358,891 @@ constellation_tracker_node_break_apart(struct xrt_frame_node *node) DRV_TRACE_MARKER(); } -static void -mark_matching_blobs(struct t_constellation_tracker *ct, - struct xrt_pose *pose, - struct blobservation *bwobs, - struct t_constellation_led_model *led_model, - struct pose_metrics_blob_match_info *blob_match_info) +static bool +constellation_tracked_device_connection_has_heading(struct t_constellation_tracked_device_connection *ctdc, + timepoint_ns capture_ns) { - /* First clear existing blob labels for this device */ - int i; - for (i = 0; i < bwobs->num_blobs; i++) { - struct blob *b = bwobs->blobs + i; - uint32_t led_object_id = LED_OBJECT_ID(b->led_id); - - /* Skip blobs which already have an ID not belonging to this device */ - if (led_object_id != led_model->id) { - continue; - } - - if (b->led_id != LED_INVALID_ID) { - b->prev_led_id = b->led_id; - } - b->led_id = LED_INVALID_ID; - } - - - /* Iterate the visible LEDs and mark matching blobs with this device ID and LED ID */ - for (i = 0; i < blob_match_info->num_visible_leds; i++) { - struct pose_metrics_visible_led_info *led_info = blob_match_info->visible_leds + i; - struct t_constellation_led *led = led_info->led; - - if (led_info->matched_blob != NULL) { - struct blob *b = led_info->matched_blob; - - b->led_id = LED_MAKE_ID(led_model->id, led->id); - CT_DEBUG(ct, "Marking LED %d/%d at %f,%f angle %f now %d (was %d)", led_model->id, led->id, - b->x, b->y, RAD_TO_DEG(acosf(led_info->facing_dot)), b->led_id, b->prev_led_id); - } else { - CT_DEBUG(ct, "No blob for device %d LED %d @ %f,%f size %f px angle %f", led_model->id, led->id, - led_info->pos_px.x, led_info->pos_px.y, 2 * led_info->led_radius_px, - RAD_TO_DEG(acosf(led_info->facing_dot))); - } + bool ret = false; + os_mutex_lock(&ctdc->lock); + if (!ctdc->disconnected && ctdc->cb->has_heading_prior) { + ret = ctdc->cb->has_heading_prior(ctdc->xdev, capture_ns); } + os_mutex_unlock(&ctdc->lock); + return ret; } static void -submit_device_pose(struct t_constellation_tracker *ct, - struct tracking_sample_device_state *dev_state, - struct constellation_tracking_sample *sample, - int view_id, - struct xrt_pose *P_cam_obj) +constellation_tracked_device_connection_notify_observation(struct t_constellation_tracked_device_connection *ctdc, + const struct t_constellation_pose_observation *obs) { - struct constellation_tracker_camera_state *cam = ct->cam + view_id; - struct tracking_sample_frame *view = sample->views + view_id; - struct constellation_tracker_device *device = ct->devices + dev_state->dev_index; - - struct pose_metrics *score = &dev_state->score; - - pose_metrics_match_pose_to_blobs(P_cam_obj, view->bwobs->blobs, view->bwobs->num_blobs, &device->led_model, - &cam->camera_model, &dev_state->blob_match_info); - mark_matching_blobs(ct, P_cam_obj, view->bwobs, &device->led_model, &dev_state->blob_match_info); - - struct xrt_pose refine_pose = *P_cam_obj; - - int num_leds_out; - int num_inliers; - - if (!ransac_pnp_pose(&refine_pose, view->bwobs->blobs, view->bwobs->num_blobs, &device->led_model, - &cam->camera_model, &num_leds_out, &num_inliers)) { - CT_DEBUG(ct, "Camera %d RANSAC-PnP refinement for device %d from %u blobs failed", view_id, - device->led_model.id, view->bwobs->num_blobs); - } else { - CT_DEBUG(ct, - "Camera %d RANSAC-PnP refinement for device %d from %u blobs had %d LEDs with %d inliers. " - "Produced pose %f,%f,%f,%f pos %f,%f,%f", - view_id, device->led_model.id, view->bwobs->num_blobs, num_leds_out, num_inliers, - refine_pose.orientation.x, refine_pose.orientation.y, refine_pose.orientation.z, - refine_pose.orientation.w, refine_pose.position.x, refine_pose.position.y, - refine_pose.position.z); - *P_cam_obj = refine_pose; + os_mutex_lock(&ctdc->lock); + if (!ctdc->disconnected) { + if (ctdc->cb->push_pose_observation) { + ctdc->cb->push_pose_observation(ctdc->xdev, obs); + } else if (ctdc->cb->push_observed_pose) { + ctdc->cb->push_observed_pose(ctdc->xdev, obs->capture_ns, &obs->pose); + } } + os_mutex_unlock(&ctdc->lock); +} - os_mutex_lock(&cam->bw_lock); - blobwatch_update_labels(cam->bw, view->bwobs, device->led_model.id); - os_mutex_unlock(&cam->bw_lock); +static const char *ct_stage_names[CT_STAGE_COUNT] = {"fast", "constrained", "p3p"}; - if (!dev_state->found_device_pose) { - math_pose_transform(&view->P_world_cam, P_cam_obj, &dev_state->final_pose); - - dev_state->found_device_pose = true; - dev_state->found_pose_view_id = view_id; - - const struct xrt_vec3 fwd = {0.0, 0.0, -1.0}; - struct xrt_vec3 dev_fwd, dev_prior_fwd, cam_prior_fwd; - - math_quat_rotate_vec3(&dev_state->final_pose.orientation, &fwd, &dev_fwd); - math_quat_rotate_vec3(&dev_state->P_world_obj_prior.orientation, &fwd, &dev_prior_fwd); - - struct xrt_pose P_cam_obj_prior; - math_pose_transform(&view->P_cam_world, &dev_state->P_world_obj_prior, &P_cam_obj_prior); - math_quat_rotate_vec3(&P_cam_obj_prior.orientation, &fwd, &cam_prior_fwd); - - CT_DEBUG(ct, - "Found a pose on cam %u device %d score match_flags 0x%x matched %u " - "blobs of %u visible LEDs. Global pose %f,%f,%f,%f pos %f,%f,%f fwd %f,%f,%f " - "cam-relative pose was %f,%f,%f,%f pos %f,%f,%f " - "Global prior %f,%f,%f,%f pos %f,%f,%f fwd %f,%f,%f " - "cam-relative prior %f,%f,%f,%f pos %f,%f,%f fwd %f,%f,%f", - view_id, device->led_model.id, score->match_flags, score->matched_blobs, score->visible_leds, - dev_state->final_pose.orientation.x, dev_state->final_pose.orientation.y, - dev_state->final_pose.orientation.z, dev_state->final_pose.orientation.w, - dev_state->final_pose.position.x, dev_state->final_pose.position.y, - dev_state->final_pose.position.z, dev_fwd.x, dev_fwd.y, dev_fwd.z, P_cam_obj->orientation.x, - P_cam_obj->orientation.y, P_cam_obj->orientation.z, P_cam_obj->orientation.w, - P_cam_obj->position.x, P_cam_obj->position.y, P_cam_obj->position.z, - dev_state->P_world_obj_prior.orientation.x, dev_state->P_world_obj_prior.orientation.y, - dev_state->P_world_obj_prior.orientation.z, dev_state->P_world_obj_prior.orientation.w, - dev_state->P_world_obj_prior.position.x, dev_state->P_world_obj_prior.position.y, - dev_state->P_world_obj_prior.position.z, dev_prior_fwd.x, dev_prior_fwd.y, dev_prior_fwd.z, - - P_cam_obj_prior.orientation.x, P_cam_obj_prior.orientation.y, P_cam_obj_prior.orientation.z, - P_cam_obj_prior.orientation.w, P_cam_obj_prior.position.x, P_cam_obj_prior.position.y, - P_cam_obj_prior.position.z, cam_prior_fwd.x, cam_prior_fwd.y, cam_prior_fwd.z); - } else if (view_id != dev_state->found_pose_view_id) { - struct xrt_pose extra_final_pose, pose_delta; - - math_pose_transform(&view->P_world_cam, P_cam_obj, &extra_final_pose); - math_quat_unrotate(&dev_state->final_pose.orientation, &extra_final_pose.orientation, - &pose_delta.orientation); - pose_delta.position = extra_final_pose.position; - math_vec3_subtract(&dev_state->final_pose.position, &pose_delta.position); - - CT_DEBUG(ct, - "Found an extra pose on cam %u device %d score match_flags 0x%x matched %u " - "blobs of %u visible LEDs. Global pose %f,%f,%f,%f pos %f,%f,%f " - "cam-relative pose %f,%f,%f,%f pos %f,%f,%f " - "delta from 1st pose %f,%f,%f,%f pos %f,%f,%f", - view_id, device->led_model.id, score->match_flags, score->matched_blobs, score->visible_leds, - extra_final_pose.orientation.x, extra_final_pose.orientation.y, extra_final_pose.orientation.z, - extra_final_pose.orientation.w, extra_final_pose.position.x, extra_final_pose.position.y, - extra_final_pose.position.z, P_cam_obj->orientation.x, P_cam_obj->orientation.y, - P_cam_obj->orientation.z, P_cam_obj->orientation.w, P_cam_obj->position.x, - P_cam_obj->position.y, P_cam_obj->position.z, pose_delta.orientation.x, - pose_delta.orientation.y, pose_delta.orientation.z, pose_delta.orientation.w, - pose_delta.position.x, pose_delta.position.y, pose_delta.position.z); - - // Mix the found position with the prior one - math_vec3_scalar_mul(0.5, &pose_delta.position); - math_vec3_accum(&pose_delta.position, &dev_state->final_pose.position); +static unsigned +joint_views(struct t_constellation_tracker *ct, + struct constellation_tracking_sample *sample, + struct constellation_constrained_view views[CONSTELLATION_MAX_CAMERAS]) +{ + unsigned n = 0; + for (unsigned v = 0; v < sample->n_views; v++) { + struct tracking_sample_frame *view = &sample->views[v]; + if (!view->bwobs) { + continue; + } + views[n++] = (struct constellation_constrained_view){ + .camera_index = v, + .P_world_cam = view->P_world_cam, + .P_cam_world = view->P_cam_world, + .calib = &ct->cam[v].camera_model, + .observation = view->bwobs, + }; } + return n; +} +static unsigned +sample_blob_count(struct constellation_tracking_sample *sample) +{ + unsigned blobs = 0; + for (unsigned v = 0; v < sample->n_views; v++) { + if (sample->views[v].bwobs) { + blobs += sample->views[v].bwobs->num_blobs; + } + } + return blobs; +} + +/* Device position of an LED model pose (OpenCV world) relative to the head at the exposure. */ +static bool +head_relative(const struct constellation_tracking_sample *sample, + const struct constellation_tracker_device *device, + const struct xrt_pose *P_world_model, + struct xrt_vec3 *out) +{ + if (!sample->have_hmd_pose) { + return false; + } + struct xrt_pose P_xrworld_model, P_xrworld_device; + pose_flip_YZ(P_world_model, &P_xrworld_model); + math_pose_transform(&P_xrworld_model, &device->led_model.P_model_device, &P_xrworld_device); + constellation_head_relative_position(&sample->P_xrworld_hmd, &P_xrworld_device.position, out); + return true; +} + +static void +format_camera_inliers(const struct t_constellation_tracker *ct, + const struct constellation_constrained_result *r, + char *buf, + size_t size) +{ + unsigned camera_inliers[XRT_TRACKING_MAX_SLAM_CAMS] = {0}; + for (unsigned i = 0; i < r->num_assignments; i++) { + if (r->assignments[i].camera_index < XRT_TRACKING_MAX_SLAM_CAMS) { + camera_inliers[r->assignments[i].camera_index]++; + } + } + size_t used = 0; + buf[0] = '\0'; + for (int c = 0; c < ct->cam_count && used < size; c++) { + used += snprintf(buf + used, size - used, "%s%u", c ? "," : "", camera_inliers[c]); + } +} + +/* Once a second per device at DEBUG, the best rejected hypothesis and where it was, so a worn log + * shows where the hand was when it could not be published. */ +static void +log_rejected_hypothesis(struct t_constellation_tracker *ct, + struct constellation_tracking_sample *sample, + struct tracking_sample_device_state *state, + const char *stage, + const char *rejection) +{ + struct constellation_tracker_device *device = &ct->devices[state->dev_index]; + struct constellation_constrained_result *r = &state->joint_result; + if (ct->log_level > U_LOGGING_DEBUG || r->num_assignments == 0 || strcmp(rejection, "none") == 0) { + return; + } + uint64_t now = os_monotonic_get_ns(); os_mutex_lock(&ct->tracked_device_lock); - if (device->have_last_seen_pose == false || sample->timestamp > device->last_seen_pose_ts) { - device->have_last_seen_pose = true; - device->last_seen_pose_ts = sample->timestamp; - device->last_seen_pose = dev_state->final_pose; - device->last_matched_blobs = score->matched_blobs; - device->last_matched_cam = view_id; - device->last_matched_cam_pose = *P_cam_obj; + bool due = now - device->diagnostics.last_reject_log_ns >= U_TIME_1S_IN_NS; + if (due) { + device->diagnostics.last_reject_log_ns = now; + } + os_mutex_unlock(&ct->tracked_device_lock); + if (!due) { + return; + } + struct xrt_vec3 head = {NAN, NAN, NAN}; + head_relative(sample, device, &r->P_world_model, &head); + char per_camera[64]; + format_camera_inliers(ct, r, per_camera, sizeof(per_camera)); + CT_DEBUG(ct, + "Tracking reject device=%s capture=%" PRIu64 + " stage=%s reason=%s hypothesis=%s head=%.3f,%.3f,%.3f inliers=%u distinct=%u" + " camera_inliers=%s unexplained=%u fit_chi2=%.1f/%.1f tilt_correction_deg=%.1f", + device->alias, sample->timestamp, stage, rejection, constellation_constrained_origin_name(r->origin), + head.x, head.y, head.z, r->inliers, r->distinct_leds, per_camera, r->unexplained_blobs, + r->fit_chi_square, r->fit_chi_square_limit, RAD_TO_DEG(r->tilt_correction_rad)); +} - /* Submit this pose observation to the fusion / real device. Flip back to OpenXR coords first, then - * apply model pose */ - struct xrt_pose P_xrworld_model; - pose_flip_YZ(&dev_state->final_pose, &P_xrworld_model); - - // Apply device -> LED model pose from xsr = P_world_device + P_device_model = model pose - struct xrt_pose P_xrworld_device; - math_pose_transform(&P_xrworld_model, &device->led_model.P_model_device, &P_xrworld_device); - - // calculate the average brightness of all the matched blobs - uint32_t average_brightness = 0; - int matched_blobs = 0; - for (int i = 0; i < dev_state->blob_match_info.num_visible_leds; i++) { - struct pose_metrics_visible_led_info *visible_led = &dev_state->blob_match_info.visible_leds[i]; - if (visible_led->matched_blob) { - average_brightness += visible_led->matched_blob->brightness; - matched_blobs++; +/* Per-exposure detail at TRACE: one line per attempt and one per camera. */ +static void +log_joint_result(struct t_constellation_tracker *ct, + struct constellation_tracking_sample *sample, + struct tracking_sample_device_state *state, + const char *stage, + const char *rejection) +{ + log_rejected_hypothesis(ct, sample, state, stage, rejection); + if (ct->log_level > U_LOGGING_TRACE) { + return; + } + struct constellation_tracker_device *device = &ct->devices[state->dev_index]; + struct constellation_constrained_result *r = &state->joint_result; + CT_TRACE(ct, + "Tracking device=%s capture=%" PRIu64 " process=%" PRIu64 + " kind=controller stage=%s hypothesis=%s trusted_position=%d heading=%d candidates=%u unique=%u" + " inliers=%u visible=%u unexplained=%u cost_m=%.6f second_m=%.6f ambiguity_chi2=%.3f" + " fit_chi2=%.2f/%.2f rank=%u vision_yaw=%d pos_var_m2=%g,%g,%g yaw_var_rad2=%g pair_trials=%u" + " hypotheses=%u gravity=%u heading=%u reject_degenerate=%u reject_unresolved_yaw=%u" + " reject_no_root=%u reject_depth=%u reject_seed=%u reject_count=%u reject_visibility=%u" + " reject_range=%u reject_prior_distance=%u reject_prior_rotation=%u budget_exhausted=%d reject=%s", + device->alias, sample->timestamp, os_monotonic_get_ns(), stage, + constellation_constrained_origin_name(r->origin), state->trusted_position, state->reliable_heading, + r->candidate_observations, r->distinct_leds, r->inliers, r->projected_visible_leds, + r->unexplained_blobs, r->cost_m, r->second_best_cost_m, r->ambiguity_likelihood_gap, r->fit_chi_square, + r->fit_chi_square_limit, r->information_rank, r->vision_determines_yaw, r->covariance[0], + r->covariance[5], r->covariance[10], r->covariance[15], r->pair_trials, r->hypotheses, + r->gravity_hypotheses, r->heading_hypotheses, r->degenerate_pairs, r->unresolved_yaw_pairs, + r->no_root_pairs, r->depth_pairs, r->seed_rejections, r->insufficient_hypotheses, + r->invisible_rejections, r->depth_rejections, r->prior_distance_rejections, r->prior_rotation_rejections, r->budget_exhausted, + rejection); + CT_TRACE(ct, "Tracking candidate device=%s capture=%" PRIu64 " stage=%s position=%.5f,%.5f,%.5f" + " orientation=%.5f,%.5f,%.5f,%.5f prior_orientation=%.5f,%.5f,%.5f,%.5f", + device->alias, sample->timestamp, stage, r->P_world_model.position.x, r->P_world_model.position.y, + r->P_world_model.position.z, r->P_world_model.orientation.x, r->P_world_model.orientation.y, + r->P_world_model.orientation.z, r->P_world_model.orientation.w, state->P_world_obj_prior.orientation.x, + state->P_world_obj_prior.orientation.y, state->P_world_obj_prior.orientation.z, + state->P_world_obj_prior.orientation.w); + for (unsigned i = 0; i < r->num_assignments; i++) { + const struct constellation_constrained_assignment *a = &r->assignments[i]; + if (a->camera_index >= sample->n_views || !sample->views[a->camera_index].bwobs || + a->blob_index >= (unsigned)sample->views[a->camera_index].bwobs->num_blobs) { + continue; + } + const struct blob *b = &sample->views[a->camera_index].bwobs->blobs[a->blob_index]; + CT_TRACE(ct, "Tracking assignment device=%s capture=%" PRIu64 " stage=%s camera=%u blob=%u" + " led=%u temporal_label=%u pixel=%.3f,%.3f residual_m=%.6f", + device->alias, sample->timestamp, stage, a->camera_index, a->blob_index, a->led_id, b->led_id, + b->x, b->y, a->residual_m); + } + for (unsigned v = 0; v < sample->n_views; v++) { + struct blobservation *obs = sample->views[v].bwobs; + unsigned inliers = 0, distinct = 0, eligible = 0, visible = 0, observations = 0; + for (unsigned k = 0; k < r->num_camera_diagnostics; k++) { + if (r->camera_diagnostics[k].camera_index == v) { + eligible = r->camera_diagnostics[k].eligible_blobs; + visible = r->camera_diagnostics[k].projected_visible_leds; + observations = r->camera_diagnostics[k].candidate_observations; } } - average_brightness /= matched_blobs; - - constellation_tracked_device_connection_notify_brightness_update(device->connection, - average_brightness); - - constellation_tracked_device_connection_notify_pose(device->connection, sample->timestamp, - &P_xrworld_device); - - // update the controller masks for this controller - if (ct->controller_masks_sink) { - struct xrt_pose P_imu_obj; - math_pose_transform(&ct->cam[view_id].P_imu_cam, P_cam_obj, &P_imu_obj); - - for (int i = 0; i < ct->cam_count; i++) { - struct constellation_tracker_camera_state *cam = &ct->cam[i]; - - struct xrt_device_masks_sample_camera *sample_camera = - &ct->controller_masks_sample.views[cam->slam_tracking_index]; - - struct xrt_device_masks_sample_device *device_mask = - &sample_camera->devices[dev_state->dev_index]; - - struct xrt_pose P_tcam_imu; - math_pose_invert(&cam->P_imu_cam, &P_tcam_imu); - - struct xrt_pose P_tcam_obj; - math_pose_transform(&P_tcam_imu, &P_imu_obj, &P_tcam_obj); - - struct pose_rect device_bounds; - pose_metrics_get_device_bounds(&P_tcam_obj, &device->led_model, &cam->camera_model, - &device_bounds, NULL, NULL); - - device_mask->enabled = pose_rect_has_area(&device_bounds); - if (device_mask->enabled) { - device_mask->rect = (struct xrt_rect_f32){ - .x = device_bounds.left, - .y = device_bounds.top, - .w = device_bounds.right - device_bounds.left, - .h = device_bounds.bottom - device_bounds.top, - }; + bool seen[MAX_OBJECT_LEDS] = {0}; + for (unsigned i = 0; i < r->num_assignments; i++) { + struct constellation_constrained_assignment *a = &r->assignments[i]; + if (a->camera_index == v) { + inliers++; + if (a->led_index < MAX_OBJECT_LEDS && !seen[a->led_index]) { + seen[a->led_index] = true; + distinct++; } } - - xrt_sink_push_device_masks(ct->controller_masks_sink, &ct->controller_masks_sample); } + CT_TRACE(ct, + "Tracking device=%s capture=%" PRIu64 " kind=controller stage=%s camera=%u detected=%d" + " eligible=%u dark_rejected=%d visible=%u candidates=%u unique=%u inliers=%u", + device->alias, sample->timestamp, stage, v, obs ? obs->num_blobs : 0, eligible, + obs ? obs->dropped_dark_blobs : 0, visible, observations, distinct, inliers); + } +} + +/* Periodic per-device summary at DEBUG, so a worn session separates missing observations, + * solver rejection, blob arbitration and driver-side rejection for each hand. Caller holds + * tracked_device_lock. */ +static void +log_device_summary_locked(struct t_constellation_tracker *ct, struct constellation_tracker_device *device, bool force) +{ + struct constellation_device_diagnostics *d = &device->diagnostics; + uint64_t now = os_monotonic_get_ns(); + if (!force && (ct->log_level > U_LOGGING_INFO || now - d->last_summary_ns < 5 * (uint64_t)U_TIME_1S_IN_NS)) { + return; + } + d->last_summary_ns = now; + CT_INFO(ct, "Tracking health device=%s model_missing=%" PRIu64 " publication_timeouts=%" PRIu64 + " last_failure=%s capture_ns=%" PRIu64 " processing_ns=%" PRIu64 + " latency_ms=%.1f since_publication_ms=%.1f", + device->alias, d->missing_model, d->publication_timeouts, + d->last_failure ? d->last_failure : "none", d->last_failure_capture_ns, + d->last_failure_processing_ns, + d->last_failure_capture_ns ? (double)(d->last_failure_processing_ns - d->last_failure_capture_ns) / U_TIME_1MS_IN_NS : 0, + d->published_processing_ns ? (double)(now - d->published_processing_ns) / U_TIME_1MS_IN_NS : -1); + char reasons[512] = ""; + size_t used = 0; + for (unsigned stage = 0; stage < CT_STAGE_COUNT; stage++) { + for (unsigned r = 1; r < CT_REJECTION_COUNT; r++) { + if (d->rejected[stage][r] && used < sizeof(reasons)) { + used += snprintf(reasons + used, sizeof(reasons) - used, " %s.%s=%" PRIu64, + ct_stage_names[stage], + constellation_constrained_rejection_name( + (enum constellation_constrained_rejection)r), + d->rejected[stage][r]); + } + } + } + char cameras[128] = ""; + size_t written = 0; + for (int c = 0; c < ct->cam_count && written < sizeof(cameras); c++) { + written += snprintf(cameras + written, sizeof(cameras) - written, "%s%" PRIu64, c ? "," : "", + d->published_by_camera[c]); + } + CT_INFO(ct, + "Tracking summary device=%s exposures=%" PRIu64 " no_attitude=%" PRIu64 " no_blobs=%" PRIu64 + " untrusted_prior=%" PRIu64 " fast=%" PRIu64 "/%" PRIu64 " constrained=%" PRIu64 "/%" PRIu64 + " p3p=%" PRIu64 "/%" PRIu64 " published=%" PRIu64 " vision_yaw=%" PRIu64 " origin_gravity=%" PRIu64 + " origin_heading=%" PRIu64 " origin_prior=%" PRIu64 " origin_p3p=%" PRIu64 " shared_blob=%" PRIu64 + " shared_published=%" PRIu64 " shared_candidate=%" PRIu64 " shared_rejected=%" PRIu64 " stale=%" PRIu64 + " track=%s confirming=%" PRIu64 " unconfirmed=%" PRIu64 " confirmed=%" PRIu64 " lost=%" PRIu64 + " jump_gravity=%" PRIu64 " jump_heading=%" PRIu64 " jump_prior=%" PRIu64 " jump_p3p=%" PRIu64 + " track_no_attitude=%" PRIu64 " contested=%" PRIu64 " best_inliers=%" PRIu64 "/%" PRIu64 "/%" PRIu64 + "/%" PRIu64 " published_by_camera=%s rejects:%s", + device->alias, d->exposures, d->no_attitude, d->no_blobs, d->untrusted_prior, + d->accepted[CT_STAGE_FAST], d->attempts[CT_STAGE_FAST], d->accepted[CT_STAGE_COLD], + d->attempts[CT_STAGE_COLD], d->accepted[CT_STAGE_P3P], d->attempts[CT_STAGE_P3P], d->published, + d->vision_yaw, d->accepted_origin[CONSTELLATION_CONSTRAINED_GRAVITY], + d->accepted_origin[CONSTELLATION_CONSTRAINED_HEADING], + d->accepted_origin[CONSTELLATION_CONSTRAINED_PRIOR], d->accepted_origin[CONSTELLATION_CONSTRAINED_P3P], + d->shared_blob, d->shared_by[CONSTELLATION_CLAIM_PUBLISHED], + d->shared_by[CONSTELLATION_CLAIM_CANDIDATE], d->shared_by[CONSTELLATION_CLAIM_REJECTED], d->stale, + constellation_track_state_name(device->track.state), d->confirming, d->unconfirmed, d->confirmed, + d->lost, d->jumps[CONSTELLATION_CONSTRAINED_GRAVITY], d->jumps[CONSTELLATION_CONSTRAINED_HEADING], + d->jumps[CONSTELLATION_CONSTRAINED_PRIOR], d->jumps[CONSTELLATION_CONSTRAINED_P3P], + d->track_no_attitude, d->contested, d->best_inliers[0], d->best_inliers[1], d->best_inliers[2], + d->best_inliers[3], cameras, used ? reasons : " none"); +} + +static void +count_attempt(struct t_constellation_tracker *ct, + struct tracking_sample_device_state *state, + enum ct_stage stage, + uint64_t capture_ns, + bool accepted, + enum constellation_constrained_rejection rejection) +{ + os_mutex_lock(&ct->tracked_device_lock); + struct constellation_device_diagnostics *d = &ct->devices[state->dev_index].diagnostics; + d->attempts[stage]++; + unsigned inliers = state->joint_result.inliers; + d->best_inliers[inliers == 0 ? 0 : inliers < 4 ? 1 : inliers < 8 ? 2 : 3]++; + if (accepted) { + d->accepted[stage]++; + } else if ((unsigned)rejection < CT_REJECTION_COUNT) { + d->rejected[stage][rejection]++; + if (capture_ns >= d->last_failure_capture_ns) { + d->last_failure = constellation_constrained_rejection_name(rejection); + d->last_failure_capture_ns = capture_ns; + d->last_failure_processing_ns = os_monotonic_get_ns(); + } + log_device_summary_locked(ct, &ct->devices[state->dev_index], false); } os_mutex_unlock(&ct->tracked_device_lock); } -/* Fast matching test based on hypothesised pose */ -static bool -device_try_global_pose(struct t_constellation_tracker *ct, - struct tracking_sample_device_state *dev_state, - struct constellation_tracking_sample *sample, - struct xrt_pose *P_world_obj_candidate) +/* Caller holds tracked_device_lock. */ +static void +log_track_transition_locked(struct t_constellation_tracker *ct, + struct constellation_tracker_device *device, + enum constellation_track_state before, + uint64_t capture_ns, + const char *reason) { - bool ret = false; - for (int view_id = 0; view_id < sample->n_views; view_id++) { - struct tracking_sample_frame *view = sample->views + view_id; - struct constellation_tracker_camera_state *cam = ct->cam + view_id; - - if (view->bwobs == NULL || view->bwobs->num_blobs == 0) { - continue; - } - - struct constellation_tracker_device *device = ct->devices + dev_state->dev_index; - blobservation *bwobs = view->bwobs; - - struct xrt_pose P_cam_obj_candidate; - math_pose_transform(&view->P_cam_world, P_world_obj_candidate, &P_cam_obj_candidate); - - struct xrt_pose P_cam_obj_prior; - math_pose_transform(&view->P_cam_world, &dev_state->P_world_obj_prior, &P_cam_obj_prior); - - pose_metrics_evaluate_pose_with_prior(&dev_state->score, &P_cam_obj_candidate, false, &P_cam_obj_prior, - &dev_state->prior_pos_error, &dev_state->prior_rot_error, - bwobs->blobs, bwobs->num_blobs, &device->led_model, - &cam->camera_model, NULL); - - if (POSE_HAS_FLAGS(&dev_state->score, POSE_MATCH_GOOD | POSE_MATCH_LED_IDS)) { - submit_device_pose(ct, dev_state, sample, view_id, &P_cam_obj_candidate); - ret = true; - } + if (before == device->track.state) { + return; } - - return ret; + const struct constellation_device_diagnostics *d = &device->diagnostics; + if (before == CONSTELLATION_TRACK_TRACKING) { + CT_INFO(ct, "Tracking drop device=%s reason=%s last_failure=%s failure_capture_ns=%" PRIu64 + " failure_processing_ns=%" PRIu64 " publication_age_ms=%.1f no_attitude=%" PRIu64 + " no_blobs=%" PRIu64 " track_no_attitude=%" PRIu64 + " jumps=%" PRIu64 ",%" PRIu64 ",%" PRIu64 ",%" PRIu64 " shared=%" PRIu64, + device->alias, reason, d->last_failure ? d->last_failure : "none", d->last_failure_capture_ns, + d->last_failure_processing_ns, (double)(capture_ns - device->last_seen_pose_ts) / U_TIME_1MS_IN_NS, + d->no_attitude, d->no_blobs, d->track_no_attitude, d->jumps[0], d->jumps[1], + d->jumps[2], d->jumps[3], d->shared_blob); + log_device_summary_locked(ct, device, true); + } + if (before == CONSTELLATION_TRACK_TRACKING && d->have_last_published_head) { + // Where the hand was last published: the edge of the tracked region when it ends here. + CT_DEBUG(ct, + "Tracking device=%s capture=%" PRIu64 + " track_state=%s->%s reason=%s last_published_head=%.3f,%.3f,%.3f", + device->alias, capture_ns, constellation_track_state_name(before), + constellation_track_state_name(device->track.state), reason, d->last_published_head.x, + d->last_published_head.y, d->last_published_head.z); + } else { + CT_DEBUG(ct, "Tracking device=%s capture=%" PRIu64 " track_state=%s->%s reason=%s", device->alias, + capture_ns, constellation_track_state_name(before), + constellation_track_state_name(device->track.state), reason); + } + device->diagnostics.confirmed += device->track.state == CONSTELLATION_TRACK_TRACKING; + device->diagnostics.lost += before == CONSTELLATION_TRACK_TRACKING; } -/* Try and recover the pose from labelled blobs */ -static bool -device_try_recover_pose(struct t_constellation_tracker *ct, - struct tracking_sample_device_state *dev_state, - struct constellation_tracking_sample *sample) +/* Caller holds tracked_device_lock. */ +static void +expire_track_locked(struct t_constellation_tracker *ct, + struct constellation_tracker_device *device, + uint64_t capture_ns) { - struct constellation_tracker_device *device = ct->devices + dev_state->dev_index; - struct t_constellation_led_model *leds_model = &device->led_model; + enum constellation_track_state before = device->track.state; + if (constellation_track_expire(&device->track, (int64_t)capture_ns)) { + device->diagnostics.publication_timeouts++; + log_track_transition_locked(ct, device, before, capture_ns, "timeout"); + } +} - for (int view_id = 0; view_id < sample->n_views; view_id++) { - struct tracking_sample_frame *view = sample->views + view_id; - struct constellation_tracker_camera_state *cam = ct->cam + view_id; +/* IMU attitude of the LED model in the OpenCV world at a capture time, from the device's current + * (yaw-corrected) attitude history. */ +static bool +device_model_attitude(struct constellation_tracker_device *device, uint64_t capture_ns, struct xrt_quat *out) +{ + struct xrt_space_relation xsr; + if (!constellation_tracked_device_connection_get_tracked_pose(device->connection, capture_ns, &xsr)) { + return false; + } + struct xrt_pose P_xrworld_model, P_world_model; + math_pose_transform(&xsr.pose, &device->led_model.P_device_model, &P_xrworld_model); + pose_flip_YZ(&P_xrworld_model, &P_world_model); + *out = P_world_model.orientation; + return true; +} - if (view->bwobs == NULL || view->bwobs->num_blobs == 0) { +/* Decide whether a verified result may be published. Returns NULL to publish, otherwise the + * reason it is held back. Caller holds tracked_device_lock. */ +static const char * +track_gate_locked(struct t_constellation_tracker *ct, + struct constellation_tracker_device *device, + const struct constellation_constrained_result *r, + uint64_t capture_ns) +{ + struct constellation_device_diagnostics *d = &device->diagnostics; + struct constellation_track_point next = {r->P_world_model, {0, 0, 0, 1}, (int64_t)capture_ns}; + if (!device_model_attitude(device, capture_ns, &next.imu_world)) { + d->track_no_attitude++; + return "track_no_attitude"; + } + struct xrt_quat ref_imu; + bool have_ref_imu = device->track.state != CONSTELLATION_TRACK_LOST && + device_model_attitude(device, (uint64_t)device->track.ref.capture_ns, &ref_imu); + enum constellation_track_state before = device->track.state; + double excess, rotation; + int64_t ref_capture_ns = device->track.ref.capture_ns; + bool ref_yaw_observed = device->track.ref_orientation_observed; + struct xrt_vec3 delta = { + next.P_world_model.position.x - device->track.ref.P_world_model.position.x, + next.P_world_model.position.y - device->track.ref.P_world_model.position.y, + next.P_world_model.position.z - device->track.ref.P_world_model.position.z, + }; + double distance = sqrt(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z); + enum constellation_track_verdict verdict = constellation_track_update( + &device->track, &next, r->vision_determines_yaw, have_ref_imu ? &ref_imu : NULL, &excess, &rotation); + switch (verdict) { + case CONSTELLATION_TRACK_PUBLISH: break; + case CONSTELLATION_TRACK_HOLD_STALE: d->stale++; break; + case CONSTELLATION_TRACK_HOLD_UNCONFIRMED: d->unconfirmed++; /* fallthrough */ + case CONSTELLATION_TRACK_HOLD_CONFIRMING: d->confirming++; break; + case CONSTELLATION_TRACK_HOLD_JUMP: + case CONSTELLATION_TRACK_HOLD_RESTART: { + d->jumps[r->origin]++; + device->jumps_since_log++; + uint64_t now = os_monotonic_get_ns(); + if (verdict == CONSTELLATION_TRACK_HOLD_RESTART || now - device->last_jump_log_ns >= U_TIME_1S_IN_NS) { + CT_INFO(ct, + "Tracking device=%s capture=%" PRIu64 + " reject=jump hypothesis=%s inliers=%u" + " position_excess_m=%.3f rotation_error_deg=%.1f have_ref_attitude=%d" + " jumps_since_last_log=%" PRIu64 + " ref_capture_ns=%" PRId64 " dt_ms=%.3f distance_m=%.3f" + " ref_yaw_observed=%d yaw_observed=%d gate=%s processing_ns=%" PRIu64, + device->alias, capture_ns, constellation_constrained_origin_name(r->origin), + r->inliers, excess, rotation * 180.0 / M_PI, have_ref_imu, device->jumps_since_log, + ref_capture_ns, ((int64_t)capture_ns - ref_capture_ns) / 1e6, distance, + ref_yaw_observed, r->vision_determines_yaw, constellation_track_verdict_name(verdict), now); + device->last_jump_log_ns = now; + device->jumps_since_log = 0; + } + d->confirming += verdict == CONSTELLATION_TRACK_HOLD_RESTART; + break; + } + } + if (verdict != CONSTELLATION_TRACK_PUBLISH) { + d->last_failure = constellation_track_verdict_name(verdict); + d->last_failure_capture_ns = capture_ns; + d->last_failure_processing_ns = os_monotonic_get_ns(); + } + log_track_transition_locked(ct, device, before, capture_ns, + verdict == CONSTELLATION_TRACK_HOLD_RESTART ? "jumps" + : verdict == CONSTELLATION_TRACK_PUBLISH ? "confirmed" + : "candidate"); + return verdict == CONSTELLATION_TRACK_PUBLISH ? NULL : constellation_track_verdict_name(verdict); +} + +/* World (OpenCV) covariance into the OpenXR frame: the YZ flip is diag(1,-1,-1). */ +static void +observation_from_result(const struct constellation_constrained_result *r, + timepoint_ns capture_ns, + const struct xrt_pose *P_xrworld_device, + struct t_constellation_pose_observation *obs) +{ + static const float flip[3] = {1, -1, -1}; + *obs = (struct t_constellation_pose_observation){ + .capture_ns = capture_ns, + .pose = *P_xrworld_device, + .orientation_observed = r->vision_determines_yaw, + .inliers = r->inliers, + .distinct_leds = r->distinct_leds, + .information_rank = r->information_rank, + .yaw_variance = r->vision_determines_yaw ? (float)r->covariance[15] : INFINITY, + .cost_m = (float)r->cost_m, + .hypothesis = constellation_constrained_origin_name(r->origin), + }; + for (unsigned i = 0; i < 3; i++) { + for (unsigned j = 0; j < 3; j++) { + obs->position_covariance[3 * i + j] = (float)r->covariance[4 * i + j] * flip[i] * flip[j]; + } + } +} + +/* Publish one rig pose per exposure. Labels are evidence from this accepted hypothesis, + * never independent ownership carried from an earlier frame. */ +static bool +submit_joint_pose(struct t_constellation_tracker *ct, + struct tracking_sample_device_state *state, + struct constellation_tracking_sample *sample, + const char *stage) +{ + struct constellation_tracker_device *device = &ct->devices[state->dev_index]; + struct constellation_constrained_result *r = &state->joint_result; + os_mutex_lock(&ct->tracked_device_lock); + const char *held = NULL; + if (device->have_last_seen_pose && sample->timestamp <= device->last_seen_pose_ts) { + device->diagnostics.stale++; + held = "stale_exposure"; + } else { + held = track_gate_locked(ct, device, r, sample->timestamp); + } + if (held != NULL) { + device->diagnostics.last_failure = held; + device->diagnostics.last_failure_capture_ns = sample->timestamp; + device->diagnostics.last_failure_processing_ns = os_monotonic_get_ns(); + log_device_summary_locked(ct, device, false); + os_mutex_unlock(&ct->tracked_device_lock); + log_joint_result(ct, sample, state, stage, held); + return false; + } + device->have_last_seen_pose = true; + device->last_seen_pose_ts = sample->timestamp; + device->last_seen_pose = r->P_world_model; + device->last_matched_blobs = r->inliers; + device->diagnostics.published++; + device->diagnostics.published_processing_ns = os_monotonic_get_ns(); + device->diagnostics.last_failure = NULL; + device->diagnostics.last_failure_capture_ns = device->diagnostics.last_failure_processing_ns = 0; + device->diagnostics.vision_yaw += r->vision_determines_yaw; + unsigned camera_inliers[XRT_TRACKING_MAX_SLAM_CAMS] = {0}; + for (unsigned i = 0; i < r->num_assignments; i++) { + if (r->assignments[i].camera_index < XRT_TRACKING_MAX_SLAM_CAMS) { + camera_inliers[r->assignments[i].camera_index]++; + } + } + for (int c = 0; c < XRT_TRACKING_MAX_SLAM_CAMS; c++) { + device->diagnostics.published_by_camera[c] += camera_inliers[c] > 0; + } + device->diagnostics.accepted_origin[r->origin]++; + state->final_pose = r->P_world_model; + state->found_device_pose = true; + state->found_pose_view_id = r->assignments[0].camera_index; + device->last_matched_cam = state->found_pose_view_id; + math_pose_transform(&sample->views[state->found_pose_view_id].P_cam_world, &state->final_pose, + &device->last_matched_cam_pose); + state->score.matched_blobs = r->inliers; + state->score.visible_leds = r->projected_visible_leds; + state->score.match_flags = POSE_MATCH_GOOD | POSE_MATCH_LED_IDS; + + uint32_t brightness = 0; + for (unsigned v = 0; v < sample->n_views; v++) { + struct tracking_sample_frame *view = &sample->views[v]; + if (!view->bwobs) { continue; } - - struct constellation_tracker_device *device = ct->devices + dev_state->dev_index; - blobservation *bwobs = view->bwobs; - - /* See if we still have enough labelled blobs to try to re-acquire the pose without a - * full search */ - int num_blobs = 0; - for (int index = 0; index < bwobs->num_blobs; index++) { - struct blob *b = bwobs->blobs + index; - if (LED_OBJECT_ID(b->led_id) == leds_model->id) { - num_blobs++; + for (int b = 0; b < view->bwobs->num_blobs; b++) { + struct blob *blob = &view->bwobs->blobs[b]; + if (LED_OBJECT_ID(blob->led_id) == device->led_model.id) { + blob->led_id = LED_INVALID_ID; } } - if (num_blobs < 4) { - continue; + for (unsigned i = 0; i < r->num_assignments; i++) { + struct constellation_constrained_assignment *a = &r->assignments[i]; + if (a->camera_index != v) { + continue; + } + assert(a->blob_index < (unsigned)view->bwobs->num_blobs); + struct blob *blob = &view->bwobs->blobs[a->blob_index]; + blob->led_id = LED_MAKE_ID(device->led_model.id, a->led_id); + brightness += blob->brightness; } - - struct xrt_pose P_cam_obj_prior; - math_pose_transform(&view->P_cam_world, &dev_state->P_world_obj_prior, &P_cam_obj_prior); - - CT_DEBUG(ct, - "Camera %d trying to reacquire device %d from %u blobs and cam-relative pose %f %f %f %f pos " - "%f %f %f " - "global prior pose %f %f %f %f pos %f %f %f", - view_id, leds_model->id, num_blobs, P_cam_obj_prior.orientation.x, - P_cam_obj_prior.orientation.y, P_cam_obj_prior.orientation.z, P_cam_obj_prior.orientation.w, - P_cam_obj_prior.position.x, P_cam_obj_prior.position.y, P_cam_obj_prior.position.z, - dev_state->P_world_obj_prior.orientation.x, dev_state->P_world_obj_prior.orientation.y, - dev_state->P_world_obj_prior.orientation.z, dev_state->P_world_obj_prior.orientation.w, - dev_state->P_world_obj_prior.position.x, dev_state->P_world_obj_prior.position.y, - dev_state->P_world_obj_prior.position.z); - - struct xrt_pose P_cam_obj = P_cam_obj_prior; - - if (!ransac_pnp_pose(&P_cam_obj, bwobs->blobs, bwobs->num_blobs, leds_model, &cam->camera_model, NULL, - NULL)) { - CT_DEBUG(ct, "Camera %d RANSAC-PnP for device %d from %u blobs failed", view_id, leds_model->id, - num_blobs); - continue; - } - - pose_metrics_evaluate_pose_with_prior(&dev_state->score, &P_cam_obj, true, &P_cam_obj_prior, - &dev_state->prior_pos_error, &dev_state->prior_rot_error, - bwobs->blobs, bwobs->num_blobs, &device->led_model, - &cam->camera_model, NULL); - - if (POSE_HAS_FLAGS(&dev_state->score, POSE_MATCH_GOOD)) { - CT_DEBUG(ct, "Camera %d RANSAC-PnP recovered pose for device %d from %u blobs", view_id, - leds_model->id, num_blobs); - submit_device_pose(ct, dev_state, sample, view_id, &P_cam_obj); - return true; - } - CT_DEBUG(ct, - "Camera %d device %d had %d prior blobs, but failed match with flags 0x%x. " - "Yielded pose %f %f %f %f pos %f %f %f (match %d of %d visible) rot_error %f %f %f pos_error " - "%f %f %f", - view_id, leds_model->id, num_blobs, dev_state->score.match_flags, P_cam_obj.orientation.x, - P_cam_obj.orientation.y, P_cam_obj.orientation.z, P_cam_obj.orientation.w, - P_cam_obj.position.x, P_cam_obj.position.y, P_cam_obj.position.z, - dev_state->score.matched_blobs, dev_state->score.visible_leds, dev_state->score.orient_error.x, - dev_state->score.orient_error.y, dev_state->score.orient_error.z, dev_state->score.pos_error.x, - dev_state->score.pos_error.y, dev_state->score.pos_error.z); + os_mutex_lock(&ct->cam[v].bw_lock); + blobwatch_update_labels(ct->cam[v].bw, view->bwobs, device->led_model.id); + os_mutex_unlock(&ct->cam[v].bw_lock); } - return false; + struct xrt_pose P_xrworld_model, P_xrworld_device; + pose_flip_YZ(&state->final_pose, &P_xrworld_model); + math_pose_transform(&P_xrworld_model, &device->led_model.P_model_device, &P_xrworld_device); + struct t_constellation_pose_observation obs; + observation_from_result(r, sample->timestamp, &P_xrworld_device, &obs); + struct xrt_vec3 head = {NAN, NAN, NAN}; + if (head_relative(sample, device, &r->P_world_model, &head)) { + device->diagnostics.last_published_head = head; + device->diagnostics.have_last_published_head = true; + } + // Once a second per device, where the hand was and which cameras saw it, so a worn log can + // relate dropouts to regions of the play space. + uint64_t now = os_monotonic_get_ns(); + if (ct->log_level <= U_LOGGING_DEBUG && now - device->diagnostics.last_pose_log_ns >= U_TIME_1S_IN_NS) { + device->diagnostics.last_pose_log_ns = now; + char per_camera[64] = ""; + size_t used = 0; + for (int c = 0; c < ct->cam_count && used < sizeof(per_camera); c++) { + used += snprintf(per_camera + used, sizeof(per_camera) - used, "%s%u", c ? "," : "", + camera_inliers[c]); + } + double pixel_error_squared = 0, maximum_pixel_error = 0; + unsigned projected = 0; + for (unsigned i = 0; i < r->num_assignments; i++) { + const struct constellation_constrained_assignment *a = &r->assignments[i]; + const struct tracking_sample_frame *view = &sample->views[a->camera_index]; + struct xrt_vec3 world_led, camera_led; + math_pose_transform_point(&r->P_world_model, &device->led_model.leds[a->led_index].pos, &world_led); + math_pose_transform_point(&view->P_cam_world, &world_led, &camera_led); + struct xrt_vec2 pixel; + if (!camera_model_project(&ct->cam[a->camera_index].camera_model, + camera_led.x, camera_led.y, camera_led.z, &pixel.x, &pixel.y)) continue; + const struct blob *blob = &view->bwobs->blobs[a->blob_index]; + double error = hypot(pixel.x - blob->x, pixel.y - blob->y); + pixel_error_squared += error * error; + maximum_pixel_error = fmax(maximum_pixel_error, error); + projected++; + } + CT_DEBUG(ct, + "Tracking pose device=%s capture=%" PRIu64 + " position=%.3f,%.3f,%.3f head=%.3f,%.3f,%.3f hypothesis=%s inliers=%u" + " distinct=%u camera_inliers=%s tilt_correction_deg=%.1f reprojection_rms_px=%.4f" + " reprojection_max_px=%.4f fit_chi2=%.3f/%.3f cost_m=%.6f", + device->alias, sample->timestamp, P_xrworld_device.position.x, P_xrworld_device.position.y, + P_xrworld_device.position.z, head.x, head.y, head.z, + constellation_constrained_origin_name(r->origin), r->inliers, r->distinct_leds, per_camera, + RAD_TO_DEG(r->tilt_correction_rad), projected ? sqrt(pixel_error_squared / projected) : NAN, + projected ? maximum_pixel_error : NAN, r->fit_chi_square, r->fit_chi_square_limit, r->cost_m); + } + constellation_tracked_device_connection_notify_brightness_update(device->connection, brightness / r->inliers); + constellation_tracked_device_connection_notify_observation(device->connection, &obs); + + if (ct->controller_masks_sink) { + for (unsigned v = 0; v < sample->n_views; v++) { + struct constellation_tracker_camera_state *cam = &ct->cam[v]; + struct xrt_pose P_cam_model; + math_pose_transform(&sample->views[v].P_cam_world, &state->final_pose, &P_cam_model); + struct xrt_device_masks_sample_device *mask = + &ct->controller_masks_sample.views[cam->slam_tracking_index].devices[state->dev_index]; + struct pose_rect bounds; + pose_metrics_get_device_bounds(&P_cam_model, &device->led_model, &cam->camera_model, &bounds, + NULL, NULL); + mask->enabled = pose_rect_has_area(&bounds); + if (mask->enabled) { + mask->rect = (struct xrt_rect_f32){bounds.left, bounds.top, bounds.right - bounds.left, + bounds.bottom - bounds.top}; + } + } + xrt_sink_push_device_masks(ct->controller_masks_sink, &ct->controller_masks_sample); + } + log_device_summary_locked(ct, device, false); + os_mutex_unlock(&ct->tracked_device_lock); + log_joint_result(ct, sample, state, stage, "none"); + return true; +} + +/* A competing candidate keeps a shared blob only with this much more inlier support. */ +#define ARBITRATION_SUPPORT_MARGIN 2 + +/* Resolve all devices before notifying fusion. A blob owned by a published pose, a clearly + * better-supported candidate, or an at-least-as-supported consistent hypothesis cannot verify + * an acquisition. Decisions use the original claims, so device order does not matter. */ +static void +arbitrate_and_submit(struct t_constellation_tracker *ct, + struct constellation_tracking_sample *sample, + bool candidates[CONSTELLATION_MAX_DEVICES], + const char *stage) +{ + const struct constellation_constrained_result *claims[CONSTELLATION_MAX_DEVICES]; + bool published[CONSTELLATION_MAX_DEVICES] = {0}; + enum constellation_constrained_claim blocked[CONSTELLATION_MAX_DEVICES]; + unsigned blockers[CONSTELLATION_MAX_DEVICES]; + for (unsigned d = 0; d < sample->n_devices; d++) { + claims[d] = &sample->devices[d].joint_result; + published[d] = sample->devices[d].found_device_pose; + } + constellation_constrained_arbitrate_with_owners(claims, published, candidates, blocked, blockers, + sample->n_devices, ARBITRATION_SUPPORT_MARGIN); + for (unsigned d = 0; d < sample->n_devices; d++) { + struct tracking_sample_device_state *state = &sample->devices[d]; + if (blocked[d] != CONSTELLATION_CLAIM_NONE) { + os_mutex_lock(&ct->tracked_device_lock); + ct->devices[state->dev_index].diagnostics.shared_blob++; + ct->devices[state->dev_index].diagnostics.shared_by[blocked[d]]++; + struct constellation_device_diagnostics *loser = &ct->devices[state->dev_index].diagnostics; + uint64_t now = os_monotonic_get_ns(); + unsigned w = blockers[d]; + if (w < sample->n_devices && now - loser->last_shared_log_ns >= U_TIME_1S_IN_NS) { + loser->last_shared_log_ns = now; + CT_DEBUG(ct, + "Tracking device=%s capture=%" PRIu64 + " reject=shared_blob claim=%s by=%s inliers=%u/%u" + " distinct=%u/%u hypothesis=%s/%s", + ct->devices[state->dev_index].alias, sample->timestamp, + constellation_constrained_claim_name(blocked[d]), + ct->devices[sample->devices[w].dev_index].alias, claims[d]->inliers, + claims[w]->inliers, claims[d]->distinct_leds, claims[w]->distinct_leds, + constellation_constrained_origin_name(claims[d]->origin), + constellation_constrained_origin_name(claims[w]->origin)); + } + for (unsigned p = 0; blocked[d] == CONSTELLATION_CLAIM_PUBLISHED && p < sample->n_devices; + p++) { + struct constellation_tracker_device *owner = &ct->devices[sample->devices[p].dev_index]; + if (p == d || !published[p] || owner->track.state == CONSTELLATION_TRACK_LOST || + !constellation_constrained_contests_published(claims[d], claims[p], + ARBITRATION_SUPPORT_MARGIN)) { + continue; + } + // The published pose stands for this exposure, but its prior must not keep + // steering association onto blobs another device explains better. + enum constellation_track_state before = owner->track.state; + constellation_track_reset(&owner->track); + owner->diagnostics.contested++; + log_track_transition_locked(ct, owner, before, sample->timestamp, "contested"); + CT_DEBUG(ct, + "Tracking device=%s capture=%" PRIu64 + " contested_by=%s inliers=%u/%u distinct=%u/%u", + owner->alias, sample->timestamp, ct->devices[state->dev_index].alias, + claims[p]->inliers, claims[d]->inliers, claims[p]->distinct_leds, + claims[d]->distinct_leds); + } + os_mutex_unlock(&ct->tracked_device_lock); + log_joint_result(ct, sample, state, stage, + blocked[d] == CONSTELLATION_CLAIM_PUBLISHED ? "shared_blob_published" + : blocked[d] == CONSTELLATION_CLAIM_CANDIDATE ? "shared_blob_candidate" + : "shared_blob_rejected"); + } + if (candidates[d]) { + submit_joint_pose(ct, state, sample, stage); + } + } +} + +static void +joint_config(struct constellation_constrained_config *config, uint64_t capture_ns, bool fast) +{ + constellation_constrained_default_config(config); + config->score_gate_m = debug_get_float_option_ct_score_gate(); + config->association_gate_m = debug_get_float_option_ct_association_gate(); + config->ray_noise_std_m = debug_get_float_option_ct_ray_noise(); + config->maximum_depth_m = debug_get_float_option_ct_max_depth(); + config->bearing_noise_std_rad = debug_get_float_option_ct_bearing_noise(); + config->minimum_cold_inliers = (unsigned)debug_get_num_option_ct_cold_inliers(); + config->tilt_prior_std_rad = DEG_TO_RAD(debug_get_float_option_ct_tilt_prior()); + config->seed = capture_ns; + if (fast) { + // The blob thread runs at the exposure rate; keep its search bounded. + config->time_budget_us = 3000; + } +} + +#define MAX_P3P_JOINT_CANDIDATES 24 +struct joint_p3p_context +{ + struct t_constellation_led_model *model; + struct constellation_constrained_view *views; + unsigned num_views; + struct constellation_constrained_config *config; + struct tracking_sample_frame *generating_view; + struct tracking_sample_device_state *state; + unsigned count; + bool overflow; + struct constellation_constrained_result candidates[MAX_P3P_JOINT_CANDIDATES]; +}; + +static void +joint_p3p_candidate(void *userdata, const struct xrt_pose *camera_pose, const struct pose_metrics *score) +{ + (void)score; + struct joint_p3p_context *ctx = userdata; + struct xrt_pose world_pose; + math_pose_transform(&ctx->generating_view->P_world_cam, camera_pose, &world_pose); + // Filter discontinuous identities before they can outrank the continuous track. + if (ctx->state->trusted_position) { + struct xrt_vec3 delta = m_vec3_sub(world_pose.position, ctx->state->P_world_obj_prior.position); + if (m_vec3_len(delta) > ctx->config->maximum_prior_distance_m) + return; + float dot = fminf(1.f, fabsf(math_quat_dot(&world_pose.orientation, &ctx->state->P_world_obj_prior.orientation))); + if (ctx->state->reliable_heading && 2 * acosf(dot) > ctx->config->maximum_prior_rotation_rad) + return; + } + struct constellation_constrained_result *result = malloc(sizeof(*result)); + if (result == NULL) { + return; + } + if (!constellation_constrained_verify_pose(ctx->model, ctx->views, ctx->num_views, &world_pose, + CONSTELLATION_CONSTRAINED_P3P, ctx->config, result)) { + free(result); + return; + } + /* Do not fill the bounded competitor set with the same P3P root generated + * from many quadruples. Matching the same support/identities and pose is evidence once. */ + for (unsigned c = 0; c < ctx->count; c++) { + struct constellation_constrained_result *old = &ctx->candidates[c]; + if (old->num_assignments != result->num_assignments) { + continue; + } + bool identical = true; + for (unsigned a = 0; a < result->num_assignments && identical; a++) { + struct constellation_constrained_assignment *claim = &result->assignments[a]; + bool found = false; + for (unsigned b = 0; b < old->num_assignments; b++) { + struct constellation_constrained_assignment *other = &old->assignments[b]; + if (claim->camera_index == other->camera_index && claim->blob_index == other->blob_index && + claim->led_id == other->led_id) { + found = true; + break; + } + } + identical = found; + } + struct xrt_vec3 delta = m_vec3_sub(old->P_world_model.position, result->P_world_model.position); + float dot = fabsf(math_quat_dot(&old->P_world_model.orientation, &result->P_world_model.orientation)); + if (identical && m_vec3_len(delta) < 0.0001f && dot > cosf(0.001f / 2)) { + if (result->cost_m < old->cost_m) { + *old = *result; + } + free(result); + return; + } + } + if (ctx->count == MAX_P3P_JOINT_CANDIDATES) { + ctx->overflow = true; + } else { + ctx->candidates[ctx->count++] = *result; + } + free(result); +} + +/* Unconstrained P3P fallback within a bounded budget. Every geometry-verified per-view root is + * jointly verified across cameras; the result keeps the cold claim if P3P produces nothing. */ +static bool +joint_p3p_fallback(struct t_constellation_tracker *ct, + struct constellation_tracking_sample *sample, + struct tracking_sample_device_state *state, + struct constellation_constrained_view *views, + unsigned n_views, + struct constellation_constrained_config *config, + enum constellation_constrained_rejection *rejection) +{ + // Without a verified P3P root, the cold result stays as the claim but is not P3P's verdict. + *rejection = CONSTELLATION_CONSTRAINED_NO_PAIR; + struct joint_p3p_context *ctx = calloc(1, sizeof(*ctx)); + if (!ctx) { + return false; + } + ctx->model = state->led_model; + ctx->state = state; + ctx->views = views; + ctx->num_views = n_views; + ctx->config = config; + uint64_t deadline = os_monotonic_get_ns() + 25 * U_TIME_1MS_IN_NS; + for (unsigned v = 0; v < sample->n_views; v++) { + struct tracking_sample_frame *view = &sample->views[v]; + uint64_t now = os_monotonic_get_ns(); + if (now >= deadline || ctx->overflow) { + break; + } + if (!view->bwobs || view->bwobs->num_blobs < 3) { + continue; + } + struct constellation_tracker_camera_state *cam = &ct->cam[v]; + struct constellation_tracker_device *device = &ct->devices[state->dev_index]; + ctx->generating_view = view; + cam->cs->max_search_ns = deadline - now; + cam->cs->pose_candidate_cb = joint_p3p_candidate; + cam->cs->pose_candidate_userdata = ctx; + for (unsigned k = 0; k < n_views; k++) { + if (views[k].camera_index == v) { + cam->cs->excluded_blobs = views[k].excluded_blobs; + break; + } + } + correspondence_search_set_blobs(cam->cs, view->bwobs->blobs, view->bwobs->num_blobs); + struct xrt_pose pose; + struct pose_metrics score; + math_pose_transform(&view->P_cam_world, &state->P_world_obj_prior, &pose); + /* Every qualifying hypothesis reaches the joint evaluator. Do not stop on + * the first strong single-view result before checking competing roots. */ + correspondence_search_find_one_pose(cam->cs, device->search_led_model, + CS_FLAG_SHALLOW_SEARCH | CS_FLAG_DEEP_SEARCH | + (state->trusted_position ? CS_FLAG_HAVE_POSE_PRIOR : 0) | + CS_FLAG_MATCH_GRAVITY | CS_FLAG_MATCH_ALL_BLOBS | + CS_FLAG_JOINT_P3P, + &pose, &state->prior_pos_error, &state->prior_rot_error, + &view->cam_gravity_vector, state->gravity_error_rad, &score); + cam->cs->pose_candidate_cb = NULL; + cam->cs->pose_candidate_userdata = NULL; + cam->cs->excluded_blobs = NULL; + cam->cs->max_search_ns = 0; + CT_TRACE(ct, + "Tracking device=%s capture=%" PRIu64 " camera=%u hypothesis=p3p trials=%u" + " checks=%u reject_gravity=%u metrics=%u verified=%u budget_exhausted=%d", + device->alias, sample->timestamp, v, cam->cs->num_trials, cam->cs->num_pose_checks, + cam->cs->num_gravity_rejects, cam->cs->num_metric_checks, ctx->count, + cam->cs->budget_exhausted); + } + bool accepted = false; + if (ctx->count) { + accepted = constellation_constrained_select_results(ctx->candidates, ctx->count, config, + &state->joint_result); + *rejection = state->joint_result.rejection; + } + if (ctx->overflow) { + state->joint_result.rejection = *rejection = CONSTELLATION_CONSTRAINED_AMBIGUOUS; + accepted = false; + } + free(ctx); + return accepted; } // Fast frame processing: blob extraction and match to existing predictions @@ -627,7 +1250,13 @@ static void constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt_frame *xf) { struct t_constellation_tracker *ct = container_of(sink, struct t_constellation_tracker, fast_process_sink); - struct xrt_space_relation xsr_base_pose; +#ifdef __linux__ + if (!ct->fast_thread_named) { + prctl(PR_SET_NAME, "ct-fast", 0, 0, 0); + ct->fast_thread_named = true; + } +#endif + struct xrt_space_relation xsr_base_pose = XRT_SPACE_RELATION_ZERO; /* Allocate a tracking sample for everything we're about to process */ struct constellation_tracking_sample *sample = constellation_tracking_sample_new(); @@ -636,7 +1265,14 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt CT_DEBUG(ct, "Starting analysis of frame %" PRIu64 " TS %" PRIu64, xf->source_sequence, xf->timestamp); /* Get the HMD's pose so we can calculate the camera view poses */ - xrt_device_get_tracked_pose(ct->hmd_xdev, XRT_INPUT_GENERIC_TRACKER_POSE, xf->timestamp, &xsr_base_pose); + xrt_result_t pose_result = + xrt_device_get_tracked_pose(ct->hmd_xdev, XRT_INPUT_GENERIC_TRACKER_POSE, xf->timestamp, &xsr_base_pose); + if (!constellation_tracking_sample_set_hmd_pose(sample, pose_result, &xsr_base_pose)) { + CT_DEBUG(ct, "Skipping exposure TS %" PRIu64 " without a tracked HMD camera pose result=%d flags=0x%x", + xf->timestamp, pose_result, xsr_base_pose.relation_flags); + constellation_tracking_sample_free(sample); + return; + } /* Split out camera views and collect blobs across all cameras */ assert(ct->cam_count <= XRT_TRACKING_MAX_SLAM_CAMS); @@ -704,7 +1340,39 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt } #endif } + for (int i = 0; i < ct->cam_count; i++) { + const blobservation *obs = sample->views[i].bwobs; + ct->camera_exposures[i]++; + if (obs == NULL) { + continue; + } + ct->camera_blobs[i] += obs->num_blobs; + ct->camera_dark_blobs[i] += obs->dropped_dark_blobs; + for (int b = 0; b < obs->num_blobs; b++) { + ct->camera_blob_area[i] += obs->blobs[b].area; + ct->camera_blob_brightness[i] += obs->blobs[b].brightness; + } + } uint64_t blob_extract_finish_ts = os_monotonic_get_ns(); + if (ct->log_level <= U_LOGGING_DEBUG && + blob_extract_finish_ts - ct->last_camera_summary_ns >= 5 * (uint64_t)U_TIME_1S_IN_NS) { + ct->last_camera_summary_ns = blob_extract_finish_ts; + /* Per camera over the last window: blobs kept per exposure, blobs dropped as too dark per + * exposure, and the mean area (pixels) and peak brightness of the kept blobs. */ + char per_camera[512] = ""; + size_t used = 0; + for (int i = 0; i < ct->cam_count && used < sizeof(per_camera); i++) { + double exposures = (double)(ct->camera_exposures[i] ? ct->camera_exposures[i] : 1); + double blobs = (double)(ct->camera_blobs[i] ? ct->camera_blobs[i] : 1); + used += snprintf(per_camera + used, sizeof(per_camera) - used, " cam%d=%.2f/%.2f/%.1f/%.0f", i, + ct->camera_blobs[i] / exposures, ct->camera_dark_blobs[i] / exposures, + ct->camera_blob_area[i] / blobs, ct->camera_blob_brightness[i] / blobs); + ct->camera_exposures[i] = ct->camera_blobs[i] = ct->camera_dark_blobs[i] = 0; + ct->camera_blob_area[i] = ct->camera_blob_brightness[i] = 0; + } + CT_DEBUG(ct, "Tracking cameras window_s=5 blobs/dark_dropped/area_px/brightness per exposure:%s", + per_camera); + } ct->last_blob_analysis_ms = (blob_extract_finish_ts - fast_analysis_start_ts) / U_TIME_1MS_IN_NS; // Ready to start processing device poses now. Make sure we have the @@ -719,11 +1387,22 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt if (!device->have_led_model) { if (!constellation_tracked_device_connection_get_led_model(device->connection, &device->led_model)) { + device->diagnostics.missing_model++; + device->diagnostics.last_failure = "model_unavailable"; + device->diagnostics.last_failure_capture_ns = xf->timestamp; + device->diagnostics.last_failure_processing_ns = os_monotonic_get_ns(); + expire_track_locked(ct, device, xf->timestamp); + log_device_summary_locked(ct, device, false); continue; // Can't do anything without the LED info } CT_INFO(ct, "Constellation Tracker: Retrieved controller LED model for device %u", device->led_model.id); + for (int i = 0; i < device->led_model.num_leds; i++) { + struct t_constellation_led *led = &device->led_model.leds[i]; + CT_TRACE(ct, "Model device=%d led=%d pos=%f,%f,%f dir=%f,%f,%f", device->led_model.id, + i, led->pos.x, led->pos.y, led->pos.z, led->dir.x, led->dir.y, led->dir.z); + } device->search_led_model = t_constellation_search_model_new(&device->led_model); device->have_led_model = true; } @@ -732,10 +1411,20 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt // Collect the controller prior pose struct xrt_space_relation xsr; + device->diagnostics.exposures++; if (!constellation_tracked_device_connection_get_tracked_pose(device->connection, xf->timestamp, &xsr)) { - CT_DEBUG(ct, "Failed to retrieve tracked pose for device %u", device->led_model.id); - continue; // Can't retrieve the pose: means the device was disconnected + device->diagnostics.no_attitude++; + device->diagnostics.last_failure = "attitude_history"; + device->diagnostics.last_failure_capture_ns = xf->timestamp; + device->diagnostics.last_failure_processing_ns = os_monotonic_get_ns(); + expire_track_locked(ct, device, xf->timestamp); + CT_TRACE(ct, + "Tracking device=%s capture=%" PRIu64 " process=%" PRIu64 + " kind=controller reject=attitude_history", + device->alias, xf->timestamp, os_monotonic_get_ns()); + log_device_summary_locked(ct, device, false); + continue; // No capture-time attitude: disconnected, not yet calibrated, or history gap } // Apply device -> LED model pose from xsr = P_world_device + P_device_model = P_world_model @@ -751,6 +1440,13 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt dev_state->prior_rot_error.x = dev_state->prior_rot_error.y = dev_state->prior_rot_error.z = MIN_ROT_ERROR; dev_state->gravity_error_rad = MIN_ROT_ERROR; + // Keep a recent optical position bound while re-confirming after a rejected jump. + expire_track_locked(ct, device, xf->timestamp); + dev_state->trusted_position = (xsr.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0 && + (device->track.state == CONSTELLATION_TRACK_TRACKING || + device->track.state == CONSTELLATION_TRACK_CONFIRMING); + dev_state->reliable_heading = + constellation_tracked_device_connection_has_heading(device->connection, xf->timestamp); dev_state->have_last_seen_pose = device->have_last_seen_pose; dev_state->last_seen_pose = device->last_seen_pose; @@ -762,43 +1458,44 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt } os_mutex_unlock(&ct->tracked_device_lock); - /* Try pose recovery strategies: - * Check the predicted pose - * Check the last seen pose - * Try recovery from labelled blobs - * @todo: Check the predicted orientation, but at the last seen position - * @todo: Try for a translational match with prior orientation - */ + /* Fast path: a trusted position prior (recent optical fix) bounds association, so the + * stock low-count gates apply. Everything else goes to the long search. */ + struct constellation_constrained_view views[CONSTELLATION_MAX_CAMERAS]; + unsigned n_views = joint_views(ct, sample, views); + unsigned blob_count = sample_blob_count(sample); + bool candidates[CONSTELLATION_MAX_DEVICES] = {0}; + for (unsigned i = 0; i < sample->n_devices; i++) { + struct tracking_sample_device_state *state = &sample->devices[i]; + if (!state->trusted_position || blob_count == 0) { + os_mutex_lock(&ct->tracked_device_lock); + ct->devices[state->dev_index].diagnostics.untrusted_prior += !state->trusted_position; + os_mutex_unlock(&ct->tracked_device_lock); + continue; + } + struct constellation_constrained_config config; + joint_config(&config, sample->timestamp, true); + candidates[i] = constellation_constrained_search(state->led_model, views, n_views, + &state->P_world_obj_prior, state->reliable_heading, + true, &config, &state->joint_result); + count_attempt(ct, state, CT_STAGE_FAST, sample->timestamp, candidates[i], state->joint_result.rejection); + if (!candidates[i]) { + log_joint_result(ct, sample, state, ct_stage_names[CT_STAGE_FAST], + constellation_constrained_rejection_name(state->joint_result.rejection)); + } + } + arbitrate_and_submit(ct, sample, candidates, ct_stage_names[CT_STAGE_FAST]); bool need_full_search = false; - for (int i = 0; i < sample->n_devices; i++) { - struct tracking_sample_device_state *dev_state = sample->devices + i; - struct constellation_tracker_device *device = ct->devices + dev_state->dev_index; - - CT_DEBUG(ct, "Doing fast match search for device %d", device->led_model.id); - - if (device_try_global_pose(ct, dev_state, sample, &dev_state->P_world_obj_prior)) { - CT_DEBUG(ct, "Found fast match search for device %d in view %d from prior pose", - device->led_model.id, dev_state->found_pose_view_id); - continue; - } - if (dev_state->have_last_seen_pose && - device_try_global_pose(ct, dev_state, sample, &dev_state->last_seen_pose)) { - CT_DEBUG(ct, "Found fast match search for device %d in view %d from last_seen pose", - device->led_model.id, dev_state->found_pose_view_id); - continue; - } - if (device_try_recover_pose(ct, dev_state, sample)) { - CT_DEBUG(ct, "Found fast match search for device %d in view %d from labelled blobs", - device->led_model.id, dev_state->found_pose_view_id); - continue; - } - if (!dev_state->found_device_pose) { - need_full_search = true; - } + for (unsigned i = 0; i < sample->n_devices; i++) { + need_full_search |= !sample->devices[i].found_device_pose; } uint64_t fast_analysis_finish_ts = os_monotonic_get_ns(); ct->last_fast_analysis_ms = (fast_analysis_finish_ts - fast_analysis_start_ts) / U_TIME_1MS_IN_NS; + CT_TRACE(ct, "Fast analysis seq=%" PRIu64 " ts=%" PRIu64 " blobs_ms=%.3f total_ms=%.3f age_ms=%.3f", + xf->source_sequence, xf->timestamp, + (double)(blob_extract_finish_ts - fast_analysis_start_ts) / U_TIME_1MS_IN_NS, + (double)(fast_analysis_finish_ts - fast_analysis_start_ts) / U_TIME_1MS_IN_NS, + (double)((int64_t)fast_analysis_finish_ts - (int64_t)xf->timestamp) / U_TIME_1MS_IN_NS); /* Send analysis results to debug view if needed */ enum debug_draw_flag debug_flags = DEBUG_DRAW_FLAG_NONE; @@ -855,6 +1552,15 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt /* Send the sample for long analysis */ os_thread_helper_lock(&ct->long_analysis_thread); if (ct->long_analysis_pending_sample != NULL) { + ct->long_samples_replaced++; + uint64_t now = os_monotonic_get_ns(); + if (now - ct->last_queue_log_ns >= 5 * (uint64_t)U_TIME_1S_IN_NS) { + ct->last_queue_log_ns = now; + CT_INFO(ct, "Tracking backlog replaced=%" PRIu64 " pending_capture_ns=%" PRIu64 + " incoming_capture_ns=%" PRIu64 " incoming_age_ms=%.1f", + ct->long_samples_replaced, ct->long_analysis_pending_sample->timestamp, + sample->timestamp, (double)(now - sample->timestamp) / U_TIME_1MS_IN_NS); + } constellation_tracking_sample_free(ct->long_analysis_pending_sample); } ct->long_analysis_pending_sample = sample; @@ -872,71 +1578,107 @@ constellation_tracker_process_frame_long(struct t_constellation_tracker *ct, { CT_DEBUG(ct, "Starting long analysis of frame TS %" PRIu64, sample->timestamp); - bool dev_found[CONSTELLATION_MAX_DEVICES] = {0}; - for (int view_id = 0; view_id < sample->n_views; view_id++) { - struct tracking_sample_frame *view = sample->views + view_id; - struct constellation_tracker_camera_state *cam = ct->cam + view_id; - - if (view->bwobs == NULL || view->bwobs->num_blobs == 0) { - continue; // no blobs in this view + /* Long path: cold constrained acquisition over all cameras, then the bounded P3P fallback. + * Gravity hypotheses are always generated; heading adds translation hypotheses. */ + struct constellation_constrained_view views[CONSTELLATION_MAX_CAMERAS]; + unsigned n_views = joint_views(ct, sample, views); + unsigned blob_count = sample_blob_count(sample); + bool candidates[CONSTELLATION_MAX_DEVICES] = {0}; + for (unsigned d = 0; d < sample->n_devices; d++) { + struct tracking_sample_device_state *state = &sample->devices[d]; + if (state->found_device_pose) { + continue; } - - correspondence_search_set_blobs(cam->cs, view->bwobs->blobs, view->bwobs->num_blobs); - - for (int d = 0; d < sample->n_devices; d++) { - struct tracking_sample_device_state *dev_state = sample->devices + d; - struct constellation_tracker_device *device = ct->devices + dev_state->dev_index; - - //! The fast analysis thread must have retrieved the LED model before - // ever sending a device for long analysis - assert(device->have_led_model == true); - - if (dev_state->found_device_pose) - continue; /* This device was already found. No need for a long search */ - - CT_DEBUG(ct, "Doing full search for device %d in view %d", device->led_model.id, view_id); - - for (int pass = 0; pass < 2; pass++) { - - if (dev_state->found_device_pose) - break; /* This device was already found on the previous pass */ - - enum correspondence_search_flags search_flags = - CS_FLAG_STOP_FOR_STRONG_MATCH | CS_FLAG_HAVE_POSE_PRIOR | CS_FLAG_MATCH_GRAVITY; - - if (pass == 0) { - /* 1st pass - quick search only */ - search_flags |= CS_FLAG_SHALLOW_SEARCH; - } else { - /* 2nd pass - do a deep search */ - search_flags |= CS_FLAG_DEEP_SEARCH; - } - - struct xrt_pose P_cam_obj; - math_pose_transform(&view->P_cam_world, &dev_state->P_world_obj_prior, &P_cam_obj); - - if (correspondence_search_find_one_pose( - cam->cs, device->search_led_model, search_flags, &P_cam_obj, - &dev_state->prior_pos_error, &dev_state->prior_rot_error, - &view->cam_gravity_vector, dev_state->gravity_error_rad, &dev_state->score)) { - CT_DEBUG(ct, "Found a pose on cam %u device %d long search pass %d", view_id, - device->led_model.id, pass); - submit_device_pose(ct, dev_state, sample, view_id, &P_cam_obj); - dev_found[d] = true; - break; - } + if (blob_count == 0) { + os_mutex_lock(&ct->tracked_device_lock); + struct constellation_tracker_device *device = &ct->devices[state->dev_index]; + device->diagnostics.no_blobs++; + device->diagnostics.last_failure = "no_blobs"; + device->diagnostics.last_failure_capture_ns = sample->timestamp; + device->diagnostics.last_failure_processing_ns = os_monotonic_get_ns(); + log_device_summary_locked(ct, device, false); + os_mutex_unlock(&ct->tracked_device_lock); + continue; + } + struct constellation_constrained_config config; + joint_config(&config, sample->timestamp, false); + candidates[d] = constellation_constrained_search(state->led_model, views, n_views, + &state->P_world_obj_prior, state->reliable_heading, + state->trusted_position, &config, &state->joint_result); + count_attempt(ct, state, CT_STAGE_COLD, sample->timestamp, candidates[d], state->joint_result.rejection); + if (candidates[d]) { + continue; + } + log_joint_result(ct, sample, state, ct_stage_names[CT_STAGE_COLD], + constellation_constrained_rejection_name(state->joint_result.rejection)); + /* An ambiguous constrained result cannot be made trustworthy by asking + * another solver to select one of the same competing identities. */ + if (state->joint_result.rejection != CONSTELLATION_CONSTRAINED_AMBIGUOUS) { + enum constellation_constrained_rejection p3p_rejection; + candidates[d] = joint_p3p_fallback(ct, sample, state, views, n_views, &config, &p3p_rejection); + count_attempt(ct, state, CT_STAGE_P3P, sample->timestamp, candidates[d], p3p_rejection); + if (!candidates[d]) { + log_joint_result(ct, sample, state, ct_stage_names[CT_STAGE_P3P], + constellation_constrained_rejection_name(p3p_rejection)); } } } + arbitrate_and_submit(ct, sample, candidates, "long"); + + /* A cold winner can keep explaining the already tracked ring, so arbitration rejects it + * forever without ever trying the other ring. After the unrestricted pass (which still + * detects contested owners), retry only the observations not published in this exposure. + * Keep indices and all confirmation/ambiguity gates; temporal labels are not ownership. */ + memset(candidates, 0, sizeof(candidates)); + for (unsigned d = 0; d < sample->n_devices; d++) { + struct tracking_sample_device_state *state = &sample->devices[d]; + if (state->found_device_pose) { + continue; + } + struct constellation_constrained_view remaining[CONSTELLATION_MAX_CAMERAS]; + memcpy(remaining, views, n_views * sizeof(*views)); + unsigned excluded = constellation_tracking_sample_exclude_published(sample, d, remaining, n_views); + if (excluded == 0) { + continue; + } + struct constellation_constrained_config config; + joint_config(&config, sample->timestamp, false); + candidates[d] = constellation_constrained_search(state->led_model, remaining, n_views, + &state->P_world_obj_prior, state->reliable_heading, + state->trusted_position, &config, &state->joint_result); + count_attempt(ct, state, CT_STAGE_COLD, sample->timestamp, candidates[d], state->joint_result.rejection); + if (!candidates[d] && state->joint_result.rejection != CONSTELLATION_CONSTRAINED_AMBIGUOUS) { + enum constellation_constrained_rejection rejection; + candidates[d] = joint_p3p_fallback(ct, sample, state, remaining, n_views, &config, &rejection); + count_attempt(ct, state, CT_STAGE_P3P, sample->timestamp, candidates[d], rejection); + } + CT_TRACE(ct, "Tracking device=%s capture=%" PRIu64 " stage=unclaimed excluded=%u accepted=%d", + ct->devices[state->dev_index].alias, sample->timestamp, excluded, candidates[d]); + } + arbitrate_and_submit(ct, sample, candidates, "unclaimed"); for (int d = 0; d < sample->n_devices; d++) { - if (!dev_found[d]) { + if (!sample->devices[d].found_device_pose) { + struct tracking_sample_device_state *state = &sample->devices[d]; + for (unsigned v = 0; v < sample->n_views; v++) { + struct tracking_sample_frame *view = &sample->views[v]; + if (!view->bwobs) continue; + for (int b = 0; b < view->bwobs->num_blobs; b++) { + struct blob *blob = &view->bwobs->blobs[b]; + if (LED_OBJECT_ID(blob->led_id) == state->led_model->id) blob->led_id = LED_INVALID_ID; + } + os_mutex_lock(&ct->cam[v].bw_lock); + blobwatch_update_labels(ct->cam[v].bw, view->bwobs, state->led_model->id); + os_mutex_unlock(&ct->cam[v].bw_lock); + } + // if a long analysis did not find the device at all, then we push that it has no brightness constellation_tracked_device_connection_notify_brightness_update( ct->devices[sample->devices[d].dev_index].connection, 0); // update the controller masks for this controller to mark it as not active if (ct->controller_masks_sink) { + os_mutex_lock(&ct->tracked_device_lock); for (int i = 0; i < ct->cam_count; i++) { struct constellation_tracker_camera_state *cam = &ct->cam[i]; @@ -951,6 +1693,7 @@ constellation_tracker_process_frame_long(struct t_constellation_tracker *ct, } xrt_sink_push_device_masks(ct->controller_masks_sink, &ct->controller_masks_sample); + os_mutex_unlock(&ct->tracked_device_lock); } } } @@ -959,6 +1702,9 @@ constellation_tracker_process_frame_long(struct t_constellation_tracker *ct, static void * constellation_tracking_long_analysis_thread(void *ptr) { +#ifdef __linux__ + prctl(PR_SET_NAME, "ct-long", 0, 0, 0); +#endif U_TRACE_SET_THREAD_NAME("Constellation tracker: device long recovery thread"); struct t_constellation_tracker *ct = (struct t_constellation_tracker *)(ptr); @@ -982,6 +1728,10 @@ constellation_tracking_long_analysis_thread(void *ptr) constellation_tracker_process_frame_long(ct, sample); uint64_t long_analysis_finish_ts = os_monotonic_get_ns(); + CT_TRACE(ct, "Long analysis ts=%" PRIu64 " total_ms=%.3f age_ms=%.3f", sample->timestamp, + (double)(long_analysis_finish_ts - long_analysis_start_ts) / U_TIME_1MS_IN_NS, + (double)((int64_t)long_analysis_finish_ts - (int64_t)sample->timestamp) / + U_TIME_1MS_IN_NS); constellation_tracking_sample_free(sample); ct->last_long_analysis_ms = @@ -1010,6 +1760,7 @@ constellation_tracker_node_destroy(struct xrt_frame_node *node) os_mutex_lock(&ct->tracked_device_lock); for (int i = 0; i < ct->num_devices; i++) { struct constellation_tracker_device *device = ct->devices + i; + log_device_summary_locked(ct, device, true); // Clean up the LED tracking model t_constellation_led_model_clear(&device->led_model); @@ -1096,8 +1847,17 @@ t_constellation_tracker_create(struct xrt_frame_context *xfctx, cam->camera_model.height = cam_cfg->roi.extent.h; t_camera_model_params_from_t_camera_calibration(&cam_cfg->calibration, &cam->camera_model.calib); + cam->camera_model.fisheye62_valid = cam_cfg->fisheye62_valid; + memcpy(cam->camera_model.fisheye62_radial, cam_cfg->fisheye62_radial, sizeof(cam_cfg->fisheye62_radial)); + cam->camera_model.fisheye62_p1 = cam_cfg->fisheye62_p1; + cam->camera_model.fisheye62_p2 = cam_cfg->fisheye62_p2; + os_mutex_init(&cam->bw_lock); cam->bw = blobwatch_new(cam_cfg->blob_min_threshold, cam_cfg->blob_detect_threshold); + CT_TRACE(ct, "Camera %d fx=%f fy=%f cx=%f cy=%f kb4=%f,%f,%f,%f", i, cam->camera_model.calib.fx, + cam->camera_model.calib.fy, cam->camera_model.calib.cx, cam->camera_model.calib.cy, + cam->camera_model.calib.fisheye.k1, cam->camera_model.calib.fisheye.k2, + cam->camera_model.calib.fisheye.k3, cam->camera_model.calib.fisheye.k4); cam->cs = correspondence_search_new(&cam->camera_model); } @@ -1245,26 +2005,28 @@ t_constellation_tracker_add_device(struct t_constellation_tracker *ct, struct constellation_tracker_device *device = ct->devices + ct->num_devices; device->connection = ctdc; device->last_matched_cam = -1; + constellation_track_init(&device->track); ct->num_devices++; const char *device_type; switch (xdev->device_type) { - case XRT_DEVICE_TYPE_HMD: device_type = "HMD"; break; - case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER: device_type = "Right"; break; - case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER: device_type = "Left"; break; - case XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER: device_type = "Any"; break; - case XRT_DEVICE_TYPE_GENERIC_TRACKER: device_type = "Tracker"; break; - default: device_type = "Unknown"; break; + case XRT_DEVICE_TYPE_HMD: device_type = "HMD"; device->alias = "hmd"; break; + case XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER: device_type = "Right"; device->alias = "right"; break; + case XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER: device_type = "Left"; device->alias = "left"; break; + case XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER: device_type = "Any"; device->alias = "any"; break; + case XRT_DEVICE_TYPE_GENERIC_TRACKER: device_type = "Tracker"; device->alias = "tracker"; break; + default: device_type = "Unknown"; device->alias = "unknown"; break; } - char dev_name[64]; - sprintf(dev_name, "Device %u - %s", ct->num_devices, device_type); - u_var_add_ro_text(ct, "Device", dev_name); + snprintf(device->name, sizeof(device->name), "Device %u - %s", ct->num_devices, device_type); + u_var_add_ro_text(ct, "Device", device->name); u_var_add_pose(ct, &device->last_seen_pose, "Last observed global pose"); u_var_add_u64(ct, &device->last_seen_pose_ts, "Last observed pose"); u_var_add_ro_i32(ct, &device->last_matched_blobs, "Last matched Blobs"); u_var_add_ro_i32(ct, &device->last_matched_cam, "Last observed camera #"); u_var_add_pose(ct, &device->last_matched_cam_pose, "Last observed camera pose"); + u_var_add_ro_u64(ct, &device->diagnostics.published, "Published optical poses"); + u_var_add_ro_u64(ct, &device->diagnostics.shared_blob, "Rejected: shared blobs"); } os_mutex_unlock(&ct->tracked_device_lock); diff --git a/src/xrt/tracking/constellation/t_led_models.c b/src/xrt/tracking/constellation/t_led_models.c index b684a6c58..e05e7aca1 100644 --- a/src/xrt/tracking/constellation/t_led_models.c +++ b/src/xrt/tracking/constellation/t_led_models.c @@ -31,6 +31,7 @@ t_constellation_led_model_init(uint8_t device_id, } led_model->leds = calloc(num_leds, sizeof(struct t_constellation_led)); led_model->num_leds = num_leds; + led_model->min_acquisition_leds = 0; led_model->bounding_points = calloc(num_bounding_points, sizeof(struct t_constellation_bounding_point)); led_model->num_bounding_points = num_bounding_points; } @@ -55,7 +56,11 @@ void t_constellation_led_model_clear(struct t_constellation_led_model *led_model) { free(led_model->leds); + free(led_model->bounding_points); led_model->leds = NULL; + led_model->bounding_points = NULL; + led_model->num_leds = 0; + led_model->num_bounding_points = 0; } struct led_candidate_sort_entry diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 0d0789d5e..e3c42de9f 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -119,3 +119,125 @@ endif() if(XRT_HAVE_VULKAN AND XRT_HAVE_D3D12) target_link_libraries(tests_aux_d3d_d3d12 PRIVATE comp_util aux_vk) endif() + +if(XRT_BUILD_DRIVER_RIFT_S) + add_executable( + tests_rift_s_position_filter tests_rift_s_position_filter.cpp + ../src/xrt/drivers/rift_s/rift_s_position_filter.c + ) + target_include_directories(tests_rift_s_position_filter PRIVATE ../src/xrt/drivers) + target_link_libraries(tests_rift_s_position_filter PRIVATE xrt-external-catch2 aux_util) + add_test(NAME tests_rift_s_position_filter COMMAND tests_rift_s_position_filter) + set_target_properties(tests_rift_s_position_filter PROPERTIES FOLDER monado_tests) + + add_executable(tests_rift_s_optics tests_rift_s_optics.cpp ../src/xrt/drivers/rift_s/rift_s_optics.c) + target_include_directories(tests_rift_s_optics PRIVATE ../src/xrt/drivers) + target_link_libraries(tests_rift_s_optics PRIVATE xrt-external-catch2 aux_util) + add_test(NAME tests_rift_s_optics COMMAND tests_rift_s_optics) + set_target_properties(tests_rift_s_optics PROPERTIES FOLDER monado_tests) +endif() + +if(XRT_MODULE_CONSTELLATION_TRACKING) + add_executable(tests_constellation tests_constellation.cpp) + target_include_directories(tests_constellation PRIVATE ../src/xrt/tracking/constellation) + target_link_libraries(tests_constellation PRIVATE xrt-external-catch2 constellation_tracker aux_math) + add_test(NAME tests_constellation COMMAND tests_constellation) +endif() + +if(XRT_BUILD_DRIVER_RIFT_S) + add_executable(tests_rift_s_tracking tests_rift_s_tracking.cpp) + target_include_directories(tests_rift_s_tracking PRIVATE ../src/xrt/drivers/rift_s) + target_link_libraries(tests_rift_s_tracking PRIVATE xrt-external-catch2 drv_rift_s aux_math aux_tracking) + add_test(NAME tests_rift_s_tracking COMMAND tests_rift_s_tracking) +endif() + +if(XRT_BUILD_DRIVER_RIFT_S AND XRT_FEATURE_STEAMVR_PLUGIN) + target_include_directories(tests_rift_s_tracking PRIVATE ../src/xrt/state_trackers/steamvr_drv) + target_link_libraries(tests_rift_s_tracking PRIVATE xrt-external-openvr) + target_compile_definitions(tests_rift_s_tracking PRIVATE TEST_RIFT_S_STEAMVR_POSE) +endif() + +if(XRT_BUILD_DRIVER_RIFT_S) + add_executable(tests_rift_s_metadata tests_rift_s_metadata.cpp) + target_include_directories(tests_rift_s_metadata PRIVATE ../src/xrt/drivers/rift_s) + target_link_libraries(tests_rift_s_metadata PRIVATE xrt-external-catch2 drv_rift_s aux_math) + add_test(NAME tests_rift_s_metadata COMMAND tests_rift_s_metadata) +endif() + +if(XRT_MODULE_CONSTELLATION_TRACKING) + add_executable(tests_constellation_constrained tests_constellation_constrained.cpp) + target_include_directories(tests_constellation_constrained PRIVATE ../src/xrt/tracking/constellation) + target_link_libraries(tests_constellation_constrained PRIVATE xrt-external-catch2 constellation_tracker aux_math) + add_test(NAME tests_constellation_constrained COMMAND tests_constellation_constrained) +endif() + +if(XRT_BUILD_DRIVER_RIFT_S) + add_executable(tests_rift_s_passthrough tests_rift_s_passthrough.cpp) + target_include_directories(tests_rift_s_passthrough PRIVATE ../src/xrt/drivers/rift_s) + target_link_libraries(tests_rift_s_passthrough PRIVATE xrt-external-catch2 drv_rift_s aux_math) + add_test(NAME tests_rift_s_passthrough COMMAND tests_rift_s_passthrough) +endif() + +if(XRT_FEATURE_STEAMVR_PLUGIN) + add_executable(tests_steamvr_camera tests_steamvr_camera.cpp ../src/xrt/state_trackers/steamvr_drv/ovrd_camera.cpp) + target_include_directories(tests_steamvr_camera PRIVATE ../src/xrt/state_trackers/steamvr_drv) + target_link_libraries(tests_steamvr_camera PRIVATE xrt-external-catch2 xrt-external-openvr aux_math aux_util) + target_compile_definitions(tests_steamvr_camera PRIVATE RIFTS_PASSTHROUGH_SHM_PATH="${CMAKE_CURRENT_BINARY_DIR}/rifts-passthrough-test.shm") + add_test(NAME tests_steamvr_camera COMMAND tests_steamvr_camera) +endif() + +if(XRT_BUILD_DRIVER_RIFT_S) + add_executable(tests_rift_s_slam_guard tests_rift_s_slam_guard.cpp ../src/xrt/drivers/rift_s/rift_s_slam_guard.c) + target_include_directories(tests_rift_s_slam_guard PRIVATE ../src/xrt/drivers) + target_link_libraries(tests_rift_s_slam_guard PRIVATE xrt-external-catch2 aux_math) + add_test(NAME tests_rift_s_slam_guard COMMAND tests_rift_s_slam_guard) + set_target_properties(tests_rift_s_slam_guard PROPERTIES FOLDER monado_tests) +endif() + +if(XRT_FEATURE_SLAM) + add_executable(tests_slam_recovery tests_slam_recovery.cpp) + target_link_libraries(tests_slam_recovery PRIVATE aux_tracking aux_util xrt-interfaces) + add_test(NAME tests_slam_recovery COMMAND tests_slam_recovery) + set_tests_properties(tests_slam_recovery PROPERTIES SKIP_RETURN_CODE 77 TIMEOUT 45) + add_executable(tests_slam_restart tests_slam_restart.cpp) + target_link_libraries(tests_slam_restart PRIVATE aux_tracking aux_util xrt-interfaces) + find_package(TBB QUIET) + if(TARGET TBB::tbb) + target_link_libraries(tests_slam_restart PRIVATE TBB::tbb) + target_compile_definitions(tests_slam_restart PRIVATE SLAM_TEST_HAVE_TBB) + endif() + add_test(NAME tests_slam_restart COMMAND tests_slam_restart) + set_tests_properties(tests_slam_restart PROPERTIES SKIP_RETURN_CODE 77 TIMEOUT 45) +endif() + +if(TARGET monado_roomsetup_vulkan) + add_executable(tests_steamvr_roomsetup_vulkan tests_steamvr_roomsetup_vulkan.cpp) + target_include_directories(tests_steamvr_roomsetup_vulkan PRIVATE ${Vulkan_INCLUDE_DIR}) + target_link_libraries(tests_steamvr_roomsetup_vulkan PRIVATE xrt-external-catch2) + add_test(NAME tests_steamvr_roomsetup_vulkan COMMAND tests_steamvr_roomsetup_vulkan) +endif() + +if(TARGET monado-roomsetup-guard) + add_executable(tests_steamvr_roomsetup_guard tests_steamvr_roomsetup_guard.cpp) + target_link_libraries(tests_steamvr_roomsetup_guard PRIVATE xrt-external-catch2 xrt-external-openvr aux_util ${CMAKE_DL_LIBS}) + add_test(NAME tests_steamvr_roomsetup_guard COMMAND tests_steamvr_roomsetup_guard) +endif() + +if(XRT_BUILD_DRIVER_RIFT_S) + add_executable(tests_rift_s_world_anchor tests_rift_s_world_anchor.cpp) + target_include_directories(tests_rift_s_world_anchor PRIVATE ../src/xrt/drivers/rift_s ${OpenCV_INCLUDE_DIRS}) + target_link_libraries(tests_rift_s_world_anchor PRIVATE xrt-external-catch2 drv_rift_s aux_math aux_util ${OpenCV_LIBRARIES}) + add_test(NAME tests_rift_s_world_anchor COMMAND tests_rift_s_world_anchor) +endif() + +if(XRT_BUILD_DRIVER_RIFT_S AND XRT_MODULE_CONSTELLATION_TRACKING) + add_executable(tests_rift_s_controller_geometry tests_rift_s_controller_geometry.cpp) + target_include_directories(tests_rift_s_controller_geometry PRIVATE ../src/xrt/tracking/constellation) + target_link_libraries(tests_rift_s_controller_geometry PRIVATE xrt-external-catch2 constellation_tracker aux_math) + add_test(NAME tests_rift_s_controller_geometry COMMAND tests_rift_s_controller_geometry) + + add_executable(tests_rift_s_controller_capture tests_rift_s_controller_capture.cpp) + target_include_directories(tests_rift_s_controller_capture PRIVATE ../src/xrt/drivers/rift_s) + target_link_libraries(tests_rift_s_controller_capture PRIVATE xrt-external-catch2 drv_rift_s aux_math aux_tracking aux_util_sink) + add_test(NAME tests_rift_s_controller_capture COMMAND tests_rift_s_controller_capture) +endif() diff --git a/tests/constellation_rift_s_data.hpp b/tests/constellation_rift_s_data.hpp new file mode 100644 index 000000000..3935fda25 --- /dev/null +++ b/tests/constellation_rift_s_data.hpp @@ -0,0 +1,36 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +// Left controller (device type 3), firmware ImuPosition.x = -0.005584116 m. +// Camera 1, raw LED frame 42358443082000 from the recorded Rift S table run. +// The down vector is the controller accelerometer direction with X negated: +// model axes are flipped Y/Z, and specific force is opposite gravity. +static const std::array rift_s_ring_leds = { + {{0, {-0.016087244f, -0.003874289f, 0.03376532f}, {-0.52706841f, 0.71386452f, 0.46108171f}, 3.5f}, + {1, {-0.026308544f, -0.012725549f, 0.03197808f}, {-0.71941994f, 0.53832866f, 0.43890456f}, 3.5f}, + {2, {-0.031247054f, -0.016434289f, 0.01680927f}, {-0.75763735f, 0.6234486f, -0.19312559f}, 3.5f}, + {3, {-0.037196064f, -0.033725319f, 0.02171323f}, {-0.95565641f, -0.00827838f, 0.29436762f}, 3.5f}, + {4, {-0.036077004f, -0.043122619f, -5.757e-05f}, {-0.89943476f, 0.04857372f, -0.43434745f}, 3.5f}, + {5, {-0.023619914f, -0.068960599f, 0.0035286f}, {-0.57938915f, -0.80424722f, 0.13226727f}, 3.5f}, + {6, {-0.008917564f, -0.070212489f, -0.01661147f}, {-0.32401356f, -0.5869508f, -0.74195955f}, 3.5f}, + {7, {0.013352056f, -0.078358879f, -0.00087879f}, {0.14082806f, -0.97575588f, 0.16753482f}, 3.5f}, + {8, {0.035553556f, -0.062195189f, -0.01110704f}, {0.66436362f, -0.41503629f, -0.62158335f}, 3.5f}, + {9, {0.042824396f, -0.059756309f, 0.00886611f}, {0.77126662f, -0.61174447f, 0.17583089f}, 3.5f}, + {10, {0.048170526f, -0.037286439f, 0.00373308f}, {0.90904575f, 0.17393345f, -0.37865945f}, 3.5f}, + {11, {0.047769726f, -0.030031509f, 0.02358948f}, {0.9435189f, 0.10477919f, 0.31431419f}, 3.5f}, + {12, {0.038411006f, -0.010875009f, 0.01950788f}, {0.7051038f, 0.6950803f, -0.14032818f}, 3.5f}, + {13, {0.035797386f, -0.010987899f, 0.03273204f}, {0.67315478f, 0.5810967f, 0.45737213f}, 3.5f}, + {14, {0.025487206f, -0.003224659f, 0.0348245f}, {0.49720891f, 0.70839529f, 0.5009585f}, 3.5f}}}; +static const std::array, 8> rift_s_ring_blobs = {{{433.487f, 83, 2, 1}, + {438.231f, 98.808f, 6, 7}, + {372.969f, 107.138f, 3, 5}, + {380.289f, 115.5f, 8, 8}, + {402.197f, 115.247f, 9, 9}, + {423.824f, 121.102f, 7, 5}, + {375.462f, 121.945f, 4, 5}, + {393.509f, 127.601f, 8, 6}}}; +static const std::array rift_s_ring_intrinsics = {190.178f, 190.178f, 315.85f, 237.907f}; +static const std::array rift_s_ring_kb4 = {0.288808f, -0.026387f, -0.0690462f, 0.0238345f}; +static const xrt_vec3 rift_s_ring_gravity = {-0.944092375f, -0.0402524732f, 0.327214497f}; +static const xrt_vec3 rift_s_ring_down = {0.792603882f, 0.601792372f, 0.0981072222f}; diff --git a/tests/tests_constellation.cpp b/tests/tests_constellation.cpp new file mode 100644 index 000000000..4517daaa6 --- /dev/null +++ b/tests/tests_constellation.cpp @@ -0,0 +1,437 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "internal/pose_metrics.h" +#include "internal/ransac_pnp.h" +#include +#include +#include +#include "math/m_vec3.h" +#include "internal/correspondence_search.h" +#include "constellation_rift_s_data.hpp" +#include "tracking/t_led_models.h" + +TEST_CASE("Constellation gravity rejects opposite tilt and is symmetric") +{ + const xrt_vec3 gravity = {0, 1, 0}; + xrt_pose prior = XRT_POSE_IDENTITY; + xrt_pose pose = XRT_POSE_IDENTITY; + const xrt_vec3 axis = {1, 0, 0}; + math_quat_from_angle_vector(M_PI / 3, &axis, &prior.orientation); + math_quat_from_angle_vector(-M_PI / 3, &axis, &pose.orientation); + CHECK(pose_metrics_gravity_error(&pose, &prior, &gravity) == Catch::Approx(2 * M_PI / 3)); + pose.orientation = {0, 0, 0, 1}; + CHECK(pose_metrics_gravity_error(&pose, &prior, &gravity) == Catch::Approx(M_PI / 3)); + CHECK(pose_metrics_gravity_error(&prior, &pose, &gravity) == Catch::Approx(M_PI / 3)); + pose.orientation = {-prior.orientation.x, -prior.orientation.y, -prior.orientation.z, -prior.orientation.w}; + CHECK(pose_metrics_gravity_error(&pose, &prior, &gravity) == Catch::Approx(0).margin(1e-5)); +} + +TEST_CASE("Constellation gravity permits yaw about camera gravity") +{ + const xrt_vec3 gravity = {0.6f, 0.8f, 0}; + xrt_pose prior = XRT_POSE_IDENTITY; + xrt_pose pose = XRT_POSE_IDENTITY; + const xrt_vec3 axis = {1, 0, 0}; + math_quat_from_angle_vector(0.7, &axis, &prior.orientation); + xrt_quat yaw; + math_quat_from_angle_vector(1.2, &gravity, &yaw); + math_quat_rotate(&yaw, &prior.orientation, &pose.orientation); + CHECK(pose_metrics_gravity_error(&pose, &prior, &gravity) == Catch::Approx(0).margin(1e-5)); +} + +TEST_CASE("Constellation PnP preserves seed on degenerate correspondences") +{ + std::array leds = {}; + std::array blobs = {}; + for (size_t i = 0; i < leds.size(); i++) { + leds[i].id = i; + blobs[i].led_id = LED_MAKE_ID(2, i); + blobs[i].x = 320; + blobs[i].y = 240; + } + t_constellation_led_model model = {}; + model.id = 2; + model.num_leds = leds.size(); + model.leds = leds.data(); + camera_model cam = {}; + cam.calib.fx = cam.calib.fy = 190; + cam.calib.cx = 320; + cam.calib.cy = 240; + cam.calib.model = T_DISTORTION_FISHEYE_KB4; + xrt_pose pose = XRT_POSE_IDENTITY; + pose.position = {0.1f, 0.2f, 0.6f}; + int count = 0, inliers = 0; + CHECK_FALSE(ransac_pnp_pose(&pose, blobs.data(), blobs.size(), &model, &cam, &count, &inliers)); + CHECK(pose.position.x == Catch::Approx(0.1)); + CHECK(pose.position.y == Catch::Approx(0.2)); + CHECK(pose.position.z == Catch::Approx(0.6)); + CHECK(pose.orientation.w == Catch::Approx(1)); +} + +TEST_CASE("Constellation PnP recovers a known camera pose") +{ + std::array leds = {}; + std::array blobs = {}; + camera_model cam = {}; + cam.calib.fx = cam.calib.fy = 190; + cam.calib.cx = 320; + cam.calib.cy = 240; + cam.calib.model = T_DISTORTION_FISHEYE_KB4; + for (size_t i = 0; i < leds.size(); i++) { + leds[i].id = i; + leds[i].pos = {i & 1 ? 0.03f : -0.03f, i & 2 ? 0.03f : -0.03f, i & 4 ? 0.02f : -0.02f}; + blobs[i].led_id = LED_MAKE_ID(2, i); + REQUIRE(t_camera_models_project(&cam.calib, leds[i].pos.x, leds[i].pos.y, leds[i].pos.z + 0.6f, + &blobs[i].x, &blobs[i].y)); + } + t_constellation_led_model model = {}; + model.id = 2; + model.num_leds = leds.size(); + model.leds = leds.data(); + xrt_pose pose = XRT_POSE_IDENTITY; + pose.position.z = 0.5f; + int count = 0, inliers = 0; + REQUIRE(ransac_pnp_pose(&pose, blobs.data(), blobs.size(), &model, &cam, &count, &inliers)); + CHECK(std::isfinite(pose.orientation.w)); + CHECK(pose.position.z == Catch::Approx(0.6).margin(1e-4)); + CHECK(count == 8); + CHECK(inliers == 8); +} + +TEST_CASE("Rift S acquires a recorded five-LED ring with an unknown position and yaw") +{ + auto leds = rift_s_ring_leds; + t_constellation_led_model model = {}; + model.id = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + model.num_leds = leds.size(); + model.leds = leds.data(); + camera_model cam = {}; + cam.width = 640; + cam.height = 480; + cam.calib.fx = rift_s_ring_intrinsics[0]; + cam.calib.fy = rift_s_ring_intrinsics[1]; + cam.calib.cx = rift_s_ring_intrinsics[2]; + cam.calib.cy = rift_s_ring_intrinsics[3]; + cam.calib.model = T_DISTORTION_FISHEYE_KB4; + cam.calib.fisheye = {rift_s_ring_kb4[0], rift_s_ring_kb4[1], rift_s_ring_kb4[2], rift_s_ring_kb4[3]}; + std::array blobs = {}; + for (size_t i = 0; i < blobs.size(); i++) { + blobs[i].x = rift_s_ring_blobs[i][0]; + blobs[i].y = rift_s_ring_blobs[i][1]; + blobs[i].width = rift_s_ring_blobs[i][2]; + blobs[i].height = rift_s_ring_blobs[i][3]; + blobs[i].led_id = LED_INVALID_ID; + } + auto *search_model = t_constellation_search_model_new(&model); + auto *cs = correspondence_search_new(&cam); + correspondence_search_set_blobs(cs, blobs.data(), blobs.size()); + xrt_vec3 pos_error = {0.1f, 0.1f, 0.1f}, rot_error = {0.5f, 0.5f, 0.5f}; + xrt_vec3 gravity = rift_s_ring_gravity; + xrt_pose prior = XRT_POSE_IDENTITY; + prior.position = {42, 42, 42}; + math_quat_from_vec_a_to_vec_b(&rift_s_ring_down, &gravity, &prior.orientation); + auto flags = static_cast(CS_FLAG_SHALLOW_SEARCH | CS_FLAG_HAVE_POSE_PRIOR | + CS_FLAG_MATCH_GRAVITY); + xrt_pose pose = prior; + pose_metrics score = {}; + CHECK_FALSE(correspondence_search_find_one_pose(cs, search_model, flags, &pose, &pos_error, &rot_error, + &gravity, M_PI / 6, &score)); + CHECK(pose.position.x == 42); + model.min_acquisition_leds = 5; + cs->max_search_ns = 1; + pose = prior; + CHECK_FALSE(correspondence_search_find_one_pose(cs, search_model, flags, &pose, &pos_error, &rot_error, + &gravity, M_PI / 6, &score)); + CHECK(cs->budget_exhausted); + CHECK(cs->num_trials == 0); + CHECK(pose.position.x == 42); + cs->max_search_ns = 0; + unsigned candidates = 0; + cs->pose_candidate_userdata = &candidates; + cs->pose_candidate_cb = [](void *userdata, const xrt_pose *candidate, const pose_metrics *metrics) { + ++*static_cast(userdata); + CHECK(std::isfinite(candidate->position.z)); + CHECK(metrics->matched_blobs >= 5); + }; + pose = prior; + REQUIRE(correspondence_search_find_one_pose(cs, search_model, flags, &pose, &pos_error, &rot_error, &gravity, + M_PI / 6, &score)); + CHECK(candidates > 0); + cs->pose_candidate_cb = nullptr; + cs->pose_candidate_userdata = nullptr; + CHECK(score.matched_blobs == 5); + CHECK(score.visible_leds == 5); + CHECK(score.unmatched_blobs == 0); + CHECK(score.reprojection_error / score.matched_blobs < 1.5); + CHECK_FALSE(POSE_HAS_FLAGS(&score, POSE_MATCH_POSITION)); + CHECK_FALSE(POSE_HAS_FLAGS(&score, POSE_MATCH_STRONG)); + CHECK(pose.position.z == Catch::Approx(0.220).margin(0.003)); + CHECK(pose_metrics_gravity_error(&pose, &prior, &gravity) < M_PI / 6); + pose = prior; + xrt_vec3 opposite_down = {-rift_s_ring_down.x, -rift_s_ring_down.y, -rift_s_ring_down.z}; + math_quat_from_vec_a_to_vec_b(&opposite_down, &gravity, &pose.orientation); + CHECK_FALSE(correspondence_search_find_one_pose(cs, search_model, flags, &pose, &pos_error, &rot_error, + &gravity, M_PI / 6, &score)); + pose = prior; + flags = static_cast(CS_FLAG_SHALLOW_SEARCH | CS_FLAG_HAVE_POSE_PRIOR); + CHECK_FALSE(correspondence_search_find_one_pose(cs, search_model, flags, &pose, &pos_error, &rot_error, + &gravity, M_PI / 6, &score)); + correspondence_search_free(cs); + t_constellation_search_model_free(search_model); +} + +TEST_CASE("Split blobs cannot count as additional LED correspondences") +{ + auto leds = rift_s_ring_leds; + t_constellation_led_model model = {}; + model.id = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + model.leds = leds.data(); + model.num_leds = leds.size(); + model.min_acquisition_leds = 5; + camera_model cam = {}; + cam.width = 640; + cam.height = 480; + cam.calib.fx = cam.calib.fy = 190; + cam.calib.cx = 320; + cam.calib.cy = 240; + cam.calib.model = T_DISTORTION_FISHEYE_KB4; + xrt_pose pose = XRT_POSE_IDENTITY; + pose.position.z = 0.5; + pose.orientation = {0.98480775f, 0, 0, 0.17364818f}; + pose_metrics_blob_match_info info = {}; + pose_metrics_match_pose_to_blobs(&pose, nullptr, 0, &model, &cam, &info); + REQUIRE(info.num_visible_leds >= 5); + std::array blobs = {}; + for (size_t i = 0; i < blobs.size(); i++) { + size_t led = i < 4 ? i : 0; + blobs[i].x = info.visible_leds[led].pos_px.x; + blobs[i].y = info.visible_leds[led].pos_px.y; + blobs[i].width = blobs[i].height = 2; + blobs[i].led_id = LED_INVALID_ID; + } + pose_metrics score = {}; + pose_metrics_evaluate_pose(&score, &pose, blobs.data(), blobs.size(), &model, &cam, nullptr); + CHECK(score.matched_blobs == 4); + CHECK(score.unmatched_blobs == 2); + CHECK_FALSE(pose_metrics_can_acquire_with_gravity(&score, &model)); +} + +TEST_CASE("Blob extraction keeps first-column peaks and actual scanline coordinates") +{ + std::array pixels = {}; + pixels[7 * 20 + 5] = 255; + pixels[8 * 20 + 5] = 100; + xrt_frame frame = {}; + frame.width = frame.height = 20; + frame.stride = 20; + frame.format = XRT_FORMAT_L8; + frame.data = pixels.data(); + auto *watch = blobwatch_new(96, 128); + blobservation *observation = nullptr; + blobwatch_process(watch, &frame, &observation); + REQUIRE(observation != nullptr); + REQUIRE(observation->num_blobs == 1); + const auto &b = observation->blobs[0]; + CHECK(b.brightness == 255); + CHECK(b.x == 5); + CHECK(b.y == Catch::Approx(7 + 100.0 / 355)); + CHECK(b.top == 7); + CHECK(b.height == 2); + blobwatch_free(watch); +} + +TEST_CASE("Constellation search acquires synthetic Rift S rings without a pose prior") +{ + for (bool mirror : {false, true}) { + auto leds = rift_s_ring_leds; + if (mirror) { + for (auto &led : leds) { + led.pos.x = -led.pos.x; + led.dir.x = -led.dir.x; + } + } + t_constellation_led_model model = {}; + model.id = mirror ? XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER : XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + model.num_leds = leds.size(); + model.leds = leds.data(); + camera_model cam = {}; + cam.width = 640; + cam.height = 480; + cam.calib.fx = rift_s_ring_intrinsics[0]; + cam.calib.fy = rift_s_ring_intrinsics[1]; + cam.calib.cx = rift_s_ring_intrinsics[2]; + cam.calib.cy = rift_s_ring_intrinsics[3]; + cam.calib.model = T_DISTORTION_FISHEYE_KB4; + cam.calib.fisheye = {rift_s_ring_kb4[0], rift_s_ring_kb4[1], rift_s_ring_kb4[2], rift_s_ring_kb4[3]}; + xrt_pose truth = XRT_POSE_IDENTITY; + truth.orientation = {0.98480775f, 0, 0, 0.17364818f}; + truth.position.z = 0.5f; + pose_metrics_blob_match_info info = {}; + pose_metrics_match_pose_to_blobs(&truth, nullptr, 0, &model, &cam, &info); + REQUIRE(info.num_visible_leds >= 7); + std::vector blobs(info.num_visible_leds); + for (size_t i = 0; i < blobs.size(); i++) { + blobs[i].x = info.visible_leds[i].pos_px.x; + blobs[i].y = info.visible_leds[i].pos_px.y; + blobs[i].width = blobs[i].height = 3; + blobs[i].led_id = LED_INVALID_ID; + } + auto *search_model = t_constellation_search_model_new(&model); + auto *cs = correspondence_search_new(&cam); + correspondence_search_set_blobs(cs, blobs.data(), blobs.size()); + xrt_pose recovered = XRT_POSE_IDENTITY; + pose_metrics score = {}; + REQUIRE(correspondence_search_find_one_pose(cs, search_model, CS_FLAG_SHALLOW_SEARCH, &recovered, + nullptr, nullptr, nullptr, 0, &score)); + CHECK(score.matched_blobs == info.num_visible_leds); + CHECK(score.reprojection_error < 1e-5); + CHECK(recovered.position.z == Catch::Approx(0.5).margin(1e-4)); + correspondence_search_free(cs); + t_constellation_search_model_free(search_model); + } +} + +TEST_CASE("Rift S recorded accelerometer tilt matches the optical tilt in the LED model frame") +{ + // The recorded down vector comes from the driver's top-level IMU calibration (acc_m/acc_b) + // alone. If the TrackedObject AccCalibration rotation also belonged between the IMU and the LED + // model, this unconstrained optical fit would disagree with it by that rotation's tilt. + auto leds = rift_s_ring_leds; + t_constellation_led_model model = {}; + model.id = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + model.num_leds = leds.size(); + model.leds = leds.data(); + model.min_acquisition_leds = 5; + camera_model cam = {}; + cam.width = 640; + cam.height = 480; + cam.calib.fx = rift_s_ring_intrinsics[0]; + cam.calib.fy = rift_s_ring_intrinsics[1]; + cam.calib.cx = rift_s_ring_intrinsics[2]; + cam.calib.cy = rift_s_ring_intrinsics[3]; + cam.calib.model = T_DISTORTION_FISHEYE_KB4; + cam.calib.fisheye = {rift_s_ring_kb4[0], rift_s_ring_kb4[1], rift_s_ring_kb4[2], rift_s_ring_kb4[3]}; + std::array blobs = {}; + for (size_t i = 0; i < blobs.size(); i++) { + blobs[i].x = rift_s_ring_blobs[i][0]; + blobs[i].y = rift_s_ring_blobs[i][1]; + blobs[i].width = rift_s_ring_blobs[i][2]; + blobs[i].height = rift_s_ring_blobs[i][3]; + blobs[i].led_id = LED_INVALID_ID; + } + auto *search_model = t_constellation_search_model_new(&model); + auto *cs = correspondence_search_new(&cam); + correspondence_search_set_blobs(cs, blobs.data(), blobs.size()); + xrt_vec3 pos_error = {1, 1, 1}, rot_error = {3.2f, 3.2f, 3.2f}; + xrt_vec3 gravity = rift_s_ring_gravity; + xrt_pose prior = XRT_POSE_IDENTITY; + math_quat_from_vec_a_to_vec_b(&rift_s_ring_down, &gravity, &prior.orientation); + // Any tilt is allowed: the optical fit must find the tilt on its own. + auto flags = static_cast(CS_FLAG_SHALLOW_SEARCH | CS_FLAG_DEEP_SEARCH | + CS_FLAG_HAVE_POSE_PRIOR | CS_FLAG_MATCH_GRAVITY); + xrt_pose pose = prior; + pose_metrics score = {}; + REQUIRE(correspondence_search_find_one_pose(cs, search_model, flags, &pose, &pos_error, &rot_error, &gravity, + M_PI, &score)); + CHECK(score.matched_blobs == 5); + + pose_metrics_blob_match_info info; + pose_metrics_match_pose_to_blobs(&pose, blobs.data(), blobs.size(), &model, &cam, &info); + for (int i = 0; i < info.num_visible_leds; i++) { + if (info.visible_leds[i].matched_blob) { + info.visible_leds[i].matched_blob->led_id = LED_MAKE_ID(model.id, info.visible_leds[i].led->id); + } + } + xrt_pose refined = pose; + int num_leds = 0, inliers = 0; + REQUIRE(ransac_pnp_pose(&refined, blobs.data(), blobs.size(), &model, &cam, &num_leds, &inliers)); + CHECK(inliers >= 5); + + xrt_vec3 imu_down = rift_s_ring_down; + math_vec3_normalize(&imu_down); + for (const xrt_pose *p : {&pose, &refined}) { + xrt_quat inverse; + math_quat_invert(&p->orientation, &inverse); + xrt_vec3 optical_down; + math_quat_rotate_vec3(&inverse, &gravity, &optical_down); + math_vec3_normalize(&optical_down); + double angle = std::acos(std::min(1.0, (double)m_vec3_dot(optical_down, imu_down))); + CHECK(angle < 3 * M_PI / 180); + } + correspondence_search_free(cs); + t_constellation_search_model_free(search_model); +} + +TEST_CASE("Native constellation search rejects noninvertible pixels without a pose change") +{ + camera_model camera{}; + camera.calib.fx = camera.calib.fy = 190; + camera.calib.cx = 320; + camera.calib.cy = 240; + camera.fisheye62_valid = true; + std::array blobs{}; + std::array leds{}; + t_constellation_led_model model{}; + model.id = 2; + model.leds = leds.data(); + model.num_leds = leds.size(); + for (unsigned i = 0; i < blobs.size(); ++i) { + blobs[i].x = NAN; + blobs[i].y = 240; + blobs[i].led_id = LED_MAKE_ID(2, i); + leds[i].id = i; + } + auto search = correspondence_search_new(&camera); + correspondence_search_set_blobs(search, blobs.data(), blobs.size()); + CHECK(search->num_points == 0); + correspondence_search_free(search); + xrt_pose pose = XRT_POSE_IDENTITY; + pose.position = {.1f, .2f, .6f}; + int count = 0, inliers = 99; + CHECK_FALSE(ransac_pnp_pose(&pose, blobs.data(), blobs.size(), &model, &camera, &count, &inliers)); + CHECK(inliers == 0); + CHECK(pose.position.x == Catch::Approx(.1)); + CHECK(pose.position.y == Catch::Approx(.2)); + CHECK(pose.position.z == Catch::Approx(.6)); + CHECK(pose.orientation.w == 1); +} + +TEST_CASE("Blob extraction merges a top-left peak without a previous scanline") +{ + std::array pixels = {}; + pixels[0] = 255; + pixels[20] = 100; + xrt_frame frame = {}; + frame.width = frame.height = 20; + frame.stride = 20; + frame.format = XRT_FORMAT_L8; + frame.data = pixels.data(); + auto *watch = blobwatch_new(96, 128); + blobservation *observation = nullptr; + blobwatch_process(watch, &frame, &observation); + REQUIRE(observation != nullptr); + REQUIRE(observation->num_blobs == 1); + CHECK(observation->blobs[0].brightness == 255); + CHECK(observation->blobs[0].x == 0); + CHECK(observation->blobs[0].y == Catch::Approx(100.0 / 355)); + CHECK(observation->blobs[0].top == 0); + CHECK(observation->blobs[0].height == 2); + blobwatch_free(watch); +} + +TEST_CASE("LED model cleanup releases both arrays and supports repeated cleanup") +{ + t_constellation_led_model model = {}; + t_constellation_led_model_init(2, nullptr, &model, 3, 4); + REQUIRE(model.leds != nullptr); + REQUIRE(model.bounding_points != nullptr); + t_constellation_led_model_clear(&model); + CHECK(model.leds == nullptr); + CHECK(model.bounding_points == nullptr); + CHECK(model.num_leds == 0); + CHECK(model.num_bounding_points == 0); + t_constellation_led_model_clear(&model); + t_constellation_led_model_init(2, nullptr, &model, 0, 0); + t_constellation_led_model_clear(&model); +} diff --git a/tests/tests_constellation_constrained.cpp b/tests/tests_constellation_constrained.cpp new file mode 100644 index 000000000..498b9509b --- /dev/null +++ b/tests/tests_constellation_constrained.cpp @@ -0,0 +1,1984 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "internal/constrained_pose.h" +#include "internal/sample.h" +#include "internal/correspondence_search.h" +#include +#include +#include +#include +#include + +namespace { +double +distance(xrt_vec3 a, xrt_vec3 b) +{ + return std::sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z)); +} +double +angle(xrt_quat a, xrt_quat b) +{ + // Relative rotation conj(a) * b in double; atan2 stays accurate near zero where acos of a float dot does not. + const double aw = a.w, ax = -a.x, ay = -a.y, az = -a.z, bw = b.w, bx = b.x, by = b.y, bz = b.z; + const double w = aw * bw - ax * bx - ay * by - az * bz; + const double x = aw * bx + ax * bw + ay * bz - az * by; + const double y = aw * by - ax * bz + ay * bw + az * bx; + const double z = aw * bz + ax * by - ay * bx + az * bw; + return 2 * std::atan2(std::sqrt(x * x + y * y + z * z), std::abs(w)); +} +xrt_quat +rotation(double angle, xrt_vec3 axis) +{ + xrt_quat q; + math_vec3_normalize(&axis); + math_quat_from_angle_vector(angle, &axis, &q); + return q; +} +xrt_quat +conjugate(const xrt_quat &q) +{ + return {-q.x, -q.y, -q.z, q.w}; +} +xrt_quat +multiply(const xrt_quat &a, const xrt_quat &b) +{ + xrt_quat q; + math_quat_rotate(&a, &b, &q); + return q; +} + +struct Scene +{ + std::array leds = {{{11, {-0.047f, -0.033f, -0.017f}, {0, 0, -1}, 3.5f}, + {7, {0.039f, -0.009f, 0.027f}, {0, 0, -1}, 3.5f}, + {23, {0.009f, 0.051f, -0.011f}, {0, 0, -1}, 3.5f}}}; + t_constellation_led_model model{}; + camera_model camera{}; + std::array observations{}; + std::array views{}; + xrt_pose truth = XRT_POSE_IDENTITY; + xrt_pose tilt = XRT_POSE_IDENTITY; + constellation_constrained_config config; + Scene() + { + model.id = 2; + model.leds = leds.data(); + model.num_leds = leds.size(); + camera.width = 640; + camera.height = 480; + camera.calib.fx = 300; + camera.calib.fy = 310; + camera.calib.cx = 320; + camera.calib.cy = 240; + camera.calib.model = T_DISTORTION_FISHEYE_KB4; + camera.calib.fisheye.k1 = 0.02; + camera.calib.fisheye.k2 = -0.005; + truth.position = {0.07f, 0.02f, 0.68f}; + truth.orientation = rotation(0.4, {0, 1, 0}); + for (unsigned k = 0; k < 2; k++) { + views[k].camera_index = k + 3; + views[k].P_world_cam = XRT_POSE_IDENTITY; + views[k].calib = &camera; + views[k].observation = &observations[k]; + } + views[1].P_world_cam.position = {0.12f, 0.02f, 0.015f}; + views[1].P_world_cam.orientation = rotation(-0.21, {0.1f, 1, 0.03f}); + for (auto &v : views) + math_pose_invert(&v.P_world_cam, &v.P_cam_world); + constellation_constrained_default_config(&config); + config.time_budget_us = 2000000; + // These fixtures exercise the stock scorer minimum (inliers > sample size) with three + // LEDs, so they relax Monado's stricter cold-acquisition and tracking policy. + config.minimum_cold_inliers = 3; + config.minimum_cold_distinct_leds = 0; + config.minimum_tracking_inliers = 0; + config.minimum_tracking_distinct_leds = 0; + } + void + observe(unsigned camera_index, std::initializer_list indices) + { + auto &o = observations[camera_index]; + o = {}; + for (unsigned led : indices) { + xrt_vec3 world, local; + math_pose_transform_point(&truth, &leds[led].pos, &world); + math_pose_transform_point(&views[camera_index].P_cam_world, &world, &local); + blob &b = o.blobs[o.num_blobs++]; + REQUIRE(t_camera_models_project(&camera.calib, local.x, local.y, local.z, &b.x, &b.y)); + b.led_id = LED_MAKE_ID(99, 42); // Temporal labels must not force an assignment. + } + } +}; +} // namespace + +TEST_CASE("A held HMD cannot turn accurate camera pixels into false controller fixes") +{ + for (bool right : {false, true}) { + Scene s; + if (right) { + for (auto &led : s.leds) + led.pos.x = -led.pos.x; + s.truth.position.x = -s.truth.position.x; + } + s.observe(0, {0, 1}); + s.observe(1, {2}); + constellation_constrained_result live; + REQUIRE(constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, false, false, + &s.config, &live)); + CHECK(distance(live.P_world_model.position, s.truth.position) < 0.0003); + + // The same pixels fit a wrong world pose perfectly if the camera's held world frame is used. + xrt_pose delta{rotation(0.5, {0, 1, 0}), {0.6f, 0, -0.2f}}, wrong; + math_pose_transform(&delta, &s.truth, &wrong); + for (auto &view : s.views) { + xrt_pose current = view.P_world_cam; + math_pose_transform(&delta, ¤t, &view.P_world_cam); + math_pose_invert(&view.P_world_cam, &view.P_cam_world); + } + constellation_constrained_result stale; + REQUIRE(constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, false, false, + &s.config, &stale)); + CHECK(distance(stale.P_world_model.position, wrong.position) < 0.0003); + CHECK(distance(stale.P_world_model.position, s.truth.position) > 0.2); + CHECK(stale.cost_m < 0.001); + + constellation_tracking_sample sample{}; + xrt_space_relation held = XRT_SPACE_RELATION_ZERO; + held.pose = delta; + held.relation_flags = static_cast(XRT_SPACE_RELATION_POSITION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + CHECK_FALSE(constellation_tracking_sample_set_hmd_pose(&sample, XRT_SUCCESS, &held)); + CHECK_FALSE(sample.have_hmd_pose); + held.relation_flags = static_cast(held.relation_flags | + XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + CHECK_FALSE(constellation_tracking_sample_set_hmd_pose(&sample, XRT_ERROR_POSE_NOT_ACTIVE, &held)); + held.pose = XRT_POSE_IDENTITY; + CHECK(constellation_tracking_sample_set_hmd_pose(&sample, XRT_SUCCESS, &held)); + CHECK(sample.have_hmd_pose); + CHECK(distance(sample.P_xrworld_hmd.position, held.pose.position) == 0); + } +} + +TEST_CASE("Constrained two-point geometry recovers 1000 randomized valid poses") +{ + std::mt19937 rng(0x534f4c); + std::uniform_real_distribution unit(-1, 1); + unsigned accepted = 0, two_roots = 0; + for (unsigned attempt = 0; accepted < 1000 && attempt < 10000; attempt++) { + xrt_vec3 points[2] = {{unit(rng) * 0.06f, unit(rng) * 0.06f, unit(rng) * 0.06f}, + {unit(rng) * 0.06f, unit(rng) * 0.06f, unit(rng) * 0.06f}}; + xrt_vec3 gravity = {unit(rng), unit(rng), unit(rng)}; + math_vec3_normalize(&gravity); + xrt_quat tilt = rotation(unit(rng), {unit(rng), unit(rng), unit(rng)}); + xrt_pose truth = XRT_POSE_IDENTITY; + truth.orientation = multiply(rotation(unit(rng) * 3.1, gravity), tilt); + truth.position = {unit(rng) * 0.2f, unit(rng) * 0.2f, 0.8f + unit(rng) * 0.4f}; + xrt_vec3 bearings[2]; + for (unsigned i = 0; i < 2; i++) { + math_pose_transform_point(&truth, &points[i], &bearings[i]); + math_vec3_normalize(&bearings[i]); + } + xrt_pose heading; + if (!constellation_constrained_heading_two_point(points, bearings, &truth.orientation, 0.001745329, + &heading, nullptr)) + continue; + REQUIRE(distance(heading.position, truth.position) < 0.0002); + xrt_pose roots[2]; + unsigned count = + constellation_constrained_gravity_two_point(points, bearings, &tilt, &gravity, 0.001745329, roots, nullptr); + REQUIRE(count >= 1); + bool recovered = false; + for (unsigned i = 0; i < count; i++) { + if (distance(roots[i].position, truth.position) < 0.0003 && + angle(roots[i].orientation, truth.orientation) < 0.001) + recovered = true; + for (unsigned p = 0; p < 2; p++) { + xrt_vec3 projected; + math_pose_transform_point(&roots[i], &points[p], &projected); + REQUIRE(projected.z > 0); + math_vec3_normalize(&projected); + CHECK(distance(projected, bearings[p]) < 0.00001); + } + } + REQUIRE(recovered); + two_roots += count == 2; + accepted++; + } + CHECK(accepted == 1000); + CHECK(two_roots > 100); +} + +TEST_CASE("Constrained solvers reject parallel rays negative depth and unresolved yaw") +{ + xrt_vec3 points[2] = {{0, 0, 0}, {0.04f, 0.03f, 0.01f}}; + xrt_vec3 bearings[2] = {{0, 0, 1}, {0, 0, 1}}; + xrt_quat q = {0, 0, 0, 1}; + xrt_vec3 gravity = {0, 1, 0}; + xrt_pose out[2]; + CHECK_FALSE(constellation_constrained_heading_two_point(points, bearings, &q, 0.001745329, out, nullptr)); + CHECK(constellation_constrained_gravity_two_point(points, bearings, &q, &gravity, 0.001745329, out, nullptr) == 0); + xrt_pose behind = XRT_POSE_IDENTITY; + behind.position.z = -0.6f; + for (unsigned i = 0; i < 2; i++) { + math_pose_transform_point(&behind, &points[i], &bearings[i]); + math_vec3_normalize(&bearings[i]); + } + CHECK_FALSE(constellation_constrained_heading_two_point(points, bearings, &q, 0.001745329, out, nullptr)); + CHECK(constellation_constrained_gravity_two_point(points, bearings, &q, &gravity, 0.001745329, out, nullptr) == 0); + points[1] = {0, 0.05f, 0}; + bearings[0] = {0, 0, 1}; + bearings[1] = {0, 0.05f, 0.6f}; + math_vec3_normalize(&bearings[1]); + CHECK(constellation_constrained_gravity_two_point(points, bearings, &q, &gravity, 0.001745329, out, nullptr) == 0); +} + +TEST_CASE("Gravity acquisition jointly verifies two plus one calibrated observations") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {2}); + constellation_constrained_result result; + REQUIRE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, false, false, &s.config, &result)); + CHECK(result.inliers == 3); + CHECK(result.distinct_leds == 3); + CHECK(result.information_rank == 4); + CHECK(result.vision_determines_yaw); + CHECK(distance(result.P_world_model.position, s.truth.position) < 0.0003); + CHECK(angle(result.P_world_model.orientation, s.truth.orientation) < 0.001); + CHECK(result.camera_diagnostics[0].inliers == 2); + CHECK(result.camera_diagnostics[1].inliers == 1); + CHECK(result.assignments[0].led_id == 11); + CHECK(result.assignments[1].led_id == 7); + CHECK(result.assignments[2].led_id == 23); + constellation_constrained_result reversed; + std::reverse(s.views.begin(), s.views.end()); + REQUIRE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, false, false, &s.config, &reversed)); + CHECK(distance(result.P_world_model.position, reversed.P_world_model.position) < 1e-6); + CHECK(result.cost_m == Catch::Approx(reversed.cost_m).margin(1e-9)); + CHECK(result.pair_trials == reversed.pair_trials); +} + +TEST_CASE("Known capture heading gives translation with three observations") +{ + Scene s; + // A vertical model baseline observes no heading: the gravity path must report unresolved yaw. + s.leds[0].pos = {0, -0.02f, 0}; + s.leds[1].pos = {0, 0.03f, 0}; + s.observe(0, {0, 1}); + s.observe(1, {0}); + s.tilt.orientation = s.truth.orientation; + constellation_constrained_result result; + CHECK_FALSE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, false, false, &s.config, &result)); + CHECK(result.unresolved_yaw_pairs > 0); + REQUIRE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, true, false, &s.config, &result)); + CHECK(result.origin == CONSTELLATION_CONSTRAINED_HEADING); + CHECK(result.inliers == 3); + CHECK(result.distinct_leds == 2); + CHECK(result.information_rank == 3); + CHECK_FALSE(result.vision_determines_yaw); + CHECK(std::isinf(result.covariance[15])); + CHECK(distance(result.P_world_model.position, s.truth.position) < 0.0001); +} + +TEST_CASE("Heading hypotheses claim optical yaw only when the fit observes it") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {2}); + s.tilt.orientation = s.truth.orientation; + constellation_constrained_result result; + REQUIRE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, true, false, &s.config, &result)); + CHECK(result.heading_hypotheses > 0); + CHECK(result.information_rank == 4); + CHECK(result.vision_determines_yaw); + CHECK(std::isfinite(result.covariance[15])); + CHECK(distance(result.P_world_model.position, s.truth.position) < 0.0001); +} + +TEST_CASE("Gravity hypotheses recover three observations despite incorrect heading yaw") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {2}); + s.tilt.orientation = rotation(-0.8, {0, 1, 0}); + constellation_constrained_result result; + CHECK_FALSE(constellation_constrained_verify_pose(&s.model, s.views.data(), 2, &s.tilt, + CONSTELLATION_CONSTRAINED_HEADING, &s.config, &result)); + REQUIRE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, true, false, &s.config, &result)); + CHECK(result.origin == CONSTELLATION_CONSTRAINED_GRAVITY); + CHECK(result.inliers == 3); + CHECK(result.information_rank == 4); + CHECK(distance(result.P_world_model.position, s.truth.position) < 0.0003); + CHECK(angle(result.P_world_model.orientation, s.truth.orientation) < 0.001); +} + +TEST_CASE("Prior path counts repeated LED camera observations and requires geometric rank") +{ + Scene s; + s.observe(0, {0}); + s.observe(1, {0}); + constellation_constrained_result result; + REQUIRE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.truth, true, true, &s.config, &result)); + CHECK(result.origin == CONSTELLATION_CONSTRAINED_PRIOR); + CHECK(result.inliers == 2); + CHECK(result.distinct_leds == 1); + CHECK(result.information_rank == 3); + CHECK(result.assignments[0].led_index == result.assignments[1].led_index); + CHECK(result.assignments[0].camera_index != result.assignments[1].camera_index); + CHECK_FALSE( + constellation_constrained_search(&s.model, s.views.data(), 1, &s.truth, true, true, &s.config, &result)); +} + +TEST_CASE("Gravity verification counts repeated physical LEDs across calibrated cameras") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {0}); + constellation_constrained_result result; + REQUIRE(constellation_constrained_verify_pose(&s.model, s.views.data(), 2, &s.truth, + CONSTELLATION_CONSTRAINED_GRAVITY, &s.config, &result)); + CHECK(result.inliers == 3); + CHECK(result.distinct_leds == 2); + CHECK(result.information_rank == 4); + CHECK(result.num_assignments == 3); +} + +TEST_CASE("Wrong geometry normal visibility and duplicate LED identities do not pass low-count verification") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {2}); + constellation_constrained_result result; + s.leds[2].pos.y += 0.02f; + CHECK_FALSE(constellation_constrained_verify_pose(&s.model, s.views.data(), 2, &s.truth, + CONSTELLATION_CONSTRAINED_GRAVITY, &s.config, &result)); + s.leds[2].pos.y -= 0.02f; + for (auto &led : s.leds) + led.dir.z = 1; + CHECK_FALSE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, false, false, &s.config, &result)); + for (auto &led : s.leds) + led.dir.z = -1; + s.leds[2].id = s.leds[0].id; + CHECK_FALSE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, false, false, &s.config, &result)); + CHECK(result.rejection == CONSTELLATION_CONSTRAINED_INVALID_INPUT); +} + +TEST_CASE("Mirrored geometry cannot acquire from the calibrated three-observation fixture") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {2}); + for (auto &led : s.leds) { + led.pos.x = -led.pos.x; + led.dir.x = -led.dir.x; + } + constellation_constrained_result result; + CHECK_FALSE( + constellation_constrained_search(&s.model, s.views.data(), 2, &s.tilt, false, false, &s.config, &result)); +} + +TEST_CASE("Competing P3P poses and shared blobs retain ambiguity and ownership evidence") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {0, 2}); + std::array hypotheses; + REQUIRE(constellation_constrained_verify_pose(&s.model, s.views.data(), 2, &s.truth, + CONSTELLATION_CONSTRAINED_P3P, &s.config, &hypotheses[0])); + hypotheses[1] = hypotheses[0]; + hypotheses[1].P_world_model.position.x += 0.1f; + hypotheses[1].assignments[0].led_index = 1; + constellation_constrained_result selected; + CHECK_FALSE(constellation_constrained_select_results(hypotheses.data(), 2, &s.config, &selected)); + CHECK(selected.rejection == CONSTELLATION_CONSTRAINED_AMBIGUOUS); + CHECK(selected.ambiguity_likelihood_gap == Catch::Approx(0)); + CHECK(constellation_constrained_results_share_blob(&hypotheses[0], &hypotheses[1])); + for (unsigned i = 0; i < hypotheses[1].num_assignments; i++) + hypotheses[1].assignments[i].camera_index += 10; + CHECK_FALSE(constellation_constrained_results_share_blob(&hypotheses[0], &hypotheses[1])); + hypotheses[1] = hypotheses[0]; + REQUIRE(constellation_constrained_select_results(hypotheses.data(), 2, &s.config, &selected)); +} + +TEST_CASE("P3P joint verification requires more observations than its three-point sample") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {2}); + constellation_constrained_result result; + CHECK_FALSE(constellation_constrained_verify_pose(&s.model, s.views.data(), 2, &s.truth, + CONSTELLATION_CONSTRAINED_P3P, &s.config, &result)); + CHECK(result.rejection == CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS); + s.observe(1, {0, 2}); + REQUIRE(constellation_constrained_verify_pose(&s.model, s.views.data(), 2, &s.truth, + CONSTELLATION_CONSTRAINED_P3P, &s.config, &result)); + CHECK(result.inliers == 4); +} + +TEST_CASE("Symmetric anonymous low-count acquisition rejects competing assignments") +{ + Scene s; + std::array symmetric{}; + for (unsigned i = 0; i < 6; i++) { + symmetric[i] = s.leds[i % 3]; + symmetric[i].id = i; + if (i >= 3) + symmetric[i].pos.x += 0.2f; + } + s.model.leds = symmetric.data(); + s.model.num_leds = symmetric.size(); + s.truth.orientation = {0, 0, 0, 1}; + s.truth.position = {0, 0, 0.6f}; + for (unsigned i = 0; i < 3; i++) { + blob &b = s.observations[0].blobs[i]; + REQUIRE(t_camera_models_project(&s.camera.calib, symmetric[i].pos.x, symmetric[i].pos.y, + symmetric[i].pos.z + 0.6f, &b.x, &b.y)); + } + s.observations[0].num_blobs = 3; + constellation_constrained_result result; + CHECK_FALSE( + constellation_constrained_search(&s.model, s.views.data(), 1, &s.tilt, false, false, &s.config, &result)); + CHECK(result.rejection == CONSTELLATION_CONSTRAINED_AMBIGUOUS); +} + +TEST_CASE("Coincident different LED identities cannot be silently assigned by a greedy score") +{ + Scene s; + s.observe(0, {0, 1}); + s.observe(1, {2}); + std::array leds = {s.leds[0], s.leds[1], s.leds[2], s.leds[2]}; + leds[3].id = 99; + s.model.leds = leds.data(); + s.model.num_leds = leds.size(); + constellation_constrained_result result; + CHECK_FALSE(constellation_constrained_verify_pose(&s.model, s.views.data(), 2, &s.truth, + CONSTELLATION_CONSTRAINED_GRAVITY, &s.config, &result)); + CHECK(result.rejection == CONSTELLATION_CONSTRAINED_AMBIGUOUS); +} + +namespace { +#include "constellation_rift_s_data.hpp" + +struct Rig +{ + camera_model camera{}; + std::vector observations; + std::vector views; + explicit Rig(unsigned count) + { + camera.width = 640; + camera.height = 480; + camera.calib.fx = rift_s_ring_intrinsics[0]; + camera.calib.fy = rift_s_ring_intrinsics[1]; + camera.calib.cx = rift_s_ring_intrinsics[2]; + camera.calib.cy = rift_s_ring_intrinsics[3]; + camera.calib.model = T_DISTORTION_FISHEYE_KB4; + camera.calib.fisheye = {rift_s_ring_kb4[0], rift_s_ring_kb4[1], rift_s_ring_kb4[2], rift_s_ring_kb4[3]}; + observations.resize(count); + views.resize(count); + const float yaw[] = {0, -0.6f, 0.6f, 0.3f, -0.3f}; + const float pitch[] = {0, 0.1f, 0.1f, -0.7f, 0.5f}; + for (unsigned k = 0; k < count; k++) { + auto &v = views[k]; + v.camera_index = k; + v.calib = &camera; + v.observation = &observations[k]; + v.P_world_cam.orientation = + multiply(rotation(yaw[k], {0, 1, 0}), rotation(pitch[k], {1, 0, 0})); + v.P_world_cam.position = {0.04f * k - 0.05f, 0.01f * k, 0}; + math_pose_invert(&v.P_world_cam, &v.P_cam_world); + } + } + void + clear() + { + for (auto &o : observations) + o = {}; + } + /* Project an LED into a view; false when it faces away or leaves the frame. */ + bool + project(unsigned k, const xrt_pose &P_world_model, const t_constellation_led &led, float &x, float &y, + double facing_cos) const + { + xrt_vec3 world, local, normal_world, normal; + math_pose_transform_point(&P_world_model, &led.pos, &world); + math_pose_transform_point(&views[k].P_cam_world, &world, &local); + math_quat_rotate_vec3(&P_world_model.orientation, &led.dir, &normal_world); + math_quat_rotate_vec3(&views[k].P_cam_world.orientation, &normal_world, &normal); + xrt_vec3 to_camera = {-local.x, -local.y, -local.z}; + math_vec3_normalize(&to_camera); + if (local.z < 0.05f || m_vec3_dot(normal, to_camera) < facing_cos) + return false; + if (!t_camera_models_project(&camera.calib, local.x, local.y, local.z, &x, &y)) + return false; + return x >= 0 && y >= 0 && x < camera.width && y < camera.height; + } + void + add_blob(unsigned k, float x, float y) + { + auto &o = observations[k]; + blob &b = o.blobs[o.num_blobs++]; + b = {}; + b.x = x; + b.y = y; + b.width = b.height = 3; + b.led_id = LED_INVALID_ID; + } +}; + +xrt_pose +world_tilt_prior(const xrt_quat &truth, double yaw, double tilt_error, std::mt19937 &rng) +{ + std::uniform_real_distribution unit(-1, 1); + xrt_pose prior = XRT_POSE_IDENTITY; + prior.position = {42, 42, 42}; + xrt_quat tilt_noise = rotation(tilt_error, {unit(rng), 0, unit(rng)}); + prior.orientation = multiply(tilt_noise, multiply(rotation(yaw, {0, 1, 0}), truth)); + return prior; +} + +struct MonteCarlo +{ + unsigned accepted = 0, wrong = 0, rejected = 0, trials = 0; + std::array reasons{}; + double worst_error_m = 0; + double worst_identity_error_m = 0; + double worst_identity_error_sigma = 0; +}; + +/* Random worn-like views: a partial ring of 3..6 observations spread over two cameras, + * background blobs, centroid noise and a small IMU tilt error. Cold acquisition, unknown yaw. */ +MonteCarlo +run_cold_acquisition(bool mirror_model, + unsigned distractors, + unsigned trials, + uint32_t seed, + bool independent_tilt = false, + bool owner_contests = false, + bool recovered_minimum_only = false) +{ + std::mt19937 rng(seed); + std::uniform_real_distribution unit(-1, 1), positive(0, 1); + std::normal_distribution noise(0, 0.3f); + auto truth_leds = rift_s_ring_leds; + auto model_leds = rift_s_ring_leds; + if (mirror_model) + for (auto &l : model_leds) { + l.pos.x = -l.pos.x; + l.dir.x = -l.dir.x; + } + t_constellation_led_model model{}; + model.id = 3; + model.leds = model_leds.data(); + model.num_leds = model_leds.size(); + t_constellation_led_model owner{}; + owner.id = 2; + owner.leds = truth_leds.data(); + owner.num_leds = truth_leds.size(); + Rig rig(2); + constellation_constrained_config config; + constellation_constrained_default_config(&config); + config.time_budget_us = 200000; + if (recovered_minimum_only) { + config.minimum_cold_inliers = 3; + config.minimum_cold_distinct_leds = 0; + config.maximum_cold_unexplained_blobs = 1000; + config.bearing_noise_std_rad = 0; + } + MonteCarlo stats; + while (stats.trials < trials) { + xrt_pose truth = XRT_POSE_IDENTITY; + truth.orientation = rotation(3.14159f * positive(rng), {unit(rng), unit(rng), unit(rng)}); + float depth = 0.25f + 0.45f * positive(rng); + truth.position = {unit(rng) * 0.4f * depth, unit(rng) * 0.3f * depth, depth}; + rig.clear(); + std::vector> visible; + for (unsigned k = 0; k < 2; k++) + for (unsigned l = 0; l < truth_leds.size(); l++) { + float x, y; + if (rig.project(k, truth, truth_leds[l], x, y, std::cos(70 * M_PI / 180))) + visible.push_back({k, l}); + } + if (visible.size() < 3) + continue; + std::shuffle(visible.begin(), visible.end(), rng); + unsigned keep = std::min(visible.size(), 3 + rng() % 4); + visible.resize(keep); + std::array, 2> truth_led; + for (auto [k, l] : visible) { + float x, y; + rig.project(k, truth, truth_leds[l], x, y, -1); + rig.add_blob(k, x + noise(rng), y + noise(rng)); + truth_led[k].push_back(int(l)); + } + for (unsigned k = 0; k < 2; k++) + for (unsigned d = 0; d < distractors; d++) { + rig.add_blob(k, 640 * positive(rng), 480 * positive(rng)); + truth_led[k].push_back(-1); + } + stats.trials++; + xrt_pose prior = world_tilt_prior(truth.orientation, 6.28 * positive(rng), 0.015, rng); + if (independent_tilt) + prior.orientation = rotation(3.14159f * positive(rng), {unit(rng), unit(rng), unit(rng)}); + constellation_constrained_result result; + bool ok = constellation_constrained_search(&model, rig.views.data(), 2, &prior, false, false, &config, + &result); + if (owner_contests) { + // The ring's own controller searches the same exposure with its own tilt. + xrt_pose owner_prior = world_tilt_prior(truth.orientation, 6.28 * positive(rng), 0.015, rng); + constellation_constrained_result owner_result; + bool owner_ok = constellation_constrained_search(&owner, rig.views.data(), 2, &owner_prior, false, + false, &config, &owner_result); + const constellation_constrained_result *claims[2] = {&result, &owner_result}; + bool published[2] = {false, false}, candidates[2] = {ok, owner_ok}; + constellation_constrained_claim blocked[2]; + constellation_constrained_arbitrate(claims, published, candidates, blocked, 2, 2); + if (ok && !candidates[0]) + result.rejection = CONSTELLATION_CONSTRAINED_SHARED_BLOB; + ok = candidates[0]; + } + if (!ok) { + stats.rejected++; + stats.reasons[result.rejection]++; + continue; + } + bool identity = true; + for (unsigned i = 0; i < result.num_assignments; i++) { + const auto &a = result.assignments[i]; + identity &= truth_led[a.camera_index][a.blob_index] == int(a.led_index); + } + double error = distance(result.P_world_model.position, truth.position); + double sigma = 0; + for (unsigned i = 0; i < 3; i++) + sigma += result.covariance[5 * i]; + sigma = std::sqrt(sigma); + if (!identity) { + stats.wrong++; + stats.worst_error_m = std::max(stats.worst_error_m, error); + } else { + stats.accepted++; + stats.worst_identity_error_sigma = std::max(stats.worst_identity_error_sigma, error / sigma); + stats.worst_identity_error_m = std::max(stats.worst_identity_error_m, error); + } + } + return stats; +} + +struct Scenario +{ + float depth_min = 0.25f, depth_max = 0.70f; + //! IMU tilt error of the prior, radians. + double tilt_error = 0.015; + //! Trusted prior: position within prior_error_m of the truth, yaw known within heading_error. + bool trusted = false; + float prior_error_m = 0.03f; + bool heading = false; + double heading_error = 0.03; + unsigned min_keep = 3, max_keep = 6; + float pixel_noise = 0.3f; +}; + +/* Like run_cold_acquisition for the owner's own model, over a chosen depth range and IMU error. */ +MonteCarlo +run_scenario(const Scenario &sc, + unsigned trials, + uint32_t seed, + const constellation_constrained_config *override = nullptr) +{ + std::mt19937 rng(seed); + std::uniform_real_distribution unit(-1, 1), positive(0, 1); + std::normal_distribution noise(0, sc.pixel_noise); + auto leds = rift_s_ring_leds; + t_constellation_led_model model{}; + model.id = 3; + model.leds = leds.data(); + model.num_leds = leds.size(); + Rig rig(2); + constellation_constrained_config config; + constellation_constrained_default_config(&config); + config.time_budget_us = 200000; + if (override) + config = *override; + MonteCarlo stats; + while (stats.trials < trials) { + xrt_pose truth = XRT_POSE_IDENTITY; + truth.orientation = rotation(3.14159f * positive(rng), {unit(rng), unit(rng), unit(rng)}); + float depth = sc.depth_min + (sc.depth_max - sc.depth_min) * positive(rng); + truth.position = {unit(rng) * 0.4f * depth, unit(rng) * 0.3f * depth, depth}; + rig.clear(); + std::vector> visible; + for (unsigned k = 0; k < 2; k++) + for (unsigned l = 0; l < leds.size(); l++) { + float x, y; + if (rig.project(k, truth, leds[l], x, y, std::cos(70 * M_PI / 180))) + visible.push_back({k, l}); + } + if (visible.size() < sc.min_keep) + continue; + std::shuffle(visible.begin(), visible.end(), rng); + unsigned keep = + std::min(visible.size(), sc.min_keep + rng() % (sc.max_keep - sc.min_keep + 1)); + visible.resize(keep); + std::array, 2> truth_led; + for (auto [k, l] : visible) { + float x = 0, y = 0; + rig.project(k, truth, leds[l], x, y, -1); + rig.add_blob(k, x + noise(rng), y + noise(rng)); + truth_led[k].push_back(int(l)); + } + stats.trials++; + double yaw = sc.heading ? sc.heading_error * unit(rng) : 6.28 * positive(rng); + xrt_pose prior = world_tilt_prior(truth.orientation, yaw, sc.tilt_error, rng); + if (sc.trusted) { + xrt_vec3 dir = {unit(rng), unit(rng), unit(rng)}; + math_vec3_normalize(&dir); + prior.position = {truth.position.x + sc.prior_error_m * dir.x, + truth.position.y + sc.prior_error_m * dir.y, + truth.position.z + sc.prior_error_m * dir.z}; + } + constellation_constrained_result result; + if (!constellation_constrained_search(&model, rig.views.data(), 2, &prior, sc.heading, sc.trusted, + &config, &result)) { + stats.rejected++; + stats.reasons[result.rejection]++; + continue; + } + bool identity = true; + for (unsigned i = 0; i < result.num_assignments; i++) { + const auto &a = result.assignments[i]; + identity &= truth_led[a.camera_index][a.blob_index] == int(a.led_index); + } + double error = distance(result.P_world_model.position, truth.position); + if (!identity) { + stats.wrong++; + stats.worst_error_m = std::max(stats.worst_error_m, error); + } else { + stats.accepted++; + stats.worst_identity_error_m = std::max(stats.worst_identity_error_m, error); + } + } + return stats; +} + +void +print(const char *name, const MonteCarlo &s) +{ + std::printf("%s: trials=%u correct_identity=%u wrong_identity=%u rejected=%u worst_wrong_m=%.4f " + "worst_correct_m=%.4f worst_correct_sigma=%.2f reasons:", + name, s.trials, s.accepted, s.wrong, s.rejected, s.worst_error_m, s.worst_identity_error_m, + s.worst_identity_error_sigma); + for (unsigned r = 0; r < s.reasons.size(); r++) + if (s.reasons[r]) + std::printf(" %s=%u", + constellation_constrained_rejection_name((constellation_constrained_rejection)r), + s.reasons[r]); + std::printf("\n"); +} +} // namespace + +TEST_CASE("Recorded Rift S frame acquires through the gravity two-point path") +{ + auto leds = rift_s_ring_leds; + t_constellation_led_model model{}; + model.id = 3; + model.leds = leds.data(); + model.num_leds = leds.size(); + Rig rig(1); + for (const auto &b : rift_s_ring_blobs) + rig.add_blob(0, b[0], b[1]); + // Camera gravity from the recording; world +Y must map onto it. + xrt_vec3 up = {0, 1, 0}, gravity = rift_s_ring_gravity; + math_quat_from_vec_a_to_vec_b(&up, &gravity, &rig.views[0].P_cam_world.orientation); + rig.views[0].P_cam_world.position = {0, 0, 0}; + math_pose_invert(&rig.views[0].P_cam_world, &rig.views[0].P_world_cam); + xrt_pose prior_cam = XRT_POSE_IDENTITY; + math_quat_from_vec_a_to_vec_b(&rift_s_ring_down, &gravity, &prior_cam.orientation); + xrt_pose prior; + math_pose_transform(&rig.views[0].P_world_cam, &prior_cam, &prior); + prior.position = {42, 42, 42}; + constellation_constrained_config config; + constellation_constrained_default_config(&config); + config.time_budget_us = 2000000; + constellation_constrained_result result; + bool acquired = constellation_constrained_search(&model, rig.views.data(), 1, &prior, false, false, &config, &result); + INFO("rejection " << constellation_constrained_rejection_name(result.rejection) << " inliers " << result.inliers + << " unexplained " << result.unexplained_blobs << " chi2 " << result.fit_chi_square << "/" + << result.fit_chi_square_limit); + REQUIRE(acquired); + CHECK(result.origin == CONSTELLATION_CONSTRAINED_GRAVITY); + CHECK(result.inliers == 5); + CHECK(result.distinct_leds == 5); + CHECK(result.vision_determines_yaw); + xrt_pose cam_model; + math_pose_transform(&rig.views[0].P_cam_world, &result.P_world_model, &cam_model); + // The independent P3P search in tests_constellation finds the same ring at 0.220 m. This fit keeps + // the measured accelerometer tilt and minimizes object-space ray distance, so allow a few mm. + CHECK(cam_model.position.z == Catch::Approx(0.220).margin(0.008)); + CHECK(result.cost_m < config.score_gate_m); + CHECK_FALSE(result.budget_exhausted); + + // Upside-down tilt has no consistent solution. + xrt_vec3 opposite = {-rift_s_ring_down.x, -rift_s_ring_down.y, -rift_s_ring_down.z}; + math_quat_from_vec_a_to_vec_b(&opposite, &gravity, &prior_cam.orientation); + math_pose_transform(&rig.views[0].P_world_cam, &prior_cam, &prior); + CHECK_FALSE( + constellation_constrained_search(&model, rig.views.data(), 1, &prior, false, false, &config, &result)); + + // The mirrored model must not explain the left ring. + for (auto &l : leds) { + l.pos.x = -l.pos.x; + l.dir.x = -l.dir.x; + } + math_quat_from_vec_a_to_vec_b(&rift_s_ring_down, &gravity, &prior_cam.orientation); + math_pose_transform(&rig.views[0].P_world_cam, &prior_cam, &prior); + CHECK_FALSE( + constellation_constrained_search(&model, rig.views.data(), 1, &prior, false, false, &config, &result)); +} + +TEST_CASE("Cold low-count acquisition never accepts a wrong pose in randomized partial-ring views") +{ + MonteCarlo clean = run_cold_acquisition(false, 0, 300, 0x5eed1); + print("clean", clean); + CHECK(clean.wrong == 0); + CHECK(clean.accepted > clean.trials / 2); + CHECK(clean.worst_identity_error_sigma < 4); + + // Background lights: an identity slip is tolerated only when it leaves the pose within 1 cm. + MonteCarlo cluttered = run_cold_acquisition(false, 4, 300, 0x5eed2); + print("cluttered", cluttered); + CHECK(cluttered.wrong <= cluttered.trials / 100); + CHECK(cluttered.worst_error_m < 0.01); + CHECK(cluttered.accepted > cluttered.trials / 3); + + // The other hand's (mirrored) model on this ring. Few observations of a self-similar ring cannot + // always separate handedness geometrically, so most fits must be rejected on their own and the + // rest by blob ownership once the ring's own controller searches the same exposure. + MonteCarlo same_tilt = run_cold_acquisition(true, 2, 300, 0x5eed3); + print("other-hand-same-tilt", same_tilt); + CHECK(same_tilt.wrong <= same_tilt.trials / 4); + MonteCarlo other_hand = run_cold_acquisition(true, 2, 300, 0x5eed4, true); + print("other-hand-independent-tilt", other_hand); + CHECK(other_hand.wrong <= other_hand.trials / 10); + MonteCarlo arbitrated = run_cold_acquisition(true, 2, 300, 0x5eed5, true, true); + print("other-hand-after-arbitration", arbitrated); + CHECK(arbitrated.wrong == 0); + MonteCarlo arbitrated_same_tilt = run_cold_acquisition(true, 2, 300, 0x5eed6, false, true); + print("other-hand-same-tilt-after-arbitration", arbitrated_same_tilt); + CHECK(arbitrated_same_tilt.wrong == 0); +} + +TEST_CASE("Stock minimum count alone admits wrong identities that the cold policy rejects") +{ + // Documents why the cold policy exists: inliers > sample size is necessary, not sufficient. + constellation_constrained_config minimum; + constellation_constrained_default_config(&minimum); + CHECK(minimum.minimum_cold_inliers == 4); + MonteCarlo strict = run_cold_acquisition(false, 4, 200, 0x5eed7); + MonteCarlo relaxed = run_cold_acquisition(false, 4, 200, 0x5eed7, false, false, true); + print("cluttered-recovered-minimum-only", relaxed); + CHECK(relaxed.wrong > strict.wrong); + CHECK(strict.wrong <= 2); +} + +namespace { +struct RingScene +{ + std::array leds = rift_s_ring_leds; + t_constellation_led_model model{}; + Rig rig; + xrt_pose truth = XRT_POSE_IDENTITY; + std::array, CONSTELLATION_CONSTRAINED_MAX_VIEWS> truth_led; + constellation_constrained_config config; + explicit RingScene(unsigned cameras) : rig(cameras) + { + model.id = 3; + model.leds = leds.data(); + model.num_leds = leds.size(); + constellation_constrained_default_config(&config); + config.time_budget_us = 2000000; + } + unsigned + observe(unsigned camera, unsigned max_leds) + { + unsigned n = 0; + for (unsigned l = 0; l < leds.size() && n < max_leds; l++) { + float x, y; + if (!rig.project(camera, truth, leds[l], x, y, std::cos(70 * M_PI / 180))) + continue; + rig.add_blob(camera, x, y); + truth_led[camera].push_back(int(l)); + n++; + } + return n; + } + //! Deterministically pick an orientation where each camera sees at least need[k] LEDs. + void + choose_pose(xrt_vec3 position, std::initializer_list need, uint32_t seed) + { + std::mt19937 rng(seed); + std::uniform_real_distribution unit(-1, 1); + truth.position = position; + for (unsigned attempt = 0; attempt < 10000; attempt++) { + truth.orientation = rotation(3.14159f * (unit(rng) + 1) / 2, {unit(rng), unit(rng), unit(rng)}); + unsigned k = 0; + bool ok = true; + for (unsigned n : need) { + unsigned seen = 0; + for (const auto &led : leds) { + float x, y; + seen += rig.project(k, truth, led, x, y, std::cos(70 * M_PI / 180)); + } + ok &= seen >= n; + k++; + } + if (ok) + return; + } + FAIL("no orientation satisfies the visibility request"); + } + bool + identity(const constellation_constrained_result &r) const + { + for (unsigned i = 0; i < r.num_assignments; i++) + if (truth_led[r.assignments[i].camera_index][r.assignments[i].blob_index] != + int(r.assignments[i].led_index)) + return false; + return r.num_assignments > 0; + } +}; +} // namespace + +TEST_CASE("Trusted prior reacquires after hand motion with the tracking minimum count") +{ + RingScene s(2); + // This fixture checks full reacquisition, including every competing prior pair. + s.config.max_prior_pair_trials = 10000; + s.choose_pose({0.03f, 0.06f, 0.42f}, {4}, 1); + REQUIRE(s.observe(0, 3) == 3); + xrt_pose prior = s.truth; + prior.position.x += 0.05f; // Held optical position lags a moving hand. + prior.position.z -= 0.03f; + constellation_constrained_result result; + // Three observations from one camera no longer publish, even with a trusted prior. + CHECK_FALSE( + constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, false, true, &s.config, &result)); + CHECK(result.rejection == CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS); + CHECK(result.inliers == 3); + + // Four observations of distinct LEDs meet the tracking minimum. + s.rig.observations[0].num_blobs = 0; + s.truth_led[0].clear(); + REQUIRE(s.observe(0, 4) == 4); + REQUIRE(constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, false, true, &s.config, + &result)); + CHECK(s.identity(result)); + CHECK(result.inliers == 4); + CHECK(distance(result.P_world_model.position, s.truth.position) < 0.005); + // With heading, the same frame also yields translation hypotheses. + REQUIRE(constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, true, true, &s.config, + &result)); + CHECK(result.heading_hypotheses > 0); + CHECK(s.identity(result)); + // A prior far from the ring finds no edges inside the broad association gate. + prior.position.x += 0.5f; + CHECK_FALSE(constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, true, true, &s.config, + &result)); + CHECK(result.pair_trials == 0); +} + +TEST_CASE("Joint camera result does not depend on camera iteration order") +{ + RingScene s(3); + s.choose_pose({0.0f, 0.05f, 0.45f}, {2, 2, 2}, 2); + unsigned total = 0; + for (unsigned k = 0; k < 3; k++) + total += s.observe(k, 2); + REQUIRE(total >= 4); + std::mt19937 rng(9); + xrt_pose prior = world_tilt_prior(s.truth.orientation, 2.0, 0, rng); + constellation_constrained_result reference; + REQUIRE(constellation_constrained_search(&s.model, s.rig.views.data(), 3, &prior, false, false, &s.config, + &reference)); + CHECK(s.identity(reference)); + std::array order = {0, 1, 2}; + unsigned permutations = 0; + do { + std::vector views; + for (unsigned k : order) + views.push_back(s.rig.views[k]); + constellation_constrained_result result; + REQUIRE(constellation_constrained_search(&s.model, views.data(), 3, &prior, false, false, &s.config, + &result)); + CHECK(distance(result.P_world_model.position, reference.P_world_model.position) < 1e-6); + CHECK(angle(result.P_world_model.orientation, reference.P_world_model.orientation) < 1e-5); + CHECK(result.cost_m == Catch::Approx(reference.cost_m).margin(1e-12)); + CHECK(result.inliers == reference.inliers); + CHECK(result.pair_trials == reference.pair_trials); + for (unsigned i = 0; i < result.num_assignments; i++) { + CHECK(result.assignments[i].camera_index == reference.assignments[i].camera_index); + CHECK(result.assignments[i].blob_index == reference.assignments[i].blob_index); + CHECK(result.assignments[i].led_index == reference.assignments[i].led_index); + } + permutations++; + } while (std::next_permutation(order.begin(), order.end())); + CHECK(permutations == 6); +} + +TEST_CASE("Cross-controller arbitration resolves blob ownership before fusion") +{ + constellation_constrained_result left{}, right{}, idle{}; + left.num_assignments = right.num_assignments = 2; + left.assignments[0] = {0, 4, 1, 1, 0}; + left.assignments[1] = {1, 7, 2, 2, 0}; + right.assignments[0] = {0, 5, 3, 3, 0}; + right.assignments[1] = {1, 7, 4, 4, 0}; // Same blob 7 in camera 1. + left.inliers = right.inliers = 5; + left.distinct_leds = right.distinct_leds = 4; + const constellation_constrained_result *claims[3] = {&left, &right, &idle}; + bool published[3] = {false, false, false}, candidates[3] = {true, true, false}; + constellation_constrained_claim blocked[3]; + + // Equal support: neither candidate can own the blob. + constellation_constrained_arbitrate(claims, published, candidates, blocked, 3, 2); + CHECK_FALSE(candidates[0]); + CHECK_FALSE(candidates[1]); + CHECK(blocked[0] == CONSTELLATION_CLAIM_CANDIDATE); + CHECK(blocked[1] == CONSTELLATION_CLAIM_CANDIDATE); + CHECK(blocked[2] == CONSTELLATION_CLAIM_NONE); + + // Clearly more support keeps the blob, independent of device order. + for (bool reversed : {false, true}) { + left.inliers = 7; + left.distinct_leds = 5; + const constellation_constrained_result *ordered[2] = {reversed ? &right : &left, + reversed ? &left : &right}; + bool pub[2] = {false, false}, cand[2] = {true, true}; + constellation_constrained_claim why[2]; + constellation_constrained_arbitrate(ordered, pub, cand, why, 2, 2); + CHECK(cand[reversed ? 1 : 0]); + CHECK_FALSE(cand[reversed ? 0 : 1]); + } + left.inliers = 5; + left.distinct_leds = 4; + + // A published fast-path result keeps its blobs; only the new claim loses. + published[0] = true; + candidates[0] = false; + candidates[1] = true; + constellation_constrained_arbitrate(claims, published, candidates, blocked, 3, 2); + CHECK(blocked[0] == CONSTELLATION_CLAIM_NONE); + CHECK(blocked[1] == CONSTELLATION_CLAIM_PUBLISHED); + CHECK_FALSE(candidates[1]); + + // The published pose keeps the blob this exposure, but a clearly better-supported candidate + // contests it so its track can end; an equal one does not. + CHECK_FALSE(constellation_constrained_contests_published(&right, &left, 2)); + right.inliers = 7; + right.distinct_leds = 5; + CHECK(constellation_constrained_contests_published(&right, &left, 2)); + CHECK_FALSE(constellation_constrained_contests_published(&right, &idle, 2)); + right.inliers = 5; + right.distinct_leds = 4; + + // A rejected hypothesis contests only when its fit was consistent and at least as supported. + published[0] = false; + for (auto reason : {CONSTELLATION_CONSTRAINED_INCONSISTENT, CONSTELLATION_CONSTRAINED_NO_PAIR, + CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS, CONSTELLATION_CONSTRAINED_AMBIGUOUS}) { + for (unsigned inliers : {4u, 5u}) { + left.rejection = reason; + left.inliers = inliers; + candidates[0] = false; + candidates[1] = true; + constellation_constrained_arbitrate(claims, published, candidates, blocked, 3, 2); + bool competitive = reason == CONSTELLATION_CONSTRAINED_TOO_FEW_INLIERS || + reason == CONSTELLATION_CONSTRAINED_AMBIGUOUS; + bool contests = competitive && inliers >= right.inliers; + CHECK(candidates[1] == !contests); + CHECK((blocked[1] == CONSTELLATION_CLAIM_REJECTED) == contests); + } + } + + // No overlap, no conflict; an empty rejected result claims nothing. + left.inliers = 5; + right.assignments[1].blob_index = 8; + candidates[0] = candidates[1] = true; + constellation_constrained_arbitrate(claims, published, candidates, blocked, 3, 2); + CHECK(candidates[0]); + CHECK(candidates[1]); + left.num_assignments = 0; + right.assignments[1].blob_index = 7; + candidates[0] = false; + candidates[1] = true; + constellation_constrained_arbitrate(claims, published, candidates, blocked, 3, 2); + CHECK(candidates[1]); +} + +TEST_CASE("Motion continuity rejects identity jumps and rotations the IMU did not see") +{ + constellation_motion_limits limits; + constellation_constrained_default_motion_limits(&limits); + constellation_track_point a{}, b{}; + a.P_world_model = XRT_POSE_IDENTITY; + a.P_world_model.position = {0.1f, -0.2f, 0.5f}; + a.imu_world = rotation(0.3, {0, 1, 0}); + a.P_world_model.orientation = rotation(1.0, {1, 0.2f, 0}); + a.capture_ns = 1000000000; + b = a; + b.capture_ns = a.capture_ns + 33000000; // one controller exposure later + double excess, rotation_error; + CHECK(constellation_constrained_motion_consistent(&a, &b, &limits, &excess, &rotation_error)); + CHECK(rotation_error < 1e-4); + + // A fast hand: 2.5 m/s for 33 ms is fine, a 30 cm hop is not. + b.P_world_model.position.x += 0.08f; + CHECK(constellation_constrained_motion_consistent(&a, &b, &limits, &excess, &rotation_error)); + b.P_world_model.position.x += 0.22f; + CHECK_FALSE(constellation_constrained_motion_consistent(&a, &b, &limits, &excess, &rotation_error)); + CHECK(excess > 0.1); + b.P_world_model.position = a.P_world_model.position; + + // Optics and IMU turning together is consistent; optics turning alone is a wrong identity. + xrt_quat turn = rotation(0.5, {0, 0, 1}); + b.P_world_model.orientation = multiply(a.P_world_model.orientation, turn); + CHECK_FALSE(constellation_constrained_motion_consistent(&a, &b, &limits, &excess, &rotation_error)); + CHECK(rotation_error == Catch::Approx(0.5).margin(1e-4)); + b.imu_world = multiply(a.imu_world, turn); + CHECK(constellation_constrained_motion_consistent(&a, &b, &limits, &excess, &rotation_error)); + + // The IMU's world yaw lags the optical one (the driver corrects 10% of a large error per + // pose). A 40 degree wrist pitch then looks like an error of tens of degrees in world coordinates, but + // the controller turned by the same amount in its own frame. + xrt_quat optical = rotation(0.4, {0.3f, 1, 0.2f}); + xrt_quat yaw_offset = rotation(70 * M_PI / 180, {0, 1, 0}); + xrt_quat pitch = rotation(40 * M_PI / 180, {1, 0, 0}); + a.P_world_model.orientation = optical; + a.imu_world = multiply(yaw_offset, optical); + b.P_world_model.orientation = multiply(optical, pitch); + b.imu_world = multiply(a.imu_world, pitch); + CHECK(constellation_constrained_motion_consistent(&a, &b, &limits, &excess, &rotation_error)); + CHECK(rotation_error < 1e-4); + xrt_quat world_optical = multiply(b.P_world_model.orientation, conjugate(a.P_world_model.orientation)); + xrt_quat world_imu = multiply(b.imu_world, conjugate(a.imu_world)); + CHECK(angle(world_optical, world_imu) > 25 * M_PI / 180); + + // Stale references and time running backwards never vouch for a pose. + b.capture_ns = a.capture_ns + 400000000; + CHECK_FALSE(constellation_constrained_motion_consistent(&a, &b, &limits, &excess, &rotation_error)); + b.capture_ns = a.capture_ns - 1; + CHECK_FALSE(constellation_constrained_motion_consistent(&a, &b, &limits, &excess, &rotation_error)); +} + +namespace { +constellation_track_point +track_point(xrt_vec3 position, xrt_quat optical, xrt_quat imu, int64_t capture_ns) +{ + constellation_track_point p{}; + p.P_world_model.position = position; + p.P_world_model.orientation = optical; + p.imu_world = imu; + p.capture_ns = capture_ns; + return p; +} +} // namespace + +TEST_CASE("Track publishes only confirmed, continuous results") +{ + constellation_track track; + constellation_track_init(&track); + const int64_t frame = 33000000; + xrt_quat q = rotation(0.7, {0.2f, 1, 0}); + xrt_vec3 p = {0.1f, -0.3f, 0.5f}; + auto at = [&](int k, xrt_vec3 position, xrt_quat optical, bool observed = true) { + constellation_track_point next = track_point(position, optical, q, k * frame); + xrt_quat ref_imu = q; + return constellation_track_update(&track, &next, observed, &ref_imu, nullptr, nullptr); + }; + + // A cold acquisition alone is never published. + CHECK(at(1, p, q) == CONSTELLATION_TRACK_HOLD_CONFIRMING); + CHECK(track.state == CONSTELLATION_TRACK_CONFIRMING); + // A wrong identity cannot confirm it: it replaces the held result instead. + xrt_vec3 off = {p.x + 0.15f, p.y, p.z}; + CHECK(at(2, off, multiply(rotation(0.5, {0, 0, 1}), q)) == CONSTELLATION_TRACK_HOLD_UNCONFIRMED); + CHECK(at(3, p, q) == CONSTELLATION_TRACK_HOLD_UNCONFIRMED); + CHECK(at(4, p, q) == CONSTELLATION_TRACK_PUBLISH); + CHECK(track.state == CONSTELLATION_TRACK_TRACKING); + + // One wrong result while tracking is held and does not move the reference. + CHECK(at(5, off, q) == CONSTELLATION_TRACK_HOLD_JUMP); + CHECK(track.ref.capture_ns == 4 * frame); + CHECK(at(6, p, q) == CONSTELLATION_TRACK_PUBLISH); + // An optical flip the gyro did not see is a jump even at the same position. + CHECK(at(7, p, multiply(rotation(M_PI, {1, 0, 0}), q)) == CONSTELLATION_TRACK_HOLD_JUMP); + // A position-only fit echoes the IMU, so its (unobserved) orientation is not tested. + CHECK(at(8, p, rotation(2.0, {0, 1, 0}), false) == CONSTELLATION_TRACK_PUBLISH); + + // Repeated disagreement ends the track; the latest result must then confirm itself. + xrt_vec3 far = {p.x - 0.4f, p.y, p.z}; + CHECK(at(9, far, q) == CONSTELLATION_TRACK_HOLD_JUMP); + CHECK(at(10, far, q) == CONSTELLATION_TRACK_HOLD_JUMP); + CHECK(at(11, far, q) == CONSTELLATION_TRACK_HOLD_RESTART); + CHECK(track.state == CONSTELLATION_TRACK_CONFIRMING); + CHECK(at(12, far, q) == CONSTELLATION_TRACK_PUBLISH); + + // Stale and gap handling. + CHECK(at(12, far, q) == CONSTELLATION_TRACK_HOLD_STALE); + CHECK_FALSE(constellation_track_expire(&track, 12 * frame + 300000000)); + CHECK(constellation_track_expire(&track, 12 * frame + 300000001)); + CHECK(track.state == CONSTELLATION_TRACK_LOST); + CHECK(at(30, off, q) == CONSTELLATION_TRACK_HOLD_CONFIRMING); + CHECK(at(31, off, q) == CONSTELLATION_TRACK_PUBLISH); + constellation_track_reset(&track); + CHECK(track.state == CONSTELLATION_TRACK_LOST); + CHECK(at(32, off, q) == CONSTELLATION_TRACK_HOLD_CONFIRMING); + + // Without the reference attitude nothing vouches for the next result. + constellation_track_point next = track_point(off, q, q, 33 * frame); + CHECK(constellation_track_update(&track, &next, true, nullptr, nullptr, nullptr) == + CONSTELLATION_TRACK_HOLD_UNCONFIRMED); +} + +TEST_CASE("Track continuity compares IMU attitudes read after a world-yaw correction") +{ + constellation_track track; + constellation_track_init(&track); + const int64_t frame = 33000000; + xrt_quat optical0 = rotation(0.3, {1, 0, 0}), turn = rotation(0.05, {0, 1, 1}); + xrt_quat optical1 = multiply(turn, optical0); + // The IMU heading is 30 degrees off until the published optical yaw corrects its history. + xrt_quat yaw_error = rotation(30 * M_PI / 180, {0, 1, 0}); + xrt_vec3 p = {0, 0, 0.6f}; + constellation_track_point first = track_point(p, optical0, multiply(yaw_error, optical0), frame); + xrt_quat stale_ref = first.imu_world; + CHECK(constellation_track_update(&track, &first, true, nullptr, nullptr, nullptr) == + CONSTELLATION_TRACK_HOLD_CONFIRMING); + constellation_track_point second = track_point(p, optical1, multiply(yaw_error, optical1), 2 * frame); + CHECK(constellation_track_update(&track, &second, true, &stale_ref, nullptr, nullptr) == + CONSTELLATION_TRACK_PUBLISH); + // Publishing applies the correction to the whole history: both reads now agree with optics. + xrt_quat optical2 = multiply(turn, optical1); + constellation_track_point third = track_point(p, optical2, optical2, 3 * frame); + double excess, rotation_error; + xrt_quat uncorrected = multiply(yaw_error, optical1); + constellation_track copy = track; + CHECK(constellation_track_update(©, &third, true, &uncorrected, &excess, &rotation_error) == + CONSTELLATION_TRACK_HOLD_JUMP); + CHECK(rotation_error == Catch::Approx(30 * M_PI / 180).margin(1e-3)); + CHECK(constellation_track_update(&track, &third, true, &optical1, &excess, &rotation_error) == + CONSTELLATION_TRACK_PUBLISH); + CHECK(rotation_error < 1e-3); +} + +TEST_CASE("Track changes yaw observation source without comparing different world headings") +{ + const int64_t frame = 33000000; + const xrt_vec3 position = {0, 0, 0.6f}; + const xrt_quat optical = rotation(0.3, {1, 0, 0}); + const xrt_quat imu = multiply(rotation(0.7, {0, 1, 0}), optical); + constellation_track track; + constellation_track_init(&track); + auto update = [&](int k, bool observed, xrt_vec3 p = {0, 0, 0.6f}) { + auto next = track_point(p, optical, imu, k * frame); + return constellation_track_update(&track, &next, observed, &imu, nullptr, nullptr); + }; + CHECK(update(1, true) == CONSTELLATION_TRACK_HOLD_CONFIRMING); + CHECK(update(2, true) == CONSTELLATION_TRACK_PUBLISH); + CHECK(update(3, false) == CONSTELLATION_TRACK_PUBLISH); + CHECK(update(4, true) == CONSTELLATION_TRACK_PUBLISH); + CHECK(update(5, false) == CONSTELLATION_TRACK_PUBLISH); + // Changing yaw sources must not hide a positional identity slip. + CHECK(update(6, true, {0.4f, 0, 0.6f}) == CONSTELLATION_TRACK_HOLD_JUMP); + CHECK(update(7, true) == CONSTELLATION_TRACK_PUBLISH); + auto flipped = track_point(position, multiply(rotation(M_PI, {1, 0, 0}), optical), imu, 8 * frame); + CHECK(constellation_track_update(&track, &flipped, true, &imu, nullptr, nullptr) == + CONSTELLATION_TRACK_HOLD_JUMP); +} + +TEST_CASE("Track never publishes randomized wrong identities on a moving hand") +{ + std::mt19937 rng(0x7ac4); + std::uniform_real_distribution unit(-1, 1), positive(0, 1); + constellation_track track; + constellation_track_init(&track); + const int64_t frame = 16666667; + xrt_vec3 p = {0, 0, 0.5f}, v = {0, 0, 0}; + xrt_quat q = XRT_QUAT_IDENTITY; + std::vector imu_history; + unsigned published = 0, published_wrong = 0, correct_results = 0, wrong_results = 0, dropped = 0; + for (int k = 1; k < 6000; k++) { + // A hand moving up to about 2 m/s and turning up to about 6 rad/s. + v = {0.9f * v.x + 0.4f * unit(rng), 0.9f * v.y + 0.4f * unit(rng), 0.9f * v.z + 0.4f * unit(rng)}; + float speed = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); + if (speed > 2) + v = {v.x * 2 / speed, v.y * 2 / speed, v.z * 2 / speed}; + p = {p.x + v.x * 0.016667f, p.y + v.y * 0.016667f, p.z + v.z * 0.016667f}; + q = multiply(rotation(0.1 * positive(rng), {unit(rng), unit(rng), unit(rng)}), q); + imu_history.push_back(q); + float draw = positive(rng); + if (draw < 0.3f) { + dropped++; + constellation_track_expire(&track, k * frame); + continue; + } + bool wrong = draw < 0.6f; + xrt_vec3 position = p; + xrt_quat orientation = q; + if (wrong) { + // Identity slips: one LED spacing or more and a rotation about an arbitrary axis. + float d = 0.04f + 0.2f * positive(rng); + xrt_vec3 dir = {unit(rng), unit(rng), unit(rng)}; + math_vec3_normalize(&dir); + position = {p.x + d * dir.x, p.y + d * dir.y, p.z + d * dir.z}; + orientation = + multiply(rotation(0.35 + 2.8 * positive(rng), {unit(rng), unit(rng), unit(rng)}), q); + wrong_results++; + } else { + correct_results++; + } + constellation_track_expire(&track, k * frame); + constellation_track_point next = track_point(position, orientation, q, k * frame); + const xrt_quat *ref_imu = nullptr; + if (track.state != CONSTELLATION_TRACK_LOST) + ref_imu = &imu_history[track.ref.capture_ns / frame - 1]; + if (constellation_track_update(&track, &next, true, ref_imu, nullptr, nullptr) == + CONSTELLATION_TRACK_PUBLISH) { + published++; + published_wrong += wrong; + } + } + printf("track: correct_results=%u wrong_results=%u dropped=%u published=%u published_wrong=%u\n", + correct_results, wrong_results, dropped, published, published_wrong); + CHECK(published_wrong == 0); + CHECK(published > correct_results * 3 / 4); +} + +TEST_CASE("Known-orientation translation stays bounded near the bearing-separation limit") +{ + std::mt19937 rng(17); + std::normal_distribution noise(0, 1e-5); + const xrt_vec3 points[2] = {{0, 0, 0}, {0.004f, 0.001f, 0.0f}}; + xrt_quat q = rotation(0.3, {0.2f, 1, 0.1f}); + for (double depth : {0.3, 0.8, 1.6}) { + xrt_pose truth = {q, {0.01f, -0.02f, float(depth)}}; + xrt_vec3 bearings[2]; + for (unsigned i = 0; i < 2; i++) { + math_pose_transform_point(&truth, &points[i], &bearings[i]); + math_vec3_normalize(&bearings[i]); + } + double separation = std::acos(std::clamp(m_vec3_dot(bearings[0], bearings[1]), -1, 1)); + enum constellation_constrained_solver_status status; + xrt_pose out; + bool ok = constellation_constrained_heading_two_point(points, bearings, &q, 0.001745329, &out, &status); + if (separation < 0.001745329) { + CHECK_FALSE(ok); + CHECK(status == CONSTELLATION_SOLVER_DEGENERATE_BEARINGS); + continue; + } + REQUIRE(ok); + CHECK(distance(out.position, truth.position) < 1e-4 * depth); + // Perturbed bearings must give a finite answer whose error scales with depth/separation. + for (unsigned trial = 0; trial < 50; trial++) { + xrt_vec3 noisy[2] = {bearings[0], bearings[1]}; + for (auto &b : noisy) { + b.x += noise(rng); + b.y += noise(rng); + math_vec3_normalize(&b); + } + if (!constellation_constrained_heading_two_point(points, noisy, &q, 0.001745329, &out, &status)) + continue; + CHECK(std::isfinite(out.position.z)); + CHECK(distance(out.position, truth.position) < 10 * 1e-5 * depth / separation + 1e-4); + } + } +} + +TEST_CASE("Dense clutter exhausts the bounded budget instead of stalling, and adjacency finds the ring first") +{ + RingScene s(2); + s.choose_pose({-0.02f, 0.04f, 0.4f}, {4, 2}, 3); + REQUIRE(s.observe(0, 4) + s.observe(1, 2) >= 5); + std::mt19937 rng(3); + std::uniform_real_distribution unit(0, 1); + for (unsigned k = 0; k < 2; k++) + for (unsigned d = 0; d < 25; d++) { + s.rig.add_blob(k, 640 * unit(rng), 480 * unit(rng)); + s.truth_led[k].push_back(-1); + } + xrt_pose prior = world_tilt_prior(s.truth.orientation, 1.0, 0, rng); + constellation_constrained_result result; + s.config.max_pair_trials = 4000; + bool ok = constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, false, false, &s.config, + &result); + CHECK(result.budget_exhausted); + CHECK(result.pair_trials == 4000); + if (ok) + CHECK(s.identity(result)); + s.config.time_budget_us = 1; + CHECK_FALSE(constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, false, false, &s.config, + &result)); + CHECK(result.budget_exhausted); + CHECK(result.pair_trials <= 1); +} + +TEST_CASE("Two controllers in one exposure acquire their own rings and never swap") +{ + std::mt19937 rng(0x7a11); + std::uniform_real_distribution unit(-1, 1), positive(0, 1); + std::normal_distribution noise(0, 0.3f); + auto left_leds = rift_s_ring_leds, right_leds = rift_s_ring_leds; + for (auto &l : right_leds) { + l.pos.x = -l.pos.x; + l.dir.x = -l.dir.x; + } + t_constellation_led_model models[2] = {}; + models[0].id = 3; + models[0].leds = left_leds.data(); + models[0].num_leds = left_leds.size(); + models[1].id = 2; + models[1].leds = right_leds.data(); + models[1].num_leds = right_leds.size(); + Rig rig(2); + constellation_constrained_config config; + constellation_constrained_default_config(&config); + config.time_budget_us = 200000; + unsigned trials = 0, acquired[2] = {0, 0}, wrong = 0; + while (trials < 150) { + xrt_pose truth[2]; + std::array, 2> owner, led_of; + rig.clear(); + unsigned visible_total[2] = {0, 0}; + for (unsigned hand = 0; hand < 2; hand++) { + truth[hand] = XRT_POSE_IDENTITY; + truth[hand].orientation = rotation(3.14159f * positive(rng), {unit(rng), unit(rng), unit(rng)}); + float depth = 0.3f + 0.3f * positive(rng); + // Hands side by side, sometimes close together. + truth[hand].position = {(hand ? 1 : -1) * (0.03f + 0.12f * positive(rng)), 0.1f * unit(rng), depth}; + const auto &leds = hand ? right_leds : left_leds; + for (unsigned k = 0; k < 2; k++) + for (unsigned l = 0; l < leds.size(); l++) { + float x, y; + if (!rig.project(k, truth[hand], leds[l], x, y, std::cos(70 * M_PI / 180)) || + positive(rng) < 0.3f) // partial occlusion by the hand + continue; + rig.add_blob(k, x + noise(rng), y + noise(rng)); + owner[k].push_back(int(hand)); + led_of[k].push_back(int(l)); + visible_total[hand]++; + } + } + if (visible_total[0] < 3 && visible_total[1] < 3) + continue; + trials++; + constellation_constrained_result results[2]; + bool candidates[2], published[2] = {false, false}; + constellation_constrained_claim blocked[2]; + for (unsigned hand = 0; hand < 2; hand++) { + xrt_pose prior = world_tilt_prior(truth[hand].orientation, 6.28 * positive(rng), 0.015, rng); + candidates[hand] = constellation_constrained_search(&models[hand], rig.views.data(), 2, &prior, false, + false, &config, &results[hand]); + } + const constellation_constrained_result *claims[2] = {&results[0], &results[1]}; + constellation_constrained_arbitrate(claims, published, candidates, blocked, 2, 2); + for (unsigned hand = 0; hand < 2; hand++) { + if (!candidates[hand]) + continue; + bool own = true; + for (unsigned i = 0; i < results[hand].num_assignments; i++) { + const auto &a = results[hand].assignments[i]; + own &= owner[a.camera_index][a.blob_index] == int(hand) && + led_of[a.camera_index][a.blob_index] == int(a.led_index); + } + if (own && distance(results[hand].P_world_model.position, truth[hand].position) < 0.03) + acquired[hand]++; + else + wrong++; + } + } + std::printf("two-hands: trials=%u left_acquired=%u right_acquired=%u wrong=%u\n", trials, acquired[0], + acquired[1], wrong); + CHECK(wrong == 0); + // The mirrored right model must acquire as readily as the left one. + CHECK(acquired[0] > trials / 4); + CHECK(acquired[1] > trials / 4); +} + +TEST_CASE("Trusted prior search stays near the prior and finds distant rings") +{ + // Arm's length and below the hips: 0.7-1.0 m from the cameras, IMU yaw unknown. The prior + // only bounds the search; a truncated, yaw-ordered pair list used to miss the correct pair + // and let a hypothesis half a metre away win. + Scenario sc; + sc.depth_min = 0.7f; + sc.depth_max = 1.0f; + sc.trusted = true; + sc.min_keep = 8; + sc.max_keep = 11; + MonteCarlo many = run_scenario(sc, 200, 0x1234 + 8); + print("trusted-far-many", many); + CHECK(many.wrong == 0); + CHECK(many.accepted >= 170); + sc.min_keep = 4; + sc.max_keep = 7; + sc.depth_min = 0.3f; + sc.depth_max = 0.6f; + MonteCarlo few = run_scenario(sc, 200, 0x1234 + 4); + print("trusted-near-few", few); + CHECK(few.wrong == 0); + CHECK(few.accepted >= 150); + CHECK(few.worst_identity_error_m < 0.05); +} + +TEST_CASE("Fast-path budget does not turn one solution into an ambiguity") +{ + // The fast path has 3 ms. When the pair search used up the budget, refinement stopped too and + // left candidates with identical LED assignments at slightly different poses, which the + // ambiguity test then treated as competing identities. + constellation_constrained_config fast; + constellation_constrained_default_config(&fast); + fast.time_budget_us = 3000; +#if defined(__SANITIZE_ADDRESS__) || defined(__SANITIZE_THREAD__) + // Instrumentation changes throughput. The native build checks the 3 ms fast path. + fast.time_budget_us = 200000; +#endif + Scenario sc; + sc.depth_min = 0.7f; + sc.depth_max = 1.0f; + sc.trusted = true; + sc.min_keep = 8; + sc.max_keep = 11; + MonteCarlo stats = run_scenario(sc, 200, 0x1234 + 8, &fast); + print("trusted-far-many-3ms", stats); + CHECK(stats.wrong == 0); + CHECK(stats.reasons[CONSTELLATION_CONSTRAINED_AMBIGUOUS] <= 20); + CHECK(stats.accepted >= 160); +} + +TEST_CASE("Bounded tilt adjustment absorbs an IMU tilt error of a few degrees") +{ + RingScene s(2); + s.choose_pose({0.02f, 0.04f, 0.40f}, {5, 4}, 11); + REQUIRE(s.observe(0, 6) >= 5); + REQUIRE(s.observe(1, 6) >= 4); + auto tilted_prior = [&](double degrees) { + xrt_pose prior = s.truth; + prior.orientation = multiply(rotation(degrees * M_PI / 180, {1, 0, 1}), s.truth.orientation); + return prior; + }; + // Angle between the world gravity directions the two orientations put in the model frame. + auto tilt_error = [&](const constellation_constrained_result &r) { + xrt_vec3 gravity = {0, 1, 0}, truth, fit; + xrt_quat truth_inverse = conjugate(s.truth.orientation), + fit_inverse = conjugate(r.P_world_model.orientation); + math_quat_rotate_vec3(&truth_inverse, &gravity, &truth); + math_quat_rotate_vec3(&fit_inverse, &gravity, &fit); + return std::acos(std::clamp(double(m_vec3_dot(truth, fit)), -1.0, 1.0)) * 180 / M_PI; + }; + constellation_constrained_config fixed = s.config; + fixed.tilt_prior_std_rad = 0; + constellation_constrained_result result; + + // 4 degrees off: the fixed-tilt fit keeps the error and absorbs it in its residuals; the + // adjusted fit moves most of the way to the true tilt. One small ring observes tilt to about + // 2 degrees, so the 3 degree prior keeps part of the error. + xrt_pose prior = tilted_prior(4); + REQUIRE( + constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, false, false, &fixed, &result)); + CHECK(tilt_error(result) == Catch::Approx(4).margin(0.1)); + CHECK(result.tilt_correction_rad == 0); + double fixed_chi_square = result.fit_chi_square; + REQUIRE(constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, false, false, &s.config, + &result)); + CHECK(s.identity(result)); + CHECK(tilt_error(result) < 1.5); + CHECK(result.tilt_correction_rad * 180 / M_PI > 2.5); + CHECK(result.fit_chi_square < fixed_chi_square / 4); + CHECK(distance(result.P_world_model.position, s.truth.position) < 0.003); + + // The adjustment never exceeds its bound, however far off the IMU is. + prior = tilted_prior(12); + constellation_constrained_search(&s.model, s.rig.views.data(), 2, &prior, false, false, &s.config, &result); + CHECK(result.tilt_correction_rad <= s.config.maximum_tilt_correction_rad + 1e-6); + + // Three distinct LEDs keep the prior tilt. + RingScene few(1); + few.choose_pose({0.02f, 0.04f, 0.40f}, {3}, 12); + REQUIRE(few.observe(0, 3) == 3); + xrt_pose few_prior = few.truth; + few_prior.orientation = multiply(rotation(2 * M_PI / 180, {1, 0, 1}), few.truth.orientation); + few_prior.position.x += 0.01f; + few.config.minimum_tracking_inliers = 3; + constellation_constrained_search(&few.model, few.rig.views.data(), 1, &few_prior, true, true, &few.config, + &result); + CHECK(result.num_assignments == 3); + CHECK(result.tilt_correction_rad == 0); +} + +TEST_CASE("Tilt adjustment recovers acquisitions lost to IMU tilt error without wrong identities") +{ + constellation_constrained_config fixed; + constellation_constrained_default_config(&fixed); + fixed.time_budget_us = 200000; + fixed.tilt_prior_std_rad = 0; + Scenario sc; + sc.min_keep = 6; + sc.max_keep = 11; + sc.tilt_error = 5 * M_PI / 180; + MonteCarlo adjusted = run_scenario(sc, 300, 0x7117); + MonteCarlo kept = run_scenario(sc, 300, 0x7117, &fixed); + print("tilt-5deg-adjusted", adjusted); + print("tilt-5deg-fixed", kept); + CHECK(adjusted.wrong <= kept.wrong); + CHECK(adjusted.accepted >= kept.accepted + 10); + CHECK(adjusted.reasons[CONSTELLATION_CONSTRAINED_INCONSISTENT] < + kept.reasons[CONSTELLATION_CONSTRAINED_INCONSISTENT] / 2); + + // No tilt error: the extra freedom costs at most a few acquisitions. + sc.tilt_error = 0; + MonteCarlo exact = run_scenario(sc, 300, 0x7118); + MonteCarlo exact_fixed = run_scenario(sc, 300, 0x7118, &fixed); + print("tilt-0deg-adjusted", exact); + print("tilt-0deg-fixed", exact_fixed); + CHECK(exact.wrong == 0); + CHECK(exact.accepted + 6 >= exact_fixed.accepted); +} + +TEST_CASE("Head-relative positions ignore head pitch and roll but follow its heading") +{ + xrt_pose head = XRT_POSE_IDENTITY; + head.position = {0.3f, 1.6f, -0.2f}; + // A hand 50 cm in front, 60 cm below and 20 cm left of the head. + auto check = [&](xrt_vec3 expected) { + xrt_vec3 forward = {0, 0, -1}, world_forward, right = {1, 0, 0}, world_right; + math_quat_rotate_vec3(&head.orientation, &forward, &world_forward); + math_quat_rotate_vec3(&head.orientation, &right, &world_right); + // Build the hand position from the head's horizontal heading. + world_forward.y = 0; + world_right.y = 0; + math_vec3_normalize(&world_forward); + math_vec3_normalize(&world_right); + xrt_vec3 hand = head.position; + hand = m_vec3_add(hand, m_vec3_mul_scalar(world_forward, -expected.z)); + hand = m_vec3_add(hand, m_vec3_mul_scalar(world_right, expected.x)); + hand.y += expected.y; + xrt_vec3 out; + constellation_head_relative_position(&head, &hand, &out); + CHECK(out.x == Catch::Approx(expected.x).margin(1e-4)); + CHECK(out.y == Catch::Approx(expected.y).margin(1e-4)); + CHECK(out.z == Catch::Approx(expected.z).margin(1e-4)); + }; + check({-0.2f, -0.6f, -0.5f}); + // Looking down 60 degrees, or turned 120 degrees and rolled: the same body-relative numbers. + head.orientation = rotation(-60 * M_PI / 180, {1, 0, 0}); + check({-0.2f, -0.6f, -0.5f}); + head.orientation = multiply(rotation(120 * M_PI / 180, {0, 1, 0}), rotation(25 * M_PI / 180, {0, 0, 1})); + check({-0.2f, -0.6f, -0.5f}); + xrt_vec3 hand = {0.3f, 1.0f, -0.2f}, out; + constellation_head_relative_position(&head, &hand, &out); + CHECK(out.y == Catch::Approx(-0.6).margin(1e-4)); + CHECK(std::hypot(out.x, out.z) < 1e-4); +} + +TEST_CASE("Confirmed heading excludes the r12 gravity-hypothesis identity jumps before selection") +{ + Scene s; + s.observe(0, {0, 1}); s.observe(1, {2}); + constellation_constrained_result result{}; + REQUIRE(constellation_constrained_search(&s.model, s.views.data(), 2, &s.truth, + true, true, &s.config, &result)); + for (double degrees : {79., 179.}) { + auto prior = s.truth; + prior.orientation = multiply(rotation(degrees * M_PI / 180, {0, 1, 0}), prior.orientation); + CHECK_FALSE(constellation_constrained_search(&s.model, s.views.data(), 2, &prior, + true, true, &s.config, &result)); + CHECK(result.prior_rotation_rejections > 0); + // Acquisition after loss still allows every heading, independently of this guard. + REQUIRE(constellation_constrained_search(&s.model, s.views.data(), 2, &prior, + false, false, &s.config, &result)); + CHECK(angle(result.P_world_model.orientation, s.truth.orientation) < .001); + } +} + +TEST_CASE("Shared blob logging selects the strongest conflict in any device order") +{ + constellation_constrained_result mine{}, weak{}, strong{}; + for (auto *r : {&mine, &weak, &strong}) { + r->num_assignments = 1; + r->assignments[0] = {0, 7, 0, 0, 0}; + r->inliers = 5; + r->distinct_leds = 4; + } + weak.rejection = CONSTELLATION_CONSTRAINED_AMBIGUOUS; + for (bool reversed : {false, true}) { + const constellation_constrained_result *claims[] = {&mine, reversed ? &strong : &weak, + reversed ? &weak : &strong}; + bool published[] = {false, reversed, !reversed}, candidates[] = {true, false, false}; + constellation_constrained_claim blocked[3]; + unsigned blockers[3]; + constellation_constrained_arbitrate_with_owners(claims, published, candidates, blocked, blockers, 3, 2); + CHECK_FALSE(candidates[0]); + CHECK(blocked[0] == CONSTELLATION_CLAIM_PUBLISHED); + CHECK(claims[blockers[0]] == &strong); + published[1] = published[2] = false; + candidates[0] = true; + candidates[reversed ? 1 : 2] = true; + constellation_constrained_arbitrate_with_owners(claims, published, candidates, blocked, blockers, 3, 2); + CHECK(blocked[0] == CONSTELLATION_CLAIM_CANDIDATE); + CHECK(claims[blockers[0]] == &strong); + } +} + +TEST_CASE("Repeated near-depth candidates cannot erase a recent optical history") +{ + constellation_track track; + constellation_track_init(&track); + xrt_quat imu = XRT_QUAT_IDENTITY; + auto update = [&](int frame, float depth) { + auto next = track_point({0, 0, depth}, imu, imu, frame * 33000000LL); + return constellation_track_update(&track, &next, true, &imu, nullptr, nullptr); + }; + CHECK(update(1, 0.9f) == CONSTELLATION_TRACK_HOLD_CONFIRMING); + REQUIRE(update(2, 0.9f) == CONSTELLATION_TRACK_PUBLISH); + CHECK(update(3, 0.2f) == CONSTELLATION_TRACK_HOLD_JUMP); + CHECK(update(4, 0.2f) == CONSTELLATION_TRACK_HOLD_JUMP); + CHECK(update(5, 0.2f) == CONSTELLATION_TRACK_HOLD_RESTART); + CHECK(update(6, 0.2f) == CONSTELLATION_TRACK_HOLD_UNCONFIRMED); + CHECK(update(7, 0.2f) == CONSTELLATION_TRACK_HOLD_UNCONFIRMED); + CHECK(track.published.P_world_model.position.z == Catch::Approx(0.9f)); +} + +TEST_CASE("P3P acquisition forwards three-camera-view roots to joint verification") +{ + Scene s; + s.observe(0, {0, 1, 2}); + s.observe(1, {0}); + for (auto &o : s.observations) + for (int i = 0; i < o.num_blobs; ++i) { + o.blobs[i].led_id = LED_INVALID_ID; + o.blobs[i].width = o.blobs[i].height = 2; + } + s.model.min_acquisition_leds = 5; + struct Context { + Scene *scene; + unsigned checked = 0; + bool recovered = false; + } context{&s}; + auto *model = t_constellation_search_model_new(&s.model); + REQUIRE(model); + auto *search = correspondence_search_new(&s.camera); + REQUIRE(search); + correspondence_search_set_blobs(search, s.observations[0].blobs, 3); + search->pose_candidate_userdata = &context; + search->pose_candidate_cb = [](void *userdata, const xrt_pose *pose, const pose_metrics *) { + auto &ctx = *static_cast(userdata); + ++ctx.checked; + constellation_constrained_result result{}; + if (constellation_constrained_verify_pose(&ctx.scene->model, ctx.scene->views.data(), 2, pose, + CONSTELLATION_CONSTRAINED_P3P, &ctx.scene->config, &result)) { + ctx.recovered |= distance(pose->position, ctx.scene->truth.position) < 0.001 && + angle(pose->orientation, ctx.scene->truth.orientation) < 0.01; + } + }; + xrt_pose seed = XRT_POSE_IDENTITY; + seed.position = {42, 42, 42}; + xrt_vec3 gravity{0, 1, 0}, pos_error{0.001f, 0.001f, 0.001f}, rot_error{0.1f, 0.1f, 0.1f}; + pose_metrics metrics{}; + auto flags = static_cast(CS_FLAG_SHALLOW_SEARCH | CS_FLAG_DEEP_SEARCH | + CS_FLAG_MATCH_GRAVITY | CS_FLAG_MATCH_ALL_BLOBS | CS_FLAG_JOINT_P3P); + correspondence_search_find_one_pose(search, model, flags, &seed, &pos_error, &rot_error, + &gravity, 0.1f, &metrics); + CHECK(context.checked > 0); + CHECK(context.recovered); + SECTION("Three observations alone cannot acquire") { + s.observe(1, {}); + context.recovered = false; + seed = XRT_POSE_IDENTITY; + correspondence_search_find_one_pose(search, model, flags, &seed, &pos_error, &rot_error, + &gravity, 0.1f, &metrics); + CHECK_FALSE(context.recovered); + } + SECTION("Published blob ownership is excluded before generating P3P roots") { + context.checked = 0; + s.views[0].excluded_blobs[1] = true; + search->excluded_blobs = s.views[0].excluded_blobs; + seed = XRT_POSE_IDENTITY; + correspondence_search_find_one_pose(search, model, flags, &seed, &pos_error, &rot_error, + &gravity, 0.1f, &metrics); + CHECK(context.checked == 0); + } + correspondence_search_free(search); + t_constellation_search_model_free(model); +} + +TEST_CASE("Cold right acquisition uses unclaimed observations after the left publishes") +{ + // Two geometrically identical objects are the hardest ownership case. Raw cold search + // cannot choose between their rings; arbitration alone never exposes the second solution. + Scene s; + const xrt_pose right = s.truth; + s.config.time_budget_us = 25000; + xrt_pose left = right; + left.position.x = -0.18f; + s.truth = left; + s.observe(0, {0, 1, 2}); + s.observe(1, {0, 1, 2}); + for (unsigned k = 0; k < 2; k++) { + for (unsigned l = 0; l < s.leds.size(); l++) { + xrt_vec3 world, local; + math_pose_transform_point(&right, &s.leds[l].pos, &world); + math_pose_transform_point(&s.views[k].P_cam_world, &world, &local); + auto &o = s.observations[k]; + auto &b = o.blobs[o.num_blobs++]; + REQUIRE(t_camera_models_project(&s.camera.calib, local.x, local.y, local.z, &b.x, &b.y)); + b.led_id = LED_MAKE_ID(99, 42); // Stale labels cannot become ownership. + } + } + constellation_tracking_sample sample{}; + sample.n_devices = 2; + sample.devices[0].found_device_pose = true; + REQUIRE(constellation_constrained_verify_pose(&s.model, s.views.data(), 2, &left, + CONSTELLATION_CONSTRAINED_P3P, &s.config, &sample.devices[0].joint_result)); + constellation_track track; + constellation_track_init(&track); + xrt_pose prior = XRT_POSE_IDENTITY; + unsigned raw_published = 0, recovered_at = 0; + for (unsigned frame = 1; frame <= 60; frame++) { + s.observations[1].num_blobs = frame % 3 ? 6 : 5; // Vary visibility without changing blob indices. + auto &result = sample.devices[1].joint_result; + bool candidates[2] = {false, constellation_constrained_search(&s.model, s.views.data(), 2, + &prior, false, false, &s.config, &result)}; + const bool published[2] = {true, false}; + const constellation_constrained_result *claims[2] = {&sample.devices[0].joint_result, &result}; + constellation_constrained_claim blocked[2]; + constellation_constrained_arbitrate(claims, published, candidates, blocked, 2, 2); + raw_published += candidates[1]; + auto remaining = s.views; + const unsigned excluded = constellation_tracking_sample_exclude_published(&sample, 1, + remaining.data(), remaining.size()); + CHECK(excluded == 6); + if (!constellation_constrained_search(&s.model, remaining.data(), 2, &prior, false, false, + &s.config, &result)) + continue; + for (unsigned a = 0; a < result.num_assignments; a++) + CHECK(result.assignments[a].blob_index >= 3); + CHECK(distance(result.P_world_model.position, right.position) < 0.001); + candidates[1] = true; + constellation_constrained_arbitrate(claims, published, candidates, blocked, 2, 2); + REQUIRE(candidates[1]); + constellation_track_point next{result.P_world_model, right.orientation, int64_t(frame) * 33333333}; + auto verdict = constellation_track_update(&track, &next, result.vision_determines_yaw, + &right.orientation, nullptr, nullptr); + if (verdict == CONSTELLATION_TRACK_PUBLISH && !recovered_at) + recovered_at = frame; + } + CHECK(raw_published == 0); + CHECK(recovered_at > 0); + CHECK(recovered_at <= 60); + // Removing the current publication re-enables all blobs, regardless of old LED labels. + sample.devices[0].found_device_pose = false; + auto remaining = s.views; + CHECK(constellation_tracking_sample_exclude_published(&sample, 1, remaining.data(), 2) == 0); + // The ownership retry does not bypass the optical/IMU continuity gate. + constellation_track_point jump{right, right.orientation, int64_t(61) * 33333333}; + jump.P_world_model.orientation = rotation(2, {0, 1, 0}); + CHECK(constellation_track_update(&track, &jump, true, &right.orientation, nullptr, nullptr) == + CONSTELLATION_TRACK_HOLD_JUMP); + std::printf("sequential ownership: raw publications=%u recovered at frame=%u / 60\n", raw_published, + recovered_at); +} + +TEST_CASE("Cold P3P uses the capture-time tilt without trusting the position") +{ + Scene s; + s.truth.orientation = multiply(rotation(0.4, {0, 1, 0}), rotation(0.65, {1, 0, 0})); + s.observe(0, {0, 1, 2}); + s.observe(1, {0}); + for (auto &o : s.observations) + for (int i = 0; i < o.num_blobs; i++) { + o.blobs[i].led_id = LED_INVALID_ID; + o.blobs[i].width = o.blobs[i].height = 2; + } + struct Context { + Scene *scene; + unsigned recovered = 0; + } context{&s}; + auto *model = t_constellation_search_model_new(&s.model); + auto *search = correspondence_search_new(&s.camera); + REQUIRE(model); + REQUIRE(search); + search->pose_candidate_userdata = &context; + search->pose_candidate_cb = [](void *userdata, const xrt_pose *pose, const pose_metrics *) { + auto &ctx = *static_cast(userdata); + constellation_constrained_result result{}; + if (constellation_constrained_verify_pose(&ctx.scene->model, ctx.scene->views.data(), 2, pose, + CONSTELLATION_CONSTRAINED_P3P, &ctx.scene->config, &result) && + distance(result.P_world_model.position, ctx.scene->truth.position) < 0.001 && + angle(result.P_world_model.orientation, ctx.scene->truth.orientation) < 0.01) + ctx.recovered++; + }; + correspondence_search_set_blobs(search, s.observations[0].blobs, 3); + const xrt_vec3 gravity = {0, 1, 0}; + xrt_vec3 camera_gravity = gravity; + auto flags = static_cast(CS_FLAG_SHALLOW_SEARCH | CS_FLAG_DEEP_SEARCH | + CS_FLAG_MATCH_GRAVITY | CS_FLAG_MATCH_ALL_BLOBS | CS_FLAG_JOINT_P3P); + // Only tilt is known. Arbitrary position and yaw must not gate cold acquisition. + xrt_pose seed = {multiply(rotation(1.7, gravity), s.truth.orientation), {42, -42, 42}}; + pose_metrics metrics{}; + correspondence_search_find_one_pose(search, model, flags, &seed, nullptr, nullptr, + &camera_gravity, 0.1f, &metrics); + CHECK(context.recovered > 0); + SECTION("An incompatible tilt still rejects the same geometry") { + context.recovered = 0; + seed = XRT_POSE_IDENTITY; + correspondence_search_find_one_pose(search, model, flags, &seed, nullptr, nullptr, + &camera_gravity, 0.1f, &metrics); + CHECK(context.recovered == 0); + } + correspondence_search_free(search); + t_constellation_search_model_free(model); +} + +TEST_CASE("Tracked ring with many LEDs keeps publishing when the prior search is truncated") +{ + /* r18 worn log: a tracked ring puts thousands of pairs on the prior edge list, beyond the + * 1000-trial cap, and the fast path rejected 1267 of 1429 tracked frames as uncertain. A + * truncated search must still verify the continuing identity, and never a wrong one. */ + Scenario sc; + sc.trusted = true; + sc.heading = true; + sc.min_keep = 8; + sc.max_keep = 11; + constellation_constrained_config config; + constellation_constrained_default_config(&config); + MonteCarlo capped = run_scenario(sc, 200, 0x5219, &config); + print("trusted-heading-default-cap", capped); + CHECK(capped.wrong == 0); + CHECK(capped.reasons[CONSTELLATION_CONSTRAINED_UNCERTAIN] == 0); + CHECK(capped.accepted >= 190); + + // Only the first few predicted pairs: the continuing identity is among them. + config.max_prior_pair_trials = 20; + MonteCarlo tight = run_scenario(sc, 200, 0x5219 + 1, &config); + print("trusted-heading-20-pairs", tight); + CHECK(tight.wrong == 0); + CHECK(tight.accepted >= 190); + + RingScene s(2); + s.choose_pose({-0.02f, 0.04f, 0.4f}, {4, 2}, 3); + REQUIRE(s.observe(0, 4) + s.observe(1, 2) >= 5); + s.config.max_prior_pair_trials = 1; + constellation_constrained_result result; + REQUIRE(constellation_constrained_search(&s.model, s.rig.views.data(), 2, &s.truth, true, true, &s.config, + &result)); + CHECK(result.budget_exhausted); + CHECK(s.identity(result)); +} diff --git a/tests/tests_rift_s_controller_capture.cpp b/tests/tests_rift_s_controller_capture.cpp new file mode 100644 index 000000000..bfeba7153 --- /dev/null +++ b/tests/tests_rift_s_controller_capture.cpp @@ -0,0 +1,39 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "rift_s_controller.h" +#include + +enum u_logging_level rift_s_log_level = U_LOGGING_ERROR; + +TEST_CASE("Rift S camera attitude bridges one radio scheduling gap with local gyro") +{ + rift_s_controller ctrl{}; + ctrl.attitude_history_count = 2; + xrt_vec3 axis = {.3f, .8f, -.4f}; + math_vec3_normalize(&axis); + xrt_quat orientation; + math_quat_from_angle_vector(.7f, &axis, &orientation); + math_quat_normalize(&orientation); + ctrl.attitude_history[0] = {1000000000, 2000000000, 2000000000, orientation, {.8f, -1.2f, .4f}, true}; + ctrl.attitude_history[1] = {1002000000, 2002000000, 2002000000, orientation, {.8f, -1.2f, .4f}, true}; + rift_s_attitude_sample result{}; + REQUIRE(rift_s_controller_get_attitude(&ctrl, 2007000000, &result)); + xrt_quat expected; + math_quat_integrate_velocity(&orientation, &ctrl.attitude_history[1].gyro, .005f, &expected); + CHECK(result.capture_ns == 2007000000); + CHECK(result.device_capture_ns == 1007000000); + CHECK(result.orientation.x == Catch::Approx(expected.x).margin(1e-7)); + CHECK(result.orientation.y == Catch::Approx(expected.y).margin(1e-7)); + CHECK(result.orientation.z == Catch::Approx(expected.z).margin(1e-7)); + CHECK(result.orientation.w == Catch::Approx(expected.w).margin(1e-7)); + CHECK(rift_s_controller_get_attitude(&ctrl, 2022000000, &result)); + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 2022000001, &result)); + CHECK(ctrl.diagnostics.last_attitude_reject == RIFT_S_ATTITUDE_TOO_NEW); + ctrl.attitude_history[0].capture_ns -= 20000000; + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 2007000000, &result)); + ctrl.attitude_history[0].capture_ns += 20000000; + ctrl.attitude_history[1].tilt_valid = false; + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 2007000000, &result)); + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 1999999999, &result)); +} diff --git a/tests/tests_rift_s_controller_geometry.cpp b/tests/tests_rift_s_controller_geometry.cpp new file mode 100644 index 000000000..57365742f --- /dev/null +++ b/tests/tests_rift_s_controller_geometry.cpp @@ -0,0 +1,344 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "internal/constrained_pose.h" +#include "internal/sample.h" +#include +#include +#include +#include "constellation_rift_s_data.hpp" + +namespace { +struct FactoryCamera +{ + std::array intrinsics; + std::array radial; + std::array tangential; + xrt_matrix_4x4 device_from_camera; +}; +// Factory camera calibration read from a Rift S headset. +const std::array factory = {{ + {{190.499924f, 190.499924f, 312.359253f, 233.292801f}, + {0.274657339f, 0.072651282f, -0.284994245f, 0.224762872f, -0.0842394754f, 0.0130622024f}, + {-0.000742965436f, -0.000313968281f}, + {{-0.000666916079f, 0.936471105f, -0.350744158f, 0.0f, 0.999993384f, -0.000631041592f, -0.00358627178f, 0.0f, + -0.00357977417f, -0.350744218f, -0.936464429f, 0.0f, -0.055822663f, -0.0286783446f, -0.0698858649f, 1.0f}}}, + {{190.177917f, 190.177917f, 315.850128f, 237.907181f}, + {0.279467911f, 0.0535824969f, -0.255054951f, 0.202277347f, -0.0756442547f, 0.0117395986f}, + {4.58915711e-05f, -0.000414691196f}, + {{-0.00550019974f, 0.936006129f, -0.351940721f, 0.0f, 0.999975204f, 0.00669397181f, 0.00217518141f, 0.0f, + 0.00439186441f, -0.351920038f, -0.936019778f, 0.0f, 0.0548296794f, -0.0278966054f, -0.0701900721f, 1.0f}}}, + {{190.294937f, 190.294937f, 318.57016f, 233.739182f}, + {0.268673301f, 0.0967906564f, -0.325616896f, 0.257986337f, -0.0972961113f, 0.0150451222f}, + {0.000216994711f, 0.000161503413f}, + {{-0.369307697f, 0.877442598f, -0.306114942f, 0.0f, -0.0468660891f, -0.346567303f, -0.936853588f, 0.0f, + -0.928124666f, -0.33164084f, 0.169112265f, 0.0f, -0.0773649067f, -0.0278907306f, -0.0490732379f, 1.0f}}}, + {{190.489212f, 190.489212f, 319.239746f, 239.368988f}, + {0.276072472f, 0.0793345198f, -0.300511897f, 0.236862913f, -0.0880616605f, 0.0134672392f}, + {0.000866621442f, -9.05699198e-05f}, + {{0.372085989f, 0.87367934f, -0.313426882f, 0.0f, -0.0441717803f, 0.353956819f, 0.934218109f, 0.0f, 0.927146614f, + -0.333764851f, 0.170294344f, 0.0f, 0.0775006264f, -0.027320113f, -0.0488821045f, 1.0f}}}, + {{189.886932f, 189.886932f, 315.196442f, 236.638504f}, + {0.275883079f, 0.0488088988f, -0.227129221f, 0.164120957f, -0.0545484312f, 0.00755312294f}, + {0.00021202871f, -0.000509813079f}, + {{-0.99999702f, -0.000720949087f, -0.00232819817f, 0.0f, -0.00232536392f, -0.00390743697f, 0.999989688f, 0.0f, + -0.000730038912f, 0.999992132f, 0.00390574872f, 0.0f, -0.000778796442f, 0.0459865034f, -0.0662369207f, 1.0f}}}, +}}; +camera_model +calibrated_camera(unsigned index) +{ + const auto &f = factory[index]; + camera_model c{}; + c.width = 640; + c.height = 480; + c.calib.fx = f.intrinsics[0]; + c.calib.fy = f.intrinsics[1]; + c.calib.cx = f.intrinsics[2]; + c.calib.cy = f.intrinsics[3]; + c.fisheye62_valid = true; + std::copy(f.radial.begin(), f.radial.end(), c.fisheye62_radial); + c.fisheye62_p1 = f.tangential[0]; + c.fisheye62_p2 = f.tangential[1]; + return c; +} +// Project LED fixtures forward without calling the production inverse. +xrt_vec2 +factory_project(const FactoryCamera &c, xrt_vec3 p) +{ + double r = std::hypot(p.x, p.y), theta = std::atan2(r, p.z); + double a = r > 1e-12 ? theta * p.x / r : 0, b = r > 1e-12 ? theta * p.y / r : 0; + double polynomial = 1; + for (unsigned i = 0; i < 6; ++i) + polynomial += c.radial[i] * std::pow(theta, 2 * i + 2); + double p1 = c.tangential[0], p2 = c.tangential[1]; + double x = a * polynomial, y = b * polynomial; + double norm2 = x * x + y * y, dot = p1 * x + p2 * y; + return {float(c.intrinsics[0] * (x + 2 * x * dot + p1 * norm2) + c.intrinsics[2]), + float(c.intrinsics[1] * (y + 2 * y * dot + p2 * norm2) + c.intrinsics[3])}; +} +double +error(xrt_vec3 a, xrt_vec3 b) +{ + return std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2) + std::pow(a.z - b.z, 2)); +} +} // namespace + +TEST_CASE("Rift S Fisheye62 matches calibrated forward pixel references") +{ + // Fixed forward references cover all five calibrations and both tangential axes. + const std::array angular = {{{0.8f, 0.6f}, {-1.1f, 0.4f}, {0.2f, -0.7f}}}; + const std::array, 5> pixels = {{ + {{{497.100364f, 371.917144f}, {49.367862f, 328.687290f}, {355.628758f, 81.468439f}}}, + {{{500.828640f, 376.514696f}, {53.946196f, 332.982240f}, {359.179827f, 86.220055f}}}, + {{{503.808216f, 372.667374f}, {56.865364f, 329.002682f}, {361.892456f, 82.231382f}}}, + {{{505.258047f, 378.673729f}, {57.725275f, 334.557148f}, {362.776597f, 87.375536f}}}, + {{{499.717526f, 374.841928f}, {54.394805f, 331.299469f}, {358.441628f, 85.310572f}}}, + }}; + for (unsigned k = 0; k < factory.size(); ++k) { + auto c = calibrated_camera(k); + for (unsigned i = 0; i < angular.size(); ++i) { + CAPTURE(k, i); + const double theta = std::hypot(angular[i].x, angular[i].y); + const double scale = std::sin(theta) / theta; + const xrt_vec3 ray = {float(angular[i].x * scale), float(angular[i].y * scale), + float(std::cos(theta))}; + xrt_vec2 pixel; + REQUIRE(camera_model_project(&c, ray.x, ray.y, ray.z, &pixel.x, &pixel.y)); + CHECK(pixel.x == Catch::Approx(pixels[k][i].x).epsilon(0).margin(0.0001)); + CHECK(pixel.y == Catch::Approx(pixels[k][i].y).epsilon(0).margin(0.0001)); + xrt_vec3 recovered; + REQUIRE(camera_model_unproject(&c, pixels[k][i].x, pixels[k][i].y, &recovered.x, &recovered.y, + &recovered.z)); + CHECK(error(recovered, ray) < 1e-6); + } + } +} + +TEST_CASE("Rift S factory Fisheye62 pixel bearings round trip across all five sensors") +{ + for (unsigned k = 0; k < factory.size(); ++k) { + auto c = calibrated_camera(k); + for (unsigned y = 0; y < 480; y += 16) + for (unsigned x = 0; x < 640; x += 16) { + xrt_vec3 ray; + REQUIRE(camera_model_unproject(&c, x, y, &ray.x, &ray.y, &ray.z)); + auto pixel = factory_project(factory[k], ray); + REQUIRE(std::hypot(pixel.x - x, pixel.y - y) < 0.0002); + } + } +} + +TEST_CASE("Rift S known LED geometry recovers offset and unit lateral gain through native calibration") +{ + auto leds = rift_s_ring_leds; + t_constellation_led_model model{}; + model.leds = leds.data(); + model.num_leds = leds.size(); + constellation_constrained_config config; + constellation_constrained_default_config(&config); + config.time_budget_us = 2000000; + // Isolate metric calibration: geometry and LED identities are known, no acquisition policy. + config.minimum_tracking_inliers = 3; + config.minimum_tracking_distinct_leds = 3; + config.minimum_tilt_distinct_leds = 256; + config.ambiguity_chi_square = 0.00001; + double max_error = 0, min_gain = 2, max_gain = 0; + unsigned recovered = 0; + for (unsigned k = 0; k < factory.size(); ++k) { + auto c = calibrated_camera(k); + xrt_pose world_camera; + math_pose_from_isometry(&factory[k].device_from_camera, &world_camera); + // Native camera rays transformed by the actual factory extrinsic into device/world coordinates. + constellation_constrained_view view{}; + view.calib = &c; + view.P_world_cam = world_camera; + math_pose_invert(&world_camera, &view.P_cam_world); + blobservation observation{}; + view.observation = &observation; + xrt_vec3 previous_truth{}, previous_recovered{}; + for (int step = -6; step <= 6; ++step) { + xrt_pose camera_pose = XRT_POSE_IDENTITY; + camera_pose.position = {step * .11f, -.04f, .45f}; + const xrt_vec3 axis = {1, 0, 0}; + math_quat_from_angle_vector(3.14159265f, &axis, &camera_pose.orientation); + xrt_pose truth; + math_pose_transform(&world_camera, &camera_pose, &truth); + observation = {}; + for (const auto &led : leds) { + xrt_vec3 local, normal; + math_pose_transform_point(&camera_pose, &led.pos, &local); + math_quat_rotate_vec3(&camera_pose.orientation, &led.dir, &normal); + xrt_vec3 to_camera = {-local.x, -local.y, -local.z}; + math_vec3_normalize(&to_camera); + if (normal.x * to_camera.x + normal.y * to_camera.y + normal.z * to_camera.z < + std::cos(LED_ANGLE * M_PI / 180)) + continue; + auto pixel = factory_project(factory[k], local); + if (pixel.x < 0 || pixel.x >= 640 || pixel.y < 0 || pixel.y >= 480) + continue; + auto &b = observation.blobs[observation.num_blobs++]; + b.x = pixel.x; + b.y = pixel.y; + } + if (observation.num_blobs < 4) + continue; + // Translation recovery from an imperfect retained pose exercises the actual multi-LED fitter. + xrt_pose prior = truth; + prior.position.x += .001f; + prior.position.y -= .001f; + constellation_constrained_result result{}; + REQUIRE(constellation_constrained_verify_pose( + &model, &view, 1, &prior, CONSTELLATION_CONSTRAINED_PRIOR, &config, &result)); + max_error = std::max(max_error, error(truth.position, result.P_world_model.position)); + if (step > -6) { + double truth_motion = error(truth.position, previous_truth); + if (truth_motion > .01) { + double gain = + error(result.P_world_model.position, previous_recovered) / truth_motion; + min_gain = std::min(min_gain, gain); + max_gain = std::max(max_gain, gain); + } + } + previous_truth = truth.position; + previous_recovered = result.P_world_model.position; + ++recovered; + } + } + INFO("recovered=" << recovered << " worst position m=" << max_error << " gain=" << min_gain << ".." + << max_gain); + CHECK(recovered >= 45); + CHECK(max_error < 0.00002); + CHECK(min_gain > .9999); + CHECK(max_gain < 1.0001); +} + +TEST_CASE("Rift S ring recovers depth with unit gain from 0.3 to 0.8 m through cold and tracked search") +{ + /* The r18 report: controllers drawn farther than the hands. Project the recorded ring through + * each factory calibration and extrinsic straight ahead of that camera, then recover it with the + * search the tracker runs: cold (gravity only, no position) and tracked (trusted prior 2 cm off, + * known heading, 3 ms fast-path budget). Depth must come back unbiased and with unit gain. */ + auto leds = rift_s_ring_leds; + t_constellation_led_model model{}; + model.id = 3; + model.leds = leds.data(); + model.num_leds = leds.size(); + double worst = 0, min_gain = 2, max_gain = 0; + unsigned cold = 0, tracked = 0; + for (unsigned k = 0; k < factory.size(); ++k) { + auto c = calibrated_camera(k); + xrt_pose world_camera; + math_pose_from_isometry(&factory[k].device_from_camera, &world_camera); + constellation_constrained_view view{}; + view.calib = &c; + view.P_world_cam = world_camera; + math_pose_invert(&world_camera, &view.P_cam_world); + blobservation observation{}; + view.observation = &observation; + for (bool trusted : {false, true}) { + double previous_depth = 0, previous_recovered = 0; + for (float depth = 0.3f; depth <= 0.801f; depth += 0.1f) { + CAPTURE(k, trusted, depth); + xrt_pose camera_pose = XRT_POSE_IDENTITY; + camera_pose.position = {0.01f, -0.04f, depth}; + const xrt_vec3 axis = {1, 0, 0}; + math_quat_from_angle_vector(3.14159265f, &axis, &camera_pose.orientation); + xrt_pose truth; + math_pose_transform(&world_camera, &camera_pose, &truth); + observation = {}; + for (const auto &led : leds) { + xrt_vec3 local, normal; + math_pose_transform_point(&camera_pose, &led.pos, &local); + math_quat_rotate_vec3(&camera_pose.orientation, &led.dir, &normal); + xrt_vec3 to_camera = {-local.x, -local.y, -local.z}; + math_vec3_normalize(&to_camera); + if (normal.x * to_camera.x + normal.y * to_camera.y + normal.z * to_camera.z < + std::cos(LED_ANGLE * M_PI / 180)) + continue; + auto pixel = factory_project(factory[k], local); + auto &b = observation.blobs[observation.num_blobs++]; + b = {}; + b.x = pixel.x; + b.y = pixel.y; + b.width = b.height = 3; + b.led_id = LED_INVALID_ID; + } + REQUIRE(observation.num_blobs >= 5); + constellation_constrained_config config; + constellation_constrained_default_config(&config); + xrt_pose prior = truth; + if (trusted) { + config.time_budget_us = 3000; +#if defined(__SANITIZE_ADDRESS__) || defined(__SANITIZE_THREAD__) + config.time_budget_us = 200000; +#endif + prior.position.x += 0.02f; + } else { + prior.position = {42, 42, 42}; + } + constellation_constrained_result result{}; + REQUIRE(constellation_constrained_search(&model, &view, 1, &prior, trusted, trusted, &config, + &result)); + (trusted ? tracked : cold)++; + worst = std::max(worst, error(truth.position, result.P_world_model.position)); + // Depth along this camera's axis, recovered and true. + xrt_vec3 in_camera; + math_pose_transform_point(&view.P_cam_world, &result.P_world_model.position, &in_camera); + if (previous_depth > 0) { + double gain = (in_camera.z - previous_recovered) / (depth - previous_depth); + min_gain = std::min(min_gain, gain); + max_gain = std::max(max_gain, gain); + } + previous_depth = depth; + previous_recovered = in_camera.z; + } + } + } + INFO("cold=" << cold << " tracked=" << tracked << " worst m=" << worst << " depth gain=" << min_gain << ".." + << max_gain); + CHECK(cold == 30); + CHECK(tracked == 30); + CHECK(worst < 0.0005); + CHECK(min_gain > 0.999); + CHECK(max_gain < 1.001); +} + +TEST_CASE("Rift S wide-angle KB4 bearing converges at the sensor diagonal") +{ + t_camera_model_params c{}; + c.model = T_DISTORTION_FISHEYE_KB4; + c.fx = c.fy = rift_s_ring_intrinsics[0]; + c.cx = rift_s_ring_intrinsics[2]; + c.cy = rift_s_ring_intrinsics[3]; + c.fisheye = {rift_s_ring_kb4[0], rift_s_ring_kb4[1], rift_s_ring_kb4[2], rift_s_ring_kb4[3]}; + const double theta = 1.55, bearing = .65; + xrt_vec3 truth = {float(std::sin(theta) * std::cos(bearing)), float(std::sin(theta) * std::sin(bearing)), + float(std::cos(theta))}; + float u, v; + xrt_vec3 recovered; + REQUIRE(t_camera_models_project(&c, truth.x, truth.y, truth.z, &u, &v)); + REQUIRE(t_camera_models_unproject(&c, u, v, &recovered.x, &recovered.y, &recovered.z)); + CHECK(error(truth, recovered) < .000001); +} + +TEST_CASE("Native camera failures leave explicit invalid coordinates") +{ + auto camera = calibrated_camera(0); + float x = 7, y = 8, z = 9; + CHECK_FALSE(camera_model_unproject(&camera, NAN, 240, &x, &y, &z)); + CHECK(std::isnan(x)); + CHECK(std::isnan(y)); + CHECK(std::isnan(z)); + CHECK_FALSE(camera_model_undistort(&camera, NAN, 240, &x, &y)); + CHECK(std::isnan(x)); + CHECK(std::isnan(y)); + CHECK_FALSE(camera_model_project(&camera, 0, 0, -1, &x, &y)); + CHECK(std::isnan(x)); + CHECK(std::isnan(y)); + camera.fisheye62_valid = false; + camera.calib.model = T_DISTORTION_FISHEYE_KB4; + CHECK_FALSE(camera_model_unproject(&camera, NAN, 240, &x, &y, &z)); + CHECK(std::isnan(x)); + CHECK(std::isnan(y)); + CHECK(std::isnan(z)); +} diff --git a/tests/tests_rift_s_metadata.cpp b/tests/tests_rift_s_metadata.cpp new file mode 100644 index 000000000..a61333d3d --- /dev/null +++ b/tests/tests_rift_s_metadata.cpp @@ -0,0 +1,268 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "rift_s_camera_metadata.h" +extern "C" { +#include "rift_s_firmware.h" +#include "rift_s_protocol.h" +#include "util/u_logging.h" +} +#include +#include +#include + +enum u_logging_level rift_s_log_level = U_LOGGING_ERROR; + +TEST_CASE("Rift S controller register decoding validates status and actual payload length") +{ + // 0x32 callback bytes, excluding HID report ID/sequence/busy: status, length, payload. + std::array response = {0, 0, 0, 0, 16, 0x00, 0x7d, 0xa0, 0x0f, 0xf4, 0x01, + 0xf4, 0x01, 0x00, 0x00, 0x80, 0x3a, 0xff, 0xff, 0xf9, 0x3d}; + rift_s_controller_config config{}; + REQUIRE(rift_s_decode_controller_config(response.data(), response.size(), &config)); + CHECK(config.accel_limit == 32000); + CHECK(config.gyro_limit == 4000); + CHECK(config.accel_hz == 500); + CHECK(config.gyro_hz == 500); + CHECK(config.accel_scale == Catch::Approx(1.0 / 1024)); + CHECK(config.gyro_scale == Catch::Approx(0.1220703)); + for (size_t len = 0; len < response.size(); len++) + CHECK_FALSE(rift_s_decode_controller_config(response.data(), len, &config)); + for (unsigned i = 0; i < 4; i++) { + auto failed = response; + failed[i] = 1; + CHECK_FALSE(rift_s_decode_controller_config(failed.data(), failed.size(), &config)); + } + response[4] = 32; // Declared length cannot exceed the received bytes. + CHECK_FALSE(rift_s_decode_controller_config(response.data(), response.size(), &config)); + response[4] = 16; + std::fill(response.begin() + 13, response.begin() + 17, 0); // Zero accel scale. + CHECK_FALSE(rift_s_decode_controller_config(response.data(), response.size(), &config)); + response[15] = 0xc0; + response[16] = 0x7f; // NaN accel scale. + CHECK_FALSE(rift_s_decode_controller_config(response.data(), response.size(), &config)); + + response = {}; + response[4] = 16; + std::memcpy(response.data() + 5, "LSM6DSL", 7); + std::array descriptor{}; + REQUIRE(rift_s_decode_controller_imu_descriptor(response.data(), response.size(), descriptor.data())); + CHECK(std::strcmp(descriptor.data(), "LSM6DSL") == 0); + for (size_t len = 0; len < response.size(); len++) + CHECK_FALSE(rift_s_decode_controller_imu_descriptor(response.data(), len, descriptor.data())); + response[0] = 1; + CHECK_FALSE(rift_s_decode_controller_imu_descriptor(response.data(), response.size(), descriptor.data())); + response[0] = 0; + response[4] = 15; + CHECK_FALSE(rift_s_decode_controller_imu_descriptor(response.data(), response.size(), descriptor.data())); + response[4] = 16; + response[5] = 0xff; + CHECK_FALSE(rift_s_decode_controller_imu_descriptor(response.data(), response.size(), descriptor.data())); + response[5] = 0; + CHECK_FALSE(rift_s_decode_controller_imu_descriptor(response.data(), response.size(), descriptor.data())); + std::memcpy(response.data() + 5, "0123456789abcdef", 16); + REQUIRE(rift_s_decode_controller_imu_descriptor(response.data(), response.size(), descriptor.data())); + CHECK(std::strcmp(descriptor.data(), "0123456789abcdef") == 0); +} + +TEST_CASE("Rift S LSM6DSL correction scales factory gyro matrix and sensor bias before applying them") +{ + rift_s_tracked_imu_calibration factory{}; + factory.num_values = 12; + factory.matrix = {{0, 2, 0, -3, 0, 0, 0, 0, 4}}; + factory.offset = {0.1f, -0.2f, 0.3f}; + constexpr double factor = 0.8714285714285714; + auto gyro = rift_s_controller_gyro_calibration_for_imu(&factory, "LSM6DSL"); + CHECK(gyro.num_values == 12); + for (unsigned i = 0; i < 9; i++) + CHECK(gyro.matrix.v[i] == (float)(factory.matrix.v[i] * factor)); + CHECK(gyro.offset.x == (float)(factory.offset.x * factor)); + CHECK(gyro.offset.y == (float)(factory.offset.y * factor)); + CHECK(gyro.offset.z == (float)(factory.offset.z * factor)); + xrt_vec3 sample{1, 2, 3}; + xrt_vec3 unbiased{sample.x - gyro.offset.x, sample.y - gyro.offset.y, sample.z - gyro.offset.z}; + xrt_vec3 corrected{}; + math_matrix_3x3_transform_vec3(&gyro.matrix, &unbiased, &corrected); + CHECK(corrected.x == Catch::Approx(2 * factor * (2 + 0.2 * factor))); + CHECK(corrected.y == Catch::Approx(-3 * factor * (1 - 0.1 * factor))); + CHECK(corrected.z == Catch::Approx(4 * factor * (3 - 0.3 * factor))); + // Re-derivation always starts from the factory data, so callbacks cannot compound the factor. + auto again = rift_s_controller_gyro_calibration_for_imu(&factory, "LSM6DSL"); + CHECK(std::memcmp(&again, &gyro, sizeof(gyro)) == 0); + for (const char *name : {"", "UNKNOWN", "lsm6dsl", "LSM6DSL-rev2"}) { + auto unchanged = rift_s_controller_gyro_calibration_for_imu(&factory, name); + CHECK(std::memcmp(&unchanged, &factory, sizeof(factory)) == 0); + } +} + +static void +put_le(uint8_t *dst, uint64_t value, size_t count) +{ + for (size_t i = 0; i < count; i++) + dst[i] = (value >> (8 * i)) & 0xff; +} + +static std::array +metadata() +{ + std::array raw{}; + raw[0] = 0x86; + put_le(raw.data() + 1, 0xabcd, 2); + put_le(raw.data() + 3, 65535, 2); + put_le(raw.data() + 16, 0x0102030405060708ull, 8); + put_le(raw.data() + 24, 123456, 4); + for (int i = 0; i < 5; i++) { + put_le(raw.data() + 28 + 2 * i, 38 + i, 2); + raw[40 + i] = 64 + 16 * i; + } + put_le(raw.data() + 46, rift_s_metadata_crc(raw.data() + 16, 30), 2); + put_le(raw.data() + 48, 0xface, 2); + return raw; +} + +TEST_CASE("Rift S metadata CRC checks the defined payload and retains kind and camera values") +{ + const uint8_t check[] = {'1', '2', '3', '4', '5', '6', '7', '8', '9'}; + CHECK(rift_s_metadata_crc(check, sizeof(check)) == 0x29b1); + auto raw = metadata(); + rift_s_frame_metadata out{}; + REQUIRE(rift_s_metadata_decode(raw.data(), &out) == RIFT_S_METADATA_OK); + CHECK(out.frame_type == 0x86); + CHECK(out.frame_ctr == 65535); + CHECK(out.frame_ts_us == 0x0102030405060708ull); + CHECK(out.frame_ctr2 == 123456); + for (int i = 0; i < 5; i++) { + CHECK(out.exposure[i] == 38 + i); + CHECK(out.gain[i] == 64 + 16 * i); + } + for (size_t i = 16; i < 48; i++) { + for (int bit = 0; bit < 8; bit++) { + auto changed = raw; + changed[i] ^= 1u << bit; + CHECK(rift_s_metadata_decode(changed.data(), &out) == RIFT_S_METADATA_BAD_CRC); + } + } + raw[0] = 0x06; // Frame kind and exposure counter are outside the firmware CRC payload. + put_le(raw.data() + 3, 0, 2); + REQUIRE(rift_s_metadata_decode(raw.data(), &out) == RIFT_S_METADATA_OK); + CHECK(out.frame_type == 0x06); + CHECK(out.frame_ctr == 0); + raw[1] ^= 1; + CHECK(rift_s_metadata_decode(raw.data(), &out) == RIFT_S_METADATA_BAD_MAGIC); +} + +TEST_CASE("Rift S top-row extraction uses stride and rejects incomplete transport data") +{ + const size_t stride = RIFT_S_METADATA_WIDTH + 128; + std::vector pixels(stride * 8); + auto raw = metadata(); + for (size_t i = 0; i < 50 * 8; i++) { + pixels[4 * stride + 4 + 8 * i] = (raw[i / 8] & (1u << (7 - i % 8))) ? 255 : 0; + } + rift_s_frame_metadata out{}; + REQUIRE(rift_s_metadata_extract(pixels.data(), RIFT_S_METADATA_WIDTH, 8, stride, pixels.size(), &out) == + RIFT_S_METADATA_OK); + CHECK(out.frame_ts_us == 0x0102030405060708ull); + CHECK(rift_s_metadata_extract(pixels.data(), RIFT_S_METADATA_WIDTH, 8, stride, 4 * stride, &out) == + RIFT_S_METADATA_BAD_LAYOUT); + CHECK(rift_s_metadata_extract(pixels.data(), RIFT_S_METADATA_WIDTH, 7, stride, pixels.size(), &out) == + RIFT_S_METADATA_BAD_LAYOUT); + CHECK(rift_s_metadata_extract(pixels.data(), RIFT_S_METADATA_WIDTH, 8, RIFT_S_METADATA_WIDTH - 1, pixels.size(), + &out) == RIFT_S_METADATA_BAD_LAYOUT); +} + +TEST_CASE("Rift S exposure counter accepts wrap and separates gaps from resets") +{ + uint16_t missing = 99; + CHECK(rift_s_metadata_counter_delta(65535, 0, &missing)); + CHECK(missing == 0); + CHECK(rift_s_metadata_counter_delta(65534, 2, &missing)); + CHECK(missing == 3); + CHECK_FALSE(rift_s_metadata_counter_delta(42, 42, &missing)); + CHECK(missing == 0); + CHECK_FALSE(rift_s_metadata_counter_delta(42, 41, &missing)); + CHECK_FALSE(rift_s_metadata_counter_delta(0, 32768, &missing)); +} + +TEST_CASE("Rift S firmware keeps numbered LED identities and each lensing model") +{ + // Reverse object insertion order to check identities rather than JSON traversal order. + char json[] = R"({"TrackedObject":{ + "FlsVersion":"1.0.10", "ImuPosition":[0.005,-0.007,0.020], + "AccCalibration":[0,1,0,-1,0,0,0,0,1,0.1,-0.2,0.3], + "GyroCalibration":[1,0,0,0,1,0,0,0,1,0.01,0.02,0.03], + "ModelPoints":{"Point1":[0.02,0.03,0.04,0,1,0,85,80,0], + "Point0":[-0.01,0.02,0.03,1,0,0,85,80,0]}, + "Lensing":{"Model1":[4,11,12,13,14],"Model0":[4,1,2,3,4]}}, + "acc_m":[1,0,0,0,0,-1,0,1,0], "acc_b":[0.1,0.2,0.3], + "gyro_m":[1,0,0,0,0,-1,0,1,0], "gyro_b":[0.01,0.02,0.03]})"; + rift_s_controller_imu_calibration c{}; + REQUIRE(rift_s_controller_parse_imu_calibration(json, &c) == 0); + CHECK(c.num_leds == 2); + CHECK(c.leds[0].pos.x == Catch::Approx(-0.01)); + CHECK(c.leds[0].dir.x == 1); + CHECK(c.leds[1].pos.x == Catch::Approx(0.02)); + CHECK(c.leds[1].dir.y == 1); + CHECK(c.imu_position.x == Catch::Approx(0.005)); + // TrackedObject IMU calibration: row-major 3x3 (elements 0..8) and offset (9..11). + CHECK(c.accel_calibration.num_values == 12); + CHECK(c.accel_calibration.matrix.v[1] == 1); + CHECK(c.accel_calibration.matrix.v[3] == -1); + CHECK(c.accel_calibration.matrix.v[8] == 1); + CHECK(c.accel_calibration.offset.x == Catch::Approx(0.1)); + CHECK(c.accel_calibration.offset.y == Catch::Approx(-0.2)); + CHECK(c.accel_calibration.offset.z == Catch::Approx(0.3)); + CHECK(c.gyro_calibration.offset.z == Catch::Approx(0.03)); + CHECK(c.num_lensing_models == 2); + for (int i = 0; i < 4; i++) { + CHECK(c.lensing_models[0].points[i] == i + 1); + CHECK(c.lensing_models[1].points[i] == i + 11); + } + xrt_vec3 raw_accel{0, 0, 1}, corrected{}; + math_matrix_3x3_transform_vec3(&c.accel.rectification, &raw_accel, &corrected); + CHECK(corrected.x == 0); + CHECK(corrected.y == -1); + CHECK(corrected.z == 0); + rift_s_controller_free_imu_calibration(&c); + CHECK(c.leds == nullptr); + CHECK(c.lensing_models == nullptr); +} + +TEST_CASE("Rift S controller IR LED register matches the Windows radio requests") +{ + // Windows sends 0x3d bytes: report id, LE64 device id, then the request. + STATIC_REQUIRE(sizeof(rift_s_hmd_radio_command_t) == 0x3d); + + std::array read{}; + REQUIRE(rift_s_encode_controller_irled_read(read.data()) == 4); + CHECK(read == std::array{0x28, 0x00, 0xe8, 0x03}); + + // The Windows runtime wrote 19/40000 to both controllers. + rift_s_controller_irled_config cfg{40000, 19}; + std::array write{}; + REQUIRE(rift_s_encode_controller_irled_write(&cfg, write.data()) == 12); + CHECK(write == std::array{0x28, 8, 0xe8, 0x03, 0x40, 0x9c, 0x00, 0x00, 0x13, 0x00, 0x00, 0x00}); + + // Read reply: LE32 status, length, LE32 period_us, LE32 ontime_us. + std::array reply = {0, 0, 0, 0, 8, 0x05, 0x82, 0x00, 0x00, 0x13, 0x00, 0x00, 0x00}; + rift_s_controller_irled_config got{}; + REQUIRE(rift_s_decode_controller_irled_config(reply.data(), reply.size(), &got)); + CHECK(got.period_us == 33285); + CHECK(got.ontime_us == 19); + for (size_t len = 0; len < reply.size(); len++) + CHECK_FALSE(rift_s_decode_controller_irled_config(reply.data(), len, &got)); + reply[4] = 7; + CHECK_FALSE(rift_s_decode_controller_irled_config(reply.data(), reply.size(), &got)); + reply[4] = 8; + reply[1] = 1; + CHECK_FALSE(rift_s_decode_controller_irled_config(reply.data(), reply.size(), &got)); + + uint32_t status = 0xffffffff; + std::array ok = {0, 0, 0, 0}; + CHECK(rift_s_controller_write_succeeded(ok.data(), ok.size(), &status)); + CHECK(status == 0); + std::array bad = {0x05, 0, 0, 0}; + CHECK_FALSE(rift_s_controller_write_succeeded(bad.data(), bad.size(), &status)); + CHECK(status == 5); + CHECK_FALSE(rift_s_controller_write_succeeded(ok.data(), 3, &status)); +} diff --git a/tests/tests_rift_s_optics.cpp b/tests/tests_rift_s_optics.cpp new file mode 100644 index 000000000..5d6041dc0 --- /dev/null +++ b/tests/tests_rift_s_optics.cpp @@ -0,0 +1,195 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 + +#include "catch_amalgamated.hpp" +#include "../src/xrt/state_trackers/steamvr_drv/ovrd_display.hpp" +extern "C" { +#include "rift_s/rift_s_optics.h" +} + +#include +#include + +namespace { + +constexpr float default_ipd_m = 0.0635f; +xrt_uv_triplet +sample(const rift_s_optics &optics, float u, float v) +{ + xrt_uv_triplet out{}; + rift_s_optics_compute(&optics, u, v, &out); + return out; +} + +rift_s_optics +make_optics(uint32_t eye, float ipd_m = default_ipd_m) +{ + rift_s_optics optics{}; + REQUIRE(rift_s_optics_init(&optics, eye, ipd_m, 1, 1)); + return optics; +} + +// Vertices of the Oculus runtime's left-eye distortion mesh (IPD 63.5 mm) inside the FOV: +// tangent-space position and where each channel lands in left-eye view pixels. +struct Vertex +{ + double tan_x, tan_y; + std::array r, g, b; +}; + +const Vertex oculus_vertices[] = { + {-0.95006, -0.98320, {111.19, 205.81}, {114.94, 209.70}, {116.69, 211.50}}, + {-0.95004, -0.27761, {13.75, 546.34}, {19.12, 547.91}, {21.65, 548.65}}, + {-0.95007, 0.42799, {28.26, 981.18}, {33.36, 978.88}, {35.73, 977.81}}, + {-0.95000, 1.13350, {136.12, 1283.08}, {139.43, 1279.13}, {140.89, 1277.39}}, + {-0.45762, -0.98331, {334.43, 132.08}, {336.78, 137.12}, {337.87, 139.47}}, + {-0.45764, -0.27765, {262.28, 510.22}, {265.27, 512.04}, {266.88, 513.02}}, + {-0.45756, 0.42798, {273.28, 1033.12}, {276.31, 1030.28}, {277.87, 1028.83}}, + {-0.45747, 1.13332, {351.87, 1354.64}, {353.93, 1349.53}, {354.89, 1347.15}}, + {0.03492, -0.98312, {629.95, 103.28}, {629.75, 108.86}, {629.66, 111.49}}, + {0.03492, -0.27760, {636.22, 495.96}, {636.03, 497.53}, {635.91, 498.49}}, + {0.03492, 0.42797, {635.28, 1053.85}, {635.06, 1051.16}, {634.94, 1049.66}}, + {0.03492, 1.13354, {628.43, 1381.96}, {628.26, 1376.39}, {628.18, 1373.79}}, + {0.52741, -0.98320, {918.76, 140.76}, {916.10, 145.72}, {914.88, 147.99}}, + {0.52739, -0.27760, {998.28, 514.59}, {994.78, 516.43}, {992.96, 517.39}}, + {0.52738, 0.42795, {986.10, 1026.78}, {982.62, 1023.96}, {980.86, 1022.53}}, + {0.52736, 1.13347, {899.41, 1346.23}, {897.12, 1341.33}, {896.12, 1339.16}}, + {1.01986, -0.98316, {1129.41, 217.39}, {1125.55, 221.11}, {1123.80, 222.80}}, + {1.01998, -0.27763, {1225.99, 551.80}, {1220.62, 553.26}, {1218.11, 553.94}}, + {1.02003, 0.42803, {1211.68, 973.31}, {1206.53, 971.14}, {1204.12, 970.13}}, + {1.01982, 1.13348, {1104.16, 1271.41}, {1100.83, 1267.71}, {1099.33, 1266.04}}, +}; + +} // namespace + +TEST_CASE("compute reproduces the Oculus runtime mesh") +{ + const auto optics = make_optics(0); + const double width = optics.tan_left + optics.tan_right; + const double height = optics.tan_up + optics.tan_down; + for (const Vertex &vertex : oculus_vertices) { + const double expected_x = (vertex.tan_x + optics.tan_left) / width; + const double expected_y = (vertex.tan_y + optics.tan_up) / height; + for (const auto *px : {&vertex.r, &vertex.g, &vertex.b}) { + const bool is_r = px == &vertex.r, is_g = px == &vertex.g; + auto pick = [&](const xrt_uv_triplet &uv) { return is_r ? uv.r : is_g ? uv.g : uv.b; }; + const float u = (float)((*px)[0] / 1280.0), v = (float)((*px)[1] / 1440.0); + const xrt_vec2 got = pick(sample(optics, u, v)); + // One pixel outward along the radius, in tangent units: the local px -> tan scale. + const double dx = (*px)[0] - optics.lens_center_px.x, dy = (*px)[1] - optics.lens_center_px.y; + const double r = std::hypot(dx, dy); + const xrt_vec2 step = pick(sample(optics, (float)(((*px)[0] + dx / r) / 1280.0), + (float)(((*px)[1] + dy / r) / 1440.0))); + const double one_px = + std::hypot((step.x - got.x) * width, (step.y - got.y) * height); + const double error = std::hypot((got.x - expected_x) * width, (got.y - expected_y) * height); + CHECK(error < one_px); + } + } +} + +TEST_CASE("lens centre maps to tangent zero") +{ + for (uint32_t eye = 0; eye < 2; eye++) { + for (float ipd_m : {0.0535f, default_ipd_m, 0.068f, 0.0735f}) { + const auto optics = make_optics(eye, ipd_m); + const auto uv = sample(optics, optics.lens_center_px.x / 1280.0f, optics.lens_center_px.y / 1440.0f); + const float x = optics.tan_left / (optics.tan_left + optics.tan_right); + const float y = optics.tan_up / (optics.tan_up + optics.tan_down); + for (const xrt_vec2 &channel : {uv.r, uv.g, uv.b}) { + CHECK(channel.x == Catch::Approx(x).margin(1e-6)); + CHECK(channel.y == Catch::Approx(y).margin(1e-6)); + } + } + } +} + +TEST_CASE("right eye mirrors the left eye") +{ + const auto left = make_optics(0, 0.068f); + const auto right = make_optics(1, 0.068f); + CHECK(right.lens_center_px.x == Catch::Approx(1280.0f - left.lens_center_px.x)); + for (const Vertex &vertex : oculus_vertices) { + const auto l = sample(left, (float)(vertex.g[0] / 1280.0), (float)(vertex.g[1] / 1440.0)); + const auto r = sample(right, (float)(1.0 - vertex.g[0] / 1280.0), (float)(vertex.g[1] / 1440.0)); + CHECK(r.g.x == Catch::Approx(1.0f - l.g.x).margin(1e-5)); + CHECK(r.g.y == Catch::Approx(l.g.y).margin(1e-5)); + CHECK(r.r.x == Catch::Approx(1.0f - l.r.x).margin(1e-5)); + CHECK(r.b.x == Catch::Approx(1.0f - l.b.x).margin(1e-5)); + } +} + + +TEST_CASE("SteamVR render size follows the upright FOV and centre pixel density") +{ + struct TestHmd + { + xrt_device device{}; + xrt_hmd_parts parts{}; + std::array optics; + } hmd; + hmd.device.hmd = &hmd.parts; + hmd.parts.view_count = 2; + hmd.parts.screens[0].w_pixels = 1440; + hmd.parts.screens[0].h_pixels = 2560; + hmd.device.compute_distortion = [](xrt_device *device, uint32_t eye, float u, float v, + xrt_uv_triplet *result) { + auto *hmd = reinterpret_cast(device); + rift_s_optics_compute(&hmd->optics[eye], u, v, result); + return XRT_SUCCESS; + }; + for (uint32_t eye = 0; eye < 2; eye++) { + hmd.optics[eye] = make_optics(eye, 0.068f); + hmd.parts.distortion.fov[eye] = hmd.optics[eye].fov; + auto &view = hmd.parts.views[eye]; + view.display.w_pixels = 1280; + view.display.h_pixels = 1440; + view.viewport.w_pixels = 1440; + view.viewport.h_pixels = 1280; + view.viewport.y_pixels = eye * 1280; + view.rot.vecs[0] = {0, 1}; + view.rot.vecs[1] = {-1, 0}; + } + uint32_t width = 0, height = 0; + ovrd_get_recommended_render_target_size(&hmd.device, 1.0f, &width, &height); + CHECK(width == Catch::Approx(1648).margin(2)); + CHECK(height == Catch::Approx(1771).margin(2)); + const double aspect = (hmd.optics[0].tan_left + hmd.optics[0].tan_right) / + (hmd.optics[0].tan_up + hmd.optics[0].tan_down); + CHECK((double)width / height == Catch::Approx(aspect).margin(0.001)); + const auto base_width = width, base_height = height; + ovrd_get_recommended_render_target_size(&hmd.device, 1.4f, &width, &height); + CHECK(width == Catch::Approx(base_width * 1.4).margin(2)); + CHECK(height == Catch::Approx(base_height * 1.4).margin(2)); + + // A wider projection needs more source pixels while preserving the same optical density. + for (uint32_t eye = 0; eye < 2; eye++) { + REQUIRE(rift_s_optics_init(&hmd.optics[eye], eye, 0.068f, 1.0f, 1.25f)); + hmd.parts.distortion.fov[eye] = hmd.optics[eye].fov; + } + ovrd_get_recommended_render_target_size(&hmd.device, 1.0f, &width, &height); + CHECK(width == Catch::Approx(base_width).margin(2)); + CHECK(height == Catch::Approx(base_height * 1.25).margin(2)); + + // Drivers without a compute model fall back to upright per-eye dimensions. + hmd.device.compute_distortion = nullptr; + ovrd_get_recommended_render_target_size(&hmd.device, 1.0f, &width, &height); + CHECK(width == 1280); + CHECK(height == 1440); +} + +TEST_CASE("OpenVR projection and distortion agree on the zero ray") +{ + for (uint32_t eye = 0; eye < 2; eye++) { + const auto optics = make_optics(eye); + const auto uv = sample(optics, optics.lens_center_px.x / 1280, optics.lens_center_px.y / 1440); + float left, right, top, bottom; + ovrd_get_projection_raw(optics.fov, &left, &right, &top, &bottom); + // SteamVR: M11=2/(B-T), M12=(B+T)/(B-T), texture V=(1-NDC_y)/2. + const xrt_vec2 zero_uv{-left / (right - left), bottom / (bottom - top)}; + for (const xrt_vec2 &channel : {uv.r, uv.g, uv.b}) { + CHECK(zero_uv.x == Catch::Approx(channel.x).margin(1e-6)); + CHECK(zero_uv.y == Catch::Approx(channel.y).margin(1e-6)); + } + } +} diff --git a/tests/tests_rift_s_passthrough.cpp b/tests/tests_rift_s_passthrough.cpp new file mode 100644 index 000000000..51412c3d6 --- /dev/null +++ b/tests/tests_rift_s_passthrough.cpp @@ -0,0 +1,209 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "rift_s_passthrough.h" +#include "os/os_time.h" +#include "util/u_frame.h" +#include + +static rift_s_camera_calibration_block +calibration() +{ + rift_s_camera_calibration_block block{}; + for (auto &c : block.cameras) { + c.device_from_camera.v[0] = c.device_from_camera.v[15] = 1; + c.device_from_camera.v[5] = c.device_from_camera.v[10] = -1; + c.projection = {320, 240, 200, 200}; + } + return block; +} + +TEST_CASE("Passthrough projects calibrated fisheye rays and sensor roll") +{ + auto block = calibration(); + auto &c = block.cameras[RIFT_S_CAMERA_FRONT_LEFT]; + xrt_vec2 pixel; + REQUIRE(rift_s_passthrough_project(&c, 0, 0, &pixel)); + CHECK(pixel.x == Catch::Approx(320)); + CHECK(pixel.y == Catch::Approx(240)); + c.distortion.k[0] = 0.1f; + c.distortion.p1 = 0.01f; + REQUIRE(rift_s_passthrough_project(&c, 1, 0, &pixel)); + double t = atan(1.0); + CHECK(pixel.x == Catch::Approx(320 + 200 * t * (1 + 0.1 * t * t))); + CHECK(pixel.y == Catch::Approx(240 + 200 * 0.01 * t * t)); + c.distortion = {}; + // Native camera has a 90 degree roll in firmware device coordinates. + c.device_from_camera.v[0] = c.device_from_camera.v[5] = 0; + c.device_from_camera.v[1] = -1; + c.device_from_camera.v[4] = -1; + REQUIRE(rift_s_passthrough_project(&c, 1, 0, &pixel)); + CHECK(pixel.x == Catch::Approx(320)); + CHECK(pixel.y == Catch::Approx(240 - 200 * t)); + c.device_from_camera.v[10] = 1; + CHECK_FALSE(rift_s_passthrough_project(&c, 0, 0, &pixel)); +} + +TEST_CASE("Passthrough keeps synchronized SLAM frames only while requested") +{ + auto block = calibration(); + auto *camera = rift_s_passthrough_create(&block); + REQUIRE(camera); + xrt_frame *left = nullptr, *right = nullptr, *out = nullptr; + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &left); + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &right); + std::fill_n(left->data, left->size, 70); + std::fill_n(right->data, right->size, 180); + left->timestamp = right->timestamp = os_monotonic_get_ns(); + rift_s_passthrough_push(camera, left, right); + CHECK_FALSE(camera->get_frame(camera, &out)); + camera->set_active(camera, true); + rift_s_passthrough_push(camera, left, right); + REQUIRE(camera->get_frame(camera, &out)); + CHECK(out->width == 640); + CHECK(out->height == 320); + CHECK(out->format == XRT_FORMAT_R8G8B8); + CHECK(out->timestamp == left->timestamp); + CHECK(out->data[160 * out->stride + 160 * 3] == 70); + CHECK(out->data[160 * out->stride + 480 * 3] == 180); + auto *held = out; + xrt_frame *cached = nullptr; + REQUIRE(camera->get_frame(camera, &cached)); + CHECK(cached == held); + xrt_frame_reference(&cached, nullptr); + left->timestamp = right->timestamp = os_monotonic_get_ns() - 300000000; + rift_s_passthrough_push(camera, left, right); + CHECK_FALSE(camera->get_frame(camera, &cached)); + camera->set_active(camera, false); + CHECK_FALSE(camera->get_frame(camera, &cached)); + camera->set_active(camera, true); + CHECK_FALSE(camera->get_frame(camera, &cached)); + // A consumer reference stays alive after stopping/destroying the provider. + rift_s_passthrough_destroy(camera); + CHECK(held->data[160 * held->stride + 160 * 3] == 70); + xrt_frame_reference(&out, nullptr); + xrt_frame_reference(&left, nullptr); + xrt_frame_reference(&right, nullptr); +} + +TEST_CASE("Passthrough rectifies the front camera quarter-turn and downward pitch") +{ + auto block = calibration(); + auto &c = block.cameras[RIFT_S_CAMERA_FRONT_LEFT]; + const float angle = 0.35f; + c.device_from_camera = {}; + c.device_from_camera.v[1] = std::cos(angle); + c.device_from_camera.v[2] = -std::sin(angle); + c.device_from_camera.v[4] = 1; + c.device_from_camera.v[9] = -std::sin(angle); + c.device_from_camera.v[10] = -std::cos(angle); + c.device_from_camera.v[15] = 1; + xrt_vec2 center; + REQUIRE(rift_s_passthrough_project(&c, 0, 0, ¢er)); + CHECK(center.x == Catch::Approx(320 + 200 * angle)); + CHECK(center.y == Catch::Approx(240)); + xrt_vec2 right; + REQUIRE(rift_s_passthrough_project(&c, 0.1f, 0, &right)); + CHECK(right.y > center.y); +} + +TEST_CASE("Passthrough rejects an incomplete sensor pair without replacing the last complete capture") +{ + auto block = calibration(); + auto *camera = rift_s_passthrough_create(&block); + REQUIRE(camera); + camera->set_active(camera, true); + xrt_frame *left = nullptr, *right = nullptr, *out = nullptr; + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &left); + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &right); + std::fill_n(left->data, left->size, 70); + std::fill_n(right->data, right->size, 180); + left->timestamp = right->timestamp = os_monotonic_get_ns(); + left->source_sequence = right->source_sequence = 10; + rift_s_passthrough_push(camera, left, right); + REQUIRE(camera->get_frame(camera, &out)); + const auto complete_timestamp = out->timestamp; + xrt_frame_reference(&out, nullptr); + + xrt_frame *bad_left = nullptr, *bad_right = nullptr; + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &bad_left); + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &bad_right); + std::fill_n(bad_left->data, bad_left->size, 75); + std::fill_n(bad_right->data, bad_right->size, 185); + bad_left->timestamp = bad_right->timestamp = complete_timestamp + 1; + bad_left->source_sequence = bad_right->source_sequence = 11; + uint8_t *right_storage = bad_right->data; + SECTION("missing left payload") { std::fill_n(bad_left->data, bad_left->size, 0); } + SECTION("missing right payload") { std::fill_n(bad_right->data, bad_right->size, 0); } + SECTION("truncated final sensor row") { --bad_right->size; } + SECTION("row stride shorter than the image") { bad_left->stride = 639; } + SECTION("different camera exposure timestamp") { ++bad_right->timestamp; } + SECTION("different source frame sequence") { ++bad_right->source_sequence; } + SECTION("empty camera") { bad_right->height = 0; } + SECTION("missing storage") { bad_right->data = nullptr; } + rift_s_passthrough_push(camera, bad_left, bad_right); + REQUIRE(camera->get_frame(camera, &out)); + CHECK(out->timestamp == complete_timestamp); + CHECK(out->data[160 * out->stride + 160 * 3] == 70); + CHECK(out->data[160 * out->stride + 480 * 3] == 180); + xrt_frame_reference(&out, nullptr); + // Once the retained raw capture expires, the cached conversion cannot renew it. + left->timestamp = right->timestamp = os_monotonic_get_ns() - 300000000; + CHECK_FALSE(camera->get_frame(camera, &out)); + rift_s_passthrough_destroy(camera); + bad_right->data = right_storage; + xrt_frame_reference(&left, nullptr); + xrt_frame_reference(&right, nullptr); + xrt_frame_reference(&bad_left, nullptr); + xrt_frame_reference(&bad_right, nullptr); +} + +TEST_CASE("Passthrough accepts a dark stereo scene and recovers both sensors together") +{ + auto block = calibration(); + auto *camera = rift_s_passthrough_create(&block); + REQUIRE(camera); + camera->set_active(camera, true); + xrt_frame *left = nullptr, *right = nullptr, *out = nullptr; + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &left); + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &right); + std::fill_n(left->data, left->size, 0); + std::fill_n(right->data, right->size, 0); + left->timestamp = right->timestamp = os_monotonic_get_ns(); + rift_s_passthrough_push(camera, left, right); + REQUIRE(camera->get_frame(camera, &out)); + CHECK(out->data[160 * out->stride + 160 * 3] == 0); + CHECK(out->data[160 * out->stride + 480 * 3] == 0); + xrt_frame_reference(&out, nullptr); + std::fill_n(left->data, left->size, 80); + std::fill_n(right->data, right->size, 190); + left->timestamp = right->timestamp = os_monotonic_get_ns(); + rift_s_passthrough_push(camera, left, right); + REQUIRE(camera->get_frame(camera, &out)); + CHECK(out->data[160 * out->stride + 160 * 3] == 80); + CHECK(out->data[160 * out->stride + 480 * 3] == 190); + xrt_frame_reference(&out, nullptr); + rift_s_passthrough_destroy(camera); + xrt_frame_reference(&left, nullptr); + xrt_frame_reference(&right, nullptr); +} + +TEST_CASE("Passthrough rectification and export share the 110 degree pinhole") +{ + auto block = calibration(); + auto *camera = rift_s_passthrough_create(&block); + REQUIRE(camera); + CHECK(camera->focal_length.x == Catch::Approx(112.033206).margin(1e-4)); + CHECK(camera->focal_length.y == camera->focal_length.x); + CHECK(2 * atan(160.0 / camera->focal_length.x) * 180 / M_PI == Catch::Approx(110)); + for (const auto &pose : camera->head_from_camera) { + CHECK(pose.orientation.w == 1); + CHECK(pose.orientation.x == 0); + } + // A tilted/rolled physical sensor is sampled along the exported head-aligned ray. + xrt_vec2 pixel; + REQUIRE(rift_s_passthrough_project(&block.cameras[RIFT_S_CAMERA_FRONT_LEFT], + (319 - camera->center.x) / camera->focal_length.x, 0, &pixel)); + CHECK(pixel.x == Catch::Approx(320 + 200 * atan(159.5 / camera->focal_length.x)).margin(1e-4)); + rift_s_passthrough_destroy(camera); +} diff --git a/tests/tests_rift_s_position_filter.cpp b/tests/tests_rift_s_position_filter.cpp new file mode 100644 index 000000000..cf7206571 --- /dev/null +++ b/tests/tests_rift_s_position_filter.cpp @@ -0,0 +1,328 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +extern "C" { +#include "rift_s/rift_s_position_filter.h" +} +#include +#include +#include + +static constexpr timepoint_ns MS = U_TIME_1MS_IN_NS; +static constexpr timepoint_ns IMU_STEP = 2 * MS; // 500 Hz +// Optical fixes every 16 IMU samples: 32 ms, about 30 Hz. +static const xrt_vec3 OPTICAL_VARIANCE = {3e-3f * 3e-3f, 3e-3f * 3e-3f, 3e-3f * 3e-3f}; + +static std::unique_ptr +make_filter() +{ + auto f = std::make_unique(); + rift_s_position_filter_reset(f.get()); + return f; +} + +static xrt_vec3 +position_at(const rift_s_position_filter &f, timepoint_ns t) +{ + xrt_vec3 p, v; + rift_s_position_filter_predict(&f, t, 100 * MS, &p, &v); + return p; +} + +TEST_CASE("Rift S loss ladder matches the Windows defaults") +{ + time_duration_ns p, v; + rift_s_tracking_loss_durations(0, 0, &p, &v); + CHECK(p == 0); + rift_s_tracking_loss_durations(1, 0, &p, &v); + CHECK(p == 250 * MS); + CHECK(v == 250 * MS); + rift_s_tracking_loss_durations(3, 0, &p, &v); + CHECK(p == 500 * MS); + CHECK(v == 500 * MS); + rift_s_tracking_loss_durations(11, 0, &p, &v); + CHECK(p == 2000 * MS); + CHECK(v == 4000 * MS); + rift_s_tracking_loss_durations(30, 21, &p, &v); + CHECK(p == 3000 * MS); + CHECK(v == 5000 * MS); +} + +TEST_CASE("Rift S position filter smooths optical noise at rest and learns the accelerometer bias") +{ + auto f = make_filter(); + std::mt19937 rng(1); + std::normal_distribution noise(0, 3e-3f); + const xrt_vec3 truth = {0.1f, 1.2f, -0.3f}; + const xrt_vec3 biased_accel = {0.2f, -0.3f, 0.1f}; // tilt/bias error seen as acceleration + double sq = 0, raw_sq = 0; + int n = 0; + for (timepoint_ns t = MS; t < 6000 * MS; t += IMU_STEP) { + rift_s_position_filter_imu(f.get(), t, &biased_accel); + if ((t / IMU_STEP) % 16 == 0) { + xrt_vec3 z = {truth.x + noise(rng), truth.y + noise(rng), truth.z + noise(rng)}; + rift_s_position_filter_fix(f.get(), t - 20 * MS, &z, &OPTICAL_VARIANCE, 0.15f); + if (t > 3000 * MS) { + xrt_vec3 p = position_at(*f, t); + sq += pow(p.x - truth.x, 2) + pow(p.y - truth.y, 2) + pow(p.z - truth.z, 2); + raw_sq += pow(z.x - truth.x, 2) + pow(z.y - truth.y, 2) + pow(z.z - truth.z, 2); + n++; + } + } + } + double rms = sqrt(sq / n), raw_rms = sqrt(raw_sq / n); + INFO("rms=" << rms << " raw_rms=" << raw_rms); + CHECK(rms < 0.75 * raw_rms); + CHECK(rms < 4.5e-3); + CHECK(f->state.x[0][2] == Catch::Approx(biased_accel.x).margin(0.05)); + CHECK(f->state.x[1][2] == Catch::Approx(biased_accel.y).margin(0.05)); + CHECK(f->resets == 1); + CHECK(f->gated == 0); +} + +TEST_CASE("Rift S position filter follows motion between delayed optical fixes") +{ + auto f = make_filter(); + const double w = 2 * M_PI * 1.5, amplitude = 0.2; // a brisk 1.5 Hz hand swing + auto truth = [&](timepoint_ns t) { return amplitude * sin(w * (double)t / U_TIME_1S_IN_NS); }; + auto accel = [&](timepoint_ns t) { return -amplitude * w * w * sin(w * (double)t / U_TIME_1S_IN_NS); }; + const timepoint_ns latency = 40 * MS; + double filter_sq = 0, held_sq = 0; + int n = 0; + double held = 0; + for (timepoint_ns t = MS; t < 4000 * MS; t += IMU_STEP) { + xrt_vec3 a = {(float)accel(t), 0, 0}; + rift_s_position_filter_imu(f.get(), t, &a); + // A fix captured at t - latency is delivered now. + if ((t / IMU_STEP) % 16 == 0 && t > latency) { + xrt_vec3 z = {(float)truth(t - latency), 0, 0}; + rift_s_position_filter_fix(f.get(), t - latency, &z, &OPTICAL_VARIANCE, 0.15f); + held = z.x; + } + if (t > 1000 * MS) { + // The compositor asks for 20 ms in the future. + timepoint_ns query = t + 20 * MS; + filter_sq += pow(position_at(*f, query).x - truth(query), 2); + held_sq += pow(held - truth(query), 2); + n++; + } + } + double filter_rms = sqrt(filter_sq / n), held_rms = sqrt(held_sq / n); + INFO("filter_rms=" << filter_rms << " held_rms=" << held_rms); + CHECK(filter_rms < 0.005); + CHECK(filter_rms < 0.05 * held_rms); + CHECK(f->gated == 0); +} + +TEST_CASE("Rift S position filter coasts with bounded drift, holds when frozen and resets on reacquire") +{ + auto f = make_filter(); + const xrt_vec3 zero = {0, 0, 0}; + const xrt_vec3 here = {0.3f, 1.0f, -0.4f}; + timepoint_ns t = MS; + for (; t < 1000 * MS; t += IMU_STEP) { + rift_s_position_filter_imu(f.get(), t, &zero); + if ((t / IMU_STEP) % 16 == 0) { + rift_s_position_filter_fix(f.get(), t, &here, &OPTICAL_VARIANCE, 0.15f); + } + } + // Optical loss with a 0.5 m/s^2 uncorrected acceleration error: coast for the 0.5 s ladder step. + f->coasting = true; + const xrt_vec3 error = {0.5f, 0, 0}; + for (timepoint_ns end = t + 500 * MS; t < end; t += IMU_STEP) { + rift_s_position_filter_imu(f.get(), t, &error); + } + xrt_vec3 coasted = {(float)f->state.x[0][0], (float)f->state.x[1][0], (float)f->state.x[2][0]}; + // The 500 ms bridge with decaying velocity: a(tau t - tau^2 (1 - e^-t/tau)) = 4.6 cm. + CHECK(fabsf(coasted.x - here.x) < 0.05f); + CHECK(fabsf(coasted.x - here.x) > 0.0f); + + rift_s_position_filter_freeze(f.get()); + for (timepoint_ns end = t + 2000 * MS; t < end; t += IMU_STEP) { + rift_s_position_filter_imu(f.get(), t, &error); + } + xrt_vec3 held = position_at(*f, t + 50 * MS); + CHECK(held.x == Catch::Approx(coasted.x).margin(1e-6)); + xrt_vec3 p, v; + rift_s_position_filter_predict(f.get(), t, 100 * MS, &p, &v); + CHECK(v.x == 0.0f); + + // Reacquire somewhere else: three consistent fixes re-initialize. + const xrt_vec3 there = {0.6f, 1.1f, -0.2f}; + CHECK(rift_s_position_filter_fix(f.get(), t - 96 * MS, &there, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_PENDING); + CHECK(rift_s_position_filter_fix(f.get(), t - 63 * MS, &there, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_PENDING); + CHECK(f->frozen); + CHECK(rift_s_position_filter_fix(f.get(), t - 30 * MS, &there, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_RESET); + CHECK_FALSE(f->frozen); + CHECK(position_at(*f, t).x == Catch::Approx(there.x).margin(0.01)); +} + +TEST_CASE("Rift S position filter resets on a confirmed large innovation and rejects fixes older than its history") +{ + auto f = make_filter(); + const xrt_vec3 zero = {0, 0, 0}; + const xrt_vec3 here = {0, 1, 0}; + timepoint_ns t = MS; + for (; t < 500 * MS; t += IMU_STEP) { + rift_s_position_filter_imu(f.get(), t, &zero); + if ((t / IMU_STEP) % 16 == 0) { + rift_s_position_filter_fix(f.get(), t, &here, &OPTICAL_VARIANCE, 0.15f); + } + } + const xrt_vec3 far = {0.5f, 1, 0}; + CHECK(rift_s_position_filter_fix(f.get(), t - 74 * MS, &far, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_PENDING); + CHECK(position_at(*f, t).x == Catch::Approx(0.0f).margin(1e-3)); + CHECK(rift_s_position_filter_fix(f.get(), t - 42 * MS, &far, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_PENDING); + CHECK(rift_s_position_filter_fix(f.get(), t - 10 * MS, &far, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_RESET); + CHECK(f->gated == 3); + CHECK(f->rejected == 2); + CHECK(position_at(*f, t).x == Catch::Approx(0.5f).margin(1e-3)); + + for (timepoint_ns end = t + 2000 * MS; t < end; t += IMU_STEP) { + rift_s_position_filter_imu(f.get(), t, &zero); + } + CHECK(rift_s_position_filter_fix(f.get(), t - 1500 * MS, &far, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_STALE); + CHECK(f->stale == 1); +} + +TEST_CASE("Rift S position filter stays close to optical under a time-varying acceleration error") +{ + // Tilt error during rotation and accelerometer scale error show up as a slowly varying + // world acceleration error; the filter must not lag or overshoot the optical fixes. + auto f = make_filter(); + std::mt19937 rng(2); + std::normal_distribution optical_noise(0, 3e-3f), accel_noise(0, 0.05f); + const double w = 2 * M_PI * 1.0, amplitude = 0.25; + auto truth = [&](timepoint_ns t) { return amplitude * sin(w * (double)t / U_TIME_1S_IN_NS); }; + auto accel = [&](timepoint_ns t) { return -amplitude * w * w * sin(w * (double)t / U_TIME_1S_IN_NS); }; + auto error = [&](timepoint_ns t) { return 0.5 * sin(2 * M_PI * 0.7 * (double)t / U_TIME_1S_IN_NS + 1); }; + const timepoint_ns latency = 40 * MS; + double sq = 0; + int n = 0; + for (timepoint_ns t = MS; t < 6000 * MS; t += IMU_STEP) { + xrt_vec3 a = {(float)(accel(t) + error(t)) + accel_noise(rng), 0, 0}; + rift_s_position_filter_imu(f.get(), t, &a); + if ((t / IMU_STEP) % 16 == 0 && t > latency) { + xrt_vec3 z = {(float)truth(t - latency) + optical_noise(rng), 0, 0}; + rift_s_position_filter_fix(f.get(), t - latency, &z, &OPTICAL_VARIANCE, 0.15f); + } + if (t > 1000 * MS) { + timepoint_ns query = t + 20 * MS; + sq += pow(position_at(*f, query).x - truth(query), 2); + n++; + } + } + double rms = sqrt(sq / n); + INFO("rms=" << rms); + CHECK(rms < 0.006); + CHECK(f->gated == 0); +} + +// r13 worn log, left hand behind the back: after a long track was held, single wrong fits ~1 m +// away re-initialized the filter, and coasting on their velocity drifted it further. +TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consistent reacquire") +{ + auto f = make_filter(); + const xrt_vec3 zero = {0, 0, 0}; + const xrt_vec3 here = {0.15f, 0.0f, 0.2f}; + timepoint_ns t = MS; + for (; t < 2000 * MS; t += IMU_STEP) { + rift_s_position_filter_imu(f.get(), t, &zero); + if ((t / IMU_STEP) % 16 == 0) { + rift_s_position_filter_fix(f.get(), t - 20 * MS, &here, &OPTICAL_VARIANCE, 0.15f); + } + } + rift_s_position_filter_freeze(f.get()); + // Scattered single fits, each one frame apart, never two in agreement. + const xrt_vec3 wrong[] = {{-0.99f, 0.29f, 0.40f}, + {-1.02f, -0.18f, 0.71f}, + {-0.63f, 0.17f, 0.07f}, + {-0.57f, 0.28f, 0.40f}, + {-1.06f, -0.24f, 0.81f}}; + for (const auto &w : wrong) { + t += 33 * MS; + rift_s_position_filter_imu(f.get(), t, &zero); + CHECK(rift_s_position_filter_fix(f.get(), t - 20 * MS, &w, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_PENDING); + xrt_vec3 p = position_at(*f, t); + CHECK(p.x == Catch::Approx(here.x).margin(1e-3)); + CHECK(p.z == Catch::Approx(here.z).margin(1e-3)); + } + CHECK(f->frozen); + // The hand comes back: three consistent fixes take over. + const xrt_vec3 back = {0.25f, 0.05f, 0.1f}; + enum rift_s_position_fix_result r = RIFT_S_POSITION_FIX_PENDING; + for (int i = 0; i < RIFT_S_POSITION_FILTER_CONFIRM_FIXES; i++) { + t += 33 * MS; + rift_s_position_filter_imu(f.get(), t, &zero); + xrt_vec3 z = {back.x + 0.01f * i, back.y, back.z}; + r = rift_s_position_filter_fix(f.get(), t - 20 * MS, &z, &OPTICAL_VARIANCE, 0.15f); + } + CHECK(r == RIFT_S_POSITION_FIX_RESET); + CHECK(position_at(*f, t).x == Catch::Approx(back.x + 0.02f).margin(0.01)); +} + +TEST_CASE("Rift S young track holds instead of coasting on velocity from a few fixes") +{ + auto f = make_filter(); + const xrt_vec3 zero = {0, 0, 0}; + timepoint_ns t = MS; + // Two fixes 33 ms apart and 3 cm apart (a wrong pair implies ~1 m/s), then nothing. + const xrt_vec3 a = {0.07f, 0.17f, 0.59f}, b = {0.10f, 0.17f, 0.59f}; + rift_s_position_filter_fix(f.get(), t, &a, &OPTICAL_VARIANCE, 0.15f); + for (timepoint_ns end = t + 33 * MS; t < end; t += IMU_STEP) + rift_s_position_filter_imu(f.get(), t, &zero); + rift_s_position_filter_fix(f.get(), t, &b, &OPTICAL_VARIANCE, 0.15f); + CHECK(f->track_updates == 2); + const double fused = f->state.x[0][0]; + f->coasting = true; + for (timepoint_ns end = t + 250 * MS; t < end; t += IMU_STEP) + rift_s_position_filter_imu(f.get(), t, &zero); + CHECK(f->state.x[0][0] == Catch::Approx(fused).margin(1e-6)); + CHECK(f->state.x[0][1] == 0.0); +} + +TEST_CASE("Optical loss bounds IMU displacement and freezes after the bridge") +{ + auto f = make_filter(); + xrt_vec3 here = {0.5f, 1.2f, -0.9f}, error = {-20, 0, 0}; + REQUIRE(rift_s_position_filter_fix(f.get(), MS, &here, &OPTICAL_VARIANCE, 0.15f) == RIFT_S_POSITION_FIX_RESET); + f->track_updates = 30; + for (timepoint_ns t = 3 * MS; t <= 1000 * MS; t += IMU_STEP) { + f->coasting = t > 121 * MS; + rift_s_position_filter_imu(f.get(), t, &error); + if (t > 121 * MS) + CHECK(fabs(position_at(*f, t).x - here.x) <= 0.101); + } + CHECK(f->frozen); + xrt_vec3 held = position_at(*f, 1000 * MS); + for (timepoint_ns t = 1001 * MS; t < 2000 * MS; t += IMU_STEP) + rift_s_position_filter_imu(f.get(), t, &error); + CHECK(position_at(*f, 2000 * MS).x == held.x); +} + +TEST_CASE("Optical-loss queries bound prediction during a radio gap") +{ + auto f = make_filter(); + xrt_vec3 here = {0.5f, 1.2f, -0.9f}, zero = {0, 0, 0}; + REQUIRE(rift_s_position_filter_fix(f.get(), MS, &here, &OPTICAL_VARIANCE, 0.15f) == RIFT_S_POSITION_FIX_RESET); + f->track_updates = 30; + rift_s_position_filter_imu(f.get(), 100 * MS, &zero); + f->state.x[0][0] = here.x - 0.09; + f->state.x[0][1] = -4; + xrt_vec3 p, v; + rift_s_position_filter_predict(f.get(), 150 * MS, 50 * MS, &p, &v); + CHECK(p.x == Catch::Approx(here.x - 0.1).margin(1e-5)); + CHECK(v.x == 0); + // Past the bridge the held estimate is not extrapolated. + rift_s_position_filter_predict(f.get(), RIFT_S_POSITION_FILTER_BRIDGE_NS + 100 * MS, 50 * MS, &p, &v); + CHECK(p.x == Catch::Approx(here.x - 0.09).margin(1e-5)); + CHECK(v.x == 0); +} diff --git a/tests/tests_rift_s_slam_guard.cpp b/tests/tests_rift_s_slam_guard.cpp new file mode 100644 index 000000000..8d66e0e04 --- /dev/null +++ b/tests/tests_rift_s_slam_guard.cpp @@ -0,0 +1,493 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "rift_s/rift_s_slam_guard.h" +#include "math/m_api.h" +#include "math/m_vec3.h" +#include + +static constexpr int64_t MS = 1000 * 1000; +static constexpr int64_t FRAME = 11 * MS; // ~90 Hz pose queries + +static xrt_quat +yaw_quat(float rad) +{ + xrt_quat q; + xrt_vec3 up = {0, 1, 0}; + math_quat_from_angle_vector(rad, &up, &q); + return q; +} + +static float +yaw_of(const xrt_quat &q) +{ + xrt_vec3 fwd = {0, 0, -1}; + xrt_vec3 out; + math_quat_rotate_vec3(&q, &fwd, &out); + return atan2f(-out.x, -out.z); +} + +static float +dist(xrt_vec3 a, xrt_vec3 b) +{ + return m_vec3_len(m_vec3_sub(a, b)); +} + +struct Sim +{ + rift_s_slam_guard g; + int64_t ts = 1000 * MS; + xrt_quat fusion = {0, 0, 0, 1}; + xrt_vec3 no_pose = {0, 0, 0}; + int resets = 0; + + Sim() + { + rift_s_slam_guard_init(&g); + } + + //! One pose query; returns whether SLAM was published. + bool + step(bool valid, xrt_pose slam, xrt_pose *out, rift_s_slam_guard_reason *reason = nullptr) + { + ts += FRAME; + rift_s_slam_guard_reason r; + bool used = rift_s_slam_guard_update(&g, valid, &slam, ts, &fusion, &no_pose, out, &r); + if (rift_s_slam_guard_take_reset(&g, ts)) { + resets++; + } + if (reason) { + *reason = r; + } + return used; + } + + //! Feed a still pose until SLAM is trusted; returns the number of queries it took. + int + settle(xrt_pose slam) + { + xrt_pose out; + for (int i = 1; i < 500; i++) { + if (step(true, slam, &out)) { + return i; + } + } + return -1; + } +}; + +TEST_CASE("No SLAM pose yet: eye height above the floor") +{ + // The Rift S origin already carries 1.6 m, so the tracker frame position is 0. + xrt_vec3 p = rift_s_slam_guard_no_pose_position(1.6f); + CHECK(p.y + 1.6f == Catch::Approx(1.6f)); + // An origin without offset gets the full height. + CHECK(rift_s_slam_guard_no_pose_position(0.0f).y == Catch::Approx(1.6f)); + + Sim s; + s.no_pose = {0, 1.6f, 0}; + xrt_pose out; + CHECK_FALSE(s.step(false, xrt_pose{}, &out)); + CHECK(out.position.y == Catch::Approx(1.6f)); + CHECK(s.g.state == RIFT_S_SLAM_GUARD_ACQUIRING); +} + +TEST_CASE("Startup: SLAM is trusted after a consistent second, aligned onto eye height") +{ + Sim s; + xrt_pose head = {{0, 0, 0, 1}, {0.1f, 0.02f, -0.1f}}; + int n = s.settle(head); + CHECK(n >= RIFT_S_SLAM_GUARD_STABLE_SAMPLES); + CHECK(n * FRAME >= RIFT_S_SLAM_GUARD_STABLE_NS); + xrt_pose out; + REQUIRE(s.step(true, head, &out)); + CHECK(dist(out.position, s.no_pose) < 1e-5f); + CHECK(s.resets == 0); + CHECK(s.g.recoveries == 0); + + // Startup junk far away is never published. + Sim junk; + xrt_pose far = {{0, 0, 0, 1}, {1e7f, 0, 0}}; + for (int i = 0; i < 200; i++) { + CHECK_FALSE(junk.step(true, far, &out)); + } +} + +TEST_CASE("SLAM started on a desk: the head is still at eye height, so the floor stays put") +{ + // SteamVR keeps the Room Setup floor as a fixed height in the driver frame. SLAM that started + // with the headset on a desk sees the worn headset 0.8 m above its origin; publishing that as is + // would lift the frame by 0.8 m against the stored floor. + Sim s; + s.no_pose = rift_s_slam_guard_no_pose_position(1.6f); // eye height with the 1.6 m origin offset + xrt_pose head = {yaw_quat(0.7f), {0.2f, 0.8f, -0.3f}}; + REQUIRE(s.settle(head) > 0); + xrt_pose out; + REQUIRE(s.step(true, head, &out)); + CHECK(out.position.y + 1.6f == Catch::Approx(1.6f).margin(1e-5)); + // Heading continues from the 3DoF orientation shown while acquiring. + CHECK(yaw_of(out.orientation) == Catch::Approx(yaw_of(s.fusion)).margin(1e-4)); + + // Standing up by 0.3 m is followed as SLAM motion, relative to eye height. + for (int i = 0; i < 30; i++) { + head.position.y += 0.01f; + REQUIRE(s.step(true, head, &out)); + } + CHECK(out.position.y + 1.6f == Catch::Approx(1.9f).margin(1e-4)); + CHECK(s.g.divergences == 0); +} + +TEST_CASE("Walking and quick head motion stay on SLAM") +{ + Sim s; + xrt_pose head = {{0, 0, 0, 1}, {0, 0, 0}}; + REQUIRE(s.settle(head) > 0); + xrt_pose out; + // 4 m/s for 1 s along X, then back. + for (int i = 0; i < 180; i++) { + head.position.x += (i < 90 ? 1 : -1) * 4.0f * FRAME / 1e9f; + REQUIRE(s.step(true, head, &out)); + } + // Repeated queries at the same time are fine. + s.ts -= FRAME; + CHECK(s.step(true, head, &out)); + CHECK(s.g.divergences == 0); +} + +TEST_CASE("Idle-headset divergence: hold the last good pose, re-init, realign without a jump") +{ + Sim s; + xrt_pose head = {{0, 0, 0, 1}, {0.3f, 0.05f, -0.2f}}; + REQUIRE(s.settle(head) > 0); + xrt_pose out; + REQUIRE(s.step(true, head, &out)); + const xrt_vec3 good = out.position; + + // The reported runaway: Basalt drifts off at 8 m/s towards (18,-16,19). + xrt_vec3 target = {18, -16, 19}; + xrt_vec3 dir = m_vec3_normalize(m_vec3_sub(target, head.position)); + xrt_pose runaway = head; + rift_s_slam_guard_reason reason; + bool rejected = false; + for (int i = 0; i < 400; i++) { + runaway.position = m_vec3_add(runaway.position, m_vec3_mul_scalar(dir, 8.0f * FRAME / 1e9f)); + bool used = s.step(true, runaway, &out, &reason); + if (!used) { + if (!rejected) { + CHECK(reason == RIFT_S_SLAM_GUARD_SPEED); + } + rejected = true; + } + CHECK(dist(out.position, good) < 0.1f); // never more than one accepted step away + CHECK(out.position.y > -0.1f); // never underground + } + REQUIRE(rejected); + CHECK(s.g.state == RIFT_S_SLAM_GUARD_DIVERGED); + CHECK(s.g.divergences == 1); + // One immediate re-init, then retries at most every 2 s while still diverged. + CHECK(s.resets >= 1); + CHECK(s.resets <= 1 + (int)(400 * FRAME / RIFT_S_SLAM_GUARD_RESET_INTERVAL_NS)); + + // Fallback: 3DoF orientation continued in the SLAM heading, position held. + const xrt_vec3 held = out.position; + s.fusion = yaw_quat(0.3f); + s.step(true, runaway, &out); + CHECK(dist(out.position, held) < 1e-6f); + CHECK(yaw_of(out.orientation) == Catch::Approx(0.3f).margin(1e-3)); + + // SLAM restarts at its new origin with an arbitrary heading; it must be consistent before use. + xrt_pose fresh = {yaw_quat(-1.2f), {0, 0, 0}}; + int n = s.settle(fresh); + REQUIRE(n > 0); + CHECK(n >= RIFT_S_SLAM_GUARD_STABLE_SAMPLES); + CHECK(s.g.recoveries == 1); + REQUIRE(s.step(true, fresh, &out)); + CHECK(dist(out.position, held) < 1e-4f); // no jump back to the restart origin + CHECK(yaw_of(out.orientation) == Catch::Approx(0.3f).margin(1e-3)); // no heading jump + + // Moving 0.5 m along the new SLAM X moves 0.5 m in the old frame, rotated by the alignment. + for (int i = 0; i < 50; i++) { + fresh.position.x += 0.01f; + REQUIRE(s.step(true, fresh, &out)); + } + CHECK(dist(out.position, held) == Catch::Approx(0.5f).margin(1e-3)); + CHECK(out.position.y == Catch::Approx(held.y).margin(1e-4)); +} + +TEST_CASE("Rejection rules: jump, distance cap and non-finite") +{ + xrt_pose out; + rift_s_slam_guard_reason reason; + { + Sim s; + xrt_pose head = {{0, 0, 0, 1}, {0, 0, 0}}; + REQUIRE(s.settle(head) > 0); + head.position.z += 0.6f; + CHECK_FALSE(s.step(true, head, &out, &reason)); + CHECK(reason == RIFT_S_SLAM_GUARD_JUMP); + CHECK(s.resets == 1); + } + { + // Slow drift (1 m/s) is not a speed outlier but stops at the 5 m cap. + Sim s; + xrt_pose head = {{0, 0, 0, 1}, {0, 0, 0}}; + REQUIRE(s.settle(head) > 0); + bool used = true; + while (used) { + head.position.x += 1.0f * FRAME / 1e9f; + used = s.step(true, head, &out, &reason); + REQUIRE(m_vec3_len(out.position) <= RIFT_S_SLAM_GUARD_MAX_DISTANCE_M); + } + CHECK(reason == RIFT_S_SLAM_GUARD_TOO_FAR); + } + { + // The reported slow sink: 1 m/s straight down stops 0.3 m under the nominal floor. + Sim s; + s.no_pose = rift_s_slam_guard_no_pose_position(1.6f); // floor at y = -1.6 in the tracker frame + xrt_pose head = {{0, 0, 0, 1}, {0, 0, 0}}; + REQUIRE(s.settle(head) > 0); + bool used = true; + while (used) { + head.position.y -= 1.0f * FRAME / 1e9f; + used = s.step(true, head, &out, &reason); + } + CHECK(reason == RIFT_S_SLAM_GUARD_HEIGHT); + CHECK(out.position.y >= -1.6f - RIFT_S_SLAM_GUARD_BELOW_FLOOR_M); + // Crouching to the floor is still fine. + Sim c; + c.no_pose = s.no_pose; + xrt_pose crouch = {{0, 0, 0, 1}, {0, 0, 0}}; + REQUIRE(c.settle(crouch) > 0); + for (int i = 0; i < 150; i++) { + crouch.position.y -= 1.0f * FRAME / 1e9f; + REQUIRE(c.step(true, crouch, &out)); + } + } + { + Sim s; + xrt_pose head = {{0, 0, 0, 1}, {0, 0, 0}}; + REQUIRE(s.settle(head) > 0); + head.position.x = NAN; + CHECK_FALSE(s.step(true, head, &out, &reason)); + CHECK(reason == RIFT_S_SLAM_GUARD_NOT_FINITE); + CHECK(std::isfinite(out.position.x)); + } +} + +TEST_CASE("Lost SLAM pose and user-chosen 3DoF hold without a divergence") +{ + Sim s; + xrt_pose head = {{0, 0, 0, 1}, {0.2f, 0, 0}}; + REQUIRE(s.settle(head) > 0); + xrt_pose out; + head.position.x += 0.01f; + REQUIRE(s.step(true, head, &out)); + const xrt_vec3 shown = out.position; + rift_s_slam_guard_reason reason; + CHECK_FALSE(s.step(false, head, &out, &reason)); + CHECK(reason == RIFT_S_SLAM_GUARD_NO_POSE); + CHECK(s.g.divergences == 0); + CHECK(s.resets == 0); + CHECK(dist(out.position, shown) < 1e-5f); + + Sim u; + REQUIRE(u.settle(head) > 0); + rift_s_slam_guard_suspend(&u.g, &u.fusion); + CHECK(u.g.state == RIFT_S_SLAM_GUARD_DIVERGED); + CHECK(u.g.divergences == 0); + CHECK_FALSE(rift_s_slam_guard_take_reset(&u.g, u.ts)); +} + +TEST_CASE("Feature gate: face-down or covered cameras hold, features back re-init") +{ + rift_s_slam_feature_gate gate = {}; + int64_t ts = 0; + auto feed = [&](int count, int frames) { + rift_s_slam_feature_event last = RIFT_S_SLAM_FEATURES_NO_CHANGE; + for (int i = 0; i < frames; i++) { + ts += 33 * MS; + rift_s_slam_feature_event e = rift_s_slam_feature_gate_update(&gate, true, count, ts); + if (e != RIFT_S_SLAM_FEATURES_NO_CHANGE) { + last = e; + } + } + return last; + }; + CHECK(feed(150, 30) == RIFT_S_SLAM_FEATURES_NO_CHANGE); + // A brief dip (a hand over the cameras for 0.3 s) is ignored. + CHECK(feed(3, 9) == RIFT_S_SLAM_FEATURES_NO_CHANGE); + CHECK(feed(150, 5) == RIFT_S_SLAM_FEATURES_NO_CHANGE); + CHECK_FALSE(gate.lost); + // Face down: lost after the dwell. + CHECK(feed(2, 30) == RIFT_S_SLAM_FEATURES_LOST); + CHECK(gate.lost); + // Unknown counts and repeated timestamps change nothing. + CHECK(rift_s_slam_feature_gate_update(&gate, false, 200, ts + 10 * MS * 1000) == + RIFT_S_SLAM_FEATURES_NO_CHANGE); + CHECK(rift_s_slam_feature_gate_update(&gate, true, 200, ts) == RIFT_S_SLAM_FEATURES_NO_CHANGE); + // Picked up again. + CHECK(feed(120, 30) == RIFT_S_SLAM_FEATURES_RETURNED); + CHECK_FALSE(gate.lost); +} + +TEST_CASE("Source guard ignores duplicate and historical timestamps") +{ + Sim s; + xrt_pose raw = XRT_POSE_IDENTITY, out; + REQUIRE(s.settle(raw) > 0); + auto before = s.g; + xrt_pose camera_query = raw; + camera_query.position.x += 0.04f; + rift_s_slam_guard_reason reason; + CHECK(rift_s_slam_guard_update(&s.g, true, &camera_query, before.good_ns - 1000000, + &s.fusion, &s.no_pose, &out, &reason)); + CHECK(s.g.good_ns == before.good_ns); + CHECK(s.g.divergences == before.divergences); + CHECK(dist(out.position, before.good.position) < 1e-6); +} + +TEST_CASE("SLAM cannot publish invalid quaternions or a gravity flip") +{ + for (auto q : {xrt_quat{0, 0, 0, 0}, xrt_quat{1, 0, 0, 0}, xrt_quat{0, 0, 0, 2}}) { + Sim s; + xrt_pose raw = XRT_POSE_IDENTITY, out; + REQUIRE(s.settle(raw) > 0); + raw.orientation = q; + rift_s_slam_guard_reason reason; + CHECK_FALSE(s.step(true, raw, &out, &reason)); + CHECK(reason == RIFT_S_SLAM_GUARD_TILT); + CHECK(out.orientation.w == Catch::Approx(1)); + CHECK(s.g.state == RIFT_S_SLAM_GUARD_DIVERGED); + } + // A physical upside-down head has the same IMU tilt and must remain valid. + xrt_pose inverted = {{1, 0, 0, 0}, {0, 1, 0}}; + CHECK(rift_s_slam_guard_pose_usable(&inverted, &inverted.orientation)); +} + +TEST_CASE("Saved boundary publishes only its recovered world frame after restart") +{ + Sim s; + s.no_pose = {0, 1.6f, 0}; + s.g.require_relocalization = true; + xrt_pose raw = {yaw_quat(-0.8f), {0.4f, 0.2f, -0.3f}}, out; + rift_s_slam_guard_reason reason; + for (unsigned i = 0; i < 160; ++i) + CHECK_FALSE(s.step(true, raw, &out, &reason)); + CHECK(reason == RIFT_S_SLAM_GUARD_RELOCALIZING); + CHECK_FALSE(s.g.world_valid); + CHECK_FALSE(s.g.boundary_confirmed); + CHECK(s.g.resets == 0); + + xrt_pose world = {yaw_quat(0.6f), {-0.7f, 1.3f, 0.3f}}; + rift_s_slam_guard_set_world(&s.g, &world); + REQUIRE(s.step(true, raw, &out)); + xrt_pose expected; + math_pose_transform(&world, &raw, &expected); + CHECK(dist(out.position, expected.position) < 1e-6); + CHECK(yaw_of(out.orientation) == Catch::Approx(-0.2f).margin(1e-6)); + CHECK_FALSE(s.g.world_transition); + CHECK(s.g.divergences == 0); + + // The replacement VIO gauge must be matched again, while the saved standing origin stays put. + s.g.reset_pending = true; + REQUIRE(rift_s_slam_guard_take_reset(&s.g, s.ts)); + rift_s_slam_guard_suspend(&s.g, &s.fusion); + raw = {yaw_quat(1.2f), {-0.3f, 0.1f, 0.2f}}; + for (unsigned i = 0; i < 160; ++i) + CHECK_FALSE(s.step(true, raw, &out, &reason)); + CHECK(reason == RIFT_S_SLAM_GUARD_RELOCALIZING); + CHECK_FALSE(s.g.world_valid); + CHECK_FALSE(s.g.boundary_confirmed); + xrt_pose inverse, recovered; + math_pose_invert(&raw, &inverse); + math_pose_transform(&expected, &inverse, &recovered); + rift_s_slam_guard_set_world(&s.g, &recovered); + REQUIRE(s.step(true, raw, &out)); + CHECK(dist(out.position, expected.position) < 1e-6); + CHECK(yaw_of(out.orientation) == Catch::Approx(yaw_of(expected.orientation)).margin(1e-6)); + CHECK_FALSE(s.g.world_transition); +} + +TEST_CASE("Restoring saved bounds revokes a room setup's unconfirmed frame") +{ + Sim s; + s.no_pose = {0, 1.6f, 0}; + xrt_pose raw = XRT_POSE_IDENTITY, out; + REQUIRE(s.settle(raw) > 0); + REQUIRE(s.g.world_valid); + s.g.require_relocalization = true; + CHECK_FALSE(rift_s_slam_guard_query(&s.g, &raw, s.ts, &s.fusion, &s.no_pose, &out)); + CHECK_FALSE(s.step(true, raw, &out)); + CHECK_FALSE(s.g.world_valid); + CHECK_FALSE(s.g.world_transition); + xrt_pose saved{yaw_quat(0.4f), {-0.2f, 1.6f, 0.3f}}; + rift_s_slam_guard_set_world(&s.g, &saved); + REQUIRE(s.settle(raw) > 0); + REQUIRE(s.step(true, raw, &out)); + CHECK(dist(out.position, saved.position) < 1e-6); + CHECK(yaw_of(out.orientation) == Catch::Approx(0.4f).margin(1e-6)); + CHECK_FALSE(s.g.world_transition); +} + +TEST_CASE("A bad predicted query cannot reset or displace the world") +{ + Sim s; + xrt_pose raw = XRT_POSE_IDENTITY, out; + REQUIRE(s.settle(raw) > 0); + auto before = s.g; + raw.position.x = 30; + CHECK_FALSE(rift_s_slam_guard_query(&s.g, &raw, s.ts + 5 * MS, &s.fusion, &s.no_pose, &out)); + CHECK(s.g.resets == before.resets); + CHECK(s.g.good_ns == before.good_ns); + CHECK(dist(s.g.align.position, before.align.position) < 1e-6); +} + +TEST_CASE("A backend with no startup poses requests recovery within two seconds") +{ + Sim s; + xrt_pose out; + for (int i = 0; i < 600; ++i) + CHECK_FALSE(s.step(false, xrt_pose{}, &out)); + CHECK(s.resets >= 2); + CHECK(s.resets <= 3); + CHECK(dist(out.position, s.no_pose) < 1e-6f); +} + +TEST_CASE("Persistent gravity failure retries recovery and retains the world pose") +{ + Sim s; + xrt_pose head = {yaw_quat(.4f), {.3f, .05f, -.2f}}, out; + REQUIRE(s.settle(head) > 0); + const xrt_pose held = s.g.good; + xrt_vec3 axis{1, 0, 0}; + math_quat_from_angle_vector(3.14159265f, &axis, &head.orientation); + for (int i = 0; i < 600; ++i) { + CHECK_FALSE(s.step(true, head, &out)); + CHECK(dist(out.position, held.position) < 1e-6f); + } + CHECK(s.resets >= 3); + CHECK(s.resets <= 4); + CHECK(s.settle({yaw_quat(-1), {0, 0, 0}}) > 0); + CHECK(dist(s.g.good.position, held.position) < 1e-5f); +} + +TEST_CASE("A fresh backend gets a full recovery interval without moving the world") +{ + Sim s; + xrt_pose head = {yaw_quat(.4f), {.3f, .05f, -.2f}}, out; + REQUIRE(s.settle(head) > 0); + const xrt_pose held = s.g.good; + rift_s_slam_guard_suspend(&s.g, &s.fusion); + for (int i = 0; i < 250; ++i) + CHECK_FALSE(s.step(false, xrt_pose{}, &out)); + const int resets = s.resets; + rift_s_slam_guard_backend_started(&s.g, s.ts); + for (int i = 0; i < 180; ++i) + CHECK_FALSE(s.step(false, xrt_pose{}, &out)); + CHECK(s.resets == resets); + CHECK(dist(out.position, held.position) < 1e-6f); + for (int i = 0; i < 10; ++i) + CHECK_FALSE(s.step(false, xrt_pose{}, &out)); + CHECK(s.resets == resets + 1); + CHECK(dist(out.position, held.position) < 1e-6f); +} diff --git a/tests/tests_rift_s_tracking.cpp b/tests/tests_rift_s_tracking.cpp new file mode 100644 index 000000000..ce75684ca --- /dev/null +++ b/tests/tests_rift_s_tracking.cpp @@ -0,0 +1,2120 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "rift_s_tracker.h" +#include "rift_s_util.h" +#include "rift_s_controller.h" +#include "rift_s_hmd.h" +#include "math/m_clock_tracking.h" +#ifdef TEST_RIFT_S_STEAMVR_POSE +#include "ovrd_pose.hpp" +#endif +#include +#include +#include +#include +#include +#include +#include + +TEST_CASE("Rift S optical yaw takes ten percent of the shortest world yaw") +{ + const xrt_vec3 axis = {0, 1, 0}; + xrt_quat optical; + for (float angle : {170.f, -170.f}) { + math_quat_from_angle_vector(DEG_TO_RAD(angle), &axis, &optical); + for (float sign : {1.f, -1.f}) { + xrt_quat imu = {0, 0, 0, 1}; + xrt_quat equivalent = {sign * optical.x, sign * optical.y, sign * optical.z, sign * optical.w}; + CHECK(rift_s_apply_optical_yaw(&imu, &equivalent) == Catch::Approx(DEG_TO_RAD(angle * 0.1f))); + CHECK(2 * atan2f(imu.y, imu.w) == Catch::Approx(DEG_TO_RAD(angle * 0.1f))); + } + } +} + +TEST_CASE("Rift S optical yaw preserves gravity alignment while tilted") +{ + const xrt_vec3 tilt_axis = {1, 0, 0}; + const xrt_vec3 yaw_axis = {0, 1, 0}; + xrt_quat imu, yaw, optical; + math_quat_from_angle_vector(DEG_TO_RAD(80), &tilt_axis, &imu); + math_quat_from_angle_vector(DEG_TO_RAD(90), &yaw_axis, &yaw); + math_quat_rotate(&yaw, &imu, &optical); + xrt_quat inverse; + xrt_vec3 before, after; + math_quat_invert(&imu, &inverse); + math_quat_rotate_vec3(&inverse, &yaw_axis, &before); + CHECK(rift_s_apply_optical_yaw(&imu, &optical) == Catch::Approx(DEG_TO_RAD(9))); + math_quat_invert(&imu, &inverse); + math_quat_rotate_vec3(&inverse, &yaw_axis, &after); + CHECK(after.x == Catch::Approx(before.x).margin(1e-6)); + CHECK(after.y == Catch::Approx(before.y).margin(1e-6)); + CHECK(after.z == Catch::Approx(before.z).margin(1e-6)); +} + +TEST_CASE("Rift S optical yaw applies small corrections and ignores the deadband") +{ + const xrt_vec3 axis = {0, 1, 0}; + xrt_quat imu = {0, 0, 0, 1}, optical; + math_quat_from_angle_vector(DEG_TO_RAD(2), &axis, &optical); + CHECK(rift_s_apply_optical_yaw(&imu, &optical) == Catch::Approx(DEG_TO_RAD(2))); + CHECK(rift_s_apply_optical_yaw(&imu, &optical) == 0); +} + +enum u_logging_level rift_s_log_level = U_LOGGING_ERROR; + +static xrt_quat test_head_orientation = XRT_QUAT_IDENTITY; + +static xrt_result_t +get_test_head_pose(xrt_device *, xrt_input_name, int64_t, xrt_space_relation *out) +{ + *out = XRT_SPACE_RELATION_ZERO; + out->pose.orientation = test_head_orientation; + out->pose.position = {1, 1.6f, 2}; + out->relation_flags = static_cast( + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + return XRT_SUCCESS; +} + +TEST_CASE("Rift S controller coordinates hold after loss and stay invalid before acquisition") +{ + rift_s_hmd hmd = {}; + hmd.base.get_tracked_pose = get_test_head_pose; + rift_s_system sys = {}; + sys.hmd = &hmd; + os_mutex_init(&sys.dev_mutex); + rift_s_controller ctrl = {}; + ctrl.sys = &sys; + ctrl.gravity_initialized = true; + ctrl.pose.orientation.w = 1; + ctrl.P_imu_device = XRT_POSE_IDENTITY; + ctrl.P_device_aim = ctrl.P_device_grip = XRT_POSE_IDENTITY; + os_mutex_init(&ctrl.mutex); + + for (auto hand : {XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER, XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER}) { + ctrl.base.device_type = hand; + for (auto input : {XRT_INPUT_TOUCH_GRIP_POSE, XRT_INPUT_TOUCH_AIM_POSE}) { + for (auto optical_ts : {uint64_t{0}, uint64_t{1000000000}, uint64_t{1999000000}}) { + // One optical fix at optical_ts (0: never fixed). + const xrt_vec3 fix = {0.3f, 1.2f, 1.5f}; + rift_s_position_filter_reset(&ctrl.position_filter); + ctrl.track_vision_updates = ctrl.track_reliable_updates = 0; + ctrl.last_fix_ns = 0; + if (optical_ts != 0) { + const xrt_vec3 variance = {1e-5f, 1e-5f, 1e-5f}; + rift_s_position_filter_fix(&ctrl.position_filter, optical_ts, &fix, &variance, + 0.15f); + ctrl.track_vision_updates = 1; + ctrl.last_fix_ns = optical_ts; + } + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base, input, 2000000000, &rel) == + XRT_SUCCESS); + CHECK((rel.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0); + CHECK((rel.relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) != 0); + CHECK(((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0) == + (optical_ts != 0)); + // Optical confidence expires, but the held position remains usable. + const bool tracked = optical_ts == 1999000000; + const bool held = optical_ts == 1000000000; + CHECK(((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0) == tracked); + CHECK(rel.pose.position.z == Catch::Approx(tracked || held ? 1.5f : 0)); + CHECK(rel.pose.position.x == Catch::Approx(tracked || held ? 0.3f : 0)); +#ifdef TEST_RIFT_S_STEAMVR_POSE + vr::DriverPose_t pose = {}; + pose.poseIsValid = true; + pose.result = vr::TrackingResult_Running_OK; + apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true); + CHECK(pose.poseIsValid == (tracked || held)); + CHECK(pose.result == (tracked || held ? vr::TrackingResult_Running_OK + : vr::TrackingResult_Running_OutOfRange)); + CHECK(pose.vecPosition[0] == Catch::Approx(tracked || held ? rel.pose.position.x : 0)); + CHECK(pose.vecPosition[2] == Catch::Approx(tracked || held ? rel.pose.position.z : 0)); + CHECK(pose.qRotation.w == Catch::Approx(1)); + // HMD call sites retain their original tracked-position requirement. + vr::DriverPose_t head_pose = {}; + apply_pose(&rel, &head_pose); + CHECK(head_pose.vecPosition[2] == Catch::Approx(tracked ? rel.pose.position.z : 0)); +#endif + } + } + } + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); + os_mutex_destroy(&sys.dev_mutex); +} + +TEST_CASE("Rift S mature track bridges a short optical gap and clears optical confidence after loss") +{ + // r18 worn log: positional validity cleared 250 ms after every fix, and fixes came about every + // 0.6 s, so both hands were invisible 60% of the time. A track with more than ten fixes keeps + // its IMU-bridged position through a 400 ms gap; a young track keeps the 250 ms ladder step. + rift_s_hmd hmd = {}; + hmd.base.get_tracked_pose = get_test_head_pose; + rift_s_system sys = {}; + sys.hmd = &hmd; + os_mutex_init(&sys.dev_mutex); + rift_s_controller ctrl = {}; + ctrl.sys = &sys; + ctrl.gravity_initialized = true; + ctrl.pose.orientation.w = 1; + ctrl.P_imu_device = ctrl.P_device_aim = ctrl.P_device_grip = XRT_POSE_IDENTITY; + os_mutex_init(&ctrl.mutex); + const timepoint_ns fix_ns = 1000000000; + const xrt_vec3 fix = {0.3f, 1.2f, 1.5f}, variance = {1e-5f, 1e-5f, 1e-5f}; + auto valid_after = [&](uint32_t updates, time_duration_ns gap) { + rift_s_position_filter_reset(&ctrl.position_filter); + rift_s_position_filter_fix(&ctrl.position_filter, fix_ns, &fix, &variance, 0.15f); + ctrl.track_vision_updates = ctrl.track_reliable_updates = updates; + ctrl.last_fix_ns = fix_ns; + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_TOUCH_GRIP_POSE, fix_ns + gap, &rel) == + XRT_SUCCESS); + return (rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0; + }; + CHECK(valid_after(30, 400 * U_TIME_1MS_IN_NS)); + CHECK(valid_after(30, 490 * U_TIME_1MS_IN_NS)); + CHECK_FALSE(valid_after(30, 510 * U_TIME_1MS_IN_NS)); + CHECK_FALSE(valid_after(1, 300 * U_TIME_1MS_IN_NS)); + os_mutex_destroy(&ctrl.mutex); + os_mutex_destroy(&sys.dev_mutex); +} + +TEST_CASE("Rift S stationary startup calibrates low-magnitude gravity and gyro bias") +{ + for (const xrt_vec3 accel : {xrt_vec3{7.6148f, 3.9714f, 1.6924f}, xrt_vec3{-6.9321f, 6.7900f, 1.8497f}}) { + rift_s_controller ctrl = {}; + m_imu_3dof_init(&ctrl.fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + os_mutex_init(&ctrl.mutex); + ctrl.accel = accel; + xrt_space_relation relation; + CHECK(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, 1000000000, + &relation) == XRT_ERROR_POSE_NOT_ACTIVE); + ctrl.gyro = {-0.014f, 0.003f, 0.017f}; + for (int i = 0; i < 125; i++) { + rift_s_controller_update_fusion(&ctrl, 1000000000ULL + i * 2000000ULL); + rift_s_controller_record_attitude(&ctrl, 1000000000ULL + i * 2000000ULL, + 1000000000ULL + i * 2000000ULL); + } + CHECK_FALSE(ctrl.gravity_initialized); + rift_s_controller_update_fusion(&ctrl, 1250000000ULL); + rift_s_controller_record_attitude(&ctrl, 1250000000ULL, 1250000000ULL); + REQUIRE(ctrl.gravity_initialized); + CHECK(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, 1250000000, + &relation) == XRT_SUCCESS); + for (int i = 126; i < 15000; i++) { + rift_s_controller_update_fusion(&ctrl, 1000000000ULL + i * 2000000ULL); + rift_s_controller_record_attitude(&ctrl, 1000000000ULL + i * 2000000ULL, + 1000000000ULL + i * 2000000ULL); + } + xrt_vec3 world_accel; + math_quat_rotate_vec3(&ctrl.pose.orientation, &accel, &world_accel); + CHECK(world_accel.x == Catch::Approx(0).margin(0.15)); + CHECK(world_accel.z == Catch::Approx(0).margin(0.15)); + CHECK(world_accel.y * ctrl.gravity_accel_scale == Catch::Approx(MATH_GRAVITY_M_S2).margin(1e-3)); + // A reading this far from gravity (8.75 m/s^2) is an offset accelerometer: the fusion does + // not level to it until the offset is known (see the optical levelling test). The 9.88 m/s^2 + // reading keeps gravity levelling. + const bool offset = fabsf(m_vec3_len(accel) - (float)MATH_GRAVITY_M_S2) > 0.4f; + CHECK(ctrl.accel_offset == offset); + CHECK(((ctrl.fusion.flags & M_IMU_3DOF_USE_GRAVITY_DUR_20MS) == 0) == offset); + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); + m_imu_3dof_close(&ctrl.fusion); + } +} + +TEST_CASE("Rift S rest statistics expose an orientation-dependent accelerometer error") +{ + // A 1 m/s^2 bias along the IMU y axis: 8.8 m/s^2 lying one way up, 10.8 the other way. + rift_s_rest_stats rest = {}; + const xrt_vec3 bias = {0, -1.0f, 0}, still = {0.01f, 0, 0}, turning = {0.5f, 0, 0}; + timepoint_ns t = 1000000000; + auto hold = [&](xrt_vec3 up, int samples, xrt_vec3 gyro) { + for (int i = 0; i < samples; i++, t += 2000000) { + xrt_vec3 accel = m_vec3_add(m_vec3_mul_scalar(up, MATH_GRAVITY_M_S2), bias); + accel.x += (i % 2 ? 0.05f : -0.05f); + rift_s_rest_stats_push(&rest, t, &accel, &gyro); + } + }; + hold({0, 1, 0}, 100, still); + CHECK(rest.windows == 0); // 200 ms is not yet a rest + hold({0, 1, 0}, 100, still); + CHECK(rest.windows == 1); + hold({0, 0, 1}, 200, turning); // moving: ignored + CHECK(rest.windows == 1); + hold({0, -1, 0}, 200, still); + hold({1, 0, 0}, 200, still); + CHECK(rest.windows == 3); + CHECK(rest.min_mps2 == Catch::Approx(MATH_GRAVITY_M_S2 - 1).margin(0.01)); + CHECK(rest.max_mps2 == Catch::Approx(MATH_GRAVITY_M_S2 + 1).margin(0.01)); + CHECK(rest.min_dir.y == Catch::Approx(1).margin(0.01)); + CHECK(rest.max_dir.y == Catch::Approx(-1).margin(0.01)); +} + +TEST_CASE("Rift S accelerometer bias from rest windows explains the right controller's worn readings") +{ + // Rest windows of the right controller in the r6 worn log: 8.802 m/s^2 along (0.87,0.43,0.23) + // at startup, and the extremes 7.876 along (-0.29,0.95,-0.15) and 9.533 along (0.94,0.06,0.33). + // The left read 9.79..9.98. One bias explains all three exactly. + const xrt_vec3 logged[3] = {{7.678f, 3.795f, 2.030f}, {-2.274f, 7.448f, -1.176f}, {8.979f, 0.573f, 3.152f}}; + const xrt_vec3 bias = {-0.067f, -1.983f, 0.355f}; + for (const auto &m : logged) + CHECK(m_vec3_len(m_vec3_sub(m, bias)) == Catch::Approx(MATH_GRAVITY_M_S2).margin(0.01)); + auto window = [&](xrt_vec3 dir) { + math_vec3_normalize(&dir); + return m_vec3_add(m_vec3_mul_scalar(dir, MATH_GRAVITY_M_S2), bias); + }; + + rift_s_accel_bias estimate = {}; + // One orientation, however often, observes nothing. + for (int i = 0; i < 40; i++) + rift_s_accel_bias_add_window(&estimate, &logged[0]); + CHECK(estimate.count == 1); + CHECK_FALSE(estimate.valid); + rift_s_accel_bias_add_window(&estimate, &logged[1]); + CHECK_FALSE(estimate.valid); // two directions + rift_s_accel_bias_add_window(&estimate, &logged[2]); + REQUIRE(estimate.valid); + CHECK(estimate.rms_before_mps2 > 1.0f); + CHECK(estimate.rms_after_mps2 < 0.05f); + // Three windows leave some of the bias to the prior; more orientations pin it down. + CHECK(m_vec3_len(m_vec3_sub(estimate.bias, bias)) < 0.5f); + for (xrt_vec3 dir : {xrt_vec3{0.3f, 0.7f, 0.65f}, xrt_vec3{-0.6f, 0.5f, 0.6f}, xrt_vec3{0.2f, 0.4f, -0.9f}}) { + xrt_vec3 mean = window(dir); + rift_s_accel_bias_add_window(&estimate, &mean); + } + REQUIRE(estimate.valid); + CHECK(m_vec3_len(m_vec3_sub(estimate.bias, bias)) < 0.1f); + // The corrected gravity direction is within half a degree in every stored orientation, where + // the raw reading was off by up to 12 degrees. + float worst_raw = 0, worst = 0; + for (unsigned i = 0; i < estimate.count; i++) { + xrt_vec3 truth = m_vec3_sub(estimate.means[i], bias), raw = estimate.means[i], + fixed = m_vec3_sub(estimate.means[i], estimate.bias); + worst_raw = std::max(worst_raw, m_vec3_angle(truth, raw)); + worst = std::max(worst, m_vec3_angle(truth, fixed)); + } + CHECK(RAD_TO_DEG(worst_raw) > 8); + CHECK(RAD_TO_DEG(worst) < 0.5); + + // A calibrated accelerometer gets (almost) no correction. + rift_s_accel_bias good = {}; + for (xrt_vec3 dir : {xrt_vec3{0, 1, 0}, xrt_vec3{1, 0, 0}, xrt_vec3{0, 0.7f, 0.7f}, xrt_vec3{-0.7f, 0.7f, 0}}) { + math_vec3_normalize(&dir); + xrt_vec3 mean = m_vec3_mul_scalar(dir, MATH_GRAVITY_M_S2); + rift_s_accel_bias_add_window(&good, &mean); + } + REQUIRE(good.valid); + CHECK(m_vec3_len(good.bias) < 0.01f); +} + +TEST_CASE("Rift S fusion uses the estimated accelerometer bias") +{ + rift_s_controller ctrl = {}; + m_imu_3dof_init(&ctrl.fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + const xrt_vec3 bias = {-0.067f, -1.983f, 0.355f}, still = {0, 0, 0}; + timepoint_ns t = 1000000000; + auto hold = [&](xrt_vec3 dir, int samples) { + math_vec3_normalize(&dir); + ctrl.accel = m_vec3_add(m_vec3_mul_scalar(dir, MATH_GRAVITY_M_S2), bias); + ctrl.gyro = still; + for (int i = 0; i < samples; i++, t += 2000000) + rift_s_controller_update_fusion(&ctrl, t); + }; + hold({0.87f, 0.43f, 0.23f}, 200); + REQUIRE(ctrl.gravity_initialized); + CHECK_FALSE(ctrl.accel_bias.valid); + // The startup scale normalizes only the startup orientation. + hold({0.94f, 0.06f, 0.33f}, 200); + CHECK(m_vec3_len(ctrl.fusion.last.accel) > MATH_GRAVITY_M_S2 + 0.5f); + hold({-0.29f, 0.95f, -0.15f}, 200); + REQUIRE(ctrl.accel_bias.valid); + hold({0.94f, 0.06f, 0.33f}, 10); + CHECK(m_vec3_len(ctrl.fusion.last.accel) == Catch::Approx(MATH_GRAVITY_M_S2).margin(0.1)); + m_imu_3dof_close(&ctrl.fusion); +} + +TEST_CASE("Rift S moving startup does not wait indefinitely for stationary samples") +{ + rift_s_controller ctrl = {}; + m_imu_3dof_init(&ctrl.fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + ctrl.accel = {0, 9.82f, 0}; + ctrl.gyro = {0.4f, 0, 0}; + for (int i = 0; i < 200; i++) { + rift_s_controller_update_fusion(&ctrl, 1000000000ULL + i * 2000000ULL); + } + CHECK(ctrl.gravity_initialized); + ctrl.gyro = {0, 0, 0}; + for (int i = 200; i < 400; i++) { + ctrl.accel.x = i % 2 ? 1 : -1; + rift_s_controller_update_fusion(&ctrl, 1000000000ULL + i * 2000000ULL); + } + CHECK(ctrl.gravity_initialized); + m_imu_3dof_close(&ctrl.fusion); +} + +static void +record_test_attitude(rift_s_controller &ctrl, int sample, float yaw) +{ + const xrt_vec3 axis = {0, 1, 0}; + math_quat_from_angle_vector(DEG_TO_RAD(yaw), &axis, &ctrl.fusion.rot); + ctrl.pose.orientation = ctrl.fusion.rot; + const timepoint_ns device_ns = 1000000000LL + sample * 2000000LL; + REQUIRE(rift_s_controller_record_attitude(&ctrl, device_ns, device_ns + 10000000000LL)); +} + +TEST_CASE("Rift S counts repeated IMU reports apart from clock errors") +{ + rift_s_controller ctrl = {}; + ctrl.device_type = RIFT_S_DEVICE_LEFT_CONTROLLER; + os_mutex_init(&ctrl.mutex); + auto imu_report = [&](uint32_t device_us, timepoint_ns arrival_ns) { + rift_s_controller_report_t report = {}; + report.num_info = 1; + report.info[0].block_id = RIFT_S_CTRL_IMU; + report.info[0].imu.timestamp = device_us; + rift_s_controller_handle_report(&ctrl, arrival_ns, &report); + }; + imu_report(1000, 1000000000); + imu_report(3000, 1002000000); + imu_report(3000, 1002100000); // radio retransmit + imu_report(3000, 1002200000); + CHECK(ctrl.diagnostics.reject_duplicate == 2); + CHECK(ctrl.diagnostics.reject_clock == 0); + imu_report(2000, 1004000000); // device time running backwards + CHECK(ctrl.diagnostics.reject_clock == 1); + CHECK(ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_BACKWARDS] == 1); + CHECK(ctrl.diagnostics.backwards_run == 1); + CHECK(ctrl.diagnostics.last_step_us == 2000); + imu_report(5000, 1004000000); + CHECK(ctrl.last_imu_device_time_ns == 5000000); + CHECK(ctrl.diagnostics.backwards_run == 0); + CHECK(ctrl.diagnostics.last_step_us == 2000); + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); +} + +TEST_CASE("Rift S IMU clock resync needs a run or a large repeated backwards step") +{ + CHECK_FALSE(rift_s_imu_clock_needs_resync(2000, 1)); // a late resend + CHECK_FALSE(rift_s_imu_clock_needs_resync(50000, RIFT_S_IMU_RESYNC_RUN - 1)); + CHECK(rift_s_imu_clock_needs_resync(50000, RIFT_S_IMU_RESYNC_RUN)); + CHECK_FALSE(rift_s_imu_clock_needs_resync(1521000000u, 1)); // one stray sample is not enough + CHECK(rift_s_imu_clock_needs_resync(1521000000u, 2)); +} + +TEST_CASE("Rift S IMU clock re-anchors after a stale first timestamp instead of rejecting for minutes") +{ + // r3 worn log after a VM reset without a headset power cycle: each controller's first + // accepted sample was ~1521 s ahead, and every later sample was rejected as clock_backwards + // (backwards_us shrinking by 1e6 per second, ~25 minutes of a hand glued to the head). + rift_s_controller ctrl = {}; + ctrl.device_type = RIFT_S_DEVICE_RIGHT_CONTROLLER; + os_mutex_init(&ctrl.mutex); + auto imu_report = [&](uint32_t device_us, timepoint_ns arrival_ns) { + rift_s_controller_report_t report = {}; + report.num_info = 1; + report.info[0].block_id = RIFT_S_CTRL_IMU; + report.info[0].imu.timestamp = device_us; + rift_s_controller_handle_report(&ctrl, arrival_ns, &report); + }; + const uint32_t base_us = 2690011380u, stale_us = base_us + 1521000000u; + timepoint_ns arrival = 5000000000LL; + imu_report(stale_us, arrival); + REQUIRE(ctrl.imu_time_valid); + // Dependent state captured against the stale anchor. + const xrt_vec3 here = {0.2f, 1.1f, -0.3f}, variance = {1e-5f, 1e-5f, 1e-5f}; + rift_s_position_filter_reset(&ctrl.position_filter); + rift_s_position_filter_fix(&ctrl.position_filter, arrival, &here, &variance, 0.15f); + ctrl.attitude_history_count = 3; + ctrl.clock_stable_samples = 32; + ctrl.optical_heading_valid = true; + ctrl.imu_to_host_ns = 123; + timepoint_ns previous = ctrl.last_imu_device_time_ns; + uint32_t accepted = 0; + for (int i = 1; i <= 500; i++) { + arrival += 2000000; + imu_report(base_us + i * 2000u, arrival); + // The 64-bit device time never runs backwards, so fusion time keeps moving forward. + CHECK(ctrl.last_imu_device_time_ns >= previous); + if (ctrl.last_imu_device_time_ns > previous) + accepted++; + previous = ctrl.last_imu_device_time_ns; + } + CHECK(ctrl.diagnostics.clock_resyncs == 1); + CHECK(ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_BACKWARDS] == 2); // the second sample, then resync + CHECK(accepted == 499); + CHECK(ctrl.diagnostics.backwards_run == 0); + CHECK(ctrl.imu_timestamp32 == base_us + 500 * 2000u); + CHECK(ctrl.diagnostics.last_step_us == 2000); + // Reset like a clock discontinuity: mapping, capture-time history and heading prior. + CHECK(ctrl.imu_clock == nullptr); + CHECK(ctrl.imu_to_host_ns == 0); + CHECK(ctrl.attitude_history_count == 0); + CHECK(ctrl.clock_stable_samples == 0); + CHECK_FALSE(ctrl.optical_heading_valid); + CHECK(ctrl.position_filter.frozen); + // After the resync, ordinary late resends are still rejected without another resync. + imu_report(base_us + 499 * 2000u, arrival + 1000000); + CHECK(ctrl.diagnostics.clock_resyncs == 1); + CHECK(ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_BACKWARDS] == 3); + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); +} + +TEST_CASE("Rift S IMU clock re-anchors after a run of small backwards steps") +{ + rift_s_controller ctrl = {}; + ctrl.device_type = RIFT_S_DEVICE_LEFT_CONTROLLER; + os_mutex_init(&ctrl.mutex); + auto imu_report = [&](uint32_t device_us, timepoint_ns arrival_ns) { + rift_s_controller_report_t report = {}; + report.num_info = 1; + report.info[0].block_id = RIFT_S_CTRL_IMU; + report.info[0].imu.timestamp = device_us; + rift_s_controller_handle_report(&ctrl, arrival_ns, &report); + }; + timepoint_ns arrival = 1000000000; + imu_report(100000, arrival); + // The device clock restarts 50 ms earlier: every sample is behind the anchor. + for (int i = 1; i < RIFT_S_IMU_RESYNC_RUN; i++) + imu_report(50000 + i * 2000, arrival += 2000000); + CHECK(ctrl.diagnostics.clock_resyncs == 0); + CHECK(ctrl.diagnostics.backwards_run == RIFT_S_IMU_RESYNC_RUN - 1); + imu_report(50000 + RIFT_S_IMU_RESYNC_RUN * 2000, arrival += 2000000); + CHECK(ctrl.diagnostics.clock_resyncs == 1); + const timepoint_ns anchored = ctrl.last_imu_device_time_ns; + // Continued by the host time since the last accepted sample (the rejected run), never backwards. + CHECK(anchored == 100000000LL + RIFT_S_IMU_RESYNC_RUN * 2000000LL); + imu_report(50000 + (RIFT_S_IMU_RESYNC_RUN + 1) * 2000, arrival += 2000000); + CHECK(ctrl.last_imu_device_time_ns == anchored + 2000000); + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); +} + +TEST_CASE("Rift S reports use the firmware TrackedObject calibration in sensor units") +{ + rift_s_controller ctrl = {}; + os_mutex_init(&ctrl.mutex); + m_imu_3dof_init(&ctrl.fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + ctrl.have_calibration = ctrl.have_config = true; + ctrl.config.accel_scale = 1.0f / 1024; + ctrl.config.gyro_scale = 0.1220703125f; + // Recorded right-controller factory arrays. + ctrl.calibration.accel_calibration = {{{.99574226f, .04186834f, -.03562105f, -.03175851f, .00306439f, + -1.00078183f, -.04157540f, 1.00271152f, .00852858f}}, + {-.20133353f, .05943762f, .07639897f}, + 12}; + ctrl.calibration.gyro_calibration = {{{.99724316f, .04099295f, -.03803329f, -.02534426f, -.00046619f, + -.99736829f, -.04205823f, .99727071f, .00495981f}}, + {.00554654f, -.00502796f, .00356880f}, + 12}; + ctrl.gyro_calibration = ctrl.calibration.gyro_calibration; + ctrl.imu_descriptor_done = true; + // Deliberately different legacy fields must not select the calibration frame. + ctrl.calibration.accel.rectification = ctrl.calibration.gyro.rectification = {{1, 0, 0, 0, 1, 0, 0, 0, 1}}; + rift_s_controller_report_t report = {}; + report.num_info = 1; + report.info[0].block_id = RIFT_S_CTRL_IMU; + report.info[0].imu.timestamp = 123000; + const int16_t accel[3] = {321, -456, 789}, gyro[3] = {-100, 200, 300}; + for (int i = 0; i < 3; i++) { + report.info[0].imu.accel[i] = accel[i]; + report.info[0].imu.gyro[i] = gyro[i]; + } + rift_s_controller_handle_report(&ctrl, 123000000, &report); + CHECK(ctrl.accel.x == Catch::Approx(2.809778870).margin(1e-6)); + CHECK(ctrl.accel.y == Catch::Approx(-7.603138362).margin(1e-6)); + CHECK(ctrl.accel.z == Catch::Approx(-4.510852398).margin(1e-6)); + CHECK(ctrl.gyro.x == Catch::Approx(-0.224496913).margin(1e-6)); + CHECK(ctrl.gyro.y == Catch::Approx(-0.628577923).margin(1e-6)); + CHECK(ctrl.gyro.z == Catch::Approx(0.442303355).margin(1e-6)); + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + m_imu_3dof_close(&ctrl.fusion); + os_mutex_destroy(&ctrl.mutex); +} + +TEST_CASE("Rift S controller IMU waits for the descriptor, then falls back to factory gyro calibration") +{ + rift_s_controller ctrl = {}; + os_mutex_init(&ctrl.mutex); + m_imu_3dof_init(&ctrl.fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + ctrl.have_calibration = ctrl.have_config = true; + ctrl.config.accel_scale = 1.0f / 1024; + ctrl.config.gyro_scale = 0.1220703125f; + ctrl.calibration.accel_calibration = {{{1, 0, 0, 0, 1, 0, 0, 0, 1}}, {}, 12}; + ctrl.calibration.gyro_calibration = {{{1, 0, 0, 0, 1, 0, 0, 0, 1}}, {}, 12}; + ctrl.gyro_calibration = ctrl.calibration.gyro_calibration; + ctrl.imu_descriptor_requested = true; + ctrl.imu_descriptor_deadline_ns = 3000000000; + rift_s_controller_report_t report = {}; + report.num_info = 1; + report.info[0].block_id = RIFT_S_CTRL_IMU; + report.info[0].imu.timestamp = 1000; + report.info[0].imu.gyro[0] = 100; + + rift_s_controller_handle_report(&ctrl, 1000000000, &report); + CHECK_FALSE(ctrl.imu_descriptor_done); + CHECK(ctrl.gyro.x == 0.0f); + + report.info[0].imu.timestamp = 5000; + rift_s_controller_handle_report(&ctrl, 3000000000, &report); + CHECK(ctrl.imu_descriptor_done); + CHECK(ctrl.gyro.x == Catch::Approx(DEG_TO_RAD(100 * 0.1220703125)).margin(1e-6)); + + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + m_imu_3dof_close(&ctrl.fusion); + os_mutex_destroy(&ctrl.mutex); +} + +TEST_CASE("Rift S head and camera output invert the firmware transform from IMU") +{ + rift_s_tracker tracker = {}; + os_mutex_init(&tracker.mutex); + tracker.pose.position = {1, 1.6f, -2}; + const xrt_vec3 y = {0, 1, 0}, x = {1, 0, 0}; + math_quat_from_angle_vector(DEG_TO_RAD(70), &y, &tracker.pose.orientation); + // Without SLAM the position is the guard's held position. + rift_s_slam_guard_init(&tracker.slam_guard); + tracker.slam_guard.have_good = true; + tracker.slam_guard.good.position = tracker.pose.position; + tracker.device_from_imu.position = {-0.000295f, -0.0190975f, -0.068692f}; + math_quat_from_angle_vector(DEG_TO_RAD(15), &x, &tracker.device_from_imu.orientation); + tracker.left_cam_from_imu.position = {0.04f, -0.02f, 0.08f}; + math_quat_from_angle_vector(DEG_TO_RAD(-30), &y, &tracker.left_cam_from_imu.orientation); + for (auto pose : {RIFT_S_TRACKER_POSE_DEVICE, RIFT_S_TRACKER_POSE_LEFT_CAMERA}) { + xrt_space_relation rel; + rift_s_tracker_get_tracked_pose(&tracker, pose, 1000000000, &rel); + // T_world_target * T_target_imu must recover T_world_imu. + xrt_pose recovered; + const xrt_pose &offset = + pose == RIFT_S_TRACKER_POSE_DEVICE ? tracker.device_from_imu : tracker.left_cam_from_imu; + math_pose_transform(&rel.pose, &offset, &recovered); + CHECK(recovered.position.x == Catch::Approx(tracker.pose.position.x).margin(1e-6)); + CHECK(recovered.position.y == Catch::Approx(tracker.pose.position.y).margin(1e-6)); + CHECK(recovered.position.z == Catch::Approx(tracker.pose.position.z).margin(1e-6)); + CHECK(std::abs(recovered.orientation.x - tracker.pose.orientation.x) < 1e-6); + CHECK(std::abs(recovered.orientation.y - tracker.pose.orientation.y) < 1e-6); + CHECK(std::abs(recovered.orientation.z - tracker.pose.orientation.z) < 1e-6); + CHECK(std::abs(recovered.orientation.w - tracker.pose.orientation.w) < 1e-6); + } + os_mutex_destroy(&tracker.mutex); +} + +TEST_CASE("Rift S IMU clock mapping holds steady through radio bursts and stalls") +{ + rift_s_controller ctrl = {}; + ctrl.gravity_initialized = true; + ctrl.fusion.rot = XRT_QUAT_IDENTITY; + std::mt19937 rng(0xc10c); + std::uniform_real_distribution jitter(0, 2e6); + const timepoint_ns host_offset = 10000000000LL, min_delay = 3000000; + time_duration_ns mean_filter = 0, mean_min = INT64_MAX, mean_max = INT64_MIN; + time_duration_ns mapped_min = INT64_MAX, mapped_max = INT64_MIN; + unsigned mean_filter_jumps = 0; + timepoint_ns last_arrival = 0; + for (int i = 0; i < 20000; i++) { + timepoint_ns device_ns = 1000000000LL + i * 2000000LL; + // Reports queue on the radio and leave in bursts every 8 ms; every 1000th burst stalls 30 ms. + timepoint_ns release = (device_ns / 8000000 + 1) * 8000000; + if ((device_ns / 8000000) % 1000 == 999) + release += 30000000; + timepoint_ns arrival = + std::max(last_arrival, release + host_offset + min_delay + (timepoint_ns)jitter(rng)); + last_arrival = arrival; + REQUIRE(rift_s_controller_record_attitude(&ctrl, device_ns, arrival)); + time_duration_ns old = mean_filter; + m_clock_offset_a2b(500, device_ns, arrival, &mean_filter); + if (old != 0 && std::llabs(mean_filter - old) > 2000000) + mean_filter_jumps++; + if (i < 1000) + continue; + time_duration_ns mapped = ctrl.imu_to_host_ns - host_offset; + mapped_min = std::min(mapped_min, mapped); + mapped_max = std::max(mapped_max, mapped); + mean_min = std::min(mean_min, mean_filter - host_offset); + mean_max = std::max(mean_max, mean_filter - host_offset); + } + printf("clock: min_skew_delay_ms=%.3f..%.3f mean_filter_delay_ms=%.3f..%.3f mean_filter_jumps=%u\n", + mapped_min / 1e6, mapped_max / 1e6, mean_min / 1e6, mean_max / 1e6, mean_filter_jumps); + // The mapped capture time stays within a millisecond of the minimum transport delay and never + // resets the history. The mean-delay filter it replaces lags a further 3 to 9 ms behind and + // wanders by several milliseconds with the burst pattern. + CHECK(ctrl.diagnostics.reject_clock == 0); + CHECK(ctrl.attitude_history_count == RIFT_S_ATTITUDE_HISTORY_CAPACITY); + CHECK(mapped_min >= min_delay); + CHECK(mapped_max - mapped_min < 1000000); + CHECK(mean_max - mean_min > 4000000); + CHECK(mean_min > mapped_max); + // Capture-time lookups succeed just before the newest sample. + const timepoint_ns newest = 1000000000LL + 19999 * 2000000LL + ctrl.imu_to_host_ns; + rift_s_attitude_sample sample; + CHECK(rift_s_controller_get_attitude(&ctrl, newest - 1000000, &sample)); + + // A real discontinuity (the device clock restarting 200 ms ahead) discards the history while the + // estimate moves, then the mapping settles on the new offset and the history refills. + timepoint_ns device_ns = 1000000000LL + 20000 * 2000000LL + 200000000LL; + const time_duration_ns settled = last_arrival + 2000000 - device_ns; + for (int i = 0; i < 400; i++, device_ns += 2000000) { + REQUIRE(rift_s_controller_record_attitude(&ctrl, device_ns, last_arrival += 2000000)); + } + CHECK(ctrl.diagnostics.reject_clock > 0); + CHECK(ctrl.diagnostics.reject_clock < 64); + // Every one of them is counted under its cause. + uint64_t by_cause = 0; + for (uint64_t n : ctrl.diagnostics.reject_clock_by) + by_cause += n; + CHECK(by_cause == ctrl.diagnostics.reject_clock); + CHECK(ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY] > 0); + CHECK(ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_BACKWARDS] == 0); + CHECK(std::llabs(ctrl.imu_to_host_ns - settled) < 1000000); + CHECK(ctrl.attitude_history_count > 300); + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); +} + +TEST_CASE("Rift S IMU clock re-anchors at once after a sleep that wraps the device counter") +{ + // r18 wake log: the controllers slept 4927 s, longer than one 2^32 us wrap of the device counter, + // so device time advanced 632 s. The mapping then crept 1/64 per sample towards the new offset + // and reset the capture history on 662 consecutive samples before tracking could start. + rift_s_controller ctrl = {}; + ctrl.gravity_initialized = true; + ctrl.fusion.rot = XRT_QUAT_IDENTITY; + const time_duration_ns delay = 3000000; + timepoint_ns device_ns = 1000000000LL, host_ns = 50000000000LL; + for (int i = 0; i < 500; i++, device_ns += 2000000, host_ns += 2000000) + REQUIRE(rift_s_controller_record_attitude(&ctrl, device_ns, host_ns + delay)); + REQUIRE(ctrl.diagnostics.reject_clock == 0); + + const time_duration_ns sleep = 4927000000000LL, lost_wrap = 4294967296000LL; + device_ns += sleep - lost_wrap; + host_ns += sleep; + unsigned first_tilt_valid = 0; + for (unsigned i = 0; i < 400; i++, device_ns += 2000000, host_ns += 2000000) { + rift_s_controller_record_attitude(&ctrl, device_ns, host_ns + delay); + rift_s_attitude_sample sample; + if (first_tilt_valid == 0 && ctrl.attitude_history_count >= 2 && + rift_s_controller_get_attitude(&ctrl, host_ns + delay - 1000000, &sample)) + first_tilt_valid = i + 1; + } + INFO("discontinuity resets=" << ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY] + << " first usable sample=" << first_tilt_valid); + CHECK(ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY] <= 2); + CHECK(ctrl.diagnostics.clock_resyncs == 1); + CHECK(first_tilt_valid > 0); + CHECK(first_tilt_valid <= 40); // 80 ms of 500 Hz reports + CHECK(std::llabs(ctrl.imu_to_host_ns - (host_ns - device_ns + delay)) < 1000000); + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); +} + +TEST_CASE("Rift S IMU clock mapping works when the device clock is ahead of the host clock") +{ + // The play-r8 worn log: controller timestamps of 92846 s (a 48-bit clock counting since the + // controllers woke, the same clock the camera metadata uses) against a host monotonic clock of + // 33474 s. The skew is then negative. Averaged as an unsigned value it jumped to about + // INT64_MAX, which showed as clock_discontinuity_ms=9223372036853 and reset the history on every + // sample. + const timepoint_ns device_start = 92846078749000LL, host_start = 33474445657118LL; + m_clock_windowed_skew_tracker *tracker = m_clock_windowed_skew_tracker_alloc(64); + REQUIRE(tracker != nullptr); + const time_duration_ns delay = 4000000; + for (int i = 0; i < 500; i++) { + timepoint_ns device_ns = device_start + i * 2000000LL; + m_clock_windowed_skew_tracker_push(tracker, host_start + i * 2000000LL + delay + (i % 5) * 300000, + device_ns); + timepoint_ns local; + REQUIRE(m_clock_windowed_skew_tracker_to_local(tracker, device_ns, &local)); + // The first samples have a window of one to a few; every one must be near the truth. + CHECK(std::llabs(local - (host_start + i * 2000000LL + delay)) < 2000000); + } + m_clock_windowed_skew_tracker_destroy(tracker); + + rift_s_controller ctrl = {}; + ctrl.gravity_initialized = true; + ctrl.fusion.rot = XRT_QUAT_IDENTITY; + for (int i = 0; i < 2000; i++) { + timepoint_ns device_ns = device_start + i * 2000000LL; + REQUIRE(rift_s_controller_record_attitude(&ctrl, device_ns, host_start + i * 2000000LL + delay)); + } + CHECK(ctrl.diagnostics.reject_clock == 0); + CHECK(ctrl.attitude_history_count == RIFT_S_ATTITUDE_HISTORY_CAPACITY); + CHECK(std::llabs(ctrl.imu_to_host_ns - (host_start - device_start + delay)) < 1000000); + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); +} + +TEST_CASE("Rift S delayed optical yaw corrects capture attitude and preserves later rotation") +{ + rift_s_controller ctrl = {}; + ctrl.gravity_initialized = ctrl.update_yaw_from_optical = true; + ctrl.device_type = RIFT_S_DEVICE_LEFT_CONTROLLER; + os_mutex_init(&ctrl.mutex); + for (int i = 0; i <= 100; i++) { + record_test_attitude(ctrl, i, i * 0.5f); + } + xrt_pose optical = XRT_POSE_IDENTITY; + const xrt_vec3 axis = {0, 1, 0}; + const timepoint_ns capture_ns = 11101000000LL; // Between samples 50 and 51, yaw 25.25 degrees. + math_quat_from_angle_vector(DEG_TO_RAD(45.25f), &axis, &optical.orientation); + optical.position = {0.3f, 1.2f, -0.7f}; + rift_s_controller_push_observed_pose(&ctrl.base, capture_ns, &optical); + CHECK(ctrl.diagnostics.optical_accepted == 1); + CHECK_FALSE(rift_s_controller_has_heading_prior(&ctrl, capture_ns)); + CHECK(2 * atan2f(ctrl.fusion.rot.y, ctrl.fusion.rot.w) == Catch::Approx(DEG_TO_RAD(52.f))); + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, capture_ns, &rel) == + XRT_SUCCESS); + CHECK(2 * atan2f(rel.pose.orientation.y, rel.pose.orientation.w) == Catch::Approx(DEG_TO_RAD(27.25f))); + CHECK((rel.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) == 0); + CHECK((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0); + // Earlier queries must not consume a position observed later in time. + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, capture_ns - 1000000, + &rel) == XRT_SUCCESS); + CHECK((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) == 0); + // A subsequent small residual establishes a heading prior after correction. + math_quat_from_angle_vector(DEG_TO_RAD(31.f), &axis, &optical.orientation); + rift_s_controller_push_observed_pose(&ctrl.base, capture_ns + 1000000, &optical); + CHECK(rift_s_controller_has_heading_prior(&ctrl, capture_ns + 1000000)); + CHECK_FALSE(rift_s_controller_has_heading_prior(&ctrl, capture_ns)); + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); +} + +TEST_CASE("Rift S capture attitude rejects stale, future, gapped and missing history") +{ + rift_s_controller ctrl = {}; + ctrl.gravity_initialized = true; + os_mutex_init(&ctrl.mutex); + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + CHECK(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, 11070000000LL, &rel) == + XRT_ERROR_POSE_NOT_ACTIVE); + for (int i = 0; i <= RIFT_S_ATTITUDE_HISTORY_CAPACITY + 32; i++) { + record_test_attitude(ctrl, i, 0); + } + CHECK(ctrl.attitude_history_count == RIFT_S_ATTITUDE_HISTORY_CAPACITY); + xrt_pose optical = XRT_POSE_IDENTITY; + rift_s_controller_push_observed_pose(&ctrl.base, 11064000000LL, &optical); + CHECK(ctrl.diagnostics.reject_history == 2); + CHECK(ctrl.diagnostics.optical_accepted == 0); + CHECK(ctrl.last_tracked_pose_ts == 0); + CHECK_FALSE(rift_s_controller_has_heading_prior(&ctrl, 11070000000LL)); + const timepoint_ns latest = 11000000000LL + (RIFT_S_ATTITUDE_HISTORY_CAPACITY + 32) * 2000000LL; + CHECK(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, latest + 20000001, &rel) == + XRT_ERROR_POSE_NOT_ACTIVE); + CHECK(rel.relation_flags == 0); + rift_s_controller_push_observed_pose(&ctrl.base, latest, &optical); + CHECK(ctrl.diagnostics.optical_accepted == 1); + rift_s_controller_push_observed_pose(&ctrl.base, latest, &optical); + CHECK(ctrl.diagnostics.reject_stale == 1); + record_test_attitude(ctrl, RIFT_S_ATTITUDE_HISTORY_CAPACITY + 50, 0); + CHECK(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, latest + 1000000, &rel) == + XRT_ERROR_POSE_NOT_ACTIVE); + optical.position.x = std::numeric_limits::quiet_NaN(); + rift_s_controller_push_observed_pose(&ctrl.base, latest + 36000000, &optical); + CHECK(ctrl.diagnostics.reject_nonfinite == 1); + CHECK(ctrl.diagnostics.optical_accepted == 1); + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); +} + +TEST_CASE("Rift S capture prior never marks synthetic fallback as tracked position") +{ + rift_s_controller ctrl = {}; + ctrl.gravity_initialized = true; + os_mutex_init(&ctrl.mutex); + for (int i = 0; i < 40; i++) { + record_test_attitude(ctrl, i, 0); + } + ctrl.pose.position = {0.2f, -0.25f, -0.45f}; + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, 11078000000LL, &rel) == + XRT_SUCCESS); + CHECK((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) == 0); + CHECK((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) == 0); + CHECK_FALSE(rift_s_controller_has_heading_prior(&ctrl, 11078000000LL)); + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); +} + +TEST_CASE("Rift S position-only optical fits never confirm the IMU heading they echo") +{ + rift_s_controller ctrl = {}; + ctrl.gravity_initialized = ctrl.update_yaw_from_optical = true; + ctrl.device_type = RIFT_S_DEVICE_RIGHT_CONTROLLER; + os_mutex_init(&ctrl.mutex); + const xrt_vec3 axis = {0, 1, 0}; + // 5 ms spacing so the bounded history spans the heading-prior age limit. + for (int i = 0; i <= 1000; i++) { + math_quat_from_angle_vector(DEG_TO_RAD(10.f), &axis, &ctrl.fusion.rot); + ctrl.pose.orientation = ctrl.fusion.rot; + const timepoint_ns device_ns = 1000000000LL + i * 5000000LL; + REQUIRE(rift_s_controller_record_attitude(&ctrl, device_ns, device_ns + 10000000000LL)); + } + const timepoint_ns first = 12010000000LL; + t_constellation_pose_observation obs = {}; + obs.capture_ns = first; + obs.pose = XRT_POSE_IDENTITY; + obs.pose.position = {0.1f, 1.1f, -0.4f}; + // The fit returns the prior orientation (10 degrees) without measuring it. + math_quat_from_angle_vector(DEG_TO_RAD(10.f), &axis, &obs.pose.orientation); + obs.orientation_observed = false; + obs.inliers = 3; + obs.distinct_leds = 2; + obs.information_rank = 3; + obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-6f; + obs.hypothesis = "heading_translation"; + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + CHECK(ctrl.diagnostics.optical_accepted == 1); + CHECK(ctrl.diagnostics.optical_position_only == 1); + CHECK(ctrl.diagnostics.optical_reliable == 0); + CHECK(ctrl.last_tracked_pose_ts == first); + CHECK(ctrl.last_position_covariance[4] == Catch::Approx(1e-6f)); + CHECK_FALSE(rift_s_controller_has_heading_prior(&ctrl, first)); + + // Even a wrong orientation in a position-only fit must not rotate the IMU state. + obs.capture_ns = first + 10000000; + math_quat_from_angle_vector(DEG_TO_RAD(70.f), &axis, &obs.pose.orientation); + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + CHECK(2 * atan2f(ctrl.fusion.rot.y, ctrl.fusion.rot.w) == Catch::Approx(DEG_TO_RAD(10.f))); + + // A fit that measures yaw confirms the heading, and counts as reliable with 3 distinct LEDs. + obs.capture_ns = first + 20000000; + math_quat_from_angle_vector(DEG_TO_RAD(11.f), &axis, &obs.pose.orientation); + obs.orientation_observed = true; + obs.distinct_leds = 3; + obs.information_rank = 4; + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + CHECK(ctrl.diagnostics.optical_reliable == 1); + CHECK(rift_s_controller_has_heading_prior(&ctrl, first + 20000000)); + + // Position-only updates keep the position tracked but let the heading prior age out. + obs.orientation_observed = false; + for (timepoint_ns t = first + 400000000; t <= first + 2400000000LL; t += 400000000) { + obs.capture_ns = t; + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + } + CHECK(ctrl.last_tracked_pose_ts == first + 2400000000LL); + CHECK_FALSE(rift_s_controller_has_heading_prior(&ctrl, first + 2400000000LL)); + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); +} + +#include "rift_s_model_check.h" +#include +#include +#include "constellation_rift_s_data.hpp" + +namespace { +struct FirmwareModel +{ + std::vector leds; + std::vector lensing = std::vector(2); + rift_s_controller_imu_calibration calibration{}; + explicit FirmwareModel(bool mirror_x) + { + // Device-frame points from the recorded left ring; the right hand is its X mirror. + for (const auto &l : rift_s_ring_leds) { + rift_s_led led{}; + led.pos = l.pos; + led.dir = m_vec3_mul_scalar(l.dir, 1.0f + 0.01f * (l.id % 3)); // firmware normals are not unit + led.angles = {85, 80, 0}; + if (mirror_x) { + led.pos.x = -led.pos.x; + led.dir.x = -led.dir.x; + } + leds.push_back(led); + } + calibration.num_leds = leds.size(); + calibration.leds = leds.data(); + calibration.num_lensing_models = lensing.size(); + calibration.lensing_models = lensing.data(); + calibration.imu_position = {mirror_x ? 0.005584116f : -0.005584116f, -0.02f, 0.01f}; + calibration.accel.rectification = calibration.gyro.rectification = {{1, 0, 0, 0, 1, 0, 0, 0, 1}}; + calibration.accel_calibration.matrix = + calibration.gyro_calibration.matrix = {{1, 0, 0, 0, 1, 0, 0, 0, 1}}; + calibration.accel_calibration.num_values = calibration.gyro_calibration.num_values = 12; + } +}; +} // namespace + +TEST_CASE("Rift S model check reports normals, IMU placement and IMU axis handedness") +{ + FirmwareModel left(false); + rift_s_controller_model_report r; + rift_s_controller_model_check(&left.calibration, &r); + CHECK(r.num_leds == 15); + CHECK(r.num_lensing_models == 2); + CHECK(r.ids_sequential); + CHECK(r.normal_length_min == Catch::Approx(1.0f).margin(1e-3)); + CHECK(r.normal_length_max == Catch::Approx(1.02f).margin(1e-3)); + // The recorded ring's normals face away from its centroid. + CHECK(r.outward_normals >= 13); + CHECK(r.angle_x_min == 85); + CHECK(r.angle_y_max == 80); + CHECK(r.accel_rectification.determinant == Catch::Approx(1)); + CHECK_FALSE(r.accel_rectification.reflection); + CHECK(r.accel_rectification.rotation_deg == Catch::Approx(0).margin(0.01)); + CHECK(r.accel_gyro_axes_deg == Catch::Approx(0).margin(0.01)); + CHECK(r.accel_calibration.rotation_deg == Catch::Approx(0).margin(0.01)); + CHECK(r.accel_calibration.determinant == Catch::Approx(1)); + CHECK(r.accel_calibration_offset_norm == 0); + CHECK(r.accel_calibration_values == 12); + CHECK(r.accel_calibration_vs_rectification_deg == Catch::Approx(0).margin(0.01)); + + // A swapped-axis accelerometer rectification is a reflection; a gyro rotated against it is reported. + left.calibration.accel.rectification = {{0, 1, 0, 1, 0, 0, 0, 0, 1}}; + left.calibration.gyro.rectification = {{0, -1, 0, 1, 0, 0, 0, 0, 1}}; + rift_s_controller_model_check(&left.calibration, &r); + CHECK(r.accel_rectification.reflection); + CHECK_FALSE(r.gyro_rectification.reflection); + CHECK(r.gyro_rectification.rotation_deg == Catch::Approx(90).margin(0.01)); + CHECK(r.accel_gyro_axes_deg > 5); + + // The TrackedObject matrix is compared with the rectification it would replace. + left.calibration.gyro_calibration.matrix = left.calibration.gyro.rectification; + left.calibration.gyro_calibration.offset = {0.01f, 0, 0}; + rift_s_controller_model_check(&left.calibration, &r); + CHECK(r.gyro_calibration.rotation_deg == Catch::Approx(90).margin(0.01)); + CHECK(r.gyro_calibration_vs_rectification_deg == Catch::Approx(0).margin(0.01)); + CHECK(r.gyro_calibration_offset_norm == Catch::Approx(0.01)); + CHECK(r.accel_calibration_vs_rectification_deg == Catch::Approx(180).margin(0.1)); +} + +TEST_CASE("Rift S gravity check compares the resting accelerometer under both calibrations") +{ + FirmwareModel model(false); + rift_s_controller_imu_calibration &c = model.calibration; + // Top-level calibration: a 90 degree axis rotation and an offset along the sample's x axis. + c.accel.rectification = {{0, -1, 0, 1, 0, 0, 0, 0, 1}}; + c.accel.offset = {0.4f, 0, 0}; + // TrackedObject uses the same sensor-space bias subtraction as the top-level calibration. + c.accel_calibration.matrix = c.accel.rectification; + c.accel_calibration.offset = c.accel.offset; + xrt_vec3 rectified = {0, 9.81f, 0}; + rift_s_controller_gravity_report check; + rift_s_controller_gravity_check(&c, &rectified, &check); + REQUIRE(check.valid); + CHECK(check.rectified_mps2 == Catch::Approx(9.81f)); + CHECK(check.tracked_mps2 == Catch::Approx(9.81f)); + CHECK(check.direction_deg == Catch::Approx(0).margin(0.05)); + + // Identity TrackedObject matrix without the offset: the wrong axes and the unremoved bias (here + // along the sample's gravity axis) are both visible. + c.accel_calibration.matrix = {{1, 0, 0, 0, 1, 0, 0, 0, 1}}; + c.accel_calibration.offset = {0, 0, 0}; + // The same sensor sample after this factory calibration. + rectified = {10.21f, 0, 0}; + rift_s_controller_gravity_check(&c, &rectified, &check); + REQUIRE(check.valid); + CHECK(check.direction_deg == Catch::Approx(90).margin(0.05)); + CHECK(check.tracked_mps2 == Catch::Approx(9.81 + 0.4).margin(1e-3)); + + // A truncated array is not interpreted. + c.accel_calibration.num_values = 9; + rift_s_controller_gravity_check(&c, &rectified, &check); + CHECK_FALSE(check.valid); +} + +TEST_CASE("Rift S hand comparison makes mirroring explicit rather than accidental") +{ + FirmwareModel left(false), right(true), unmirrored(false); + rift_s_controller_hand_comparison cmp; + rift_s_controller_compare_hands(&left.calibration, &right.calibration, &cmp); + CHECK(cmp.best == RIFT_S_HANDS_MIRROR_X); + CHECK(cmp.position_rms_m[RIFT_S_HANDS_MIRROR_X] < 1e-6); + CHECK(cmp.normal_mean_deg[RIFT_S_HANDS_MIRROR_X] < 0.1); + CHECK(cmp.imu_difference_m[RIFT_S_HANDS_MIRROR_X] < 1e-6); + CHECK(cmp.position_rms_m[RIFT_S_HANDS_IDENTICAL] > 0.005); + // A right model that is a copy of the left (no mirroring) is detected. + rift_s_controller_compare_hands(&left.calibration, &unmirrored.calibration, &cmp); + CHECK(cmp.best == RIFT_S_HANDS_IDENTICAL); + CHECK(cmp.imu_difference_m[RIFT_S_HANDS_MIRROR_X] > 0.01); +} + +TEST_CASE("Rift S left and right LED models keep firmware identity, unit normals and the IMU origin") +{ + FirmwareModel left_fw(false), right_fw(true); + rift_s_controller left = {}, right = {}; + left.base.device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + right.base.device_type = XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER; + for (auto *c : {&left, &right}) { + os_mutex_init(&c->mutex); + c->have_calibration = true; + c->P_device_imu = XRT_POSE_IDENTITY; + } + left.calibration = left_fw.calibration; + right.calibration = right_fw.calibration; + for (auto *c : {&left, &right}) { + c->P_device_imu.position = c->calibration.imu_position; + math_pose_invert(&c->P_device_imu, &c->P_imu_device); + } + t_constellation_led_model lm{}, rm{}; + REQUIRE(rift_s_controller_get_led_model(&left.base, &lm)); + REQUIRE(rift_s_controller_get_led_model(&right.base, &rm)); + CHECK(lm.id == XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER); + CHECK(rm.id == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER); + CHECK(lm.id != rm.id); + REQUIRE(lm.num_leds == rm.num_leds); + for (unsigned i = 0; i < lm.num_leds; i++) { + const auto &l = lm.leds[i], &r = rm.leds[i]; + CHECK(l.id == i); + CHECK(r.id == i); + CHECK(m_vec3_len(l.dir) == Catch::Approx(1).margin(1e-5)); + CHECK(m_vec3_len(r.dir) == Catch::Approx(1).margin(1e-5)); + // IMU-relative, then OpenCV flip (x, -y, -z): the mirror stays on X for both hands. + const auto &fw = left_fw.leds[i]; + CHECK(l.pos.x == Catch::Approx(fw.pos.x - left_fw.calibration.imu_position.x)); + CHECK(l.pos.y == Catch::Approx(-(fw.pos.y - left_fw.calibration.imu_position.y))); + CHECK(l.pos.z == Catch::Approx(-(fw.pos.z - left_fw.calibration.imu_position.z))); + CHECK(r.pos.x == Catch::Approx(-l.pos.x).margin(1e-6)); + CHECK(r.pos.y == Catch::Approx(l.pos.y).margin(1e-6)); + CHECK(r.pos.z == Catch::Approx(l.pos.z).margin(1e-6)); + CHECK(r.dir.x == Catch::Approx(-l.dir.x).margin(1e-6)); + CHECK(r.dir.y == Catch::Approx(l.dir.y).margin(1e-6)); + } + t_constellation_led_model_clear(&lm); + t_constellation_led_model_clear(&rm); + for (auto *c : {&left, &right}) + os_mutex_destroy(&c->mutex); +} + +namespace { +struct PublishedPoseController +{ + rift_s_controller ctrl = {}; + explicit PublishedPoseController(bool right) + { + ctrl.base.device_type = + right ? XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER : XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + ctrl.gravity_initialized = true; + ctrl.clock_stable_samples = 32; + ctrl.P_device_imu = XRT_POSE_IDENTITY; + ctrl.P_device_imu.position = FirmwareModel(right).calibration.imu_position; + math_pose_invert(&ctrl.P_device_imu, &ctrl.P_imu_device); + rift_s_controller_pose_frames(!right, &ctrl.P_device_aim, &ctrl.P_device_grip); + os_mutex_init(&ctrl.mutex); + } + ~PublishedPoseController() + { + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); + } + void + measured(xrt_quat orientation, xrt_vec3 imu_position, timepoint_ns ts) + { + ctrl.pose = {orientation, imu_position}; + ctrl.fusion.rot = orientation; + ctrl.last_imu_capture_ns = ctrl.last_fix_ns = ts; + ctrl.last_tracked_pose = ctrl.pose; + ctrl.last_tracked_pose_ts = ts; + ctrl.track_vision_updates = ctrl.track_reliable_updates = 10; + const xrt_vec3 variance = {1e-5f, 1e-5f, 1e-5f}; + rift_s_position_filter_reset(&ctrl.position_filter); + rift_s_position_filter_fix(&ctrl.position_filter, ts, &imu_position, &variance, 0.15f); + REQUIRE(rift_s_controller_record_attitude(&ctrl, ts, ts)); + } + xrt_space_relation + query(xrt_input_name input, timepoint_ns ts) + { + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base, input, ts, &rel) == XRT_SUCCESS); + return rel; + } +}; + +// Independent expectations from the native Oculus driver constants and model components. +xrt_vec3 +rotate_x(float deg, xrt_vec3 v) +{ + const float c = cosf(DEG_TO_RAD(deg)), s = sinf(DEG_TO_RAD(deg)); + return {v.x, c * v.y - s * v.z, s * v.y + c * v.z}; +} + +// Native driver quaternion (x,w)=(0.3371,0.94147), normalized before rigid-pose math. +constexpr float native_model_pitch = 39.40053259f; +xrt_vec3 +model_to_device(bool right, xrt_vec3 model) +{ + const xrt_vec3 origin = {right ? 0.007f : -0.007f, 0.036124f, -0.037617f}; + return m_vec3_add(rotate_x(native_model_pitch, model), origin); +} + +xrt_vec3 +grip_in_device(bool) +{ + // LibOVR LCON grip, independent of the render model and the implementation. + return {0, -0.03f, 0.04f}; +} + +void +check_vec(xrt_vec3 actual, xrt_vec3 expected) +{ + CHECK(actual.x == Catch::Approx(expected.x).margin(2e-6)); + CHECK(actual.y == Catch::Approx(expected.y).margin(2e-6)); + CHECK(actual.z == Catch::Approx(expected.z).margin(2e-6)); +} +} // namespace + +TEST_CASE("Rift S loss hold and reacquire keep the published hand valid and continuous") +{ + const timepoint_ns ms = U_TIME_1MS_IN_NS; + const xrt_vec3 here = {0.3f, 1.4f, -0.4f}, error = {4, -6, 2}; + for (bool right : {false, true}) { + PublishedPoseController fixture(right); + auto &ctrl = fixture.ctrl; + const timepoint_ns start = os_monotonic_get_ns() - 2000 * ms; + fixture.measured(XRT_QUAT_IDENTITY, here, start); + ctrl.track_vision_updates = ctrl.track_reliable_updates = ctrl.position_filter.track_updates = 30; + // Optical loss while the IMU keeps rotating the wrist. Its acceleration error must + // never move the IMU origin farther than the existing 10 cm coast bound. + for (timepoint_ns t = start + 2 * ms; t <= start + 2000 * ms; t += 2 * ms) { + const xrt_vec3 up = {0, 1, 0}; + math_quat_from_angle_vector(float(t - start) / U_TIME_1S_IN_NS, &up, &ctrl.pose.orientation); + ctrl.fusion.rot = ctrl.pose.orientation; + ctrl.last_imu_capture_ns = t; + REQUIRE(rift_s_controller_record_attitude(&ctrl, t, t)); + ctrl.position_filter.coasting = t - start > 120 * ms; + rift_s_position_filter_imu(&ctrl.position_filter, t, &error); + for (auto input : {XRT_INPUT_TOUCH_AIM_POSE, XRT_INPUT_TOUCH_GRIP_POSE}) { + auto rel = fixture.query(input, t); + REQUIRE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0); + if (t - start > 500 * ms) { + CHECK_FALSE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != + 0); + CHECK_FALSE( + (rel.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0); + } + const auto &offset = + input == XRT_INPUT_TOUCH_AIM_POSE ? ctrl.P_device_aim : ctrl.P_device_grip; + xrt_pose imu_device_offset, expected; + math_pose_transform(&ctrl.P_imu_device, &offset, &imu_device_offset); + const xrt_pose anchor = {ctrl.pose.orientation, here}; + math_pose_transform(&anchor, &imu_device_offset, &expected); + CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, expected.position)) <= 0.10001f); + CHECK(rel.pose.orientation.y == Catch::Approx(expected.orientation.y).margin(2e-6)); +#ifdef TEST_RIFT_S_STEAMVR_POSE + const auto model_offset = + rift_s_touch_grip_to_model(right ? XRT_HAND_RIGHT : XRT_HAND_LEFT); + if (input == XRT_INPUT_TOUCH_GRIP_POSE) + math_pose_transform(&rel.pose, &model_offset, &rel.pose); + vr::DriverPose_t pose{}; + pose.poseIsValid = true; + pose.result = vr::TrackingResult_Running_OK; + apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true); + REQUIRE(pose.poseIsValid); + CHECK(pose.result == vr::TrackingResult_Running_OK); + CHECK(pose.vecPosition[1] > 1.0); +#endif + } + } + REQUIRE(ctrl.position_filter.frozen); + // The first two fits remain candidates. The third resumes the unchanged optical + // filter, while only the displayed translation blends out over 100 ms. + const auto held = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, os_monotonic_get_ns()); + t_constellation_pose_observation obs{}; + obs.pose = {ctrl.pose.orientation, {-0.2f, 1.5f, -0.5f}}; + obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-5f; + for (int i = 0; i < 3; ++i) { + obs.capture_ns = start + (1894 + 33 * i) * ms; + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + CHECK(ctrl.position_filter.frozen == (i < 2)); + const auto rel = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, os_monotonic_get_ns()); + CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, held.pose.position)) < 0.01f); + } + REQUIRE(ctrl.diagnostics.optical_accepted == 1); + const timepoint_ns recovered = os_monotonic_get_ns(); + auto previous = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, recovered); + for (int i = 1; i <= 12; ++i) { + auto rel = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, recovered + i * 10 * ms); + CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, previous.pose.position)) < 0.09f); + REQUIRE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0); + previous = rel; + } + // At the end the presentation offset is exactly gone, including future queries. + xrt_vec3 p, v; + rift_s_position_filter_predict(&ctrl.position_filter, recovered + 120 * ms, 100 * ms, &p, &v); + xrt_pose imu_device_offset, expected; + math_pose_transform(&ctrl.P_imu_device, &ctrl.P_device_grip, &imu_device_offset); + const xrt_pose target = {ctrl.pose.orientation, p}; + math_pose_transform(&target, &imu_device_offset, &expected); + check_vec(previous.pose.position, expected.position); + // Presentation queries never replace the capture-time optical association prior. + const auto prior = fixture.query(XRT_INPUT_GENERIC_TRACKER_POSE, obs.capture_ns); + check_vec(prior.pose.position, obs.pose.position); + } +} + +TEST_CASE("Rift S grip stays at its center during rotation for either hand") +{ + const xrt_vec3 center = {0.3f, 1.2f, -0.5f}; + const xrt_vec3 axes[] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; + for (bool right : {false, true}) { + PublishedPoseController fixture(right); + auto &ctrl = fixture.ctrl; + timepoint_ns ts = 1000000000; + for (const auto &axis : axes) { + for (float angle : {0.f, 30.f, 90.f, 170.f}) { + CAPTURE(right, angle, axis.x, axis.y, axis.z); + xrt_quat rotation; + math_quat_from_angle_vector(DEG_TO_RAD(angle), &axis, &rotation); + // Independently place the measured IMU around the render-model grip center. + const xrt_vec3 lever = m_vec3_sub(grip_in_device(right), ctrl.P_device_imu.position); + xrt_vec3 rotated; + math_quat_rotate_vec3(&rotation, &lever, &rotated); + fixture.measured(rotation, m_vec3_sub(center, rotated), ts); + auto grip = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, ts); + CHECK((grip.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0); + check_vec(grip.pose.position, center); + // Grip +Y and -Z: the native driver root plus the model grip component. + for (auto [local, hand] : + {std::pair{xrt_vec3{0, 1, 0}, rotate_x(native_model_pitch + 20.6f, {0, 1, 0})}, + {xrt_vec3{0, 0, -1}, rotate_x(native_model_pitch + 20.6f, {0, 0, -1})}}) { + xrt_vec3 actual, expected; + math_quat_rotate_vec3(&grip.pose.orientation, &local, &actual); + math_quat_rotate_vec3(&rotation, &hand, &expected); + check_vec(actual, expected); + } + ts += 2000000; + } + } + } +} + +TEST_CASE("Rift S aim and grip mirror between hands and keep the Windows grip-to-aim offset") +{ + PublishedPoseController left(false), right(true); + const timepoint_ns ts = 1000000000; + const xrt_vec3 imu_left = {-0.2f, 1.2f, -0.5f}, imu_right = {0.2f, 1.2f, -0.5f}; + left.measured(XRT_QUAT_IDENTITY, imu_left, ts); + right.measured(XRT_QUAT_IDENTITY, imu_right, ts); + for (auto input : {XRT_INPUT_TOUCH_AIM_POSE, XRT_INPUT_TOUCH_GRIP_POSE}) { + auto l = left.query(input, ts), r = right.query(input, ts); + check_vec(r.pose.position, {-l.pose.position.x, l.pose.position.y, l.pose.position.z}); + CHECK(r.pose.orientation.x == Catch::Approx(l.pose.orientation.x)); + CHECK(r.pose.orientation.y == Catch::Approx(l.pose.orientation.y)); + CHECK(r.pose.orientation.z == Catch::Approx(l.pose.orientation.z)); + CHECK(r.pose.orientation.w == Catch::Approx(l.pose.orientation.w)); + } + for (auto *fixture : {&left, &right}) { + auto aim = fixture->query(XRT_INPUT_TOUCH_AIM_POSE, ts); + auto grip = fixture->query(XRT_INPUT_TOUCH_GRIP_POSE, ts); + // Aim is forward in the native hand frame, within the rounded model constants. + xrt_vec3 forward; + const xrt_vec3 minus_z = {0, 0, -1}; + math_quat_rotate_vec3(&aim.pose.orientation, &minus_z, &forward); + check_vec(forward, rotate_x(native_model_pitch - 39.4f, minus_z)); + // Grip in the aim frame is the Windows LCON pair: grip (+60 deg X, (0, -0.03, +0.04)) and + // aim (identity, (0, 0, -0.055)) in the Windows hand frame. + xrt_pose aim_inv, aim_grip; + math_pose_invert(&aim.pose, &aim_inv); + math_pose_transform(&aim_inv, &grip.pose, &aim_grip); + CHECK(aim_grip.position.x == Catch::Approx(0).margin(1e-4)); + CHECK(aim_grip.position.y == Catch::Approx(-0.03).margin(1e-3)); + CHECK(aim_grip.position.z == Catch::Approx(0.095).margin(1e-3)); + xrt_vec3 grip_minus_z; + math_quat_rotate_vec3(&aim_grip.orientation, &minus_z, &grip_minus_z); + check_vec(grip_minus_z, rotate_x(60.0f, minus_z)); + } +} + +TEST_CASE("Rift S published frames cannot change capture-time tracking or prediction state") +{ + for (bool right : {false, true}) { + PublishedPoseController fixture(right); + auto &ctrl = fixture.ctrl; + timepoint_ns ts = 1000000000; + const xrt_vec3 axis = {1, 0, 0}; + for (float angle : {0.f, 45.f, 90.f, 135.f, 179.f}) { + CAPTURE(right, angle); + xrt_quat rotation; + math_quat_from_angle_vector(DEG_TO_RAD(angle), &axis, &rotation); + ctrl.fusion.last.gyro = {0.5f, -0.3f, 0.2f}; + fixture.measured(rotation, {0.2f, 1.3f, -0.4f}, ts); + const auto before = fixture.query(XRT_INPUT_GENERIC_TRACKER_POSE, ts); + const auto fusion = ctrl.fusion; + const auto filter = ctrl.position_filter; + const auto optical_pose = ctrl.last_tracked_pose; + const auto history = + ctrl.attitude_history[ctrl.attitude_history_start + ctrl.attitude_history_count - 1]; + for (auto input : {XRT_INPUT_TOUCH_AIM_POSE, XRT_INPUT_TOUCH_GRIP_POSE}) { + fixture.query(input, ts); + fixture.query(input, ts + 20000000); // Render-time extrapolation must also be isolated. + } + // Even arbitrary publication adjustments cannot alter constellation's prior. + ctrl.P_device_aim.position = {0.2f, 0.4f, 0.8f}; + ctrl.P_device_grip.orientation = {0, 1, 0, 0}; + fixture.query(XRT_INPUT_TOUCH_AIM_POSE, ts + 20000000); + fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, ts + 20000000); + const auto after = fixture.query(XRT_INPUT_GENERIC_TRACKER_POSE, ts); + CHECK(std::memcmp(&before, &after, sizeof(before)) == 0); + CHECK(std::memcmp(&fusion, &ctrl.fusion, sizeof(fusion)) == 0); + CHECK(std::memcmp(&filter, &ctrl.position_filter, sizeof(filter)) == 0); + CHECK(std::memcmp(&optical_pose, &ctrl.last_tracked_pose, sizeof(optical_pose)) == 0); + CHECK(std::memcmp( + &history, + &ctrl.attitude_history[ctrl.attitude_history_start + ctrl.attitude_history_count - 1], + sizeof(history)) == 0); + CHECK(ctrl.last_tracked_pose_ts == ts); + check_vec(after.pose.position, ctrl.last_tracked_pose.position); + rift_s_controller_pose_frames(!right, &ctrl.P_device_aim, &ctrl.P_device_grip); + ts += 2000000; + } + } +} + +TEST_CASE("Rift S grip lever arm cancels IMU velocity for a twist about its center") +{ + for (bool right : {false, true}) { + PublishedPoseController fixture(right); + auto &ctrl = fixture.ctrl; + const xrt_vec3 axis = {0, 1, 0}; + xrt_quat rotation; + math_quat_from_angle_vector(DEG_TO_RAD(120.f), &axis, &rotation); + const xrt_vec3 lever = m_vec3_sub(grip_in_device(right), ctrl.P_device_imu.position); + xrt_vec3 rotated, omega; + ctrl.fusion.last.gyro = {0.7f, -0.4f, 0.2f}; + math_quat_rotate_vec3(&rotation, &lever, &rotated); + math_quat_rotate_vec3(&rotation, &ctrl.fusion.last.gyro, &omega); + fixture.measured(rotation, m_vec3_sub(xrt_vec3{0.3f, 1.2f, -0.5f}, rotated), 1000000000); + const xrt_vec3 imu_radius = m_vec3_mul_scalar(rotated, -1); + xrt_vec3 imu_velocity; + math_vec3_cross(&omega, &imu_radius, &imu_velocity); + ctrl.position_filter.state.x[0][1] = imu_velocity.x; + ctrl.position_filter.state.x[1][1] = imu_velocity.y; + ctrl.position_filter.state.x[2][1] = imu_velocity.z; + const auto grip = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, 1000000000); + CHECK((grip.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0); + check_vec(grip.linear_velocity, {0, 0, 0}); + check_vec(grip.angular_velocity, omega); + } +} + +namespace { +xrt_quat +quat_from_axis_deg(xrt_vec3 axis, float deg) +{ + math_vec3_normalize(&axis); + xrt_quat q; + math_quat_from_angle_vector(DEG_TO_RAD(deg), &axis, &q); + return q; +} + +// Device-frame up of an attitude (IMU to world). +xrt_vec3 +device_up(const xrt_quat &attitude) +{ + xrt_quat inverse; + math_quat_invert(&attitude, &inverse); + const xrt_vec3 up = {0, 1, 0}; + xrt_vec3 out; + math_quat_rotate_vec3(&inverse, &up, &out); + return out; +} + +float +tilt_error_deg(const xrt_quat &a, const xrt_quat &b) +{ + return RAD_TO_DEG(m_vec3_angle(device_up(a), device_up(b))); +} +} // namespace + +TEST_CASE("Rift S optical tilt correction levels the IMU tilt and leaves heading alone") +{ + const xrt_quat imu = quat_from_axis_deg({0.3f, 1, -0.2f}, 70); + xrt_quat optical; + const xrt_quat tilt = quat_from_axis_deg({1, 0, 0.4f}, 9); + math_quat_rotate(&tilt, &imu, &optical); + xrt_quat full, half, corrected; + float angle = rift_s_optical_tilt_correction(&imu, &optical, 1.0f, &full); + CHECK(RAD_TO_DEG(angle) == Catch::Approx(tilt_error_deg(imu, optical)).margin(1e-3)); + // The correction axis is horizontal: a pure world tilt, no heading change. + xrt_vec3 axis = {full.x, full.y, full.z}; + CHECK(fabsf(axis.y) < 1e-6f); + math_quat_rotate(&full, &imu, &corrected); + CHECK(tilt_error_deg(corrected, optical) < 1e-3f); + rift_s_optical_tilt_correction(&imu, &optical, 0.5f, &half); + math_quat_rotate(&half, &imu, &corrected); + CHECK(tilt_error_deg(corrected, optical) == Catch::Approx(tilt_error_deg(imu, optical) / 2).margin(1e-2)); + // Equal tilt: nothing to do. + rift_s_optical_tilt_correction(&imu, &imu, 1.0f, &full); + CHECK(full.w == Catch::Approx(1)); +} + +TEST_CASE("Rift S attitude bias recovers the right controller's offset from levelled rest windows") +{ + // r14 worn log: the right accelerometer needed -0.058, -1.998, 0.018 m/s^2. + const xrt_vec3 truth = {-0.058f, -1.998f, 0.018f}; + std::mt19937 rng(7); + std::normal_distribution noise_deg(0, 2.5f); // single optical fits tilt by 2-3 degrees + std::uniform_real_distribution any(-1, 1); + rift_s_attitude_bias b = {}; + // All windows in one orientation: no spread needed once the attitude is known. + const xrt_quat held = quat_from_axis_deg({0.2f, 0.5f, 1}, 40); + for (int i = 0; i < RIFT_S_ATTITUDE_BIAS_SAMPLES; i++) { + const xrt_vec3 g = {0, (float)MATH_GRAVITY_M_S2, 0}; + xrt_quat inverse; + math_quat_invert(&held, &inverse); + xrt_vec3 reading; + math_quat_rotate_vec3(&inverse, &g, &reading); + reading = m_vec3_add(reading, truth); + // The levelled attitude carries optical tilt noise. + xrt_quat noisy, wobble = quat_from_axis_deg({any(rng), 0, any(rng)}, noise_deg(rng)); + math_quat_rotate(&wobble, &held, &noisy); + rift_s_attitude_bias_add(&b, &reading, &noisy); + if (i + 1 < RIFT_S_ATTITUDE_BIAS_MIN_SAMPLES) + CHECK_FALSE(b.valid); + } + REQUIRE(b.valid); + CHECK(m_vec3_len(m_vec3_sub(b.bias, truth)) < 0.25f); + // Inconsistent windows (e.g. moving) are not accepted. + rift_s_attitude_bias bad = {}; + for (int i = 0; i < 16; i++) { + xrt_vec3 reading = {any(rng) * 4, 9.8f + any(rng) * 4, any(rng) * 4}; + rift_s_attitude_bias_add(&bad, &reading, &held); + } + CHECK_FALSE(bad.valid); +} + +namespace { +struct FusionRig +{ + rift_s_controller ctrl = {}; + xrt_vec3 bias; + xrt_quat truth = XRT_QUAT_IDENTITY; + timepoint_ns t = 1000000000; + int sample = 0; + + explicit FusionRig(xrt_vec3 accel_bias) : bias(accel_bias) + { + m_imu_3dof_init(&ctrl.fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + ctrl.device_type = RIFT_S_DEVICE_RIGHT_CONTROLLER; + os_mutex_init(&ctrl.mutex); + } + ~FusionRig() + { + if (ctrl.imu_clock != nullptr) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + m_imu_3dof_close(&ctrl.fusion); + os_mutex_destroy(&ctrl.mutex); + } + // 2 ms IMU samples; @p omega is the true body rate (rad/s, IMU frame). Every 16th sample an + // optical fit of the true attitude arrives when @p optical. + void + run(int samples, xrt_vec3 omega, bool optical) + { + for (int i = 0; i < samples; i++, sample++, t += 2000000) { + xrt_quat step; + xrt_vec3 dtheta = m_vec3_mul_scalar(omega, 0.002f); + float a = m_vec3_len(dtheta); + step = a > 0 ? quat_from_axis_deg(dtheta, RAD_TO_DEG(a)) : xrt_quat XRT_QUAT_IDENTITY; + math_quat_rotate(&truth, &step, &truth); // body-frame rate + xrt_quat inverse; + math_quat_invert(&truth, &inverse); + const xrt_vec3 g = {0, (float)MATH_GRAVITY_M_S2, 0}; + math_quat_rotate_vec3(&inverse, &g, &ctrl.accel); + ctrl.accel = m_vec3_add(ctrl.accel, bias); + ctrl.gyro = omega; + rift_s_controller_update_fusion(&ctrl, t); + if (rift_s_controller_record_attitude(&ctrl, t, t)) + ctrl.last_imu_capture_ns = t; + if (optical && sample % 16 == 0 && ctrl.gravity_initialized) { + t_constellation_pose_observation obs = {}; + obs.capture_ns = t; + obs.pose.orientation = truth; + obs.pose.position = {0.2f, 1.2f, -0.4f}; + obs.orientation_observed = true; + obs.inliers = obs.distinct_leds = 6; + obs.information_rank = 4; + obs.yaw_variance = NAN; + obs.hypothesis = "test"; + for (float &c : obs.position_covariance) + c = NAN; + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + } + } + } + float + tilt_error() const + { + return tilt_error_deg(ctrl.fusion.rot, truth); + } +}; +} // namespace + +TEST_CASE("Rift S offset accelerometer levels to optical tilt, learns its offset, then levels to gravity") +{ + // r14 worn log, right controller: rest readings 7.9..9.4 m/s^2 from a -2 m/s^2 offset. Before + // fix-r15 the fusion levelled to that reading, up to ~11 degrees off, and applied the stored + // error quickly while rotating: tilt corrections at the optical solver's 6 degree limit. + FusionRig rig({-0.058f, -1.998f, 0.018f}); + rig.truth = quat_from_axis_deg({1, 0, 0.3f}, 35); + const xrt_vec3 still = {0, 0, 0}; + rig.run(200, still, false); + REQUIRE(rig.ctrl.gravity_initialized); + CHECK(rig.ctrl.accel_offset); + CHECK((rig.ctrl.fusion.flags & M_IMU_3DOF_USE_GRAVITY_DUR_20MS) == 0); + const float startup_error = rig.tilt_error(); + CHECK(startup_error > 5.0f); // the offset reading tilts the startup attitude + // Turning without optics: gyro only, the startup error neither grows nor jumps. + rig.run(250, {0.0f, 1.5f, 0.6f}, false); + rig.run(100, still, false); + CHECK(rig.tilt_error() < startup_error + 0.5f); + // Optical fits level the attitude within a few seconds... + rig.run(1500, still, true); + CHECK(rig.tilt_error() < 1.0f); + CHECK(rig.ctrl.optical_tilt_updates >= 30); + // ...after which rest windows give the offset and gravity levelling returns. + rig.run(2000, still, true); + REQUIRE(rig.ctrl.attitude_bias.valid); + CHECK_FALSE(rig.ctrl.accel_offset); + CHECK((rig.ctrl.fusion.flags & M_IMU_3DOF_USE_GRAVITY_DUR_20MS) != 0); + CHECK(m_vec3_len(m_vec3_sub(rig.ctrl.attitude_bias.bias, rig.bias)) < 0.2f); + // Now rotations and new rest orientations keep the attitude level without optics. + for (xrt_vec3 omega : {xrt_vec3{2.0f, 0.5f, 0}, xrt_vec3{0, -1.5f, 1.2f}, xrt_vec3{-1.0f, 0, -2.0f}}) { + rig.run(300, omega, false); + rig.run(500, still, false); + CHECK(rig.tilt_error() < 1.5f); + } +} + +TEST_CASE("Rift S accurate accelerometer keeps gravity levelling with optical tilt") +{ + // r14 worn log, left controller: startup 9.88 m/s^2, bias about 0.1 m/s^2. + FusionRig rig({-0.016f, 0.104f, -0.064f}); + rig.truth = quat_from_axis_deg({0.4f, 0, 1}, 25); + const xrt_vec3 still = {0, 0, 0}; + rig.run(200, still, false); + REQUIRE(rig.ctrl.gravity_initialized); + CHECK_FALSE(rig.ctrl.accel_offset); + CHECK((rig.ctrl.fusion.flags & M_IMU_3DOF_USE_GRAVITY_DUR_20MS) != 0); + rig.run(500, still, true); + CHECK(rig.ctrl.optical_tilt_updates > 0); + CHECK(rig.tilt_error() < 1.0f); + // Acceleration during motion is still rejected by the fusion gate. + rig.ctrl.accel = m_vec3_mul_scalar(rig.ctrl.accel, 2); + rift_s_controller_update_fusion(&rig.ctrl, rig.t); + CHECK(rig.ctrl.fusion.grav.is_accel); +} + +TEST_CASE("Rift S capture attitude diagnostics distinguish bounds tilt and radio gaps") +{ + rift_s_controller ctrl{}; + rift_s_attitude_sample sample{}; + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 1, &sample)); + CHECK(ctrl.diagnostics.last_attitude_reject == RIFT_S_ATTITUDE_EMPTY); + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 0, &sample)); + CHECK(ctrl.diagnostics.last_attitude_reject == RIFT_S_ATTITUDE_INVALID_TIME); + ctrl.attitude_history_count = 2; + ctrl.attitude_history[0] = {1000000000, 1000000000, 1000000000, {0, 0, 0, 1}, {0, 0, 0}, true}; + ctrl.attitude_history[1] = {1002000000, 1002000000, 1002000000, {0, 0, 0, 1}, {0, 0, 0}, true}; + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 999999999, &sample)); + CHECK(ctrl.diagnostics.last_attitude_reject == RIFT_S_ATTITUDE_TOO_OLD); + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 1022000001, &sample)); + CHECK(ctrl.diagnostics.last_attitude_reject == RIFT_S_ATTITUDE_TOO_NEW); + CHECK(rift_s_controller_get_attitude(&ctrl, 1001000000, &sample)); + CHECK(sample.capture_ns == 1001000000); + ctrl.attitude_history[1].tilt_valid = false; + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 1001000000, &sample)); + CHECK(ctrl.diagnostics.last_attitude_reject == RIFT_S_ATTITUDE_TILT_UNREADY); + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 1002000000, &sample)); + CHECK(ctrl.diagnostics.last_attitude_reject == RIFT_S_ATTITUDE_TILT_UNREADY); + ctrl.attitude_history[1].tilt_valid = true; + ctrl.attitude_history[1].capture_ns = 1012000000; + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 1001000000, &sample)); + CHECK(ctrl.diagnostics.last_attitude_reject == RIFT_S_ATTITUDE_RADIO_GAP); + CHECK(ctrl.diagnostics.last_attitude_reject_ns == 1001000000); + CHECK(ctrl.diagnostics.attitude_rejected[RIFT_S_ATTITUDE_TILT_UNREADY] == 2); +} + +TEST_CASE("Rift S moving left hand acquires optical tracking during its first camera exposures") +{ + FusionRig rig({-.016f, .104f, -.064f}); + rig.ctrl.base.device_type = XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER; + rig.ctrl.update_yaw_from_optical = true; + rig.truth = quat_from_axis_deg({1, 0, .3f}, 65); + // Unlike the old stationary gate, bootstrap succeeds above 0.1 rad/s. + rig.run(160, {.8f, -.6f, .4f}, true); + REQUIRE(rig.ctrl.gravity_initialized); + CHECK(rig.ctrl.gravity_moving); + CHECK(rig.ctrl.diagnostics.optical_accepted >= 2); + CHECK(rig.tilt_error() < 2); + xrt_space_relation relation{}; + CHECK(rift_s_controller_get_tracked_pose(&rig.ctrl.base, XRT_INPUT_GENERIC_TRACKER_POSE, rig.t - 33307700, + &relation) == XRT_SUCCESS); + CHECK(rig.ctrl.diagnostics.reject_clock == 0); + CHECK(rig.ctrl.fusion.gyro_bias.value.x == 0); // actual motion must never become gyro bias +} + +TEST_CASE("Rift S r13 left clock timing retains capture attitude across late resends") +{ + FusionRig rig({0, 0, 0}); + rig.run(160, {.4f, .2f, 0}, false); + // r13 left device clock was ahead of host by 1459.856 s. The 24 backwards + // reports were isolated resends by 1997/1998 us, not clock discontinuities. + if (rig.ctrl.imu_clock) + m_clock_windowed_skew_tracker_destroy(rig.ctrl.imu_clock); + rig.ctrl.imu_clock = nullptr; + rig.ctrl.attitude_history_count = rig.ctrl.attitude_history_start = 0; + rig.ctrl.clock_stable_samples = 0; + rig.ctrl.imu_to_host_ns = 0; + const timepoint_ns host = 381548007979LL, offset = 1459857273021LL; + for (int i = 0; i < 1024; i++) { + timepoint_ns capture = host + i * 1997000LL; + REQUIRE(rift_s_controller_record_attitude(&rig.ctrl, capture + offset, capture + (i % 3) * 300000)); + if (i % 42 == 0 && i > 0) + CHECK_FALSE(rift_s_controller_record_attitude(&rig.ctrl, capture + offset - 1997000, + capture + 2000000)); + } + CHECK(rig.ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_HISTORY_ORDER] == 24); + CHECK(rig.ctrl.diagnostics.reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY] == 0); + const timepoint_ns camera_capture = host + 1023 * 1997000LL - 33307700; + rift_s_attitude_sample at_camera{}; + REQUIRE(rift_s_controller_get_attitude(&rig.ctrl, camera_capture, &at_camera)); + t_constellation_pose_observation obs{}; + obs.capture_ns = camera_capture; + obs.pose = {at_camera.orientation, {.2f, 1.2f, -.4f}}; + obs.orientation_observed = true; + obs.distinct_leds = obs.inliers = 6; + rift_s_controller_push_pose_observation(&rig.ctrl.base, &obs); + CHECK(rig.ctrl.diagnostics.optical_accepted == 1); +} + +TEST_CASE("HMD capture-time IMU attitude predicts with a world-space gyro") +{ + rift_s_tracker tracker = {}; + os_mutex_init(&tracker.mutex); + os_mutex_init(&tracker.slam_mutex); + m_imu_3dof_init(&tracker.fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + m_relation_history_create(&tracker.fusion.history); + rift_s_slam_guard_init(&tracker.slam_guard); + tracker.ready_for_data = tracker.have_hw2mono = true; + tracker.last_frame_time = 1000000000; + tracker.device_from_imu = tracker.left_cam_from_imu = XRT_POSE_IDENTITY; + const xrt_vec3 axis = {1, 0, 0}; + math_quat_from_angle_vector(DEG_TO_RAD(65), &axis, &tracker.fusion.i3dof.rot); + const xrt_vec3 accel = {0, 4.145f, -8.89f}, gyro = {0, 0.5f, 0}; + rift_s_tracker_imu_update(&tracker, 1000000000, &accel, &gyro); + xrt_space_relation recorded, predicted; + REQUIRE(m_relation_history_get(tracker.fusion.history, 1000000000, &recorded) != + M_RELATION_HISTORY_RESULT_INVALID); + REQUIRE(m_relation_history_get(tracker.fusion.history, 1005000000, &predicted) != + M_RELATION_HISTORY_RESULT_INVALID); + xrt_quat expected; + math_quat_integrate_velocity(&recorded.pose.orientation, &gyro, 0.005, &expected); + float dot = std::abs(expected.x * predicted.pose.orientation.x + expected.y * predicted.pose.orientation.y + + expected.z * predicted.pose.orientation.z + expected.w * predicted.pose.orientation.w); + CHECK(dot > 0.999999f); + m_relation_history_destroy(&tracker.fusion.history); + m_imu_3dof_close(&tracker.fusion.i3dof); + os_mutex_destroy(&tracker.slam_mutex); + os_mutex_destroy(&tracker.mutex); +} + +#ifdef TEST_RIFT_S_STEAMVR_POSE +TEST_CASE("Rift S Touch model and world transforms preserve estimated coordinates and confidence") +{ + for (bool position_valid : {false, true}) { + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + rel.pose = {XRT_QUAT_IDENTITY, {0.4f, 1.2f, -0.6f}}; + rel.relation_flags = static_cast(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + (position_valid ? XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT : 0)); + xrt_pose model{XRT_QUAT_IDENTITY, {0.1f, 0.2f, 0.3f}}; + xrt_pose world{{0, float(std::sqrt(0.5)), 0, float(std::sqrt(0.5))}, {1, 2, 3}}; + transform_rift_s_touch_relation(&rel, &model, &world); + check_vec(rel.pose.position, {0.7f, 3.4f, 2.5f}); + CHECK(((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0) == position_valid); + CHECK(((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0) == position_valid); + vr::DriverPose_t pose{}; + apply_rift_s_touch_pose(&rel, &pose); + REQUIRE(pose.poseIsValid); + CHECK(pose.result == (position_valid ? vr::TrackingResult_Running_OK : vr::TrackingResult_Running_OutOfRange)); + CHECK(pose.vecPosition[0] == Catch::Approx(0.7f)); + CHECK(pose.vecPosition[1] == Catch::Approx(3.4f)); + CHECK(pose.vecPosition[2] == Catch::Approx(2.5f)); + } +} + +TEST_CASE("Rift S Touch OpenVR status follows orientation and position validity independently") +{ + for (bool connected : {false, true}) { + for (bool orientation_valid : {false, true}) { + for (bool position_valid : {false, true}) { + for (bool tracked : {false, true}) { + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + rel.pose = {XRT_QUAT_IDENTITY, {0.3f, 1.4f, -0.4f}}; + rel.linear_velocity = {1, -2, 3}; + rel.angular_velocity = {0.2f, 0.3f, -0.4f}; + rel.relation_flags = static_cast( + (orientation_valid ? XRT_SPACE_RELATION_ORIENTATION_VALID_BIT : 0) | + (position_valid ? XRT_SPACE_RELATION_POSITION_VALID_BIT : 0) | + (tracked ? XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_POSITION_TRACKED_BIT : 0) | + XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); + vr::DriverPose_t pose{}; + pose.deviceIsConnected = connected; + pose.poseIsValid = true; + pose.vecVelocity[0] = 99; + apply_rift_s_touch_pose(&rel, &pose); + CAPTURE(connected, orientation_valid, position_valid, tracked); + CHECK(pose.deviceIsConnected == connected); + CHECK(pose.poseIsValid == orientation_valid); + CHECK(pose.willDriftInYaw == !position_valid); + CHECK(pose.result == (!orientation_valid ? vr::TrackingResult_Uninitialized + : position_valid ? vr::TrackingResult_Running_OK + : vr::TrackingResult_Running_OutOfRange)); + CHECK(pose.vecPosition[0] == Catch::Approx(orientation_valid ? 0.3f : 0)); + CHECK(pose.qRotation.w == Catch::Approx(orientation_valid ? 1 : 0)); + CHECK(pose.vecVelocity[0] == (orientation_valid ? 1 : 0)); + CHECK(pose.vecAngularVelocity[1] == Catch::Approx(orientation_valid ? 0.3f : 0)); + // A reused output must clear invalid derivatives without changing pose status. + rel.relation_flags = static_cast( + rel.relation_flags & ~(XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT)); + apply_rift_s_touch_pose(&rel, &pose); + CHECK(pose.vecVelocity[0] == 0); + CHECK(pose.vecAngularVelocity[1] == 0); + } + } + } + } +} + +TEST_CASE("Rift S Touch OpenVR adapter rejects a nonfinite estimate") +{ + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + rel.pose = XRT_POSE_IDENTITY; + rel.relation_flags = static_cast(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT); + vr::DriverPose_t pose{}; + pose.deviceIsConnected = true; + rel.pose.position.x = std::numeric_limits::quiet_NaN(); + apply_rift_s_touch_pose(&rel, &pose); + CHECK_FALSE(pose.poseIsValid); + CHECK(pose.deviceIsConnected); + CHECK(pose.result == vr::TrackingResult_Running_OutOfRange); + CHECK(pose.vecPosition[0] == 0); +} + +TEST_CASE("SteamVR held pose clears velocity retained from the last tracked pose") +{ + xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; + rel.pose = XRT_POSE_IDENTITY; + rel.pose.position = {0.3f, 1.4f, -0.4f}; + rel.relation_flags = static_cast( + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); + rel.linear_velocity = {1, -2, 3}; + rel.angular_velocity = {0.2f, 0.3f, -0.4f}; + vr::DriverPose_t pose{}; + pose.poseIsValid = true; + pose.result = vr::TrackingResult_Running_OK; + apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true); + CHECK(pose.vecVelocity[1] == -2); + CHECK(pose.vecAngularVelocity[1] == Catch::Approx(0.3f)); + // GetPose reuses this DriverPose every frame. Invalid velocity must clear the old + // value, while current IMU angular velocity continues to be copied. + rel.relation_flags = + static_cast(rel.relation_flags & ~XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); + apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true); + REQUIRE(pose.poseIsValid); + CHECK(pose.result == vr::TrackingResult_Running_OK); + for (double v : pose.vecVelocity) + CHECK(v == 0); + CHECK(pose.vecAngularVelocity[1] == Catch::Approx(0.3f)); + // Invalid or overflowing angular velocity must not retain a prior valid sample either. + rel.angular_velocity.x = std::numeric_limits::infinity(); + apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true); + for (double v : pose.vecAngularVelocity) + CHECK(v == 0); +} + +TEST_CASE("Rift S Touch without a position fix rests below the head, never at the floor origin") +{ + // r18: before the first fix SteamVR drew the orientation-only controllers at the tracking + // origin, under the floor. The resting position follows the head heading but not its pitch. + const float sizes[] = {1.6f, 1.1f}; + for (float height : sizes) { + xrt_pose head = {XRT_QUAT_IDENTITY, {0.4f, height, -0.2f}}; + auto left = nominal_hand_position(head, XRT_HAND_LEFT); + auto right = nominal_hand_position(head, XRT_HAND_RIGHT); + check_vec(left, {0.2f, height - 0.5f, -0.5f}); + check_vec(right, {0.6f, height - 0.5f, -0.5f}); + // Turned 90 degrees left (facing -X) and looking 80 degrees down. + xrt_quat yaw, pitch; + const xrt_vec3 up = {0, 1, 0}, side = {1, 0, 0}; + math_quat_from_angle_vector(DEG_TO_RAD(90.f), &up, &yaw); + math_quat_from_angle_vector(DEG_TO_RAD(-80.f), &side, &pitch); + math_quat_rotate(&yaw, &pitch, &head.orientation); + left = nominal_hand_position(head, XRT_HAND_LEFT); + check_vec(left, {0.1f, height - 0.5f, 0.0f}); + CHECK(left.y > 0.5f); + } +} + +TEST_CASE("SteamVR pose conversion rejects nonfinite pose components") +{ + xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; + relation.pose = XRT_POSE_IDENTITY; + relation.pose.position.y = 1.6f; + relation.relation_flags = static_cast(XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT); + vr::DriverPose_t pose{}; + pose.poseIsValid = true; + apply_pose(&relation, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT); + CHECK(pose.vecPosition[1] == Catch::Approx(1.6f)); + relation.pose.position.x = std::numeric_limits::quiet_NaN(); + apply_pose(&relation, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT); + CHECK_FALSE(pose.poseIsValid); + CHECK(std::isfinite(pose.vecPosition[0])); + CHECK(pose.result == vr::TrackingResult_Running_OutOfRange); +} +#endif + +TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the association prior") +{ + rift_s_controller ctrl = {}; + ctrl.gravity_initialized = ctrl.update_yaw_from_optical = true; + ctrl.device_type = RIFT_S_DEVICE_RIGHT_CONTROLLER; + ctrl.fusion.rot = ctrl.pose.orientation = XRT_QUAT_IDENTITY; + os_mutex_init(&ctrl.mutex); + rift_s_position_filter_reset(&ctrl.position_filter); + for (int i = 0; i < 100; ++i) { + timepoint_ns ns = 1000000000LL + i * 2000000LL; + REQUIRE(rift_s_controller_record_attitude(&ctrl, ns, ns)); + } + t_constellation_pose_observation obs = {}; + obs.capture_ns = 1100000000LL; + obs.pose = XRT_POSE_IDENTITY; + obs.pose.position = {0.1f, 1.1f, -0.4f}; + obs.orientation_observed = true; + obs.inliers = obs.distinct_leds = 6; + obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-6f; + obs.hypothesis = "test"; + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + const xrt_pose trusted = ctrl.last_tracked_pose; + const timepoint_ns trusted_ns = ctrl.last_tracked_pose_ts; + const auto tilt_updates = ctrl.optical_tilt_updates; + const xrt_vec3 yaw_axis = {0, 1, 0}; + obs.pose.position.x += 1; + math_quat_from_angle_vector(DEG_TO_RAD(30.f), &yaw_axis, &obs.pose.orientation); + for (int i = 1; i <= 2; ++i) { + obs.capture_ns = trusted_ns + i * 10000000LL; + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + CHECK(ctrl.last_tracked_pose_ts == trusted_ns); + CHECK(ctrl.last_tracked_pose.position.x == trusted.position.x); + CHECK(ctrl.fusion.rot.y == Catch::Approx(0).margin(1e-6)); + CHECK(ctrl.optical_tilt_updates == tilt_updates); + CHECK(ctrl.diagnostics.optical_accepted == 1); + } + // A continuous fit at the trusted position clears the rejected candidate. + obs.capture_ns += 10000000LL; + obs.pose = trusted; + rift_s_controller_push_pose_observation(&ctrl.base, &obs); + CHECK(ctrl.position_filter.candidate_count == 0); + CHECK(ctrl.last_tracked_pose_ts == obs.capture_ns); + CHECK(ctrl.diagnostics.optical_accepted == 2); + if (ctrl.imu_clock) + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + os_mutex_destroy(&ctrl.mutex); +} + +#ifdef TEST_RIFT_S_STEAMVR_POSE +TEST_CASE("Rift S complete grip to SteamVR model chain preserves metric position in a rotated world") +{ + const xrt_vec3 world_axis = {0, 1, 0}; + xrt_quat world_rotation; + math_quat_from_angle_vector(DEG_TO_RAD(73.f), &world_axis, &world_rotation); + const xrt_pose world = {world_rotation, {1.2f, 0.4f, -2.1f}}; + for (bool right : {false, true}) { + PublishedPoseController fixture(right); + for (float angle : {0.f, 90.f}) { + CAPTURE(right, angle); + const xrt_vec3 axis = {1, 0, 0}; + xrt_quat local_rotation; + math_quat_from_angle_vector(DEG_TO_RAD(angle), &axis, &local_rotation); + xrt_pose local = {local_rotation, {right ? 0.3f : -0.3f, 0.8f, -0.6f}}, measured; + math_pose_transform(&world, &local, &measured); + const timepoint_ns ts = 1000000000 + (angle > 0 ? 2000000 : 0); + fixture.measured(measured.orientation, measured.position, ts); + const auto imu = fixture.query(XRT_INPUT_GENERIC_TRACKER_POSE, ts); + check_vec(imu.pose.position, measured.position); + const auto grip = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, ts); + const auto grip_model = rift_s_touch_grip_to_model(right ? XRT_HAND_RIGHT : XRT_HAND_LEFT); + xrt_space_relation model = grip; + const xrt_pose origin = XRT_POSE_IDENTITY; + transform_rift_s_touch_relation(&model, &grip_model, &origin); + // Independent device-from-model lever arm, minus firmware IMU origin. + const xrt_vec3 model_in_device = model_to_device(right, {0, 0, 0}); + xrt_vec3 lever = m_vec3_sub(model_in_device, fixture.ctrl.P_device_imu.position), rotated; + math_quat_rotate_vec3(&measured.orientation, &lever, &rotated); + const auto expected = m_vec3_add(measured.position, rotated); + check_vec(model.pose.position, expected); + const xrt_vec3 forward = {0, 0, -1}; + xrt_vec3 expected_forward, actual_forward; + const auto device_forward = rotate_x(native_model_pitch, forward); + math_quat_rotate_vec3(&measured.orientation, &device_forward, &expected_forward); + math_quat_rotate_vec3(&model.pose.orientation, &forward, &actual_forward); + check_vec(actual_forward, expected_forward); + vr::DriverPose_t driver{}; + driver.poseIsValid = true; + apply_rift_s_touch_pose(&model, &driver); + REQUIRE(driver.poseIsValid); + CHECK(driver.vecPosition[0] == Catch::Approx(expected.x).margin(2e-6)); + CHECK(driver.vecPosition[1] == Catch::Approx(expected.y).margin(2e-6)); + CHECK(driver.vecPosition[2] == Catch::Approx(expected.z).margin(2e-6)); + } + } +} +#endif + +TEST_CASE("Rift S controller does not integrate missing radio motion") +{ + rift_s_controller ctrl = {}; + m_imu_3dof_init(&ctrl.fusion, 0); + ctrl.accel = {0, 9.81f, 0}; + ctrl.gyro = {0, 1, 0}; + for (int i = 0; i < 40; ++i) { + timepoint_ns t = 1000000000LL + i * 2000000LL; + rift_s_controller_update_fusion(&ctrl, t); + REQUIRE(rift_s_controller_record_attitude(&ctrl, t, t)); + } + const xrt_quat before = ctrl.fusion.rot; + const xrt_vec3 position = {0.3f, 1.1f, -0.5f}, variance = {1e-5f, 1e-5f, 1e-5f}; + rift_s_position_filter_reset(&ctrl.position_filter); + rift_s_position_filter_fix(&ctrl.position_filter, 1078000000LL, &position, &variance, 0.15f); + ctrl.optical_heading_valid = true; + rift_s_controller_update_fusion(&ctrl, 3000000000LL); + CHECK(ctrl.fusion.rot.y == Catch::Approx(before.y)); + CHECK(ctrl.fusion.rot.w == Catch::Approx(before.w)); + CHECK(ctrl.attitude_history_count == 0); + CHECK_FALSE(ctrl.optical_heading_valid); + CHECK(ctrl.position_filter.frozen); + REQUIRE(rift_s_controller_record_attitude(&ctrl, 3000000000LL, 3000000000LL)); + rift_s_attitude_sample sample{}; + CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 2000000000LL, &sample)); + rift_s_controller_update_fusion(&ctrl, 3002000000LL); + CHECK(ctrl.fusion.rot.y != before.y); + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + m_imu_3dof_close(&ctrl.fusion); +} + +TEST_CASE("Rift S controller rejects extreme finite IMU and recovers attitude") +{ + rift_s_controller ctrl = {}; + m_imu_3dof_init(&ctrl.fusion, 0); + ctrl.accel = {0, 9.81f, 0}; + rift_s_controller_update_fusion(&ctrl, 1000000000LL); + ctrl.gyro = {1e30f, 0, 0}; + rift_s_controller_update_fusion(&ctrl, 1002000000LL); + CHECK(ctrl.diagnostics.reject_nonfinite == 1); + CHECK(ctrl.fusion.last.timestamp_ns == 1000000000LL); + CHECK(std::isfinite(ctrl.fusion.rot.w)); + ctrl.gyro = {0, 0, 0}; + ctrl.fusion.rot.w = NAN; // Recovery also covers a previously poisoned backend state. + rift_s_controller_update_fusion(&ctrl, 1004000000LL); + CHECK(std::isfinite(ctrl.fusion.rot.w)); + REQUIRE(rift_s_controller_record_attitude(&ctrl, 1004000000LL, 1004000000LL)); + CHECK(ctrl.attitude_history_count == 1); + m_clock_windowed_skew_tracker_destroy(ctrl.imu_clock); + m_imu_3dof_close(&ctrl.fusion); +} + +#ifdef TEST_RIFT_S_STEAMVR_POSE +TEST_CASE("SteamVR pose conversion does not publish overflowed rotated velocity") +{ + xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; + relation.pose = XRT_POSE_IDENTITY; + const xrt_vec3 axis = {0, 0, 1}; + math_quat_from_angle_vector(DEG_TO_RAD(45), &axis, &relation.pose.orientation); + relation.relation_flags = static_cast(XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); + const float huge = std::numeric_limits::max(); + relation.angular_velocity = {huge, huge, huge}; + vr::DriverPose_t pose{}; + apply_pose(&relation, &pose); + for (double value : pose.vecAngularVelocity) + CHECK(std::isfinite(value)); +} +#endif + +TEST_CASE("HMD IMU ingestion drops backend input during session replacement") +{ + rift_s_tracker tracker{}; + os_mutex_init(&tracker.mutex); + os_mutex_init(&tracker.slam_mutex); + m_imu_3dof_init(&tracker.fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + tracker.ready_for_data = tracker.have_hw2mono = true; + tracker.last_frame_time = tracker.last_camera_arrival_ns = os_monotonic_get_ns(); + struct Counter + { + xrt_imu_sink sink; + int calls = 0; + } counter{}; + counter.sink.push_imu = [](xrt_imu_sink *sink, xrt_imu_sample *) { + ++reinterpret_cast(sink)->calls; + }; + tracker.slam_sinks.imu = &counter.sink; + const xrt_vec3 accel{0, 9.81f, 0}, gyro{}; + os_mutex_lock(&tracker.slam_mutex); + rift_s_tracker_imu_update(&tracker, tracker.last_frame_time, &accel, &gyro); + CHECK(counter.calls == 0); + CHECK(tracker.fusion.last_imu_local_timestamp_ns == tracker.last_frame_time); + os_mutex_unlock(&tracker.slam_mutex); + rift_s_tracker_imu_update(&tracker, tracker.last_frame_time + 4000000, &accel, &gyro); + CHECK(counter.calls == 1); + m_imu_3dof_close(&tracker.fusion.i3dof); + os_mutex_destroy(&tracker.slam_mutex); + os_mutex_destroy(&tracker.mutex); +} + +TEST_CASE("Rift S Touch native frames reproduce Oculus controller constants") +{ + for (bool left : {true, false}) { + CAPTURE(left); + xrt_pose model_device, model_grip, device_model, aim, grip; + rift_s_controller_model_frames(left, &model_device, &model_grip); + math_pose_invert(&model_device, &device_model); + // Official SteamVR Oculus driver pose, independently checked against the LCON components. + const double norm = std::hypot(0.3371, 0.94147); + CHECK(device_model.orientation.x == Catch::Approx(0.3371 / norm).margin(2e-7)); + CHECK(device_model.orientation.w == Catch::Approx(0.94147 / norm).margin(2e-7)); + check_vec(device_model.position, {left ? -0.007f : 0.007f, 0.036124f, -0.037617f}); + rift_s_controller_pose_frames(left, &aim, &grip); + // LibOVR LCON table. The rounded driver quaternion/model components differ by 0.000533 degrees. + check_vec(aim.position, {0, 0, -0.055f}); + check_vec(grip.position, {0, -0.03f, 0.04f}); + CHECK(std::abs(aim.orientation.x) < 5e-6f); + CHECK(aim.orientation.w == Catch::Approx(1).margin(2e-7)); + CHECK(grip.orientation.x == Catch::Approx(0.5).margin(5e-6)); + CHECK(grip.orientation.w == Catch::Approx(0.866025403784).margin(5e-6)); +#ifdef TEST_RIFT_S_STEAMVR_POSE + PublishedPoseController fixture(!left); + const xrt_vec3 factory_imu = left ? xrt_vec3{-0.00558411609381f, -0.00711983907968f, 0.0198550093919f} + : xrt_vec3{0.0055856378749f, -0.00711983907968f, 0.0198451988399f}; + fixture.ctrl.P_device_imu.position = factory_imu; + math_pose_invert(&fixture.ctrl.P_device_imu, &fixture.ctrl.P_imu_device); + const xrt_vec3 world_imu = {0.2f, 1.4f, -0.8f}; + fixture.measured(XRT_QUAT_IDENTITY, world_imu, 1000000000); + auto published = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, 1000000000); + const xrt_pose grip_model = rift_s_touch_grip_to_model(left ? XRT_HAND_LEFT : XRT_HAND_RIGHT); + math_pose_transform(&published.pose, &grip_model, &published.pose); + check_vec(published.pose.position, + m_vec3_add(world_imu, m_vec3_sub(device_model.position, factory_imu))); + CHECK(published.pose.orientation.x == Catch::Approx(0.3371 / norm).margin(2e-7)); + CHECK(published.pose.orientation.w == Catch::Approx(0.94147 / norm).margin(2e-7)); +#endif + } +} diff --git a/tests/tests_rift_s_world_anchor.cpp b/tests/tests_rift_s_world_anchor.cpp new file mode 100644 index 000000000..83d495157 --- /dev/null +++ b/tests/tests_rift_s_world_anchor.cpp @@ -0,0 +1,836 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "rift_s_world_anchor.h" +#include "rift_s_slam_guard.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "math/m_api.h" + +namespace { +rift_s_world_anchor_config +calibration(bool native = true) +{ + rift_s_world_anchor_config c{}; + const float k[2][6] = { + {0.274657339f, 0.072651282f, -0.284994245f, 0.224762872f, -0.0842394754f, 0.0130622024f}, + {0.279467911f, 0.0535824969f, -0.255054951f, 0.202277347f, -0.0756442547f, 0.0117395986f}}; + c.camera[0].projection = {312.359253f, 233.292801f, 190.499924f, 190.499924f}; + c.camera[1].projection = {315.850128f, 237.907181f, 190.177917f, 190.177917f}; + for (unsigned e = 0; e < 2; ++e) { + c.camera[e].roi.extent = {640, 480}; + std::copy_n(k[e], 6, c.camera[e].distortion.k); + } + c.camera[0].distortion.p1 = -0.000742965436f; + c.camera[0].distortion.p2 = -0.000313968281f; + c.camera[1].distortion.p1 = 4.58915711e-05f; + c.camera[1].distortion.p2 = -0.000414691196f; + c.imu_from_camera[0] = {{0.695778f, 0.695791f, -0.127312f, 0.124737f}, {-0.055528f, -0.009581f, -0.001194f}}; + c.imu_from_camera[1] = {{0.693510f, 0.697892f, -0.125286f, 0.127646f}, {0.055125f, -0.008799f, -0.001498f}}; + for (unsigned e = 0; e < 2; ++e) { + auto &p = c.imu_from_camera[e]; + if (!native) + p.orientation = {0, 0, 0, 1}; + double n = std::sqrt(p.orientation.x * p.orientation.x + p.orientation.y * p.orientation.y + + p.orientation.z * p.orientation.z + p.orientation.w * p.orientation.w); + p.orientation.x /= n; + p.orientation.y /= n; + p.orientation.z /= n; + p.orientation.w /= n; + } + return c; +} +cv::Matx33d +rotation(xrt_quat q) +{ + double x = q.x, y = q.y, z = q.z, w = q.w; + return {1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w), + 2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w), + 2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)}; +} +cv::Vec3d +translation(xrt_pose p) +{ + return {p.position.x, p.position.y, p.position.z}; +} +std::array, 2> +observations(const rift_s_world_anchor_config &c, xrt_pose head) +{ + std::array, 2> out; + for (int y = 50; y <= 430; y += 45) + for (int x = 55; x <= 590; x += 55) { + xrt_vec2 pixel{float(x), float(y)}; + xrt_vec3 ray; + REQUIRE(rift_s_world_anchor_unproject(&c.camera[0], &pixel, &ray)); + double depth = 1.2 + 0.15 * ((x / 55 + 2 * y / 45) % 9); + cv::Vec3d in_imu = + rotation(c.imu_from_camera[0].orientation) * (cv::Vec3d{ray.x, ray.y, ray.z} * depth) + + translation(c.imu_from_camera[0]); + cv::Vec3d world = rotation(head.orientation) * in_imu + translation(head); + for (unsigned e = 0; e < 2; ++e) { + cv::Vec3d native = rotation(c.imu_from_camera[e].orientation).t() * + (in_imu - translation(c.imu_from_camera[e])); + xrt_vec3 p{float(native[0]), float(native[1]), float(native[2])}; + xrt_vec2 projected; + REQUIRE(rift_s_world_anchor_project(&c.camera[e], &p, &projected)); + out[e].push_back({{float(world[0]), float(world[1]), float(world[2])}, projected}); + } + } + return out; +} +bool +solve(const rift_s_world_anchor_config &c, + const std::array, 2> &o, + rift_s_world_anchor_result &r) +{ + const rift_s_world_anchor_observation *ptr[2] = {o[0].data(), o[1].data()}; + uint32_t n[2] = {uint32_t(o[0].size()), uint32_t(o[1].size())}; + return rift_s_world_anchor_solve(&c, ptr, n, &r); +} +struct Images +{ + cv::Mat images[RIFT_S_WORLD_MAX_CAMERAS]; + xrt_frame frames[RIFT_S_WORLD_MAX_CAMERAS]{}; + Images(const rift_s_world_anchor_config &c, float head_x = 0, float yaw = 0) + { + cv::Mat small(160, 200, CV_8UC1); + cv::RNG random(19); + random.fill(small, cv::RNG::UNIFORM, 0, 256); + cv::Mat texture; + cv::resize(small, texture, {2000, 1600}, 0, 0, cv::INTER_CUBIC); + for (unsigned e = 0; e < (c.camera_count ? c.camera_count : 2); ++e) { + cv::Mat map(480, 640, CV_32FC2); + for (int y = 0; y < 480; ++y) + for (int x = 0; x < 640; ++x) { + xrt_vec2 pixel{float(x), float(y)}; + xrt_vec3 ray{}; + if (!rift_s_world_anchor_unproject(&c.camera[e], &pixel, &ray)) { + map.at(y, x) = {-1, -1}; + continue; + } + auto r = rotation({0, float(std::sin(yaw / 2)), 0, float(std::cos(yaw / 2))}); + auto d = r * rotation(c.imu_from_camera[e].orientation) * + cv::Vec3d{ray.x, ray.y, ray.z}; + auto origin = r * translation(c.imu_from_camera[e]) + cv::Vec3d{head_x, 0, 0}; + double scale = (2.3 - origin[2]) / d[2]; + map.at(y, x) = + scale > 0 ? cv::Vec2f{float(1000 + 100 * (d[0] * scale + origin[0])), + float(800 + 100 * (d[1] * scale + origin[1]))} + : cv::Vec2f{-1, -1}; + } + cv::remap(texture, images[e], map, cv::Mat(), cv::INTER_LINEAR); + frames[e].format = XRT_FORMAT_L8; + frames[e].width = 640; + frames[e].height = 480; + frames[e].stride = images[e].step; + frames[e].size = images[e].total(); + frames[e].data = images[e].data; + frames[e].timestamp = 1000000000; + } + } +}; +using Anchor = std::unique_ptr; +} // namespace + +TEST_CASE("Room recovery matches the native Fisheye62 pixel references") +{ + auto c = calibration(); + const xrt_vec2 angular[] = {{0.8f, 0.6f}, {-1.1f, 0.4f}, {0.2f, -0.7f}}; + const xrt_vec2 pixels[2][3] = { + {{497.100364f, 371.917144f}, {49.367862f, 328.687290f}, {355.628758f, 81.468439f}}, + {{500.828640f, 376.514696f}, {53.946196f, 332.982240f}, {359.179827f, 86.220055f}}, + }; + for (unsigned camera = 0; camera < 2; ++camera) { + for (unsigned i = 0; i < 3; ++i) { + CAPTURE(camera, i); + double theta = std::hypot(angular[i].x, angular[i].y), scale = std::sin(theta) / theta; + xrt_vec3 ray{float(angular[i].x * scale), float(angular[i].y * scale), float(std::cos(theta))}; + xrt_vec2 pixel; + REQUIRE(rift_s_world_anchor_project(&c.camera[camera], &ray, &pixel)); + CHECK(pixel.x == Catch::Approx(pixels[camera][i].x).epsilon(0).margin(0.0001)); + CHECK(pixel.y == Catch::Approx(pixels[camera][i].y).epsilon(0).margin(0.0001)); + xrt_vec3 recovered; + REQUIRE(rift_s_world_anchor_unproject(&c.camera[camera], &pixels[camera][i], &recovered)); + CHECK(std::hypot(std::hypot(recovered.x - ray.x, recovered.y - ray.y), recovered.z - ray.z) < 1e-6); + } + } +} + +TEST_CASE("Rift S world anchor inverts all six radial and both tangential camera terms") +{ + auto c = calibration(); + double worst = 0; + for (unsigned e = 0; e < 2; ++e) + for (int y = 10; y < 480; y += 17) + for (int x = 10; x < 640; x += 23) { + xrt_vec2 pixel{float(x), float(y)}, roundtrip; + xrt_vec3 ray; + INFO("pixel " << x << "," << y); + REQUIRE(rift_s_world_anchor_unproject(&c.camera[e], &pixel, &ray)); + REQUIRE(rift_s_world_anchor_project(&c.camera[e], &ray, &roundtrip)); + worst = + std::max(worst, double(std::hypot(roundtrip.x - pixel.x, roundtrip.y - pixel.y))); + } + CHECK(worst < 0.0001); +} +TEST_CASE("Rift S stereo anchors recover IMU pose without position bias or lateral gain") +{ + auto c = calibration(); + for (float x : {-2.f, 0.f, 2.f}) { + xrt_pose expected{{0, 0.24740396f, 0, 0.96891242f}, {x, 1.3f, -0.45f}}; + auto obs = observations(c, expected); + rift_s_world_anchor_result r{}; + REQUIRE(solve(c, obs, r)); + CHECK(r.relocalized); + CHECK(r.inliers[0] >= 80); + CHECK(r.inliers[1] >= 80); + CHECK(cv::norm(translation(expected) - translation(r.world_from_imu)) < 0.00005); + CHECK(r.max_reprojection_px < 0.001); + CHECK(cv::norm(cv::Mat(rotation(expected.orientation) - rotation(r.world_from_imu.orientation))) < + 0.0001); + } +} +TEST_CASE("Rift S stereo anchors reject camera disagreement and concentrated false matches") +{ + auto c = calibration(); + xrt_pose expected{{0, 0, 0, 1}, {0, 1.3f, 0}}; + auto obs = observations(c, expected); + rift_s_world_anchor_result r{}; + for (auto &o : obs[1]) + o.world_point.x += 0.12f; + CHECK_FALSE(solve(c, obs, r)); + obs = observations(c, expected); + for (auto &eye : obs) + for (auto &o : eye) + o.pixel = {320 + 0.02f * (o.pixel.x - 320), 240 + 0.02f * (o.pixel.y - 240)}; + CHECK_FALSE(solve(c, obs, r)); + obs = observations(c, expected); + for (auto &eye : obs) + for (size_t i = 0; i < eye.size() * 3 / 4; ++i) + eye[i].world_point.x += float(i % 7) * 0.2f; + CHECK_FALSE(solve(c, obs, r)); +} +TEST_CASE("Rift S visual keyframes persist and invalid maps fail closed") +{ + auto c = calibration(false); + Images images(c); + char directory[] = "/tmp/rift-s-anchor-XXXXXX"; + REQUIRE(mkdtemp(directory)); + std::string path = std::string(directory) + "/map"; + Anchor anchor(rift_s_world_anchor_create(&c, path.c_str()), rift_s_world_anchor_destroy); + REQUIRE(anchor); + CHECK_FALSE(rift_s_world_anchor_has_map(anchor.get())); + rift_s_world_anchor_result r{}; + xrt_pose trusted{{0, 0, 0, 1}, {0, 0, 0}}; + REQUIRE(rift_s_world_anchor_process(anchor.get(), &images.frames[0], &images.frames[1], &trusted, &r)); + CHECK(r.recorded); + CHECK(r.keyframes == 1); + REQUIRE(rift_s_world_anchor_save(anchor.get())); + anchor.reset(rift_s_world_anchor_create(&c, path.c_str())); + REQUIRE(rift_s_world_anchor_has_map(anchor.get())); + REQUIRE(rift_s_world_anchor_process(anchor.get(), &images.frames[0], &images.frames[1], nullptr, &r)); + CHECK(r.relocalized); + CHECK(cv::norm(translation(r.world_from_imu)) < 0.03); + rift_s_world_anchor_clear(anchor.get()); + CHECK_FALSE(rift_s_world_anchor_has_map(anchor.get())); + REQUIRE(rift_s_world_anchor_reload(anchor.get())); + images.frames[1].timestamp += 1; + CHECK_FALSE(rift_s_world_anchor_process(anchor.get(), &images.frames[0], &images.frames[1], nullptr, &r)); + auto changed = c; + changed.camera[0].projection.fx += 0.01f; + Anchor mismatch(rift_s_world_anchor_create(&changed, path.c_str()), rift_s_world_anchor_destroy); + REQUIRE(mismatch); + CHECK_FALSE(rift_s_world_anchor_has_map(mismatch.get())); + { + std::fstream file(path, std::ios::in | std::ios::out | std::ios::binary); + file.seekg(80); + char original; + REQUIRE(file.get(original)); + file.seekp(80); + file.put(char(original ^ 0x7f)); + } + CHECK_FALSE(rift_s_world_anchor_reload(anchor.get())); + CHECK_FALSE(rift_s_world_anchor_has_map(anchor.get())); + CHECK_FALSE(rift_s_world_anchor_save(anchor.get())); + rift_s_world_anchor_clear(anchor.get()); + REQUIRE(rift_s_world_anchor_save(anchor.get())); + Anchor empty(rift_s_world_anchor_create(&c, path.c_str()), rift_s_world_anchor_destroy); + REQUIRE(empty); + CHECK_FALSE(rift_s_world_anchor_has_map(empty.get())); + unlink(path.c_str()); + rmdir(directory); +} + +TEST_CASE("A changed boundary cannot reuse an older physical world's map") +{ + auto c = calibration(false); + char directory[] = "/tmp/rift-s-boundary-XXXXXX"; + REQUIRE(mkdtemp(directory)); + std::string path = std::string(directory) + "/map", boundary = std::string(directory) + "/boundary"; + const char *original = R"({"jsonid":"chaperone_info","universes":[{"universeID":"2", + "standing":{"yaw":0.3,"translation":[0.5,0.1,-0.2]},"play_area":[2,3], + "collision_bounds":[[[0,0,0],[0,2,0],[1,2,0],[1,0,0]]],"time":"before"}]})"; + std::ofstream(boundary) << original; + c.require_boundary = true; + std::snprintf(c.boundary_path, sizeof(c.boundary_path), "%s", boundary.c_str()); + Images images(c); + Anchor anchor(rift_s_world_anchor_create(&c, path.c_str()), rift_s_world_anchor_destroy); + REQUIRE(anchor); + rift_s_world_anchor_result result{}; + CHECK(rift_s_world_anchor_has_boundary(anchor.get())); + xrt_pose trusted{{0, 0, 0, 1}, {0, 0, 0}}; + REQUIRE(rift_s_world_anchor_process(anchor.get(), &images.frames[0], &images.frames[1], &trusted, &result)); + REQUIRE(rift_s_world_anchor_save(anchor.get())); + REQUIRE(rift_s_world_anchor_reload(anchor.get())); + SECTION("SteamVR metadata and JSON formatting do not change the room frame") { + std::ofstream(boundary) << R"({ "universes": [{"time":"after", "play_area":[2.0,3.0], + "standing":{"translation":[0.5,0.1,-0.2],"yaw":0.3},"universeID":"2", + "collision_bounds":[[[0.0,0,0],[0,2.0,0],[1.0,2,0],[1,0,0]]]}], + "jsonid":"chaperone_info" })"; + REQUIRE(rift_s_world_anchor_reload(anchor.get())); + CHECK(rift_s_world_anchor_has_map(anchor.get())); + REQUIRE(rift_s_world_anchor_save(anchor.get())); + } + SECTION("A changed standing yaw cannot silently rebind a live map") { + std::string changed = original; + changed.replace(changed.find("0.3"), 3, "0.4"); + std::ofstream(boundary) << changed; + CHECK_FALSE(rift_s_world_anchor_save(anchor.get())); + CHECK_FALSE(rift_s_world_anchor_reload(anchor.get())); + } + SECTION("A changed play area cannot reuse the saved map") { + std::string changed = original; + changed.replace(changed.find("[2,3]"), 5, "[2,4]"); + std::ofstream(boundary) << changed; + CHECK_FALSE(rift_s_world_anchor_save(anchor.get())); + CHECK_FALSE(rift_s_world_anchor_reload(anchor.get())); + } + SECTION("Changed collision bounds cannot reuse the saved map") { + std::string changed = original; + changed.replace(changed.find("[1,2,0]"), 7, "[1.5,2,0]"); + std::ofstream(boundary) << changed; + CHECK_FALSE(rift_s_world_anchor_save(anchor.get())); + CHECK_FALSE(rift_s_world_anchor_reload(anchor.get())); + } + // Accept wrote the new boundary but saving its new map failed. The old map remains on disk. + std::string changed = original; + changed.replace(changed.find("[0.5,0.1,-0.2]"), 14, "[0.6,0.1,-0.2]"); + std::ofstream(boundary) << changed; + CHECK_FALSE(rift_s_world_anchor_reload(anchor.get())); + CHECK_FALSE(rift_s_world_anchor_has_map(anchor.get())); + CHECK(rift_s_world_anchor_has_boundary(anchor.get())); + unlink(boundary.c_str()); + CHECK_FALSE(rift_s_world_anchor_has_boundary(anchor.get())); + CHECK_FALSE(rift_s_world_anchor_save(anchor.get())); + unlink(path.c_str()); + rmdir(directory); +} + +TEST_CASE("Legacy maps migrate only while their exact saved boundary still matches") +{ + for (uint32_t version : {2u, 3u}) { + CAPTURE(version); + auto c = calibration(false); + char directory[] = "/tmp/rift-s-legacy-XXXXXX"; + REQUIRE(mkdtemp(directory)); + std::string path = std::string(directory) + "/map", boundary = std::string(directory) + "/boundary"; + const std::string original = R"({"jsonid":"chaperone_info","universes":[{"universeID":"2", + "standing":{"yaw":0.3,"translation":[0.5,0.1,-0.2]},"play_area":[2,3], + "collision_bounds":[[[0,0,0],[0,2,0],[1,2,0],[1,0,0]]],"time":"before"}]})"; + std::ofstream(boundary) << original; + c.require_boundary = true; + std::snprintf(c.boundary_path, sizeof(c.boundary_path), "%s", boundary.c_str()); + Images images(c); + Anchor anchor(rift_s_world_anchor_create(&c, path.c_str()), rift_s_world_anchor_destroy); + xrt_pose trusted = XRT_POSE_IDENTITY; + rift_s_world_anchor_result result{}; + REQUIRE(rift_s_world_anchor_process(anchor.get(), &images.frames[0], &images.frames[1], &trusted, &result)); + REQUIRE(rift_s_world_anchor_save(anchor.get())); + std::ifstream input(path, std::ios::binary); + std::vector bytes((std::istreambuf_iterator(input)), {}); + auto read32 = [&](size_t offset) { + REQUIRE(offset + 4 <= bytes.size()); + return uint32_t(bytes[offset]) | uint32_t(bytes[offset + 1]) << 8 | + uint32_t(bytes[offset + 2]) << 16 | uint32_t(bytes[offset + 3]) << 24; + }; + if (version == 2) { + std::vector legacy(bytes.begin(), bytes.begin() + 28); + size_t offset = 28; + uint32_t count = read32(24); + for (unsigned frame = 0; frame < count; ++frame) { + uint32_t points = read32(offset + 28); + legacy.insert(legacy.end(), bytes.begin() + offset, bytes.begin() + offset + 32); + offset += 32; + for (unsigned point = 0; point < points; ++point, offset += 80) + legacy.insert(legacy.end(), bytes.begin() + offset, bytes.begin() + offset + 76); + } + REQUIRE(offset + 8 == bytes.size()); + legacy.resize(legacy.size() + 8); + bytes = std::move(legacy); + } + auto fnv = [](const uint8_t *data, size_t size) { + uint64_t h = 14695981039346656037ULL; + for (size_t i = 0; i < size; ++i) + h = (h ^ data[i]) * 1099511628211ULL; + return h; + }; + auto put64 = [&](size_t offset, uint64_t value) { + for (unsigned i = 0; i < 8; ++i) + bytes[offset + i] = uint8_t(value >> (8 * i)); + }; + bytes[4] = uint8_t(version); + put64(16, fnv(reinterpret_cast(original.data()), original.size())); + put64(bytes.size() - 8, fnv(bytes.data(), bytes.size() - 8)); + std::ofstream file(path, std::ios::binary | std::ios::trunc); + file.write(reinterpret_cast(bytes.data()), bytes.size()); + file.close(); + REQUIRE(rift_s_world_anchor_reload(anchor.get())); + CHECK(rift_s_world_anchor_has_map(anchor.get())); + std::string rewritten = original; + rewritten.replace(rewritten.find("before"), 6, "after"); + std::ofstream(boundary) << rewritten; + CHECK_FALSE(rift_s_world_anchor_reload(anchor.get())); + CHECK_FALSE(rift_s_world_anchor_save(anchor.get())); + std::ifstream retained(path, std::ios::binary); + std::vector unchanged((std::istreambuf_iterator(retained)), {}); + CHECK(unchanged == bytes); + std::ofstream(boundary) << original; + REQUIRE(rift_s_world_anchor_reload(anchor.get())); + REQUIRE(rift_s_world_anchor_save(anchor.get())); + std::ifstream upgraded(path, std::ios::binary); + upgraded.seekg(4); + CHECK(upgraded.get() == 4); + std::ofstream(boundary) << rewritten; + REQUIRE(rift_s_world_anchor_reload(anchor.get())); + CHECK(rift_s_world_anchor_has_map(anchor.get())); + unlink(path.c_str()); + unlink(boundary.c_str()); + rmdir(directory); + } +} + +TEST_CASE("Private installed calibration loads its saved room map", "[.world-map-check]") +{ + const char *config_path = std::getenv("RIFT_S_WORLD_CHECK_CONFIG"); + const char *map_path = std::getenv("RIFT_S_WORLD_CHECK_MAP"); + REQUIRE(config_path); + REQUIRE(map_path); + rift_s_world_anchor_config config{}; + std::ifstream input(config_path, std::ios::binary); + REQUIRE(input.read(reinterpret_cast(&config), sizeof(config))); + Anchor anchor(rift_s_world_anchor_create(&config, map_path), rift_s_world_anchor_destroy); + REQUIRE(anchor); + CHECK(rift_s_world_anchor_has_boundary(anchor.get())); + CHECK(rift_s_world_anchor_has_map(anchor.get())); +} + +// Real camera replay stays opt-in. Recordings contain private room images and never enter the repo. +TEST_CASE("Cross-session camera map replay", "[.camera-replay]") +{ + const char *session_a = std::getenv("RIFT_S_WORLD_REPLAY_A"), *session_b = std::getenv("RIFT_S_WORLD_REPLAY_B"); + REQUIRE(session_a); + REQUIRE(session_b); + rift_s_world_anchor_config c{}; + std::ifstream config(std::string(session_a) + "/calibration.bin", std::ios::binary); + config.seekg(0, std::ios::end); + const auto bytes = config.tellg(); + config.seekg(0); + struct LegacyConfig + { + rift_s_camera_calibration camera[2]; + xrt_pose imu_from_camera[2]; + bool require_boundary; + char boundary_path[1024]; + }; + if (bytes == std::streampos(sizeof(LegacyConfig))) { + LegacyConfig old{}; + REQUIRE(config.read(reinterpret_cast(&old), sizeof(old))); + std::copy_n(old.camera, 2, c.camera); + std::copy_n(old.imu_from_camera, 2, c.imu_from_camera); + c.camera_count = 2; + } else { + REQUIRE(bytes == std::streampos(sizeof(c))); + REQUIRE(config.read(reinterpret_cast(&c), sizeof(c))); + } + const unsigned camera_count = c.camera_count ? c.camera_count : 2; + const char *boundary = std::getenv("RIFT_S_WORLD_REPLAY_BOUNDARY"); + c.require_boundary = boundary != nullptr; + c.boundary_path[0] = 0; + if (boundary) { + REQUIRE(std::strlen(boundary) < sizeof(c.boundary_path)); + std::snprintf(c.boundary_path, sizeof(c.boundary_path), "%s", boundary); + } + const char *override_path = std::getenv("RIFT_S_WORLD_REPLAY_MAP"); + std::string path = override_path ? override_path : std::string(session_a) + "/replay-map.bin"; + bool load_only = std::getenv("RIFT_S_WORLD_REPLAY_LOAD_MAP") != nullptr; + // A caller-selected path is never overwritten unless explicitly requested. + if (!load_only) { + REQUIRE((!override_path || access(path.c_str(), F_OK) != 0 || + std::getenv("RIFT_S_WORLD_REPLAY_RESET_MAP"))); + unlink(path.c_str()); + } + Anchor map(rift_s_world_anchor_create(&c, path.c_str()), rift_s_world_anchor_destroy); + REQUIRE(map); + auto read_frame = [](const std::string &dir, unsigned index, unsigned eye, int64_t ts, + std::vector &storage, xrt_frame &frame) { + char name[64]; + std::snprintf(name, sizeof(name), "/%04u-%u.pgm", index, eye); + std::ifstream file(dir + name, std::ios::binary); + std::string magic; + unsigned width, height, maximum; + REQUIRE(file >> magic >> width >> height >> maximum); + REQUIRE(magic == "P5"); + REQUIRE(maximum == 255); + REQUIRE(width <= 2048); + REQUIRE(height <= 2048); + file.get(); + storage.resize(width * height); + REQUIRE(file.read(reinterpret_cast(storage.data()), storage.size())); + frame = {}; + frame.width = width; + frame.height = height; + frame.stride = width; + frame.size = storage.size(); + frame.data = storage.data(); + frame.format = XRT_FORMAT_L8; + frame.timestamp = ts; + }; + unsigned keyframes = 0, matched = 0, attempted = 0, confirmations = 0; + int64_t start_ns = 0, confirmed_ns = 0, previous_ns = 0; + xrt_pose previous = XRT_POSE_IDENTITY, confirmed = XRT_POSE_IDENTITY, confirmed_head = XRT_POSE_IDENTITY; + double max_error = 0, max_vote_translation = 0, max_vote_angle = 0; + for (const auto &[dir, record] : + {std::pair{std::string(session_a), true}, std::pair{std::string(session_b), false}}) { + if (record && load_only) { + REQUIRE(rift_s_world_anchor_has_map(map.get())); + continue; + } + std::ifstream poses(dir + "/poses.csv"); + REQUIRE(poses.good()); + std::string line; + if (!record) { + if (!load_only) + REQUIRE(rift_s_world_anchor_save(map.get())); + if (std::getenv("RIFT_S_WORLD_REPLAY_RECORD_ONLY")) { + REQUIRE(keyframes > 0); + std::printf("Recorded map keyframes=%u\n", keyframes); + return; + } + map.reset(); + map = Anchor(rift_s_world_anchor_create(&c, path.c_str()), rift_s_world_anchor_destroy); + REQUIRE(rift_s_world_anchor_has_map(map.get())); + } + while (std::getline(poses, line)) { + unsigned index; + long long ts; + int valid; + xrt_pose raw; + REQUIRE(std::sscanf(line.c_str(), "%u,%lld,%d,%f,%f,%f,%f,%f,%f,%f", &index, &ts, &valid, + &raw.orientation.x, &raw.orientation.y, &raw.orientation.z, + &raw.orientation.w, &raw.position.x, &raw.position.y, + &raw.position.z) == 10); + if (!record && index >= 100 && std::getenv("RIFT_S_WORLD_REPLAY_SHORT")) + break; + if (!record && !start_ns) + start_ns = ts; + if (record && !valid) + continue; + std::vector storage[RIFT_S_WORLD_MAX_CAMERAS]; + xrt_frame frames[RIFT_S_WORLD_MAX_CAMERAS]; + const xrt_frame *ptr[RIFT_S_WORLD_MAX_CAMERAS]{}; + for (unsigned eye = 0; eye < camera_count; ++eye) { + read_frame(dir, index, eye, ts, storage[eye], frames[eye]); + ptr[eye] = &frames[eye]; + } + rift_s_world_anchor_result result{}; + bool processed = rift_s_world_anchor_process_rig_gravity( + map.get(), ptr, camera_count, record ? &raw : nullptr, + valid ? &raw.orientation : nullptr, &result); + if (record) { + keyframes = std::max(keyframes, result.keyframes); + if (index % 12 == 0) + std::printf( + "Record index=%u features=%u/%u/%u/%u/%u points_by_camera=%u/%u/%u/%u/%u " + "stereo=%u points=%u keyframes=%u\n", + index, result.features[0], result.features[1], result.features[2], + result.features[3], result.features[4], result.camera_points[0], + result.camera_points[1], result.camera_points[2], result.camera_points[3], + result.camera_points[4], result.stereo_matches, result.triangulated, + result.keyframes); + continue; + } + attempted++; + if (index % 12 == 0) + std::printf( + "Attempt index=%u features=%u/%u/%u/%u/%u matches=%u/%u/%u/%u/%u " + "inliers=%u/%u/%u/%u/%u support=%u error_px=%.3f " + "matched=%d rejection=%s\n", + index, result.features[0], result.features[1], result.features[2], + result.features[3], result.features[4], result.matches[0], result.matches[1], + result.matches[2], result.matches[3], result.matches[4], result.inliers[0], + result.inliers[1], result.inliers[2], result.inliers[3], result.inliers[4], + result.supporting_cameras, result.max_reprojection_px, result.relocalized, + result.rejection ? result.rejection : "none"); + if (!processed || !result.relocalized || !valid) { + if (!valid || ts - previous_ns > 3000000000LL) + confirmations = 0; + continue; + } + matched++; + xrt_pose inverse, candidate; + math_pose_invert(&raw, &inverse); + math_pose_transform(&result.world_from_imu, &inverse, &candidate); + xrt_vec3 up{0, 1, 0}, world_up; + math_quat_rotate_vec3(&candidate.orientation, &up, &world_up); + if (world_up.y <= 0.9961947f) { + confirmations = 0; + continue; + } + rift_s_slam_guard_yaw_of(&candidate.orientation, &candidate.orientation); + xrt_vec3 rotated; + math_quat_rotate_vec3(&candidate.orientation, &raw.position, &rotated); + candidate.position = {result.world_from_imu.position.x - rotated.x, + result.world_from_imu.position.y - rotated.y, + result.world_from_imu.position.z - rotated.z}; + double distance = cv::norm(translation(candidate) - translation(previous)); + cv::Vec3d rv; + cv::Rodrigues(rotation(candidate.orientation).t() * rotation(previous.orientation), rv); + double angle = cv::norm(rv); + bool agrees = confirmations && ts > previous_ns && ts - previous_ns <= 3000000000LL && + distance < 0.04 && angle < 3 * M_PI / 180; + if (agrees) { + max_vote_translation = std::max(max_vote_translation, distance); + max_vote_angle = std::max(max_vote_angle, angle); + } + confirmations = agrees ? confirmations + 1 : 1; + previous = candidate; + previous_ns = ts; + max_error = std::max(max_error, double(result.max_reprojection_px)); + if (confirmations >= 3 && !confirmed_ns) { + confirmed_ns = ts; + confirmed = candidate; + confirmed_head = result.world_from_imu; + } + std::printf( + "Replay time_s=%.3f inliers=%u/%u error_px=%.3f votes=%u transform=%.4f,%.4f,%.4f\n", + (ts - start_ns) * 1e-9, result.inliers[0], result.inliers[1], result.max_reprojection_px, + confirmations, candidate.position.x, candidate.position.y, candidate.position.z); + } + } + std::printf( + "Replay summary keyframes=%u attempts=%u matched=%u confirmed_s=%.3f max_error_px=%.3f" + " vote_translation_m=%.6f vote_angle_deg=%.6f transform=%.4f,%.4f,%.4f" + " head_position=%.6f,%.6f,%.6f head_orientation=%.6f,%.6f,%.6f,%.6f\n", + keyframes, attempted, matched, confirmed_ns ? (confirmed_ns - start_ns) * 1e-9 : -1.0, max_error, + max_vote_translation, max_vote_angle * 180 / M_PI, confirmed.position.x, confirmed.position.y, + confirmed.position.z, confirmed_head.position.x, confirmed_head.position.y, confirmed_head.position.z, + confirmed_head.orientation.x, confirmed_head.orientation.y, confirmed_head.orientation.z, + confirmed_head.orientation.w); + if (!load_only) + REQUIRE(keyframes > 0); + REQUIRE(confirmed_ns > 0); +} + +TEST_CASE("Native fisheye recovery tolerates subpixel peripheral feature noise") +{ + auto c = calibration(); + xrt_pose expected{{0, 0.24740396f, 0, 0.96891242f}, {0.2f, 0.1f, -0.45f}}; + auto noisy = observations(c, expected); + for (unsigned eye = 0; eye < 2; ++eye) { + for (unsigned i = 0; i < noisy[eye].size(); ++i) { + // Corner localization noise is measured in native pixels, not the nonlinear + // perspective plane used internally by OpenCV's PnP implementation. + noisy[eye][i].pixel.x += float(0.6 * std::sin(i * 1.7)); + noisy[eye][i].pixel.y += float(0.6 * std::cos(i * 2.3)); + } + } + rift_s_world_anchor_result result{}; + REQUIRE(solve(c, noisy, result)); + CHECK(cv::norm(translation(result.world_from_imu) - translation(expected)) < 0.025); + CHECK(result.inliers[0] >= 60); + CHECK(result.inliers[1] >= 60); +} + +TEST_CASE("Native reprojection drops a few wrong correspondences without losing the room") +{ + auto c = calibration(); + xrt_pose expected{{0, 0.24740396f, 0, 0.96891242f}, {0.2f, 0.1f, -0.45f}}; + auto obs = observations(c, expected); + for (auto &eye : obs) + for (unsigned i = 0; i < eye.size(); i += 13) + eye[i].pixel.x += 4; + rift_s_world_anchor_result result{}; + REQUIRE(solve(c, obs, result)); + CHECK(result.max_reprojection_px <= 2.5f); + CHECK(cv::norm(translation(result.world_from_imu) - translation(expected)) < 0.025); + CHECK(result.inliers[0] >= 60); + CHECK(result.inliers[1] >= 60); +} + +TEST_CASE("Stereo map rejects unrelated camera images") +{ + auto c = calibration(false); + Images images(c); + cv::RNG random(251); + random.fill(images.images[1], cv::RNG::UNIFORM, 0, 256); + Anchor anchor(rift_s_world_anchor_create(&c, nullptr), rift_s_world_anchor_destroy); + xrt_pose trusted = XRT_POSE_IDENTITY; + rift_s_world_anchor_result result{}; + CHECK_FALSE(rift_s_world_anchor_process(anchor.get(), &images.frames[0], &images.frames[1], &trusted, &result)); + CHECK_FALSE(rift_s_world_anchor_has_map(anchor.get())); +} + +TEST_CASE("Five camera recovery accepts side support and rejects conflicting room clusters") +{ + auto c = calibration(false); + c.camera_count = 5; + for (unsigned e = 2; e < 5; ++e) { + c.camera[e] = c.camera[0]; + c.imu_from_camera[e] = XRT_POSE_IDENTITY; + } + c.imu_from_camera[2] = {{0, -0.70710678f, 0, 0.70710678f}, {-0.07f, 0, 0}}; + c.imu_from_camera[3] = {{0, 0.70710678f, 0, 0.70710678f}, {0.07f, 0, 0}}; + c.imu_from_camera[4] = {{-0.70710678f, 0, 0, 0.70710678f}, {0, -0.04f, 0}}; + xrt_pose expected{{0, 0.24740396f, 0, 0.96891242f}, {1.2f, 1.3f, -0.45f}}; + std::vector o[5]; + const rift_s_world_anchor_observation *ptr[5]{}; + uint32_t n[5]{}; + for (unsigned e = 0; e < 5; ++e) { + for (int y = 50; y <= 430; y += 45) + for (int x = 55; x <= 590; x += 55) { + xrt_vec2 pixel{float(x), float(y)}; + xrt_vec3 ray; + REQUIRE(rift_s_world_anchor_unproject(&c.camera[e], &pixel, &ray)); + double depth = 1.2 + 0.15 * ((x / 55 + 2 * y / 45) % 9); + auto imu = rotation(c.imu_from_camera[e].orientation) * + (cv::Vec3d{ray.x, ray.y, ray.z} * depth) + + translation(c.imu_from_camera[e]); + auto world = rotation(expected.orientation) * imu + translation(expected); + o[e].push_back({{float(world[0]), float(world[1]), float(world[2])}, pixel}); + } + ptr[e] = o[e].data(); + n[e] = o[e].size(); + } + rift_s_world_anchor_result r{}; + SECTION("Side cameras recover with front and top cameras unavailable") + { + n[0] = n[1] = n[4] = 0; + REQUIRE(rift_s_world_anchor_solve_rig(&c, ptr, n, 5, &r)); + CHECK(r.supporting_cameras == 2); + CHECK(cv::norm(translation(expected) - translation(r.world_from_imu)) < 0.00005); + } + SECTION("One unrelated confident camera cannot overturn four agreeing cameras") + { + for (auto &obs : o[4]) + obs.world_point.x += 0.4f; + REQUIRE(rift_s_world_anchor_solve_rig(&c, ptr, n, 5, &r)); + CHECK(r.supporting_cameras == 4); + CHECK(cv::norm(translation(expected) - translation(r.world_from_imu)) < 0.00005); + } + SECTION("Two conflicting confident clusters cannot confirm a boundary") + { + for (unsigned e : {2u, 3u}) + for (auto &obs : o[e]) + obs.world_point.x += 0.4f; + CHECK_FALSE(rift_s_world_anchor_solve_rig(&c, ptr, n, 5, &r)); + CHECK(std::string(r.rejection) == "camera_disagreement"); + } +} + +TEST_CASE("Temporal room mapping supplies landmarks without camera baseline") +{ + auto c = calibration(false); + c.camera_count = 5; + for (unsigned e = 0; e < 5; ++e) { + c.camera[e] = c.camera[0]; + c.imu_from_camera[e] = XRT_POSE_IDENTITY; + } + Images initial(c), moved(c, 0.12f, 0.15f); + Anchor anchor(rift_s_world_anchor_create(&c, "/tmp/rift-s-temporal-unsaved.bin"), rift_s_world_anchor_destroy); + REQUIRE(anchor); + const xrt_frame *ptr[5]; + for (unsigned e = 0; e < 5; ++e) + ptr[e] = &initial.frames[e]; + xrt_pose trusted = XRT_POSE_IDENTITY; + rift_s_world_anchor_result r{}; + CHECK_FALSE(rift_s_world_anchor_process_rig(anchor.get(), ptr, 5, &trusted, &r)); + trusted.position.x = 0.12f; + trusted.orientation = {0, float(std::sin(0.075)), 0, float(std::cos(0.075))}; + for (unsigned e = 0; e < 5; ++e) { + moved.frames[e].timestamp += 300000000LL; + ptr[e] = &moved.frames[e]; + } + REQUIRE(rift_s_world_anchor_process_rig(anchor.get(), ptr, 5, &trusted, &r)); + CHECK(r.recorded); + for (unsigned e = 0; e < 5; ++e) + CHECK(r.camera_points[e] >= 40); + REQUIRE(rift_s_world_anchor_process_rig(anchor.get(), ptr, 5, nullptr, &r)); + CHECK(r.supporting_cameras == 5); + CHECK(cv::norm(translation(trusted) - translation(r.world_from_imu)) < 0.03); +} + +TEST_CASE("Room recovery solves yaw and translation with exposure gravity") +{ + auto c = calibration(); + xrt_quat tilt{0.14943813f, 0, 0, 0.98877108f}; + xrt_quat yaw{0, 0.24740396f, 0, 0.96891242f}; + xrt_pose expected{tilt, {0.4f, -0.2f, 0.6f}}; + math_quat_rotate(&yaw, &tilt, &expected.orientation); + auto o = observations(c, expected); + for (unsigned eye = 0; eye < 2; ++eye) + for (size_t i = 0; i < o[eye].size(); ++i) { + if (i % 11 == 0) + o[eye][i].pixel.x += 60; + else { + o[eye][i].pixel.x += float(0.25 * std::sin(double(i))); + o[eye][i].pixel.y += float(0.25 * std::cos(double(i))); + } + } + const rift_s_world_anchor_observation *obs[2] = {o[0].data(), o[1].data()}; + uint32_t counts[2] = {uint32_t(o[0].size()), uint32_t(o[1].size())}; + rift_s_world_anchor_result result{}; + REQUIRE(rift_s_world_anchor_solve_rig_gravity(&c, obs, counts, 2, &tilt, &result)); + CHECK(cv::norm(translation(result.world_from_imu) - translation(expected)) < 0.01); + xrt_quat inverse, alignment; + math_quat_invert(&tilt, &inverse); + math_quat_rotate(&result.world_from_imu.orientation, &inverse, &alignment); + CHECK(std::abs(alignment.x) < 1e-5); + CHECK(std::abs(alignment.z) < 1e-5); + CHECK(std::abs(std::abs(alignment.y) - yaw.y) < 0.002); + SECTION("Invalid gravity cannot produce a localized pose") { + tilt.x = std::numeric_limits::quiet_NaN(); + CHECK_FALSE(rift_s_world_anchor_solve_rig_gravity(&c, obs, counts, 2, &tilt, &result)); + CHECK_FALSE(result.relocalized); + } +} + +TEST_CASE("Automatic room updates reject VIO drift in an overlapping view") +{ + auto c = calibration(false); + Images images(c); + Anchor map(rift_s_world_anchor_create(&c, nullptr), rift_s_world_anchor_destroy); + REQUIRE(map); + const xrt_frame *frames[2] = {&images.frames[0], &images.frames[1]}; + xrt_pose trusted = XRT_POSE_IDENTITY; + rift_s_world_anchor_result result{}; + REQUIRE(rift_s_world_anchor_process_rig(map.get(), frames, 2, &trusted, &result)); + // The camera stayed still; a plausible but drifting VIO pose must not poison its map. + trusted.position.x = 0.3f; + for (unsigned eye = 0; eye < 2; ++eye) + images.frames[eye].timestamp += 3000000000LL; + CHECK_FALSE(rift_s_world_anchor_process_rig_gravity(map.get(), frames, 2, &trusted, + &trusted.orientation, &result)); + CHECK(std::string(result.rejection) == "map_update_disagreement"); + CHECK(result.keyframes == 1); + REQUIRE(rift_s_world_anchor_process_rig_gravity(map.get(), frames, 2, nullptr, + &trusted.orientation, &result)); + CHECK(cv::norm(translation(result.world_from_imu)) < 0.03); +} diff --git a/tests/tests_slam_recovery.cpp b/tests/tests_slam_recovery.cpp new file mode 100644 index 000000000..ca2e40f28 --- /dev/null +++ b/tests/tests_slam_recovery.cpp @@ -0,0 +1,222 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +// Actual-backend motion, queue-stall recovery and latency check. No hardware required. +#include "tracking/t_tracking.h" +#include "xrt/xrt_tracking.h" +#include "xrt/xrt_frame.h" +#include "os/os_time.h" +#include "util/u_frame.h" +#include +#include +#include +#include +#include +#include +#include + +struct Motion +{ + double x = 0, yaw = 0, ax = 0, wy = 0; +}; + +static Motion +motion(double t) +{ + if (t <= 1) + return {}; + t -= 1; + static const double yaw_amplitude = std::getenv("SLAM_TEST_FAST_MOTION") ? 0.65 : 0.25; + return {0.10 * (1 - std::cos(3 * t)), yaw_amplitude * (1 - std::cos(5 * t)), 0.9 * std::cos(3 * t), + 5 * yaw_amplitude * std::sin(5 * t)}; +} + +// Textured points at distinct depths. KB4 projection matches the supplied cameras. +static void +render(xrt_frame *frame, double baseline, double t) +{ + std::memset(frame->data, 20, frame->size); + const auto m = motion(t); + const double c = std::cos(m.yaw), s = std::sin(m.yaw); + uint32_t rng = 42; + auto random = [&] { + rng = rng * 1664525u + 1013904223u; + return double(rng >> 8) / 16777216; + }; + for (int i = 0; i < 1200; ++i) { + double x = (random() - .5) * 7 - m.x; + double y = (random() - .5) * 5; + double z = 1.5 + random() * 3; + double cx = c * x - s * z - baseline; + double cz = s * x + c * z; + std::array texture; + for (auto &v : texture) + v = random() > .5 ? 220 : 50; + if (cz < .5) + continue; + double r = std::hypot(cx, y), theta = std::atan2(r, cz); + double u = 320 + 200 * theta * cx / r, v = 240 + 200 * theta * y / r; + int ix = int(std::floor(u)), iy = int(std::floor(v)); + if (ix < 5 || ix >= 635 || iy < 5 || iy >= 475) + continue; + // Bilinear translation keeps feature motion continuous across pixel boundaries. + for (int py = -3; py <= 3; ++py) + for (int px = -3; px <= 3; ++px) { + double a = u - ix, b = v - iy; + auto tex = [&](int dx, int dy) { + return texture[std::clamp(dy, 0, 6) * 7 + std::clamp(dx, 0, 6)]; + }; + double value = (1 - a) * (1 - b) * tex(px + 3, py + 3) + + a * (1 - b) * tex(px + 2, py + 3) + (1 - a) * b * tex(px + 3, py + 2) + + a * b * tex(px + 2, py + 2); + frame->data[(iy + py) * frame->stride + ix + px] = uint8_t(value); + } + } +} + +int +main() +{ + const char *library = std::getenv("VIT_SYSTEM_LIBRARY_PATH"); + if (!library) + return 77; + t_slam_calibration calibration{}; + const char *camera_count = std::getenv("SLAM_TEST_CAM_COUNT"); + const int cameras = camera_count && std::strcmp(camera_count, "5") == 0 ? 5 : 2; + calibration.cam_count = cameras; + calibration.imu.frequency = 200; + for (auto *c : {&calibration.imu.base.accel, &calibration.imu.base.gyro}) + for (int i = 0; i < 3; ++i) { + c->transform[i][i] = 1; + c->bias_std[i] = .001; + c->noise_std[i] = .01; + } + for (int i = 0; i < cameras; ++i) { + auto &c = calibration.cams[i]; + c.frequency = 30; + c.base.image_size_pixels = {640, 480}; + c.base.distortion_model = T_DISTORTION_FISHEYE_KB4; + c.base.intrinsics[0][0] = c.base.intrinsics[1][1] = 200; + c.base.intrinsics[0][2] = 320; + c.base.intrinsics[1][2] = 240; + c.base.intrinsics[2][2] = 1; + for (int k = 0; k < 4; ++k) + c.T_imu_cam.v[k * 5] = 1; + c.T_imu_cam.v[12] = cameras == 2 ? (i ? .05 : -.05) : (i - 2) * .05; + } + t_slam_tracker_config config{}; + t_slam_fill_default_config(&config); + config.vit_system_library_path = library; + config.cam_count = cameras; + config.submit_from_start = true; + config.slam_calib = &calibration; + config.slam_ui = false; + const char *directory = std::getenv("SLAM_TEST_BENCH_DIR"); + config.write_csvs = directory != nullptr; + if (directory) + config.csv_path = directory; + config.timing_stat = true; + config.features_stat = true; + xrt_frame_context context{}; + xrt_tracked_slam *tracker = nullptr; + xrt_slam_sinks *sinks = nullptr; + if (t_slam_create(&context, &config, &tracker, &sinks) || t_slam_start(tracker)) + return 1; + struct Cleanup + { + xrt_frame_context &context; + ~Cleanup() + { + xrt_frame_context_destroy_nodes(&context); + } + } cleanup{context}; + std::vector frames(cameras); + for (auto &frame : frames) + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &frame); + struct FrameCleanup + { + std::vector &frames; + ~FrameCleanup() + { + for (auto &f : frames) + xrt_frame_reference(&f, nullptr); + } + } frame_cleanup{frames}; + auto stereo = [&](int64_t ts, double t) { + for (int eye = 0; eye < cameras; ++eye) { + render(frames[eye], calibration.cams[eye].T_imu_cam.v[12], t); + frames[eye]->timestamp = ts; + xrt_sink_push_frame(sinks->cams[eye], frames[eye]); + } + }; + int64_t last_sample = 0; + std::vector lag; + auto stream = [&](double seconds) { + const int64_t begin = os_monotonic_get_ns(); + int64_t imu_ts = begin; + int count = 0; + for (int i = 0; i < int(seconds * 30); ++i) { + const int64_t ts = begin + i * 33333333LL; + const int64_t delay = ts - int64_t(os_monotonic_get_ns()); + if (delay > 0) + os_nanosleep(delay); + while (imu_ts <= ts + 10000000) { + auto m = motion(double(imu_ts - begin) / 1e9); + xrt_imu_sample imu{}; + imu.timestamp_ns = imu_ts; + imu.accel_m_s2 = {std::cos(m.yaw) * m.ax, 9.81, std::sin(m.yaw) * m.ax}; + imu.gyro_rad_secs.y = m.wy; + xrt_sink_push_imu(sinks->imu, &imu); + imu_ts += 5000000; + } + stereo(ts, double(i) / 30); + int64_t sample = 0; + xrt_space_relation relation{}, query{}; + if (t_slam_get_latest_sample(tracker, os_monotonic_get_ns(), &sample, &relation, &query) && + sample != last_sample && std::isfinite(relation.pose.position.x) && + std::isfinite(relation.pose.position.y) && std::isfinite(relation.pose.position.z)) { + last_sample = sample; + if (sample >= begin) { + ++count; + lag.push_back(double(int64_t(os_monotonic_get_ns()) - sample) / 1e6); + } + } + } + return count; + }; + if (stream(6) < 90) { + std::fputs("Motion stream did not stay live\n", stderr); + return 2; + } + std::sort(lag.begin(), lag.end()); + std::printf("MOTION samples=%zu lag_median_ms=%.3f lag_p95_ms=%.3f lag_max_ms=%.3f\n", lag.size(), + lag[lag.size() / 2], lag[lag.size() * 95 / 100], lag.back()); + if (directory) + return 0; // Benchmark mode measures the intact pipeline only. + + // A frame waiting for an unreachable future IMU wedges the frontend. Estimator-only + // reset cannot clear it; this models the persistent stale pipeline after a sensor gap. + stereo(os_monotonic_get_ns() + 3600000000000LL, 0); + os_nanosleep(100000000); + if (t_slam_reset(tracker)) + return 3; + last_sample = 0; + const int stuck = stream(3); + std::printf("STUCK estimator_reset_fresh_samples=%d\n", stuck); + if (stuck > 1) + return 4; + const int64_t restart_begin = os_monotonic_get_ns(); + // Deliberately use the default runtime policy, without SLAM_BACKEND_RECREATE=true. + if (tracker->restart(tracker)) { + std::fputs("Default policy leaves backend stuck\n", stderr); + return 5; + } + const double restart_ms = double(os_monotonic_get_ns() - restart_begin) / 1e6; + last_sample = 0; + lag.clear(); + const int recovered = stream(3); + std::printf("RECOVERY restart_ms=%.3f fresh_samples=%d first_pose_lag_ms=%.3f\n", restart_ms, recovered, + lag.empty() ? -1 : lag.front()); + if (restart_ms > 2000 || recovered < 45) + return 6; + return 0; +} diff --git a/tests/tests_slam_restart.cpp b/tests/tests_slam_restart.cpp new file mode 100644 index 000000000..7d2c8f4f9 --- /dev/null +++ b/tests/tests_slam_restart.cpp @@ -0,0 +1,187 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +// Hardware-free lifecycle check of the actual VIT backend through Monado. +#include "tracking/t_tracking.h" +#include "xrt/xrt_tracking.h" +#include "xrt/xrt_frame.h" +#include "os/os_time.h" +#include "util/u_frame.h" +#include +#include +#include +#include +#include +#ifdef SLAM_TEST_HAVE_TBB +#include +#endif +int +main() +{ + if (!std::getenv("VIT_SYSTEM_LIBRARY_PATH")) { + puts("Set VIT_SYSTEM_LIBRARY_PATH to run the backend lifecycle check"); + return 77; + } + t_slam_calibration calibration{}; + calibration.cam_count = 2; + // Use the default Basalt pipeline IMU rate for this lifecycle-only check. + calibration.imu.frequency = 200; + for (auto *c : {&calibration.imu.base.accel, &calibration.imu.base.gyro}) { + for (int i = 0; i < 3; ++i) { + c->transform[i][i] = 1; + c->bias_std[i] = 0.001; + c->noise_std[i] = 0.01; + } + } + for (int i = 0; i < 2; ++i) { + auto &c = calibration.cams[i]; + c.frequency = 30; + c.base.image_size_pixels = {640, 480}; + c.base.distortion_model = T_DISTORTION_FISHEYE_KB4; + c.base.intrinsics[0][0] = c.base.intrinsics[1][1] = 200; + c.base.intrinsics[0][2] = 320; + c.base.intrinsics[1][2] = 240; + c.base.intrinsics[2][2] = 1; + for (int k = 0; k < 4; ++k) + c.T_imu_cam.v[k * 5] = 1; + c.T_imu_cam.v[12] = i ? 0.05 : -0.05; + } + // Exercise the production default. The backend must advertise safe recreation. + t_slam_tracker_config config{}; + t_slam_fill_default_config(&config); + config.vit_system_library_path = std::getenv("VIT_SYSTEM_LIBRARY_PATH"); + config.cam_count = 2; + config.submit_from_start = true; + config.slam_calib = &calibration; + config.slam_ui = false; + config.write_csvs = false; + config.features_stat = config.timing_stat = false; +#ifdef SLAM_TEST_HAVE_TBB + const auto previous_worker_limit = tbb::global_control::active_value(tbb::global_control::max_allowed_parallelism); + auto worker_limit_ok = [&] { + const char *expected = std::getenv("SLAM_TEST_WORKER_LIMIT"); + if (!expected) + return true; + const auto actual = tbb::global_control::active_value(tbb::global_control::max_allowed_parallelism); + if (actual == std::strtoul(expected, nullptr, 10)) + return true; + std::fprintf(stderr, "Expected Basalt TBB worker limit %s, got %zu\n", expected, actual); + return false; + }; +#else + if (std::getenv("SLAM_TEST_WORKER_LIMIT")) { + std::fputs("TBB is required to check the backend worker limit\n", stderr); + return 1; + } +#endif + xrt_frame_context context{}; + xrt_tracked_slam *tracker = nullptr; + xrt_slam_sinks *sinks = nullptr; + if (t_slam_create(&context, &config, &tracker, &sinks) != 0 || t_slam_start(tracker) != 0) + return 1; +#ifdef SLAM_TEST_HAVE_TBB + if (!worker_limit_ok()) { + xrt_frame_context_destroy_nodes(&context); + return 7; + } +#endif + std::atomic reading{true}; + std::thread reader([&] { + while (reading) { + int count; + timepoint_ns timestamp; + t_slam_get_feature_count(tracker, &count, ×tamp); + xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; + tracker->get_tracked_pose(tracker, os_monotonic_get_ns(), &relation); + os_nanosleep(1000000); + } + }); + // Join the reader on early return as well as successful teardown. + struct ReaderGuard + { + std::atomic &reading; + std::thread &reader; + ~ReaderGuard() + { + reading = false; + if (reader.joinable()) + reader.join(); + } + } reader_guard{reading, reader}; + for (int i = 0; i < 3; ++i) { + xrt_space_relation relation = XRT_SPACE_RELATION_ZERO; + tracker->get_tracked_pose(tracker, os_monotonic_get_ns(), &relation); + if (relation.relation_flags != 0 || tracker->restart(tracker) != 0) + return 2; + } + auto push_stereo = [&](timepoint_ns timestamp) { + for (int eye = 0; eye < 2; ++eye) { + xrt_frame *frame = nullptr; + u_frame_create_one_off(XRT_FORMAT_L8, 640, 480, &frame); + frame->timestamp = timestamp; + for (unsigned y = 0; y < frame->height; ++y) + for (unsigned x = 0; x < frame->width; ++x) + frame->data[y * frame->stride + x] = + ((x + eye * 5) / 16 + y / 16) % 2 ? 220 : 30; + xrt_sink_push_frame(sinks->cams[eye], frame); + xrt_frame_reference(&frame, nullptr); + } + }; + + // Cameras without IMUs can fill flow/VIO queues; submissions and restart must stay live. + const timepoint_ns camera_begin = os_monotonic_get_ns(); + for (int i = 0; i < 32; ++i) + push_stereo(camera_begin + i * 33333333); + if (tracker->restart(tracker) != 0) + return 5; + + // An in-flight frame waiting for another IMU must survive the shutdown sentinel. + xrt_imu_sample sparse_imu{}; + sparse_imu.timestamp_ns = os_monotonic_get_ns(); + sparse_imu.accel_m_s2.z = 9.81; + xrt_sink_push_imu(sinks->imu, &sparse_imu); + push_stereo(sparse_imu.timestamp_ns + 20000000); + os_nanosleep(200000000); + if (tracker->restart(tracker) != 0) + return 6; + + // More than the backend's 300-sample capacity, with no images. + const timepoint_ns imu_begin = os_monotonic_get_ns() - 3000000000; + for (int i = 0; i < 4000; ++i) { + xrt_imu_sample imu{}; + imu.timestamp_ns = imu_begin + i * 5000000LL; + imu.accel_m_s2.z = 9.81; + xrt_sink_push_imu(sinks->imu, &imu); + } + std::fputs("No-image IMU submissions finished; restarting\n", stderr); + if (tracker->restart(tracker) != 0) + return 4; + + // Exercise teardown with real frontend work and a camera backlog, not just sentinels. + for (int delay_ms : {0, 10, 100}) { + const timepoint_ns begin = os_monotonic_get_ns() - 1000000000; + for (int i = 0; i < 160; ++i) { + xrt_imu_sample imu{}; + imu.timestamp_ns = begin + i * 5000000LL; + imu.accel_m_s2.z = 9.81; + xrt_sink_push_imu(sinks->imu, &imu); + } + for (int i = 0; i < 14; ++i) { + push_stereo(begin + 20000000 + i * 33333333); + } + os_nanosleep(delay_ms * 1000000); + if (tracker->restart(tracker) != 0) + return 3; + } +#ifdef SLAM_TEST_HAVE_TBB + if (!worker_limit_ok()) + return 7; +#endif + reading = false; + reader.join(); + xrt_frame_context_destroy_nodes(&context); +#ifdef SLAM_TEST_HAVE_TBB + if (tbb::global_control::active_value(tbb::global_control::max_allowed_parallelism) != previous_worker_limit) + return 8; +#endif + puts("Actual Basalt lifecycle: empty, saturated no-image IMU and queued stereo/IMU restarts passed"); +} diff --git a/tests/tests_steamvr_camera.cpp b/tests/tests_steamvr_camera.cpp new file mode 100644 index 000000000..5f2858d48 --- /dev/null +++ b/tests/tests_steamvr_camera.cpp @@ -0,0 +1,529 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "ovrd_camera.hpp" +#include "ovrd_roomsetup_frame.hpp" +#include "os/os_time.h" +#include "util/u_frame.h" +#include +#include +#include + +struct CameraFixture : xrt_camera +{ + xrt_frame *frame = nullptr; + std::mutex lock; + CameraFixture() : xrt_camera{} + { + view_width = view_height = 4; + focal_length = {2, 2}; + center = {1.5f, 1.5f}; + set_active = [](xrt_camera *, bool) {}; + get_frame = [](xrt_camera *base, xrt_frame **out) { + auto &c = *static_cast(base); + std::lock_guard guard(c.lock); + if (!c.frame) + return false; + xrt_frame_reference(out, c.frame); + return true; + }; + } + ~CameraFixture() + { + xrt_frame_reference(&frame, nullptr); + } + void + publish(uint8_t value) + { + std::lock_guard guard(lock); + xrt_frame *next = nullptr; + u_frame_create_one_off(XRT_FORMAT_R8G8B8, 8, 4, &next); + memset(next->data, value, next->size); + next->timestamp = os_monotonic_get_ns(); + xrt_frame_reference(&frame, next); + xrt_frame_reference(&next, nullptr); + } +}; + +static const vr::CameraVideoStreamFrame_t * +wait_frame(MonadoCamera &camera) +{ + for (int i = 0; i < 100; ++i) { + if (auto *f = camera.GetVideoStreamFrame()) + return f; + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + return nullptr; +} + +TEST_CASE("SteamVR camera buffers stay immutable until release and reject stale restart frames") +{ + CameraFixture provider; + xrt_tracking_origin origin{}; + origin.initial_offset = XRT_POSE_IDENTITY; + xrt_device device{}; + device.camera = &provider; + device.tracking_origin = &origin; + device.get_tracked_pose = [](xrt_device *, xrt_input_name, int64_t, xrt_space_relation *out) { + *out = XRT_SPACE_RELATION_ZERO; + out->pose = XRT_POSE_IDENTITY; + out->pose.position = {1, 2, 3}; + out->relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; + return XRT_SUCCESS; + }; + MonadoCamera camera(&device); + CHECK_FALSE(camera.SetCameraFrameBuffering(2, nullptr, 96)); + CHECK_FALSE(camera.SetCameraVideoStreamFormat(vr::CVS_FORMAT_NV12)); + REQUIRE(camera.StartVideoStream()); + provider.publish(70); + auto *held = wait_frame(camera); + REQUIRE(held); + CHECK(held->m_nFrameSequence == 0); + CHECK(held->m_nWidth == 8); + CHECK(held->m_nImageDataSize == 128); + CHECK(held->m_RawTrackedDevicePose.bPoseIsValid); + CHECK(held->m_RawTrackedDevicePose.mDeviceToAbsoluteTracking.m[1][3] == 2); + auto *data = reinterpret_cast(held->m_pImageData); + CHECK(data[0] == 70); + provider.publish(180); + auto *newer = wait_frame(camera); + REQUIRE(newer); + CHECK(newer != held); + CHECK(data[0] == 70); + CHECK(reinterpret_cast(newer->m_pImageData)[0] == 180); + camera.ReleaseVideoStreamFrame(newer); + camera.StopVideoStream(); + CHECK(data[0] == 70); + CHECK_FALSE(camera.StartVideoStream()); + camera.ReleaseVideoStreamFrame(held); + REQUIRE(camera.StartVideoStream()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + CHECK(camera.GetVideoStreamFrame() == nullptr); + provider.publish(210); + auto *restarted = wait_frame(camera); + REQUIRE(restarted); + CHECK(restarted->m_nFrameSequence == 0); + camera.ReleaseVideoStreamFrame(restarted); + camera.StopVideoStream(); +} + +#include +#include +#include + +TEST_CASE("MonadoCamera exports rectified frames to shared memory and tracks controller buttons") +{ + CameraFixture provider; + xrt_tracking_origin origin{}; + origin.initial_offset = XRT_POSE_IDENTITY; + xrt_device device{}; + device.camera = &provider; + device.tracking_origin = &origin; + device.get_tracked_pose = [](xrt_device *, xrt_input_name, int64_t, xrt_space_relation *out) { + *out = XRT_SPACE_RELATION_ZERO; + out->pose = XRT_POSE_IDENTITY; + out->relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; + return XRT_SUCCESS; + }; + + MonadoCamera camera(&device); + + int fd = open(RIFTS_PASSTHROUGH_SHM_PATH, O_RDWR); + REQUIRE(fd >= 0); + auto *shm = static_cast( + mmap(nullptr, sizeof(struct rifts_passthrough_shm), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0)); + REQUIRE(shm != MAP_FAILED); + + CHECK(shm->magic == RIFTS_PASSTHROUGH_MAGIC); + CHECK(shm->version == RIFTS_PASSTHROUGH_VERSION); + CHECK(shm->width == 8); + CHECK(shm->height == 4); + + uint64_t seq0 = shm->sequence; + provider.publish(100); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + CHECK(shm->sequence == seq0); + + shm->client_active = os_monotonic_get_ns() / U_TIME_1MS_IN_NS; + provider.publish(123); + for (int i = 0; i < 50 && (__atomic_load_n(&shm->sequence, __ATOMIC_ACQUIRE) <= seq0 || + (__atomic_load_n(&shm->sequence, __ATOMIC_ACQUIRE) & 1)); + ++i) + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + CHECK(shm->sequence > seq0); + CHECK(shm->pixels[0] == 123); + CHECK(shm->pixels[1] == 123); + CHECK(shm->pixels[2] == 123); + CHECK(shm->pixels[3] == 255); + + uint32_t count0 = shm->toggle_count; + MonadoCamera::NotifyButton(XRT_INPUT_TOUCH_SYSTEM_CLICK, true); + CHECK(shm->toggle_count == count0 + 1); + MonadoCamera::NotifyButton(XRT_INPUT_TOUCH_SYSTEM_CLICK, true); + CHECK(shm->toggle_count == count0 + 1); + MonadoCamera::NotifyButton(XRT_INPUT_TOUCH_SYSTEM_CLICK, false); + + MonadoCamera::NotifyButton(XRT_INPUT_TOUCH_B_CLICK, true); + CHECK(shm->toggle_count == count0 + 1); + MonadoCamera::NotifyButton(XRT_INPUT_TOUCH_Y_CLICK, true); + CHECK(shm->toggle_count == count0 + 1); + MonadoCamera::NotifyButton(XRT_INPUT_TOUCH_MENU_CLICK, true); + CHECK(shm->toggle_count == count0 + 1); + MonadoCamera::NotifyButton(XRT_INPUT_TOUCH_MENU_CLICK, false); + MonadoCamera::NotifyButton(XRT_INPUT_TOUCH_MENU_CLICK, true); + CHECK(shm->toggle_count == count0 + 2); + + shm->client_active = 0; + munmap(shm, sizeof(struct rifts_passthrough_shm)); + close(fd); +} + +#include +#include + +struct NativeQueueFixture : vr::IVRBlockQueue, vr::IVRPaths +{ + std::vector pixels; + std::map names; + std::map> values; + std::mutex mutex; + std::atomic released{0}; + std::atomic busy{false}; + bool destroyed = false; + uint32_t header_size = 0, block_count = 0; + std::string queue_path; + vr::EBlockQueueError + Create(uint64_t *out, const char *path, uint32_t size, uint32_t header, uint32_t count, uint32_t) override + { + queue_path = path; + header_size = header; + block_count = count; + pixels.resize(size); + *out = 100; + return vr::EBlockQueueError_BlockQueueError_None; + } + vr::EBlockQueueError + Connect(uint64_t *, const char *) override + { + return vr::EBlockQueueError_BlockQueueError_InvalidParam; + } + vr::EBlockQueueError + Destroy(uint64_t) override + { + destroyed = true; + return vr::EBlockQueueError_BlockQueueError_None; + } + vr::EBlockQueueError + AcquireWriteOnlyBlock(uint64_t, uint64_t *out, void **data) override + { + if (busy) + return vr::EBlockQueueError_BlockQueueError_BlockNotAvailable; + mutex.lock(); + *out = 101; + *data = pixels.data(); + return vr::EBlockQueueError_BlockQueueError_None; + } + vr::EBlockQueueError + ReleaseWriteOnlyBlock(uint64_t, uint64_t) override + { + mutex.unlock(); + ++released; + return vr::EBlockQueueError_BlockQueueError_None; + } + vr::EBlockQueueError + WaitAndAcquireReadOnlyBlock(uint64_t, uint64_t *, void **, vr::EBlockQueueReadType, uint32_t) override + { + return vr::EBlockQueueError_BlockQueueError_InvalidParam; + } + vr::EBlockQueueError + AcquireReadOnlyBlock(uint64_t, uint64_t *, void **, vr::EBlockQueueReadType) override + { + return vr::EBlockQueueError_BlockQueueError_InvalidParam; + } + vr::EBlockQueueError + ReleaseReadOnlyBlock(uint64_t, uint64_t) override + { + return vr::EBlockQueueError_BlockQueueError_InvalidParam; + } + vr::EBlockQueueError + QueueHasReader(uint64_t, bool *out) override + { + *out = true; + return vr::EBlockQueueError_BlockQueueError_None; + } + vr::ETrackedPropertyError + ReadPathBatch(uint64_t, vr::PathRead_t *, uint32_t) override + { + return vr::TrackedProp_InvalidOperation; + } + vr::ETrackedPropertyError + WritePathBatch(uint64_t, vr::PathWrite_t *writes, uint32_t count) override + { + for (uint32_t i = 0; i < count; ++i) { + auto *first = static_cast(writes[i].pvBuffer); + values[writes[i].ulPath] = {first, first + writes[i].unBufferSize}; + writes[i].eError = vr::TrackedProp_Success; + } + return vr::TrackedProp_Success; + } + vr::ETrackedPropertyError + StringToHandle(uint64_t *out, const char *path) override + { + *out = names.emplace(path, names.size() + 1).first->second; + return vr::TrackedProp_Success; + } + vr::ETrackedPropertyError + HandleToString(uint64_t, const char *, uint32_t, uint32_t *) override + { + return vr::TrackedProp_InvalidOperation; + } + template + T + value(const char *path) + { + T out{}; + auto &data = values.at(names.at(path)); + REQUIRE(data.size() == sizeof(T)); + memcpy(&out, data.data(), sizeof(out)); + return out; + } +}; + +TEST_CASE("Native camera queue publishes stable frames and recovers from a busy compositor") +{ + CameraFixture provider; + xrt_device device{}; + xrt_tracking_origin origin{}; + origin.initial_offset = XRT_POSE_IDENTITY; + device.camera = &provider; + device.tracking_origin = &origin; + device.get_tracked_pose = [](xrt_device *, xrt_input_name, int64_t, xrt_space_relation *out) { + *out = XRT_SPACE_RELATION_ZERO; + out->pose = XRT_POSE_IDENTITY; + return XRT_SUCCESS; + }; + NativeQueueFixture queue; + { + MonadoCamera camera(&device, &queue, &queue); + REQUIRE(camera.native_ready()); + CHECK(queue.queue_path == "/lighthouse/camera/raw_frames"); + CHECK(queue.header_size == 512); + CHECK(queue.block_count == 4); + CHECK(queue.value("/format") == vr::CVS_FORMAT_RGBX32); + CHECK(queue.value("/width") == 8); + CHECK(queue.value("/height") == 4); + queue.busy = true; + REQUIRE(camera.StartVideoStream()); + provider.publish(90); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + CHECK(queue.released == 0); + queue.busy = false; + provider.publish(145); + for (int i = 0; i < 100 && queue.released == 0; ++i) + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + REQUIRE(queue.released == 1); + { + std::lock_guard lock(queue.mutex); + CHECK(queue.pixels[0] == 145); + CHECK(queue.pixels[3] == 255); + CHECK(queue.value("/frame_size") == 128); + CHECK(queue.value("/server_time_ticks") > 0); + CHECK(queue.value("/frame_time_monotonic") > 0); + CHECK(queue.value("/delivery_rate") > 0); + } + camera.StopVideoStream(); + } + CHECK(queue.destroyed); +} + +TEST_CASE("2D camera projection rays use the same local region for both eye textures") +{ + CameraFixture provider; + xrt_device device{}; + device.camera = &provider; + MonadoCamera camera(&device); + for (uint32_t eye = 0; eye < 2; ++eye) { + for (int type = 0; type < vr::MAX_CAMERA_FRAME_TYPES; ++type) { + vr::HmdMatrix44_t projection{}; + REQUIRE(camera.GetCameraProjection(eye, static_cast(type), + 0.1f, 10.0f, &projection)); + // Camera-local center and a ray one pixel to the right. + float center = (1 - projection.m[0][2]) * 0.5f; + CHECK(center == Catch::Approx(provider.center.x / 8)); + float next = (projection.m[0][0] / provider.focal_length.x - projection.m[0][2] + 1) * 0.5f; + CHECK(next - center == Catch::Approx(1.0f / 8)); + float near_depth = (-0.1f * projection.m[2][2] + projection.m[2][3]) / 0.1f; + float far_depth = (-10.0f * projection.m[2][2] + projection.m[2][3]) / 10.0f; + CHECK(near_depth == Catch::Approx(0).margin(1e-6)); + CHECK(far_depth == Catch::Approx(1)); + } + } +} + +TEST_CASE("Room View exported stereo rays stay at the correct pixels through downward pitch and yaw") +{ + using namespace roomsetup; + CameraFixture provider; + provider.view_width = provider.view_height = 320; + provider.focal_length = {112.033206f, 112.033206f}; + provider.center = {159.5f, 159.5f}; + xrt_device device{}; + device.camera = &provider; + MonadoCamera camera(&device); + auto rotation = [](float pitch, float yaw) { + auto p = identity(), y = identity(); + p[5] = p[10] = std::cos(pitch); p[6] = -std::sin(pitch); p[9] = std::sin(pitch); + y[0] = y[10] = std::cos(yaw); y[2] = std::sin(yaw); y[8] = -std::sin(yaw); + return multiply(y, p); + }; + auto apply = [](const Matrix &m, const std::array &p) { + std::array out{}; + for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j) out[i] += m[4*i+j] * p[j]; + return out; + }; + float worst_pixel_error = 0; + for (float pitch_deg : {0.f, -45.f, -90.f}) for (float yaw_deg : {-40.f, 0.f, 40.f}) { + auto head = rotation(pitch_deg * float(M_PI) / 180, yaw_deg * float(M_PI) / 180); + auto capture = rotation((pitch_deg + 2) * float(M_PI) / 180, (yaw_deg - 4) * float(M_PI) / 180); + capture[3] = .2f; capture[7] = 1.7f; capture[11] = .1f; + for (uint32_t eye_index = 0; eye_index < 2; ++eye_index) { + vr::HmdMatrix44_t exported{}; + REQUIRE(camera.GetCameraProjection(eye_index, vr::VRTrackedCameraFrameType_Undistorted, + .05f, 15.f, &exported)); + Matrix projection{}; + memcpy(projection.data(), exported.m, sizeof(exported)); + for (float height : {.9f, 1.7f}) { + auto eye = head; eye[3] = eye_index ? .032f : -.032f; eye[7] = height; + auto view = inverse_rigid(eye), old = multiply(projection, inverse_rigid(capture)); + std::array uniform{}; + auto model = identity(); + memcpy(uniform.data(), model.data(), 64); memcpy(uniform.data()+16, view.data(), 64); + memcpy(uniform.data()+32, projection.data(), 64); memcpy(uniform.data()+48, old.data(), 64); + REQUIRE(correct_camera(uniform.data(), sizeof(uniform))); + Matrix fixed{}; memcpy(fixed.data(), uniform.data()+48, 64); + for (float x : {-.7f, 0.f, .7f}) for (float y : {-.7f, 0.f, .7f}) { + // The depth-free reference is P * R_capture^-1 * R_eye * eye_ray. + auto direction = apply(eye, {x, y, -1, 0}); + auto reference = apply(old, direction); + for (float depth : {.25f, 1.f, 15.f}) { + auto world = apply(eye, {x*depth, y*depth, -depth, 1}); + auto clip = apply(fixed, world); + // Packed X normalization and eye-half selection together yield 320 local pixels. + float error = std::max(std::abs(clip[0]/clip[3] - reference[0]/reference[3]) * 320, + std::abs(clip[1]/clip[3] - reference[1]/reference[3]) * 160); + worst_pixel_error = std::max(worst_pixel_error, error); + CHECK(error < .002f); + // Independently project the known point back into the current eye. + auto eye_point = apply(view, world); + CHECK(eye_point[0]/-eye_point[2] == Catch::Approx(x).margin(1e-5)); + CHECK(eye_point[1]/-eye_point[2] == Catch::Approx(y).margin(1e-5)); + } + } + } + } + } + printf("Room View pitch/yaw/stereo worst pixel error: %.9g\n", worst_pixel_error); +} + +struct BlockedCameraFixture : CameraFixture +{ + std::condition_variable cv; + bool entered = false, release = false, consumed = false; + BlockedCameraFixture() + { + get_frame = [](xrt_camera *base, xrt_frame **out) { + auto &c = *static_cast(base); + std::unique_lock guard(c.lock); + if (!c.frame) + return false; + if (!c.entered) { + c.entered = true; + c.cv.notify_all(); + c.cv.wait_for(guard, std::chrono::milliseconds(500), [&] { return c.release; }); + // A newly captured image from an old request still belongs to that generation. + c.frame->timestamp = os_monotonic_get_ns(); + } + xrt_frame_reference(out, c.frame); + xrt_frame_reference(&c.frame, nullptr); + c.consumed = true; + c.cv.notify_all(); + return true; + }; + } +}; + +TEST_CASE("SteamVR camera stop and restart discard an in-flight provider request") +{ + BlockedCameraFixture provider; + xrt_tracking_origin origin{}; + origin.initial_offset = XRT_POSE_IDENTITY; + xrt_device device{}; + device.camera = &provider; + device.tracking_origin = &origin; + device.get_tracked_pose = [](xrt_device *, xrt_input_name, int64_t, xrt_space_relation *out) { + *out = XRT_SPACE_RELATION_ZERO; + out->pose = XRT_POSE_IDENTITY; + return XRT_SUCCESS; + }; + struct Callback : vr::ICameraVideoSinkCallback + { + std::atomic count{0}; + void + OnCameraVideoSinkCallback() override + { + ++count; + } + } callback; + NativeQueueFixture queue; + MonadoCamera camera(&device, &queue, &queue); + camera.SetCameraVideoSinkCallback(&callback); + REQUIRE(camera.StartVideoStream()); + provider.publish(80); + { + std::unique_lock lock(provider.lock); + REQUIRE(provider.cv.wait_for(lock, std::chrono::milliseconds(200), [&] { return provider.entered; })); + } + camera.StopVideoStream(); + REQUIRE(camera.StartVideoStream()); + { + std::unique_lock lock(provider.lock); + provider.release = true; + provider.cv.notify_all(); + REQUIRE(provider.cv.wait_for(lock, std::chrono::milliseconds(200), [&] { return provider.consumed; })); + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + CHECK(queue.released == 0); + CHECK(callback.count == 0); + CHECK(camera.GetVideoStreamFrame() == nullptr); + provider.publish(190); + auto *frame = wait_frame(camera); + REQUIRE(frame); + CHECK(frame->m_nFrameSequence == 0); + CHECK(reinterpret_cast(frame->m_pImageData)[0] == 190); + camera.ReleaseVideoStreamFrame(frame); + camera.StopVideoStream(); + CHECK(queue.released == 1); + CHECK(callback.count == 1); + { + std::lock_guard lock(queue.mutex); + CHECK(queue.value("/frame_sequence") == 0); + } + camera.SetCameraVideoSinkCallback(nullptr); +} + +TEST_CASE("SteamVR camera projection rejects nonfinite clipping planes") +{ + CameraFixture provider; + xrt_device device{}; + device.camera = &provider; + MonadoCamera camera(&device); + vr::HmdMatrix44_t projection{}; + for (float invalid : {std::numeric_limits::quiet_NaN(), std::numeric_limits::infinity()}) { + CHECK_FALSE( + camera.GetCameraProjection(0, vr::VRTrackedCameraFrameType_Undistorted, invalid, 100, &projection)); + CHECK_FALSE( + camera.GetCameraProjection(0, vr::VRTrackedCameraFrameType_Undistorted, .1f, invalid, &projection)); + } + CHECK(camera.GetCameraProjection(0, vr::VRTrackedCameraFrameType_Undistorted, 1e30f, 2e30f, &projection)); + CHECK(std::isfinite(projection.m[2][3])); +} diff --git a/tests/tests_steamvr_roomsetup_guard.cpp b/tests/tests_steamvr_roomsetup_guard.cpp new file mode 100644 index 000000000..81df9c401 --- /dev/null +++ b/tests/tests_steamvr_roomsetup_guard.cpp @@ -0,0 +1,201 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#define main roomsetup_guard_entry +#include "../src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp" +#undef main + +namespace { +struct LogFixture +{ + fs::path path = fs::temp_directory_path() / ("monado-roomsetup-guard-test-" + std::to_string(getpid())); + OverflowLog log{path}; + void write(const std::string &text, bool append = false) + { + std::ofstream stream(path, append ? std::ios::app : std::ios::trunc); + stream << text; + REQUIRE(stream.good()); + } + ~LogFixture() { std::error_code error; fs::remove(path, error); } +}; +} + +TEST_CASE("Native setup guard ignores old failures and recognizes a new split assertion") +{ + LogFixture f; + f.write("Vulkan transfer ring buffer overflow; transfer size 99400!\n"); + f.log.begin(); + CHECK_FALSE(f.log.overflowed()); + f.write("ordinary texture upload\nVulkan transfer ring ", true); + CHECK_FALSE(f.log.overflowed()); + f.write("buffer overflow; transfer size 144760!\n", true); + CHECK(f.log.overflowed()); + CHECK_FALSE(f.log.overflowed()); + f.log.begin(); + CHECK_FALSE(f.log.overflowed()); +} + +TEST_CASE("Native setup guard handles truncated logs and bounds each read") +{ + LogFixture f; + f.write(std::string(70000, 'x')); + f.log.begin(); + f.write("ordinary restart\n"); + CHECK_FALSE(f.log.overflowed()); + f.write(std::string(70000, 'x') + "Vulkan transfer ring buffer overflow\n", true); + CHECK_FALSE(f.log.overflowed()); + CHECK(f.log.overflowed()); +} + +TEST_CASE("Saved floor converts from raw into setup without HMD height") +{ + vr::HmdMatrix34_t standing{{{0, 0, -1, .8f}, {0, 1, 0, -1.8f}, {1, 0, 0, .4f}}}; + vr::HmdMatrix34_t setup{{{1, 0, 0, -.2f}, {0, 1, 0, -.7f}, {0, 0, 1, .3f}}}; + REQUIRE(roomsetup::saved_floor(standing, setup)); + CHECK(*roomsetup::saved_floor(standing, setup) == Catch::Approx(1.1f)); + standing.m[1][3] += 1.6f; + setup.m[1][3] += 1.6f; + CHECK(*roomsetup::saved_floor(standing, setup) == Catch::Approx(1.1f)); +} + +TEST_CASE("Setup RGB projection keeps eye rays independent of surface depth") +{ + using namespace roomsetup; + auto projection = Matrix{1.2f, 0, .1f, 0, 0, 1.4f, -.2f, 0, 0, 0, -1.01f, -.1f, 0, 0, -1, 0}; + auto capture = identity(); + capture[3] = .2f; + capture[7] = 1.7f; + capture[11] = .1f; + auto old = multiply(projection, inverse_rigid(capture)); + for (float height : {1.7f, .9f, 1.7f}) { + for (float pitch : {0.f, -.9f}) { + auto eye = identity(); + eye[5] = eye[10] = std::cos(pitch); + eye[6] = -std::sin(pitch); + eye[9] = std::sin(pitch); + eye[3] = -.03f; + eye[7] = height; + auto view = inverse_rigid(eye); + std::array uniform{}; + auto model = identity(); + memcpy(uniform.data(), model.data(), 64); + memcpy(uniform.data() + 16, view.data(), 64); + memcpy(uniform.data() + 32, projection.data(), 64); + memcpy(uniform.data() + 48, old.data(), 64); + REQUIRE(correct_camera(uniform.data(), sizeof(uniform))); + // An eye-space ray (.15, -.1, -1) rotates into capture space. + float direction[] = {.15f, -.1f * std::cos(pitch) + std::sin(pitch), + -.1f * std::sin(pitch) - std::cos(pitch)}; + float expected_w = -direction[2]; + float expected_u = (1.2f * direction[0] + .1f * direction[2]) / expected_w; + float expected_v = (1.4f * direction[1] - .2f * direction[2]) / expected_w; + for (float depth : {.25f, 1.f, 15.f}) { + float point[4] = {eye[3] + depth * direction[0], eye[7] + depth * direction[1], + depth * direction[2], 1}; + float clip[4]{}; + for (int i = 0; i < 4; ++i) + for (int j = 0; j < 4; ++j) + clip[i] += uniform[48 + 4 * i + j] * point[j]; + CHECK(clip[0] / clip[3] == Catch::Approx(expected_u).margin(1e-5)); + CHECK(clip[1] / clip[3] == Catch::Approx(expected_v).margin(1e-5)); + CHECK(clip[2] == Catch::Approx(-1.01f * depth * direction[2] - .1f).margin(1e-5)); + } + CHECK_FALSE(correct_camera(uniform.data(), sizeof(uniform) - 4)); + uniform[0] = 2; + CHECK_FALSE(correct_camera(uniform.data(), sizeof(uniform))); + } + } +} + +TEST_CASE("Saved floor sends a numeric detection and only seeds manual entry once") +{ + struct Messages : roomsetup::Mailbox + { + std::vector sent; + int + Register(const char *, uint64_t *handle) override + { + *handle = 1; + return 0; + } + int + Unregister(uint64_t) override + { + return 0; + } + int + Send(uint64_t, const char *destination, const char *json) override + { + CHECK(std::string(destination) == "vrcompositor_systemlayer"); + sent.emplace_back(json); + return 0; + } + int + Read(uint64_t, char *, uint32_t, uint32_t *) override + { + return 0; + } + } messages; + struct Properties : roomsetup::Paths + { + int + Read(uint64_t, roomsetup::PathRead *batch, uint32_t count) override + { + REQUIRE(count == 2); + vr::HmdMatrix34_t standing{{{1, 0, 0, 0}, {0, 1, 0, -.2f}, {0, 0, 1, 0}}}; + vr::HmdMatrix34_t setup{{{1, 0, 0, 0}, {0, 1, 0, .7f}, {0, 0, 1, 0}}}; + memcpy(batch[0].buffer, &standing, sizeof(standing)); + memcpy(batch[1].buffer, &setup, sizeof(setup)); + for (uint32_t i = 0; i < count; ++i) + batch[i].tag = 20; + return 0; + } + } paths; + roomsetup::SavedFloor floor{&messages, &paths}; + REQUIRE(floor.publish()); + REQUIRE(messages.sent.size() == 1); + auto *result = cJSON_Parse(messages.sent[0].c_str()); + REQUIRE(result); + CHECK(cJSON_IsNumber(cJSON_GetObjectItem(result, "pos"))); + CHECK(cJSON_GetNumberValue(cJSON_GetObjectItem(result, "pos")) == Catch::Approx(.9)); + CHECK(cJSON_GetObjectItem(result, "position") == nullptr); + cJSON_Delete(result); + REQUIRE(floor.publish(true)); + REQUIRE(messages.sent.size() == 3); + auto *reset = cJSON_Parse(messages.sent[2].c_str()); + REQUIRE(reset); + CHECK(cJSON_GetNumberValue(cJSON_GetObjectItem(reset, "value_source")) == 2); + cJSON_Delete(reset); +} + +TEST_CASE("Boundary warning retries failed delivery and removal") +{ + struct Notifications : vr::IVRNotifications { + unsigned creates = 0, removes = 0; + vr::EVRNotificationError CreateNotification(vr::VROverlayHandle_t, uint64_t, + vr::EVRNotificationType, const char *, vr::EVRNotificationStyle, + const vr::NotificationBitmap_t *, vr::VRNotificationId *id) override { + if (++creates == 1) return vr::VRNotificationError_InvalidOverlayHandle; + *id = 7; + return vr::VRNotificationError_OK; + } + vr::EVRNotificationError RemoveNotification(vr::VRNotificationId) override { + return ++removes == 1 ? vr::VRNotificationError_InvalidOverlayHandle : vr::VRNotificationError_OK; + } + } api; + std::string delivered, warning = "Boundary not confirmed, look around."; + vr::VRNotificationId id = 0; + CHECK_FALSE(update_world_notification(api, 1, warning, delivered, id)); + CHECK(delivered.empty()); + REQUIRE(update_world_notification(api, 1, warning, delivered, id)); + CHECK(delivered == warning); + CHECK(id == 7); + REQUIRE(update_world_notification(api, 1, warning, delivered, id)); + CHECK(api.creates == 2); + CHECK_FALSE(update_world_notification(api, 1, "", delivered, id)); + CHECK(delivered == warning); + CHECK(id == 7); + REQUIRE(update_world_notification(api, 1, "", delivered, id)); + CHECK(delivered.empty()); + CHECK(id == 0); +} diff --git a/tests/tests_steamvr_roomsetup_vulkan.cpp b/tests/tests_steamvr_roomsetup_vulkan.cpp new file mode 100644 index 000000000..10375c6ce --- /dev/null +++ b/tests/tests_steamvr_roomsetup_vulkan.cpp @@ -0,0 +1,536 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "../src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_vulkan.cpp" +#include + +namespace { +VkImageCreateInfo created{}; +std::array storage; +uint32_t copy_count; +VkImageCopy copied{}; +std::vector bindings; +VkIndexType drawn_type; +uint32_t idle_calls; +VkResult idle_result; +uint64_t *idle_cursor; +VkResult submit_result = VK_SUCCESS; +VkResult VKAPI_CALL fake_submit(VkQueue, uint32_t, const VkSubmitInfo *, VkFence) { return submit_result; } +VkResult VKAPI_CALL fake_idle(VkQueue) { + ++idle_calls; + if (idle_cursor) *idle_cursor += 4; + return idle_result; +} +void VKAPI_CALL fake_bind(VkCommandBuffer, VkBuffer, VkDeviceSize, VkIndexType type) { bindings.push_back(type); } +void VKAPI_CALL fake_draw(VkCommandBuffer, uint32_t, uint32_t, uint32_t, int32_t, uint32_t) +{ + drawn_type = bindings.back(); +} +void VKAPI_CALL fake_update(VkCommandBuffer, VkBuffer, VkDeviceSize, VkDeviceSize, const void *) {} +void VKAPI_CALL fake_buffer_copy(VkCommandBuffer, VkBuffer, VkBuffer, uint32_t, const VkBufferCopy *) {} +VkResult VKAPI_CALL fake_begin(VkCommandBuffer, const VkCommandBufferBeginInfo *) { return VK_SUCCESS; } +VkResult VKAPI_CALL fake_create(VkDevice, const VkImageCreateInfo *info, const VkAllocationCallbacks *, VkImage *out) +{ + created = *info; + *out = reinterpret_cast(uintptr_t{42}); + return VK_SUCCESS; +} +VkResult VKAPI_CALL fake_map(VkDevice, VkDeviceMemory, VkDeviceSize offset, VkDeviceSize, VkMemoryMapFlags, void **out) +{ + *out = storage.data() + offset; + return VK_SUCCESS; +} +void VKAPI_CALL fake_copy(VkCommandBuffer, VkImage, VkImageLayout, VkImage, VkImageLayout, uint32_t count, const VkImageCopy *regions) +{ + copy_count += count; + copied = regions[0]; +} +VkResult VKAPI_CALL fake_create_buffer(VkDevice, const VkBufferCreateInfo *, const VkAllocationCallbacks *, VkBuffer *out) +{ + *out = reinterpret_cast(uintptr_t{43}); + return VK_SUCCESS; +} +PFN_vkVoidFunction VKAPI_CALL fake_proc(VkDevice, const char *name) +{ + if (!strcmp(name, "vkQueueSubmit")) return reinterpret_cast(fake_submit); + if (!strcmp(name, "vkQueueWaitIdle")) return reinterpret_cast(fake_idle); + if (!strcmp(name, "vkCmdBindIndexBuffer")) return reinterpret_cast(fake_bind); + if (!strcmp(name, "vkCmdDrawIndexed")) return reinterpret_cast(fake_draw); + if (!strcmp(name, "vkCmdUpdateBuffer")) return reinterpret_cast(fake_update); + if (!strcmp(name, "vkCmdCopyBuffer")) return reinterpret_cast(fake_buffer_copy); + if (!strcmp(name, "vkBeginCommandBuffer")) return reinterpret_cast(fake_begin); + if (!strcmp(name, "vkCreateBuffer")) return reinterpret_cast(fake_create_buffer); + if (!strcmp(name, "vkCreateImage")) return reinterpret_cast(fake_create); + if (!strcmp(name, "vkMapMemory")) return reinterpret_cast(fake_map); + if (!strcmp(name, "vkCmdCopyImage")) return reinterpret_cast(fake_copy); + return nullptr; +} +struct Fixture +{ + void *dispatch = this; + VkDevice handle = reinterpret_cast(&dispatch); + std::shared_ptr state = std::make_shared(Device{fake_proc, handle, true, {}}); + Fixture() + { + devices.emplace(dispatch, state); + storage.fill(0x5a); + copy_count = 0; + bindings.clear(); + } + ~Fixture() { devices.erase(dispatch); } +}; +} + +TEST_CASE("SteamVR SDF empty image gets valid storage without changing other image types") +{ + Fixture f; + VkImageCreateInfo info{}; + info.sType = VK_STRUCTURE_TYPE_IMAGE_CREATE_INFO; + info.imageType = VK_IMAGE_TYPE_2D; + info.format = VK_FORMAT_R8_UNORM; + info.extent = {0, 0, 1}; + info.mipLevels = info.arrayLayers = 1; + info.samples = VK_SAMPLE_COUNT_1_BIT; + info.tiling = VK_IMAGE_TILING_LINEAR; + info.usage = VK_IMAGE_USAGE_TRANSFER_SRC_BIT | VK_IMAGE_USAGE_TRANSFER_DST_BIT; + VkImage image; + REQUIRE(vkCreateImage(f.handle, &info, nullptr, &image) == VK_SUCCESS); + CHECK(created.extent.width == 1); + CHECK(created.extent.height == 1); + CHECK(info.extent.width == 0); + CHECK(f.state->images.at(image).empty); + info.format = VK_FORMAT_R8G8B8A8_UNORM; + REQUIRE(vkCreateImage(f.handle, &info, nullptr, &image) == VK_SUCCESS); + CHECK(created.extent.width == 0); + CHECK(created.extent.height == 0); +} + +TEST_CASE("SteamVR SDF initializes sparse gaps using row pitch and keeps previous strokes") +{ + Fixture f; + auto memory = reinterpret_cast(uintptr_t{7}); + auto image = reinterpret_cast(uintptr_t{42}); + Image state{{5, 3, 1}, false, true}; + state.memory = memory; + state.offset = 8; + state.layout.offset = 4; + state.layout.rowPitch = 16; + f.state->images.emplace(image, state); + void *mapped; + REQUIRE(vkMapMemory(f.handle, memory, 8, 41, 0, &mapped) == VK_SUCCESS); + for (size_t i = 0; i < storage.size(); ++i) { + bool pixel = i >= 12 && i <= 48 && (i - 12) % 16 < 5; + CHECK(storage[i] == (pixel ? 255 : 0x5a)); + } + storage[12] = 91; + REQUIRE(vkMapMemory(f.handle, memory, 8, VK_WHOLE_SIZE, 0, &mapped) == VK_SUCCESS); + CHECK(storage[12] == 91); +} + +TEST_CASE("SteamVR SDF leaves partial mappings and unrelated allocations alone") +{ + Fixture f; + auto memory = reinterpret_cast(uintptr_t{7}); + Image state{{5, 3, 1}, false, true}; + state.memory = memory; + state.layout.rowPitch = 16; + f.state->images.emplace(reinterpret_cast(uintptr_t{42}), state); + void *mapped; + REQUIRE(vkMapMemory(f.handle, memory, 0, 36, 0, &mapped) == VK_SUCCESS); + CHECK_FALSE(f.state->images.begin()->second.initialized); + CHECK(std::all_of(storage.begin(), storage.end(), [](auto value) { return value == 0x5a; })); + REQUIRE(vkMapMemory(f.handle, reinterpret_cast(uintptr_t{8}), 0, VK_WHOLE_SIZE, 0, &mapped) == VK_SUCCESS); + CHECK(std::all_of(storage.begin(), storage.end(), [](auto value) { return value == 0x5a; })); +} + +TEST_CASE("SteamVR copies an empty sentinel but does not copy empty history into a drawing") +{ + Fixture f; + auto src = reinterpret_cast(uintptr_t{1}); + auto dst = reinterpret_cast(uintptr_t{2}); + f.state->images.emplace(src, Image{{1, 1, 1}, true, true}); + f.state->images.emplace(dst, Image{{1, 1, 1}, true, false}); + VkImageCopy region{}; + region.extent = {0, 0, 1}; + vkCmdCopyImage(reinterpret_cast(f.handle), src, VK_IMAGE_LAYOUT_GENERAL, dst, + VK_IMAGE_LAYOUT_GENERAL, 1, ®ion); + CHECK(copy_count == 1); + CHECK(copied.extent.width == 1); + CHECK(copied.extent.height == 1); + f.state->images.at(dst).empty = false; + vkCmdCopyImage(reinterpret_cast(f.handle), src, VK_IMAGE_LAYOUT_GENERAL, dst, + VK_IMAGE_LAYOUT_GENERAL, 1, ®ion); + CHECK(copy_count == 1); +} + +TEST_CASE("SteamVR playspace index correction distinguishes 32-bit uploads from real 16-bit data") +{ + Fixture f; + auto command = reinterpret_cast(f.handle); + auto index = reinterpret_cast(uintptr_t{20}); + auto vertex = reinterpret_cast(uintptr_t{21}); + auto pipeline = reinterpret_cast(uintptr_t{22}); + std::vector mesh; + for (uint32_t row = 0; row < 7; ++row) { + for (uint32_t column = 0; column < 7; ++column) { + uint32_t corner = row * 8 + column; + mesh.insert(mesh.end(), {corner, corner + 1, corner + 8, corner + 1, corner + 9, corner + 8}); + } + } + f.state->buffers.emplace(index, Buffer{mesh.size() * 4, VK_BUFFER_USAGE_INDEX_BUFFER_BIT}); + f.state->buffers.emplace(vertex, Buffer{64 * 16, VK_BUFFER_USAGE_VERTEX_BUFFER_BIT}); + f.state->strides[pipeline] = 16; + f.state->images.emplace(reinterpret_cast(uintptr_t{23}), Image{{512, 512, 1}, false, false}); + f.state->draws[command] = Draw{pipeline, vertex, index}; + vkCmdBindIndexBuffer(command, index, 0, VK_INDEX_TYPE_UINT16); + vkCmdUpdateBuffer(command, index, 0, mesh.size() * 4, mesh.data()); + REQUIRE(f.state->buffers.at(index).indices.has_value()); + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT32); + CHECK(bindings.back() == VK_INDEX_TYPE_UINT16); + CHECK(f.state->draws.at(command).index_type == VK_INDEX_TYPE_UINT16); + + SECTION("index A/B switch leaves SDF resources and caller binding intact") { + f.state->index_fix = false; + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + CHECK_FALSE(f.state->images.empty()); + CHECK(bindings.back() == VK_INDEX_TYPE_UINT16); + } + SECTION("SDF A/B switch does not disable index repair") { + f.state->sdf_fix = false; + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT32); + } + SECTION("outline draws can use a subset of a reserved 32-bit index buffer") { + f.state->strides[pipeline] = 28; + f.state->buffers.at(vertex).size = 64 * 28; + vkCmdDrawIndexed(command, 42, 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT32); + } + SECTION("reserved vertex storage can exceed the referenced vertex range") { + f.state->buffers.at(vertex).size = 80 * 16; + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT32); + } + SECTION("genuine 16-bit mesh uploads remain unchanged") { + std::vector compact(mesh.begin(), mesh.end()); + f.state->buffers.at(index).size = compact.size() * 2; + vkCmdUpdateBuffer(command, index, 0, compact.size() * 2, compact.data()); + vkCmdDrawIndexed(command, compact.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + } + SECTION("nonmatching pipeline layouts remain unchanged") { + f.state->strides[pipeline] = 0; + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + } + SECTION("out-of-bounds meshes remain unchanged") { + f.state->buffers.at(vertex).size = 63 * 16; + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + } + SECTION("partial uploads invalidate the inferred index format") { + vkCmdUpdateBuffer(command, index, 4, 4, mesh.data()); + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + } + SECTION("an uploaded prefix covers draws without requiring unused index capacity") { + f.state->buffers.at(index).size += 1024; + vkCmdUpdateBuffer(command, index, 0, mesh.size() * 4, mesh.data()); + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT32); + vkCmdDrawIndexed(command, mesh.size() + 3, 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + } + SECTION("staging copies validate only the copied index range and honor memory offsets") { + auto staging = reinterpret_cast(uintptr_t{30}); + auto memory = reinterpret_cast(uintptr_t{31}); + std::vector bytes(64 + mesh.size() * 4, 0x5a); + memcpy(bytes.data() + 64, mesh.data(), mesh.size() * 4); + Buffer source{mesh.size() * 4 + 16, VK_BUFFER_USAGE_TRANSFER_SRC_BIT}; + source.memory = memory; source.memory_offset = 48; + f.state->buffers.emplace(staging, source); + f.state->mappings[memory] = Mapping{bytes.data(), 0, bytes.size()}; + VkBufferCopy region{16, 0, mesh.size() * 4}; + vkCmdCopyBuffer(command, staging, index, 1, ®ion); + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT32); + f.state->mappings[memory].size = bytes.size() - 1; + vkCmdCopyBuffer(command, staging, index, 1, ®ion); + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + } + SECTION("unobserved copies invalidate the inferred index format") { + VkBufferCopy transfer{0, 0, 4}; + vkCmdCopyBuffer(command, vertex, index, 1, &transfer); + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + } + SECTION("starting another command buffer recording clears stale bindings") { + VkCommandBufferBeginInfo begin{VK_STRUCTURE_TYPE_COMMAND_BUFFER_BEGIN_INFO}; + REQUIRE(vkBeginCommandBuffer(command, &begin) == VK_SUCCESS); + vkCmdDrawIndexed(command, mesh.size(), 1, 0, 0, 0); + CHECK(drawn_type == VK_INDEX_TYPE_UINT16); + } +} + +TEST_CASE("SteamVR index inference rejects degenerate and wide meshes") +{ + std::array indices{0, 1, 2}; + REQUIRE(index_upload(sizeof(indices), indices.data()).has_value()); + indices = {0, 0, 0}; + CHECK_FALSE(index_upload(sizeof(indices), indices.data()).has_value()); + indices = {0, 1, 65536}; + CHECK_FALSE(index_upload(sizeof(indices), indices.data()).has_value()); + CHECK_FALSE(index_upload(8, indices.data()).has_value()); +} + +TEST_CASE("SteamVR index workaround accepts only the measured playspace vertex layouts") +{ + VkVertexInputBindingDescription binding{0, 16, VK_VERTEX_INPUT_RATE_VERTEX}; + std::array attributes{{ + {0, 0, VK_FORMAT_R32G32_SFLOAT, 0}, {1, 0, VK_FORMAT_R32G32_SFLOAT, 8}}}; + VkPipelineVertexInputStateCreateInfo vertex{}; + vertex.vertexBindingDescriptionCount = 1; vertex.pVertexBindingDescriptions = &binding; + vertex.vertexAttributeDescriptionCount = 2; vertex.pVertexAttributeDescriptions = attributes.data(); + VkPipelineInputAssemblyStateCreateInfo assembly{}; + assembly.topology = VK_PRIMITIVE_TOPOLOGY_TRIANGLE_LIST; + VkGraphicsPipelineCreateInfo pipeline{}; + pipeline.pVertexInputState = &vertex; pipeline.pInputAssemblyState = &assembly; + CHECK(playspace_stride(pipeline) == 16); + binding.stride = 28; + attributes = {{{0, 0, VK_FORMAT_R32G32B32_SFLOAT, 0}, {1, 0, VK_FORMAT_R32G32B32A32_SFLOAT, 12}}}; + CHECK(playspace_stride(pipeline) == 28); + assembly.primitiveRestartEnable = VK_TRUE; + CHECK(playspace_stride(pipeline) == 0); + assembly.primitiveRestartEnable = VK_FALSE; + attributes[1].location = 0; + CHECK(playspace_stride(pipeline) == 0); +} + +TEST_CASE("SteamVR ring identification requires the complete Vulkan-backed identity") +{ + RingRecovery ring{}; + RingFields fields{uintptr_t{42}, reinterpret_cast(uintptr_t{20}), reinterpret_cast(uintptr_t{21}), ring_capacity}; + ring.identity = fields; + Buffer buffer{ring_capacity, VK_BUFFER_USAGE_TRANSFER_SRC_BIT, fields.memory, 0}; + Mapping map{reinterpret_cast(fields.mapped), 0, VK_WHOLE_SIZE}; + REQUIRE(ring.matches(fields, buffer, map)); + SECTION("wrong mapped pointer") { fields.mapped++; } + SECTION("wrong buffer") { fields.buffer = VK_NULL_HANDLE; } + SECTION("wrong memory") { fields.memory = VK_NULL_HANDLE; } + SECTION("overflowed cursor") { fields.used++; } + SECTION("wrong capacity") { buffer.size--; } + SECTION("other buffer use") { buffer.usage |= VK_BUFFER_USAGE_VERTEX_BUFFER_BIT; } + SECTION("offset binding") { buffer.memory_offset = 4; } + SECTION("partial map") { map.size = ring_capacity - 1; } + CHECK_FALSE(ring.matches(fields, buffer, map)); +} + +TEST_CASE("SteamVR ring reset drains submitted reads and rejects outstanding reservations") +{ + Fixture f; + f.state->ring_allowed = true; + auto buffer = reinterpret_cast(uintptr_t{20}); + auto memory = reinterpret_cast(uintptr_t{21}); + auto command = reinterpret_cast(f.handle); + auto queue = reinterpret_cast(f.handle); + RingFields fields{reinterpret_cast(storage.data()), buffer, memory, ring_capacity - ring_margin + 64}; + f.state->buffers.emplace(buffer, Buffer{ring_capacity, VK_BUFFER_USAGE_TRANSFER_SRC_BIT, memory, 0}); + f.state->mappings.emplace(memory, Mapping{storage.data(), 0, VK_WHOLE_SIZE}); + identify_ring(f.state, memory, reinterpret_cast(&fields)); + REQUIRE(f.state->rings.size() == 1); + auto &ring = f.state->rings.at(buffer); + REQUIRE_FALSE(ring.disabled); + ring.copy(command, fields.used); + ring.commands.at(command).ended = true; + idle_calls = 0; + idle_result = VK_SUCCESS; + idle_cursor = nullptr; + const uint64_t original = fields.used; + REQUIRE(ring.owner == 0); + submit_result = VK_SUCCESS; + VkSubmitInfo info{}; + info.sType = VK_STRUCTURE_TYPE_SUBMIT_INFO; + info.commandBufferCount = 1; + info.pCommandBuffers = &command; + + SECTION("successful submit drains exactly its queue before resetting") { + recover_rings(f.state, queue, {command}); + CHECK(idle_calls == 1); + CHECK(fields.used == 0); + CHECK(ring.resets == 1); + CHECK_FALSE(ring.disabled); + } + SECTION("binary semaphore waits still drain the submitted upload") { + auto semaphore = reinterpret_cast(uintptr_t{22}); + VkPipelineStageFlags stage = VK_PIPELINE_STAGE_TRANSFER_BIT; + info.waitSemaphoreCount = 1; info.pWaitSemaphores = &semaphore; info.pWaitDstStageMask = &stage; + REQUIRE(vkQueueSubmit(queue, 1, &info, VK_NULL_HANDLE) == VK_SUCCESS); + CHECK(idle_calls == 1); CHECK(fields.used == 0); + CHECK(ring.owner == static_cast(syscall(SYS_gettid))); + } + SECTION("nonzero timeline waits cannot block future CPU signal work") { + uint64_t value = 1; + VkTimelineSemaphoreSubmitInfo timeline{}; + timeline.sType = VK_STRUCTURE_TYPE_TIMELINE_SEMAPHORE_SUBMIT_INFO; + timeline.waitSemaphoreValueCount = 1; timeline.pWaitSemaphoreValues = &value; + info.pNext = &timeline; + REQUIRE(vkQueueSubmit(queue, 1, &info, VK_NULL_HANDLE) == VK_SUCCESS); + CHECK(idle_calls == 0); CHECK(fields.used == original); CHECK(ring.disabled); + } + SECTION("failed submission cannot retire or reset the upload") { + submit_result = VK_ERROR_DEVICE_LOST; + CHECK(vkQueueSubmit(queue, 1, &info, VK_NULL_HANDLE) == submit_result); + CHECK(idle_calls == 0); CHECK(fields.used == original); CHECK(ring.pending()); + } + SECTION("upload authors cannot change after the first submission") { + ring.owner = static_cast(syscall(SYS_gettid)) + 1; + recover_rings(f.state, queue, {command}); + CHECK(idle_calls == 0); CHECK(fields.used == original); CHECK(ring.disabled); + } + SECTION("another upload remains unsubmitted") { + auto other = reinterpret_cast(uintptr_t{22}); + ring.copy(other, original); + recover_rings(f.state, queue, {command}); + CHECK(idle_calls == 0); + CHECK(fields.used == original); + } + SECTION("CPU reserved bytes beyond the last recorded copy") { + fields.used++; + recover_rings(f.state, queue, {command}); + CHECK(idle_calls == 0); + CHECK(fields.used == original + 1); + } + SECTION("queue drain failure leaves the cursor unchanged and disables recovery") { + idle_result = VK_ERROR_DEVICE_LOST; + recover_rings(f.state, queue, {command}); + CHECK(fields.used == original); + CHECK(ring.disabled); + CHECK(ring.resets == 0); + } + SECTION("reservation during drain leaves the changed cursor untouched") { + idle_cursor = &fields.used; + recover_rings(f.state, queue, {command}); + CHECK(fields.used == original + 4); + CHECK(ring.disabled); + CHECK(ring.resets == 0); + } + SECTION("stale allocator identity never drains or writes") { + fields.memory = VK_NULL_HANDLE; + recover_rings(f.state, queue, {command}); + CHECK(idle_calls == 0); + CHECK(fields.used == original); + CHECK(ring.disabled); + } + SECTION("still-recording commands cannot reset") { + ring.commands.at(command).ended = false; + recover_rings(f.state, queue, {command}); + CHECK(idle_calls == 0); + CHECK(fields.used == original); + CHECK(ring.disabled); + } + SECTION("repeated submission without a fresh recording is rejected") { + ring.commands.at(command).submitted = true; + recover_rings(f.state, queue, {command}); + CHECK(idle_calls == 0); + CHECK(fields.used == original); + CHECK(ring.disabled); + } + idle_cursor = nullptr; +} + +TEST_CASE("SteamVR unknown builds and inaccessible allocator storage fail closed") +{ + CHECK_FALSE(supported_ring_build()); + RingFields fields{}; + CHECK_FALSE(local_read(1, &fields, sizeof(fields))); + CHECK_FALSE(local_write(1, 0)); + Fixture f; + f.state->ring_allowed = true; + auto buffer = reinterpret_cast(uintptr_t{20}); + auto memory = reinterpret_cast(uintptr_t{21}); + f.state->buffers.emplace(buffer, Buffer{ring_capacity, VK_BUFFER_USAGE_TRANSFER_SRC_BIT, memory, 0}); + f.state->mappings.emplace(memory, Mapping{storage.data(), 0, VK_WHOLE_SIZE}); + identify_ring(f.state, memory, reinterpret_cast(uintptr_t{1})); + REQUIRE(f.state->rings.size() == 1); + CHECK(f.state->rings.at(buffer).disabled); +} + + +TEST_CASE("Room View tracks destination uniform buffers for camera correction") +{ + Fixture f; + VkBufferCreateInfo info{}; + info.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO; + info.size = 400; + info.usage = VK_BUFFER_USAGE_UNIFORM_BUFFER_BIT | VK_BUFFER_USAGE_TRANSFER_DST_BIT; + VkBuffer buffer; + REQUIRE(vkCreateBuffer(f.handle, &info, nullptr, &buffer) == VK_SUCCESS); + REQUIRE(f.state->buffers.count(buffer) == 1); + CHECK(f.state->buffers.at(buffer).usage == info.usage); + CHECK(f.state->buffers.at(buffer).size == 400); +} + +namespace { +std::vector display_modes; +VkResult VKAPI_CALL fake_display_modes(VkPhysicalDevice, VkDisplayKHR, uint32_t *count, VkDisplayModePropertiesKHR *out) +{ + if (!out) { + *count = display_modes.size(); + return VK_SUCCESS; + } + *count = std::min(*count, display_modes.size()); + std::copy_n(display_modes.begin(), *count, out); + return *count < display_modes.size() ? VK_INCOMPLETE : VK_SUCCESS; +} +std::vector enumerate_modes() +{ + uint32_t count = 0; + alias_single_display_mode(fake_display_modes, nullptr, nullptr, &count, nullptr); + std::vector modes(count); + alias_single_display_mode(fake_display_modes, nullptr, nullptr, &count, modes.data()); + return modes; +} +// SteamVR 2.17.10 direct-mode rate choice: 90 Hz until a matching mode beats an earlier match. +float steamvr_rate(const std::vector &modes, float desired) +{ + float rate = 90.0f, best = 0.0f; + std::vector seen; + for (const auto &mode : modes) { + float candidate = mode.parameters.refreshRate * 0.001f; + if (std::find(seen.begin(), seen.end(), candidate) != seen.end()) continue; + float difference = std::fabs(candidate - desired); + if (seen.empty()) best = difference; + else if (best > difference) rate = candidate, best = difference; + seen.push_back(candidate); + } + return rate; +} +} // namespace + +TEST_CASE("SteamVR adopts the refresh rate of a single-mode HMD panel") +{ + VkDisplayModeKHR handle = reinterpret_cast(uintptr_t{7}); + display_modes = {{handle, {{1440, 2560}, 80000}}}; + CHECK(steamvr_rate(display_modes, 80.0f) == 90.0f); + + auto modes = enumerate_modes(); + REQUIRE(modes.size() == 2); + CHECK(modes[0].displayMode == handle); + CHECK(modes[1].displayMode == handle); + CHECK(modes[1].parameters.refreshRate == 80000); + CHECK(steamvr_rate(modes, 80.0f) == 80.0f); + + uint32_t count = 1; + VkDisplayModePropertiesKHR first{}; + CHECK(alias_single_display_mode(fake_display_modes, nullptr, nullptr, &count, &first) == VK_INCOMPLETE); + CHECK(count == 1); + + display_modes = {{handle, {{1440, 1600}, 90000}}, {handle, {{1440, 1600}, 144000}}}; + modes = enumerate_modes(); + REQUIRE(modes.size() == 2); + CHECK(modes[0].parameters.refreshRate == 90000); + CHECK(modes[1].parameters.refreshRate == 144000); +} diff --git a/tests/tests_vec3_angle.cpp b/tests/tests_vec3_angle.cpp index 6b0743a1a..18cb71cb1 100644 --- a/tests/tests_vec3_angle.cpp +++ b/tests/tests_vec3_angle.cpp @@ -18,3 +18,14 @@ TEST_CASE("Vec3Angle") CHECK(m_vec3_angle({1, 0, 0}, {0, 1, 0}) == Catch::Approx(M_PI / 2)); CHECK(m_vec3_angle({1, 0, 0}, {sqrt2_2, sqrt2_2, 0}) == Catch::Approx(M_PI / 4)); } + +TEST_CASE("Vec3Angle remains finite for nearly parallel gravity directions") +{ + for (float x : {.03f, .1f, .7f}) for (float y : {.9f, 1.f, 9.82f}) { + xrt_vec3 a = {x, y, -.3f}; + xrt_vec3 b = {x + 1e-7f, y, -.3f}; + CHECK(isfinite(m_vec3_angle(a, b))); + CHECK(m_vec3_angle(a, b) < .001f); + CHECK(m_vec3_angle(a, m_vec3_mul_scalar(b, -1)) == Catch::Approx(M_PI).margin(.001)); + } +}