From 255604c72e9ca244976b6f842992df90c0430b57 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Tue, 1 Oct 2019 22:16:24 +0200 Subject: [PATCH] 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 "