From 8a0831f2b9e458d8f1976c27f7865166ad8de6a4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 09:11:54 -0600 Subject: Moved pose to linmath to support stronger typing --- Makefile | 5 +- include/libsurvive/survive_types.h | 9 ++-- redist/Makefile | 5 ++ redist/linmath.c | 94 +++++++++++++----------------------- redist/linmath.h | 74 +++++++++++++++-------------- redist/lintest.c | 97 ++++++++++---------------------------- simple_pose_test.c | 2 +- src/poser.c | 10 ++-- src/poser_daveortho.c | 4 +- src/poser_epnp.c | 62 ++---------------------- src/poser_sba.c | 6 +-- src/survive_reproject.c | 6 +-- 12 files changed, 126 insertions(+), 248 deletions(-) diff --git a/Makefile b/Makefile index dc1c4e7..ace84d4 100644 --- a/Makefile +++ b/Makefile @@ -119,7 +119,8 @@ calibrate_tcc : $(LIBSURVIVE_C) clean : rm -rf */*/*.o *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so test_minimal_cv test_epnp test_epnp_ocv calibrate_client redist/*.o redist/*~ tools/data_server/data_server tools/lighthousefind/lighthousefind tools/lighthousefind_tori/lighthousefind-tori tools/plot_lighthouse/plot_lighthouse tools/process_rawcap/process_to_points redist/jsmntest redist/lintest +.test_redist: + cd redist && make .run_tests; - - +.run_tests: .test_redist diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 7253df0..3ea6253 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -1,6 +1,7 @@ #ifndef _SURVIVE_TYPES_H #define _SURVIVE_TYPES_H +#include "linmath.h" #include "stdint.h" #ifdef __cplusplus @@ -21,12 +22,8 @@ extern "C" { #define _FLT_format2(f) f##_format #define _FLT_format(f) _FLT_format2(f) #define FLT_format _FLT_format(FLT) - -typedef struct SurvivePose -{ - FLT Pos[3]; - FLT Rot[4]; //This is the [wxyz] quaternion, in wxyz format. -} SurvivePose; + +typedef LinmathPose SurvivePose; //Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools. //It will also require a recompile. TODO: revisit this and correct the comment once fixed. diff --git a/redist/Makefile b/redist/Makefile index 1437758..58536c9 100644 --- a/redist/Makefile +++ b/redist/Makefile @@ -9,6 +9,11 @@ lintest : lintest.c linmath.c linmath.h test_dcl : test_dcl.c dclhelpers.c minimal_opencv.c ../src/epnp/epnp.c gcc -o $@ $^ os_generic.c -DFLT=double -lpthread -lcblas -lm -llapacke -O3 -msse2 -ftree-vectorize +.run_tests: clean all + ./lintest + ./test_dcl + ./jsmntest + clean : rm -rf *.o *~ jsmntest lintest diff --git a/redist/linmath.c b/redist/linmath.c index c302f5b..16f771e 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -183,24 +183,18 @@ void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. - - - -void quatsetnone(FLT * q) -{ +void quatsetnone(LinmathQuat q) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } -void quatcopy(FLT * qout, const FLT * qin) -{ +void quatcopy(LinmathQuat qout, const LinmathQuat qin) { qout[0] = qin[0]; qout[1] = qin[1]; qout[2] = qin[2]; qout[3] = qin[3]; } -void quatfromeuler( FLT * q, const FLT * euler ) -{ +void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler) { FLT X = euler[0]/2.0f; //roll FLT Y = euler[1]/2.0f; //pitch FLT Z = euler[2]/2.0f; //yaw @@ -221,16 +215,14 @@ void quatfromeuler( FLT * q, const FLT * euler ) quatnormalize( q, q ); } -void quattoeuler( FLT * euler, const FLT * q ) -{ +void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q) { //According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009) euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2])); euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1])); euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3])); } -void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) -{ +void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians) { FLT v[3]; normalize3d( v, axis ); @@ -243,25 +235,20 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ) quatnormalize( q, q ); } -FLT quatmagnitude( const FLT * q ) -{ +FLT quatmagnitude(const LinmathQuat q) { return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); } -FLT quatinvsqmagnitude( const FLT * q ) -{ +FLT quatinvsqmagnitude(const LinmathQuat q) { return ((FLT)1.)/FLT_SQRT((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3])); } - -void quatnormalize( FLT * qout, const FLT * qin ) -{ +void quatnormalize(LinmathQuat qout, const LinmathQuat qin) { FLT imag = quatinvsqmagnitude( qin ); quatscale( qout, qin, imag ); } -void quattomatrix(FLT * matrix44, const FLT * qin) -{ +void quattomatrix(FLT *matrix44, const LinmathQuat qin) { FLT q[4]; quatnormalize(q, qin); @@ -332,8 +319,7 @@ void quatfrommatrix33(FLT *q, const FLT *m) { } } -void quatfrommatrix( FLT * q, const FLT * matrix44 ) -{ +void quatfrommatrix(LinmathQuat q, const FLT *matrix44) { //Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ FLT tr = matrix44[0] + matrix44[5] + matrix44[10]; @@ -366,8 +352,7 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 ) // Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ -void quattomatrix33(FLT * matrix33, const FLT * qin) -{ +void quattomatrix33(FLT *matrix33, const LinmathQuat qin) { FLT q[4]; quatnormalize(q, qin); @@ -399,39 +384,34 @@ void quattomatrix33(FLT * matrix33, const FLT * qin) matrix33[8] = 1 - xx - yy; } -void quatgetconjugate(FLT * qout, const FLT * qin) -{ +void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin) { qout[0] = qin[0]; qout[1] = -qin[1]; qout[2] = -qin[2]; qout[3] = -qin[3]; } -void quatgetreciprocal( FLT * qout, const FLT * qin ) -{ +void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin) { FLT m = quatinvsqmagnitude(qin); quatgetconjugate( qout, qin ); quatscale( qout, qout, m ); } -void quatsub( FLT * qout, const FLT * a, const FLT * b ) -{ +void quatsub(LinmathQuat qout, const FLT *a, const FLT *b) { qout[0] = a[0] - b[0]; qout[1] = a[1] - b[1]; qout[2] = a[2] - b[2]; qout[3] = a[3] - b[3]; } -void quatadd( FLT * qout, const FLT * a, const FLT * b ) -{ +void quatadd(LinmathQuat qout, const FLT *a, const FLT *b) { qout[0] = a[0] + b[0]; qout[1] = a[1] + b[1]; qout[2] = a[2] + b[2]; qout[3] = a[3] + b[3]; } -void quatrotateabout( FLT * qout, const FLT * q1, const FLT * q2 ) -{ +void quatrotateabout(LinmathQuat qout, const LinmathQuat q1, const LinmathQuat q2) { //NOTE: Does not normalize qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]); qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]); @@ -439,44 +419,37 @@ void quatrotateabout( FLT * qout, const FLT * q1, const FLT * q2 ) qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]); } -void quatscale( FLT * qout, const FLT * qin, FLT s ) -{ +void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s) { qout[0] = qin[0] * s; qout[1] = qin[1] * s; qout[2] = qin[2] * s; qout[3] = qin[3] * s; } - -FLT quatinnerproduct( const FLT * qa, const FLT * qb ) -{ +FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb) { return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]); } -void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb ) -{ +void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) { outvec3[0] = (qa[0]*qb[1])-(qa[1]*qb[0])-(qa[2]*qb[3])+(qa[3]*qb[2]); outvec3[1] = (qa[0]*qb[2])+(qa[1]*qb[3])-(qa[2]*qb[0])-(qa[3]*qb[1]); outvec3[2] = (qa[0]*qb[3])-(qa[1]*qb[2])+(qa[2]*qb[1])-(qa[3]*qb[0]); } -void quatevenproduct( FLT * q, FLT * qa, FLT * qb ) -{ +void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb) { q[0] = (qa[0]*qb[0])-(qa[1]*qb[1])-(qa[2]*qb[2])-(qa[3]*qb[3]); q[1] = (qa[0]*qb[1])+(qa[1]*qb[0]); q[2] = (qa[0]*qb[2])+(qa[2]*qb[0]); q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]); } -void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb ) -{ +void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) { outvec3[0] = (qa[2]*qb[3])-(qa[3]*qb[2]); outvec3[1] = (qa[3]*qb[1])-(qa[1]*qb[3]); outvec3[2] = (qa[1]*qb[2])-(qa[2]*qb[1]); } -void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) -{ +void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t) { FLT an[4]; FLT bn[4]; quatnormalize( an, qa ); @@ -511,8 +484,7 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ) } } -void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ) -{ +void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in) { //See: http://www.geeks3d.com/20141201/how-to-rotate-a-vertex-by-a-quaternion-in-glsl/ FLT tmp[3]; @@ -665,19 +637,19 @@ void matrix44transpose(FLT * mout, const FLT * minm ) } -void ApplyPoseToPoint(LINMATH_POINT pout, const LINMATH_POSE pose, const LINMATH_POINT pin) { - quatrotatevector( pout, &pose[3], pin ); - add3d( pout, pout, &pose[0] ); +void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin) { + quatrotatevector(pout, pose->Rot, pin); + add3d(pout, pout, pose->Pos); } -void ApplyPoseToPose(LINMATH_POSE pout, const LINMATH_POSE lhs_pose, const LINMATH_POSE rhs_pose) { - ApplyPoseToPoint(pout, lhs_pose, rhs_pose); - quatrotateabout(&pout[3], &lhs_pose[3], &rhs_pose[3]); +void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose) { + ApplyPoseToPoint(pout->Pos, lhs_pose, rhs_pose->Pos); + quatrotateabout(pout->Rot, lhs_pose->Rot, rhs_pose->Rot); } -void InvertPose(LINMATH_POSE poseout, const LINMATH_POSE pose) { - quatgetreciprocal(&poseout[3], &pose[3]); +void InvertPose(LinmathPose *poseout, const LinmathPose *pose) { + quatgetreciprocal(poseout->Rot, pose->Rot); - quatrotatevector(&poseout[0], &poseout[3], &pose[0]); - scale3d(&poseout[0], &poseout[0], -1); + quatrotatevector(poseout->Pos, poseout->Rot, pose->Pos); + scale3d(poseout->Pos, poseout->Pos, -1); } diff --git a/redist/linmath.h b/redist/linmath.h index 57b98d3..782a13d 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -47,6 +47,14 @@ #define FLT_FABS FLT_FABS__ #endif +typedef FLT LinmathQuat[4]; // This is the [wxyz] quaternion, in wxyz format. +typedef FLT LinmathPoint3d[3]; +typedef FLT linmathVec3d[3]; + +typedef struct LinmathPose { + LinmathPoint3d Pos; + LinmathQuat Rot; +} LinmathPose; //NOTE: Inputs may never be output with cross product. void cross3d( FLT * out, const FLT * a, const FLT * b ); @@ -72,52 +80,48 @@ FLT anglebetween3d( FLT * a, FLT * b ); void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest); -void axisanglefromquat(FLT *angle, FLT *axis, FLT *quat); - +void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat); //Quaternion things... -void quatsetnone( FLT * q ); -void quatcopy( FLT * qout, const FLT * qin ); -void quatfromeuler( FLT * q, const FLT * euler ); -void quattoeuler( FLT * euler, const FLT * q ); -void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians ); -FLT quatmagnitude( const FLT * q ); -FLT quatinvsqmagnitude( const FLT * q ); -void quatnormalize( FLT * qout, const FLT * qin ); //Safe for in to be same as out. -void quattomatrix( FLT * matrix44, const FLT * q ); -void quatfrommatrix( FLT * q, const FLT * matrix44 ); -void quatfrommatrix33(FLT *q, const FLT *matrix33); -void quatgetconjugate( FLT * qout, const FLT * qin ); -void quatgetreciprocal( FLT * qout, const FLT * qin ); -void quatsub( FLT * qout, const FLT * a, const FLT * b ); -void quatadd( FLT * qout, const FLT * a, const FLT * b ); -void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ); //same as quat multiply, not piecewise multiply. -void quatscale( FLT * qout, const FLT * qin, FLT s ); -FLT quatinnerproduct( const FLT * qa, const FLT * qb ); -void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb ); -void quatevenproduct( FLT * q, FLT * qa, FLT * qb ); -void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb ); -void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t ); -void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in ); -void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest); - -//Poses are Position: [x, y, z] Quaternion: [q, x, y, z] -typedef FLT *LINMATH_POSE; - -// Points are [x, y, z] -typedef FLT *LINMATH_POINT; +typedef FLT LinmathEulerAngle[3]; + +void quatsetnone(LinmathQuat q); +void quatcopy(LinmathQuat q, const LinmathQuat qin); +void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler); +void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q); +void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians); +FLT quatmagnitude(const LinmathQuat q); +FLT quatinvsqmagnitude(const LinmathQuat q); +void quatnormalize(LinmathQuat qout, const LinmathQuat qin); // Safe for in to be same as out. +void quattomatrix(FLT *matrix44, const LinmathQuat q); +void quatfrommatrix(LinmathQuat q, const FLT *matrix44); +void quatfrommatrix33(LinmathQuat q, const FLT *matrix33); +void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin); +void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin); +void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); +void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b); +void quatrotateabout(LinmathQuat qout, const LinmathQuat a, + const LinmathQuat b); // same as quat multiply, not piecewise multiply. +void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s); +FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb); +void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); +void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb); +void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb); +void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t); +void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in); +void quatfrom2vectors(LinmathQuat q, const FLT *src, const FLT *dest); // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space -void ApplyPoseToPoint(LINMATH_POINT pout, const LINMATH_POSE pose, const LINMATH_POINT pin); +void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin); // This is the quat equivalent of 'pout = lhs_pose * rhs_pose' if poses were a 4x4 matrix in homogenous space -void ApplyPoseToPose(LINMATH_POSE pout, const LINMATH_POSE lhs_pose, const LINMATH_POSE rhs_pose); +void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose); // This is the quat equivlant of 'pose_in^-1'; so that ApplyPoseToPose(..., InvertPose(..., pose_in), pose_in) == // Identity ( [0, 0, 0], [1, 0, 0, 0] ) // by definition. -void InvertPose(LINMATH_POSE poseout, const LINMATH_POSE pose_in); +void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in); // Matrix Stuff diff --git a/redist/lintest.c b/redist/lintest.c index 4765e68..4c53da7 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -32,72 +32,72 @@ void printFLTA(FLT *a, int length) { void testInvertPose() { - FLT rotAroundYOffset[7] = {1, 1, 1, .5, 0, .5, 0}; - FLT pose_out[7]; + LinmathPose rotAroundYOffset = {1, 1, 1, .5, 0, .5, 0}; + LinmathPose pose_out; { - FLT expected[] = {1, -1, -1, 0.7071068, 0, -0.7071068, 0}; - InvertPose(pose_out, rotAroundYOffset); - ASSERT_FLTA_EQUALS(pose_out, expected, 7); + LinmathPose expected = {1, -1, -1, 0.7071068, 0, -0.7071068, 0}; + InvertPose(&pose_out, &rotAroundYOffset); + ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7); FLT identity[] = {0, 0, 0, 1, 0, 0, 0}; - ApplyPoseToPose(pose_out, expected, rotAroundYOffset); - quatnormalize(&pose_out[3], &pose_out[3]); - ASSERT_FLTA_EQUALS(pose_out, identity, 7); + ApplyPoseToPose(&pose_out, &expected, &rotAroundYOffset); + quatnormalize(pose_out.Rot, pose_out.Rot); + ASSERT_FLTA_EQUALS(pose_out.Pos, identity, 7); } } void testApplyPoseToPose() { - FLT rotAroundYOffset[7] = {1, 1, 1, 0, 0, 1, 0}; - FLT pose_out[7]; + LinmathPose rotAroundYOffset = {1, 1, 1, 0, 0, 1, 0}; + LinmathPose pose_out; { - FLT pt[] = {0, 1, 0, 0, 0, 1, 0}; - FLT expected[] = {1, 2, 1, -1, 0, 0, 0}; - ApplyPoseToPose(pose_out, rotAroundYOffset, pt); - quatnormalize(&pose_out[3], &pose_out[3]); - ASSERT_FLTA_EQUALS(pose_out, expected, 7); + LinmathPose pt = {0, 1, 0, 0, 0, 1, 0}; + LinmathPose expected = {1, 2, 1, -1, 0, 0, 0}; + ApplyPoseToPose(&pose_out, &rotAroundYOffset, &pt); + quatnormalize(pose_out.Rot, pose_out.Rot); + ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7); } { - FLT pt[] = {0, 1, 0, 0, 1, 0, 0}; - FLT expected[] = {1, 2, 1, 0, 0, 0, -1}; - ApplyPoseToPose(pose_out, rotAroundYOffset, pt); - ASSERT_FLTA_EQUALS(pose_out, expected, 7); + LinmathPose pt = {0, 1, 0, 0, 1, 0, 0}; + LinmathPose expected = {1, 2, 1, 0, 0, 0, -1}; + ApplyPoseToPose(&pose_out, &rotAroundYOffset, &pt); + ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7); } } void testApplyPoseToPoint() { - FLT rotAroundY[7] = {0, 0, 0, 0, 0, 1, 0}; + LinmathPose rotAroundY = {0, 0, 0, 0, 0, 1, 0}; FLT pt_out[3]; { FLT pt[3] = {0, 1, 0}; FLT expected[3] = {0, 1, 0}; - ApplyPoseToPoint(pt_out, rotAroundY, pt); + ApplyPoseToPoint(pt_out, &rotAroundY, pt); ASSERT_FLTA_EQUALS(pt_out, expected, 3); } { FLT pt[3] = {1, 1, 0}; FLT expected[3] = {-1, 1, 0}; - ApplyPoseToPoint(pt_out, rotAroundY, pt); + ApplyPoseToPoint(pt_out, &rotAroundY, pt); ASSERT_FLTA_EQUALS(pt_out, expected, 3); } - FLT rotAroundYOffset[7] = {1, 1, 1, 0, 0, 1, 0}; + LinmathPose rotAroundYOffset = {1, 1, 1, 0, 0, 1, 0}; { FLT pt[3] = {0, 1, 0}; FLT expected[3] = {1, 2, 1}; - ApplyPoseToPoint(pt_out, rotAroundYOffset, pt); + ApplyPoseToPoint(pt_out, &rotAroundYOffset, pt); ASSERT_FLTA_EQUALS(pt_out, expected, 3); } { FLT pt[3] = {1, 1, 0}; FLT expected[3] = {0, 2, 1}; - ApplyPoseToPoint(pt_out, rotAroundYOffset, pt); + ApplyPoseToPoint(pt_out, &rotAroundYOffset, pt); ASSERT_FLTA_EQUALS(pt_out, expected, 3); } } @@ -163,53 +163,8 @@ int main() printf( "%f %f %f\n", PFTHREE( pOut1 ) ); printf( "%f %f %f\n", PFTHREE( pOut2 ) ); */ - return -1; - -#endif + return 0; -#if 0 - - FLT e[3] = { 1,1,3.14 }; - FLT q[4]; - FLT m[16]; - FLT pt[3] = { 1, 1, 1 }; - - q[0] = 0; - q[1] = 0; - q[2] = 0; - q[3] = 1; - - quatrotatevector( pt, q, pt ); - printf( "%f %f %f\n", PFTHREE( pt ) ); - printf( "\n" ); - - quatfromeuler( q, e ); - printf( "%f %f %f %f\n\n", PFFOUR( q ) ); - quattomatrix(m,q); - printf( "%f %f %f %f\n", PFFOUR( &m[0] ) ); - printf( "%f %f %f %f\n", PFFOUR( &m[4] ) ); - printf( "%f %f %f %f\n", PFFOUR( &m[8] ) ); - printf( "%f %f %f %f\n\n", PFFOUR( &m[12] ) ); - quatfrommatrix(q,m ); - printf( "%f %f %f %f\n\n", PFFOUR( q ) ); - quattoeuler( e,q ); - printf( "E: %f %f %f\n", e[0], e[1], e[2] ); - - - FLT pfromlh[3] = { 0, 1, 0 }; - FLT p[3] = { 0, 1, 0 }; - quatrotatevector( p, q, p ); - printf( "%f %f %f\n", PFTHREE( p ) ); - printf( "Flipping rotation\n" ); - q[0] *= -1; //Wow that was easy. - quatrotatevector( p, q, p ); - printf( "%f %f %f\n", PFTHREE( p ) ); - - //Try setting up a pose. -// FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] ); -// ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose ); -//void InvertPose( FLT * poseout, const FLT * pose ); - #endif } diff --git a/simple_pose_test.c b/simple_pose_test.c index 91d80d7..34c15de 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -71,7 +71,7 @@ void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePos hpos[1] = pose->Pos[1]; hpos[2] = pose->Pos[2]; FLT hposin[3] = { 0, 0, 1 }; - ApplyPoseToPoint(hpos2, &pose->Pos[0], hposin); + ApplyPoseToPoint(hpos2, pose, hposin); fflush(stdout); } diff --git a/src/poser.c b/src/poser.c index 1ed63da..3fb4fe8 100644 --- a/src/poser.c +++ b/src/poser.c @@ -38,17 +38,17 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui // Start by just moving from whatever arbitrary space into object space. SurvivePose arb2object; - InvertPose(arb2object.Pos, object2arb.Pos); + InvertPose(&arb2object, &object2arb); SurvivePose lighthouse2obj; - ApplyPoseToPose(lighthouse2obj.Pos, arb2object.Pos, lighthouse2arb.Pos); + ApplyPoseToPose(&lighthouse2obj, &arb2object, &lighthouse2arb); // Now find the space with the same origin, but rotated so that gravity is up SurvivePose lighthouse2objUp = {}, object2objUp = {}; quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); // Calculate the pose of the lighthouse in this space - ApplyPoseToPose(lighthouse2objUp.Pos, object2objUp.Pos, lighthouse2obj.Pos); + ApplyPoseToPose(&lighthouse2objUp, &object2objUp, &lighthouse2obj); // Purposefully only set this once. It should only depend on the first (calculated) lighthouse if (quatmagnitude(objUp2world->Rot) == 0) { @@ -61,8 +61,8 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui // Find find the poses that map to the above SurvivePose obj2world, lighthouse2world; - ApplyPoseToPose(obj2world.Pos, objUp2world->Pos, object2objUp.Pos); - ApplyPoseToPose(lighthouse2world.Pos, objUp2world->Pos, lighthouse2objUp.Pos); + ApplyPoseToPose(&obj2world, objUp2world, &object2objUp); + ApplyPoseToPose(&lighthouse2world, objUp2world, &lighthouse2objUp); so->ctx->lighthouseposeproc(so->ctx, lighthouse, &lighthouse2world, &obj2world); } diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index a7ff691..c922b2e 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -105,7 +105,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) SurvivePose lh2world = so->ctx->bsd[lhid].Pose; SurvivePose obj2world; - ApplyPoseToPose(obj2world.Pos, lh2world.Pos, objpose.Pos); + ApplyPoseToPose(&obj2world, &lh2world, &objpose); PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); if (0) { @@ -195,7 +195,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) */ SurvivePose poseout; - InvertPose(poseout.Pos, objpose.Pos); + InvertPose(&poseout, &objpose); printf("INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]); diff --git a/src/poser_epnp.c b/src/poser_epnp.c index cf4294f..401ea2a 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -39,13 +39,13 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer cvGEMM(&R, &Tmp, -1, 0, 0, &T, 0); } - FLT tmp[4]; + LinmathQuat tmp; quatfrommatrix33(tmp, r[0]); // Typical camera applications have Z facing forward; the vive is contrarian and has Z going out of the // back of the lighthouse. Think of this as a rotation on the Y axis a full 180 degrees -- the quat for that is // [0 0x 1y 0z] - const FLT rt[4] = {0, 0, 1, 0}; + const LinmathQuat rt = {0, 0, 1, 0}; quatrotateabout(rtn.Rot, tmp, rt); if (!cameraToWorld) { // We have to pre-multiply the rt transform here, which means we have to also offset our position by @@ -56,50 +56,9 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer return rtn; } -/* -static int survive_standardize_calibration(SurviveObject* so, - FLT upvec[3], - SurvivePose* _object2world, - SurvivePose* _lighthouses2world0, - SurvivePose* _lighthouses2world1) { - SurvivePose object2world = {}; - SurvivePose lighthouses2world0 = *_lighthouses2world0; - SurvivePose lighthouses2world1 = *_lighthouses2world1; - - const FLT up[3] = {0, 0, 1}; - quatfrom2vectors(object2world.Rot, so->activations.accel, _object2world->Pos); - - FLT tx[4][4]; - quattomatrix(tx, object2world.Rot); - SurvivePose additionalTx = {0}; - - SurvivePose lighthouse2world = {}; - // Lighthouse is now a tx from camera -> object - ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos); - - if(quatmagnitude(additionalTx.Rot) == 0) { - SurvivePose desiredPose = lighthouse2world; - desiredPose.Pos[0] = 0.; - - quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos); - } - SurvivePose finalTx = {}; - ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos); - -} -*/ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) { - SurvivePose object2world = {//.Rot = { 0.7325378, 0.4619398, 0.1913417, 0.4619398} - .Rot = {1.}}; - const FLT up[3] = {0, 0, 1}; - - SurvivePose actual; - // quatfrom2vectors(object2world.Rot, so->activations.accel, up); - // quatfrom2vectors(object2world.Rot, up, so->activations.accel); - SurvivePose additionalTx = {0}; - for (int lh = 0; lh < 2; lh++) { epnp pnp = {.fu = 1, .fv = 1}; epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); @@ -122,22 +81,7 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) } SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true); - FLT euler[3]; - quattoeuler(euler, lighthouse2object.Rot); - SurvivePose lighthouse2world = {}; - // Lighthouse is now a tx from camera -> object - ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos); - - if (false && quatmagnitude(additionalTx.Rot) == 0) { - SurvivePose desiredPose = lighthouse2world; - desiredPose.Pos[0] = 0.; - - quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos); - } - SurvivePose finalTx = lighthouse2world; - // ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos); - - PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &finalTx, &object2world); + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0); epnp_dtor(&pnp); } diff --git a/src/poser_sba.c b/src/poser_sba.c index b5962f8..870f541 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -36,7 +36,7 @@ void metric_function(int j, int i, double *aj, double *xij, void *adata) { SurvivePose obj2world = ctx->obj_pose; FLT sensorInWorld[3] = {}; - ApplyPoseToPoint(sensorInWorld, obj2world.Pos, &so->sensor_locations[i * 3]); + ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]); survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, xij); } @@ -134,7 +134,7 @@ void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, voi quatnormalize(obj.Rot, obj.Rot); FLT xyz[3]; - ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]); + ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; @@ -157,7 +157,7 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { quatnormalize(obj.Rot, obj.Rot); FLT xyz[3]; - ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]); + ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 7bfc7d7..eabdb07 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -75,13 +75,13 @@ static FLT gibf(bool useSin, FLT v) { void survive_reproject_from_pose_with_config( const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { - FLT invq[4]; + LinmathQuat invq; quatgetreciprocal(invq, pose->Rot); - FLT tvec[3]; + LinmathPoint3d tvec; quatrotatevector(tvec, invq, pose->Pos); - FLT t_pt[3]; + LinmathPoint3d t_pt; quatrotatevector(t_pt, invq, pt); for (int i = 0; i < 3; i++) t_pt[i] = t_pt[i] - tvec[i]; -- cgit v1.2.3