From ae522f8a06848d467c835d87772580fa7cceb5cd Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 8 Feb 2017 11:42:46 -0700 Subject: Replaced rotation algorithm & cleanup --- tools/lighthousefind_tori/main.c | 334 +++++++++++++++++++-------------------- 1 file changed, 166 insertions(+), 168 deletions(-) (limited to 'tools/lighthousefind_tori/main.c') diff --git a/tools/lighthousefind_tori/main.c b/tools/lighthousefind_tori/main.c index 14d9423..aa51448 100644 --- a/tools/lighthousefind_tori/main.c +++ b/tools/lighthousefind_tori/main.c @@ -21,52 +21,52 @@ int best_hmd_target = 0; static void printTrackedObject(TrackedObject *to) { - for (unsigned int i = 0; i < to->numSensors; i++) - { - printf("%2.2d: [%5.5f,%5.5f] (%5.5f,%5.5f,%5.5f)\n", - i, - to->sensor[i].theta, - to->sensor[i].phi, - to->sensor[i].point.x, - to->sensor[i].point.y, - to->sensor[i].point.z - ); - } + for (unsigned int i = 0; i < to->numSensors; i++) + { + printf("%2.2d: [%5.5f,%5.5f] (%5.5f,%5.5f,%5.5f)\n", + i, + to->sensor[i].theta, + to->sensor[i].phi, + to->sensor[i].point.x, + to->sensor[i].point.y, + to->sensor[i].point.z + ); + } } static void runTheNumbers() { - TrackedObject *to; + TrackedObject *to; - to = malloc(sizeof(TrackedObject)+(PTS * sizeof(TrackedSensor))); + to = malloc(sizeof(TrackedObject)+(PTS * sizeof(TrackedSensor))); - int sensorCount = 0; + int sensorCount = 0; - for (int i = 0; i < PTS; i++) - { - // if there are enough valid counts for both the x and y sweeps for sensor i - if ((hmd_point_counts[2*i] > MIN_HITS_FOR_VALID) && - (hmd_point_counts[2*i + 1] > MIN_HITS_FOR_VALID)) - { - to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0]; - to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1]; - to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2]; - to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0]; - to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1]; - sensorCount++; - } - } + for (int i = 0; i < PTS; i++) + { + // if there are enough valid counts for both the x and y sweeps for sensor i + if ((hmd_point_counts[2*i] > MIN_HITS_FOR_VALID) && + (hmd_point_counts[2*i + 1] > MIN_HITS_FOR_VALID)) + { + to->sensor[sensorCount].point.x = hmd_points[i * 3 + 0]; + to->sensor[sensorCount].point.y = hmd_points[i * 3 + 1]; + to->sensor[sensorCount].point.z = hmd_points[i * 3 + 2]; + to->sensor[sensorCount].theta = hmd_point_angles[i * 2 + 0] + LINMATHPI / 2; + to->sensor[sensorCount].phi = hmd_point_angles[i * 2 + 1] + LINMATHPI / 2; + sensorCount++; + } + } - to->numSensors = sensorCount; + to->numSensors = sensorCount; - printf("Using %d sensors to find lighthouse.\n", sensorCount); + printf("Using %d sensors to find lighthouse.\n", sensorCount); - Point lh = SolveForLighthouse(to, 1); + Point lh = SolveForLighthouse(to, 1); - printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z); + printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z); - //printTrackedObject(to); - free(to); + //printTrackedObject(to); + free(to); } @@ -74,146 +74,144 @@ int LoadData(char Camera, const char * datafile) { - //First, read the positions of all the sensors on the HMD. - FILE * f = fopen("HMD_points.csv", "r"); - int pt = 0; - if (!f) - { - fprintf(stderr, "error: can't open hmd points.\n"); - return -5; - } - while (!feof(f) && !ferror(f) && pt < PTS) - { - float fx, fy, fz; - int r = fscanf(f, "%g %g %g\n", &fx, &fy, &fz); - hmd_points[pt * 3 + 0] = fx; - hmd_points[pt * 3 + 1] = fy; - hmd_points[pt * 3 + 2] = fz; - pt++; - if (r != 3) - { - fprintf(stderr, "Not enough entries on line %d of points\n", pt); - return -8; - } - } - if (pt < PTS) - { - fprintf(stderr, "Not enough points.\n"); - return -9; - } - fclose(f); - printf("Loaded %d points\n", pt); - - //Read all the normals on the HMD into hmd_norms. - f = fopen("HMD_normals.csv", "r"); - int nrm = 0; - if (!f) - { - fprintf(stderr, "error: can't open hmd points.\n"); - return -5; - } - while (!feof(f) && !ferror(f) && nrm < PTS) - { - float fa, fb, fc; - int r = fscanf(f, "%g %g %g\n", &fa, &fb, &fc); - hmd_norms[nrm * 3 + 0] = fa; - hmd_norms[nrm * 3 + 1] = fb; - hmd_norms[nrm * 3 + 2] = fc; - nrm++; - if (r != 3) - { - fprintf(stderr, "Not enough entries on line %d of normals\n", nrm); - return -8; - } - } - if (nrm < PTS) - { - fprintf(stderr, "Not enough points.\n"); - return -9; - } - if (nrm != pt) - { - fprintf(stderr, "point/normal counts disagree.\n"); - return -9; - } - fclose(f); - printf("Loaded %d norms\n", nrm); - - //Actually load the processed data! - int xck = 0; - f = fopen(datafile, "r"); - if (!f) - { - fprintf(stderr, "Error: cannot open %s\n", datafile); - exit(-11); - } - int lineno = 0; - while (!feof(f)) - { - //Format: - // HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936 - lineno++; - char devn[10]; - char inn[10]; - int id; - int pointct; - double avgTime; - double avgLen; - double stddevTime; - double stddevLen; - int ct = fscanf(f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen); - if (ct == 0) continue; - if (ct != 8) - { - fprintf(stderr, "Malformatted line, %d in processed_data.txt\n", lineno); - } - if (strcmp(devn, "HMD") != 0) continue; - - if (inn[0] != Camera) continue; - - int isy = inn[1] == 'Y'; - -// hmd_point_angles[id * 2 + isy] = (avgTime - 200000) / 200000 * 3.1415926535 / 2.0; - hmd_point_angles[id * 2 + isy] = (FLT)(avgTime / 48000000 * 60 * M_PI * 2); - - hmd_point_counts[id * 2 + isy] = pointct; - } - fclose(f); - - - int targpd; - int maxhits = 0; - - for (targpd = 0; targpd < PTS; targpd++) - { - int hits = hmd_point_counts[targpd * 2 + 0]; - if (hits > hmd_point_counts[targpd * 2 + 1]) hits = hmd_point_counts[targpd * 2 + 1]; - //Need an X and a Y lock. - - if (hits > maxhits) { maxhits = hits; best_hmd_target = targpd; } - } - if (maxhits < MIN_HITS_FOR_VALID) - { - fprintf(stderr, "Error: Not enough data for a primary fix.\n"); - } - - return 0; + //First, read the positions of all the sensors on the HMD. + FILE * f = fopen("HMD_points.csv", "r"); + int pt = 0; + if (!f) + { + fprintf(stderr, "error: can't open hmd points.\n"); + return -5; + } + while (!feof(f) && !ferror(f) && pt < PTS) + { + float fx, fy, fz; + int r = fscanf(f, "%g %g %g\n", &fx, &fy, &fz); + hmd_points[pt * 3 + 0] = fx; + hmd_points[pt * 3 + 1] = fy; + hmd_points[pt * 3 + 2] = fz; + pt++; + if (r != 3) + { + fprintf(stderr, "Not enough entries on line %d of points\n", pt); + return -8; + } + } + if (pt < PTS) + { + fprintf(stderr, "Not enough points.\n"); + return -9; + } + fclose(f); + printf("Loaded %d points\n", pt); + + //Read all the normals on the HMD into hmd_norms. + f = fopen("HMD_normals.csv", "r"); + int nrm = 0; + if (!f) + { + fprintf(stderr, "error: can't open hmd points.\n"); + return -5; + } + while (!feof(f) && !ferror(f) && nrm < PTS) + { + float fa, fb, fc; + int r = fscanf(f, "%g %g %g\n", &fa, &fb, &fc); + hmd_norms[nrm * 3 + 0] = fa; + hmd_norms[nrm * 3 + 1] = fb; + hmd_norms[nrm * 3 + 2] = fc; + nrm++; + if (r != 3) + { + fprintf(stderr, "Not enough entries on line %d of normals\n", nrm); + return -8; + } + } + if (nrm < PTS) + { + fprintf(stderr, "Not enough points.\n"); + return -9; + } + if (nrm != pt) + { + fprintf(stderr, "point/normal counts disagree.\n"); + return -9; + } + fclose(f); + printf("Loaded %d norms\n", nrm); + + //Actually load the processed data! + int xck = 0; + f = fopen(datafile, "r"); + if (!f) + { + fprintf(stderr, "Error: cannot open %s\n", datafile); + exit(-11); + } + int lineno = 0; + while (!feof(f)) + { + //Format: + // HMD LX 0 3433 173656.227498 327.160210 36.342361 2.990936 + lineno++; + char devn[10]; + char inn[10]; + int id; + int pointct; + double avgTime; + double avgLen; + double stddevTime; + double stddevLen; + int ct = fscanf(f, "%9s %9s %d %d %lf %lf %lf %lf\n", devn, inn, &id, &pointct, &avgTime, &avgLen, &stddevTime, &stddevLen); + if (ct == 0) continue; + if (ct != 8) + { + fprintf(stderr, "Malformatted line, %d in processed_data.txt\n", lineno); + } + if (strcmp(devn, "HMD") != 0) continue; + + if (inn[0] != Camera) continue; + + int isy = inn[1] == 'Y'; + + hmd_point_angles[id * 2 + isy] = ((FLT)avgTime - 200000) * LINMATHPI / 200000 / 2; + + hmd_point_counts[id * 2 + isy] = pointct; + } + fclose(f); + + + int targpd; + int maxhits = 0; + + for (targpd = 0; targpd < PTS; targpd++) + { + int hits = hmd_point_counts[targpd * 2 + 0]; + if (hits > hmd_point_counts[targpd * 2 + 1]) hits = hmd_point_counts[targpd * 2 + 1]; + //Need an X and a Y lock. + + if (hits > maxhits) { maxhits = hits; best_hmd_target = targpd; } + } + if (maxhits < MIN_HITS_FOR_VALID) + { + fprintf(stderr, "Error: Not enough data for a primary fix.\n"); + } + + return 0; } int main(int argc, char ** argv) { - if (argc != 3) - { - fprintf(stderr, "Error: usage: lighthousefind-torus [camera (L or R)] [datafile]\n"); - exit(-1); - } + if (argc != 3) + { + fprintf(stderr, "Error: usage: lighthousefind-torus [camera (L or R)] [datafile]\n"); + exit(-1); + } - //Load either 'L' (LH1) or 'R' (LH2) data. - if (LoadData(argv[1][0], argv[2])) return 5; + //Load either 'L' (LH1) or 'R' (LH2) data. + if (LoadData(argv[1][0], argv[2])) return 5; + runTheNumbers(); - runTheNumbers(); - - return 0; + return 0; } -- cgit v1.2.3