diff --git a/src/xrt/auxiliary/util/u_distortion_mesh.c b/src/xrt/auxiliary/util/u_distortion_mesh.c index b640657af..70ecc4f68 100644 --- a/src/xrt/auxiliary/util/u_distortion_mesh.c +++ b/src/xrt/auxiliary/util/u_distortion_mesh.c @@ -324,54 +324,54 @@ u_compute_distortion_ns_p2d(struct u_ns_p2d_values *values, int view, float u, f /* * - * Moses's "variable-IPD 2D" distortion - * If Moses goes away or stops using North Star for some reason, please remove this - as of june 2021 nobody else is - * using it. + * Moses Turner's mesh-grid-based North Star distortion correction. + * This is a relatively ad-hoc thing I wrote; if this ends up going unused feel free to remove it. * */ bool -u_compute_distortion_ns_vipd(struct u_ns_vipd_values *values, int view, float u, float v, struct xrt_uv_triplet *result) +u_compute_distortion_ns_meshgrid( + struct u_ns_meshgrid_values *values, int view, float u, float v, struct xrt_uv_triplet *result) { - int u_index_int = (int)floorf(u * 64); - int v_index_int = (int)floorf(v * 64); - float u_index_frac = (u * 64) - u_index_int; - float v_index_frac = (v * 64) - v_index_int; + int u_edge_num = (values->num_grid_points_u - 1); + int v_edge_num = (values->num_grid_points_v - 1); - float x_ray; - float y_ray; + int u_index_int = floorf(u * u_edge_num); + int v_index_int = floorf(v * v_edge_num); + float u_index_frac = (u * u_edge_num) - u_index_int; + float v_index_frac = (v * v_edge_num) - v_index_int; - if (u_index_frac > 0.0001) { - // Probably this codepath if grid size is not 65x65 + // Imagine this like a ray coming out of your eye with x, y coordinate bearing and z coordinate -1.0f + struct xrt_vec2 bearing; + + int stride = values->num_grid_points_u; + + + if (u_index_frac > 0.000001 || v_index_frac > 0.000001) { // {top,bottom}-{left,right} notation might be inaccurate. The code *works* right now but don't take its // word when reading - struct xrt_vec2 topleft = values->grid_for_use.grid[view][v_index_int][u_index_int]; - struct xrt_vec2 topright = values->grid_for_use.grid[view][v_index_int][u_index_int + 1]; - struct xrt_vec2 bottomleft = values->grid_for_use.grid[view][v_index_int + 1][u_index_int]; - struct xrt_vec2 bottomright = values->grid_for_use.grid[view][v_index_int + 1][u_index_int + 1]; - struct xrt_vec2 leftcorrect = {(float)math_map_ranges(v_index_frac, 0, 1, topleft.x, bottomleft.x), - (float)math_map_ranges(v_index_frac, 0, 1, topleft.y, bottomleft.y)}; - struct xrt_vec2 rightcorrect = {(float)math_map_ranges(v_index_frac, 0, 1, topright.x, bottomright.x), - (float)math_map_ranges(v_index_frac, 0, 1, topright.y, bottomright.y)}; - y_ray = (float)math_map_ranges(u_index_frac, 0, 1, leftcorrect.x, rightcorrect.x); - x_ray = (float)math_map_ranges(u_index_frac, 0, 1, leftcorrect.y, rightcorrect.y); + struct xrt_vec2 topleft = values->grid[view][(v_index_int * stride) + u_index_int]; + struct xrt_vec2 topright = values->grid[view][(v_index_int * stride) + u_index_int + 1]; + struct xrt_vec2 bottomleft = values->grid[view][((v_index_int + 1) * stride) + u_index_int]; + struct xrt_vec2 bottomright = values->grid[view][((v_index_int + 1) * stride) + u_index_int + 1]; + struct xrt_vec2 left_point_on_line_segment = m_vec2_lerp(topleft, bottomleft, v_index_frac); + struct xrt_vec2 right_point_on_line_segment = m_vec2_lerp(topright, bottomright, v_index_frac); + + bearing = m_vec2_lerp(left_point_on_line_segment, right_point_on_line_segment, u_index_frac); } else { - // probably this path if grid size is 65x65 like normal - x_ray = values->grid_for_use.grid[view][v_index_int][u_index_int].y; - y_ray = values->grid_for_use.grid[view][v_index_int][u_index_int].x; + int acc_idx = (v_index_int * stride) + u_index_int; + bearing = values->grid[view][acc_idx]; } struct xrt_fov fov = values->fov[view]; - float left_ray_bound = tanf(fov.angle_left); - float right_ray_bound = tanf(fov.angle_right); - float up_ray_bound = tanf(fov.angle_up); - float down_ray_bound = tanf(fov.angle_down); - // printf("%f %f", fov.angle_down, fov.angle_up); + float left_ray_bound = tan(fov.angle_left); + float right_ray_bound = tan(fov.angle_right); + float up_ray_bound = tan(fov.angle_up); + float down_ray_bound = tan(fov.angle_down); - float u_eye = (float)math_map_ranges(x_ray, left_ray_bound, right_ray_bound, 0, 1); - - float v_eye = (float)math_map_ranges(y_ray, down_ray_bound, up_ray_bound, 0, 1); + float u_eye = math_map_ranges(bearing.x, left_ray_bound, right_ray_bound, 0, 1); + float v_eye = math_map_ranges(bearing.y, down_ray_bound, up_ray_bound, 0, 1); // boilerplate, put the UV coordinates in all the RGB slots result->r.x = u_eye; @@ -380,8 +380,6 @@ u_compute_distortion_ns_vipd(struct u_ns_vipd_values *values, int view, float u, result->g.y = v_eye; result->b.x = u_eye; result->b.y = v_eye; - // printf("%f %f\n", values->grid_for_use.grid[view][v_index_int][u_index_int].y, - // values->grid_for_use.grid[view][v_index_int][u_index_int].x); return true; } diff --git a/src/xrt/auxiliary/util/u_distortion_mesh.h b/src/xrt/auxiliary/util/u_distortion_mesh.h index b67396a6b..dfac70922 100644 --- a/src/xrt/auxiliary/util/u_distortion_mesh.h +++ b/src/xrt/auxiliary/util/u_distortion_mesh.h @@ -110,7 +110,7 @@ u_compute_distortion_cardboard(struct u_cardboard_distortion_values *values, /* * - * North Star 2D/Polynomial distortion. + * Values for North Star 2D/Polynomial distortion correction. * */ @@ -134,32 +134,28 @@ u_compute_distortion_ns_p2d(struct u_ns_p2d_values *values, int view, float u, f /* * - * North Star 2D/"VIPD" distortion. + * Values for Moses Turner's North Star distortion correction. * */ -struct u_ns_vipd_grid -{ - struct xrt_vec2 grid[2][65][65]; -}; - -struct u_ns_vipd_values +struct u_ns_meshgrid_values { int number_of_ipds; float *ipds; - struct u_ns_vipd_grid *grids; - struct u_ns_vipd_grid grid_for_use; + int num_grid_points_u; + int num_grid_points_v; + struct xrt_vec2 *grid[2]; struct xrt_fov fov[2]; // left, right float ipd; }; /*! - * Distortion correction implementation for North Star 2D/"VIPD". + * Moses Turner's North Star distortion correction implementation * * @ingroup aux_distortion */ bool -u_compute_distortion_ns_vipd( - struct u_ns_vipd_values *values, int view, float u, float v, struct xrt_uv_triplet *result); +u_compute_distortion_ns_meshgrid( + struct u_ns_meshgrid_values *values, int view, float u, float v, struct xrt_uv_triplet *result); /* diff --git a/src/xrt/drivers/north_star/ns_hmd.c b/src/xrt/drivers/north_star/ns_hmd.c index 52e13ae6e..3ed896d9a 100644 --- a/src/xrt/drivers/north_star/ns_hmd.c +++ b/src/xrt/drivers/north_star/ns_hmd.c @@ -109,132 +109,6 @@ good: memcpy(right_fov, &out_fov, sizeof(struct xrt_fov)); } -bool -ns_vipd_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_vipd(&ns->dist_vipd, view, u, v, result); -} - -bool -ns_vipd_parse(struct ns_hmd *ns) -{ - - struct u_ns_vipd_values *temp_data = &ns->dist_vipd; - const struct cJSON *config_json = ns->config_json; - - const cJSON *grids_json = u_json_get(config_json, "grids"); - if (grids_json == NULL) - goto cleanup_vipd; - - const cJSON *current_element = NULL; - char *current_key = NULL; - - cJSON_ArrayForEach(current_element, grids_json) - { // Note to people reviewing this: this is definitely not super safe. Tried to add as many null-checks as - // possible etc. but is probably a waste of time, it takes a while to do this right and the only person using - // this code is me -Moses - current_key = current_element->string; - float ipd = strtof(current_key, NULL) / 1000; - if (!((ipd < .100) && (ipd > .030))) { - U_LOG_E("Nonsense IPD in grid %d, skipping", temp_data->number_of_ipds + 1); - continue; - } - - temp_data->number_of_ipds += 1; - temp_data->ipds = realloc(temp_data->ipds, temp_data->number_of_ipds * sizeof(float)); - temp_data->ipds[temp_data->number_of_ipds - 1] = ipd; - temp_data->grids = realloc(temp_data->grids, temp_data->number_of_ipds * sizeof(struct u_ns_vipd_grid)); - - for (int view = 0; view <= 1; view++) { - const struct cJSON *grid_root = u_json_get(current_element, view ? "right" : "left"); - // if view is 0, then left. if view is 1, then right - for (int lv = 0; lv < 65; lv++) { - struct cJSON *v_axis = cJSON_GetArrayItem(grid_root, lv); - - for (int lu = 0; lu < 65; lu++) { - struct cJSON *cell = cJSON_GetArrayItem(v_axis, lu + 1); - - struct cJSON *cellX = cJSON_GetArrayItem(cell, 0); - struct cJSON *cellY = cJSON_GetArrayItem(cell, 1); - if (grid_root == NULL || cell == NULL || v_axis == NULL || cellX == NULL || - cellY == NULL) { - NS_ERROR(ns, - "VIPD distortion config is malformed in some way, bailing."); - goto cleanup_vipd; - } - temp_data->grids[temp_data->number_of_ipds - 1].grid[view][lv][lu].x = - (float)cellX->valuedouble; - temp_data->grids[temp_data->number_of_ipds - 1].grid[view][lv][lu].y = - (float)cellY->valuedouble; - } - } - } - } - - float baseline = try_get_ipd(ns, config_json); - - struct u_ns_vipd_grid *high_grid = {0}; - struct u_ns_vipd_grid *low_grid = {0}; - float interp = 0; - for (int i = 1; i < temp_data->number_of_ipds; i++) { - NS_DEBUG(ns, "looking at %f lower and %f upper\n", temp_data->ipds[i - 1], temp_data->ipds[i]); - if ((baseline >= temp_data->ipds[i - 1]) && (baseline <= temp_data->ipds[i])) { - NS_DEBUG(ns, "okay, IPD is between %f and %f\n", temp_data->ipds[i - 1], temp_data->ipds[i]); - high_grid = &temp_data->grids[i - 1]; - low_grid = &temp_data->grids[i]; - interp = math_map_ranges(baseline, temp_data->ipds[i - 1], temp_data->ipds[i], 0, 1); - NS_DEBUG(ns, "interp is %f\n", interp); - break; - } - } - - for (int view = 0; view <= 1; view++) { - for (int lv = 0; lv < 65; lv++) { - for (int lu = 0; lu < 65; lu++) { - temp_data->grid_for_use.grid[view][lv][lu].x = math_map_ranges( - interp, 0, 1, low_grid->grid[view][lv][lu].x, high_grid->grid[view][lv][lu].x); - temp_data->grid_for_use.grid[view][lv][lu].y = math_map_ranges( - interp, 0, 1, low_grid->grid[view][lv][lu].y, high_grid->grid[view][lv][lu].y); - } - } - } - - try_get_fov(ns, config_json, &temp_data->fov[0], &temp_data->fov[1]); - - memcpy(&ns->base.hmd->distortion.fov[0], &temp_data->fov[0], sizeof(struct xrt_fov)); - memcpy(&ns->base.hmd->distortion.fov[1], &temp_data->fov[1], sizeof(struct xrt_fov)); - - printf("%f %f %f %f\n", ns->base.hmd->distortion.fov[1].angle_down, ns->base.hmd->distortion.fov[1].angle_left, - ns->base.hmd->distortion.fov[1].angle_right, ns->base.hmd->distortion.fov[1].angle_up); - - 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_vipd_mesh_calc; - - return true; - -cleanup_vipd: - memset(&ns->dist_vipd, 0, sizeof(struct u_ns_vipd_values)); - return false; -} - /* * * "2D Polynomial" distortion; original implementation by Johnathon Zelstadt @@ -415,6 +289,120 @@ cleanup_l3d: return false; } + +/* + * + * Moses Turner's distortion correction + * + */ + +bool +ns_meshgrid_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_meshgrid(&ns->dist_meshgrid, view, u, v, result); +} + +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) { + goto cleanup_mt; + } + int version = 0; + u_json_get_int(u_json_get(config_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); + + 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->ipd = try_get_ipd(ns, ns->config_json); + + const cJSON *current_element = config_json; + + + for (int view = 0; view <= 1; view++) { + const struct cJSON *grid_root = u_json_get(current_element, view ? "right" : "left"); + grid_root = u_json_get(grid_root, "grid"); + // if view is 0, then left. if view is 1, then right + for (int lv = 0; lv < values->num_grid_points_v; lv++) { + struct cJSON *v_axis = cJSON_GetArrayItem(grid_root, lv); + + for (int lu = 0; lu < values->num_grid_points_u; lu++) { + struct cJSON *cell = cJSON_GetArrayItem(v_axis, lu); + + struct cJSON *cellX = cJSON_GetArrayItem(cell, 0); + struct cJSON *cellY = cJSON_GetArrayItem(cell, 1); + if (grid_root == NULL || cell == NULL || v_axis == NULL || cellX == NULL || + cellY == NULL) { + NS_ERROR(ns, "Distortion config file is malformed in some way, bailing"); + goto cleanup_mt; + } + float *x_ptr = &values->grid[view][(lv * values->num_grid_points_u) + lu].x; + float *y_ptr = &values->grid[view][(lv * values->num_grid_points_u) + lu].y; + u_json_get_float(cellX, x_ptr); + u_json_get_float(cellY, y_ptr); + } + } + } + + float baseline = values->ipd; + + + try_get_fov(ns, config_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->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; + + return true; + +cleanup_mt: + memset(&ns->dist_meshgrid, 0, sizeof(struct u_ns_meshgrid_values)); + return false; +} + /* * * Common functions @@ -429,6 +417,10 @@ ns_hmd_destroy(struct xrt_device *xdev) // Remove the variable tracking. u_var_remove_root(ns); + if (ns->free_distortion_values != NULL) { + ns->free_distortion_values(ns); + } + u_device_free(&ns->base); } @@ -563,10 +555,10 @@ ns_hmd_create(const char *config_path) goto cleanup; // don't need to print any error, ns_config_load did that for us 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_vipd_parse}; + 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 VIPD is last because Moses is the only one that uses it + // 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++) { diff --git a/src/xrt/drivers/north_star/ns_hmd.h b/src/xrt/drivers/north_star/ns_hmd.h index 5869008ac..cedfe94e0 100644 --- a/src/xrt/drivers/north_star/ns_hmd.h +++ b/src/xrt/drivers/north_star/ns_hmd.h @@ -110,10 +110,11 @@ struct ns_hmd 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_vipd_values dist_vipd; + struct u_ns_meshgrid_values dist_meshgrid; }; enum u_logging_level log_level;