diff --git a/src/xrt/drivers/euroc/euroc_player.cpp b/src/xrt/drivers/euroc/euroc_player.cpp index 733aa6683..df0e754bb 100644 --- a/src/xrt/drivers/euroc/euroc_player.cpp +++ b/src/xrt/drivers/euroc/euroc_player.cpp @@ -25,6 +25,8 @@ #include #include +DEBUG_GET_ONCE_OPTION(euroc_gt_device_name, "EUROC_GT_DEVICE_NAME", NULL) + #define EUROC_PLAYER_STR "Euroc Player" #define CLAMP(X, A, B) (MIN(MAX((X), (A)), (B))) @@ -37,8 +39,11 @@ using std::string; using std::vector; using img_sample = pair; +using gt_entry = pair; + using imu_samples = vector; using img_samples = vector; +using gt_trajectory = vector; enum euroc_player_ui_state { @@ -78,6 +83,7 @@ struct euroc_player char path[256]; bool is_stereo; bool is_colored; + bool has_gt; //!< Whether this dataset has groundtruth data available uint32_t width; uint32_t height; } dataset; @@ -90,6 +96,7 @@ struct euroc_player imu_samples *imus; //!< List of all IMU samples read from the dataset img_samples *left_imgs; //!< List of all image names to read from the dataset img_samples *right_imgs; //!< List of all image names to read from the dataset + gt_trajectory *gt; //!< List of all groundtruth poses read from the dataset // Timestamp correction fields (can be disabled through `use_source_ts`) timepoint_ns base_ts; //!< First imu0 timestamp, samples timestamps are relative to this @@ -176,6 +183,59 @@ euroc_player_preload_imu_data(struct euroc_player *ep, return true; } +//! Parse and load ground truth trajectory into `trajectory`. +//! If read_n > 0, read at most that amount of samples +//! Returns whether the appropriate data.csv file could be opened +//! @note Groundtruth data can come from different devices so we use the first of: +//! 1. vicon0: found in euroc "vicon room" datasets +//! 2. mocap0: found in TUM-VI datasets with euroc format +//! 3. state_groundtruth_estimate0: found in euroc as a postprocessed ground truth (we only use first 7 columns) +//! 4. leica0: found in euroc "machine hall" datasets, only positional ground truth +//! You can also add your own gt device name with the EUROC_GT_DEVICE_NAME environment variable. +static bool +euroc_player_preload_gt_data(const string &dataset_path, gt_trajectory *trajectory, int64_t read_n = -1) +{ + vector gt_devices = {"vicon0", "mocap0", "state_groundtruth_estimate0", "leica0"}; + const char *user_gtdev = debug_get_option_euroc_gt_device_name(); + if (user_gtdev) { + gt_devices.insert(gt_devices.begin(), user_gtdev); + } + + ifstream fin; + for (const string &device : gt_devices) { + string csv_filename = dataset_path + "/mav0/" + device + "/data.csv"; + fin = ifstream{csv_filename}; + if (fin.is_open()) { + break; + } + } + + if (!fin.is_open()) { + return false; + } + + constexpr int COLUMN_COUNT = 7; // EuRoC groundtruth columns: ts px py pz qw qx qy qz + string line; + getline(fin, line); // Skip header line + + while (getline(fin, line) && read_n-- != 0) { + timepoint_ns timestamp; + float v[COLUMN_COUNT] = {0, 0, 0, 1, 0, 0, 0}; // Set identity orientation for leica0 + size_t i = 0; + size_t j = line.find(','); + timestamp = stoll(line.substr(i, j)); + for (size_t k = 0; k < COLUMN_COUNT && j != string::npos; k++) { + i = j; + j = line.find(',', i + 1); + v[k] = stof(line.substr(i + 1, j)); + } + + xrt_pose pose = {{v[4], v[5], v[6], v[3]}, {v[0], v[1], v[2]}}; + trajectory->emplace_back(timestamp, pose); + } + return true; +} + //! Parse and load image names and timestamps into `samples` //! If read_n > 0, read at most that amount of samples //! Returns whether the appropriate data.csv file could be opened @@ -223,6 +283,11 @@ euroc_player_preload(struct euroc_player *ep) ep->right_imgs->clear(); euroc_player_preload_img_data(ep->dataset.path, ep->right_imgs, false); } + + if (ep->dataset.has_gt) { + ep->gt->clear(); + euroc_player_preload_gt_data(ep->dataset.path, ep->gt); + } } //! Skips the first seconds of the dataset as specified by the user @@ -253,16 +318,19 @@ euroc_player_fill_dataset_info(struct euroc_player *ep, const char *path) snprintf(ep->dataset.path, sizeof(ep->dataset.path), "%s", path); img_samples samples; - imu_samples _; + imu_samples _1; + gt_trajectory _2; bool has_right_camera = euroc_player_preload_img_data(ep->dataset.path, &samples, false, 0); bool has_left_camera = euroc_player_preload_img_data(ep->dataset.path, &samples, true, 1); - bool has_imu = euroc_player_preload_imu_data(NULL, ep->dataset.path, &_, 0); + bool has_imu = euroc_player_preload_imu_data(NULL, ep->dataset.path, &_1, 0); + bool has_gt = euroc_player_preload_gt_data(ep->dataset.path, &_2, 0); bool is_valid_dataset = has_left_camera && has_imu; EUROC_ASSERT(is_valid_dataset, "Invalid dataset %s", path); cv::Mat first_left_img = cv::imread(samples[0].second, cv::IMREAD_ANYCOLOR); ep->dataset.is_stereo = has_right_camera; ep->dataset.is_colored = first_left_img.channels() == 3; + ep->dataset.has_gt = has_gt; ep->dataset.width = first_left_img.cols; ep->dataset.height = first_left_img.rows; EUROC_INFO(ep, "dataset information\n\tpath: %s\n\tis_stereo: %d, is_colored: %d, width: %d, height: %d", @@ -645,6 +713,7 @@ euroc_player_destroy(struct xrt_frame_node *node) { struct euroc_player *ep = container_of(node, struct euroc_player, node); + delete ep->gt; delete ep->imus; delete ep->left_imgs; delete ep->right_imgs; @@ -775,7 +844,8 @@ euroc_player_create(struct xrt_frame_context *xfctx, const char *path) XRT_STEREO_FORMAT_NONE, }; - // Using pointers to not mix std::vector with a C-compatible struct + // Using pointers to not mix vector with a C-compatible struct + ep->gt = new gt_trajectory{}; ep->imus = new imu_samples{}; ep->left_imgs = new img_samples{}; ep->right_imgs = new img_samples{};