diff options
| -rw-r--r-- | include/libsurvive/poser.h | 10 | ||||
| -rw-r--r-- | include/libsurvive/survive.h | 10 | ||||
| -rw-r--r-- | include/libsurvive/survive_types.h | 3 | ||||
| -rw-r--r-- | src/poser.c | 59 | ||||
| -rw-r--r-- | src/poser_charlesslow.c | 3 | ||||
| -rw-r--r-- | src/poser_epnp.c | 64 | ||||
| -rw-r--r-- | src/poser_sba.c | 38 | ||||
| -rw-r--r-- | src/poser_turveytori.c | 24 | ||||
| -rw-r--r-- | src/survive_process.c | 35 | ||||
| -rw-r--r-- | src/survive_sensor_activations.c | 11 | ||||
| -rw-r--r-- | test.c | 4 | ||||
| -rw-r--r-- | tools/viz/survive_viewer.js | 18 |
12 files changed, 215 insertions, 64 deletions
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 6c74c52..6b5b09d 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -18,7 +18,8 @@ typedef enum PoserType_t } 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 { @@ -29,10 +30,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..e52edd2 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,57 @@ 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 = *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_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 44af9c1..fa71dd2 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -20,14 +20,19 @@ typedef struct { survive_calibration_config calibration_config; PoserData *pdfs; SurviveObject *so; + SurvivePose obj_pose; + SurvivePose camera_params[2]; } sba_context; 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) { @@ -71,9 +76,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 { @@ -109,6 +115,7 @@ 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(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, SurviveSensorActivations *scene, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { @@ -132,7 +139,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"); @@ -212,6 +219,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; @@ -220,9 +228,10 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd double *meas = malloc(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; @@ -232,7 +241,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 { @@ -249,8 +258,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; @@ -262,22 +269,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; 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_process.c b/src/survive_process.c index b697f4a..a49b632 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -115,9 +115,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; @@ -132,19 +133,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 42de833..dce353c 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -6,6 +6,17 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, 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]; @@ -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() { |