From 4430b3ad6b4eca837e7a380bc1a92d0a789eea59 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Mar 2017 12:38:23 -0700 Subject: Starting to port Octavio2895's algorithm to C --- tools/lighthousefind_radii/Makefile | 10 ++ tools/lighthousefind_radii/lighthousefind_radii.c | 205 ++++++++++++++++++++++ 2 files changed, 215 insertions(+) create mode 100644 tools/lighthousefind_radii/Makefile create mode 100644 tools/lighthousefind_radii/lighthousefind_radii.c diff --git a/tools/lighthousefind_radii/Makefile b/tools/lighthousefind_radii/Makefile new file mode 100644 index 0000000..b60e07c --- /dev/null +++ b/tools/lighthousefind_radii/Makefile @@ -0,0 +1,10 @@ +all : lighthousefind_radii + +CFLAGS:=-g -O4 -DUSE_DOUBLE -I../../redist -flto +LDFLAGS:=$(CFLAGS) -lm + +lighthousefind : lighthousefind.o ../../redist/linmath.c + gcc -o $@ $^ $(LDFLAGS) + +clean : + rm -rf *.o *~ lighthousefind_radii diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c new file mode 100644 index 0000000..cc2d365 --- /dev/null +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -0,0 +1,205 @@ +#include +#include +#include "linmath.h" +#include +#include +#include + +#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(); + +typedef struct +{ + FLT x; + FLT y; + FLT z; +} Point; + +typedef struct +{ + unsigned char index1; + unsigned char index2; + FLT KnownDistance; +} PointPair; + +typedef struct +{ + FLT radius; + FLT HorizAngle; + FLT VertAngle; +} RadiusGuess; + +#define SQUARED(x) ((x)*(x)) + +FLT calculateFitness(RadiusGuess *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + fitness += SQUARED(radii[pairs[i].index1].radius) + + SQUARED(radii[pairs[i].index2].radius) + - 2 * radii[pairs[i].index1].radius * radii[pairs[i].index2].radius + * FLT_SIN( radii[pairs[i].index1].HorizAngle) * FLT_SIN(radii[pairs[i].index2].HorizAngle) + * FLT_COS( radii[pairs[i].index1].VertAngle - radii[pairs[i].index2].VertAngle ) + + FLT_COS(radii[pairs[i].index1].VertAngle) * FLT_COS(radii[pairs[i].index2].VertAngle); + } + + return FLT_SQRT(fitness); +} + +int main( int argc, char ** argv ) +{ + + if( argc != 3 ) + { + fprintf( stderr, "Error: usage: camfind [camera (L or R)] [datafile]\n" ); + exit( -1 ); + } + + //Load either 'L' (LH1) or 'R' (LH2) data. + if( LoadData( argv[1][0], argv[2] ) ) return 5; + + + + + +} + + + + + +int LoadData( char Camera, const char * datafile ) +{ + + //First, read the positions of all the sensors on the HMD. + FILE * f = fopen( "HMD_points.csv", "r" ); + int pt = 0; + if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; } + while(!feof(f) && !ferror(f) && pt < PTS) + { + float fa, fb, fc; + int r = fscanf( f,"%g %g %g\n", &fa, &fb, &fc ); + hmd_points[pt*3+0] = fa; + hmd_points[pt*3+1] = fb; + hmd_points[pt*3+2] = fc; + pt++; + if( r != 3 ) + { + fprintf( stderr, "Not enough entries on line %d of points\n", pt ); + return -8; + } + } + if( pt < PTS ) + { + fprintf( stderr, "Not enough points.\n" ); + return -9; + } + fclose( f ); + printf( "Loaded %d points\n", pt ); + + //Read all the normals on the HMD into hmd_norms. + f = fopen( "HMD_normals.csv", "r" ); + int nrm = 0; + if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; } + while(!feof(f) && !ferror(f) && nrm < PTS) + { + float fa, fb, fc; + int r = fscanf( f,"%g %g %g\n", &fa, &fb, &fc ); + hmd_norms[nrm*3+0] = fa; + hmd_norms[nrm*3+1] = fb; + hmd_norms[nrm*3+2] = fc; + nrm++; + if( r != 3 ) + { + fprintf( stderr, "Not enough entries on line %d of normals\n", nrm ); + return -8; + } + } + if( nrm < PTS ) + { + fprintf( stderr, "Not enough points.\n" ); + return -9; + } + if( nrm != pt ) + { + fprintf( stderr, "point/normal counts disagree.\n" ); + return -9; + } + fclose( f ); + printf( "Loaded %d norms\n", nrm ); + + //Actually load the processed data! + int xck = 0; + f = fopen( datafile, "r" ); + if( !f ) + { + fprintf( stderr, "Error: cannot open %s\n", datafile ); + exit (-11); + } + int lineno = 0; + while( !feof( f ) ) + { + //Format: + // HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936 + lineno++; + char devn[10]; + char inn[10]; + int id; + int pointct; + FLT avgTime; + FLT avgLen; + FLT stddevTime; + FLT stddevLen; + int ct = fscanf( f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen ); + if( ct == 0 ) continue; + if( ct != 8 ) + { + fprintf( stderr, "Malformatted line, %d in processed_data.txt\n", lineno ); + } + if( strcmp( devn, "HMD" ) != 0 ) continue; + + if( inn[0] != Camera ) continue; + + int isy = inn[1] == 'Y'; + + hmd_point_angles[id*2+isy] = ( avgTime - 200000 ) / 200000 * 3.1415926535/2.0; + hmd_point_counts[id*2+isy] = pointct; + } + fclose( f ); + + + int targpd; + int maxhits = 0; + + for( targpd = 0; targpd < PTS; targpd++ ) + { + int hits = hmd_point_counts[targpd*2+0]; + if( hits > hmd_point_counts[targpd*2+1] ) hits = hmd_point_counts[targpd*2+1]; + //Need an X and a Y lock. + + if( hits > maxhits ) { maxhits = hits; best_hmd_target = targpd; } + } + if( maxhits < MIN_HITS_FOR_VALID ) + { + fprintf( stderr, "Error: Not enough data for a primary fix.\n" ); + } + + return 0; +} + -- cgit v1.2.3 From 4cf9b32dd9d6985540ac1776edaaa1f6e658e7ca Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Mar 2017 13:21:09 -0700 Subject: Adding gradient --- tools/lighthousefind_radii/lighthousefind_radii.c | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c index cc2d365..a5dfacf 100644 --- a/tools/lighthousefind_radii/lighthousefind_radii.c +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -51,17 +51,36 @@ FLT calculateFitness(RadiusGuess *radii, PointPair *pairs, size_t numPairs) FLT fitness = 0; for (size_t i = 0; i < numPairs; i++) { - fitness += SQUARED(radii[pairs[i].index1].radius) + FLT estimatedDistanceBetweenPoints = + SQUARED(radii[pairs[i].index1].radius) + SQUARED(radii[pairs[i].index2].radius) - 2 * radii[pairs[i].index1].radius * radii[pairs[i].index2].radius * FLT_SIN( radii[pairs[i].index1].HorizAngle) * FLT_SIN(radii[pairs[i].index2].HorizAngle) * FLT_COS( radii[pairs[i].index1].VertAngle - radii[pairs[i].index2].VertAngle ) + FLT_COS(radii[pairs[i].index1].VertAngle) * FLT_COS(radii[pairs[i].index2].VertAngle); + fitness += SQUARED(estimatedDistanceBetweenPoints); } return FLT_SQRT(fitness); } +#define MAX_RADII 32 + +// note gradientOut will be of the same degree as numRadii +void getGradient(FLT *gradientOut, RadiusGuess *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision) +{ + FLT baseline = calculateFitness(radii, pairs, numPairs); + + for (size_t i = 0; i++; i < numRadii) + { + RadiusGuess tmpPlus[MAX_RADII]; + memcpy(tmpPlus, radii, sizeof(&radii) * numRadii); + tmpPlus[i].radius += precision; + gradientOut[i] = calculateFitness(tmpPlus, pairs, numPairs) - baseline; + } + + return; +} int main( int argc, char ** argv ) { -- cgit v1.2.3 From d60a009d8a514a1e5c2258c5a3dc0f0e937c1ef3 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Mar 2017 14:19:35 -0700 Subject: Adding estimate refinement --- tools/lighthousefind_radii/lighthousefind_radii.c | 162 ++++++++++++++++++++-- 1 file changed, 149 insertions(+), 13 deletions(-) diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c index a5dfacf..bc0b7ea 100644 --- a/tools/lighthousefind_radii/lighthousefind_radii.c +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -37,27 +37,33 @@ typedef struct FLT KnownDistance; } PointPair; +//typedef struct +//{ +// FLT radius; +// FLT HorizAngle; +// FLT VertAngle; +//} RadiusGuess; + typedef struct { - FLT radius; FLT HorizAngle; FLT VertAngle; -} RadiusGuess; +} SensorAngles; #define SQUARED(x) ((x)*(x)) -FLT calculateFitness(RadiusGuess *radii, PointPair *pairs, size_t numPairs) +FLT calculateFitness(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].radius) - + SQUARED(radii[pairs[i].index2].radius) - - 2 * radii[pairs[i].index1].radius * radii[pairs[i].index2].radius - * FLT_SIN( radii[pairs[i].index1].HorizAngle) * FLT_SIN(radii[pairs[i].index2].HorizAngle) - * FLT_COS( radii[pairs[i].index1].VertAngle - radii[pairs[i].index2].VertAngle ) - + FLT_COS(radii[pairs[i].index1].VertAngle) * FLT_COS(radii[pairs[i].index2].VertAngle); + 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); } @@ -67,20 +73,150 @@ FLT calculateFitness(RadiusGuess *radii, PointPair *pairs, size_t numPairs) #define MAX_RADII 32 // note gradientOut will be of the same degree as numRadii -void getGradient(FLT *gradientOut, RadiusGuess *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision) +void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision) { FLT baseline = calculateFitness(radii, pairs, numPairs); for (size_t i = 0; i++; i < numRadii) { - RadiusGuess tmpPlus[MAX_RADII]; + FLT tmpPlus[MAX_RADII]; memcpy(tmpPlus, radii, sizeof(&radii) * numRadii); - tmpPlus[i].radius += precision; - gradientOut[i] = calculateFitness(tmpPlus, pairs, numPairs) - baseline; + tmpPlus[i] += precision; + gradientOut[i] = calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline; + } + + return; +} + +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 RefineEstimateUsingGradientDescent(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); + 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.01; g *= 0.99) + { + 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] - gradient1[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(&lastPoint) * numRadii); + +#ifdef RADII_DEBUG + printf("+"); +#endif + } + else + { +#ifdef RADII_DEBUG + printf("-"); +#endif + // if it wasn't a match, back off on the distance we jump + g *= 0.7; + + } + + + } + printf("\ni=%d\n", i); +} + + int main( int argc, char ** argv ) { -- cgit v1.2.3 From f49413aeec65b9efaa172aba6fb17bf491c6f550 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Mar 2017 14:50:45 -0700 Subject: Major block in place --- tools/lighthousefind_radii/lighthousefind_radii.c | 134 +++++++++++++++++++--- 1 file changed, 121 insertions(+), 13 deletions(-) diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c index bc0b7ea..696afa6 100644 --- a/tools/lighthousefind_radii/lighthousefind_radii.c +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -23,6 +23,8 @@ FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; FLT RunTest( int print ); void PrintOpti(); +#define MAX_POINT_PAIRS 100 + typedef struct { FLT x; @@ -30,6 +32,20 @@ typedef struct 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; @@ -37,12 +53,13 @@ typedef struct FLT KnownDistance; } PointPair; -//typedef struct -//{ -// FLT radius; -// FLT HorizAngle; -// FLT VertAngle; -//} RadiusGuess; +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 { @@ -75,9 +92,9 @@ FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t // note gradientOut will be of the same degree as numRadii void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision) { - FLT baseline = calculateFitness(radii, pairs, numPairs); + FLT baseline = calculateFitness(angles, radii, pairs, numPairs); - for (size_t i = 0; i++; i < numRadii) + for (size_t i = 0; i < numRadii; i++) { FLT tmpPlus[MAX_RADII]; memcpy(tmpPlus, radii, sizeof(&radii) * numRadii); @@ -114,7 +131,10 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles { int i = 0; FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs); - memcpy(estimateOut, initialEstimate, sizeof(&estimateOut) * numRadii); + if (estimateOut != initialEstimate) + { + memcpy(estimateOut, initialEstimate, sizeof(&estimateOut) * numRadii); + } // The values below are somewhat magic, and definitely tunable @@ -141,7 +161,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles // let's get 3 iterations of gradient descent here. FLT gradient1[MAX_RADII]; - getGradient(&gradient1, angles, point1, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + getGradient(gradient1, angles, point1, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); normalizeAndMultiplyVector(gradient1, numRadii, g); FLT point2[MAX_RADII]; @@ -150,7 +170,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles point2[i] = point1[i] + gradient1[i]; } FLT gradient2[MAX_RADII]; - getGradient(&gradient2, angles, point2, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + getGradient(gradient2, angles, point2, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); normalizeAndMultiplyVector(gradient2, numRadii, g); FLT point3[MAX_RADII]; @@ -195,7 +215,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles //} lastMatchFitness = newMatchFitness; - memcpy(estimateOut, point4, sizeof(&lastPoint) * numRadii); + memcpy(estimateOut, point4, sizeof(&estimateOut) * numRadii); #ifdef RADII_DEBUG printf("+"); @@ -216,6 +236,94 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles printf("\ni=%d\n", i); } +void SolveForLighthouse(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +{ + FLT estimate[MAX_RADII] = {1}; + + SensorAngles angles[MAX_RADII]; + PointPair pairs[MAX_POINT_PAIRS]; + + size_t pairCount = 0; + + 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 + i; 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++; + } + } + + RefineEstimateUsingGradientDescent(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) + + getc(stdin); + return; +} + +static void runTheNumbers() +{ + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (PTS * sizeof(TrackedSensor))); + + int sensorCount = 0; + + for (int i = 0; i < PTS; i++) + { + // if there are enough valid counts for both the x and y sweeps for sensor i + if ((hmd_point_counts[2 * i] > MIN_HITS_FOR_VALID) && + (hmd_point_counts[2 * i + 1] > MIN_HITS_FOR_VALID)) + { + to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0]; + to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1]; + to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2]; + to->sensor[sensorCount].normal.x = hmd_norms[i * 3 + 0]; + to->sensor[sensorCount].normal.y = hmd_norms[i * 3 + 1]; + to->sensor[sensorCount].normal.z = hmd_norms[i * 3 + 2]; + to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0] + LINMATHPI / 2; + to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1] + LINMATHPI / 2; + sensorCount++; + } + } + + to->numSensors = sensorCount; + + printf("Using %d sensors to find lighthouse.\n", sensorCount); + + Point lh; + for (int i = 0; i < 1; i++) + { + SolveForLighthouse(&lh, NULL, to); + //(0.156754, -2.403268, 2.280167) + //assert(fabs((lh.x / 0.1419305302702402) - 1) < 0.00001); + //assert(fabs((lh.y / 2.5574949720325431) - 1) < 0.00001); + //assert(fabs((lh.z / 2.2451193935772080) - 1) < 0.00001); + //assert(lh.x > 0); + //assert(lh.y > 0); + //assert(lh.z > 0); + } + + printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z); + + //printTrackedObject(to); + free(to); +} int main( int argc, char ** argv ) { @@ -229,7 +337,7 @@ int main( int argc, char ** argv ) //Load either 'L' (LH1) or 'R' (LH2) data. if( LoadData( argv[1][0], argv[2] ) ) return 5; - + runTheNumbers(); -- cgit v1.2.3 From 4dae5dfbd06328b6b196ec250b559bf5e14b4a6c Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Mar 2017 15:28:57 -0700 Subject: progress... maybe? --- tools/lighthousefind_radii/lighthousefind_radii.c | 26 +++++++++++++++-------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c index 696afa6..6681b82 100644 --- a/tools/lighthousefind_radii/lighthousefind_radii.c +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -97,7 +97,7 @@ void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numR for (size_t i = 0; i < numRadii; i++) { FLT tmpPlus[MAX_RADII]; - memcpy(tmpPlus, radii, sizeof(&radii) * numRadii); + memcpy(tmpPlus, radii, sizeof(*radii) * numRadii); tmpPlus[i] += precision; gradientOut[i] = calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline; } @@ -133,7 +133,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs); if (estimateOut != initialEstimate) { - memcpy(estimateOut, initialEstimate, sizeof(&estimateOut) * numRadii); + memcpy(estimateOut, initialEstimate, sizeof(*estimateOut) * numRadii); } @@ -150,14 +150,14 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles // 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.01; g *= 0.99) + for (FLT g = 0.4; g > 0.00001; g *= 0.99) { i++; FLT point1[MAX_RADII]; - memcpy(point1, estimateOut, sizeof(&point1) * numRadii); + memcpy(point1, estimateOut, sizeof(*point1) * numRadii); // let's get 3 iterations of gradient descent here. FLT gradient1[MAX_RADII]; @@ -189,7 +189,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles FLT specialGradient[MAX_RADII]; for (size_t i = 0; i < numRadii; i++) { - specialGradient[i] = point3[i] - gradient1[i]; + specialGradient[i] = point3[i] - point1[i]; } // The second parameter to this function is very much a tunable parameter. Different values will result @@ -207,7 +207,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs); - if (newMatchFitness > lastMatchFitness) + if (newMatchFitness < lastMatchFitness) { //if (logFile) //{ @@ -215,7 +215,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles //} lastMatchFitness = newMatchFitness; - memcpy(estimateOut, point4, sizeof(&estimateOut) * numRadii); + memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii); #ifdef RADII_DEBUG printf("+"); @@ -238,13 +238,20 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles void SolveForLighthouse(Point *objPosition, FLT *objOrientation, TrackedObject *obj) { - FLT estimate[MAX_RADII] = {1}; + FLT estimate[MAX_RADII]; + + for (size_t i = 0; i < MAX_RADII; i++) + { + estimate[i] = 1; + } SensorAngles angles[MAX_RADII]; PointPair pairs[MAX_POINT_PAIRS]; size_t pairCount = 0; + obj->numSensors = 4; // TODO: HACK!!!! + for (size_t i = 0; i < obj->numSensors; i++) { angles[i].HorizAngle = obj->sensor[i].theta; @@ -253,7 +260,7 @@ void SolveForLighthouse(Point *objPosition, FLT *objOrientation, TrackedObject * for (size_t i = 0; i < obj->numSensors - 1; i++) { - for (size_t j = i + i; j < obj->numSensors; j++) + for (size_t j = i + 1; j < obj->numSensors; j++) { pairs[pairCount].index1 = i; pairs[pairCount].index2 = j; @@ -262,6 +269,7 @@ void SolveForLighthouse(Point *objPosition, FLT *objOrientation, TrackedObject * } } + RefineEstimateUsingGradientDescent(estimate, angles, estimate, obj->numSensors, pairs, pairCount, NULL); // we should now have an estimate of the radii. -- cgit v1.2.3 From b9677a259e3fb090b085a3a3bdee53fb60e5db98 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 9 Mar 2017 11:09:59 -0700 Subject: Converging --- tools/lighthousefind_radii/lighthousefind_radii.c | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c index 6681b82..2040236 100644 --- a/tools/lighthousefind_radii/lighthousefind_radii.c +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -81,7 +81,7 @@ FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t * 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); + fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); } return FLT_SQRT(fitness); @@ -99,7 +99,7 @@ void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numR FLT tmpPlus[MAX_RADII]; memcpy(tmpPlus, radii, sizeof(*radii) * numRadii); tmpPlus[i] += precision; - gradientOut[i] = calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline; + gradientOut[i] = -(calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline); } return; @@ -150,7 +150,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles // 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.99) + for (FLT g = 0.4; g > 0.00001; g *= 0.9999) { i++; @@ -218,13 +218,15 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii); #ifdef RADII_DEBUG - printf("+"); + printf("+ %0.9f (%0.9f): %f, %f, %f, %f\n", newMatchFitness, g, estimateOut[0], estimateOut[1], estimateOut[2], estimateOut[3]); #endif } else { #ifdef RADII_DEBUG - printf("-"); +// printf("-"); + printf("- %0.9f (%0.9f): %f, %f, %f, %f\n", newMatchFitness, g, estimateOut[0], estimateOut[1], estimateOut[2], estimateOut[3]); + #endif // if it wasn't a match, back off on the distance we jump g *= 0.7; @@ -242,7 +244,7 @@ void SolveForLighthouse(Point *objPosition, FLT *objOrientation, TrackedObject * for (size_t i = 0; i < MAX_RADII; i++) { - estimate[i] = 1; + estimate[i] = 2.4; } SensorAngles angles[MAX_RADII]; -- cgit v1.2.3 From 0ce8c32915d1bebb37f10b1c5d2b63666a5d62a3 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 9 Mar 2017 11:46:03 -0700 Subject: More Debug stuff --- tools/lighthousefind_radii/lighthousefind_radii.c | 33 ++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c index 2040236..10f20da 100644 --- a/tools/lighthousefind_radii/lighthousefind_radii.c +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -207,6 +207,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs); + if (newMatchFitness < lastMatchFitness) { //if (logFile) @@ -218,21 +219,47 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii); #ifdef RADII_DEBUG - printf("+ %0.9f (%0.9f): %f, %f, %f, %f\n", newMatchFitness, g, estimateOut[0], estimateOut[1], estimateOut[2], estimateOut[3]); + printf("+ %d %0.9f (%0.9f) ", i, newMatchFitness, g); #endif + g = g * 1.1; } else { #ifdef RADII_DEBUG // printf("-"); - printf("- %0.9f (%0.9f): %f, %f, %f, %f\n", newMatchFitness, g, estimateOut[0], estimateOut[1], estimateOut[2], estimateOut[3]); - + printf("- %d %0.9f (%0.9f) ", i, newMatchFitness, g); #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); -- cgit v1.2.3 From 8136850901bd817fdb23842e10febace47e5b18a Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 9 Mar 2017 13:30:20 -0700 Subject: Different calculation of distance between point guesses --- tools/lighthousefind_radii/lighthousefind_radii.c | 62 +++++++++++++++++++++-- 1 file changed, 57 insertions(+), 5 deletions(-) diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c index 10f20da..8a684cb 100644 --- a/tools/lighthousefind_radii/lighthousefind_radii.c +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -69,7 +69,7 @@ typedef struct #define SQUARED(x) ((x)*(x)) -FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +FLT calculateFitnessOld(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) { FLT fitness = 0; for (size_t i = 0; i < numPairs; i++) @@ -78,8 +78,8 @@ FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t 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_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); } @@ -87,6 +87,58 @@ FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t return FLT_SQRT(fitness); } +FLT calculateFitness(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); +} + #define MAX_RADII 32 // note gradientOut will be of the same degree as numRadii @@ -221,7 +273,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles #ifdef RADII_DEBUG printf("+ %d %0.9f (%0.9f) ", i, newMatchFitness, g); #endif - g = g * 1.1; + g = g * 1.05; } else { @@ -279,7 +331,7 @@ void SolveForLighthouse(Point *objPosition, FLT *objOrientation, TrackedObject * size_t pairCount = 0; - obj->numSensors = 4; // TODO: HACK!!!! + obj->numSensors = 7; // TODO: HACK!!!! for (size_t i = 0; i < obj->numSensors; i++) { -- cgit v1.2.3 From 1d9db12d7e115f2b8994f014e37f1086c17e90fd Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 10 Mar 2017 12:44:10 -0700 Subject: Cleanup & torus updates --- redist/linmath.c | 147 ---------------------------- tools/lighthousefind_tori/torus_localizer.c | 141 +++++++++++++++++++++++++- 2 files changed, 136 insertions(+), 152 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index 1c5c25b..dec7d64 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -481,150 +481,3 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) } -///////////////////////////////////////Matrix Rotations//////////////////////////////////// -////Originally from Stack Overflow -////Under cc by-sa 3.0 -//// http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw -//// Copyright 2014 by Campbell Barton -//// Copyright 2017 by Michael Turvey -// -///** -//* Calculate a rotation matrix from 2 normalized vectors. -//* -//* v1 and v2 must be unit length. -//*/ -//void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]) -//{ -// FLT axis[3]; -// /* avoid calculating the angle */ -// FLT angle_sin; -// FLT angle_cos; -// -// cross3d(axis, v1, v2); -// -// angle_sin = normalize_v3(axis); -// angle_cos = dot3d(v1, v2); -// -// if (angle_sin > FLT_EPSILON) { -// axis_calc: -// axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos); -// } -// else { -// /* Degenerate (co-linear) vectors */ -// if (angle_cos > 0.0f) { -// /* Same vectors, zero rotation... */ -// unit_m3(m); -// } -// else { -// /* Colinear but opposed vectors, 180 rotation... */ -// get_orthogonal_vector(axis, v1); -// normalize_v3(axis); -// angle_sin = 0.0f; /* sin(M_PI) */ -// angle_cos = -1.0f; /* cos(M_PI) */ -// goto axis_calc; -// } -// } -//} - -//void get_orthogonal_vector(FLT out[3], const FLT in[3]) -//{ -//#ifdef USE_DOUBLE -// const FLT x = fabs(in[0]); -// const FLT y = fabs(in[1]); -// const FLT z = fabs(in[2]); -//#else -// const FLT x = fabsf(in[0]); -// const FLT y = fabsf(in[1]); -// const FLT z = fabsf(in[2]); -//#endif -// -// if (x > y && x > z) -// { -// // x is dominant -// out[0] = -in[1] - in[2]; -// out[1] = in[0]; -// out[2] = in[0]; -// } -// else if (y > z) -// { -// // y is dominant -// out[0] = in[1]; -// out[1] = -in[0] - in[2]; -// out[2] = in[1]; -// } -// else -// { -// // z is dominant -// out[0] = in[2]; -// out[1] = in[2]; -// out[2] = -in[0] - in[1]; -// } -//} -// -//void unit_m3(FLT mat[3][3]) -//{ -// mat[0][0] = 1; -// mat[0][1] = 0; -// mat[0][2] = 0; -// mat[1][0] = 0; -// mat[1][1] = 1; -// mat[1][2] = 0; -// mat[2][0] = 0; -// mat[2][1] = 0; -// mat[2][2] = 1; -//} - - -//FLT normalize_v3(FLT vect[3]) -//{ -// FLT distance = dot3d(vect, vect); -// -// if (distance < 1.0e-35f) -// { -// // distance is too short, just go to zero. -// vect[0] = 0; -// vect[1] = 0; -// vect[2] = 0; -// distance = 0; -// } -// else -// { -// distance = FLT_SQRT((FLT)distance); -// scale3d(vect, vect, 1.0f / distance); -// } -// -// return distance; -//} - -///* axis must be unit length */ -//void axis_angle_normalized_to_mat3_ex( -// FLT mat[3][3], const FLT axis[3], -// const FLT angle_sin, const FLT angle_cos) -//{ -// FLT nsi[3], ico; -// FLT n_00, n_01, n_11, n_02, n_12, n_22; -// -// ico = (1.0f - angle_cos); -// nsi[0] = axis[0] * angle_sin; -// nsi[1] = axis[1] * angle_sin; -// nsi[2] = axis[2] * angle_sin; -// -// n_00 = (axis[0] * axis[0]) * ico; -// n_01 = (axis[0] * axis[1]) * ico; -// n_11 = (axis[1] * axis[1]) * ico; -// n_02 = (axis[0] * axis[2]) * ico; -// n_12 = (axis[1] * axis[2]) * ico; -// n_22 = (axis[2] * axis[2]) * ico; -// -// mat[0][0] = n_00 + angle_cos; -// mat[0][1] = n_01 + nsi[2]; -// mat[0][2] = n_02 - nsi[1]; -// mat[1][0] = n_01 - nsi[2]; -// mat[1][1] = n_11 + angle_cos; -// mat[1][2] = n_12 + nsi[0]; -// mat[2][0] = n_02 + nsi[1]; -// mat[2][1] = n_12 - nsi[0]; -// mat[2][2] = n_22 + angle_cos; -//} - - diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c index fd74b22..894bbce 100644 --- a/tools/lighthousefind_tori/torus_localizer.c +++ b/tools/lighthousefind_tori/torus_localizer.c @@ -7,12 +7,12 @@ #include "visualization.h" -static double distance(Point a, Point b) +static FLT distance(Point a, Point b) { - double x = a.x - b.x; - double y = a.y - b.y; - double z = a.z - b.z; - return sqrt(x*x + y*y + z*z); + 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) @@ -457,6 +457,137 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } +// 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 RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + 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 t1[3] = { 1, tan(theta), 0 }; + FLT t2[3] = { 1, tan(theta), 1 }; + + FLT tNorm[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNorm, t1, t2); + + // distance for this plane is d= fabs(A*x + B*y)/sqrt(A^2+B^2) (z term goes away since this plane is "vertical") + // where A is + //FLT d = + } +} + +static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + 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 = getNormalizedVector(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 = getNormalizedVector(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 = getNormalizedVector(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); + + 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; + + } + + + } + printf("\ni=%d\n", i); + + return lastPoint; +} + +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[3] = { 0, 0, 1 }; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + // not correcting for phi, but that's less important. + + // Step 2, optimize the quaternion to match the data. + +} + + Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) { PointsAndAngle pna[MAX_POINT_PAIRS]; -- cgit v1.2.3