+9 -1

src/xrt/drivers/CMakeLists.txt

··· 204 204 drv_rift_s STATIC 205 205 rift_s/rift_s_builder.c 206 206 rift_s/rift_s_interface.h 207 207 + rift_s/rift_s_camera.c 208 208 + rift_s/rift_s_camera.h 207 209 rift_s/rift_s_controller.c 208 210 rift_s/rift_s_controller.h 209 211 rift_s/rift_s_firmware.c ··· 214 216 rift_s/rift_s_protocol.h 215 217 rift_s/rift_s_radio.c 216 218 rift_s/rift_s_radio.h 219 219 + rift_s/rift_s_tracker.c 220 220 + rift_s/rift_s_tracker.h 221 221 + rift_s/rift_s_util.cpp 222 222 + rift_s/rift_s_util.h 217 223 rift_s/rift_s.c 218 224 rift_s/rift_s.h 219 225 ) 226 226 + target_include_directories(drv_rift_s SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) 220 227 target_link_libraries( 221 228 drv_rift_s 222 229 PRIVATE ··· 224 231 aux_util 225 232 aux_math 226 233 xrt-external-cjson 227 227 - ) 234 234 + xrt-external-tinyceres 235 235 + ) 228 236 list(APPEND ENABLED_HEADSET_DRIVERS rift-s) 229 237 endif() 230 238

+153 -2

src/xrt/drivers/rift_s/rift_s.c

··· 45 45 #include "rift_s.h" 46 46 #include "rift_s_hmd.h" 47 47 #include "rift_s_controller.h" 48 48 + #include "rift_s_camera.h" 48 49 49 50 static void * 50 51 rift_s_run_thread(void *ptr); 51 52 static void 52 53 rift_s_system_free(struct rift_s_system *sys); 53 54 55 55 + static int 56 56 + read_camera_calibration(struct os_hid_device *hid_hmd, struct rift_s_camera_calibration_block *calibration) 57 57 + { 58 58 + char *json = NULL; 59 59 + int json_len = 0; 60 60 + 61 61 + int ret = rift_s_read_firmware_block(hid_hmd, RIFT_S_FIRMWARE_BLOCK_CAMERA_CALIB, &json, &json_len); 62 62 + if (ret < 0) 63 63 + return ret; 64 64 + 65 65 + ret = rift_s_parse_camera_calibration_block(json, calibration); 66 66 + free(json); 67 67 + 68 68 + return ret; 69 69 + } 70 70 + 71 71 + static int 72 72 + read_hmd_fw_imu_calibration(struct os_hid_device *hid_hmd, struct rift_s_imu_calibration *imu_calibration) 73 73 + { 74 74 + char *json = NULL; 75 75 + int json_len = 0; 76 76 + 77 77 + int ret = rift_s_read_firmware_block(hid_hmd, RIFT_S_FIRMWARE_BLOCK_IMU_CALIB, &json, &json_len); 78 78 + if (ret < 0) 79 79 + return ret; 80 80 + 81 81 + ret = rift_s_parse_imu_calibration(json, imu_calibration); 82 82 + free(json); 83 83 + 84 84 + return ret; 85 85 + } 86 86 + 87 87 + static int 88 88 + read_hmd_proximity_threshold(struct os_hid_device *hid_hmd, int *proximity_threshold) 89 89 + { 90 90 + char *json = NULL; 91 91 + int json_len = 0; 92 92 + 93 93 + int ret = rift_s_read_firmware_block(hid_hmd, RIFT_S_FIRMWARE_BLOCK_THRESHOLD, &json, &json_len); 94 94 + if (ret < 0) 95 95 + return ret; 96 96 + 97 97 + ret = rift_s_parse_proximity_threshold(json, proximity_threshold); 98 98 + free(json); 99 99 + 100 100 + return ret; 101 101 + } 102 102 + 103 103 + static int 104 104 + read_hmd_config(struct os_hid_device *hid_hmd, struct rift_s_hmd_config *config) 105 105 + { 106 106 + int ret; 107 107 + 108 108 + ret = rift_s_read_firmware_version(hid_hmd); 109 109 + if (ret < 0) { 110 110 + RIFT_S_ERROR("Failed to read Rift S firmware version"); 111 111 + return ret; 112 112 + } 113 113 + 114 114 + ret = rift_s_read_panel_info(hid_hmd, &config->panel_info); 115 115 + if (ret < 0) { 116 116 + RIFT_S_ERROR("Failed to read Rift S device info"); 117 117 + return ret; 118 118 + } 119 119 + 120 120 + ret = rift_s_read_imu_config_info(hid_hmd, &config->imu_config_info); 121 121 + if (ret < 0) { 122 122 + RIFT_S_ERROR("Failed to read IMU configuration block"); 123 123 + return ret; 124 124 + } 125 125 + 126 126 + ret = read_hmd_fw_imu_calibration(hid_hmd, &config->imu_calibration); 127 127 + if (ret < 0) { 128 128 + RIFT_S_ERROR("Failed to read IMU configuration block"); 129 129 + return ret; 130 130 + } 131 131 + 132 132 + /* Configure the proximity sensor threshold */ 133 133 + ret = read_hmd_proximity_threshold(hid_hmd, &config->proximity_threshold); 134 134 + if (ret < 0) { 135 135 + RIFT_S_ERROR("Failed to read proximity sensor firmware block"); 136 136 + return ret; 137 137 + } 138 138 + 139 139 + ret = read_camera_calibration(hid_hmd, &config->camera_calibration); 140 140 + if (ret < 0) { 141 141 + RIFT_S_ERROR("Failed to read HMD camera calibration block"); 142 142 + return ret; 143 143 + } 144 144 + 145 145 + return 0; 146 146 + } 147 147 + 54 148 struct rift_s_system * 55 55 - rift_s_system_create(const unsigned char *hmd_serial_no, 149 149 + rift_s_system_create(struct xrt_prober *xp, 150 150 + const unsigned char *hmd_serial_no, 56 151 struct os_hid_device *hid_hmd, 57 152 struct os_hid_device *hid_status, 58 153 struct os_hid_device *hid_controllers) ··· 85 180 goto cleanup; 86 181 } 87 182 183 183 + if (read_hmd_config(hid_hmd, &sys->hmd_config) < 0) { 184 184 + RIFT_S_ERROR("Failed to read HMD configuration"); 185 185 + goto cleanup; 186 186 + } 187 187 + 188 188 + sys->tracker = rift_s_tracker_create(&sys->base, &sys->xfctx, &sys->hmd_config); 189 189 + if (sys->tracker == NULL) { 190 190 + RIFT_S_ERROR("Failed to init tracking"); 191 191 + goto cleanup; 192 192 + } 193 193 + 88 194 rift_s_radio_state_init(&sys->radio_state); 89 195 90 196 /* Create the HMD now. Controllers are created in the 91 197 * rift_s_system_get_controller() call later */ 92 92 - struct rift_s_hmd *hmd = rift_s_hmd_create(sys, hmd_serial_no); 198 198 + struct rift_s_hmd *hmd = rift_s_hmd_create(sys, hmd_serial_no, &sys->hmd_config); 93 199 if (hmd == NULL) { 94 200 RIFT_S_ERROR("Failed to create Oculus Rift S device."); 95 201 goto cleanup; ··· 118 224 // Two seconds seems to be needed for the display connection to stabilise 119 225 os_nanosleep((uint64_t)U_TIME_1S_IN_NS * 2); 120 226 227 227 + // Start the camera input 228 228 + struct rift_s_camera *cam = 229 229 + rift_s_camera_create(xp, &sys->xfctx, (const char *)hmd_serial_no, sys->handles[HMD_HID], sys->tracker, 230 230 + &sys->hmd_config.camera_calibration); 231 231 + if (cam == NULL) { 232 232 + RIFT_S_ERROR("Failed to open Rift S camera device"); 233 233 + goto cleanup; 234 234 + } 235 235 + os_mutex_lock(&sys->dev_mutex); 236 236 + sys->cam = cam; 237 237 + os_mutex_unlock(&sys->dev_mutex); 238 238 + 239 239 + rift_s_tracker_start(sys->tracker); 240 240 + 121 241 RIFT_S_DEBUG("Oculus Rift S driver ready"); 122 242 123 243 return sys; ··· 136 256 /* Stop the packet reading thread */ 137 257 os_thread_helper_destroy(&sys->oth); 138 258 259 259 + /* Stop all the frame processing (has to happen before the cameras 260 260 + * and tracker are destroyed */ 261 261 + xrt_frame_context_destroy_nodes(&sys->xfctx); 262 262 + 139 263 rift_s_radio_state_clear(&sys->radio_state); 140 264 141 265 if (sys->handles[HMD_HID]) { ··· 149 273 os_hid_destroy(sys->handles[i]); 150 274 } 151 275 276 276 + /* Free the camera */ 277 277 + if (sys->cam != NULL) { 278 278 + rift_s_camera_destroy(sys->cam); 279 279 + } 280 280 + 281 281 + if (sys->tracker != NULL) { 282 282 + rift_s_tracker_destroy(sys->tracker); 283 283 + } 284 284 + 152 285 os_mutex_destroy(&sys->dev_mutex); 153 286 154 287 free(sys); ··· 189 322 return &sys->radio_state; 190 323 } 191 324 325 325 + struct rift_s_tracker * 326 326 + rift_s_system_get_tracker(struct rift_s_system *sys) 327 327 + { 328 328 + return sys->tracker; 329 329 + } 330 330 + 192 331 struct xrt_device * 193 332 rift_s_system_get_hmd(struct rift_s_system *sys) 194 333 { ··· 233 372 } 234 373 235 374 os_mutex_unlock(&sys->dev_mutex); 375 375 + } 376 376 + 377 377 + struct xrt_device * 378 378 + rift_s_system_get_hand_tracking_device(struct rift_s_system *sys) 379 379 + { 380 380 + return rift_s_tracker_get_hand_tracking_device(sys->tracker); 236 381 } 237 382 238 383 /* Packet reading / handling */ ··· 420 565 421 566 if (success) { 422 567 rift_s_radio_update(&sys->radio_state, sys->handles[HMD_HID]); 568 568 + 569 569 + os_mutex_lock(&sys->dev_mutex); 570 570 + if (sys->cam != NULL) { 571 571 + rift_s_camera_update(sys->cam, sys->handles[HMD_HID]); 572 572 + } 573 573 + os_mutex_unlock(&sys->dev_mutex); 423 574 } 424 575 425 576 os_thread_helper_lock(&sys->oth);

+38 -1

src/xrt/drivers/rift_s/rift_s.h

··· 20 20 #include "os/os_threading.h" 21 21 #include "util/u_logging.h" 22 22 #include "xrt/xrt_defines.h" 23 23 + #include "xrt/xrt_frame.h" 24 24 + #include "xrt/xrt_frameserver.h" 25 25 + #include "xrt/xrt_prober.h" 23 26 #include "xrt/xrt_tracking.h" 24 27 28 28 + #include "rift_s_firmware.h" 25 29 #include "rift_s_protocol.h" 26 30 #include "rift_s_radio.h" 31 31 + #include "rift_s_tracker.h" 27 32 28 33 #ifndef RIFT_S_H 29 34 #define RIFT_S_H 30 35 31 36 struct rift_s_hmd; 32 37 struct rift_s_controller; 38 38 + struct rift_s_camera; 33 39 34 40 extern enum u_logging_level rift_s_log_level; 35 41 ··· 45 51 #define STATUS_HID 1 46 52 #define CONTROLLER_HID 2 47 53 54 54 + /* All HMD Configuration / calibration info */ 55 55 + struct rift_s_hmd_config 56 56 + { 57 57 + rift_s_panel_info_t panel_info; 58 58 + int proximity_threshold; 59 59 + 60 60 + /* Camera calibration block from firmware */ 61 61 + struct rift_s_camera_calibration_block camera_calibration; 62 62 + 63 63 + struct rift_s_imu_config_info_t imu_config_info; 64 64 + struct rift_s_imu_calibration imu_calibration; 65 65 + }; 66 66 + 48 67 /* Structure to track online devices and type */ 49 68 struct rift_s_tracked_device 50 69 { ··· 72 91 /* Device lock protects device access */ 73 92 struct os_mutex dev_mutex; 74 93 94 94 + /* All configuration data for the HMD, stored 95 95 + * here for sharing to child objects */ 96 96 + struct rift_s_hmd_config hmd_config; 97 97 + 98 98 + /* 3dof/SLAM tracker that provides HMD pose */ 99 99 + struct rift_s_tracker *tracker; 100 100 + 75 101 /* HMD device */ 76 102 struct rift_s_hmd *hmd; 77 103 78 104 /* Controller devices */ 79 105 struct rift_s_controller *controllers[MAX_TRACKED_DEVICES]; 106 106 + 107 107 + /* Video feed handling */ 108 108 + struct xrt_frame_context xfctx; 109 109 + struct rift_s_camera *cam; 80 110 }; 81 111 82 112 struct rift_s_system * 83 83 - rift_s_system_create(const unsigned char *hmd_serial_no, 113 113 + rift_s_system_create(struct xrt_prober *xp, 114 114 + const unsigned char *hmd_serial_no, 84 115 struct os_hid_device *hid_hmd, 85 116 struct os_hid_device *hid_status, 86 117 struct os_hid_device *hid_controllers); ··· 90 121 rift_s_radio_state * 91 122 rift_s_system_radio(struct rift_s_system *sys); 92 123 124 124 + struct rift_s_tracker * 125 125 + rift_s_system_get_tracker(struct rift_s_system *sys); 126 126 + 93 127 struct xrt_device * 94 128 rift_s_system_get_hmd(struct rift_s_system *sys); 95 129 void ··· 99 133 rift_s_system_get_controller(struct rift_s_system *sys, int index); 100 134 void 101 135 rift_s_system_remove_controller(struct rift_s_system *sys, struct rift_s_controller *ctrl); 136 136 + 137 137 + struct xrt_device * 138 138 + rift_s_system_get_hand_tracking_device(struct rift_s_system *sys); 102 139 103 140 void 104 141 rift_s_system_reference(struct rift_s_system **dst, struct rift_s_system *src);

+27 -5

src/xrt/drivers/rift_s/rift_s_builder.c

··· 14 14 15 15 #include "os/os_hid.h" 16 16 17 17 + #include "xrt/xrt_config_drivers.h" 17 18 #include "xrt/xrt_prober.h" 18 19 19 20 #include "util/u_builders.h" ··· 22 23 #include "util/u_logging.h" 23 24 #include "util/u_system_helpers.h" 24 25 #include "util/u_trace_marker.h" 26 26 + 27 27 + #ifdef XRT_BUILD_DRIVER_HANDTRACKING 28 28 + #include "ht_ctrl_emu/ht_ctrl_emu_interface.h" 29 29 + #endif 25 30 26 31 #include "rift_s_interface.h" 27 32 #include "rift_s.h" ··· 136 141 goto fail; 137 142 } 138 143 139 139 - struct rift_s_system *sys = rift_s_system_create(hmd_serial_no, hid_hmd, hid_status, hid_controllers); 144 144 + struct rift_s_system *sys = rift_s_system_create(xp, hmd_serial_no, hid_hmd, hid_status, hid_controllers); 140 145 if (sys == NULL) { 141 146 RIFT_S_ERROR("Failed to initialise Oculus Rift S driver"); 142 147 goto fail; 143 148 } 144 149 145 145 - struct xrt_device *xdev = rift_s_system_get_hmd(sys); 146 146 - usysd->base.xdevs[usysd->base.xdev_count++] = xdev; 147 147 - usysd->base.roles.head = xdev; 150 150 + struct xrt_device *hmd_xdev = rift_s_system_get_hmd(sys); 151 151 + usysd->base.xdevs[usysd->base.xdev_count++] = hmd_xdev; 152 152 + usysd->base.roles.head = hmd_xdev; 148 153 149 149 - xdev = rift_s_system_get_controller(sys, 0); 154 154 + struct xrt_device *xdev = rift_s_system_get_controller(sys, 0); 150 155 usysd->base.xdevs[usysd->base.xdev_count++] = xdev; 151 156 usysd->base.roles.left = xdev; 152 157 153 158 xdev = rift_s_system_get_controller(sys, 1); 154 159 usysd->base.xdevs[usysd->base.xdev_count++] = xdev; 155 160 usysd->base.roles.right = xdev; 161 161 + 162 162 + #ifdef XRT_BUILD_DRIVER_HANDTRACKING 163 163 + struct xrt_device *ht_xdev = rift_s_system_get_hand_tracking_device(sys); 164 164 + if (ht_xdev != NULL) { 165 165 + // Create hand-tracked controllers 166 166 + RIFT_S_DEBUG("Creating emulated hand tracking controllers"); 167 167 + 168 168 + struct xrt_device *two_hands[2]; 169 169 + cemu_devices_create(hmd_xdev, ht_xdev, two_hands); 170 170 + 171 171 + usysd->base.roles.hand_tracking.left = two_hands[0]; 172 172 + usysd->base.roles.hand_tracking.right = two_hands[1]; 173 173 + 174 174 + usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[0]; 175 175 + usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[1]; 176 176 + } 177 177 + #endif 156 178 157 179 *out_xsysd = &usysd->base; 158 180

+482

src/xrt/drivers/rift_s/rift_s_camera.c

··· 1 1 + /* 2 2 + * Copyright 2021, Collabora, Ltd. 3 3 + * Copyright 2022 Jan Schmidt 4 4 + * SPDX-License-Identifier: BSL-1.0 5 5 + * 6 6 + */ 7 7 + 8 8 + /*! 9 9 + * @file 10 10 + * @brief Oculus Rift S camera handling 11 11 + * 12 12 + * The Rift S camera module, handles reception and dispatch 13 13 + * of camera frames. 14 14 + * 15 15 + * @author Jan Schmidt <jan@centricular.com> 16 16 + * @ingroup drv_rift_s 17 17 + */ 18 18 + #include <asm/byteorder.h> 19 19 + #include <string.h> 20 20 + #include <inttypes.h> 21 21 + 22 22 + #include "rift_s.h" 23 23 + #include "rift_s_camera.h" 24 24 + 25 25 + #include "os/os_threading.h" 26 26 + 27 27 + #include "xrt/xrt_defines.h" 28 28 + #include "xrt/xrt_frame.h" 29 29 + #include "xrt/xrt_frameserver.h" 30 30 + 31 31 + #include "util/u_autoexpgain.h" 32 32 + #include "util/u_debug.h" 33 33 + #include "util/u_var.h" 34 34 + #include "util/u_sink.h" 35 35 + #include "util/u_frame.h" 36 36 + #include "util/u_trace_marker.h" 37 37 + 38 38 + #define DEFAULT_EXPOSURE 6000 39 39 + #define DEFAULT_GAIN 127 40 40 + 41 41 + #define RIFT_S_MIN_EXPOSURE 38 42 42 + #define RIFT_S_MAX_EXPOSURE 14022 43 43 + 44 44 + #define RIFT_S_MIN_GAIN 16 45 45 + #define RIFT_S_MAX_GAIN 255 46 46 + 47 47 + //! Specifies whether the user wants to enable autoexposure from the start. 48 48 + DEBUG_GET_ONCE_BOOL_OPTION(rift_s_autoexposure, "RIFT_S_AUTOEXPOSURE", true) 49 49 + 50 50 + struct rift_s_camera 51 51 + { 52 52 + struct os_mutex lock; 53 53 + 54 54 + struct rift_s_tracker *tracker; 55 55 + 56 56 + struct rift_s_camera_calibration_block *camera_calibration; 57 57 + 58 58 + struct xrt_frame_sink in_sink; // Receive raw frames and split them 59 59 + 60 60 + struct u_sink_debug debug_sinks[2]; 61 61 + 62 62 + rift_s_camera_report_t camera_report; 63 63 + 64 64 + uint16_t last_slam_exposure, target_exposure; 65 65 + uint8_t last_slam_gain, target_gain; 66 66 + 67 67 + bool manual_control; //!< Whether to control exp/gain manually or with aeg 68 68 + struct u_var_draggable_u16 exposure_ui; //! Widget to control `exposure` value 69 69 + struct u_autoexpgain *aeg; 70 70 + }; 71 71 + 72 72 + struct rift_s_camera_finder 73 73 + { 74 74 + const char *hmd_serial_no; 75 75 + 76 76 + struct xrt_fs *xfs; 77 77 + struct xrt_frame_context *xfctx; 78 78 + }; 79 79 + 80 80 + union rift_s_frame_data { 81 81 + struct 82 82 + { 83 83 + uint8_t frame_type; // 0x06 or 0x86 (controller or SLAM exposure) 84 84 + __le16 magic_abcd; // 0xabcd 85 85 + __le16 frame_ctr; // Increments every exposure 86 86 + __le32 const1; // QHWH 87 87 + uint8_t pad1[7]; // all zeroes padding to 16 bytes 88 88 + __le64 frame_ts; // microseconds 89 89 + __le32 frame_ctr2; // Another frame counter, but only increments on alternate frames @ 30Hz 90 90 + __le16 slam_exposure[5]; // One 16-bit per camera. Exposure duration? 91 91 + uint8_t pad2[2]; // zero padding 92 92 + uint8_t slam_gain[5]; // One byte per camera. 0x40 or 0xf0 depending on frame type 93 93 + uint8_t pad3; // zero padding 94 94 + __le16 unknown1; // changes every frame. No clear pattern 95 95 + __le16 magic_face; // 0xface 96 96 + 97 97 + } __attribute__((packed)) data; 98 98 + uint8_t raw[50]; 99 99 + }; 100 100 + 101 101 + static void 102 102 + update_expgain(struct rift_s_camera *cam, struct xrt_frame *xf); 103 103 + 104 104 + static void 105 105 + receive_cam_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf); 106 106 + 107 107 + static void 108 108 + on_video_device(struct xrt_prober *xp, 109 109 + struct xrt_prober_device *pdev, 110 110 + const char *product, 111 111 + const char *manufacturer, 112 112 + const char *serial, 113 113 + void *ptr) 114 114 + { 115 115 + struct rift_s_camera_finder *finder = (struct rift_s_camera_finder *)ptr; 116 116 + 117 117 + /* Already found a device? */ 118 118 + if (finder->xfs != NULL) 119 119 + return; 120 120 + 121 121 + if (product == NULL || manufacturer == NULL || serial == NULL) { 122 122 + return; 123 123 + } 124 124 + 125 125 + RIFT_S_TRACE("Inspecting video device %s - %s serial %s", manufacturer, product, serial); 126 126 + 127 127 + if ((strcmp(product, "Rift S Sensor") == 0) && (strcmp(manufacturer, "Oculus VR") == 0)) { 128 128 + // && (strcmp(serial, finder->hmd_serial_no) == 0)) { 129 129 + // Serial no seems to be all zeros right now, so ignore it 130 130 + xrt_prober_open_video_device(xp, pdev, finder->xfctx, &finder->xfs); 131 131 + return; 132 132 + } 133 133 + } 134 134 + 135 135 + struct rift_s_camera * 136 136 + rift_s_camera_create(struct xrt_prober *xp, 137 137 + struct xrt_frame_context *xfctx, 138 138 + const char *hmd_serial_no, 139 139 + struct os_hid_device *hid, 140 140 + struct rift_s_tracker *tracker, 141 141 + struct rift_s_camera_calibration_block *camera_calibration) 142 142 + { 143 143 + struct rift_s_camera_finder finder = { 144 144 + 0, 145 145 + }; 146 146 + 147 147 + DRV_TRACE_MARKER(); 148 148 + 149 149 + /* Set up the finder with the HMD serial number and frame server context we want */ 150 150 + finder.xfctx = xfctx; 151 151 + finder.hmd_serial_no = hmd_serial_no; 152 152 + 153 153 + /* Re-probe devices. The v4l2 camera device should have appeared by now */ 154 154 + int retry_count = 5; 155 155 + do { 156 156 + xrt_result_t xret = xrt_prober_probe(xp); 157 157 + 158 158 + if (xret != XRT_SUCCESS) { 159 159 + return NULL; 160 160 + } 161 161 + 162 162 + xrt_prober_list_video_devices(xp, on_video_device, &finder); 163 163 + if (finder.xfs != NULL) { 164 164 + break; 165 165 + } 166 166 + 167 167 + /* Sleep 1 second before retry */ 168 168 + os_nanosleep((uint64_t)U_TIME_1S_IN_NS); 169 169 + } while (retry_count-- > 0); 170 170 + 171 171 + if (finder.xfs == NULL) { 172 172 + RIFT_S_ERROR("Didn't find Rift S camera device"); 173 173 + return NULL; 174 174 + } 175 175 + 176 176 + struct rift_s_camera *cam = U_TYPED_CALLOC(struct rift_s_camera); 177 177 + 178 178 + if (os_mutex_init(&cam->lock) != 0) { 179 179 + RIFT_S_ERROR("Failed to init camera configuration mutex"); 180 180 + goto cleanup; 181 181 + } 182 182 + 183 183 + // Store the tracker 184 184 + cam->tracker = tracker; 185 185 + cam->camera_calibration = camera_calibration; 186 186 + 187 187 + /* Configure default camera settings */ 188 188 + rift_s_protocol_camera_report_init(&cam->camera_report); 189 189 + cam->camera_report.uvc_enable = 0x1; 190 190 + cam->camera_report.radio_sync_flag = 0x1; 191 191 + 192 192 + /* Store the defaults from the init() call into our current settings */ 193 193 + cam->last_slam_exposure = cam->camera_report.slam_frame_exposures[0]; 194 194 + cam->last_slam_gain = cam->camera_report.slam_frame_gains[0]; 195 195 + 196 196 + cam->target_exposure = DEFAULT_EXPOSURE; 197 197 + cam->target_gain = DEFAULT_GAIN; 198 198 + 199 199 + rift_s_camera_update(cam, hid); 200 200 + 201 201 + cam->in_sink.push_frame = receive_cam_frame; 202 202 + 203 203 + bool enable_aeg = debug_get_bool_option_rift_s_autoexposure(); 204 204 + int frame_delay = 205 205 + 3; // WMR takes about three frames until the cmd changes the image. TODO: Confirm this for Rift S 206 206 + cam->aeg = u_autoexpgain_create(U_AEG_STRATEGY_TRACKING, enable_aeg, frame_delay); 207 207 + 208 208 + u_sink_debug_init(&cam->debug_sinks[0]); 209 209 + u_sink_debug_init(&cam->debug_sinks[1]); 210 210 + 211 211 + struct xrt_frame_sink *tmp = &cam->in_sink; 212 212 + 213 213 + struct xrt_fs_mode *modes = NULL; 214 214 + uint32_t count; 215 215 + 216 216 + xrt_fs_enumerate_modes(finder.xfs, &modes, &count); 217 217 + 218 218 + bool found_mode = false; 219 219 + uint32_t selected_mode = 0; 220 220 + 221 221 + for (; selected_mode < count; selected_mode++) { 222 222 + if (modes[selected_mode].format == XRT_FORMAT_YUYV422) { 223 223 + found_mode = true; 224 224 + break; 225 225 + } 226 226 + if (modes[selected_mode].format == XRT_FORMAT_MJPEG) { 227 227 + u_sink_create_format_converter(xfctx, XRT_FORMAT_L8, tmp, &tmp); 228 228 + found_mode = true; 229 229 + break; 230 230 + } 231 231 + } 232 232 + 233 233 + if (!found_mode) { 234 234 + selected_mode = 0; 235 235 + RIFT_S_ERROR("Couldn't find compatible camera input format."); 236 236 + goto cleanup; 237 237 + } 238 238 + 239 239 + free(modes); 240 240 + 241 241 + u_var_add_root(cam, "Oculus Rift S Cameras", true); 242 242 + 243 243 + u_var_add_bool(cam, &cam->manual_control, "Manual exposure and gain control"); 244 244 + cam->exposure_ui.val = &cam->target_exposure; 245 245 + cam->exposure_ui.min = RIFT_S_MIN_EXPOSURE; 246 246 + cam->exposure_ui.max = RIFT_S_MAX_EXPOSURE; 247 247 + cam->exposure_ui.step = 25; 248 248 + 249 249 + u_var_add_draggable_u16(cam, &cam->exposure_ui, "Exposure"); 250 250 + u_var_add_u8(cam, &cam->target_gain, "Gain"); 251 251 + u_var_add_gui_header(cam, NULL, "Auto exposure and gain control"); 252 252 + u_autoexpgain_add_vars(cam->aeg, cam); 253 253 + 254 254 + u_var_add_gui_header(cam, NULL, "Camera Streams"); 255 255 + u_var_add_sink_debug(cam, &cam->debug_sinks[0], "Tracking Streams"); 256 256 + u_var_add_sink_debug(cam, &cam->debug_sinks[1], "Controller Streams"); 257 257 + 258 258 + /* Finally, start the video feed */ 259 259 + xrt_fs_stream_start(finder.xfs, tmp, XRT_FS_CAPTURE_TYPE_TRACKING, selected_mode); 260 260 + 261 261 + return cam; 262 262 + 263 263 + cleanup: 264 264 + rift_s_camera_destroy(cam); 265 265 + return NULL; 266 266 + } 267 267 + 268 268 + void 269 269 + rift_s_camera_destroy(struct rift_s_camera *cam) 270 270 + { 271 271 + u_var_remove_root(cam); 272 272 + os_mutex_destroy(&cam->lock); 273 273 + free(cam); 274 274 + } 275 275 + 276 276 + static bool 277 277 + parse_frame_data(const struct xrt_frame *xf, union rift_s_frame_data *row_data) 278 278 + { 279 279 + /* Parse out the bits encoded as 8x8 blocks in the top rows */ 280 280 + unsigned int x, out_x; 281 281 + 282 282 + if (xf->width != 50 * 8 * 8 || xf->height < 8) 283 283 + return false; 284 284 + 285 285 + uint8_t *pix = &xf->data[xf->width * 4]; 286 286 + 287 287 + int bit = 7; 288 288 + for (x = 4, out_x = 0; x < xf->width; x += 8) { 289 289 + uint8_t val = 0; 290 290 + if (pix[x] > 128) 291 291 + val = 1 << bit; 292 292 + 293 293 + if (bit == 7) { 294 294 + row_data->raw[out_x] = val; 295 295 + } else { 296 296 + row_data->raw[out_x] |= val; 297 297 + } 298 298 + if (bit > 0) 299 299 + bit--; 300 300 + else { 301 301 + bit = 7; 302 302 + out_x++; 303 303 + } 304 304 + } 305 305 + 306 306 + /* Check magic numbers */ 307 307 + if (__le16_to_cpu(row_data->data.magic_abcd) != 0xabcd) 308 308 + return false; 309 309 + if (__le16_to_cpu(row_data->data.magic_face) != 0xface) 310 310 + return false; 311 311 + 312 312 + return true; 313 313 + } 314 314 + 315 315 + static int 316 316 + get_y_offset(struct rift_s_camera *cam, enum rift_s_camera_id cam_id, union rift_s_frame_data *row_data) 317 317 + { 318 318 + /* There's a magic formula for computing the vertical offset of each camera view 319 319 + * based on exposure, due to some internals of the headset. This formula extracted 320 320 + * through trial and error */ 321 321 + int exposure = __le16_to_cpu(row_data->data.slam_exposure[cam_id]); 322 322 + int y_offset = (exposure + 275) / 38; 323 323 + 324 324 + if (y_offset > 375) { 325 325 + y_offset = 375; 326 326 + } else if (y_offset < 8) { 327 327 + y_offset = 8; 328 328 + } 329 329 + 330 330 + return y_offset; 331 331 + } 332 332 + 333 333 + static struct xrt_frame * 334 334 + rift_s_camera_extract_frame(struct rift_s_camera *cam, 335 335 + enum rift_s_camera_id cam_id, 336 336 + struct xrt_frame *full_frame, 337 337 + union rift_s_frame_data *row_data) 338 338 + { 339 339 + struct rift_s_camera_calibration *calib = &cam->camera_calibration->cameras[cam_id]; 340 340 + struct xrt_rect roi = calib->roi; 341 341 + 342 342 + roi.offset.h = get_y_offset(cam, cam_id, row_data); 343 343 + 344 344 + struct xrt_frame *xf_crop = NULL; 345 345 + 346 346 + u_frame_create_roi(full_frame, roi, &xf_crop); 347 347 + 348 348 + return xf_crop; 349 349 + } 350 350 + 351 351 + static void 352 352 + receive_cam_frame(struct xrt_frame_sink *sink, struct xrt_frame *xf) 353 353 + { 354 354 + struct rift_s_camera *cam = container_of(sink, struct rift_s_camera, in_sink); 355 355 + bool release_xf = false; 356 356 + 357 357 + RIFT_S_TRACE("cam img t=%" PRIu64 " source_t=%" PRIu64, xf->timestamp, xf->source_timestamp); 358 358 + 359 359 + // If the format is YUYV422 we need to override it to L8 and double the width 360 360 + // because the v4l2 device provides the wrong format description for the actual video 361 361 + // data 362 362 + if (xf->format == XRT_FORMAT_YUYV422) { 363 363 + struct xrt_rect roi = {.offset = {0, 0}, .extent = {.w = xf->width, .h = xf->height}}; 364 364 + struct xrt_frame *xf_l8 = NULL; 365 365 + 366 366 + u_frame_create_roi(xf, roi, &xf_l8); 367 367 + xf_l8->width = 2 * xf->width; 368 368 + xf_l8->format = XRT_FORMAT_L8; 369 369 + 370 370 + xf = xf_l8; 371 371 + release_xf = true; 372 372 + } 373 373 + 374 374 + // Dump mid-row of the 8 pix data line 375 375 + union rift_s_frame_data row_data; 376 376 + 377 377 + if (!parse_frame_data(xf, &row_data)) { 378 378 + RIFT_S_TRACE("Invalid frame top-row data. Skipping"); 379 379 + return; 380 380 + } 381 381 + 382 382 + RIFT_S_DEBUG("frame ctr %u ts %" PRIu64 383 383 + " µS pair ctr %u " 384 384 + "exposure[0] %u gain[0] %u unk %u", 385 385 + (uint16_t)__le16_to_cpu(row_data.data.frame_ctr), (uint64_t)__le64_to_cpu(row_data.data.frame_ts), 386 386 + (uint32_t)__le32_to_cpu(row_data.data.frame_ctr2), 387 387 + (uint16_t)__le16_to_cpu(row_data.data.slam_exposure[0]), row_data.data.slam_gain[0], 388 388 + (uint16_t)__le16_to_cpu(row_data.data.unknown1)); 389 389 + 390 390 + // rift_s_hexdump_buffer("Row data", row_data.raw, sizeof(row_data.row)); 391 391 + 392 392 + // If the top left pixel is > 128, send as SLAM frame else controller 393 393 + if (row_data.data.frame_type & 0x80) { 394 394 + int y_offset = get_y_offset(cam, 0, &row_data); 395 395 + struct xrt_rect roi = {.offset = {0, y_offset}, .extent = {.w = xf->width, .h = 480}}; 396 396 + 397 397 + struct xrt_frame *xf_crop = NULL; 398 398 + u_frame_create_roi(xf, roi, &xf_crop); 399 399 + u_sink_debug_push_frame(&cam->debug_sinks[0], xf_crop); 400 400 + xrt_frame_reference(&xf_crop, NULL); 401 401 + 402 402 + /* Extract left and right frames and push to the tracker */ 403 403 + struct xrt_frame *left = rift_s_camera_extract_frame(cam, RIFT_S_CAMERA_FRONT_LEFT, xf, &row_data); 404 404 + struct xrt_frame *right = rift_s_camera_extract_frame(cam, RIFT_S_CAMERA_FRONT_RIGHT, xf, &row_data); 405 405 + 406 406 + /* Update the exposure for all cameras based on the auto exposure for the left camera view */ 407 407 + update_expgain(cam, left); 408 408 + 409 409 + uint64_t frame_ts_ns = (uint64_t)__le64_to_cpu(row_data.data.frame_ts) * OS_NS_PER_USEC; 410 410 + rift_s_tracker_push_slam_frames(cam->tracker, frame_ts_ns, left, right); 411 411 + 412 412 + xrt_frame_reference(&left, NULL); 413 413 + xrt_frame_reference(&right, NULL); 414 414 + } else { 415 415 + struct xrt_rect roi = {.offset = {0, 40}, .extent = {.w = xf->width, .h = 480}}; 416 416 + struct xrt_frame *xf_crop = NULL; 417 417 + 418 418 + u_frame_create_roi(xf, roi, &xf_crop); 419 419 + u_sink_debug_push_frame(&cam->debug_sinks[1], xf_crop); 420 420 + xrt_frame_reference(&xf_crop, NULL); 421 421 + } 422 422 + if (release_xf) 423 423 + xrt_frame_reference(&xf, NULL); 424 424 + } 425 425 + 426 426 + static void 427 427 + update_expgain(struct rift_s_camera *cam, struct xrt_frame *xf) 428 428 + { 429 429 + if (!cam->manual_control && xf != NULL) { 430 430 + u_autoexpgain_update(cam->aeg, xf); 431 431 + 432 432 + uint16_t new_target_exposure; 433 433 + uint8_t new_target_gain; 434 434 + 435 435 + new_target_exposure = 436 436 + CLAMP(u_autoexpgain_get_exposure(cam->aeg), RIFT_S_MIN_EXPOSURE, RIFT_S_MAX_EXPOSURE); 437 437 + new_target_gain = CLAMP(u_autoexpgain_get_gain(cam->aeg), RIFT_S_MIN_GAIN, RIFT_S_MAX_GAIN); 438 438 + 439 439 + if (cam->target_exposure != new_target_exposure || cam->target_gain != new_target_gain) { 440 440 + RIFT_S_DEBUG("AEG exposure now %u (cur %u) gain %u (cur %u)", new_target_exposure, 441 441 + cam->target_exposure, new_target_gain, cam->target_gain); 442 442 + 443 443 + os_mutex_lock(&cam->lock); 444 444 + cam->target_exposure = new_target_exposure; 445 445 + cam->target_gain = new_target_gain; 446 446 + os_mutex_unlock(&cam->lock); 447 447 + } 448 448 + } 449 449 + } 450 450 + 451 451 + /* Called from the Rift S system device USB loop, so we can check 452 452 + * and send an exposure/gain change command if needed */ 453 453 + void 454 454 + rift_s_camera_update(struct rift_s_camera *cam, struct os_hid_device *hid) 455 455 + { 456 456 + bool need_update = false; 457 457 + int i; 458 458 + 459 459 + os_mutex_lock(&cam->lock); 460 460 + if (cam->target_exposure != cam->last_slam_exposure) { 461 461 + for (i = 0; i < 5; i++) { 462 462 + cam->camera_report.slam_frame_exposures[i] = cam->target_exposure; 463 463 + } 464 464 + cam->last_slam_exposure = cam->target_exposure; 465 465 + need_update = true; 466 466 + } 467 467 + 468 468 + if (cam->target_gain != cam->last_slam_gain) { 469 469 + for (i = 0; i < 5; i++) { 470 470 + cam->camera_report.slam_frame_gains[i] = cam->target_gain; 471 471 + } 472 472 + cam->last_slam_gain = cam->target_gain; 473 473 + need_update = true; 474 474 + } 475 475 + os_mutex_unlock(&cam->lock); 476 476 + 477 477 + if (need_update) { 478 478 + if (rift_s_protocol_send_camera_report(hid, &cam->camera_report) < 0) { 479 479 + RIFT_S_WARN("Failed to update camera settings"); 480 480 + } 481 481 + } 482 482 + }

+38

src/xrt/drivers/rift_s/rift_s_camera.h

··· 1 1 + /* 2 2 + * Copyright 2013, Fredrik Hultin. 3 3 + * Copyright 2013, Jakob Bornecrantz. 4 4 + * Copyright 2016 Philipp Zabel 5 5 + * Copyright 2019-2022 Jan Schmidt 6 6 + * SPDX-License-Identifier: BSL-1.0 7 7 + */ 8 8 + 9 9 + /*! 10 10 + * @file 11 11 + * @brief Oculus Rift S camera handling 12 12 + * @author Jan Schmidt <jan@centricular.com> 13 13 + * @ingroup drv_rift_s 14 14 + */ 15 15 + 16 16 + #pragma once 17 17 + 18 18 + #include "os/os_hid.h" 19 19 + #include "xrt/xrt_prober.h" 20 20 + 21 21 + #include "rift_s_firmware.h" 22 22 + #include "rift_s_tracker.h" 23 23 + 24 24 + struct rift_s_camera; 25 25 + 26 26 + struct rift_s_camera * 27 27 + rift_s_camera_create(struct xrt_prober *xp, 28 28 + struct xrt_frame_context *xfctx, 29 29 + const char *hmd_serial_no, 30 30 + struct os_hid_device *hid, 31 31 + struct rift_s_tracker *tracker, 32 32 + struct rift_s_camera_calibration_block *camera_calibration); 33 33 + 34 34 + void 35 35 + rift_s_camera_destroy(struct rift_s_camera *cam); 36 36 + 37 37 + void 38 38 + rift_s_camera_update(struct rift_s_camera *cam, struct os_hid_device *hid);

+1

src/xrt/drivers/rift_s/rift_s_firmware.c

··· 22 22 #include "util/u_json.h" 23 23 #include "util/u_misc.h" 24 24 25 25 + #include "rift_s.h" 25 26 #include "rift_s_firmware.h" 26 27 27 28 #define JSON_INT(a, b, c) u_json_get_int(u_json_get(a, b), c)

+1 -3

src/xrt/drivers/rift_s/rift_s_firmware.h

··· 20 20 #include "math/m_mathinclude.h" 21 21 #include "math/m_api.h" 22 22 23 23 - #include "rift_s.h" 24 24 - 25 23 enum rift_s_firmware_block 26 24 { 27 25 RIFT_S_FIRMWARE_BLOCK_SERIAL_NUM = 0x0B, ··· 32 30 RIFT_S_FIRMWARE_BLOCK_LENS_CALIB = 0x12 33 31 }; 34 32 35 35 - enum rift_s_camera_ids 33 33 + enum rift_s_camera_id 36 34 { 37 35 RIFT_S_CAMERA_TOP = 0x0, 38 36 RIFT_S_CAMERA_SIDE_LEFT = 0x1,

+43 -123

src/xrt/drivers/rift_s/rift_s_hmd.c

··· 62 62 63 63 U_ZERO(out_relation); 64 64 65 65 - // Estimate pose at timestamp at_timestamp_ns! 66 66 - os_mutex_lock(&hmd->mutex); 67 67 - math_quat_normalize(&hmd->pose.orientation); 68 68 - out_relation->pose = hmd->pose; 69 69 - out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | 70 70 - XRT_SPACE_RELATION_POSITION_VALID_BIT | 71 71 - XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 72 72 - os_mutex_unlock(&hmd->mutex); 65 65 + rift_s_tracker_get_tracked_pose(hmd->tracker, RIFT_S_TRACKER_POSE_DEVICE, at_timestamp_ns, out_relation); 73 66 } 74 67 75 68 static void ··· 88 81 void 89 82 rift_s_hmd_handle_report(struct rift_s_hmd *hmd, timepoint_ns local_ts, rift_s_hmd_report_t *report) 90 83 { 91 91 - const uint32_t TICK_LEN_US = 1000000 / hmd->imu_config.imu_hz; 84 84 + struct rift_s_imu_config_info_t *imu_config = &hmd->config->imu_config_info; 85 85 + struct rift_s_imu_calibration *imu_calibration = &hmd->config->imu_calibration; 86 86 + 87 87 + const uint32_t TICK_LEN_US = 1000000 / imu_config->imu_hz; 92 88 uint32_t dt = TICK_LEN_US; 93 89 94 94 - os_mutex_lock(&hmd->mutex); 90 90 + /* Check that there's at least 1 valid sample */ 91 91 + if (report->samples[0].marker & 0x80) 92 92 + return; 95 93 96 94 if (hmd->last_imu_timestamp_ns != 0) { 97 95 /* Avoid wrap-around on 32-bit device times */ 98 96 dt = report->timestamp - hmd->last_imu_timestamp32; 99 97 } else { 100 100 - hmd->last_imu_timestamp_ns = report->timestamp; 98 98 + hmd->last_imu_timestamp_ns = (timepoint_ns)(report->timestamp) * OS_NS_PER_USEC; 99 99 + hmd->last_imu_timestamp32 = report->timestamp; 101 100 } 102 102 - hmd->last_imu_timestamp32 = report->timestamp; 103 103 - hmd->last_imu_local_timestamp_ns = local_ts; 104 101 105 105 - const float gyro_scale = 1.0 / hmd->imu_config.gyro_scale; 106 106 - const float accel_scale = MATH_GRAVITY_M_S2 / hmd->imu_config.accel_scale; 107 107 - const float temperature_scale = 1.0 / hmd->imu_config.temperature_scale; 108 108 - const float temperature_offset = hmd->imu_config.temperature_offset; 102 102 + const float gyro_scale = 1.0 / imu_config->gyro_scale; 103 103 + const float accel_scale = MATH_GRAVITY_M_S2 / imu_config->accel_scale; 104 104 + const float temperature_scale = 1.0 / imu_config->temperature_scale; 105 105 + const float temperature_offset = imu_config->temperature_offset; 109 106 110 107 for (int i = 0; i < 3; i++) { 111 108 rift_s_hmd_imu_sample_t *s = report->samples + i; ··· 113 110 if (s->marker & 0x80) 114 111 break; /* Sample (and remaining ones) are invalid */ 115 112 116 116 - struct xrt_vec3 gyro, accel; 113 113 + struct xrt_vec3 raw_accel, raw_gyro; 114 114 + struct xrt_vec3 accel, gyro; 117 115 118 118 - gyro.x = DEG_TO_RAD(gyro_scale * s->gyro[0]); 119 119 - gyro.y = DEG_TO_RAD(gyro_scale * s->gyro[1]); 120 120 - gyro.z = DEG_TO_RAD(gyro_scale * s->gyro[2]); 116 116 + raw_gyro.x = DEG_TO_RAD(gyro_scale * s->gyro[0]); 117 117 + raw_gyro.y = DEG_TO_RAD(gyro_scale * s->gyro[1]); 118 118 + raw_gyro.z = DEG_TO_RAD(gyro_scale * s->gyro[2]); 121 119 122 122 - accel.x = accel_scale * s->accel[0]; 123 123 - accel.y = accel_scale * s->accel[1]; 124 124 - accel.z = accel_scale * s->accel[2]; 120 120 + raw_accel.x = accel_scale * s->accel[0]; 121 121 + raw_accel.y = accel_scale * s->accel[1]; 122 122 + raw_accel.z = accel_scale * s->accel[2]; 125 123 126 124 /* Apply correction offsets first, then rectify */ 127 127 - accel = m_vec3_sub(accel, hmd->imu_calibration.accel.offset_at_0C); 128 128 - gyro = m_vec3_sub(gyro, hmd->imu_calibration.gyro.offset); 125 125 + accel = m_vec3_sub(raw_accel, imu_calibration->accel.offset_at_0C); 126 126 + gyro = m_vec3_sub(raw_gyro, imu_calibration->gyro.offset); 129 127 130 130 - math_matrix_3x3_transform_vec3(&hmd->imu_calibration.accel.rectification, &accel, &hmd->raw_accel); 131 131 - math_matrix_3x3_transform_vec3(&hmd->imu_calibration.gyro.rectification, &gyro, &hmd->raw_gyro); 128 128 + math_matrix_3x3_transform_vec3(&imu_calibration->accel.rectification, &raw_accel, &accel); 129 129 + math_matrix_3x3_transform_vec3(&imu_calibration->gyro.rectification, &raw_gyro, &gyro); 132 130 133 131 /* FIXME: This doesn't seem to produce the right numbers, but it's OK - we don't use it anyway */ 134 132 hmd->temperature = temperature_scale * (s->temperature - temperature_offset) + 25; 135 133 136 134 #if 0 137 137 - printf ("Sample %d dt %f accel %f %f %f gyro %f %f %f\n", 138 138 - i, dt_sec, hmd->raw_accel.x, hmd->raw_accel.y, hmd->raw_accel.z, 139 139 - hmd->raw_gyro.x, hmd->raw_gyro.y, hmd->raw_gyro.z); 135 135 + RIFT_S_DEBUG("Sample %d dt %f ts %" PRIu64 " report ts %u " 136 136 + "accel %f %f %f (len %f) gyro %f %f %f", 137 137 + i, (double)(dt) / (1000000), hmd->last_imu_timestamp_ns, 138 138 + report->timestamp, 139 139 + accel.x, accel.y, accel.z, m_vec3_len(raw_accel), 140 140 + gyro.x, gyro.y, gyro.z); 140 141 #endif 141 142 142 142 - // Do 3DOF fusion 143 143 - m_imu_3dof_update(&hmd->fusion, hmd->last_imu_timestamp_ns, &hmd->raw_accel, &hmd->raw_gyro); 143 143 + // Send the sample to the pose tracker 144 144 + rift_s_tracker_imu_update(hmd->tracker, hmd->last_imu_timestamp_ns, local_ts, &accel, &gyro); 144 145 145 146 hmd->last_imu_timestamp_ns += (uint64_t)dt * OS_NS_PER_USEC; 147 147 + hmd->last_imu_timestamp32 += dt; 146 148 dt = TICK_LEN_US; 147 149 } 148 148 - 149 149 - hmd->pose.orientation = hmd->fusion.rot; 150 150 - os_mutex_unlock(&hmd->mutex); 151 150 } 152 151 153 152 static bool ··· 173 172 } 174 173 #endif 175 174 176 176 - static int 177 177 - read_hmd_calibration(struct rift_s_hmd *hmd, struct os_hid_device *hid_hmd) 178 178 - { 179 179 - char *json = NULL; 180 180 - int json_len = 0; 181 181 - 182 182 - int ret = rift_s_read_firmware_block(hid_hmd, RIFT_S_FIRMWARE_BLOCK_IMU_CALIB, &json, &json_len); 183 183 - if (ret < 0) 184 184 - return ret; 185 185 - 186 186 - ret = rift_s_parse_imu_calibration(json, &hmd->imu_calibration); 187 187 - free(json); 188 188 - 189 189 - if (ret < 0) 190 190 - return ret; 191 191 - 192 192 - ret = rift_s_read_firmware_block(hid_hmd, RIFT_S_FIRMWARE_BLOCK_CAMERA_CALIB, &json, &json_len); 193 193 - if (ret < 0) 194 194 - return ret; 195 195 - 196 196 - ret = rift_s_parse_camera_calibration_block(json, &hmd->camera_calibration); 197 197 - free(json); 198 198 - 199 199 - return ret; 200 200 - } 201 201 - 202 202 - static int 203 203 - rift_s_hmd_read_proximity_threshold(struct rift_s_hmd *hmd, struct os_hid_device *hid_hmd) 204 204 - { 205 205 - char *json = NULL; 206 206 - int json_len = 0; 207 207 - 208 208 - int ret = rift_s_read_firmware_block(hid_hmd, RIFT_S_FIRMWARE_BLOCK_THRESHOLD, &json, &json_len); 209 209 - if (ret < 0) 210 210 - return ret; 211 211 - 212 212 - ret = rift_s_parse_proximity_threshold(json, &hmd->proximity_threshold); 213 213 - free(json); 214 214 - 215 215 - return ret; 216 216 - } 217 217 - 218 175 static void 219 176 rift_s_hmd_destroy(struct xrt_device *xdev) 220 177 { ··· 230 187 231 188 u_var_remove_root(hmd); 232 189 233 233 - m_imu_3dof_close(&hmd->fusion); 234 234 - 235 235 - os_mutex_destroy(&hmd->mutex); 236 236 - 237 190 u_device_free(&hmd->base); 238 191 } 239 192 240 193 struct rift_s_hmd * 241 241 - rift_s_hmd_create(struct rift_s_system *sys, const unsigned char *hmd_serial_no) 194 194 + rift_s_hmd_create(struct rift_s_system *sys, const unsigned char *hmd_serial_no, struct rift_s_hmd_config *config) 242 195 { 243 243 - int ret; 244 244 - 245 196 DRV_TRACE_MARKER(); 246 197 247 198 enum u_device_alloc_flags flags = ··· 255 206 /* Take a reference to the rift_s_system */ 256 207 rift_s_system_reference(&hmd->sys, sys); 257 208 209 209 + hmd->config = config; 210 210 + 258 211 hmd->base.tracking_origin = &sys->base; 259 212 260 213 hmd->base.update_inputs = rift_s_update_inputs; ··· 263 216 hmd->base.destroy = rift_s_hmd_destroy; 264 217 hmd->base.name = XRT_DEVICE_GENERIC_HMD; 265 218 hmd->base.device_type = XRT_DEVICE_TYPE_HMD; 266 266 - hmd->pose.orientation.w = 1.0f; // All other values set to zero by U_DEVICE_ALLOCATE (which calls U_CALLOC) 267 219 268 268 - m_imu_3dof_init(&hmd->fusion, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 269 269 - 270 270 - // Pose / state lock 271 271 - ret = os_mutex_init(&hmd->mutex); 272 272 - if (ret != 0) { 273 273 - RIFT_S_ERROR("Failed to init mutex!"); 274 274 - goto cleanup; 275 275 - } 220 220 + hmd->tracker = rift_s_system_get_tracker(sys); 276 221 277 222 // Print name. 278 223 snprintf(hmd->base.str, XRT_DEVICE_NAME_LEN, "Oculus Rift S"); ··· 285 230 286 231 struct os_hid_device *hid_hmd = rift_s_system_hid_handle(hmd->sys); 287 232 288 288 - if (rift_s_read_panel_info(hid_hmd, &hmd->panel_info) < 0) { 289 289 - RIFT_S_ERROR("Failed to read Rift S device info"); 290 290 - goto cleanup; 291 291 - } 292 292 - 293 293 - if (rift_s_read_firmware_version(hid_hmd) < 0) { 294 294 - RIFT_S_ERROR("Failed to read Rift S firmware version"); 295 295 - goto cleanup; 296 296 - } 233 233 + RIFT_S_DEBUG("Configuring firmware provided proximity sensor threshold %u", config->proximity_threshold); 297 234 298 298 - if (rift_s_read_imu_config(hid_hmd, &hmd->imu_config) < 0) { 299 299 - RIFT_S_ERROR("Failed to read IMU configuration block"); 300 300 - goto cleanup; 301 301 - } 302 302 - 303 303 - if (read_hmd_calibration(hmd, hid_hmd) < 0) 304 304 - goto cleanup; 305 305 - 306 306 - /* Configure the proximity sensor threshold */ 307 307 - if (rift_s_hmd_read_proximity_threshold(hmd, hid_hmd) < 0) 308 308 - goto cleanup; 309 309 - 310 310 - RIFT_S_DEBUG("Configuring firmware provided proximity sensor threshold %u", hmd->proximity_threshold); 311 311 - 312 312 - if (rift_s_protocol_set_proximity_threshold(hid_hmd, (uint16_t)hmd->proximity_threshold) < 0) 235 235 + if (rift_s_protocol_set_proximity_threshold(hid_hmd, (uint16_t)config->proximity_threshold) < 0) 313 236 goto cleanup; 314 237 315 238 #if 0 ··· 402 325 // Setup variable tracker: Optional but useful for debugging 403 326 u_var_add_root(hmd, "Oculus Rift S", true); 404 327 405 405 - u_var_add_gui_header(hmd, NULL, "Tracking"); 406 406 - u_var_add_pose(hmd, &hmd->pose, "pose"); 407 407 - 408 408 - u_var_add_gui_header(hmd, NULL, "3DoF Tracking"); 409 409 - m_imu_3dof_add_vars(&hmd->fusion, hmd, ""); 328 328 + /* Add tracker variables to the HMD debug */ 329 329 + rift_s_tracker_add_debug_ui(hmd->tracker, hmd); 410 330 411 331 u_var_add_gui_header(hmd, NULL, "Misc"); 412 332 u_var_add_log_level(hmd, &rift_s_log_level, "log_level");

+7 -16

src/xrt/drivers/rift_s/rift_s_hmd.h

··· 21 21 #include "xrt/xrt_device.h" 22 22 23 23 #include "rift_s.h" 24 24 - #include "rift_s_protocol.h" 25 25 - #include "rift_s_firmware.h" 26 24 27 25 /* Oculus Rift S HMD Internal Interface */ 28 26 #ifndef RIFT_S_HMD_H ··· 33 31 struct xrt_device base; 34 32 35 33 struct rift_s_system *sys; 34 34 + /* HMD config info (belongs to the system, which we have a ref to */ 35 35 + struct rift_s_hmd_config *config; 36 36 37 37 - /* 3DOF fusion */ 38 38 - struct os_mutex mutex; 37 37 + /* Pose tracker provided by the system */ 38 38 + struct rift_s_tracker *tracker; 39 39 + 40 40 + /* Tracking to extend 32-bit HMD time to 64-bit nanoseconds */ 39 41 uint32_t last_imu_timestamp32; /* 32-bit µS device timestamp */ 40 42 timepoint_ns last_imu_timestamp_ns; 41 41 - timepoint_ns last_imu_local_timestamp_ns; 42 42 - struct m_imu_3dof fusion; 43 43 - struct xrt_pose pose; 44 44 - struct xrt_vec3 raw_mag, raw_accel, raw_gyro; 45 43 46 44 /* Auxiliary state */ 47 45 float temperature; 48 46 bool display_on; 49 47 50 50 - /* Configuration / calibration info */ 51 51 - rift_s_panel_info_t panel_info; 52 52 - rift_s_imu_config_t imu_config; 53 53 - struct rift_s_imu_calibration imu_calibration; 54 54 - int proximity_threshold; 55 55 - struct rift_s_camera_calibration_block camera_calibration; 56 56 - 57 48 /* Temporary distortion values for mesh calc */ 58 49 struct u_panotools_values distortion_vals[2]; 59 50 }; 60 51 61 52 struct rift_s_hmd * 62 62 - rift_s_hmd_create(struct rift_s_system *sys, const unsigned char *hmd_serial_no); 53 53 + rift_s_hmd_create(struct rift_s_system *sys, const unsigned char *hmd_serial_no, struct rift_s_hmd_config *config); 63 54 void 64 55 rift_s_hmd_handle_report(struct rift_s_hmd *hmd, timepoint_ns local_ts, rift_s_hmd_report_t *report); 65 56 void

+37 -69

src/xrt/drivers/rift_s/rift_s_protocol.c

··· 317 317 os_hid_set_feature(hid, buf, 6); 318 318 } 319 319 320 320 + void 321 321 + rift_s_protocol_camera_report_init(rift_s_camera_report_t *camera_report) 322 322 + { 323 323 + int i; 324 324 + 325 325 + /* One slot per camera: */ 326 326 + camera_report->id = 0x05; 327 327 + camera_report->uvc_enable = 0x0; 328 328 + camera_report->radio_sync_flag = 0x0; 329 329 + 330 330 + camera_report->marker[0] = 0x26; 331 331 + camera_report->marker[1] = 0x0; 332 332 + camera_report->marker[2] = 0x40; 333 333 + 334 334 + for (i = 0; i < 5; i++) { 335 335 + camera_report->slam_frame_exposures[i] = 0x36b3; 336 336 + camera_report->slam_frame_gains[i] = 0xf0; 337 337 + camera_report->unknown32[i] = 0x04bc; 338 338 + } 339 339 + } 340 340 + 320 341 int 321 321 - rift_s_send_camera_report(struct os_hid_device *hid, bool enable, bool radio_sync_bit) 342 342 + rift_s_protocol_send_camera_report(struct os_hid_device *hid, rift_s_camera_report_t *camera_report) 322 343 { 323 323 - /* 324 324 - * 05 O1 O2 P1 P1 P2 P2 P3 P3 P4 P4 P5 P5 E1 E1 E3 325 325 - * E4 E5 U1 U2 U3 A1 A1 A1 A1 A2 A2 A2 A2 A3 A3 A3 326 326 - * A3 A4 A4 A4 A4 A5 A5 A5 A5 327 327 - * 328 328 - * O1 = Camera stream on (0x00 = off, 0x1 = on) 329 329 - * O2 = Radio Sync maybe? 330 330 - * Px = Vertical offset / position of camera x passthrough view 331 331 - * Ex = Exposure of camera x passthrough view 332 332 - * Ax = ? of camera x. 4 byte LE, Always seems to take values 0x3f0-0x4ff 333 333 - * but I can't see the effect on the image 334 334 - * U1U2U3 = 26 00 40 always? 335 335 - */ 336 336 - unsigned char buf[41] = { 337 337 - #if 0 338 338 - 0x05, 0x01, 0x01, 0xb3, 0x36, 0xb3, 0x36, 0xb3, 0x36, 0xb3, 0x36, 0xb3, 0x36, 0xf0, 0xf0, 0xf0, 339 339 - 0xf0, 0xf0, 0x26, 0x00, 0x40, 0x7a, 0x04, 0x00, 0x00, 0xa7, 0x04, 0x00, 0x00, 0xa7, 0x04, 0x00, 340 340 - 0x00, 0xa5, 0x04, 0x00, 0x00, 0xa8, 0x04, 0x00, 0x00 341 341 - #else 342 342 - 0x05, 343 343 - 0x01, 344 344 - 0x01, 345 345 - 0xb3, 346 346 - 0x36, 347 347 - 0xb3, 348 348 - 0x36, 349 349 - 0xb3, 350 350 - 0x36, 351 351 - 0xb3, 352 352 - 0x36, 353 353 - 0xb3, 354 354 - 0x36, 355 355 - 0xf0, 356 356 - 0xf0, 357 357 - 0xf0, 358 358 - 0xf0, 359 359 - 0xf0, 360 360 - 0x26, 361 361 - 0x00, 362 362 - 0x40, 363 363 - 0x7a, 364 364 - 0x04, 365 365 - 0x00, 366 366 - 0x00, 367 367 - 0xa7, 368 368 - 0x04, 369 369 - 0x00, 370 370 - 0x00, 371 371 - 0xa7, 372 372 - 0x04, 373 373 - 0x00, 374 374 - 0x00, 375 375 - 0xa5, 376 376 - 0x04, 377 377 - 0x00, 378 378 - 0x00, 379 379 - 0xa8, 380 380 - 0x04, 381 381 - 0x00, 382 382 - 0x00 383 383 - #endif 384 384 - }; 344 344 + return os_hid_set_feature(hid, (uint8_t *)camera_report, sizeof(*camera_report)); 345 345 + } 346 346 + 347 347 + static int 348 348 + rift_s_enable_camera(struct os_hid_device *hid, bool enable, bool radio_sync_bit) 349 349 + { 350 350 + rift_s_camera_report_t camera_report; 351 351 + 352 352 + rift_s_protocol_camera_report_init(&camera_report); 385 353 386 386 - buf[1] = enable ? 0x1 : 0x0; 387 387 - buf[2] = radio_sync_bit ? 0x1 : 0x0; 354 354 + camera_report.uvc_enable = enable ? 0x1 : 0x0; 355 355 + camera_report.radio_sync_flag = radio_sync_bit ? 0x1 : 0x0; 388 356 389 389 - return os_hid_set_feature(hid, buf, 41); 357 357 + return rift_s_protocol_send_camera_report(hid, &camera_report); 390 358 } 391 359 392 360 int ··· 433 401 } 434 402 435 403 int 436 436 - rift_s_read_imu_config(struct os_hid_device *hid, rift_s_imu_config_t *imu_config) 404 404 + rift_s_read_imu_config_info(struct os_hid_device *hid, struct rift_s_imu_config_info_t *imu_config) 437 405 { 438 406 uint8_t buf[FEATURE_BUFFER_SIZE]; 439 407 int res; ··· 442 410 if (res < 21) 443 411 return -1; 444 412 445 445 - *imu_config = *(rift_s_imu_config_t *)buf; 413 413 + *imu_config = *(struct rift_s_imu_config_info_t *)buf; 446 414 447 415 return 0; 448 416 } ··· 494 462 /* Send camera report with enable=true enables the streaming. The 495 463 * 2nd byte seems something to do with sync, but doesn't always work, 496 464 * not sure why yet. */ 497 497 - return rift_s_send_camera_report(hid, enable, false); 465 465 + return rift_s_enable_camera(hid, enable, false); 498 466 } 499 467 500 468 /* Read the list of devices on the radio link */

+38 -3

src/xrt/drivers/rift_s/rift_s_protocol.h

··· 179 179 int16_t unknown_zero2; 180 180 } rift_s_hmd_report_t; 181 181 182 182 + /* Read/Write using report 5 */ 183 183 + /* 184 184 + * 05 O1 O2 P1 P1 P2 P2 P3 P3 P4 P4 P5 P5 E1 E1 E3 185 185 + * E4 E5 U1 U2 U3 A1 A1 A1 A1 A2 A2 A2 A2 A3 A3 A3 186 186 + * A3 A4 A4 A4 A4 A5 A5 A5 A5 187 187 + * 188 188 + * O1 = Camera stream on (0x00 = off, 0x1 = on) 189 189 + * O2 = Radio Sync? (Usage not clear, but seems to sometimes affect sync) 190 190 + * Px = Exposure *and* Vertical offset / position of camera x passthrough view 191 191 + * Seems to take values from 0x1db7-0x36b3. Values above 0x36c6 are ignored. 192 192 + * Ex = Gain of camera x passthrough view 193 193 + * U1U2U3 = 26 00 40 always? 194 194 + * Ax = ? of camera x. 4 byte LE, Always seems to take values 0x3b0-0x4ff 195 195 + * but I can't see the effect on the images, either controller or passthrough 196 196 + */ 197 197 + typedef struct 198 198 + { 199 199 + uint8_t id; 200 200 + uint8_t uvc_enable; 201 201 + uint8_t radio_sync_flag; 202 202 + /* One slot per camera: */ 203 203 + uint16_t slam_frame_exposures[5]; 204 204 + uint8_t slam_frame_gains[5]; 205 205 + 206 206 + uint8_t marker[3]; // 0x26 0x00 0x40 207 207 + 208 208 + uint32_t unknown32[5]; 209 209 + } rift_s_camera_report_t; 210 210 + 182 211 /* Read using report 6 */ 183 212 typedef struct 184 213 { ··· 191 220 } rift_s_panel_info_t; 192 221 193 222 /* Read using report 9 */ 194 194 - typedef struct 223 223 + struct rift_s_imu_config_info_t 195 224 { 196 225 uint8_t cmd; 197 226 uint32_t imu_hz; ··· 199 228 float accel_scale; /* Accel = reading * g / accel_scale */ 200 229 float temperature_scale; /* Temperature = reading / scale + offset */ 201 230 float temperature_offset; 202 202 - } rift_s_imu_config_t; 231 231 + }; 203 232 204 233 /* Packet read from endpoint 11 (0x0b) */ 205 234 typedef struct ··· 241 270 int 242 271 rift_s_read_panel_info(struct os_hid_device *hid, rift_s_panel_info_t *panel_info); 243 272 int 244 244 - rift_s_read_imu_config(struct os_hid_device *hid, rift_s_imu_config_t *imu_config); 273 273 + rift_s_read_imu_config_info(struct os_hid_device *hid, struct rift_s_imu_config_info_t *imu_config); 245 274 int 246 275 rift_s_read_fw_proximity_threshold(struct os_hid_device *hid, int *proximity_threshold); 247 276 int ··· 253 282 254 283 void 255 284 rift_s_send_keepalive(struct os_hid_device *hid); 285 285 + 286 286 + void 287 287 + rift_s_protocol_camera_report_init(rift_s_camera_report_t *camera_report); 288 288 + int 289 289 + rift_s_protocol_send_camera_report(struct os_hid_device *hid, rift_s_camera_report_t *camera_report); 290 290 + 256 291 bool 257 292 rift_s_parse_hmd_report(rift_s_hmd_report_t *report, const unsigned char *buf, int size); 258 293 bool

-2

src/xrt/drivers/rift_s/rift_s_radio.c

··· 26 26 #include "rift_s_radio.h" 27 27 #include "rift_s_protocol.h" 28 28 29 29 - #define MIN(a, b) ((a) < (b) ? (a) : (b)) 30 30 - 31 29 /* Struct that forms a double linked queue of pending commands, 32 30 * with the head being the currently active command */ 33 31 struct rift_s_radio_command

+701

src/xrt/drivers/rift_s/rift_s_tracker.c

··· 1 1 + /* 2 2 + * Copyright 2013, Fredrik Hultin. 3 3 + * Copyright 2013, Jakob Bornecrantz. 4 4 + * Copyright 2016 Philipp Zabel 5 5 + * Copyright 2019-2022 Jan Schmidt 6 6 + * SPDX-License-Identifier: BSL-1.0 7 7 + * 8 8 + */ 9 9 + /*! 10 10 + * @file 11 11 + * @brief Driver code for Oculus Rift S headsets 12 12 + * 13 13 + * Implementation for the HMD 3dof and 6dof tracking 14 14 + * 15 15 + * @author Jan Schmidt <jan@centricular.com> 16 16 + * @ingroup drv_rift_s 17 17 + */ 18 18 + #include <stdlib.h> 19 19 + #include <string.h> 20 20 + #include <stdio.h> 21 21 + #include <time.h> 22 22 + #include <assert.h> 23 23 + #include <inttypes.h> 24 24 + 25 25 + #include "math/m_api.h" 26 26 + #include "math/m_space.h" 27 27 + #include "math/m_vec3.h" 28 28 + 29 29 + #include "os/os_time.h" 30 30 + 31 31 + #include "util/u_debug.h" 32 32 + #include "util/u_device.h" 33 33 + #include "util/u_sink.h" 34 34 + #include "util/u_trace_marker.h" 35 35 + #include "util/u_var.h" 36 36 + 37 37 + #include "xrt/xrt_config_drivers.h" 38 38 + #include "xrt/xrt_device.h" 39 39 + 40 40 + #ifdef XRT_BUILD_DRIVER_HANDTRACKING 41 41 + #include "../drivers/ht/ht_interface.h" 42 42 + #include "../multi_wrapper/multi.h" 43 43 + #endif 44 44 + 45 45 + #include "rift_s.h" 46 46 + #include "rift_s_interface.h" 47 47 + #include "rift_s_util.h" 48 48 + #include "rift_s_tracker.h" 49 49 + 50 50 + #ifdef XRT_FEATURE_SLAM 51 51 + static const bool slam_supported = true; 52 52 + #else 53 53 + static const bool slam_supported = false; 54 54 + #endif 55 55 + 56 56 + #ifdef XRT_BUILD_DRIVER_HANDTRACKING 57 57 + static const bool hand_supported = true; 58 58 + #else 59 59 + static const bool hand_supported = false; 60 60 + #endif 61 61 + 62 62 + //! Specifies whether the user wants to use a SLAM tracker. 63 63 + DEBUG_GET_ONCE_BOOL_OPTION(rift_s_slam, "RIFT_S_SLAM", true) 64 64 + 65 65 + //! Specifies whether the user wants to use the hand tracker. 66 66 + DEBUG_GET_ONCE_BOOL_OPTION(rift_s_handtracking, "RIFT_S_HANDTRACKING", true) 67 67 + 68 68 + static void 69 69 + rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev, 70 70 + enum xrt_input_name name, 71 71 + uint64_t at_timestamp_ns, 72 72 + struct xrt_space_relation *out_relation); 73 73 + 74 74 + static void 75 75 + rift_s_tracker_switch_method_cb(void *t_ptr) 76 76 + { 77 77 + DRV_TRACE_MARKER(); 78 78 + 79 79 + struct rift_s_tracker *t = t_ptr; 80 80 + t->slam_over_3dof = !t->slam_over_3dof; 81 81 + struct u_var_button *btn = &t->gui.switch_tracker_btn; 82 82 + 83 83 + if (t->slam_over_3dof) { // Use SLAM 84 84 + snprintf(btn->label, sizeof(btn->label), "Switch to 3DoF Tracking"); 85 85 + } else { // Use 3DoF 86 86 + snprintf(btn->label, sizeof(btn->label), "Switch to SLAM Tracking"); 87 87 + 88 88 + os_mutex_lock(&t->mutex); 89 89 + m_imu_3dof_reset(&t->fusion.i3dof); 90 90 + t->fusion.i3dof.rot = t->pose.orientation; 91 91 + os_mutex_unlock(&t->mutex); 92 92 + } 93 93 + } 94 94 + 95 95 + XRT_MAYBE_UNUSED struct t_imu_calibration 96 96 + rift_s_create_slam_imu_calib() 97 97 + { 98 98 + /* FIXME: Validate these hard coded standard deviations against 99 99 + * some actual at-rest IMU measurements */ 100 100 + const double a_bias_std = 0.001; 101 101 + const double a_noise_std = 0.016; 102 102 + 103 103 + const double g_bias_std = 0.0001; 104 104 + const double g_noise_std = 0.000282; 105 105 + 106 106 + /* we pass already corrected accel and gyro 107 107 + * readings to Basalt, so the transforms and 108 108 + * offsets are just identity / zero matrices */ 109 109 + struct t_imu_calibration calib = { 110 110 + .accel = 111 111 + { 112 112 + .transform = 113 113 + { 114 114 + {1.0, 0.0, 0.0}, 115 115 + {0.0, 1.0, 0.0}, 116 116 + {0.0, 0.0, 1.0}, 117 117 + }, 118 118 + .offset = 119 119 + { 120 120 + 0, 121 121 + }, 122 122 + .bias_std = {a_bias_std, a_bias_std, a_bias_std}, 123 123 + .noise_std = {a_noise_std, a_noise_std, a_noise_std}, 124 124 + }, 125 125 + .gyro = 126 126 + { 127 127 + .transform = 128 128 + { 129 129 + {1.0, 0.0, 0.0}, 130 130 + {0.0, 1.0, 0.0}, 131 131 + {0.0, 0.0, 1.0}, 132 132 + }, 133 133 + .offset = 134 134 + { 135 135 + 0, 136 136 + }, 137 137 + .bias_std = {g_bias_std, g_bias_std, g_bias_std}, 138 138 + .noise_std = {g_noise_std, g_noise_std, g_noise_std}, 139 139 + }, 140 140 + }; 141 141 + 142 142 + return calib; 143 143 + } 144 144 + 145 145 + //! IMU extrinsics, frequency 146 146 + static struct t_slam_calib_extras 147 147 + rift_s_create_extra_slam_calib(struct rift_s_hmd_config *hmd_config) 148 148 + { 149 149 + /* SLAM frames are every 2nd frame of 60Hz camera feed */ 150 150 + const int CAMERA_FREQUENCY = 30; 151 151 + 152 152 + struct rift_s_camera_calibration_block *camera_calibration = &hmd_config->camera_calibration; 153 153 + struct rift_s_camera_calibration *left = &camera_calibration->cameras[RIFT_S_CAMERA_FRONT_LEFT]; 154 154 + struct rift_s_camera_calibration *right = &camera_calibration->cameras[RIFT_S_CAMERA_FRONT_RIGHT]; 155 155 + 156 156 + /* Compute the IMU from cam transform for each cam */ 157 157 + struct xrt_pose device_from_imu, imu_from_device; 158 158 + 159 159 + math_pose_from_isometry(&hmd_config->imu_calibration.device_from_imu, &device_from_imu); 160 160 + math_pose_invert(&device_from_imu, &imu_from_device); 161 161 + 162 162 + struct xrt_pose device_from_left, device_from_right; 163 163 + math_pose_from_isometry(&left->device_from_camera, &device_from_left); 164 164 + math_pose_from_isometry(&right->device_from_camera, &device_from_right); 165 165 + 166 166 + struct xrt_pose P_imu_left_cam, P_imu_right_cam; 167 167 + math_pose_transform(&imu_from_device, &device_from_left, &P_imu_left_cam); 168 168 + math_pose_transform(&imu_from_device, &device_from_right, &P_imu_right_cam); 169 169 + 170 170 + struct xrt_matrix_4x4 T_imu_left_cam, T_imu_right_cam; 171 171 + math_matrix_4x4_isometry_from_pose(&P_imu_left_cam, &T_imu_left_cam); 172 172 + math_matrix_4x4_isometry_from_pose(&P_imu_right_cam, &T_imu_right_cam); 173 173 + 174 174 + RIFT_S_DEBUG("IMU left cam pose %f %f %f orient %f %f %f %f", P_imu_left_cam.position.x, 175 175 + P_imu_left_cam.position.y, P_imu_left_cam.position.z, P_imu_left_cam.orientation.x, 176 176 + P_imu_left_cam.orientation.y, P_imu_left_cam.orientation.z, P_imu_left_cam.orientation.w); 177 177 + 178 178 + RIFT_S_DEBUG("IMU right cam pose %f %f %f orient %f %f %f %f", P_imu_right_cam.position.x, 179 179 + P_imu_right_cam.position.y, P_imu_right_cam.position.z, P_imu_right_cam.orientation.x, 180 180 + P_imu_right_cam.orientation.y, P_imu_right_cam.orientation.z, P_imu_right_cam.orientation.w); 181 181 + 182 182 + double imu_frequency = hmd_config->imu_config_info.imu_hz; 183 183 + 184 184 + struct t_slam_calib_extras calib = { 185 185 + .imu_frequency = imu_frequency, 186 186 + .cams = 187 187 + { 188 188 + { 189 189 + .frequency = CAMERA_FREQUENCY, 190 190 + .T_imu_cam = T_imu_left_cam, 191 191 + .rpmax = 0.0, 192 192 + }, 193 193 + { 194 194 + .frequency = CAMERA_FREQUENCY, 195 195 + .T_imu_cam = T_imu_right_cam, 196 196 + .rpmax = 0.0, 197 197 + }, 198 198 + }, 199 199 + }; 200 200 + return calib; 201 201 + } 202 202 + 203 203 + static struct xrt_slam_sinks * 204 204 + rift_s_create_slam_tracker(struct rift_s_tracker *t, struct xrt_frame_context *xfctx) 205 205 + { 206 206 + DRV_TRACE_MARKER(); 207 207 + 208 208 + struct xrt_slam_sinks *sinks = NULL; 209 209 + 210 210 + #ifdef XRT_FEATURE_SLAM 211 211 + struct t_slam_tracker_config config = {0}; 212 212 + t_slam_fill_default_config(&config); 213 213 + 214 214 + /* No need to refcount these parameters */ 215 215 + config.stereo_calib = t->stereo_calib; 216 216 + config.imu_calib = &t->slam_imu_calib; 217 217 + config.extra_calib = &t->slam_extra_calib; 218 218 + 219 219 + int create_status = t_slam_create(xfctx, &config, &t->tracking.slam, &sinks); 220 220 + if (create_status != 0) { 221 221 + return NULL; 222 222 + } 223 223 + 224 224 + int start_status = t_slam_start(t->tracking.slam); 225 225 + if (start_status != 0) { 226 226 + return NULL; 227 227 + } 228 228 + 229 229 + RIFT_S_DEBUG("Rift S SLAM tracker successfully started"); 230 230 + #endif 231 231 + 232 232 + return sinks; 233 233 + } 234 234 + 235 235 + static int 236 236 + rift_s_create_hand_tracker(struct rift_s_tracker *t, 237 237 + struct xrt_frame_context *xfctx, 238 238 + struct xrt_slam_sinks **out_sinks, 239 239 + struct xrt_device **out_device) 240 240 + { 241 241 + DRV_TRACE_MARKER(); 242 242 + 243 243 + struct xrt_slam_sinks *sinks = NULL; 244 244 + struct xrt_device *device = NULL; 245 245 + 246 246 + #ifdef XRT_BUILD_DRIVER_HANDTRACKING 247 247 + 248 248 + //!@todo What's a sensible boundary for Rift S? 249 249 + struct t_image_boundary_info boundary_info; 250 250 + boundary_info.views[0].type = HT_IMAGE_BOUNDARY_NONE; 251 251 + boundary_info.views[1].type = HT_IMAGE_BOUNDARY_NONE; 252 252 + 253 253 + int create_status = ht_device_create(xfctx, // 254 254 + t->stereo_calib, // 255 255 + HT_ALGORITHM_MERCURY, // 256 256 + boundary_info, 257 257 + &sinks, // 258 258 + &device); 259 259 + if (create_status != 0) { 260 260 + return create_status; 261 261 + } 262 262 + 263 263 + if (device != NULL) { 264 264 + // Attach tracking override that links hand pose to the SLAM tracked position 265 265 + // The hand poses need to be rotated 90° because of the way we passed 266 266 + // the stereo camera configuration to the hand tracker. 267 267 + struct xrt_pose left_cam_rotated_from_imu; 268 268 + struct xrt_pose cam_rotate = {.orientation = {.x = 1.0, .y = 0.0, .z = 0.0, .w = 0.0}, 269 269 + .position = {0, 0, 0}}; 270 270 + math_pose_transform(&cam_rotate, &t->left_cam_from_imu, &left_cam_rotated_from_imu); 271 271 + 272 272 + device = multi_create_tracking_override(XRT_TRACKING_OVERRIDE_ATTACHED, device, &t->base, 273 273 + XRT_INPUT_GENERIC_TRACKER_POSE, &left_cam_rotated_from_imu); 274 274 + } 275 275 + 276 276 + RIFT_S_DEBUG("Rift S HMD hand tracker successfully created"); 277 277 + #endif 278 278 + 279 279 + *out_sinks = sinks; 280 280 + *out_device = device; 281 281 + 282 282 + return 0; 283 283 + } 284 284 + 285 285 + void 286 286 + rift_s_tracker_add_debug_ui(struct rift_s_tracker *t, void *root) 287 287 + { 288 288 + u_var_add_gui_header(root, NULL, "Tracking"); 289 289 + 290 290 + if (t->tracking.slam_enabled) { 291 291 + t->gui.switch_tracker_btn.cb = rift_s_tracker_switch_method_cb; 292 292 + t->gui.switch_tracker_btn.ptr = t; 293 293 + u_var_add_button(root, &t->gui.switch_tracker_btn, "Switch to 3DoF Tracking"); 294 294 + } 295 295 + 296 296 + u_var_add_pose(root, &t->pose, "Tracked Pose"); 297 297 + 298 298 + u_var_add_gui_header(root, NULL, "3DoF Tracking"); 299 299 + m_imu_3dof_add_vars(&t->fusion.i3dof, root, ""); 300 300 + 301 301 + u_var_add_gui_header(root, NULL, "SLAM Tracking"); 302 302 + u_var_add_ro_text(root, t->gui.slam_status, "Tracker status"); 303 303 + 304 304 + u_var_add_gui_header(root, NULL, "Hand Tracking"); 305 305 + u_var_add_ro_text(root, t->gui.hand_status, "Tracker status"); 306 306 + } 307 307 + 308 308 + /*! 309 309 + * Procedure to setup trackers: 3dof, SLAM and hand tracking. 310 310 + * 311 311 + * Determines which trackers to initialize 312 312 + * 313 313 + * @param xfctx the frame server that will own processing nodes 314 314 + * @param hmd_config HMD configuration and firmware info 315 315 + * 316 316 + * @return initialised tracker on success, NULL if creation fails 317 317 + */ 318 318 + struct rift_s_tracker * 319 319 + rift_s_tracker_create(struct xrt_tracking_origin *origin, 320 320 + struct xrt_frame_context *xfctx, 321 321 + struct rift_s_hmd_config *hmd_config) 322 322 + { 323 323 + struct rift_s_tracker *t = U_DEVICE_ALLOCATE(struct rift_s_tracker, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0); 324 324 + if (t == NULL) { 325 325 + return NULL; 326 326 + } 327 327 + 328 328 + t->base.tracking_origin = origin; 329 329 + t->base.get_tracked_pose = rift_s_tracker_get_tracked_pose_imu; 330 330 + 331 331 + // Pose / state lock 332 332 + int ret = os_mutex_init(&t->mutex); 333 333 + if (ret != 0) { 334 334 + RIFT_S_ERROR("Failed to init mutex!"); 335 335 + rift_s_tracker_destroy(t); 336 336 + return NULL; 337 337 + } 338 338 + 339 339 + // Compute IMU and camera device poses for get_tracked_pose relations 340 340 + math_pose_from_isometry(&hmd_config->imu_calibration.device_from_imu, &t->device_from_imu); 341 341 + 342 342 + struct xrt_pose device_from_left_cam; 343 343 + struct rift_s_camera_calibration *left_cam = &hmd_config->camera_calibration.cameras[RIFT_S_CAMERA_FRONT_LEFT]; 344 344 + math_pose_from_isometry(&left_cam->device_from_camera, &device_from_left_cam); 345 345 + 346 346 + struct xrt_pose left_cam_from_device; 347 347 + math_pose_invert(&device_from_left_cam, &left_cam_from_device); 348 348 + math_pose_transform(&left_cam_from_device, &t->device_from_imu, &t->left_cam_from_imu); 349 349 + 350 350 + // Decide whether to initialize the SLAM tracker 351 351 + bool slam_wanted = debug_get_bool_option_rift_s_slam(); 352 352 + bool slam_enabled = slam_supported && slam_wanted; 353 353 + 354 354 + // Decide whether to initialize the hand tracker 355 355 + bool hand_wanted = debug_get_bool_option_rift_s_handtracking(); 356 356 + bool hand_enabled = hand_supported && hand_wanted; 357 357 + 358 358 + t->tracking.slam_enabled = slam_enabled; 359 359 + t->tracking.hand_enabled = hand_enabled; 360 360 + 361 361 + t->slam_over_3dof = slam_enabled; // We prefer SLAM over 3dof tracking if possible 362 362 + 363 363 + const char *slam_status = t->tracking.slam_enabled ? "Enabled" 364 364 + : !slam_wanted ? "Disabled by the user (envvar set to false)" 365 365 + : !slam_supported ? "Unavailable (not built)" 366 366 + : NULL; 367 367 + 368 368 + const char *hand_status = t->tracking.hand_enabled ? "Enabled" 369 369 + : !hand_wanted ? "Disabled by the user (envvar set to false)" 370 370 + : !hand_supported ? "Unavailable (not built)" 371 371 + : NULL; 372 372 + 373 373 + assert(slam_status != NULL && hand_status != NULL); 374 374 + 375 375 + snprintf(t->gui.slam_status, sizeof(t->gui.slam_status), "%s", slam_status); 376 376 + snprintf(t->gui.hand_status, sizeof(t->gui.hand_status), "%s", hand_status); 377 377 + 378 378 + // Initialize 3DoF tracker 379 379 + m_imu_3dof_init(&t->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 380 380 + 381 381 + t->pose.orientation.w = 1.0f; // All other values set to zero by U_DEVICE_ALLOCATE (which calls U_CALLOC) 382 382 + 383 383 + // Construct the stereo camera calibration for the front cameras 384 384 + t->stereo_calib = rift_s_create_stereo_camera_calib_rotated(&hmd_config->camera_calibration); 385 385 + t->slam_imu_calib = rift_s_create_slam_imu_calib(); 386 386 + t->slam_extra_calib = rift_s_create_extra_slam_calib(hmd_config); 387 387 + 388 388 + // Initialize the input sinks for the camera to send to 389 389 + 390 390 + // Initialize SLAM tracker 391 391 + struct xrt_slam_sinks *slam_sinks = NULL; 392 392 + if (t->tracking.slam_enabled) { 393 393 + slam_sinks = rift_s_create_slam_tracker(t, xfctx); 394 394 + if (slam_sinks == NULL) { 395 395 + RIFT_S_WARN("Unable to setup the SLAM tracker"); 396 396 + rift_s_tracker_destroy(t); 397 397 + return NULL; 398 398 + } 399 399 + } 400 400 + 401 401 + // Initialize hand tracker 402 402 + struct xrt_slam_sinks *hand_sinks = NULL; 403 403 + struct xrt_device *hand_device = NULL; 404 404 + if (t->tracking.hand_enabled) { 405 405 + int hand_status = rift_s_create_hand_tracker(t, xfctx, &hand_sinks, &hand_device); 406 406 + if (hand_status != 0 || hand_sinks == NULL || hand_device == NULL) { 407 407 + RIFT_S_WARN("Unable to setup the hand tracker"); 408 408 + rift_s_tracker_destroy(t); 409 409 + return NULL; 410 410 + } 411 411 + } 412 412 + 413 413 + // Setup sinks depending on tracking configuration 414 414 + struct xrt_slam_sinks entry_sinks = {0}; 415 415 + if (slam_enabled && hand_enabled) { 416 416 + struct xrt_frame_sink *entry_left_sink = NULL; 417 417 + struct xrt_frame_sink *entry_right_sink = NULL; 418 418 + 419 419 + u_sink_split_create(xfctx, slam_sinks->left, hand_sinks->left, &entry_left_sink); 420 420 + u_sink_split_create(xfctx, slam_sinks->right, hand_sinks->right, &entry_right_sink); 421 421 + 422 422 + entry_sinks = (struct xrt_slam_sinks){ 423 423 + .left = entry_left_sink, 424 424 + .right = entry_right_sink, 425 425 + .imu = slam_sinks->imu, 426 426 + .gt = slam_sinks->gt, 427 427 + }; 428 428 + } else if (slam_enabled) { 429 429 + entry_sinks = *slam_sinks; 430 430 + } else if (hand_enabled) { 431 431 + entry_sinks = *hand_sinks; 432 432 + } else { 433 433 + entry_sinks = (struct xrt_slam_sinks){0}; 434 434 + } 435 435 + 436 436 + t->slam_sinks = entry_sinks; 437 437 + t->handtracker = hand_device; 438 438 + 439 439 + return t; 440 440 + } 441 441 + 442 442 + void 443 443 + rift_s_tracker_destroy(struct rift_s_tracker *t) 444 444 + { 445 445 + t_stereo_camera_calibration_reference(&t->stereo_calib, NULL); 446 446 + 447 447 + m_imu_3dof_close(&t->fusion.i3dof); 448 448 + os_mutex_destroy(&t->mutex); 449 449 + } 450 450 + 451 451 + struct xrt_slam_sinks * 452 452 + rift_s_tracker_get_slam_sinks(struct rift_s_tracker *t) 453 453 + { 454 454 + return &t->in_slam_sinks; 455 455 + } 456 456 + 457 457 + struct xrt_device * 458 458 + rift_s_tracker_get_hand_tracking_device(struct rift_s_tracker *t) 459 459 + { 460 460 + return t->handtracker; 461 461 + } 462 462 + 463 463 + /*! 464 464 + * Convert a hardware timestamp into monotonic clock. Updates offset estimate. 465 465 + * @note Only used with IMU samples as they have the smallest USB transmission time. 466 466 + * 467 467 + * @param t struct rift_s_tracker 468 468 + * @param local_timestamp_ns Monotonic timestamp at which the IMU sample was received 469 469 + * @param device_ts HMD Hardware timestamp, gets converted to local monotonic clock. 470 470 + */ 471 471 + static timepoint_ns 472 472 + clock_hw2mono_update(struct rift_s_tracker *t, timepoint_ns local_timestamp_ns, uint64_t device_ts) 473 473 + { 474 474 + const double alpha = 0.9995; // Weight to put on accumulated hw2mono clock offset 475 475 + timepoint_ns hw = device_ts; 476 476 + 477 477 + /* Only do updates if the monotonic time increased 478 478 + * (otherwise we're processing packets that arrived 479 479 + * at the same time - so only take the earliest) */ 480 480 + if (local_timestamp_ns > t->last_hw2mono_local_ts) { 481 481 + time_duration_ns old_hw2mono = t->hw2mono; 482 482 + time_duration_ns got_hw2mono = local_timestamp_ns - hw; 483 483 + time_duration_ns new_hw2mono = old_hw2mono * alpha + got_hw2mono * (1.0 - alpha); 484 484 + if (old_hw2mono == 0) { // hw2mono was not set for the first time yet 485 485 + new_hw2mono = got_hw2mono; 486 486 + } 487 487 + 488 488 + time_duration_ns new_hw2mono_out = hw + new_hw2mono; 489 489 + 490 490 + if (new_hw2mono_out >= t->last_hw2mono_out) { 491 491 + t->last_hw2mono_out = new_hw2mono_out; 492 492 + t->hw2mono = new_hw2mono; 493 493 + t->have_hw2mono = true; 494 494 + t->last_hw2mono_local_ts = local_timestamp_ns; 495 495 + } else { 496 496 + RIFT_S_WARN("Monotonic time map went backward (%" PRIu64 ", %" PRIu64 ") => %" PRIu64 497 497 + " < %" PRIu64 ". Reporting %" PRIu64, 498 498 + hw, local_timestamp_ns, new_hw2mono_out, t->last_hw2mono_out, hw + t->hw2mono); 499 499 + } 500 500 + } else { 501 501 + t->last_hw2mono_out = hw + t->hw2mono; 502 502 + } 503 503 + 504 504 + return t->last_hw2mono_out; 505 505 + } 506 506 + 507 507 + //! Camera specific logic for clock conversion 508 508 + static void 509 509 + clock_hw2mono_get(struct rift_s_tracker *t, uint64_t device_ts, timepoint_ns *out) 510 510 + { 511 511 + *out = t->hw2mono + device_ts; 512 512 + } 513 513 + 514 514 + void 515 515 + rift_s_tracker_imu_update(struct rift_s_tracker *t, 516 516 + uint64_t timestamp_ns, 517 517 + timepoint_ns local_timestamp_ns_orig, 518 518 + const struct xrt_vec3 *accel, 519 519 + const struct xrt_vec3 *gyro) 520 520 + { 521 521 + os_mutex_lock(&t->mutex); 522 522 + 523 523 + /* Ignore packets before we're ready */ 524 524 + if (!t->ready_for_data) { 525 525 + os_mutex_unlock(&t->mutex); 526 526 + return; 527 527 + } 528 528 + 529 529 + /* Get the smoothed monotonic time estimate for this IMU sample */ 530 530 + timepoint_ns local_timestamp_ns = clock_hw2mono_update(t, local_timestamp_ns_orig, timestamp_ns); 531 531 + 532 532 + if (t->fusion.last_imu_local_timestamp_ns != 0 && local_timestamp_ns < t->fusion.last_imu_local_timestamp_ns) { 533 533 + RIFT_S_WARN("IMU time went backward by %" PRId64 " ns", 534 534 + local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns); 535 535 + } else { 536 536 + m_imu_3dof_update(&t->fusion.i3dof, timestamp_ns, accel, gyro); 537 537 + } 538 538 + 539 539 + RIFT_S_TRACE("IMU timestamp %" PRIu64 " (dt %f) local %" PRIu64 " hw2mono %" PRIu64 " (dt %f) offset %" PRId64, 540 540 + timestamp_ns, (double)(timestamp_ns - t->fusion.last_imu_timestamp_ns) / 1000000000.0, 541 541 + local_timestamp_ns_orig, local_timestamp_ns, 542 542 + (double)(local_timestamp_ns - t->fusion.last_imu_local_timestamp_ns) / 1000000000.0, t->hw2mono); 543 543 + 544 544 + t->fusion.last_angular_velocity = *gyro; 545 545 + t->fusion.last_imu_timestamp_ns = timestamp_ns; 546 546 + t->fusion.last_imu_local_timestamp_ns = local_timestamp_ns; 547 547 + 548 548 + t->pose.orientation = t->fusion.i3dof.rot; 549 549 + 550 550 + os_mutex_unlock(&t->mutex); 551 551 + 552 552 + if (t->slam_sinks.imu) { 553 553 + /* Push IMU sample to the SLAM tracker */ 554 554 + struct xrt_vec3_f64 accel64 = {accel->x, accel->y, accel->z}; 555 555 + struct xrt_vec3_f64 gyro64 = {gyro->x, gyro->y, gyro->z}; 556 556 + struct xrt_imu_sample sample = { 557 557 + .timestamp_ns = local_timestamp_ns, .accel_m_s2 = accel64, .gyro_rad_secs = gyro64}; 558 558 + 559 559 + xrt_sink_push_imu(t->slam_sinks.imu, &sample); 560 560 + } 561 561 + } 562 562 + 563 563 + #define UPPER_32BITS(x) ((x)&0xffffffff00000000ULL) 564 564 + 565 565 + void 566 566 + rift_s_tracker_push_slam_frames(struct rift_s_tracker *t, 567 567 + uint64_t frame_ts_ns, 568 568 + struct xrt_frame *left_frame, 569 569 + struct xrt_frame *right_frame) 570 570 + { 571 571 + timepoint_ns frame_time; 572 572 + 573 573 + os_mutex_lock(&t->mutex); 574 574 + 575 575 + /* Ignore packets before we're ready */ 576 576 + if (!t->ready_for_data) { 577 577 + os_mutex_unlock(&t->mutex); 578 578 + return; 579 579 + } 580 580 + 581 581 + if (!t->have_hw2mono) { 582 582 + /* Drop any frames before we have IMU */ 583 583 + os_mutex_unlock(&t->mutex); 584 584 + return; 585 585 + } 586 586 + 587 587 + /* Ensure the input timestamp is within 32-bits of the IMU 588 588 + * time, because the timestamps are reported and extended to 64-bits 589 589 + * separately and can end up in different epochs */ 590 590 + uint64_t adj_frame_ts_ns = frame_ts_ns + t->camera_ts_offset; 591 591 + int64_t frame_to_imu_uS = (adj_frame_ts_ns / 1000 - t->fusion.last_imu_timestamp_ns / 1000); 592 592 + 593 593 + if (frame_to_imu_uS < -(int64_t)(1ULL << 31) || frame_to_imu_uS > (int64_t)(1ULL << 31)) { 594 594 + t->camera_ts_offset = 595 595 + (UPPER_32BITS(t->fusion.last_imu_timestamp_ns / 1000) - UPPER_32BITS(frame_ts_ns / 1000)) * 1000; 596 596 + RIFT_S_DEBUG("Applying epoch offset to frame times of %" PRId64 " (frame->imu was %" PRId64 " µS)", 597 597 + t->camera_ts_offset, frame_to_imu_uS); 598 598 + } 599 599 + frame_ts_ns += t->camera_ts_offset; 600 600 + 601 601 + clock_hw2mono_get(t, frame_ts_ns, &frame_time); 602 602 + 603 603 + if (frame_time < t->last_frame_time) { 604 604 + RIFT_S_WARN("Camera frame time went backward by %" PRId64 " ns", frame_time - t->last_frame_time); 605 605 + os_mutex_unlock(&t->mutex); 606 606 + return; 607 607 + } 608 608 + 609 609 + RIFT_S_TRACE("SLAM frame timestamp %" PRIu64 " local %" PRIu64, frame_ts_ns, frame_time); 610 610 + 611 611 + t->last_frame_time = frame_time; 612 612 + os_mutex_unlock(&t->mutex); 613 613 + 614 614 + if (t->slam_sinks.left) { 615 615 + left_frame->timestamp = frame_time; 616 616 + xrt_sink_push_frame(t->slam_sinks.left, left_frame); 617 617 + } 618 618 + 619 619 + if (t->slam_sinks.right) { 620 620 + right_frame->timestamp = frame_time; 621 621 + xrt_sink_push_frame(t->slam_sinks.right, right_frame); 622 622 + } 623 623 + } 624 624 + 625 625 + //! Specific pose correction for Basalt to OpenXR coordinates 626 626 + XRT_MAYBE_UNUSED static inline void 627 627 + rift_s_tracker_correct_pose_from_basalt(struct xrt_pose *pose) 628 628 + { 629 629 + struct xrt_quat q = {0.70710678, 0, 0, -0.70710678}; 630 630 + math_quat_rotate(&q, &pose->orientation, &pose->orientation); 631 631 + math_quat_rotate_vec3(&q, &pose->position, &pose->position); 632 632 + } 633 633 + 634 634 + static void 635 635 + rift_s_tracker_get_tracked_pose_imu(struct xrt_device *xdev, 636 636 + enum xrt_input_name name, 637 637 + uint64_t at_timestamp_ns, 638 638 + struct xrt_space_relation *out_relation) 639 639 + { 640 640 + struct rift_s_tracker *tracker = (struct rift_s_tracker *)(xdev); 641 641 + assert(name == XRT_INPUT_GENERIC_TRACKER_POSE); 642 642 + 643 643 + rift_s_tracker_get_tracked_pose(tracker, RIFT_S_TRACKER_POSE_IMU, at_timestamp_ns, out_relation); 644 644 + } 645 645 + 646 646 + void 647 647 + rift_s_tracker_get_tracked_pose(struct rift_s_tracker *t, 648 648 + enum rift_s_tracker_pose pose, 649 649 + uint64_t at_timestamp_ns, 650 650 + struct xrt_space_relation *out_relation) 651 651 + { 652 652 + struct xrt_relation_chain xrc = {0}; 653 653 + 654 654 + if (pose == RIFT_S_TRACKER_POSE_DEVICE) { 655 655 + m_relation_chain_push_pose(&xrc, &t->device_from_imu); 656 656 + } else if (pose == RIFT_S_TRACKER_POSE_LEFT_CAMERA) { 657 657 + m_relation_chain_push_pose(&xrc, &t->left_cam_from_imu); 658 658 + } 659 659 + 660 660 + if (t->tracking.slam_enabled && t->slam_over_3dof) { 661 661 + struct xrt_space_relation imu_relation = XRT_SPACE_RELATION_ZERO; 662 662 + 663 663 + // Get the IMU pose from the SLAM tracker 664 664 + xrt_tracked_slam_get_tracked_pose(t->tracking.slam, at_timestamp_ns, &imu_relation); 665 665 + 666 666 + #if defined(XRT_HAVE_BASALT_SLAM) 667 667 + rift_s_tracker_correct_pose_from_basalt(&imu_relation.pose); 668 668 + #endif 669 669 + 670 670 + imu_relation.relation_flags = (enum xrt_space_relation_flags)( 671 671 + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 672 672 + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 673 673 + 674 674 + m_relation_chain_push_relation(&xrc, &imu_relation); 675 675 + } else { 676 676 + struct xrt_space_relation imu_relation = XRT_SPACE_RELATION_ZERO; 677 677 + 678 678 + os_mutex_lock(&t->mutex); 679 679 + // TODO: Estimate pose at timestamp at_timestamp_ns 680 680 + math_quat_normalize(&t->pose.orientation); 681 681 + imu_relation.pose = t->pose; 682 682 + imu_relation.angular_velocity = t->fusion.last_angular_velocity; 683 683 + imu_relation.relation_flags = (enum xrt_space_relation_flags)( 684 684 + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | 685 685 + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 686 686 + 687 687 + m_relation_chain_push_relation(&xrc, &imu_relation); 688 688 + 689 689 + os_mutex_unlock(&t->mutex); 690 690 + } 691 691 + 692 692 + m_relation_chain_resolve(&xrc, out_relation); 693 693 + } 694 694 + 695 695 + void 696 696 + rift_s_tracker_start(struct rift_s_tracker *t) 697 697 + { 698 698 + os_mutex_lock(&t->mutex); 699 699 + t->ready_for_data = true; 700 700 + os_mutex_unlock(&t->mutex); 701 701 + }

+159

src/xrt/drivers/rift_s/rift_s_tracker.h

··· 1 1 + /* 2 2 + * Copyright 2013, Fredrik Hultin. 3 3 + * Copyright 2013, Jakob Bornecrantz. 4 4 + * Copyright 2016 Philipp Zabel 5 5 + * Copyright 2019-2022 Jan Schmidt 6 6 + * SPDX-License-Identifier: BSL-1.0 7 7 + */ 8 8 + 9 9 + /*! 10 10 + * @file 11 11 + * @brief HMD tracker handling 12 12 + * @author Jan Schmidt <jan@centricular.com> 13 13 + * @ingroup drv_rift_s 14 14 + */ 15 15 + 16 16 + #pragma once 17 17 + 18 18 + #include "math/m_imu_3dof.h" 19 19 + #include "os/os_threading.h" 20 20 + #include "util/u_var.h" 21 21 + #include "xrt/xrt_defines.h" 22 22 + #include "xrt/xrt_device.h" 23 23 + 24 24 + #include "tracking/t_tracking.h" 25 25 + 26 26 + #include "rift_s_firmware.h" 27 27 + 28 28 + /* Oculus Rift S HMD Tracking */ 29 29 + #ifndef RIFT_S_TRACKER_H 30 30 + #define RIFT_S_TRACKER_H 31 31 + 32 32 + struct rift_s_hmd_config; 33 33 + 34 34 + enum rift_s_tracker_pose 35 35 + { 36 36 + RIFT_S_TRACKER_POSE_IMU, 37 37 + RIFT_S_TRACKER_POSE_LEFT_CAMERA, 38 38 + RIFT_S_TRACKER_POSE_DEVICE, 39 39 + }; 40 40 + 41 41 + struct rift_s_tracker 42 42 + { 43 43 + struct xrt_device base; 44 44 + 45 45 + //! Protects shared access to 3dof and pose storage 46 46 + struct os_mutex mutex; 47 47 + 48 48 + //! Don't process IMU / video until started 49 49 + bool ready_for_data; 50 50 + 51 51 + struct 52 52 + { 53 53 + //! Main fusion calculator. 54 54 + struct m_imu_3dof i3dof; 55 55 + 56 56 + //! The last angular velocity from the IMU, for prediction. 57 57 + struct xrt_vec3 last_angular_velocity; 58 58 + 59 59 + //! When did we get the last IMU sample, device clock 60 60 + uint64_t last_imu_timestamp_ns; 61 61 + 62 62 + //! Last IMU sample local system clock 63 63 + timepoint_ns last_imu_local_timestamp_ns; 64 64 + } fusion; 65 65 + 66 66 + //! Fields related to camera-based tracking (SLAM and hand tracking) 67 67 + struct 68 68 + { 69 69 + //! SLAM tracker. 70 70 + //! @todo Right now, we are not consistent in how we interface with 71 71 + //! trackers. In particular, we have a @ref xrt_tracked_slam field but not 72 72 + //! an equivalent for hand tracking. 73 73 + struct xrt_tracked_slam *slam; 74 74 + 75 75 + //! Set at start. Whether the SLAM tracker was initialized. 76 76 + bool slam_enabled; 77 77 + 78 78 + //! Set at start. Whether the hand tracker was initialized. 79 79 + bool hand_enabled; 80 80 + } tracking; 81 81 + 82 82 + // Correction offset poses from firmware 83 83 + struct xrt_pose device_from_imu; 84 84 + struct xrt_pose left_cam_from_imu; 85 85 + 86 86 + //!< Estimated offset from HMD device timestamp to local monotonic clock 87 87 + bool have_hw2mono; 88 88 + time_duration_ns hw2mono; 89 89 + time_duration_ns last_hw2mono_out; 90 90 + timepoint_ns last_hw2mono_local_ts; 91 91 + timepoint_ns last_frame_time; 92 92 + 93 93 + //! Adjustment to apply to camera timestamps to bring them into the 94 94 + // same 32-bit range as the IMU times 95 95 + int64_t camera_ts_offset; 96 96 + 97 97 + //! Whether to track the HMD with 6dof SLAM or fallback to the `fusion` 3dof tracker 98 98 + bool slam_over_3dof; 99 99 + 100 100 + //! Last tracked pose 101 101 + struct xrt_pose pose; 102 102 + 103 103 + /* Stereo calibration for the front 2 cameras */ 104 104 + struct t_stereo_camera_calibration *stereo_calib; 105 105 + struct t_imu_calibration slam_imu_calib; 106 106 + struct t_slam_calib_extras slam_extra_calib; 107 107 + 108 108 + /* Input sinks that the camera delivers SLAM frames to */ 109 109 + struct xrt_slam_sinks in_slam_sinks; 110 110 + 111 111 + /* SLAM/HT sinks we deliver imu and frame data to */ 112 112 + struct xrt_slam_sinks slam_sinks; 113 113 + 114 114 + struct xrt_device *handtracker; 115 115 + 116 116 + struct 117 117 + { 118 118 + struct u_var_button hmd_screen_enable_btn; 119 119 + struct u_var_button switch_tracker_btn; 120 120 + char hand_status[128]; 121 121 + char slam_status[128]; 122 122 + } gui; 123 123 + }; 124 124 + 125 125 + struct rift_s_tracker * 126 126 + rift_s_tracker_create(struct xrt_tracking_origin *origin, 127 127 + struct xrt_frame_context *xfctx, 128 128 + struct rift_s_hmd_config *hmd_config); 129 129 + void 130 130 + rift_s_tracker_start(struct rift_s_tracker *t); 131 131 + void 132 132 + rift_s_tracker_destroy(struct rift_s_tracker *t); 133 133 + void 134 134 + rift_s_tracker_add_debug_ui(struct rift_s_tracker *t, void *root); 135 135 + 136 136 + struct xrt_slam_sinks * 137 137 + rift_s_tracker_get_slam_sinks(struct rift_s_tracker *t); 138 138 + struct xrt_device * 139 139 + rift_s_tracker_get_hand_tracking_device(struct rift_s_tracker *t); 140 140 + 141 141 + void 142 142 + rift_s_tracker_imu_update(struct rift_s_tracker *t, 143 143 + uint64_t timestamp_ns, 144 144 + timepoint_ns local_timestamp_ns, 145 145 + const struct xrt_vec3 *accel, 146 146 + const struct xrt_vec3 *gyro); 147 147 + 148 148 + void 149 149 + rift_s_tracker_push_slam_frames(struct rift_s_tracker *t, 150 150 + uint64_t frame_ts_ns, 151 151 + struct xrt_frame *left, 152 152 + struct xrt_frame *right); 153 153 + void 154 154 + rift_s_tracker_get_tracked_pose(struct rift_s_tracker *t, 155 155 + enum rift_s_tracker_pose pose, 156 156 + uint64_t at_timestamp_ns, 157 157 + struct xrt_space_relation *out_relation); 158 158 + 159 159 + #endif

+352

src/xrt/drivers/rift_s/rift_s_util.cpp

··· 1 1 + /* 2 2 + * Copyright 2022 Jan Schmidt 3 3 + * SPDX-License-Identifier: BSL-1.0 4 4 + * 5 5 + */ 6 6 + /*! 7 7 + * @file 8 8 + * @brief Driver code for Oculus Rift S headsets 9 9 + * 10 10 + * Utility functions for generating a stereo camera calibration, 11 11 + * and converting the Rift S Fisheye62 distortion parameters into 12 12 + * OpenCV-compatible Kannala-Brandt parameters 13 13 + * 14 14 + * @author Jan Schmidt <jan@centricular.com> 15 15 + * @ingroup drv_rift_s 16 16 + */ 17 17 + #include <stdbool.h> 18 18 + #include <math.h> 19 19 + 20 20 + #include "tinyceres/tiny_solver.hpp" 21 21 + #include "tinyceres/tiny_solver_autodiff_function.hpp" 22 22 + 23 23 + #include "rift_s_util.h" 24 24 + 25 25 + using ceres::TinySolver; 26 26 + using ceres::TinySolverAutoDiffFunction; 27 27 + 28 28 + const int N_KB4_DISTORT_PARAMS = 4; 29 29 + 30 30 + template <typename T> 31 31 + bool 32 32 + fisheye62_undistort_func(struct t_camera_calibration *calib, 33 33 + const double *distortion_params, 34 34 + const T point[2], 35 35 + T *out_point) 36 36 + { 37 37 + const T x = point[0]; 38 38 + const T y = point[1]; 39 39 + 40 40 + const T r2 = x * x + y * y; 41 41 + const T r = sqrt(r2); 42 42 + 43 43 + const double fx = calib->intrinsics[0][0]; 44 44 + const double fy = calib->intrinsics[1][1]; 45 45 + 46 46 + const double cx = calib->intrinsics[0][2]; 47 47 + const double cy = calib->intrinsics[1][2]; 48 48 + 49 49 + if (r < 1e-8) { 50 50 + out_point[0] = fx * x + cx; 51 51 + out_point[1] = fy * y + cy; 52 52 + return true; 53 53 + } 54 54 + 55 55 + const T theta = atan(r); 56 56 + const T theta2 = theta * theta; 57 57 + 58 58 + const T xp = x * theta / r; 59 59 + const T yp = y * theta / r; 60 60 + 61 61 + const double k1 = distortion_params[0]; 62 62 + const double k2 = distortion_params[1]; 63 63 + const double k3 = distortion_params[2]; 64 64 + const double k4 = distortion_params[3]; 65 65 + const double k5 = distortion_params[4]; 66 66 + const double k6 = distortion_params[5]; 67 67 + const double p1 = distortion_params[6]; 68 68 + const double p2 = distortion_params[7]; 69 69 + 70 70 + /* 1 + k1 * theta^2 + k2 * theta^4 + k3 * theta^6 + k4 * theta^8 + k5 * theta^10 + k6 * theta^12 */ 71 71 + T r_theta = k6 * theta2; 72 72 + r_theta += k5; 73 73 + r_theta *= theta2; 74 74 + r_theta += k4; 75 75 + r_theta *= theta2; 76 76 + r_theta += k3; 77 77 + r_theta *= theta2; 78 78 + r_theta += k2; 79 79 + r_theta *= theta2; 80 80 + r_theta += k1; 81 81 + r_theta *= theta2; 82 82 + r_theta += 1; 83 83 + 84 84 + T delta_x = 2 * p1 * xp * yp + p2 * (theta2 + xp * xp * 2.0); 85 85 + T delta_y = 2 * p2 * xp * yp + p1 * (theta2 + yp * yp * 2.0); 86 86 + 87 87 + const T mx = xp * r_theta + delta_x; 88 88 + const T my = yp * r_theta + delta_y; 89 89 + 90 90 + out_point[0] = fx * mx + cx; 91 91 + out_point[1] = fy * my + cy; 92 92 + 93 93 + return true; 94 94 + } 95 95 + 96 96 + struct UndistortCostFunctor 97 97 + { 98 98 + UndistortCostFunctor(struct t_camera_calibration *calib, double *distortion_params, double point[2]) 99 99 + : m_calib(calib), m_distortion_params(distortion_params) 100 100 + { 101 101 + m_point[0] = point[0]; 102 102 + m_point[1] = point[1]; 103 103 + } 104 104 + 105 105 + struct t_camera_calibration *m_calib; 106 106 + double *m_distortion_params; 107 107 + double m_point[2]; 108 108 + 109 109 + template <typename T> 110 110 + bool 111 111 + operator()(const T *const x, T *residual) const 112 112 + { 113 113 + T out_point[2]; 114 114 + 115 115 + if (!fisheye62_undistort_func(m_calib, m_distortion_params, x, out_point)) 116 116 + return false; 117 117 + 118 118 + residual[0] = out_point[0] - m_point[0]; 119 119 + residual[1] = out_point[1] - m_point[1]; 120 120 + return true; 121 121 + } 122 122 + }; 123 123 + 124 124 + template <typename T> 125 125 + bool 126 126 + kb4_distort_func(struct t_camera_calibration *calib, const T *distortion_params, const double point[2], T *out_point) 127 127 + { 128 128 + const double x = point[0]; 129 129 + const double y = point[1]; 130 130 + 131 131 + const double r2 = x * x + y * y; 132 132 + const double r = sqrt(r2); 133 133 + 134 134 + const double fx = calib->intrinsics[0][0]; 135 135 + const double fy = calib->intrinsics[1][1]; 136 136 + 137 137 + const double cx = calib->intrinsics[0][2]; 138 138 + const double cy = calib->intrinsics[1][2]; 139 139 + 140 140 + if (r < 1e-8) { 141 141 + out_point[0] = T(fx * x + cx); 142 142 + out_point[1] = T(fy * y + cy); 143 143 + return true; 144 144 + } 145 145 + 146 146 + const double theta = atan(r); 147 147 + const double theta2 = theta * theta; 148 148 + 149 149 + const T k1 = distortion_params[0]; 150 150 + const T k2 = distortion_params[1]; 151 151 + const T k3 = distortion_params[2]; 152 152 + const T k4 = distortion_params[3]; 153 153 + 154 154 + T r_theta = k4 * theta2; 155 155 + 156 156 + r_theta += k3; 157 157 + r_theta *= theta2; 158 158 + r_theta += k2; 159 159 + r_theta *= theta2; 160 160 + r_theta += k1; 161 161 + r_theta *= theta2; 162 162 + r_theta += 1; 163 163 + r_theta *= theta; 164 164 + 165 165 + const T mx = x * r_theta / r; 166 166 + const T my = y * r_theta / r; 167 167 + 168 168 + out_point[0] = fx * mx + cx; 169 169 + out_point[1] = fy * my + cy; 170 170 + 171 171 + return true; 172 172 + } 173 173 + 174 174 + struct TargetPoint 175 175 + { 176 176 + double point[2]; 177 177 + double distorted[2]; 178 178 + }; 179 179 + 180 180 + struct DistortParamKB4CostFunctor 181 181 + { 182 182 + DistortParamKB4CostFunctor(struct t_camera_calibration *calib, int nSteps, TargetPoint *targetPointGrid) 183 183 + : m_calib(calib), m_nSteps(nSteps), m_targetPointGrid(targetPointGrid) 184 184 + {} 185 185 + 186 186 + struct t_camera_calibration *m_calib; 187 187 + int m_nSteps; 188 188 + TargetPoint *m_targetPointGrid; 189 189 + 190 190 + template <typename T> 191 191 + bool 192 192 + operator()(const T *const distort_params, T *residual) const 193 193 + { 194 194 + T out_point[2]; 195 195 + 196 196 + for (int y_index = 0; y_index < m_nSteps; y_index++) { 197 197 + for (int x_index = 0; x_index < m_nSteps; x_index++) { 198 198 + int residual_index = 2 * (y_index * m_nSteps + x_index); 199 199 + TargetPoint *p = &m_targetPointGrid[(y_index * m_nSteps) + x_index]; 200 200 + 201 201 + if (!kb4_distort_func<T>(m_calib, distort_params, p->point, out_point)) 202 202 + return false; 203 203 + 204 204 + residual[residual_index + 0] = out_point[0] - p->distorted[0]; 205 205 + residual[residual_index + 1] = out_point[1] - p->distorted[1]; 206 206 + } 207 207 + } 208 208 + 209 209 + return true; 210 210 + } 211 211 + }; 212 212 + 213 213 + #define STEPS 21 214 214 + static bool 215 215 + convert_camera_calibration(struct rift_s_camera_calibration *rift_s_cam, struct t_camera_calibration *tcc) 216 216 + { 217 217 + 218 218 + tcc->image_size_pixels.h = rift_s_cam->roi.extent.h; 219 219 + tcc->image_size_pixels.w = rift_s_cam->roi.extent.w; 220 220 + tcc->intrinsics[0][0] = rift_s_cam->projection.fx; 221 221 + tcc->intrinsics[1][1] = rift_s_cam->projection.fy; 222 222 + tcc->intrinsics[0][2] = rift_s_cam->projection.cx; 223 223 + tcc->intrinsics[1][2] = rift_s_cam->projection.cy; 224 224 + tcc->intrinsics[2][2] = 1.0; 225 225 + tcc->use_fisheye = true; 226 226 + 227 227 + TargetPoint xy[STEPS * STEPS]; 228 228 + 229 229 + /* Convert fisheye62 params to KB4: */ 230 230 + double fisheye62_distort_params[8]; 231 231 + for (int i = 0; i < 6; i++) { 232 232 + fisheye62_distort_params[i] = rift_s_cam->distortion.k[i]; 233 233 + } 234 234 + fisheye62_distort_params[6] = rift_s_cam->distortion.p1; 235 235 + fisheye62_distort_params[7] = rift_s_cam->distortion.p2; 236 236 + 237 237 + /* Calculate Fisheye62 distortion grid by finding the viewplane coordinates that 238 238 + * project onto the points of grid spaced across the pixel image plane */ 239 239 + for (int y_index = 0; y_index < STEPS; y_index++) { 240 240 + for (int x_index = 0; x_index < STEPS; x_index++) { 241 241 + int x = x_index * (tcc->image_size_pixels.w - 1) / (STEPS - 1); 242 242 + int y = y_index * (tcc->image_size_pixels.h - 1) / (STEPS - 1); 243 243 + TargetPoint *p = &xy[(y_index * STEPS) + x_index]; 244 244 + 245 245 + p->distorted[0] = x; 246 246 + p->distorted[1] = y; 247 247 + 248 248 + Eigen::Matrix<double, 2, 1> result(0, 0); 249 249 + 250 250 + using AutoDiffUndistortFunction = TinySolverAutoDiffFunction<UndistortCostFunctor, 2, 2>; 251 251 + UndistortCostFunctor undistort_functor(tcc, fisheye62_distort_params, p->distorted); 252 252 + AutoDiffUndistortFunction f(undistort_functor); 253 253 + 254 254 + TinySolver<AutoDiffUndistortFunction> solver; 255 255 + solver.Solve(f, &result); 256 256 + 257 257 + p->point[0] = result[0]; 258 258 + p->point[1] = result[1]; 259 259 + } 260 260 + } 261 261 + 262 262 + /* Use the calculated distortion grid to solve for kb4 params */ 263 263 + { 264 264 + Eigen::Matrix<double, N_KB4_DISTORT_PARAMS, 1> kb4_distort_params; 265 265 + 266 266 + using AutoDiffDistortParamKB4Function = 267 267 + TinySolverAutoDiffFunction<DistortParamKB4CostFunctor, 2 * STEPS * STEPS, N_KB4_DISTORT_PARAMS>; 268 268 + DistortParamKB4CostFunctor distort_param_kb4_functor(tcc, STEPS, xy); 269 269 + AutoDiffDistortParamKB4Function f(distort_param_kb4_functor); 270 270 + 271 271 + TinySolver<AutoDiffDistortParamKB4Function> solver; 272 272 + solver.Solve(f, &kb4_distort_params); 273 273 + 274 274 + for (int i = 0; i < 4; i++) 275 275 + tcc->distortion_fisheye[i] = kb4_distort_params[i]; 276 276 + } 277 277 + 278 278 + return true; 279 279 + } 280 280 + 281 281 + /*! 282 282 + * Allocate and populate an OpenCV-compatible @ref t_stereo_camera_calibration pointer from 283 283 + * the Rift S config. 284 284 + * 285 285 + * This requires fitting a KB4 fisheye polynomial to the 6 radial + 2 tangential 'Fisheye62' 286 286 + * parameters provided by the Rift S. 287 287 + * 288 288 + */ 289 289 + struct t_stereo_camera_calibration * 290 290 + rift_s_create_stereo_camera_calib_rotated(struct rift_s_camera_calibration_block *camera_calibration) 291 291 + { 292 292 + struct t_stereo_camera_calibration *calib = NULL; 293 293 + t_stereo_camera_calibration_alloc(&calib, 8); 294 294 + 295 295 + struct rift_s_camera_calibration *left = &camera_calibration->cameras[RIFT_S_CAMERA_FRONT_LEFT]; 296 296 + struct rift_s_camera_calibration *right = &camera_calibration->cameras[RIFT_S_CAMERA_FRONT_RIGHT]; 297 297 + 298 298 + // Intrinsics 299 299 + for (int view = 0; view < 2; view++) { 300 300 + struct t_camera_calibration *tcc = &calib->view[view]; 301 301 + struct rift_s_camera_calibration *cam_config; 302 302 + 303 303 + if (view == 0) { 304 304 + cam_config = left; 305 305 + } else { 306 306 + cam_config = right; 307 307 + } 308 308 + 309 309 + if (!convert_camera_calibration(cam_config, tcc)) 310 310 + goto fail; 311 311 + } 312 312 + 313 313 + struct xrt_pose device_from_left, device_from_right; 314 314 + struct xrt_pose right_from_device, right_from_left; 315 315 + 316 316 + struct xrt_matrix_3x3 right_from_left_rot; 317 317 + 318 318 + /* Compute the transform from the left eye to the right eye 319 319 + * by using the config provided camera->device transform 320 320 + */ 321 321 + math_pose_from_isometry(&left->device_from_camera, &device_from_left); 322 322 + math_pose_from_isometry(&right->device_from_camera, &device_from_right); 323 323 + 324 324 + math_pose_invert(&device_from_right, &right_from_device); 325 325 + math_pose_transform(&device_from_left, &right_from_device, &right_from_left); 326 326 + 327 327 + math_matrix_3x3_from_quat(&right_from_left.orientation, &right_from_left_rot); 328 328 + 329 329 + /* Rotate the position in the camera extrinsics 90° to 330 330 + * compensate for the front cams being rotated. That means hand poses 331 331 + * are detected and come out rotated too, so need correcting 332 332 + * in the tracking override offset */ 333 333 + calib->camera_translation[0] = -right_from_left.position.y; 334 334 + calib->camera_translation[1] = right_from_left.position.x; 335 335 + calib->camera_translation[2] = right_from_left.position.z; 336 336 + 337 337 + calib->camera_rotation[0][0] = right_from_left_rot.v[0]; 338 338 + calib->camera_rotation[0][1] = right_from_left_rot.v[1]; 339 339 + calib->camera_rotation[0][2] = right_from_left_rot.v[2]; 340 340 + calib->camera_rotation[1][0] = right_from_left_rot.v[3]; 341 341 + calib->camera_rotation[1][1] = right_from_left_rot.v[4]; 342 342 + calib->camera_rotation[1][2] = right_from_left_rot.v[5]; 343 343 + calib->camera_rotation[2][0] = right_from_left_rot.v[6]; 344 344 + calib->camera_rotation[2][1] = right_from_left_rot.v[7]; 345 345 + calib->camera_rotation[2][2] = right_from_left_rot.v[8]; 346 346 + 347 347 + return calib; 348 348 + 349 349 + fail: 350 350 + t_stereo_camera_calibration_reference(&calib, NULL); 351 351 + return NULL; 352 352 + }

+28

src/xrt/drivers/rift_s/rift_s_util.h

··· 1 1 + /* 2 2 + * Copyright 2022 Jan Schmidt 3 3 + * SPDX-License-Identifier: BSL-1.0 4 4 + */ 5 5 + 6 6 + /*! 7 7 + * @file 8 8 + * @brief Oculus Rift S utility functions 9 9 + * @author Jan Schmidt <jan@centricular.com> 10 10 + * @ingroup drv_rift_s 11 11 + */ 12 12 + 13 13 + #pragma once 14 14 + 15 15 + #include "tracking/t_tracking.h" 16 16 + 17 17 + #include "rift_s_firmware.h" 18 18 + 19 19 + #ifdef __cplusplus 20 20 + extern "C" { 21 21 + #endif 22 22 + 23 23 + struct t_stereo_camera_calibration * 24 24 + rift_s_create_stereo_camera_calib_rotated(struct rift_s_camera_calibration_block *camera_calibration); 25 25 + 26 26 + #ifdef __cplusplus 27 27 + } 28 28 + #endif