The open source OpenXR runtime
d/rift: Basic lens distortion
Needs work, use Util_Render_Stereo::DistortionMeshMakeVertex in LibOVR as a reference.
Part-of: <https://gitlab.freedesktop.org/monado/monado/-/merge_requests/2428>
+276
-6
+201
-5
src/xrt/drivers/rift/rift_hmd.c
+201
-5
src/xrt/drivers/rift/rift_hmd.c
···
33
33
#include "xrt/xrt_results.h"
34
34
35
35
#include <stdio.h>
36
-
36
+
#include <assert.h>
37
37
38
38
/*
39
39
*
···
154
154
}
155
155
156
156
static int
157
+
rift_get_lens_distortion(struct rift_hmd *hmd, struct rift_lens_distortion_report *lens_distortion)
158
+
{
159
+
uint8_t buf[REPORT_MAX_SIZE] = {0};
160
+
161
+
int result = rift_get_report(hmd, FEATURE_REPORT_LENS_DISTORTION, buf, sizeof(buf));
162
+
if (result < 0) {
163
+
return result;
164
+
}
165
+
166
+
memcpy(lens_distortion, buf + 1, sizeof(*lens_distortion));
167
+
168
+
return 0;
169
+
}
170
+
171
+
static int
157
172
rift_set_config(struct rift_hmd *hmd, struct rift_config_report *config)
158
173
{
159
174
return rift_send_report(hmd, FEATURE_REPORT_CONFIG, &config, sizeof(*config));
···
174
189
u_var_remove_root(hmd);
175
190
176
191
m_relation_history_destroy(&hmd->relation_hist);
192
+
193
+
if (hmd->lens_distortions)
194
+
free(hmd->lens_distortions);
177
195
178
196
u_device_free(&hmd->base);
179
197
}
···
244
262
return XRT_SUCCESS;
245
263
}
246
264
265
+
static float
266
+
rift_catmull_rom_spline(struct rift_catmull_rom_distortion_data *catmull, float scaled_value)
267
+
{
268
+
float scaled_value_floor = floorf(scaled_value);
269
+
scaled_value_floor = CLAMP(scaled_value_floor, 0, CATMULL_COEFFICIENTS - 1);
270
+
271
+
float t = scaled_value - scaled_value_floor;
272
+
int k = (int)scaled_value_floor;
273
+
274
+
float p0, p1, m0, m1;
275
+
switch (k) {
276
+
case 0:
277
+
p0 = 1.0f;
278
+
m0 = (catmull->k[1] - catmull->k[0]);
279
+
p1 = catmull->k[1];
280
+
m1 = 0.5f * (catmull->k[2] - catmull->k[0]);
281
+
break;
282
+
default:
283
+
p0 = catmull->k[k];
284
+
m0 = 0.5f * (catmull->k[k + 1] - catmull->k[k - 1]);
285
+
p1 = catmull->k[k + 1];
286
+
m1 = 0.5f * (catmull->k[k + 2] - catmull->k[k]);
287
+
break;
288
+
case CATMULL_COEFFICIENTS - 2:
289
+
p0 = catmull->k[CATMULL_COEFFICIENTS - 2];
290
+
m0 = 0.5f * (catmull->k[CATMULL_COEFFICIENTS - 1] - catmull->k[CATMULL_COEFFICIENTS - 2]);
291
+
p1 = catmull->k[CATMULL_COEFFICIENTS - 1];
292
+
m1 = catmull->k[CATMULL_COEFFICIENTS - 1] - catmull->k[CATMULL_COEFFICIENTS - 2];
293
+
break;
294
+
case CATMULL_COEFFICIENTS - 1:
295
+
p0 = catmull->k[CATMULL_COEFFICIENTS - 1];
296
+
m0 = catmull->k[CATMULL_COEFFICIENTS - 1] - catmull->k[CATMULL_COEFFICIENTS - 2];
297
+
p1 = p0 + m0;
298
+
m1 = m0;
299
+
break;
300
+
}
301
+
302
+
float omt = 1.0f - t;
303
+
304
+
float res = (p0 * (1.0f + 2.0f * t) + m0 * t) * omt * omt + (p1 * (1.0f + 2.0f * omt) - m1 * omt) * t * t;
305
+
306
+
return res;
307
+
}
308
+
309
+
static float
310
+
rift_distortion_distance_squared(struct rift_lens_distortion *lens_distortion, float distance_squared)
311
+
{
312
+
float scale = 1.0f;
313
+
314
+
switch (lens_distortion->distortion_version) {
315
+
case RIFT_LENS_DISTORTION_LCSV_CATMULL_ROM_10_VERSION_1: {
316
+
struct rift_catmull_rom_distortion_data data = lens_distortion->data.lcsv_catmull_rom_10;
317
+
318
+
float scaled_distance_squared =
319
+
(float)(CATMULL_COEFFICIENTS - 1) * distance_squared / (data.max_r * data.max_r);
320
+
321
+
return rift_catmull_rom_spline(&data, scaled_distance_squared);
322
+
}
323
+
default: return scale;
324
+
}
325
+
}
326
+
327
+
static struct xrt_vec3
328
+
rift_distortion_distance_squared_split_chroma(struct rift_lens_distortion *lens_distortion, float distance_squared)
329
+
{
330
+
float scale = rift_distortion_distance_squared(lens_distortion, distance_squared);
331
+
332
+
struct xrt_vec3 scale_split;
333
+
scale_split.x = scale;
334
+
scale_split.y = scale;
335
+
scale_split.z = scale;
336
+
337
+
switch (lens_distortion->distortion_version) {
338
+
case RIFT_LENS_DISTORTION_LCSV_CATMULL_ROM_10_VERSION_1: {
339
+
struct rift_catmull_rom_distortion_data data = lens_distortion->data.lcsv_catmull_rom_10;
340
+
341
+
scale_split.x *= 1.0f + data.chromatic_abberation[0] + distance_squared * data.chromatic_abberation[1];
342
+
scale_split.z *= 1.0f + data.chromatic_abberation[2] + distance_squared * data.chromatic_abberation[3];
343
+
break;
344
+
}
345
+
}
346
+
347
+
return scale_split;
348
+
}
349
+
247
350
static bool
248
351
rift_hmd_compute_distortion(struct xrt_device *dev, uint32_t view, float u, float v, struct xrt_uv_triplet *out_result)
249
352
{
353
+
#define ZERO_ONE_TO_N_ONE_ONE(x) ((x * 2) - 1)
354
+
#define N_ONE_ONE_TO_ZERO_ONE(x) ((x + 1) / 2)
355
+
250
356
struct rift_hmd *hmd = rift_hmd(dev);
251
357
252
-
return false;
358
+
struct rift_lens_distortion *distortion = &hmd->lens_distortions[0];
359
+
360
+
float display_width_meters = (float)hmd->display_info.display_width / 1000000.0f;
361
+
float display_height_meters = (float)hmd->display_info.display_height / 1000000.0f;
362
+
363
+
float tan_eye_angle_scale_x =
364
+
display_width_meters / distortion->data.lcsv_catmull_rom_10.meters_per_tan_angle_at_center * 0.25f;
365
+
float tan_eye_angle_scale_y =
366
+
display_height_meters / distortion->data.lcsv_catmull_rom_10.meters_per_tan_angle_at_center * 0.5f;
367
+
368
+
u = ZERO_ONE_TO_N_ONE_ONE(u) * tan_eye_angle_scale_x;
369
+
v = ZERO_ONE_TO_N_ONE_ONE(v) * tan_eye_angle_scale_y;
370
+
371
+
float distance_squared = fabsf(u * u + v * v);
372
+
373
+
struct xrt_vec3 chroma_distortions =
374
+
rift_distortion_distance_squared_split_chroma(distortion, distance_squared);
375
+
376
+
out_result->r = (struct xrt_vec2){N_ONE_ONE_TO_ZERO_ONE(u * chroma_distortions.x),
377
+
N_ONE_ONE_TO_ZERO_ONE(v * chroma_distortions.x)};
378
+
out_result->g = (struct xrt_vec2){N_ONE_ONE_TO_ZERO_ONE(u * chroma_distortions.y),
379
+
N_ONE_ONE_TO_ZERO_ONE(v * chroma_distortions.y)};
380
+
out_result->b = (struct xrt_vec2){N_ONE_ONE_TO_ZERO_ONE(u * chroma_distortions.z),
381
+
N_ONE_ONE_TO_ZERO_ONE(v * chroma_distortions.z)};
382
+
383
+
return true;
384
+
#undef ZERO_ONE_TO_N_ONE_ONE
385
+
#undef N_ONE_ONE_TO_ZERO_ONE
386
+
}
387
+
388
+
static float
389
+
rift_decode_fixed_point_uint16(uint16_t value, uint16_t zero_value, int fractional_bits)
390
+
{
391
+
float value_float = (float)value;
392
+
value_float -= (float)zero_value;
393
+
value_float *= 1.0f / (float)(1 << fractional_bits);
394
+
return value_float;
395
+
}
396
+
397
+
static void
398
+
rift_parse_distortion_report(struct rift_lens_distortion_report *report, struct rift_lens_distortion *out)
399
+
{
400
+
out->distortion_version = report->distortion_version;
401
+
402
+
switch (report->distortion_version) {
403
+
case RIFT_LENS_DISTORTION_LCSV_CATMULL_ROM_10_VERSION_1: {
404
+
struct rift_catmull_rom_distortion_report_data report_data = report->data.lcsv_catmull_rom_10;
405
+
struct rift_catmull_rom_distortion_data data;
406
+
407
+
for (uint16_t i = 0; i < CATMULL_COEFFICIENTS; i += 1) {
408
+
data.k[i] = rift_decode_fixed_point_uint16(report_data.k[i], 0, 14);
409
+
}
410
+
data.max_r = rift_decode_fixed_point_uint16(report_data.max_r, 0, 14);
411
+
data.meters_per_tan_angle_at_center =
412
+
rift_decode_fixed_point_uint16(report_data.meters_per_tan_angle_at_center, 0, 19);
413
+
for (uint16_t i = 0; i < CHROMATIC_ABBERATION_COEFFEICENT_COUNT; i += 1) {
414
+
data.chromatic_abberation[i] =
415
+
rift_decode_fixed_point_uint16(report_data.chromatic_abberation[i], 0x8000, 19);
416
+
}
417
+
418
+
out->data.lcsv_catmull_rom_10 = data;
419
+
break;
420
+
}
421
+
default: return;
422
+
}
253
423
}
254
424
255
425
struct rift_hmd *
···
304
474
// update the config
305
475
result = rift_set_config(hmd, &hmd->config);
306
476
if (result < 0) {
307
-
HMD_ERROR(hmd, "Failed to enable the override power config flag, reason %d", result);
477
+
HMD_ERROR(hmd, "Failed to set the device config, reason %d", result);
478
+
u_device_free(&hmd->base);
479
+
return NULL;
480
+
}
481
+
482
+
// get the lens distortions
483
+
struct rift_lens_distortion_report lens_distortion;
484
+
result = rift_get_lens_distortion(hmd, &lens_distortion);
485
+
if (result < 0) {
486
+
HMD_ERROR(hmd, "Failed to get lens distortion, reason %d", result);
308
487
u_device_free(&hmd->base);
309
488
return NULL;
310
489
}
311
490
491
+
hmd->num_lens_distortions = lens_distortion.num_distortions;
492
+
hmd->lens_distortions = calloc(lens_distortion.num_distortions, sizeof(struct rift_lens_distortion));
493
+
494
+
rift_parse_distortion_report(&lens_distortion, &hmd->lens_distortions[lens_distortion.distortion_idx]);
495
+
// TODO: actually verify we initialize all the distortions. if the headset is working correctly, this should
496
+
// have happened, but you never know.
497
+
for (uint16_t i = 1; i < hmd->num_lens_distortions; i++) {
498
+
result = rift_get_lens_distortion(hmd, &lens_distortion);
499
+
if (result < 0) {
500
+
HMD_ERROR(hmd, "Failed to get lens distortion idx %d, reason %d", i, result);
501
+
u_device_free(&hmd->base);
502
+
return NULL;
503
+
}
504
+
505
+
rift_parse_distortion_report(&lens_distortion, &hmd->lens_distortions[lens_distortion.distortion_idx]);
506
+
}
507
+
312
508
// This list should be ordered, most preferred first.
313
509
size_t idx = 0;
314
510
hmd->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
···
321
517
hmd->base.destroy = rift_hmd_destroy;
322
518
323
519
// Distortion information, fills in xdev->compute_distortion().
324
-
u_distortion_mesh_set_none(&hmd->base);
520
+
// u_distortion_mesh_set_none(&hmd->base);
325
521
326
522
// populate this with something more complex if required
327
-
// hmd->base.compute_distortion = rift_hmd_compute_distortion;
523
+
hmd->base.compute_distortion = rift_hmd_compute_distortion;
328
524
329
525
hmd->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
330
526
hmd->log_level = debug_get_log_option_rift_log();
+75
-1
src/xrt/drivers/rift/rift_interface.h
+75
-1
src/xrt/drivers/rift/rift_interface.h
···
38
38
FEATURE_REPORT_DK1_KEEP_ALIVE = 8, // get + set
39
39
FEATURE_REPORT_DISPLAY_INFO = 9, // get + set
40
40
FEATURE_REPORT_SERIAL = 10, // get + set
41
+
41
42
// DK2
42
43
FEATURE_REPORT_TRACKING = 12, // get + set
43
44
FEATURE_REPORT_DISPLAY = 13, // get + set
···
80
81
RIFT_DISTORTION_TYPE_K = 2,
81
82
};
82
83
84
+
enum rift_lens_type
85
+
{
86
+
// firmware indirectly states lens type A is 0
87
+
RIFT_LENS_TYPE_A = 0,
88
+
// firmware does not state what lens type B is, 1 is an educated guess
89
+
RIFT_LENS_TYPE_B = 1,
90
+
};
91
+
92
+
#define CATMULL_COEFFICIENTS 11
93
+
#define CHROMATIC_ABBERATION_COEFFEICENT_COUNT 4
94
+
95
+
enum rift_lens_distortion_version
96
+
{
97
+
// no distortion data is stored
98
+
RIFT_LENS_DISTORTION_NONE = 0,
99
+
// standard distortion matrix
100
+
RIFT_LENS_DISTORTION_LCSV_CATMULL_ROM_10_VERSION_1 = 1,
101
+
};
102
+
83
103
/*
84
104
*
85
105
* Packed structs for USB communication (borrowed from Rokid driver)
···
101
121
uint16_t command_id;
102
122
uint8_t config_flags;
103
123
uint8_t interval;
104
-
uint16_t sample_rate; // always 1000hz on DK1/DK2
124
+
// sample rate of the IMU, always 1000hz on DK1/DK2, read-only
125
+
uint16_t sample_rate;
105
126
} RIFT_PACKED;
106
127
107
128
struct rift_display_info_report
···
125
146
float distortion[6];
126
147
} RIFT_PACKED;
127
148
149
+
struct rift_catmull_rom_distortion_report_data
150
+
{
151
+
// eye relief setting, in micrometers from front surface of lens
152
+
uint16_t eye_relief;
153
+
// the k coeffecients of the distortion
154
+
uint16_t k[CATMULL_COEFFICIENTS];
155
+
uint16_t max_r;
156
+
uint16_t meters_per_tan_angle_at_center;
157
+
uint16_t chromatic_abberation[CHROMATIC_ABBERATION_COEFFEICENT_COUNT];
158
+
uint8_t unused[14];
159
+
} RIFT_PACKED;
160
+
161
+
struct rift_lens_distortion_report
162
+
{
163
+
uint16_t command_id;
164
+
// the amount of distortions on this device
165
+
uint8_t num_distortions;
166
+
// the index of this distortion in the devices array
167
+
uint8_t distortion_idx;
168
+
// unused bitmask field
169
+
uint8_t bitmask;
170
+
// the type of the lenses
171
+
uint16_t lens_type;
172
+
// the version of the lens distortion data
173
+
uint16_t distortion_version;
174
+
175
+
union {
176
+
struct rift_catmull_rom_distortion_report_data lcsv_catmull_rom_10;
177
+
} RIFT_PACKED data;
178
+
} RIFT_PACKED;
179
+
128
180
#define IN_REPORT_DK2 11
129
181
struct dk2_report_keepalive_mux
130
182
{
···
137
189
#pragma pack(pop)
138
190
#endif
139
191
192
+
struct rift_catmull_rom_distortion_data
193
+
{
194
+
// the k coeffecients of the distortion
195
+
float k[CATMULL_COEFFICIENTS];
196
+
float max_r;
197
+
float meters_per_tan_angle_at_center;
198
+
float chromatic_abberation[CHROMATIC_ABBERATION_COEFFEICENT_COUNT];
199
+
};
200
+
201
+
struct rift_lens_distortion
202
+
{
203
+
// the version of the lens distortion data
204
+
uint16_t distortion_version;
205
+
206
+
union {
207
+
struct rift_catmull_rom_distortion_data lcsv_catmull_rom_10;
208
+
} RIFT_PACKED data;
209
+
};
210
+
140
211
enum rift_variant
141
212
{
142
213
RIFT_VARIANT_DK1,
···
183
254
enum rift_variant variant;
184
255
struct rift_config_report config;
185
256
struct rift_display_info_report display_info;
257
+
258
+
struct rift_lens_distortion *lens_distortions;
259
+
uint16_t num_lens_distortions;
186
260
};
187
261
188
262
struct rift_hmd *