#include "linmath.h" #include "survive_cal.h" #include #include #include #include #include #include #include #include typedef struct { int something; //Stuff } DummyData; static FLT RunOpti( SurviveObject * so, PoserDataFullScene * fs, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat ); int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) { PoserType pt = pd->pt; SurviveContext * ctx = so->ctx; DummyData * dd = so->PoserData; if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) ); switch( pt ) { case POSERDATA_IMU: { PoserDataIMU * imu = (PoserDataIMU*)pd; //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); break; } case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)pd; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); break; } case POSERDATA_FULL_SCENE: { PoserDataFullScene * fs = (PoserDataFullScene*)pd; int p; FLT * hmd_points = so->sensor_locations; for( p = 0; p < so->sensor_ct; p++ ) { printf( "%f %f %f\n", hmd_points[p*3+0], hmd_points[p*3+1], hmd_points[p*3+2] ); } SurvivePose additionalTx = {0}; int lh, cycle; FLT dz, dy, dx; 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] = 0; LighthouseQuat[2] = 0; LighthouseQuat[3] = 0; 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 = -fullrange; 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(so, fs, lh, 0, LighthousePos, LighthouseQuat); if( cycle == 0 ) { FLT sk = ft*10.; if( sk > 1 ) sk = 1; uint8_t cell = (uint8_t)((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.1 ) { //Error too high SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste ); return -1; } RunOpti(so, fs, lh, 1, LighthousePos, LighthouseQuat); SurvivePose lighthousePose; copy3d(lighthousePose.Pos, LighthousePos); quatcopy(lighthousePose.Rot, LighthouseQuat); const FLT rt[4] = {0, 0, 1, 0}; FLT tmp[4]; quatrotateabout(tmp, lighthousePose.Rot, rt); memcpy(lighthousePose.Rot, tmp, sizeof(FLT) * 4); PoserData_lighthouse_pose_func(pd, so, lh, &additionalTx, &lighthousePose, 0); #define ALT_COORDS #ifdef ALT_COORDS so->FromLHPose[lh].Pos[0] = LighthousePos[0]; so->FromLHPose[lh].Pos[1] = LighthousePos[1]; so->FromLHPose[lh].Pos[2] = LighthousePos[2]; so->FromLHPose[lh].Rot[0] =-LighthouseQuat[0]; so->FromLHPose[lh].Rot[1] = LighthouseQuat[1]; so->FromLHPose[lh].Rot[2] = LighthouseQuat[2]; so->FromLHPose[lh].Rot[3] = LighthouseQuat[3]; quatrotatevector( so->FromLHPose[lh].Pos, so->FromLHPose[lh].Rot, so->FromLHPose[lh].Pos ); #else so->FromLHPose[lh].Pos[0] = LighthousePos[0]; so->FromLHPose[lh].Pos[1] = LighthousePos[1]; so->FromLHPose[lh].Pos[2] = LighthousePos[2]; so->FromLHPose[lh].Rot[0] = LighthouseQuat[0]; so->FromLHPose[lh].Rot[1] = LighthouseQuat[1]; so->FromLHPose[lh].Rot[2] = LighthouseQuat[2]; so->FromLHPose[lh].Rot[3] = LighthouseQuat[3]; #endif } return 0; } case POSERDATA_DISASSOCIATE: { free( dd ); so->PoserData = 0; //printf( "Need to disassociate.\n" ); break; } } return -1; } REGISTER_LINKTIME( PoserCharlesSlow ); static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, 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 = hmd->sensor_locations; FLT * hmd_normals = hmd->sensor_normals; int dpts = hmd->sensor_ct; 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 < dpts; p++ ) { int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; FLT me_to_dot[3]; sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] ); FLT 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 < dpts; p++ ) { int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; FLT out[2] = {0}; survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); //Find out where our ray shoots forth from. FLT ax = out[0]; FLT ay = out[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]; quatnormalize( LighthouseQuat, LighthouseQuat ); //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]; quatnormalize( LighthouseQuat, LighthouseQuat ); 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 ); quatnormalize( ConcatQuat, ConcatQuat ); } 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 ); quatnormalize( ConcatQuat, ConcatQuat ); } quatnormalize( ConcatQuat, ConcatQuat ); quatnormalize( LighthouseQuat, LighthouseQuat ); quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat ); //Checked. 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. FLT errorsq = 0.0; int count = 0; for( p = 0; p < dpts; p++ ) { int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; //Find out where our ray shoots forth from. FLT out[2] = {0}; survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); // Find out where our ray shoots forth from. FLT ax = out[0]; FLT ay = out[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); }