The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

d/euroc: Improve player tolerance to more datasets

Makes all usable euroc and tumvi datasets stream properly by fixing
some border cases that a couple of datasets had.

+43 -23
+43 -23
src/xrt/drivers/euroc/euroc_player.cpp
··· 20 20 21 21 #include "euroc_driver.h" 22 22 23 + #include <algorithm> 23 24 #include <chrono> 24 25 #include <stdio.h> 25 26 #include <fstream> ··· 38 39 #define CLAMP(X, A, B) (MIN(MAX((X), (A)), (B))) 39 40 40 41 using std::async; 42 + using std::find_if; 41 43 using std::ifstream; 42 44 using std::is_same_v; 43 45 using std::launch; ··· 83 85 struct os_thread_helper play_thread; 84 86 enum u_logging_level log_level; 85 87 struct xrt_fs_mode mode; //!< The only fs mode the euroc dataset provides 86 - bool is_running; //!< Set only at start and stop of frameserver stream 88 + bool is_running; //!< Set only at start, stop and end of frameserver stream 87 89 88 90 //! Contains information about the source dataset; set only at start 89 91 struct ··· 107 109 gt_trajectory *gt; //!< List of all groundtruth poses read from the dataset 108 110 109 111 // Timestamp correction fields (can be disabled through `use_source_ts`) 110 - timepoint_ns base_ts; //!< First imu0 timestamp, samples timestamps are relative to this 112 + timepoint_ns base_ts; //!< First sample timestamp, stream timestamps are relative to this 111 113 timepoint_ns start_ts; //!< When did the dataset started to be played 112 114 timepoint_ns offset_ts; //!< Amount of ns to offset start_ns (pauses, skips, etc) 113 115 ··· 152 154 // Euroc functionality 153 155 154 156 //! Parse and load all IMU samples into `samples`, assumes data.csv is well formed 155 - //! If `ep` is not null, will set `ep->base_ts` with the first timestamp read 156 157 //! If `read_n` > 0, read at most that amount of samples 157 158 //! Returns whether the appropriate data.csv file could be opened 158 159 static bool 159 - euroc_player_preload_imu_data(struct euroc_player *ep, 160 - const string &dataset_path, 161 - imu_samples *samples, 162 - int64_t read_n = -1) 160 + euroc_player_preload_imu_data(const string &dataset_path, imu_samples *samples, int64_t read_n = -1) 163 161 { 164 162 string csv_filename = dataset_path + "/mav0/imu0/data.csv"; 165 163 ifstream fin{csv_filename}; ··· 170 168 constexpr int COLUMN_COUNT = 6; // EuRoC imu columns: ts wx wy wz ax ay az 171 169 string line; 172 170 getline(fin, line); // Skip header line 173 - bool set_base_ts = ep != NULL; 174 171 175 172 while (getline(fin, line) && read_n-- != 0) { 176 173 timepoint_ns timestamp; ··· 184 181 v[k] = stod(line.substr(i + 1, j)); 185 182 } 186 183 187 - // Save first IMU sample timestamp 188 - if (set_base_ts) { 189 - ep->base_ts = timestamp; 190 - set_base_ts = false; 191 - } 192 - 193 184 xrt_imu_sample sample{timestamp, {v[3], v[4], v[5]}, {v[0], v[1], v[2]}}; 194 185 samples->push_back(sample); 195 186 } ··· 283 274 return true; 284 275 } 285 276 277 + //! Trims left and right sequences so that they start and end at the same sample 278 + //! Note that this function does not guarantee that the dataset is free of framedrops. 279 + static void 280 + euroc_player_match_stereo_seqs(struct euroc_player *ep) 281 + { 282 + img_samples ls = *ep->left_imgs; 283 + img_samples rs = *ep->right_imgs; 284 + 285 + // Assumes dataset is properly formatted with monotonically increasing timestamps 286 + timepoint_ns first_ts = MAX(ls.at(0).first, rs.at(0).first); 287 + timepoint_ns last_ts = MIN(ls.back().first, rs.back().first); 288 + 289 + auto is_first = [first_ts](const img_sample &s) { return s.first == first_ts; }; 290 + auto is_last = [last_ts](const img_sample &s) { return s.first == last_ts; }; 291 + 292 + img_samples::iterator lfirst = find_if(ls.begin(), ls.end(), is_first); 293 + img_samples::iterator llast = find_if(ls.begin(), ls.end(), is_last); 294 + EUROC_ASSERT_(lfirst != ls.end() && llast != ls.end()); 295 + 296 + img_samples::iterator rfirst = find_if(rs.begin(), rs.end(), is_first); 297 + img_samples::iterator rlast = find_if(rs.begin(), rs.end(), is_last); 298 + EUROC_ASSERT_(rfirst != rs.end() && rlast != rs.end()); 299 + 300 + ep->left_imgs->assign(lfirst, llast + 1); 301 + ep->right_imgs->assign(rfirst, rlast + 1); 302 + } 303 + 286 304 static void 287 305 euroc_player_preload(struct euroc_player *ep) 288 306 { 289 307 ep->imus->clear(); 290 - euroc_player_preload_imu_data(ep, ep->dataset.path, ep->imus); 308 + euroc_player_preload_imu_data(ep->dataset.path, ep->imus); 291 309 292 310 ep->left_imgs->clear(); 293 311 euroc_player_preload_img_data(ep->dataset.path, ep->left_imgs, true); ··· 295 313 if (ep->dataset.is_stereo) { 296 314 ep->right_imgs->clear(); 297 315 euroc_player_preload_img_data(ep->dataset.path, ep->right_imgs, false); 316 + 317 + euroc_player_match_stereo_seqs(ep); 298 318 } 299 319 300 320 if (ep->dataset.has_gt) { ··· 321 341 time_duration_ns skip_first_ns = skip_first_s * U_TIME_1S_IN_NS; 322 342 timepoint_ns skipped_ts = ep->base_ts + skip_first_ns; 323 343 324 - while (ep->imus->at(ep->imu_seq).timestamp_ns < skipped_ts) { 344 + while (ep->imu_seq < ep->imus->size() && ep->imus->at(ep->imu_seq).timestamp_ns < skipped_ts) { 325 345 ep->imu_seq++; 326 346 } 327 347 328 - while (ep->left_imgs->at(ep->img_seq).first < skipped_ts) { 348 + while (ep->img_seq < ep->left_imgs->size() && ep->left_imgs->at(ep->img_seq).first < skipped_ts) { 329 349 ep->img_seq++; 330 350 } 331 351 ··· 346 366 gt_trajectory _2; 347 367 bool has_right_camera = euroc_player_preload_img_data(ep->dataset.path, &samples, false, 0); 348 368 bool has_left_camera = euroc_player_preload_img_data(ep->dataset.path, &samples, true, 1); 349 - bool has_imu = euroc_player_preload_imu_data(NULL, ep->dataset.path, &_1, 0); 369 + bool has_imu = euroc_player_preload_imu_data(ep->dataset.path, &_1, 0); 350 370 bool has_gt = euroc_player_preload_gt_data(ep->dataset.path, &_2, 0); 351 371 bool is_valid_dataset = has_left_camera && has_imu; 352 372 EUROC_ASSERT(is_valid_dataset, "Invalid dataset %s", path); ··· 381 401 return its; 382 402 } 383 403 384 - //! @returns maps a timestamp to current time (wrt. ep->start_ts) 385 - //! from the original euroc timestamp (uses first imu0 timestamp as base time) 404 + //! @returns maps a dataset timestamp to current time 386 405 static timepoint_ns 387 406 euroc_player_mapped_ts(struct euroc_player *ep, timepoint_ns ts) 388 407 { ··· 583 602 EUROC_INFO(ep, "Starting euroc playback"); 584 603 585 604 euroc_player_preload(ep); 586 - euroc_player_user_skip(ep); 587 - 605 + ep->base_ts = MIN(ep->left_imgs->at(0).first, ep->imus->at(0).timestamp_ns); 588 606 ep->start_ts = os_monotonic_get_ts(); 607 + euroc_player_user_skip(ep); 589 608 590 609 // Push all IMU samples now if requested 591 610 if (ep->playback.send_all_imus_first) { ··· 608 627 // Wait for the end of both streams 609 628 serve_imgs.get(); 610 629 serve_imus.get(); 630 + 631 + ep->is_running = false; 611 632 612 633 EUROC_INFO(ep, "Euroc dataset playback finished"); 613 634 euroc_player_set_ui_state(ep, STREAM_ENDED); ··· 761 782 { 762 783 struct euroc_player *ep = container_of(node, struct euroc_player, node); 763 784 euroc_player_stream_stop(&ep->base); 764 - return; 765 785 } 766 786 767 787 static void