diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/poser.c | 4 | ||||
-rw-r--r-- | src/poser_daveortho.c | 293 | ||||
-rw-r--r-- | src/poser_sba.c | 174 | ||||
-rwxr-xr-x | src/survive_cal.c | 37 | ||||
-rw-r--r-- | src/survive_data.c | 46 | ||||
-rw-r--r-- | src/survive_process.c | 19 | ||||
-rw-r--r-- | src/survive_sensor_activations.c | 3 |
7 files changed, 435 insertions, 141 deletions
diff --git a/src/poser.c b/src/poser.c index e52edd2..1ed63da 100644 --- a/src/poser.c +++ b/src/poser.c @@ -31,7 +31,9 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui // We might want to go a step further and affix the first lighthouse in a given pose that preserves up so that // it doesn't matter where on that surface the object is. - SurvivePose object2arb = *object_pose; + SurvivePose object2arb = {.Rot = {1.}}; + if (object_pose) + object2arb = *object_pose; SurvivePose lighthouse2arb = *lighthouse_pose; // Start by just moving from whatever arbitrary space into object space. diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 2bf57c6..769ce81 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -8,7 +8,7 @@ #include <dclapack.h> #include <linmath.h> -static int LH_ID; +// Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s void OrthoSolve( FLT T[4][4], // OUTPUT: 4x4 transformation matrix @@ -20,7 +20,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 +32,8 @@ 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,13 +46,94 @@ 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 ); + 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: { + PoserDataLight *l = (PoserDataLight *)pd; + int lhid = l->lh; + + dd->nextaxis = l->acode & 1; + + if (dd->nextaxis == 0) { + 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 > 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. + + 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(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(objpose.Rot, objpose.Rot); + + SurvivePose poseout; + InvertPose(poseout.Pos, objpose.Pos); + SurvivePose pose2lh = objpose; + // SurvivePose pose2lh = poseout; + + SurvivePose lh2world = so->ctx->bsd[lhid].Pose; + SurvivePose world2obj, obj2world; + ApplyPoseToPose(world2obj.Pos, lh2world.Pos, pose2lh.Pos); + InvertPose(obj2world.Pos, world2obj.Pos); + PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); + + if (0) { + 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 ); + break; } case POSERDATA_FULL_SCENE: { PoserDataFullScene * fs = (PoserDataFullScene*)pd; + int LH_ID; + printf("PDFS\n"); + SurvivePose alignLh0ToXAxis = {}; for( LH_ID = 0; LH_ID < 2; LH_ID++ ) { int i; @@ -76,38 +160,54 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) //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] ); - - 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] ) ); + 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]); + + 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] = 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]; + /* + 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]); + + PoserData_lighthouse_pose_func(&fs->hdr, so, LH_ID, &alignLh0ToXAxis, &poseout, 0); + 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; @@ -146,14 +246,12 @@ REGISTER_LINKTIME( PoserDaveOrtho ); oy=(c)*(x)-(a)*(z); \ oz=(a)*(y)-(b)*(x); } -void OrthoSolve( - FLT T[4][4], // OUTPUT: 4x4 transformation matrix - FLT S_out[2][SENSORS_PER_OBJECT], // OUTPUT: array of screenspace points - 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) -{ - int i,j,k; +void OrthoSolve(FLT T[4][4], // OUTPUT: 4x4 transformation matrix + 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) { + int i,j,k; FLT R[3][3]; // OUTPUT: 3x3 rotation matrix FLT trans[3]; // INPUT: x,y,z translation vector @@ -266,11 +364,11 @@ 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); -// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, 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); - //-------------------- + //-------------------- // Rescale the axies to be of the proper length //-------------------- FLT x[3][1] = { {M[0][0]*ydist}, {0.0}, {M[1][0]*ydist} }; @@ -308,9 +406,14 @@ 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; } - z_y = -z_y; + 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; } @@ -437,60 +540,104 @@ 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 + + /* 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[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; - - FLT T2[4][4]; +#endif //------------------- // 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); -#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[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] ); //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] ); #endif - normalize3d( &T[0][0], &T[0][0] ); - normalize3d( &T[1][0], &T[1][0] ); - normalize3d( &T[2][0], &T[2][0] ); - //Change handedness +// printf( " In Axis on headset \n" ); +// printf( " x y z\n" ); +// PRINT_MAT(T,4,4); - T[1][0]*=-1; - T[1][1]*=-1; - T[1][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); + +// Fix handedness for rotations... +// T[1][0]*=-1; +// T[1][1]*=-1; +// T[1][2]*=-1; // PRINT_MAT(T,4,4); +#if 0 #if 1 @@ -554,6 +701,6 @@ 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/poser_sba.c b/src/poser_sba.c index fa71dd2..b5962f8 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -24,6 +24,12 @@ typedef struct { SurvivePose camera_params[2]; } 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; @@ -56,6 +62,25 @@ 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; @@ -95,6 +120,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; @@ -115,14 +164,108 @@ 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); } -// Optimizes for object assuming given LH +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; @@ -132,8 +275,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; @@ -157,8 +298,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) { @@ -213,9 +352,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose } } - free(vmask); - free(meas); - return info[1] / meas_size * 2; } @@ -224,8 +360,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); sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, @@ -290,9 +426,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... @@ -307,11 +440,18 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { switch (pd->pt) { case POSERDATA_LIGHT: { SurviveSensorActivations *scene = &so->activations; - PoserDataLight *lightData = (PoserDataLight *)pd; - + /* + 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_cal.c b/src/survive_cal.c index cb242ae..3367aa0 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")); } } @@ -138,9 +138,9 @@ 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 = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1"); + 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,21 @@ 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++ ) - { - 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; + // 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++) { + 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; + } } } diff --git a/src/survive_data.c b/src/survive_data.c index befcbe7..c586658 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 @@ -218,7 +219,22 @@ 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; -// if (offset_from < 380000 && offset_from > 70000) + // 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 +345,7 @@ 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! - - // 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); - } - } + lcd->global.sent_out_ootx_bits = 0; //fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n"); // initialize here. @@ -384,7 +375,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } */ } - // printf("%d %d\n", acode, lcd->per_sweep.activeLighthouse ); } diff --git a/src/survive_process.c b/src/survive_process.c index a49b632..1402dab 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -19,7 +19,24 @@ 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; diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index dce353c..4d1801c 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -26,5 +26,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 |