Merge pull request #34 from mwturvey/PoserUpdates2

Poser updates2

4 files changed, 703 insertions, 152 deletions

diff --git a/redist/linmath.c b/redist/linmath.c
index 72c00a4..37d8e7a 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -516,150 +516,3 @@ void matrix44copy(FLT * mout, const FLT * minm )
 	memcpy( mout, minm, sizeof( FLT ) * 16 );
 }
 
-///////////////////////////////////////Matrix Rotations////////////////////////////////////
-////Originally from Stack Overflow 
-////Under cc by-sa 3.0
-////  http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw
-//// Copyright 2014 by Campbell Barton
-//// Copyright 2017 by Michael Turvey
-//
-///**
-//* Calculate a rotation matrix from 2 normalized vectors.
-//*
-//* v1 and v2 must be unit length.
-//*/
-//void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3])
-//{
-//	FLT axis[3];
-//	/* avoid calculating the angle */
-//	FLT angle_sin;
-//	FLT angle_cos;
-//
-//	cross3d(axis, v1, v2);
-//
-//	angle_sin = normalize_v3(axis);
-//	angle_cos = dot3d(v1, v2);
-//
-//	if (angle_sin > FLT_EPSILON) {
-//	axis_calc:
-//		axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos);
-//	}
-//	else {
-//		/* Degenerate (co-linear) vectors */
-//		if (angle_cos > 0.0f) {
-//			/* Same vectors, zero rotation... */
-//			unit_m3(m);
-//		}
-//		else {
-//			/* Colinear but opposed vectors, 180 rotation... */
-//			get_orthogonal_vector(axis, v1);
-//			normalize_v3(axis);
-//			angle_sin = 0.0f;  /* sin(M_PI) */
-//			angle_cos = -1.0f;  /* cos(M_PI) */
-//			goto axis_calc;
-//		}
-//	}
-//}
-
-//void get_orthogonal_vector(FLT out[3], const FLT in[3])
-//{
-//#ifdef USE_DOUBLE
-//	const FLT x = fabs(in[0]);
-//	const FLT y = fabs(in[1]);
-//	const FLT z = fabs(in[2]);
-//#else
-//	const FLT x = fabsf(in[0]);
-//	const FLT y = fabsf(in[1]);
-//	const FLT z = fabsf(in[2]);
-//#endif
-//
-//	if (x > y && x > z)
-//	{
-//		// x is dominant
-//		out[0] = -in[1] - in[2];
-//		out[1] = in[0];
-//		out[2] = in[0];
-//	}
-//	else if (y > z)
-//	{
-//		// y is dominant
-//		out[0] = in[1];
-//		out[1] = -in[0] - in[2];
-//		out[2] = in[1];
-//	}
-//	else
-//	{
-//		// z is dominant
-//		out[0] = in[2];
-//		out[1] = in[2];
-//		out[2] = -in[0] - in[1];
-//	}
-//}
-//
-//void unit_m3(FLT mat[3][3])
-//{
-//	mat[0][0] = 1;
-//	mat[0][1] = 0;
-//	mat[0][2] = 0;
-//	mat[1][0] = 0;
-//	mat[1][1] = 1;
-//	mat[1][2] = 0;
-//	mat[2][0] = 0;
-//	mat[2][1] = 0;
-//	mat[2][2] = 1;
-//}
-
-
-//FLT normalize_v3(FLT vect[3])
-//{
-//	FLT distance = dot3d(vect, vect);
-//
-//	if (distance < 1.0e-35f)
-//	{
-//		// distance is too short, just go to zero.
-//		vect[0] = 0;
-//		vect[1] = 0;
-//		vect[2] = 0;
-//		distance = 0;
-//	}
-//	else
-//	{
-//		distance = FLT_SQRT((FLT)distance);
-//		scale3d(vect, vect, 1.0f / distance);
-//	}
-//
-//	return distance;
-//}
-
-///* axis must be unit length */
-//void axis_angle_normalized_to_mat3_ex(
-//	FLT mat[3][3], const FLT axis[3],
-//	const FLT angle_sin, const FLT angle_cos)
-//{
-//	FLT nsi[3], ico;
-//	FLT n_00, n_01, n_11, n_02, n_12, n_22;
-//
-//	ico = (1.0f - angle_cos);
-//	nsi[0] = axis[0] * angle_sin;
-//	nsi[1] = axis[1] * angle_sin;
-//	nsi[2] = axis[2] * angle_sin;
-//
-//	n_00 = (axis[0] * axis[0]) * ico;
-//	n_01 = (axis[0] * axis[1]) * ico;
-//	n_11 = (axis[1] * axis[1]) * ico;
-//	n_02 = (axis[0] * axis[2]) * ico;
-//	n_12 = (axis[1] * axis[2]) * ico;
-//	n_22 = (axis[2] * axis[2]) * ico;
-//
-//	mat[0][0] = n_00 + angle_cos;
-//	mat[0][1] = n_01 + nsi[2];
-//	mat[0][2] = n_02 - nsi[1];
-//	mat[1][0] = n_01 - nsi[2];
-//	mat[1][1] = n_11 + angle_cos;
-//	mat[1][2] = n_12 + nsi[0];
-//	mat[2][0] = n_02 + nsi[1];
-//	mat[2][1] = n_12 - nsi[0];
-//	mat[2][2] = n_22 + angle_cos;
-//}
-
-
diff --git a/tools/lighthousefind_radii/Makefile b/tools/lighthousefind_radii/Makefile
new file mode 100644
index 0000000..b60e07c
--- /dev/null
+++ b/tools/lighthousefind_radii/Makefile
@@ -0,0 +1,10 @@
+all : lighthousefind_radii
+
+CFLAGS:=-g -O4 -DUSE_DOUBLE -I../../redist -flto
+LDFLAGS:=$(CFLAGS) -lm 
+
+lighthousefind : lighthousefind.o ../../redist/linmath.c
+	gcc -o $@ $^  $(LDFLAGS)
+
+clean :
+	rm -rf *.o *~ lighthousefind_radii
diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c
new file mode 100644
index 0000000..8a684cb
--- /dev/null
+++ b/tools/lighthousefind_radii/lighthousefind_radii.c
@@ -0,0 +1,557 @@
+#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;
+int LoadData( char Camera, const char * FileData );
+
+//Values used for RunTest()
+FLT LighthousePos[3] = { 0, 0, 0 };
+FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
+
+FLT RunTest( int print );
+void PrintOpti();
+
+#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.
+} 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))
+
+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);
+}
+
+FLT calculateFitness(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);
+}
+
+#define MAX_RADII 32
+
+// note gradientOut will be of the same degree as numRadii
+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;
+}
+
+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 RefineEstimateUsingGradientDescent(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) ", i, newMatchFitness, g);
+#endif
+			g = g * 1.05;
+		}
+		else
+		{
+#ifdef RADII_DEBUG
+//			printf("-");
+			printf("- %d %0.9f (%0.9f) ", i, newMatchFitness, g);
+#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("\ni=%d\n", i);
+}
+
+void SolveForLighthouse(Point *objPosition, FLT *objOrientation, TrackedObject *obj)
+{
+	FLT estimate[MAX_RADII];
+
+	for (size_t i = 0; i < MAX_RADII; i++)
+	{
+		estimate[i] = 2.4;
+	}
+
+	SensorAngles angles[MAX_RADII];
+	PointPair pairs[MAX_POINT_PAIRS];
+
+	size_t pairCount = 0;
+
+	obj->numSensors = 7; // 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 (size_t i = 0; i < obj->numSensors - 1; i++)
+	{
+		for (size_t 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++;
+		}
+	}
+
+
+	RefineEstimateUsingGradientDescent(estimate, angles, estimate, obj->numSensors, pairs, pairCount, NULL);
+
+	// we should now have an estimate of the radii.
+
+	for (size_t i = 0; i < obj->numSensors; 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)
+
+	getc(stdin);
+	return;
+}
+
+static void runTheNumbers()
+{
+	TrackedObject *to;
+
+	to = malloc(sizeof(TrackedObject) + (PTS * sizeof(TrackedSensor)));
+
+	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].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++;
+		}
+	}
+
+	to->numSensors = sensorCount;
+
+	printf("Using %d sensors to find lighthouse.\n", sensorCount);
+
+	Point lh;
+	for (int i = 0; i < 1; i++)
+	{
+		SolveForLighthouse(&lh, NULL, to);
+		//(0.156754, -2.403268, 2.280167)
+		//assert(fabs((lh.x / 0.1419305302702402) - 1) < 0.00001);
+		//assert(fabs((lh.y / 2.5574949720325431) - 1) < 0.00001);
+		//assert(fabs((lh.z / 2.2451193935772080) - 1) < 0.00001);
+		//assert(lh.x > 0);
+		//assert(lh.y > 0);
+		//assert(lh.z > 0);
+	}
+
+	printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z);
+
+	//printTrackedObject(to);
+	free(to);
+}
+
+int main( int argc, char ** argv )
+{
+
+	if( argc != 3 )
+	{
+		fprintf( stderr, "Error: usage: camfind [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;
+
+	runTheNumbers();
+
+
+	
+}
+
+
+
+
+
+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 fa, fb, fc;
+		int r = fscanf( f,"%g %g %g\n", &fa, &fb, &fc );
+		hmd_points[pt*3+0] = fa;
+		hmd_points[pt*3+1] = fb;
+		hmd_points[pt*3+2] = fc;
+		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;
+		FLT avgTime;
+		FLT avgLen;
+		FLT stddevTime;
+		FLT 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_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;
+}
+
diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index fd74b22..894bbce 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -7,12 +7,12 @@
 #include "visualization.h"
 
 
-static double distance(Point a, Point b)
+static FLT distance(Point a, Point b)
 {
-	double x = a.x - b.x;
-	double y = a.y - b.y;
-	double z = a.z - b.z;
-	return sqrt(x*x + y*y + z*z);
+	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)
@@ -457,6 +457,137 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 }
 
 
+// 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 RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj)
+{
+	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 t1[3] = { 1, tan(theta), 0 };
+		FLT t2[3] = { 1, tan(theta), 1 };
+
+		FLT tNorm[3];
+
+		// the normal is the cross of two vectors on the plane.
+		cross3d(tNorm, t1, t2);
+
+		// distance for this plane is d= fabs(A*x + B*y)/sqrt(A^2+B^2) (z term goes away since this plane is "vertical")
+		// where A is 
+		//FLT d = 
+	}
+}
+
+static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+{
+	int i = 0;
+	FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount);
+	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 = 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;
+#ifdef TORI_DEBUG
+			printf("+");
+#endif
+		}
+		else
+		{
+#ifdef TORI_DEBUG
+			printf("-");
+#endif
+			g *= 0.7;
+
+		}
+
+
+	}
+	printf("\ni=%d\n", i);
+
+	return lastPoint;
+}
+
+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[3] = { 0, 0, 1 };
+	FLT quat1[4];
+	quatfromaxisangle(quat1, zAxis, theta);
+	// not correcting for phi, but that's less important.
+
+	// Step 2, optimize the quaternion to match the data.
+
+}
+
+
 Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 {
 	PointsAndAngle pna[MAX_POINT_PAIRS];