aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/poser_octavioradii.c542
-rw-r--r--src/poser_turveytori.c2
-rw-r--r--winbuild/libsurvive/libsurvive.vcxproj1
-rw-r--r--winbuild/libsurvive/libsurvive.vcxproj.filters3
4 files changed, 547 insertions, 1 deletions
diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c
new file mode 100644
index 0000000..84d69fb
--- /dev/null
+++ b/src/poser_octavioradii.c
@@ -0,0 +1,542 @@
+#include <survive.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+typedef struct
+{
+ int something;
+ //Stuff
+} 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;
+int LoadData(char Camera, const char * FileData);
+
+//Values used for RunTest()
+FLT LighthousePos[3] = { 0, 0, 0 };
+FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
+
+FLT RunTest(int print);
+void PrintOpti();
+
+#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.
+} 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("\ni=%d\n", i);
+}
+
+void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj)
+{
+ FLT estimate[MAX_RADII];
+
+ for (size_t i = 0; i < MAX_RADII; i++)
+ {
+ estimate[i] = 2.2;
+ }
+
+ 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 (size_t i = 0; i < obj->numSensors - 1; i++)
+ {
+ for (size_t 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 (size_t i = 0; i < obj->numSensors; 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;
+}
+
+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) );
+
+ 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;
+ //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 );
+ 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)
+ 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)
+ 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
index e9e5b7a..4e602f3 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -299,7 +299,7 @@ void estimateToroidalAndPoloidalAngleOfPoint(
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));
+ //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta));
return angle;
}
diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj
index 05fec8c..c794382 100644
--- a/winbuild/libsurvive/libsurvive.vcxproj
+++ b/winbuild/libsurvive/libsurvive.vcxproj
@@ -153,6 +153,7 @@
<ClCompile Include="..\..\src\poser_charlesslow.c" />
<ClCompile Include="..\..\src\poser_daveortho.c" />
<ClCompile Include="..\..\src\poser_dummy.c" />
+ <ClCompile Include="..\..\src\poser_octavioradii.c" />
<ClCompile Include="..\..\src\poser_turveytori.c" />
<ClCompile Include="..\..\src\survive.c" />
<ClCompile Include="..\..\src\survive_cal.c" />
diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters
index 8bb09b2..e7d44e2 100644
--- a/winbuild/libsurvive/libsurvive.vcxproj.filters
+++ b/winbuild/libsurvive/libsurvive.vcxproj.filters
@@ -87,6 +87,9 @@
<ClCompile Include="..\..\src\poser_turveytori.c">
<Filter>Source Files</Filter>
</ClCompile>
+ <ClCompile Include="..\..\src\poser_octavioradii.c">
+ <Filter>Source Files</Filter>
+ </ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="..\..\src\ootx_decoder.h">