mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-28 18:46:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/ns: Switch NS driver to builders

Browse source
This commit is contained in:
Moses Turner 2022-09-21 07:11:02 -05:00
parent 6fdd790da0
commit 45e52dee8f
9 changed files with 788 additions and 400 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
src/xrt/drivers/CMakeLists.txt
View file

@ -105,7 +105,6 @@ if(XRT_BUILD_DRIVER_NS)
north_star/ns_hmd.h
north_star/ns_hmd.c
north_star/ns_interface.h
north_star/ns_prober.c
)
target_link_libraries(drv_ns PRIVATE xrt-interfaces aux_math xrt-external-cjson)
list(APPEND ENABLED_HEADSET_DRIVERS ns)

409
src/xrt/drivers/north_star/ns_hmd.c
View file

@ -32,14 +32,22 @@
DEBUG_GET_ONCE_LOG_OPTION(ns_log, "NS_LOG", U_LOGGING_INFO)
#define printf_pose(pose) \
printf("%f %f %f %f %f %f %f\n", pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, \
pose.orientation.y, pose.orientation.z, pose.orientation.w);
/*
*
* Printing functions.
*
*/
#define NS_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)
#define NS_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__)
#define NS_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__)
#define NS_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__)
#define NS_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__)
static float
try_get_ipd(struct ns_hmd *ns, const struct cJSON *json)
{ //
{
const char *things[] = {"baseline", "ipd", "IPD"};
bool done = false;
float out;
@ -50,7 +58,8 @@ try_get_ipd(struct ns_hmd *ns, const struct cJSON *json)
}
if (!done) {
NS_INFO(ns,
"No key `baseline (or ipd, or IPD)` in your config file. Guessing the IPD is 64 millimeters");
"No key `baseline` (or `ipd`, or `IPD`) in your config file. "
"Guessing the IPD is 64 millimeters");
out = 64.0f;
}
if (out > 250.0f) {
@ -68,7 +77,7 @@ try_get_ipd(struct ns_hmd *ns, const struct cJSON *json)
}
static void
try_get_fov(struct ns_hmd *ns, const struct cJSON *json, struct xrt_fov *left_fov, struct xrt_fov *right_fov)
try_get_fov(struct ns_hmd *ns, const struct cJSON *json, struct xrt_fov *out_left_fov, struct xrt_fov *out_right_fov)
{
const char *things[] = {"fov", "FOV"};
float out_float;
@ -105,108 +114,59 @@ good:
assert(fabsf(out_fov.angle_down) < M_PI_2);
assert(fabsf(out_fov.angle_left) < M_PI_2);
assert(fabsf(out_fov.angle_right) < M_PI_2);
memcpy(left_fov, &out_fov, sizeof(struct xrt_fov));
memcpy(right_fov, &out_fov, sizeof(struct xrt_fov));
*out_left_fov = out_fov;
*out_right_fov = out_fov;
}
/*
*
* "2D Polynomial" distortion; original implementation by Johnathon Zelstadt
* Sometimes known as "v2", filename is often NorthStarCalibration.json
*
*/
static bool
ns_p2d_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result)
{
struct ns_hmd *ns = ns_hmd(xdev);
return u_compute_distortion_ns_p2d(&ns->dist_p2d, view, u, v, result);
}
bool
ns_p2d_parse(struct ns_hmd *ns)
ns_p2d_parse(struct ns_hmd *ns, const cJSON *json)
{
struct xrt_pose temp_eyes_center_to_eye[2];
// convenience names
const struct cJSON *config_json = ns->config_json;
struct u_ns_p2d_values *values = &ns->config.dist_p2d;
// Note that x and y are flipped. We have to flip 'em at some point - the polynomial calibrator has a strange
// definition of x and y. "opencv treats column major over row major (as in, Y,X for image look up)" -Dr. Damo
if (u_json_get_float_array(u_json_get(config_json, "left_uv_to_rect_x"), ns->dist_p2d.y_coefficients_right,
16) != 16)
if (u_json_get_float_array(u_json_get(json, "left_uv_to_rect_x"), values->y_coefficients_right, 16) != 16)
goto cleanup_p2d;
if (u_json_get_float_array(u_json_get(config_json, "left_uv_to_rect_y"), ns->dist_p2d.x_coefficients_right,
16) != 16)
if (u_json_get_float_array(u_json_get(json, "left_uv_to_rect_y"), values->x_coefficients_right, 16) != 16)
goto cleanup_p2d;
if (u_json_get_float_array(u_json_get(config_json, "right_uv_to_rect_x"), ns->dist_p2d.y_coefficients_left,
16) != 16)
if (u_json_get_float_array(u_json_get(json, "right_uv_to_rect_x"), values->y_coefficients_left, 16) != 16)
goto cleanup_p2d;
if (u_json_get_float_array(u_json_get(config_json, "right_uv_to_rect_y"), ns->dist_p2d.x_coefficients_left,
16) != 16)
if (u_json_get_float_array(u_json_get(json, "right_uv_to_rect_y"), values->x_coefficients_left, 16) != 16)
goto cleanup_p2d;
// at this point, locked into using this distortion method - we can touch anything and not worry about side
// effects
float baseline = try_get_ipd(ns, config_json);
ns->config.distortion_type = NS_DISTORTION_TYPE_POLYNOMIAL_2D;
math_pose_identity(&temp_eyes_center_to_eye[0]);
math_pose_identity(&temp_eyes_center_to_eye[1]);
temp_eyes_center_to_eye[0].position.x = -baseline / 2;
temp_eyes_center_to_eye[1].position.x = baseline / 2;
float baseline = try_get_ipd(ns, json);
try_get_fov(ns, config_json, &ns->dist_p2d.fov[0], &ns->dist_p2d.fov[1]);
math_pose_identity(&ns->config.head_pose_to_eye[0]);
math_pose_identity(&ns->config.head_pose_to_eye[1]);
ns->config.head_pose_to_eye[0].position.x = -baseline / 2;
ns->config.head_pose_to_eye[1].position.x = baseline / 2;
memcpy(&ns->base.hmd->distortion.fov[0], &ns->dist_p2d.fov[0], sizeof(struct xrt_fov));
memcpy(&ns->base.hmd->distortion.fov[1], &ns->dist_p2d.fov[1], sizeof(struct xrt_fov));
try_get_fov(ns, json, &values->fov[0], &values->fov[1]);
ns->base.compute_distortion = &ns_p2d_mesh_calc;
memcpy(&ns->head_pose_to_eye, &temp_eyes_center_to_eye, sizeof(struct xrt_pose) * 2);
ns->config.fov[0] = values->fov[0];
ns->config.fov[1] = values->fov[1];
return true;
cleanup_p2d:
memset(&ns->dist_p2d, 0, sizeof(struct u_ns_p2d_values));
return false;
}
/*
*
* "Original 3D" undistortion, by Leap Motion
* Sometimes known as "v1", config file name is often "Calibration.json"
*
*/
static bool
ns_3d_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result)
{
struct ns_hmd *ns = ns_hmd(xdev);
struct ns_3d_data *data = &ns->dist_3d;
struct xrt_vec2 uv = {u, v};
struct xrt_vec2 warped_uv = {0.0f, 0.0f};
ns_3d_display_uv_to_render_uv(uv, &warped_uv, &data->eyes[view]);
result->r.x = warped_uv.x;
result->r.y = warped_uv.y;
result->g.x = warped_uv.x;
result->g.y = warped_uv.y;
result->b.x = warped_uv.x;
result->b.y = warped_uv.y;
return true;
}
static void
ns_3d_fov_calculate(struct xrt_fov *fov, struct xrt_quat projection)
ns_3d_fov_calculate(struct xrt_quat projection, struct xrt_fov *out_fov)
{
// Million thanks to Nico Zobernig for figuring this out
fov->angle_left = atanf(projection.x);
fov->angle_right = atanf(projection.y);
fov->angle_up = atanf(projection.z);
fov->angle_down = atanf(projection.w);
out_fov->angle_left = atanf(projection.x);
out_fov->angle_right = atanf(projection.y);
out_fov->angle_up = atanf(projection.z);
out_fov->angle_down = atanf(projection.w);
}
/*
@ -215,20 +175,10 @@ ns_3d_fov_calculate(struct xrt_fov *fov, struct xrt_quat projection)
*
*/
static bool
ns_3d_leap_parse(struct ns_3d_leap *leap, const struct cJSON *leap_data)
{
u_json_get_string_into_array(u_json_get(leap_data, "name"), leap->name, 64);
u_json_get_string_into_array(u_json_get(leap_data, "serial"), leap->serial, 64);
if (!u_json_get_vec3(u_json_get(u_json_get(leap_data, "localPose"), "position"), &leap->pose.position))
return false;
if (!u_json_get_quat(u_json_get(u_json_get(leap_data, "localPose"), "rotation"), &leap->pose.orientation))
return false;
return true;
}
static bool
ns_3d_eye_parse(struct ns_3d_eye *eye, const struct cJSON *eye_data)
ns_3d_eye_parse(struct ns_hmd *ns, struct ns_3d_eye *eye, const struct cJSON *eye_data)
{
if (!u_json_get_float(u_json_get(eye_data, "ellipseMinorAxis"), &eye->ellipse_minor_axis))
return false;
@ -259,86 +209,66 @@ ns_3d_eye_parse(struct ns_3d_eye *eye, const struct cJSON *eye_data)
}
bool
ns_3d_parse(struct ns_hmd *ns)
ns_3d_parse(struct ns_hmd *ns, const cJSON *json)
{
struct ns_3d_data *our_ns_3d_data = &ns->dist_3d;
struct ns_3d_values *values = &ns->config.dist_3d;
if (!ns_3d_eye_parse(&our_ns_3d_data->eyes[0], u_json_get(ns->config_json, "leftEye")))
if (!ns_3d_eye_parse(ns, &values->eyes[0], u_json_get(json, "leftEye")))
goto cleanup_l3d;
if (!ns_3d_eye_parse(&our_ns_3d_data->eyes[1], u_json_get(ns->config_json, "rightEye")))
goto cleanup_l3d;
if (!ns_3d_leap_parse(&our_ns_3d_data->leap, u_json_get(ns->config_json, "leapTracker")))
if (!ns_3d_eye_parse(ns, &values->eyes[1], u_json_get(json, "rightEye")))
goto cleanup_l3d;
// Locked in, okay to touch anything inside ns struct
ns_3d_fov_calculate(&ns->base.hmd->distortion.fov[0], our_ns_3d_data->eyes[0].camera_projection);
ns_3d_fov_calculate(&ns->base.hmd->distortion.fov[1], our_ns_3d_data->eyes[1].camera_projection);
ns->config.distortion_type = NS_DISTORTION_TYPE_GEOMETRIC_3D;
ns->head_pose_to_eye[0] = our_ns_3d_data->eyes[0].eye_pose; // Left eye.
ns->head_pose_to_eye[1] = our_ns_3d_data->eyes[1].eye_pose; // Right eye.
ns_3d_fov_calculate(values->eyes[0].camera_projection, &ns->config.fov[0]);
ns_3d_fov_calculate(values->eyes[1].camera_projection, &ns->config.fov[1]);
our_ns_3d_data->eyes[0].optical_system = ns_3d_create_optical_system(&our_ns_3d_data->eyes[0]);
our_ns_3d_data->eyes[1].optical_system = ns_3d_create_optical_system(&our_ns_3d_data->eyes[1]);
ns->config.head_pose_to_eye[0] = values->eyes[0].eye_pose; // Left eye.
ns->config.head_pose_to_eye[1] = values->eyes[1].eye_pose; // Right eye.
ns->base.compute_distortion = &ns_3d_mesh_calc;
values->eyes[0].optical_system = ns_3d_create_optical_system(&values->eyes[0]);
values->eyes[1].optical_system = ns_3d_create_optical_system(&values->eyes[1]);
return true;
cleanup_l3d:
memset(&ns->dist_3d, 0, sizeof(struct ns_3d_data));
ns_3d_free_optical_system(&values->eyes[0].optical_system);
ns_3d_free_optical_system(&values->eyes[1].optical_system);
return false;
}
/*
*
* Moses Turner's distortion correction
* Moses Turner's meshgrid-based distortion correction
*
*/
bool
ns_meshgrid_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result)
ns_mt_parse(struct ns_hmd *ns, const cJSON *json)
{
struct ns_hmd *ns = ns_hmd(xdev);
return u_compute_distortion_ns_meshgrid(&ns->dist_meshgrid, view, u, v, result);
}
struct u_ns_meshgrid_values *values = &ns->config.dist_meshgrid;
void
ns_meshgrid_free_values(struct ns_hmd *ns)
{
free(ns->dist_meshgrid.ipds);
free(ns->dist_meshgrid.grid[0]);
free(ns->dist_meshgrid.grid[1]);
}
bool
ns_meshgrid_parse(struct ns_hmd *ns)
{
struct u_ns_meshgrid_values *values = &ns->dist_meshgrid;
const struct cJSON *config_json = ns->config_json;
if (strcmp(cJSON_GetStringValue(u_json_get(config_json, "type")), "Moses Turner's distortion correction") !=
0) {
if (strcmp(cJSON_GetStringValue(u_json_get(json, "type")), "Moses Turner's distortion correction") != 0) {
goto cleanup_mt;
}
int version = 0;
u_json_get_int(u_json_get(config_json, "version"), &version);
u_json_get_int(u_json_get(json, "version"), &version);
if (version != 2) {
goto cleanup_mt;
}
u_json_get_int(u_json_get(config_json, "num_grid_points_x"), &values->num_grid_points_u);
u_json_get_int(u_json_get(config_json, "num_grid_points_y"), &values->num_grid_points_v);
u_json_get_int(u_json_get(json, "num_grid_points_x"), &values->num_grid_points_u);
u_json_get_int(u_json_get(json, "num_grid_points_y"), &values->num_grid_points_v);
values->grid[0] =
realloc(values->grid[0], sizeof(struct xrt_vec2) * values->num_grid_points_u * values->num_grid_points_v);
values->grid[1] =
realloc(values->grid[1], sizeof(struct xrt_vec2) * values->num_grid_points_u * values->num_grid_points_v);
values->grid[0] = U_TYPED_ARRAY_CALLOC(struct xrt_vec2, values->num_grid_points_u * values->num_grid_points_v);
values->grid[1] = U_TYPED_ARRAY_CALLOC(struct xrt_vec2, values->num_grid_points_u * values->num_grid_points_v);
values->ipd = try_get_ipd(ns, ns->config_json);
values->ipd = try_get_ipd(ns, json);
const cJSON *current_element = config_json;
const cJSON *current_element = json;
for (int view = 0; view <= 1; view++) {
@ -365,44 +295,52 @@ ns_meshgrid_parse(struct ns_hmd *ns)
}
}
}
// locked in
ns->config.distortion_type = NS_DISTORTION_TYPE_MOSES_MESHGRID;
float baseline = values->ipd;
try_get_fov(ns, config_json, &values->fov[0], &values->fov[1]);
try_get_fov(ns, json, &values->fov[0], &values->fov[1]);
ns->base.hmd->distortion.fov[0] = values->fov[0];
ns->base.hmd->distortion.fov[1] = values->fov[1];
ns->config.fov[0] = values->fov[0];
ns->config.fov[1] = values->fov[1];
ns->head_pose_to_eye[0].orientation.x = 0.0f;
ns->head_pose_to_eye[0].orientation.y = 0.0f;
ns->head_pose_to_eye[0].orientation.z = 0.0f;
ns->head_pose_to_eye[0].orientation.w = 1.0f;
ns->head_pose_to_eye[0].position.x = -baseline / 2;
ns->head_pose_to_eye[0].position.y = 0.0f;
ns->head_pose_to_eye[0].position.z = 0.0f;
ns->head_pose_to_eye[1].orientation.x = 0.0f;
ns->head_pose_to_eye[1].orientation.y = 0.0f;
ns->head_pose_to_eye[1].orientation.z = 0.0f;
ns->head_pose_to_eye[1].orientation.w = 1.0f;
ns->head_pose_to_eye[1].position.x = baseline / 2;
ns->head_pose_to_eye[1].position.y = 0.0f;
ns->head_pose_to_eye[1].position.z = 0.0f;
ns->base.compute_distortion = &ns_meshgrid_mesh_calc;
ns->free_distortion_values = ns_meshgrid_free_values;
math_pose_identity(&ns->config.head_pose_to_eye[0]);
math_pose_identity(&ns->config.head_pose_to_eye[1]);
ns->config.head_pose_to_eye[0].position.x = -baseline / 2;
ns->config.head_pose_to_eye[1].position.x = baseline / 2;
return true;
cleanup_mt:
memset(&ns->dist_meshgrid, 0, sizeof(struct u_ns_meshgrid_values));
free(values->grid[0]);
free(values->grid[1]);
return false;
}
static bool
ns_optical_config_parse(struct ns_hmd *ns)
{
if (ns_3d_parse(ns, ns->config_json)) {
NS_INFO(ns, "Using Gemetric 3D display distortion correction!");
return true;
}
if (ns_p2d_parse(ns, ns->config_json)) {
NS_INFO(ns, "Using Polynomial 2D display distortion correction!");
return true;
}
if (ns_mt_parse(ns, ns->config_json)) {
NS_INFO(ns, "Using Moses's meshgrid-based display distortion correction!");
return true;
}
U_LOG_E("Couldn't find a valid display distortion correction!");
return false;
}
/*
*
* Common functions
@ -413,12 +351,17 @@ static void
ns_hmd_destroy(struct xrt_device *xdev)
{
struct ns_hmd *ns = ns_hmd(xdev);
NS_DEBUG(ns, "Called!");
// Remove the variable tracking.
u_var_remove_root(ns);
if (ns->free_distortion_values != NULL) {
ns->free_distortion_values(ns);
if (ns->config.distortion_type == NS_DISTORTION_TYPE_GEOMETRIC_3D) {
ns_3d_free_optical_system(&ns->config.dist_3d.eyes[0].optical_system);
ns_3d_free_optical_system(&ns->config.dist_3d.eyes[1].optical_system);
} else if (ns->config.distortion_type == NS_DISTORTION_TYPE_MOSES_MESHGRID) {
free(ns->config.dist_meshgrid.grid[0]);
free(ns->config.dist_meshgrid.grid[1]);
}
u_device_free(&ns->base);
@ -426,7 +369,10 @@ ns_hmd_destroy(struct xrt_device *xdev)
static void
ns_hmd_update_inputs(struct xrt_device *xdev)
{}
{
struct ns_hmd *ns = ns_hmd(xdev);
NS_DEBUG(ns, "Called!");
}
static void
ns_hmd_get_tracked_pose(struct xrt_device *xdev,
@ -435,6 +381,7 @@ ns_hmd_get_tracked_pose(struct xrt_device *xdev,
struct xrt_space_relation *out_relation)
{
struct ns_hmd *ns = ns_hmd(xdev);
NS_DEBUG(ns, "Called!");
if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
NS_ERROR(ns, "unknown input name");
@ -453,90 +400,53 @@ ns_hmd_get_view_poses(struct xrt_device *xdev,
struct xrt_fov *out_fovs,
struct xrt_pose *out_poses)
{
struct ns_hmd *ns = ns_hmd(xdev);
NS_DEBUG(ns, "Called!");
// Use this to take care of most stuff, then fix up below.
u_device_get_view_poses(xdev, default_eye_relation, at_timestamp_ns, view_count, out_head_relation, out_fovs,
out_poses);
// Fix fix.
struct ns_hmd *ns = ns_hmd(xdev);
for (uint32_t i = 0; i < view_count && i < ARRAY_SIZE(ns->head_pose_to_eye); i++) {
out_poses[i] = ns->head_pose_to_eye[i];
for (uint32_t i = 0; i < view_count && i < ARRAY_SIZE(ns->config.head_pose_to_eye); i++) {
out_poses[i] = ns->config.head_pose_to_eye[i];
}
}
static bool
ns_config_load(struct ns_hmd *ns, const char *config_path)
bool
ns_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result)
{
// Get the path to the JSON file
bool json_allocated = false;
if (config_path == NULL || strcmp(config_path, "/") == 0) {
NS_INFO(ns,
"Configuration path \"%s\" does not lead to a "
"configuration JSON file. Set the NS_CONFIG_PATH env "
"variable to your JSON.",
config_path);
struct ns_hmd *ns = ns_hmd(xdev);
NS_DEBUG(ns, "Called!");
// struct xrt_vec2 warped_uv;
switch (ns->config.distortion_type) {
case NS_DISTORTION_TYPE_GEOMETRIC_3D: {
struct xrt_vec2 uv = {u, v};
struct xrt_vec2 warped_uv = {0.0f, 0.0f};
ns_3d_display_uv_to_render_uv(uv, &warped_uv, &ns->config.dist_3d.eyes[view]);
result->r.x = warped_uv.x;
result->r.y = warped_uv.y;
result->g.x = warped_uv.x;
result->g.y = warped_uv.y;
result->b.x = warped_uv.x;
result->b.y = warped_uv.y;
return true;
}
case NS_DISTORTION_TYPE_POLYNOMIAL_2D: {
return u_compute_distortion_ns_p2d(&ns->config.dist_p2d, view, u, v, result);
}
case NS_DISTORTION_TYPE_MOSES_MESHGRID: {
return u_compute_distortion_ns_meshgrid(&ns->config.dist_meshgrid, view, u, v, result);
}
default: {
assert(false);
return false;
}
// Open the JSON file and put its contents into a string
FILE *config_file = fopen(config_path, "r");
if (config_file == NULL) {
NS_INFO(ns, "The configuration file at path \"%s\" was unable to load", config_path);
goto parse_error;
}
fseek(config_file, 0, SEEK_END); // Go to end of file
long file_size = ftell(config_file); // See offset we're at. This should be the file size in bytes.
rewind(config_file); // Go back to the beginning of the file
if (file_size == 0) {
NS_INFO(ns, "Empty config file!");
goto parse_error;
} else if (file_size > 3 * pow(1024, 2)) { // 3 MiB
NS_INFO(ns, "Huge config file! (%f MiB!!) Something's wrong here.", ((float)file_size) / pow(1024, 2));
goto parse_error;
}
char *json = calloc(file_size + 1, 1);
json_allocated = true;
size_t ret = fread(json, 1, file_size, config_file);
if ((long)ret != file_size) {
NS_ERROR(ns, "Failed to read configuration file at path \"%s\"", config_path);
goto parse_error;
}
fclose(config_file);
config_file = NULL;
json[file_size] = '\0';
ns->config_json = cJSON_Parse(json);
if (ns->config_json == NULL) {
const char *error_ptr = cJSON_GetErrorPtr();
NS_INFO(ns, "The JSON file at path \"%s\" was unable to parse", config_path);
if (error_ptr != NULL) {
NS_INFO(ns, "because of an error before %s", error_ptr);
}
goto parse_error;
}
// this function is not supposed to return true if ns->config_json is NULL
assert(ns->config_json != NULL);
free(json);
return true;
parse_error:
if (config_file != NULL) {
fclose(config_file);
config_file = NULL;
}
if (json_allocated) {
free(json);
}
NS_INFO(ns, "Are you sure you're using the right configuration file?");
return false;
}
/*
*
* Create function.
@ -544,36 +454,23 @@ parse_error:
*/
struct xrt_device *
ns_hmd_create(const char *config_path)
ns_hmd_create(const cJSON *config_json)
{
enum u_device_alloc_flags flags =
(enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
struct ns_hmd *ns = U_DEVICE_ALLOCATE(struct ns_hmd, flags, 1, 0);
ns->config_json = config_json;
ns_optical_config_parse(ns);
ns->log_level = debug_get_log_option_ns_log();
NS_DEBUG(ns, "Called!");
if (!ns_config_load(ns, config_path))
goto cleanup; // don't need to print any error, ns_config_load did that for us
ns->base.hmd->distortion.fov[0] = ns->config.fov[0];
ns->base.hmd->distortion.fov[1] = ns->config.fov[1];
int number_wrap = 3; // number of elements in below array of function pointers. Const to stop compiler warnings.
bool (*wrap_func_ptr[3])(struct ns_hmd *) = {ns_3d_parse, ns_p2d_parse, ns_meshgrid_parse};
// C syntax is weird here. This is an array of pointers to functions with arguments (struct ns_system * system)
// that all return a boolean value. The array should be roughly in descending order of how likely we think the
// user means to use each method For now `meshgrid` is last because Moses is the only one that uses it
bool found_config_wrap = false;
for (int i = 0; i < number_wrap; i++) {
if (wrap_func_ptr[i](ns)) { // wrap_func_ptr[i](ns) is a function call!
U_LOG_I("North Star: Using config wrap %i", i);
found_config_wrap = true;
break;
}
} // This will segfault at function ?? if you use GDB and the length is wrong.
if (!found_config_wrap) {
NS_INFO(ns, "North Star: Config file seems to be invalid.");
goto cleanup;
}
ns->base.compute_distortion = ns_mesh_calc;
ns->base.update_inputs = ns_hmd_update_inputs;
ns->base.get_tracked_pose = ns_hmd_get_tracked_pose;
ns->base.get_view_poses = ns_hmd_get_view_poses;
@ -645,8 +542,4 @@ ns_hmd_create(const char *config_path)
return &ns->base;
cleanup:
ns_hmd_destroy(&ns->base);
return NULL;
}

79
src/xrt/drivers/north_star/ns_hmd.h
View file

@ -27,18 +27,6 @@
extern "C" {
#endif
/*
*
* Printing functions.
*
*/
#define NS_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)
#define NS_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__)
#define NS_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__)
#define NS_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__)
#define NS_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__)
/*
*
* 3D distortion structs
@ -53,19 +41,6 @@ extern "C" {
*/
struct ns_3d_optical_system;
/*!
* Configuration information about the LMC or Rigel sensor according to the
* configuration file.
*
* @ingroup drv_ns
*/
struct ns_3d_leap
{
char name[64];
char serial[64];
struct xrt_pose pose;
};
/*!
* Distortion information about an eye parsed from the configuration file.
*
@ -81,6 +56,7 @@ struct ns_3d_eye
struct xrt_pose eye_pose;
// This is more of a vec4 than a quat
struct xrt_quat camera_projection;
struct xrt_matrix_4x4 sphere_to_world_space;
@ -89,12 +65,37 @@ struct ns_3d_eye
struct ns_optical_system *optical_system;
};
struct ns_3d_data
struct ns_3d_values
{
struct ns_3d_eye eyes[2];
struct ns_3d_leap leap;
};
enum ns_distortion_type
{
NS_DISTORTION_TYPE_INVALID,
NS_DISTORTION_TYPE_GEOMETRIC_3D,
NS_DISTORTION_TYPE_POLYNOMIAL_2D,
NS_DISTORTION_TYPE_MOSES_MESHGRID,
};
// The config json data gets dumped in here.
// In general, `target_builder_north_star.c` sets up tracking, and `ns_hmd.c` sets up distortion/optics.
struct ns_optics_config
{
struct xrt_pose head_pose_to_eye[2]; // left, right
struct xrt_fov fov[2]; // left,right
enum ns_distortion_type distortion_type;
union {
struct ns_3d_values dist_3d;
struct u_ns_p2d_values dist_p2d;
struct u_ns_meshgrid_values dist_meshgrid;
};
};
/*!
* Information about the whole North Star headset.
*
@ -106,16 +107,8 @@ struct ns_hmd
{
struct xrt_device base;
struct xrt_space_relation no_tracker_relation;
const char *config_path;
struct cJSON *config_json;
struct xrt_pose head_pose_to_eye[2]; // left, right
void (*free_distortion_values)(struct ns_hmd *hmd);
union {
struct ns_3d_data dist_3d;
struct u_ns_p2d_values dist_p2d;
struct u_ns_meshgrid_values dist_meshgrid;
};
const cJSON *config_json;
struct ns_optics_config config;
enum u_logging_level log_level;
};
@ -138,15 +131,6 @@ ns_hmd(struct xrt_device *xdev)
}
/*!
* Create a North Star hmd.
*
* @ingroup drv_ns
*/
struct xrt_device *
ns_hmd_create(const char *config_path);
/*!
* Convert the display UV to the render UV using the distortion mesh.
*
@ -159,6 +143,9 @@ ns_3d_display_uv_to_render_uv(struct xrt_vec2 in, struct xrt_vec2 *out, struct n
struct ns_optical_system *
ns_3d_create_optical_system(struct ns_3d_eye *eye);
void
ns_3d_free_optical_system(struct ns_optical_system **system);
#ifdef __cplusplus
}

9
src/xrt/drivers/north_star/ns_interface.h
View file

@ -9,6 +9,8 @@
*/
#pragma once
#include "util/u_json.h"
#include "xrt/xrt_device.h"
#ifdef __cplusplus
extern "C" {
@ -23,12 +25,13 @@ extern "C" {
*/
/*!
* Create a probe for NS devices.
* Creates a North Star HMD.
*
* @ingroup drv_ns
*/
struct xrt_auto_prober *
ns_create_auto_prober(void);
struct xrt_device *
ns_hmd_create(const cJSON *config_json);
/*!
* @dir drivers/north_star

83
src/xrt/drivers/north_star/ns_prober.c
View file

@ -1,83 +0,0 @@
// Copyright 2019-2020, Collabora, Ltd.
// Copyright 2020, Nova King.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief North Star prober code.
* @author Nova King <technobaboo@gmail.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_ns
*/
#include <stdio.h>
#include <stdlib.h>
#include "xrt/xrt_prober.h"
#include "util/u_misc.h"
#include "util/u_debug.h"
#include "ns_interface.h"
#include "ns_hmd.h"
DEBUG_GET_ONCE_OPTION(ns_config_path, "NS_CONFIG_PATH", NULL)
/*!
* @implements xrt_auto_prober
*/
struct ns_prober
{
struct xrt_auto_prober base;
const char *config_path;
};
//! @private @memberof ns_prober
static inline struct ns_prober *
ns_prober(struct xrt_auto_prober *p)
{
return (struct ns_prober *)p;
}
//! @public @memberof ns_prober
static void
ns_prober_destroy(struct xrt_auto_prober *p)
{
struct ns_prober *nsp = ns_prober(p);
free(nsp);
}
//! @public @memberof ns_prober
static int
ns_prober_autoprobe(struct xrt_auto_prober *xap,
cJSON *attached_data,
bool no_hmds,
struct xrt_prober *xp,
struct xrt_device **out_xdevs)
{
struct ns_prober *nsp = ns_prober(xap);
if (no_hmds) {
return 0;
}
if (nsp->config_path == NULL) {
return 0;
}
out_xdevs[0] = ns_hmd_create(nsp->config_path);
return 1;
}
struct xrt_auto_prober *
ns_create_auto_prober()
{
struct ns_prober *nsp = U_TYPED_CALLOC(struct ns_prober);
nsp->base.name = "northstar";
nsp->base.destroy = ns_prober_destroy;
nsp->base.lelo_dallas_autoprobe = ns_prober_autoprobe;
nsp->config_path = debug_get_option_ns_config_path();
return &nsp->base;
}

5
src/xrt/targets/common/CMakeLists.txt
View file

@ -37,6 +37,11 @@ if(XRT_BUILD_DRIVER_SIMULAVR)
target_sources(target_lists PRIVATE target_builder_simulavr.c)
endif()
if(XRT_BUILD_DRIVER_NS)
target_sources(target_lists PRIVATE target_builder_north_star.c)
target_link_libraries(target_lists PRIVATE drv_ns)
endif()
###
# Drivers
#

9
src/xrt/targets/common/target_builder_interface.h
View file

@ -32,6 +32,10 @@
#define T_BUILDER_SIMULAVR
#endif
#if defined(XRT_BUILD_DRIVER_NS)
#define T_BUILDER_NS
#endif
// Always enabled.
#define T_BUILDER_LEGACY
@ -81,3 +85,8 @@ t_builder_lighthouse_create(void);
struct xrt_builder *
t_builder_simula_create(void);
#endif
#ifdef T_BUILDER_NS
struct xrt_builder *
t_builder_north_star_create(void);
#endif

581
src/xrt/targets/common/target_builder_north_star.c Normal file
View file

@ -0,0 +1,581 @@
// Copyright 2022, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief System builder for North Star headsets
* @author Nova King <technobaboo@gmail.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @author Moses Turner <moses@collabora.com>
* @ingroup xrt_iface
*/
#include "math/m_api.h"
#include "math/m_space.h"
#include "multi_wrapper/multi.h"
#include "realsense/rs_interface.h"
#include "tracking/t_hand_tracking.h"
#include "tracking/t_tracking.h"
#include "xrt/xrt_config_drivers.h"
#include "xrt/xrt_device.h"
#include "xrt/xrt_prober.h"
#include "util/u_builders.h"
#include "util/u_config_json.h"
#include "util/u_debug.h"
#include "util/u_device.h"
#include "util/u_sink.h"
#include "util/u_system_helpers.h"
#include "util/u_file.h"
#include "util/u_pretty_print.h"
#include "target_builder_interface.h"
#include "north_star/ns_interface.h"
#ifdef XRT_BUILD_DRIVER_ULV2
#include "ultraleap_v2/ulv2_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_REALSENSE
#include "realsense/rs_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_DEPTHAI
#include "depthai/depthai_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_TWRAP
#include "twrap/twrap_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_HANDTRACKING
#include "ht/ht_interface.h"
#endif
#include "ht_ctrl_emu/ht_ctrl_emu_interface.h"
#include "xrt/xrt_frameserver.h"
#include "xrt/xrt_results.h"
#include "xrt/xrt_tracking.h"
#include <assert.h>
#include "math/m_mathinclude.h"
DEBUG_GET_ONCE_OPTION(ns_config_path, "NS_CONFIG_PATH", NULL)
DEBUG_GET_ONCE_LOG_OPTION(ns_log, "NS_LOG", U_LOGGING_WARN)
#define NS_TRACE(...) U_LOG_IFL_T(debug_get_log_option_ns_log(), __VA_ARGS__)
#define NS_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_ns_log(), __VA_ARGS__)
#define NS_INFO(...) U_LOG_IFL_I(debug_get_log_option_ns_log(), __VA_ARGS__)
#define NS_WARN(...) U_LOG_IFL_W(debug_get_log_option_ns_log(), __VA_ARGS__)
#define NS_ERROR(...) U_LOG_IFL_E(debug_get_log_option_ns_log(), __VA_ARGS__)
static const char *driver_list[] = {
"north_star",
};
struct ns_ultraleap_device
{
bool active;
// Users input `P_middleofeyes_to_trackingcenter_oxr`, and we invert it into this pose.
// It's a lot simpler to (and everybody does) care about the transform from the eyes center to the device,
// but tracking overrides care about this value.
struct xrt_pose P_trackingcenter_to_middleofeyes_oxr;
};
struct ns_depthai_device
{
bool active;
struct xrt_pose P_imu_to_left_camera_basalt;
struct xrt_pose P_middleofeyes_to_imu_oxr;
};
struct ns_t265
{
bool active;
struct xrt_pose P_middleofeyes_to_trackingcenter_oxr;
};
struct ns_builder
{
struct xrt_builder base;
const char *config_path;
cJSON *config_json;
struct ns_ultraleap_device ultraleap_device;
struct ns_depthai_device depthai_device;
struct ns_t265 t265;
};
static bool
ns_config_load(struct ns_builder *nsb)
{
const char *file_content = u_file_read_content_from_path(nsb->config_path);
if (file_content == NULL) {
U_LOG_E("The file at \"%s\" was unable to load. Either there wasn't a file there or it was empty.",
nsb->config_path);
return false;
}
// leaks?
cJSON *config_json = cJSON_Parse(file_content);
if (config_json == NULL) {
const char *error_ptr = cJSON_GetErrorPtr();
U_LOG_E("The JSON file at path \"%s\" was unable to parse", nsb->config_path);
if (error_ptr != NULL) {
U_LOG_E("because of an error before %s", error_ptr);
}
free((void *)file_content);
return false;
}
nsb->config_json = config_json;
free((void *)file_content);
return true;
}
static void
ns_tracking_config_parse_depthai(struct ns_builder *nsb, bool *out_config_valid)
{
*out_config_valid = true;
const cJSON *root = u_json_get(nsb->config_json, "depthaiDevice");
if (root == NULL) {
*out_config_valid = true;
// not invalid, but doesn't exist. active is not set and won't be used
return;
}
*out_config_valid = *out_config_valid && //
u_json_get_bool(u_json_get(root, "active"), &nsb->depthai_device.active);
*out_config_valid = *out_config_valid && //
u_json_get_pose(u_json_get(root, "P_imu_to_left_camera_basalt"),
&nsb->depthai_device.P_imu_to_left_camera_basalt);
*out_config_valid = *out_config_valid && //
u_json_get_pose(u_json_get(root, "P_middleofeyes_to_imu_oxr"),
&nsb->depthai_device.P_middleofeyes_to_imu_oxr);
}
static void
ns_tracking_config_parse_ultraleap(struct ns_builder *nsb, bool *out_config_valid)
{
*out_config_valid = true;
const cJSON *root = u_json_get(nsb->config_json, "leapTracker");
if (root == NULL) {
// not invalid, but doesn't exist. active is not set and won't be used
return;
}
struct xrt_pose P_middleofeyes_to_trackingcenter_oxr;
struct xrt_pose localpose_unity = XRT_POSE_IDENTITY;
if (u_json_get_pose_permissive(u_json_get(root, "localPose"), &localpose_unity)) {
NS_INFO(
"Found key `localPose` in your Ultraleap tracker config. Converting this from Unity's coordinate "
"space to OpenXR's coordinate space.");
NS_INFO(
"If you just want to specify the offset in OpenXR coordinates, use key "
"`P_middleofeyes_to_trackingcenter` instead.");
// This is the conversion from Unity to OpenXR coordinates.
// Unity: X+ Right; Y+ Up; Z+ Forward
// OpenXR: X+ Right; Y+ Up; Z- Forward
// Check tests_quat_change_of_basis to understand the quaternion element negations.
P_middleofeyes_to_trackingcenter_oxr.position.x = localpose_unity.position.x;
P_middleofeyes_to_trackingcenter_oxr.position.y = localpose_unity.position.y;
P_middleofeyes_to_trackingcenter_oxr.position.z = -localpose_unity.position.z;
P_middleofeyes_to_trackingcenter_oxr.orientation.x = localpose_unity.orientation.x;
P_middleofeyes_to_trackingcenter_oxr.orientation.y = localpose_unity.orientation.y;
P_middleofeyes_to_trackingcenter_oxr.orientation.z = -localpose_unity.orientation.z;
P_middleofeyes_to_trackingcenter_oxr.orientation.w = -localpose_unity.orientation.w;
*out_config_valid = *out_config_valid && true;
} else {
*out_config_valid = *out_config_valid && //
u_json_get_pose(u_json_get(root, "P_middleofeyes_to_trackingcenter_oxr"),
&P_middleofeyes_to_trackingcenter_oxr);
}
math_pose_invert(&P_middleofeyes_to_trackingcenter_oxr,
&nsb->ultraleap_device.P_trackingcenter_to_middleofeyes_oxr);
nsb->ultraleap_device.active = true;
}
static void
ns_tracking_config_parse_t265(struct ns_builder *nsb, bool *out_config_valid)
{
*out_config_valid = true;
const cJSON *root = u_json_get(nsb->config_json, "t265");
if (root == NULL) {
// not invalid, but doesn't exist. active is not set and won't be used
return;
}
*out_config_valid = *out_config_valid && //
u_json_get_bool(u_json_get(root, "active"), &nsb->t265.active);
*out_config_valid = *out_config_valid && //
u_json_get_pose(u_json_get(root, "P_middleofeyes_to_trackingcenter_oxr"),
&nsb->t265.P_middleofeyes_to_trackingcenter_oxr);
}
void
ns_compute_depthai_ht_offset(struct xrt_pose *P_imu_to_left_camera_basalt, struct xrt_pose *out_pose)
{
struct xrt_pose deg180 = XRT_POSE_IDENTITY;
struct xrt_vec3 plusx = {1, 0, 0};
struct xrt_vec3 plusz = {0, 0, -1};
math_quat_from_plus_x_z(&plusx, &plusz, &deg180.orientation);
struct xrt_relation_chain xrc = {0};
// Remember, relation_chains are backwards.
// This comes "after" P_imo_to_left_cam_basalt, and rotates from the usual camera coordinate space (+Y down +Z
// forward) to OpenXR/hand tracking's output coordinate space (+Y up +Z backwards)
m_relation_chain_push_pose_if_not_identity(&xrc, &deg180);
// This comes "first" and goes from the head tracking's output space (IMU) to where the left camera is,
// according to the config file.
m_relation_chain_push_pose_if_not_identity(&xrc, P_imu_to_left_camera_basalt);
struct xrt_space_relation rel = {0};
m_relation_chain_resolve(&xrc, &rel);
math_pose_invert(&rel.pose, out_pose);
}
#ifdef XRT_BUILD_DRIVER_DEPTHAI
static xrt_result_t
ns_setup_depthai_device(struct ns_builder *nsb,
struct u_system_devices *usysd,
struct xrt_device **out_hand_device,
struct xrt_device **out_head_device)
{
struct depthai_slam_startup_settings settings = {0};
settings.frames_per_second = 60;
settings.half_size_ov9282 = true;
settings.want_cameras = true;
settings.want_imu = true;
struct xrt_fs *the_fs = depthai_fs_slam(&usysd->xfctx, &settings);
if (the_fs == NULL) {
return XRT_ERROR_DEVICE_CREATION_FAILED;
}
struct t_stereo_camera_calibration *calib = NULL;
depthai_fs_get_stereo_calibration(the_fs, &calib);
struct xrt_slam_sinks *hand_sinks = NULL;
struct t_camera_extra_info extra_camera_info;
extra_camera_info.views[0].camera_orientation = CAMERA_ORIENTATION_180;
extra_camera_info.views[1].camera_orientation = CAMERA_ORIENTATION_180;
extra_camera_info.views[0].boundary_type = HT_IMAGE_BOUNDARY_NONE;
extra_camera_info.views[1].boundary_type = HT_IMAGE_BOUNDARY_NONE;
int create_status = ht_device_create(&usysd->xfctx, //
calib, //
HT_ALGORITHM_MERCURY, //
extra_camera_info, //
&hand_sinks, //
out_hand_device);
t_stereo_camera_calibration_reference(&calib, NULL);
if (create_status != 0) {
return XRT_ERROR_DEVICE_CREATION_FAILED;
}
struct xrt_slam_sinks *slam_sinks = NULL;
twrap_slam_create_device(&usysd->xfctx, XRT_DEVICE_DEPTHAI, &slam_sinks, out_head_device);
struct xrt_slam_sinks entry_sinks = {0};
struct xrt_frame_sink *entry_left_sink = NULL;
struct xrt_frame_sink *entry_right_sink = NULL;
u_sink_split_create(&usysd->xfctx, slam_sinks->left, hand_sinks->left, &entry_left_sink);
u_sink_split_create(&usysd->xfctx, slam_sinks->right, hand_sinks->right, &entry_right_sink);
entry_sinks = (struct xrt_slam_sinks){
.left = entry_left_sink,
.right = entry_right_sink,
.imu = slam_sinks->imu,
.gt = slam_sinks->gt,
};
struct xrt_slam_sinks dummy_slam_sinks = {0};
dummy_slam_sinks.imu = entry_sinks.imu;
u_sink_force_genlock_create(&usysd->xfctx, entry_sinks.left, entry_sinks.right, &dummy_slam_sinks.left,
&dummy_slam_sinks.right);
xrt_fs_slam_stream_start(the_fs, &dummy_slam_sinks);
return XRT_SUCCESS;
}
#endif
// Note: We're just checking for the config file's existence
static xrt_result_t
ns_estimate_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_builder_estimate *estimate)
{
struct ns_builder *nsb = (struct ns_builder *)xb;
U_ZERO(estimate);
nsb->config_path = debug_get_option_ns_config_path();
if (nsb->config_path == NULL) {
return XRT_SUCCESS;
}
struct xrt_prober_device **xpdevs = NULL;
size_t xpdev_count = 0;
xrt_result_t xret = XRT_SUCCESS;
// Lock the device list
xret = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count);
if (xret != XRT_SUCCESS) {
return xret;
}
estimate->maybe.head = true;
estimate->certain.head = true;
bool hand_tracking = false;
#ifdef XRT_BUILD_DRIVER_ULV2
hand_tracking =
hand_tracking || u_builder_find_prober_device(xpdevs, xpdev_count, ULV2_VID, ULV2_PID, XRT_BUS_TYPE_USB);
#endif
#ifdef XRT_BUILD_DRIVER_REALSENSE
estimate->certain.dof6 =
estimate->certain.dof6 || u_builder_find_prober_device(xpdevs, xpdev_count, REALSENSE_MOVIDIUS_VID,
REALSENSE_MOVIDIUS_PID, XRT_BUS_TYPE_USB);
estimate->certain.dof6 =
estimate->certain.dof6 || u_builder_find_prober_device(xpdevs, xpdev_count, //
REALSENSE_TM2_VID, REALSENSE_TM2_PID, //
XRT_BUS_TYPE_USB);
#endif
#ifdef XRT_BUILD_DRIVER_DEPTHAI
bool depthai = u_builder_find_prober_device(xpdevs, xpdev_count, DEPTHAI_VID, DEPTHAI_PID, XRT_BUS_TYPE_USB);
#ifdef XRT_FEATURE_SLAM
estimate->certain.dof6 = estimate->certain.dof6 || depthai;
#endif
#ifdef XRT_BUILD_DRIVER_HANDTRACKING
hand_tracking = hand_tracking || depthai;
#endif
#endif
estimate->certain.left = estimate->certain.right = estimate->maybe.left = estimate->maybe.right = hand_tracking;
xret = xrt_prober_unlock_list(xp, &xpdevs);
if (xret != XRT_SUCCESS) {
return xret;
}
return XRT_SUCCESS;
}
static xrt_result_t
ns_open_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_system_devices **out_xsysd)
{
struct ns_builder *nsb = (struct ns_builder *)xb;
struct u_system_devices *usysd = u_system_devices_allocate();
xrt_result_t result = XRT_SUCCESS;
if (out_xsysd == NULL || *out_xsysd != NULL) {
NS_ERROR("Invalid output system pointer");
result = XRT_ERROR_DEVICE_CREATION_FAILED;
goto end;
}
bool load_success = ns_config_load(nsb);
if (!load_success) {
result = XRT_ERROR_DEVICE_CREATION_FAILED;
goto end;
}
struct xrt_device *ns_hmd = ns_hmd_create(nsb->config_json);
if (ns_hmd == NULL) {
result = XRT_ERROR_DEVICE_CREATION_FAILED;
goto end;
}
bool config_valid = true;
ns_tracking_config_parse_depthai(nsb, &config_valid);
if (!config_valid) {
NS_ERROR("DepthAI device config was invalid!");
}
ns_tracking_config_parse_ultraleap(nsb, &config_valid);
if (!config_valid) {
NS_ERROR("Leap device config was invalid!");
}
ns_tracking_config_parse_t265(nsb, &config_valid);
if (!config_valid) {
NS_ERROR("T265 device config was invalid!");
}
struct xrt_device *hand_device = NULL;
struct xrt_device *slam_device = NULL;
struct xrt_pose head_offset = XRT_POSE_IDENTITY;
// True if hand tracker is parented to the head tracker (DepthAI), false if hand tracker is parented to
// middle-of-eyes (Ultraleap etc.)
bool hand_parented_to_head_tracker = true;
struct xrt_pose hand_offset = XRT_POSE_IDENTITY;
// bool got_head_tracker = false;
// For now we use DepthAI for head tracking + hand tracking OR t265 for head + ultraleap for hand.
// Mixing systems with more atomicity coming later™️
if (nsb->depthai_device.active) {
#ifdef XRT_BUILD_DRIVER_DEPTHAI
NS_INFO("Using DepthAI device!");
ns_setup_depthai_device(nsb, usysd, &hand_device, &slam_device);
head_offset = nsb->depthai_device.P_middleofeyes_to_imu_oxr;
ns_compute_depthai_ht_offset(&nsb->depthai_device.P_imu_to_left_camera_basalt, &hand_offset);
// got_head_tracker = true;
#else
NS_ERROR("DepthAI head+hand tracker specified in config but DepthAI support was not compiled in!");
#endif
} else {
if (nsb->t265.active) {
#ifdef XRT_BUILD_DRIVER_REALSENSE
slam_device = rs_create_tracked_device_internal_slam();
head_offset = nsb->t265.P_middleofeyes_to_trackingcenter_oxr;
// got_head_tracker = true;
#else
NS_ERROR(
"Realsense head tracker specified in config but Realsense support was not compiled in!");
#endif
}
if (nsb->ultraleap_device.active) {
#ifdef XRT_BUILD_DRIVER_ULV2
ulv2_create_device(&hand_device);
hand_offset = nsb->ultraleap_device.P_trackingcenter_to_middleofeyes_oxr;
hand_parented_to_head_tracker = false;
#else
NS_ERROR(
"Ultraleap hand tracker specified in config but Ultraleap support was not compiled in!");
#endif
}
}
struct xrt_device *head_wrap = NULL;
if (slam_device != NULL) {
usysd->base.xdevs[usysd->base.xdev_count++] = slam_device;
head_wrap = multi_create_tracking_override(XRT_TRACKING_OVERRIDE_DIRECT, ns_hmd, slam_device,
XRT_INPUT_GENERIC_TRACKER_POSE, &head_offset);
} else {
// No head tracker, no head tracking.
head_wrap = ns_hmd;
}
usysd->base.xdevs[usysd->base.xdev_count++] = head_wrap;
usysd->base.roles.head = head_wrap;
if (hand_device != NULL) {
// note: hand_parented_to_head_tracker is always false when slam_device is NULL
struct xrt_device *hand_wrap = multi_create_tracking_override(
XRT_TRACKING_OVERRIDE_ATTACHED, hand_device,
hand_parented_to_head_tracker ? slam_device : head_wrap,
hand_parented_to_head_tracker ? XRT_INPUT_GENERIC_TRACKER_POSE : XRT_INPUT_GENERIC_HEAD_POSE,
&hand_offset);
struct xrt_device *two_hands[2];
cemu_devices_create(head_wrap, hand_wrap, two_hands);
// usysd->base.xdev_count = 0;
usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[0];
usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[1];
usysd->base.roles.hand_tracking.left = two_hands[0];
usysd->base.roles.hand_tracking.right = two_hands[1];
usysd->base.roles.left = two_hands[0];
usysd->base.roles.right = two_hands[1];
}
end:
if (result == XRT_SUCCESS) {
*out_xsysd = &usysd->base;
} else {
u_system_devices_destroy(&usysd);
}
if (nsb->config_json != NULL) {
cJSON_Delete(nsb->config_json);
}
return result;
}
static void
ns_destroy(struct xrt_builder *xb)
{
free(xb);
}
/*
*
* 'Exported' functions.
*
*/
struct xrt_builder *
t_builder_north_star_create(void)
{
struct ns_builder *sb = U_TYPED_CALLOC(struct ns_builder);
sb->base.estimate_system = ns_estimate_system;
sb->base.open_system = ns_open_system;
sb->base.destroy = ns_destroy;
sb->base.identifier = "north_star";
sb->base.name = "North Star headset";
sb->base.driver_identifiers = driver_list;
sb->base.driver_identifier_count = ARRAY_SIZE(driver_list);
return &sb->base;
}

12
src/xrt/targets/common/target_lists.c
View file

@ -28,10 +28,6 @@
#include "ohmd/oh_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_NS
#include "north_star/ns_interface.h"
#endif
#ifdef XRT_BUILD_DRIVER_PSMV
#include "psmv/psmv_interface.h"
#endif
@ -108,6 +104,9 @@ xrt_builder_create_func_t target_builder_list[] = {
t_builder_remote_create,
#endif // T_BUILDER_REMOTE
#ifdef T_BUILDER_NS
t_builder_north_star_create,
#endif
#ifdef T_BUILDER_LEGACY
t_builder_legacy_create,
#endif // T_BUILDER_LEGACY
@ -182,11 +181,6 @@ xrt_auto_prober_create_func_t target_auto_list[] = {
oh_create_auto_prober,
#endif
#ifdef XRT_BUILD_DRIVER_NS
// North star driver here for now.
ns_create_auto_prober,
#endif
#ifdef XRT_BUILD_DRIVER_ANDROID
android_create_auto_prober,
#endif