plot_lighthouse/Makefile

10 files changed, 710 insertions, 52 deletions

diff --git a/tools/lighthousefind_tori/main.c b/tools/lighthousefind_tori/main.c
index aa51448..ee56b37 100644
--- a/tools/lighthousefind_tori/main.c
+++ b/tools/lighthousefind_tori/main.c
@@ -51,6 +51,9 @@ static void runTheNumbers()
 			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].normal.x = hmd_norms[i * 3 + 0];
+			to->sensor[sensorCount].normal.y = hmd_norms[i * 3 + 1];
+			to->sensor[sensorCount].normal.z = hmd_norms[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++;
diff --git a/tools/lighthousefind_tori/tori_includes.h b/tools/lighthousefind_tori/tori_includes.h
index 4cfbcdc..a2b9082 100644
--- a/tools/lighthousefind_tori/tori_includes.h
+++ b/tools/lighthousefind_tori/tori_includes.h
@@ -4,13 +4,15 @@
 #include <stddef.h>
 #include <math.h>
 #include <stdint.h>
+#include "linmath.h"
 
+#define PointToFlts(x) ((FLT*)(x))
 
 typedef struct
 {
-	double x;
-	double y;
-	double z;
+	FLT x;
+	FLT y;
+	FLT z;
 } Point;
 
 typedef struct
diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 22a0ce2..837b745 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,461 @@ 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);
+
+	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);
+	//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;
+
+	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;
+}
+
+// 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);
+	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.
+	//   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 = getNormalizedVector(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 = getNormalizedVector(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 = getNormalizedVector(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);
+
+		if (newMatchFitness > lastMatchFitness)
+		{
+			if (logFile)
+			{
+				writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
+			}
+
+			lastMatchFitness = newMatchFitness;
+			lastPoint = point4;
+			printf("+");
+		}
+		else
+		{
+			printf("-");
+			g *= 0.7;
+
+		}
+
+
+	}
+	printf("\ni=%d\n", i);
+
+	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(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 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)
+{
+	PointsAndAngle pna[MAX_POINT_PAIRS];
+
+	Point avgNorm = { 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 = 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++;
+			}
+		}
+
+		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)
 	{
-		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);
+
+	// 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
+	// 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 = getNormalizedVector(avgNorm, 8); 
+
+	Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
+
+	FLT pf1[3] = { refinedEstimageGd.x, refinedEstimageGd.y, refinedEstimageGd.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);
+
+		//FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z };
+
+		//FLT a2 = anglebetween3d(pf2, avgNormF);
+
+	}
+
+	//FLT fitPc = getPointFitness(refinedEstimatePc, pna, pnaCount);
+	FLT fitGd = getPointFitness(refinedEstimageGd, pna, pnaCount);
+	//FLT fitGd2 = getPointFitness(refinedEstimageGd2, pna, pnaCount);
+
+	printf("Fitness is %f\n", fitGd);
+
+	if (logFile)
+	{
+		updateHeader(logFile);
+		fclose(logFile);
+	}
+	//fgetc(stdin);
+	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");
diff --git a/tools/ootx_decode/HMD_Datagen.c b/tools/ootx_decode/HMD_Datagen.c
new file mode 100644
index 0000000..15ed62c
--- /dev/null
+++ b/tools/ootx_decode/HMD_Datagen.c
@@ -0,0 +1,103 @@
+// (C) 2017 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+/* generate data to test ootx decoding */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdint.h>
+#include <time.h>
+#include <stdlib.h>
+#include <zlib.h>
+
+//this program is broken and does not produce useable data.
+
+uint32_t time_stamp = -525198892;
+
+char* fmt_str = "L Y HMD %d 5 1 206230 %d\n";
+
+void print_bit(uint8_t data);
+void print_preamble();
+void print_uint16(uint16_t d);
+void print_uint32(uint32_t d);
+void print_payload(char* data, uint16_t length);
+
+
+int main(int argc, char* argv[])
+{
+	char* str = "Hello World!";
+//	printf("%s\n", str);
+
+	srand(time(NULL));
+
+	print_preamble();
+
+	uint16_t payload_lenth = strlen(str);
+	uint32_t crc = crc32( 0L, Z_NULL, 0 );
+	crc = crc32( crc, (uint8_t*)str,payload_lenth);
+
+	print_uint16(payload_lenth);
+	print_payload(str,payload_lenth);
+	print_uint32(crc);
+
+	return 0;
+}
+
+void print_preamble() {
+	int i;
+	for (i=0;i<17;++i) print_bit(0);
+	print_bit(1);
+}
+
+void print_uint16(uint16_t d) {
+	int i;
+	for (i=0;i<16;++i) {
+		print_bit(d & 0x0001);
+		d>>=1;
+	}
+	print_bit(1);
+}
+
+void print_uint32(uint32_t d) {
+	int i = 0;
+	for (;i<16;++i) {
+		print_bit(d & 0x01);
+		d>>=1;
+	}
+	print_bit(1);
+
+	for (;i<32;++i) {
+		print_bit(d & 0x01);
+		d>>=1;
+	}
+	print_bit(1);
+}
+
+void print_payload(char* data, uint16_t length) {
+	int i;
+	for(i=0;i<length;i+=2) {
+		uint16_t d = *((uint16_t*)(data+i));
+//		printf("%d\n", d);
+		print_uint16(d);
+	}
+}
+
+void print_bit(uint8_t data) {
+	uint32_t length = 3000 + (rand()%2)*500 + data*1000 + (rand()%2)*2000;
+	length -= rand()%500;
+	printf(fmt_str, time_stamp, length);
+
+	time_stamp++;
+
+	/*
+	//to decode
+	// 3000 + x*500 + dbit*1000 + y*2000
+	length -= 3000;
+	if (length>=2000) { length-=2000; y = 0x01; }
+	if (length>=1000)  { length-=1000; dbit = 0x01; }
+	if (length>=500)  { x = 0x01; }
+	*/
+
+	//fire off a callback when a full OOTX packet is received
+}
diff --git a/tools/ootx_decode/Makefile b/tools/ootx_decode/Makefile
new file mode 100644
index 0000000..b3b07b4
--- /dev/null
+++ b/tools/ootx_decode/Makefile
@@ -0,0 +1,7 @@
+all: ootx_decode hmd_datagen
+
+hmd_datagen: HMD_Datagen.c 
+	gcc -Wall HMD_Datagen.c -lz -o hmd_datagen
+
+ootx_decode: ootx_decode.c ../../src/ootx_decoder.c ../../src/ootx_decoder.h
+	gcc -Wall ootx_decode.c ../../src/ootx_decoder.c -lz -o ootx_decode -I ../../src/
diff --git a/tools/ootx_decode/ootx_decode.c b/tools/ootx_decode/ootx_decode.c
new file mode 100644
index 0000000..1823aaa
--- /dev/null
+++ b/tools/ootx_decode/ootx_decode.c
@@ -0,0 +1,121 @@
+// (C) 2017 Joshua Allen, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+/* ootx data decoder test*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+#include <assert.h>
+
+#include "ootx_decoder.h"
+
+void my_test(ootx_decoder_context *ctx, ootx_packet* packet) {
+	packet->data[packet->length] = 0;
+	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
+}
+
+void my_test2(ootx_decoder_context *ctx, ootx_packet* packet) {
+	printf("completed ootx packet\n");
+
+	lighthouse_info_v6 lhi;
+	init_lighthouse_info_v6(&lhi,packet->data);
+	print_lighthouse_info_v6(&lhi);
+//	packet->data[packet->length] = 0;
+//	printf("%d %s 0x%X\n", packet->length, packet->data, packet->crc32);
+}
+
+
+void print_crc32(uint32_t crc) {
+//	uint8_t* p = (uint32_t*)&crc;
+//	uint8_t i = 0;
+
+	printf("%X\n", crc);
+}
+
+void write_to_file(uint8_t *d, uint16_t length){
+	FILE *fp = fopen("binary.data","w");
+	fwrite(d, length, 1, fp);
+	fclose(fp);
+}
+
+void bad_crc(ootx_decoder_context *ctx, ootx_packet* packet, uint32_t crc) {
+	printf("CRC mismatch\n");
+
+	printf("r:");
+	print_crc32(packet->crc32);
+
+	printf("c:");
+	print_crc32(crc);
+	write_to_file(packet->data,packet->length);
+}
+
+ootx_decoder_context ctx[2];
+
+void cnlohr_code_test() {
+	ootx_packet_clbk = my_test2;
+	ootx_bad_crc_clbk = bad_crc;
+
+	char* line = NULL;
+	size_t line_len = 0;
+	char trash[100] = "";
+	int8_t lh_id = 0x00;
+	uint32_t ticks = 0x00;
+	int32_t delta = 0x00;
+	uint8_t acode = 0x00;
+
+	ootx_decoder_context *c_ctx = ctx;
+
+	while (getline(&line,&line_len,stdin)>0) {
+			//R Y HMD -1575410734 -2 7 19714 6485
+			sscanf(line,"%s %s %s %s %hhd %hhd %d %d",
+			trash,
+			trash,
+			trash,
+			trash,
+			&lh_id,
+			&acode,
+			&delta,
+			&ticks);
+
+//		int8_t lh = lighthouse_code=='R'?0:1;
+//		printf("LH:%d %s\n", lh_id, line);
+		int8_t lh = (lh_id*-1)-1;
+		if (lh_id < 0) {
+//			uint8_t bit = 0x01; //bit for debugging purposes
+
+			//change to newly found lighthouse
+			c_ctx = ctx+lh;
+
+//			uint8_t dbit = ootx_decode_bit(ticks);
+//			printf("LH:%d ticks:%d bit:%X %s", lh, ticks, dbit, line);
+
+//			ootx_process_bit(c_ctx, ticks);
+			ootx_pump_bit( c_ctx, (acode&0x02)>>1 );
+/*
+			uint16_t s = *(c_ctx->payload_size);
+			uint16_t fwv = *(c_ctx->buffer+2);
+			uint16_t pv = 0x3f & fwv; //protocol version
+			fwv>>=6; //firmware version
+
+			//this will print after any messages from ootx_pump
+//			if (c_ctx->found_preamble>0) printf("LH:%d s:%d 0x%x fw:%d pv:%d bo:%d bit:%d\t%s", current_lighthouse, s, s, fwv, pv, c_ctx->buf_offset, bit, line);
+*/	
+		}
+	}
+}
+
+int main(int argc, char* argv[])
+{
+	ootx_init_decoder_context(ctx);
+	ootx_init_decoder_context(ctx+1);
+
+	cnlohr_code_test();
+
+	ootx_free_decoder_context(ctx);
+	ootx_free_decoder_context(ctx+1);
+
+	return 0;
+}
diff --git a/tools/plot_lighthouse/Makefile b/tools/plot_lighthouse/Makefile
index 07bdcb9..38eece0 100644
--- a/tools/plot_lighthouse/Makefile
+++ b/tools/plot_lighthouse/Makefile
@@ -1,7 +1,7 @@
 UNAME := $(shell uname)
 
 ifeq ($(UNAME), Linux)
-CFLAGS:= -I../../redist -lGL -lGLU -lglut
+CFLAGS:= -lGL -lGLU -lglut -I../../redist -DLINUX -lm -lpthread
 endif
 
 # Darwin is Mac OSX !!
diff --git a/tools/plot_lighthouse/main.c b/tools/plot_lighthouse/main.c
index 8088828..4a64c28 100644
--- a/tools/plot_lighthouse/main.c
+++ b/tools/plot_lighthouse/main.c
@@ -13,6 +13,11 @@
 #include "glutil.h"
 #include "fileutil.h"
 
+#ifdef LINUX
+#include <GL/freeglut.h>
+#endif
+
+
 // Required to set up a window
 #define WIDTH  800
 #define HEIGHT 600
diff --git a/tools/process_rawcap/process_to_points.c b/tools/process_rawcap/process_to_points.c
index a1e835d..3ccfcc1 100644
--- a/tools/process_rawcap/process_to_points.c
+++ b/tools/process_rawcap/process_to_points.c
@@ -192,7 +192,8 @@ int main( int argc, char ** argv )
 			stddevtim += Sdiff * Sdiff;
 			stddevlen += Ldiff * Ldiff;
 
-			Sdiff/=4;
+			//Cast a wider net for the histogram.
+			//Sdiff/=4;
 
 			int llm = Sdiff + (HISTOGRAMSIZE/2.0);
 			if( llm < 0 ) llm = 0;