diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp index 3b312a020..9b58b4eaa 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp @@ -68,21 +68,21 @@ DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", fal DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_SP_SO_IA_SL)) DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false) DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/") +DEBUG_GET_ONCE_BOOL_OPTION(slam_timing_stat, "SLAM_TIMING_STAT", true) //! Namespace for the interface to the external SLAM tracking system namespace xrt::auxiliary::tracking::slam { constexpr int UI_TIMING_POSE_COUNT = 192; constexpr int UI_GTDIFF_POSE_COUNT = 192; +constexpr int NUM_CAMS = 2; //!< This should be used as little as possible to allow setups that are not stereo using std::ifstream; using std::make_shared; -using std::make_unique; using std::map; using std::ofstream; using std::shared_ptr; using std::string; using std::to_string; -using std::unique_ptr; using std::vector; using std::filesystem::create_directories; using Trajectory = map; @@ -367,6 +367,8 @@ struct TrackerSlam //! Tracker timing info for performance evaluation struct { + bool ext_available = false; //!< Whether the SLAM system supports the timing extension + bool ext_enabled = false; //!< Whether the timing extension is enabled float dur_ms[UI_TIMING_POSE_COUNT]; //!< Timing durations in ms int idx = 0; //!< Index of latest entry in @ref dur_ms u_var_combo start_ts; //!< UI combo box to select initial timing measurement @@ -374,9 +376,9 @@ struct TrackerSlam int start_ts_idx; //!< Selected initial timing measurement in @ref start_ts int end_ts_idx; //!< Selected final timing measurement in @ref end_ts struct u_var_timing ui; //!< Realtime UI for tracker durations - bool ext_enabled = false; //!< Whether the SLAM system supports the timing extension vector columns; //!< Column names of the measured timestamps string joined_columns; //!< Column names as a null separated string + struct u_var_button enable_btn; //!< Toggle tracker timing reports } timing; //! Ground truth related fields @@ -401,6 +403,25 @@ struct TrackerSlam static void timing_ui_setup(TrackerSlam &t) { + // Setup toggle button + static const char *msg[2] = {"[OFF] Enable timing", "[ON] Disable timing"}; + u_var_button_cb cb = [](void *t_ptr) { + TrackerSlam *t = (TrackerSlam *)t_ptr; + u_var_button &btn = t->timing.enable_btn; + bool &e = t->timing.ext_enabled; + e = !e; + snprintf(btn.label, sizeof(btn.label), "%s", msg[e]); + const auto params = make_shared(e); + shared_ptr _; + t->slam->use_feature(F_ENABLE_POSE_EXT_TIMING, params, _); + }; + t.timing.enable_btn.cb = cb; + t.timing.enable_btn.disabled = !t.timing.ext_available; + t.timing.enable_btn.ptr = &t; + u_var_add_button(&t, &t.timing.enable_btn, msg[t.timing.ext_enabled]); + + // Setup graph + // Construct null-separated array of options for the combo box using namespace std::string_literals; t.timing.joined_columns = ""; @@ -440,9 +461,8 @@ timing_ui_push(TrackerSlam &t, const pose &p) vector tss = {p.timestamp, now}; // Add extra timestamps if the SLAM tracker provides them - if (t.timing.ext_enabled) { - shared_ptr ext = p.find_pose_extension(pose_ext_type::TIMING); - SLAM_DASSERT(ext != nullptr, "An enabled extension was null"); + shared_ptr ext = p.find_pose_extension(pose_ext_type::TIMING); + if (ext) { pose_ext_timing pet = *std::static_pointer_cast(ext); tss.insert(tss.begin() + 1, pet.timing.begin(), pet.timing.end()); } @@ -615,8 +635,10 @@ flush_poses(TrackerSlam &t) gt_ui_push(t, nts, rel.pose); t.slam_traj_writer->push(nts, rel.pose); - auto tss = timing_ui_push(t, np); - t.slam_times_writer->push(tss); + if (t.timing.ext_enabled) { + auto tss = timing_ui_push(t, np); + t.slam_times_writer->push(tss); + } dequeued = t.slam->try_dequeue_pose(tracked_pose); } @@ -788,7 +810,7 @@ add_camera_calibration(const TrackerSlam &t, const t_stereo_camera_calibration *stereo_calib, const t_slam_calib_extras *extra_calib) { - for (int i = 0; i < 2; i++) { + for (int i = 0; i < NUM_CAMS; i++) { const t_camera_calibration &view = stereo_calib->view[i]; const auto &extra = extra_calib->cams[i]; const auto params = make_shared(); @@ -1050,6 +1072,7 @@ t_slam_fill_default_config(struct t_slam_tracker_config *config) config->prediction = t_slam_prediction_type(debug_get_num_option_slam_prediction_type()); config->write_csvs = debug_get_bool_option_slam_write_csvs(); config->csv_path = debug_get_option_slam_csv_path(); + config->timing_stat = debug_get_bool_option_slam_timing_stat(); config->stereo_calib = NULL; config->imu_calib = NULL; config->extra_calib = NULL; @@ -1138,19 +1161,25 @@ t_slam_create(struct xrt_frame_context *xfctx, t.gt.trajectory = new Trajectory{}; - // Probe for timing extension. - // We provide two timing columns by default, even if the external system does - // not support the timing extension for reporting internal timestamps. - t.timing.columns = {"sampled", "received_by_monado"}; - bool has_timing_extension = t.slam->supports_feature(F_ENABLE_POSE_EXT_TIMING); - if (has_timing_extension) { - const auto params = make_shared(true); - shared_ptr result; - t.slam->use_feature(FID_EPET, params, result); - t.timing.ext_enabled = true; + // Setup timing extension + // Probe for timing extension. + bool has_timing_extension = t.slam->supports_feature(F_ENABLE_POSE_EXT_TIMING); + t.timing.ext_available = has_timing_extension; + + // We provide two timing columns by default, even if there is no extension support + t.timing.columns = {"sampled", "received_by_monado"}; + + if (has_timing_extension) { + bool enable_timing_extension = config->timing_stat; + + const auto params = make_shared(enable_timing_extension); + shared_ptr result; + t.slam->use_feature(F_ENABLE_POSE_EXT_TIMING, params, result); vector cols = *std::static_pointer_cast(result); + t.timing.columns.insert(t.timing.columns.begin() + 1, cols.begin(), cols.end()); + t.timing.ext_enabled = enable_timing_extension; } // Setup CSV files diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h index 4bd41eb2f..c79a88df7 100644 --- a/src/xrt/auxiliary/tracking/t_tracking.h +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -469,6 +469,7 @@ struct t_slam_tracker_config enum t_slam_prediction_type prediction; //!< Which level of prediction to use bool write_csvs; //!< Whether to enable CSV writers from the start for later analysis const char *csv_path; //!< Path to write CSVs to + bool timing_stat; //!< Enable timing metric in external system // Instead of a slam_config file you can set custom calibration data const struct t_stereo_camera_calibration *stereo_calib; //!< Camera calibration data