aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/poser_daveortho.c68
-rw-r--r--src/poser_octavioradii.c721
-rw-r--r--src/poser_turveytori.c1604
-rwxr-xr-xsrc/survive_cal.c100
-rw-r--r--src/survive_cal.h14
-rw-r--r--src/survive_config.c10
-rw-r--r--src/survive_data.c318
-rw-r--r--src/survive_process.c13
-rwxr-xr-xsrc/survive_vive.c133
9 files changed, 2870 insertions, 111 deletions
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 80f65a9..906b21e 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -456,41 +456,73 @@ PRINT(ab,2,1);
//-------------------
// Orthogonalize the matrix
//-------------------
-
- PRINT_MAT(T,4,4);
-matrix44transpose(T2, T);
-//matrix44copy(T2,T);
- cross3d( &T2[0][0], &T2[1][0], &T2[2][0] );
- cross3d( &T2[2][0], &T2[0][0], &T2[1][0] );
- normalize3d( &T2[0][0], &T2[0][0] );
- normalize3d( &T2[1][0], &T2[1][0] );
- normalize3d( &T2[2][0], &T2[2][0] );
-//matrix44copy(T2,T);
-matrix44transpose(T, T2);
PRINT_MAT(T,4,4);
+#if 1
+// matrix44transpose(T2, T); //Transpose so we are
+ matrix44copy(T2,T);
+ cross3d( &T2[1][0], &T2[0][0], &T2[2][0] );
+ cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place.
+ matrix44copy(T,T2);
+// matrix44transpose(T, T2);
+
+#endif
+
+ normalize3d( &T[0][0], &T[0][0] );
+ normalize3d( &T[1][0], &T[1][0] );
+ normalize3d( &T[2][0], &T[2][0] );
+ //Change handedness
+
+ T[1][0]*=-1;
+ T[1][1]*=-1;
+ T[1][2]*=-1;
+
+/*
+ //Check Orthogonality. Yep. It's orthogonal.
+ FLT tmp[3];
+ cross3d( tmp, &T[0][0], &T[1][0] );
+ printf( "M3: %f\n", magnitude3d( tmp ) );
+ cross3d( tmp, &T[2][0], &T[1][0] );
+ printf( "M3: %f\n", magnitude3d( tmp ) );
+ cross3d( tmp, &T[2][0], &T[0][0] );
+ printf( "M3: %f\n", magnitude3d( tmp ) );
+*/
+
+// PRINT_MAT(T,4,4);
+#if 1
- //memcpy(T2,T,16*sizeof(float));
-// matrix44copy(T2,T);
+ //matrix44copy(T2,T);
matrix44transpose(T2,T);
+
quatfrommatrix( quat, &T2[0][0] );
+ printf( "QM: %f\n", quatmagnitude( quat ) );
quatnormalize(quatNorm,quat);
quattoeuler(euler,quatNorm);
- quattomatrix( T2, quatNorm );
+ quattomatrix( &T2[0][0], quatNorm );
+
+ PRINT_MAT(T2,4,4);
printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat));
- PRINT(T,4,4);
+// PRINT(T,4,4);
// matrix44copy(temp,T2);
matrix44transpose(temp,T2);
- memcpy(T2,temp,16*sizeof(float));
+
+// matrix44transpose(T2, temp);
+// memcpy(T2,temp,16*sizeof(float));
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
- T[i][j] = T2[j][i];
+ T[i][j] = temp[i][j];
}
}
- PRINT(T2,4,4);
+
+/* PRINT(T2,4,4); */
+#endif
+
+ T[1][0]*=-1;
+ T[1][1]*=-1;
+ T[1][2]*=-1;
/*
diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c
new file mode 100644
index 0000000..0d8674c
--- /dev/null
+++ b/src/poser_octavioradii.c
@@ -0,0 +1,721 @@
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+typedef struct
+{
+#define OLD_ANGLES_BUFF_LEN 3
+ FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+ int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis.
+ int lastAxis[NUM_LIGHTHOUSES];
+
+ int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+} OctavioRadiiData;
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "linmath.h"
+#include <string.h>
+#include <stdint.h>
+#include <math.h>
+
+#define PTS 32
+#define MAX_CHECKS 40000
+#define MIN_HITS_FOR_VALID 10
+
+FLT hmd_points[PTS * 3];
+FLT hmd_norms[PTS * 3];
+FLT hmd_point_angles[PTS * 2];
+int hmd_point_counts[PTS * 2];
+int best_hmd_target = 0;
+
+//Values used for RunTest()
+FLT LighthousePos[3] = { 0, 0, 0 };
+FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
+
+
+#define MAX_POINT_PAIRS 100
+
+typedef struct
+{
+ FLT x;
+ FLT y;
+ FLT z;
+} Point;
+
+typedef struct
+{
+ Point point; // location of the sensor on the tracked object;
+ Point normal; // unit vector indicating the normal for the sensor
+ double theta; // "horizontal" angular measurement from lighthouse radians
+ double phi; // "vertical" angular measurement from lighthouse in radians.
+ int id;
+} TrackedSensor;
+
+typedef struct
+{
+ size_t numSensors;
+ TrackedSensor sensor[0];
+} TrackedObject;
+
+typedef struct
+{
+ unsigned char index1;
+ unsigned char index2;
+ FLT KnownDistance;
+} PointPair;
+
+static FLT distance(Point a, Point b)
+{
+ FLT x = a.x - b.x;
+ FLT y = a.y - b.y;
+ FLT z = a.z - b.z;
+ return FLT_SQRT(x*x + y*y + z*z);
+}
+
+typedef struct
+{
+ FLT HorizAngle;
+ FLT VertAngle;
+} SensorAngles;
+
+#define SQUARED(x) ((x)*(x))
+
+static FLT calculateFitnessOld(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
+{
+ FLT fitness = 0;
+ for (size_t i = 0; i < numPairs; i++)
+ {
+ FLT estimatedDistanceBetweenPoints =
+ SQUARED(radii[pairs[i].index1])
+ + SQUARED(radii[pairs[i].index2])
+ - 2 * radii[pairs[i].index1] * radii[pairs[i].index2]
+ * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle)
+ * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle)
+ + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle);
+
+ fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance);
+ }
+
+ return FLT_SQRT(fitness);
+}
+
+// angles is an array of angles between a sensor pair
+// pairs is an array of point pairs
+// radii is the guess at the radii of those angles
+static FLT calculateFitnessOld2(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
+{
+ FLT fitness = 0;
+ for (size_t i = 0; i < numPairs; i++)
+ {
+ // These are the vectors that represent the direction for the two points.
+ // TODO: optimize by precomputing the tangent.
+ FLT v1[3], v2[3], diff[3];
+
+ v1[0] = 1;
+ v2[0] = 1;
+ v1[1] = tan(angles[pairs[i].index1].HorizAngle); // can be precomputed
+ v2[1] = tan(angles[pairs[i].index2].HorizAngle); // can be precomputed
+ v1[2] = tan(angles[pairs[i].index1].VertAngle); // can be precomputed
+ v2[2] = tan(angles[pairs[i].index2].VertAngle); // can be precomputed
+
+ // Now, normalize the vectors
+ normalize3d(v1, v1); // can be precomputed
+ normalize3d(v2, v2); // can be precomputed
+
+ // Now, given the specified radii, find where the new points are
+ scale3d(v1, v1, radii[pairs[i].index1]);
+ scale3d(v2, v2, radii[pairs[i].index2]);
+
+ // Cool, now find the vector between these two points
+ // TODO: optimize the following two funcs into one.
+ sub3d(diff, v1, v2);
+
+ FLT distance = magnitude3d(diff);
+
+ FLT t1 = magnitude3d(v1);
+ FLT t2 = magnitude3d(v2);
+
+
+
+ FLT estimatedDistanceBetweenPoints =
+
+ SQUARED(radii[pairs[i].index1])
+ + SQUARED(radii[pairs[i].index2])
+ - 2 * radii[pairs[i].index1] * radii[pairs[i].index2]
+ * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle)
+ * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle)
+ + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle);
+
+
+ //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance);
+ fitness += SQUARED(distance - pairs[i].KnownDistance);
+ }
+
+ return FLT_SQRT(fitness);
+}
+
+static FLT angleBetweenSensors(SensorAngles *a, SensorAngles *b)
+{
+ FLT angle = FLT_ACOS(FLT_COS(a->VertAngle - b->VertAngle)*FLT_COS(a->HorizAngle - b->HorizAngle));
+ //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta));
+
+ return angle;
+}
+
+// angles is an array of angles between a sensor pair
+// pairs is an array of point pairs
+// radii is the guess at the radii of those angles
+static FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs)
+{
+ FLT fitness = 0;
+ for (size_t i = 0; i < numPairs; i++)
+ {
+
+ FLT angle = angleBetweenSensors(&angles[pairs[i].index1], &angles[pairs[i].index2]);
+
+ // now we have a side-angle-side triangle, and we need to find the third side.
+
+ // The Law of Cosines says: a^2 = b^2 + c^2 ? 2bc * cosA,
+ // where A is the angle opposite side a.
+
+ // Transform this to:
+ // a = sqrt(b^2 + c^2 - 2bc * cosA) and we know the length of the missing side!
+
+ FLT b2 = (SQUARED(radii[pairs[i].index1]));
+ FLT c2 = (SQUARED(radii[pairs[i].index2]));
+ FLT bc2 = (2 * radii[pairs[i].index1] * radii[pairs[i].index2]);
+ FLT cosA = (FLT_COS(angle));
+
+ FLT angleInDegrees = angle * 180 / LINMATHPI;
+
+ FLT dist = sqrt( (SQUARED(radii[pairs[i].index1])) +
+ (SQUARED(radii[pairs[i].index2])) -
+ ( (2 * radii[pairs[i].index1] * radii[pairs[i].index2]) *
+ (FLT_COS(angle))));
+
+
+ FLT fitnessAdder = SQUARED(dist - pairs[i].KnownDistance);
+
+ if (isnan(fitnessAdder))
+ {
+ int a = 0;
+ }
+
+ //printf(" %2d %f\n", i, fitnessAdder);
+
+ //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance);
+ fitness += SQUARED(dist - pairs[i].KnownDistance);
+ }
+
+ //fitness = 1 / fitness;
+ return FLT_SQRT(fitness);
+}
+
+#define MAX_RADII 32
+
+// note gradientOut will be of the same degree as numRadii
+static void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision)
+{
+ FLT baseline = calculateFitness(angles, radii, pairs, numPairs);
+
+ for (size_t i = 0; i < numRadii; i++)
+ {
+ FLT tmpPlus[MAX_RADII];
+ memcpy(tmpPlus, radii, sizeof(*radii) * numRadii);
+ tmpPlus[i] += precision;
+ gradientOut[i] = -(calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline);
+ }
+
+ return;
+}
+
+static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT desiredMagnitude)
+{
+ FLT distanceIn = 0;
+
+ for (size_t i = 0; i < count; i++)
+ {
+ distanceIn += SQUARED(vectorToNormalize[i]);
+ }
+ distanceIn = FLT_SQRT(distanceIn);
+
+
+ FLT scale = desiredMagnitude / distanceIn;
+
+ for (size_t i = 0; i < count; i++)
+ {
+ vectorToNormalize[i] *= scale;
+ }
+
+ return;
+}
+
+
+static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
+{
+ int i = 0;
+ FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs);
+ if (estimateOut != initialEstimate)
+ {
+ memcpy(estimateOut, initialEstimate, sizeof(*estimateOut) * numRadii);
+ }
+
+
+ // The values below are somewhat magic, and definitely tunable
+ // The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+ // bigger values may be faster, especially when the initial guess is wildly off.
+ // The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+ // if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+ // That's fairly unlikely with the lighthouse problem, from expereince.
+ // The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+ // to a size that doesn't jump us out of our minima.
+ // The terminal value of g represents how close we want to get to the local minima before we're "done"
+ // The change in value of g for each iteration is intentionally very close to 1.
+ // in fact, it probably could probably be 1 without any issue. The main place where g is decremented
+ // is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+ // In those cases, we don't take the jump, and instead lower the value of g and try again.
+ for (FLT g = 0.4; g > 0.00001; g *= 0.9999)
+ {
+ i++;
+
+
+
+ FLT point1[MAX_RADII];
+ memcpy(point1, estimateOut, sizeof(*point1) * numRadii);
+
+ // let's get 3 iterations of gradient descent here.
+ FLT gradient1[MAX_RADII];
+ getGradient(gradient1, angles, point1, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/);
+ normalizeAndMultiplyVector(gradient1, numRadii, g);
+
+ FLT point2[MAX_RADII];
+ for (size_t i = 0; i < numRadii; i++)
+ {
+ point2[i] = point1[i] + gradient1[i];
+ }
+ FLT gradient2[MAX_RADII];
+ getGradient(gradient2, angles, point2, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/);
+ normalizeAndMultiplyVector(gradient2, numRadii, g);
+
+ FLT point3[MAX_RADII];
+ for (size_t i = 0; i < numRadii; i++)
+ {
+ point3[i] = point2[i] + gradient2[i];
+ }
+
+ // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+ // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+ // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging
+ // converging gradient descent makes. Instead of using the gradient as the best indicator of
+ // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+ // following *that* vector. As it turns out, this works *amazingly* well.
+
+ FLT specialGradient[MAX_RADII];
+ for (size_t i = 0; i < numRadii; i++)
+ {
+ specialGradient[i] = point3[i] - point1[i];
+ }
+
+ // The second parameter to this function is very much a tunable parameter. Different values will result
+ // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well
+ // It's not clear what would be optimum here.
+ normalizeAndMultiplyVector(specialGradient, numRadii, g / 4);
+
+
+ FLT point4[MAX_RADII];
+ for (size_t i = 0; i < numRadii; i++)
+ {
+ point4[i] = point3[i] + specialGradient[i];
+ }
+
+
+ FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs);
+
+
+ if (newMatchFitness < lastMatchFitness)
+ {
+ //if (logFile)
+ //{
+ // writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
+ //}
+
+ lastMatchFitness = newMatchFitness;
+ memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii);
+
+#ifdef RADII_DEBUG
+ printf("+ %d %0.9f (%0.9f) \n", i, newMatchFitness, g);
+#endif
+ g = g * 1.05;
+ }
+ else
+ {
+//#ifdef RADII_DEBUG
+ // printf("-");
+ //printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]);
+//#endif
+ // if it wasn't a match, back off on the distance we jump
+ g *= 0.7;
+
+ }
+
+#ifdef RADII_DEBUG
+ FLT avg = 0;
+ FLT diffFromAvg[MAX_RADII];
+
+ for (size_t m = 0; m < numRadii; m++)
+ {
+ avg += estimateOut[m];
+ }
+ avg = avg / numRadii;
+
+ for (size_t m = 0; m < numRadii; m++)
+ {
+ diffFromAvg[m] = estimateOut[m] - avg;;
+ }
+ printf("[avg:%f] ", avg);
+
+ for (size_t x = 0; x < numRadii; x++)
+ {
+ printf("%f, ", diffFromAvg[x]);
+ //printf("%f, ", estimateOut[x]);
+ }
+ printf("\n");
+
+
+#endif
+
+
+ }
+ printf(" i=%d ", i);
+}
+
+static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj)
+{
+ FLT estimate[MAX_RADII];
+
+ for (size_t i = 0; i < MAX_RADII; i++)
+ {
+ estimate[i] = 2.38;
+ }
+
+
+ //for (int i=0; i < obj->numSensors; i++)
+ //{
+ // printf("%d, ", obj->sensor[i].id);
+ //}
+
+ SensorAngles angles[MAX_RADII];
+ PointPair pairs[MAX_POINT_PAIRS];
+
+ size_t pairCount = 0;
+
+ //obj->numSensors = 5; // TODO: HACK!!!!
+
+ for (size_t i = 0; i < obj->numSensors; i++)
+ {
+ angles[i].HorizAngle = obj->sensor[i].theta;
+ angles[i].VertAngle = obj->sensor[i].phi;
+ }
+
+ for (unsigned char i = 0; i < obj->numSensors - 1; i++)
+ {
+ for (unsigned char j = i + 1; j < obj->numSensors; j++)
+ {
+ pairs[pairCount].index1 = i;
+ pairs[pairCount].index2 = j;
+ pairs[pairCount].KnownDistance = distance(obj->sensor[i].point, obj->sensor[j].point);
+ pairCount++;
+ }
+ }
+
+
+ RefineEstimateUsingGradientDescentRadii(estimate, angles, estimate, obj->numSensors, pairs, pairCount, NULL);
+
+ // we should now have an estimate of the radii.
+
+ //for (int i = 0; i < obj->numSensors; i++)
+ for (int i = 0; i < 1; i++)
+ {
+ printf("radius[%d]: %f\n", i, estimate[i]);
+ }
+
+ // (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
+
+ return;
+}
+
+static void QuickPose(SurviveObject *so)
+{
+ OctavioRadiiData * td = so->PoserData;
+
+
+ //for (int i=0; i < so->nr_locations; i++)
+ //{
+ // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]];
+ // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]];
+ // //FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]];
+ // //FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]];
+ // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n",
+ // // i,
+ // // x0,
+ // // y0,
+ // // x1,
+ // // y1
+ // // );
+ // printf("%2d: %8.8f, %8.8f \n",
+ // i,
+ // x0,
+ // y0
+ // );
+ //}
+ //printf("\n");
+
+ TrackedObject *to;
+
+ to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+ {
+ int sensorCount = 0;
+
+ for (int i = 0; i < so->nr_locations; i++)
+ {
+ int lh = 0;
+ //printf("%d[%d], ",i,td->hitCount[i][lh][0]);
+
+ int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+ int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+ if ((td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0))
+
+
+ {
+ if (td->hitCount[i][lh][0] > 10 && td->hitCount[i][lh][1] > 10)
+ {
+ 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] };
+
+ to->sensor[sensorCount].normal.x = norm[0];
+ to->sensor[sensorCount].normal.y = norm[1];
+ to->sensor[sensorCount].normal.z = norm[2];
+ to->sensor[sensorCount].point.x = point[0];
+ to->sensor[sensorCount].point.y = point[1];
+ to->sensor[sensorCount].point.z = point[2];
+ to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+ to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+ to->sensor[sensorCount].id=i;
+
+
+
+ //printf("%2d: %8.8f, %8.8f \n",
+ // i,
+ // to->sensor[sensorCount].theta,
+ // to->sensor[sensorCount].phi
+ // );
+
+ sensorCount++;
+ }
+ }
+ }
+ //printf("\n");
+ to->numSensors = sensorCount;
+
+ if (sensorCount > 4)
+ {
+ FLT pos[3];
+ FLT orient[4];
+ SolveForLighthouseRadii(pos, orient, to);
+ }
+
+
+ }
+
+
+ free(to);
+
+}
+
+
+int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
+{
+ PoserType pt = pd->pt;
+ SurviveContext * ctx = so->ctx;
+ OctavioRadiiData * dd = so->PoserData;
+
+ if( !dd )
+ {
+ so->PoserData = dd = malloc( sizeof(OctavioRadiiData) );
+ memset(dd, 0, sizeof(OctavioRadiiData));
+ }
+
+
+ switch( pt )
+ {
+ case POSERDATA_IMU:
+ {
+ PoserDataIMU * imu = (PoserDataIMU*)pd;
+ //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] );
+ break;
+ }
+ case POSERDATA_LIGHT:
+ {
+ PoserDataLight * l = (PoserDataLight*)pd;
+
+ if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0)
+ {
+ // should never happen. Famous last words...
+ break;
+ }
+ int axis = l->acode & 0x1;
+
+ //printf("%d ", l->sensor_id);
+
+
+ //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+ if ((dd->lastAxis[l->lh] != (l->acode & 0x1)) )
+ {
+ int lastAxis = dd->lastAxis[l->lh];
+ //printf("\n");
+ //if (0 == l->lh)
+ // printf("or[%d,%d] ", l->lh,lastAxis);
+
+ for (int i=0; i < SENSORS_PER_OBJECT; i++)
+ {
+ //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+ int index = dd->angleIndex[l->lh][axis];
+ if (dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] != 0)
+ {
+ //if (0 == l->lh)
+ // printf("%d ", i);
+
+ dd->hitCount[i][l->lh][axis]++;
+ }
+ else
+ {
+ dd->hitCount[i][l->lh][axis] *= 0.5;
+ }
+ }
+ //if (0 == l->lh)
+ // printf("\n");
+ //int foo = l->acode & 0x1;
+ //printf("%d", foo);
+
+
+ //if (axis)
+ {
+ if (0 == l->lh && axis) // only once per full cycle...
+ {
+ static unsigned int counter = 1;
+
+ counter++;
+
+ // let's just do this occasionally for now...
+ if (counter % 4 == 0)
+ QuickPose(so);
+ }
+ // axis changed, time to increment the circular buffer index.
+
+
+ dd->angleIndex[l->lh][axis]++;
+ dd->angleIndex[l->lh][axis] = dd->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
+
+ // and clear out the data.
+ for (int i=0; i < SENSORS_PER_OBJECT; i++)
+ {
+ dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] = 0;
+ }
+
+ }
+ dd->lastAxis[l->lh] = axis;
+ }
+
+ //if (0 == l->lh)
+ // printf("(%d) ", l->sensor_id);
+
+ //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+ dd->oldAngles[l->sensor_id][axis][l->lh][dd->angleIndex[l->lh][axis]] = l->angle;
+ break; }
+ case POSERDATA_FULL_SCENE:
+ {
+ TrackedObject *to;
+
+ PoserDataFullScene * fs = (PoserDataFullScene*)pd;
+
+ to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+ //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+ //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space)
+ //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
+
+ //to->numSensors = so->nr_locations;
+ {
+ int sensorCount = 0;
+
+ for (int i = 0; i < so->nr_locations; i++)
+ {
+ if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0
+ {
+ to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0];
+ to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1];
+ to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2];
+ to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0];
+ to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1];
+ to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2];
+ to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+ to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+ to->sensor[sensorCount].id=i;
+ sensorCount++;
+ }
+ }
+
+ to->numSensors = sensorCount;
+
+ Point position;
+ FLT orientation[4];
+
+ SolveForLighthouseRadii(&position, orientation, to);
+ }
+ {
+ int sensorCount = 0;
+ int lh = 1;
+
+ for (int i = 0; i < so->nr_locations; i++)
+ {
+ if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1)
+ {
+ to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0];
+ to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1];
+ to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2];
+ to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0];
+ to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1];
+ to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2];
+ to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+ to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+ to->sensor[sensorCount].id=i;
+ sensorCount++;
+ }
+ }
+
+ to->numSensors = sensorCount;
+
+ Point position;
+ FLT orientation[4];
+
+ SolveForLighthouseRadii(&position, orientation, to);
+ }
+ //printf( "Full scene data.\n" );
+ break;
+ }
+ case POSERDATA_DISASSOCIATE:
+ {
+ free( dd );
+ so->PoserData = 0;
+ //printf( "Need to disassociate.\n" );
+ break;
+ }
+ }
+ return 0;
+}
+
+
+REGISTER_LINKTIME( PoserOctavioRadii );
+
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
new file mode 100644
index 0000000..c251040
--- /dev/null
+++ b/src/poser_turveytori.c
@@ -0,0 +1,1604 @@
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <memory.h>
+#include <assert.h>
+#include "linmath.h"
+#include <stddef.h>
+#include <math.h>
+#include <stdint.h>
+#if defined(__FreeBSD__) || defined(__APPLE__)
+#include <stdlib.h>
+#else
+#include <malloc.h> //for alloca
+#endif
+
+
+#define PointToFlts(x) ((FLT*)(x))
+
+typedef struct
+{
+ FLT x;
+ FLT y;
+ FLT z;
+} Point;
+
+void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) {}
+void updateHeader(FILE * file) {}
+void writeAxes(FILE * file) {}
+void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) {}
+void writePcdHeader(FILE * file) {}
+void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) {}
+void markPointWithStar(FILE *file, Point point, unsigned int color) {}
+
+typedef struct
+{
+ Point point; // location of the sensor on the tracked object;
+ Point normal; // unit vector indicating the normal for the sensor
+ double theta; // "horizontal" angular measurement from lighthouse radians
+ double phi; // "vertical" angular measurement from lighthouse in radians.
+} TrackedSensor;
+
+typedef struct
+{
+ size_t numSensors;
+ TrackedSensor sensor[0];
+} TrackedObject;
+
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846264338327
+#endif
+
+#define SQUARED(x) ((x)*(x))
+
+typedef union
+{
+ struct
+ {
+ unsigned char Blue;
+ unsigned char Green;
+ unsigned char Red;
+ unsigned char Alpha;
+ };
+ uint32_t long_value;
+} RGBValue;
+
+static RGBValue RED = { .Red = 255,.Green = 0,.Blue = 0,.Alpha = 125 };
+static RGBValue GREEN = { .Red = 0,.Green = 255,.Blue = 0,.Alpha = 125 };
+static RGBValue BLUE = { .Red = 0,.Green = 0,.Blue = 255,.Alpha = 125 };
+
+static const double WORLD_BOUNDS = 100;
+#define MAX_TRACKED_POINTS 40
+
+static const float DefaultPointsPerOuterDiameter = 60;
+
+typedef struct
+{
+ FLT down[3]; // populated by the IMU for posing
+ //Stuff
+
+#define OLD_ANGLES_BUFF_LEN 3
+ FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance
+ int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis.
+ int lastAxis[NUM_LIGHTHOUSES];
+} ToriData;
+
+
+
+
+
+
+
+static FLT distance(Point a, Point b)
+{
+ FLT x = a.x - b.x;
+ FLT y = a.y - b.y;
+ FLT z = a.z - b.z;
+ return FLT_SQRT(x*x + y*y + z*z);
+}
+
+Matrix3x3 GetRotationMatrixForTorus(Point a, Point b)
+{
+ Matrix3x3 result;
+ FLT v1[3] = { 0, 0, 1 };
+ FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z };
+
+ normalize3d(v2, v2);
+
+ rotation_between_vecs_to_m3(&result, v1, v2);
+
+ // Useful for debugging...
+ //FLT v2b[3];
+ //rotate_vec(v2b, v1, result);
+
+ return result;
+}
+
+typedef struct
+{
+ Point a;
+ Point b;
+ FLT angle;
+ FLT tanAngle; // tangent of angle
+ Matrix3x3 rotation;
+ Matrix3x3 invRotation; // inverse of rotation
+ char ai;
+ char bi;
+} PointsAndAngle;
+
+
+Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin)
+{
+ Point q;
+
+ double pf[3] = { p.x, p.y, p.z };
+ q.x = rot.val[0][0] * p.x + rot.val[1][0] * p.y + rot.val[2][0] * p.z + newOrigin.x;
+ q.y = rot.val[0][1] * p.x + rot.val[1][1] * p.y + rot.val[2][1] * p.z + newOrigin.y;
+ q.z = rot.val[0][2] * p.x + rot.val[1][2] * p.y + rot.val[2][2] * p.z + newOrigin.z;
+
+ return q;
+}
+
+double angleFromPoints(Point p1, Point p2, Point center)
+{
+ Point v1, v2, v1norm, v2norm;
+ v1.x = p1.x - center.x;
+ v1.y = p1.y - center.y;
+ v1.z = p1.z - center.z;
+
+ v2.x = p2.x - center.x;
+ v2.y = p2.y - center.y;
+ v2.z = p2.z - center.z;
+
+ double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z);
+ v1norm.x = v1.x / v1mag;
+ v1norm.y = v1.y / v1mag;
+ v1norm.z = v1.z / v1mag;
+
+ double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z);
+ v2norm.x = v2.x / v2mag;
+ v2norm.y = v2.y / v2mag;
+ v2norm.z = v2.z / v2mag;
+
+ double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z;
+
+ double angle = acos(res);
+
+ return angle;
+}
+
+Point midpoint(Point a, Point b)
+{
+ Point m;
+ m.x = (a.x + b.x) / 2;
+ m.y = (a.y + b.y) / 2;
+ m.z = (a.z + b.z) / 2;
+
+ return m;
+}
+
+// What we're doing here is:
+// * Given a point in space
+// * And points and a lighthouse angle that implicitly define a torus
+// * for that torus, what is the toroidal angle of the plane that will go through that point in space
+// * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space?
+void estimateToroidalAndPoloidalAngleOfPoint(
+ PointsAndAngle *pna,
+ Point point,
+ double *toroidalSin,
+ double *toroidalCos,
+ double *poloidalAngle,
+ double *poloidalSin)
+{
+ // We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from
+ // the tracked object coordinate system into the "easy" or "default" coordinate system of the torus.
+ // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system.
+ Matrix3x3 rot = pna->invRotation;
+ Point origin;
+ origin.x = 0;
+ origin.y = 0;
+ origin.z = 0;
+
+ Point m = midpoint(pna->a, pna->b);
+
+ // in this new coordinate system, we'll rename all of the points we care about to have an "F" after them
+ // This will be their representation in the "friendly" coordinate system
+ Point pointF;
+
+ // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also
+ // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system
+ // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation.
+ pointF.x = point.x - m.x;
+ pointF.y = point.y - m.y;
+ pointF.z = point.z - m.z;
+
+ // now we'll undo the rotation part.
+ pointF = RotateAndTranslatePoint(pointF, rot, origin);
+
+ // hooray, now pointF is in our more-friendly coordinate system.
+
+ // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy.
+ // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the
+ // angle between a vector to pointF and a vector along the x axis.
+
+ FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x));
+
+ *toroidalSin = pointF.y / toroidalHyp;
+
+ *toroidalCos = pointF.x / toroidalHyp;
+
+ //*toroidalAngle = atan(pointF.y / pointF.x);
+ //if (pointF.x < 0)
+ //{
+ // *toroidalAngle += M_PI;
+ //}
+
+ //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001);
+ //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001);
+
+ //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001);
+ //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001);
+
+ // SCORE!! We've got the toroidal angle. We're half done!
+
+ // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to
+ // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle
+ // so that the point we're focusin on will lie on the x/z plane. We then should translate the
+ // torus so that the center of the poloidal circle is at the origin. At that point, it will
+ // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a
+ // vector from the origin to the point.
+
+ // okay, instead of rotating the torus & point by the toroidal angle to get the point on
+ // the xz plane, we're going to take advantage of the radial symmetry of the torus
+ // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't
+ // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point
+ // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent
+ // this new coordinate system
+
+ Point pointG;
+ pointG.z = pointF.z;
+ pointG.y = 0;
+ pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y));
+
+ // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane,
+ // our next step will be to shift it down so that the center of the poloidal circle is at the origin.
+ // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat
+ // this as a 2D problem. I think we're getting close...
+
+ // I stole these lines from the torus generator. Gonna need the poloidal radius.
+ double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance.
+ double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
+ double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
+
+ // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all.
+ // The shift along the X axis will be the toroidal radius.
+
+ Point pointH;
+ pointH.z = pointG.z;
+ pointH.y = pointG.y;
+ pointH.x = pointG.x - toroidalRadius;
+
+ // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle,
+ // that will be the poloidal angle we're looking for. (crosses fingers)
+
+ FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x));
+
+ *poloidalSin = pointH.z / poloidalHyp;
+
+
+ *poloidalAngle = atan(pointH.z / pointH.x);
+ if (pointH.x < 0)
+ {
+ *poloidalAngle += M_PI;
+ }
+
+ //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001);
+ //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001);
+
+
+
+ // Wow, that ended up being not so much code, but a lot of interesting trig.
+ // can't remember the last time I spent so much time working through each line of code.
+
+ return;
+}
+
+#define MAX_POINT_PAIRS 100
+
+FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b)
+{
+ FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta));
+ //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta));
+
+ return angle;
+}
+
+// This provides a pretty good estimate of the angle above, probably better
+// the further away the lighthouse is. But, it's not crazy-precise.
+// It's main advantage is speed.
+FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
+{
+ FLT p = (a->phi - b->phi);
+ FLT d = (a->theta - b->theta);
+
+ FLT adjd = FLT_SIN((a->phi + b->phi) / 2);
+ FLT adjP = FLT_SIN((a->theta + b->theta) / 2);
+ FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd));
+ return pythAngle;
+}
+
+Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin)
+{
+ Point result;
+
+ FLT distanceBetweenPoints = distance(pna->a, pna->b);
+ Point m = midpoint(pna->a, pna->b);
+ Matrix3x3 rot = pna->rotation;
+
+ FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
+ FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
+
+ result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos;
+ result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin;
+ result.z = poloidalRadius*poloidalSin;
+ result = RotateAndTranslatePoint(result, rot, m);
+
+ return result;
+}
+
+FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
+{
+
+ double toroidalSin = 0;
+ double toroidalCos = 0;
+ double poloidalAngle = 0;
+ double poloidalSin = 0;
+
+ estimateToroidalAndPoloidalAngleOfPoint(
+ pna,
+ pointIn,
+ &toroidalSin,
+ &toroidalCos,
+ &poloidalAngle,
+ &poloidalSin);
+
+ Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin);
+
+ FLT dist = distance(pointIn, torusPoint);
+
+ // This is some voodoo black magic. This is here to solve the problem that the origin
+ // (which is near the center of all the tori) erroniously will rank as a good match.
+ // through a lot of empiracle testing on how to compensate for this, the "fudge factor"
+ // below ended up being the best fit. As simple as it is, I have a strong suspicion
+ // that there's some crazy complex thesis-level math that could be used to derive this
+ // but it works so we'll run with it.
+ // Note that this may be resulting in a skewing of the found location by several millimeters.
+ // it is not clear if this is actually removing existing skew (to get a more accurate value)
+ // or if it is introducing an undesirable skew.
+ double fudge = FLT_SIN((poloidalAngle - M_PI) / 2);
+ dist = dist / fudge;
+
+ return dist;
+}
+
+FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint)
+{
+ FLT fitness;
+
+ FLT resultSum = 0;
+ FLT *fitnesses = alloca(sizeof(FLT) * pnaCount);
+ int i=0, j=0;
+
+ FLT worstFitness = 0;
+
+ for (size_t i = 0; i < pnaCount; i++)
+ {
+ fitness = getPointFitnessForPna(pointIn, &(pna[i]));
+
+ if (worstFitness < fitness)
+ {
+ i = pna[i].ai;
+ j = pna[i].bi;
+ worstFitness = fitness;
+ }
+
+ fitnesses[i] = fitness;
+ if (deubgPrint)
+ {
+ printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness);
+ }
+ }
+
+ for (size_t i = 0; i < pnaCount; i++)
+ {
+ // TODO: This is an UGLY HACK!!! It is NOT ROBUST and MUST BE BETTER
+ // Right now, we're just throwing away the single worst fitness value
+ // this works frequently, but we REALLY need to do a better job of determing
+ // exactly when we should throw away a bad value. I'm thinking that decision
+ // alone could be a master's thesis, so lots of work to be done.
+ // This is just a stupid general approach that helps in a lot of cases,
+ // but is NOT suitable for long term use.
+ //if (pna[i].bi != i && pna[i].bi != j && pna[i].ai != i && pna[i].ai != j)
+ if (fitnesses[i] != worstFitness)
+ resultSum += SQUARED(fitnesses[i]);
+ }
+ return 1 / FLT_SQRT(resultSum);
+}
+
+// TODO: Use a central point instead of separate "minus" points for each axis. This will reduce
+// the number of fitness calls by 1/3.
+Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision)
+{
+ Point result;
+
+ Point tmpXplus = pointIn;
+ Point tmpXminus = pointIn;
+ tmpXplus.x = pointIn.x + precision;
+ tmpXminus.x = pointIn.x - precision;
+ result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0);
+
+ Point tmpYplus = pointIn;
+ Point tmpYminus = pointIn;
+ tmpYplus.y = pointIn.y + precision;
+ tmpYminus.y = pointIn.y - precision;
+ result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0);
+
+ Point tmpZplus = pointIn;
+ Point tmpZminus = pointIn;
+ tmpZplus.z = pointIn.z + precision;
+ tmpZminus.z = pointIn.z - precision;
+ result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0);
+
+ return result;
+}
+
+Point getNormalizedAndScaledVector(Point vectorIn, FLT desiredMagnitude)
+{
+ FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z));
+
+ FLT scale = desiredMagnitude / distanceIn;
+
+ Point result = vectorIn;
+
+ result.x *= scale;
+ result.y *= scale;
+ result.z *= scale;
+
+ return result;
+}
+
+Point getAvgPoints(Point a, Point b)
+{
+ Point result;
+ result.x = (a.x + b.x) / 2;
+ result.y = (a.y + b.y) / 2;
+ result.z = (a.z + b.z) / 2;
+ return result;
+}
+
+
+// This is modifies the basic gradient descent algorithm to better handle the shallow valley case,
+// which appears to be typical of this convergence.
+static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+{
+ int i = 0;
+ FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount, 0);
+ Point lastPoint = initialEstimate;
+
+ // The values below are somewhat magic, and definitely tunable
+ // The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+ // bigger values may be faster, especially when the initial guess is wildly off.
+ // The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+ // if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+ // That's fairly unlikely with the lighthouse problem, from expereince.
+ // The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+ // to a size that doesn't jump us out of our minima.
+ // The terminal value of g represents how close we want to get to the local minima before we're "done"
+ // The change in value of g for each iteration is intentionally very close to 1.
+ // in fact, it probably could probably be 1 without any issue. The main place where g is decremented
+ // is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+ // In those cases, we don't take the jump, and instead lower the value of g and try again.
+ for (FLT g = 0.2; g > 0.00001; g *= 0.99)
+ {
+ i++;
+ Point point1 = lastPoint;
+ // let's get 3 iterations of gradient descent here.
+ Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/);
+ Point gradientN1 = getNormalizedAndScaledVector(gradient1, g);
+
+ Point point2;
+ point2.x = point1.x + gradientN1.x;
+ point2.y = point1.y + gradientN1.y;
+ point2.z = point1.z + gradientN1.z;
+
+ Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/);
+ Point gradientN2 = getNormalizedAndScaledVector(gradient2, g);
+
+ Point point3;
+ point3.x = point2.x + gradientN2.x;
+ point3.y = point2.y + gradientN2.y;
+ point3.z = point2.z + gradientN2.z;
+
+ // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+ // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+ // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging
+ // converging gradient descent makes. Instead of using the gradient as the best indicator of
+ // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+ // following *that* vector. As it turns out, this works *amazingly* well.
+
+ Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y };
+
+ // The second parameter to this function is very much a tunable parameter. Different values will result
+ // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well
+ // It's not clear what would be optimum here.
+ specialGradient = getNormalizedAndScaledVector(specialGradient, g / 4);
+
+ Point point4;
+
+ point4.x = point3.x + specialGradient.x;
+ point4.y = point3.y + specialGradient.y;
+ point4.z = point3.z + specialGradient.z;
+
+ FLT newMatchFitness = getPointFitness(point4, pna, pnaCount, 0);
+
+ if (newMatchFitness > lastMatchFitness)
+ {
+ if (logFile)
+ {
+ writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
+ }
+
+ lastMatchFitness = newMatchFitness;
+ lastPoint = point4;
+#ifdef TORI_DEBUG
+ printf("+");
+#endif
+ }
+ else
+ {
+#ifdef TORI_DEBUG
+ printf("-");
+#endif
+ g *= 0.7;
+
+ }
+
+ // from empiracle evidence, we're probably "good enough" at this point.
+ // So, even though we could still improve, we're likely to be improving
+ // very slowly, and we should just take what we've got and move on.
+ // This also seems to happen almost only when data is a little more "dirty"
+ // because the tracker is being rotated.
+ if (i > 120)
+ {
+ //printf("i got big");
+ break;
+ }
+ }
+ printf(" i=%3d ", i);
+
+ return lastPoint;
+}
+
+
+// interesting-- this is one place where we could use any sensors that are only hit by
+// just an x or y axis to make our estimate better. TODO: bring that data to this fn.
+FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *obj)
+{
+ FLT fitness = 0;
+ for (size_t i = 0; i < obj->numSensors; i++)
+ {
+ // first, get the normal of the plane for the horizonal sweep
+ FLT theta = obj->sensor[i].theta;
+ // make two vectors that lie on the plane
+ FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 };
+ FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 };
+
+ FLT tNormH[3];
+
+ // the normal is the cross of two vectors on the plane.
+ cross3d(tNormH, t1H, t2H);
+
+ normalize3d(tNormH, tNormH);
+
+ // Now do the same for the vertical sweep
+
+ // first, get the normal of the plane for the horizonal sweep
+ FLT phi = obj->sensor[i].phi;
+ // make two vectors that lie on the plane
+ FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)};
+ FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)};
+
+ FLT tNormV[3];
+
+ // the normal is the cross of two vectors on the plane.
+ cross3d(tNormV, t1V, t2V);
+
+ normalize3d(tNormV, tNormV);
+
+
+ // First, where is the sensor in the object's reference frame?
+ FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z};
+ // Where is the point, in the reference frame of the lighthouse?
+ // This has two steps, first we translate from the object's location being the
+ // origin to the lighthouse being the origin.
+ // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse.
+
+ FLT sensor_in_lh_reference_frame[3];
+ sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z});
+
+ quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame);
+
+ // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs.
+
+ // We need an arbitrary vector from the plane to the point.
+ // Since the plane goes through the origin, this is trivial.
+ // The sensor point itself is such a vector!
+
+ // And go calculate the distances!
+ // TODO: don't need to ABS these because we square them below.
+ FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH));
+ FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV));
+
+
+ fitness += SQUARED(dH);
+ fitness += SQUARED(dV);
+ }
+
+ fitness = FLT_SQRT(fitness);
+
+ return fitness;
+}
+
+FLT RotationEstimateFitnessAxisAngle(Point lh, FLT *AxisAngle, TrackedObject *obj)
+{
+ // For this fitness calculator, we're going to use the rotation information to figure out where
+ // we expect to see the tracked object sensors, and we'll do a sum of squares to grade
+ // the quality of the guess formed by the AxisAngle;
+
+ FLT fitness = 0;
+
+ // for each point in the tracked object
+ for (int i=0; i< obj->numSensors; i++)
+ {
+
+
+
+ // let's see... we need to figure out where this sensor should be in the LH reference frame.
+ FLT sensorLocation[3] = {obj->sensor[i].point.x-lh.x, obj->sensor[i].point.y-lh.y, obj->sensor[i].point.z-lh.z};
+
+ // And this puppy needs to be rotated...
+
+ rotatearoundaxis(sensorLocation, sensorLocation, AxisAngle, AxisAngle[3]);
+
+ // Now, the vector indicating the position of the sensor, as seen by the lighthouse is:
+ FLT realVectFromLh[3] = {1, tan(obj->sensor[i].theta - LINMATHPI/2), tan(obj->sensor[i].phi - LINMATHPI/2)};
+
+ // and the vector we're calculating given the rotation passed in is the same as the sensor location:
+ FLT calcVectFromLh[3] = {sensorLocation[0], sensorLocation[1], sensorLocation[2]};
+
+ FLT angleBetween = anglebetween3d( realVectFromLh, calcVectFromLh );
+
+ fitness += SQUARED(angleBetween);
+ }
+
+ return 1/FLT_SQRT(fitness);
+}
+
+// This figures out how far away from the scanned planes each point is, then does a sum of squares
+// for the fitness.
+//
+// interesting-- this is one place where we could use any sensors that are only hit by
+// just an x or y axis to make our estimate better. TODO: bring that data to this fn.
+FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, TrackedObject *obj)
+{
+ FLT fitness = 0;
+ for (size_t i = 0; i < obj->numSensors; i++)
+ {
+ // first, get the normal of the plane for the horizonal sweep
+ FLT theta = obj->sensor[i].theta;
+ // make two vectors that lie on the plane
+ FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 };
+ FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 };
+
+ FLT tNormH[3];
+
+ // the normal is the cross of two vectors on the plane.
+ cross3d(tNormH, t1H, t2H);
+
+ normalize3d(tNormH, tNormH);
+
+ // Now do the same for the vertical sweep
+
+ // first, get the normal of the plane for the horizonal sweep
+ FLT phi = obj->sensor[i].phi;
+ // make two vectors that lie on the plane
+ FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)};
+ FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)};
+
+ FLT tNormV[3];
+
+ // the normal is the cross of two vectors on the plane.
+ cross3d(tNormV, t1V, t2V);
+
+ normalize3d(tNormV, tNormV);
+
+
+ // First, where is the sensor in the object's reference frame?
+ FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z};
+ // Where is the point, in the reference frame of the lighthouse?
+ // This has two steps, first we translate from the object's location being the
+ // origin to the lighthouse being the origin.
+ // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse.
+
+ FLT sensor_in_lh_reference_frame[3];
+ sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z});
+
+ //quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame);
+ rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]);
+
+ // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs.
+
+ // We need an arbitrary vector from the plane to the point.
+ // Since the plane goes through the origin, this is trivial.
+ // The sensor point itself is such a vector!
+
+ // And go calculate the distances!
+ // TODO: don't need to ABS these because we square them below.
+ FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH));
+ FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV));
+
+
+ fitness += SQUARED(dH);
+ fitness += SQUARED(dV);
+ }
+
+ fitness = FLT_SQRT(fitness);
+
+ return 1/fitness;
+}
+
+// interesting-- this is one place where we could use any sensors that are only hit by
+// 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)
+{
+ FLT fitness = 0;
+ for (size_t i = 0; i < obj->numSensors; i++)
+ {
+ // first, get the normal of the plane for the horizonal sweep
+ FLT theta = obj->sensor[i].theta;
+ // make two vectors that lie on the plane
+ FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 };
+ FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 };
+
+ FLT tNormH[3];
+
+ // the normal is the cross of two vectors on the plane.
+ cross3d(tNormH, t1H, t2H);
+
+ normalize3d(tNormH, tNormH);
+
+ // Now do the same for the vertical sweep
+
+ // first, get the normal of the plane for the horizonal sweep
+ FLT phi = obj->sensor[i].phi;
+ // make two vectors that lie on the plane
+ FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)};
+ FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)};
+
+ FLT tNormV[3];
+
+ // the normal is the cross of two vectors on the plane.
+ cross3d(tNormV, t1V, t2V);
+
+ normalize3d(tNormV, tNormV);
+
+
+ // First, where is the sensor in the object's reference frame?
+ FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z};
+ // Where is the point, in the reference frame of the lighthouse?
+ // This has two steps, first we translate from the object's location being the
+ // origin to the lighthouse being the origin.
+ // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse.
+
+ FLT sensor_in_lh_reference_frame[3];
+ sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z});
+
+ quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame);
+ //rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]);
+
+ // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs.
+
+ // We need an arbitrary vector from the plane to the point.
+ // Since the plane goes through the origin, this is trivial.
+ // The sensor point itself is such a vector!
+
+ // And go calculate the distances!
+ // TODO: don't need to ABS these because we square them below.
+ FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH));
+ FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV));
+
+
+ fitness += SQUARED(dH);
+ fitness += SQUARED(dV);
+ }
+
+ fitness = FLT_SQRT(fitness);
+
+ return 1/fitness;
+}
+
+
+void getRotationGradientQuaternion(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision)
+{
+
+ FLT baseFitness = RotationEstimateFitnessQuaternion(lhPoint, quaternion, obj);
+
+ FLT tmp0plus[4];
+ quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0});
+ gradientOut[0] = RotationEstimateFitnessQuaternion(lhPoint, tmp0plus, obj) - baseFitness;
+
+ FLT tmp1plus[4];
+ quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0});
+ gradientOut[1] = RotationEstimateFitnessQuaternion(lhPoint, tmp1plus, obj) - baseFitness;
+
+ FLT tmp2plus[4];
+ quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0});
+ gradientOut[2] = RotationEstimateFitnessQuaternion(lhPoint, tmp2plus, obj) - baseFitness;
+
+ FLT tmp3plus[4];
+ quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision});
+ gradientOut[3] = RotationEstimateFitnessQuaternion(lhPoint, tmp3plus, obj) - baseFitness;
+
+ return;
+}
+
+void getRotationGradientAxisAngle(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision)
+{
+
+ FLT baseFitness = RotationEstimateFitnessAxisAngle(lhPoint, quaternion, obj);
+
+ FLT tmp0plus[4];
+ quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0});
+ gradientOut[0] = RotationEstimateFitnessAxisAngle(lhPoint, tmp0plus, obj) - baseFitness;
+
+ FLT tmp1plus[4];
+ quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0});
+ gradientOut[1] = RotationEstimateFitnessAxisAngle(lhPoint, tmp1plus, obj) - baseFitness;
+
+ FLT tmp2plus[4];
+ quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0});
+ gradientOut[2] = RotationEstimateFitnessAxisAngle(lhPoint, tmp2plus, obj) - baseFitness;
+
+ FLT tmp3plus[4];
+ quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision});
+ gradientOut[3] = RotationEstimateFitnessAxisAngle(lhPoint, tmp3plus, obj) - baseFitness;
+
+ return;
+}
+
+//void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude)
+//{
+// quatnormalize(vectorToScale, vectorToScale);
+// quatscale(vectorToScale, vectorToScale, desiredMagnitude);
+// return;
+//}
+void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude)
+{
+ quatnormalize(vectorToScale, vectorToScale);
+ quatscale(vectorToScale, vectorToScale, desiredMagnitude);
+ //vectorToScale[3] = desiredMagnitude;
+
+ return;
+}
+
+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]);
+
+ printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]);
+}
+
+static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
+{
+ int i = 0;
+ FLT lastMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, initialEstimate, obj);
+
+ quatcopy(rotOut, initialEstimate);
+
+ // The values below are somewhat magic, and definitely tunable
+ // The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+ // bigger values may be faster, especially when the initial guess is wildly off.
+ // The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+ // if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+ // That's fairly unlikely with the lighthouse problem, from expereince.
+ // The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+ // to a size that doesn't jump us out of our minima.
+ // The terminal value of g represents how close we want to get to the local minima before we're "done"
+ // The change in value of g for each iteration is intentionally very close to 1.
+ // in fact, it probably could probably be 1 without any issue. The main place where g is decremented
+ // is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+ // In those cases, we don't take the jump, and instead lower the value of g and try again.
+ for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99)
+ {
+ i++;
+ FLT point1[4];
+ quatcopy(point1, rotOut);
+ // let's get 3 iterations of gradient descent here.
+ FLT gradient1[4];
+
+ normalize3d(point1, point1);
+
+ getRotationGradientAxisAngle(gradient1, lhPoint, point1, obj, g/10000);
+ getNormalizedAndScaledRotationGradient(gradient1,g);
+
+ FLT point2[4];
+ quatadd(point2, gradient1, point1);
+ //quatnormalize(point2,point2);
+
+ normalize3d(point1, point1);
+
+ FLT gradient2[4];
+ getRotationGradientAxisAngle(gradient2, lhPoint, point2, obj, g/10000);
+ getNormalizedAndScaledRotationGradient(gradient2,g);
+
+ FLT point3[4];
+ quatadd(point3, gradient2, point2);
+
+ normalize3d(point1, point1);
+
+ //quatnormalize(point3,point3);
+
+ // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+ // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+ // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging
+ // converging gradient descent makes. Instead of using the gradient as the best indicator of
+ // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+ // following *that* vector. As it turns out, this works *amazingly* well.
+
+ FLT specialGradient[4];
+ quatsub(specialGradient,point3,point1);
+
+ // The second parameter to this function is very much a tunable parameter. Different values will result
+ // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well
+ // It's not clear what would be optimum here.
+ getNormalizedAndScaledRotationGradient(specialGradient,g/4);
+
+ FLT point4[4];
+ quatadd(point4, specialGradient, point3);
+ //quatnormalize(point4,point4);
+ normalize3d(point1, point1);
+
+ FLT newMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, point4, obj);
+
+ if (newMatchFitness > lastMatchFitness)
+ {
+
+ lastMatchFitness = newMatchFitness;
+ quatcopy(rotOut, point4);
+//#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;
+
+ }
+ else
+ {
+//#ifdef TORI_DEBUG
+ //printf("- , %f\n", point4[3]);
+//#endif
+ g *= 0.7;
+
+ }
+
+ if (i > 998)
+ {
+ //printf("Ri got big");
+ break;
+ }
+ }
+ printf(" Ri=%d ", i);
+}
+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);
+ printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+}
+
+
+
+static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
+{
+ int i = 0;
+ FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj);
+
+ quatcopy(rotOut, initialEstimate);
+
+ // The values below are somewhat magic, and definitely tunable
+ // The initial vlue of g will represent the biggest step that the gradient descent can take at first.
+ // bigger values may be faster, especially when the initial guess is wildly off.
+ // The downside to a bigger starting guess is that if we've picked a good guess at the local minima
+ // if there are other local minima, we may accidentally jump to such a local minima and get stuck there.
+ // That's fairly unlikely with the lighthouse problem, from expereince.
+ // The other downside is that if it's too big, we may have to spend a few iterations before it gets down
+ // to a size that doesn't jump us out of our minima.
+ // The terminal value of g represents how close we want to get to the local minima before we're "done"
+ // The change in value of g for each iteration is intentionally very close to 1.
+ // in fact, it probably could probably be 1 without any issue. The main place where g is decremented
+ // is in the block below when we've made a jump that results in a worse fitness than we're starting at.
+ // In those cases, we don't take the jump, and instead lower the value of g and try again.
+ for (FLT g = 0.1; g > 0.000000001; g *= 0.99)
+ {
+ i++;
+ FLT point1[4];
+ quatcopy(point1, rotOut);
+ // let's get 3 iterations of gradient descent here.
+ FLT gradient1[4];
+
+ //normalize3d(point1, point1);
+
+ getRotationGradientQuaternion(gradient1, lhPoint, point1, obj, g/10000);
+ getNormalizedAndScaledRotationGradient(gradient1,g);
+
+ FLT point2[4];
+ quatadd(point2, gradient1, point1);
+ quatnormalize(point2,point2);
+
+ //normalize3d(point1, point1);
+
+ FLT gradient2[4];
+ getRotationGradientQuaternion(gradient2, lhPoint, point2, obj, g/10000);
+ getNormalizedAndScaledRotationGradient(gradient2,g);
+
+ FLT point3[4];
+ quatadd(point3, gradient2, point2);
+
+ //normalize3d(point1, point1);
+
+ quatnormalize(point3,point3);
+
+ // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley?
+ // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic
+ // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging
+ // converging gradient descent makes. Instead of using the gradient as the best indicator of
+ // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically
+ // following *that* vector. As it turns out, this works *amazingly* well.
+
+ FLT specialGradient[4];
+ quatsub(specialGradient,point3,point1);
+
+ // The second parameter to this function is very much a tunable parameter. Different values will result
+ // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well
+ // It's not clear what would be optimum here.
+ getNormalizedAndScaledRotationGradient(specialGradient,g/4);
+
+ FLT point4[4];
+ quatadd(point4, specialGradient, point3);
+ quatnormalize(point4,point4);
+ //normalize3d(point1, point1);
+
+ FLT newMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, point4, obj);
+
+ if (newMatchFitness > lastMatchFitness)
+ {
+
+ lastMatchFitness = newMatchFitness;
+ quatcopy(rotOut, point4);
+//#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);
+ }
+ else
+ {
+//#ifdef TORI_DEBUG
+ //printf("- , %f\n", point4[3]);
+//#endif
+ g *= 0.7;
+ printf("-");
+ }
+
+
+ }
+ printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness);
+}
+
+
+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 };
+ 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, int lh, int setLhCalibration)
+{
+ //printf("Solving for Lighthouse\n");
+
+ //printf("obj->numSensors = %d;\n", obj->numSensors);
+
+ //for (int i=0; i < obj->numSensors; i++)
+ //{
+ // printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x);
+ // printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y);
+ // printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z);
+ // printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x);
+ // printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y);
+ // printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z);
+ // printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi);
+ // printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta);
+ //}
+ PointsAndAngle pna[MAX_POINT_PAIRS];
+
+ volatile size_t sizeNeeded = sizeof(pna);
+
+ Point avgNorm = { 0 };
+
+ FLT smallestAngle = 20.0;
+ FLT largestAngle = 0;
+
+ size_t pnaCount = 0;
+ for (unsigned int i = 0; i < obj->numSensors; i++)
+ {
+ for (unsigned int j = 0; j < i; j++)
+ {
+ if (pnaCount < MAX_POINT_PAIRS)
+ {
+ pna[pnaCount].a = obj->sensor[i].point;
+ pna[pnaCount].b = obj->sensor[j].point;
+
+ pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]);
+ //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+ pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle);
+
+ if (pna[pnaCount].angle < smallestAngle)
+ {
+ smallestAngle = pna[pnaCount].angle;
+ }
+
+ if (pna[pnaCount].angle > largestAngle)
+ {
+ largestAngle = pna[pnaCount].angle;
+ }
+
+ double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
+
+ pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b);
+ pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation);
+ pna[pnaCount].ai = i;
+ pna[pnaCount].bi = j;
+
+
+
+ pnaCount++;
+ }
+ }
+
+ avgNorm.x += obj->sensor[i].normal.x;
+ avgNorm.y += obj->sensor[i].normal.y;
+ avgNorm.z += obj->sensor[i].normal.z;
+ }
+ avgNorm.x = avgNorm.x / obj->numSensors;
+ avgNorm.y = avgNorm.y / obj->numSensors;
+ avgNorm.z = avgNorm.z / obj->numSensors;
+
+ FLT avgNormF[3] = { avgNorm.x, avgNorm.y, avgNorm.z };
+
+
+ FILE *logFile = NULL;
+ if (doLogOutput)
+ {
+ logFile = fopen("pointcloud2.pcd", "wb");
+ writePcdHeader(logFile);
+ writeAxes(logFile);
+ }
+
+
+ // Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile);
+
+
+ // arbitrarily picking a value of 8 meters out to start from.
+ // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse
+ // since this is least likely to pick the incorrect "mirror" point that would send us
+ // back into the search for the correct point (see "if (a1 > M_PI / 2)" below)
+ Point p1 = getNormalizedAndScaledVector(avgNorm, 8);
+
+ Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
+
+ FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z };
+
+ FLT a1 = anglebetween3d(pf1, avgNormF);
+
+ if (a1 > M_PI / 2)
+ {
+ Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z };
+ refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
+
+ //FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z };
+
+ //FLT a2 = anglebetween3d(pf2, avgNormF);
+
+ }
+
+ 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);
+ //printf("Distance is %f, Fitness is %f\n", distance, fitGd);
+
+ FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
+ SolveForRotation(rot, obj, refinedEstimateGd);
+ FLT objPos[3];
+
+ WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd);
+
+ FLT rotQuat[4];
+
+ quatfromaxisangle(rotQuat, rot, rot[3]);
+
+ //{
+ FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
+
+ quatrotatevector(tmpPos, rotQuat, tmpPos);
+ //}
+
+ //static int foo = 0;
+
+ //if (0 == foo)
+ if (setLhCalibration)
+ {
+ //foo = 1;
+ if (so->ctx->bsd[lh].PositionSet)
+ {
+ printf("Warning: resetting base station calibration data");
+ }
+
+ FLT invRot[4];
+ quatgetreciprocal(invRot, rotQuat);
+
+ so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x;
+ so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y;
+ so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z;
+ so->ctx->bsd[lh].Pose.Rot[0] = invRot[0];
+ so->ctx->bsd[lh].Pose.Rot[1] = invRot[1];
+ so->ctx->bsd[lh].Pose.Rot[2] = invRot[2];
+ so->ctx->bsd[lh].Pose.Rot[3] = invRot[3];
+ so->ctx->bsd[lh].PositionSet = 1;
+ }
+
+ FLT wcPos[3]; // position in wold coordinates
+
+ quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos);
+
+ FLT newOrientation[4];
+ quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot );
+
+ wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0];
+ wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1];
+ wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2];
+
+ so->OutPose.Pos[0] = wcPos[0];
+ so->OutPose.Pos[1] = wcPos[1];
+ so->OutPose.Pos[2] = wcPos[2];
+
+ so->OutPose.Rot[0] = newOrientation[0];
+ so->OutPose.Rot[1] = newOrientation[1];
+ so->OutPose.Rot[2] = newOrientation[2];
+ so->OutPose.Rot[3] = newOrientation[3];
+
+ printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]);
+
+ if (logFile)
+ {
+ updateHeader(logFile);
+ fclose(logFile);
+ }
+
+ return refinedEstimateGd;
+}
+
+
+
+
+
+
+
+
+static void QuickPose(SurviveObject *so, int lh)
+{
+
+
+ ToriData * td = so->PoserData;
+
+ if (! so->ctx->bsd[lh].PositionSet)
+ {
+ // we don't know where we are! Augh!!!
+ return;
+ }
+
+ //for (int i=0; i < so->nr_locations; i++)
+ //{
+ // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]];
+ // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]];
+ // FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]];
+ // FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]];
+ // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n",
+ // // i,
+ // // x0,
+ // // y0,
+ // // x1,
+ // // y1
+ // // );
+ // printf("%2d: %8.8f, %8.8f \n",
+ // i,
+ // x0,
+ // y0
+ // );
+ //}
+ //printf("\n");
+
+ TrackedObject *to;
+
+ to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+ {
+ int sensorCount = 0;
+
+ //// TODO: remove, for debug purposes only!
+ //FLT downQuat[4];
+ //FLT negZ[3] = { 0,0,-1 };
+ ////quatfrom2vectors(downQuat, negZ, td->down);
+ //quatfrom2vectors(downQuat, td->down, negZ);
+ //// end TODO
+
+
+ for (int i = 0; i < so->nr_locations; i++)
+ {
+ int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+ int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN;
+ if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0)
+ {
+ 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] };
+
+ // TODO: remove these two lines!!!
+ //quatrotatevector(norm, downQuat, norm);
+ //quatrotatevector(point, downQuat, point);
+
+ to->sensor[sensorCount].normal.x = norm[0];
+ to->sensor[sensorCount].normal.y = norm[1];
+ to->sensor[sensorCount].normal.z = norm[2];
+ to->sensor[sensorCount].point.x = point[0];
+ to->sensor[sensorCount].point.y = point[1];
+ to->sensor[sensorCount].point.z = point[2];
+ to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+ to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+
+
+ sensorCount++;
+ }
+ }
+ to->numSensors = sensorCount;
+
+ if (sensorCount > 4)
+ {
+ FLT pos[3], quat[4];
+
+ SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
+ printf("!\n");
+ }
+
+
+ }
+
+
+ free(to);
+
+}
+
+
+
+int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
+{
+ PoserType pt = poserData->pt;
+ SurviveContext * ctx = so->ctx;
+ ToriData * td = so->PoserData;
+
+
+ if (!td)
+ {
+ so->PoserData = td = malloc(sizeof(ToriData));
+ memset(td, 0, sizeof(ToriData));
+ }
+
+ switch( pt )
+ {
+ case POSERDATA_IMU:
+ {
+ PoserDataIMU * tmpImu = (PoserDataIMU*)poserData;
+
+ // store off data we can use for figuring out what direction is down when doing calibration.
+ //TODO: looks like the data mask isn't getting set correctly.
+ //if (tmpImu->datamask & 1) // accelerometer data is present
+ {
+ td->down[0] = td->down[0] * 0.98 + 0.02 * tmpImu->accel[0];
+ td->down[1] = td->down[1] * 0.98 + 0.02 * tmpImu->accel[1];
+ td->down[2] = td->down[2] * 0.98 + 0.02 * tmpImu->accel[2];
+ }
+ //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, tmpImu->accel[0], tmpImu->accel[1], tmpImu->accel[2], tmpImu->gyro[0], tmpImu->gyro[1], tmpImu->gyro[2] );
+ //printf( "Down: (%f %f %f)\n", td->down[0], td->down[1], td->down[2] );
+ break;
+ }
+ case POSERDATA_LIGHT:
+ {
+ PoserDataLight * l = (PoserDataLight*)poserData;
+
+ if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0)
+ {
+ // should never happen. Famous last words...
+ break;
+ }
+ int axis = l->acode & 0x1;
+ //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
+ if ((td->lastAxis[l->lh] != (l->acode & 0x1)) )
+ {
+
+
+ if (0 == l->lh && axis) // only once per full cycle...
+ {
+ static unsigned int counter = 1;
+
+ counter++;
+
+ // let's just do this occasionally for now...
+ if (counter % 2 == 0)
+ QuickPose(so, 0);
+ }
+ // axis changed, time to increment the circular buffer index.
+ td->angleIndex[l->lh][axis]++;
+ td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
+
+ // and clear out the data.
+ for (int i=0; i < SENSORS_PER_OBJECT; i++)
+ {
+ td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0;
+ }
+
+ td->lastAxis[l->lh] = axis;
+ }
+
+ td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle;
+ break;
+ }
+ case POSERDATA_FULL_SCENE:
+ {
+ TrackedObject *to;
+
+ PoserDataFullScene * fs = (PoserDataFullScene*)poserData;
+
+ to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor)));
+
+ // if we rotate the internal reference frame of of the tracked object from having -z being arbitrary
+ // to being the down direction as defined by the accelerometer, then when we have come up
+ // with world coordinate system, it will have Z oriented correctly.
+
+ // let's get the quaternion that represents this rotation.
+ FLT downQuat[4];
+ FLT negZ[3] = { 0,0,1 };
+ //quatfrom2vectors(downQuat, negZ, td->down);
+ quatfrom2vectors(downQuat, td->down, negZ);
+
+ {
+ int sensorCount = 0;
+
+
+ for (int i = 0; i < so->nr_locations; i++)
+ {
+ if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0
+ {
+ 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);
+
+ to->sensor[sensorCount].normal.x = norm[0];
+ to->sensor[sensorCount].normal.y = norm[1];
+ to->sensor[sensorCount].normal.z = norm[2];
+ to->sensor[sensorCount].point.x = point[0];
+ to->sensor[sensorCount].point.y = point[1];
+ to->sensor[sensorCount].point.z = point[2];
+ to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+ to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+
+ sensorCount++;
+ }
+ }
+ to->numSensors = sensorCount;
+
+ FLT pos[3], quat[4];
+
+ SolveForLighthouse(pos, quat, to, so, 0, 0, 1);
+ }
+ {
+ int sensorCount = 0;
+ int lh = 1;
+
+ for (int i = 0; i < so->nr_locations; i++)
+ {
+ if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1)
+ {
+ 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);
+
+ to->sensor[sensorCount].normal.x = norm[0];
+ to->sensor[sensorCount].normal.y = norm[1];
+ to->sensor[sensorCount].normal.z = norm[2];
+ to->sensor[sensorCount].point.x = point[0];
+ to->sensor[sensorCount].point.y = point[1];
+ to->sensor[sensorCount].point.z = point[2];
+ to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+ to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+ sensorCount++;
+ }
+ }
+
+ to->numSensors = sensorCount;
+
+ FLT pos[3], quat[4];
+
+ SolveForLighthouse(pos, quat, to, so, 0, 1, 1);
+ }
+
+ free(to);
+ //printf( "Full scene data.\n" );
+ break;
+ }
+ case POSERDATA_DISASSOCIATE:
+ {
+ free( so->PoserData );
+ so->PoserData = NULL;
+ //printf( "Need to disassociate.\n" );
+ break;
+ }
+ }
+ return 0;
+}
+
+
+REGISTER_LINKTIME( PoserTurveyTori );
+
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 04931cc..ae92bad 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -30,6 +30,11 @@ int mkdir(const char *);
#define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
+ //at stage 1, is when it branches to stage two or stage 7
+ //stage 2 checks for the presence of two watchmen and an HMD visible to both lighthouses.
+ //Stage 3 collects a bunch of data for statistical averageing
+ //stage 4 does the calculation for the poses (NOT DONE!)
+ //Stage 5 = System Calibrate.d
static void handle_calibration( struct SurviveCalData *cd );
@@ -117,33 +122,51 @@ void survive_cal_install( struct SurviveContext * ctx )
cd->stage = 1;
cd->ctx = ctx;
- cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" );
- cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" );
- cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" );
+ cd->numPoseObjects = 0;
- if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 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 );
+ size_t requiredTrackersFound = 0;
+
+ for (int j=0; j < ctx->objs_ct; j++)
{
- SV_ERROR( "Error: cannot find all devices needed for calibration." );
- free( cd );
- return;
+ // Add the tracker if we allow all trackers for calibration, or if it's in the list
+ // of required trackers.
+ int isRequiredTracker = strstr(RequiredTrackersForCal, ctx->objs[j]->codename) != NULL;
+
+ if (isRequiredTracker)
+ {
+ requiredTrackersFound++;
+ }
+
+ if (AllowAllTrackersForCal || isRequiredTracker)
+ {
+ if (MAX_DEVICES_TO_CAL > cd->numPoseObjects)
+ {
+ cd->poseobjects[j] = ctx->objs[j];
+ cd->numPoseObjects++;
+
+ SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename);
+ }
+ else
+ {
+ SV_INFO("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", ctx->objs[j]->codename);
+ }
+ }
+
}
-//XXX TODO MWTourney, work on your code here.
-/*
- if( !cd->hmd )
- {
- cd->hmd = survive_get_so_by_name( ctx, "TR0" );
+ // If we want to mandate that certain devices have been found
- if( !cd->hmd )
+ if (strlen(RequiredTrackersForCal) > 0)
+ {
+ if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4))
{
- SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" );
+ SV_ERROR( "Error: Did not find all devices required for calibration." );
free( cd );
return;
}
- SV_INFO( "HMD not found, calibrating using Tracker" );
}
-*/
-
const char * DriverName;
const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" );
@@ -166,7 +189,7 @@ void survive_cal_install( struct SurviveContext * ctx )
}
-void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
+void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
{
struct SurviveContext * ctx = so->ctx;
struct SurviveCalData * cd = ctx->calptr;
@@ -184,8 +207,10 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
//Collecting OOTX data.
if( sensor_id < 0 )
{
+ //fprintf(stderr, "%s\n", so->codename);
int lhid = -sensor_id-1;
- if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' )
+ // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD)
+ if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] )
{
uint8_t dbit = (acode & 2)>>1;
ootx_pump_bit( &cd->ootx_decoders[lhid], dbit );
@@ -193,7 +218,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
int i;
for( i = 0; i < NUM_LIGHTHOUSES; i++ )
if( ctx->bsd[i].OOTXSet == 0 ) break;
- if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on.
+ if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse.
}
break;
case 3: //Look for light sync lengths.
@@ -202,10 +227,13 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
else if( acode < -4 ) break;
int lh = (-acode) - 3;
- if( strcmp( so->codename, "WM0" ) == 0 )
- sensor_id += 32;
- if( strcmp( so->codename, "WM1" ) == 0 )
- sensor_id += 64;
+ for (int i=0; i < cd->numPoseObjects; i++)
+ {
+ if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 )
+ {
+ sensor_id += i*32;
+ }
+ }
cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length;
break;
@@ -213,9 +241,10 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
}
+
}
-void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh )
{
struct SurviveContext * ctx = so->ctx;
struct SurviveCalData * cd = ctx->calptr;
@@ -223,14 +252,18 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
if( !cd ) return;
int sensid = sensor_id;
- if( strcmp( so->codename, "WM0" ) == 0 )
- sensid += 32;
- if( strcmp( so->codename, "WM1" ) == 0 )
- sensid += 64;
+
+ for (int i=0; i < cd->numPoseObjects; i++)
+ {
+ if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 )
+ {
+ sensid += i*32;
+ }
+ }
if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return;
- int lighthouse = acode>>2;
+ int lighthouse = lh;
int axis = acode & 1;
switch( cd->stage )
@@ -274,7 +307,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
int min_peaks = PTS_BEFORE_COMMON;
int i, j, k;
cd->found_common = 1;
- for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
+ for( i = 0; i < cd->numPoseObjects; i++ )
+ //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
for( j = 0; j < NUM_LIGHTHOUSES; j++ )
{
int sensors_visible = 0;
@@ -552,11 +586,11 @@ static void handle_calibration( struct SurviveCalData *cd )
int obj;
//Poses of lighthouses relative to objects.
- SurvivePose objphl[POSE_OBJECTS][NUM_LIGHTHOUSES];
+ SurvivePose objphl[MAX_POSE_OBJECTS][NUM_LIGHTHOUSES];
FILE * fobjp = fopen( "calinfo/objposes.csv", "w" );
- for( obj = 0; obj < POSE_OBJECTS; obj++ )
+ for( obj = 0; obj < cd->numPoseObjects; obj++ )
{
int i, j;
PoserDataFullScene fsd;
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 45b77f6..ae644d1 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -29,17 +29,19 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri
//void survive_cal_teardown( struct SurviveContext * ctx );
//Called from survive_default_light_process
-void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
-void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
+void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
+void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
-#define MAX_SENSORS_TO_CAL 96
+#define MAX_SENSORS_PER_DEVICE 32
+#define MAX_DEVICES_TO_CAL 3
+#define MAX_SENSORS_TO_CAL (MAX_SENSORS_PER_DEVICE * MAX_DEVICES_TO_CAL)
#define MIN_PTS_BEFORE_CAL 24
#define DRPTS 32 //Number of samples required in collection phase.
-#define POSE_OBJECTS 3
+#define MAX_POSE_OBJECTS 10
#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
struct SurviveCalData
@@ -69,7 +71,9 @@ struct SurviveCalData
int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution.
- SurviveObject * poseobjects[POSE_OBJECTS];
+ SurviveObject * poseobjects[MAX_POSE_OBJECTS];
+
+ size_t numPoseObjects;
PoserCB ConfigPoserFn;
diff --git a/src/survive_config.c b/src/survive_config.c
index 0810280..005cfaf 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -175,7 +175,13 @@ const char* config_set_str(config_group *cg, const char *tag, const char* value)
if (cv == NULL) cv = next_unused_entry(cg);
sstrcpy(&(cv->tag), tag);
- sstrcpy(&(cv->data), value);
+
+ if (NULL != value){
+ sstrcpy(&(cv->data), value);
+ }
+ else {
+ sstrcpy(&(cv->data), "");
+ }
cv->type = CONFIG_STRING;
return value;
@@ -357,9 +363,11 @@ void handle_tag_value(char* tag, char** values, uint8_t count) {
print_json_value(tag,values,count);
config_group* cg = cg_stack[cg_stack_head];
+ if (NULL != *values){
if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules
if (parse_floats(tag,values,count) > 0) return;
+ }
//should probably also handle string arrays
config_set_str(cg,tag,values[0]);
diff --git a/src/survive_data.c b/src/survive_data.c
index 0873f7f..3f16c7c 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -5,6 +5,311 @@
#include <stdint.h>
#include <string.h>
+typedef struct
+{
+ unsigned int sweep_time[SENSORS_PER_OBJECT];
+ unsigned int sweep_len[SENSORS_PER_OBJECT];
+} lightcaps_sweep_data;
+typedef struct
+{
+ int recent_sync_time;
+ int activeLighthouse;
+ int activeSweepStartTime;
+ int activeAcode;
+
+ int lh_pulse_len[NUM_LIGHTHOUSES];
+ int lh_start_time[NUM_LIGHTHOUSES];
+ int current_lh; // used knowing which sync pulse we're looking at.
+
+} lightcap2_per_sweep_data;
+
+typedef struct
+{
+ double acode_offset;
+} lightcap2_global_data;
+
+typedef struct
+{
+ lightcaps_sweep_data sweep;
+ lightcap2_per_sweep_data per_sweep;
+ lightcap2_global_data global;
+} lightcap2_data;
+
+
+//static lightcap2_global_data lcgd = { 0 };
+
+int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen)
+{
+ double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset;
+
+ int modifiedPulseLen = pulseLen - (int)oldOffset;
+
+ double newOffset = (((pulseLen) + 250) % 500) - 250;
+
+ ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1;
+
+//fprintf(stderr, " %f\n", oldOffset);
+#define ACODE_OFFSET 0
+ if (pulseLen < 3250 - ACODE_OFFSET) return 0;
+ if (pulseLen < 3750 - ACODE_OFFSET) return 1;
+ if (pulseLen < 4250 - ACODE_OFFSET) return 2;
+ if (pulseLen < 4750 - ACODE_OFFSET) return 3;
+ if (pulseLen < 5250 - ACODE_OFFSET) return 4;
+ if (pulseLen < 5750 - ACODE_OFFSET) return 5;
+ if (pulseLen < 6250 - ACODE_OFFSET) return 6;
+ return 7;
+}
+void handle_lightcap2_process_sweep_data(SurviveObject *so)
+{
+ lightcap2_data *lcd = so->disambiguator_data;
+
+ // look at all of the sensors we found, and process the ones that were hit.
+ // TODO: find the sensor(s) with the longest pulse length, and assume
+ // those are the "highest quality". Then, reject any pulses that are sufficiently
+ // different from those values, assuming that they are reflections.
+ {
+ unsigned int longest_pulse = 0;
+ unsigned int timestamp_of_longest_pulse = 0;
+ for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+ {
+ if (lcd->sweep.sweep_len[i] > longest_pulse)
+ {
+ longest_pulse = lcd->sweep.sweep_len[i];
+ timestamp_of_longest_pulse = lcd->sweep.sweep_time[i];
+ }
+ }
+
+ int allZero = 1;
+ for (int q=0; q< 32; q++)
+ if (lcd->sweep.sweep_len[q] != 0)
+ allZero=0;
+ //if (!allZero)
+ // printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse);
+ for (int i = 0; i < SENSORS_PER_OBJECT; i++)
+ {
+ {
+ static int counts[SENSORS_PER_OBJECT][2] = {0};
+
+ if (lcd->per_sweep.activeLighthouse == 0 && !allZero)
+ {
+ if (lcd->sweep.sweep_len[i] != 0)
+ {
+ //printf("%d ", i);
+ //counts[i][lcd->per_sweep.activeAcode & 1] ++;
+ }
+ else
+ {
+ counts[i][lcd->per_sweep.activeAcode & 1] =0;
+ }
+
+ //if (counts[i][0] > 10 && counts[i][1] > 10)
+ //{
+ //printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]);
+ //}
+ }
+ }
+
+
+ if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it
+ {
+
+ int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2;
+
+ if (offset_from < 380000 && offset_from > 70000)
+ {
+ //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i])
+ {
+ so->ctx->lightproc(so, i, lcd->per_sweep.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->per_sweep.activeLighthouse);
+ }
+ }
+ }
+ }
+ //if (!allZero)
+ // printf(" ..:..\n");
+
+ }
+ // clear out sweep data (could probably limit this to only after a "first" sync.
+ // this is slightly more robust, so doing it here for now.
+ memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data));
+}
+void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le )
+{
+ //fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length);
+ lightcap2_data *lcd = so->disambiguator_data;
+
+ //static unsigned int recent_sync_time = 0;
+ //static unsigned int recent_sync_count = -1;
+ //static unsigned int activeSweepStartTime;
+
+ int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
+
+ // Process any sweep data we have
+ handle_lightcap2_process_sweep_data(so);
+
+ int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time);
+
+ //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length);
+ // need to store up sync pulses, so we can take the earliest starting time for all sensors.
+ if (time_since_last_sync < 2400)
+ {
+ lcd->per_sweep.recent_sync_time = le->timestamp;
+ // it's the same sync pulse;
+ so->sync_set_number = 1;
+ so->recent_sync_time = le->timestamp;
+
+ lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
+ lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
+
+ if (!(acode >> 2 & 1)) // if the skip bit is not set
+ {
+ lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh;
+ lcd->per_sweep.activeSweepStartTime = le->timestamp;
+ lcd->per_sweep.activeAcode = acode;
+ }
+ else
+ {
+ lcd->per_sweep.activeLighthouse = -1;
+ lcd->per_sweep.activeSweepStartTime = 0;
+ lcd->per_sweep.activeAcode = 0;
+ }
+ }
+ else if (time_since_last_sync < 24000)
+ {
+ lcd->per_sweep.recent_sync_time = le->timestamp;
+ // I do believe we are lighthouse B
+ lcd->per_sweep.current_lh = 1;
+ lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
+ lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
+
+ if (!(acode >> 2 & 1)) // if the skip bit is not set
+ {
+ if (lcd->per_sweep.activeLighthouse != -1)
+ {
+ static int pulseWarningCount=0;
+
+ if (pulseWarningCount < 5)
+ {
+ pulseWarningCount++;
+ // hmm, it appears we got two non-skip pulses at the same time. That should never happen
+ fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n");
+ }
+ }
+ lcd->per_sweep.activeLighthouse = 1;
+ lcd->per_sweep.activeSweepStartTime = le->timestamp;
+ lcd->per_sweep.activeAcode = acode;
+ }
+
+ }
+ else if (time_since_last_sync > 370000)
+ {
+ // looks like this is the first sync pulse. Cool!
+
+ // first, send out the sync pulse data for the last round (for OOTX decoding
+ {
+ if (lcd->per_sweep.lh_pulse_len[0] != 0)
+ {
+ so->ctx->lightproc(
+ so,
+ -1,
+ handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]),
+ lcd->per_sweep.lh_pulse_len[0],
+ lcd->per_sweep.lh_start_time[0],
+ 0,
+ 0);
+ }
+ if (lcd->per_sweep.lh_pulse_len[1] != 0)
+ {
+ so->ctx->lightproc(
+ so,
+ -2,
+ handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]),
+ lcd->per_sweep.lh_pulse_len[1],
+ lcd->per_sweep.lh_start_time[1],
+ 0,
+ 1);
+ }
+ }
+
+ //fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n");
+ // initialize here.
+ memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep));
+ lcd->per_sweep.activeLighthouse = -1;
+
+
+
+ lcd->per_sweep.recent_sync_time = le->timestamp;
+ // I do believe we are lighthouse A
+ lcd->per_sweep.current_lh = 0;
+ lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length;
+ lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp;
+
+ int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length);
+
+ if (!(acode >> 2 & 1)) // if the skip bit is not set
+ {
+ lcd->per_sweep.activeLighthouse = 0;
+ lcd->per_sweep.activeSweepStartTime = le->timestamp;
+ lcd->per_sweep.activeAcode = acode;
+ }
+ }
+}
+
+void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
+{
+ lightcap2_data *lcd = so->disambiguator_data;
+
+ // If we see multiple "hits" on the sweep for a given sensor,
+ // assume that the longest (i.e. strongest signal) is most likely
+ // the non-reflected signal.
+
+ //if (le->length < 80)
+ //{
+ // // this is a low-quality read. Better to throw it out than to use it.
+ // //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+ // return;
+ //}
+ //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length);
+ //fprintf(stderr, ".");
+
+ if (lcd->sweep.sweep_len[le->sensor_id] < le->length)
+ {
+ lcd->sweep.sweep_len[le->sensor_id] = le->length;
+ lcd->sweep.sweep_time[le->sensor_id] = le->timestamp;
+ }
+}
+
+void handle_lightcap2( SurviveObject * so, LightcapElement * le )
+{
+ SurviveContext * ctx = so->ctx;
+
+ if (so->disambiguator_data == NULL)
+ {
+ fprintf(stderr, "Initializing Disambiguator Data\n");
+ so->disambiguator_data = malloc(sizeof(lightcap2_data));
+ memset(so->disambiguator_data, 0, sizeof(lightcap2_data));
+ }
+
+ if( le->sensor_id > SENSORS_PER_OBJECT )
+ {
+ return;
+ }
+
+ if (le->length > 6750)
+ {
+ // Should never get a reading so high. Odd.
+ return;
+ }
+ if (le->length >= 2750)
+ {
+ // Looks like a sync pulse, process it!
+ handle_lightcap2_sync(so, le);
+ return;
+ }
+
+ // must be a sweep pulse, process it!
+ handle_lightcap2_sweep(so, le);
+
+}
int32_t decode_acode(uint32_t length, int32_t main_divisor) {
//+50 adds a small offset and seems to help always get it right.
@@ -19,7 +324,10 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) {
//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
void handle_lightcap( SurviveObject * so, LightcapElement * le )
{
- SurviveContext * ctx = so->ctx;
+ SurviveContext * ctx = so->ctx;
+ handle_lightcap2(so,le);
+ return;
+
//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
if( le->sensor_id > SENSORS_PER_OBJECT )
@@ -120,7 +428,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz.
int base_station = is_new_pulse;
//printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length );
- ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length );
+ ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station);
}
}
@@ -160,8 +468,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time;
int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0];
- ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] );
- ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] );
+ ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 );
+ ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 );
so->recent_sync_time = so->last_sync_time[1];
@@ -204,7 +512,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
//Make sure pulse is in valid window
if( offset_from < 380000 && offset_from > 70000 )
{
- ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
+ ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number );
}
}
else
diff --git a/src/survive_process.c b/src/survive_process.c
index 9295638..3af2da9 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -6,14 +6,14 @@
//XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
//proper angles, then from there perform the rest of the solution.
-void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length )
+void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
{
SurviveContext * ctx = so->ctx;
- int base_station = acode >> 2;
+ int base_station = lh;
int axis = acode & 1;
if( ctx->calptr )
{
- survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length );
+ survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh);
}
//We don't use sync times, yet.
@@ -37,16 +37,16 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
#endif
FLT length_sec = length / (FLT)so->timebase_hz;
- ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle );
+ ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh);
}
-void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh)
{
SurviveContext * ctx = so->ctx;
if( ctx->calptr )
{
- survive_cal_angle( so, sensor_id, acode, timecode, length, angle );
+ survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh );
}
if( so->PoserFn )
{
@@ -57,6 +57,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
.timecode = timecode,
.length = length,
.angle = angle,
+ .lh = lh,
};
so->PoserFn( so, (PoserData *)&l );
}
diff --git a/src/survive_vive.c b/src/survive_vive.c
index f6465b2..5224728 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -47,6 +47,7 @@ const short vidpids[] = {
0x28de, 0x2012, 0, //Valve Watchman, USB connected
#ifdef HIDAPI
0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap)
+ 0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap)
0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap)
#endif
}; //length MAX_USB_INTERFACES*2
@@ -60,35 +61,38 @@ const char * devnames[] = {
"Wired Watchman 1",
#ifdef HIDAPI
"HMD Lightcap",
+ "Tracker 0 Lightcap",
"Wired Watchman 1 Lightcap",
#endif
}; //length MAX_USB_INTERFACES
#define USB_DEV_HMD 0
-#define USB_DEV_LIGHTHOUSE 1
+#define USB_DEV_HMD_IMU_LH 1
#define USB_DEV_WATCHMAN1 2
#define USB_DEV_WATCHMAN2 3
#define USB_DEV_TRACKER0 4
#define USB_DEV_W_WATCHMAN1 5 // Wired Watchman attached via USB
#ifdef HIDAPI
-#define USB_DEV_LIGHTHOUSEB 6
-#define USB_DEV_W_WATCHMAN1_LIGHTCAP 7
-#define MAX_USB_DEVS 8
+#define USB_DEV_HMD_IMU_LHB 6
+#define USB_DEV_TRACKER0_LIGHTCAP 7
+#define USB_DEV_W_WATCHMAN1_LIGHTCAP 8
+#define MAX_USB_DEVS 9
#else
#define MAX_USB_DEVS 6
#endif
#define USB_IF_HMD 0
-#define USB_IF_LIGHTHOUSE 1
+#define USB_IF_HMD_IMU_LH 1
#define USB_IF_WATCHMAN1 2
#define USB_IF_WATCHMAN2 3
#define USB_IF_TRACKER0 4
#define USB_IF_W_WATCHMAN1 5
#define USB_IF_LIGHTCAP 6
-#define USB_IF_W_WATCHMAN1_LIGHTCAP 7
-#define MAX_INTERFACES 8
+#define USB_IF_TRACKER0_LIGHTCAP 7
+#define USB_IF_W_WATCHMAN1_LIGHTCAP 8
+#define MAX_INTERFACES 9
typedef struct SurviveUSBInterface SurviveUSBInterface;
typedef struct SurviveViveData SurviveViveData;
@@ -340,7 +344,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
if (cur_dev->vendor_id == vendor_id &&
cur_dev->product_id == product_id)
{
- if( menum == enumid )
+ if( cur_dev->interface_number == enumid )
{
path_to_open = cur_dev->path;
break;
@@ -467,17 +471,22 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
#endif
if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD, sv->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; }
- if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
+ if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; }
if( sv->udev[USB_DEV_WATCHMAN2] && AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2")) { return -9; }
if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0, sv->udev[USB_DEV_TRACKER0], 0x81, survive_data_cb, "Tracker 1")) { return -10; }
if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1, sv->udev[USB_DEV_W_WATCHMAN1], 0x81, survive_data_cb, "Wired Watchman 1")) { return -11; }
#ifdef HIDAPI
//Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!!
- if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+ if( sv->udev[USB_DEV_HMD_IMU_LHB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LHB], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+
+ // This is a HACK! But it works. Need to investigate further
+ sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64;
+ if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; }
if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; }
#else
if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; }
+ if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; }
if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; }
#endif
SV_INFO( "All enumerated devices attached." );
@@ -508,17 +517,37 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
if( r != sizeof( vive_magic_power_on ) ) return 5;
}
- if (sv->udev[USB_DEV_LIGHTHOUSE])
+ if (sv->udev[USB_DEV_HMD_IMU_LH])
+ {
+ static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+ r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+ if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+
+ static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates)
+ r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+ if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+ }
+
+ if (sv->udev[USB_DEV_W_WATCHMAN1])
+ {
+ static uint8_t vive_magic_power_on[5] = { 0x04 };
+ r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+ if( r != sizeof( vive_magic_power_on ) ) return 5;
+ }
+
+#ifdef HIDAPI
+ if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP])
{
static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
- r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+ r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates)
- r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+ r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
}
+#else
if (sv->udev[USB_DEV_W_WATCHMAN1])
{
static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
@@ -530,23 +559,57 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
}
+#endif
+
+ if (sv->udev[USB_DEV_TRACKER0])
+ {
+ static uint8_t vive_magic_power_on[5] = { 0x04 };
+ r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+ if( r != sizeof( vive_magic_power_on ) ) return 5;
+ }
+//#ifdef HIDAPI
+// if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP])
+// {
+// static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+// if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+//
+// static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates)
+// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+// if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+// }
+//#else
+ if (sv->udev[USB_DEV_TRACKER0])
+ {
+ static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 };
+ r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+ if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+
+ static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates)
+ r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) );
+ if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5;
+ }
+
+//#endif
+
#if 0
for( int i = 0; i < 256; i++ )
{
static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 };
- r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+ //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+ r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
SV_INFO( "UCR: %d", r );
if( r != sizeof( vive_controller_haptic_pulse ) ) return 5;
OGUSleep( 1000 );
}
#endif
- if (sv->udev[USB_DEV_TRACKER0])
- {
- static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 };
- r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
- if( r != sizeof( vive_magic_power_on ) ) return 5;
- }
+ //if (sv->udev[USB_DEV_TRACKER0])
+ //{
+ // static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 };
+ // r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+ // if( r != sizeof( vive_magic_power_on ) ) return 5;
+ //}
SV_INFO( "Powered unit on." );
}
@@ -771,7 +834,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
qty-=2;
int propset = 0;
int doimu = 0;
- int i;
if( (type & 0xf0) == 0xf0 )
{
@@ -848,10 +910,9 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
*readdata = type; //Put 'type' back on stack.
uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to
-//#define DEBUG_WATCHMAN
#ifdef DEBUG_WATCHMAN
printf( "_%s ", w->codename);
- for( i = 0; i < qty; i++ )
+ for(int i = 0; i < qty; i++ )
{
printf( "%02x ", readdata[i] );
}
@@ -1042,8 +1103,9 @@ void survive_data_cb( SurviveUSBInterface * si )
headset->ison = 1;
break;
}
- case USB_IF_LIGHTHOUSE:
+ case USB_IF_HMD_IMU_LH:
case USB_IF_W_WATCHMAN1:
+ case USB_IF_TRACKER0:
{
int i;
//printf( "%d -> ", size );
@@ -1095,22 +1157,6 @@ void survive_data_cb( SurviveUSBInterface * si )
}
break;
}
- case USB_IF_TRACKER0:
- {
- SurviveObject * w = obj;
- if( id == 32 )
- {
- // TODO: Looks like this will need to be handle_tracker, since
- // it appears the interface is sufficiently different.
- // More work needd to reverse engineer it.
- //handle_wired_watchman( w, readdata, size);
- }
- else
- {
- SV_INFO( "Unknown tracker code %d\n", id );
- }
- break;
- }
case USB_IF_LIGHTCAP:
{
int i;
@@ -1126,6 +1172,7 @@ void survive_data_cb( SurviveUSBInterface * si )
break;
}
case USB_IF_W_WATCHMAN1_LIGHTCAP:
+ case USB_IF_TRACKER0_LIGHTCAP:
{
int i=0;
for( i = 0; i < 7; i++ )
@@ -1134,7 +1181,7 @@ void survive_data_cb( SurviveUSBInterface * si )
unsigned short *length = (unsigned short *)(&(readdata[2]));
unsigned long *time = (unsigned long *)(&(readdata[4]));
LightcapElement le;
- le.sensor_id = POP2;
+ le.sensor_id = (uint8_t)POP2;
le.length = POP2;
le.timestamp = POP4;
if( le.sensor_id == 0xff ) break;
@@ -1348,7 +1395,7 @@ int DriverRegHTCVive( SurviveContext * ctx )
}
//Next, pull out the config stuff.
- if( sv->udev[USB_DEV_HMD] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); }
+ if( sv->udev[USB_DEV_HMD_IMU_LH] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); }
if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); }
if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); }
if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); }
@@ -1403,7 +1450,7 @@ int DriverRegHTCVive( SurviveContext * ctx )
*/
//Add the drivers.
- if( sv->udev[USB_DEV_HMD] ) { survive_add_object( ctx, hmd ); }
+ if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); }
if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); }
if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); }
if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); }