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/linmath.c | 39 ++++++++++++++++++++++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) (limited to 'redist/linmath.c') 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}}; -- cgit v1.2.3