From a630cc4252f3a64ca89e40fb726f103789734baa Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 14 Mar 2017 01:34:10 -0400 Subject: Update calibration system to use distances. Need to process through that data tomorrow. --- src/poser_charlesslow.c | 26 +++-- src/poser_dummy.c | 2 +- src/survive_cal.c | 248 +++++++++++++++++++----------------------------- src/survive_cal.h | 7 +- 4 files changed, 121 insertions(+), 162 deletions(-) (limited to 'src') diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index 3cc7d9d..def8323 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -8,8 +8,6 @@ #include #include -static int LH_ID; - typedef struct { int something; @@ -45,6 +43,17 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) { PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + + int p; + FLT * hmd_points = so->sensor_locations; + + for( p = 0; p < so->nr_locations; p++ ) + { + printf( "%f %f %f\n", hmd_points[p*3+0], hmd_points[p*3+1], hmd_points[p*3+2] ); + } + + int lh, cycle; FLT dz, dy, dx; for( lh = 0; lh < 2; lh++ ) @@ -99,7 +108,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) if( cycle == 4 ) splits = 5; //Wwe search throug the whole space. - for( dz = 0; dz < fullrange; dz += fullrange/splits ) + for( dz = -fullrange; dz < fullrange; dz += fullrange/splits ) for( dy = -fullrange; dy < fullrange; dy += fullrange/splits ) for( dx = -fullrange; dx < fullrange; dx += fullrange/splits ) { @@ -181,7 +190,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) #endif } - break; + return 0; } case POSERDATA_DISASSOCIATE: { @@ -191,7 +200,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) break; } } - + return -1; } @@ -208,6 +217,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr quatsetnone( LighthouseQuat ); FLT * hmd_points = hmd->sensor_locations; FLT * hmd_normals = hmd->sensor_normals; + int dpts = hmd->nr_locations; int first = 1, second = 0; @@ -215,7 +225,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr //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++ ) + 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; @@ -231,7 +241,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr for( i = 0; i < iters; i++ ) { first = 1; - for( p = 0; p < 32; p++ ) + 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; @@ -313,7 +323,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr //Step 2: Determine error. float errorsq = 0.0; int count = 0; - for( p = 0; p < 32; p++ ) + 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; diff --git a/src/poser_dummy.c b/src/poser_dummy.c index 67f8edb..839c43e 100644 --- a/src/poser_dummy.c +++ b/src/poser_dummy.c @@ -44,7 +44,7 @@ int PoserDummy( SurviveObject * so, PoserData * pd ) break; } } - + return 0; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 1a3be2d..ce8fd89 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -20,6 +20,8 @@ #define PTS_BEFORE_COMMON 32 #define NEEDED_COMMON_POINTS 10 +#define MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL 4 + #define NEEDED_TIMES_OF_COMMON 5 #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4)) @@ -111,7 +113,19 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->stage = 1; cd->ctx = ctx; - cd->hmd = survive_get_so_by_name( ctx, "HMD" ); + cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); + cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); + cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + + if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + { + SV_ERROR( "Error: cannot find all devices needed for calibration." ); + free( cd ); + return; + } + +//XXX TODO MWTourney, work on your code here. +/* if( !cd->hmd ) { cd->hmd = survive_get_so_by_name( ctx, "TR0" ); @@ -124,11 +138,11 @@ void survive_cal_install( struct SurviveContext * ctx ) } SV_INFO( "HMD not found, calibrating using Tracker" ); } - +*/ const char * DriverName; - const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserDaveOrtho" ); + const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" ); PoserCB PreferredPoserCB = 0; const char * FirstPoser = 0; printf( "Available posers:\n" ); @@ -158,6 +172,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int switch( cd->stage ) { default: + case 2: //Taking in angle data. We don't care about light data anymore. case 0: //Default, inactive. break; @@ -177,8 +192,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; - case 2: //Taking in angle data. - break; } } @@ -211,13 +224,17 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int ct = cd->all_counts[sensid][lighthouse][axis]++; cd->all_lengths[sensid][lighthouse][axis][ct] = length; cd->all_angles[sensid][lighthouse][axis][ct] = angle; + + int dev = sensid / 32; + if( ct > cd->peak_counts ) { cd->peak_counts = ct; } + //Determine if there is a sensor on a watchman visible from both lighthouses. - if( sensid >= 32 ) +/* if( sensid >= 32 ) { int k; int ok = 1; @@ -230,11 +247,32 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin } } if( ok ) cd->found_common = 1; - } + }*/ if( cd->peak_counts >= PTS_BEFORE_COMMON ) { -/* int tfc = cd->times_found_common; + int min_peaks = PTS_BEFORE_COMMON; + int i, j, k; + cd->found_common = 1; + for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) + for( j = 0; j < NUM_LIGHTHOUSES; j++ ) + { + int sensors_visible = 0; + for( k = 0; k < SENSORS_PER_OBJECT; k++ ) + { + if( cd->all_counts[k+i*SENSORS_PER_OBJECT][j][0] > NEEDED_COMMON_POINTS && + cd->all_counts[k+i*SENSORS_PER_OBJECT][j][1] > NEEDED_COMMON_POINTS ) + sensors_visible++; + } + if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) + { + printf( "Dev %d, LH %d not enough visible points found.\n", i, j ); + cd->found_common = 0; + return; + } + } + + int tfc = cd->times_found_common; if( cd->found_common ) { if( tfc >= NEEDED_TIMES_OF_COMMON ) @@ -256,12 +294,12 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin SV_INFO( "Stage 2 bad - redoing. %d %d %d", cd->peak_counts, cd->found_common, tfc ); reset_calibration( cd ); cd->times_found_common = 0; - }*/ + } - SV_INFO( "Stage 2 moving to stage 3. %d %d", cd->peak_counts, cd->found_common ); +/* SV_INFO( "Stage 2 moving to stage 3. %d %d", cd->peak_counts, cd->found_common ); reset_calibration( cd ); cd->stage = 3; - cd->found_common = 1; + cd->found_common = 1;*/ } break; @@ -355,8 +393,6 @@ static void handle_calibration( struct SurviveCalData *cd ) FLT Sdiff2 = Sdiff * Sdiff; FLT Ldiff2 = Ldiff * Ldiff; - if( Sdiff2 > max_outlier_angle ) max_outlier_angle = Sdiff2; - if( Ldiff2 > max_outlier_length ) max_outlier_length = Ldiff2; if( Sdiff2 > OUTLIER_ANGLE || Ldiff2 > OUTLIER_LENGTH ) { @@ -364,6 +400,8 @@ static void handle_calibration( struct SurviveCalData *cd ) } else { + if( Sdiff2 > max_outlier_angle ) max_outlier_angle = Sdiff2; + if( Ldiff2 > max_outlier_length ) max_outlier_length = Ldiff2; count++; } } @@ -448,161 +486,69 @@ static void handle_calibration( struct SurviveCalData *cd ) } fclose( hists ); fclose( ptinfo ); -/* - //Comb through data and make sure we still have a sensor on a WM that - //We don't do this anymore. - int bcp_senid = 0; - int bcp_count = 0; - for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) - { - 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]; + int obj; - if( ct0 > ct1 ) ct0 = ct1; - if( ct0 > ct2 ) ct0 = ct2; - if( ct0 > ct3 ) ct0 = ct3; + //Poses of lighthouses relative to objects. + SurvivePose objphl[POSE_OBJECTS][NUM_LIGHTHOUSES]; - if( ct0 > bcp_count ) { bcp_count = ct0; bcp_senid = sen; } - } + FILE * fobjp = fopen( "calinfo/objposes.csv", "w" ); - if( bcp_count < DRPTS_NEEDED_FOR_AVG ) - { - SV_INFO( "Stage 3 could not find a suitable common point on a watchman" ); - reset_calibration( cd ); - return; - } - cd->senid_of_checkpt = bcp_senid; -*/ - - int i, j; - PoserDataFullScene fsd; - fsd.pt = POSERDATA_FULL_SCENE; - for( j = 0; j < NUM_LIGHTHOUSES; j++ ) - for( i = 0; i < SENSORS_PER_OBJECT; i++ ) + for( obj = 0; obj < POSE_OBJECTS; obj++ ) { - int gotdata = 0; - - int dataindex = i*(2*NUM_LIGHTHOUSES)+j*2+0; - - if( cd->ctsweeps[dataindex+0] < DRPTS_NEEDED_FOR_AVG || - cd->ctsweeps[dataindex+1] < DRPTS_NEEDED_FOR_AVG ) + int i, j; + PoserDataFullScene fsd; + fsd.pt = POSERDATA_FULL_SCENE; + for( j = 0; j < NUM_LIGHTHOUSES; j++ ) + for( i = 0; i < SENSORS_PER_OBJECT; i++ ) { - fsd.lengths[i][j][0] = -1; - fsd.lengths[i][j][1] = -1; - continue; - } - fsd.lengths[i][j][0] = cd->avglens[dataindex+0]; - fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; - fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; - fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; - } + int gotdata = 0; - cd->ConfigPoserFn( cd->hmd, (PoserData*)&fsd ); - if( 1 ) - { - static int notfirstcal = 0; - SV_INFO( "Stage 4 succeeded. Inverting %d", notfirstcal ); - - if( !notfirstcal ) - { - // XXX This part is /all/ wrong. - // XXX This part is /all/ wrong. - // XXX This part is /all/ wrong. + int dataindex = (i+obj*32)*(2*NUM_LIGHTHOUSES)+j*2+0; - //OK! We've arrived. Now, we have to get the LH's pose from. - int lh; - for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + if( cd->ctsweeps[dataindex+0] < DRPTS_NEEDED_FOR_AVG || + cd->ctsweeps[dataindex+1] < DRPTS_NEEDED_FOR_AVG ) { - SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh]; - SurvivePose * lhp = &ctx->bsd[lh].Pose; - - -/* lhp->Pos[0] = objfromlh->Pos[0]; - lhp->Pos[1] = objfromlh->Pos[1]; - lhp->Pos[2] = objfromlh->Pos[2];*/ - - lhp->Rot[0] =-objfromlh->Rot[0]; - lhp->Rot[1] = objfromlh->Rot[1]; - lhp->Rot[2] = objfromlh->Rot[2]; - lhp->Rot[3] = objfromlh->Rot[3]; - - quatrotatevector( lhp->Pos, lhp->Rot, objfromlh->Pos ); - - //Write lhp from the inverse of objfromlh - //quatrotatevector( lhp->Pos, lhp->Rot, lhp->Pos ); - - - fprintf( stderr, "%f, %f, %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] ); - fprintf( stderr, "%f, %f, %f, %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] ); + fsd.lengths[i][j][0] = -1; + fsd.lengths[i][j][1] = -1; + continue; + } + fsd.lengths[i][j][0] = cd->avglens[dataindex+0]; + fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; + fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; + fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + } - /* - -0.204066 3.238746 -0.856369 - 0.812203 -0.264897 0.505599 0.120520 - -0.204066 3.238746 -0.856369 + int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd ); + if( r ) + { + SV_INFO( "Failed calibration on dev %d\n", obj ); + reset_calibration( cd ); + cd->stage = 2; + fclose( fobjp ); + return; + } - 0.020036 3.162476 -0.117896 - -0.322354 0.450869 0.346281 0.756898 - 0.020036 3.162476 -0.117896 - */ - /* Facing up, moved -x 1m. - ====> 0.446818 -0.309120 -0.747630 ====> -0.222356 -0.701865 -0.558656 - ====> -0.341064 0.099785 0.887015 ====> 0.619095 0.727263 0.029786 - IN PLACE, but rotated 90 * up. - ====> 0.374516 -0.370583 -0.606996 ====> -0.120238 -0.670330 -0.426896 - ====> -0.231758 0.070437 0.765982 ====> 0.497615 0.625761 0.078759 - */ + int lh; + for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + { + SurvivePose * objfromlh = &cd->poseobjects[obj]->FromLHPose[lh]; //The pose is here + SurvivePose * lhp = &ctx->bsd[lh].Pose; //Need to somehow put pose here. - printf( "\n" ); + memcpy( &objphl[obj][lh], objfromlh, sizeof( SurvivePose ) ); - } - notfirstcal = 1; - } - else - { - for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) - { - SurvivePose * objfromlh = &cd->hmd->FromLHPose[lh]; - SurvivePose * lhp = &ctx->bsd[lh].Pose; - - FLT pos[3]; - quatrotatevector( pos, lhp->Rot, objfromlh->Pos ); - - pos[0] -= lhp->Pos[0]; - pos[1] -= lhp->Pos[1]; - pos[2] -= lhp->Pos[2]; - - //FLT rot[4] = { - // [0], - // lhp->Rot[1], - // lhp->Rot[2], - // lhp->Rot[3] }; - //quatrotatevector( pos, lhp->Rot, pos ); - - fprintf( stderr, "%f, %f, %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] ); - fprintf( stderr, "%f, %f, %f, %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] ); - fprintf( stderr, "%f, %f, %f\n", lhp->Pos[0], lhp->Pos[1], lhp->Pos[2] ); - fprintf( stderr, "%f, %f, %f, %f\n", lhp->Rot[0], lhp->Rot[1], lhp->Rot[2], lhp->Rot[3] ); - - fprintf( stderr, "====> %f %f %f\n", - pos[0], pos[1], pos[2] ); - - } + fprintf( fobjp, "%f %f %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] ); + fprintf( fobjp, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] ); } - fprintf( stderr, "\n" ); + } + fclose( fobjp ); - reset_calibration( cd ); -// cd->stage = 5; - } - else - { - SV_INFO( "Stage 4 failed." ); - reset_calibration( cd ); - } + + SV_INFO( "Stage 4 succeeded." ); + reset_calibration( cd ); + cd->stage = 5; } diff --git a/src/survive_cal.h b/src/survive_cal.h index ce61962..3d62051 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -35,7 +35,10 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MAX_SENSORS_TO_CAL 96 #define MIN_PTS_BEFORE_CAL 24 -#define DRPTS 32 +#define DRPTS 128 + +#define POSE_OBJECTS 3 + #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData { @@ -61,7 +64,7 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. - SurviveObject * hmd; + SurviveObject * poseobjects[POSE_OBJECTS]; PoserCB ConfigPoserFn; -- cgit v1.2.3