Merge branch 'uzh' into 'master'

UZH

See merge request basalt/basalt!28

include/basalt/io/dataset_io_euroc.h

@@ -67,7 +67,7 @@ class EurocVioDataset : public VioDataset {
   Eigen::vector<Sophus::SE3d>
       device_pose_data;  // TODO: change to eigen aligned
 
-  int64_t mocap_to_imu_offset_ns;
+  int64_t mocap_to_imu_offset_ns = 0;
 
   std::vector<std::unordered_map<int64_t, double>> exposure_times;
 

include/basalt/io/dataset_io_uzh.h

@@ -0,0 +1,319 @@
+/**
+BSD 3-Clause License
+
+This file is part of the Basalt project.
+https://gitlab.com/VladyslavUsenko/basalt.git
+
+Copyright (c) 2019, Vladyslav Usenko 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.
+*/
+
+#pragma once
+
+#include <basalt/io/dataset_io.h>
+
+#include <experimental/filesystem>
+namespace fs = std::experimental::filesystem;
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+class UzhVioDataset : public VioDataset {
+  size_t num_cams;
+
+  std::string path;
+
+  std::vector<int64_t> image_timestamps;
+  std::unordered_map<int64_t, std::string> left_image_path;
+  std::unordered_map<int64_t, std::string> right_image_path;
+
+  // vector of images for every timestamp
+  // assumes vectors size is num_cams for every timestamp with null pointers for
+  // missing frames
+  // std::unordered_map<int64_t, std::vector<ImageData>> image_data;
+
+  Eigen::vector<AccelData> accel_data;
+  Eigen::vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;        // ordered gt timestamps
+  Eigen::vector<Sophus::SE3d> gt_pose_data;  // TODO: change to eigen aligned
+
+  std::vector<int64_t> device_pose_timestamps;  // ordered gt timestamps
+  Eigen::vector<Sophus::SE3d>
+      device_pose_data;  // TODO: change to eigen aligned
+
+  int64_t mocap_to_imu_offset_ns = 0;
+
+  std::vector<std::unordered_map<int64_t, double>> exposure_times;
+
+ public:
+  ~UzhVioDataset(){};
+
+  size_t get_num_cams() const { return num_cams; }
+
+  std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
+
+  const Eigen::vector<AccelData> &get_accel_data() const { return accel_data; }
+  const Eigen::vector<GyroData> &get_gyro_data() const { return gyro_data; }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const {
+    return gt_pose_data;
+  }
+  const std::vector<int64_t> &get_device_pose_timestamps() const {
+    return device_pose_timestamps;
+  }
+  const Eigen::vector<Sophus::SE3d> &get_device_pose_data() const {
+    return device_pose_data;
+  }
+  int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
+
+  std::vector<ImageData> get_image_data(int64_t t_ns) {
+    std::vector<ImageData> res(num_cams);
+
+    for (size_t i = 0; i < num_cams; i++) {
+      std::string full_image_path =
+          path + "/" +
+          (i == 0 ? left_image_path.at(t_ns) : right_image_path.at(t_ns));
+
+      if (file_exists(full_image_path)) {
+        cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED);
+
+        if (img.type() == CV_8UC1) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+
+          const uint8_t *data_in = img.ptr();
+          uint16_t *data_out = res[i].img->ptr;
+
+          size_t full_size = img.cols * img.rows;
+          for (size_t i = 0; i < full_size; i++) {
+            int val = data_in[i];
+            val = val << 8;
+            data_out[i] = val;
+          }
+        } else if (img.type() == CV_8UC3) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+
+          const uint8_t *data_in = img.ptr();
+          uint16_t *data_out = res[i].img->ptr;
+
+          size_t full_size = img.cols * img.rows;
+          for (size_t i = 0; i < full_size; i++) {
+            int val = data_in[i * 3];
+            val = val << 8;
+            data_out[i] = val;
+          }
+        } else if (img.type() == CV_16UC1) {
+          res[i].img.reset(new ManagedImage<uint16_t>(img.cols, img.rows));
+          std::memcpy(res[i].img->ptr, img.ptr(),
+                      img.cols * img.rows * sizeof(uint16_t));
+
+        } else {
+          std::cerr << "img.fmt.bpp " << img.type() << std::endl;
+          std::abort();
+        }
+
+        auto exp_it = exposure_times[i].find(t_ns);
+        if (exp_it != exposure_times[i].end()) {
+          res[i].exposure = exp_it->second;
+        }
+      }
+    }
+
+    return res;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend class UzhIO;
+};
+
+class UzhIO : public DatasetIoInterface {
+ public:
+  UzhIO() {}
+
+  void read(const std::string &path) {
+    if (!fs::exists(path))
+      std::cerr << "No dataset found in " << path << std::endl;
+
+    data.reset(new UzhVioDataset);
+
+    data->num_cams = 2;
+    data->path = path;
+
+    read_image_timestamps(path);
+
+    std::cout << "Loaded " << data->get_image_timestamps().size()
+              << " timestamps, " << data->left_image_path.size()
+              << " left images and " << data->right_image_path.size()
+              << std::endl;
+
+    //    {
+    //      int64_t t_ns = data->get_image_timestamps()[0];
+    //      std::cout << t_ns << " " << data->left_image_path.at(t_ns) << " "
+    //                << data->right_image_path.at(t_ns) << std::endl;
+    //    }
+
+    read_imu_data(path + "/imu.txt");
+
+    std::cout << "Loaded " << data->get_gyro_data().size() << " imu msgs."
+              << std::endl;
+
+    if (file_exists(path + "/groundtruth.txt")) {
+      read_gt_data_pose(path + "/groundtruth.txt");
+    }
+
+    data->exposure_times.resize(data->num_cams);
+  }
+
+  void reset() { data.reset(); }
+
+  VioDatasetPtr get_data() { return data; }
+
+ private:
+  void read_exposure(const std::string &path,
+                     std::unordered_map<int64_t, double> &exposure_data) {
+    exposure_data.clear();
+
+    std::ifstream f(path + "exposure.csv");
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      char tmp;
+      int64_t timestamp, exposure_int;
+      Eigen::Vector3d gyro, accel;
+
+      ss >> timestamp >> tmp >> exposure_int;
+
+      exposure_data[timestamp] = exposure_int * 1e-9;
+    }
+  }
+
+  void read_image_timestamps(const std::string &path) {
+    {
+      std::ifstream f(path + "/left_images.txt");
+      std::string line;
+      while (std::getline(f, line)) {
+        if (line[0] == '#') continue;
+        std::stringstream ss(line);
+        int tmp;
+        double t_s;
+        std::string path;
+        ss >> tmp >> t_s >> path;
+
+        int64_t t_ns = t_s * 1e9;
+
+        data->image_timestamps.emplace_back(t_ns);
+        data->left_image_path[t_ns] = path;
+      }
+    }
+
+    {
+      std::ifstream f(path + "/right_images.txt");
+      std::string line;
+      while (std::getline(f, line)) {
+        if (line[0] == '#') continue;
+        std::stringstream ss(line);
+        int tmp;
+        double t_s;
+        std::string path;
+        ss >> tmp >> t_s >> path;
+
+        int64_t t_ns = t_s * 1e9;
+
+        data->right_image_path[t_ns] = path;
+      }
+    }
+  }
+
+  void read_imu_data(const std::string &path) {
+    data->accel_data.clear();
+    data->gyro_data.clear();
+
+    std::ifstream f(path);
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      int tmp;
+      double timestamp;
+      Eigen::Vector3d gyro, accel;
+
+      ss >> tmp >> timestamp >> gyro[0] >> gyro[1] >> gyro[2] >> accel[0] >>
+          accel[1] >> accel[2];
+
+      int64_t t_ns = timestamp * 1e9;
+
+      data->accel_data.emplace_back();
+      data->accel_data.back().timestamp_ns = t_ns;
+      data->accel_data.back().data = accel;
+
+      data->gyro_data.emplace_back();
+      data->gyro_data.back().timestamp_ns = t_ns;
+      data->gyro_data.back().data = gyro;
+    }
+  }
+
+  void read_gt_data_pose(const std::string &path) {
+    data->gt_timestamps.clear();
+    data->gt_pose_data.clear();
+
+    std::ifstream f(path);
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      int tmp;
+      double timestamp;
+      Eigen::Quaterniond q;
+      Eigen::Vector3d pos;
+
+      ss >> tmp >> timestamp >> pos[0] >> pos[1] >> pos[2] >> q.x() >> q.y() >>
+          q.z() >> q.w();
+
+      int64_t t_ns = timestamp * 1e9;
+      t_ns += -99902802;
+
+      data->gt_timestamps.emplace_back(t_ns);
+      data->gt_pose_data.emplace_back(q, pos);
+    }
+  }
+
+  std::shared_ptr<UzhVioDataset> data;
+};
+
+}  // namespace basalt

src/calibrate_imu.cpp

@@ -73,9 +73,9 @@ int main(int argc, char **argv) {
   app.add_option("--accel-noise-std", accel_noise_std,
                  "Accelerometer noise std");
 
-  app.add_option("--gyro-bias-std", accel_bias_std,
+  app.add_option("--gyro-bias-std", gyro_bias_std,
                  "Gyroscope bias random walk std");
-  app.add_option("--accel-bias-std", gyro_bias_std,
+  app.add_option("--accel-bias-std", accel_bias_std,
                  "Accelerometer bias random walk std");
 
   app.add_option("--cache-name", cache_dataset_name,

src/io/dataset_io.cpp

@@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <basalt/io/dataset_io.h>
 #include <basalt/io/dataset_io_euroc.h>
 #include <basalt/io/dataset_io_rosbag.h>
+#include <basalt/io/dataset_io_uzh.h>
 
 namespace basalt {
 
@@ -46,6 +47,8 @@ DatasetIoInterfacePtr DatasetIoFactory::getDatasetIo(
     return DatasetIoInterfacePtr(new EurocIO);
   } else if (dataset_type == "bag") {
     return DatasetIoInterfacePtr(new RosbagIO(with_images));
+  } else if (dataset_type == "uzh") {
+    return DatasetIoInterfacePtr(new UzhIO);
   } else {
     std::cerr << "Dataset type " << dataset_type << " is not supported"
               << std::endl;

src/time_alignment.cpp

@@ -66,17 +66,15 @@ int main(int argc, char **argv) {
   std::string output_gyro_path;
   std::string output_mocap_path;
 
-  bool show_gui = false;
+  bool show_gui = true;
 
   CLI::App app{"Calibrate time offset"};
 
   app.add_option("-d,--dataset-path", dataset_path, "Path to dataset")
       ->required();
-  app.add_option("--calibration", calibration_path, "Path to calibration file")
+  app.add_option("--calibration", calibration_path, "Path to calibration file");
-      ->required();
   app.add_option("--mocap-calibration", mocap_calibration_path,
-                 "Path to mocap calibration file")
+                 "Path to mocap calibration file");
-      ->required();
   app.add_option("--dataset-type", dataset_type, "Dataset type <euroc, bag>.")
       ->required();
 
@@ -101,6 +99,10 @@ int main(int argc, char **argv) {
 
   basalt::VioDatasetPtr vio_dataset;
 
+  const bool use_calib =
+      !(calibration_path.empty() || mocap_calibration_path.empty());
+
+  if (use_calib) {
     std::ifstream is(calibration_path);
 
     if (is.good()) {
@@ -123,6 +125,7 @@ int main(int argc, char **argv) {
       std::cerr << "No mocap calibration found" << std::endl;
       std::abort();
     }
+  }
 
   basalt::DatasetIoInterfacePtr dataset_io =
       basalt::DatasetIoFactory::getDatasetIo(dataset_type);
@@ -148,7 +151,11 @@ int main(int argc, char **argv) {
       gyro_timestamps.push_back(vio_dataset->get_gyro_data()[i].timestamp_ns);
 
       Eigen::Vector3d measurement = vio_dataset->get_gyro_data()[i].data;
+      if (use_calib) {
         gyro_data.push_back(calib.calib_gyro_bias.getCalibrated(measurement));
+      } else {
+        gyro_data.push_back(measurement);
+      }
     }
     std::cout << "saturated gyro measurement count: " << saturation_count
               << std::endl;
@@ -156,7 +163,8 @@ int main(int argc, char **argv) {
 
   // compute rotational velocity from mocap data
   {
-    Sophus::SE3d T_mark_i = mocap_calib.T_i_mark.inverse();
+    Sophus::SE3d T_mark_i;
+    if (use_calib) T_mark_i = mocap_calib.T_i_mark.inverse();
 
     int saturation_count = 0;
     for (size_t i = 1; i < vio_dataset->get_gt_timestamps().size() - 1; i++) {
@@ -373,7 +381,8 @@ int main(int argc, char **argv) {
           std::cout << "Saving aligned dataset in "
                     << dataset_path + "mav0/gt/data.csv" << std::endl;
           // output corrected mocap data
-          Sophus::SE3d T_mark_i = mocap_calib.T_i_mark.inverse();
+          Sophus::SE3d T_mark_i;
+          if (use_calib) T_mark_i = mocap_calib.T_i_mark.inverse();
           fs::create_directory(dataset_path + "mav0/gt/");
           std::ofstream gt_out_stream;
           gt_out_stream.open(dataset_path + "mav0/gt/data.csv");

src/vi_estimator/keypoint_vio.cpp

@@ -125,6 +125,13 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
     while (true) {
       vision_data_queue.pop(curr_frame);
 
+      if (!curr_frame.get()) {
+        break;
+      }
+
+      // Correct camera time offset
+      // curr_frame->t_ns += calib.cam_time_offset_ns;
+
       if (!initialized) {
         Eigen::Vector3d vel_w_i_init;
         vel_w_i_init.setZero();
@@ -150,10 +157,6 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
         initialized = true;
       }
 
-      if (!curr_frame.get()) {
-        break;
-      }
-
       if (prev_frame) {
         // preintegrate measurements
 
@@ -179,6 +182,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
         }
 
         if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) {
+          if (!data.get()) break;
           int64_t tmp = data->t_ns;
           data->t_ns = curr_frame->t_ns;
           meas->integrate(*data, accel_cov, gyro_cov);