aboutsummaryrefslogtreecommitdiff
path: root/tools
diff options
context:
space:
mode:
Diffstat (limited to 'tools')
-rw-r--r--tools/planetest/Makefile10
-rw-r--r--tools/planetest/camfind.c385
-rw-r--r--tools/planetest/linmath.c326
-rw-r--r--tools/planetest/linmath.h59
-rw-r--r--tools/planetest2/Makefile2
-rw-r--r--tools/planetest2/camfind.c278
6 files changed, 158 insertions, 902 deletions
diff --git a/tools/planetest/Makefile b/tools/planetest/Makefile
deleted file mode 100644
index 37f02d7..0000000
--- a/tools/planetest/Makefile
+++ /dev/null
@@ -1,10 +0,0 @@
-all : camfind
-
-CFLAGS:=-g -O4 -DFLT=float -ffast-math
-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
deleted file mode 100644
index 1e9b2b9..0000000
--- a/tools/planetest/camfind.c
+++ /dev/null
@@ -1,385 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include "linmath.h"
-#include <string.h>
-#include <math.h>
-
-#define PTS 32
-#define MAX_CHECKS 20000
-
-FLT hmd_points[PTS*3];
-FLT hmd_norms[PTS*3];
-FLT hmd_point_angles[PTS*2];
-int hmd_point_counts[PTS*2];
-int LoadData( char Camera );
-
-//Values used for RunTest()
-FLT LighthousePos[3] = { 0, 0, 0 };
-FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
-
-//Actual values
-FLT xyzypr[6] = { 0, 2, 2, 0, 0, 0 };
-
-
-FLT RunOpti( int axis );
-FLT RunTest( int print );
-void PrintOpti();
-
-int main()
-{
- int i;
-
- //Load either 'L' (LH1) or 'R' (LH2) data.
- if( LoadData( 'R' ) ) return 5;
-
- int opti = 0;
- int cycle = 0;
- int axis = 0;
-
- FLT dx, dy, dz;
-
- FLT bestxyzypr[6];
- memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) );
-
- FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD)
- for( cycle = 0; cycle < 20; cycle ++ )
- {
-
- //Adjust position, one axis at a time, over and over until we zero in.
-#define FULLSEARCH
-
-#ifndef FULLSEARCH
- for( axis = 0; axis < 3; axis++ )
-#endif
- {
- FLT bestxyzyprrunning[6];
- FLT beste = 1e20;
-
- //Try a bunch of points in this axis.
-#ifdef FULLSEARCH
- for( dz = -fullrange; dz <= fullrange; dz += fullrange/3 )
- for( dy = -fullrange; dy <= fullrange; dy += fullrange/3 )
- for( dx = -fullrange; dx <= fullrange; dx += fullrange/3 )
- {
-#else
- for( dx = -fullrange; dx <= fullrange; dx += fullrange/5 )
- {
-#endif
- //Specificially adjust one axis at a time, searching for the best.
- memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) );
-#ifdef FULLSEARCH
- xyzypr[0] += dx;
- xyzypr[1] += dy;
- xyzypr[2] += dz;
-#else
- xyzypr[axis] += dx;
-#endif
- //if( axis == 2 && xyzypr[2] < 0 ) continue;
-
- xyzypr[3] = 0;
- xyzypr[4] = 0;
- xyzypr[5] = 0;
- FLT ft;
- int reopt = 0;
-
- //Try refining the search for the best orientation several times.
- for( reopt = 0; reopt < 4; reopt++ )
- {
- //XXX This search mechanism is insufficient :(
- //Search through each axis every time.
- for( opti = 3; opti < 6; opti++ )
- {
- ft = RunOpti( opti );
- }
- if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); }
- }
- //printf( " %f %f %f %f\n", xyzypr[0], xyzypr[1], xyzypr[2], ft );
-#ifndef FULLSEARCH
- memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) );
-#endif
- }
-#ifdef FULLSEARCH
- memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) );
-#endif
-
- //Print out the quality of the lock this time.
- FLT dist = sqrt(bestxyzypr[0]*bestxyzypr[0] + bestxyzypr[1]*bestxyzypr[1] + bestxyzypr[2]*bestxyzypr[2]);
- printf( "%f %f %f (%f) %f %f %f = %f\n", bestxyzypr[0], bestxyzypr[1], bestxyzypr[2], dist, bestxyzypr[3], bestxyzypr[4], bestxyzypr[5], beste );
- }
-/*
- if( bestxyzypr[2] < 0 )
- {
- bestxyzypr[0] *= -1;
- bestxyzypr[1] *= -1;
- bestxyzypr[2] *= -1;
- }
-*/
- //Every cycle, tighten up the search area.
- fullrange *= 0.6;
- }
-
- //Use bestxyzypr
- memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) );
-
-
- //Print out plane accuracies with these settings.
- PrintOpti();
-}
-
-void PrintOpti()
-{
- FLT xyzyprchange[6];
- memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) );
- quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
- memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
- FLT ft = RunTest(1);
- printf( "Final RMS: %f\n", ft );
-}
-
-
-FLT RunOpti( int axis )
-{
- FLT xyzyprchange[6];
- memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) );
- int i;
- FLT minv = 1e20;
- FLT fv = -3.3;
- FLT bv = 0;
-
-
- //Coarse linear search first, try to figure out about where
- for( i = 0; i < 33*2; i++ )
- {
- xyzyprchange[axis] = fv;
- quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
- memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
- FLT ft = RunTest(0);
- //printf( " %d / %f = %f\n", ft, fv );
- if( ft < minv ) { minv = ft; bv = fv; }
- fv += 0.1;
- }
-
- xyzyprchange[axis] = bv;
-#if 1
- //Now, do a more precise binary-ish search.
- FLT jumpamount = 0.15;
- for( i = 0; i < 20; i++ )
- {
- FLT orig = xyzyprchange[axis];
-
- minv = 1e20;
-
- //When searching, consider 'less' 'this' and 'more'
-
- fv = xyzyprchange[axis] = orig;
- quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
- memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
- FLT ft = RunTest(0);
- if( ft < minv ) { minv = ft; bv = fv; }
-
- fv = xyzyprchange[axis] = orig + jumpamount;
- quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
- memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
- ft = RunTest(0);
- if( ft < minv ) { minv = ft; bv = fv; }
-
- fv = xyzyprchange[axis] = orig - jumpamount;
- quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
- memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
- ft = RunTest(0);
- if( ft < minv ) { minv = ft; bv = fv; }
-
- xyzyprchange[axis] = bv;
-
- //Tighten up search area. Doing by 0.5 can cause unusual instabilities.
- jumpamount *= 0.6;
- }
-#endif
- xyzypr[axis] = bv;
- return minv;
-}
-
-
-FLT RunTest( int print )
-{
- int k;
- FLT totprob = 0.0;
- int ict = 0;
- for( k = 0; k < 64; k++ )
- {
- if( hmd_point_counts[k] == 0 ) continue;
- int axis = k%2;
- int pt = k/2;
- FLT angle = hmd_point_angles[k];
- if( print ) printf( "%d %d : angle: %f / ", axis, pt, angle );
-
- //XXX TODO: This is critical. We need to properly define the planes.
- FLT plane_normal[3] = { 0, 0, 0 };
- if( axis == 0 )
- {
- plane_normal[1] = cos(angle);
- plane_normal[2] =-sin(angle);
- }
- else
- {
- plane_normal[0] =-cos(angle);
- plane_normal[2] =-sin(angle);
- }
-
- quatrotatevector( plane_normal, LighthouseQuat, plane_normal ); //Rotate plane normal by concatenated rotation.
-
- //plane_normal is our normal / LighthousePos is our point.
- FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] };
-
-//May be able to remove this.
-// FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+2],-hmd_points[pt*3+1] }; //XXX WRONG Why does this produce the right answers?
-
- //FLT w0[] = { 0, 0, 0 };
- FLT d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]);
- FLT D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d;
- //Point line distance assuming ||normal|| = 1.
-
- FLT delta[] = { LighthousePos[0]-hmd_points[pt*3+0],LighthousePos[1]-hmd_points[pt*3+1],LighthousePos[2]-hmd_points[pt*3+2] };
- FLT dot = delta[0] * hmd_norms[pt*3+0] + delta[1] * hmd_norms[pt*3+1] + delta[2] * hmd_norms[pt*3+2];
-// if( dot < -0.04 ) totprob+=10000000000;
- if( print ) printf( " %f %f N:%f\n", d, D, dot );
- totprob += (D*D); //Calculate RMS distance of incorrectitude.
-
- ict++;
- }
- return sqrt(totprob/ict);
-}
-
-
-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 );
-
-
- 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\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 );
-
-
- int xck = 0;
- f = fopen( "third_test_with_time_lengths.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 length;
- int rk = sscanf( lineptr, "%9s %9s %9s %d %d %d %d %d\n", lf, xory, dev, &timestamp, &sensorid, &code, &offset, &length );
- if( lf[0] == Camera && rk == 8 )
- {
- //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];
- hmd_point_angles[i*2+0] = (hmd_point_angles[i*2+0] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution?
- hmd_point_angles[i*2+1] = (hmd_point_angles[i*2+1] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution?
- }
-
-
-
-
- return 0;
-}
diff --git a/tools/planetest/linmath.c b/tools/planetest/linmath.c
deleted file mode 100644
index 88643b4..0000000
--- a/tools/planetest/linmath.c
+++ /dev/null
@@ -1,326 +0,0 @@
-//Copyright 2013 <>< C. N. Lohr. This file licensed under the terms of the MIT license.
-
-#include "linmath.h"
-#include <math.h>
-
-void cross3d( FLT * out, const FLT * a, const FLT * 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( FLT * out, const FLT * a, const FLT * b )
-{
- out[0] = a[0] - b[0];
- out[1] = a[1] - b[1];
- out[2] = a[2] - b[2];
-}
-
-void add3d( FLT * out, const FLT * a, const FLT * b )
-{
- out[0] = a[0] + b[0];
- out[1] = a[1] + b[1];
- out[2] = a[2] + b[2];
-}
-
-void scale3d( FLT * out, const FLT * a, FLT scalar )
-{
- out[0] = a[0] * scalar;
- out[1] = a[1] * scalar;
- out[2] = a[2] * scalar;
-}
-
-void normalize3d( FLT * out, const FLT * in )
-{
- FLT 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;
-}
-
-FLT dot3d( const FLT * a, const FLT * b )
-{
- return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
-}
-
-int compare3d( const FLT * a, const FLT * b, FLT 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( FLT * out, const FLT * 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( FLT * q )
-{
- q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1;
-}
-
-void quatcopy( FLT * qout, const FLT * qin )
-{
- qout[0] = qin[0];
- qout[1] = qin[1];
- qout[2] = qin[2];
- qout[3] = qin[3];
-}
-
-void quatfromeuler( FLT * q, const FLT * euler )
-{
- FLT X = euler[0]/2.0f; //roll
- FLT Y = euler[1]/2.0f; //pitch
- FLT Z = euler[2]/2.0f; //yaw
-
- FLT cx = cosf(X);
- FLT sx = sinf(X);
- FLT cy = cosf(Y);
- FLT sy = sinf(Y);
- FLT cz = cosf(Z);
- FLT 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( FLT * euler, const FLT * 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( FLT * q, const FLT * axis, FLT radians )
-{
- FLT v[3];
- normalize3d( v, axis );
-
- FLT 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 );
-}
-
-FLT quatmagnitude( const FLT * q )
-{
- return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
-}
-
-FLT quatinvsqmagnitude( const FLT * q )
-{
- return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
-}
-
-
-void quatnormalize( FLT * qout, const FLT * qin )
-{
- FLT imag = quatinvsqmagnitude( qin );
- quatscale( qout, qin, imag );
-}
-
-void quattomatrix( FLT * matrix44, const FLT * qin )
-{
- FLT q[4];
- quatnormalize( q, qin );
-
- //Reduced calulation for speed
- FLT xx = 2*q[0]*q[0];
- FLT xy = 2*q[0]*q[1];
- FLT xz = 2*q[0]*q[2];
- FLT xw = 2*q[0]*q[3];
-
- FLT yy = 2*q[1]*q[1];
- FLT yz = 2*q[1]*q[2];
- FLT yw = 2*q[1]*q[3];
-
- FLT zz = 2*q[2]*q[2];
- FLT 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( FLT * qout, const FLT * qin )
-{
- qout[0] = qin[0];
- qout[1] = -qin[1];
- qout[2] = -qin[2];
- qout[3] = -qin[3];
-}
-
-void quatgetreciprocal( FLT * qout, const FLT * qin )
-{
- FLT m = quatinvsqmagnitude(qin);
- quatgetconjugate( qout, qin );
- quatscale( qout, qout, m );
-}
-
-void quatsub( FLT * qout, const FLT * a, const FLT * 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( FLT * qout, const FLT * a, const FLT * 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( FLT * qout, const FLT * a, const FLT * b )
-{
- FLT q1[4];
- FLT 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( FLT * qout, const FLT * qin, FLT s )
-{
- qout[0] = qin[0] * s;
- qout[1] = qin[1] * s;
- qout[2] = qin[2] * s;
- qout[3] = qin[3] * s;
-}
-
-
-FLT quatinnerproduct( const FLT * qa, const FLT * qb )
-{
- return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]);
-}
-
-void quatouterproduct( FLT * outvec3, FLT * qa, FLT * 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( FLT * q, FLT * qa, FLT * 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( FLT * outvec3, FLT * qa, FLT * 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( FLT * q, const FLT * qa, const FLT * qb, FLT t )
-{
- FLT an[4];
- FLT bn[4];
- quatnormalize( an, qa );
- quatnormalize( bn, qb );
- FLT cosTheta = quatinnerproduct(an,bn);
- FLT 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));
-
- FLT 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
- {
- FLT aside[4];
- FLT 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( FLT * vec3out, const FLT * quat, const FLT * vec3in )
-{
- FLT tquat[4];
- FLT vquat[4];
- FLT 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
deleted file mode 100644
index 881ca70..0000000
--- a/tools/planetest/linmath.h
+++ /dev/null
@@ -1,59 +0,0 @@
-//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( FLT * out, const FLT * a, const FLT * b );
-
-void sub3d( FLT * out, const FLT * a, const FLT * b );
-
-void add3d( FLT * out, const FLT * a, const FLT * b );
-
-void scale3d( FLT * out, const FLT * a, FLT scalar );
-
-void normalize3d( FLT * out, const FLT * in );
-
-FLT dot3d( const FLT * a, const FLT * b );
-
-//Returns 0 if equal. If either argument is null, 0 will ALWAYS be returned.
-int compare3d( const FLT * a, const FLT * b, FLT epsilon );
-
-void copy3d( FLT * out, const FLT * in );
-
-
-
-
-//Quaternion things...
-
-void quatsetnone( FLT * q );
-void quatcopy( FLT * qout, const FLT * qin );
-void quatfromeuler( FLT * q, const FLT * euler );
-void quattoeuler( FLT * euler, const FLT * q );
-void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians );
-FLT quatmagnitude( const FLT * q );
-FLT quatinvsqmagnitude( const FLT * q );
-void quatnormalize( FLT * qout, const FLT * qin ); //Safe for in to be same as out.
-void quattomatrix( FLT * matrix44, const FLT * q );
-void quatgetconjugate( FLT * qout, const FLT * qin );
-void quatgetreciprocal( FLT * qout, const FLT * qin );
-void quatsub( FLT * qout, const FLT * a, const FLT * b );
-void quatadd( FLT * qout, const FLT * a, const FLT * b );
-void quatrotateabout( FLT * qout, const FLT * a, const FLT * b ); //same as quat multiply, not piecewise multiply.
-void quatscale( FLT * qout, const FLT * qin, FLT s );
-FLT quatinnerproduct( const FLT * qa, const FLT * qb );
-void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb );
-void quatevenproduct( FLT * q, FLT * qa, FLT * qb );
-void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb );
-void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t );
-void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in );
-
-
-#endif
-
-
-
diff --git a/tools/planetest2/Makefile b/tools/planetest2/Makefile
index dd19480..b48b099 100644
--- a/tools/planetest2/Makefile
+++ b/tools/planetest2/Makefile
@@ -1,6 +1,6 @@
all : camfind
-CFLAGS:=-g -O4 -DFLT=float -ffast-math -I../../redist -flto
+CFLAGS:=-g -O4 -DFLT=float -I../../redist -flto
LDFLAGS:=$(CFLAGS) -lm
camfind : camfind.o ../../redist/linmath.c
diff --git a/tools/planetest2/camfind.c b/tools/planetest2/camfind.c
index 54fd8a1..c987e46 100644
--- a/tools/planetest2/camfind.c
+++ b/tools/planetest2/camfind.c
@@ -5,7 +5,7 @@
#include <math.h>
#define PTS 32
-#define MAX_CHECKS 20000
+#define MAX_CHECKS 30000
#define MIN_HITS_FOR_VALID 10
FLT hmd_points[PTS*3];
@@ -18,8 +18,6 @@ int LoadData( char Camera );
//Values used for RunTest()
FLT LighthousePos[3] = { 0, 0, 0 };
FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
-FLT BarrelRotate[4]; //XXX TODO: Concatenate these.
-
FLT RunOpti( int print );
FLT RunTest( int print );
@@ -30,7 +28,7 @@ int main()
int i;
//Load either 'L' (LH1) or 'R' (LH2) data.
- if( LoadData( 'R' ) ) return 5;
+ if( LoadData( 'L' ) ) return 5;
int opti = 0;
int cycle = 0;
@@ -52,10 +50,14 @@ int main()
FLT bestxyzrunning[3];
FLT beste = 1e20;
- FLT splits = 2;
- if( cycle == 0 ) splits = 25;
- //XXX TODO: Switch to polar coordinate search
+ FLT splits = 4;
+ if( cycle == 0 ) splits = 24;
+ if( cycle == 1 ) splits = 13;
+ if( cycle == 2 ) splits = 10;
+ if( cycle == 3 ) splits = 8;
+ if( cycle == 4 ) splits = 5;
+
for( dz = 0; dz <= fullrange; dz += fullrange/splits )
for( dy = -fullrange; dy <= fullrange; dy += fullrange/splits )
for( dx = -fullrange; dx <= fullrange; dx += fullrange/splits )
@@ -70,8 +72,6 @@ int main()
//Try refining the search for the best orientation several times.
ft = RunOpti(0);
if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); }
-
- //printf( " %f %f %f %f\n", LighthousePos[0], LighthousePos[1], LighthousePos[2], ft );
}
memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) );
@@ -81,7 +81,7 @@ int main()
}
//Every cycle, tighten up the search area.
- fullrange *= 0.6;
+ fullrange *= 0.25;
}
//Use bestxyz
@@ -90,29 +90,7 @@ int main()
//Optimize the quaternion for lighthouse rotation
RunOpti(1);
- //STAGE 2: Determine optimal distance from target
- {
- FLT dist = 0.1;
- FLT best_dist = 0;
- FLT best_err = 1e20;
- for( ; dist < 10; dist+=0.01 )
- {
- FLT nrmvect[3];
- normalize3d( nrmvect, bestxyz );
- scale3d( LighthousePos, nrmvect, dist );
- FLT res = RunTest( 0 );
- if( res < best_err )
- {
- best_dist = dist;
- best_err = res;
- }
- }
-
- //Apply the best res.
- normalize3d( LighthousePos, bestxyz );
- scale3d( LighthousePos, LighthousePos, best_dist );
- printf( "Best distance: %f\n", best_dist );
- }
+ printf( "Best Quat: %f %f %f %f\n", PFFOUR( LighthouseQuat ) );
//Print out plane accuracies with these settings.
FLT ft = RunTest(1);
@@ -121,97 +99,131 @@ int main()
FLT RunOpti( int print )
{
- int i;
+ int i, p;
FLT UsToTarget[3];
+ FLT LastUsToTarget[3];
+ FLT mux = .9;
+ quatsetnone( LighthouseQuat );
- {
- //XXX TODO: We should use several points to determine initial rotation optimization to object.
- //By using only one point the "best" we could be rotating somewhere wrong. We do use
- //several points for the rotate-about-this-vector rotation in stage 2.
-
- //Find out where our ray shoots forth from.
- FLT ax = hmd_point_angles[best_hmd_target*2+0];
- FLT ay = hmd_point_angles[best_hmd_target*2+1];
+ int first = 1, second = 0;
- //NOTE: Inputs may never be output with cross product.
- //Create a fictitious normalized ray. Imagine the lighthouse is pointed
- //straight in the +z direction, this is the lighthouse ray to the point.
- FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
- RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+ //First check to see if this is a valid viewpoint.
- //Find a ray from us to the target point.
- sub3d( UsToTarget, &hmd_points[best_hmd_target*3], LighthousePos );
- normalize3d( UsToTarget, UsToTarget );
-
- FLT AxisToRotate[3];
- cross3d( AxisToRotate, RayShootOut, UsToTarget );
- //Rotate the lighthouse around this axis to point at the HMD.
-
- FLT RotateAmount = -acos( dot3d( RayShootOut, UsToTarget ) ); //XXX TODO How to determine if negative.
- quatfromaxisangle( LighthouseQuat, AxisToRotate, RotateAmount ); //Tested, working!
+ for( p = 0; p < 32; p++ )
+ {
+ if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue;
+ FLT me_to_dot[3];
+ sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] );
+ float dot = dot3d( &hmd_norms[p*3], me_to_dot );
+ if( dot < -.01 ) { return 1000; }
}
- //Now our lighthouse's ray is pointed at the HMD's dot, but, we need to
- //rotate the lighthouse such that it is oriented optimally. We do this
- //by finding what the optimal rotation aroud our face would be for all
- //remaining points.
- FLT rotate_radians = 0;
- int points_found_to_rotate = 0;
+ int iters = 6;
- float rots[PTS];
- for( i = 0; i < PTS; i++ )
+ for( i = 0; i < iters; i++ )
{
- if( i == best_hmd_target ) continue;
- int xhits = hmd_point_counts[i*2+0];
- int yhits = hmd_point_counts[i*2+1];
- int xyhits = (xhits<yhits)?xhits:yhits;
- if( xyhits < MIN_HITS_FOR_VALID ) continue;
-
- //Find a point on the object to point at.
- FLT HMDRayPoint[3];
- sub3d( HMDRayPoint, &hmd_points[i*3], LighthousePos );
- normalize3d( HMDRayPoint, HMDRayPoint );
-
- FLT RayShootOut[3] = { sin(hmd_point_angles[i*2+0]), sin(hmd_point_angles[i*2+1]), 0 };
- RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
- //Find the corresponding vector from camera out if it was rotated by "RotateAmount"
- quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut );
-
- //Figure out rotation to rotate around UsToTarget, from RayShootOut to HMDRayPoint
- FLT SphSurf_Point[3]; //Relative to our fixed point, how much to rotate?
- FLT SphSurf_Ray[3];
- sub3d( SphSurf_Point, HMDRayPoint, UsToTarget );
- sub3d( SphSurf_Ray, RayShootOut, UsToTarget );
- normalize3d( SphSurf_Point, SphSurf_Point );
- normalize3d( SphSurf_Ray, SphSurf_Ray );
-
- FLT rotate = acos( dot3d( SphSurf_Point, SphSurf_Ray ) );
+ first = 1;
+ for( p = 0; p < 32; p++ )
+ {
+ if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue;
+
+ //Find out where our ray shoots forth from.
+ FLT ax = hmd_point_angles[p*2+0];
+ FLT ay = hmd_point_angles[p*2+1];
+
+ //NOTE: Inputs may never be output with cross product.
+ //Create a fictitious normalized ray. Imagine the lighthouse is pointed
+ //straight in the +z direction, this is the lighthouse ray to the point.
+ FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+ RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+ FLT RayShootOutWorld[3];
+
+ //Rotate that ray by the current rotation estimation.
+ quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut );
+
+ //Find a ray from us to the target point.
+ sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+ if( magnitude3d( UsToTarget ) < 0.0001 ) { continue; }
+ normalize3d( UsToTarget, UsToTarget );
+
+ FLT RotatedLastUs[3];
+ quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget );
+
+ //Rotate the lighthouse around this axis to point at the HMD.
+ //If it's the first time, the axis is synthesized, if it's after that, use most recent point.
+ FLT ConcatQuat[4];
+ FLT AxisToRotate[3];
+ if( first )
+ {
+ cross3d( AxisToRotate, RayShootOutWorld, UsToTarget );
+ if( magnitude3d(AxisToRotate) < 0.0001 ) break;
+ normalize3d( AxisToRotate, AxisToRotate );
+ //Don't need to worry about being negative, cross product will fix it.
+ FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget );
+ quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ }
+ else
+ {
+ FLT Target[3];
+ FLT Actual[3];
+
+ copy3d( AxisToRotate, LastUsToTarget );
+ //Us to target = normalized ray from us to where we should be.
+ //RayShootOut = where we would be pointing.
+ sub3d( Target, UsToTarget, AxisToRotate ); //XXX XXX XXX WARNING THIS MESSES STUFF UP.
+ sub3d( Actual, RayShootOutWorld, AxisToRotate );
+ if( magnitude3d( Actual ) < 0.0001 || magnitude3d( Target ) < 0.0001 ) { continue; }
+ normalize3d( Target, Target );
+ normalize3d( Actual, Actual );
+
+ cross3d( AxisToRotate, Actual, Target ); //XXX Check: AxisToRotate should be equal to LastUsToTarget.
+ if( magnitude3d( AxisToRotate ) < 0.000001 ) { continue; }
+ normalize3d( AxisToRotate,AxisToRotate );
+
+ //printf( "%f %f %f === %f %f %f : ", PFTHREE( AxisToRotate ), PFTHREE( LastUsToTarget ) );
+ FLT RotateAmount = anglebetween3d( Actual, Target ) * mux;
+ //printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount );
+ quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+ }
- //XXX TODO: How to determine if rotate amount should be negative!!!
+ quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Chekcked. This appears to be
- if( print ) printf( " %f ", rotate );
- rotate_radians += rotate;
- rots[points_found_to_rotate++] = rotate;
+ mux = mux * 0.94;
+ if( second ) { second = 0; }
+ if( first ) { first = 0; second = 1; }
+ copy3d( LastUsToTarget, RayShootOutWorld );
+ }
}
- //Get average barrel rotation
- rotate_radians/=points_found_to_rotate;
-
- //Find the standard of deviation of our data.
- float stddev = 0.0;
- for( i = 0; i < points_found_to_rotate; i++ )
+ //Step 2: Determine error.
+ float errorsq = 0.0;
+ int count = 0;
+ for( p = 0; p < 32; p++ )
{
- float dr = rots[i]-rotate_radians;
- stddev += dr*dr;
- }
- stddev/=points_found_to_rotate;
- stddev = sqrt(stddev);
- if( print ) printf( " = %f\n", stddev );
+ if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue;
+
+ //Find out where our ray shoots forth from.
+ FLT ax = hmd_point_angles[p*2+0];
+ FLT ay = hmd_point_angles[p*2+1];
+ FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+ RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
- quatfromaxisangle( BarrelRotate, UsToTarget, rotate_radians ); //Rotate around barrel
- //quatrotateabout( LighthouseQuat, LighthouseQuat, BarrelRotate ); //Concatenate the rotation into the lighthouse quat rotation.
+ //Rotate that ray by the current rotation estimation.
+ quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut );
- return stddev;
+ //Point-line distance.
+ //Line defined by LighthousePos & Direction: RayShootOut
+
+ //Find a ray from us to the target point.
+ sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+ FLT xproduct[3];
+ cross3d( xproduct, UsToTarget, RayShootOut );
+ FLT dist = magnitude3d( xproduct );
+ errorsq += dist*dist;
+ if( print ) printf( "%f (%d(%d/%d))\n", dist, p, hmd_point_counts[p*2+0], hmd_point_counts[p*2+1] );
+ }
+ if( print ) printf( " = %f\n", sqrt( errorsq ) );
+ return sqrt(errorsq);
}
@@ -225,33 +237,28 @@ FLT RunTest( int print )
if( hmd_point_counts[k] == 0 ) continue;
int axis = k%2;
int pt = k/2;
+
FLT angle = hmd_point_angles[k];
- if( print ) printf( "%d %d : angle: %f / ", axis, pt, angle );
+ if( print ) printf( "%3d %3d : angle: %10f / ", axis, pt, angle );
//XXX TODO: This is critical. We need to properly define the planes.
FLT plane_normal[3] = { 0, 0, 0 };
- if( axis == 1 )
+ if( axis == 0 )
{
- plane_normal[1] = cos(angle);
+ plane_normal[0] = cos(angle);
plane_normal[2] =-sin(angle);
}
else
{
- plane_normal[0] = cos(angle);
+ plane_normal[1] = cos(angle);
plane_normal[2] =-sin(angle);
}
quatrotatevector( plane_normal, LighthouseQuat, plane_normal ); //Rotate plane normal by concatenated rotation.
- quatrotatevector( plane_normal, BarrelRotate, plane_normal ); //Rotate plane normal by concatenated rotation.
//plane_normal is our normal / LighthousePos is our point.
FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] };
-
-//May be able to remove this.
-// FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+2],-hmd_points[pt*3+1] }; //XXX WRONG Why does this produce the right answers?
-
- //FLT w0[] = { 0, 0, 0 };
FLT d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]);
FLT D = plane_normal[0] * w0[0] + plane_normal[1] * w0[1] + plane_normal[2] * w0[2] + d;
//Point line distance assuming ||normal|| = 1.
@@ -259,11 +266,39 @@ FLT RunTest( int print )
FLT delta[] = { LighthousePos[0]-hmd_points[pt*3+0],LighthousePos[1]-hmd_points[pt*3+1],LighthousePos[2]-hmd_points[pt*3+2] };
FLT dot = delta[0] * hmd_norms[pt*3+0] + delta[1] * hmd_norms[pt*3+1] + delta[2] * hmd_norms[pt*3+2];
// if( dot < -0.04 ) totprob+=10000000000;
- if( print ) printf( " %f %f N:%f\n", d, D, dot );
+ if( print ) printf( " %10f %10f N:%f\n", d, D, dot );
totprob += (D*D); //Calculate RMS distance of incorrectitude.
ict++;
}
+
+ if( print )
+ {
+ int p;
+ printf( "POS: %f %f %f %f\n", PFFOUR(LighthousePos ) );
+ printf( "QUAT: %f %f %f %f\n", PFFOUR(LighthouseQuat ) );
+ printf( "Imagespace comparison:\n" );
+ for( p = 0; p < 32; p++ )
+ {
+ if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue;
+
+ FLT us_to_targ[3];
+ sub3d( us_to_targ, &hmd_points[p*3] , LighthousePos );
+
+ //Unrotate us_to_targ.
+ FLT unrotate[4];
+ quatgetconjugate( unrotate, LighthouseQuat );
+
+ quatrotatevector( us_to_targ, unrotate, us_to_targ );
+ normalize3d( us_to_targ, us_to_targ );
+ FLT x = asin( us_to_targ[0] );
+ FLT y = asin( us_to_targ[1] );
+
+ printf( "%f %f %f %f\n", hmd_point_angles[p*2+0], hmd_point_angles[p*2+1], x, y );
+ }
+ }
+
+
return sqrt(totprob/ict);
}
@@ -331,7 +366,7 @@ int LoadData( char Camera )
int xck = 0;
- f = fopen( "third_test_with_time_lengths.csv", "r" );
+ f = fopen( "testfive.csv", "r" );
if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; }
while( !feof(f) && !ferror(f) )
{
@@ -415,3 +450,4 @@ int LoadData( char Camera )
return 0;
}
+