From 243f3bea242c82d14bc2e191421010f300114c9f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Feb 2017 10:13:29 -0700 Subject: Remove a bunch of dead code --- tools/lighthousefind_tori/torus_localizer.c | 513 +--------------------------- 1 file changed, 10 insertions(+), 503 deletions(-) (limited to 'tools') diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c index f69d6ab..25a5e7f 100644 --- a/tools/lighthousefind_tori/torus_localizer.c +++ b/tools/lighthousefind_tori/torus_localizer.c @@ -32,16 +32,6 @@ Matrix3x3 GetRotationMatrixForTorus(Point a, Point b) return result; } -//void TestGetRotationMatrixForTorus() -//{ -// // a={x=0.079967096447944641 y=0.045225199311971664 z=0.034787099808454514 } -// // b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 } -// -// // a={x=0.050822000950574875 y=0.052537899464368820 z=0.033285100013017654 } -// // b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 } -// -//} - typedef struct { Point a; @@ -101,117 +91,6 @@ Point midpoint(Point a, Point b) return m; } - - - -// This torus generator creates a point cloud of the given torus, and attempts to keep the -// density of the points fairly uniform across the surface of the torus. -void partialTorusGenerator( - Point p1, - Point p2, - double toroidalStartAngle, - double toroidalEndAngle, - double poloidalStartAngle, - double poloidalEndAngle, - double lighthouseAngle, - double toroidalPrecision, - Point **pointCloud) -{ - double poloidalRadius = 0; - double toroidalRadius = 0; - - Point m = midpoint(p1, p2); - double distanceBetweenPoints = distance(p1, p2); - - // ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally - // thinking the tori converge at the location of the tracked object instead of at the lighthouse. - double centralAngleToIgnore = lighthouseAngle * 3; - - Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2); - - toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); - - poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); - - double poloidalPrecision = M_PI * 2 / toroidalPrecision; - - - unsigned int pointCount = 0; - - // This loop tries to (cheaply) figure out an upper bound on the number of points we'll have in our point cloud - for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) - { - // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle - // We do this so our point cloud will have a more even distribution of points across the surface of the torus. - double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; - - double step_distance = 2 * M_PI / steps; - - pointCount += (unsigned int)((toroidalEndAngle - toroidalStartAngle) / step_distance + 2); - } - - *pointCloud = malloc(pointCount * sizeof(Point) ); - - assert(0 != *pointCloud); - - (*pointCloud)[pointCount - 1].x = -1000; - (*pointCloud)[pointCount - 1].y = -1000; - (*pointCloud)[pointCount - 1].z = -1000; // need a better magic number or flag, but this'll do for now. - - size_t currentPoint = 0; - - for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision) - { - // here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle - // We do this so our point cloud will have a more even distribution of points across the surface of the torus. - double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision; - - double step_distance = 2 * M_PI / steps; - - //for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40) - for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += step_distance) - { - if (currentPoint >= pointCount - 1) - { - int a = 0; - } - assert(currentPoint < pointCount - 1); - (*pointCloud)[currentPoint].x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep); - (*pointCloud)[currentPoint].y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep); - (*pointCloud)[currentPoint].z = poloidalRadius*sin(poloidalStep); - (*pointCloud)[currentPoint] = RotateAndTranslatePoint((*pointCloud)[currentPoint], rot, m); - - // TODO: HACK!!! Instead of doing anything with normals, we're "assuming" that all sensors point directly up - // and hence we know that nothing with a negative z value is a possible lightouse location. - // Before this code can go live, we'll have to take the normals into account and remove this hack. - if ((*pointCloud)[currentPoint].z > 0) - { - currentPoint++; - } - } - } - -#ifdef TORI_DEBUG - printf("%d / %d\n", currentPoint, pointCount); -#endif - (*pointCloud)[currentPoint].x = -1000; - (*pointCloud)[currentPoint].y = -1000; - (*pointCloud)[currentPoint].z = -1000; -} - -void torusGenerator(Point p1, Point p2, double lighthouseAngle, Point **pointCloud) -{ - - double centralAngleToIgnore = lighthouseAngle * 6; - - centralAngleToIgnore = 20.0 / 180.0 * M_PI; - - partialTorusGenerator(p1, p2, 0, M_PI * 2, centralAngleToIgnore + M_PI, M_PI * 3 - centralAngleToIgnore, lighthouseAngle, DefaultPointsPerOuterDiameter, pointCloud); - - return; -} - - // What we're doing here is: // * Given a point in space // * And points and a lighthouse angle that implicitly define a torus @@ -337,64 +216,6 @@ void estimateToroidalAndPoloidalAngleOfPoint( return; } -double FindSmallestDistance(Point p, Point* cloud) -{ - Point *cp = cloud; - double smallestDistance = 10000000000000.0; - - while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) - { - double distance = (SQUARED(cp->x - p.x) + SQUARED(cp->y - p.y) + SQUARED(cp->z - p.z)); - if (distance < smallestDistance) - { - smallestDistance = distance; - } - cp++; - } - smallestDistance = sqrt(smallestDistance); - return smallestDistance; -} - -// Given a cloud and a list of clouds, find the point on masterCloud that best matches clouds. -Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds) -{ - - Point bestMatch = { 0 }; - double bestDistance = 10000000000000.0; - Point *cp = masterCloud; - int point = 0; - while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000) - { - point++; -#ifdef TORI_DEBUG - if (point % 100 == 0) - { - printf("."); - } -#endif - double currentDistance = 0; - for (int i = 0; i < numClouds; i++) - { - if (clouds[i] == masterCloud) - { - continue; - } - Point* cloud = clouds[i]; - currentDistance += FindSmallestDistance(*cp, cloud); - } - - if (currentDistance < bestDistance) - { - bestDistance = currentDistance; - bestMatch = *cp; - } - cp++; - } - - return bestMatch; -} - - #define MAX_POINT_PAIRS 100 double angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) @@ -415,131 +236,6 @@ double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) return pythAngle; } -Point GetInitialEstimate(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) -{ - Point **pointCloud = malloc(sizeof(void*)* pnaCount); - - - for (unsigned int i = 0; i < pnaCount; i++) - { - torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i])); - if (logFile) - { - writePointCloud(logFile, pointCloud[i], COLORS[i%MAX_COLORS]); - } - - } - - Point bestMatchA = findBestPointMatch(pointCloud[0], pointCloud, pnaCount); - - for (unsigned int i = 0; i < pnaCount; i++) - { - free(pointCloud[i]); - pointCloud[i] = NULL; - } - - if (logFile) - { - markPointWithStar(logFile, bestMatchA, 0xFF0000); - } -#ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z); -#endif - - return bestMatchA; -} - -Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile) -{ - // Now, let's add an extra patch or torus near the point we just found. - - double toroidalAngle = 0; - double poloidalAngle = 0; - - double toroidalCos = 0; - double toroidalSin = 0; - - - Point **pointCloud2 = malloc(sizeof(void*)* pnaCount); - - for (unsigned int i = 0; i < pnaCount; i++) - { - estimateToroidalAndPoloidalAngleOfPoint( - &(pna[i]), - initialEstimate, - &toroidalAngle, - &toroidalSin, - &toroidalCos, - &poloidalAngle); - - partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.1, toroidalAngle + 0.1, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i])); - - if (logFile) - { - writePointCloud(logFile, pointCloud2[i], COLORS[i%MAX_COLORS]); - } - - } - - Point bestMatchB = findBestPointMatch(pointCloud2[0], pointCloud2, pnaCount); - - for (unsigned int i = 0; i < pnaCount; i++) - { - free(pointCloud2[i]); - pointCloud2[i] = NULL; - } - - if (logFile) - { - markPointWithStar(logFile, bestMatchB, 0x00FF00); - } -#ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z); -#endif - - Point **pointCloud3 = malloc(sizeof(void*)* pnaCount); - - for (unsigned int i = 0; i < pnaCount; i++) - { - estimateToroidalAndPoloidalAngleOfPoint( - &(pna[i]), - bestMatchB, - &toroidalAngle, - &toroidalSin, - &toroidalCos, - &poloidalAngle); - - partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.1, poloidalAngle + 0.1, pna[i].angle, 3000, &(pointCloud3[i])); - - if (logFile) - { - writePointCloud(logFile, pointCloud3[i], COLORS[i%MAX_COLORS]); - } - - } - - Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount); - - for (unsigned int i = 0; i < pnaCount; i++) - { - free(pointCloud3[i]); - pointCloud3[i] = NULL; - } - - if (logFile) - { - markPointWithStar(logFile, bestMatchC, 0xFFFFFF); - } -#ifdef TORI_DEBUG - printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z); -#endif - - - - - return bestMatchC; -} - Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double toroidalSin, double toroidalCos, double poloidalAngle) { Point result; @@ -551,8 +247,6 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle)); double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); - //result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*cos(toroidalAngle); - //result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*sin(toroidalAngle); result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos; result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin; result.z = poloidalRadius*sin(poloidalAngle); @@ -591,14 +285,11 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) // 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); - //fudge *= fudge; dist = dist / fudge; return dist; } -//Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile) -// FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount) { FLT fitness; @@ -664,55 +355,6 @@ Point getAvgPoints(Point a, Point b) return result; } -// 0.95 is good value for descent -// 0.1 is a good value for starting precision. -static Point RefineEstimateUsingGradientDescent(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile, FLT descent, FLT startingPrecision) -{ - int i = 0; - FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); - Point lastPoint = initialEstimate; - Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/); - - for (FLT f = startingPrecision; f > 0.0001; f *= descent) - { - Point gradient = getGradient(lastPoint, pna, pnaCount, f / 1000 /*somewhat arbitrary*/); - gradient = getNormalizedVector(gradient, f); - - //printf("Gradient: (%f, %f, %f)\n", gradient.x, gradient.y, gradient.z); - - // gradient = getAvgPoints(gradient, lastGradient); // doesn't seem to help much. might hurt in some cases. - - Point newPoint; - newPoint.x = lastPoint.x + gradient.x; - newPoint.y = lastPoint.y + gradient.y; - newPoint.z = lastPoint.z + gradient.z; - - FLT newMatchFitness = getPointFitness(newPoint, pna, pnaCount); - - if (newMatchFitness > lastMatchFitness) - { - lastMatchFitness = newMatchFitness; - lastPoint = newPoint; - //printf("%f\n", newMatchFitness); - lastGradient = gradient; - - if (logFile) - { - writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); - } - } - else - { - //printf("-"); - } - - i++; - } - - //printf("i = %d\n", i); - - return lastPoint; -} // This is modifies the basic gradient descent algorithm to better handle the shallow valley case, // which appears to be typical of this convergence. @@ -721,7 +363,6 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, int i = 0; FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); Point lastPoint = initialEstimate; - //Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/); // 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. @@ -788,11 +429,15 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, lastMatchFitness = newMatchFitness; lastPoint = point4; +#ifdef TORI_DEBUG printf("+"); +#endif } else { +#ifdef TORI_DEBUG printf("-"); +#endif g *= 0.7; } @@ -804,97 +449,6 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, return lastPoint; } -// This torus generator creates a point cloud of the given torus, and attempts to keep the -// density of the points fairly uniform across the surface of the torus. -void AnalyzeToroidalImpact( - Point p1, - Point p2, - double lighthouseAngle, - double *vals, - PointsAndAngle *pna, - size_t pnaCount) -{ - double poloidalRadius = 0; - double toroidalRadius = 0; - - Point m = midpoint(p1, p2); - double distanceBetweenPoints = distance(p1, p2); - - // ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally - // thinking the tori converge at the location of the tracked object instead of at the lighthouse. - double centralAngleToIgnore = lighthouseAngle * 3; - - Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2); - - toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle)); - - poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); - - unsigned int pointCount = 0; - - size_t currentPoint = 0; - - for (size_t ps = 0; ps < 180; ps++) - { - - //for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40) - for (double toroidalStep = 0; toroidalStep < M_PI / 2; toroidalStep += M_PI / 180 * 2) - { - double poloidalStep = M_PI + M_PI / 180 * 2 * ps; - Point tmp; - - tmp.x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep); - tmp.y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep); - tmp.z = poloidalRadius*sin(poloidalStep); - tmp = RotateAndTranslatePoint(tmp, rot, m); - - vals[ps] += getPointFitness(tmp, pna, pnaCount); - - } - - vals[ps] = vals[ps] / 180; // average. - } - -} - -void AnalyzePoloidalImpact(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) -{ - Point **pointCloud = malloc(sizeof(void*)* pnaCount); - - double vals[200][180] = { 0 }; - - - for (unsigned int i = 0; i < pnaCount; i++) - { - //double tmpVals[180] = { 0 }; - - AnalyzeToroidalImpact( - pna[i].a, - pna[i].b, - pna[i].angle, - vals[i], - pna, - pnaCount); - - - //for (int j = 0; j < 180; j++) - //{ - // vals[j] += tmpVals[j]; - //} - - } - - for (int i = 0; i < 180; i++) - { - printf("%d", i * 2); - for (unsigned int j = 0; j < pnaCount; j++) - { - printf(", %f", vals[j][i]); - } - printf("\n"); - } -} - Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) { @@ -941,15 +495,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) writeAxes(logFile); } - //Point initialEstimate = GetInitialEstimate(obj, pna, pnaCount, logFile); - - //Point refinedEstimatePc = RefineEstimateUsingPointCloud(initialEstimate, pna, pnaCount, obj, logFile); - - //Point refinedEstimageGd = RefineEstimateUsingGradientDescent(initialEstimate, pna, pnaCount, logFile, 0.95, 0.1); // Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile); - // AnalyzePoloidalImpact(obj, 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 @@ -957,16 +505,16 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) Point p1 = getNormalizedVector(avgNorm, 8); - Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); + Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); - FLT pf1[3] = { refinedEstimageGd.x, refinedEstimageGd.y, refinedEstimageGd.z }; + FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; FLT a1 = anglebetween3d(pf1, avgNormF); if (a1 > M_PI / 2) { - Point p2 = { .x = -refinedEstimageGd.x, .y = -refinedEstimageGd.y, .z = -refinedEstimageGd.z }; - refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile); + 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 }; @@ -974,9 +522,7 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) } - //FLT fitPc = getPointFitness(refinedEstimatePc, pna, pnaCount); - FLT fitGd = getPointFitness(refinedEstimageGd, pna, pnaCount); - //FLT fitGd2 = getPointFitness(refinedEstimageGd2, pna, pnaCount); + FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); printf("Fitness is %f\n", fitGd); @@ -986,45 +532,6 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) fclose(logFile); } //fgetc(stdin); - return refinedEstimageGd; + return refinedEstimateGd; } -static Point makeUnitPoint(Point *p) -{ - Point newP; - double r = sqrt(p->x*p->x + p->y*p->y + p->z*p->z); - newP.x = p->x / r; - newP.y = p->y / r; - newP.z = p->z / r; - - return newP; -} - -static double getPhi(Point p) -{ - // double phi = acos(p.z / (sqrt(p.x*p.x + p.y*p.y + p.z*p.z))); - // double phi = atan(sqrt(p.x*p.x + p.y*p.y)/p.z); - double phi = atan(p.x / p.z); - return phi; -} - -static double getTheta(Point p) -{ - //double theta = atan(p.y / p.x); - double theta = atan(p.x / p.y); - return theta; -} - -// subtraction -static Point PointSub(Point a, Point b) -{ - Point newPoint; - - newPoint.x = a.x - b.x; - newPoint.y = a.y - b.y; - newPoint.z = a.z - b.z; - - return newPoint; -} - - -- cgit v1.2.3