steam-frame/lbgos--rift-s-linux
1
0 Fork 0
mirror of https://github.com/lbgos/rift-s-linux.git synced 2026-10-17 13:00:59 +02:00
Code Issues Packages Projects Releases 2 Wiki Activity

Rift S: SLAM guard, Touch controller tracking, native room setup and SteamVR integration

Browse Source 
Squashed from the working branch. Adds the Basalt patches, constrained
constellation pose solver, Rift S optics, world anchor and boundary
persistence, SteamVR camera queue and room setup guard, and tests.
This commit is contained in:
lbgos committed 2026-10-09 22:49:57 +03:00
1 parent b1c850a015
commit dff6cab92c
114 files changed
+24951 -1227
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

LICENSES/BSD-3-Clause-Rectus.txt
+28
View File
@@ -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.
README-monado.md
+278
View File
@@ -0,0 +1,278 @@
# Monado - XR Runtime (XRT)
<!--
Copyright 2018-2021, Collabora, Ltd.
SPDX-License-Identifier: CC-BY-4.0
This must stay in sync with the last section!
-->
> * Main homepage and documentation: <https://monado.freedesktop.org/>
> * Promotional homepage: <https://monado.dev>
> * Maintained at <https://gitlab.freedesktop.org/monado/monado>
> * Latest API documentation: <https://monado.pages.freedesktop.org/monado>
> * Continuously-updated changelog of the default branch:
> <https://monado.pages.freedesktop.org/monado/_c_h_a_n_g_e_l_o_g.html>
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
<rylie.pavlik@collabora.com> and using the associated key from
<https://keys.openpgp.org/vks/v1/by-fingerprint/45207B2B1E53E1F2755FF63CC5A2D593A61DBC9D>.
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 <https://www.freedesktop.org/wiki/CodeOfConduct/>,
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 <rylie.pavlik@collabora.com>
* Frederic Plourde <frederic.plourde@collabora.com>
* Jakob Bornecrantz <tbornecrantz@nvidia.com>
* freedesktop.org contacts: see most recent list at <https://www.freedesktop.org/wiki/CodeOfConduct/>
Code of Conduct section excerpt adapted from the
[Contributor Covenant](https://www.contributor-covenant.org), version 1.4.1,
available at
<https://www.contributor-covenant.org/version/1/4/code-of-conduct.html>, and
from the freedesktop.org-specific version of that code, available at
<https://www.freedesktop.org/wiki/CodeOfConduct/>, used under CC-BY-4.0.
README.md
+30 -258
View File
@@ -1,278 +1,50 @@
# Monado - XR Runtime (XRT)
# Rift S on Linux
<!--
Copyright 2018-2021, Collabora, Ltd.
Oculus Rift S support for Monado and SteamVR on Linux. It has inside-out head tracking with Basalt SLAM, Touch controller tracking from the controller LEDs, and room setup with a play-area boundary.
SPDX-License-Identifier: CC-BY-4.0
This is a fork of thaytan's Monado Rift S branch, `dev-constellation-controller-tracking`. The original Monado readme is in `README-monado.md`.
This must stay in sync with the last section!
-->
## Status
> * Main homepage and documentation: <https://monado.freedesktop.org/>
> * Promotional homepage: <https://monado.dev>
> * Maintained at <https://gitlab.freedesktop.org/monado/monado>
> * Latest API documentation: <https://monado.pages.freedesktop.org/monado>
> * Continuously-updated changelog of the default branch:
> <https://monado.pages.freedesktop.org/monado/_c_h_a_n_g_e_l_o_g.html>
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
<rylie.pavlik@collabora.com> and using the associated key from
<https://keys.openpgp.org/vks/v1/by-fingerprint/45207B2B1E53E1F2755FF63CC5A2D593A61DBC9D>.
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 <https://www.freedesktop.org/wiki/CodeOfConduct/>,
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 <rylie.pavlik@collabora.com>
* Frederic Plourde <frederic.plourde@collabora.com>
* Jakob Bornecrantz <tbornecrantz@nvidia.com>
* freedesktop.org contacts: see most recent list at <https://www.freedesktop.org/wiki/CodeOfConduct/>
Code of Conduct section excerpt adapted from the
[Contributor Covenant](https://www.contributor-covenant.org), version 1.4.1,
available at
<https://www.contributor-covenant.org/version/1/4/code-of-conduct.html>, and
from the freedesktop.org-specific version of that code, available at
<https://www.freedesktop.org/wiki/CodeOfConduct/>, used under CC-BY-4.0.
Contact lbgos at contact@lbgos.dev.
patches/basalt/0001-keep-imu-frequency-per-tracker.patch
+27
View File
@@ -0,0 +1,27 @@
--- a/src/vit/vit_tracker.cpp
+++ b/src/vit/vit_tracker.cpp
@@ -127,6 +127,7 @@
vector<bool> cam;
} calib_data_ready;
uint32_t cam_count;
+ double imu_calibration_frequency = -1;
Calibration<double> 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
patches/basalt/0002-limit-workers-with-driver-calibration.patch
+12
View File
@@ -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>(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;
}
patches/basalt/0003-safe-recreation-and-realtime-worker-budget.patch
+193
View File
@@ -0,0 +1,193 @@
--- a/src/vit/vit_tracker.cpp
+++ b/src/vit/vit_tracker.cpp
@@ -55,6 +55,7 @@
#include <sophus/se3.hpp>
#include <cstdio>
+#include <cstdlib>
#include <memory>
#include <string>
#include <thread>
@@ -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>(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<double>::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<bool> 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 <typename T>
+ static void push_latest(tbb::concurrent_bounded_queue<T> &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<float>();
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<Scalar>()),
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;
scripts/install-rifts-passthrough-overlay.py
Executable
+48
View File
@@ -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()
scripts/rift-s-controller-capture.sh
Executable
+62
View File
@@ -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"
scripts/rifts-passthrough-overlay.py
Executable
+123
View File
@@ -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()
scripts/rifts-passthrough-probe.py
Executable
+50
View File
@@ -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()
scripts/rifts_passthrough.py
+58
View File
@@ -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("<IIIIIIIIQQ")
class Stream:
def __init__(self):
self.fd = os.open(PATH, os.O_RDWR | os.O_NOFOLLOW | os.O_CLOEXEC)
try:
fcntl.flock(self.fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
self.mm = mmap.mmap(self.fd, 0)
magic, version, self.width, self.height, stride, fmt, *_ = HEADER.unpack_from(self.mm)
if magic != 0x54535052 or version != 2 or fmt != 0:
raise ValueError("Incompatible camera export; install matching driver and scripts")
self.size = stride * self.height
if not (0 < self.width <= 640 and 0 < self.height <= 320 and stride == self.width * 4
and len(self.mm) >= 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("<I", self.mm, 24, heartbeat)
@property
def toggles(self):
return struct.unpack_from("<I", self.mm, 28)[0]
def frame(self, previous):
for _ in range(3):
sequence = struct.unpack_from("<Q", self.mm, 32)[0]
if sequence == previous or sequence == 0 or sequence & 1:
return None
timestamp = struct.unpack_from("<Q", self.mm, 40)[0]
pixels = self.mm[HEADER.size:HEADER.size + self.size]
if sequence == struct.unpack_from("<Q", self.mm, 32)[0]:
age_ms = (time.monotonic_ns() - timestamp) / 1e6
if 0 <= age_ms <= 250:
return sequence, timestamp, pixels, age_ms
return None
def close(self):
self.activate(False)
self.mm.close()
os.close(self.fd)
scripts/steamvr-camera-probe.py
Executable
+55
View File
@@ -0,0 +1,55 @@
#!/usr/bin/env python3
# Copyright 2026, lbgos
# SPDX-License-Identifier: BSL-1.0
"""Check driver_monado's tracked camera through an existing SteamVR session.
Requires Python openvr. Opens no V4L2/HID devices and writes no camera images.
"""
import ctypes
import json
import statistics
import time
import openvr
openvr.init(openvr.VRApplication_Background)
handle = None
try:
camera = openvr.VRTrackedCamera()
if not camera.hasCamera(0):
raise SystemExit("The active SteamVR driver does not advertise a camera")
kind = openvr.VRTrackedCameraFrameType_Undistorted
width, height, size = camera.getCameraFrameSize(0, kind)
pixels = (ctypes.c_uint8 * size)()
handle = camera.acquireVideoStreamingService(0)
seen = set()
arrivals = []
black = 0
last_error = None
end = time.monotonic() + 5
while time.monotonic() < end:
try:
header = camera.getVideoStreamFrameBuffer(handle, kind, pixels, size)
if header.nFrameSequence not in seen:
seen.add(header.nFrameSequence)
arrivals.append(time.monotonic())
# Ignore alpha when SteamVR converts RGBX32 to public RGBA buffers.
channels = header.nBytesPerPixel
if channels not in (3, 4) or header.nWidth * header.nHeight * channels > 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()
src/external/openvr_includes/openvr_driver.h
Vendored
+2
View File
@@ -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
};
src/xrt/auxiliary/bindings/steamvr_profiles.py
+24 -3
View File
@@ -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)
src/xrt/auxiliary/math/m_clock_tracking.c
+4 -1
View File
@@ -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;
}
src/xrt/auxiliary/math/m_vec3.h
+3 -1
View File
@@ -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
src/xrt/auxiliary/tracking/t_camera_models.h
+6 -2
View File
@@ -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;
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+290 -37
View File
@@ -40,12 +40,15 @@
#include <opencv2/core/mat.hpp>
#include <opencv2/core/version.hpp>
#include <algorithm>
#include <atomic>
#include <deque>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <map>
#include <mutex>
#include <shared_mutex>
#include <string>
#include <vector>
@@ -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<timing_sample>
TimingWriter(const string &dir, const string &fn, bool e, const vector<string> &cn)
: CSVWriter<timing_sample>(dir, fn, e, cn)
{}
void
set_columns(const vector<string> &columns)
{
column_names = columns;
}
};
//! Writes feature information specific to a particular estimated pose
@@ -260,6 +272,16 @@ struct FeaturesWriter : public CSVWriter<feature_count_sample>
*/
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(&lts, &lr);
int64_t lts = nts;
bool have_previous = t.slam_rels.get_latest(&lts, &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, &params);
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<timepoint_ns>(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) {
src/xrt/auxiliary/tracking/t_tracking.h
+22
View File
@@ -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
src/xrt/auxiliary/tracking/t_vit_loader.c
+6
View File
@@ -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;
}
src/xrt/auxiliary/tracking/t_vit_loader.h
+2
View File
@@ -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;
src/xrt/compositor/main/comp_renderer.c
+40
View File
@@ -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;
src/xrt/compositor/render/render_distortion.c
+41 -4
View File
@@ -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;
}
src/xrt/compositor/render/render_interface.h
+4
View File
@@ -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
src/xrt/drivers/CMakeLists.txt
+15
View File
@@ -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()
src/xrt/drivers/rift_s/rift_s.c
+25
View File
@@ -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);
src/xrt/drivers/rift_s/rift_s.h
+8
View File
@@ -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;
src/xrt/drivers/rift_s/rift_s_camera.c
+185 -78
View File
@@ -16,12 +16,13 @@
* @ingroup drv_rift_s
*/
#include <string.h>
#include <stdio.h>
#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);
}
src/xrt/drivers/rift_s/rift_s_camera_metadata.h
+104
View File
@@ -0,0 +1,104 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#pragma once
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#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;
}
src/xrt/drivers/rift_s/rift_s_controller.c
+1621 -121
View File
File diff suppressed because it is too large. Load diff
src/xrt/drivers/rift_s/rift_s_controller.h
+304 -11
View File
@@ -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
src/xrt/drivers/rift_s/rift_s_firmware.c
+83 -4
View File
@@ -18,6 +18,7 @@
/* Oculus Rift S Driver - firmware JSON parsing functions */
#include <string.h>
#include <stdio.h>
#include <math.h>
#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) ||
src/xrt/drivers/rift_s/rift_s_firmware.h
+20 -6
View File
@@ -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);
src/xrt/drivers/rift_s/rift_s_hmd.c
+105 -44
View File
@@ -26,13 +26,17 @@
#include <stdio.h>
#include <time.h>
#include <assert.h>
#include <ctype.h>
#include <unistd.h>
#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);
src/xrt/drivers/rift_s/rift_s_hmd.h
+9 -3
View File
@@ -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 *
src/xrt/drivers/rift_s/rift_s_model_check.cpp
+207
View File
@@ -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 <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <algorithm>
#include <cmath>
#include <limits>
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<M3> 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<M3> 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<float>::infinity();
out->angle_x_min = out->angle_y_min = std::numeric_limits<float>::infinity();
out->angle_x_max = out->angle_y_max = -std::numeric_limits<float>::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<float>::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<float>::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";
}
}
src/xrt/drivers/rift_s/rift_s_model_check.h
+117
View File
@@ -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
src/xrt/drivers/rift_s/rift_s_optics.c
+149
View File
@@ -0,0 +1,149 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#include "rift_s_optics.h"
#include <math.h>
#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);
}
}
src/xrt/drivers/rift_s/rift_s_optics.h
+37
View File
@@ -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);
src/xrt/drivers/rift_s/rift_s_passthrough.c
+263
View File
@@ -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 <inttypes.h>
#include <math.h>
#include <stdlib.h>
#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);
}
src/xrt/drivers/rift_s/rift_s_passthrough.h
+23
View File
@@ -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
src/xrt/drivers/rift_s/rift_s_position_filter.c
+334
View File
@@ -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 <math.h>
#include <string.h>
//! 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);
}
src/xrt/drivers/rift_s/rift_s_position_filter.h
+156
View File
@@ -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
src/xrt/drivers/rift_s/rift_s_protocol.c
+111
View File
@@ -25,6 +25,7 @@
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#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)
src/xrt/drivers/rift_s/rift_s_protocol.h
+49
View File
@@ -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... */
src/xrt/drivers/rift_s/rift_s_radio.c
+14 -2
View File
@@ -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;
src/xrt/drivers/rift_s/rift_s_radio.h
+9
View File
@@ -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,
src/xrt/drivers/rift_s/rift_s_slam_guard.c
+463
View File
@@ -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 <math.h>
#include <string.h>
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;
}
src/xrt/drivers/rift_s/rift_s_slam_guard.h
+226
View File
@@ -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 <stdbool.h>
#include <stdint.h>
#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
src/xrt/drivers/rift_s/rift_s_tracker.c
+669 -44
View File
@@ -20,6 +20,7 @@
#include <string.h>
#include <stdio.h>
#include <time.h>
#include <math.h>
#include <assert.h>
#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)
{
src/xrt/drivers/rift_s/rift_s_tracker.h
+53
View File
@@ -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
src/xrt/drivers/rift_s/rift_s_util.cpp
+29 -1
View File
@@ -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;
}
src/xrt/drivers/rift_s/rift_s_util.h
+5
View File
@@ -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);
src/xrt/drivers/rift_s/rift_s_world_anchor.cpp
+1490
View File
File diff suppressed because it is too large. Load diff
src/xrt/drivers/rift_s/rift_s_world_anchor.h
+126
View File
@@ -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
src/xrt/drivers/rift_s/rift_s_world_control.h
+15
View File
@@ -0,0 +1,15 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#pragma once
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
// 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);
}
src/xrt/include/tracking/t_constellation_tracking.h
+33
View File
@@ -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 *
src/xrt/include/tracking/t_led_models.h
+11
View File
@@ -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
src/xrt/include/xrt/xrt_camera.h
+17
View File
@@ -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);
};
src/xrt/include/xrt/xrt_device.h
+10
View File
@@ -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);
src/xrt/include/xrt/xrt_tracking.h
+7
View File
@@ -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.
src/xrt/state_trackers/steamvr_drv/CMakeLists.txt
+18 -1
View File
@@ -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()
src/xrt/state_trackers/steamvr_drv/monado_roomsetup_vulkan.json.in
+13
View File
@@ -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"}
}
}
src/xrt/state_trackers/steamvr_drv/ovrd_camera.cpp
+573
View File
@@ -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 <algorithm>
#include <cmath>
#include <cstring>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/resource.h>
#include <sys/syscall.h>
#include <pthread.h>
#include <sched.h>
#include <unistd.h>
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 *> 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<struct rifts_passthrough_shm *>(
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<uint8_t>(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<id_t>(syscall(SYS_gettid)), 10);
int64_t last_timestamp = 0;
std::vector<uint8_t> 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<uint32_t>(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<int64_t>(frame->timestamp) -
static_cast<int64_t>(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<uintptr_t>(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<int32_t>(camera->view_width * 2),
static_cast<int32_t>(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);
}
src/xrt/state_trackers/steamvr_drv/ovrd_camera.hpp
+132
View File
@@ -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 <atomic>
#include <mutex>
#include <thread>
#include <vector>
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<Buffer> buffers;
std::vector<std::vector<uint8_t>> 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<bool> 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<MonadoCamera *> s_instance;
void
run();
uint32_t
bytes() const
{
return camera->view_width * 2 * camera->view_height * 4;
}
};
src/xrt/state_trackers/steamvr_drv/ovrd_camera_queue.hpp
+162
View File
@@ -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
src/xrt/state_trackers/steamvr_drv/ovrd_display.hpp
+57
View File
@@ -0,0 +1,57 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#pragma once
#include "xrt/xrt_device.h"
#include <algorithm>
#include <cmath>
// 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);
}
src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp
+348 -105
View File
@@ -15,9 +15,18 @@
#include <cassert>
#include <array>
#include <thread>
#include <mutex>
#include <cstdio>
#include <cmath>
#include <algorithm>
#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 <memory>
#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<MonadoCamera> 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<vr::ETrackedDeviceProperty>(11003), enabled);
properties->SetBoolProperty(m_ulPropertyContainer, static_cast<vr::ETrackedDeviceProperty>(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::IVRBlockQueue *>(vr::VRDriverContext()->GetGenericInterface(vr::IVRBlockQueue_Version, &queue_error));
auto paths = static_cast<vr::IVRPaths *>(vr::VRDriverContext()->GetGenericInterface(vr::IVRPaths_Version, &queue_error));
m_camera = std::make_unique<MonadoCamera>(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<vr::ETrackedDeviceProperty>(11003);
constexpr auto supports_vrlink_requests = static_cast<vr::ETrackedDeviceProperty>(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);
src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp
+180
View File
@@ -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 <cmath>
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<enum xrt_space_relation_flags>(
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<enum xrt_space_relation_flags>(
(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};
}
src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp
+110
View File
@@ -0,0 +1,110 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#pragma once
#include "openvr.h"
#include "ovrd_roomsetup_frame.hpp"
#include <cmath>
#include <cstdio>
#include <optional>
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<float>
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<float>{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
src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_frame.hpp
+182
View File
@@ -0,0 +1,182 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#pragma once
#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <fcntl.h>
#include <string>
#include <sys/mman.h>
#include <sys/stat.h>
#include <time.h>
#include <unistd.h>
namespace roomsetup {
using Matrix = std::array<float, 16>;
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<FrameState *>(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<float, 98> 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<unsigned char *>(bytes) + 192, fixed.data(), sizeof(fixed));
return true;
}
} // namespace roomsetup
src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp
+414
View File
@@ -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 <cjson/cJSON.h>
#include <chrono>
#include <algorithm>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <dlfcn.h>
#include <filesystem>
#include <fstream>
#include <optional>
#include <memory>
#include <poll.h>
#include <sys/syscall.h>
#include <sys/stat.h>
#include <thread>
#include <unistd.h>
#include <vector>
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_t>(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::nanoseconds>(
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::nanoseconds>(
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 &notifications,
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<int>(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<size_t>(std::min<uint64_t>(size - cursor, 65536)), '\0');
stream.read(bytes.data(), bytes.size());
bytes.resize(static_cast<size_t>(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<char> exported;
};
static std::optional<Snapshot> 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::nanoseconds>(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<int>(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<uint32_t (*)(vr::EVRInitError *, vr::EVRApplicationType, const char *)>(dlsym(library, "VR_InitInternal2"));
auto interface = reinterpret_cast<void *(*)(const char *, vr::EVRInitError *)>(dlsym(library, "VR_GetGenericInterface"));
auto shutdown = reinterpret_cast<void (*)()>(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<vr::IVRSystem *>(interface(vr::IVRSystem_Version, &error));
auto *setup = static_cast<vr::IVRChaperoneSetup *>(interface(vr::IVRChaperoneSetup_Version, &error));
auto *overlays = static_cast<vr::IVROverlay *>(interface(vr::IVROverlay_Version, &error));
auto *notifications = static_cast<vr::IVRNotifications *>(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<Snapshot> 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<roomsetup::Mailbox *>(interface("IVRMailbox_001", &error));
auto *paths = static_cast<roomsetup::Paths *>(interface("IVRPaths_001", &error));
auto floor = std::make_unique<roomsetup::SavedFloor>(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;
}
src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard_process.hpp
+83
View File
@@ -0,0 +1,83 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#pragma once
#ifdef __linux__
#include <dlfcn.h>
#include <cstdio>
#include <cstring>
#include <filesystem>
#include <fcntl.h>
#include <signal.h>
#include <spawn.h>
#include <sys/wait.h>
#include <unistd.h>
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<void *>(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
src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_vulkan.cpp
+1089
View File
File diff suppressed because it is too large. Load diff
src/xrt/state_trackers/steamvr_drv/rifts_passthrough_shm.h
+36
View File
@@ -0,0 +1,36 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#pragma once
#include <stdint.h>
#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
src/xrt/state_trackers/steamvr_drv/steamvr_bindings/ovrd_bindings.py
+2 -2
View File
@@ -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,
src/xrt/targets/common/target_builder_rift_s.c
+3 -2
View File
@@ -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.
src/xrt/targets/steamvr_drv/CMakeLists.txt
+18 -1
View File
@@ -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()
src/xrt/targets/steamvr_drv/resources/input/monado_touch_compositor.json
+279
View File
@@ -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"
}
}
]
}
}
}
src/xrt/targets/steamvr_drv/resources/settings/default.vrsettings
+2 -1
View File
@@ -1,5 +1,6 @@
{
"driver_monado" : {
"enable" : true
"enable" : true,
"enableNativeRoomSetup" : true
}
}
src/xrt/tracking/constellation/CMakeLists.txt
+3
View File
@@ -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
src/xrt/tracking/constellation/internal/blobwatch.c
+6 -7
View File
@@ -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);
}
}
src/xrt/tracking/constellation/internal/camera_model.h
+98
View File
@@ -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);
}
src/xrt/tracking/constellation/internal/constrained_pose.cpp
+1614
View File
File diff suppressed because it is too large. Load diff
src/xrt/tracking/constellation/internal/constrained_pose.h
+424
View File
@@ -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
src/xrt/tracking/constellation/internal/correspondence_search.c
+82 -42
View File
@@ -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;
}
src/xrt/tracking/constellation/internal/correspondence_search.h
+23 -2
View File
@@ -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
src/xrt/tracking/constellation/internal/debug_draw.cpp
+6 -6
View File
@@ -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;
src/xrt/tracking/constellation/internal/pose_metrics.c
+46 -3
View File
@@ -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;
}
src/xrt/tracking/constellation/internal/pose_metrics.h
+11
View File
@@ -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);
src/xrt/tracking/constellation/internal/ransac_pnp.cpp
+40 -12
View File
@@ -28,6 +28,7 @@
#endif
#include <iostream>
#include <cmath>
using namespace std;
@@ -49,15 +50,17 @@ quat_to_3x3(cv::Mat &mat, struct xrt_quat *me)
mat.at<double>(2, 2) = 1 - 2 * me->x * me->x - 2 * me->y * me->y;
}
static void
static bool
undistort_blob_points(std::vector<cv::Point2f> in_points,
std::vector<cv::Point2f> &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<int>(row)];
double depth = R.at<double>(2, 0) * point.x + R.at<double>(2, 1) * point.y +
R.at<double>(2, 2) * point.z + tvec.at<double>(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<double>(0) * inorm;
v.y = rvec.at<double>(1) * inorm;
v.z = rvec.at<double>(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<double>(0) / angle;
v.y = rvec.at<double>(1) / angle;
v.z = rvec.at<double>(2) / angle;
math_quat_from_angle_vector(angle, &v, &pose->orientation);
}
pose->position.x = tvec.at<double>(0);
pose->position.y = tvec.at<double>(1);
pose->position.z = tvec.at<double>(2);
src/xrt/tracking/constellation/internal/sample.c
+69
View File
@@ -9,6 +9,11 @@
*/
#include "sample.h"
#include "math/m_api.h"
#include "math/m_vec3.h"
#include <math.h>
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);
}
src/xrt/tracking/constellation/internal/sample.h
+31
View File
@@ -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
src/xrt/tracking/constellation/t_constellation_tracking.c
+1185 -423
View File
File diff suppressed because it is too large. Load diff
src/xrt/tracking/constellation/t_led_models.c
+5
View File
@@ -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
tests/CMakeLists.txt
+122
View File
@@ -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()
tests/constellation_rift_s_data.hpp
+36
View File
@@ -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<t_constellation_led, 15> 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<std::array<float, 4>, 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<float, 4> rift_s_ring_intrinsics = {190.178f, 190.178f, 315.85f, 237.907f};
static const std::array<float, 4> 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};
tests/tests_constellation.cpp
+437
View File
@@ -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 <array>
#include <cmath>
#include <vector>
#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<t_constellation_led, 5> leds = {};
std::array<blob, 5> 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<t_constellation_led, 8> leds = {};
std::array<blob, 8> 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<blob, 8> 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<correspondence_search_flags>(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<unsigned *>(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<correspondence_search_flags>(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<blob, 6> 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<uint8_t, 400> 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<blob> 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<blob, 8> 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<correspondence_search_flags>(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<blob, 5> blobs{};
std::array<t_constellation_led, 5> 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<uint8_t, 400> 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);
}
tests/tests_constellation_constrained.cpp
+1984
View File
File diff suppressed because it is too large. Load diff
tests/tests_rift_s_controller_capture.cpp
+39
View File
@@ -0,0 +1,39 @@
// Copyright 2026, lbgos
// SPDX-License-Identifier: BSL-1.0
#include "catch_amalgamated.hpp"
#include "rift_s_controller.h"
#include <cmath>
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));
}
Loaded 100 of 114 files, more files were not shown because too many files have changed in this diff. Show more