diff --git a/README.md b/README.md
index ee2d7d3..b020290 100644
--- a/README.md
+++ b/README.md
@@ -1,75 +1,155 @@
-[](https://gitlab.com/VladyslavUsenko/basalt/commits/master)
+# Basalt for Monado
-## Basalt
-For more information see https://vision.in.tum.de/research/vslam/basalt
+This is a fork of [Basalt](https://gitlab.com/VladyslavUsenko/basalt) with some
+modifications so that it can be used from Monado for SLAM tracking. Many thanks
+to the Basalt authors.
-
+Follow this file for instructions on how to get Basalt up and running with
+Monado. This README tries to be as concise as possible, but there are many
+details that need to be addressed on it, so please do not skip any section,
+otherwise it is likely that it won't work. Having said that, this guide has
+been tested in limited setups, so please report any changes you had to make
+in order to get it working in different ones.
-This project contains tools for:
-* Camera, IMU and motion capture calibration.
-* Visual-inertial odometry and mapping.
-* Simulated environment to test different components of the system.
-
-Some reusable components of the system are available as a separate [header-only library](https://gitlab.com/VladyslavUsenko/basalt-headers) ([Documentation](https://vladyslavusenko.gitlab.io/basalt-headers/)).
-
-There is also a [Github mirror](https://github.com/VladyslavUsenko/basalt-mirror) of this project to enable easy forking.
-
-## Related Publications
-Visual-Inertial Odometry and Mapping:
-* **Visual-Inertial Mapping with Non-Linear Factor Recovery**, V. Usenko, N. Demmel, D. Schubert, J. Stückler, D. Cremers, In IEEE Robotics and Automation Letters (RA-L) [[DOI:10.1109/LRA.2019.2961227]](https://doi.org/10.1109/LRA.2019.2961227) [[arXiv:1904.06504]](https://arxiv.org/abs/1904.06504).
-
-Calibration (explains implemented camera models):
-* **The Double Sphere Camera Model**, V. Usenko and N. Demmel and D. Cremers, In 2018 International Conference on 3D Vision (3DV), [[DOI:10.1109/3DV.2018.00069]](https://doi.org/10.1109/3DV.2018.00069), [[arXiv:1807.08957]](https://arxiv.org/abs/1807.08957).
-
-Calibration (demonstrates how these tools can be used for dataset calibration):
-* **The TUM VI Benchmark for Evaluating Visual-Inertial Odometry**, D. Schubert, T. Goll, N. Demmel, V. Usenko, J. Stückler, D. Cremers, In 2018 International Conference on Intelligent Robots and Systems (IROS), [[DOI:10.1109/IROS.2018.8593419]](https://doi.org/10.1109/IROS.2018.8593419), [[arXiv:1804.06120]](https://arxiv.org/abs/1804.06120).
-
-Calibration (describes B-spline trajectory representation used in camera-IMU calibration):
-* **Efficient Derivative Computation for Cumulative B-Splines on Lie Groups**, C. Sommer, V. Usenko, D. Schubert, N. Demmel, D. Cremers, In 2020 Conference on Computer Vision and Pattern Recognition (CVPR), [[DOI:10.1109/CVPR42600.2020.01116]](https://doi.org/10.1109/CVPR42600.2020.01116), [[arXiv:1911.08860]](https://arxiv.org/abs/1911.08860).
-
-Optimization (describes square-root optimization and marginalization used in VIO/VO):
-* **Square Root Marginalization for Sliding-Window Bundle Adjustment**, N. Demmel, D. Schubert, C. Sommer, D. Cremers, V. Usenko, In 2021 International Conference on Computer Vision (ICCV), [[arXiv:2109.02182]](https://arxiv.org/abs/2109.02182)
+## Index
+- [Basalt for Monado](#basalt-for-monado)
+ - [Index](#index)
+ - [Installation](#installation)
+ - [Build and Install Directories](#build-and-install-directories)
+ - [Dependencies](#dependencies)
+ - [Build Basalt](#build-basalt)
+ - [Running Basalt](#running-basalt)
+ - [Monado Specifics](#monado-specifics)
+ - [Notes on Basalt Usage](#notes-on-basalt-usage)
+ - [Using Real Hardware](#using-real-hardware)
## Installation
-### APT installation for Ubuntu 20.04 and 18.04 (Fast)
-Set up keys, add the repository to the sources list, update the Ubuntu package index and install Basalt:
-```
-sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-keys 0AD9A3000D97B6C9
-sudo sh -c 'echo "deb [arch=amd64] http://packages.usenko.eu/ubuntu $(lsb_release -sc) $(lsb_release -sc)/main" > /etc/apt/sources.list.d/basalt.list'
-sudo apt-get update
-sudo apt-get dist-upgrade
-sudo apt-get install basalt
+
+This was tested on both Ubuntu 20.04 and 18.04, be sure to open an issue if the
+steps don't work for you.
+
+### Build and Install Directories
+
+To not clutter your system directories, let's set two environment variables,
+`$bsltdeps` and `$bsltinstall` that point to existing empty build and install
+directories respectively. These directories will contain everything produced in
+this guide besides installed apt dependencies.
+
+```bash
+# Change the paths accordingly
+export bsltinstall=/home/mateo/Documents/apps/bsltinstall
+export bsltdeps=/home/mateo/Documents/apps/bsltdeps
```
-### Source installation for Ubuntu >= 18.04 and MacOS >= 10.14 Mojave
-Clone the source code for the project and build it. For MacOS you should have [Homebrew](https://brew.sh/) installed.
-```
-git clone --recursive https://gitlab.com/VladyslavUsenko/basalt.git
-cd basalt
-./scripts/install_deps.sh
-mkdir build
-cd build
-cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo
-make -j8
+Let's extend our system paths with those.
+
+```bash
+export PATH=$bsltinstall/bin:$PATH
+export PKG_CONFIG_PATH=$bsltinstall/lib/pkgconfig:$PKG_CONFIG_PATH # for compile time pkg-config
+export LD_LIBRARY_PATH=$bsltinstall/lib/:$LD_LIBRARY_PATH # for runtime ld
+export LIBRARY_PATH=$bsltinstall/lib/:$LIBRARY_PATH # for compile time gcc
```
-## Usage
-* [Camera, IMU and Mocap calibration. (TUM-VI, Euroc, UZH-FPV and Kalibr datasets)](doc/Calibration.md)
-* [Visual-inertial odometry and mapping. (TUM-VI and Euroc datasets)](doc/VioMapping.md)
-* [Visual odometry (no IMU). (KITTI dataset)](doc/Vo.md)
-* [Simulation tools to test different components of the system.](doc/Simulation.md)
-* [Batch evaluation tutorial (ICCV'21 experiments)](doc/BatchEvaluation.md)
+### Dependencies
-## Device support
-* [Tutorial on Camera-IMU and Motion capture calibration with Realsense T265.](doc/Realsense.md)
+Most dependencies will be automatically built by basalt, however there are some
+known issues you might need to deal with (click to open the ones that might
+affect you).
-## Development
-* [Development environment setup.](doc/DevSetup.md)
+
+ Issues with GCC 11
-## Licence
-The code is provided under a BSD 3-clause license. See the LICENSE file for details.
-Note also the different licenses of thirdparty submodules.
+ If you are using GCC 11 you might also get some issues with pangolin as there is now a
+ [name clash with Pagolin `_serialize()` name](https://github.com/stevenlovegrove/Pangolin/issues/657),
+ it [should be fixed](https://gcc.gnu.org/bugzilla/show_bug.cgi?id=100438#c12)
+ in newer versions of GCC-11. For fixing it yourself, you can cherry-pick
+ [these commits](https://github.com/stevenlovegrove/Pangolin/pull/658/commits),
+ or use a different GCC version.
+ (see
+ [this discord thread](https://discord.com/channels/556527313823596604/556527314670714901/904339906288050196)
+ in the Monado server for more info).
+
-Some improvements are ported back from the fork
-[granite](https://github.com/DLR-RM/granite) (MIT license).
+### Build Basalt
+
+```bash
+cd $bsltdeps
+git clone --recursive git@gitlab.freedesktop.org:mateosss/basalt.git
+./basalt/scripts/install_deps.sh
+sed -i "s#/home/mateo/Documents/apps/bsltdeps/#$bsltdeps/#" basalt/data/monado/*.toml
+cd basalt && mkdir build && cd build
+cmake .. -DCMAKE_INSTALL_PREFIX=$bsltinstall -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBUILD_TESTS=off -DBASALT_INSTANTIATIONS_DOUBLE=off
+make install -j12
+```
+
+### Running Basalt
+
+This step is optional but you can try Basalt without Monado with one of the following methods:
+
+- Through an EuRoC dataset (be sure to [download
+ one](http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/)
+ first): `basalt_vio --dataset-path /path/to/euroc/V1_01_easy --cam-calib
+ $bsltdeps/basalt/data/euroc_ds_calib.json --dataset-type euroc --config-path
+ $bsltdeps/basalt/data/euroc_config.json --marg-data ~/Desktop/euroc_marg_data
+ --show-gui 1`
+- With a RealSense T265 (you'll need to get a `t265_calib.json` yourself as
+ detailed [below](#configuring-basalt) but meanwhile you can try with [this
+ file](https://gitlab.com/VladyslavUsenko/basalt/-/issues/52) instead):
+ `basalt_rs_t265_vio --cam-calib $bsltdeps/basalt/data/t265_calib.json
+ --config-path $bsltdeps/basalt/data/euroc_config.json`
+- With a RealSense D455 (and maybe this also works for a D435):
+ `basalt_rs_t265_vio --is-d455 --cam-calib
+ $bsltdeps/basalt/data/d455_calib.json --config-path
+ $bsltdeps/basalt/data/euroc_config.json`
+
+### Monado Specifics
+
+You'll need to compile Monado with the same Eigen used in Basalt, and with the
+same flags. For that, set these with CMake (or equivalent flags for meson):
+`-DEIGEN3_INCLUDE_DIR=$bsltdeps/basalt/thirdparty/basalt-headers/thirdparty/eigen
+-DCMAKE_C_FLAGS="-march=native" -DCMAKE_CXX_FLAGS="-march=native"` otherwise
+Monado will automatically use your system's Eigen, and having mismatched Eigen
+version/flags can cause a lot of headaches.
+
+Run an OpenXR app like `hello_xr` with the following environment variables set
+
+```bash
+export EUROC_PATH=/path/to/euroc/V1_01_easy/ # Set euroc dataset path. You can get a dataset from http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_01_easy/V1_01_easy.zip
+export EUROC_LOG=debug
+export EUROC_HMD=false # if false, a fake controller will be tracked, else a fake HMD
+export SLAM_LOG=debug
+export SLAM_CONFIG=$bsltdeps/basalt/data/monado/euroc.toml # Point to Basalt config file for Euroc
+export OXR_DEBUG_GUI=1 # We will need the debug ui to start streaming the dataset
+```
+
+Finally, run the XR app and press start in the euroc player debug ui and you
+should see a controller being tracked with Basalt from the euroc dataset.
+
+## Notes on Basalt Usage
+
+- Tracking is not perfect, [this](https://youtu.be/mIgRHmxbaC8) and
+ [this](https://youtu.be/gxu3Ve8VCnI) show how it looks, as well as the
+ problems that it has (difficulties with rotation-only movements, wiggliness on
+ fast movements, etc)
+- This fork only works with Stereo-IMU setups, but adapting Basalt to work with
+ other configurations should feasible (see
+ [granite](https://github.com/DLR-RM/granite)).
+- Basalt is _fast_. While the standard sampling rate is stereo 640x480 at 30fps
+ I've been able to make it work at 848x480 at 60fps without problems on a
+ laptop.
+- Some things that might cause crashes:
+ - Using images with bad exposure and gain values, or being in a dark room.
+ - Shaking causes drift that can diverge if maintained for long periods of
+ time.
+ - Continuously making sudden 90 degree rotations in which the new scene does not share
+ features with the previous scene.
+ - Moving too fast and/or making rotation only movements over extended periods
+ of time.
+
+## Using Real Hardware
+
+Monado has a couple of drivers supporting SLAM tracking (and thus Basalt). Here is how to set them up:
+
+- [RealSense Driver](doc/monado/Realsense.md)
+- [WMR Driver](doc/monado/WMR.md)
diff --git a/data/monado/wmr-tools/odysseyplus_wmrcalib_example.json b/data/monado/wmr-tools/odysseyplus_wmrcalib_example.json
new file mode 100644
index 0000000..9440599
--- /dev/null
+++ b/data/monado/wmr-tools/odysseyplus_wmrcalib_example.json
@@ -0,0 +1,223 @@
+{
+ "CalibrationInformation": {
+ "Cameras": [{
+ "Intrinsics": {
+ "ModelParameterCount": 15,
+ "ModelParameters": [0.5067707896232605, 0.51088905334472656, 0.42040637135505676, 0.56076663732528687, 0.6257319450378418, 0.46612036228179932, 0.0041795829311013222, 0.89431935548782349, 0.54253977537155151, 0.06621214747428894, 0.0027008021716028452, -0.00058499001897871494, -4.2882973502855748e-05, -0.00018502399325370789, 2.7941114902496338],
+ "ModelType": "CALIBRATION_LensDistortionModelRational6KT"
+ },
+ "Location": "CALIBRATION_CameraLocationHT0",
+ "Purpose": "CALIBRATION_CameraPurposeHeadTracking",
+ "MetricRadius": 2.7941114902496338,
+ "Rt": {
+ "Rotation": [1, 0, 0, 0, 1, 0, 0, 0, 1],
+ "Translation": [0, 0, 0]
+ },
+ "SensorHeight": 480,
+ "SensorWidth": 640,
+ "Shutter": "CALIBRATION_ShutterTypeUndefined",
+ "ThermalAdjustmentParams": {
+ "Params": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ }
+ }, {
+ "Intrinsics": {
+ "ModelParameterCount": 15,
+ "ModelParameters": [0.50554186105728149, 0.52156180143356323, 0.42205807566642761, 0.56302011013031006, 0.55718272924423218, 0.22437196969985962, 0.0068156048655509949, 0.83317267894744873, 0.26174271106719971, 0.043505862355232239, 0.00676964595913887, -0.0012049071956425905, 0.0001220563062815927, 0.00011782468209275976, 2.7899987697601318],
+ "ModelType": "CALIBRATION_LensDistortionModelRational6KT"
+ },
+ "Location": "CALIBRATION_CameraLocationHT1",
+ "Purpose": "CALIBRATION_CameraPurposeHeadTracking",
+ "MetricRadius": 2.7899987697601318,
+ "Rt": {
+ "Rotation": [0.68708890676498413, -0.0066884770058095455, -0.726542592048645, -0.004866383969783783, 0.99989283084869385, -0.013807035051286221, 0.72655707597732544, 0.013022295199334621, 0.68698269128799438],
+ "Translation": [-0.096558056771755219, -0.00065802858443930745, -0.041434925049543381]
+ },
+ "SensorHeight": 480,
+ "SensorWidth": 640,
+ "Shutter": "CALIBRATION_ShutterTypeUndefined",
+ "ThermalAdjustmentParams": {
+ "Params": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ }
+ }, {
+ "Intrinsics": {
+ "ModelParameterCount": 15,
+ "ModelParameters": [0.50590163469314575, 0.501025915145874, 0.43491056561470032, 0.51977533102035522, 1.3840854167938232, -1.6851117610931396, 6.9683127403259277, 1.4408130645751953, -1.7367498874664307, 6.9408659934997559, 0.015325400047004223, -9.7191208624280989e-05, -0.00023150799097493291, 0.00017367169493809342, 1.3999999761581421],
+ "ModelType": "CALIBRATION_LensDistortionModelRational6KT"
+ },
+ "Location": "CALIBRATION_CameraLocationDO0",
+ "Purpose": "CALIBRATION_CameraPurposeDisplayObserver",
+ "MetricRadius": 1.3999999761581421,
+ "Rt": {
+ "Rotation": [1, 0, 0, 0, 1, 0, 0, 0, 1],
+ "Translation": [0, 0, 0]
+ },
+ "SensorHeight": 2048,
+ "SensorWidth": 2448,
+ "Shutter": "CALIBRATION_ShutterTypeUndefined",
+ "ThermalAdjustmentParams": {
+ "Params": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ }
+ }, {
+ "Intrinsics": {
+ "ModelParameterCount": 15,
+ "ModelParameters": [0.505301833152771, 0.50203627347946167, 0.43675145506858826, 0.52202123403549194, 9.5410280227661133, -19.506465911865234, 31.453699111938477, 9.64380931854248, -19.527170181274414, 31.339166641235352, 0.016977680847048759, 0.0032255123369395733, -0.00076866301242262125, 0.000584927445743233, 1.3999999761581421],
+ "ModelType": "CALIBRATION_LensDistortionModelRational6KT"
+ },
+ "Location": "CALIBRATION_CameraLocationDO1",
+ "Purpose": "CALIBRATION_CameraPurposeDisplayObserver",
+ "MetricRadius": 1.3999999761581421,
+ "Rt": {
+ "Rotation": [0.99999630451202393, 0.0025376041885465384, -0.00095876428531482816, -0.0025385897606611252, 0.99999624490737915, -0.0010279536945745349, 0.00095615215832367539, 0.0010303838644176722, 0.99999898672103882],
+ "Translation": [-0.0659172385931015, 7.41809417377226e-05, -0.00013815540296491235]
+ },
+ "SensorHeight": 2048,
+ "SensorWidth": 2448,
+ "Shutter": "CALIBRATION_ShutterTypeUndefined",
+ "ThermalAdjustmentParams": {
+ "Params": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ }
+ }],
+ "Displays": [{
+ "Affine": [907.99212646484375, -0.55761176347732544, 785.1776123046875, -0, 908.129150390625, 796.96160888671875, 0, 0, 1],
+ "AssignedEye": "CALIBRATION_DisplayEyeLeft",
+ "DisplayHeight": 1600,
+ "DisplayWidth": 2880,
+ "DistortionRed": {
+ "ModelParameterCount": 5,
+ "ModelParameters": [787.31629583849372, 795.24291853725526, 3.9138434409665894e-07, 2.8758159734654908e-13, 4.9106499642880268e-19],
+ "ModelType": "CALIBRATION_DisplayDistortionModelPolynomial3K"
+ },
+ "DistortionGreen": {
+ "ModelParameterCount": 5,
+ "ModelParameters": [786.6702072553893, 794.69238050015588, 4.2007202329361826e-07, 1.8878438698927907e-13, 7.0250600169383861e-19],
+ "ModelType": "CALIBRATION_DisplayDistortionModelPolynomial3K"
+ },
+ "DistortionBlue": {
+ "ModelParameterCount": 5,
+ "ModelParameters": [785.89252155554175, 797.67656573162583, 5.0202131325863806e-07, -1.2813487061581019e-13, 1.2345214819185544e-18],
+ "ModelType": "CALIBRATION_DisplayDistortionModelPolynomial3K"
+ },
+ "EyePositionCorrectionModel": {
+ "ModelParameterCount": 0,
+ "ModelParameters": [],
+ "ModelType": "CALIBRATION_EyePositionCorrectionModelIpdTranslational"
+ },
+ "HorizontalFieldOfView": 0,
+ "VerticalFieldOfView": 0,
+ "VisibleAreaCenter": {
+ "X": 785.1776315693005,
+ "Y": 796.96162857569425
+ },
+ "VisibleAreaRadius": 820,
+ "VBlankToPhotonLatency": 0,
+ "Rt": {
+ "Rotation": [0.915799617767334, 0.0045000603422522545, -0.40161022543907166, 0.13639208674430847, 0.93702936172485352, 0.32151702046394348, 0.377767413854599, -0.3492216169834137, 0.85751736164093018],
+ "Translation": [-0.021123100072145462, 0.018961008638143539, 0.079663842916488647]
+ }
+ }, {
+ "Affine": [907.9639892578125, -0.49046245217323303, 2103.386474609375, -0, 907.97808837890625, 801.86834716796875, 0, 0, 1],
+ "AssignedEye": "CALIBRATION_DisplayEyeRight",
+ "DisplayHeight": 1600,
+ "DisplayWidth": 2880,
+ "DistortionRed": {
+ "ModelParameterCount": 5,
+ "ModelParameters": [2101.5711896433636, 800.96838064339022, 3.9383231143483177e-07, 2.4224056018878508e-13, 5.8448606879296118e-19],
+ "ModelType": "CALIBRATION_DisplayDistortionModelPolynomial3K"
+ },
+ "DistortionGreen": {
+ "ModelParameterCount": 5,
+ "ModelParameters": [2101.1059339164444, 799.96334797691441, 4.2454822843541872e-07, 1.412440585952293e-13, 7.9387878585863295e-19],
+ "ModelType": "CALIBRATION_DisplayDistortionModelPolynomial3K"
+ },
+ "DistortionBlue": {
+ "ModelParameterCount": 5,
+ "ModelParameters": [2100.8534549194974, 802.57825131804134, 5.1161917803099739e-07, -1.9534633908166077e-13, 1.3528407038619122e-18],
+ "ModelType": "CALIBRATION_DisplayDistortionModelPolynomial3K"
+ },
+ "EyePositionCorrectionModel": {
+ "ModelParameterCount": 0,
+ "ModelParameters": [],
+ "ModelType": "CALIBRATION_EyePositionCorrectionModelIpdTranslational"
+ },
+ "HorizontalFieldOfView": 0,
+ "VerticalFieldOfView": 0,
+ "VisibleAreaCenter": {
+ "X": 2103.3864389371893,
+ "Y": 801.86837363199038
+ },
+ "VisibleAreaRadius": 820,
+ "VBlankToPhotonLatency": 0,
+ "Rt": {
+ "Rotation": [0.9165765643119812, 0.0027716662734746933, -0.39984956383705139, 0.13801105320453644, 0.93633246421813965, 0.322853684425354, 0.37528696656227112, -0.35110378265380859, 0.85783785581588745],
+ "Translation": [-0.086859606206417084, 0.018304631114006042, 0.080222859978675842]
+ }
+ }],
+ "InertialSensors": [{
+ "BiasTemperatureModel": [0.016492774710059166, 0, 0, 0, -0.01642640121281147, 0, 0, 0, -0.0045625129714608192, 0, 0, 0],
+ "BiasUncertainty": [9.9999997473787516e-05, 9.9999997473787516e-05, 9.9999997473787516e-05],
+ "Id": "CALIBRATION_InertialSensorId_ICM20602",
+ "MixingMatrixTemperatureModel": [1.0000957250595093, 0, 0, 0, -0.00082343036774545908, 0, 0, 0, -0.0015050634974613786, 0, 0, 0, -0.00082384591223672032, 0, 0, 0, 0.99958902597427368, 0, 0, 0, -4.7339185584860388e-06, 0, 0, 0, -0.0015064212493598461, 0, 0, 0, -4.7357993935293052e-06, 0, 0, 0, 0.99919366836547852, 0, 0, 0],
+ "ModelTypeMask": 16,
+ "Noise": [0.00095000001601874828, 0.00095000001601874828, 0.00095000001601874828, 0, 0, 0],
+ "Rt": {
+ "Rotation": [0.91831707954406738, 0.0026458536740392447, -0.39583677053451538, 0.13994280993938446, 0.93323278427124023, 0.33089667558670044, 0.3702833354473114, -0.35926258563995361, 0.85663330554962158],
+ "Translation": [0, 0, 0]
+ },
+ "SecondOrderScaling": [0, 0, 0, 0, 0, 0, 0, 0, 0],
+ "SensorType": "CALIBRATION_InertialSensorType_Gyro",
+ "TemperatureBounds": [5, 60],
+ "TemperatureC": 0
+ }, {
+ "BiasTemperatureModel": [-0.18563582003116608, 0, 0, 0, 0.057621683925390244, 0, 0, 0, -0.13435612618923187, 0, 0, 0],
+ "BiasUncertainty": [0.0099999997764825821, 0.0099999997764825821, 0.0099999997764825821],
+ "Id": "CALIBRATION_InertialSensorId_ICM20602",
+ "MixingMatrixTemperatureModel": [0.99813401699066162, 0, 0, 0, 0.00030715670436620712, 0, 0, 0, -0.00016754241369199008, 0, 0, 0, 0.00030630044057033956, 0, 0, 0, 1.0009244680404663, 0, 0, 0, 0.00029482797253876925, 0, 0, 0, -0.0001675997773418203, 0, 0, 0, 0.000295753387035802, 0, 0, 0, 0.99779242277145386, 0, 0, 0],
+ "ModelTypeMask": 56,
+ "Noise": [0.010700000450015068, 0.010700000450015068, 0.010700000450015068, 0, 0, 0],
+ "Rt": {
+ "Rotation": [0.918566882610321, 0.0016602915711700916, -0.39526209235191345, 0.13899369537830353, 0.93476790189743042, 0.32693997025489807, 0.37002113461494446, -0.35525518655776978, 0.85841602087020874],
+ "Translation": [-0.083942756056785583, -0.0025952591095119715, 0.0026445253752171993]
+ },
+ "SecondOrderScaling": [0, 0, 0, 0, 0, 0, 0, 0, 0],
+ "SensorType": "CALIBRATION_InertialSensorType_Accelerometer",
+ "TemperatureBounds": [5, 60],
+ "TemperatureC": 0
+ }, {
+ "BiasTemperatureModel": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
+ "BiasUncertainty": [0, 0, 0],
+ "Id": "CALIBRATION_InertialSensorId_AK09916",
+ "MixingMatrixTemperatureModel": [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0],
+ "ModelTypeMask": 0,
+ "Noise": [0.699999988079071, 0.699999988079071, 0.699999988079071, 0, 0, 0],
+ "Rt": {
+ "Rotation": [0.918566882610321, 0.0016602915711700916, -0.39526209235191345, 0.13899369537830353, 0.93476790189743042, 0.32693997025489807, 0.37002113461494446, -0.35525518655776978, 0.85841602087020874],
+ "Translation": [0, 0, 0]
+ },
+ "SecondOrderScaling": [0, 0, 0, 0, 0, 0, 0, 0, 0],
+ "SensorType": "CALIBRATION_InertialSensorType_Magnetometer",
+ "TemperatureBounds": [0, 0],
+ "TemperatureC": 0
+ }],
+ "Metadata": {
+ "SerialId": "XQ8198CM61077ZA",
+ "FactoryCalDate": "6/24/2019 6:38:33 AM GMT",
+ "Version": {
+ "Major": 1,
+ "Minor": 2
+ },
+ "DeviceName": "GOERTEK-BUILD-MP"
+ },
+ "TemperatureData": {
+ "ACCEL_0": {
+ "Average": 36.477929875996843,
+ "Min": 34.54,
+ "Max": 37.92
+ },
+ "GYRO_0": {
+ "Average": 36.477925294725082,
+ "Min": 34.54,
+ "Max": 37.92
+ }
+ }
+ }
+}
diff --git a/data/monado/wmr-tools/wmr2bslt_calib.py b/data/monado/wmr-tools/wmr2bslt_calib.py
new file mode 100755
index 0000000..46f5b0c
--- /dev/null
+++ b/data/monado/wmr-tools/wmr2bslt_calib.py
@@ -0,0 +1,207 @@
+#!/usr/bin/env python3
+
+# Run with ./wmr2bslt_calib.py your_wmrcalib.json > your_calib.json
+
+import json
+import argparse
+import numpy as np
+from numpy.linalg import inv
+
+from math import sqrt
+
+
+def get(j, name):
+ assert name in ["HT0", "HT1", "Gyro", "Accelerometer"]
+ is_imu = name in ["Gyro", "Accelerometer"]
+ calib = j["CalibrationInformation"]
+ sensors = calib["InertialSensors" if is_imu else "Cameras"]
+ name_key = "SensorType" if is_imu else "Location"
+ sensor = next(filter(lambda s: s[name_key].endswith(name), sensors))
+ return sensor
+
+
+def rt2mat(rt):
+ R33 = np.array(rt["Rotation"]).reshape(3, 3)
+ t31 = np.array(rt["Translation"]).reshape(3, 1)
+ T34 = np.hstack((R33, t31))
+ T44 = np.vstack((T34, [0, 0, 0, 1]))
+ return T44
+
+
+def rmat2quat(r):
+ w = sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2]) / 2
+ w4 = 4 * w
+ x = (r[2, 1] - r[1, 2]) / w4
+ y = (r[0, 2] - r[2, 0]) / w4
+ z = (r[1, 0] - r[0, 1]) / w4
+ return np.array([x, y, z, w])
+
+
+def extrinsics(j, cam):
+ # NOTE: The `Rt` field seems to be a transform from the sensor to HT0 (i.e.,
+ # from HT0 space to sensor space). For basalt we need the transforms
+ # expressed w.r.t IMU origin.
+
+ # NOTE: The gyro and magnetometer translations are 0, probably because an
+ # HMD is a rigid body. Therefore the accelerometer is considered as the IMU
+ # origin.
+
+ imu = get(j, "Accelerometer")
+ T_i_c0 = rt2mat(imu["Rt"])
+
+ T = None
+ if cam == "HT0":
+ T = T_i_c0
+ elif cam == "HT1":
+ cam1 = get(j, "HT1")
+ T_c1_c0 = rt2mat(cam1["Rt"])
+ T_c0_c1 = inv(T_c1_c0)
+ T_i_c1 = T_i_c0 @ T_c0_c1
+ T = T_i_c1
+ else:
+ assert False
+
+ q = rmat2quat(T[0:3, 0:3])
+ p = T[0:3, 3]
+ return {
+ "px": p[0],
+ "py": p[1],
+ "pz": p[2],
+ "qx": q[0],
+ "qy": q[1],
+ "qz": q[2],
+ "qw": q[3],
+ }
+
+
+def resolution(j, cam):
+ camera = get(j, cam)
+ width = camera["SensorWidth"]
+ height = camera["SensorHeight"]
+ return [width, height]
+
+
+def intrinsics(j, cam):
+ # https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/2feb3425259bf803749065bb6d628c6c180f8e77/include/k4a/k4atypes.h#L1024-L1046
+ camera = get(j, cam)
+ model_params = camera["Intrinsics"]["ModelParameters"]
+ assert (
+ camera["Intrinsics"]["ModelType"]
+ == "CALIBRATION_LensDistortionModelRational6KT"
+ )
+ width = camera["SensorWidth"]
+ height = camera["SensorHeight"]
+ return {
+ "camera_type": "pinhole-radtan8",
+ "intrinsics": {
+ "fx": model_params[2] * width,
+ "fy": model_params[3] * height,
+ "cx": model_params[0] * width,
+ "cy": model_params[1] * height,
+ "k1": model_params[4],
+ "k2": model_params[5],
+ "p1": model_params[13],
+ "p2": model_params[12],
+ "k3": model_params[6],
+ "k4": model_params[7],
+ "k5": model_params[8],
+ "k6": model_params[9],
+ },
+ }
+
+
+def calib_accel_bias(j):
+ # https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/2feb3425259bf803749065bb6d628c6c180f8e77/include/k4ainternal/calibration.h#L48-L77
+ # https://vladyslavusenko.gitlab.io/basalt-headers/classbasalt_1_1CalibAccelBias.html#details
+ # https://gitlab.com/VladyslavUsenko/basalt-headers/-/issues/8
+ accel = get(j, "Accelerometer")
+ bias = accel["BiasTemperatureModel"]
+ align = accel["MixingMatrixTemperatureModel"]
+ return [
+ -bias[0 * 4],
+ -bias[1 * 4],
+ -bias[2 * 4],
+ align[0 * 4] - 1, # [0, 0]
+ align[3 * 4], # [1, 0]
+ align[6 * 4], # [2, 0]
+ align[4 * 4] - 1, # [1, 1]
+ align[7 * 4], # [2, 1]
+ align[8 * 4] - 1, # [2, 2]
+ ]
+
+
+def calib_gyro_bias(j):
+ # https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/2feb3425259bf803749065bb6d628c6c180f8e77/include/k4ainternal/calibration.h#L48-L77
+ # https://vladyslavusenko.gitlab.io/basalt-headers/classbasalt_1_1CalibGyroBias.html#details
+ gyro = get(j, "Gyro")
+ bias = gyro["BiasTemperatureModel"]
+ align = gyro["MixingMatrixTemperatureModel"]
+ return [
+ -bias[0 * 4],
+ -bias[1 * 4],
+ -bias[2 * 4],
+ align[0 * 4] - 1, # [0, 0]
+ align[3 * 4], # [1, 0]
+ align[6 * 4], # [2, 0]
+ align[1 * 4], # [0, 1]
+ align[4 * 4] - 1, # [1, 1]
+ align[7 * 4], # [2, 1]
+ align[2 * 4], # [0, 2]
+ align[5 * 4], # [1, 2]
+ align[8 * 4] - 1, # [2, 2]
+ ]
+
+
+def noise_std(j, name):
+ imu = get(j, name)
+ return imu["Noise"][0:3]
+
+
+def bias_std(j, name):
+ imu = get(j, name)
+ return list(map(sqrt, imu["BiasUncertainty"]))
+
+
+def main():
+ parser = argparse.ArgumentParser()
+ parser.add_argument("wmr_json_file", help="Input WMR json calibration file")
+ args = parser.parse_args()
+ in_fn = args.wmr_json_file
+
+ with open(in_fn) as f:
+ j = json.load(f)
+
+ # We get 250 packets with 4 samples each per second, totalling 1000 samples per second.
+ # But in monado we just average those 4 samples to reduce the noise. So we have 250hz.
+ IMU_UPDATE_RATE = 250
+
+ # This is a very rough offset in pixels between the two cameras. I manually
+ # measured it for some particular point in some particular pair of images
+ # for my Odyssey+. In reality this offset changes based on distance to the
+ # point, nonetheless it helps to get some features tracked in the right
+ # camera.
+ VIEW_OFFSET = 247
+
+ out_calib = {
+ "value0": {
+ "T_imu_cam": [extrinsics(j, "HT0"), extrinsics(j, "HT1")],
+ "intrinsics": [intrinsics(j, "HT0"), intrinsics(j, "HT1")],
+ "resolution": [resolution(j, "HT0"), resolution(j, "HT1")],
+ "calib_accel_bias": calib_accel_bias(j),
+ "calib_gyro_bias": calib_gyro_bias(j),
+ "imu_update_rate": IMU_UPDATE_RATE,
+ "accel_noise_std": noise_std(j, "Accelerometer"),
+ "gyro_noise_std": noise_std(j, "Gyro"),
+ "accel_bias_std": bias_std(j, "Accelerometer"),
+ "gyro_bias_std": bias_std(j, "Gyro"),
+ "cam_time_offset_ns": 0,
+ "view_offset": VIEW_OFFSET,
+ "vignette": [],
+ }
+ }
+
+ print(json.dumps(out_calib, indent=4))
+
+
+if __name__ == "__main__":
+ main()
diff --git a/doc/monado/Realsense.md b/doc/monado/Realsense.md
new file mode 100644
index 0000000..49f8568
--- /dev/null
+++ b/doc/monado/Realsense.md
@@ -0,0 +1,137 @@
+# Using a RealSense Camera
+
+After making sure that everything works by running the EuRoC datasets, it should
+be possible to use the `realsense` driver from Monado to get any RealSense
+camera that has an IMU and one or more cameras to get tracked with SLAM.
+However, this was only tested on a D455, so if you are having problems with
+another device, please open an issue. Also, open an issue if you manage to make
+it work with other devices so that I can add it to this README.
+
+## Index
+
+- [Using a RealSense Camera](#using-a-realsense-camera)
+ - [Index](#index)
+ - [Overview of the Setup (D455)](#overview-of-the-setup-d455)
+ - [SLAM-Tracked RealSense Driver](#slam-tracked-realsense-driver)
+ - [RealSense-Tracked Qwerty Driver](#realsense-tracked-qwerty-driver)
+ - [Non-D455 RealSense Devices](#non-d455-realsense-devices)
+ - [Configuring the RealSense Pipeline](#configuring-the-realsense-pipeline)
+ - [Configuring Basalt](#configuring-basalt)
+
+## Overview of the Setup (D455)
+
+Let's first assume you have a RealSense D455, which is the one that works with
+the defaults. Even if you have another RealSense device follow this section, you
+might at least get something working, although not at its best.
+
+### SLAM-Tracked RealSense Driver
+
+Set these environment variables:
+
+- `export RS_HDEV_LOG=debug`: Make our realsense device logs more verbose
+- `export RS_SOURCE_INDEX=0`: Indicate that we want to use the first RealSense device connected as data source
+- `export RS_TRACKING=2`: Only try to use "host-slam". See other options
+ [here](https://gitlab.freedesktop.org/mateosss/monado/-/blob/64e70e76ad6d47e4bd1a0dfa164bff8597a50ce8/src/xrt/drivers/realsense/rs_prober.c#L33-39).
+- `export SLAM_CONFIG=$bsltdeps/basalt/data/monado/d455.toml`:
+ Configuration file for Basalt and the D455.
+
+### RealSense-Tracked Qwerty Driver
+
+You now have a RealSense device that you can use to track another device, for
+example, let's track a keyboard-and-mouse controlled HMD provided by the
+`qwerty` driver.
+
+Set these environment variables to enable the qwerty driver and stream to the
+SLAM system on start:
+
+```bash
+export QWERTY_ENABLE=true QWERTY_COMBINE=true SLAM_SUBMIT_FROM_START=true
+```
+
+And then modify your tracking overrides in your monado configuration file
+(`~/.config/monado/config_v0.json`) by updating the json object with:
+
+```js
+{
+ "tracking": {
+ "tracking_overrides": [
+ {
+ "target_device_serial": "Qwerty HMD", // Or "Qwerty Left Controller"
+ "tracker_device_serial": "Intel RealSense Host-SLAM",
+ "type": "direct",
+ "offset": {
+ "orientation": { "x": 0, "y": 0, "z": 0, "w": 1 },
+ "position": { "x": 0, "y": 0, "z": 0 }
+ },
+ "xrt_input_name": "XRT_INPUT_GENERIC_TRACKER_POSE"
+ }
+ ],
+ }
+}
+```
+
+And that's it! You can now start an OpenXR application with Monado and get your
+view tracked with your D455 camera.
+
+## Non-D455 RealSense Devices
+
+While I was unable to test other devices because I don't have access to them, it
+should be possible to make them work by:
+
+### Configuring the RealSense Pipeline
+
+[These
+fields](https://gitlab.freedesktop.org/mateosss/monado/-/blob/9e1b7e2203ef49abb939cc8fc92afa16fcc9cb3a/src/xrt/drivers/realsense/rs_hdev.c#L118-129)
+determine your RealSense streaming configuration, and
+[these](https://gitlab.freedesktop.org/mateosss/monado/-/blob/b26a6023226a4623381215fc159da3b4bcb27c9b/src/xrt/drivers/realsense/rs_hdev.c#L47-61)
+are their current defaults that work on a D455. You can change those fields by
+setting any of them in your `config_v0.json` inside a `config_realsense_hdev`
+field. Also note that as we already set `RS_HDEV_LOG=debug`, you should see the
+values they are currently taking at the start of Monado.
+
+For example, let's say you have a realsense device which has two fisheye cameras
+that support streaming 640x360 at 30fps (a T265 I think), then a configuration
+like this should work:
+
+```js
+"config_realsense_hdev": {
+ "stereo": true,
+ "video_format": 9, // 9 gets casted to RS2_FORMAT_Y8 (see https://git.io/Jzkfw), grayscale
+ "video_width": 640, // I am assuming the T265 supports 640x360 streams at 30fps
+ "video_height": 360,
+ "video_fps": 30,
+ "gyro_fps": 0, // 0 indicates any
+ "accel_fps": 0,
+ "stream_type": 4, // 4 gets casted to RS2_STREAM_FISHEYE (see https://git.io/Jzkvq)
+ "stream1_index": -1, // If there were more than one possible stream with these properties select them, -1 is for auto
+ "stream2_index": -1,
+}
+```
+
+The particular values you could set here are very dependent on your camera. I
+recommend seeing the values that get output by running the [rs-sensor-control
+example](https://dev.intelrealsense.com/docs/rs-sensor-control) from the
+RealSense API.
+
+### Configuring Basalt
+
+As you might've noticed, we set `SLAM_CONFIG` to
+`$bsltdeps/basalt/data/monado/d455.toml` which is [this](data/monado/d455.toml)
+config file that I added for the D455. This file points to a [calibration
+file](data/d455_calib.json) and a [VIO configuration
+file](data/euroc_config.json).
+
+For the tracking to be as good as possible you should set the
+intrinsics/extrinsics of the device in a similar calibration file and point to
+it with the `SLAM_CONFIG` config file. You can obtain that information from the
+previously mentioned
+[rs-sensor-control](https://dev.intelrealsense.com/docs/rs-sensor-control)
+utility. Issues like [this
+(T265)](https://gitlab.com/VladyslavUsenko/basalt/-/issues/52) and [this
+(D435)](https://gitlab.com/VladyslavUsenko/basalt/-/issues/50) provide
+configuration files tried by other users. Additionally Basalt provides custom
+[calibration
+tools](https://gitlab.com/VladyslavUsenko/basalt/-/blob/master/doc/Calibration.md)
+that can work for any camera-IMU setup or tools like
+[`basalt_rs_t265_record`](https://gitlab.freedesktop.org/mateosss/basalt/-/blob/5a365bf6fb14ce5b044b76f742337e1d6865557e/src/rs_t265_record.cpp#L207) or the `euroc_recorder` in Monado
+that can help creating an initial calibration file for RealSense devices.
diff --git a/doc/monado/WMR.md b/doc/monado/WMR.md
new file mode 100644
index 0000000..8db0d40
--- /dev/null
+++ b/doc/monado/WMR.md
@@ -0,0 +1,39 @@
+# Windows Mixed Reality Headsets
+
+We'll need to make a Basalt config file for your headset, let's say it's a
+Reverb G2.
+
+First, let's get your WMR device json config block. To get that json, set the
+environment variable `WMR_LOG=debug` and run Monado with your WMR headset connected.
+The headset json is printed on start after the line `DEBUG [wmr_read_config] JSON config:`.
+Copy that to a file called `reverbg2_wmrcalib.json`.
+
+Now let's convert this WMR json to a Basalt calibration file with:
+
+```bash
+$bsltdeps/basalt/data/monado/wmr-tools/wmr2bslt_calib.py reverbg2_wmrcalib.json > $bsltdeps/basalt/data/reverbg2_calib.json
+```
+
+Finally, we'll need to create the main config file for Basalt that references
+this calibration file we just created. For that let's copy the config that is
+already present for the Odyssey+:
+
+```bash
+cp $bsltdeps/basalt/data/monado/odysseyplus_rt8.toml $bsltdeps/basalt/data/monado/reverbg2.toml
+```
+
+And edit the `cam-calib` field in the `reverbg2.toml` file to point to your `reverbg2_calib.json` file.
+
+And that's it, now you just need to reference this `reverbg2.toml` in the
+`SLAM_CONFIG` environment variable before launching Monado with `export
+SLAM_CONFIG=$bsltdeps/basalt/data/monado/reverbg2.toml` and Basalt will use the
+appropriate calibration for your headset.
+
+By default, the UI box `SLAM Tracker` has the option `Submit data to SLAM`
+disabled so that you first manually configure the exposure and gain values in
+the `WMR Camera` box. You can enable it yourself in the UI or enable it at start
+by setting the environment variable `SLAM_SUBMIT_FROM_START=true`.
+
+# Video Walkthrough
+
+Here is a 15 minute walkthrough with some tips for using a WMR headset with Monado and Basalt that should help complement the guide found in the [README.md](README.md) file: