From c9ab4a3a7bceadc9c0aa00d66f535a42efc3adae Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Tue, 1 Oct 2019 14:40:10 +0200 Subject: [PATCH 1/7] small fixes for tutorial --- doc/Realsense.md | 8 ++++---- src/calibration/cam_calib.cpp | 2 ++ src/time_alignment.cpp | 32 ++++---------------------------- 3 files changed, 10 insertions(+), 32 deletions(-) diff --git a/doc/Realsense.md b/doc/Realsense.md index 8d141c3..b75ce3d 100644 --- a/doc/Realsense.md +++ b/doc/Realsense.md @@ -101,7 +101,7 @@ For the motion capture recording we use [ros_vrpn_client](https://github.com/eth **Important for recording the dataset:** * Set the `skip_frames` slider to 1 to use the full framerate. * Reduce the exposure time to reduce the motion blur. -* Move the setup such that all axis of accelerometer and gyro are exited. This means moving with acceleration along X, Y and Z axis and rotating around those axes. +* Move the setup such that all axes of accelerometer and gyro are excited. This means moving with acceleration along X, Y and Z axes and rotating around those axes. * Do not forget to simultaneously record motion capture data. * Rename the dataset to `imu_calib`. @@ -131,9 +131,9 @@ To perform the calibration follow these steps: ## Generating Time-Aligned Ground Truth Since motion capture system and the PC where the dataset was recorded might not have the same clock we need to perform the time synchronization. Additionally we need to transform the coordinate frame of the GT data to the IMU frame (originally it is in the coordinate frame attached to the markers). -The raw motion capture data is stored in the `mav/mocap0/` folder. We can find the time offset by minimizing the error between gyro measurements and rotational velocities computed from the motion capture data. The resulting trajectory (time aligned and transformed to the IMU frame) will be written to `mav/gt/` and automatically loaded when available. +The raw motion capture data is stored in the `mav/mocap0/` folder. We can find the time offset by minimizing the error between gyro measurements and rotational velocities computed from the motion capture data. If you press the `save_aligned_dataset` button the resulting trajectory (time aligned and transformed to the IMU frame) will be written to `mav/gt/data.csv` and automatically loaded when available. ``` -basalt_time_alignment --dataset-path ~/t265_calib_data/sequence0 --dataset-type euroc --calibration ~/t265_calib_results/calibration.json --mocap-calibration ~/t265_calib_results/mocap_calibration.json --save-gt +basalt_time_alignment --dataset-path ~/t265_calib_data/sequence0 --dataset-type euroc --calibration ~/t265_calib_results/calibration.json --mocap-calibration ~/t265_calib_results/mocap_calibration.json ``` You should be able to see that, despite some noise, rotational velocity computed from the motion capture data aligns well with gyro measurements. ![t265_time_align_gyro](/doc/img/t265_time_align_gyro.png) @@ -151,7 +151,7 @@ After the system processes the whole sequence you can use `align_se3` button to ## Running Visual-Inertial Odometry Live -It is also possible to run the odometry live with the camera If no calibration files are provided the factory calibration will be used. +It is also possible to run the odometry live with the camera. If no calibration files are provided the factory calibration will be used. ``` basalt_rs_t265_vio --cam-calib ~/t265_calib_results/calibration.json --config-path /usr/etc/basalt/euroc_config.json ``` \ No newline at end of file diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index b6dad47..3822cb0 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -229,6 +229,8 @@ void CamCalib::computeVign() { ve.save_vign_png(cache_path); calib_opt->setVignette(ve.get_vign_param()); + + std::cout << "Saved vignette png files to " << cache_path << std::endl; } void CamCalib::setNumCameras(size_t n) { diff --git a/src/time_alignment.cpp b/src/time_alignment.cpp index bea3823..0008b53 100644 --- a/src/time_alignment.cpp +++ b/src/time_alignment.cpp @@ -66,8 +66,6 @@ int main(int argc, char **argv) { std::string output_gyro_path; std::string output_mocap_path; - bool save_new_gt = false; - double max_offset_s = 10.0; bool show_gui = true; @@ -97,8 +95,6 @@ int main(int argc, char **argv) { "Maximum offset for a grid search in seconds."); app.add_flag("--show-gui", show_gui, "Show GUI for debugging"); - app.add_flag("--save-gt", save_new_gt, - "Save time aligned data to mav0/gt/ folder"); try { app.parse(argc, argv); @@ -106,6 +102,10 @@ int main(int argc, char **argv) { return app.exit(e); } + if (!dataset_path.empty() && dataset_path[dataset_path.length() - 1] != '/') { + dataset_path += '/'; + } + basalt::VioDatasetPtr vio_dataset; const bool use_calib = @@ -355,30 +355,6 @@ int main(int argc, char **argv) { } } - if (save_new_gt) { - fs::create_directory(dataset_path + "/mav0/gt/"); - - std::ofstream f(dataset_path + "/mav0/gt/data.csv"); - - for (size_t i = 0; i < vio_dataset->get_gt_timestamps().size(); i++) { - const int64_t corrected_time = vio_dataset->get_gt_timestamps()[i] + - vio_dataset->get_mocap_to_imu_offset_ns() + - best_offset_refined_ns; - - if (corrected_time >= min_time && corrected_time <= max_time) { - const Sophus::SE3d &p = vio_dataset->get_gt_pose_data()[i]; - - f << corrected_time << ',' << p.translation().x() << ',' - << p.translation().y() << ',' << p.translation().z() << ',' - << p.unit_quaternion().w() << ',' << p.unit_quaternion().x() << ',' - << p.unit_quaternion().y() << ',' << p.unit_quaternion().z() - << std::endl; - } - } - - f.close(); - } - if (show_gui) { static constexpr int UI_WIDTH = 280; From fbda5bb955e3c34f0f22a87b168887379506c779 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Tue, 1 Oct 2019 17:06:26 +0200 Subject: [PATCH 2/7] fix t265 tutorial --- doc/Realsense.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/Realsense.md b/doc/Realsense.md index b75ce3d..71038d9 100644 --- a/doc/Realsense.md +++ b/doc/Realsense.md @@ -141,6 +141,8 @@ You should be able to see that, despite some noise, rotational velocity computed You can also switch to the error function plot and see that there is a clear minimum corresponding to the computed time offset. ![t265_time_align_error](/doc/img/t265_time_align_error.png) +**Note:** If you want to run the time alignment again you should delete the `~/t265_calib_data/sequence0/mav/gt` folder first. + ## Running Visual-Inertial Odometry Now we can run the visual-inertial odometry on the recorded dataset: ``` From 275e4ceb6669162189f80659a9185c0e064831db Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Tue, 1 Oct 2019 17:22:42 +0200 Subject: [PATCH 3/7] fix t265 tutorial --- README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 527e638..e8ba4d8 100644 --- a/README.md +++ b/README.md @@ -57,11 +57,13 @@ NOTE: It is possible to compile the code on Ubuntu 16.04, but you need to instal ## Usage * [Camera, IMU and Mocap calibration. (TUM-VI, Euroc, UZH-FPV and Kalibr datasets)](doc/Calibration.md) -* [Tutorial on Camera-IMU and Motion capture calibration with Realsense T265.](doc/Realsense.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) +## Device support +* [Tutorial on Camera-IMU and Motion capture calibration with Realsense T265.](doc/Realsense.md) + ## Development * [Development environment setup.](doc/DevSetup.md) From 1205a9d40c87f7517f65c1afa0a5be87ac75285b Mon Sep 17 00:00:00 2001 From: Nikolaus Demmel Date: Tue, 1 Oct 2019 17:11:34 +0000 Subject: [PATCH 4/7] Fix typo in Realsense.md --- doc/Realsense.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/Realsense.md b/doc/Realsense.md index 71038d9..9bae79f 100644 --- a/doc/Realsense.md +++ b/doc/Realsense.md @@ -84,7 +84,7 @@ To perform the calibration follow these steps: * `init_cam_extr` initialize transformations between multiple cameras. * `init_opt` initialize optimization. * `opt_until_converge` optimize until convergence. -* `opt_cam_poses` some initial poses computed from the initialized intrinsics can be far from optimum and not converge to the right minimum. To improve the final result we can re-initialize poses with optimized intrinsics. +* `init_cam_poses` some initial poses computed from the initialized intrinsics can be far from optimum and not converge to the right minimum. To improve the final result we can re-initialize poses with optimized intrinsics. * `init_opt` initialize optimization with new initial poses. * `opt_until_converge` optimize until convergence. * `compute_vign` after optimizing geometric models compute the vignetting of the cameras. From 255604c72e9ca244976b6f842992df90c0430b57 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Tue, 1 Oct 2019 22:16:24 +0200 Subject: [PATCH 5/7] fix realsense tutorial --- doc/Realsense.md | 2 +- include/basalt/io/dataset_io.h | 3 +- include/basalt/io/dataset_io_euroc.h | 8 +++-- src/io/dataset_io.cpp | 4 +-- src/time_alignment.cpp | 49 +++++++++++++++++++++++++--- 5 files changed, 55 insertions(+), 11 deletions(-) diff --git a/doc/Realsense.md b/doc/Realsense.md index 9bae79f..802766d 100644 --- a/doc/Realsense.md +++ b/doc/Realsense.md @@ -141,7 +141,7 @@ You should be able to see that, despite some noise, rotational velocity computed You can also switch to the error function plot and see that there is a clear minimum corresponding to the computed time offset. ![t265_time_align_error](/doc/img/t265_time_align_error.png) -**Note:** If you want to run the time alignment again you should delete the `~/t265_calib_data/sequence0/mav/gt` folder first. +**Note:** If you want to run the time alignment again you should delete the `~/t265_calib_data/sequence0/mav/gt` folder first. If GT data already exist you will see the `save_aligned_dataset(disabled)` button which will **NOT** overwrite it. ## Running Visual-Inertial Odometry Now we can run the visual-inertial odometry on the recorded dataset: diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h index 4ef84c1..43b7fbb 100644 --- a/include/basalt/io/dataset_io.h +++ b/include/basalt/io/dataset_io.h @@ -148,7 +148,8 @@ typedef std::shared_ptr DatasetIoInterfacePtr; class DatasetIoFactory { public: - static DatasetIoInterfacePtr getDatasetIo(const std::string &dataset_type); + static DatasetIoInterfacePtr getDatasetIo(const std::string &dataset_type, + bool load_mocap_as_gt = false); }; } // namespace basalt diff --git a/include/basalt/io/dataset_io_euroc.h b/include/basalt/io/dataset_io_euroc.h index c66a980..91daa08 100644 --- a/include/basalt/io/dataset_io_euroc.h +++ b/include/basalt/io/dataset_io_euroc.h @@ -148,7 +148,7 @@ class EurocVioDataset : public VioDataset { class EurocIO : public DatasetIoInterface { public: - EurocIO() {} + EurocIO(bool load_mocap_as_gt) : load_mocap_as_gt(load_mocap_as_gt) {} void read(const std::string &path) { if (!fs::exists(path)) @@ -163,9 +163,10 @@ class EurocIO : public DatasetIoInterface { read_imu_data(path + "/mav0/imu0/"); - if (file_exists(path + "/mav0/state_groundtruth_estimate0/data.csv")) { + if (!load_mocap_as_gt && + file_exists(path + "/mav0/state_groundtruth_estimate0/data.csv")) { read_gt_data_state(path + "/mav0/state_groundtruth_estimate0/"); - } else if (file_exists(path + "/mav0/gt/data.csv")) { + } else if (!load_mocap_as_gt && file_exists(path + "/mav0/gt/data.csv")) { read_gt_data_pose(path + "/mav0/gt/"); } else if (file_exists(path + "/mav0/mocap0/data.csv")) { read_gt_data_pose(path + "/mav0/mocap0/"); @@ -304,6 +305,7 @@ class EurocIO : public DatasetIoInterface { } std::shared_ptr data; + bool load_mocap_as_gt; }; // namespace basalt } // namespace basalt diff --git a/src/io/dataset_io.cpp b/src/io/dataset_io.cpp index 2c5d789..31a3cca 100644 --- a/src/io/dataset_io.cpp +++ b/src/io/dataset_io.cpp @@ -42,10 +42,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace basalt { DatasetIoInterfacePtr DatasetIoFactory::getDatasetIo( - const std::string &dataset_type) { + const std::string &dataset_type, bool load_mocap_as_gt) { if (dataset_type == "euroc") { // return DatasetIoInterfacePtr(); - return DatasetIoInterfacePtr(new EurocIO); + return DatasetIoInterfacePtr(new EurocIO(load_mocap_as_gt)); } else if (dataset_type == "bag") { return DatasetIoInterfacePtr(new RosbagIO); } else if (dataset_type == "uzh") { diff --git a/src/time_alignment.cpp b/src/time_alignment.cpp index 0008b53..5ec0028 100644 --- a/src/time_alignment.cpp +++ b/src/time_alignment.cpp @@ -1,3 +1,37 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko, Michael Loipführer and Nikolaus Demmel. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* 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. + +* 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. +*/ #include #include @@ -137,7 +171,7 @@ int main(int argc, char **argv) { } basalt::DatasetIoInterfacePtr dataset_io = - basalt::DatasetIoFactory::getDatasetIo(dataset_type); + basalt::DatasetIoFactory::getDatasetIo(dataset_type, true); dataset_io->read(dataset_path); vio_dataset = dataset_io->get_data(); @@ -380,11 +414,18 @@ int main(int argc, char **argv) { pangolin::Var show_error("ui.show_error", false, false, true); + std::string save_button_name = "ui.save_aligned_dataset"; + // Disable save_aligned_dataset button if GT data already exists + if (fs::exists(fs::path(dataset_path + "mav0/gt/data.csv"))) { + save_button_name += "(disabled)"; + } + pangolin::Var> save_aligned_dataset( - "ui.save_aligned_dataset", [&]() { + save_button_name, [&]() { if (fs::exists(fs::path(dataset_path + "mav0/gt/data.csv"))) { - std::cout << "Aligned grount truth data already exists, skipping." - << std::endl; + std::cout << "Aligned ground-truth data already exists, skipping. " + "If you want to run the calibration again delete " + << dataset_path << "mav0/gt/ folder." << std::endl; return; } std::cout << "Saving aligned dataset in " From 61d9e67d485530be6a11013cea9378d5148d5a3d Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 2 Oct 2019 16:33:09 +0200 Subject: [PATCH 6/7] fix t265 tutorial --- doc/Realsense.md | 6 +++--- thirdparty/basalt-headers | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/doc/Realsense.md b/doc/Realsense.md index 71038d9..3aa7f6d 100644 --- a/doc/Realsense.md +++ b/doc/Realsense.md @@ -27,7 +27,7 @@ basalt_rs_t265_record --dataset-path ~/t265_calib_data/ --manual-exposure * `--dataset-path` specifies the location where the recorded dataset will be stored. In this case it will be stored in `~/t265_calib_data//`. * `--manual-exposure` disables the autoexposure. In this tutorial the autoexposure is disabled for all calibration sequences, but for the VIO sequence (sequence0) we enable it. -![t265_record](doc/img/t265_record.png) +![t265_record](/doc/img/t265_record.png) The GUI elements have the following meaning: * `webp_quality` compression quality. The highest value (101) means lossless compression. For photometric calibration it is important not to have any compression artifacts, so we record these calibration sequences with lossless compression. @@ -59,7 +59,7 @@ Run the response function calibration: basalt_response_calib.py -d ~/t265_calib_data/response_calib ``` You should see the response function and the irradiance image similar to the one shown below. For the details of the algorithm see Section 2.3.1 of [[arXiv:1607.02555]](https://arxiv.org/abs/1607.02555). The results suggest that the response function used in the camera is linear. -![t265_inv_resp_irradiance](doc/img/t265_inv_resp_irradiance.png) +![t265_inv_resp_irradiance](/doc/img/t265_inv_resp_irradiance.png) ## Multi-Camera Geometric and Vignette Calibration For the camera calibration we need to record a dataset with a static aprilgrid pattern. @@ -96,7 +96,7 @@ To perform the calibration follow these steps: ## IMU and Motion Capture Calibration After calibrating cameras we can proceed to geometric and time calibration of the cameras, IMU and motion capture system. Setting up the motion capture system is specific for your setup. -For the motion capture recording we use [ros_vrpn_client](https://github.com/ethz-asl/ros_vrpn_client) with [basalt_capture_mocap.py](scripts/basalt_capture_mocap.py). We record the data to the `mocap0` folder and then move it to the `mav0` directory of the camera dataset. This script is provided as an example. Motion capture setup is different in every particular case. +For the motion capture recording we use [ros_vrpn_client](https://github.com/ethz-asl/ros_vrpn_client) with [basalt_capture_mocap.py](/scripts/basalt_capture_mocap.py). We record the data to the `mocap0` folder and then move it to the `mav0` directory of the camera dataset. This script is provided as an example. Motion capture setup is different in every particular case. **Important for recording the dataset:** * Set the `skip_frames` slider to 1 to use the full framerate. diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index ee71c01..ff561d2 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit ee71c0172400419853f675d385ee7e06242facaf +Subproject commit ff561d2369d8dd159ff1e7316e451bec41544ebe From d8560c555071f381d97f16aad1a9927c1e9e419d Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Wed, 2 Oct 2019 16:34:43 +0200 Subject: [PATCH 7/7] fix t265 tutorial --- doc/Realsense.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/Realsense.md b/doc/Realsense.md index 5a2961b..e1b5647 100644 --- a/doc/Realsense.md +++ b/doc/Realsense.md @@ -1,6 +1,6 @@ # Tutorial on Camera-IMU and Motion Capture Calibration with Realsense T265 -![Realsense](doc/img/realsense_setup.jpg) +![Realsense](/doc/img/realsense_setup.jpg) In this tutorial we explain how to perform photometric and geometric calibration of the multi-camera setup and then calibrate the transformations between cameras, IMU and the motion capture marker setup. To make sure the calibration is successful we recommend to rigidly attach markers to the camera as shown on the figure above.