v01

include/basalt/io/dataset_io.h

@@ -0,0 +1,178 @@
+/**
+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 <array>
+#include <fstream>
+#include <iomanip>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <cereal/archives/binary.hpp>
+#include <cereal/archives/json.hpp>
+#include <cereal/types/bitset.hpp>
+#include <cereal/types/deque.hpp>
+#include <cereal/types/map.hpp>
+#include <cereal/types/memory.hpp>
+#include <cereal/types/polymorphic.hpp>
+#include <cereal/types/set.hpp>
+#include <cereal/types/string.hpp>
+#include <cereal/types/unordered_map.hpp>
+#include <cereal/types/utility.hpp>
+#include <cereal/types/vector.hpp>
+
+#include <Eigen/Dense>
+#include <basalt/utils/sophus_utils.hpp>
+
+#include <basalt/image/image.h>
+#include <basalt/utils/assert.h>
+
+#include <basalt/camera/generic_camera.hpp>
+#include <basalt/camera/stereographic_param.hpp>
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+struct ImageData {
+  ImageData() : exposure(0) {}
+
+  ManagedImage<uint16_t>::Ptr img;
+  double exposure;
+};
+
+struct Observations {
+  Eigen::aligned_vector<Eigen::Vector2d> pos;
+  std::vector<int> id;
+};
+
+struct GyroData {
+  int64_t timestamp_ns;
+  Eigen::Vector3d data;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct AccelData {
+  int64_t timestamp_ns;
+  Eigen::Vector3d data;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct PoseData {
+  int64_t timestamp_ns;
+  Sophus::SE3d data;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct MocapPoseData {
+  int64_t timestamp_ns;
+  Sophus::SE3d data;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct AprilgridCornersData {
+  int64_t timestamp_ns;
+  int cam_id;
+
+  Eigen::aligned_vector<Eigen::Vector2d> corner_pos;
+  std::vector<int> corner_id;
+};
+
+class VioDataset {
+ public:
+  virtual ~VioDataset(){};
+
+  virtual size_t get_num_cams() const = 0;
+
+  virtual std::vector<int64_t> &get_image_timestamps() = 0;
+
+  virtual const Eigen::aligned_vector<AccelData> &get_accel_data() const = 0;
+  virtual const Eigen::aligned_vector<GyroData> &get_gyro_data() const = 0;
+  virtual const std::vector<int64_t> &get_gt_timestamps() const = 0;
+  virtual const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data()
+      const = 0;
+  virtual int64_t get_mocap_to_imu_offset_ns() const = 0;
+  virtual std::vector<ImageData> get_image_data(int64_t t_ns) = 0;
+  virtual std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) = 0;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+typedef std::shared_ptr<VioDataset> VioDatasetPtr;
+
+class DatasetIoInterface {
+ public:
+  virtual void read(const std::string &path) = 0;
+  virtual void reset() = 0;
+  virtual VioDatasetPtr get_data() = 0;
+
+  virtual ~DatasetIoInterface(){};
+};
+
+typedef std::shared_ptr<DatasetIoInterface> DatasetIoInterfacePtr;
+
+class DatasetIoFactory {
+ public:
+  static DatasetIoInterfacePtr getDatasetIo(const std::string &dataset_type,
+                                            bool load_mocap_as_gt = false);
+};
+
+}  // namespace basalt
+
+namespace cereal {
+
+template <class Archive>
+void serialize(Archive &archive, basalt::ManagedImage<uint8_t> &m) {
+  archive(m.w);
+  archive(m.h);
+
+  m.Reinitialise(m.w, m.h);
+
+  archive(binary_data(m.ptr, m.size()));
+}
+
+template <class Archive>
+void serialize(Archive &ar, basalt::GyroData &c) {
+  ar(c.timestamp_ns, c.data);
+}
+
+template <class Archive>
+void serialize(Archive &ar, basalt::AccelData &c) {
+  ar(c.timestamp_ns, c.data);
+}
+
+}  // namespace cereal

include/basalt/io/dataset_io_euroc.h

@@ -0,0 +1,336 @@
+/**
+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.
+*/
+#ifndef DATASET_IO_EUROC_H
+#define DATASET_IO_EUROC_H
+
+#include <basalt/io/dataset_io.h>
+#include <basalt/utils/filesystem.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+class EurocVioDataset : public VioDataset {
+  size_t num_cams;
+
+  std::string path;
+
+  std::vector<int64_t> image_timestamps;
+  std::unordered_map<int64_t, std::string> 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::aligned_vector<AccelData> accel_data;
+  Eigen::aligned_vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;  // ordered gt timestamps
+  Eigen::aligned_vector<Sophus::SE3d>
+      gt_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:
+  ~EurocVioDataset(){};
+
+  size_t get_num_cams() const { return num_cams; }
+
+  std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
+
+  const Eigen::aligned_vector<AccelData> &get_accel_data() const {
+    return accel_data;
+  }
+  const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
+    return gyro_data;
+  }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
+    return gt_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);
+
+    const std::vector<std::string> folder = {"/mav0/cam0/", "/mav0/cam1/"};
+
+    for (size_t i = 0; i < num_cams; i++) {
+      std::string full_image_path =
+          path + folder[i] + "data/" + image_path[t_ns];
+
+      if (fs::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;
+  }
+
+  // returns a vector of 2 images: for the first and the second camera at the defined timestamp.
+  std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
+    std::vector<cv::Mat> res(num_cams);
+
+    const std::vector<std::string> folder = {"/mav0/cam0/", "/mav0/cam1/"};
+
+    for (size_t i = 0; i < num_cams; i++) {
+      std::string full_image_path =
+          path + folder[i] + "data/" + image_path[t_ns];
+
+      if (fs::exists(full_image_path)) {
+        cv::Mat img = cv::imread(full_image_path, cv::IMREAD_COLOR);
+
+        res.push_back(img);
+        // The rest of the body from get_image_data() was deleted.
+      }
+    }
+    return res;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend class EurocIO;
+};
+
+class EurocIO : public DatasetIoInterface {
+ public:
+  EurocIO(bool load_mocap_as_gt) : load_mocap_as_gt(load_mocap_as_gt) {}
+
+  void read(const std::string &path) {
+    if (!fs::exists(path))
+      std::cerr << "No dataset found in " << path << std::endl;
+
+    data.reset(new EurocVioDataset);
+
+    data->num_cams = 2;
+    data->path = path;
+
+    read_image_timestamps(path + "/mav0/cam0/");
+
+    read_imu_data(path + "/mav0/imu0/");
+
+    if (!load_mocap_as_gt &&
+        fs::exists(path + "/mav0/state_groundtruth_estimate0/data.csv")) {
+      read_gt_data_state(path + "/mav0/state_groundtruth_estimate0/");
+    } else if (!load_mocap_as_gt && fs::exists(path + "/mav0/gt/data.csv")) {
+      read_gt_data_pose(path + "/mav0/gt/");
+    } else if (fs::exists(path + "/mav0/mocap0/data.csv")) {
+      read_gt_data_pose(path + "/mav0/mocap0/");
+    }
+
+    data->exposure_times.resize(data->num_cams);
+    if (fs::exists(path + "/mav0/cam0/exposure.csv")) {
+      std::cout << "Loading exposure times for cam0" << std::endl;
+      read_exposure(path + "/mav0/cam0/", data->exposure_times[0]);
+    }
+    if (fs::exists(path + "/mav0/cam1/exposure.csv")) {
+      std::cout << "Loading exposure times for cam1" << std::endl;
+      read_exposure(path + "/mav0/cam1/", data->exposure_times[1]);
+    }
+  }
+
+  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 + "data.csv");
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+      std::stringstream ss(line);
+      char tmp;
+      int64_t t_ns;
+      std::string path;
+      ss >> t_ns >> tmp >> path;
+
+      data->image_timestamps.emplace_back(t_ns);
+      data->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 + "data.csv");
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      char tmp;
+      uint64_t timestamp;
+      Eigen::Vector3d gyro, accel;
+
+      ss >> timestamp >> tmp >> gyro[0] >> tmp >> gyro[1] >> tmp >> gyro[2] >>
+          tmp >> accel[0] >> tmp >> accel[1] >> tmp >> accel[2];
+
+      data->accel_data.emplace_back();
+      data->accel_data.back().timestamp_ns = timestamp;
+      data->accel_data.back().data = accel;
+
+      data->gyro_data.emplace_back();
+      data->gyro_data.back().timestamp_ns = timestamp;
+      data->gyro_data.back().data = gyro;
+    }
+  }
+
+  void read_gt_data_state(const std::string &path) {
+    data->gt_timestamps.clear();
+    data->gt_pose_data.clear();
+
+    std::ifstream f(path + "data.csv");
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      char tmp;
+      uint64_t timestamp;
+      Eigen::Quaterniond q;
+      Eigen::Vector3d pos, vel, accel_bias, gyro_bias;
+
+      ss >> timestamp >> tmp >> pos[0] >> tmp >> pos[1] >> tmp >> pos[2] >>
+          tmp >> q.w() >> tmp >> q.x() >> tmp >> q.y() >> tmp >> q.z() >> tmp >>
+          vel[0] >> tmp >> vel[1] >> tmp >> vel[2] >> tmp >> accel_bias[0] >>
+          tmp >> accel_bias[1] >> tmp >> accel_bias[2] >> tmp >> gyro_bias[0] >>
+          tmp >> gyro_bias[1] >> tmp >> gyro_bias[2];
+
+      data->gt_timestamps.emplace_back(timestamp);
+      data->gt_pose_data.emplace_back(q, pos);
+    }
+  }
+
+  void read_gt_data_pose(const std::string &path) {
+    data->gt_timestamps.clear();
+    data->gt_pose_data.clear();
+
+    std::ifstream f(path + "data.csv");
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      char tmp;
+      uint64_t timestamp;
+      Eigen::Quaterniond q;
+      Eigen::Vector3d pos;
+
+      ss >> timestamp >> tmp >> pos[0] >> tmp >> pos[1] >> tmp >> pos[2] >>
+          tmp >> q.w() >> tmp >> q.x() >> tmp >> q.y() >> tmp >> q.z();
+
+      data->gt_timestamps.emplace_back(timestamp);
+      data->gt_pose_data.emplace_back(q, pos);
+    }
+  }
+
+  std::shared_ptr<EurocVioDataset> data;
+  bool load_mocap_as_gt;
+};  // namespace basalt
+
+}  // namespace basalt
+
+#endif  // DATASET_IO_H

include/basalt/io/dataset_io_kitti.h

@@ -0,0 +1,220 @@
+/**
+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 <basalt/utils/filesystem.h>
+
+#include <opencv2/highgui/highgui.hpp>
+
+namespace basalt {
+
+class KittiVioDataset : public VioDataset {
+  size_t num_cams;
+
+  std::string path;
+
+  std::vector<int64_t> image_timestamps;
+  std::unordered_map<int64_t, std::string> 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::aligned_vector<AccelData> accel_data;
+  Eigen::aligned_vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;  // ordered gt timestamps
+  Eigen::aligned_vector<Sophus::SE3d>
+      gt_pose_data;  // TODO: change to eigen aligned
+
+  int64_t mocap_to_imu_offset_ns;
+
+ public:
+  ~KittiVioDataset(){};
+
+  size_t get_num_cams() const { return num_cams; }
+
+  std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
+
+  const Eigen::aligned_vector<AccelData> &get_accel_data() const {
+    return accel_data;
+  }
+  const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
+    return gyro_data;
+  }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
+    return gt_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);
+
+    const std::vector<std::string> folder = {"/image_0/", "/image_1/"};
+
+    for (size_t i = 0; i < num_cams; i++) {
+      std::string full_image_path = path + folder[i] + image_path[t_ns];
+
+      if (fs::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 {
+          std::cerr << "img.fmt.bpp " << img.type() << std::endl;
+          std::abort();
+        }
+      }
+    }
+
+    return res;
+  }
+
+  std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
+    std::vector<cv::Mat> res(num_cams);
+
+    const std::vector<std::string> folder = {"/image_0/", "/image_1/"};
+
+    for (size_t i = 0; i < num_cams; i++) {
+      std::string full_image_path =
+          path + folder[i] + image_path[t_ns];
+
+      if (fs::exists(full_image_path)) {
+        cv::Mat img = cv::imread(full_image_path, cv::IMREAD_GRAYSCALE);
+
+        res.push_back(img);
+        // The rest of the body from get_image_data() was deleted.
+      }
+    }
+    return res;
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend class KittiIO;
+};
+
+class KittiIO : public DatasetIoInterface {
+ public:
+  KittiIO() {}
+
+  void read(const std::string &path) {
+    if (!fs::exists(path))
+      std::cerr << "No dataset found in " << path << std::endl;
+
+    data.reset(new KittiVioDataset);
+
+    data->num_cams = 2;
+    data->path = path;
+
+    read_image_timestamps(path + "/times.txt");
+
+    if (fs::exists(path + "/poses.txt")) {
+      read_gt_data_pose(path + "/poses.txt");
+    }
+  }
+
+  void reset() { data.reset(); }
+
+  VioDatasetPtr get_data() { return data; }
+
+ private:
+  void read_image_timestamps(const std::string &path) {
+    std::ifstream f(path);
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+      std::stringstream ss(line);
+
+      double t_s;
+      ss >> t_s;
+
+      int64_t t_ns = t_s * 1e9;
+
+      std::stringstream ss1;
+      ss1 << std::setfill('0') << std::setw(6) << data->image_timestamps.size()
+          << ".png";
+
+      data->image_timestamps.emplace_back(t_ns);
+      data->image_path[t_ns] = ss1.str();
+    }
+  }
+
+  void read_gt_data_pose(const std::string &path) {
+    data->gt_timestamps.clear();
+    data->gt_pose_data.clear();
+
+    int i = 0;
+
+    std::ifstream f(path);
+    std::string line;
+    while (std::getline(f, line)) {
+      if (line[0] == '#') continue;
+
+      std::stringstream ss(line);
+
+      Eigen::Matrix3d rot;
+      Eigen::Vector3d pos;
+
+      ss >> rot(0, 0) >> rot(0, 1) >> rot(0, 2) >> pos[0] >> rot(1, 0) >>
+          rot(1, 1) >> rot(1, 2) >> pos[1] >> rot(2, 0) >> rot(2, 1) >>
+          rot(2, 2) >> pos[2];
+
+      data->gt_timestamps.emplace_back(data->image_timestamps[i]);
+      data->gt_pose_data.emplace_back(Eigen::Quaterniond(rot), pos);
+      i++;
+    }
+  }
+
+  std::shared_ptr<KittiVioDataset> data;
+};
+
+}  // namespace basalt

include/basalt/io/dataset_io_rosbag.h

@@ -0,0 +1,406 @@
+/**
+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.
+*/
+#ifndef DATASET_IO_ROSBAG_H
+#define DATASET_IO_ROSBAG_H
+
+#include <mutex>
+#include <optional>
+
+#include <basalt/io/dataset_io.h>
+
+// Hack to access private functions
+#define private public
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#undef private
+
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/Imu.h>
+
+#include <basalt/utils/filesystem.h>
+
+namespace basalt {
+
+class RosbagVioDataset : public VioDataset {
+  std::shared_ptr<rosbag::Bag> bag;
+  std::mutex m;
+
+  size_t num_cams;
+
+  std::vector<int64_t> image_timestamps;
+
+  // 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<std::optional<rosbag::IndexEntry>>>
+      image_data_idx;
+
+  Eigen::aligned_vector<AccelData> accel_data;
+  Eigen::aligned_vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;  // ordered gt timestamps
+  Eigen::aligned_vector<Sophus::SE3d>
+      gt_pose_data;  // TODO: change to eigen aligned
+
+  int64_t mocap_to_imu_offset_ns;
+
+ public:
+  ~RosbagVioDataset() {}
+
+  size_t get_num_cams() const { return num_cams; }
+
+  std::vector<int64_t> &get_image_timestamps() { return image_timestamps; }
+
+  const Eigen::aligned_vector<AccelData> &get_accel_data() const {
+    return accel_data;
+  }
+  const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
+    return gyro_data;
+  }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
+    return gt_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);
+
+    auto it = image_data_idx.find(t_ns);
+
+    if (it != image_data_idx.end())
+      for (size_t i = 0; i < num_cams; i++) {
+        ImageData &id = res[i];
+
+        if (!it->second[i].has_value()) continue;
+
+        m.lock();
+        sensor_msgs::ImageConstPtr img_msg =
+            bag->instantiateBuffer<sensor_msgs::Image>(*it->second[i]);
+        m.unlock();
+
+        //        std::cerr << "img_msg->width " << img_msg->width << "
+        //        img_msg->height "
+        //                  << img_msg->height << std::endl;
+
+        id.img.reset(
+            new ManagedImage<uint16_t>(img_msg->width, img_msg->height));
+
+        if (!img_msg->header.frame_id.empty() &&
+            std::isdigit(img_msg->header.frame_id[0])) {
+          id.exposure = std::stol(img_msg->header.frame_id) * 1e-9;
+        } else {
+          id.exposure = -1;
+        }
+
+        if (img_msg->encoding == "mono8") {
+          const uint8_t *data_in = img_msg->data.data();
+          uint16_t *data_out = id.img->ptr;
+
+          for (size_t i = 0; i < img_msg->data.size(); i++) {
+            int val = data_in[i];
+            val = val << 8;
+            data_out[i] = val;
+          }
+
+        } else if (img_msg->encoding == "mono16") {
+          std::memcpy(id.img->ptr, img_msg->data.data(), img_msg->data.size());
+        } else {
+          std::cerr << "Encoding " << img_msg->encoding << " is not supported."
+                    << std::endl;
+          std::abort();
+        }
+      }
+
+    return res;
+  }
+
+  std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
+    cv::Mat mat = cv::Mat(1, 10, CV_32F, 1);
+    return mat;
+  }
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  friend class RosbagIO;
+};
+
+class RosbagIO : public DatasetIoInterface {
+ public:
+  RosbagIO() {}
+
+  void read(const std::string &path) {
+    if (!fs::exists(path))
+      std::cerr << "No dataset found in " << path << std::endl;
+
+    data.reset(new RosbagVioDataset);
+
+    data->bag.reset(new rosbag::Bag);
+    data->bag->open(path, rosbag::bagmode::Read);
+
+    rosbag::View view(*data->bag);
+
+    // get topics
+    std::vector<const rosbag::ConnectionInfo *> connection_infos =
+        view.getConnections();
+
+    std::set<std::string> cam_topics;
+    std::string imu_topic;
+    std::string mocap_topic;
+    std::string point_topic;
+
+    for (const rosbag::ConnectionInfo *info : connection_infos) {
+      //      if (info->topic.substr(0, 4) == std::string("/cam")) {
+      //        cam_topics.insert(info->topic);
+      //      } else if (info->topic.substr(0, 4) == std::string("/imu")) {
+      //        imu_topic = info->topic;
+      //      } else if (info->topic.substr(0, 5) == std::string("/vrpn") ||
+      //                 info->topic.substr(0, 6) == std::string("/vicon")) {
+      //        mocap_topic = info->topic;
+      //      }
+
+      if (info->datatype == std::string("sensor_msgs/Image")) {
+        cam_topics.insert(info->topic);
+      } else if (info->datatype == std::string("sensor_msgs/Imu") &&
+                 info->topic.rfind("/fcu", 0) != 0) {
+        imu_topic = info->topic;
+      } else if (info->datatype ==
+                     std::string("geometry_msgs/TransformStamped") ||
+                 info->datatype == std::string("geometry_msgs/PoseStamped")) {
+        mocap_topic = info->topic;
+      } else if (info->datatype == std::string("geometry_msgs/PointStamped")) {
+        point_topic = info->topic;
+      }
+    }
+
+    std::cout << "imu_topic: " << imu_topic << std::endl;
+    std::cout << "mocap_topic: " << mocap_topic << std::endl;
+    std::cout << "cam_topics: ";
+    for (const std::string &s : cam_topics) std::cout << s << " ";
+    std::cout << std::endl;
+
+    std::map<std::string, int> topic_to_id;
+    int idx = 0;
+    for (const std::string &s : cam_topics) {
+      topic_to_id[s] = idx;
+      idx++;
+    }
+
+    data->num_cams = cam_topics.size();
+
+    int num_msgs = 0;
+
+    int64_t min_time = std::numeric_limits<int64_t>::max();
+    int64_t max_time = std::numeric_limits<int64_t>::min();
+
+    std::vector<geometry_msgs::TransformStampedConstPtr> mocap_msgs;
+    std::vector<geometry_msgs::PointStampedConstPtr> point_msgs;
+
+    std::vector<int64_t>
+        system_to_imu_offset_vec;  // t_imu = t_system + system_to_imu_offset
+    std::vector<int64_t> system_to_mocap_offset_vec;  // t_mocap = t_system +
+                                                      // system_to_mocap_offset
+
+    std::set<int64_t> image_timestamps;
+
+    for (const rosbag::MessageInstance &m : view) {
+      const std::string &topic = m.getTopic();
+
+      if (cam_topics.find(topic) != cam_topics.end()) {
+        sensor_msgs::ImageConstPtr img_msg =
+            m.instantiate<sensor_msgs::Image>();
+        int64_t timestamp_ns = img_msg->header.stamp.toNSec();
+
+        auto &img_vec = data->image_data_idx[timestamp_ns];
+        if (img_vec.size() == 0) img_vec.resize(data->num_cams);
+
+        img_vec[topic_to_id.at(topic)] = m.index_entry_;
+        image_timestamps.insert(timestamp_ns);
+
+        min_time = std::min(min_time, timestamp_ns);
+        max_time = std::max(max_time, timestamp_ns);
+      }
+
+      if (imu_topic == topic) {
+        sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
+        int64_t time = imu_msg->header.stamp.toNSec();
+
+        data->accel_data.emplace_back();
+        data->accel_data.back().timestamp_ns = time;
+        data->accel_data.back().data = Eigen::Vector3d(
+            imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y,
+            imu_msg->linear_acceleration.z);
+
+        data->gyro_data.emplace_back();
+        data->gyro_data.back().timestamp_ns = time;
+        data->gyro_data.back().data = Eigen::Vector3d(
+            imu_msg->angular_velocity.x, imu_msg->angular_velocity.y,
+            imu_msg->angular_velocity.z);
+
+        min_time = std::min(min_time, time);
+        max_time = std::max(max_time, time);
+
+        int64_t msg_arrival_time = m.getTime().toNSec();
+        system_to_imu_offset_vec.push_back(time - msg_arrival_time);
+      }
+
+      if (mocap_topic == topic) {
+        geometry_msgs::TransformStampedConstPtr mocap_msg =
+            m.instantiate<geometry_msgs::TransformStamped>();
+
+        // Try different message type if instantiate did not work
+        if (!mocap_msg) {
+          geometry_msgs::PoseStampedConstPtr mocap_pose_msg =
+              m.instantiate<geometry_msgs::PoseStamped>();
+
+          geometry_msgs::TransformStampedPtr mocap_new_msg(
+              new geometry_msgs::TransformStamped);
+          mocap_new_msg->header = mocap_pose_msg->header;
+          mocap_new_msg->transform.rotation = mocap_pose_msg->pose.orientation;
+          mocap_new_msg->transform.translation.x =
+              mocap_pose_msg->pose.position.x;
+          mocap_new_msg->transform.translation.y =
+              mocap_pose_msg->pose.position.y;
+          mocap_new_msg->transform.translation.z =
+              mocap_pose_msg->pose.position.z;
+
+          mocap_msg = mocap_new_msg;
+        }
+
+        int64_t time = mocap_msg->header.stamp.toNSec();
+
+        mocap_msgs.push_back(mocap_msg);
+
+        int64_t msg_arrival_time = m.getTime().toNSec();
+        system_to_mocap_offset_vec.push_back(time - msg_arrival_time);
+      }
+
+      if (point_topic == topic) {
+        geometry_msgs::PointStampedConstPtr mocap_msg =
+            m.instantiate<geometry_msgs::PointStamped>();
+
+        int64_t time = mocap_msg->header.stamp.toNSec();
+
+        point_msgs.push_back(mocap_msg);
+
+        int64_t msg_arrival_time = m.getTime().toNSec();
+        system_to_mocap_offset_vec.push_back(time - msg_arrival_time);
+      }
+
+      num_msgs++;
+    }
+
+    data->image_timestamps.clear();
+    data->image_timestamps.insert(data->image_timestamps.begin(),
+                                  image_timestamps.begin(),
+                                  image_timestamps.end());
+
+    if (system_to_mocap_offset_vec.size() > 0) {
+      int64_t system_to_imu_offset =
+          system_to_imu_offset_vec[system_to_imu_offset_vec.size() / 2];
+
+      int64_t system_to_mocap_offset =
+          system_to_mocap_offset_vec[system_to_mocap_offset_vec.size() / 2];
+
+      data->mocap_to_imu_offset_ns =
+          system_to_imu_offset - system_to_mocap_offset;
+    }
+
+    data->gt_pose_data.clear();
+    data->gt_timestamps.clear();
+
+    if (!mocap_msgs.empty())
+      for (size_t i = 0; i < mocap_msgs.size() - 1; i++) {
+        auto mocap_msg = mocap_msgs[i];
+
+        int64_t time = mocap_msg->header.stamp.toNSec();
+
+        Eigen::Quaterniond q(
+            mocap_msg->transform.rotation.w, mocap_msg->transform.rotation.x,
+            mocap_msg->transform.rotation.y, mocap_msg->transform.rotation.z);
+
+        Eigen::Vector3d t(mocap_msg->transform.translation.x,
+                          mocap_msg->transform.translation.y,
+                          mocap_msg->transform.translation.z);
+
+        int64_t timestamp_ns = time + data->mocap_to_imu_offset_ns;
+        data->gt_timestamps.emplace_back(timestamp_ns);
+        data->gt_pose_data.emplace_back(q, t);
+      }
+
+    if (!point_msgs.empty())
+      for (size_t i = 0; i < point_msgs.size() - 1; i++) {
+        auto point_msg = point_msgs[i];
+
+        int64_t time = point_msg->header.stamp.toNSec();
+
+        Eigen::Vector3d t(point_msg->point.x, point_msg->point.y,
+                          point_msg->point.z);
+
+        int64_t timestamp_ns = time;  // + data->mocap_to_imu_offset_ns;
+        data->gt_timestamps.emplace_back(timestamp_ns);
+        data->gt_pose_data.emplace_back(Sophus::SO3d(), t);
+      }
+
+    std::cout << "Total number of messages: " << num_msgs << std::endl;
+    std::cout << "Image size: " << data->image_data_idx.size() << std::endl;
+
+    std::cout << "Min time: " << min_time << " max time: " << max_time
+              << " mocap to imu offset: " << data->mocap_to_imu_offset_ns
+              << std::endl;
+
+    std::cout << "Number of mocap poses: " << data->gt_timestamps.size()
+              << std::endl;
+  }
+
+  void reset() { data.reset(); }
+
+  VioDatasetPtr get_data() {
+    // return std::dynamic_pointer_cast<VioDataset>(data);
+    return data;
+  }
+
+ private:
+  std::shared_ptr<RosbagVioDataset> data;
+};
+
+}  // namespace basalt
+
+#endif  // DATASET_IO_ROSBAG_H

include/basalt/io/dataset_io_uzh.h

@@ -0,0 +1,316 @@
+/**
+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 <basalt/utils/filesystem.h>
+
+#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::aligned_vector<AccelData> accel_data;
+  Eigen::aligned_vector<GyroData> gyro_data;
+
+  std::vector<int64_t> gt_timestamps;  // ordered gt timestamps
+  Eigen::aligned_vector<Sophus::SE3d>
+      gt_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::aligned_vector<AccelData> &get_accel_data() const {
+    return accel_data;
+  }
+  const Eigen::aligned_vector<GyroData> &get_gyro_data() const {
+    return gyro_data;
+  }
+  const std::vector<int64_t> &get_gt_timestamps() const {
+    return gt_timestamps;
+  }
+  const Eigen::aligned_vector<Sophus::SE3d> &get_gt_pose_data() const {
+    return gt_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 (fs::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;
+  }
+
+  std::vector<cv::Mat> get_image_data_cv_mat(int64_t t_ns) {
+    cv::Mat mat = cv::Mat(1, 10, CV_32F, 1);
+    return mat;
+  }
+  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 (fs::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;
+
+      data->gt_timestamps.emplace_back(t_ns);
+      data->gt_pose_data.emplace_back(q, pos);
+    }
+  }
+
+  std::shared_ptr<UzhVioDataset> data;
+};
+
+}  // namespace basalt

include/basalt/io/marg_data_io.h

@@ -0,0 +1,78 @@
+/**
+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 <memory>
+#include <thread>
+
+#include <basalt/utils/imu_types.h>
+
+namespace basalt {
+
+class MargDataSaver {
+ public:
+  using Ptr = std::shared_ptr<MargDataSaver>;
+
+  MargDataSaver(const std::string& path);
+  ~MargDataSaver() {
+    saving_thread->join();
+    saving_img_thread->join();
+  }
+  tbb::concurrent_bounded_queue<MargData::Ptr> in_marg_queue;
+
+ private:
+  std::shared_ptr<std::thread> saving_thread;
+  std::shared_ptr<std::thread> saving_img_thread;
+
+  tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr> save_image_queue;
+};
+
+class MargDataLoader {
+ public:
+  using Ptr = std::shared_ptr<MargDataLoader>;
+
+  MargDataLoader();
+
+  void start(const std::string& path);
+  ~MargDataLoader() {
+    if (processing_thread) processing_thread->join();
+  }
+
+  tbb::concurrent_bounded_queue<MargData::Ptr>* out_marg_queue;
+
+ private:
+  std::shared_ptr<std::thread> processing_thread;
+};
+}  // namespace basalt