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 --- redist/json_helpers.c | 35 ++++++++++++-------- redist/json_helpers.h | 9 ++--- redist/linmath.c | 39 +++++++++++++++++++++- redist/linmath.h | 6 ++++ redist/minimal_opencv.c | 23 +++++++++++++ redist/minimal_opencv.h | 1 + src/poser_sba.c | 4 +-- src/survive_default_devices.c | 77 ++++++++++++++++++++++++++++++++++++++----- src/survive_vive.c | 4 +++ 9 files changed, 169 insertions(+), 29 deletions(-) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 0741c84..fa76c16 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -213,32 +213,41 @@ void json_load_file(const char* path) { free(JSON_STRING); } -int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) { - uint16_t i = 0; - - if (count==0) return 0; - - if (*f!=NULL) free(*f); - *f = malloc(sizeof(FLT) * count); - - for(i=0;iend; char* s = str+token->start; #ifdef USE_DOUBLE - (*f)[i] = strtod(s, &end); + f[i] = strtod(s, &end); #else - (*f)[i] = strtof(s, &end); + f[i] = strtof(s, &end); #endif if (s == end) { - free(*f); - *f=NULL; return 0; //not a float } token++; } + return count; +} +int parse_float_array(char *str, jsmntok_t *token, FLT **f, uint8_t count) { + uint16_t i = 0; + + if (count == 0) + return 0; + + if (*f != NULL) + free(*f); + *f = malloc(sizeof(FLT) * count); + + int rtn = parse_float_array_in_place(str, token, *f, count); + if (rtn == 0) { + free(*f); + *f = 0; + } + return count; } diff --git a/redist/json_helpers.h b/redist/json_helpers.h index 3ebf66b..2152d85 100644 --- a/redist/json_helpers.h +++ b/redist/json_helpers.h @@ -3,9 +3,10 @@ #ifndef JSON_HELPERS_H #define JSON_HELPERS_H -#include -#include #include "survive_types.h" +#include +#include +#include void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count); void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count); @@ -13,6 +14,7 @@ void json_write_uint32(FILE* f, const char* tag, uint32_t v); void json_write_float(FILE* f, const char* tag, float v); void json_write_str(FILE* f, const char* tag, const char* v); +int parse_float_array_in_place(char *str, jsmntok_t *token, FLT *values, uint8_t count); int parse_float_array(char* str, jsmntok_t* token, FLT** values, uint8_t count); void json_load_file(const char* path); @@ -20,5 +22,4 @@ extern void (*json_begin_object)(char* tag); extern void (*json_end_object)(); extern void (*json_tag_value)(char* tag, char** values, uint8_t count); - -#endif \ No newline at end of file +#endif diff --git a/redist/linmath.c b/redist/linmath.c index f4c3635..80eeaa5 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -5,6 +5,8 @@ #include #include +#include "minimal_opencv.h" + inline void cross3d(FLT *out, const FLT *a, const FLT *b) { out[0] = a[1] * b[2] - a[2] * b[1]; out[1] = a[2] * b[0] - a[0] * b[2]; @@ -286,7 +288,6 @@ inline void quattomatrix(FLT *matrix44, const LinmathQuat qin) { matrix44[14] = 0; matrix44[15] = 1; } - inline void quatfrommatrix33(FLT *q, const FLT *m) { FLT m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7], m22 = m[8]; @@ -641,5 +642,41 @@ inline void PoseToMatrix(FLT *matrix44, const LinmathPose *pose_in) { matrix44[11] = pose_in->Pos[2]; } +#include "stdio.h" + +void KabschCentered(LinmathQuat qout, const FLT *ptsA, const FLT *ptsB, int num_pts) { + // Note: The following follows along with https://en.wikipedia.org/wiki/Kabsch_algorithm + // for the most part but we use some transpose identities to let avoid unneeded transposes + CvMat A = cvMat(num_pts, 3, CV_64F, (FLT *)ptsA); + CvMat B = cvMat(num_pts, 3, CV_64F, (FLT *)ptsB); + + double _C[9] = {}; + CvMat C = cvMat(3, 3, CV_64F, _C); + cvGEMM(&B, &A, 1, 0, 0, &C, GEMM_1_T); + + double _U[9] = {}; + double _W[9] = {}; + double _VT[9] = {}; + CvMat U = cvMat(3, 3, CV_64F, _U); + CvMat W = cvMat(3, 3, CV_64F, _W); + CvMat VT = cvMat(3, 3, CV_64F, _VT); + + cvSVD(&C, &W, &U, &VT, CV_SVD_V_T | CV_SVD_MODIFY_A); + + double _R[9] = {}; + CvMat R = cvMat(3, 3, CV_64F, _R); + cvGEMM(&U, &VT, 1, 0, 0, &R, 0); + + // Enforce RH rule + if (cvDet(&R) < 0.) { + _U[2] *= -1; + _U[5] *= -1; + _U[8] *= -1; + cvGEMM(&U, &VT, 1, 0, 0, &R, 0); + } + + quatfrommatrix33(qout, _R); +} + LinmathQuat LinmathQuat_Identity = {1.0}; LinmathPose LinmathPose_Identity = {.Rot = {1.0}}; diff --git a/redist/linmath.h b/redist/linmath.h index aff46a3..f961568 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -139,6 +139,12 @@ LINMATH_EXPORT void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_po LINMATH_EXPORT void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in); LINMATH_EXPORT void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in); + +// Given num_pts in coordinate system B, and in coordinate system A, find the optimal RHS rotation +// which transforms from B to A. +// +// This assumes that the space A and B share an origin. +LINMATH_EXPORT void KabschCentered(LinmathQuat B2Atx, const FLT *ptsA, const FLT *ptsB, int num_pts); // Matrix Stuff typedef struct { diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c index c9cacf3..da7681c 100644 --- a/redist/minimal_opencv.c +++ b/redist/minimal_opencv.c @@ -374,3 +374,26 @@ SURVIVE_LOCAL_ONLY void cvReleaseMat(CvMat **mat) { free(*mat); *mat = 0; } + +SURVIVE_LOCAL_ONLY double cvDet(const CvMat *M) { + assert(M->rows == M->cols); + assert(M->rows <= 3 && "cvDet unimplemented for matrices >3"); + assert(CV_64F == CV_MAT_TYPE(M->type) && "cvDet unimplemented for float"); + double *m = M->data.db; + + switch (M->rows) { + case 1: + return m[0]; + case 2: { + return m[0] * m[3] - m[1] * m[2]; + } + case 3: { + double m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7], + m22 = m[8]; + + return m00 * (m11 * m22 - m12 * m21) - m01 * (m10 * m22 - m12 * m20) + m02 * (m10 * m21 - m11 * m20); + } + default: + abort(); + } +} diff --git a/redist/minimal_opencv.h b/redist/minimal_opencv.h index fa07c84..59b80eb 100644 --- a/redist/minimal_opencv.h +++ b/redist/minimal_opencv.h @@ -198,6 +198,7 @@ void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr, int flags); void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta, double scale); void cvTranspose(const CvMat *M, CvMat *dst); void print_mat(const CvMat *M); +double cvDet(const CvMat *M); #define CV_SVD 1 #define CV_SVD_MODIFY_A 1 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