aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--tools/lighthousefind_tori/torus_localizer.c267
-rw-r--r--tools/lighthousefind_tori/visualization.c4
2 files changed, 224 insertions, 47 deletions
diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 22a0ce2..15177f2 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -400,45 +400,18 @@ double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd));
return pythAngle;
}
-Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
-{
- PointsAndAngle pna[MAX_POINT_PAIRS];
-
- 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 = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
-
- double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
-
- pnaCount++;
- }
- }
- }
+Point GetInitialEstimate(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+{
Point **pointCloud = malloc(sizeof(void*)* pnaCount);
- FILE *f = NULL;
- if (doLogOutput)
- {
- f = fopen("pointcloud2.pcd", "wb");
- writePcdHeader(f);
- writeAxes(f);
- }
for (unsigned int i = 0; i < pnaCount; i++)
{
torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i]));
- if (doLogOutput)
+ if (logFile)
{
- writePointCloud(f, pointCloud[i], COLORS[i%MAX_COLORS]);
+ writePointCloud(logFile, pointCloud[i], COLORS[i%MAX_COLORS]);
}
}
@@ -451,13 +424,19 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
pointCloud[i] = NULL;
}
- if (doLogOutput)
+ if (logFile)
{
- markPointWithStar(f, bestMatchA, 0xFF0000);
+ 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;
@@ -473,15 +452,15 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
pna[i].a,
pna[i].b,
pna[i].angle,
- bestMatchA,
+ initialEstimate,
&toroidalAngle,
&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 (doLogOutput)
+ if (logFile)
{
- writePointCloud(f, pointCloud2[i], COLORS[i%MAX_COLORS]);
+ writePointCloud(logFile, pointCloud2[i], COLORS[i%MAX_COLORS]);
}
}
@@ -494,9 +473,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
pointCloud2[i] = NULL;
}
- if (doLogOutput)
+ if (logFile)
{
- markPointWithStar(f, bestMatchB, 0x00FF00);
+ markPointWithStar(logFile, bestMatchB, 0x00FF00);
}
#ifdef TORI_DEBUG
printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z);
@@ -516,9 +495,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
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 (doLogOutput)
+ if (logFile)
{
- writePointCloud(f, pointCloud3[i], COLORS[i%MAX_COLORS]);
+ writePointCloud(logFile, pointCloud3[i], COLORS[i%MAX_COLORS]);
}
}
@@ -531,24 +510,222 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
pointCloud3[i] = NULL;
}
- if (doLogOutput)
+ if (logFile)
{
- markPointWithStar(f, bestMatchC, 0xFFFFFF);
+ 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 poloidalAngle)
+{
+ Point result;
+
+ double distanceBetweenPoints = distance(pna->a, pna->b);
+ Point m = midpoint(pna->a, pna->b);
+ Matrix3x3 rot = GetRotationMatrixForTorus(pna->a, pna->b);
+
+ double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
+ double poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+
+ result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*cos(toroidalAngle);
+ result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*sin(toroidalAngle);
+ result.z = poloidalRadius*sin(poloidalAngle);
+ result = RotateAndTranslatePoint(result, rot, m);
+
+ return result;
+}
+
+FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
+{
+
+ double toroidalAngle = 0;
+ double poloidalAngle = 0;
+
+ estimateToroidalAndPoloidalAngleOfPoint(
+ pna->a,
+ pna->b,
+ pna->angle,
+ pointIn,
+ &toroidalAngle,
+ &poloidalAngle);
+
+ Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, poloidalAngle);
+
+ return distance(pointIn, torusPoint);
+}
+
+//Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile)
+//
+FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount)
+{
+ FLT fitness;
+
+ FLT resultSum=0;
+
+ for (size_t i = 0; i < pnaCount; i++)
+ {
+ fitness = getPointFitnessForPna(pointIn, &(pna[i]));
+ //printf("Distance[%d]: %f\n", i, fitness);
+ resultSum += SQUARED(fitness);
+ }
+
+ return 1/FLT_SQRT(resultSum);
+}
+
+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) - getPointFitness(tmpXminus, pna, pnaCount);
+
+ Point tmpYplus = pointIn;
+ Point tmpYminus = pointIn;
+ tmpYplus.y = pointIn.y + precision;
+ tmpYminus.y = pointIn.y - precision;
+ result.y = getPointFitness(tmpYplus, pna, pnaCount) - getPointFitness(tmpYminus, pna, pnaCount);
+
+ Point tmpZplus = pointIn;
+ Point tmpZminus = pointIn;
+ tmpZplus.z = pointIn.z + precision;
+ tmpZminus.z = pointIn.z - precision;
+ result.z = getPointFitness(tmpZplus, pna, pnaCount) - getPointFitness(tmpZminus, pna, pnaCount);
+
+ return result;
+}
+
+Point getNormalizedVector(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;
+}
+
+// 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;
+}
+
+Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
+{
+ PointsAndAngle pna[MAX_POINT_PAIRS];
+
+ 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 = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+
+ double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
+
+ pnaCount++;
+ }
+ }
+ }
+
+ FILE *logFile = NULL;
if (doLogOutput)
{
- updateHeader(f);
- fclose(f);
+ logFile = fopen("pointcloud2.pcd", "wb");
+ writePcdHeader(logFile);
+ writeAxes(logFile);
}
+ Point initialEstimate = GetInitialEstimate(obj, pna, pnaCount, logFile);
+ //Point refinedEstimatePc = RefineEstimateUsingPointCloud(initialEstimate, pna, pnaCount, obj, logFile);
- return bestMatchC;
+ Point refinedEstimageGd = RefineEstimateUsingGradientDescent(initialEstimate, pna, pnaCount, logFile, 0.95, 0.1);
+
+ //FLT fitPc = getPointFitness(refinedEstimatePc, pna, pnaCount);
+ FLT fitGd = getPointFitness(refinedEstimageGd, pna, pnaCount);
+
+ if (logFile)
+ {
+ updateHeader(logFile);
+ fclose(logFile);
+ }
+
+ return refinedEstimageGd;
}
static Point makeUnitPoint(Point *p)
diff --git a/tools/lighthousefind_tori/visualization.c b/tools/lighthousefind_tori/visualization.c
index 12cbfee..098e0e2 100644
--- a/tools/lighthousefind_tori/visualization.c
+++ b/tools/lighthousefind_tori/visualization.c
@@ -60,10 +60,10 @@ void writePcdHeader(FILE * file)
fprintf(file, "SIZE 4 4 4 4\n");
fprintf(file, "TYPE F F F U\n");
fprintf(file, "COUNT 1 1 1 1\n");
- fprintf(file, "WIDTH \n");
+ fprintf(file, "WIDTH \n");
fprintf(file, "HEIGHT 1\n");
fprintf(file, "VIEWPOINT 0 0 0 1 0 0 0\n");
- fprintf(file, "POINTS \n");
+ fprintf(file, "POINTS \n");
fprintf(file, "DATA ascii\n");
//fprintf(file, "100000.0, 100000.0, 100000\n");