Add charles' camfind (DOES NOT WORK!!!)

4 files changed, 654 insertions, 0 deletions

diff --git a/tools/planetest/Makefile b/tools/planetest/Makefile
new file mode 100644
index 0000000..0e55f12
--- /dev/null
+++ b/tools/planetest/Makefile
@@ -0,0 +1,10 @@
+all : camfind
+
+CFLAGS:=-g -O4
+LDFLAGS:=$(CFLAGS) -lm
+
+camfind : camfind.o linmath.o
+	gcc -o $@ $^  $(LDFLAGS)
+
+clean :
+	rm -rf *.o *~ camfind
diff --git a/tools/planetest/camfind.c b/tools/planetest/camfind.c
new file mode 100644
index 0000000..c1fba0e
--- /dev/null
+++ b/tools/planetest/camfind.c
@@ -0,0 +1,259 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include "linmath.h"
+#include <string.h>
+
+#define PTS 32
+#define MAX_CHECKS 20000
+
+double hmd_points[PTS*3];
+double hmd_point_angles[PTS*2];
+int    hmd_point_counts[PTS*2];
+int LoadData( char Camera );
+
+//Values used for RunTest()
+float LighthousePos[3] = { 0, 0,0 };
+float LighthouseQuat[4] = { 1, 0, 0, 0 };
+
+//Actual values
+float xyzypr[6] = { 0, 0, 1, 0, 0, 0 };
+
+
+float RunOpti( int axis );
+float RunTest();
+
+int main()
+{
+	int i;
+
+	if( LoadData( 'L' ) ) return 5;
+
+	int opti = 0;
+	int cycle = 0;
+	int axis = 0;
+
+	float dx;
+
+	float bestxyzypr[6];
+	memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) );
+
+	float fullrange = 5;
+	for( cycle = 0; cycle < 10; cycle ++ )
+	{
+		for( axis = 0; axis < 3; axis++ )
+		{
+			float bestxyzyprrunning[6];
+			float beste = 1e20;
+			for( dx = -fullrange; dx < fullrange; dx += fullrange/100 )
+			{
+				memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) );
+				xyzypr[axis] += dx;
+				xyzypr[3] = 0;
+				xyzypr[4] = 0;
+				xyzypr[5] = 0;
+				float ft;
+				for( opti = 3; opti < 6; opti++ )
+				{
+					ft = RunOpti( opti );
+				}
+				if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); }
+			}
+			printf( "%f %f %f %f %f %f = %f\n", xyzypr[0], xyzypr[1], xyzypr[2], xyzypr[3], xyzypr[4], xyzypr[5], beste );
+			memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) );
+		}
+
+		fullrange *= 0.2;
+	}
+}
+
+
+float RunOpti( int axis )
+{
+	float xyzyprchange[6];
+	memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) );
+	int i;
+	float minv = 1e20;
+	float fv = -1.3;
+	float bv = 0;
+
+	//Coarse linear search
+	for( i = 0; i < 26; i++ )
+	{
+		xyzyprchange[axis] = fv;
+		quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
+		memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
+		float ft = RunTest();
+		if( ft < minv ) { minv = ft; bv = fv; }
+		fv += 0.1;
+	}
+
+	xyzyprchange[axis] = bv;
+
+	//Now, do a more precise binary search.
+	float jumpamount = 0.1;
+	for( i = 0; i < 30; i++ )
+	{
+		float orig = xyzyprchange[axis];
+
+		minv = 1e20;
+
+		fv = xyzyprchange[axis] = orig;
+		quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
+		memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
+		float ft = RunTest();
+		if( ft < minv ) { minv = ft; bv = fv; }
+
+		fv = xyzyprchange[axis] = orig + jumpamount;
+		quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
+		memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
+		ft = RunTest();
+		if( ft < minv ) { minv = ft; bv = fv; }
+
+		fv = xyzyprchange[axis] = orig - jumpamount;
+		quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
+		memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
+		ft = RunTest();
+		if( ft < minv ) { minv = ft; bv = fv; }
+
+		xyzyprchange[axis] = bv;
+
+		jumpamount *= 0.5;
+	}
+
+
+
+	xyzypr[axis] = bv;
+	return minv;
+}
+
+
+float RunTest()
+{
+	int k;
+	float totprob = 0.0;
+	for( k = 0; k < 64; k++ )
+	{
+		if( hmd_point_counts[k] == 0 ) continue;
+		int axis = k%2;
+		int pt = k/2;
+		float angle = (hmd_point_angles[k] - 200000) / 200000 * 3.1415926535;
+		if( axis == 0 ) angle = -angle;
+		float thiseuler[3] = { 0, 0, 0 };
+		thiseuler[axis] = angle;  
+		//axis = 0: x changes.  +y is rotated plane normal.
+		//axis = 1: y changes.  +x is rotated plane normal.
+		float planequat[4];
+		quatfromeuler( planequat, thiseuler );
+		quatrotateabout( planequat, planequat, LighthouseQuat );  //Order of rotations may be wrong.
+		float plane_normal[3] = { 0, 0, 0 };
+		plane_normal[!axis] = 1;
+		quatrotatevector( plane_normal, planequat, plane_normal );
+
+		//plane_normal is our normal / LighthousePos is our point.
+		float w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] };
+		//float w0[] = { 0, 0, 0 };
+		float d = plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2];
+		float D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] - d;  //Point line distance assuming ||normal|| = 1.
+
+		totprob += D*D;
+
+		//printf( "  : %f %f %f %f  %f %f\n", plane_normal[0], plane_normal[1], plane_normal[2], 1.0, angle, hmd_point_angles[k] );
+	}
+	return totprob;
+}
+
+
+int LoadData( char Camera )
+{
+	int calpts[MAX_CHECKS*4]; //X (0) or Y (1), ID, offset
+	int calptscount;
+
+	FILE * f = fopen( "correct_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\n", pt );
+			return -8;
+		}
+	}
+	if( pt < PTS )
+	{
+		fprintf( stderr, "Not enough points.\n" );
+		return -9;
+	}
+	fclose( f );
+	printf( "Loaded %d points\n", pt );
+
+
+	int xck = 0;
+	f = fopen( "second_full_test.csv", "r" );
+	if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; }
+	while( !feof(f) && !ferror(f) )
+	{
+		char * lineptr;
+		size_t n;
+		lineptr = 0;
+		n = 0;
+		ssize_t r = getline( &lineptr, &n, f );
+		char lf[10];
+		char xory[10];
+		char dev[10];
+		int timestamp;
+		int sensorid;
+		int offset;
+		int code;
+		int rk = sscanf( lineptr, "%9s %9s %9s %d %d %d %d\n", lf, xory, dev, &timestamp, &sensorid, &code, &offset );
+		if( lf[0] == Camera && rk == 7 )
+		{
+			//calpts
+			if( xory[0] == 'X' )
+			{
+				calpts[xck*3+0] = 0;
+			}
+			else if( xory[0] == 'Y' )
+			{
+				calpts[xck*3+0] = 1;
+			}
+			else
+			{
+				printf( "Confusing line\n" );
+				continue;
+			}
+
+			calpts[xck*3+1] = sensorid;
+			calpts[xck*3+2] = offset;
+			xck++;
+		}
+		if( lineptr ) free( lineptr );
+	}
+	printf( "Cal points: %d\n", xck );
+	calptscount = xck;
+
+	int i;
+	for( i = 0; i < calptscount; i++ )
+	{
+		int isy = calpts[i*3+0];
+		int pt = calpts[i*3+1];
+		int ofs = calpts[i*3+2];
+		hmd_point_counts[pt*2+isy]++;
+		hmd_point_angles[pt*2+isy]+=ofs;
+	}
+	for( i = 0; i < 32; i++ )
+	{
+		if( hmd_point_counts[i*2+0] < 100 ) hmd_point_counts[i*2+0] = 0;
+		if( hmd_point_counts[i*2+1] < 100 ) hmd_point_counts[i*2+1] = 0;
+
+		hmd_point_angles[i*2+0]/=hmd_point_counts[i*2+0];
+		hmd_point_angles[i*2+1]/=hmd_point_counts[i*2+1];
+	}
+	return 0;
+}
diff --git a/tools/planetest/linmath.c b/tools/planetest/linmath.c
new file mode 100644
index 0000000..dec8c70
--- /dev/null
+++ b/tools/planetest/linmath.c
@@ -0,0 +1,326 @@
+//Copyright 2013 <>< C. N. Lohr.  This file licensed under the terms of the MIT license.
+
+#include "linmath.h"
+#include <math.h>
+
+void cross3d( float * out, const float * a, const float * b )
+{
+	out[0] = a[1]*b[2] - a[2]*b[1];
+	out[1] = a[2]*b[0] - a[0]*b[2];
+	out[2] = a[0]*b[1] - a[1]*b[0];
+}
+
+void sub3d( float * out, const float * a, const float * b )
+{
+	out[0] = a[0] - b[0];
+	out[1] = a[1] - b[1];
+	out[2] = a[2] - b[2];
+}
+
+void add3d( float * out, const float * a, const float * b )
+{
+	out[0] = a[0] + b[0];
+	out[1] = a[1] + b[1];
+	out[2] = a[2] + b[2];
+}
+
+void scale3d( float * out, const float * a, float scalar )
+{
+	out[0] = a[0] * scalar;
+	out[1] = a[1] * scalar;
+	out[2] = a[2] * scalar;
+}
+
+void normalize3d( float * out, const float * in )
+{
+	float r = 1./sqrtf( in[0] * in[0] + in[1] * in[1] + in[2] * in[2] );
+	out[0] = in[0] * r;
+	out[1] = in[1] * r;
+	out[2] = in[2] * r;
+}
+
+float dot3d( const float * a, const float * b )
+{
+	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+}
+
+int compare3d( const float * a, const float * b, float epsilon )
+{
+	if( !a || !b ) return 0;
+	if( a[2] - b[2] > epsilon ) return 1;
+	if( b[2] - a[2] > epsilon ) return -1;
+	if( a[1] - b[1] > epsilon ) return 1;
+	if( b[1] - a[1] > epsilon ) return -1;
+	if( a[0] - b[0] > epsilon ) return 1;
+	if( b[0] - a[0] > epsilon ) return -1;
+	return 0;
+}
+
+void copy3d( float * out, const float * in )
+{
+	out[0] = in[0];
+	out[1] = in[1];
+	out[2] = in[2];
+}
+
+
+
+/////////////////////////////////////QUATERNIONS//////////////////////////////////////////
+//Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
+//Under the mit/X11 license.
+
+
+
+
+void quatsetnone( float * q )
+{
+	q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1;
+}
+
+void quatcopy( float * qout, const float * qin )
+{
+	qout[0] = qin[0];
+	qout[1] = qin[1];
+	qout[2] = qin[2];
+	qout[3] = qin[3];
+}
+
+void quatfromeuler( float * q, const float * euler )
+{
+	float X = euler[0]/2.0f; //roll
+	float Y = euler[1]/2.0f; //pitch
+	float Z = euler[2]/2.0f; //yaw
+
+	float cx = cosf(X);
+	float sx = sinf(X);
+	float cy = cosf(Y);
+	float sy = sinf(Y);
+	float cz = cosf(Z);
+	float sz = sinf(Z);
+
+	//Correct according to
+	//http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles
+	q[0] = cx*cy*cz+sx*sy*sz;//q1
+	q[1] = sx*cy*cz-cx*sy*sz;//q2
+	q[2] = cx*sy*cz+sx*cy*sz;//q3
+	q[3] = cx*cy*sz-sx*sy*cz;//q4
+	quatnormalize( q, q );
+}
+
+void quattoeuler( float * euler, const float * q )
+{
+	//According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009)
+	euler[0] = atan2( 2 * (q[0]*q[1] + q[2]*q[3]), 1 - 2 * (q[1]*q[1] + q[2]*q[2] ) );
+	euler[1] = asin( 2 * (q[0] *q[2] - q[3]*q[1] ) );
+	euler[2] = atan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3] ) );
+}
+
+void quatfromaxisangle( float * q, const float * axis, float radians )
+{
+	float v[3];
+	normalize3d( v, axis );
+	
+	float sn = sin(radians/2.0f);
+	q[0] = cos(radians/2.0f);
+	q[1] = sn * v[0];
+	q[2] = sn * v[1];
+	q[3] = sn * v[2];
+
+	quatnormalize( q, q );
+}
+
+float quatmagnitude( const float * q )
+{
+	return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
+}
+
+float quatinvsqmagnitude( const float * q )
+{
+	return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
+}
+
+
+void quatnormalize( float * qout, const float * qin )
+{
+	float imag = quatinvsqmagnitude( qin );
+	quatscale( qout, qin, imag );
+}
+
+void quattomatrix( float * matrix44, const float * qin )
+{
+	float q[4];
+	quatnormalize( q, qin );
+	
+	//Reduced calulation for speed
+	float xx = 2*q[0]*q[0];
+	float xy = 2*q[0]*q[1];
+	float xz = 2*q[0]*q[2];
+	float xw = 2*q[0]*q[3];
+	
+	float yy = 2*q[1]*q[1];
+	float yz = 2*q[1]*q[2];
+	float yw = 2*q[1]*q[3];
+	
+	float zz = 2*q[2]*q[2];
+	float zw = 2*q[2]*q[3];
+
+	//opengl major
+	matrix44[0] = 1-yy-zz;
+	matrix44[1] = xy-zw;
+	matrix44[2] = xz+yw;
+	matrix44[3] = 0;
+
+	matrix44[4] = xy+zw;
+	matrix44[5] = 1-xx-zz;
+	matrix44[6] = yz-xw;
+	matrix44[7] = 0;
+
+	matrix44[8] = xz-yw;
+	matrix44[9] = yz+xw;
+	matrix44[10] = 1-xx-yy;
+	matrix44[11] = 0;
+
+	matrix44[12] = 0;
+	matrix44[13] = 0;
+	matrix44[14] = 0;
+	matrix44[15] = 1;
+}
+
+void quatgetconjugate( float * qout, const float * qin )
+{
+	qout[0] = qin[0];
+	qout[1] = -qin[1];
+	qout[2] = -qin[2];
+	qout[3] = -qin[3];
+}
+
+void quatgetreciprocal( float * qout, const float * qin )
+{
+	float m = quatinvsqmagnitude(qin);
+	quatgetconjugate( qout, qin );
+	quatscale( qout, qout, m );
+}
+
+void quatsub( float * qout, const float * a, const float * b )
+{
+	qout[0] = a[0] - b[0];
+	qout[1] = a[1] - b[1];
+	qout[2] = a[2] - b[2];
+	qout[3] = a[3] - b[3];
+}
+
+void quatadd( float * qout, const float * a, const float * b )
+{
+	qout[0] = a[0] + b[0];
+	qout[1] = a[1] + b[1];
+	qout[2] = a[2] + b[2];
+	qout[3] = a[3] + b[3];
+}
+
+void quatrotateabout( float * qout, const float * a, const float * b )
+{
+	float q1[4];
+	float q2[4];
+
+	quatnormalize( q1, a );
+	quatnormalize( q2, b );
+
+	qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]);
+	qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]);
+	qout[2] = (q1[0]*q2[2])-(q1[1]*q2[3])+(q1[2]*q2[0])+(q1[3]*q2[1]);
+	qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]);
+}
+
+void quatscale( float * qout, const float * qin, float s )
+{
+	qout[0] = qin[0] * s;
+	qout[1] = qin[1] * s;
+	qout[2] = qin[2] * s;
+	qout[3] = qin[3] * s;
+}
+
+
+float quatinnerproduct( const float * qa, const float * qb )
+{
+	return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]);
+}
+
+void quatouterproduct( float * outvec3, float * qa, float * qb )
+{
+	outvec3[0] = (qa[0]*qb[1])-(qa[1]*qb[0])-(qa[2]*qb[3])+(qa[3]*qb[2]);
+	outvec3[1] = (qa[0]*qb[2])+(qa[1]*qb[3])-(qa[2]*qb[0])-(qa[3]*qb[1]);
+	outvec3[2] = (qa[0]*qb[3])-(qa[1]*qb[2])+(qa[2]*qb[1])-(qa[3]*qb[0]);
+}
+
+void quatevenproduct( float * q, float * qa, float * qb )
+{
+	q[0] = (qa[0]*qb[0])-(qa[1]*qb[1])-(qa[2]*qb[2])-(qa[3]*qb[3]);
+	q[1] = (qa[0]*qb[1])+(qa[1]*qb[0]);
+	q[2] = (qa[0]*qb[2])+(qa[2]*qb[0]);
+	q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]);
+}
+
+void quatoddproduct( float * outvec3, float * qa, float * qb )
+{
+	outvec3[0] = (qa[2]*qb[3])-(qa[3]*qb[2]);
+	outvec3[1] = (qa[3]*qb[1])-(qa[1]*qb[3]);
+	outvec3[2] = (qa[1]*qb[2])-(qa[2]*qb[1]);
+}
+
+void quatslerp( float * q, const float * qa, const float * qb, float t )
+{
+	float an[4];
+	float bn[4];
+	quatnormalize( an, qa );
+	quatnormalize( bn, qb );
+	float cosTheta = quatinnerproduct(an,bn);
+	float sinTheta;
+
+	//Careful: If cosTheta is exactly one, or even if it's infinitesimally over, it'll
+	// cause SQRT to produce not a number, and screw everything up.
+	if ( 1 - (cosTheta*cosTheta) <= 0 )
+		sinTheta = 0;
+	else
+		sinTheta = sqrt(1 - (cosTheta*cosTheta));
+
+	float Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions
+
+	if(fabs(Theta) < DEFAULT_EPSILON )
+		quatcopy( q, qa );
+	else if(fabs(sinTheta) < DEFAULT_EPSILON )
+	{
+		quatadd( q, qa, qb );
+		quatscale( q, q, 0.5 );
+	}
+	else
+	{
+		float aside[4];
+		float bside[4];
+		quatscale( bside, qb, sin( t * Theta ) );
+		quatscale( aside, qa, sin((1-t)*Theta) );
+		quatadd( q, aside, bside );
+		quatscale( q, q, 1./sinTheta );
+	}
+}
+
+void quatrotatevector( float * vec3out, const float * quat, const float * vec3in )
+{
+	float tquat[4];
+	float vquat[4];
+	float qrecp[4];
+	vquat[0] = 0;
+	vquat[1] = vec3in[0];
+	vquat[2] = vec3in[1];
+	vquat[3] = vec3in[2];
+
+	quatrotateabout( tquat, quat, vquat );
+	quatgetreciprocal( qrecp, quat );
+	quatrotateabout( vquat, tquat, qrecp );
+
+	vec3out[0] = vquat[1];
+	vec3out[1] = vquat[2];
+	vec3out[2] = vquat[3];
+}
+
+
+
diff --git a/tools/planetest/linmath.h b/tools/planetest/linmath.h
new file mode 100644
index 0000000..344a547
--- /dev/null
+++ b/tools/planetest/linmath.h
@@ -0,0 +1,59 @@
+//Copyright 2013 <>< C. N. Lohr.  This file licensed under the terms of the MIT license.
+
+#ifndef _LINMATH_H
+#define _LINMATH_H
+
+//Yes, I know it's kind of arbitrary.
+#define DEFAULT_EPSILON 0.001
+
+
+//NOTE: Inputs may never be output with cross product.
+void cross3d( float * out, const float * a, const float * b );
+
+void sub3d( float * out, const float * a, const float * b );
+
+void add3d( float * out, const float * a, const float * b );
+
+void scale3d( float * out, const float * a, float scalar );
+
+void normalize3d( float * out, const float * in );
+
+float dot3d( const float * a, const float * b );
+
+//Returns 0 if equal.  If either argument is null, 0 will ALWAYS be returned.
+int compare3d( const float * a, const float * b, float epsilon );
+
+void copy3d( float * out, const float * in );
+
+
+
+
+//Quaternion things...
+
+void quatsetnone( float * q );
+void quatcopy( float * qout, const float * qin );
+void quatfromeuler( float * q, const float * euler );
+void quattoeuler( float * euler, const float * q );
+void quatfromaxisangle( float * q, const float * axis, float radians );
+float quatmagnitude( const float * q );
+float quatinvsqmagnitude( const float * q );
+void quatnormalize( float * qout, const float * qin );  //Safe for in to be same as out.
+void quattomatrix( float * matrix44, const float * q );
+void quatgetconjugate( float * qout, const float * qin );
+void quatgetreciprocal( float * qout, const float * qin );
+void quatsub( float * qout, const float * a, const float * b );
+void quatadd( float * qout, const float * a, const float * b );
+void quatrotateabout( float * qout, const float * a, const float * b );  //same as quat multiply, not piecewise multiply.
+void quatscale( float * qout, const float * qin, float s );
+float quatinnerproduct( const float * qa, const float * qb );
+void quatouterproduct( float * outvec3, float * qa, float * qb );
+void quatevenproduct( float * q, float * qa, float * qb );
+void quatoddproduct( float * outvec3, float * qa, float * qb );
+void quatslerp( float * q, const float * qa, const float * qb, float t );
+void quatrotatevector( float * vec3out, const float * quat, const float * vec3in );
+
+
+#endif
+
+
+