The open source OpenXR runtime
0
fork

Configure Feed

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

t/slam: Add configuration options to tracker

Specifically:
- SLAM_PREDICTION_TYPE
- SLAM_CSV_PATH

+22 -4
+22 -4
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
··· 25 25 #include <opencv2/core/mat.hpp> 26 26 #include <opencv2/core/version.hpp> 27 27 28 + #include <filesystem> 28 29 #include <fstream> 29 30 #include <iomanip> 30 31 #include <map> ··· 58 59 DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_WARN) 59 60 60 61 //! Config file path, format is specific to the SLAM implementation in use 61 - DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", NULL) 62 + DEBUG_GET_OPTION(slam_config, "SLAM_CONFIG", NULL) 62 63 63 64 //! Whether to submit data to the SLAM tracker without user action 64 65 DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false) 65 66 67 + //! Which level of prediction to use, @see prediction_type. 68 + DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", 2) 69 + 66 70 //! Whether to enable CSV writers from the start for later analysis 67 71 DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false) 72 + 73 + //! Path to write CSVs to 74 + DEBUG_GET_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/") 68 75 69 76 //! Namespace for the interface to the external SLAM tracking system 70 77 namespace xrt::auxiliary::tracking::slam { ··· 79 86 using std::string; 80 87 using std::unique_ptr; 81 88 using std::vector; 89 + using std::filesystem::create_directories; 82 90 using Trajectory = map<timepoint_ns, xrt_pose>; 83 91 84 92 using xrt::auxiliary::math::RelationHistory; ··· 192 200 void 193 201 create() 194 202 { 195 - file = ofstream{filename}; 203 + string csv_path = debug_get_option_slam_csv_path(); 204 + create_directories(csv_path); 205 + file = ofstream{csv_path + "/" + filename}; 196 206 file << "#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], " 197 207 "q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []" CSV_EOL; 198 208 file << std::fixed << std::setprecision(CSV_PRECISION); ··· 237 247 void 238 248 create() 239 249 { 240 - file = ofstream{filename}; 250 + string csv_path = debug_get_option_slam_csv_path(); 251 + create_directories(csv_path); 252 + file = ofstream{csv_path + "/" + filename}; 241 253 file << "#"; 242 254 for (const string &col : column_names) { 243 255 string delimiter = &col != &column_names.back() ? "," : CSV_EOL; ··· 317 329 // Prediction 318 330 319 331 //!< Type of prediction to use 320 - prediction_type pred_type = PREDICTION_SP_SO_IA_SL; 332 + prediction_type pred_type; 321 333 u_var_combo pred_combo; //!< UI combo box to select @ref pred_type 322 334 RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data 323 335 struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples ··· 800 812 { 801 813 auto &t = *container_of(xts, TrackerSlam, base); 802 814 815 + //! @todo This should not be cached, the same timestamp can be requested at a 816 + //! later time on the frame for a better prediction. 803 817 if (when_ns == t.last_ts) { 804 818 *out_relation = t.last_rel; 805 819 return; ··· 871 885 static void 872 886 push_frame(TrackerSlam &t, struct xrt_frame *frame, bool is_left) 873 887 { 888 + SLAM_DASSERT(t.last_left_ts != INT64_MIN || is_left, "First frame was a right frame"); 889 + 874 890 // Construct and send the image sample 875 891 cv::Mat img = t.cv_wrapper->wrap(frame); 876 892 SLAM_DASSERT_(frame->timestamp < INT64_MAX); ··· 1020 1036 xrt_frame_context_add(xfctx, &t.node); 1021 1037 1022 1038 t.euroc_recorder = euroc_recorder_create(xfctx, NULL); 1039 + 1040 + t.pred_type = (prediction_type)debug_get_num_option_slam_prediction_type(); 1023 1041 1024 1042 m_filter_euro_vec3_init(&t.filter.pos_oe, t.filter.min_cutoff, t.filter.beta, t.filter.min_dcutoff); 1025 1043 m_filter_euro_quat_init(&t.filter.rot_oe, t.filter.min_cutoff, t.filter.beta, t.filter.min_dcutoff);