From f92f5dc93cbb53a99da51984541a7e4a70605639 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 25 Feb 2017 13:29:07 -0500 Subject: Update files, working lhfind whithin libsurvive. --- 81-vive.rules | 27 ++--- Makefile | 4 +- include/survive.h | 6 +- src/ootx_decoder.c | 2 +- src/survive_cal.c | 27 ++--- src/survive_cal.h | 3 +- src/survive_cal_lhfind.c | 272 ++++++++++++++++++++++++++++++++++++++++++++++- src/survive_internal.h | 14 +-- 8 files changed, 315 insertions(+), 40 deletions(-) diff --git a/81-vive.rules b/81-vive.rules index d317e91..ab38087 100644 --- a/81-vive.rules +++ b/81-vive.rules @@ -1,13 +1,14 @@ -#libsurvive - -SUBSYSTEM=="usb", ATTR{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC HMD -SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Light input -SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Watchman - -#SUBSYSTEM=="usb", ATTR{idVendor}=="abcd", GROUP="adm", MODE="0666" -#SUBSYSTEM=="usb", ATTR{idProduct}=="f003", ATTRS{idVendor}=="abcd", MODE="0666", GROUP="colorchord" -# OpenVR (SteamVR) / OSVR-HTC-Vive - -KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC -KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Valve -KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Valve +# HTC Vive HID Sensor naming and permissioning +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTRS{idProduct}=="2c87", TAG+="uaccess" +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2101", TAG+="uaccess" +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2000", TAG+="uaccess" +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="1043", TAG+="uaccess" +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2050", TAG+="uaccess" +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2011", TAG+="uaccess" +KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2012", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="0bb4", ATTRS{idProduct}=="2c87", TAG+="uaccess" +# HTC Camera USB Node +SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8328", TAG+="uaccess" +# HTC Mass Storage Node +SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8200", TAG+="uaccess" +SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8a12", TAG+="uaccess" diff --git a/Makefile b/Makefile index 5724274..fb74e75 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ all : lib data_recorder test calibrate -CFLAGS:=-Iinclude -fPIC -g -O0 -Iredist -flto +CFLAGS:=-Iinclude -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g @@ -20,7 +20,7 @@ calibrate : calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFuncti lib: mkdir lib -lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o $(DEBUGSTUFF) $(CALS) +lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o $(DEBUGSTUFF) $(CALS) gcc -o $@ $^ $(LDFLAGS) -shared clean : diff --git a/include/survive.h b/include/survive.h index 3e8dc35..1b9c29c 100644 --- a/include/survive.h +++ b/include/survive.h @@ -4,7 +4,11 @@ #include #ifndef FLT +#ifdef USE_DOUBLE #define FLT double +#else +#define FLT float +#endif #endif struct SurviveContext; @@ -33,6 +37,7 @@ struct SurviveObject FLT * sensor_locations; FLT * sensor_normals; + int8_t nr_locations; //Timing sensitive data (mostly for disambiguation) int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) @@ -43,7 +48,6 @@ struct SurviveObject int32_t pulse_max_for_sweep; //1,800 for normal vive hardware. (guessed) int32_t pulse_synctime_offset; //20,000 for normal vive hardware. (guessed) int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) - int8_t nr_locations; //Flood info, for calculating which laser is currently sweeping. int8_t oldcode; diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c index 8ec16f2..e35c24d 100644 --- a/src/ootx_decoder.c +++ b/src/ootx_decoder.c @@ -112,7 +112,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) { // printf("drop %d\n", dbit); if( !dbit ) { - printf("Bad sync bit\n"); + //printf("Bad sync bit\n"); ootx_reset_buffer(ctx); } ctx->bits_processed = 0; diff --git a/src/survive_cal.c b/src/survive_cal.c index 62cf698..760692c 100644 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -146,7 +146,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int sensid = sensor_id; if( strcmp( so->codename, "WM0" ) == 0 ) sensid += 32; - if( strcmp( so->codename, "WM1" ) == 1 ) + if( strcmp( so->codename, "WM1" ) == 0 ) sensid += 64; if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return; @@ -246,13 +246,15 @@ static void handle_calibration( struct SurviveCalData *cd ) #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) +/* FLT avgsweeps[MAX_CAL_PT_DAT]; FLT avglens[MAX_CAL_PT_DAT]; FLT stdsweeps[MAX_CAL_PT_DAT]; FLT stdlens[MAX_CAL_PT_DAT]; int ctsweeps[MAX_CAL_PT_DAT]; +*/ - memset( ctsweeps, 0, sizeof( ctsweeps ) ); + memset( cd->ctsweeps, 0, sizeof( cd->ctsweeps ) ); //Either advance to stage 4 or go resetting will go back to stage 2. //What is stage 4? Are we done then? @@ -317,6 +319,7 @@ static void handle_calibration( struct SurviveCalData *cd ) if( count < DRPTS_NEEDED_FOR_AVG ) { + printf( "DPAVG %d\n", count ); //Not enough for this point to be considered. continue; } @@ -385,12 +388,12 @@ static void handle_calibration( struct SurviveCalData *cd ) fprintf( ptinfo, "%d %d %d %d %f %f %f %f %f %f\n", sen, lh, axis, count, avgsweep, avglen*1000000, stddevang*1000000000, stddevlen*1000000000, max_outlier_length*1000000000, max_outlier_angle*1000000000 ); - int dataindex = sen*4+lh*2+axis; - avgsweeps[dataindex] = avgsweep; - avglens[dataindex] = avglen; - stdsweeps[dataindex] = stddevang; - stdlens[dataindex] = stddevlen; - ctsweeps[dataindex] = count; + int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; + cd->avgsweeps[dataindex] = avgsweep; + cd->avglens[dataindex] = avglen; + cd->stdsweeps[dataindex] = stddevang; + cd->stdlens[dataindex] = stddevlen; + cd->ctsweeps[dataindex] = count; } fclose( hists ); fclose( ptinfo ); @@ -400,10 +403,10 @@ static void handle_calibration( struct SurviveCalData *cd ) int bcp_count = 0; for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) { - int ct0 = ctsweeps[sen*4+0]; - int ct1 = ctsweeps[sen*4+0]; - int ct2 = ctsweeps[sen*4+0]; - int ct3 = ctsweeps[sen*4+0]; + int ct0 = cd->ctsweeps[sen*4+0]; + int ct1 = cd->ctsweeps[sen*4+0]; + int ct2 = cd->ctsweeps[sen*4+0]; + int ct3 = cd->ctsweeps[sen*4+0]; if( ct0 > ct1 ) ct0 = ct1; if( ct0 > ct2 ) ct0 = ct2; diff --git a/src/survive_cal.h b/src/survive_cal.h index bf6161d..0a772f1 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -31,7 +31,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #define MAX_SENSORS_TO_CAL 96 -#define DRPTS 512 +#define DRPTS 1024 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData { @@ -48,6 +48,7 @@ struct SurviveCalData int8_t times_found_common; //For camfind (4+) + //Index is calculated with: int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; FLT avgsweeps[MAX_CAL_PT_DAT]; FLT avglens[MAX_CAL_PT_DAT]; FLT stdsweeps[MAX_CAL_PT_DAT]; diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c index d879f30..19f732f 100644 --- a/src/survive_cal_lhfind.c +++ b/src/survive_cal_lhfind.c @@ -1,11 +1,22 @@ #include "survive_cal.h" +#include +#include +#include "linmath.h" -//Stub file for doing lhfind (this doesn't work) +#define MAX_CHECKS 40000 +#define MIN_HITS_FOR_VALID 10 + + +FLT static RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ); + +//Values used for RunTest() -//Return int survive_cal_lhfind( struct SurviveCalData * cd ) { struct SurviveContext * ctx = cd->ctx; + int cycle, i; + int lh = 0; + FLT dx, dy, dz; //Use the following: // FLT avgsweeps[MAX_CAL_PT_DAT]; @@ -16,7 +27,262 @@ int survive_cal_lhfind( struct SurviveCalData * cd ) // // Check your solution against point: senid_of_checkpt's data. - return -1; //Return 0 if success. + + + for( lh = 0; lh < 2; lh++ ) + { + FLT beste = 1e20; + + FLT LighthousePos[3]; + FLT LighthouseQuat[4]; + + LighthousePos[0] = 0; + LighthousePos[1] = 0; + LighthousePos[2] = 0; + LighthouseQuat[0] = 1; + LighthouseQuat[1] = 1; + LighthouseQuat[2] = 1; + LighthouseQuat[3] = 1; + + FLT bestxyz[3]; + memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) ); + + //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) + + + //Sweep whole area 30 times + for( cycle = 0; cycle < 30; cycle ++ ) + { + + //Adjust position, one axis at a time, over and over until we zero in. + { + FLT bestxyzrunning[3]; + beste = 1e20; + + FILE * f; + if( cycle == 0 ) + { + char filename[1024]; + sprintf( filename, "calinfo/%d_lighthouse.dat", lh ); + f = fopen( filename, "wb" ); + } + + //We split the space into this many groups (times 2) and + //if we're on the first cycle, we want to do a very linear + //search. As we refine our search we can then use a more + //binary search technique. + FLT splits = 4; + if( cycle == 0 ) splits = 32; + if( cycle == 1 ) splits = 13; + if( cycle == 2 ) splits = 10; + if( cycle == 3 ) splits = 8; + if( cycle == 4 ) splits = 5; + + //Wwe search throug the whole space. + for( dz = 0; dz < fullrange; dz += fullrange/splits ) + for( dy = -fullrange; dy < fullrange; dy += fullrange/splits ) + for( dx = -fullrange; dx < fullrange; dx += fullrange/splits ) + { + //Specificially adjust one axis at a time, searching for the best. + memcpy( LighthousePos, bestxyz, sizeof( LighthousePos ) ); + LighthousePos[0] += dx; //These are adjustments to the "best" from last frame. + LighthousePos[1] += dy; + LighthousePos[2] += dz; + + FLT ft; + //Try refining the search for the best orientation several times. + ft = RunOpti(cd, lh, 0, LighthousePos, LighthouseQuat); + if( cycle == 0 ) + { + float sk = ft*10.; + if( sk > 1 ) sk = 1; + uint8_t cell = (1.0 - sk) * 255; + FLT epsilon = 0.1; + + if( dz == 0 ) { /* Why is dz special? ? */ + if ( dx > -epsilon && dx < epsilon ) + cell = 255; + if ( dy > -epsilon && dy < epsilon ) + cell = 128; + } + + fprintf( f, "%c", cell ); + } + + if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); } + } + + if( cycle == 0 ) + { + fclose( f ); + } + memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) ); + + //Print out the quality of the lock this time. + 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.25; + } + + if( beste > 0.005 ) + { + //Error too high + SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste ); + return -1; + } + + cd->ctx->bsd[lh].PositionSet = 1; + copy3d( cd->ctx->bsd[lh].Position, LighthousePos ); + quatcopy( cd->ctx->bsd[lh].Quaternion, LighthouseQuat ); + } + + return 0; //Return 0 if success. } + + + + +static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ) +{ + int i, p; + FLT UsToTarget[3]; + FLT LastUsToTarget[3]; + FLT mux = .9; + quatsetnone( LighthouseQuat ); + FLT * hmd_points = cd->ctx->headset.sensor_locations; + FLT * hmd_normals = cd->ctx->headset.sensor_normals; + + int first = 1, second = 0; + + //First check to see if this is a valid viewpoint. + //If a sensor is pointed away from where we are testing a possible lighthouse position. + //BUT We get data from that light house, then we KNOW this is not a possible + //lighthouse position. + for( p = 0; p < 32; p++ ) + { + int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; + if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue; + FLT me_to_dot[3]; + sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] ); + float dot = dot3d( &hmd_normals[p*3], me_to_dot ); + if( dot < -.01 ) { return 1000; } + } + int iters = 6; + + //Iterate over a refinement of the quaternion that constitutes the + //lighthouse. + for( i = 0; i < iters; i++ ) + { + first = 1; + for( p = 0; p < 32; p++ ) + { + int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; + if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue; + + //Find out where our ray shoots forth from. + FLT ax = cd->avgsweeps[dataindex+0]; + FLT ay = cd->avgsweeps[dataindex+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 ); + } + + quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Chekcked. This appears to be + + mux = mux * 0.94; + if( second ) { second = 0; } + if( first ) { first = 0; second = 1; } + copy3d( LastUsToTarget, RayShootOutWorld ); + } + } + + //Step 2: Determine error. + float errorsq = 0.0; + int count = 0; + for( p = 0; p < 32; p++ ) + { + int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; + if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue; + + //Find out where our ray shoots forth from. + FLT ax = cd->avgsweeps[dataindex+0]; + FLT ay = cd->avgsweeps[dataindex+1]; + FLT RayShootOut[3] = { sin(ax), sin(ay), 0 }; + RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) ); + + //Rotate that ray by the current rotation estimation. + quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut ); + + //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, cd->ctsweeps[dataindex+0], cd->ctsweeps[dataindex+1] ); + } + if( print ) printf( " = %f\n", sqrt( errorsq ) ); + return sqrt(errorsq); +} + diff --git a/src/survive_internal.h b/src/survive_internal.h index 0d0b8a4..19aa956 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -67,16 +67,16 @@ struct SurviveCalData; struct BaseStationData { uint8_t PositionSet:1; - float Position[3]; - float Quaternion[4]; + FLT Position[3]; + FLT Quaternion[4]; uint8_t OOTXSet:1; uint32_t BaseStationID; - float fcalphase[2]; - float fcaltilt[2]; - float fcalcurve[2]; - float fcalgibpha[2]; - float fcalgibmag[2]; + FLT fcalphase[2]; + FLT fcaltilt[2]; + FLT fcalcurve[2]; + FLT fcalgibpha[2]; + FLT fcalgibmag[2]; }; struct SurviveContext -- cgit v1.2.3