aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <jdavidberger@gmail.com>2018-07-01 04:13:34 +0000
committerJustin Berger <jdavidberger@gmail.com>2018-07-01 04:13:34 +0000
commit47141c873ffd3d7af62bfba40b1adbcce0df6574 (patch)
tree245575a52f46b7fdd163b264eb17ee74863d0c28
parent70ac578a3edfdac2223bb0419445621cfb153c72 (diff)
downloadlibsurvive-47141c873ffd3d7af62bfba40b1adbcce0df6574.tar.gz
libsurvive-47141c873ffd3d7af62bfba40b1adbcce0df6574.tar.bz2
Finalized reading in and transforming of accel/gyro data
-rw-r--r--redist/json_helpers.c35
-rw-r--r--redist/json_helpers.h9
-rw-r--r--redist/linmath.c39
-rw-r--r--redist/linmath.h6
-rw-r--r--redist/minimal_opencv.c23
-rw-r--r--redist/minimal_opencv.h1
-rw-r--r--src/poser_sba.c4
-rw-r--r--src/survive_default_devices.c77
-rwxr-xr-xsrc/survive_vive.c4
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,27 +213,18 @@ 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;i<count;++i) {
+int parse_float_array_in_place(char *str, jsmntok_t *token, FLT *f, uint8_t count) {
+ for (int i = 0; i < count; ++i) {
char* end = str + token->end;
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++;
@@ -242,3 +233,21 @@ int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) {
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 <stdint.h>
-#include <jsmn.h>
#include "survive_types.h"
+#include <jsmn.h>
+#include <stdint.h>
+#include <stdio.h>
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 <math.h>
#include <string.h>
+#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