From 17db4d9c369ee8633b05e1d19b6fbb3d61007598 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 09:34:08 -0700 Subject: Adding octavioradii OctavioRadii converges to the right radius!!!! --- src/poser_octavioradii.c | 542 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 542 insertions(+) create mode 100644 src/poser_octavioradii.c (limited to 'src/poser_octavioradii.c') 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 +#include +#include + +typedef struct +{ + int something; + //Stuff +} OctavioRadiiData; + +#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(); + +#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 ); + -- cgit v1.2.3 From 51b8cf7083d712eac730fe90f383abfabc7f2676 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 11:49:51 -0700 Subject: Tweaking --- src/poser_octavioradii.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/poser_octavioradii.c') diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 84d69fb..18d4026 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -350,7 +350,7 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a { //#ifdef RADII_DEBUG // printf("-"); - printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); + //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; @@ -394,7 +394,7 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj for (size_t i = 0; i < MAX_RADII; i++) { - estimate[i] = 2.2; + estimate[i] = 2.38; } SensorAngles angles[MAX_RADII]; -- cgit v1.2.3 From ef039bb3ee8ce95f25e86baddbc4b3d5f21e6743 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 10:20:05 -0700 Subject: Fixing compiler warnings --- src/poser_octavioradii.c | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) (limited to 'src/poser_octavioradii.c') diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 18d4026..3893085 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -24,14 +24,11 @@ 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 @@ -410,9 +407,9 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj angles[i].VertAngle = obj->sensor[i].phi; } - for (size_t i = 0; i < obj->numSensors - 1; i++) + for (unsigned char i = 0; i < obj->numSensors - 1; i++) { - for (size_t j = i + 1; j < obj->numSensors; j++) + for (unsigned char j = i + 1; j < obj->numSensors; j++) { pairs[pairCount].index1 = i; pairs[pairCount].index2 = j; @@ -426,7 +423,7 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (size_t i = 0; i < obj->numSensors; i++) + for (int i = 0; i < obj->numSensors; i++) { printf("radius[%d]: %f\n", i, estimate[i]); } @@ -494,7 +491,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) Point position; FLT orientation[4]; - SolveForLighthouseRadii(&position, &orientation, to); + SolveForLighthouseRadii(&position, orientation, to); } { int sensorCount = 0; @@ -521,7 +518,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) Point position; FLT orientation[4]; - SolveForLighthouseRadii(&position, &orientation, to); + SolveForLighthouseRadii(&position, orientation, to); } //printf( "Full scene data.\n" ); break; -- cgit v1.2.3 From 3ca8ba3d69de226ae8835bb29e45c7bcd35793fe Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:39:47 -0700 Subject: Tori Poser Works! There's a ton of code cruft, and the algorithm is currently too slow. BUT I can track an object using only 1 lighthouse for tracking, at (I believe) an update rate of at least 7.5 HZ. By tracking, I know the position and orientation of the lighthouses relative to the tracked object, and I know the tracked object's location relative to the lighthouse. I don't have the orientation of the tracked object relative to the lighthouse yet, but that should be easy given the rest of the "knowns." --- src/poser_octavioradii.c | 198 +++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 190 insertions(+), 8 deletions(-) (limited to 'src/poser_octavioradii.c') diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 3893085..0d8674c 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -4,8 +4,12 @@ typedef struct { - int something; - //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]; + + int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; } OctavioRadiiData; #include @@ -45,6 +49,7 @@ typedef struct 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 @@ -382,10 +387,10 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a } - printf("\ni=%d\n", i); + printf(" i=%d ", i); } -void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) { FLT estimate[MAX_RADII]; @@ -394,6 +399,12 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj 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]; @@ -423,22 +434,119 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (int i = 0; i < obj->numSensors; i++) + //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) ); + if( !dd ) + { + so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + memset(dd, 0, sizeof(OctavioRadiiData)); + } + switch( pt ) { @@ -451,9 +559,81 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) 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 ); - break; - } + 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; @@ -482,6 +662,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) 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++; } } @@ -509,6 +690,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) 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++; } } -- cgit v1.2.3