diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 137c605..572b8e9 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -350,6 +350,7 @@ enum SurviveCalFlag { SVCal_All = SVCal_Gib | SVCal_Curve | SVCal_Tilt | SVCal_Phase }; +struct SurviveContext_private; struct SurviveContext { int lh_version_configed; int lh_version_forced; @@ -400,7 +401,7 @@ struct SurviveContext { struct config_group *temporary_config_values; // Set per-session, from command-line. Not saved but override global_config_values // Additional details that we don't want / need to expose to every single include - void *private_members; + struct SurviveContext_private *private_members; bool request_floor_set; FLT floor_offset; @@ -647,6 +648,7 @@ SURVIVE_EXPORT void survive_default_new_object_process(SurviveObject *so); SURVIVE_EXPORT double survive_run_time(const SurviveContext *ctx); SURVIVE_EXPORT double survive_run_time_since_epoch(const SurviveContext *ctx); +SURVIVE_EXPORT const SurvivePose* survive_external_to_world(const SurviveContext *ctx); SURVIVE_EXPORT size_t survive_input_event_count(const SurviveContext *ctx); ////////////////////// Survive Drivers //////////////////////////// diff --git a/redist/linmath.h b/redist/linmath.h index 463f62e..7ef6a47 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -265,12 +265,12 @@ LINMATH_EXPORT void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in); // which transforms from B to A. // // This assumes that the space A and B share an origin. -LINMATH_EXPORT void KabschCentered(LinmathQuat B2Atx, const FLT *ptsA, const FLT *ptsB, int num_pts); -LINMATH_EXPORT void KabschCenteredScaled(LinmathQuat B2Atx, FLT *scale, const FLT *ptsA, const FLT *ptsB, int num_pts); +LINMATH_EXPORT void KabschCentered(LinmathQuat A2Btx, const FLT *ptsA, const FLT *ptsB, int num_pts); +LINMATH_EXPORT void KabschCenteredScaled(LinmathQuat A2Btx, FLT *scale, const FLT *ptsA, const FLT *ptsB, int num_pts); // Same as above except it solves for the center for you -LINMATH_EXPORT void Kabsch(LinmathPose *B2Atx, const FLT *ptsA, const FLT *ptsB, int num_pts); -LINMATH_EXPORT void KabschScaled(LinmathPose *B2Atx, FLT *scale, const FLT *ptsA, const FLT *ptsB, int num_pts); +LINMATH_EXPORT void Kabsch(LinmathPose *A2Btx, const FLT *ptsA, const FLT *ptsB, int num_pts); +LINMATH_EXPORT void KabschScaled(LinmathPose *A2Btx, FLT *scale, const FLT *ptsA, const FLT *ptsB, int num_pts); // Matrix Stuff typedef struct { diff --git a/redist/lintest.c b/redist/lintest.c index 207b25d..d2d90b6 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -120,29 +120,29 @@ void testApplyPoseToPoint() { void testKabsch() { FLT pts[] = {0, 0, 0, 100, 100, 100, 10, 0, 10, 50, 50, 0, 0, 0, 1000, -100, 0, 100}; - LinmathPose tx = {.Pos = { 0 }, .Rot = {4, 3, 2, 1}}; + LinmathPose pts2txPts = {.Pos = { 0 }, .Rot = {4, 3, 2, 1}}; - LinmathPose tx2 = {.Pos = {1, 2, 3}, .Rot = {1, 2, 3, 4}}; + LinmathPose pts2tx2Pts = {.Pos = {1, 2, 3}, .Rot = {1, 2, 3, 4}}; - quatnormalize(tx.Rot, tx.Rot); - quatnormalize(tx2.Rot, tx2.Rot); + quatnormalize(pts2txPts.Rot, pts2txPts.Rot); + quatnormalize(pts2tx2Pts.Rot, pts2tx2Pts.Rot); const int N = sizeof(pts) / sizeof(FLT) / 3; FLT *txPts = alloca(N * 3 * sizeof(FLT)); FLT *txPts2 = alloca(N * 3 * sizeof(FLT)); for (int i = 0; i < N; i++) { - ApplyPoseToPoint(txPts + i * 3, &tx, pts + i * 3); - ApplyPoseToPoint(txPts2 + i * 3, &tx2, pts + i * 3); + ApplyPoseToPoint(txPts + i * 3, &pts2txPts, pts + i * 3); + ApplyPoseToPoint(txPts2 + i * 3, &pts2tx2Pts, pts + i * 3); } LinmathQuat should_be_tx = { 0 }; KabschCentered(should_be_tx, pts, txPts, N); - ASSERT_FLTA_EQUALS(should_be_tx, tx.Rot, 4); + ASSERT_FLTA_EQUALS(should_be_tx, pts2txPts.Rot, 4); LinmathPose should_be_tx2 = { 0 }; Kabsch(&should_be_tx2, pts, txPts2, N); - ASSERT_FLTA_EQUALS(should_be_tx2.Pos, tx2.Pos, 7); + ASSERT_FLTA_EQUALS(should_be_tx2.Pos, pts2tx2Pts.Pos, 7); } static void testKabsch2() { diff --git a/src/driver_playback.c b/src/driver_playback.c index e268cea..c2eb233 100644 --- a/src/driver_playback.c +++ b/src/driver_playback.c @@ -244,8 +244,9 @@ static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver, bool static int parse_and_run_lhpose(const char *line, struct SurvivePlaybackData *driver) { SurvivePose pose; int lh = -1; - int rr = sscanf(line, "%d LH_POSE " SurvivePose_sformat "\n", &lh, &pose.Pos[0], &pose.Pos[1], &pose.Pos[2], - &pose.Rot[0], &pose.Rot[1], &pose.Rot[2], &pose.Rot[3]); + uint32_t basestationId = 0; + int rr = sscanf(line, "%d LH_POSE " SurvivePose_sformat "%u\n", &lh, &pose.Pos[0], &pose.Pos[1], &pose.Pos[2], + &pose.Rot[0], &pose.Rot[1], &pose.Rot[2], &pose.Rot[3], &basestationId); SurviveContext *ctx = driver->ctx; if (driver->outputCalculatedPose) { diff --git a/src/survive.c b/src/survive.c index f23d99e..ae4e55c 100644 --- a/src/survive.c +++ b/src/survive.c @@ -28,6 +28,8 @@ #define z_const const #endif +#include "survive_private.h" + #define DEFAULT_CONFIG_PATH "config.json" STATIC_CONFIG_ITEM(SURVIVE_VERBOSE, "v", 'i', "Verbosity level", 0) STATIC_CONFIG_ITEM(BLACKLIST_DEVS, "blacklist-devs", 's', "List any devs (or substrings of devs) to blacklist.", "") @@ -254,16 +256,6 @@ SURVIVE_EXPORT int8_t survive_get_bsd_idx(SurviveContext *ctx, survive_channel c return -1; } -struct SurviveContext_private { - og_sema_t poll_sema; - survive_run_time_fn runTimeFn; - void *runTimeFnUser; - double lastRunTime; - - double callbackStatsTimeBetween; - double lastCallbackStats; -}; - void survive_get_ctx_lock(SurviveContext *ctx) { struct SurviveContext_private *pctx = ctx->private_members; // SV_VERBOSE(100, "Trying to get lock on %lx", pthread_self()); @@ -331,6 +323,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv, void *userDat ctx->poll_min_time_ms = 10; struct SurviveContext_private *pctx = ctx->private_members = SV_CALLOC(sizeof(struct SurviveContext_private)); + pctx->external2world.Rot[0] = 1; pctx->poll_sema = OGCreateSema(); diff --git a/src/survive_api.c b/src/survive_api.c index 2ac6bce..0e26816 100644 --- a/src/survive_api.c +++ b/src/survive_api.c @@ -448,8 +448,9 @@ FLT survive_simple_object_get_latest_pose(const SurviveSimpleObject *sao, Surviv timecode = SurviveSensorActivations_runtime(&sao->data.so->activations, sao->data.so->OutPose_timecode) * 1e-6; break; case SurviveSimpleObject_EXTERNAL: - if (pose) - *pose = sao->data.seo.pose; + if (pose) { + ApplyPoseToPose(pose, survive_external_to_world(sao->actx->ctx), &sao->data.seo.pose); + } break; default: { diff --git a/src/survive_private.h b/src/survive_private.h new file mode 100644 index 0000000..2739c37 --- /dev/null +++ b/src/survive_private.h @@ -0,0 +1,19 @@ +#pragma once + +struct SurviveExternalPose { + char name[32]; + SurvivePose pose; +}; + +struct SurviveContext_private { + og_sema_t poll_sema; + survive_run_time_fn runTimeFn; + void *runTimeFnUser; + double lastRunTime; + + double callbackStatsTimeBetween; + double lastCallbackStats; + + struct SurviveExternalPose ExternalPoses[16]; + SurvivePose external2world; +}; \ No newline at end of file diff --git a/src/survive_process.c b/src/survive_process.c index caa8419..4fc140d 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -13,6 +13,8 @@ #include "survive_kalman_tracker.h" #include "survive_str.h" +#include "survive_private.h" + void survive_default_button_process(SurviveObject *so, enum SurviveInputEvent eventType, enum SurviveButton buttonId, const enum SurviveAxis *axisIds, const SurviveAxisVal_t *axisValues) { } @@ -75,6 +77,76 @@ static inline bool check_str(const char* blacklist, const char* name) { } return false; } + +static inline void calculate_external2world(SurviveContext *ctx) { + SurvivePose externalLH[NUM_GEN2_LIGHTHOUSES] = { 0 }; + for(int i = 0;i < SURVIVE_ARRAY_SIZE(ctx->private_members->ExternalPoses) && ctx->private_members->ExternalPoses->name[0] != 0;i++) { + for (int j = 0; j < ctx->activeLighthouses; j++) { + if(!ctx->bsd[j].PositionSet) continue; + + char buf[32] = {0}; + snprintf(buf, 32, "LHB-%08X", ctx->bsd[j].BaseStationID); + if (strcmp(buf, ctx->private_members->ExternalPoses[i].name) == 0) { + externalLH[j] = ctx->private_members->ExternalPoses[i].pose; + } + + snprintf(buf, 32, "previous_LH%d", ctx->bsd[j].mode); + if (strcmp(buf, ctx->private_members->ExternalPoses[i].name) == 0) { + externalLH[j] = ctx->private_members->ExternalPoses[i].pose; + } + } + } + + LinmathVec3d pts[] = { + {1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}, + }; + + int num_pairs = 0; + CN_CREATE_STACK_MAT(ptsExtLH, ctx->activeLighthouses * SURVIVE_ARRAY_SIZE(pts), 3); + CN_CREATE_STACK_MAT(ptsWorldLH, ctx->activeLighthouses * SURVIVE_ARRAY_SIZE(pts), 3); + for (int j = 0; j < ctx->activeLighthouses; j++) { + if (quatiszero(externalLH[j].Rot)) continue; + + for(int h = 0;h < SURVIVE_ARRAY_SIZE(pts);h++) { + ApplyPoseToPoint(ptsExtLH.data + (num_pairs) * 3, &externalLH[j], pts[h]); + ApplyPoseToPoint(ptsWorldLH.data + (num_pairs) * 3, survive_get_lighthouse_true_position(ctx, j), pts[h]); + (ptsWorldLH.data + (num_pairs) * 3)[2] -= ctx->floor_offset; + num_pairs++; + } + } + if(num_pairs == 0) { + ctx->private_members->external2world = (SurvivePose) { .Rot = { 1 }}; + } else { + Kabsch(&ctx->private_members->external2world, ptsExtLH.data, ptsWorldLH.data, num_pairs); + survive_recording_write_to_output(ctx->recptr, "EXTERNAL_TO_WORLD " SurvivePose_format "\n", SURVIVE_POSE_EXPAND(ctx->private_members->external2world)); + } +} + +const SurvivePose* survive_external_to_world(const SurviveContext *ctx) { + return &ctx->private_members->external2world; +} + +static inline void insert_external_pose(SurviveContext *ctx, const char *name, const SurvivePose *pose) { + bool isLH = strncmp(name, "LHB-", 4) == 0 || + strncmp(name, "previous_LH", strlen("previous_LH")) == 0; + + for(int i = 0;i < SURVIVE_ARRAY_SIZE(ctx->private_members->ExternalPoses);i++) { + if(ctx->private_members->ExternalPoses[i].name[0] == 0) { + strncpy(ctx->private_members->ExternalPoses[i].name, name, 16); + } + if(strncmp(name, ctx->private_members->ExternalPoses[i].name, 16) == 0) { + ctx->private_members->ExternalPoses[i].pose = *pose; + break; + } + } + + if(isLH) { + calculate_external2world(ctx); + } +} + void survive_default_external_pose_process(SurviveContext *ctx, const char *name, const SurvivePose *pose) { if (strncmp(name, "LHB", 3) == 0) { bool useExternal = survive_configb(ctx, USE_EXTERNAL_LH_TAG, SC_GET, 0); @@ -107,6 +179,8 @@ void survive_default_external_pose_process(SurviveContext *ctx, const char *name return; } + insert_external_pose(ctx, name, pose); + survive_recording_external_pose_process(ctx, name, pose); } @@ -131,7 +205,7 @@ void survive_default_raw_lighthouse_pose_process(SurviveContext *ctx, uint8_t li if (ctx->bsd[lighthouse].PositionSet) { FLT d = dist3d(lighthouse_pose->Pos, ctx->bsd[lighthouse].Pose.Pos); if (d < ctx->settings.lh_max_update || d > ctx->settings.lh_max_nudge_distance) { - ctx->bsd[lighthouse].Pose = *lighthouse_pose; + ctx->bsd[lighthouse].true_pos = ctx->bsd[lighthouse].Pose = *lighthouse_pose; } else { ctx->bsd[lighthouse].old_pos = ctx->bsd[lighthouse].Pose; ctx->bsd[lighthouse].true_pos = *lighthouse_pose; @@ -144,7 +218,7 @@ void survive_default_raw_lighthouse_pose_process(SurviveContext *ctx, uint8_t li ctx->bsd[lighthouse].mode, SURVIVE_POSE_EXPAND(*lighthouse_pose)); } } else { - ctx->bsd[lighthouse].Pose = *lighthouse_pose; + ctx->bsd[lighthouse].true_pos = ctx->bsd[lighthouse].Pose = *lighthouse_pose; ctx->bsd[lighthouse].PositionSet = 1; } survive_kalman_lighthouse_update_position(ctx->bsd[lighthouse].tracker, lighthouse_pose); @@ -172,6 +246,7 @@ void survive_default_raw_lighthouse_pose_process(SurviveContext *ctx, uint8_t li SurvivePose external_pose = ctx->bsd[lighthouse].Pose; external_pose.Pos[2] -= ctx->floor_offset; + calculate_external2world(ctx); SURVIVE_INVOKE_HOOK(lighthouse_pose, ctx, lighthouse, &external_pose); } diff --git a/src/survive_recording.c b/src/survive_recording.c index 4f987d7..ad08a22 100644 --- a/src/survive_recording.c +++ b/src/survive_recording.c @@ -157,9 +157,9 @@ void survive_recording_lighthouse_process(SurviveContext *ctx, uint8_t lighthous int8_t mode = ctx->bsd[lighthouse].mode; survive_recording_write_to_output( recordingData, - "%d LH_POSE " FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF "\r\n", mode, + "%d LH_POSE " FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF " %u\r\n", mode, 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]); + lh_pose->Rot[3], ctx->bsd[lighthouse].BaseStationID); } void survive_recording_velocity_process(SurviveObject *so, uint8_t lighthouse, const SurviveVelocity *pose) { SurviveRecordingData *recordingData = so->ctx->recptr; diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 341f962..de2f333 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -570,8 +570,9 @@ function update_trails() { $(function() { $("#trails").change(update_trails); }); function update_external() { - var val = this.checked ? 1.5 : 0; - external_group.position.set(0, val, 0); + var val = $("#external_displace")[0].checked ? 1.5 : 0; + external_group.position.set(external2world[0], external2world[1] + val, external2world[2]); + external_group.quaternion.set(external2world[4], external2world[5], external2world[6], external2world[3]); } $(function() { $("#external_displace").change(update_external); }); @@ -954,8 +955,13 @@ function add_poly(v) { scene.add(line); } } +var external2world = [0, 0, 0, 0, 0, 0, 1]; var survive_log_handlers = { + "EXTERNAL_TO_WORLD": function(v) { + external2world = v.slice(2).map(parseFloat); + update_external(); + }, "LH_POSE" : function(v) { var obj = { lighthouse : parseInt(v[1]), @@ -1118,7 +1124,7 @@ var survive_log_handlers = { function add_survive_log_handler(name, entry) { survive_log_handlers[name] = entry; } function process_survive_handlers(msg) { - var s = msg.split(' ').filter(function(x) { return x; }); + var s = msg.replace('\t', ' ').split(' ').filter(function(x) { return x; }); var handled = false; if (survive_log_handlers[s[2]]) { survive_log_handlers[s[2]](s);