From 1d0fc86003ba352e81ea93f77133c738ed53c8b9 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Sun, 4 Aug 2019 16:36:46 +0200 Subject: [PATCH] removed device data --- include/basalt/io/dataset_io.h | 2 -- include/basalt/io/dataset_io_euroc.h | 41 ++------------------------- include/basalt/io/dataset_io_rosbag.h | 11 +------ include/basalt/io/dataset_io_uzh.h | 11 +------ src/vio.cpp | 18 ------------ 5 files changed, 4 insertions(+), 79 deletions(-) diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h index 69d527c..1a209ed 100644 --- a/include/basalt/io/dataset_io.h +++ b/include/basalt/io/dataset_io.h @@ -127,8 +127,6 @@ class VioDataset { virtual const Eigen::vector &get_gyro_data() const = 0; virtual const std::vector &get_gt_timestamps() const = 0; virtual const Eigen::vector &get_gt_pose_data() const = 0; - virtual const std::vector &get_device_pose_timestamps() const = 0; - virtual const Eigen::vector &get_device_pose_data() const = 0; virtual int64_t get_mocap_to_imu_offset_ns() const = 0; virtual std::vector get_image_data(int64_t t_ns) = 0; diff --git a/include/basalt/io/dataset_io_euroc.h b/include/basalt/io/dataset_io_euroc.h index f55bea6..c66a980 100644 --- a/include/basalt/io/dataset_io_euroc.h +++ b/include/basalt/io/dataset_io_euroc.h @@ -63,10 +63,6 @@ class EurocVioDataset : public VioDataset { std::vector gt_timestamps; // ordered gt timestamps Eigen::vector gt_pose_data; // TODO: change to eigen aligned - std::vector device_pose_timestamps; // ordered gt timestamps - Eigen::vector - device_pose_data; // TODO: change to eigen aligned - int64_t mocap_to_imu_offset_ns = 0; std::vector> exposure_times; @@ -86,12 +82,7 @@ class EurocVioDataset : public VioDataset { const Eigen::vector &get_gt_pose_data() const { return gt_pose_data; } - const std::vector &get_device_pose_timestamps() const { - return device_pose_timestamps; - } - const Eigen::vector &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 get_image_data(int64_t t_ns) { @@ -180,10 +171,6 @@ class EurocIO : public DatasetIoInterface { read_gt_data_pose(path + "/mav0/mocap0/"); } - if (file_exists(path + "/mav0/realsense0/data.csv")) { - read_device_data_pose(path + "/mav0/realsense0/"); - } - data->exposure_times.resize(data->num_cams); if (file_exists(path + "/mav0/cam0/exposure.csv")) { std::cout << "Loading exposure times for cam0" << std::endl; @@ -316,32 +303,8 @@ class EurocIO : public DatasetIoInterface { } } - void read_device_data_pose(const std::string &path) { - data->device_pose_timestamps.clear(); - data->device_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->device_pose_timestamps.emplace_back(timestamp); - data->device_pose_data.emplace_back(q, pos); - } - } - std::shared_ptr data; -}; +}; // namespace basalt } // namespace basalt diff --git a/include/basalt/io/dataset_io_rosbag.h b/include/basalt/io/dataset_io_rosbag.h index 6b679da..0a13ddb 100644 --- a/include/basalt/io/dataset_io_rosbag.h +++ b/include/basalt/io/dataset_io_rosbag.h @@ -77,10 +77,6 @@ class RosbagVioDataset : public VioDataset { std::vector gt_timestamps; // ordered gt timestamps Eigen::vector gt_pose_data; // TODO: change to eigen aligned - std::vector device_pose_timestamps; // ordered gt timestamps - Eigen::vector - device_pose_data; // TODO: change to eigen aligned - int64_t mocap_to_imu_offset_ns; public: @@ -98,12 +94,7 @@ class RosbagVioDataset : public VioDataset { const Eigen::vector &get_gt_pose_data() const { return gt_pose_data; } - const std::vector &get_device_pose_timestamps() const { - return device_pose_timestamps; - } - const Eigen::vector &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 get_image_data(int64_t t_ns) { diff --git a/include/basalt/io/dataset_io_uzh.h b/include/basalt/io/dataset_io_uzh.h index 05969e3..77fbc2a 100644 --- a/include/basalt/io/dataset_io_uzh.h +++ b/include/basalt/io/dataset_io_uzh.h @@ -64,10 +64,6 @@ class UzhVioDataset : public VioDataset { std::vector gt_timestamps; // ordered gt timestamps Eigen::vector gt_pose_data; // TODO: change to eigen aligned - std::vector device_pose_timestamps; // ordered gt timestamps - Eigen::vector - device_pose_data; // TODO: change to eigen aligned - int64_t mocap_to_imu_offset_ns = 0; std::vector> exposure_times; @@ -87,12 +83,7 @@ class UzhVioDataset : public VioDataset { const Eigen::vector &get_gt_pose_data() const { return gt_pose_data; } - const std::vector &get_device_pose_timestamps() const { - return device_pose_timestamps; - } - const Eigen::vector &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 get_image_data(int64_t t_ns) { diff --git a/src/vio.cpp b/src/vio.cpp index 07440d5..d95e91c 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -101,7 +101,6 @@ pangolin::Var continue_btn("ui.continue", false, false, true); pangolin::Var continue_fast("ui.continue_fast", true, false, true); Button align_svd_btn("ui.align_svd", &alignButton); -Button align_device_svd_btn("ui.align_device_svd", &alignDeviceButton); pangolin::Var follow("ui.follow", true, false, true); @@ -117,9 +116,6 @@ Eigen::vector vio_t_w_i; std::vector gt_t_ns; Eigen::vector gt_t_w_i; -std::vector device_pose_t_ns; -Eigen::vector device_pose_t_w_i; - std::string marg_data_path; size_t last_frame_processed = 0; @@ -246,13 +242,6 @@ int main(int argc, char** argv) { gt_t_ns.push_back(vio_dataset->get_gt_timestamps()[i]); gt_t_w_i.push_back(vio_dataset->get_gt_pose_data()[i].translation()); } - - for (size_t i = 0; i < vio_dataset->get_device_pose_data().size(); i++) { - device_pose_t_ns.push_back(vio_dataset->get_device_pose_timestamps()[i]); - device_pose_t_w_i.push_back( - vio_dataset->get_device_pose_data()[i].translation()); - } - std::cout << "Len " << device_pose_t_ns.size() << std::endl; } const int64_t start_t_ns = vio_dataset->get_image_timestamps().front(); @@ -577,10 +566,6 @@ void draw_scene() { glColor3ubv(gt_color); if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i); - u_int8_t device_color[3]{150, 150, 0}; - glColor3ubv(device_color); - if (show_device_gt) pangolin::glDrawLineStrip(device_pose_t_w_i); - size_t frame_id = show_frame; int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; auto it = vis_map.find(t_ns); @@ -689,6 +674,3 @@ void draw_plots() { } void alignButton() { basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i); } -void alignDeviceButton() { - basalt::alignSVD(device_pose_t_ns, device_pose_t_w_i, gt_t_ns, gt_t_w_i); -}