From f7fe8f908f743e9cc287d0efc5dd5db8c41fc38f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 14 Dec 2016 22:47:55 -0500 Subject: add framework for second test. --- tools/planetest2/Makefile | 10 ++ tools/planetest2/camfind.c | 385 +++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 395 insertions(+) create mode 100644 tools/planetest2/Makefile create mode 100644 tools/planetest2/camfind.c (limited to 'tools') diff --git a/tools/planetest2/Makefile b/tools/planetest2/Makefile new file mode 100644 index 0000000..2c16261 --- /dev/null +++ b/tools/planetest2/Makefile @@ -0,0 +1,10 @@ +all : camfind + +CFLAGS:=-g -O4 -DFLT=float -ffast-math +LDFLAGS:=$(CFLAGS) -lm + +camfind : camfind.o ../../redist/linmath.c + gcc -o $@ $^ $(LDFLAGS) + +clean : + rm -rf *.o *~ camfind diff --git a/tools/planetest2/camfind.c b/tools/planetest2/camfind.c new file mode 100644 index 0000000..1e9b2b9 --- /dev/null +++ b/tools/planetest2/camfind.c @@ -0,0 +1,385 @@ +#include +#include +#include "linmath.h" +#include +#include + +#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, ×tamp, &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; +} -- cgit v1.2.3