aboutsummaryrefslogtreecommitdiff
path: root/src/poser_turveytori.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/poser_turveytori.c')
-rw-r--r--src/poser_turveytori.c227
1 files changed, 184 insertions, 43 deletions
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 80e8d89..94d572e 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1,4 +1,5 @@
#include <survive.h>
+#include "survive_config.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -15,6 +16,8 @@
#endif
+static int ttDebug = 0;
+
#define PointToFlts(x) ((FLT*)(x))
typedef struct
@@ -85,7 +88,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;
@@ -412,7 +416,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 +589,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
break;
}
}
- printf(" i=%3d ", i);
+ if (ttDebug) printf(" i=%3d ", i);
return lastPoint;
}
@@ -772,6 +776,20 @@ 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);
+
+ return throwaway;
+ }
+
+
+
FLT fitness = 0;
for (size_t i = 0; i < obj->numSensors; i++)
{
@@ -910,7 +928,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,25 +1029,63 @@ 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)
+//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);
- 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
@@ -1105,9 +1161,9 @@ 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("+");
- WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
+ g *= 1.04;
+ //printf("+");
+ //WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
}
else
{
@@ -1115,12 +1171,13 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
//printf("- , %f\n", point4[3]);
//#endif
g *= 0.7;
- printf("-");
+ //printf("-");
+ //printf("%3f", lastMatchFitness);
}
}
- printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness);
+ if (ttDebug) printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness);
}
@@ -1129,9 +1186,9 @@ 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 zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 };
FLT quat1[4];
quatfromaxisangle(quat1, zAxis, theta);
@@ -1143,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);
@@ -1150,6 +1208,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)
{
@@ -1280,21 +1364,29 @@ 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!
+ 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
@@ -1302,22 +1394,30 @@ 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(objPos, rot, refinedEstimateGd);
- FLT rotQuat[4];
+ FLT rotQuat2[4];
+ FLT rot2[4];
- quatfromaxisangle(rotQuat, rot, rot[3]);
+ quatfromaxisangle(rotQuat2, rot, rot[3]);
+ 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};
@@ -1405,7 +1505,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 +1527,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 +1638,14 @@ 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 (so->ctx->rawposeproc)
+ {
+ so->ctx->rawposeproc(so, lh, pos, quat);
+ }
+
+ if (ttDebug) printf("!\n");
}
@@ -1547,6 +1664,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)
{
@@ -1657,8 +1782,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);
@@ -1693,8 +1818,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);
@@ -1720,9 +1845,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++)
// {
@@ -1732,6 +1857,22 @@ 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);
+
+ config_save(ctx, "config.json");
free(to);
//printf( "Full scene data.\n" );