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 + 6 files changed, 95 insertions(+), 18 deletions(-) (limited to 'redist') 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 -- cgit v1.2.3