+2 -2

src/xrt/drivers/euroc/euroc_interface.h

reviewed

··· 34 34 */ 35 35 struct euroc_player_playback_config 36 36 { 37 37 - bool stereo; //!< Whether to stream both left and right sinks or only left 37 37 + int cam_count; //!< Number of cameras to stream from the dataset 38 38 bool color; //!< If RGB available but this is false, images will be loaded in grayscale 39 39 bool gt; //!< Whether to send groundtruth data (if available) to the SLAM tracker 40 40 bool skip_perc; //!< Whether @ref skip_first represents percentage or seconds ··· 57 57 struct euroc_player_dataset_info 58 58 { 59 59 char path[256]; 60 60 - bool is_stereo; 60 60 + int cam_count; 61 61 bool is_colored; 62 62 bool has_gt; //!< Whether this dataset has groundtruth data available 63 63 const char *gt_device_name;

+152 -127

src/xrt/drivers/euroc/euroc_player.cpp

reviewed

··· 7 7 * @ingroup drv_euroc 8 8 */ 9 9 10 10 + #include "xrt/xrt_frame.h" 10 11 #include "xrt/xrt_tracking.h" 11 12 #include "xrt/xrt_frameserver.h" 12 13 #include "os/os_threading.h" ··· 25 26 #include <algorithm> 26 27 #include <chrono> 27 28 #include <cstring> 29 29 + #include <stdint.h> 28 30 #include <stdio.h> 29 31 #include <fstream> 30 32 #include <future> ··· 33 35 //! @see euroc_player_playback_config 34 36 DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN) 35 37 DEBUG_GET_ONCE_OPTION(gt_device_name, "EUROC_GT_DEVICE_NAME", nullptr) 36 36 - DEBUG_GET_ONCE_OPTION(stereo, "EUROC_STEREO", nullptr) 38 38 + DEBUG_GET_ONCE_OPTION(cam_count, "EUROC_CAM_COUNT", nullptr) 37 39 DEBUG_GET_ONCE_OPTION(color, "EUROC_COLOR", nullptr) 38 40 DEBUG_GET_ONCE_OPTION(gt, "EUROC_GT", nullptr) 39 41 DEBUG_GET_ONCE_OPTION(skip_first, "EUROC_SKIP_FIRST", "0%") ··· 48 50 49 51 #define EUROC_PLAYER_STR "Euroc Player" 50 52 53 53 + //! Match max cameras to slam sinks max camera count 54 54 + #define EUROC_MAX_CAMS XRT_TRACKING_MAX_SLAM_CAMS 55 55 + 51 56 using std::async; 52 57 using std::find_if; 53 58 using std::ifstream; 54 59 using std::is_same_v; 55 60 using std::launch; 61 61 + using std::max_element; 56 62 using std::pair; 57 63 using std::stof; 58 64 using std::string; 65 65 + using std::to_string; 59 66 using std::vector; 60 67 61 68 using img_sample = pair<timepoint_ns, string>; ··· 85 92 struct xrt_frame_node node; 86 93 87 94 // Sinks 88 88 - struct xrt_frame_sink left_sink; //!< Intermediate sink for left camera frames 89 89 - struct xrt_frame_sink right_sink; //!< Intermediate sink for right camera frames 90 90 - struct xrt_imu_sink imu_sink; //!< Intermediate sink for IMU samples 91 91 - struct xrt_slam_sinks in_sinks; //!< Pointers to intermediate sinks 92 92 - struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks 95 95 + struct xrt_frame_sink cam_sinks[EUROC_MAX_CAMS]; //!< Intermediate sink for each camera frames 96 96 + struct xrt_imu_sink imu_sink; //!< Intermediate sink for IMU samples 97 97 + struct xrt_slam_sinks in_sinks; //!< Pointers to intermediate sinks 98 98 + struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks 93 99 94 100 enum u_logging_level log_level; //!< Log messages with this priority and onwards 95 101 struct euroc_player_dataset_info dataset; //!< Contains information about the source dataset ··· 99 105 timepoint_ns last_pause_ts; //!< Last time the stream was paused 100 106 struct os_thread_helper play_thread; 101 107 102 102 - //! Next frame number to use, index in `left_imgs` and `right_imgs`. 108 108 + //! Next frame number to use, index in `imgs[i]`. 103 109 //! Note that this expects that both cameras provide the same amount of frames. 104 110 //! Furthermore, it is also expected that their timestamps match. 105 105 - uint64_t img_seq; //!< Next frame number to use, index in `{left, right}_imgs` 106 106 - uint64_t imu_seq; //!< Next imu sample number to use, index in `imus` 107 107 - imu_samples *imus; //!< List of all IMU samples read from the dataset 108 108 - img_samples *left_imgs; //!< List of all image names to read from the dataset 109 109 - img_samples *right_imgs; //!< List of all image names to read from the dataset 110 110 - gt_trajectory *gt; //!< List of all groundtruth poses read from the dataset 111 111 + uint64_t img_seq; //!< Next frame number to use, index in `imgs[i]` 112 112 + uint64_t imu_seq; //!< Next imu sample number to use, index in `imus` 113 113 + imu_samples *imus; //!< List of all IMU samples read from the dataset 114 114 + vector<img_samples> *imgs; //!< List of all image names to read from the dataset per camera 115 115 + gt_trajectory *gt; //!< List of all groundtruth poses read from the dataset 111 116 112 117 // Timestamp correction fields (can be disabled through `use_source_ts`) 113 118 timepoint_ns base_ts; //!< First sample timestamp, stream timestamps are relative to this ··· 119 124 struct u_var_button start_btn; 120 125 struct u_var_button pause_btn; 121 126 char progress_text[128]; 122 122 - struct u_sink_debug ui_left_sink; //!< Sink to display left frames in UI 123 123 - struct u_sink_debug ui_right_sink; //!< Sink to display right frames in UI 124 124 - struct m_ff_vec3_f32 *gyro_ff; //!< Used for displaying IMU data 125 125 - struct m_ff_vec3_f32 *accel_ff; //!< Same as `gyro_ff` 127 127 + struct u_sink_debug ui_cam_sinks[EUROC_MAX_CAMS]; //!< Sinks to display cam frames in UI 128 128 + struct m_ff_vec3_f32 *gyro_ff; //!< Used for displaying IMU data 129 129 + struct m_ff_vec3_f32 *accel_ff; //!< Same as `gyro_ff` 126 130 }; 127 131 128 132 static void ··· 235 239 //! If read_n > 0, read at most that amount of samples 236 240 //! Returns whether the appropriate data.csv file could be opened 237 241 static bool 238 238 - euroc_player_preload_img_data(const string &dataset_path, img_samples *samples, bool is_left, int64_t read_n = -1) 242 242 + euroc_player_preload_img_data(const string &dataset_path, img_samples &samples, size_t cam_id, int64_t read_n = -1) 239 243 { 240 244 // Parse image data, assumes data.csv is well formed 241 241 - string cam_name = is_left ? "cam0" : "cam1"; 245 245 + string cam_name = "cam" + to_string(cam_id); 242 246 string imgs_path = dataset_path + "/mav0/" + cam_name + "/data"; 243 247 string csv_filename = dataset_path + "/mav0/" + cam_name + "/data.csv"; 244 248 ifstream fin{csv_filename}; ··· 260 264 261 265 string img_name = imgs_path + "/" + img_name_tail; 262 266 img_sample sample{timestamp, img_name}; 263 263 - samples->push_back(sample); 267 267 + samples.push_back(sample); 264 268 } 265 269 return true; 266 270 } 267 271 268 268 - //! Trims left and right sequences so that they start and end at the same sample 269 269 - //! Note that this function does not guarantee that the dataset is free of framedrops. 272 272 + //! Trims cameras sequences so that they all start and end at the same sample 273 273 + //! Note that this function does not guarantee that the dataset is free of framedrops 274 274 + //! and it assumes it is properly formatted with monotonically increasing timestamps. 270 275 static void 271 271 - euroc_player_match_stereo_seqs(struct euroc_player *ep) 276 276 + euroc_player_match_cams_seqs(struct euroc_player *ep) 272 277 { 273 273 - img_samples ls = *ep->left_imgs; 274 274 - img_samples rs = *ep->right_imgs; 278 278 + // Find newest first timestamp and oldest last timestamp 279 279 + timepoint_ns first_ts = INT64_MIN; 280 280 + timepoint_ns last_ts = INT64_MAX; 281 281 + for (const img_samples &imgs : *ep->imgs) { 282 282 + EUROC_ASSERT(!imgs.empty(), "Camera with no samples"); 275 283 276 276 - // Assumes dataset is properly formatted with monotonically increasing timestamps 277 277 - timepoint_ns first_ts = MAX(ls.at(0).first, rs.at(0).first); 278 278 - timepoint_ns last_ts = MIN(ls.back().first, rs.back().first); 284 284 + timepoint_ns cam_first_ts = imgs.front().first; 285 285 + if (cam_first_ts > first_ts) { 286 286 + first_ts = cam_first_ts; 287 287 + } 288 288 + 289 289 + timepoint_ns cam_last_ts = imgs.back().first; 290 290 + if (cam_last_ts < last_ts) { 291 291 + last_ts = cam_last_ts; 292 292 + } 293 293 + } 279 294 280 295 auto is_first = [first_ts](const img_sample &s) { return s.first == first_ts; }; 281 296 auto is_last = [last_ts](const img_sample &s) { return s.first == last_ts; }; 282 297 283 283 - img_samples::iterator lfirst = find_if(ls.begin(), ls.end(), is_first); 284 284 - img_samples::iterator llast = find_if(ls.begin(), ls.end(), is_last); 285 285 - EUROC_ASSERT_(lfirst != ls.end() && llast != ls.end()); 286 286 - 287 287 - img_samples::iterator rfirst = find_if(rs.begin(), rs.end(), is_first); 288 288 - img_samples::iterator rlast = find_if(rs.begin(), rs.end(), is_last); 289 289 - EUROC_ASSERT_(rfirst != rs.end() && rlast != rs.end()); 290 290 - 291 291 - ep->left_imgs->assign(lfirst, llast + 1); 292 292 - ep->right_imgs->assign(rfirst, rlast + 1); 298 298 + for (img_samples &imgs : *ep->imgs) { 299 299 + img_samples::iterator new_first = find_if(imgs.begin(), imgs.end(), is_first); 300 300 + img_samples::iterator new_last = find_if(imgs.begin(), imgs.end(), is_last); 301 301 + EUROC_ASSERT_(new_first != imgs.end() && new_last != imgs.end()); 302 302 + imgs.assign(new_first, new_last + 1); 303 303 + } 293 304 } 294 305 295 306 static void ··· 298 309 ep->imus->clear(); 299 310 euroc_player_preload_imu_data(ep->dataset.path, ep->imus); 300 311 301 301 - ep->left_imgs->clear(); 302 302 - euroc_player_preload_img_data(ep->dataset.path, ep->left_imgs, true); 312 312 + for (size_t i = 0; i < ep->imgs->size(); i++) { 313 313 + ep->imgs->at(i).clear(); 314 314 + euroc_player_preload_img_data(ep->dataset.path, ep->imgs->at(i), i); 315 315 + } 303 316 304 304 - if (ep->dataset.is_stereo) { 305 305 - ep->right_imgs->clear(); 306 306 - euroc_player_preload_img_data(ep->dataset.path, ep->right_imgs, false); 307 307 - 308 308 - euroc_player_match_stereo_seqs(ep); 309 309 - } 317 317 + euroc_player_match_cams_seqs(ep); 310 318 311 319 if (ep->dataset.has_gt) { 312 320 ep->gt->clear(); ··· 322 330 float skip_first_s = 0; 323 331 if (ep->playback.skip_perc) { 324 332 float skip_percentage = ep->playback.skip_first; 325 325 - timepoint_ns last_ts = MAX(ep->left_imgs->back().first, ep->imus->back().timestamp_ns); 333 333 + timepoint_ns last_ts = MAX(ep->imgs->at(0).back().first, ep->imus->back().timestamp_ns); 326 334 double dataset_length_s = (last_ts - ep->base_ts) / U_TIME_1S_IN_NS; 327 335 skip_first_s = dataset_length_s * skip_percentage / 100.0f; 328 336 } else { ··· 336 344 ep->imu_seq++; 337 345 } 338 346 339 339 - while (ep->img_seq < ep->left_imgs->size() && ep->left_imgs->at(ep->img_seq).first < skipped_ts) { 347 347 + while (ep->img_seq < ep->imgs->at(0).size() && ep->imgs->at(0).at(ep->img_seq).first < skipped_ts) { 340 348 ep->img_seq++; 341 349 } 342 350 ··· 348 356 static void 349 357 euroc_player_fill_dataset_info(const char *path, euroc_player_dataset_info *dataset) 350 358 { 351 351 - snprintf(dataset->path, sizeof(dataset->path), "%s", path); 359 359 + (void)snprintf(dataset->path, sizeof(dataset->path), "%s", path); 352 360 img_samples samples; 353 361 imu_samples _1; 354 362 gt_trajectory _2; 355 355 - bool has_right_camera = euroc_player_preload_img_data(dataset->path, &samples, false, 0); 356 356 - bool has_left_camera = euroc_player_preload_img_data(dataset->path, &samples, true, 1); 363 363 + 364 364 + size_t i = 0; 365 365 + bool has_camera = euroc_player_preload_img_data(dataset->path, samples, i, 1); 366 366 + while ((has_camera = euroc_player_preload_img_data(dataset->path, samples, ++i, 0))) { 367 367 + } 368 368 + size_t cam_count = i; 369 369 + EUROC_ASSERT(cam_count <= EUROC_MAX_CAMS, "Increase EUROC_MAX_CAMS (dataset with %zu cams)", cam_count); 370 370 + 357 371 bool has_imu = euroc_player_preload_imu_data(dataset->path, &_1, 0); 358 372 bool has_gt = euroc_player_preload_gt_data(dataset->path, &dataset->gt_device_name, &_2, 0); 359 359 - bool is_valid_dataset = has_left_camera && has_imu; 373 373 + bool is_valid_dataset = cam_count > 0 && has_imu; 360 374 EUROC_ASSERT(is_valid_dataset, "Invalid dataset %s", path); 361 375 362 362 - cv::Mat first_left_img = cv::imread(samples[0].second, cv::IMREAD_ANYCOLOR); 363 363 - dataset->is_stereo = has_right_camera; 364 364 - dataset->is_colored = first_left_img.channels() == 3; 376 376 + cv::Mat first_cam0_img = cv::imread(samples[0].second, cv::IMREAD_ANYCOLOR); 377 377 + dataset->cam_count = (int)cam_count; 378 378 + dataset->is_colored = first_cam0_img.channels() == 3; 365 379 dataset->has_gt = has_gt; 366 366 - dataset->width = first_left_img.cols; 367 367 - dataset->height = first_left_img.rows; 380 380 + dataset->width = first_cam0_img.cols; 381 381 + dataset->height = first_cam0_img.rows; 368 382 } 369 383 370 384 ··· 408 422 } 409 423 410 424 static void 411 411 - euroc_player_load_next_frame(struct euroc_player *ep, bool is_left, struct xrt_frame *&xf) 425 425 + euroc_player_load_next_frame(struct euroc_player *ep, int cam_index, struct xrt_frame *&xf) 412 426 { 413 427 using xrt::auxiliary::tracking::FrameMat; 414 414 - img_sample sample = is_left ? ep->left_imgs->at(ep->img_seq) : ep->right_imgs->at(ep->img_seq); 428 428 + img_sample sample = ep->imgs->at(cam_index).at(ep->img_seq); 415 429 ep->playback.scale = CLAMP(ep->playback.scale, 1.0 / 16, 4); 416 430 417 431 // Load will be influenced by these playback options ··· 421 435 // Load image from disk 422 436 timepoint_ns timestamp = euroc_player_mapped_playback_ts(ep, sample.first); 423 437 string img_name = sample.second; 424 424 - EUROC_TRACE(ep, "%s img t = %ld filename = %s", is_left ? "left" : "right", timestamp, img_name.c_str()); 438 438 + EUROC_TRACE(ep, "cam%d img t = %ld filename = %s", cam_index, timestamp, img_name.c_str()); 425 439 cv::ImreadModes read_mode = allow_color ? cv::IMREAD_ANYCOLOR : cv::IMREAD_GRAYSCALE; 426 440 cv::Mat img = cv::imread(img_name, read_mode); // If colored, reads in BGR order 427 441 ··· 450 464 static void 451 465 euroc_player_push_next_frame(struct euroc_player *ep) 452 466 { 453 453 - bool stereo = ep->playback.stereo; 467 467 + int cam_count = ep->playback.cam_count; 468 468 + 469 469 + vector<xrt_frame *> xfs(cam_count, nullptr); 470 470 + for (int i = 0; i < cam_count; i++) { 471 471 + euroc_player_load_next_frame(ep, i, xfs[i]); 472 472 + } 454 473 455 455 - struct xrt_frame *left_xf = NULL; 456 456 - struct xrt_frame *right_xf = NULL; 457 457 - euroc_player_load_next_frame(ep, true, left_xf); 458 458 - if (stereo) { 459 459 - // TODO: Some SLAM systems expect synced frames, but that's not an 460 460 - // EuRoC requirement. Adapt to work with unsynced datasets too. 461 461 - euroc_player_load_next_frame(ep, false, right_xf); 462 462 - EUROC_ASSERT(left_xf->timestamp == right_xf->timestamp, "Unsynced stereo frames"); 474 474 + // TODO: Some SLAM systems expect synced frames, but that's not an 475 475 + // EuRoC requirement. Adapt to work with unsynced datasets too. 476 476 + for (int i = 1; i < cam_count; i++) { 477 477 + EUROC_ASSERT(xfs[i - 1]->timestamp == xfs[i]->timestamp, "Unsynced frames"); 463 478 } 479 479 + 464 480 ep->img_seq++; 465 481 466 466 - xrt_sink_push_frame(ep->in_sinks.cams[0], left_xf); 467 467 - if (stereo) { 468 468 - xrt_sink_push_frame(ep->in_sinks.cams[1], right_xf); 482 482 + for (int i = 0; i < cam_count; i++) { 483 483 + xrt_sink_push_frame(ep->in_sinks.cams[i], xfs[i]); 469 484 } 470 485 471 471 - xrt_frame_reference(&left_xf, NULL); 472 472 - xrt_frame_reference(&right_xf, NULL); 486 486 + for (int i = 0; i < cam_count; i++) { 487 487 + xrt_frame_reference(&xfs[i], NULL); 488 488 + } 473 489 474 474 - snprintf(ep->progress_text, sizeof(ep->progress_text), "Frames %lu/%lu - IMUs %lu/%lu", ep->img_seq, 475 475 - ep->left_imgs->size(), ep->imu_seq, ep->imus->size()); 490 490 + size_t fcount = ep->imgs->at(0).size(); 491 491 + (void)snprintf(ep->progress_text, sizeof(ep->progress_text), "Playback %.2f%% - Frame %lu/%lu - IMU %lu/%lu", 492 492 + float(ep->img_seq) / float(fcount) * 100, ep->img_seq, fcount, ep->imu_seq, ep->imus->size()); 476 493 477 494 if (ep->playback.print_progress) { 478 478 - printf("Playback %.2f%% - Frame %lu/%lu\r", float(ep->img_seq) / float(ep->left_imgs->size()) * 100, 479 479 - ep->img_seq, ep->left_imgs->size()); 480 480 - fflush(stdout); 495 495 + printf("%s\r", ep->progress_text); 496 496 + (void)fflush(stdout); 481 497 } 482 498 } 483 499 ··· 509 525 if constexpr (is_same_v<SamplesType, imu_samples>) { 510 526 return ep->imus->at(ep->imu_seq).timestamp_ns; 511 527 } else { 512 512 - return ep->left_imgs->at(ep->img_seq).first; 528 528 + return ep->imgs->at(0).at(ep->img_seq).first; 513 529 } 514 530 } 515 531 ··· 550 566 if constexpr (is_imu) { 551 567 samples = ep->imus; 552 568 } else { 553 553 - samples = ep->left_imgs; 569 569 + samples = &ep->imgs->at(0); 554 570 } 555 571 uint64_t *sample_seq = is_imu ? &ep->imu_seq : &ep->img_seq; 556 572 auto push_next_sample = is_imu ? euroc_player_push_next_imu : euroc_player_push_next_frame; ··· 588 604 EUROC_INFO(ep, "Starting euroc playback"); 589 605 590 606 euroc_player_preload(ep); 591 591 - ep->base_ts = MIN(ep->left_imgs->at(0).first, ep->imus->at(0).timestamp_ns); 607 607 + ep->base_ts = MIN(ep->imgs->at(0).at(0).first, ep->imus->at(0).timestamp_ns); 592 608 ep->start_ts = os_monotonic_get_ts(); 593 609 euroc_player_user_skip(ep); 594 610 ··· 656 672 return false; 657 673 } 658 674 659 659 - static void 660 660 - receive_left_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) 661 661 - { 662 662 - struct euroc_player *ep = container_of(sink, struct euroc_player, left_sink); 663 663 - EUROC_TRACE(ep, "left img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp); 664 664 - u_sink_debug_push_frame(&ep->ui_left_sink, xf); 665 665 - if (ep->out_sinks.cams[0]) { 666 666 - xrt_sink_push_frame(ep->out_sinks.cams[0], xf); 675 675 + #define DEFINE_RECEIVE_CAM(cam_id) \ 676 676 + static void receive_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *xf) \ 677 677 + { \ 678 678 + struct euroc_player *ep = container_of(sink, struct euroc_player, cam_sinks[cam_id]); \ 679 679 + EUROC_TRACE(ep, "cam%d img t=%ld source_t=%ld", cam_id, xf->timestamp, xf->source_timestamp); \ 680 680 + u_sink_debug_push_frame(&ep->ui_cam_sinks[cam_id], xf); \ 681 681 + if (ep->out_sinks.cams[cam_id]) { \ 682 682 + xrt_sink_push_frame(ep->out_sinks.cams[cam_id], xf); \ 683 683 + } \ 667 684 } 668 668 - } 685 685 + 686 686 + 687 687 + DEFINE_RECEIVE_CAM(0) 688 688 + DEFINE_RECEIVE_CAM(1) 689 689 + DEFINE_RECEIVE_CAM(2) 690 690 + DEFINE_RECEIVE_CAM(3) 691 691 + DEFINE_RECEIVE_CAM(4) 669 692 670 670 - static void 671 671 - receive_right_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) 672 672 - { 673 673 - struct euroc_player *ep = container_of(sink, struct euroc_player, right_sink); 674 674 - EUROC_TRACE(ep, "right img t=%ld source_t=%ld", xf->timestamp, xf->source_timestamp); 675 675 - u_sink_debug_push_frame(&ep->ui_right_sink, xf); 676 676 - if (ep->out_sinks.cams[1]) { 677 677 - xrt_sink_push_frame(ep->out_sinks.cams[1], xf); 678 678 - } 679 679 - } 693 693 + //! Be sure to define the same number of definition as EUROC_MAX_CAMS and to add them to `receive_cam`. 694 694 + static void (*receive_cam[EUROC_MAX_CAMS])(struct xrt_frame_sink *, struct xrt_frame *) = { 695 695 + receive_cam0, // 696 696 + receive_cam1, // 697 697 + receive_cam2, // 698 698 + receive_cam3, // 699 699 + receive_cam4, // 700 700 + }; 680 701 681 702 static void 682 703 receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) ··· 715 736 if (xs == NULL && capture_type == XRT_FS_CAPTURE_TYPE_TRACKING) { 716 737 EUROC_INFO(ep, "Starting Euroc Player in tracking mode"); 717 738 if (ep->out_sinks.cams[0] == NULL) { 718 718 - EUROC_WARN(ep, "No left sink provided, will keep running but tracking is unlikely to work"); 739 739 + EUROC_WARN(ep, "No cam0 sink provided, will keep running but tracking is unlikely to work"); 719 740 } 720 741 if (ep->playback.play_from_start) { 721 742 euroc_player_start_btn_cb(ep); 722 743 } 723 744 } else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) { 724 724 - EUROC_INFO(ep, "Starting Euroc Player in calibration mode, will stream only left frames right away"); 745 745 + EUROC_INFO(ep, "Starting Euroc Player in calibration mode, will stream only cam0 frames right away"); 725 746 ep->out_sinks.cams[0] = xs; 726 747 euroc_player_start_btn_cb(ep); 727 748 } else { ··· 777 798 778 799 delete ep->gt; 779 800 delete ep->imus; 780 780 - delete ep->left_imgs; 781 781 - delete ep->right_imgs; 801 801 + delete ep->imgs; 782 802 783 803 u_var_remove_root(ep); 784 784 - u_sink_debug_destroy(&ep->ui_left_sink); 785 785 - u_sink_debug_destroy(&ep->ui_right_sink); 804 804 + for (int i = 0; i < ep->dataset.cam_count; i++) { 805 805 + u_sink_debug_destroy(&ep->ui_cam_sinks[i]); 806 806 + } 786 807 m_ff_vec3_f32_free(&ep->gyro_ff); 787 808 m_ff_vec3_f32_free(&ep->accel_ff); 788 809 ··· 854 875 euroc_player_setup_gui(struct euroc_player *ep) 855 876 { 856 877 // Set sinks to display in UI 857 857 - u_sink_debug_init(&ep->ui_left_sink); 858 858 - u_sink_debug_init(&ep->ui_right_sink); 878 878 + for (int i = 0; i < ep->dataset.cam_count; i++) { 879 879 + u_sink_debug_init(&ep->ui_cam_sinks[i]); 880 880 + } 859 881 m_ff_vec3_f32_alloc(&ep->gyro_ff, 1000); 860 882 m_ff_vec3_f32_alloc(&ep->accel_ff, 1000); 861 883 ··· 876 898 877 899 u_var_add_gui_header(ep, NULL, "Playback Options"); 878 900 u_var_add_ro_text(ep, "Set these before starting the stream", "Note"); 879 879 - u_var_add_bool(ep, &ep->playback.stereo, "Stereo (if available)"); 901 901 + u_var_add_i32(ep, &ep->playback.cam_count, "Use N cams (if available)"); 880 902 u_var_add_bool(ep, &ep->playback.color, "Color (if available)"); 881 903 u_var_add_bool(ep, &ep->playback.gt, "Groundtruth (if available)"); 882 904 u_var_add_bool(ep, &ep->playback.skip_perc, "Skip percentage, otherwise skips seconds"); ··· 890 912 u_var_add_gui_header(ep, NULL, "Streams"); 891 913 u_var_add_ro_ff_vec3_f32(ep, ep->gyro_ff, "Gyroscope"); 892 914 u_var_add_ro_ff_vec3_f32(ep, ep->accel_ff, "Accelerometer"); 893 893 - u_var_add_sink_debug(ep, &ep->ui_left_sink, "Left Camera"); 894 894 - u_var_add_sink_debug(ep, &ep->ui_right_sink, "Right Camera"); 915 915 + for (int i = 0; i < ep->dataset.cam_count; i++) { 916 916 + char label[] = "Camera NNNN"; 917 917 + (void)snprintf(label, sizeof(label), "Camera %d", i); 918 918 + u_var_add_sink_debug(ep, &ep->ui_cam_sinks[i], label); 919 919 + } 895 920 } 896 921 897 922 extern "C" void ··· 902 927 euroc_player_fill_dataset_info(dataset_path, &dataset); 903 928 904 929 struct euroc_player_playback_config playback = {}; 905 905 - const char *stereo = debug_get_option_stereo(); 930 930 + const char *cam_count = debug_get_option_cam_count(); 906 931 const char *color = debug_get_option_color(); 907 932 const char *gt = debug_get_option_gt(); 908 933 const char *skip_option = debug_get_option_skip_first(); 909 909 - playback.stereo = stereo == nullptr ? dataset.is_stereo : debug_string_to_bool(stereo); 934 934 + playback.cam_count = (int)debug_string_to_num(cam_count, dataset.cam_count); 910 935 playback.color = color == nullptr ? dataset.is_colored : debug_string_to_bool(color); 911 936 playback.gt = gt == nullptr ? dataset.has_gt : debug_string_to_bool(gt); 912 937 playback.skip_perc = string(skip_option).back() == '%'; ··· 955 980 // xrt_fs interface as it will be managed through two different sinks. 956 981 XRT_STEREO_FORMAT_NONE, 957 982 }; 958 958 - EUROC_INFO(ep, "dataset information\n\tpath: %s\n\tis_stereo: %d, is_colored: %d, width: %d, height: %d", 959 959 - ep->dataset.path, ep->dataset.is_stereo, ep->dataset.is_colored, ep->dataset.width, 983 983 + EUROC_INFO(ep, "dataset information\n\tpath: %s\n\tcam_count: %d, is_colored: %d, width: %d, height: %d", 984 984 + ep->dataset.path, ep->dataset.cam_count, ep->dataset.is_colored, ep->dataset.width, 960 985 ep->dataset.height); 961 986 962 987 // Using pointers to not mix vector with a C-compatible struct 963 988 ep->gt = new gt_trajectory{}; 964 989 ep->imus = new imu_samples{}; 965 965 - ep->left_imgs = new img_samples{}; 966 966 - ep->right_imgs = new img_samples{}; 990 990 + ep->imgs = new vector<img_samples>(ep->dataset.cam_count); 967 991 968 992 euroc_player_setup_gui(ep); 969 993 970 970 - ep->left_sink.push_frame = receive_left_frame; 971 971 - ep->right_sink.push_frame = receive_right_frame; 994 994 + EUROC_ASSERT(receive_cam[ARRAY_SIZE(receive_cam) - 1] != nullptr, "See `receive_cam` docs"); 995 995 + ep->in_sinks.cam_count = ep->dataset.cam_count; 996 996 + for (int i = 0; i < ep->dataset.cam_count; i++) { 997 997 + ep->cam_sinks[i].push_frame = receive_cam[i]; 998 998 + ep->in_sinks.cams[i] = &ep->cam_sinks[i]; 999 999 + } 972 1000 ep->imu_sink.push_imu = receive_imu_sample; 973 973 - ep->in_sinks.cam_count = 2; 974 974 - ep->in_sinks.cams[0] = &ep->left_sink; 975 975 - ep->in_sinks.cams[1] = &ep->right_sink; 976 1001 ep->in_sinks.imu = &ep->imu_sink; 977 1002 978 1003 struct xrt_fs *xfs = &ep->base; ··· 983 1008 xfs->stream_stop = euroc_player_stream_stop; 984 1009 xfs->is_running = euroc_player_is_running; 985 1010 986 986 - snprintf(xfs->name, sizeof(xfs->name), EUROC_PLAYER_STR); 987 987 - snprintf(xfs->product, sizeof(xfs->product), EUROC_PLAYER_STR " Product"); 988 988 - snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), EUROC_PLAYER_STR " Manufacturer"); 989 989 - snprintf(xfs->serial, sizeof(xfs->serial), EUROC_PLAYER_STR " Serial"); 1011 1011 + (void)snprintf(xfs->name, sizeof(xfs->name), EUROC_PLAYER_STR); 1012 1012 + (void)snprintf(xfs->product, sizeof(xfs->product), EUROC_PLAYER_STR " Product"); 1013 1013 + (void)snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), EUROC_PLAYER_STR " Manufacturer"); 1014 1014 + (void)snprintf(xfs->serial, sizeof(xfs->serial), EUROC_PLAYER_STR " Serial"); 990 1015 xfs->source_id = 0xECD0FEED; 991 1016 992 1017 struct xrt_frame_node *xfn = &ep->node;

+1

src/xrt/drivers/euroc/euroc_runner.c

reviewed

··· 92 92 { 93 93 struct euroc_player_config *ep_config = make_euroc_player_config(euroc_path); 94 94 struct t_slam_tracker_config *st_config = make_slam_tracker_config(slam_config, output_path); 95 95 + st_config->cam_count = ep_config->dataset.cam_count; 95 96 96 97 // Frame context that will manage SLAM tracker and euroc player lifetimes 97 98 struct xrt_frame_context xfctx = {0};

+5

src/xrt/state_trackers/prober/p_tracking.c

reviewed

··· 29 29 #include <string.h> 30 30 31 31 #ifdef XRT_BUILD_DRIVER_EUROC 32 32 + #include "euroc/euroc_interface.h" 32 33 #include "util/u_debug.h" 33 34 DEBUG_GET_ONCE_OPTION(euroc_path, "EUROC_PATH", NULL) 34 35 #endif ··· 254 255 xrt_prober_open_video_device(&fact->p->base, NULL, &fact->xfctx, &fact->xfs); 255 256 assert(fact->xfs->source_id == 0xECD0FEED && "xfs is not Euroc, unsynced open_video_device?"); 256 257 258 258 + struct euroc_player_config ep_config; 259 259 + euroc_player_fill_default_config_for(&ep_config, debug_get_option_euroc_path()); 260 260 + 257 261 #ifdef XRT_FEATURE_SLAM 258 262 struct t_slam_tracker_config st_config; 259 263 t_slam_fill_default_config(&st_config); 264 264 + st_config.cam_count = ep_config.dataset.cam_count; 260 265 261 266 int ret = t_slam_create(&fact->xfctx, &st_config, &fact->xts, &sinks); 262 267 if (ret != 0) {