From ee12999bcb71f9b4b3c48bd0c1874f7ac251f355 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Tue, 20 Mar 2018 21:34:42 -0400 Subject: significant improvements to Orthodave. --- src/poser_daveortho.c | 138 ++++++++++++++++++++++++++++++++++++++++---------- src/survive_process.c | 18 ++++++- 2 files changed, 129 insertions(+), 27 deletions(-) (limited to 'src') diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 2bf57c6..0e88a4e 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -8,6 +8,8 @@ #include #include +//Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s + static int LH_ID; void OrthoSolve( @@ -20,7 +22,9 @@ void OrthoSolve( typedef struct { - int something; + int nextaxis; + float lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + float angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //Stuff } DummyData; @@ -30,7 +34,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) SurviveContext * ctx = so->ctx; DummyData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = malloc( sizeof( DummyData ) ); + if( !dd ) so->PoserData = dd = calloc( sizeof( DummyData ), 1 ); switch( pt ) { @@ -43,7 +47,77 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) 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 ); + 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: + { + PoserDataLight * l = (PoserDataLight*)pd; + int lhid = l->lh; + + dd->nextaxis = l->acode & 1; + + if( dd->nextaxis == 0 && l->lh == 0 && so->codename[0] == 'H' ) + { + int i; + int max_hits = 0; + FLT S_in[2][SENSORS_PER_OBJECT]; + FLT X_in[3][SENSORS_PER_OBJECT]; + for( i = 0; i < SENSORS_PER_OBJECT; i++ ) + { + //Load all our valid points into something the LHFinder can use. + if( dd->lengths[i][lhid][0] > 0 && dd->lengths[i][lhid][1] > 0 ) + { + S_in[0][max_hits] = dd->angles[i][lhid][0]; + S_in[1][max_hits] = dd->angles[i][lhid][1]; + dd->lengths[i][lhid][0] = -1; + dd->lengths[i][lhid][1] = -1; + X_in[0][max_hits] = so->sensor_locations[i*3+0]; + X_in[1][max_hits] = so->sensor_locations[i*3+1]; + X_in[2][max_hits] = so->sensor_locations[i*3+2]; + max_hits++; + } + + } + if( 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] }; + FLT MT[4][4]; + + //matrix44transpose( MT, &tOut[0][0] ); + matrix44copy( &MT[0][0], &tOut[0][0] ); + + quatfrommatrix( quat, &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++ ) + { + 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( 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 ); + break; } case POSERDATA_FULL_SCENE: @@ -148,7 +222,7 @@ REGISTER_LINKTIME( PoserDaveOrtho ); void OrthoSolve( FLT T[4][4], // OUTPUT: 4x4 transformation matrix - FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points + FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points (May not be populated!!!) FLT S_in[2][SENSORS_PER_OBJECT], // INPUT: array of screenspace points FLT X_in[3][SENSORS_PER_OBJECT], // INPUT: array of offsets int nPoints) @@ -437,12 +511,26 @@ PRINT(ab,2,1); FLT transdist = sqrt( trans[0]*trans[0] + trans[1]*trans[1] + trans[2]*trans[2] ); //------------------- - // Pack into the 4x4 transformation matrix + // Pack into the 4x4 transformation matrix, and put into correct "world"-space. I.e. where is the object, relative to the lighthouse. //------------------- + +#if 0 + //Don't do any transformation to the correct space. T[0][0]=R[0][0]; T[0][1]=R[0][1]; T[0][2]=R[0][2]; T[0][3]=trans[0]; T[1][0]=R[1][0]; T[1][1]=R[1][1]; T[1][2]=R[1][2]; T[1][3]=trans[1]; T[2][0]=R[2][0]; T[2][1]=R[2][1]; T[2][2]=R[2][2]; T[2][3]=trans[2]; T[3][0]=0.0; T[3][1]=0.0; T[3][2]=0.0; T[3][3]=1.0; +#else + + //XXX XXX XXX FOR ANYONE LOOKING TO CONTROL THE COORDINATE FRAME, THESE ARE THE LINES!!! + 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; + +#endif + + FLT T2[4][4]; @@ -450,47 +538,44 @@ PRINT(ab,2,1); //------------------- // Orthogonalize the matrix //------------------- - FLT temp[4][4]; - FLT quat[4], quatNorm[4]; - FLT euler[3]; + //FLT temp[4][4]; + //FLT quat[4], quatNorm[4]; + //FLT euler[3]; //------------------- // Orthogonalize the matrix //------------------- - PRINT_MAT(T,4,4); + //PRINT_MAT(T,4,4); #if 1 // matrix44transpose(T2, T); //Transpose so we are matrix44copy((FLT*)T2,(FLT*)T); - cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. - matrix44copy((FLT*)T,(FLT*)T2); + cross3d( &T2[1][0], &T2[0][0], &T2[2][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] ); -#endif +#else normalize3d( &T[0][0], &T[0][0] ); normalize3d( &T[1][0], &T[1][0] ); normalize3d( &T[2][0], &T[2][0] ); - //Change handedness +#endif - T[1][0]*=-1; - T[1][1]*=-1; - T[1][2]*=-1; + //Fix handedness for rotations... + T[0][0]*=-1; + T[0][1]*=-1; + T[0][2]*=-1; + +// PRINT_MAT(T,4,4); -/* - //Check Orthogonality. Yep. It's orthogonal. - FLT tmp[3]; - cross3d( tmp, &T[0][0], &T[1][0] ); - printf( "M3: %f\n", magnitude3d( tmp ) ); - cross3d( tmp, &T[2][0], &T[1][0] ); - printf( "M3: %f\n", magnitude3d( tmp ) ); - cross3d( tmp, &T[2][0], &T[0][0] ); - printf( "M3: %f\n", magnitude3d( tmp ) ); -*/ // PRINT_MAT(T,4,4); +#if 0 #if 1 @@ -554,6 +639,7 @@ PRINT(ab,2,1); //S_out[1][i] = Tz; // printf("point %i Txyz %f %f %f in %f %f out %f %f morph %f %f\n", i, Tx,Ty,Tz, S_in[0][i], S_in[1][i], S_out[0][i], S_out[1][i], S_morph[0][i], S_morph[1][i]); } +#endif } diff --git a/src/survive_process.c b/src/survive_process.c index b697f4a..f0928b4 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -19,7 +19,23 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode } //We don't use sync times, yet. - if( acode < -1 ) return; + if( sensor_id <= -1 ) + { + if( so->PoserFn ) + { + PoserDataLight l = { + .hdr = { .pt = POSERDATA_SYNC, }, + .sensor_id = sensor_id, + .acode = acode, + .timecode = timecode, + .length = length, + .angle = 0, + .lh = lh, + }; + so->PoserFn( so, (PoserData *)&l ); + } + return; + } if( base_station > NUM_LIGHTHOUSES ) return; -- cgit v1.2.3 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 From 805b5961274a50a171d0facd791429f6b8e04d63 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 21 Mar 2018 02:30:12 -0400 Subject: Make turvey's disambiguator send out sync pulses before all sweep happens. --- src/survive_data.c | 53 +++++++++++++++++++++++++++-------------------------- 1 file changed, 27 insertions(+), 26 deletions(-) (limited to 'src') diff --git a/src/survive_data.c b/src/survive_data.c index befcbe7..d8a26af 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -41,6 +41,7 @@ typedef struct typedef struct { double acode_offset; + int sent_out_ootx_bits; } lightcap2_global_data; typedef struct @@ -162,6 +163,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { lightcap2_data *lcd = so->disambiguator_data; + while(remove_outliers(so)); // look at all of the sensors we found, and process the ones that were hit. @@ -218,6 +220,30 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) //printf("%4d\n", lcd->sweep.sweep_len[i]); int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + + // first, send out the sync pulse data for the last round (for OOTX decoding + if( !lcd->global.sent_out_ootx_bits ) { + if (lcd->per_sweep.lh_max_pulse_length[0] != 0) + { + so->ctx->lightproc( so, -1, + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]), + lcd->per_sweep.lh_max_pulse_length[0], + lcd->per_sweep.lh_start_time[0], + 0, + 0); + } + if (lcd->per_sweep.lh_max_pulse_length[1] != 0) + { + so->ctx->lightproc( so, -2, + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]), + lcd->per_sweep.lh_max_pulse_length[1], + lcd->per_sweep.lh_start_time[1], + 0, + 1); + } + lcd->global.sent_out_ootx_bits = 1; + } + // if (offset_from < 380000 && offset_from > 70000) { //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) @@ -329,32 +355,8 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) //this should probably be fixed. Maybe some kind of timing based guess at which lighthouse. // looks like this is the first sync pulse. Cool! + lcd->global.sent_out_ootx_bits = 0; - // first, send out the sync pulse data for the last round (for OOTX decoding - { - if (lcd->per_sweep.lh_max_pulse_length[0] != 0) - { - so->ctx->lightproc( - so, - -1, - handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]), - lcd->per_sweep.lh_max_pulse_length[0], - lcd->per_sweep.lh_start_time[0], - 0, - 0); - } - if (lcd->per_sweep.lh_max_pulse_length[1] != 0) - { - so->ctx->lightproc( - so, - -2, - handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]), - lcd->per_sweep.lh_max_pulse_length[1], - lcd->per_sweep.lh_start_time[1], - 0, - 1); - } - } //fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n"); // initialize here. @@ -384,7 +386,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } */ } - // printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse ); } -- cgit v1.2.3 From be3fa4562f9578472de1ded5588df8dc502898c6 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 00:53:46 -0600 Subject: Tweaked thresholds, played with single sweep solver --- src/poser_sba.c | 186 +++++++++++++++++++++++++++++++++++---- src/survive_sensor_activations.c | 6 +- 2 files changed, 171 insertions(+), 21 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index 44af9c1..a29dffe 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -22,6 +22,12 @@ typedef struct { SurviveObject *so; } sba_context; +typedef struct { + sba_context hdr; + int acode; + int lh; +} sba_context_single_sweep; + void metric_function(int j, int i, double *aj, double *xij, void *adata) { sba_context *ctx = (sba_context *)(adata); SurviveObject *so = ctx->so; @@ -51,6 +57,26 @@ size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char * return measCount; } +size_t construct_input_from_scene_single_sweep(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene, + char *vmask, double *meas, int acode, int lh) { + size_t rtn = 0; + + for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { + const uint32_t *data_timecode = scene->timecode[sensor][lh]; + if (pdl->timecode - data_timecode[acode & 1] <= SurviveSensorActivations_default_tolerance) { + double *a = scene->angles[sensor][lh]; + vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; + meas[rtn++] = a[acode & 0x1]; + } else { + vmask[sensor * NUM_LIGHTHOUSES + lh] = 0; + + } + } + + return rtn; +} + + size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask, double *meas) { size_t rtn = 0; @@ -89,6 +115,30 @@ void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_po } void *GetDriver(const char *name); +void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + + sba_context_single_sweep *ctx = (sba_context_single_sweep *)(adata); + SurviveObject *so = ctx->hdr.so; + int lh = ctx->lh; + int acode = ctx->acode; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + quatnormalize(obj.Rot, obj.Rot); + FLT xyz[3]; + ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]); + + // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + + FLT out[2]; + survive_reproject_from_pose_with_config(so->ctx, &ctx->hdr.calibration_config, lh, camera, xyz, out); + *xij = out[acode]; +} + void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { SurvivePose obj = *(SurvivePose *)bi; int sensor_idx = j >> 1; @@ -109,13 +159,116 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); } + + +static double run_sba_find_3d_structure_single_sweep(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, + SurviveSensorActivations *scene, + int acode, int lh, + int max_iterations /* = 50*/, + double max_reproj_error /* = 0.005*/) { + double *covx = 0; + + char *vmask = alloca(sizeof(char) * so->sensor_ct); + double *meas = alloca(sizeof(double) * so->sensor_ct); + size_t meas_size = construct_input_from_scene_single_sweep(so, pdl, scene, vmask, meas, acode, lh); + + static int failure_count = 500; + if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) { + if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { + SurviveContext *ctx = so->ctx; + SV_INFO("Can't solve for position with just %lu measurements", meas_size); + failure_count = 0; + } + return -1; + } + failure_count = 0; + + SurvivePose soLocation = so->OutPose; + bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]); + + { + const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); + PoserCB driver = (PoserCB)GetDriver(subposer); + SurviveContext *ctx = so->ctx; + if (driver) { + PoserData hdr = pdl->hdr; + memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions + pdl->hdr.pt = hdr.pt; + pdl->hdr.rawposeproc = sba_set_position; + + sba_set_position_t locations = {}; + pdl->hdr.userdata = &locations; + driver(so, &pdl->hdr); + pdl->hdr = hdr; + + if (locations.hasInfo == false) { + + return -1; + } else if (locations.hasInfo) { + soLocation = locations.poses; + } + } else { + SV_INFO("Not using a seed poser for SBA; results will likely be way off"); + } + } + + double opts[SBA_OPTSSZ] = {}; + double info[SBA_INFOSZ] = {}; + + sba_context_single_sweep ctx = { + .hdr = { options, &pdl->hdr, so }, + .acode = acode, + .lh = lh + }; + + opts[0] = SBA_INIT_MU; + opts[1] = SBA_STOP_THRESH; + opts[2] = SBA_STOP_THRESH; + opts[3] = SBA_STOP_THRESH; + opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); + opts[4] = 0.0; + + int status = sba_str_levmar(1, // Number of 3d points + 0, // Number of 3d points to fix in spot + so->sensor_ct, vmask, + soLocation.Pos, // Reads as the full pose though + 7, // pnp -- SurvivePose + meas, // x* -- measurement data + 0, // cov data + 1, // mnp -- 2 points per image + str_metric_function_single_sweep, + 0, // jacobia of metric_func + &ctx, // user data + max_iterations, // Max iterations + 0, // verbosity + opts, // options + info); // info + + if (status > 0) { + quatnormalize(soLocation.Rot, soLocation.Rot); + PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); + + SurviveContext *ctx = so->ctx; + // Docs say info[0] should be divided by meas; I don't buy it really... + static int cnt = 0; + if (cnt++ > 1000 || meas_size < 8) { + SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); + cnt = 0; + } + } + + return info[1] / meas_size * 2; +} + + static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, SurviveSensorActivations *scene, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; - char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); - double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); + char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); + double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas); static int failure_count = 500; @@ -125,8 +278,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose SV_INFO("Can't solve for position with just %lu measurements", meas_size); failure_count = 0; } - free(vmask); - free(meas); return -1; } failure_count = 0; @@ -150,8 +301,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose pdl->hdr = hdr; if (locations.hasInfo == false) { - free(vmask); - free(meas); return -1; } else if (locations.hasInfo) { @@ -206,9 +355,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose } } - free(vmask); - free(meas); - return info[1] / meas_size * 2; } @@ -216,8 +362,8 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; - char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); - double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); + char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); + double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); size_t meas_size = construct_input(so, pdfs, vmask, meas); SurvivePose camera_params[2] = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}; @@ -282,9 +428,6 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd // Docs say info[0] should be divided by meas; I don't buy it really... // std::cerr << info[0] / meas.size() * 2 << " original reproj error" << std::endl; - free(vmask); - free(meas); - { SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... @@ -299,11 +442,18 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { switch (pd->pt) { case POSERDATA_LIGHT: { SurviveSensorActivations *scene = &so->activations; - PoserDataLight *lightData = (PoserDataLight *)pd; - - survive_calibration_config config = *survive_calibration_default_config(); - FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5); +/* + static int last_acode = -1; + static int last_lh = -1; + if(last_lh != lightData->lh || last_acode != lightData->acode) { + */ + survive_calibration_config config = *survive_calibration_default_config(); + FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5); + /*} + last_lh = lightData->lh; + last_acode = lightData->acode; + */ return 0; } case POSERDATA_FULL_SCENE: { diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index 42de833..d14bbec 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -3,7 +3,8 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance, uint32_t timecode_now, uint32_t idx, int lh) { const uint32_t *data_timecode = self->timecode[idx][lh]; - return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance); + return !(timecode_now - data_timecode[0] > tolerance || + timecode_now - data_timecode[1] > tolerance); } void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData) { @@ -15,5 +16,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa *data_timecode = lightData->timecode; } -// Roughly 31ms at a 48mhz clock rate -uint32_t SurviveSensorActivations_default_tolerance = 500000; \ No newline at end of file +uint32_t SurviveSensorActivations_default_tolerance = (uint32_t) (48000000/*mhz*/ * (16.7 * 2/*ms*/) / 1000); \ No newline at end of file -- cgit v1.2.3