+61 -52

src/xrt/auxiliary/tracking/t_tracker_slam.cpp

··· 11 11 #include "xrt/xrt_tracking.h" 12 12 #include "xrt/xrt_frameserver.h" 13 13 #include "util/u_debug.h" 14 14 + #include "util/u_logging.h" 15 15 + #include "util/u_misc.h" 14 16 #include "util/u_var.h" 15 17 #include "os/os_threading.h" 16 18 #include "math/m_filter_fifo.h" ··· 20 22 #include "math/m_space.h" 21 23 #include "math/m_vec3.h" 22 24 #include "tracking/t_euroc_recorder.h" 25 25 + #include "tracking/t_tracking.h" 23 26 24 27 #include <slam_tracker.hpp> 25 28 #include <opencv2/core/mat.hpp> ··· 55 58 #define SLAM_DASSERT_(predicate) SLAM_ASSERT_(predicate) 56 59 #endif 57 60 58 58 - //! SLAM tracking logging level 61 61 + //! @see t_slam_tracker_config 59 62 DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_WARN) 60 60 - 61 61 - //! Config file path, format is specific to the SLAM implementation in use 62 62 - DEBUG_GET_OPTION(slam_config, "SLAM_CONFIG", NULL) 63 63 - 64 64 - //! Whether to submit data to the SLAM tracker without user action 63 63 + DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", nullptr) 65 64 DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false) 66 66 - 67 67 - //! Which level of prediction to use, @see prediction_type. 68 68 - DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", 2) 69 69 - 70 70 - //! Whether to enable CSV writers from the start for later analysis 65 65 + DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_SP_SO_IA_SL)) 71 66 DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false) 72 72 - 73 73 - //! Path to write CSVs to 74 74 - DEBUG_GET_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/") 67 67 + DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/") 75 68 76 69 //! Namespace for the interface to the external SLAM tracking system 77 70 namespace xrt::auxiliary::tracking::slam { ··· 193 186 bool enabled; // Modified through UI 194 187 195 188 private: 189 189 + string directory; 196 190 string filename; 197 191 ofstream file; 198 192 bool created = false; ··· 200 194 void 201 195 create() 202 196 { 203 203 - string csv_path = debug_get_option_slam_csv_path(); 204 204 - create_directories(csv_path); 205 205 - file = ofstream{csv_path + "/" + filename}; 197 197 + create_directories(directory); 198 198 + file = ofstream{directory + "/" + filename}; 206 199 file << "#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], " 207 200 "q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []" CSV_EOL; 208 201 file << std::fixed << std::setprecision(CSV_PRECISION); ··· 210 203 211 204 212 205 public: 213 213 - TrajectoryWriter(const string &fn, bool e) : enabled(e), filename(fn) {} 206 206 + TrajectoryWriter(const string &dir, const string &fn, bool e) : enabled(e), directory(dir), filename(fn) {} 214 207 215 208 void 216 209 push(timepoint_ns ts, const xrt_pose &pose) ··· 239 232 bool enabled; // Modified through UI 240 233 241 234 private: 235 235 + string directory; 242 236 string filename; 243 237 vector<string> column_names; 244 238 ofstream file; ··· 247 241 void 248 242 create() 249 243 { 250 250 - string csv_path = debug_get_option_slam_csv_path(); 251 251 - create_directories(csv_path); 252 252 - file = ofstream{csv_path + "/" + filename}; 244 244 + create_directories(directory); 245 245 + file = ofstream{directory + "/" + filename}; 253 246 file << "#"; 254 247 for (const string &col : column_names) { 255 248 string delimiter = &col != &column_names.back() ? "," : CSV_EOL; ··· 258 251 } 259 252 260 253 public: 261 261 - TimingWriter(const string &fn, bool e, const vector<string> &cn) : enabled(e), filename(fn), column_names(cn) {} 254 254 + TimingWriter(const string &dir, const string &fn, bool e, const vector<string> &cn) 255 255 + : enabled(e), directory(dir), filename(fn), column_names(cn) 256 256 + {} 262 257 263 258 void 264 259 push(const vector<timepoint_ns> &timestamps) ··· 279 274 } 280 275 }; 281 276 282 282 - //! SLAM prediction type. Naming scheme as follows: 283 283 - //! P: position, O: orientation, A: angular velocity, L: linear velocity 284 284 - //! S: From SLAM poses (slow, precise), I: From IMU data (fast, noisy) 285 285 - enum prediction_type 286 286 - { 287 287 - PREDICTION_NONE = 0, //!< No prediction, always return the last SLAM tracked pose 288 288 - PREDICTION_SP_SO_SA_SL, //!< Predicts from last two SLAM poses only 289 289 - PREDICTION_SP_SO_IA_SL, //!< Predicts from last SLAM pose with angular velocity computed from IMU 290 290 - PREDICTION_SP_SO_IA_IL, //!< Predicts from last SLAM pose with angular and linear velocity computed from IMU 291 291 - PREDICTION_COUNT, 292 292 - }; 293 293 - 294 277 /*! 295 278 * Main implementation of @ref xrt_tracked_slam. This is an adapter class for 296 279 * SLAM tracking that wraps an external SLAM implementation. ··· 328 311 329 312 // Prediction 330 313 331 331 - //!< Type of prediction to use 332 332 - prediction_type pred_type; 314 314 + //! Type of prediction to use 315 315 + t_slam_prediction_type pred_type; 333 316 u_var_combo pred_combo; //!< UI combo box to select @ref pred_type 334 317 RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data 335 318 struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples ··· 645 628 static void 646 629 predict_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation) 647 630 { 648 648 - bool valid_pred_type = t.pred_type >= PREDICTION_NONE && t.pred_type <= PREDICTION_SP_SO_IA_IL; 631 631 + bool valid_pred_type = t.pred_type >= SLAM_PRED_NONE && t.pred_type <= SLAM_PRED_SP_SO_IA_IL; 649 632 SLAM_DASSERT(valid_pred_type, "Invalid prediction type (%d)", t.pred_type); 650 633 651 634 // Get last relation computed purely from SLAM data ··· 660 643 } 661 644 662 645 // Use only last SLAM pose without prediction if PREDICTION_NONE 663 663 - if (t.pred_type == PREDICTION_NONE) { 646 646 + if (t.pred_type == SLAM_PRED_NONE) { 664 647 *out_relation = rel; 665 648 return; 666 649 } 667 650 668 651 // Use only SLAM data if asking for an old point in time or PREDICTION_SP_SO_SA_SL 669 652 SLAM_DASSERT_(rel_ts < INT64_MAX); 670 670 - if (t.pred_type == PREDICTION_SP_SO_SA_SL || when_ns <= (int64_t)rel_ts) { 653 653 + if (t.pred_type == SLAM_PRED_SP_SO_SA_SL || when_ns <= (int64_t)rel_ts) { 671 654 t.slam_rels.get(when_ns, out_relation); 672 655 return; 673 656 } 674 657 675 658 // Update angular velocity with gyro data 676 676 - if (t.pred_type >= PREDICTION_SP_SO_IA_SL) { 659 659 + if (t.pred_type >= SLAM_PRED_SP_SO_IA_SL) { 677 660 xrt_vec3 avg_gyro{}; 678 661 m_ff_vec3_f32_filter(t.gyro_ff, rel_ts, when_ns, &avg_gyro); 679 662 math_quat_rotate_derivative(&rel.pose.orientation, &avg_gyro, &rel.angular_velocity); 680 663 } 681 664 682 665 // Update linear velocity with accel data 683 683 - if (t.pred_type >= PREDICTION_SP_SO_IA_IL) { 666 666 + if (t.pred_type >= SLAM_PRED_SP_SO_IA_IL) { 684 667 xrt_vec3 avg_accel{}; 685 668 m_ff_vec3_f32_filter(t.accel_ff, rel_ts, when_ns, &avg_accel); 686 669 xrt_vec3 world_accel{}; ··· 754 737 static void 755 738 setup_ui(TrackerSlam &t) 756 739 { 757 757 - t.pred_combo.count = PREDICTION_COUNT; 740 740 + t.pred_combo.count = SLAM_PRED_COUNT; 758 741 t.pred_combo.options = "None\0Interpolate SLAM poses\0Also gyro\0Also accel (needs gravity correction)\0\0"; 759 742 t.pred_combo.value = (int *)&t.pred_type; 760 743 m_ff_vec3_f32_alloc(&t.gyro_ff, 1000); ··· 977 960 return ret; 978 961 } 979 962 963 963 + extern "C" void 964 964 + t_slam_fill_default_config(struct t_slam_tracker_config *config) 965 965 + { 966 966 + config->log_level = debug_get_log_option_slam_log(); 967 967 + config->slam_config = debug_get_option_slam_config(); 968 968 + config->submit_from_start = debug_get_bool_option_slam_submit_from_start(); 969 969 + config->prediction = t_slam_prediction_type(debug_get_num_option_slam_prediction_type()); 970 970 + config->write_csvs = debug_get_bool_option_slam_write_csvs(); 971 971 + config->csv_path = debug_get_option_slam_csv_path(); 972 972 + } 973 973 + 980 974 extern "C" int 981 981 - t_slam_create(struct xrt_frame_context *xfctx, struct xrt_tracked_slam **out_xts, struct xrt_slam_sinks **out_sink) 975 975 + t_slam_create(struct xrt_frame_context *xfctx, 976 976 + struct t_slam_tracker_config *config, 977 977 + struct xrt_tracked_slam **out_xts, 978 978 + struct xrt_slam_sinks **out_sink) 982 979 { 983 983 - enum u_logging_level log_level = debug_get_log_option_slam_log(); 980 980 + struct t_slam_tracker_config *default_config = nullptr; 981 981 + if (config == nullptr) { 982 982 + default_config = U_TYPED_CALLOC(struct t_slam_tracker_config); 983 983 + t_slam_fill_default_config(default_config); 984 984 + config = default_config; 985 985 + } 986 986 + 987 987 + enum u_logging_level log_level = config->log_level; 984 988 985 989 // Check that the external SLAM system built is compatible 986 990 int ima = IMPLEMENTATION_VERSION_MAJOR; ··· 997 1001 U_LOG_IFL_I(log_level, "Initializing compatible external SLAM system."); 998 1002 999 1003 // Check the user has provided a SLAM_CONFIG file 1000 1000 - const char *config_file = debug_get_option_slam_config(); 1004 1004 + const char *config_file = config->slam_config; 1001 1005 if (!config_file) { 1002 1006 U_LOG_IFL_W(log_level, 1003 1007 "SLAM tracker requires a config file set with the SLAM_CONFIG environment variable"); ··· 1025 1029 t.sinks.imu = &t.imu_sink; 1026 1030 t.sinks.gt = &t.gt_sink; 1027 1031 1028 1028 - t.submit = debug_get_bool_option_slam_submit_from_start(); 1032 1032 + t.submit = config->submit_from_start; 1029 1033 1030 1034 t.node.break_apart = t_slam_node_break_apart; 1031 1035 t.node.destroy = t_slam_node_destroy; ··· 1037 1041 1038 1042 t.euroc_recorder = euroc_recorder_create(xfctx, NULL); 1039 1043 1040 1040 - t.pred_type = (prediction_type)debug_get_num_option_slam_prediction_type(); 1044 1044 + t.pred_type = config->prediction; 1041 1045 1042 1046 m_filter_euro_vec3_init(&t.filter.pos_oe, t.filter.min_cutoff, t.filter.beta, t.filter.min_dcutoff); 1043 1047 m_filter_euro_quat_init(&t.filter.rot_oe, t.filter.min_cutoff, t.filter.beta, t.filter.min_dcutoff); ··· 1059 1063 } 1060 1064 1061 1065 // Setup CSV files 1062 1062 - bool write_csvs = debug_get_bool_option_slam_write_csvs(); 1063 1063 - t.slam_times_writer = new TimingWriter{"timing.csv", write_csvs, t.timing.columns}; 1064 1064 - t.slam_traj_writer = new TrajectoryWriter{"tracking.csv", write_csvs}; 1065 1065 - t.pred_traj_writer = new TrajectoryWriter{"prediction.csv", write_csvs}; 1066 1066 - t.filt_traj_writer = new TrajectoryWriter{"filtering.csv", write_csvs}; 1066 1066 + bool write_csvs = config->write_csvs; 1067 1067 + string dir = config->csv_path; 1068 1068 + t.slam_times_writer = new TimingWriter{dir, "timing.csv", write_csvs, t.timing.columns}; 1069 1069 + t.slam_traj_writer = new TrajectoryWriter{dir, "tracking.csv", write_csvs}; 1070 1070 + t.pred_traj_writer = new TrajectoryWriter{dir, "prediction.csv", write_csvs}; 1071 1071 + t.filt_traj_writer = new TrajectoryWriter{dir, "filtering.csv", write_csvs}; 1067 1072 1068 1073 setup_ui(t); 1074 1074 + 1075 1075 + if (default_config != nullptr) { 1076 1076 + free(default_config); 1077 1077 + } 1069 1078 1070 1079 *out_xts = &t.base; 1071 1080 *out_sink = &t.sinks;

+40 -1

src/xrt/auxiliary/tracking/t_tracking.h

··· 11 11 12 12 #pragma once 13 13 14 14 + #include "util/u_logging.h" 14 15 #include "xrt/xrt_frame.h" 15 16 #include "util/u_misc.h" 16 17 ··· 382 383 int 383 384 t_hand_start(struct xrt_tracked_hand *xth); 384 385 386 386 + //! SLAM prediction type. Naming scheme as follows: 387 387 + //! P: position, O: orientation, A: angular velocity, L: linear velocity 388 388 + //! S: From SLAM poses (slow, precise), I: From IMU data (fast, noisy) 389 389 + enum t_slam_prediction_type 390 390 + { 391 391 + SLAM_PRED_NONE = 0, //!< No prediction, always return the last SLAM tracked pose 392 392 + SLAM_PRED_SP_SO_SA_SL, //!< Predicts from last two SLAM poses only 393 393 + SLAM_PRED_SP_SO_IA_SL, //!< Predicts from last SLAM pose with angular velocity computed from IMU 394 394 + SLAM_PRED_SP_SO_IA_IL, //!< Predicts from last SLAM pose with angular and linear velocity computed from IMU 395 395 + SLAM_PRED_COUNT, 396 396 + }; 397 397 + 398 398 + /*! 399 399 + * SLAM tracker configuration. 400 400 + * 401 401 + * @see xrt_tracked_slam 402 402 + */ 403 403 + struct t_slam_tracker_config 404 404 + { 405 405 + enum u_logging_level log_level; //!< SLAM tracking logging level 406 406 + const char *slam_config; //!< Config file path, format is specific to the SLAM implementation in use 407 407 + bool submit_from_start; //!< Whether to submit data to the SLAM tracker without user action 408 408 + enum t_slam_prediction_type prediction; //! Which level of prediction to use 409 409 + bool write_csvs; //!< Whether to enable CSV writers from the start for later analysis 410 410 + const char *csv_path; //!< Path to write CSVs to 411 411 + }; 412 412 + 413 413 + /*! 414 414 + * Fills in a @ref t_slam_tracker_config with default values. 415 415 + * 416 416 + * @see xrt_tracked_Slam 417 417 + */ 418 418 + void 419 419 + t_slam_fill_default_config(struct t_slam_tracker_config *config); 420 420 + 385 421 /*! 386 422 * @public @memberof xrt_tracked_slam 387 423 */ 388 424 int 389 389 - t_slam_create(struct xrt_frame_context *xfctx, struct xrt_tracked_slam **out_xts, struct xrt_slam_sinks **out_sink); 425 425 + t_slam_create(struct xrt_frame_context *xfctx, 426 426 + struct t_slam_tracker_config *config, 427 427 + struct xrt_tracked_slam **out_xts, 428 428 + struct xrt_slam_sinks **out_sink); 390 429 391 430 /*! 392 431 * @public @memberof xrt_tracked_slam

+1 -1

src/xrt/drivers/wmr/wmr_hmd.c

··· 1195 1195 struct xrt_slam_sinks *sinks = NULL; 1196 1196 1197 1197 #ifdef XRT_HAVE_SLAM 1198 1198 - int create_status = t_slam_create(&wh->slam.xfctx, &wh->slam.tracker, &sinks); 1198 1198 + int create_status = t_slam_create(&wh->slam.xfctx, NULL, &wh->slam.tracker, &sinks); 1199 1199 if (create_status != 0) { 1200 1200 return NULL; 1201 1201 }

+2 -2

src/xrt/state_trackers/prober/p_tracking.c

··· 264 264 assert(fact->xfs->source_id == 0xECD0FEED && "xfs is not Euroc, unsynced open_video_device?"); 265 265 266 266 #ifdef XRT_HAVE_SLAM 267 267 - int ret = t_slam_create(&fact->xfctx, &fact->xts, &sinks); 267 267 + int ret = t_slam_create(&fact->xfctx, NULL, &fact->xts, &sinks); 268 268 if (ret != 0) { 269 269 U_LOG_W("Unable to initialize SLAM tracking, the Euroc driver will not be tracked"); 270 270 } ··· 289 289 assert(fact->xfs->source_id == 0x2EA15E115E && "xfs is not RealSense, unsynced open_video_device?"); 290 290 291 291 #ifdef XRT_HAVE_SLAM 292 292 - int ret = t_slam_create(&fact->xfctx, &fact->xts, &sinks); 292 292 + int ret = t_slam_create(&fact->xfctx, NULL, &fact->xts, &sinks); 293 293 if (ret != 0) { 294 294 U_LOG_W("Unable to initialize SLAM tracking, the RealSense driver will not be tracked"); 295 295 }