Merge branch 'master' of gitlab.vision.in.tum.de:slam/basalt

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.
@@ -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:

include/basalt/io/dataset_io.h

@@ -148,7 +148,8 @@ typedef std::shared_ptr<DatasetIoInterface> 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

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<EurocVioDataset> data;
+  bool load_mocap_as_gt;
 };  // namespace basalt
 
 }  // namespace basalt

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") {

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 <pangolin/display/image_view.h>
 #include <pangolin/gl/gldraw.h>
@@ -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<bool> show_error("ui.show_error", false, false, true);
 
-    pangolin::Var<std::function<void(void)>> save_aligned_dataset(
-        "ui.save_aligned_dataset", [&]() {
+    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"))) {
-            std::cout << "Aligned grount truth data already exists, skipping."
-                      << std::endl;
+      save_button_name += "(disabled)";
+    }
+
+    pangolin::Var<std::function<void(void)>> save_aligned_dataset(
+        save_button_name, [&]() {
+          if (fs::exists(fs::path(dataset_path + "mav0/gt/data.csv"))) {
+            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 "