diff options
author | Mike Turvey <mturvey6@gmail.com> | 2017-12-28 08:25:02 -0700 |
---|---|---|
committer | Mike Turvey <mturvey6@gmail.com> | 2017-12-28 08:25:02 -0700 |
commit | 6dea9ec51d21c9f58a6c82837b6dea0e67c20207 (patch) | |
tree | 2c00afcd3e941e1f203b204d8f396075527061ad /src/poser_turveytori.c | |
parent | ec564d70daa8c1a66018f9606b02b873ae792c84 (diff) | |
download | libsurvive-6dea9ec51d21c9f58a6c82837b6dea0e67c20207.tar.gz libsurvive-6dea9ec51d21c9f58a6c82837b6dea0e67c20207.tar.bz2 |
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.
Diffstat (limited to 'src/poser_turveytori.c')
-rw-r--r-- | src/poser_turveytori.c | 13 |
1 files changed, 8 insertions, 5 deletions
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}; |