From e36bead967ebaaf1f04b0af5a0c224778da970c8 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Fri, 27 May 2022 14:13:37 +0000 Subject: [PATCH] Function without config file This makes WMR headset tracking work out of the box. --- README.md | 5 +- data/reverbg1_calib.json | 2 +- doc/monado/WMR.md | 76 +++++++++++++++++++++--- src/monado/slam_tracker.cpp | 92 ++++++++++++++++++++++++------ src/utils/vio_config.cpp | 12 ++-- thirdparty/monado/slam_tracker.hpp | 27 +++++---- 6 files changed, 171 insertions(+), 43 deletions(-) diff --git a/README.md b/README.md index b020290..abc185a 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,8 @@ in order to get it working in different ones. ## Installation 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. +steps don't work for you. The main branch of this fork is +[`xrtslam`](https://gitlab.freedesktop.org/mateosss/basalt/-/tree/xrtslam). ### Build and Install Directories @@ -106,7 +107,7 @@ This step is optional but you can try Basalt without Monado with one of the foll ### 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): +same flags. For that, set these with CMake: `-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 diff --git a/data/reverbg1_calib.json b/data/reverbg1_calib.json index 70b485a..94bcd6c 100644 --- a/data/reverbg1_calib.json +++ b/data/reverbg1_calib.json @@ -115,7 +115,7 @@ 0.009999999873689375 ], "cam_time_offset_ns": 0, - "view_offset": [247, 0], + "view_offset": [0, 0], "vignette": [] } } diff --git a/doc/monado/WMR.md b/doc/monado/WMR.md index 8db0d40..7a11cba 100644 --- a/doc/monado/WMR.md +++ b/doc/monado/WMR.md @@ -1,6 +1,22 @@ # Windows Mixed Reality Headsets -We'll need to make a Basalt config file for your headset, let's say it's a +Monado should work out of the box with WMR devices and Basalt without any input +on your part. So if you successfully followed the guide in the main README and +Monado detects your WMR headset, then tracking should already be working for you. + +If it does not work, double check that you read everything in the guide, from +top to bottom. + +If it still doesn't work, triple check it. + +Now if you are still experiencing issues, crashes or would like to debug the +pipeline for whatever reason, the rest of this document should help you with +that. + +## Making a custom Basalt config file + +It's a good idea to make a Basalt config file for your headset so that you can +easily tweak it if needed. Let's say we are trying to make a config file for a Reverb G2. First, let's get your WMR device json config block. To get that json, set the @@ -22,18 +38,62 @@ already present for the Odyssey+: 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 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. +That's it! now you have a Basalt config file that you can use for your headset. + +## Set Monado options + +Let's set a couple environment variables in Monado that will help us debug the +SLAM pipeline. + +- `SLAM_CONFIG=$bsltdeps/basalt/data/monado/reverbg2.toml`: Tell Monado where + the Basalt `toml` config you just created is. Notice that the `show-gui` + property is enabled in this `toml` file so you will start seeing the Basalt + visualizer when opening Monado. Furthermore the `config-path` key points to a + Basalt specific config file for tweaking the VIO pipeline. + +- `OXR_DEBUG_GUI=on`: Enable Monado's own debug GUI. + +- `SLAM_SUBMIT_FROM_START=off`: Do not send frames to Basalt from the start, + rather wait until we check the checkbox in the Monado GUI box called "SLAM + Tracker". + +- `WMR_AUTOEXPOSURE=off`: Disable autoexposure to have one less moving part, we + will manually adjust it instead on the "WMR Camera" box, by moving the + "Brightness" slider on the "Auto exposure and gain control" section. + +## Controling auto exposure 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 +## Recalibrating my device (TODO) -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: +It's not a bad idea to recalibrate your headset manually with the tools Basalt provides. + +TODO: Specify better the steps, but roughly they would be: + +1. Get calibration target from Kalibr: https://github.com/ethz-asl/kalibr/wiki/downloads +2. Open the pdf in a flat monitor, measure dimensions with a ruler and put them on aprilgrid_6x6.json +3. Record an EuRoC dataset from Monado in which you move the headset around the target (link an example sequence) +4. Run + [basalt_calibrate](https://gitlab.com/VladyslavUsenko/basalt/-/blob/master/doc/Calibration.md#camera-calibration) + on + [euroc](https://gitlab.com/VladyslavUsenko/basalt/-/blob/master/doc/Calibration.md#euroc-dataset). +5. Run + [basalt_calibrate_vio](https://gitlab.com/VladyslavUsenko/basalt/-/blob/master/doc/Calibration.md#camera-imu-mocap-calibration) + on + [euroc](https://gitlab.com/VladyslavUsenko/basalt/-/blob/master/doc/Calibration.md#camera-imu-calibration). + +# Video Walkthrough (DEPRECATED) + +_This video is not up to date anymore but might be useful to see how things +worked before. Now, `view_offset` is automatically computed, exposure and gain +are automatically set too, so in general there is no manual input needed from +the user._ + +~~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: ~~ diff --git a/src/monado/slam_tracker.cpp b/src/monado/slam_tracker.cpp index 8a1bf4a..b918732 100644 --- a/src/monado/slam_tracker.cpp +++ b/src/monado/slam_tracker.cpp @@ -68,7 +68,7 @@ struct slam_tracker::implementation { private: // Options parsed from unified config file - bool show_gui = true; + bool show_gui = false; string cam_calib_path; string config_path; string marg_data_path; @@ -83,6 +83,11 @@ struct slam_tracker::implementation { static constexpr int NUM_CAMS = 2; // VIO members + struct { + bool imu = false; + bool cam0 = false; + bool cam1 = false; + } calib_data_ready; Calibration calib; VioConfig vio_config; OpticalFlowBase::Ptr opt_flow_ptr; @@ -116,6 +121,11 @@ struct slam_tracker::implementation { public: implementation(const string &unified_config) { + if (unified_config == "DEFAULT") { + // For the pipeline to work now, the user will need to use add_cam/imu_calibration + return; + } + load_unified_config(unified_config); vio_config.load(config_path); @@ -156,6 +166,7 @@ struct slam_tracker::implementation { cereal::JSONInputArchive archive(os); archive(calib); cout << "Loaded camera with " << calib.intrinsics.size() << " cameras\n"; + calib_data_ready.imu = calib_data_ready.cam0 = calib_data_ready.cam1 = true; } else { std::cerr << "could not load camera calibration " << calib_path << "\n"; std::abort(); @@ -216,6 +227,13 @@ struct slam_tracker::implementation { cout << "Finished queues_printer\n"; } + void print_calibration() { + std::stringstream ss{}; + cereal::JSONOutputArchive write_to_stream(ss); + write_to_stream(calib); + cout << "Calibration: " << ss.str() << "\n"; + } + public: void initialize() { // Overwrite camera calibration data @@ -223,11 +241,22 @@ struct slam_tracker::implementation { apply_cam_calibration(c); } + bool calib_from_monado = added_cam_calibs.size() == 2; + bool view_offset_unknown = calib.view_offset(0) == 0 && calib.view_offset(1) == 0; + if (calib_from_monado || view_offset_unknown) { + compute_view_offset(); + cout << "Computed view_offset = " << calib.view_offset.transpose() << "\n"; + } + // Overwrite IMU calibration data for (const auto &c : added_imu_calibs) { apply_imu_calibration(c); } + ASSERT(calib_data_ready.imu, "Missing IMU calibration"); + ASSERT(calib_data_ready.cam0, "Missing left camera (cam0) calibration"); + ASSERT(calib_data_ready.cam1, "Missing right camera (cam1) calibration"); + // NOTE: This factory also starts the optical flow opt_flow_ptr = OpticalFlowFactory::getOpticalFlow(vio_config, calib); image_data_queue = &opt_flow_ptr->input_queue; @@ -362,39 +391,68 @@ struct slam_tracker::implementation { return true; } + void compute_view_offset() { + constexpr double DISTANCE_TO_WALL = 2; // In meters + double width = calib.resolution[0][0]; + double height = calib.resolution[0][1]; + Sophus::SE3d T_i_c0 = calib.T_i_c[0]; + Sophus::SE3d T_i_c1 = calib.T_i_c[1]; + Sophus::SE3d T_c1_i = T_i_c1.inverse(); + Sophus::SE3d T_c1_c0 = T_c1_i * T_i_c0; // Maps a point in c0 space to c1 space + Eigen::Vector4d p3d{0, 0, DISTANCE_TO_WALL, 1}; + Eigen::Vector4d p3d_in_c1 = T_c1_c0 * p3d; + Eigen::Vector2d p2d; + calib.intrinsics[1].project(p3d_in_c1, p2d); + calib.view_offset.x() = (width / 2) - p2d.x(); + calib.view_offset.y() = (height / 2) - p2d.y(); + } + void add_cam_calibration(const cam_calibration &cam_calib) { added_cam_calibs.push_back(cam_calib); } void apply_cam_calibration(const cam_calibration &cam_calib) { using Scalar = double; - int i = cam_calib.cam_index; + size_t i = cam_calib.cam_index; - const auto &tci = cam_calib.T_cam_imu; - Eigen::Matrix3d rci; - rci << tci(0, 0), tci(0, 1), tci(0, 2), tci(1, 0), tci(1, 1), tci(1, 2), tci(2, 0), tci(2, 1), tci(2, 2); - Eigen::Quaterniond q(rci); - Eigen::Vector3d p{tci(0, 3), tci(1, 3), tci(2, 3)}; - calib.T_i_c[i] = Calibration::SE3(q, p); + const auto &tic = cam_calib.t_imu_cam; + Eigen::Matrix3d ric; + ric << tic(0, 0), tic(0, 1), tic(0, 2), tic(1, 0), tic(1, 1), tic(1, 2), tic(2, 0), tic(2, 1), tic(2, 2); + Eigen::Quaterniond q(ric); + Eigen::Vector3d p{tic(0, 3), tic(1, 3), tic(2, 3)}; + ASSERT_(calib.T_i_c.size() == i); + calib.T_i_c.push_back(Calibration::SE3(q, p)); GenericCamera model; - const vector &cmp = cam_calib.model_params; - if (cam_calib.model == cam_calibration::cam_model::pinhole) { + const vector &d = cam_calib.distortion; + if (cam_calib.distortion_model == "none") { + ASSERT_(d.size() == 0); PinholeCamera::VecN mp; mp << cam_calib.fx, cam_calib.fy, cam_calib.cx, cam_calib.cy; PinholeCamera pinhole(mp); model.variant = pinhole; - } else if (cam_calib.model == cam_calibration::cam_model::fisheye) { + } else if (cam_calib.distortion_model == "kb4") { + ASSERT_(d.size() == 4); KannalaBrandtCamera4::VecN mp; - mp << cam_calib.fx, cam_calib.fy, cam_calib.cx, cam_calib.cy, cmp[0], cmp[1], cmp[2], cmp[3]; + mp << cam_calib.fx, cam_calib.fy, cam_calib.cx, cam_calib.cy, d[0], d[1], d[2], d[3]; KannalaBrandtCamera4 kannala_brandt(mp); model.variant = kannala_brandt; + } else if (cam_calib.distortion_model == "rt8") { + ASSERT_(d.size() == 9); // 8 and rpmax + PinholeRadtan8Camera::VecN mp; + mp << cam_calib.fx, cam_calib.fy, cam_calib.cx, cam_calib.cy, d[0], d[1], d[2], d[3], d[4], d[5], d[6], d[7]; + Scalar rpmax = d[8]; + PinholeRadtan8Camera pinhole_radtan8(mp, rpmax); + model.variant = pinhole_radtan8; } else { - ASSERT(false, "Unsupported camera model (%d)", static_cast(cam_calib.model)); + ASSERT(false, "Unsupported camera model (%s)", cam_calib.distortion_model.c_str()); } - calib.intrinsics[i] = model; + ASSERT_(calib.intrinsics.size() == i); + calib.intrinsics.push_back(model); - calib.resolution[i] = {cam_calib.width, cam_calib.height}; + ASSERT_(calib.resolution.size() == i); + calib.resolution.push_back({cam_calib.width, cam_calib.height}); - // NOTE: ignoring cam_calib.distortion_model and distortion_params as basalt can't use them + calib_data_ready.cam0 |= i == 0; + calib_data_ready.cam1 |= i == 1; } void add_imu_calibration(const imu_calibration &imu_calib) { added_imu_calibs.push_back(imu_calib); } @@ -448,6 +506,8 @@ struct slam_tracker::implementation { calib.gyro_noise_std = {gyro.noise_std(0), gyro.noise_std(1), gyro.noise_std(2)}; calib.gyro_bias_std = {gyro.bias_std(0), gyro.bias_std(1), gyro.bias_std(2)}; + + calib_data_ready.imu = true; } shared_ptr> enable_pose_ext_timing() { diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 0e554ed..21f3449 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -48,7 +48,7 @@ VioConfig::VioConfig() { // optical_flow_type = "patch"; optical_flow_type = "frame_to_frame"; optical_flow_detection_grid_size = 50; - optical_flow_max_recovered_dist2 = 0.09f; + optical_flow_max_recovered_dist2 = 0.04f; optical_flow_pattern = 51; optical_flow_max_iterations = 5; optical_flow_levels = 3; @@ -74,12 +74,14 @@ VioConfig::VioConfig() { vio_enforce_realtime = false; - vio_use_lm = false; + vio_use_lm = true; vio_lm_lambda_initial = 1e-4; vio_lm_lambda_min = 1e-6; vio_lm_lambda_max = 1e2; + // vio_lm_landmark_damping_variant = 1; + // vio_lm_pose_damping_variant = 1; - vio_scale_jacobian = true; + vio_scale_jacobian = false; vio_init_pose_weight = 1e8; vio_init_ba_weight = 1e1; @@ -104,9 +106,9 @@ VioConfig::VioConfig() { mapper_no_factor_weights = false; mapper_use_factors = true; - mapper_use_lm = false; + mapper_use_lm = true; mapper_lm_lambda_min = 1e-32; - mapper_lm_lambda_max = 1e2; + mapper_lm_lambda_max = 1e3; } void VioConfig::save(const std::string& filename) { diff --git a/thirdparty/monado/slam_tracker.hpp b/thirdparty/monado/slam_tracker.hpp index 71e4eb8..0274c65 100644 --- a/thirdparty/monado/slam_tracker.hpp +++ b/thirdparty/monado/slam_tracker.hpp @@ -27,7 +27,7 @@ namespace xrt::auxiliary::tracking::slam { // For implementation: same as IMPLEMENTATION_VERSION_* // For user: expected IMPLEMENTATION_VERSION_*. Should be checked in runtime. -constexpr int HEADER_VERSION_MAJOR = 2; //!< API Breakages +constexpr int HEADER_VERSION_MAJOR = 3; //!< API Breakages constexpr int HEADER_VERSION_MINOR = 0; //!< Backwards compatible API changes constexpr int HEADER_VERSION_PATCH = 0; //!< Backw. comp. .h-implemented changes @@ -185,17 +185,22 @@ private: constexpr int FID_##SHORT_NAME = ID; \ constexpr int F_##NAME = ID; +/*! + * Container of parameters for a pinhole camera calibration (fx, fy, cx, cy) + * with an optional distortion. + * + *`distortion_model` and its corresponding `distortion` parameters are not + * standardized in this struct to facilitate implementation prototyping. + */ struct cam_calibration { - enum class cam_model { pinhole, fisheye }; - int cam_index; //!< For multi-camera setups. For stereo 0 ~ left, 1 ~ right. - int width, height; // model_params; - cv::Matx T_cam_imu; //!< Transformation from camera to imu space + int width, height; // distortion; //!< Parameters for the distortion_model + cv::Matx t_imu_cam; //!< Transformation from IMU to camera }; struct inertial_calibration { @@ -204,7 +209,7 @@ struct inertial_calibration { //! This transform will be applied to raw measurements. cv::Matx transform; - //! Offset to apply to raw measurements to; called bias in other contexts. + //! Offset to add to raw measurements to; called bias in other contexts. cv::Matx offset; // Parameters for the random processes that model this IMU. See section "2.1