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 ++++++++++++++++++++++++++++++++++----------- src/survive.c | 6 +++++- src/survive_cal.c | 10 +++++++--- src/survive_config.c | 2 +- 4 files changed, 47 insertions(+), 16 deletions(-) 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) { diff --git a/src/survive.c b/src/survive.c index 057462f..97c839b 100755 --- a/src/survive.c +++ b/src/survive.c @@ -96,7 +96,8 @@ SurviveContext * survive_init( int headless ) } i = 0; - const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefaultPoser", "PoserDummy" ); + //const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy"); + const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori"); PoserCB PreferredPoserCB = 0; const char * FirstPoser = 0; printf( "Available posers:\n" ); @@ -119,6 +120,9 @@ SurviveContext * survive_init( int headless ) ctx->objs[i]->PoserFn = PreferredPoserCB; } + // saving the config extra to make sure that the user has a config file they can change. + config_save(ctx, "config.json"); + return ctx; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 0dbadd3..87d8c0b 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -124,8 +124,11 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->numPoseObjects = 0; - const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" ); - const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 ); + // setting the required trackers for calibration to be permissive to make it easier for a newbie to start-- + // basically, libsurvive will detect whatever they have plugged in and start using that. +// const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1"); + const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", ""); + const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 1 ); size_t requiredTrackersFound = 0; for (int j=0; j < ctx->objs_ct; j++) @@ -169,7 +172,8 @@ void survive_cal_install( struct SurviveContext * ctx ) } const char * DriverName; - const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" ); +// const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow"); + const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserTurveyTori"); PoserCB PreferredPoserCB = 0; const char * FirstPoser = 0; printf( "Available posers:\n" ); diff --git a/src/survive_config.c b/src/survive_config.c index a24364f..0961651 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -103,7 +103,7 @@ void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8 } } - assert(found); // throw an assertion if we didn't find it... Is this good? not necessarily? +// assert(found); // throw an assertion if we didn't find it... Is this good? not necessarily? if (!found) { return; -- cgit v1.2.3