diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/poser_daveortho.c | 68 | ||||
-rw-r--r-- | src/poser_octavioradii.c | 721 | ||||
-rw-r--r-- | src/poser_turveytori.c | 1604 | ||||
-rwxr-xr-x | src/survive_cal.c | 100 | ||||
-rw-r--r-- | src/survive_cal.h | 14 | ||||
-rw-r--r-- | src/survive_config.c | 10 | ||||
-rw-r--r-- | src/survive_data.c | 318 | ||||
-rw-r--r-- | src/survive_process.c | 13 | ||||
-rwxr-xr-x | src/survive_vive.c | 133 |
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 ); } |