From a9739c252fab9b32684148625b08b2cfb89a3751 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Wed, 20 Dec 2017 18:19:03 -0700 Subject: Save Calibration Output To Config --- src/poser_turveytori.c | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 80e8d89..d0b9ab8 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1732,6 +1732,10 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // } //} + config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0); + config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1); + + config_save(ctx, "config.json"); free(to); //printf( "Full scene data.\n" ); -- cgit v1.2.3 From 283a395d8d63e4532173f9497f2ea8cd154a4758 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 22 Dec 2017 23:06:26 -0700 Subject: Configuration Changes Added some config options. Changed a few defaults. Better output from PoserTurveyTori. Hopefully, this will make it easier for someone new to get up and running more easily. --- src/poser_turveytori.c | 45 ++++++++++++++++++++++++++++++++++----------- 1 file changed, 34 insertions(+), 11 deletions(-) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d0b9ab8..ae4592d 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -15,6 +15,8 @@ #endif +static int ttDebug = 0; + #define PointToFlts(x) ((FLT*)(x)) typedef struct @@ -412,7 +414,7 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu } fitnesses[i] = FLT_FABS(fitness); - if (deubgPrint) + if (0) { printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness); } @@ -585,7 +587,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, break; } } - printf(" i=%3d ", i); + if (ttDebug) printf(" i=%3d ", i); return lastPoint; } @@ -910,7 +912,7 @@ static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point l rotatearoundaxis(posOut, posOut, rotation, rotation[3]); - printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); + if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -1011,7 +1013,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini break; } } - printf(" Ri=%d ", i); + if (ttDebug) printf(" Ri=%d ", i); } static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { @@ -1020,7 +1022,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); quatrotatevector(objPoint, rotation, objPoint); - printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } @@ -1106,7 +1108,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; - printf("+"); + if (ttDebug) printf("+"); WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); } else @@ -1115,12 +1117,12 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("- , %f\n", point4[3]); //#endif g *= 0.7; - printf("-"); + if (ttDebug) printf("-"); } } - printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1280,7 +1282,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + if (ttDebug) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; // this is axis/ angle rotation, not a quaternion! @@ -1405,7 +1407,16 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob so->FromLHPose[lh].Rot[2] = so->OutPose.Rot[2]; so->FromLHPose[lh].Rot[3] = so->OutPose.Rot[3]; - printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + if (ttDebug) printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + + posOut[0] = wcPos[0]; + posOut[1] = wcPos[1]; + posOut[2] = wcPos[2]; + + quatOut[0] = so->OutPose.Rot[0]; + quatOut[1] = so->OutPose.Rot[1]; + quatOut[2] = so->OutPose.Rot[2]; + quatOut[3] = so->OutPose.Rot[3]; if (logFile) { @@ -1418,6 +1429,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob toriData->lastLhPos[lh].y = refinedEstimateGd.y; toriData->lastLhPos[lh].z = refinedEstimateGd.z; + return refinedEstimateGd; } @@ -1528,7 +1540,10 @@ static void QuickPose(SurviveObject *so, int lh) SolveForLighthouse(pos, quat, to, so, 0, lh, 0); - printf("!\n"); + + printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + + if (ttDebug) printf("!\n"); } @@ -1547,6 +1562,14 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) SurviveContext * ctx = so->ctx; ToriData * td = so->PoserData; + static int firstRun = 1; + + if (firstRun) + { + ttDebug = config_read_uint32(ctx->global_config_values, "TurveyToriDebug", 0); + + firstRun = 0; + } if (!td) { -- cgit v1.2.3 From ec564d70daa8c1a66018f9606b02b873ae792c84 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 28 Dec 2017 08:03:37 -0700 Subject: Start work on determining rotation using quaternions only Rotation was previously approximated using axis/angle This change starts down the path of using quaternions exclusively. This change appears to give at least as good as answers as the axis/angle model in basic cases (also only tested with 1 lighthouse), but it is currently much slower and runs in unpredictable time. --- src/poser_turveytori.c | 150 ++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 122 insertions(+), 28 deletions(-) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ae4592d..fed5762 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -87,7 +87,8 @@ typedef struct int lastAxis[NUM_LIGHTHOUSES]; Point lastLhPos[NUM_LIGHTHOUSES]; - FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; +// FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4]; + FLT lastLhRotQuat[NUM_LIGHTHOUSES][4]; } ToriData; @@ -774,6 +775,21 @@ FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, Tra // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj) { + +// TODO: ************************************************************************************************** THIS LIES!!!! NEED TO DO THIS IN QUATERNIONS!!!!!!!!!!!!!!!!! + { + FLT axisAngle[4]; + + axisanglefromquat(&(axisAngle[3]), axisAngle, quaternion); + + FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); + + int a = throwaway; + return throwaway; + } + + + FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { @@ -1015,23 +1031,61 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } if (ttDebug) printf(" Ri=%d ", i); } -static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) +//static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) +//{ +// FLT reverseRotation[4] = { rotation[0], rotation[1], rotation[2], -rotation[3] }; +// FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; +// +// //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); +// quatrotatevector(objPoint, rotation, objPoint); +// if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +//} +static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; - FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; - + posOut[0] = -lhPoint.x; + posOut[1] = -lhPoint.y; + posOut[2] = -lhPoint.z; + + FLT inverseRotation[4]; + + quatgetreciprocal(inverseRotation, rotation); + + FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; + //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - quatrotatevector(objPoint, rotation, objPoint); - if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + quatrotatevector(posOut, inverseRotation, posOut); +// if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } +//static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) +//{ +// posOut[0] = -lhPoint.x; +// posOut[1] = -lhPoint.y; +// posOut[2] = -lhPoint.z; +// +// rotatearoundaxis(posOut, posOut, rotation, rotation[3]); +// +// if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); +//} static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; + FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj); + //{ + // FLT axisAngle[4]; + + // axisanglefromquat(&(axisAngle[3]), axisAngle, initialEstimate); + + // FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); + + // int a = throwaway; + //} + + quatcopy(rotOut, initialEstimate); // The values below are somewhat magic, and definitely tunable @@ -1108,8 +1162,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; - if (ttDebug) printf("+"); - WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); + printf("+"); + //WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); } else { @@ -1117,12 +1171,12 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("- , %f\n", point4[3]); //#endif g *= 0.7; - if (ttDebug) printf("-"); + printf("-"); } } - if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1133,7 +1187,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2}; + FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 }; FLT quat1[4]; quatfromaxisangle(quat1, zAxis, theta); @@ -1152,6 +1206,32 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } +void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh) +{ + + // Step 1, create initial quaternion for guess. + // This should have the lighthouse directly facing the tracked object. + Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; + FLT theta = atan2(-lh.x, -lh.y); + FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 }; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + + //quatfrom2vectors(0,0) + // not correcting for phi, but that's less important. + + + // Step 2, optimize the axis/ angle to match the data. + //RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + + + //// Step 2, optimize the quaternion to match the data. + RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); + + //WhereIsTheTrackedObjectQuaternion(rotOut, lh); + +} + static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration) { @@ -1286,17 +1366,25 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; // this is axis/ angle rotation, not a quaternion! + FLT rotQuat[4]; // this is a quaternion! // if we've already guessed at the rotation of the lighthouse, // then let's use that as a starting guess, because it's probably // going to make convergence happen much faster. - if (toriData->lastLhRotAxisAngle[lh][0] != 0) - { - rot[0] = toriData->lastLhRotAxisAngle[lh][0]; - rot[1] = toriData->lastLhRotAxisAngle[lh][1]; - rot[2] = toriData->lastLhRotAxisAngle[lh][2]; - rot[3] = toriData->lastLhRotAxisAngle[lh][3]; - } + //if (toriData->lastLhRotAxisAngle[lh][0] != 0) + //{ + // rot[0] = toriData->lastLhRotAxisAngle[lh][0]; + // rot[1] = toriData->lastLhRotAxisAngle[lh][1]; + // rot[2] = toriData->lastLhRotAxisAngle[lh][2]; + // rot[3] = toriData->lastLhRotAxisAngle[lh][3]; + //} + //if (toriData->lastLhRotQuat[lh][0] != 0) + //{ + // rotQuat[0] = toriData->lastLhRotQuat[lh][0]; + // rotQuat[1] = toriData->lastLhRotQuat[lh][1]; + // rotQuat[2] = toriData->lastLhRotQuat[lh][2]; + // rotQuat[3] = toriData->lastLhRotQuat[lh][3]; + //} // Given the relative position of the lighthouse // to the tracked object, in the tracked object's coordinate @@ -1304,22 +1392,28 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob // tracked object's coordinate system. // TODO: I believe this could be radically improved // using an SVD. + SolveForRotationQuat(rotQuat, obj, refinedEstimateGd); SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; + FLT objPos2[3]; - { - toriData->lastLhRotAxisAngle[lh][0] = rot[0]; - toriData->lastLhRotAxisAngle[lh][1] = rot[1]; - toriData->lastLhRotAxisAngle[lh][2] = rot[2]; - toriData->lastLhRotAxisAngle[lh][3] = rot[3]; - } + //{ + // toriData->lastLhRotQuat[lh][0] = rotQuat[0]; + // toriData->lastLhRotQuat[lh][1] = rotQuat[1]; + // toriData->lastLhRotQuat[lh][2] = rotQuat[2]; + // toriData->lastLhRotQuat[lh][3] = rotQuat[3]; + //} + + WhereIsTheTrackedObjectAxisAngle(objPos2, rot, refinedEstimateGd); + WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd); - WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + FLT rotQuat2[4]; + FLT rot2[4]; - FLT rotQuat[4]; + quatfromaxisangle(rotQuat2, rot, rot[3]); + axisanglefromquat(&(rot2[3]), rot2, rotQuat); - quatfromaxisangle(rotQuat, rot, rot[3]); //{ //FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; -- cgit v1.2.3 From 6dea9ec51d21c9f58a6c82837b6dea0e67c20207 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 28 Dec 2017 08:25:02 -0700 Subject: MUCH better tracking Works with 2 lighthouses. Tracking from both lighthouses agree *much* better than before Inverting the tracker no longer screws up tracking Still much work to do to remove all axis angle and speed up/ make predictable the algorithm to estimate the rotation of the LH relative to the tracked object. --- src/poser_turveytori.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index fed5762..bd165ec 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1161,8 +1161,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //#ifdef TORI_DEBUG //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif - g *= 1.02; - printf("+"); + g *= 1.04; + //printf("+"); //WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); } else @@ -1171,7 +1171,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in //printf("- , %f\n", point4[3]); //#endif g *= 0.7; - printf("-"); + //printf("-"); + //printf("%3f", lastMatchFitness); } @@ -1404,8 +1405,6 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob // toriData->lastLhRotQuat[lh][3] = rotQuat[3]; //} - WhereIsTheTrackedObjectAxisAngle(objPos2, rot, refinedEstimateGd); - WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd); FLT rotQuat2[4]; @@ -1415,6 +1414,10 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob axisanglefromquat(&(rot2[3]), rot2, rotQuat); +// WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); // this is the original axis angle one + WhereIsTheTrackedObjectAxisAngle(objPos, rot2, refinedEstimateGd); // this one is axis angle, but using data derived by quaternions. + // WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd); <--------------This is hte one we need to use, might need to be fixed. + //{ //FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; -- cgit v1.2.3 From 8eda10ed94b66a8bb388cc5710dc6b45b1901993 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 28 Dec 2017 08:56:43 -0700 Subject: Use accelerometer to determine "up" The accelerometer will be used to determine "up" instead of blindly using +z during calibration --- src/poser_turveytori.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index bd165ec..ac979c7 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1777,8 +1777,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - //quatrotatevector(norm, downQuat, norm); - //quatrotatevector(point, downQuat, point); + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); //rotatearoundaxis(norm, norm, axis, angle); //rotatearoundaxis(point, point, axis, angle); @@ -1813,8 +1813,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - //quatrotatevector(norm, downQuat, norm); - //quatrotatevector(point, downQuat, point); + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); //rotatearoundaxis(norm, norm, axis, angle); //rotatearoundaxis(point, point, axis, angle); @@ -1840,9 +1840,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } - // This code block rotates the lighthouse fixes to accound for any time the tracked object + // This code block rotates the lighthouse fixes to account for any time the tracked object // is oriented other than +z = up - // This REALLY DOESN'T WORK!!! + //This REALLY DOESN'T WORK!!! //{ // for (int lh = 0; lh < 2; lh++) // { -- cgit v1.2.3 From 735a8bd11070b0c563e891ff8b70ce297a52a367 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 03:54:29 -0700 Subject: Add standard output mechanism for posers Added a raw pose output/ callback that the posers can call when they have calculated a pose. --- src/poser_turveytori.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ac979c7..28e533e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1177,7 +1177,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in } - printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); + if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1638,7 +1638,11 @@ static void QuickPose(SurviveObject *so, int lh) SolveForLighthouse(pos, quat, to, so, 0, lh, 0); - printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); + if (so->ctx->rawposeproc) + { + so->ctx->rawposeproc(so, lh, pos, quat); + } if (ttDebug) printf("!\n"); } -- cgit v1.2.3 From ed85661121c3ed8769a69d059fd3162aec33c6fe Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Fri, 5 Jan 2018 04:09:54 -0700 Subject: Update test.c to do calibration --- src/poser_turveytori.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 28e533e..ad30f6c 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1856,6 +1856,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // } //} + for (int i=0; i < ctx->activeLighthouses; i++) + { + printf("Lighthouse Pose: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", + i, + ctx->bsd[i].Pose.Pos[0], + ctx->bsd[i].Pose.Pos[1], + ctx->bsd[i].Pose.Pos[2], + ctx->bsd[i].Pose.Rot[0], + ctx->bsd[i].Pose.Rot[1], + ctx->bsd[i].Pose.Rot[2], + ctx->bsd[i].Pose.Rot[3]); + } config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0); config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1); -- cgit v1.2.3 From 91fc90bf9171fe588fd11492bdae7419d5ccd921 Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Mon, 15 Jan 2018 17:10:15 +0100 Subject: fix segfault in config_set_lighthouse() also include the survive_config.h header to avoid various implicit function declaration warnings on gcc --- src/poser_turveytori.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index ad30f6c..59ff25e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -1868,8 +1869,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) ctx->bsd[i].Pose.Rot[2], ctx->bsd[i].Pose.Rot[3]); } - config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0); - config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1); + config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0); + config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1); config_save(ctx, "config.json"); -- cgit v1.2.3 From 7817da63526f35d10d6d4f6b3d9c02280719e023 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 15 Jan 2018 21:50:10 -0700 Subject: Fix compiler warnings --- src/poser_turveytori.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 59ff25e..de1516a 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1,5 +1,5 @@ #include -#include +#include "survive_config.h" #include #include #include @@ -785,7 +785,6 @@ FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObj FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj); - int a = throwaway; return throwaway; } @@ -1397,7 +1396,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob SolveForRotationQuat(rotQuat, obj, refinedEstimateGd); SolveForRotation(rot, obj, refinedEstimateGd); FLT objPos[3]; - FLT objPos2[3]; + //FLT objPos2[3]; //{ // toriData->lastLhRotQuat[lh][0] = rotQuat[0]; -- cgit v1.2.3 From b2eb7569a8963917116c4520e15b17f0578a2509 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 15 Jan 2018 22:40:14 -0700 Subject: Fix a few warnings --- src/poser_turveytori.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/poser_turveytori.c') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index de1516a..94d572e 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1050,7 +1050,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point quatgetreciprocal(inverseRotation, rotation); - FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; + //FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z }; //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); quatrotatevector(posOut, inverseRotation, posOut); @@ -1186,7 +1186,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 1, create initial quaternion for guess. // This should have the lighthouse directly facing the tracked object. - Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; + //Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 }; FLT quat1[4]; @@ -1200,6 +1200,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + // TODO: Need to use the quaternion version here!!! //// Step 2, optimize the quaternion to match the data. //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); -- cgit v1.2.3