From 47141c873ffd3d7af62bfba40b1adbcce0df6574 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Jul 2018 04:13:34 +0000 Subject: Finalized reading in and transforming of accel/gyro data --- src/poser_sba.c | 4 +-- src/survive_default_devices.c | 77 ++++++++++++++++++++++++++++++++++++++----- src/survive_vive.c | 4 +++ 3 files changed, 74 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index 8437d4d..1e8637c 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -109,12 +109,12 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; if (cov) { *(cov++) = d->sensor_variance + - abs((int32_t)pdl->timecode - (int32_t)scene->timecode[sensor][lh][0]) * + fabs((float)pdl->timecode - (float)scene->timecode[sensor][lh][0]) * d->sensor_variance_per_second / (double)so->timebase_hz; *(cov++) = 0; *(cov++) = 0; *(cov++) = d->sensor_variance + - abs((int32_t)pdl->timecode - (int32_t)scene->timecode[sensor][lh][1]) * + fabs((float)pdl->timecode - (float)scene->timecode[sensor][lh][1]) * d->sensor_variance_per_second / (double)so->timebase_hz; } meas[rtn++] = a[0]; diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index d88b087..9e292f4 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -55,7 +55,7 @@ SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name, } static int jsoneq(const char *json, jsmntok_t *tok, const char *s) { - if (tok->type == JSMN_STRING && (int)strlen(s) == tok->end - tok->start && + if (tok && tok->type == JSMN_STRING && (int)strlen(s) == tok->end - tok->start && strncmp(json + tok->start, s, tok->end - tok->start) == 0) { return 0; } @@ -124,9 +124,42 @@ static void print_stack_spot(char *d, stack_entry_t *entry) { printf("-> %.*s\n", entry->key->end - entry->key->start, d + entry->key->start); } -static int process_jsonarray(SurviveObject *so, char *ct0conf, stack_entry_t *stack) { +typedef struct { + FLT position[3]; + FLT plus_x[3]; + FLT plus_z[3]; +} vive_pose_t; + +int solve_vive_pose(SurvivePose *pose, const vive_pose_t *vpose) { + if (vpose->plus_x[0] == 0.0 && vpose->plus_x[1] == 0.0 && vpose->plus_x[2] == 0.0) + return 0; + + if (vpose->plus_z[0] == 0.0 && vpose->plus_z[1] == 0.0 && vpose->plus_z[2] == 0.0) + return 0; + + FLT axis[] = {1, 0, 0, 0, 0, 1}; + + KabschCentered(pose->Rot, axis, vpose->plus_x, 2); + + // Not really sure about this; but seems right? Could also be pose->Rot * vpose->position + copy3d(pose->Pos, vpose->position); + + return 1; +} + +typedef struct { + SurviveObject *so; + vive_pose_t imu_pose; +} scratch_space_t; + +static scratch_space_t scratch_space_init(SurviveObject *so) { return (scratch_space_t){.so = so}; } + +static int process_jsonarray(scratch_space_t *scratch, char *ct0conf, stack_entry_t *stack) { + SurviveObject *so = scratch->so; jsmntok_t *tk = stack->key; SurviveContext *ctx = so->ctx; + + /// CONTEXT FREE FIELDS if (jsoneq(ct0conf, tk, "modelPoints") == 0) { if (ParsePoints(ctx, so, ct0conf, &so->sensor_locations, tk)) { return -1; @@ -136,7 +169,6 @@ static int process_jsonarray(SurviveObject *so, char *ct0conf, stack_entry_t *st return -1; } } - else if (jsoneq(ct0conf, tk, "acc_bias") == 0) { int32_t count = (tk + 1)->size; FLT *values = NULL; @@ -172,10 +204,34 @@ static int process_jsonarray(SurviveObject *so, char *ct0conf, stack_entry_t *st } } + /// Context sensitive fields + else if (stack->previous && jsoneq(ct0conf, stack->previous->key, "imu") == 0) { + + struct field { + const char *name; + FLT *vals; + }; + + struct field imufields[] = {{"plus_x", scratch->imu_pose.plus_x}, + {"plus_z", scratch->imu_pose.plus_z}, + {"position", scratch->imu_pose.position}}; + + for (int i = 0; i < sizeof(imufields) / sizeof(struct field); i++) { + if (jsoneq(ct0conf, tk, imufields[i].name) == 0) { + int32_t count = (tk + 1)->size; + assert(count == 3); + if (count == 3) { + parse_float_array_in_place(ct0conf, tk + 2, imufields[i].vals, count); + } + break; + } + } + } + return 0; } -static int process_jsontok(SurviveObject *so, char *d, stack_entry_t *stack, jsmntok_t *t, int count) { +static int process_jsontok(scratch_space_t *scratch, char *d, stack_entry_t *stack, jsmntok_t *t, int count) { int i, j, k; assert(count >= 0); if (count == 0) { @@ -192,16 +248,16 @@ static int process_jsontok(SurviveObject *so, char *d, stack_entry_t *stack, jsm for (i = 0; i < t->size; i++) { entry.key = t + 1 + j; print_stack_spot(d, &entry); - j += process_jsontok(so, d, &entry, entry.key, count - j); + j += process_jsontok(scratch, d, &entry, entry.key, count - j); - j += process_jsontok(so, d, &entry, t + 1 + j, count - j); + j += process_jsontok(scratch, d, &entry, t + 1 + j, count - j); } return j + 1; } else if (t->type == JSMN_ARRAY) { - process_jsonarray(so, d, stack); + process_jsonarray(scratch, d, stack); j = 0; for (i = 0; i < t->size; i++) { - j += process_jsontok(so, d, stack, t + 1 + j, count - j); + j += process_jsontok(scratch, d, stack, t + 1 + j, count - j); } return j + 1; } @@ -228,7 +284,10 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { return -2; } - process_jsontok(so, ct0conf, 0, t, r); + scratch_space_t scratch = scratch_space_init(so); + process_jsontok(&scratch, ct0conf, 0, t, r); + + solve_vive_pose(&so->relative_imu_pose, &scratch.imu_pose); // Handle device-specific sacling. if (strcmp(so->codename, "HMD") == 0) { diff --git a/src/survive_vive.c b/src/survive_vive.c index 5bf0800..cc2303c 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1021,6 +1021,8 @@ void calibrate_acc(SurviveObject* so, FLT* agm) { agm[1] *= so->acc_scale[1]; agm[2] *= so->acc_scale[2]; } + + quatrotatevector(agm, so->relative_imu_pose.Rot, agm); } void calibrate_gyro(SurviveObject* so, FLT* agm) { @@ -1035,6 +1037,8 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) { agm[1] *= so->gyro_scale[1]; agm[2] *= so->gyro_scale[2]; } + + quatrotatevector(agm, so->relative_imu_pose.Rot, agm); } typedef struct -- cgit v1.2.3