t/slam: Allow timing extension to be toggled on runtime

+46 -16

2 changed files

expand all

src

xrt

auxiliary

tracking

t_tracker_slam.cpp

t_tracking.h

+45 -16

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

··· 68 68 DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_SP_SO_IA_SL)) 69 69 DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false) 70 70 DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/") 71 + DEBUG_GET_ONCE_BOOL_OPTION(slam_timing_stat, "SLAM_TIMING_STAT", true) 71 72 72 73 //! Namespace for the interface to the external SLAM tracking system 73 74 namespace xrt::auxiliary::tracking::slam { 74 75 constexpr int UI_TIMING_POSE_COUNT = 192; 75 76 constexpr int UI_GTDIFF_POSE_COUNT = 192; 77 + constexpr int NUM_CAMS = 2; //!< This should be used as little as possible to allow setups that are not stereo 76 78 77 79 using std::ifstream; 78 80 using std::make_shared; 79 - using std::make_unique; 80 81 using std::map; 81 82 using std::ofstream; 82 83 using std::shared_ptr; 83 84 using std::string; 84 85 using std::to_string; 85 - using std::unique_ptr; 86 86 using std::vector; 87 87 using std::filesystem::create_directories; 88 88 using Trajectory = map<timepoint_ns, xrt_pose>; ··· 367 367 //! Tracker timing info for performance evaluation 368 368 struct 369 369 { 370 + bool ext_available = false; //!< Whether the SLAM system supports the timing extension 371 + bool ext_enabled = false; //!< Whether the timing extension is enabled 370 372 float dur_ms[UI_TIMING_POSE_COUNT]; //!< Timing durations in ms 371 373 int idx = 0; //!< Index of latest entry in @ref dur_ms 372 374 u_var_combo start_ts; //!< UI combo box to select initial timing measurement ··· 374 376 int start_ts_idx; //!< Selected initial timing measurement in @ref start_ts 375 377 int end_ts_idx; //!< Selected final timing measurement in @ref end_ts 376 378 struct u_var_timing ui; //!< Realtime UI for tracker durations 377 - bool ext_enabled = false; //!< Whether the SLAM system supports the timing extension 378 379 vector<string> columns; //!< Column names of the measured timestamps 379 380 string joined_columns; //!< Column names as a null separated string 381 + struct u_var_button enable_btn; //!< Toggle tracker timing reports 380 382 } timing; 381 383 382 384 //! Ground truth related fields ··· 401 403 static void 402 404 timing_ui_setup(TrackerSlam &t) 403 405 { 406 + // Setup toggle button 407 + static const char *msg[2] = {"[OFF] Enable timing", "[ON] Disable timing"}; 408 + u_var_button_cb cb = [](void *t_ptr) { 409 + TrackerSlam *t = (TrackerSlam *)t_ptr; 410 + u_var_button &btn = t->timing.enable_btn; 411 + bool &e = t->timing.ext_enabled; 412 + e = !e; 413 + snprintf(btn.label, sizeof(btn.label), "%s", msg[e]); 414 + const auto params = make_shared<FPARAMS_EPET>(e); 415 + shared_ptr<void> _; 416 + t->slam->use_feature(F_ENABLE_POSE_EXT_TIMING, params, _); 417 + }; 418 + t.timing.enable_btn.cb = cb; 419 + t.timing.enable_btn.disabled = !t.timing.ext_available; 420 + t.timing.enable_btn.ptr = &t; 421 + u_var_add_button(&t, &t.timing.enable_btn, msg[t.timing.ext_enabled]); 422 + 423 + // Setup graph 424 + 404 425 // Construct null-separated array of options for the combo box 405 426 using namespace std::string_literals; 406 427 t.timing.joined_columns = ""; ··· 440 461 vector<timepoint_ns> tss = {p.timestamp, now}; 441 462 442 463 // Add extra timestamps if the SLAM tracker provides them 443 - if (t.timing.ext_enabled) { 444 - shared_ptr<pose_extension> ext = p.find_pose_extension(pose_ext_type::TIMING); 445 - SLAM_DASSERT(ext != nullptr, "An enabled extension was null"); 464 + shared_ptr<pose_extension> ext = p.find_pose_extension(pose_ext_type::TIMING); 465 + if (ext) { 446 466 pose_ext_timing pet = *std::static_pointer_cast<pose_ext_timing>(ext); 447 467 tss.insert(tss.begin() + 1, pet.timing.begin(), pet.timing.end()); 448 468 } ··· 615 635 gt_ui_push(t, nts, rel.pose); 616 636 t.slam_traj_writer->push(nts, rel.pose); 617 637 618 - auto tss = timing_ui_push(t, np); 619 - t.slam_times_writer->push(tss); 638 + if (t.timing.ext_enabled) { 639 + auto tss = timing_ui_push(t, np); 640 + t.slam_times_writer->push(tss); 641 + } 620 642 621 643 dequeued = t.slam->try_dequeue_pose(tracked_pose); 622 644 } ··· 788 810 const t_stereo_camera_calibration *stereo_calib, 789 811 const t_slam_calib_extras *extra_calib) 790 812 { 791 - for (int i = 0; i < 2; i++) { 813 + for (int i = 0; i < NUM_CAMS; i++) { 792 814 const t_camera_calibration &view = stereo_calib->view[i]; 793 815 const auto &extra = extra_calib->cams[i]; 794 816 const auto params = make_shared<FPARAMS_ACC>(); ··· 1050 1072 config->prediction = t_slam_prediction_type(debug_get_num_option_slam_prediction_type()); 1051 1073 config->write_csvs = debug_get_bool_option_slam_write_csvs(); 1052 1074 config->csv_path = debug_get_option_slam_csv_path(); 1075 + config->timing_stat = debug_get_bool_option_slam_timing_stat(); 1053 1076 config->stereo_calib = NULL; 1054 1077 config->imu_calib = NULL; 1055 1078 config->extra_calib = NULL; ··· 1138 1161 1139 1162 t.gt.trajectory = new Trajectory{}; 1140 1163 1164 + // Setup timing extension 1165 + 1141 1166 // Probe for timing extension. 1142 - // We provide two timing columns by default, even if the external system does 1143 - // not support the timing extension for reporting internal timestamps. 1144 - t.timing.columns = {"sampled", "received_by_monado"}; 1145 1167 bool has_timing_extension = t.slam->supports_feature(F_ENABLE_POSE_EXT_TIMING); 1168 + t.timing.ext_available = has_timing_extension; 1169 + 1170 + // We provide two timing columns by default, even if there is no extension support 1171 + t.timing.columns = {"sampled", "received_by_monado"}; 1172 + 1146 1173 if (has_timing_extension) { 1147 - const auto params = make_shared<FPARAMS_EPET>(true); 1174 + bool enable_timing_extension = config->timing_stat; 1175 + 1176 + const auto params = make_shared<FPARAMS_EPET>(enable_timing_extension); 1148 1177 shared_ptr<void> result; 1149 - t.slam->use_feature(FID_EPET, params, result); 1150 - t.timing.ext_enabled = true; 1178 + t.slam->use_feature(F_ENABLE_POSE_EXT_TIMING, params, result); 1179 + vector<string> cols = *std::static_pointer_cast<FRESULT_EPET>(result); 1151 1180 1152 - vector<string> cols = *std::static_pointer_cast<FRESULT_EPET>(result); 1153 1181 t.timing.columns.insert(t.timing.columns.begin() + 1, cols.begin(), cols.end()); 1182 + t.timing.ext_enabled = enable_timing_extension; 1154 1183 } 1155 1184 1156 1185 // Setup CSV files

src/xrt/auxiliary/tracking/t_tracking.h

··· 469 469 enum t_slam_prediction_type prediction; //!< Which level of prediction to use 470 470 bool write_csvs; //!< Whether to enable CSV writers from the start for later analysis 471 471 const char *csv_path; //!< Path to write CSVs to 472 + bool timing_stat; //!< Enable timing metric in external system 472 473 473 474 // Instead of a slam_config file you can set custom calibration data 474 475 const struct t_stereo_camera_calibration *stereo_calib; //!< Camera calibration data

Configure Feed

Configure Feed