From 2a4a9aa210d041a23ed2587f95e3fb250f0965aa Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Tue, 15 Mar 2022 16:28:34 -0300 Subject: [PATCH] t/slam: Add configuration options to tracker Specifically: - SLAM_PREDICTION_TYPE - SLAM_CSV_PATH --- src/xrt/auxiliary/tracking/t_tracker_slam.cpp | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp index 84fad61da..783f7fabb 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -58,14 +59,20 @@ DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_WARN) //! Config file path, format is specific to the SLAM implementation in use -DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", NULL) +DEBUG_GET_OPTION(slam_config, "SLAM_CONFIG", NULL) //! Whether to submit data to the SLAM tracker without user action DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false) +//! Which level of prediction to use, @see prediction_type. +DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", 2) + //! Whether to enable CSV writers from the start for later analysis DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false) +//! Path to write CSVs to +DEBUG_GET_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/") + //! Namespace for the interface to the external SLAM tracking system namespace xrt::auxiliary::tracking::slam { constexpr int UI_TIMING_POSE_COUNT = 192; @@ -79,6 +86,7 @@ using std::shared_ptr; using std::string; using std::unique_ptr; using std::vector; +using std::filesystem::create_directories; using Trajectory = map; using xrt::auxiliary::math::RelationHistory; @@ -192,7 +200,9 @@ private: void create() { - file = ofstream{filename}; + string csv_path = debug_get_option_slam_csv_path(); + create_directories(csv_path); + file = ofstream{csv_path + "/" + filename}; file << "#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], " "q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []" CSV_EOL; file << std::fixed << std::setprecision(CSV_PRECISION); @@ -237,7 +247,9 @@ private: void create() { - file = ofstream{filename}; + string csv_path = debug_get_option_slam_csv_path(); + create_directories(csv_path); + file = ofstream{csv_path + "/" + filename}; file << "#"; for (const string &col : column_names) { string delimiter = &col != &column_names.back() ? "," : CSV_EOL; @@ -317,7 +329,7 @@ struct TrackerSlam // Prediction //!< Type of prediction to use - prediction_type pred_type = PREDICTION_SP_SO_IA_SL; + prediction_type pred_type; u_var_combo pred_combo; //!< UI combo box to select @ref pred_type RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples @@ -800,6 +812,8 @@ t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, stru { auto &t = *container_of(xts, TrackerSlam, base); + //! @todo This should not be cached, the same timestamp can be requested at a + //! later time on the frame for a better prediction. if (when_ns == t.last_ts) { *out_relation = t.last_rel; return; @@ -871,6 +885,8 @@ t_slam_imu_sink_push(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) static void push_frame(TrackerSlam &t, struct xrt_frame *frame, bool is_left) { + SLAM_DASSERT(t.last_left_ts != INT64_MIN || is_left, "First frame was a right frame"); + // Construct and send the image sample cv::Mat img = t.cv_wrapper->wrap(frame); SLAM_DASSERT_(frame->timestamp < INT64_MAX); @@ -1021,6 +1037,8 @@ t_slam_create(struct xrt_frame_context *xfctx, struct xrt_tracked_slam **out_xts t.euroc_recorder = euroc_recorder_create(xfctx, NULL); + t.pred_type = (prediction_type)debug_get_num_option_slam_prediction_type(); + m_filter_euro_vec3_init(&t.filter.pos_oe, t.filter.min_cutoff, t.filter.beta, t.filter.min_dcutoff); m_filter_euro_quat_init(&t.filter.rot_oe, t.filter.min_cutoff, t.filter.beta, t.filter.min_dcutoff);