-1

src/xrt/drivers/CMakeLists.txt

··· 105 105 north_star/ns_hmd.h 106 106 north_star/ns_hmd.c 107 107 north_star/ns_interface.h 108 108 - north_star/ns_prober.c 109 108 ) 110 109 target_link_libraries(drv_ns PRIVATE xrt-interfaces aux_math xrt-external-cjson) 111 110 list(APPEND ENABLED_HEADSET_DRIVERS ns)

+145 -252

src/xrt/drivers/north_star/ns_hmd.c

··· 32 32 33 33 DEBUG_GET_ONCE_LOG_OPTION(ns_log, "NS_LOG", U_LOGGING_INFO) 34 34 35 35 - #define printf_pose(pose) \ 36 36 - printf("%f %f %f %f %f %f %f\n", pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, \ 37 37 - pose.orientation.y, pose.orientation.z, pose.orientation.w); 35 35 + /* 36 36 + * 37 37 + * Printing functions. 38 38 + * 39 39 + */ 40 40 + 41 41 + #define NS_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) 42 42 + #define NS_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) 43 43 + #define NS_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) 44 44 + #define NS_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) 45 45 + #define NS_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) 38 46 39 47 40 48 static float 41 49 try_get_ipd(struct ns_hmd *ns, const struct cJSON *json) 42 42 - { // 50 50 + { 43 51 const char *things[] = {"baseline", "ipd", "IPD"}; 44 52 bool done = false; 45 53 float out; ··· 50 58 } 51 59 if (!done) { 52 60 NS_INFO(ns, 53 53 - "No key `baseline (or ipd, or IPD)` in your config file. Guessing the IPD is 64 millimeters"); 61 61 + "No key `baseline` (or `ipd`, or `IPD`) in your config file. " 62 62 + "Guessing the IPD is 64 millimeters"); 54 63 out = 64.0f; 55 64 } 56 65 if (out > 250.0f) { ··· 68 77 } 69 78 70 79 static void 71 71 - try_get_fov(struct ns_hmd *ns, const struct cJSON *json, struct xrt_fov *left_fov, struct xrt_fov *right_fov) 80 80 + try_get_fov(struct ns_hmd *ns, const struct cJSON *json, struct xrt_fov *out_left_fov, struct xrt_fov *out_right_fov) 72 81 { 73 82 const char *things[] = {"fov", "FOV"}; 74 83 float out_float; ··· 105 114 assert(fabsf(out_fov.angle_down) < M_PI_2); 106 115 assert(fabsf(out_fov.angle_left) < M_PI_2); 107 116 assert(fabsf(out_fov.angle_right) < M_PI_2); 108 108 - memcpy(left_fov, &out_fov, sizeof(struct xrt_fov)); 109 109 - memcpy(right_fov, &out_fov, sizeof(struct xrt_fov)); 117 117 + *out_left_fov = out_fov; 118 118 + *out_right_fov = out_fov; 110 119 } 111 120 112 112 - /* 113 113 - * 114 114 - * "2D Polynomial" distortion; original implementation by Johnathon Zelstadt 115 115 - * Sometimes known as "v2", filename is often NorthStarCalibration.json 116 116 - * 117 117 - */ 118 118 - 119 119 - static bool 120 120 - ns_p2d_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) 121 121 - { 122 122 - struct ns_hmd *ns = ns_hmd(xdev); 123 123 - return u_compute_distortion_ns_p2d(&ns->dist_p2d, view, u, v, result); 124 124 - } 125 121 126 122 127 123 bool 128 128 - ns_p2d_parse(struct ns_hmd *ns) 124 124 + ns_p2d_parse(struct ns_hmd *ns, const cJSON *json) 129 125 { 130 130 - 131 131 - struct xrt_pose temp_eyes_center_to_eye[2]; 132 132 - 133 133 - // convenience names 134 134 - const struct cJSON *config_json = ns->config_json; 126 126 + struct u_ns_p2d_values *values = &ns->config.dist_p2d; 135 127 136 128 // Note that x and y are flipped. We have to flip 'em at some point - the polynomial calibrator has a strange 137 129 // definition of x and y. "opencv treats column major over row major (as in, Y,X for image look up)" -Dr. Damo 138 138 - if (u_json_get_float_array(u_json_get(config_json, "left_uv_to_rect_x"), ns->dist_p2d.y_coefficients_right, 139 139 - 16) != 16) 130 130 + if (u_json_get_float_array(u_json_get(json, "left_uv_to_rect_x"), values->y_coefficients_right, 16) != 16) 140 131 goto cleanup_p2d; 141 141 - if (u_json_get_float_array(u_json_get(config_json, "left_uv_to_rect_y"), ns->dist_p2d.x_coefficients_right, 142 142 - 16) != 16) 132 132 + if (u_json_get_float_array(u_json_get(json, "left_uv_to_rect_y"), values->x_coefficients_right, 16) != 16) 143 133 goto cleanup_p2d; 144 144 - if (u_json_get_float_array(u_json_get(config_json, "right_uv_to_rect_x"), ns->dist_p2d.y_coefficients_left, 145 145 - 16) != 16) 134 134 + if (u_json_get_float_array(u_json_get(json, "right_uv_to_rect_x"), values->y_coefficients_left, 16) != 16) 146 135 goto cleanup_p2d; 147 147 - if (u_json_get_float_array(u_json_get(config_json, "right_uv_to_rect_y"), ns->dist_p2d.x_coefficients_left, 148 148 - 16) != 16) 136 136 + if (u_json_get_float_array(u_json_get(json, "right_uv_to_rect_y"), values->x_coefficients_left, 16) != 16) 149 137 goto cleanup_p2d; 150 138 151 139 // at this point, locked into using this distortion method - we can touch anything and not worry about side 152 140 // effects 153 153 - float baseline = try_get_ipd(ns, config_json); 141 141 + ns->config.distortion_type = NS_DISTORTION_TYPE_POLYNOMIAL_2D; 154 142 155 155 - math_pose_identity(&temp_eyes_center_to_eye[0]); 156 156 - math_pose_identity(&temp_eyes_center_to_eye[1]); 157 157 - temp_eyes_center_to_eye[0].position.x = -baseline / 2; 158 158 - temp_eyes_center_to_eye[1].position.x = baseline / 2; 143 143 + float baseline = try_get_ipd(ns, json); 159 144 160 160 - try_get_fov(ns, config_json, &ns->dist_p2d.fov[0], &ns->dist_p2d.fov[1]); 145 145 + math_pose_identity(&ns->config.head_pose_to_eye[0]); 146 146 + math_pose_identity(&ns->config.head_pose_to_eye[1]); 147 147 + ns->config.head_pose_to_eye[0].position.x = -baseline / 2; 148 148 + ns->config.head_pose_to_eye[1].position.x = baseline / 2; 161 149 162 162 - memcpy(&ns->base.hmd->distortion.fov[0], &ns->dist_p2d.fov[0], sizeof(struct xrt_fov)); 163 163 - memcpy(&ns->base.hmd->distortion.fov[1], &ns->dist_p2d.fov[1], sizeof(struct xrt_fov)); 150 150 + try_get_fov(ns, json, &values->fov[0], &values->fov[1]); 164 151 165 165 - ns->base.compute_distortion = &ns_p2d_mesh_calc; 166 166 - memcpy(&ns->head_pose_to_eye, &temp_eyes_center_to_eye, sizeof(struct xrt_pose) * 2); 152 152 + ns->config.fov[0] = values->fov[0]; 153 153 + ns->config.fov[1] = values->fov[1]; 167 154 168 155 return true; 169 156 170 157 cleanup_p2d: 171 171 - memset(&ns->dist_p2d, 0, sizeof(struct u_ns_p2d_values)); 172 158 return false; 173 159 } 174 160 175 161 176 176 - /* 177 177 - * 178 178 - * "Original 3D" undistortion, by Leap Motion 179 179 - * Sometimes known as "v1", config file name is often "Calibration.json" 180 180 - * 181 181 - */ 182 182 - 183 183 - static bool 184 184 - ns_3d_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) 185 185 - { 186 186 - struct ns_hmd *ns = ns_hmd(xdev); 187 187 - struct ns_3d_data *data = &ns->dist_3d; 188 188 - struct xrt_vec2 uv = {u, v}; 189 189 - struct xrt_vec2 warped_uv = {0.0f, 0.0f}; 190 190 - 191 191 - ns_3d_display_uv_to_render_uv(uv, &warped_uv, &data->eyes[view]); 192 192 - 193 193 - result->r.x = warped_uv.x; 194 194 - result->r.y = warped_uv.y; 195 195 - result->g.x = warped_uv.x; 196 196 - result->g.y = warped_uv.y; 197 197 - result->b.x = warped_uv.x; 198 198 - result->b.y = warped_uv.y; 199 199 - return true; 200 200 - } 201 201 - 202 162 static void 203 203 - ns_3d_fov_calculate(struct xrt_fov *fov, struct xrt_quat projection) 163 163 + ns_3d_fov_calculate(struct xrt_quat projection, struct xrt_fov *out_fov) 204 164 { 205 165 // Million thanks to Nico Zobernig for figuring this out 206 206 - fov->angle_left = atanf(projection.x); 207 207 - fov->angle_right = atanf(projection.y); 208 208 - fov->angle_up = atanf(projection.z); 209 209 - fov->angle_down = atanf(projection.w); 166 166 + out_fov->angle_left = atanf(projection.x); 167 167 + out_fov->angle_right = atanf(projection.y); 168 168 + out_fov->angle_up = atanf(projection.z); 169 169 + out_fov->angle_down = atanf(projection.w); 210 170 } 211 171 212 172 /* ··· 215 175 * 216 176 */ 217 177 218 218 - static bool 219 219 - ns_3d_leap_parse(struct ns_3d_leap *leap, const struct cJSON *leap_data) 220 220 - { 221 221 - u_json_get_string_into_array(u_json_get(leap_data, "name"), leap->name, 64); 222 222 - u_json_get_string_into_array(u_json_get(leap_data, "serial"), leap->serial, 64); 223 223 - if (!u_json_get_vec3(u_json_get(u_json_get(leap_data, "localPose"), "position"), &leap->pose.position)) 224 224 - return false; 225 225 - if (!u_json_get_quat(u_json_get(u_json_get(leap_data, "localPose"), "rotation"), &leap->pose.orientation)) 226 226 - return false; 227 227 - return true; 228 228 - } 178 178 + 229 179 230 180 static bool 231 231 - ns_3d_eye_parse(struct ns_3d_eye *eye, const struct cJSON *eye_data) 181 181 + ns_3d_eye_parse(struct ns_hmd *ns, struct ns_3d_eye *eye, const struct cJSON *eye_data) 232 182 { 233 183 if (!u_json_get_float(u_json_get(eye_data, "ellipseMinorAxis"), &eye->ellipse_minor_axis)) 234 184 return false; ··· 259 209 } 260 210 261 211 bool 262 262 - ns_3d_parse(struct ns_hmd *ns) 212 212 + ns_3d_parse(struct ns_hmd *ns, const cJSON *json) 263 213 { 264 264 - struct ns_3d_data *our_ns_3d_data = &ns->dist_3d; 214 214 + struct ns_3d_values *values = &ns->config.dist_3d; 265 215 266 266 - if (!ns_3d_eye_parse(&our_ns_3d_data->eyes[0], u_json_get(ns->config_json, "leftEye"))) 216 216 + 217 217 + if (!ns_3d_eye_parse(ns, &values->eyes[0], u_json_get(json, "leftEye"))) 267 218 goto cleanup_l3d; 268 268 - if (!ns_3d_eye_parse(&our_ns_3d_data->eyes[1], u_json_get(ns->config_json, "rightEye"))) 269 269 - goto cleanup_l3d; 270 270 - if (!ns_3d_leap_parse(&our_ns_3d_data->leap, u_json_get(ns->config_json, "leapTracker"))) 219 219 + if (!ns_3d_eye_parse(ns, &values->eyes[1], u_json_get(json, "rightEye"))) 271 220 goto cleanup_l3d; 272 221 273 222 // Locked in, okay to touch anything inside ns struct 274 274 - ns_3d_fov_calculate(&ns->base.hmd->distortion.fov[0], our_ns_3d_data->eyes[0].camera_projection); 275 275 - ns_3d_fov_calculate(&ns->base.hmd->distortion.fov[1], our_ns_3d_data->eyes[1].camera_projection); 223 223 + ns->config.distortion_type = NS_DISTORTION_TYPE_GEOMETRIC_3D; 276 224 277 277 - ns->head_pose_to_eye[0] = our_ns_3d_data->eyes[0].eye_pose; // Left eye. 278 278 - ns->head_pose_to_eye[1] = our_ns_3d_data->eyes[1].eye_pose; // Right eye. 225 225 + ns_3d_fov_calculate(values->eyes[0].camera_projection, &ns->config.fov[0]); 226 226 + ns_3d_fov_calculate(values->eyes[1].camera_projection, &ns->config.fov[1]); 279 227 280 280 - our_ns_3d_data->eyes[0].optical_system = ns_3d_create_optical_system(&our_ns_3d_data->eyes[0]); 281 281 - our_ns_3d_data->eyes[1].optical_system = ns_3d_create_optical_system(&our_ns_3d_data->eyes[1]); 228 228 + ns->config.head_pose_to_eye[0] = values->eyes[0].eye_pose; // Left eye. 229 229 + ns->config.head_pose_to_eye[1] = values->eyes[1].eye_pose; // Right eye. 282 230 283 283 - ns->base.compute_distortion = &ns_3d_mesh_calc; 231 231 + values->eyes[0].optical_system = ns_3d_create_optical_system(&values->eyes[0]); 232 232 + values->eyes[1].optical_system = ns_3d_create_optical_system(&values->eyes[1]); 284 233 285 234 return true; 286 235 287 236 cleanup_l3d: 288 288 - memset(&ns->dist_3d, 0, sizeof(struct ns_3d_data)); 237 237 + ns_3d_free_optical_system(&values->eyes[0].optical_system); 238 238 + ns_3d_free_optical_system(&values->eyes[1].optical_system); 289 239 return false; 290 240 } 291 241 292 242 293 243 /* 294 244 * 295 295 - * Moses Turner's distortion correction 245 245 + * Moses Turner's meshgrid-based distortion correction 296 246 * 297 247 */ 298 248 299 249 bool 300 300 - ns_meshgrid_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) 250 250 + ns_mt_parse(struct ns_hmd *ns, const cJSON *json) 301 251 { 302 302 - struct ns_hmd *ns = ns_hmd(xdev); 303 303 - return u_compute_distortion_ns_meshgrid(&ns->dist_meshgrid, view, u, v, result); 304 304 - } 252 252 + struct u_ns_meshgrid_values *values = &ns->config.dist_meshgrid; 305 253 306 306 - void 307 307 - ns_meshgrid_free_values(struct ns_hmd *ns) 308 308 - { 309 309 - free(ns->dist_meshgrid.ipds); 310 310 - free(ns->dist_meshgrid.grid[0]); 311 311 - free(ns->dist_meshgrid.grid[1]); 312 312 - } 313 313 - 314 314 - bool 315 315 - ns_meshgrid_parse(struct ns_hmd *ns) 316 316 - { 317 317 - 318 318 - struct u_ns_meshgrid_values *values = &ns->dist_meshgrid; 319 319 - const struct cJSON *config_json = ns->config_json; 320 320 - 321 321 - if (strcmp(cJSON_GetStringValue(u_json_get(config_json, "type")), "Moses Turner's distortion correction") != 322 322 - 0) { 254 254 + if (strcmp(cJSON_GetStringValue(u_json_get(json, "type")), "Moses Turner's distortion correction") != 0) { 323 255 goto cleanup_mt; 324 256 } 325 257 int version = 0; 326 326 - u_json_get_int(u_json_get(config_json, "version"), &version); 258 258 + u_json_get_int(u_json_get(json, "version"), &version); 327 259 if (version != 2) { 328 260 goto cleanup_mt; 329 261 } 330 262 331 331 - u_json_get_int(u_json_get(config_json, "num_grid_points_x"), &values->num_grid_points_u); 332 332 - u_json_get_int(u_json_get(config_json, "num_grid_points_y"), &values->num_grid_points_v); 263 263 + u_json_get_int(u_json_get(json, "num_grid_points_x"), &values->num_grid_points_u); 264 264 + u_json_get_int(u_json_get(json, "num_grid_points_y"), &values->num_grid_points_v); 333 265 334 334 - values->grid[0] = 335 335 - realloc(values->grid[0], sizeof(struct xrt_vec2) * values->num_grid_points_u * values->num_grid_points_v); 336 336 - values->grid[1] = 337 337 - realloc(values->grid[1], sizeof(struct xrt_vec2) * values->num_grid_points_u * values->num_grid_points_v); 266 266 + values->grid[0] = U_TYPED_ARRAY_CALLOC(struct xrt_vec2, values->num_grid_points_u * values->num_grid_points_v); 267 267 + values->grid[1] = U_TYPED_ARRAY_CALLOC(struct xrt_vec2, values->num_grid_points_u * values->num_grid_points_v); 338 268 339 339 - values->ipd = try_get_ipd(ns, ns->config_json); 269 269 + values->ipd = try_get_ipd(ns, json); 340 270 341 341 - const cJSON *current_element = config_json; 271 271 + const cJSON *current_element = json; 342 272 343 273 344 274 for (int view = 0; view <= 1; view++) { ··· 365 295 } 366 296 } 367 297 } 298 298 + // locked in 299 299 + ns->config.distortion_type = NS_DISTORTION_TYPE_MOSES_MESHGRID; 368 300 369 301 float baseline = values->ipd; 370 302 371 303 372 372 - try_get_fov(ns, config_json, &values->fov[0], &values->fov[1]); 304 304 + try_get_fov(ns, json, &values->fov[0], &values->fov[1]); 373 305 374 374 - ns->base.hmd->distortion.fov[0] = values->fov[0]; 375 375 - ns->base.hmd->distortion.fov[1] = values->fov[1]; 306 306 + ns->config.fov[0] = values->fov[0]; 307 307 + ns->config.fov[1] = values->fov[1]; 376 308 377 377 - ns->head_pose_to_eye[0].orientation.x = 0.0f; 378 378 - ns->head_pose_to_eye[0].orientation.y = 0.0f; 379 379 - ns->head_pose_to_eye[0].orientation.z = 0.0f; 380 380 - ns->head_pose_to_eye[0].orientation.w = 1.0f; 381 381 - ns->head_pose_to_eye[0].position.x = -baseline / 2; 382 382 - ns->head_pose_to_eye[0].position.y = 0.0f; 383 383 - ns->head_pose_to_eye[0].position.z = 0.0f; 384 384 - 309 309 + math_pose_identity(&ns->config.head_pose_to_eye[0]); 310 310 + math_pose_identity(&ns->config.head_pose_to_eye[1]); 311 311 + ns->config.head_pose_to_eye[0].position.x = -baseline / 2; 312 312 + ns->config.head_pose_to_eye[1].position.x = baseline / 2; 385 313 314 314 + return true; 386 315 387 387 - ns->head_pose_to_eye[1].orientation.x = 0.0f; 388 388 - ns->head_pose_to_eye[1].orientation.y = 0.0f; 389 389 - ns->head_pose_to_eye[1].orientation.z = 0.0f; 390 390 - ns->head_pose_to_eye[1].orientation.w = 1.0f; 391 391 - ns->head_pose_to_eye[1].position.x = baseline / 2; 392 392 - ns->head_pose_to_eye[1].position.y = 0.0f; 393 393 - ns->head_pose_to_eye[1].position.z = 0.0f; 316 316 + cleanup_mt: 317 317 + free(values->grid[0]); 318 318 + free(values->grid[1]); 319 319 + return false; 320 320 + } 394 321 395 395 - ns->base.compute_distortion = &ns_meshgrid_mesh_calc; 396 322 397 397 - ns->free_distortion_values = ns_meshgrid_free_values; 398 323 399 399 - return true; 400 400 - 401 401 - cleanup_mt: 402 402 - memset(&ns->dist_meshgrid, 0, sizeof(struct u_ns_meshgrid_values)); 324 324 + static bool 325 325 + ns_optical_config_parse(struct ns_hmd *ns) 326 326 + { 327 327 + if (ns_3d_parse(ns, ns->config_json)) { 328 328 + NS_INFO(ns, "Using Gemetric 3D display distortion correction!"); 329 329 + return true; 330 330 + } 331 331 + if (ns_p2d_parse(ns, ns->config_json)) { 332 332 + NS_INFO(ns, "Using Polynomial 2D display distortion correction!"); 333 333 + return true; 334 334 + } 335 335 + if (ns_mt_parse(ns, ns->config_json)) { 336 336 + NS_INFO(ns, "Using Moses's meshgrid-based display distortion correction!"); 337 337 + return true; 338 338 + } 339 339 + U_LOG_E("Couldn't find a valid display distortion correction!"); 403 340 return false; 404 341 } 342 342 + 405 343 406 344 /* 407 345 * ··· 413 351 ns_hmd_destroy(struct xrt_device *xdev) 414 352 { 415 353 struct ns_hmd *ns = ns_hmd(xdev); 354 354 + NS_DEBUG(ns, "Called!"); 416 355 417 356 // Remove the variable tracking. 418 357 u_var_remove_root(ns); 419 358 420 420 - if (ns->free_distortion_values != NULL) { 421 421 - ns->free_distortion_values(ns); 359 359 + if (ns->config.distortion_type == NS_DISTORTION_TYPE_GEOMETRIC_3D) { 360 360 + ns_3d_free_optical_system(&ns->config.dist_3d.eyes[0].optical_system); 361 361 + ns_3d_free_optical_system(&ns->config.dist_3d.eyes[1].optical_system); 362 362 + } else if (ns->config.distortion_type == NS_DISTORTION_TYPE_MOSES_MESHGRID) { 363 363 + free(ns->config.dist_meshgrid.grid[0]); 364 364 + free(ns->config.dist_meshgrid.grid[1]); 422 365 } 423 366 424 367 u_device_free(&ns->base); ··· 426 369 427 370 static void 428 371 ns_hmd_update_inputs(struct xrt_device *xdev) 429 429 - {} 372 372 + { 373 373 + struct ns_hmd *ns = ns_hmd(xdev); 374 374 + NS_DEBUG(ns, "Called!"); 375 375 + } 430 376 431 377 static void 432 378 ns_hmd_get_tracked_pose(struct xrt_device *xdev, ··· 435 381 struct xrt_space_relation *out_relation) 436 382 { 437 383 struct ns_hmd *ns = ns_hmd(xdev); 384 384 + NS_DEBUG(ns, "Called!"); 438 385 439 386 if (name != XRT_INPUT_GENERIC_HEAD_POSE) { 440 387 NS_ERROR(ns, "unknown input name"); ··· 453 400 struct xrt_fov *out_fovs, 454 401 struct xrt_pose *out_poses) 455 402 { 403 403 + struct ns_hmd *ns = ns_hmd(xdev); 404 404 + NS_DEBUG(ns, "Called!"); 405 405 + 456 406 // Use this to take care of most stuff, then fix up below. 457 407 u_device_get_view_poses(xdev, default_eye_relation, at_timestamp_ns, view_count, out_head_relation, out_fovs, 458 408 out_poses); 459 409 460 410 // Fix fix. 461 461 - struct ns_hmd *ns = ns_hmd(xdev); 462 462 - for (uint32_t i = 0; i < view_count && i < ARRAY_SIZE(ns->head_pose_to_eye); i++) { 463 463 - out_poses[i] = ns->head_pose_to_eye[i]; 411 411 + for (uint32_t i = 0; i < view_count && i < ARRAY_SIZE(ns->config.head_pose_to_eye); i++) { 412 412 + out_poses[i] = ns->config.head_pose_to_eye[i]; 464 413 } 465 414 } 466 415 467 467 - static bool 468 468 - ns_config_load(struct ns_hmd *ns, const char *config_path) 416 416 + bool 417 417 + ns_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) 469 418 { 470 470 - // Get the path to the JSON file 471 471 - bool json_allocated = false; 472 472 - if (config_path == NULL || strcmp(config_path, "/") == 0) { 473 473 - NS_INFO(ns, 474 474 - "Configuration path \"%s\" does not lead to a " 475 475 - "configuration JSON file. Set the NS_CONFIG_PATH env " 476 476 - "variable to your JSON.", 477 477 - config_path); 478 478 - return false; 479 479 - } 480 480 - // Open the JSON file and put its contents into a string 481 481 - FILE *config_file = fopen(config_path, "r"); 482 482 - if (config_file == NULL) { 483 483 - NS_INFO(ns, "The configuration file at path \"%s\" was unable to load", config_path); 484 484 - goto parse_error; 485 485 - } 419 419 + struct ns_hmd *ns = ns_hmd(xdev); 420 420 + NS_DEBUG(ns, "Called!"); 421 421 + // struct xrt_vec2 warped_uv; 422 422 + switch (ns->config.distortion_type) { 423 423 + case NS_DISTORTION_TYPE_GEOMETRIC_3D: { 424 424 + struct xrt_vec2 uv = {u, v}; 425 425 + struct xrt_vec2 warped_uv = {0.0f, 0.0f}; 486 426 487 487 - fseek(config_file, 0, SEEK_END); // Go to end of file 488 488 - long file_size = ftell(config_file); // See offset we're at. This should be the file size in bytes. 489 489 - rewind(config_file); // Go back to the beginning of the file 427 427 + ns_3d_display_uv_to_render_uv(uv, &warped_uv, &ns->config.dist_3d.eyes[view]); 490 428 491 491 - if (file_size == 0) { 492 492 - NS_INFO(ns, "Empty config file!"); 493 493 - goto parse_error; 494 494 - } else if (file_size > 3 * pow(1024, 2)) { // 3 MiB 495 495 - NS_INFO(ns, "Huge config file! (%f MiB!!) Something's wrong here.", ((float)file_size) / pow(1024, 2)); 496 496 - goto parse_error; 429 429 + result->r.x = warped_uv.x; 430 430 + result->r.y = warped_uv.y; 431 431 + result->g.x = warped_uv.x; 432 432 + result->g.y = warped_uv.y; 433 433 + result->b.x = warped_uv.x; 434 434 + result->b.y = warped_uv.y; 435 435 + return true; 497 436 } 498 498 - 499 499 - char *json = calloc(file_size + 1, 1); 500 500 - json_allocated = true; 501 501 - 502 502 - size_t ret = fread(json, 1, file_size, config_file); 503 503 - if ((long)ret != file_size) { 504 504 - NS_ERROR(ns, "Failed to read configuration file at path \"%s\"", config_path); 505 505 - goto parse_error; 437 437 + case NS_DISTORTION_TYPE_POLYNOMIAL_2D: { 438 438 + return u_compute_distortion_ns_p2d(&ns->config.dist_p2d, view, u, v, result); 506 439 } 507 507 - fclose(config_file); 508 508 - config_file = NULL; 509 509 - json[file_size] = '\0'; 510 510 - 511 511 - ns->config_json = cJSON_Parse(json); 512 512 - if (ns->config_json == NULL) { 513 513 - const char *error_ptr = cJSON_GetErrorPtr(); 514 514 - NS_INFO(ns, "The JSON file at path \"%s\" was unable to parse", config_path); 515 515 - if (error_ptr != NULL) { 516 516 - NS_INFO(ns, "because of an error before %s", error_ptr); 517 517 - } 518 518 - goto parse_error; 440 440 + case NS_DISTORTION_TYPE_MOSES_MESHGRID: { 441 441 + return u_compute_distortion_ns_meshgrid(&ns->config.dist_meshgrid, view, u, v, result); 519 442 } 520 520 - 521 521 - // this function is not supposed to return true if ns->config_json is NULL 522 522 - assert(ns->config_json != NULL); 523 523 - free(json); 524 524 - 525 525 - return true; 526 526 - 527 527 - parse_error: 528 528 - if (config_file != NULL) { 529 529 - fclose(config_file); 530 530 - config_file = NULL; 443 443 + default: { 444 444 + assert(false); 445 445 + return false; 531 446 } 532 532 - if (json_allocated) { 533 533 - free(json); 534 447 } 535 535 - NS_INFO(ns, "Are you sure you're using the right configuration file?"); 536 536 - return false; 537 448 } 538 538 - 539 449 540 450 /* 541 451 * ··· 544 454 */ 545 455 546 456 struct xrt_device * 547 547 - ns_hmd_create(const char *config_path) 457 457 + ns_hmd_create(const cJSON *config_json) 548 458 { 549 459 enum u_device_alloc_flags flags = 550 460 (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE); 551 461 struct ns_hmd *ns = U_DEVICE_ALLOCATE(struct ns_hmd, flags, 1, 0); 552 552 - ns->log_level = debug_get_log_option_ns_log(); 553 462 554 554 - if (!ns_config_load(ns, config_path)) 555 555 - goto cleanup; // don't need to print any error, ns_config_load did that for us 463 463 + ns->config_json = config_json; 464 464 + ns_optical_config_parse(ns); 556 465 557 557 - int number_wrap = 3; // number of elements in below array of function pointers. Const to stop compiler warnings. 558 558 - bool (*wrap_func_ptr[3])(struct ns_hmd *) = {ns_3d_parse, ns_p2d_parse, ns_meshgrid_parse}; 559 559 - // C syntax is weird here. This is an array of pointers to functions with arguments (struct ns_system * system) 560 560 - // that all return a boolean value. The array should be roughly in descending order of how likely we think the 561 561 - // user means to use each method For now `meshgrid` is last because Moses is the only one that uses it 466 466 + ns->log_level = debug_get_log_option_ns_log(); 467 467 + NS_DEBUG(ns, "Called!"); 562 468 563 563 - bool found_config_wrap = false; 564 564 - for (int i = 0; i < number_wrap; i++) { 565 565 - if (wrap_func_ptr[i](ns)) { // wrap_func_ptr[i](ns) is a function call! 566 566 - U_LOG_I("North Star: Using config wrap %i", i); 567 567 - found_config_wrap = true; 568 568 - break; 569 569 - } 570 570 - } // This will segfault at function ?? if you use GDB and the length is wrong. 469 469 + ns->base.hmd->distortion.fov[0] = ns->config.fov[0]; 470 470 + ns->base.hmd->distortion.fov[1] = ns->config.fov[1]; 571 471 572 572 - if (!found_config_wrap) { 573 573 - NS_INFO(ns, "North Star: Config file seems to be invalid."); 574 574 - goto cleanup; 575 575 - } 576 472 473 473 + ns->base.compute_distortion = ns_mesh_calc; 577 474 ns->base.update_inputs = ns_hmd_update_inputs; 578 475 ns->base.get_tracked_pose = ns_hmd_get_tracked_pose; 579 476 ns->base.get_view_poses = ns_hmd_get_view_poses; ··· 645 542 646 543 647 544 return &ns->base; 648 648 - 649 649 - cleanup: 650 650 - ns_hmd_destroy(&ns->base); 651 651 - return NULL; 652 545 }

+33 -46

src/xrt/drivers/north_star/ns_hmd.h

··· 29 29 30 30 /* 31 31 * 32 32 - * Printing functions. 33 33 - * 34 34 - */ 35 35 - 36 36 - #define NS_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) 37 37 - #define NS_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) 38 38 - #define NS_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) 39 39 - #define NS_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) 40 40 - #define NS_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) 41 41 - 42 42 - /* 43 43 - * 44 32 * 3D distortion structs 45 33 * Sometimes known as "v1", config file name is often "Calibration.json" 46 34 * ··· 54 42 struct ns_3d_optical_system; 55 43 56 44 /*! 57 57 - * Configuration information about the LMC or Rigel sensor according to the 58 58 - * configuration file. 59 59 - * 60 60 - * @ingroup drv_ns 61 61 - */ 62 62 - struct ns_3d_leap 63 63 - { 64 64 - char name[64]; 65 65 - char serial[64]; 66 66 - struct xrt_pose pose; 67 67 - }; 68 68 - 69 69 - /*! 70 45 * Distortion information about an eye parsed from the configuration file. 71 46 * 72 47 * @ingroup drv_ns ··· 81 56 82 57 struct xrt_pose eye_pose; 83 58 59 59 + // This is more of a vec4 than a quat 84 60 struct xrt_quat camera_projection; 85 61 86 62 struct xrt_matrix_4x4 sphere_to_world_space; ··· 89 65 struct ns_optical_system *optical_system; 90 66 }; 91 67 92 92 - struct ns_3d_data 68 68 + struct ns_3d_values 93 69 { 94 70 struct ns_3d_eye eyes[2]; 95 95 - struct ns_3d_leap leap; 71 71 + }; 72 72 + 73 73 + enum ns_distortion_type 74 74 + { 75 75 + NS_DISTORTION_TYPE_INVALID, 76 76 + NS_DISTORTION_TYPE_GEOMETRIC_3D, 77 77 + NS_DISTORTION_TYPE_POLYNOMIAL_2D, 78 78 + NS_DISTORTION_TYPE_MOSES_MESHGRID, 79 79 + }; 80 80 + 81 81 + // The config json data gets dumped in here. 82 82 + // In general, `target_builder_north_star.c` sets up tracking, and `ns_hmd.c` sets up distortion/optics. 83 83 + struct ns_optics_config 84 84 + { 85 85 + struct xrt_pose head_pose_to_eye[2]; // left, right 86 86 + struct xrt_fov fov[2]; // left,right 87 87 + 88 88 + 89 89 + 90 90 + enum ns_distortion_type distortion_type; 91 91 + union { 92 92 + struct ns_3d_values dist_3d; 93 93 + struct u_ns_p2d_values dist_p2d; 94 94 + struct u_ns_meshgrid_values dist_meshgrid; 95 95 + }; 96 96 }; 97 97 + 97 98 98 99 /*! 99 100 * Information about the whole North Star headset. ··· 106 107 { 107 108 struct xrt_device base; 108 109 struct xrt_space_relation no_tracker_relation; 109 109 - const char *config_path; 110 110 - struct cJSON *config_json; 111 111 - struct xrt_pose head_pose_to_eye[2]; // left, right 112 112 - 113 113 - void (*free_distortion_values)(struct ns_hmd *hmd); 114 114 - union { 115 115 - struct ns_3d_data dist_3d; 116 116 - struct u_ns_p2d_values dist_p2d; 117 117 - struct u_ns_meshgrid_values dist_meshgrid; 118 118 - }; 110 110 + const cJSON *config_json; 111 111 + struct ns_optics_config config; 119 112 120 113 enum u_logging_level log_level; 121 114 }; ··· 139 132 140 133 141 134 /*! 142 142 - * Create a North Star hmd. 143 143 - * 144 144 - * @ingroup drv_ns 145 145 - */ 146 146 - struct xrt_device * 147 147 - ns_hmd_create(const char *config_path); 148 148 - 149 149 - 150 150 - /*! 151 135 * Convert the display UV to the render UV using the distortion mesh. 152 136 * 153 137 * @ingroup drv_ns ··· 158 142 159 143 struct ns_optical_system * 160 144 ns_3d_create_optical_system(struct ns_3d_eye *eye); 145 145 + 146 146 + void 147 147 + ns_3d_free_optical_system(struct ns_optical_system **system); 161 148 162 149 163 150 #ifdef __cplusplus

+6 -3

src/xrt/drivers/north_star/ns_interface.h

··· 9 9 */ 10 10 11 11 #pragma once 12 12 + #include "util/u_json.h" 13 13 + #include "xrt/xrt_device.h" 12 14 13 15 #ifdef __cplusplus 14 16 extern "C" { ··· 23 25 */ 24 26 25 27 /*! 26 26 - * Create a probe for NS devices. 28 28 + * Creates a North Star HMD. 27 29 * 28 30 * @ingroup drv_ns 29 31 */ 30 30 - struct xrt_auto_prober * 31 31 - ns_create_auto_prober(void); 32 32 + 33 33 + struct xrt_device * 34 34 + ns_hmd_create(const cJSON *config_json); 32 35 33 36 /*! 34 37 * @dir drivers/north_star

-83

src/xrt/drivers/north_star/ns_prober.c

··· 1 1 - // Copyright 2019-2020, Collabora, Ltd. 2 2 - // Copyright 2020, Nova King. 3 3 - // SPDX-License-Identifier: BSL-1.0 4 4 - /*! 5 5 - * @file 6 6 - * @brief North Star prober code. 7 7 - * @author Nova King <technobaboo@gmail.com> 8 8 - * @author Jakob Bornecrantz <jakob@collabora.com> 9 9 - * @ingroup drv_ns 10 10 - */ 11 11 - 12 12 - #include <stdio.h> 13 13 - #include <stdlib.h> 14 14 - 15 15 - #include "xrt/xrt_prober.h" 16 16 - 17 17 - #include "util/u_misc.h" 18 18 - #include "util/u_debug.h" 19 19 - 20 20 - #include "ns_interface.h" 21 21 - #include "ns_hmd.h" 22 22 - 23 23 - 24 24 - DEBUG_GET_ONCE_OPTION(ns_config_path, "NS_CONFIG_PATH", NULL) 25 25 - 26 26 - /*! 27 27 - * @implements xrt_auto_prober 28 28 - */ 29 29 - struct ns_prober 30 30 - { 31 31 - struct xrt_auto_prober base; 32 32 - const char *config_path; 33 33 - }; 34 34 - 35 35 - //! @private @memberof ns_prober 36 36 - static inline struct ns_prober * 37 37 - ns_prober(struct xrt_auto_prober *p) 38 38 - { 39 39 - return (struct ns_prober *)p; 40 40 - } 41 41 - 42 42 - //! @public @memberof ns_prober 43 43 - static void 44 44 - ns_prober_destroy(struct xrt_auto_prober *p) 45 45 - { 46 46 - struct ns_prober *nsp = ns_prober(p); 47 47 - 48 48 - free(nsp); 49 49 - } 50 50 - 51 51 - //! @public @memberof ns_prober 52 52 - static int 53 53 - ns_prober_autoprobe(struct xrt_auto_prober *xap, 54 54 - cJSON *attached_data, 55 55 - bool no_hmds, 56 56 - struct xrt_prober *xp, 57 57 - struct xrt_device **out_xdevs) 58 58 - { 59 59 - struct ns_prober *nsp = ns_prober(xap); 60 60 - 61 61 - if (no_hmds) { 62 62 - return 0; 63 63 - } 64 64 - 65 65 - if (nsp->config_path == NULL) { 66 66 - return 0; 67 67 - } 68 68 - 69 69 - out_xdevs[0] = ns_hmd_create(nsp->config_path); 70 70 - return 1; 71 71 - } 72 72 - 73 73 - struct xrt_auto_prober * 74 74 - ns_create_auto_prober() 75 75 - { 76 76 - struct ns_prober *nsp = U_TYPED_CALLOC(struct ns_prober); 77 77 - nsp->base.name = "northstar"; 78 78 - nsp->base.destroy = ns_prober_destroy; 79 79 - nsp->base.lelo_dallas_autoprobe = ns_prober_autoprobe; 80 80 - nsp->config_path = debug_get_option_ns_config_path(); 81 81 - 82 82 - return &nsp->base; 83 83 - }

+5

src/xrt/targets/common/CMakeLists.txt

··· 37 37 target_sources(target_lists PRIVATE target_builder_simulavr.c) 38 38 endif() 39 39 40 40 + if(XRT_BUILD_DRIVER_NS) 41 41 + target_sources(target_lists PRIVATE target_builder_north_star.c) 42 42 + target_link_libraries(target_lists PRIVATE drv_ns) 43 43 + endif() 44 44 + 40 45 ### 41 46 # Drivers 42 47 #

+9

src/xrt/targets/common/target_builder_interface.h

··· 32 32 #define T_BUILDER_SIMULAVR 33 33 #endif 34 34 35 35 + #if defined(XRT_BUILD_DRIVER_NS) 36 36 + #define T_BUILDER_NS 37 37 + #endif 38 38 + 35 39 // Always enabled. 36 40 #define T_BUILDER_LEGACY 37 41 ··· 81 85 struct xrt_builder * 82 86 t_builder_simula_create(void); 83 87 #endif 88 88 + 89 89 + #ifdef T_BUILDER_NS 90 90 + struct xrt_builder * 91 91 + t_builder_north_star_create(void); 92 92 + #endif

+581

src/xrt/targets/common/target_builder_north_star.c

··· 1 1 + // Copyright 2022, Collabora, Ltd. 2 2 + // SPDX-License-Identifier: BSL-1.0 3 3 + /*! 4 4 + * @file 5 5 + * @brief System builder for North Star headsets 6 6 + * @author Nova King <technobaboo@gmail.com> 7 7 + * @author Jakob Bornecrantz <jakob@collabora.com> 8 8 + * @author Moses Turner <moses@collabora.com> 9 9 + * @ingroup xrt_iface 10 10 + */ 11 11 + 12 12 + #include "math/m_api.h" 13 13 + #include "math/m_space.h" 14 14 + #include "multi_wrapper/multi.h" 15 15 + #include "realsense/rs_interface.h" 16 16 + #include "tracking/t_hand_tracking.h" 17 17 + #include "tracking/t_tracking.h" 18 18 + 19 19 + #include "xrt/xrt_config_drivers.h" 20 20 + #include "xrt/xrt_device.h" 21 21 + #include "xrt/xrt_prober.h" 22 22 + 23 23 + #include "util/u_builders.h" 24 24 + #include "util/u_config_json.h" 25 25 + #include "util/u_debug.h" 26 26 + #include "util/u_device.h" 27 27 + #include "util/u_sink.h" 28 28 + #include "util/u_system_helpers.h" 29 29 + #include "util/u_file.h" 30 30 + #include "util/u_pretty_print.h" 31 31 + 32 32 + #include "target_builder_interface.h" 33 33 + 34 34 + #include "north_star/ns_interface.h" 35 35 + 36 36 + #ifdef XRT_BUILD_DRIVER_ULV2 37 37 + #include "ultraleap_v2/ulv2_interface.h" 38 38 + #endif 39 39 + 40 40 + #ifdef XRT_BUILD_DRIVER_REALSENSE 41 41 + #include "realsense/rs_interface.h" 42 42 + #endif 43 43 + 44 44 + #ifdef XRT_BUILD_DRIVER_DEPTHAI 45 45 + #include "depthai/depthai_interface.h" 46 46 + #endif 47 47 + 48 48 + #ifdef XRT_BUILD_DRIVER_TWRAP 49 49 + #include "twrap/twrap_interface.h" 50 50 + #endif 51 51 + 52 52 + #ifdef XRT_BUILD_DRIVER_HANDTRACKING 53 53 + #include "ht/ht_interface.h" 54 54 + #endif 55 55 + 56 56 + #include "ht_ctrl_emu/ht_ctrl_emu_interface.h" 57 57 + 58 58 + #include "xrt/xrt_frameserver.h" 59 59 + #include "xrt/xrt_results.h" 60 60 + #include "xrt/xrt_tracking.h" 61 61 + 62 62 + #include <assert.h> 63 63 + #include "math/m_mathinclude.h" 64 64 + 65 65 + DEBUG_GET_ONCE_OPTION(ns_config_path, "NS_CONFIG_PATH", NULL) 66 66 + DEBUG_GET_ONCE_LOG_OPTION(ns_log, "NS_LOG", U_LOGGING_WARN) 67 67 + 68 68 + 69 69 + #define NS_TRACE(...) U_LOG_IFL_T(debug_get_log_option_ns_log(), __VA_ARGS__) 70 70 + #define NS_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_ns_log(), __VA_ARGS__) 71 71 + #define NS_INFO(...) U_LOG_IFL_I(debug_get_log_option_ns_log(), __VA_ARGS__) 72 72 + #define NS_WARN(...) U_LOG_IFL_W(debug_get_log_option_ns_log(), __VA_ARGS__) 73 73 + #define NS_ERROR(...) U_LOG_IFL_E(debug_get_log_option_ns_log(), __VA_ARGS__) 74 74 + 75 75 + static const char *driver_list[] = { 76 76 + "north_star", 77 77 + }; 78 78 + 79 79 + struct ns_ultraleap_device 80 80 + { 81 81 + bool active; 82 82 + 83 83 + // Users input `P_middleofeyes_to_trackingcenter_oxr`, and we invert it into this pose. 84 84 + // It's a lot simpler to (and everybody does) care about the transform from the eyes center to the device, 85 85 + // but tracking overrides care about this value. 86 86 + struct xrt_pose P_trackingcenter_to_middleofeyes_oxr; 87 87 + }; 88 88 + 89 89 + struct ns_depthai_device 90 90 + { 91 91 + bool active; 92 92 + struct xrt_pose P_imu_to_left_camera_basalt; 93 93 + struct xrt_pose P_middleofeyes_to_imu_oxr; 94 94 + }; 95 95 + 96 96 + struct ns_t265 97 97 + { 98 98 + bool active; 99 99 + struct xrt_pose P_middleofeyes_to_trackingcenter_oxr; 100 100 + }; 101 101 + 102 102 + struct ns_builder 103 103 + { 104 104 + struct xrt_builder base; 105 105 + 106 106 + const char *config_path; 107 107 + cJSON *config_json; 108 108 + 109 109 + struct ns_ultraleap_device ultraleap_device; 110 110 + struct ns_depthai_device depthai_device; 111 111 + struct ns_t265 t265; 112 112 + }; 113 113 + 114 114 + 115 115 + static bool 116 116 + ns_config_load(struct ns_builder *nsb) 117 117 + { 118 118 + const char *file_content = u_file_read_content_from_path(nsb->config_path); 119 119 + if (file_content == NULL) { 120 120 + U_LOG_E("The file at \"%s\" was unable to load. Either there wasn't a file there or it was empty.", 121 121 + nsb->config_path); 122 122 + return false; 123 123 + } 124 124 + 125 125 + // leaks? 126 126 + cJSON *config_json = cJSON_Parse(file_content); 127 127 + 128 128 + if (config_json == NULL) { 129 129 + const char *error_ptr = cJSON_GetErrorPtr(); 130 130 + U_LOG_E("The JSON file at path \"%s\" was unable to parse", nsb->config_path); 131 131 + if (error_ptr != NULL) { 132 132 + U_LOG_E("because of an error before %s", error_ptr); 133 133 + } 134 134 + free((void *)file_content); 135 135 + return false; 136 136 + } 137 137 + nsb->config_json = config_json; 138 138 + free((void *)file_content); 139 139 + return true; 140 140 + } 141 141 + 142 142 + static void 143 143 + ns_tracking_config_parse_depthai(struct ns_builder *nsb, bool *out_config_valid) 144 144 + { 145 145 + *out_config_valid = true; 146 146 + const cJSON *root = u_json_get(nsb->config_json, "depthaiDevice"); 147 147 + 148 148 + if (root == NULL) { 149 149 + *out_config_valid = true; 150 150 + // not invalid, but doesn't exist. active is not set and won't be used 151 151 + return; 152 152 + } 153 153 + 154 154 + *out_config_valid = *out_config_valid && // 155 155 + u_json_get_bool(u_json_get(root, "active"), &nsb->depthai_device.active); 156 156 + 157 157 + *out_config_valid = *out_config_valid && // 158 158 + u_json_get_pose(u_json_get(root, "P_imu_to_left_camera_basalt"), 159 159 + &nsb->depthai_device.P_imu_to_left_camera_basalt); 160 160 + 161 161 + *out_config_valid = *out_config_valid && // 162 162 + u_json_get_pose(u_json_get(root, "P_middleofeyes_to_imu_oxr"), 163 163 + &nsb->depthai_device.P_middleofeyes_to_imu_oxr); 164 164 + } 165 165 + 166 166 + static void 167 167 + ns_tracking_config_parse_ultraleap(struct ns_builder *nsb, bool *out_config_valid) 168 168 + { 169 169 + *out_config_valid = true; 170 170 + const cJSON *root = u_json_get(nsb->config_json, "leapTracker"); 171 171 + if (root == NULL) { 172 172 + // not invalid, but doesn't exist. active is not set and won't be used 173 173 + return; 174 174 + } 175 175 + 176 176 + struct xrt_pose P_middleofeyes_to_trackingcenter_oxr; 177 177 + 178 178 + struct xrt_pose localpose_unity = XRT_POSE_IDENTITY; 179 179 + 180 180 + if (u_json_get_pose_permissive(u_json_get(root, "localPose"), &localpose_unity)) { 181 181 + NS_INFO( 182 182 + "Found key `localPose` in your Ultraleap tracker config. Converting this from Unity's coordinate " 183 183 + "space to OpenXR's coordinate space."); 184 184 + NS_INFO( 185 185 + "If you just want to specify the offset in OpenXR coordinates, use key " 186 186 + "`P_middleofeyes_to_trackingcenter` instead."); 187 187 + 188 188 + 189 189 + // This is the conversion from Unity to OpenXR coordinates. 190 190 + // Unity: X+ Right; Y+ Up; Z+ Forward 191 191 + // OpenXR: X+ Right; Y+ Up; Z- Forward 192 192 + // Check tests_quat_change_of_basis to understand the quaternion element negations. 193 193 + P_middleofeyes_to_trackingcenter_oxr.position.x = localpose_unity.position.x; 194 194 + P_middleofeyes_to_trackingcenter_oxr.position.y = localpose_unity.position.y; 195 195 + P_middleofeyes_to_trackingcenter_oxr.position.z = -localpose_unity.position.z; 196 196 + 197 197 + 198 198 + P_middleofeyes_to_trackingcenter_oxr.orientation.x = localpose_unity.orientation.x; 199 199 + P_middleofeyes_to_trackingcenter_oxr.orientation.y = localpose_unity.orientation.y; 200 200 + P_middleofeyes_to_trackingcenter_oxr.orientation.z = -localpose_unity.orientation.z; 201 201 + P_middleofeyes_to_trackingcenter_oxr.orientation.w = -localpose_unity.orientation.w; 202 202 + 203 203 + *out_config_valid = *out_config_valid && true; 204 204 + } else { 205 205 + *out_config_valid = *out_config_valid && // 206 206 + u_json_get_pose(u_json_get(root, "P_middleofeyes_to_trackingcenter_oxr"), 207 207 + &P_middleofeyes_to_trackingcenter_oxr); 208 208 + } 209 209 + 210 210 + math_pose_invert(&P_middleofeyes_to_trackingcenter_oxr, 211 211 + &nsb->ultraleap_device.P_trackingcenter_to_middleofeyes_oxr); 212 212 + nsb->ultraleap_device.active = true; 213 213 + } 214 214 + 215 215 + static void 216 216 + ns_tracking_config_parse_t265(struct ns_builder *nsb, bool *out_config_valid) 217 217 + { 218 218 + *out_config_valid = true; 219 219 + const cJSON *root = u_json_get(nsb->config_json, "t265"); 220 220 + 221 221 + if (root == NULL) { 222 222 + // not invalid, but doesn't exist. active is not set and won't be used 223 223 + return; 224 224 + } 225 225 + 226 226 + *out_config_valid = *out_config_valid && // 227 227 + u_json_get_bool(u_json_get(root, "active"), &nsb->t265.active); 228 228 + 229 229 + *out_config_valid = *out_config_valid && // 230 230 + u_json_get_pose(u_json_get(root, "P_middleofeyes_to_trackingcenter_oxr"), 231 231 + &nsb->t265.P_middleofeyes_to_trackingcenter_oxr); 232 232 + } 233 233 + 234 234 + void 235 235 + ns_compute_depthai_ht_offset(struct xrt_pose *P_imu_to_left_camera_basalt, struct xrt_pose *out_pose) 236 236 + { 237 237 + struct xrt_pose deg180 = XRT_POSE_IDENTITY; 238 238 + 239 239 + 240 240 + struct xrt_vec3 plusx = {1, 0, 0}; 241 241 + struct xrt_vec3 plusz = {0, 0, -1}; 242 242 + 243 243 + math_quat_from_plus_x_z(&plusx, &plusz, &deg180.orientation); 244 244 + 245 245 + struct xrt_relation_chain xrc = {0}; 246 246 + 247 247 + // Remember, relation_chains are backwards. 248 248 + // This comes "after" P_imo_to_left_cam_basalt, and rotates from the usual camera coordinate space (+Y down +Z 249 249 + // forward) to OpenXR/hand tracking's output coordinate space (+Y up +Z backwards) 250 250 + m_relation_chain_push_pose_if_not_identity(&xrc, &deg180); 251 251 + // This comes "first" and goes from the head tracking's output space (IMU) to where the left camera is, 252 252 + // according to the config file. 253 253 + m_relation_chain_push_pose_if_not_identity(&xrc, P_imu_to_left_camera_basalt); 254 254 + 255 255 + struct xrt_space_relation rel = {0}; 256 256 + 257 257 + m_relation_chain_resolve(&xrc, &rel); 258 258 + 259 259 + 260 260 + math_pose_invert(&rel.pose, out_pose); 261 261 + } 262 262 + 263 263 + #ifdef XRT_BUILD_DRIVER_DEPTHAI 264 264 + static xrt_result_t 265 265 + ns_setup_depthai_device(struct ns_builder *nsb, 266 266 + struct u_system_devices *usysd, 267 267 + struct xrt_device **out_hand_device, 268 268 + struct xrt_device **out_head_device) 269 269 + { 270 270 + struct depthai_slam_startup_settings settings = {0}; 271 271 + 272 272 + settings.frames_per_second = 60; 273 273 + settings.half_size_ov9282 = true; 274 274 + settings.want_cameras = true; 275 275 + settings.want_imu = true; 276 276 + 277 277 + struct xrt_fs *the_fs = depthai_fs_slam(&usysd->xfctx, &settings); 278 278 + 279 279 + if (the_fs == NULL) { 280 280 + return XRT_ERROR_DEVICE_CREATION_FAILED; 281 281 + } 282 282 + 283 283 + struct t_stereo_camera_calibration *calib = NULL; 284 284 + depthai_fs_get_stereo_calibration(the_fs, &calib); 285 285 + 286 286 + 287 287 + 288 288 + struct xrt_slam_sinks *hand_sinks = NULL; 289 289 + 290 290 + struct t_camera_extra_info extra_camera_info; 291 291 + extra_camera_info.views[0].camera_orientation = CAMERA_ORIENTATION_180; 292 292 + extra_camera_info.views[1].camera_orientation = CAMERA_ORIENTATION_180; 293 293 + 294 294 + extra_camera_info.views[0].boundary_type = HT_IMAGE_BOUNDARY_NONE; 295 295 + extra_camera_info.views[1].boundary_type = HT_IMAGE_BOUNDARY_NONE; 296 296 + 297 297 + int create_status = ht_device_create(&usysd->xfctx, // 298 298 + calib, // 299 299 + HT_ALGORITHM_MERCURY, // 300 300 + extra_camera_info, // 301 301 + &hand_sinks, // 302 302 + out_hand_device); 303 303 + t_stereo_camera_calibration_reference(&calib, NULL); 304 304 + if (create_status != 0) { 305 305 + return XRT_ERROR_DEVICE_CREATION_FAILED; 306 306 + } 307 307 + 308 308 + struct xrt_slam_sinks *slam_sinks = NULL; 309 309 + twrap_slam_create_device(&usysd->xfctx, XRT_DEVICE_DEPTHAI, &slam_sinks, out_head_device); 310 310 + 311 311 + struct xrt_slam_sinks entry_sinks = {0}; 312 312 + struct xrt_frame_sink *entry_left_sink = NULL; 313 313 + struct xrt_frame_sink *entry_right_sink = NULL; 314 314 + 315 315 + u_sink_split_create(&usysd->xfctx, slam_sinks->left, hand_sinks->left, &entry_left_sink); 316 316 + u_sink_split_create(&usysd->xfctx, slam_sinks->right, hand_sinks->right, &entry_right_sink); 317 317 + 318 318 + 319 319 + entry_sinks = (struct xrt_slam_sinks){ 320 320 + .left = entry_left_sink, 321 321 + .right = entry_right_sink, 322 322 + .imu = slam_sinks->imu, 323 323 + .gt = slam_sinks->gt, 324 324 + }; 325 325 + 326 326 + struct xrt_slam_sinks dummy_slam_sinks = {0}; 327 327 + dummy_slam_sinks.imu = entry_sinks.imu; 328 328 + 329 329 + u_sink_force_genlock_create(&usysd->xfctx, entry_sinks.left, entry_sinks.right, &dummy_slam_sinks.left, 330 330 + &dummy_slam_sinks.right); 331 331 + 332 332 + xrt_fs_slam_stream_start(the_fs, &dummy_slam_sinks); 333 333 + 334 334 + return XRT_SUCCESS; 335 335 + } 336 336 + #endif 337 337 + 338 338 + // Note: We're just checking for the config file's existence 339 339 + static xrt_result_t 340 340 + ns_estimate_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_builder_estimate *estimate) 341 341 + { 342 342 + struct ns_builder *nsb = (struct ns_builder *)xb; 343 343 + U_ZERO(estimate); 344 344 + 345 345 + nsb->config_path = debug_get_option_ns_config_path(); 346 346 + 347 347 + if (nsb->config_path == NULL) { 348 348 + return XRT_SUCCESS; 349 349 + } 350 350 + 351 351 + 352 352 + struct xrt_prober_device **xpdevs = NULL; 353 353 + size_t xpdev_count = 0; 354 354 + xrt_result_t xret = XRT_SUCCESS; 355 355 + 356 356 + // Lock the device list 357 357 + xret = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count); 358 358 + if (xret != XRT_SUCCESS) { 359 359 + return xret; 360 360 + } 361 361 + 362 362 + estimate->maybe.head = true; 363 363 + estimate->certain.head = true; 364 364 + 365 365 + bool hand_tracking = false; 366 366 + 367 367 + #ifdef XRT_BUILD_DRIVER_ULV2 368 368 + hand_tracking = 369 369 + hand_tracking || u_builder_find_prober_device(xpdevs, xpdev_count, ULV2_VID, ULV2_PID, XRT_BUS_TYPE_USB); 370 370 + #endif 371 371 + 372 372 + #ifdef XRT_BUILD_DRIVER_REALSENSE 373 373 + estimate->certain.dof6 = 374 374 + estimate->certain.dof6 || u_builder_find_prober_device(xpdevs, xpdev_count, REALSENSE_MOVIDIUS_VID, 375 375 + REALSENSE_MOVIDIUS_PID, XRT_BUS_TYPE_USB); 376 376 + estimate->certain.dof6 = 377 377 + estimate->certain.dof6 || u_builder_find_prober_device(xpdevs, xpdev_count, // 378 378 + REALSENSE_TM2_VID, REALSENSE_TM2_PID, // 379 379 + XRT_BUS_TYPE_USB); 380 380 + #endif 381 381 + 382 382 + #ifdef XRT_BUILD_DRIVER_DEPTHAI 383 383 + bool depthai = u_builder_find_prober_device(xpdevs, xpdev_count, DEPTHAI_VID, DEPTHAI_PID, XRT_BUS_TYPE_USB); 384 384 + #ifdef XRT_FEATURE_SLAM 385 385 + estimate->certain.dof6 = estimate->certain.dof6 || depthai; 386 386 + #endif 387 387 + #ifdef XRT_BUILD_DRIVER_HANDTRACKING 388 388 + hand_tracking = hand_tracking || depthai; 389 389 + #endif 390 390 + #endif 391 391 + 392 392 + estimate->certain.left = estimate->certain.right = estimate->maybe.left = estimate->maybe.right = hand_tracking; 393 393 + 394 394 + 395 395 + 396 396 + xret = xrt_prober_unlock_list(xp, &xpdevs); 397 397 + if (xret != XRT_SUCCESS) { 398 398 + return xret; 399 399 + } 400 400 + 401 401 + return XRT_SUCCESS; 402 402 + } 403 403 + 404 404 + 405 405 + 406 406 + static xrt_result_t 407 407 + ns_open_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_system_devices **out_xsysd) 408 408 + { 409 409 + struct ns_builder *nsb = (struct ns_builder *)xb; 410 410 + 411 411 + 412 412 + struct u_system_devices *usysd = u_system_devices_allocate(); 413 413 + xrt_result_t result = XRT_SUCCESS; 414 414 + 415 415 + if (out_xsysd == NULL || *out_xsysd != NULL) { 416 416 + NS_ERROR("Invalid output system pointer"); 417 417 + result = XRT_ERROR_DEVICE_CREATION_FAILED; 418 418 + goto end; 419 419 + } 420 420 + 421 421 + 422 422 + bool load_success = ns_config_load(nsb); 423 423 + if (!load_success) { 424 424 + result = XRT_ERROR_DEVICE_CREATION_FAILED; 425 425 + goto end; 426 426 + } 427 427 + 428 428 + struct xrt_device *ns_hmd = ns_hmd_create(nsb->config_json); 429 429 + if (ns_hmd == NULL) { 430 430 + result = XRT_ERROR_DEVICE_CREATION_FAILED; 431 431 + goto end; 432 432 + } 433 433 + 434 434 + 435 435 + bool config_valid = true; 436 436 + ns_tracking_config_parse_depthai(nsb, &config_valid); 437 437 + if (!config_valid) { 438 438 + NS_ERROR("DepthAI device config was invalid!"); 439 439 + } 440 440 + 441 441 + ns_tracking_config_parse_ultraleap(nsb, &config_valid); 442 442 + if (!config_valid) { 443 443 + NS_ERROR("Leap device config was invalid!"); 444 444 + } 445 445 + 446 446 + ns_tracking_config_parse_t265(nsb, &config_valid); 447 447 + if (!config_valid) { 448 448 + NS_ERROR("T265 device config was invalid!"); 449 449 + } 450 450 + 451 451 + struct xrt_device *hand_device = NULL; 452 452 + struct xrt_device *slam_device = NULL; 453 453 + 454 454 + struct xrt_pose head_offset = XRT_POSE_IDENTITY; 455 455 + 456 456 + // True if hand tracker is parented to the head tracker (DepthAI), false if hand tracker is parented to 457 457 + // middle-of-eyes (Ultraleap etc.) 458 458 + bool hand_parented_to_head_tracker = true; 459 459 + struct xrt_pose hand_offset = XRT_POSE_IDENTITY; 460 460 + 461 461 + // bool got_head_tracker = false; 462 462 + 463 463 + 464 464 + 465 465 + // For now we use DepthAI for head tracking + hand tracking OR t265 for head + ultraleap for hand. 466 466 + // Mixing systems with more atomicity coming later™️ 467 467 + if (nsb->depthai_device.active) { 468 468 + #ifdef XRT_BUILD_DRIVER_DEPTHAI 469 469 + NS_INFO("Using DepthAI device!"); 470 470 + ns_setup_depthai_device(nsb, usysd, &hand_device, &slam_device); 471 471 + head_offset = nsb->depthai_device.P_middleofeyes_to_imu_oxr; 472 472 + ns_compute_depthai_ht_offset(&nsb->depthai_device.P_imu_to_left_camera_basalt, &hand_offset); 473 473 + // got_head_tracker = true; 474 474 + #else 475 475 + NS_ERROR("DepthAI head+hand tracker specified in config but DepthAI support was not compiled in!"); 476 476 + #endif 477 477 + 478 478 + 479 479 + } else { 480 480 + if (nsb->t265.active) { 481 481 + #ifdef XRT_BUILD_DRIVER_REALSENSE 482 482 + slam_device = rs_create_tracked_device_internal_slam(); 483 483 + head_offset = nsb->t265.P_middleofeyes_to_trackingcenter_oxr; 484 484 + // got_head_tracker = true; 485 485 + #else 486 486 + NS_ERROR( 487 487 + "Realsense head tracker specified in config but Realsense support was not compiled in!"); 488 488 + #endif 489 489 + } 490 490 + if (nsb->ultraleap_device.active) { 491 491 + #ifdef XRT_BUILD_DRIVER_ULV2 492 492 + ulv2_create_device(&hand_device); 493 493 + hand_offset = nsb->ultraleap_device.P_trackingcenter_to_middleofeyes_oxr; 494 494 + hand_parented_to_head_tracker = false; 495 495 + #else 496 496 + NS_ERROR( 497 497 + "Ultraleap hand tracker specified in config but Ultraleap support was not compiled in!"); 498 498 + #endif 499 499 + } 500 500 + } 501 501 + 502 502 + 503 503 + 504 504 + struct xrt_device *head_wrap = NULL; 505 505 + 506 506 + if (slam_device != NULL) { 507 507 + usysd->base.xdevs[usysd->base.xdev_count++] = slam_device; 508 508 + head_wrap = multi_create_tracking_override(XRT_TRACKING_OVERRIDE_DIRECT, ns_hmd, slam_device, 509 509 + XRT_INPUT_GENERIC_TRACKER_POSE, &head_offset); 510 510 + } else { 511 511 + // No head tracker, no head tracking. 512 512 + head_wrap = ns_hmd; 513 513 + } 514 514 + 515 515 + usysd->base.xdevs[usysd->base.xdev_count++] = head_wrap; 516 516 + usysd->base.roles.head = head_wrap; 517 517 + 518 518 + if (hand_device != NULL) { 519 519 + // note: hand_parented_to_head_tracker is always false when slam_device is NULL 520 520 + struct xrt_device *hand_wrap = multi_create_tracking_override( 521 521 + XRT_TRACKING_OVERRIDE_ATTACHED, hand_device, 522 522 + hand_parented_to_head_tracker ? slam_device : head_wrap, 523 523 + hand_parented_to_head_tracker ? XRT_INPUT_GENERIC_TRACKER_POSE : XRT_INPUT_GENERIC_HEAD_POSE, 524 524 + &hand_offset); 525 525 + struct xrt_device *two_hands[2]; 526 526 + cemu_devices_create(head_wrap, hand_wrap, two_hands); 527 527 + 528 528 + 529 529 + // usysd->base.xdev_count = 0; 530 530 + usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[0]; 531 531 + usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[1]; 532 532 + 533 533 + 534 534 + usysd->base.roles.hand_tracking.left = two_hands[0]; 535 535 + usysd->base.roles.hand_tracking.right = two_hands[1]; 536 536 + 537 537 + usysd->base.roles.left = two_hands[0]; 538 538 + usysd->base.roles.right = two_hands[1]; 539 539 + } 540 540 + 541 541 + 542 542 + 543 543 + end: 544 544 + if (result == XRT_SUCCESS) { 545 545 + *out_xsysd = &usysd->base; 546 546 + } else { 547 547 + u_system_devices_destroy(&usysd); 548 548 + } 549 549 + if (nsb->config_json != NULL) { 550 550 + cJSON_Delete(nsb->config_json); 551 551 + } 552 552 + 553 553 + return result; 554 554 + } 555 555 + 556 556 + static void 557 557 + ns_destroy(struct xrt_builder *xb) 558 558 + { 559 559 + free(xb); 560 560 + } 561 561 + 562 562 + /* 563 563 + * 564 564 + * 'Exported' functions. 565 565 + * 566 566 + */ 567 567 + 568 568 + struct xrt_builder * 569 569 + t_builder_north_star_create(void) 570 570 + { 571 571 + struct ns_builder *sb = U_TYPED_CALLOC(struct ns_builder); 572 572 + sb->base.estimate_system = ns_estimate_system; 573 573 + sb->base.open_system = ns_open_system; 574 574 + sb->base.destroy = ns_destroy; 575 575 + sb->base.identifier = "north_star"; 576 576 + sb->base.name = "North Star headset"; 577 577 + sb->base.driver_identifiers = driver_list; 578 578 + sb->base.driver_identifier_count = ARRAY_SIZE(driver_list); 579 579 + 580 580 + return &sb->base; 581 581 + }

+3 -9

src/xrt/targets/common/target_lists.c

··· 28 28 #include "ohmd/oh_interface.h" 29 29 #endif 30 30 31 31 - #ifdef XRT_BUILD_DRIVER_NS 32 32 - #include "north_star/ns_interface.h" 33 33 - #endif 34 34 - 35 31 #ifdef XRT_BUILD_DRIVER_PSMV 36 32 #include "psmv/psmv_interface.h" 37 33 #endif ··· 108 104 t_builder_remote_create, 109 105 #endif // T_BUILDER_REMOTE 110 106 107 107 + #ifdef T_BUILDER_NS 108 108 + t_builder_north_star_create, 109 109 + #endif 111 110 #ifdef T_BUILDER_LEGACY 112 111 t_builder_legacy_create, 113 112 #endif // T_BUILDER_LEGACY ··· 180 179 #ifdef XRT_BUILD_DRIVER_OHMD 181 180 // OpenHMD almost as the end as we want to override it with native drivers. 182 181 oh_create_auto_prober, 183 183 - #endif 184 184 - 185 185 - #ifdef XRT_BUILD_DRIVER_NS 186 186 - // North star driver here for now. 187 187 - ns_create_auto_prober, 188 182 #endif 189 183 190 184 #ifdef XRT_BUILD_DRIVER_ANDROID