matrixfurry.com / monado
The open source OpenXR runtime
0
fork

Configure Feed

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

t/slam: Allow drivers to send calibration info to SLAM tracker

There were a couple of properties that didn't fit well in our current
calibration structs and so I created t_slam_calib_extras struct to store them.

Mateo de Mayo 8adbdc27 dad70ab7

+131 -16
+100 -12
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
··· 8 8 */ 9 9 10 10 #include "xrt/xrt_config_have.h" 11 + #include "xrt/xrt_defines.h" 11 12 #include "xrt/xrt_tracking.h" 12 13 #include "xrt/xrt_frameserver.h" 13 14 #include "util/u_debug.h" ··· 15 16 #include "util/u_misc.h" 16 17 #include "util/u_var.h" 17 18 #include "os/os_threading.h" 19 + #include "math/m_api.h" 18 20 #include "math/m_filter_fifo.h" 19 21 #include "math/m_filter_one_euro.h" 20 22 #include "math/m_predict.h" ··· 32 34 #include <fstream> 33 35 #include <iomanip> 34 36 #include <map> 37 + #include <string> 35 38 36 39 #define SLAM_TRACE(...) U_LOG_IFL_T(t.log_level, __VA_ARGS__) 37 40 #define SLAM_DEBUG(...) U_LOG_IFL_D(t.log_level, __VA_ARGS__) ··· 59 62 #endif 60 63 61 64 //! @see t_slam_tracker_config 62 - DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_WARN) 65 + DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_INFO) 63 66 DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", nullptr) 64 67 DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false) 65 68 DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_SP_SO_IA_SL)) ··· 72 75 constexpr int UI_GTDIFF_POSE_COUNT = 192; 73 76 74 77 using std::ifstream; 78 + using std::make_shared; 75 79 using std::make_unique; 76 80 using std::map; 77 81 using std::ofstream; 78 82 using std::shared_ptr; 79 83 using std::string; 84 + using std::to_string; 80 85 using std::unique_ptr; 81 86 using std::vector; 82 87 using std::filesystem::create_directories; ··· 778 783 // Later, gt_ui_setup will setup the tracking error UI if ground truth becomes available 779 784 } 780 785 786 + static void 787 + add_camera_calibration(const TrackerSlam &t, 788 + const t_stereo_camera_calibration *stereo_calib, 789 + const t_slam_calib_extras *extra_calib) 790 + { 791 + for (int i = 0; i < 2; i++) { 792 + const t_camera_calibration &view = stereo_calib->view[i]; 793 + const auto &extra = extra_calib->cams[i]; 794 + const auto params = make_shared<FPARAMS_ACC>(); 795 + 796 + params->cam_index = i; 797 + params->width = view.image_size_pixels.w; 798 + params->height = view.image_size_pixels.h; 799 + params->frequency = extra.frequency; 800 + 801 + params->fx = view.intrinsics[0][0]; 802 + params->fy = view.intrinsics[1][1]; 803 + params->cx = view.intrinsics[0][2]; 804 + params->cy = view.intrinsics[1][2]; 805 + 806 + params->distortion_model = view.use_fisheye ? "kb4" : string{"rt"} + to_string(view.distortion_num); 807 + if (view.use_fisheye) { // Kannala-brandt pinhole (OpenCV's "fisheye") 808 + params->distortion.assign(view.distortion_fisheye, std::end(view.distortion_fisheye)); 809 + SLAM_ASSERT_(params->distortion.size() == 4); 810 + } else { // Radial-tangential pinhole 811 + params->distortion.assign(view.distortion, view.distortion + view.distortion_num); 812 + 813 + if (params->distortion_model == "rt8") { // rt8 has a ninth parameter rpmax ("metric_radius") 814 + params->distortion.push_back(extra.rpmax); 815 + } 816 + } 817 + 818 + xrt_matrix_4x4 T; // Row major T_imu_cam 819 + math_matrix_4x4_transpose(&extra.T_imu_cam, &T); 820 + params->t_imu_cam = cv::Matx<float, 4, 4>{T.v}; 821 + 822 + shared_ptr<FRESULT_ACC> result{}; 823 + t.slam->use_feature(F_ADD_CAMERA_CALIBRATION, params, result); 824 + } 825 + } 826 + 827 + static void 828 + add_imu_calibration(const TrackerSlam &t, const t_imu_calibration *imu_calib, const t_slam_calib_extras *extra_calib) 829 + { 830 + const auto params = make_shared<FPARAMS_AIC>(); 831 + params->imu_index = 0; // Multiple IMU setups unsupported 832 + params->frequency = extra_calib->imu_frequency; 833 + 834 + const t_inertial_calibration &accel = imu_calib->accel; 835 + params->accel.transform = cv::Matx<double, 3, 3>{&accel.transform[0][0]}; 836 + params->accel.offset = cv::Matx<double, 3, 1>{&accel.offset[0]}; 837 + params->accel.bias_std = cv::Matx<double, 3, 1>{&accel.bias_std[0]}; 838 + params->accel.noise_std = cv::Matx<double, 3, 1>{&accel.noise_std[0]}; 839 + 840 + const t_inertial_calibration &gyro = imu_calib->gyro; 841 + params->gyro.transform = cv::Matx<double, 3, 3>{&gyro.transform[0][0]}; 842 + params->gyro.offset = cv::Matx<double, 3, 1>{&gyro.offset[0]}; 843 + params->gyro.bias_std = cv::Matx<double, 3, 1>{&gyro.bias_std[0]}; 844 + params->gyro.noise_std = cv::Matx<double, 3, 1>{&gyro.noise_std[0]}; 845 + 846 + shared_ptr<FRESULT_AIC> result{}; 847 + t.slam->use_feature(F_ADD_IMU_CALIBRATION, params, result); 848 + } 849 + 781 850 } // namespace xrt::auxiliary::tracking::slam 782 851 783 852 using namespace xrt::auxiliary::tracking::slam; ··· 960 1029 config->prediction = t_slam_prediction_type(debug_get_num_option_slam_prediction_type()); 961 1030 config->write_csvs = debug_get_bool_option_slam_write_csvs(); 962 1031 config->csv_path = debug_get_option_slam_csv_path(); 1032 + config->stereo_calib = NULL; 1033 + config->imu_calib = NULL; 1034 + config->extra_calib = NULL; 963 1035 } 964 1036 965 1037 extern "C" int ··· 968 1040 struct xrt_tracked_slam **out_xts, 969 1041 struct xrt_slam_sinks **out_sink) 970 1042 { 971 - struct t_slam_tracker_config *default_config = nullptr; 1043 + struct t_slam_tracker_config default_config = {}; 972 1044 if (config == nullptr) { 973 - default_config = U_TYPED_CALLOC(struct t_slam_tracker_config); 974 - t_slam_fill_default_config(default_config); 975 - config = default_config; 1045 + t_slam_fill_default_config(&default_config); 1046 + config = &default_config; 976 1047 } 977 1048 978 1049 enum u_logging_level log_level = config->log_level; ··· 993 1064 994 1065 // Check the user has provided a SLAM_CONFIG file 995 1066 const char *config_file = config->slam_config; 996 - if (!config_file) { 997 - U_LOG_IFL_W(log_level, 998 - "SLAM tracker requires a config file set with the SLAM_CONFIG environment variable"); 1067 + bool some_calib = config->stereo_calib || config->imu_calib; 1068 + if (!config_file && !some_calib) { 1069 + U_LOG_IFL_W(log_level, "Unable to determine sensor calibration, did you forgot to set SLAM_CONFIG?"); 999 1070 return -1; 1071 + } 1072 + 1073 + if (!config_file) { 1074 + // Indicate the external system to use default (non-calibration) settings 1075 + config_file = "DEFAULT"; 1000 1076 } 1001 1077 1002 1078 auto &t = *(new TrackerSlam{}); ··· 1008 1084 std::string config_file_string = std::string(config_file); 1009 1085 t.slam = new slam_tracker{config_file_string}; 1010 1086 1087 + // Try to send camera calibration data to the SLAM system 1088 + if (config->stereo_calib && config->extra_calib && t.slam->supports_feature(F_ADD_CAMERA_CALIBRATION)) { 1089 + SLAM_INFO("Sending Camera calibration from Monado"); 1090 + add_camera_calibration(t, config->stereo_calib, config->extra_calib); 1091 + } else { 1092 + SLAM_INFO("Cameras will use the calibration provided by the SLAM_CONFIG file"); 1093 + } 1094 + 1095 + // Try to send IMU calibration data to the SLAM system 1096 + if (config->imu_calib && config->extra_calib && t.slam->supports_feature(F_ADD_IMU_CALIBRATION)) { 1097 + SLAM_INFO("Sending IMU calibration from Monado"); 1098 + add_imu_calibration(t, config->imu_calib, config->extra_calib); 1099 + } else { 1100 + SLAM_INFO("The IMU will use the calibration provided by the SLAM_CONFIG file"); 1101 + } 1102 + 1011 1103 t.slam->initialize(); 1012 1104 1013 1105 t.left_sink.push_frame = t_slam_frame_sink_push_left; ··· 1062 1154 t.filt_traj_writer = new TrajectoryWriter{dir, "filtering.csv", write_csvs}; 1063 1155 1064 1156 setup_ui(t); 1065 - 1066 - if (default_config != nullptr) { 1067 - free(default_config); 1068 - } 1069 1157 1070 1158 *out_xts = &t.base; 1071 1159 *out_sink = &t.sinks;
+31 -4
src/xrt/auxiliary/tracking/t_tracking.h
··· 12 12 #pragma once 13 13 14 14 #include "util/u_logging.h" 15 + #include "xrt/xrt_defines.h" 15 16 #include "xrt/xrt_frame.h" 16 17 #include "util/u_misc.h" 17 18 ··· 422 423 423 424 424 425 425 - //! SLAM prediction type. Naming scheme as follows: 426 - //! P: position, O: orientation, A: angular velocity, L: linear velocity 427 - //! S: From SLAM poses (slow, precise), I: From IMU data (fast, noisy) 426 + /*! 427 + * SLAM prediction type. Naming scheme as follows: 428 + * P: position, O: orientation, A: angular velocity, L: linear velocity 429 + * S: From SLAM poses (slow, precise), I: From IMU data (fast, noisy) 430 + * 431 + * @see xrt_tracked_slam 432 + */ 428 433 enum t_slam_prediction_type 429 434 { 430 435 SLAM_PRED_NONE = 0, //!< No prediction, always return the last SLAM tracked pose ··· 435 440 }; 436 441 437 442 /*! 443 + * This struct complements calibration data from @ref 444 + * t_stereo_camera_calibration and @ref t_imu_calibration 445 + * 446 + * @see xrt_tracked_slam 447 + */ 448 + struct t_slam_calib_extras 449 + { 450 + double imu_frequency; //! IMU samples per second 451 + struct 452 + { 453 + double frequency; //!< Camera FPS 454 + struct xrt_matrix_4x4 T_imu_cam; //!< Transform IMU to camera. Column major. 455 + float rpmax; //!< Used for rt8 calibrations. Rpmax or "metric_radius" property. 456 + } cams[2]; 457 + }; 458 + 459 + /*! 438 460 * SLAM tracker configuration. 439 461 * 440 462 * @see xrt_tracked_slam ··· 444 466 enum u_logging_level log_level; //!< SLAM tracking logging level 445 467 const char *slam_config; //!< Config file path, format is specific to the SLAM implementation in use 446 468 bool submit_from_start; //!< Whether to submit data to the SLAM tracker without user action 447 - enum t_slam_prediction_type prediction; //! Which level of prediction to use 469 + enum t_slam_prediction_type prediction; //!< Which level of prediction to use 448 470 bool write_csvs; //!< Whether to enable CSV writers from the start for later analysis 449 471 const char *csv_path; //!< Path to write CSVs to 472 + 473 + // Instead of a slam_config file you can set custom calibration data 474 + const struct t_stereo_camera_calibration *stereo_calib; //!< Camera calibration data 475 + const struct t_imu_calibration *imu_calib; //!< IMU calibration data 476 + const struct t_slam_calib_extras *extra_calib; //!< Extra calibration data 450 477 }; 451 478 452 479 /*!