The open source OpenXR runtime
d/vive: Use raw imu samples for slam
authored by
Mateo de Mayo
and committed by
Simon Zeni
a9b85c99
6afed4dc
+58
-49
+58
-49
src/xrt/drivers/vive/vive_device.c
+58
-49
src/xrt/drivers/vive/vive_device.c
···
330
330
return 0;
331
331
}
332
332
333
+
static inline void
334
+
convert_imu_to_openxr(struct vive_device *d, struct xrt_vec3 *gyro, struct xrt_vec3 *accel)
335
+
{
336
+
switch (d->config.variant) {
337
+
case VIVE_VARIANT_VIVE: { // flip all except x axis
338
+
accel->x = +accel->x;
339
+
accel->y = -accel->y;
340
+
accel->z = -accel->z;
341
+
gyro->x = +gyro->x;
342
+
gyro->y = -gyro->y;
343
+
gyro->z = -gyro->z;
344
+
break;
345
+
}
346
+
case VIVE_VARIANT_PRO: { // flip all except y axis
347
+
accel->x = -accel->x;
348
+
accel->y = +accel->y;
349
+
accel->z = -accel->z;
350
+
gyro->x = -gyro->x;
351
+
gyro->y = +gyro->y;
352
+
gyro->z = -gyro->z;
353
+
break;
354
+
}
355
+
case VIVE_VARIANT_PRO2: {
356
+
accel->x = -accel->x;
357
+
accel->y = accel->y;
358
+
accel->z = -accel->z;
359
+
gyro->x = -gyro->x;
360
+
gyro->y = gyro->y;
361
+
gyro->z = -gyro->z;
362
+
break;
363
+
}
364
+
case VIVE_VARIANT_INDEX: { // Flip all axis and re-order.
365
+
struct xrt_vec3 accel_fixed;
366
+
accel_fixed.x = -accel->y;
367
+
accel_fixed.y = -accel->x;
368
+
accel_fixed.z = -accel->z;
369
+
*accel = accel_fixed;
370
+
371
+
struct xrt_vec3 gyro_fixed;
372
+
gyro_fixed.x = -gyro->y;
373
+
gyro_fixed.y = -gyro->x;
374
+
gyro_fixed.z = -gyro->z;
375
+
*gyro = gyro_fixed;
376
+
377
+
break;
378
+
}
379
+
default: {
380
+
VIVE_ERROR(d, "Unhandled Vive variant");
381
+
return;
382
+
}
383
+
}
384
+
}
385
+
333
386
static void
334
387
update_imu(struct vive_device *d, const void *buffer)
335
388
{
···
390
443
scale * acc_scale[1] * acc[1] - acc_bias[1],
391
444
scale * acc_scale[2] * acc[2] - acc_bias[2],
392
445
};
446
+
struct xrt_vec3 raw_accel = {.x = scale * acc[0], .y = scale * acc[1], .z = scale * acc[2]};
393
447
394
448
VIVE_TRACE(d, "ACC %f %f %f (%f - %f, %f - %f, %f - %f)", //
395
449
acceleration.x, //
···
426
480
scale * gyro_scale[1] * gyro[1] - gyro_bias[1],
427
481
scale * gyro_scale[2] * gyro[2] - gyro_bias[2],
428
482
};
483
+
struct xrt_vec3 raw_gyro = {.x = scale * gyro[0], .y = scale * gyro[1], .z = scale * gyro[2]};
429
484
430
485
VIVE_TRACE(d, "GYRO %f %f %f (%f - %f, %f - %f, %f - %f)", //
431
486
angular_velocity.x, //
···
439
494
gyro_bias[2]); //
440
495
441
496
442
-
switch (d->config.variant) {
443
-
case VIVE_VARIANT_VIVE:
444
-
// flip all except x axis
445
-
acceleration.x = +acceleration.x;
446
-
acceleration.y = -acceleration.y;
447
-
acceleration.z = -acceleration.z;
448
-
449
-
angular_velocity.x = +angular_velocity.x;
450
-
angular_velocity.y = -angular_velocity.y;
451
-
angular_velocity.z = -angular_velocity.z;
452
-
break;
453
-
case VIVE_VARIANT_PRO: {
454
-
// flip all except y axis
455
-
acceleration.x = -acceleration.x;
456
-
acceleration.y = +acceleration.y;
457
-
acceleration.z = -acceleration.z;
458
-
459
-
angular_velocity.x = -angular_velocity.x;
460
-
angular_velocity.y = +angular_velocity.y;
461
-
angular_velocity.z = -angular_velocity.z;
462
-
break;
463
-
}
464
-
case VIVE_VARIANT_PRO2: {
465
-
acceleration.x = -acceleration.x;
466
-
acceleration.y = acceleration.y;
467
-
acceleration.z = -acceleration.z;
468
-
469
-
angular_velocity.x = -angular_velocity.x;
470
-
angular_velocity.y = angular_velocity.y;
471
-
angular_velocity.z = -angular_velocity.z;
472
-
} break;
473
-
474
-
case VIVE_VARIANT_INDEX: {
475
-
// Flip all axis and re-order.
476
-
struct xrt_vec3 acceleration_fixed;
477
-
acceleration_fixed.x = -acceleration.y;
478
-
acceleration_fixed.y = -acceleration.x;
479
-
acceleration_fixed.z = -acceleration.z;
480
-
acceleration = acceleration_fixed;
481
-
482
-
struct xrt_vec3 angular_velocity_fixed;
483
-
angular_velocity_fixed.x = -angular_velocity.y;
484
-
angular_velocity_fixed.y = -angular_velocity.x;
485
-
angular_velocity_fixed.z = -angular_velocity.z;
486
-
angular_velocity = angular_velocity_fixed;
487
-
} break;
488
-
default: VIVE_ERROR(d, "Unhandled Vive variant"); return;
489
-
}
497
+
convert_imu_to_openxr(d, &acceleration, &angular_velocity);
498
+
convert_imu_to_openxr(d, &raw_accel, &raw_gyro);
490
499
491
500
d->imu.sequence = seq;
492
501
···
504
513
assert(j > 0);
505
514
uint32_t age = j <= 0 ? 0 : (uint32_t)(j - 1);
506
515
507
-
vive_source_push_imu_packet(d->source, age, d->imu.last_sample_ts_ns, acceleration, angular_velocity);
516
+
vive_source_push_imu_packet(d->source, age, d->imu.last_sample_ts_ns, raw_accel, raw_gyro);
508
517
}
509
518
}
510
519