From 7f4ac3104156a25b39495af0e3146d3542283d53 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 21 Mar 2018 01:53:22 -0400 Subject: Update dave's ortho and survive_cal to allow calling of one item even if multiple are connected. Something may still be flipped. --- src/poser_daveortho.c | 177 ++++++++++++++++++++++++++++++++------------------ src/survive_cal.c | 36 +++++----- 2 files changed, 132 insertions(+), 81 deletions(-) (limited to 'src') diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 0e88a4e..e2e8d2e 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -10,7 +10,6 @@ //Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s -static int LH_ID; void OrthoSolve( FLT T[4][4], // OUTPUT: 4x4 transformation matrix @@ -47,8 +46,11 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)pd; - dd->lengths[l->sensor_id][l->lh][dd->nextaxis] = l->length; - dd->angles [l->sensor_id][l->lh][dd->nextaxis] = l->angle; + if( l->length > dd->lengths[l->sensor_id][l->lh][dd->nextaxis] ) + { + dd->lengths[l->sensor_id][l->lh][dd->nextaxis] = l->length; + dd->angles [l->sensor_id][l->lh][dd->nextaxis] = l->angle; + } break; } case POSERDATA_SYNC: @@ -58,7 +60,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) dd->nextaxis = l->acode & 1; - if( dd->nextaxis == 0 && l->lh == 0 && so->codename[0] == 'H' ) + if( dd->nextaxis == 0 && so->codename[0] == 'H' ) { int i; int max_hits = 0; @@ -80,40 +82,41 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) } } - if( max_hits ) + if( max_hits > 2 ) { + //if( lhid == 0 ) { printf( "\033[H\033[2J" ); } + //printf( "%d\n", max_hits ); FLT tOut[4][4]; FLT S_out[2][SENSORS_PER_OBJECT]; OrthoSolve( tOut, S_out, S_in, X_in, max_hits ); //Now, we need to solve where we are as a function of where //the lighthouses are. - FLT quat[4]; - FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] }; + + SurvivePose objpose; FLT MT[4][4]; + objpose.Pos[0] = tOut[0][3]; + objpose.Pos[1] = tOut[1][3]; + objpose.Pos[2] = tOut[2][3]; + //matrix44transpose( MT, &tOut[0][0] ); matrix44copy( &MT[0][0], &tOut[0][0] ); - quatfrommatrix( quat, &MT[0][0] ); - + quatfrommatrix( objpose.Rot, &MT[0][0] ); //printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) ); //quat[2] -= 0.005; //fixes up lh0 in test data set. - quatnormalize( quat, quat ); - printf( "QUAT: %f %f %f %f = %f [%f %f %f]\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat), posoff[0], posoff[1], posoff[2] ); - -if(0) - for( i = 0; i < max_hits;i++ ) + quatnormalize( objpose.Rot, objpose.Rot ); + + SurvivePose poseout; + InvertPose(poseout.Pos, objpose.Pos); + if( 0 ) { - FLT pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] }; - quatrotatevector( pt, quat, pt ); - add3d( pt, pt, posoff ); - printf( "OUT %f %f %f ANGLE %f %f AOUT %f %f\n", - pt[0], pt[1], pt[2], - S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) ); + printf( "INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2] ); + printf( "OUQUAT: %f %f %f %f = %f [%f %f %f]\n", poseout.Rot[0], poseout.Rot[1], poseout.Rot[2], poseout.Rot[3], quatmagnitude(poseout.Rot), poseout.Pos[0], poseout.Pos[1], poseout.Pos[2] ); } - } + } } //if( so->codename[0] == 'H' ) printf( "LIG:%s %d @ %d rad, %f s (AX %d) (TC %d)\n", so->codename, l->sensor_id, l->lh, l->length, dd->nextaxis, l->timecode ); @@ -123,6 +126,8 @@ if(0) case POSERDATA_FULL_SCENE: { PoserDataFullScene * fs = (PoserDataFullScene*)pd; + int LH_ID; + printf( "PDFS\n" ); for( LH_ID = 0; LH_ID < 2; LH_ID++ ) { @@ -150,38 +155,53 @@ if(0) //Now, we need to solve where we are as a function of where //the lighthouses are. - FLT quat[4]; - FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] }; + //FLT quat[4]; + //FLT posoff[3] = { tOut[0][3], tOut[1][3], tOut[2][3] }; + SurvivePose objpose; FLT MT[4][4]; + objpose.Pos[0] = tOut[0][3]; + objpose.Pos[1] = tOut[1][3]; + objpose.Pos[2] = tOut[2][3]; + //matrix44transpose( MT, &tOut[0][0] ); matrix44copy( &MT[0][0], &tOut[0][0] ); - quatfrommatrix( quat, &MT[0][0] ); - + quatfrommatrix( objpose.Rot, &MT[0][0] ); //printf( "QUAT: %f %f %f %f = %f\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat) ); //quat[2] -= 0.005; //fixes up lh0 in test data set. - quatnormalize( quat, quat ); - printf( "QUAT: %f %f %f %f = %f [%f %f %f]\n", quat[0], quat[1], quat[2], quat[3], quatmagnitude(quat), posoff[0], posoff[1], posoff[2] ); + quatnormalize( objpose.Rot, objpose.Rot ); + printf( "QUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2] ); - for( i = 0; i < max_hits;i++ ) - { - FLT pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] }; - quatrotatevector( pt, quat, pt ); - add3d( pt, pt, posoff ); - printf( "OUT %f %f %f ANGLE %f %f AOUT %f %f\n", - pt[0], pt[1], pt[2], - S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) ); - } + if( 0 ); + for( i = 0; i < max_hits;i++ ) + { + FLT pt[3] = { X_in[0][i], X_in[1][i], X_in[2][i] }; + quatrotatevector( pt, objpose.Rot, pt ); + add3d( pt, pt, objpose.Pos ); + printf( "OUT %f %f %f ANGLE %f %f AOUT %f %f\n", + pt[0], pt[1], pt[2], + S_in[0][i], S_in[1][i], atan2( pt[0], pt[1] ), atan2( pt[2], pt[1] ) ); + } + + /* + so->FromLHPose[LH_ID].Pos[0] = objpose.Pos[0]; + so->FromLHPose[LH_ID].Pos[1] = objpose.Pos[1]; + so->FromLHPose[LH_ID].Pos[2] = objpose.Pos[2]; + so->FromLHPose[LH_ID].Rot[0] = objpose.Rot[0]; + so->FromLHPose[LH_ID].Rot[1] = objpose.Rot[1]; + so->FromLHPose[LH_ID].Rot[2] = objpose.Rot[2]; + so->FromLHPose[LH_ID].Rot[3] = objpose.Rot[3]; + */ + + + SurvivePose poseout; + InvertPose(poseout.Pos, objpose.Pos); + printf( "INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2] ); - so->FromLHPose[LH_ID].Pos[0] = posoff[0]; - so->FromLHPose[LH_ID].Pos[1] = posoff[1]; - so->FromLHPose[LH_ID].Pos[2] = posoff[2]; - so->FromLHPose[LH_ID].Rot[0] = quat[0]; - so->FromLHPose[LH_ID].Rot[1] = quat[1]; - so->FromLHPose[LH_ID].Rot[2] = quat[2]; - so->FromLHPose[LH_ID].Rot[3] = quat[3]; + PoserData_lighthouse_pose_func(&fs->hdr, so, LH_ID, &poseout); + printf( "OUQUAT: %f %f %f %f = %f [%f %f %f]\n", poseout.Rot[0], poseout.Rot[1], poseout.Rot[2], poseout.Rot[3], quatmagnitude(poseout.Rot), poseout.Pos[0], poseout.Pos[1], poseout.Pos[2] ); } break; @@ -340,8 +360,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); */ // FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; // FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; - FLT ydist = 1.0 / urhat_len; - printf("ydist %f\n", ydist); + FLT ydist = 1.0 / urhat_len; //TRICKY XXX Dave operates with "y forward" for some reason... +// printf("ydist %f\n", ydist); // printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- @@ -382,8 +402,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); // printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far - //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } - if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + //if ( i == 0 && j == 0 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } z_y = -z_y; } y_y = -y_y; @@ -522,10 +542,27 @@ PRINT(ab,2,1); T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; #else + +/* WRONG: This transposes. + T[0][0]=-R[0][0]; T[0][1]=-R[0][1]; T[0][2]=-R[0][2]; T[0][3]=-trans[0]; + T[2][0]= R[1][0]; T[2][1]= R[1][1]; T[2][2]= R[1][2]; T[2][3]=-trans[1]; + T[1][0]= R[2][0]; T[1][1]= R[2][1]; T[1][2]= R[2][2]; T[1][3]=trans[2]; + T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; +*/ + + //XXX XXX XXX FOR ANYONE LOOKING TO CONTROL THE COORDINATE FRAME, THESE ARE THE LINES!!! + + +/** This is probably all wonky and backwards but "looks" right except with multiple transforms, perhaps it's transposed. + T[0][0]=-R[0][0]; T[0][1]= R[0][1]; T[0][2]= R[0][2]; T[0][3]=-trans[0]; + T[2][0]=-R[1][0]; T[2][1]=-R[1][1]; T[2][2]= R[1][2]; T[2][3]=-trans[1];//This Z axis has problems with getting magnitudes. We can re=make it from the other two axes. + T[1][0]=-R[2][0]; T[1][1]= R[2][1]; T[1][2]=-R[2][2]; T[1][3]= trans[2]; + T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; +*/ T[0][0]=-R[0][0]; T[0][1]=-R[0][1]; T[0][2]=-R[0][2]; T[0][3]=-trans[0]; - T[2][0]=-R[1][0]; T[2][1]=-R[1][1]; T[2][2]=-R[1][2]; T[2][3]=-trans[1]; - T[1][0]=R[2][0]; T[1][1]=R[2][1]; T[1][2]=R[2][2]; T[1][3]=trans[2]; + T[1][0]= R[2][0]; T[1][1]= R[2][1]; T[1][2]= R[2][2]; T[2][3]=-trans[1]; + T[2][0]= R[1][0]; T[2][1]= R[1][1]; T[2][2]= R[1][2]; T[1][3]= trans[2]; T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; #endif @@ -533,8 +570,6 @@ PRINT(ab,2,1); - FLT T2[4][4]; - //------------------- // Orthogonalize the matrix //------------------- @@ -546,33 +581,47 @@ PRINT(ab,2,1); //------------------- // Orthogonalize the matrix //------------------- - //PRINT_MAT(T,4,4); -#if 1 + cross3d( &T[2][0], &T[0][0], &T[1][0] ); //Generate Z from X/Y because Z is kinda rekt. + + //cross3d( &T[1][0], &T[2][0], &T[0][0] ); //Renormalize rotations... + //cross3d( &T[0][0], &T[1][0], &T[2][0] ); //Renormalize rotations... + + + //XXX XXX TODO + //We could further normalize things... + + + +#if 0 // matrix44transpose(T2, T); //Transpose so we are matrix44copy((FLT*)T2,(FLT*)T); - cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. - cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); + cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); //Replace axes in-place. + cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); // matrix44copy((FLT*)T,(FLT*)T2); // matrix44transpose(T, T2); normalize3d( &T[0][0], &T2[0][0] ); normalize3d( &T[1][0], &T2[1][0] ); normalize3d( &T[2][0], &T2[2][0] ); - - #else - normalize3d( &T[0][0], &T[0][0] ); - normalize3d( &T[1][0], &T[1][0] ); - normalize3d( &T[2][0], &T[2][0] ); +// normalize3d( &T[0][0], &T[0][0] ); +// normalize3d( &T[1][0], &T[1][0] ); +// normalize3d( &T[2][0], &T[2][0] ); #endif - //Fix handedness for rotations... - T[0][0]*=-1; - T[0][1]*=-1; - T[0][2]*=-1; +// printf( " In Axis on headset \n" ); +// printf( " x y z\n" ); +// PRINT_MAT(T,4,4); + +// PRINT_MAT(T,4,4); // PRINT_MAT(T,4,4); + //Fix handedness for rotations... +// T[1][0]*=-1; +// T[1][1]*=-1; +// T[1][2]*=-1; + // PRINT_MAT(T,4,4); #if 0 diff --git a/src/survive_cal.c b/src/survive_cal.c index cb242ae..2d97317 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -80,7 +80,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) lighthouses_completed++; if (lighthouses_completed >= NUM_LIGHTHOUSES) { - config_save(ctx, "config.json"); + config_save(ctx, survive_configs( ctx, "configfile", SC_GET, "config.json" ) ); } } @@ -139,8 +139,8 @@ void survive_cal_install( struct SurviveContext * ctx ) // setting the required trackers for calibration to be permissive to make it easier for a newbie to start-- // basically, libsurvive will detect whatever they have plugged in and start using that. // const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1"); - const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", ""); - const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 1 ); + const char * RequiredTrackersForCal = survive_configs( ctx, "requiredtrackersforcal", SC_SETCONFIG, "" ); + const uint32_t AllowAllTrackersForCal = survive_configi( ctx, "allowalltrackersforcal", SC_SETCONFIG, 1 ); size_t requiredTrackersFound = 0; for (int j=0; j < ctx->objs_ct; j++) @@ -334,22 +334,24 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int i, j, k; cd->found_common = 1; for( i = 0; i < cd->numPoseObjects; i++ ) - //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) - for( j = 0; j < ctx->activeLighthouses; j++ ) { - int sensors_visible = 0; - for( k = 0; k < SENSORS_PER_OBJECT; k++ ) + //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) + for( j = 0; j < ctx->activeLighthouses; j++ ) { - 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 ); - reset_calibration( cd ); - cd->found_common = 0; - return; + 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 ); + reset_calibration( cd ); + cd->found_common = 0; + return; + } } } -- cgit v1.2.3