+33 -35

src/xrt/auxiliary/util/u_distortion_mesh.c

··· 324 324 325 325 /* 326 326 * 327 327 - * Moses's "variable-IPD 2D" distortion 328 328 - * If Moses goes away or stops using North Star for some reason, please remove this - as of june 2021 nobody else is 329 329 - * using it. 327 327 + * Moses Turner's mesh-grid-based North Star distortion correction. 328 328 + * This is a relatively ad-hoc thing I wrote; if this ends up going unused feel free to remove it. 330 329 * 331 330 */ 332 331 333 332 bool 334 334 - u_compute_distortion_ns_vipd(struct u_ns_vipd_values *values, int view, float u, float v, struct xrt_uv_triplet *result) 333 333 + u_compute_distortion_ns_meshgrid( 334 334 + struct u_ns_meshgrid_values *values, int view, float u, float v, struct xrt_uv_triplet *result) 335 335 { 336 336 - int u_index_int = (int)floorf(u * 64); 337 337 - int v_index_int = (int)floorf(v * 64); 338 338 - float u_index_frac = (u * 64) - u_index_int; 339 339 - float v_index_frac = (v * 64) - v_index_int; 336 336 + int u_edge_num = (values->num_grid_points_u - 1); 337 337 + int v_edge_num = (values->num_grid_points_v - 1); 340 338 341 341 - float x_ray; 342 342 - float y_ray; 339 339 + int u_index_int = floorf(u * u_edge_num); 340 340 + int v_index_int = floorf(v * v_edge_num); 341 341 + float u_index_frac = (u * u_edge_num) - u_index_int; 342 342 + float v_index_frac = (v * v_edge_num) - v_index_int; 343 343 + 344 344 + // Imagine this like a ray coming out of your eye with x, y coordinate bearing and z coordinate -1.0f 345 345 + struct xrt_vec2 bearing; 346 346 + 347 347 + int stride = values->num_grid_points_u; 348 348 + 343 349 344 344 - if (u_index_frac > 0.0001) { 345 345 - // Probably this codepath if grid size is not 65x65 350 350 + if (u_index_frac > 0.000001 || v_index_frac > 0.000001) { 346 351 // {top,bottom}-{left,right} notation might be inaccurate. The code *works* right now but don't take its 347 352 // word when reading 348 348 - struct xrt_vec2 topleft = values->grid_for_use.grid[view][v_index_int][u_index_int]; 349 349 - struct xrt_vec2 topright = values->grid_for_use.grid[view][v_index_int][u_index_int + 1]; 350 350 - struct xrt_vec2 bottomleft = values->grid_for_use.grid[view][v_index_int + 1][u_index_int]; 351 351 - struct xrt_vec2 bottomright = values->grid_for_use.grid[view][v_index_int + 1][u_index_int + 1]; 352 352 - struct xrt_vec2 leftcorrect = {(float)math_map_ranges(v_index_frac, 0, 1, topleft.x, bottomleft.x), 353 353 - (float)math_map_ranges(v_index_frac, 0, 1, topleft.y, bottomleft.y)}; 354 354 - struct xrt_vec2 rightcorrect = {(float)math_map_ranges(v_index_frac, 0, 1, topright.x, bottomright.x), 355 355 - (float)math_map_ranges(v_index_frac, 0, 1, topright.y, bottomright.y)}; 356 356 - y_ray = (float)math_map_ranges(u_index_frac, 0, 1, leftcorrect.x, rightcorrect.x); 357 357 - x_ray = (float)math_map_ranges(u_index_frac, 0, 1, leftcorrect.y, rightcorrect.y); 353 353 + struct xrt_vec2 topleft = values->grid[view][(v_index_int * stride) + u_index_int]; 354 354 + struct xrt_vec2 topright = values->grid[view][(v_index_int * stride) + u_index_int + 1]; 355 355 + struct xrt_vec2 bottomleft = values->grid[view][((v_index_int + 1) * stride) + u_index_int]; 356 356 + struct xrt_vec2 bottomright = values->grid[view][((v_index_int + 1) * stride) + u_index_int + 1]; 357 357 + struct xrt_vec2 left_point_on_line_segment = m_vec2_lerp(topleft, bottomleft, v_index_frac); 358 358 + struct xrt_vec2 right_point_on_line_segment = m_vec2_lerp(topright, bottomright, v_index_frac); 359 359 + 360 360 + bearing = m_vec2_lerp(left_point_on_line_segment, right_point_on_line_segment, u_index_frac); 358 361 } else { 359 359 - // probably this path if grid size is 65x65 like normal 360 360 - x_ray = values->grid_for_use.grid[view][v_index_int][u_index_int].y; 361 361 - y_ray = values->grid_for_use.grid[view][v_index_int][u_index_int].x; 362 362 + int acc_idx = (v_index_int * stride) + u_index_int; 363 363 + bearing = values->grid[view][acc_idx]; 362 364 } 363 365 364 366 struct xrt_fov fov = values->fov[view]; 365 367 366 366 - float left_ray_bound = tanf(fov.angle_left); 367 367 - float right_ray_bound = tanf(fov.angle_right); 368 368 - float up_ray_bound = tanf(fov.angle_up); 369 369 - float down_ray_bound = tanf(fov.angle_down); 370 370 - // printf("%f %f", fov.angle_down, fov.angle_up); 371 371 - 372 372 - float u_eye = (float)math_map_ranges(x_ray, left_ray_bound, right_ray_bound, 0, 1); 368 368 + float left_ray_bound = tan(fov.angle_left); 369 369 + float right_ray_bound = tan(fov.angle_right); 370 370 + float up_ray_bound = tan(fov.angle_up); 371 371 + float down_ray_bound = tan(fov.angle_down); 373 372 374 374 - float v_eye = (float)math_map_ranges(y_ray, down_ray_bound, up_ray_bound, 0, 1); 373 373 + float u_eye = math_map_ranges(bearing.x, left_ray_bound, right_ray_bound, 0, 1); 374 374 + float v_eye = math_map_ranges(bearing.y, down_ray_bound, up_ray_bound, 0, 1); 375 375 376 376 // boilerplate, put the UV coordinates in all the RGB slots 377 377 result->r.x = u_eye; ··· 380 380 result->g.y = v_eye; 381 381 result->b.x = u_eye; 382 382 result->b.y = v_eye; 383 383 - // printf("%f %f\n", values->grid_for_use.grid[view][v_index_int][u_index_int].y, 384 384 - // values->grid_for_use.grid[view][v_index_int][u_index_int].x); 385 383 386 384 return true; 387 385 }

+9 -13

src/xrt/auxiliary/util/u_distortion_mesh.h

··· 110 110 111 111 /* 112 112 * 113 113 - * North Star 2D/Polynomial distortion. 113 113 + * Values for North Star 2D/Polynomial distortion correction. 114 114 * 115 115 */ 116 116 ··· 134 134 135 135 /* 136 136 * 137 137 - * North Star 2D/"VIPD" distortion. 137 137 + * Values for Moses Turner's North Star distortion correction. 138 138 * 139 139 */ 140 140 - struct u_ns_vipd_grid 141 141 - { 142 142 - struct xrt_vec2 grid[2][65][65]; 143 143 - }; 144 144 - 145 145 - struct u_ns_vipd_values 140 140 + struct u_ns_meshgrid_values 146 141 { 147 142 int number_of_ipds; 148 143 float *ipds; 149 149 - struct u_ns_vipd_grid *grids; 150 150 - struct u_ns_vipd_grid grid_for_use; 144 144 + int num_grid_points_u; 145 145 + int num_grid_points_v; 146 146 + struct xrt_vec2 *grid[2]; 151 147 struct xrt_fov fov[2]; // left, right 152 148 float ipd; 153 149 }; 154 150 155 151 /*! 156 156 - * Distortion correction implementation for North Star 2D/"VIPD". 152 152 + * Moses Turner's North Star distortion correction implementation 157 153 * 158 154 * @ingroup aux_distortion 159 155 */ 160 156 bool 161 161 - u_compute_distortion_ns_vipd( 162 162 - struct u_ns_vipd_values *values, int view, float u, float v, struct xrt_uv_triplet *result); 157 157 + u_compute_distortion_ns_meshgrid( 158 158 + struct u_ns_meshgrid_values *values, int view, float u, float v, struct xrt_uv_triplet *result); 163 159 164 160 165 161 /*

+120 -128

src/xrt/drivers/north_star/ns_hmd.c

··· 109 109 memcpy(right_fov, &out_fov, sizeof(struct xrt_fov)); 110 110 } 111 111 112 112 - bool 113 113 - ns_vipd_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) 114 114 - { 115 115 - struct ns_hmd *ns = ns_hmd(xdev); 116 116 - return u_compute_distortion_ns_vipd(&ns->dist_vipd, view, u, v, result); 117 117 - } 118 118 - 119 119 - bool 120 120 - ns_vipd_parse(struct ns_hmd *ns) 121 121 - { 122 122 - 123 123 - struct u_ns_vipd_values *temp_data = &ns->dist_vipd; 124 124 - const struct cJSON *config_json = ns->config_json; 125 125 - 126 126 - const cJSON *grids_json = u_json_get(config_json, "grids"); 127 127 - if (grids_json == NULL) 128 128 - goto cleanup_vipd; 129 129 - 130 130 - const cJSON *current_element = NULL; 131 131 - char *current_key = NULL; 132 132 - 133 133 - cJSON_ArrayForEach(current_element, grids_json) 134 134 - { // Note to people reviewing this: this is definitely not super safe. Tried to add as many null-checks as 135 135 - // possible etc. but is probably a waste of time, it takes a while to do this right and the only person using 136 136 - // this code is me -Moses 137 137 - current_key = current_element->string; 138 138 - float ipd = strtof(current_key, NULL) / 1000; 139 139 - if (!((ipd < .100) && (ipd > .030))) { 140 140 - U_LOG_E("Nonsense IPD in grid %d, skipping", temp_data->number_of_ipds + 1); 141 141 - continue; 142 142 - } 143 143 - 144 144 - temp_data->number_of_ipds += 1; 145 145 - temp_data->ipds = realloc(temp_data->ipds, temp_data->number_of_ipds * sizeof(float)); 146 146 - temp_data->ipds[temp_data->number_of_ipds - 1] = ipd; 147 147 - temp_data->grids = realloc(temp_data->grids, temp_data->number_of_ipds * sizeof(struct u_ns_vipd_grid)); 148 148 - 149 149 - for (int view = 0; view <= 1; view++) { 150 150 - const struct cJSON *grid_root = u_json_get(current_element, view ? "right" : "left"); 151 151 - // if view is 0, then left. if view is 1, then right 152 152 - for (int lv = 0; lv < 65; lv++) { 153 153 - struct cJSON *v_axis = cJSON_GetArrayItem(grid_root, lv); 154 154 - 155 155 - for (int lu = 0; lu < 65; lu++) { 156 156 - struct cJSON *cell = cJSON_GetArrayItem(v_axis, lu + 1); 157 157 - 158 158 - struct cJSON *cellX = cJSON_GetArrayItem(cell, 0); 159 159 - struct cJSON *cellY = cJSON_GetArrayItem(cell, 1); 160 160 - if (grid_root == NULL || cell == NULL || v_axis == NULL || cellX == NULL || 161 161 - cellY == NULL) { 162 162 - NS_ERROR(ns, 163 163 - "VIPD distortion config is malformed in some way, bailing."); 164 164 - goto cleanup_vipd; 165 165 - } 166 166 - temp_data->grids[temp_data->number_of_ipds - 1].grid[view][lv][lu].x = 167 167 - (float)cellX->valuedouble; 168 168 - temp_data->grids[temp_data->number_of_ipds - 1].grid[view][lv][lu].y = 169 169 - (float)cellY->valuedouble; 170 170 - } 171 171 - } 172 172 - } 173 173 - } 174 174 - 175 175 - float baseline = try_get_ipd(ns, config_json); 176 176 - 177 177 - struct u_ns_vipd_grid *high_grid = {0}; 178 178 - struct u_ns_vipd_grid *low_grid = {0}; 179 179 - float interp = 0; 180 180 - for (int i = 1; i < temp_data->number_of_ipds; i++) { 181 181 - NS_DEBUG(ns, "looking at %f lower and %f upper\n", temp_data->ipds[i - 1], temp_data->ipds[i]); 182 182 - if ((baseline >= temp_data->ipds[i - 1]) && (baseline <= temp_data->ipds[i])) { 183 183 - NS_DEBUG(ns, "okay, IPD is between %f and %f\n", temp_data->ipds[i - 1], temp_data->ipds[i]); 184 184 - high_grid = &temp_data->grids[i - 1]; 185 185 - low_grid = &temp_data->grids[i]; 186 186 - interp = math_map_ranges(baseline, temp_data->ipds[i - 1], temp_data->ipds[i], 0, 1); 187 187 - NS_DEBUG(ns, "interp is %f\n", interp); 188 188 - break; 189 189 - } 190 190 - } 191 191 - 192 192 - for (int view = 0; view <= 1; view++) { 193 193 - for (int lv = 0; lv < 65; lv++) { 194 194 - for (int lu = 0; lu < 65; lu++) { 195 195 - temp_data->grid_for_use.grid[view][lv][lu].x = math_map_ranges( 196 196 - interp, 0, 1, low_grid->grid[view][lv][lu].x, high_grid->grid[view][lv][lu].x); 197 197 - temp_data->grid_for_use.grid[view][lv][lu].y = math_map_ranges( 198 198 - interp, 0, 1, low_grid->grid[view][lv][lu].y, high_grid->grid[view][lv][lu].y); 199 199 - } 200 200 - } 201 201 - } 202 202 - 203 203 - try_get_fov(ns, config_json, &temp_data->fov[0], &temp_data->fov[1]); 204 204 - 205 205 - memcpy(&ns->base.hmd->distortion.fov[0], &temp_data->fov[0], sizeof(struct xrt_fov)); 206 206 - memcpy(&ns->base.hmd->distortion.fov[1], &temp_data->fov[1], sizeof(struct xrt_fov)); 207 207 - 208 208 - printf("%f %f %f %f\n", ns->base.hmd->distortion.fov[1].angle_down, ns->base.hmd->distortion.fov[1].angle_left, 209 209 - ns->base.hmd->distortion.fov[1].angle_right, ns->base.hmd->distortion.fov[1].angle_up); 210 210 - 211 211 - ns->head_pose_to_eye[0].orientation.x = 0.0f; 212 212 - ns->head_pose_to_eye[0].orientation.y = 0.0f; 213 213 - ns->head_pose_to_eye[0].orientation.z = 0.0f; 214 214 - ns->head_pose_to_eye[0].orientation.w = 1.0f; 215 215 - ns->head_pose_to_eye[0].position.x = -baseline / 2; 216 216 - ns->head_pose_to_eye[0].position.y = 0.0f; 217 217 - ns->head_pose_to_eye[0].position.z = 0.0f; 218 218 - 219 219 - 220 220 - 221 221 - ns->head_pose_to_eye[1].orientation.x = 0.0f; 222 222 - ns->head_pose_to_eye[1].orientation.y = 0.0f; 223 223 - ns->head_pose_to_eye[1].orientation.z = 0.0f; 224 224 - ns->head_pose_to_eye[1].orientation.w = 1.0f; 225 225 - ns->head_pose_to_eye[1].position.x = baseline / 2; 226 226 - ns->head_pose_to_eye[1].position.y = 0.0f; 227 227 - ns->head_pose_to_eye[1].position.z = 0.0f; 228 228 - 229 229 - ns->base.compute_distortion = &ns_vipd_mesh_calc; 230 230 - 231 231 - return true; 232 232 - 233 233 - cleanup_vipd: 234 234 - memset(&ns->dist_vipd, 0, sizeof(struct u_ns_vipd_values)); 235 235 - return false; 236 236 - } 237 237 - 238 112 /* 239 113 * 240 114 * "2D Polynomial" distortion; original implementation by Johnathon Zelstadt ··· 415 289 return false; 416 290 } 417 291 292 292 + 293 293 + /* 294 294 + * 295 295 + * Moses Turner's distortion correction 296 296 + * 297 297 + */ 298 298 + 299 299 + bool 300 300 + ns_meshgrid_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) 301 301 + { 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 + } 305 305 + 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) { 323 323 + goto cleanup_mt; 324 324 + } 325 325 + int version = 0; 326 326 + u_json_get_int(u_json_get(config_json, "version"), &version); 327 327 + if (version != 2) { 328 328 + goto cleanup_mt; 329 329 + } 330 330 + 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); 333 333 + 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); 338 338 + 339 339 + values->ipd = try_get_ipd(ns, ns->config_json); 340 340 + 341 341 + const cJSON *current_element = config_json; 342 342 + 343 343 + 344 344 + for (int view = 0; view <= 1; view++) { 345 345 + const struct cJSON *grid_root = u_json_get(current_element, view ? "right" : "left"); 346 346 + grid_root = u_json_get(grid_root, "grid"); 347 347 + // if view is 0, then left. if view is 1, then right 348 348 + for (int lv = 0; lv < values->num_grid_points_v; lv++) { 349 349 + struct cJSON *v_axis = cJSON_GetArrayItem(grid_root, lv); 350 350 + 351 351 + for (int lu = 0; lu < values->num_grid_points_u; lu++) { 352 352 + struct cJSON *cell = cJSON_GetArrayItem(v_axis, lu); 353 353 + 354 354 + struct cJSON *cellX = cJSON_GetArrayItem(cell, 0); 355 355 + struct cJSON *cellY = cJSON_GetArrayItem(cell, 1); 356 356 + if (grid_root == NULL || cell == NULL || v_axis == NULL || cellX == NULL || 357 357 + cellY == NULL) { 358 358 + NS_ERROR(ns, "Distortion config file is malformed in some way, bailing"); 359 359 + goto cleanup_mt; 360 360 + } 361 361 + float *x_ptr = &values->grid[view][(lv * values->num_grid_points_u) + lu].x; 362 362 + float *y_ptr = &values->grid[view][(lv * values->num_grid_points_u) + lu].y; 363 363 + u_json_get_float(cellX, x_ptr); 364 364 + u_json_get_float(cellY, y_ptr); 365 365 + } 366 366 + } 367 367 + } 368 368 + 369 369 + float baseline = values->ipd; 370 370 + 371 371 + 372 372 + try_get_fov(ns, config_json, &values->fov[0], &values->fov[1]); 373 373 + 374 374 + ns->base.hmd->distortion.fov[0] = values->fov[0]; 375 375 + ns->base.hmd->distortion.fov[1] = values->fov[1]; 376 376 + 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 + 385 385 + 386 386 + 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; 394 394 + 395 395 + ns->base.compute_distortion = &ns_meshgrid_mesh_calc; 396 396 + 397 397 + ns->free_distortion_values = ns_meshgrid_free_values; 398 398 + 399 399 + return true; 400 400 + 401 401 + cleanup_mt: 402 402 + memset(&ns->dist_meshgrid, 0, sizeof(struct u_ns_meshgrid_values)); 403 403 + return false; 404 404 + } 405 405 + 418 406 /* 419 407 * 420 408 * Common functions ··· 428 416 429 417 // Remove the variable tracking. 430 418 u_var_remove_root(ns); 419 419 + 420 420 + if (ns->free_distortion_values != NULL) { 421 421 + ns->free_distortion_values(ns); 422 422 + } 431 423 432 424 u_device_free(&ns->base); 433 425 } ··· 563 555 goto cleanup; // don't need to print any error, ns_config_load did that for us 564 556 565 557 int number_wrap = 3; // number of elements in below array of function pointers. Const to stop compiler warnings. 566 566 - bool (*wrap_func_ptr[3])(struct ns_hmd *) = {ns_3d_parse, ns_p2d_parse, ns_vipd_parse}; 558 558 + bool (*wrap_func_ptr[3])(struct ns_hmd *) = {ns_3d_parse, ns_p2d_parse, ns_meshgrid_parse}; 567 559 // C syntax is weird here. This is an array of pointers to functions with arguments (struct ns_system * system) 568 560 // that all return a boolean value. The array should be roughly in descending order of how likely we think the 569 569 - // user means to use each method For now VIPD is last because Moses is the only one that uses it 561 561 + // user means to use each method For now `meshgrid` is last because Moses is the only one that uses it 570 562 571 563 bool found_config_wrap = false; 572 564 for (int i = 0; i < number_wrap; i++) {

+2 -1

src/xrt/drivers/north_star/ns_hmd.h

··· 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); 113 114 union { 114 115 struct ns_3d_data dist_3d; 115 116 struct u_ns_p2d_values dist_p2d; 116 116 - struct u_ns_vipd_values dist_vipd; 117 117 + struct u_ns_meshgrid_values dist_meshgrid; 117 118 }; 118 119 119 120 enum u_logging_level log_level;