From 69bb821947118bf18456cd309c9e6ac4420c6f31 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Thu, 15 Dec 2016 02:13:43 -0500 Subject: Get a little closer. Rotational optimizer "looks" like it's working, but don't know why it's not. --- tools/planetest2/Makefile | 4 +- tools/planetest2/camfind.c | 292 +++++++++++++++++++++++++-------------------- 2 files changed, 167 insertions(+), 129 deletions(-) (limited to 'tools') diff --git a/tools/planetest2/Makefile b/tools/planetest2/Makefile index 2c16261..dd19480 100644 --- a/tools/planetest2/Makefile +++ b/tools/planetest2/Makefile @@ -1,7 +1,7 @@ all : camfind -CFLAGS:=-g -O4 -DFLT=float -ffast-math -LDFLAGS:=$(CFLAGS) -lm +CFLAGS:=-g -O4 -DFLT=float -ffast-math -I../../redist -flto +LDFLAGS:=$(CFLAGS) -lm camfind : camfind.o ../../redist/linmath.c gcc -o $@ $^ $(LDFLAGS) diff --git a/tools/planetest2/camfind.c b/tools/planetest2/camfind.c index 1e9b2b9..40de785 100644 --- a/tools/planetest2/camfind.c +++ b/tools/planetest2/camfind.c @@ -6,22 +6,25 @@ #define PTS 32 #define MAX_CHECKS 20000 +#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 hmd_point_counts[PTS*2]; +int best_hmd_target = 0; 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. -//Actual values -FLT xyzypr[6] = { 0, 2, 2, 0, 0, 0 }; +//Actual values XXX TODO: I don't think this is needed. +FLT xyz[3] = { 0, 0, 0 }; -FLT RunOpti( int axis ); +FLT RunOpti( int print ); FLT RunTest( int print ); void PrintOpti(); @@ -30,7 +33,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; @@ -38,165 +41,183 @@ int main() FLT dx, dy, dz; - FLT bestxyzypr[6]; - memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) ); + FLT bestxyz[3]; + memcpy( bestxyz, xyz, sizeof( xyz ) ); + //STAGE1 1: Detemine vectoral position from lighthouse to target. Does not determine lighthouse-target distance. + //This also is constantly optimizing the lighthouse quaternion for optimal spotting. FLT fullrange = 5; //Maximum search space for positions. (Relative to HMD) - for( cycle = 0; cycle < 20; cycle ++ ) + for( cycle = 0; cycle < 30; 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 bestxyzrunning[3]; FLT beste = 1e20; + FLT splits = 2; + if( cycle == 0 ) splits = 25; + //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 ) + for( dz = 0; dz <= fullrange; dz += fullrange/splits ) + for( dy = -fullrange; dy <= fullrange; dy += fullrange/splits ) + for( dx = -fullrange; dx <= fullrange; dx += fullrange/splits ) { -#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; + memcpy( xyz, bestxyz, sizeof( xyz ) ); + xyz[0] += dx; + xyz[1] += dy; + xyz[2] += dz; + //if( axis == 2 && xyz[2] < 0 ) continue; + FLT ft; //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 + ft = RunOpti(0); + if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, xyz, sizeof( xyz ) ); } + + //printf( " %f %f %f %f\n", xyz[0], xyz[1], xyz[2], ft ); } -#ifdef FULLSEARCH - memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) ); -#endif + memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) ); //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; + FLT dist = sqrt(bestxyz[0]*bestxyz[0] + bestxyz[1]*bestxyz[1] + bestxyz[2]*bestxyz[2]); + printf( "%f %f %f (%f) = %f\n", bestxyz[0], bestxyz[1], bestxyz[2], dist, beste ); } -*/ + //Every cycle, tighten up the search area. fullrange *= 0.6; } - //Use bestxyzypr - memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) ); + //Use bestxyz + memcpy( LighthousePos, bestxyz, sizeof( xyz ) ); + memcpy( xyz, bestxyz, sizeof( xyz ) ); + //Optimize the quaternion for lighthouse rotation + RunOpti(1); - //Print out plane accuracies with these settings. - PrintOpti(); -} + //STAGE 2: Determine optimal distance from target + { + FLT dist = 0.1; + FLT best_dist = 0; + FLT best_err = 1e20; + for( ; dist < 10; dist+=0.1 ) + { + FLT nrmvect[3]; + normalize3d( nrmvect, bestxyz ); + scale3d( LighthousePos, nrmvect, dist ); + FLT res = RunTest( 0 ); + printf( "%f = %f\n", dist, res ); + if( res < best_err ) + { + best_dist = dist; + best_err = res; + } + } -void PrintOpti() -{ - FLT xyzyprchange[6]; - memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); - quatfromeuler( LighthouseQuat, &xyzyprchange[3] ); - memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) ); + best_dist = 3; + //Apply the best res. + normalize3d( LighthousePos, bestxyz ); + scale3d( LighthousePos, LighthousePos, best_dist ); + printf( "Best distance: %f\n", best_dist ); + } + + //Print out plane accuracies with these settings. FLT ft = RunTest(1); printf( "Final RMS: %f\n", ft ); } - -FLT RunOpti( int axis ) +FLT RunOpti( int print ) { - FLT xyzyprchange[6]; - memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) ); int i; - FLT minv = 1e20; - FLT fv = -3.3; - FLT bv = 0; - + FLT UsToTarget[3]; - //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; - } + memcpy( LighthousePos, xyz, sizeof(xyz) ); - 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]; + //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]; - minv = 1e20; + //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]) ); - //When searching, consider 'less' 'this' and 'more' + //Find a ray from us to the target point. + sub3d( UsToTarget, &hmd_points[best_hmd_target*3], LighthousePos ); + normalize3d( UsToTarget, UsToTarget ); - 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; } + FLT AxisToRotate[3]; + cross3d( AxisToRotate, RayShootOut, UsToTarget ); + //Rotate the lighthouse around this axis to point at the HMD. - 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; } + FLT RotateAmount = acos( dot3d( RayShootOut, UsToTarget ) ); + quatfromaxisangle( LighthouseQuat, AxisToRotate, RotateAmount ); //Tested, working! + } - 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; } + //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; - xyzyprchange[axis] = bv; + float rots[PTS]; + for( i = 0; i < PTS; 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 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" ); + } -- cgit v1.2.3