diff options
| -rw-r--r-- | data_recorder.c | 4 | ||||
| -rw-r--r-- | include/libsurvive/poser.h | 22 | ||||
| -rw-r--r-- | include/libsurvive/survive.h | 10 | ||||
| -rw-r--r-- | include/libsurvive/survive_types.h | 3 | ||||
| -rw-r--r-- | src/poser.c | 61 | ||||
| -rw-r--r-- | src/poser_charlesslow.c | 3 | ||||
| -rw-r--r-- | src/poser_daveortho.c | 254 | ||||
| -rw-r--r-- | src/poser_epnp.c | 64 | ||||
| -rw-r--r-- | src/poser_sba.c | 318 | ||||
| -rw-r--r-- | src/poser_turveytori.c | 24 | ||||
| -rwxr-xr-x | src/survive_cal.c | 27 | ||||
| -rw-r--r-- | src/survive_data.c | 31 | ||||
| -rw-r--r-- | src/survive_process.c | 48 | ||||
| -rw-r--r-- | src/survive_sensor_activations.c | 16 | ||||
| -rw-r--r-- | test.c | 4 | ||||
| -rw-r--r-- | tools/viz/survive_viewer.js | 18 |
16 files changed, 524 insertions, 383 deletions
diff --git a/data_recorder.c b/data_recorder.c index a8e012d..7e5384a 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -62,8 +62,8 @@ int my_config_process(SurviveObject *so, char *ct0conf, int len) { return survive_default_htc_config_process(so, ct0conf, len); } -void my_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose) { - survive_default_lighthouse_pose_process(ctx, lighthouse, lh_pose); +void my_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, SurvivePose *obj) { + survive_default_lighthouse_pose_process(ctx, lighthouse, lh_pose, obj); write_to_output("%d LH_POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, lh_pose->Pos[0], lh_pose->Pos[1], lh_pose->Pos[2], lh_pose->Rot[0], lh_pose->Rot[1], lh_pose->Rot[2], lh_pose->Rot[3]); diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index ccd36ce..b0b1a7b 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -8,18 +8,19 @@ extern "C" { #endif -typedef enum PoserType_t -{ +typedef enum PoserType_t { POSERDATA_NONE = 0, POSERDATA_IMU, - POSERDATA_LIGHT, //Single lighting event. - POSERDATA_FULL_SCENE, //Full, statified X, Y sweep data for both lighthouses. - POSERDATA_DISASSOCIATE, //If you get this, it doesn't contain data. It just tells you to please disassociate from the current SurviveObject and delete your poserdata. - POSERDATA_SYNC, //Sync pulse. + POSERDATA_LIGHT, // Single lighting event. + POSERDATA_FULL_SCENE, // Full, statified X, Y sweep data for both lighthouses. + POSERDATA_DISASSOCIATE, // If you get this, it doesn't contain data. It just tells you to please disassociate from + // the current SurviveObject and delete your poserdata. + POSERDATA_SYNC, // Sync pulse. } PoserType; typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user); -typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user); +typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose, void *user); typedef struct { @@ -30,10 +31,11 @@ typedef struct } PoserData; void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); -void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); +void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, + /* OUTPARAM */ SurvivePose *objUp2world, SurvivePose *lighthouse_poses, + SurvivePose *object_pose); -typedef struct -{ +typedef struct PoserDataIMU { PoserData hdr; uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present. FLT accel[3]; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 180d83c..747d076 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -16,14 +16,21 @@ extern "C" { typedef struct { FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes (Angles in LH space) uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks + + FLT accel[3]; + FLT gyro[3]; + FLT mag[3]; } SurviveSensorActivations; struct PoserDataLight; +struct PoserDataIMU; /** * Adds a lightData packet to the table. */ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData); +void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData); + /** * Returns true iff both angles for the given sensor and lighthouse were seen at most `tolerance` ticks before the given * `timecode_now`. @@ -252,7 +259,8 @@ void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); -void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose); +void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, + SurvivePose *obj_pose); int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 5f5dd1f..7253df0 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -53,7 +53,8 @@ typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); typedef void (*raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); -typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose); +typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose); typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); diff --git a/src/poser.c b/src/poser.c index ec7865e..1ed63da 100644 --- a/src/poser.c +++ b/src/poser.c @@ -1,7 +1,7 @@ +#include "math.h" +#include <linmath.h> #include <stdint.h> - -#include "poser.h" -#include "survive_internal.h" +#include <survive.h> void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { if (poser_data->rawposeproc) { @@ -11,10 +11,59 @@ void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uin } } -void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { +void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, + SurvivePose *objUp2world, SurvivePose *lighthouse_pose, SurvivePose *object_pose) { if (poser_data->lighthouseposeproc) { - poser_data->lighthouseposeproc(so, lighthouse, pose, poser_data->userdata); + poser_data->lighthouseposeproc(so, lighthouse, lighthouse_pose, object_pose, poser_data->userdata); } else { - so->ctx->lighthouseposeproc(so->ctx, lighthouse, pose); + const FLT up[3] = {0, 0, 1}; + + // Assume that the space solved for is valid but completely arbitrary. We are going to do a few things: + // a) Using the gyro data, normalize it so that gravity is pushing straight down along Z + // c) Assume the object is at origin + // b) Place the first lighthouse on the X axis by rotating around Z + // + // This calibration setup has the benefit that as long as the user is calibrating on the same flat surface, + // calibration results will be roughly identical between all posers no matter the orientation the object is + // lying + // in. + // + // 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 = {.Rot = {1.}}; + if (object_pose) + object2arb = *object_pose; + SurvivePose lighthouse2arb = *lighthouse_pose; + + // Start by just moving from whatever arbitrary space into object space. + SurvivePose arb2object; + InvertPose(arb2object.Pos, object2arb.Pos); + + SurvivePose lighthouse2obj; + ApplyPoseToPose(lighthouse2obj.Pos, arb2object.Pos, lighthouse2arb.Pos); + + // Now find the space with the same origin, but rotated so that gravity is up + SurvivePose lighthouse2objUp = {}, object2objUp = {}; + quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); + + // Calculate the pose of the lighthouse in this space + ApplyPoseToPose(lighthouse2objUp.Pos, object2objUp.Pos, lighthouse2obj.Pos); + + // Purposefully only set this once. It should only depend on the first (calculated) lighthouse + if (quatmagnitude(objUp2world->Rot) == 0) { + // Find what angle we need to rotate about Z by to get to 90 degrees. + FLT ang = atan2(lighthouse2objUp.Pos[1], lighthouse2objUp.Pos[0]); + FLT euler[3] = {0, 0, M_PI / 2. - ang}; + + quatfromeuler(objUp2world->Rot, euler); + } + + // Find find the poses that map to the above + SurvivePose obj2world, lighthouse2world; + ApplyPoseToPose(obj2world.Pos, objUp2world->Pos, object2objUp.Pos); + ApplyPoseToPose(lighthouse2world.Pos, objUp2world->Pos, lighthouse2objUp.Pos); + + so->ctx->lighthouseposeproc(so->ctx, lighthouse, &lighthouse2world, &obj2world); } } diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index 3b781f4..c7d9033 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -53,6 +53,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) printf( "%f %f %f\n", hmd_points[p*3+0], hmd_points[p*3+1], hmd_points[p*3+2] ); } + SurvivePose additionalTx = {}; int lh, cycle; FLT dz, dy, dx; @@ -174,7 +175,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) quatrotateabout(tmp, lighthousePose.Rot, rt); memcpy(lighthousePose.Rot, tmp, sizeof(FLT) * 4); - PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose); + PoserData_lighthouse_pose_func(pd, so, lh, &additionalTx, &lighthousePose, 0); #define ALT_COORDS #ifdef ALT_COORDS diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e2e8d2e..769ce81 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -8,8 +8,7 @@ #include <dclapack.h> #include <linmath.h> -//Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s - +// 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 @@ -22,8 +21,8 @@ void OrthoSolve( typedef struct { int nextaxis; - float lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; - float angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + float lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + float angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //Stuff } DummyData; @@ -33,7 +32,8 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) SurviveContext * ctx = so->ctx; DummyData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = calloc( sizeof( DummyData ), 1 ); + if (!dd) + so->PoserData = dd = calloc(sizeof(DummyData), 1); switch( pt ) { @@ -46,80 +46,84 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)pd; - if( l->length > dd->lengths[l->sensor_id][l->lh][dd->nextaxis] ) - { + 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; + dd->angles[l->sensor_id][l->lh][dd->nextaxis] = l->angle; } break; } - case POSERDATA_SYNC: - { - PoserDataLight * l = (PoserDataLight*)pd; + case POSERDATA_SYNC: { + PoserDataLight *l = (PoserDataLight *)pd; int lhid = l->lh; dd->nextaxis = l->acode & 1; - if( dd->nextaxis == 0 && so->codename[0] == 'H' ) - { + 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 ) - { + 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]; + 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 ); + 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 ); + 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. + // Now, we need to solve where we are as a function of where + // the lighthouses are. - SurvivePose objpose; + 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] ); + // matrix44transpose( MT, &tOut[0][0] ); + matrix44copy(&MT[0][0], &tOut[0][0]); - quatfrommatrix( objpose.Rot, &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( objpose.Rot, objpose.Rot ); + // 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); - 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] ); + 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 ); + // 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; } @@ -127,8 +131,9 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) { PoserDataFullScene * fs = (PoserDataFullScene*)pd; int LH_ID; - printf( "PDFS\n" ); + printf("PDFS\n"); + SurvivePose alignLh0ToXAxis = {}; for( LH_ID = 0; LH_ID < 2; LH_ID++ ) { int i; @@ -155,9 +160,9 @@ 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] }; - SurvivePose objpose; + // 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]; @@ -167,23 +172,23 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) //matrix44transpose( MT, &tOut[0][0] ); matrix44copy( &MT[0][0], &tOut[0][0] ); - quatfrommatrix( objpose.Rot, &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( 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] ) ); - } + 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] = objpose.Pos[0]; @@ -195,13 +200,14 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) 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] ); + 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, &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] ); + 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; @@ -240,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 (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; +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 @@ -360,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; //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); + 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} }; @@ -402,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 == 0 && 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; } @@ -531,8 +540,9 @@ 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, and put into correct "world"-space. I.e. where is the object, relative to the lighthouse. - //------------------- +// 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. @@ -542,56 +552,60 @@ 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[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; + /* 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; #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 //------------------- - 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... + 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 @@ -617,12 +631,11 @@ PRINT(ab,2,1); // PRINT_MAT(T,4,4); - //Fix handedness for rotations... +// Fix handedness for rotations... // T[1][0]*=-1; // T[1][1]*=-1; // T[1][2]*=-1; - // PRINT_MAT(T,4,4); #if 0 @@ -689,6 +702,5 @@ PRINT(ab,2,1); // 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_epnp.c b/src/poser_epnp.c index dfe86ff..cf4294f 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -56,8 +56,49 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer return rtn; } +/* +static int survive_standardize_calibration(SurviveObject* so, + FLT upvec[3], + SurvivePose* _object2world, + SurvivePose* _lighthouses2world0, + SurvivePose* _lighthouses2world1) { + SurvivePose object2world = {}; + SurvivePose lighthouses2world0 = *_lighthouses2world0; + SurvivePose lighthouses2world1 = *_lighthouses2world1; + const FLT up[3] = {0, 0, 1}; + quatfrom2vectors(object2world.Rot, so->activations.accel, _object2world->Pos); + + FLT tx[4][4]; + quattomatrix(tx, object2world.Rot); + + SurvivePose additionalTx = {0}; + + SurvivePose lighthouse2world = {}; + // Lighthouse is now a tx from camera -> object + ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos); + + if(quatmagnitude(additionalTx.Rot) == 0) { + SurvivePose desiredPose = lighthouse2world; + desiredPose.Pos[0] = 0.; + + quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos); + } + SurvivePose finalTx = {}; + ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos); + +} +*/ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) { + SurvivePose object2world = {//.Rot = { 0.7325378, 0.4619398, 0.1913417, 0.4619398} + .Rot = {1.}}; + const FLT up[3] = {0, 0, 1}; + + SurvivePose actual; + // quatfrom2vectors(object2world.Rot, so->activations.accel, up); + // quatfrom2vectors(object2world.Rot, up, so->activations.accel); + + SurvivePose additionalTx = {0}; for (int lh = 0; lh < 2; lh++) { epnp pnp = {.fu = 1, .fv = 1}; @@ -80,11 +121,27 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) continue; } - SurvivePose lighthouse = solve_correspondence(so, &pnp, true); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &lighthouse); + SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true); + FLT euler[3]; + quattoeuler(euler, lighthouse2object.Rot); + SurvivePose lighthouse2world = {}; + // Lighthouse is now a tx from camera -> object + ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos); + + if (false && quatmagnitude(additionalTx.Rot) == 0) { + SurvivePose desiredPose = lighthouse2world; + desiredPose.Pos[0] = 0.; + + quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos); + } + SurvivePose finalTx = lighthouse2world; + // ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos); + + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &finalTx, &object2world); epnp_dtor(&pnp); } + return 0; } @@ -103,6 +160,8 @@ static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActiv } int PoserEPNP(SurviveObject *so, PoserData *pd) { + + SurviveSensorActivations *scene = &so->activations; switch (pd->pt) { case POSERDATA_IMU: { // Really should use this... @@ -110,7 +169,6 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { return 0; } case POSERDATA_LIGHT: { - SurviveSensorActivations *scene = &so->activations; PoserDataLight *lightData = (PoserDataLight *)pd; SurvivePose posers[2]; diff --git a/src/poser_sba.c b/src/poser_sba.c index a29dffe..b5962f8 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -20,20 +20,25 @@ typedef struct { survive_calibration_config calibration_config; PoserData *pdfs; SurviveObject *so; + SurvivePose obj_pose; + SurvivePose camera_params[2]; } sba_context; typedef struct { - sba_context hdr; - int acode; - int lh; + 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; - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, - &so->sensor_locations[i * 3], xij); + SurvivePose obj2world = ctx->obj_pose; + FLT sensorInWorld[3] = {}; + ApplyPoseToPoint(sensorInWorld, obj2world.Pos, &so->sensor_locations[i * 3]); + survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, + xij); } size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { @@ -57,26 +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; +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; + return rtn; } - size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask, double *meas) { size_t rtn = 0; @@ -97,9 +101,10 @@ size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, return rtn; } -void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user) { - SurvivePose *poses = (SurvivePose *)(user); - poses[lighthouse] = *pose; +void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose, void *user) { + sba_context *ctx = (sba_context *)user; + ctx->camera_params[lighthouse] = *pose; + ctx->obj_pose = *obj_pose; } typedef struct { @@ -116,27 +121,27 @@ 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; + 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; + 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); + 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]); + 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; + // 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]; + 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) { @@ -159,108 +164,100 @@ 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 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 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*/, @@ -283,7 +280,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose failure_count = 0; SurvivePose soLocation = so->OutPose; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]); + bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); @@ -358,6 +355,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose return info[1] / meas_size * 2; } +// Optimizes for LH position assuming object is posed at 0 static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; @@ -366,9 +364,10 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd 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}; + sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + .obj_pose = so->OutPose}; - if (true || so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0) { + { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; @@ -378,7 +377,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions pdfs->hdr.pt = hdr.pt; pdfs->hdr.lighthouseposeproc = sba_set_cameras; - pdfs->hdr.userdata = camera_params; + pdfs->hdr.userdata = &sbactx; driver(so, &pdfs->hdr); pdfs->hdr = hdr; } else { @@ -395,8 +394,6 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd double opts[SBA_OPTSSZ] = {}; double info[SBA_INFOSZ] = {}; - sba_context ctx = {options, &pdfs->hdr, so}; - opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; opts[2] = SBA_STOP_THRESH; @@ -408,22 +405,23 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd NUM_LIGHTHOUSES, // Number of cameras -- 2 lighthouses 0, // Number of cameras to not modify vmask, // boolean vis mask - (double *)&camera_params[0], // camera parameters + (double *)&sbactx.camera_params[0], // camera parameters sizeof(SurvivePose) / sizeof(double), // The number of floats that are in a camera param meas, // 2d points for 3d objs covx, // covariance of measurement. Null sets to identity 2, // 2 points per image metric_function, 0, // jacobia of metric_func - &ctx, // user data + &sbactx, // user data max_iterations, // Max iterations 0, // verbosity opts, // options info); // info if (status >= 0) { - PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &camera_params[0]); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &camera_params[1]); + SurvivePose additionalTx = {}; + PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &additionalTx, &sbactx.camera_params[0], &sbactx.obj_pose); + PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &additionalTx, &sbactx.camera_params[1], &sbactx.obj_pose); } // 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; @@ -443,17 +441,17 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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; - */ + /* + 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/poser_turveytori.c b/src/poser_turveytori.c index db8d638..2d3f802 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1235,7 +1235,7 @@ void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh) } static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, PoserData *pd, - char doLogOutput, const int lh, const int setLhCalibration) { + char doLogOutput, SurvivePose *additionalTx, const int lh, const int setLhCalibration) { ToriData *toriData = so->PoserData; //printf("Solving for Lighthouse\n"); @@ -1443,7 +1443,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob lighthousePose.Pos[1] = refinedEstimateGd.y; lighthousePose.Pos[2] = refinedEstimateGd.z; - PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose); + SurvivePose assumedObj = {}; + FLT negZ[3] = {0, 0, 1}; + quatfrom2vectors(assumedObj.Rot, toriData->down, negZ); + + PoserData_lighthouse_pose_func(pd, so, lh, additionalTx, &lighthousePose, &assumedObj); } @@ -1528,7 +1532,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob return refinedEstimateGd; } -static void QuickPose(SurviveObject *so, PoserData *pd, int lh) { +static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalTx, int lh) { ToriData * td = so->PoserData; @@ -1622,7 +1626,7 @@ static void QuickPose(SurviveObject *so, PoserData *pd, int lh) { // SolveForLighthouse(pos, quat, to, so, 0, lh, 0); //} - SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, lh, 0); + SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, lh, 0); //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); if (so->ctx->rawposeproc) @@ -1693,6 +1697,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } int axis = l->acode & 0x1; //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 ); + + SurvivePose additionalTx; if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { @@ -1705,7 +1711,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 4 == 0) - QuickPose(so, poserData, 0); + QuickPose(so, poserData, &additionalTx, 0); } if (1 == l->lh && axis) // only once per full cycle... { @@ -1715,7 +1721,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 4 == 0) - QuickPose(so, poserData, 1); + QuickPose(so, poserData, &additionalTx, 1); } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; @@ -1755,7 +1761,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT axis[3]; angleaxisfrom2vect(&angle, axis, td->down, negZ); //angleaxisfrom2vect(&angle, &axis, negZ, td->down); - + SurvivePose additionalTx; { int sensorCount = 0; @@ -1790,7 +1796,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT pos[3], quat[4]; - SolveForLighthouse(pos, quat, to, so, poserData, 0, 0, 1); + SolveForLighthouse(pos, quat, to, so, poserData, 0, &additionalTx, 0, 1); } { int sensorCount = 0; @@ -1826,7 +1832,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT pos[3], quat[4]; - SolveForLighthouse(pos, quat, to, so, poserData, 0, 1, 1); + SolveForLighthouse(pos, quat, to, so, poserData, 0, &additionalTx, 1, 1); } diff --git a/src/survive_cal.c b/src/survive_cal.c index 2d97317..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, survive_configs( ctx, "configfile", SC_GET, "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 = survive_configs( ctx, "requiredtrackersforcal", SC_SETCONFIG, "" ); - const uint32_t AllowAllTrackersForCal = survive_configi( ctx, "allowalltrackersforcal", SC_SETCONFIG, 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++) @@ -335,20 +335,17 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin 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++ ) - { + // 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 ) + 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 ); + 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 d8a26af..c586658 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -163,7 +163,6 @@ 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. @@ -220,31 +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; - // 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->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); + 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 (offset_from < 380000 && offset_from > 70000) { //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { @@ -357,7 +347,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // looks like this is the first sync pulse. Cool! lcd->global.sent_out_ootx_bits = 0; - //fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n"); // initialize here. memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); diff --git a/src/survive_process.c b/src/survive_process.c index f0928b4..1402dab 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -19,12 +19,13 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode } //We don't use sync times, yet. - if( sensor_id <= -1 ) - { - if( so->PoserFn ) - { + if (sensor_id <= -1) { + if (so->PoserFn) { PoserDataLight l = { - .hdr = { .pt = POSERDATA_SYNC, }, + .hdr = + { + .pt = POSERDATA_SYNC, + }, .sensor_id = sensor_id, .acode = acode, .timecode = timecode, @@ -32,7 +33,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode .angle = 0, .lh = lh, }; - so->PoserFn( so, (PoserData *)&l ); + so->PoserFn(so, (PoserData *)&l); } return; } @@ -131,9 +132,10 @@ void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, Sur so->FromLHPose[lighthouse] = *pose; } -void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { - if (pose) { - ctx->bsd[lighthouse].Pose = *pose; +void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose) { + if (lighthouse_pose) { + ctx->bsd[lighthouse].Pose = *lighthouse_pose; ctx->bsd[lighthouse].PositionSet = 1; } else { ctx->bsd[lighthouse].PositionSet = 0; @@ -148,19 +150,21 @@ int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len } void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { - if( so->PoserFn ) - { - PoserDataIMU imu = { - .hdr = - { - .pt = POSERDATA_IMU, - }, - .datamask = mask, - .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]}, - .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]}, - .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]}, - .timecode = timecode, - }; + PoserDataIMU imu = { + .hdr = + { + .pt = POSERDATA_IMU, + }, + .datamask = mask, + .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]}, + .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]}, + .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]}, + .timecode = timecode, + }; + + SurviveSensorActivations_add_imu(&so->activations, &imu); + + if (so->PoserFn) { so->PoserFn( so, (PoserData *)&imu ); } } diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index d14bbec..4d1801c 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -3,10 +3,20 @@ 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_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData) { + for (int i = 0; i < 3; i++) { + self->accel[i] = .98 * self->accel[i] + .02 * imuData->accel[i]; + } + for (int i = 0; i < 3; i++) { + self->gyro[i] = .98 * self->gyro[i] + .02 * imuData->gyro[i]; + } + for (int i = 0; i < 3; i++) { + self->mag[i] = .98 * self->mag[i] + .02 * imuData->mag[i]; + } +} void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData) { int axis = (lightData->acode & 1); uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis]; @@ -16,4 +26,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa *data_timecode = lightData->timecode; } -uint32_t SurviveSensorActivations_default_tolerance = (uint32_t) (48000000/*mhz*/ * (16.7 * 2/*ms*/) / 1000);
\ 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 @@ -84,8 +84,8 @@ void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t butt } } -void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { - survive_default_lighthouse_pose_process(ctx, lighthouse, pose); +void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose, obj_pose); printf("Lighthouse: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 3aa2cf7..ab6a200 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -280,18 +280,21 @@ $(function() { create_object(obj); } else if (obj.type === "imu") { if (objs[obj.tracker]) { - if (!downAxes[obj.tracker]) { + if (!downAxes[obj.tracker] && objs[obj.tracker]) { downAxes[obj.tracker] = new THREE.Geometry(); - downAxes[obj.tracker].vertices.push( - new THREE.Vector3(0, 0, 0), - new THREE.Vector3(obj.accelgyro[0], obj.accelgyro[1], obj.accelgyro[2])); + downAxes[obj.tracker].vertices.push(new THREE.Vector3(0, 0, 0), new THREE.Vector3(0, 0, 0)); var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff})); - objs[obj.tracker].add(line); - } else { + scene.add(line); + } + + if (objs[obj.tracker].position) { var q = obj.accelgyro; + + downAxes[obj.tracker].vertices[0] = objs[obj.tracker].position; downAxes[obj.tracker].vertices[1].fromArray(q); + downAxes[obj.tracker].vertices[1].add(objs[obj.tracker].position); downAxes[obj.tracker].verticesNeedUpdate = true; } } @@ -382,6 +385,9 @@ init() { var skyBoxMaterial = new THREE.MeshBasicMaterial({color : 0x888888, side : THREE.BackSide}); var skyBox = new THREE.Mesh(skyBoxGeometry, skyBoxMaterial); scene.add(skyBox); + + var axes = new THREE.AxesHelper(5); + scene.add(axes); } function animate() { |