diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/poser_epnp.c | 54 | ||||
-rw-r--r-- | src/poser_sba.c | 193 | ||||
-rw-r--r-- | src/survive.c | 4 | ||||
-rw-r--r-- | src/survive_process.c | 4 | ||||
-rw-r--r-- | src/survive_statebased_disambiguator.c | 469 | ||||
-rwxr-xr-x | src/survive_vive.c | 10 |
6 files changed, 565 insertions, 169 deletions
diff --git a/src/poser_epnp.c b/src/poser_epnp.c index c294c0c..7e86542 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -98,11 +98,10 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) return 0; } -static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActivations *scene, - const PoserDataLight *lightData) { - int lh = lightData->lh; +static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActivations *scene, uint32_t timecode, + int lh) { for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) { - if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, lightData->timecode, + if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timecode, sensor_idx, lh)) { double *angles = scene->angles[sensor_idx][lh]; epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0], @@ -125,46 +124,55 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { PoserDataLight *lightData = (PoserDataLight *)pd; SurvivePose posers[2]; - bool hasData[2] = {0, 0}; - for (int lh = 0; lh < 1; lh++) { + int meas[2] = {0, 0}; + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { if (so->ctx->bsd[lh].PositionSet) { epnp pnp = {.fu = 1, .fv = 1}; epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); - add_correspondences(so, &pnp, scene, lightData); + add_correspondences(so, &pnp, scene, lightData->timecode, lh); static int required_meas = -1; if (required_meas == -1) required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4); if (pnp.number_of_correspondences > required_meas) { - SurvivePose pose = solve_correspondence(so, &pnp, false); + SurvivePose objInLh = solve_correspondence(so, &pnp, false); + if (quatmagnitude(objInLh.Rot) != 0) { + SurvivePose *lh2world = &so->ctx->bsd[lh].Pose; - SurvivePose txPose = {0}; - quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos); - for (int i = 0; i < 3; i++) { - txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i]; + SurvivePose txPose = {.Rot = {1}}; + ApplyPoseToPose(&txPose, lh2world, &objInLh); + posers[lh] = txPose; + meas[lh] = pnp.number_of_correspondences; } - - quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot); - - posers[lh] = txPose; - hasData[lh] = 1; } epnp_dtor(&pnp); } } - if (hasData[0] && hasData[1]) { + if (meas[0] > 0 && meas[1] > 0) { SurvivePose interpolate = {0}; - for (size_t i = 0; i < 3; i++) { - interpolate.Pos[i] = (posers[0].Pos[i] + posers[1].Pos[i]) / 2.; + bool winnerTakesAll = true; // Not convinced slerp does the right thing, will change this when i am + + if (winnerTakesAll) { + int winner = meas[0] > meas[1] ? 0 : 1; + PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]); + } else { + double a, b; + a = meas[0] * meas[0]; + b = meas[1] * meas[1]; + + double t = a + b; + for (size_t i = 0; i < 3; i++) { + interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t); + } + quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t)); + PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); } - quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, .5); - PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate); } else { - if (hasData[lightData->lh]) + if (meas[lightData->lh]) PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]); } return 0; diff --git a/src/poser_sba.c b/src/poser_sba.c index 069e1d0..0ad38ac 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -33,16 +33,22 @@ typedef struct { typedef struct SBAData { int last_acode; int last_lh; + int failures_to_reset; int failures_to_reset_cntr; int successes_to_reset; int successes_to_reset_cntr; + FLT sensor_variance; + FLT sensor_variance_per_second; + int sensor_time_window; + int required_meas; + SurviveObject *so; } SBAData; -void metric_function(int j, int i, double *aj, double *xij, void *adata) { +static void metric_function(int j, int i, double *aj, double *xij, void *adata) { sba_context *ctx = (sba_context *)(adata); SurviveObject *so = ctx->so; @@ -53,7 +59,7 @@ void metric_function(int j, int i, double *aj, double *xij, void *adata) { xij); } -size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { +static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { size_t measCount = 0; size_t size = so->sensor_ct * NUM_LIGHTHOUSES; // One set per lighthouse for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { @@ -74,35 +80,27 @@ size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char * return measCount; } -size_t construct_input_from_scene_single_sweep(const SurviveObject *so, PoserDataLight *pdl, - SurviveSensorActivations *scene, char *vmask, double *meas, int acode, - int lh) { - size_t rtn = 0; - - for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { - const uint32_t *data_timecode = scene->timecode[sensor][lh]; - if (pdl->timecode - data_timecode[acode & 1] <= SurviveSensorActivations_default_tolerance) { - double *a = scene->angles[sensor][lh]; - vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; - meas[rtn++] = a[acode & 0x1]; - } else { - vmask[sensor * NUM_LIGHTHOUSES + lh] = 0; - } - } - - return rtn; -} - -size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene, - char *vmask, double *meas) { +static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask, + double *meas, double *cov) { size_t rtn = 0; + SurviveObject *so = d->so; for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { - if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, pdl->timecode, - sensor, lh)) { + if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { double *a = scene->angles[sensor][lh]; vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; + + if (cov) { + *(cov++) = d->sensor_variance + + abs(pdl->timecode - scene->timecode[sensor][lh][0]) * d->sensor_variance_per_second / + (double)so->timebase_hz; + *(cov++) = 0; + *(cov++) = 0; + *(cov++) = d->sensor_variance + + abs(pdl->timecode - scene->timecode[sensor][lh][1]) * d->sensor_variance_per_second / + (double)so->timebase_hz; + } meas[rtn++] = a[0]; meas[rtn++] = a[1]; } else { @@ -127,7 +125,7 @@ typedef struct { SurvivePose poses; } sba_set_position_t; -void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) { +static void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) { sba_set_position_t *user = _user; assert(user->hasInfo == false); user->hasInfo = 1; @@ -135,7 +133,7 @@ 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) { +static 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; @@ -159,7 +157,7 @@ void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, voi *xij = out[acode]; } -void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { +static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { SurvivePose obj = *(SurvivePose *)bi; int sensor_idx = j >> 1; int lh = j & 1; @@ -178,114 +176,26 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { SurvivePose *camera = &so->ctx->bsd[lh].Pose; survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); } -#if 0 -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 < d->required_meas) { - 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 %u measurements", (unsigned int)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 = {0}; - 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] = {0}; - double info[SBA_INFOSZ] = {0}; - - 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 < d->required_meas) { - SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (unsigned int)meas_size); - SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); - cnt = 0; - } - } - return info[1] / meas_size * 2; -} -#endif static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl, - SurviveObject *so, SurviveSensorActivations *scene, - int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { + SurviveSensorActivations *scene, int max_iterations /* = 50*/, + double max_reproj_error /* = 0.005*/) { double *covx = 0; + SurviveObject *so = d->so; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); - size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas); + double *cov = + d->sensor_variance_per_second > 0. ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES) : 0; + size_t meas_size = construct_input_from_scene(d, pdl, scene, vmask, meas, cov); static int failure_count = 500; - if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < d->required_meas) { - if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { + bool hasAllBSDs = true; + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) + hasAllBSDs &= so->ctx->bsd[lh].PositionSet; + + if (!hasAllBSDs || meas_size < d->required_meas) { + if (hasAllBSDs && failure_count++ == 500) { SurviveContext *ctx = so->ctx; SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); failure_count = 0; @@ -344,7 +254,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o soLocation.Pos, // Reads as the full pose though 7, // pnp -- SurvivePose meas, // x* -- measurement data - 0, // cov data + cov, // cov data 2, // mnp -- 2 points per image str_metric_function, 0, // jacobia of metric_func @@ -354,6 +264,14 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o opts, // options info); // info + if (currentPositionValid) { + FLT distp[3]; + sub3d(distp, so->OutPose.Pos, soLocation.Pos); + FLT distance = magnitude3d(distp); + ; + if (distance > 1.) + status = -1; + } if (status > 0) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); @@ -421,7 +339,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd opts[4] = 0.0; int status = sba_mot_levmar(so->sensor_ct, // number of 3d points - NUM_LIGHTHOUSES, // Number of cameras -- 2 lighthouses + so->ctx->activeLighthouses, // Number of cameras -- 2 lighthouses 0, // Number of cameras to not modify vmask, // boolean vis mask (double *)&sbactx.camera_params[0], // camera parameters @@ -439,7 +357,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd if (status >= 0) { SurvivePose additionalTx = {0}; - for (int i = 0; i < NUM_LIGHTHOUSES; i++) { + for (int i = 0; i < so->ctx->activeLighthouses; i++) { if (quatmagnitude(sbactx.camera_params[i].Rot) != 0) { PoserData_lighthouse_pose_func(&pdfs->hdr, so, i, &additionalTx, &sbactx.camera_params[i], &sbactx.obj_pose); @@ -474,7 +392,16 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); - SV_INFO("Initializing SBA with %d required measurements", d->required_meas); + d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, 1600000 * 4); + d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 0.001); + d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); + d->so = so; + + SV_INFO("Initializing SBA:"); + SV_INFO("\tsba-required-meas: %d", d->required_meas); + SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance); + SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second); + SV_INFO("\tsba-time-window: %d", d->sensor_time_window); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -489,7 +416,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { FLT error = -1; if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { survive_calibration_config config = *survive_calibration_default_config(); - error = run_sba_find_3d_structure(d, config, lightData, so, scene, 50, .5); + error = run_sba_find_3d_structure(d, config, lightData, scene, 50, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } diff --git a/src/survive.c b/src/survive.c index 73d6474..899d206 100644 --- a/src/survive.c +++ b/src/survive.c @@ -219,7 +219,7 @@ void *GetDriverByConfig(SurviveContext *ctx, const char *name, const char *confi int prefixLen = strlen(name); if (verbose > 1) - SV_INFO("Available %s:", name); + SV_INFO("Available %ss:", name); while ((DriverName = GetDriverNameMatching(name, i++))) { void *p = GetDriver(DriverName); @@ -237,7 +237,7 @@ void *GetDriverByConfig(SurviveContext *ctx, const char *name, const char *confi if (verbose > 1) SV_INFO("Totals %d %ss.", i - 1, name); if (verbose > 0) - SV_INFO("Using %s for %s", name, configname); + SV_INFO("Using '%s' for %s", picked, configname); return func; } diff --git a/src/survive_process.c b/src/survive_process.c index 6136148..ad7ce97 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -80,7 +80,9 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode .lh = lh, }; - SurviveSensorActivations_add(&so->activations, &l); + // Simulate the use of only one lighthouse in playback mode. + if (lh < ctx->activeLighthouses) + SurviveSensorActivations_add(&so->activations, &l); survive_recording_angle_process(so, sensor_id, acode, timecode, length, angle, lh); diff --git a/src/survive_statebased_disambiguator.c b/src/survive_statebased_disambiguator.c new file mode 100644 index 0000000..4aa47ba --- /dev/null +++ b/src/survive_statebased_disambiguator.c @@ -0,0 +1,469 @@ +// +#include "survive_internal.h" +#include <assert.h> +#include <math.h> /* for sqrt */ +#include <stdint.h> +#include <stdlib.h> +#include <string.h> + +//#define DEBUG_TB(...) SV_INFO(__VA_ARGS__) +#define DEBUG_TB(...) +/** + * The lighthouses go in the following order: + * + * Ticks State + * 0 ACode 0b1x0 (4) <--- B + * 20 000 ACode 0b0x0 (0) <--- A/c + * LH A X Sweep + * 400 000 ACode 0b1x1 (5) <--- B + * 420 000 ACode 0b0x1 (1) <--- A/c + * LH A Y SWEEP + * 800 000 ACode 0b0x0 (0) <--- B + * 820 000 ACode 0b1x0 (4) <--- A/c + * LH B X Sweep + * 1 200 000 ACode 0b0x1 (1) <--- B + * 1 220 000 ACode 0b1x1 (5) <--- A/c + * LH B Y SWEEP + * 1 600 000 < REPEAT > + * + * NOTE: Obviously you cut the data bit out for this + * + * This disambiguator works by finding where in that order it is, and tracking along with it. + * It is able to maintain this tracking for extended periods of time without further data + * by knowing the modulo of the start of the cycle and calculating appropriatly although this + * will run into issues when the timestamp rolls over or we simply drift off in accuracy. + * + * Neither case is terminal though; it will just have to find the modulo again which only takes + * a handful of pulses. + * + * The main advantage to this scheme is that its reasonably fast and is able to deal with being + * close enough to the lighthouse that the lengths are in a valid sync pulse range. + */ + +// Every pulse_window seems roughly 20k ticks long. That leaves ~360 to the capture window +#define PULSE_WINDOW 20000 +#define CAPTURE_WINDOW 360000 + +enum LighthouseState { + LS_UNKNOWN = 0, + + LS_WaitLHA_ACode4 = 1, + LS_WaitLHA_ACode0, + LS_SweepAX, + LS_WaitLHA_ACode5, + LS_WaitLHA_ACode1, + LS_SweepAY, + LS_WaitLHB_ACode0, + LS_WaitLHB_ACode4, + LS_SweepBX, + LS_WaitLHB_ACode1, + LS_WaitLHB_ACode5, + LS_SweepBY, + + LS_END +}; + +typedef struct { + int acode, lh, axis, window, offset; + bool is_sweep; +} LighthouseStateParameters; + +// clang-format off +const LighthouseStateParameters LS_Params[LS_END + 1] = { + {.acode = -1, .lh = -1, .axis = -1, .window = -1}, + + {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 + {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 + {.acode = 4, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW, .is_sweep = 1}, // 40000 + + {.acode = 5, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 + {.acode = 1, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 + {.acode = 5, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW, .is_sweep = 1}, // 440000 + + {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 + {.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 + {.acode = 0, .lh = 0, .axis = 0, .window = CAPTURE_WINDOW, .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW, .is_sweep = 1}, // 840000 + + {.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1200000 + {.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1220000 + {.acode = 1, .lh = 0, .axis = 1, .window = CAPTURE_WINDOW, .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW, .is_sweep = 1}, // 1240000 + + {.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW} // 1600000 +}; +// clang-format on + +enum LighthouseState LighthouseState_findByOffset(int offset) { + for (int i = 2; i < LS_END + 1; i++) { + if (LS_Params[i].offset > offset) + return i - 1; + } + assert(false); + return -1; +} + +typedef struct { + SurviveObject *so; + /* We keep the last sync time per LH because lightproc expects numbers relative to it */ + uint32_t time_of_last_sync[NUM_LIGHTHOUSES]; + + /* Keep running average of sync signals as they come in */ + uint64_t last_sync_timestamp; + uint64_t last_sync_length; + int last_sync_count; + + /** This part of the structure is general use when we know our state */ + enum LighthouseState state; + uint32_t mod_offset; + int confidence; + + /** This rest of the structure is dedicated to finding a state when we are unknown */ + int encoded_acodes; + + int stabalize; + bool lastWasSync; + + LightcapElement sweep_data[]; +} Disambiguator_data_t; + +static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { + if (recent > prior) + return recent - prior; + return (0xFFFFFFFF - prior) + recent; +} + +static int find_acode(uint32_t pulseLen) { + const static int offset = 50; + if (pulseLen < 2500 + offset) + return -1; + + if (pulseLen < 3000 + offset) + return 0; + if (pulseLen < 3500 + offset) + return 1; + if (pulseLen < 4000 + offset) + return 2; + if (pulseLen < 4500 + offset) + return 3; + if (pulseLen < 5000 + offset) + return 4; + if (pulseLen < 5500 + offset) + return 5; + if (pulseLen < 6000 + offset) + return 6; + if (pulseLen < 6500 + offset) + return 7; + + return -1; +} + +static bool overlaps(const LightcapElement *a, const LightcapElement *b) { + int overlap = 0; + if (a->timestamp < b->timestamp && a->length + a->timestamp > b->timestamp) + overlap = a->length + a->timestamp - b->timestamp; + else if (b->timestamp < a->timestamp && b->length + b->timestamp > a->timestamp) + overlap = b->length + b->timestamp - a->timestamp; + + return overlap > a->length / 2; +} + +const int SKIP_BIT = 4; +const int DATA_BIT = 2; +const int AXIS_BIT = 1; + +#define LOWER_SYNC_TIME 2250 +#define UPPER_SYNC_TIME 6750 + +LightcapElement get_last_sync(Disambiguator_data_t *d) { + if (d->last_sync_count == 0) { + return (LightcapElement){0}; + } + + return (LightcapElement){.timestamp = (d->last_sync_timestamp + d->last_sync_count / 2) / d->last_sync_count, + .length = (d->last_sync_length + d->last_sync_count / 2) / d->last_sync_count, + .sensor_id = -d->last_sync_count}; +} + +enum LightcapClassification { LCC_SWEEP, LCC_SYNC }; +static enum LightcapClassification naive_classify(Disambiguator_data_t *d, const LightcapElement *le) { + bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME; + + if (clearlyNotSync) { + return LCC_SWEEP; + } else { + return LCC_SYNC; + } +} + +#define ACODE_TIMING(acode) \ + ((3000 + ((acode)&1) * 500 + (((acode) >> 1) & 1) * 1000 + (((acode) >> 2) & 1) * 2000) - 250) +#define ACODE(s, d, a) ((s << 2) | (d << 1) | a) +#define SWEEP 0xFF + +static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState state, const LightcapElement *le) { + assert(LS_Params[state].is_sweep == 0); // Doesn't work for sweep data + SurviveContext *ctx = d->so->ctx; + DEBUG_TB("Solve for mod %d (%u - %u) = %u", state, le->timestamp, LS_Params[state].offset, + (le->timestamp - LS_Params[state].offset)); + + return (le->timestamp - LS_Params[state].offset); +} + +static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, + enum LighthouseState new_state); +static enum LighthouseState CheckEncodedAcode(Disambiguator_data_t *d, uint8_t newByte) { + + // We chain together acodes / sweep indicators to form an int we can just switch on. + SurviveContext *ctx = d->so->ctx; + d->encoded_acodes &= 0xFF; + d->encoded_acodes = (d->encoded_acodes << 8) | newByte; + + LightcapElement lastSync = get_last_sync(d); + + // These combinations are checked for specificaly to allow for the case one lighthouse is either + // missing or completely occluded. + switch (d->encoded_acodes) { + case (ACODE(0, 1, 0) << 8) | SWEEP: + d->mod_offset = SolveForMod_Offset(d, LS_SweepAX - 1, &lastSync); + + return (LS_SweepAX + 1); + case (ACODE(0, 1, 1) << 8) | SWEEP: + d->mod_offset = SolveForMod_Offset(d, LS_SweepAY - 1, &lastSync); + + return (LS_SweepAY + 1); + case (SWEEP << 8) | (ACODE(0, 1, 1)): + d->mod_offset = SolveForMod_Offset(d, LS_WaitLHB_ACode1, &lastSync); + + return (LS_WaitLHB_ACode1 + 1); + case (SWEEP << 8) | (ACODE(1, 1, 0)): + d->mod_offset = SolveForMod_Offset(d, LS_WaitLHA_ACode4, &lastSync); + + return (LS_WaitLHA_ACode4 + 1); + } + + return LS_UNKNOWN; +} +static enum LighthouseState EndSweep(Disambiguator_data_t *d, const LightcapElement *le) { + return CheckEncodedAcode(d, SWEEP); +} +static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapElement *le) { + LightcapElement lastSync = get_last_sync(d); + int acode = find_acode(lastSync.length) > 0; + if (acode > 0) { + return CheckEncodedAcode(d, (acode | DATA_BIT)); + } else { + // If we can't resolve an acode, just reset + d->encoded_acodes = 0; + } + return LS_UNKNOWN; +} + +static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) { + enum LightcapClassification classification = naive_classify(d, le); + + if (classification == LCC_SYNC) { + LightcapElement lastSync = get_last_sync(d); + + // Handle the case that this is a new SYNC coming in + if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { + + if (d->lastWasSync && timestamp_diff(lastSync.timestamp, le->timestamp) > 30000) { + // Missed a sweep window; clear encoded values. + d->encoded_acodes = 0; + } + + // Now that the previous two states are in, check to see if they tell us where we are + enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le); + if (new_state != LS_UNKNOWN) + return new_state; + + // Otherwise, just reset the sync registers and do another + d->last_sync_timestamp = le->timestamp; + d->last_sync_length = le->length; + d->last_sync_count = 1; + } else { + d->last_sync_timestamp += le->timestamp; + d->last_sync_length += le->length; + d->last_sync_count++; + } + + d->lastWasSync = true; + } else { + // If this is the start of a new sweep, check to see if the end of the sync solves + // the state + if (d->lastWasSync) { + enum LighthouseState new_state = EndSync(d, le); + if (new_state != LS_UNKNOWN) + return new_state; + } + d->lastWasSync = false; + } + + return LS_UNKNOWN; +} + +static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, + enum LighthouseState new_state) { + + SurviveContext *ctx = d->so->ctx; + if (new_state >= LS_END) + new_state = 1; + + d->encoded_acodes = 0; + d->state = new_state; + + d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0; + memset(d->sweep_data, 0, sizeof(LightcapElement) * d->so->sensor_ct); + + return new_state; +} + +static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); +static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { + // Just ignore small signals; this has a measurable impact on signal quality + if (le->length < 100) + return; + + // We know what state we are in, so we verify that state as opposed to + // trying to suss out the acode. + + // Calculate what it would be with and without data + uint32_t time_error_d0 = abs(ACODE_TIMING(target_acode) - le->length); + uint32_t time_error_d1 = abs(ACODE_TIMING(target_acode | DATA_BIT) - le->length); + + // Take the least of the two erors + uint32_t error = time_error_d0 > time_error_d1 ? time_error_d1 : time_error_d0; + + // Errors do happen; either reflections or some other noise. Our scheme here is to + // keep a tally of hits and misses, and if we ever go into the negatives reset + // the state machine to find the state again. + if (error > 1250) { + // Penalize semi-harshly -- if it's ever off track it will take this many syncs + // to reset + const int penalty = 3; + if (d->confidence < penalty) { + SurviveContext *ctx = d->so->ctx; + SetState(d, le, LS_UNKNOWN); + SV_INFO("WARNING: Disambiguator got lost; refinding state."); + } + d->confidence -= penalty; + return; + } + + if (d->confidence < 100) + d->confidence++; + + // If its a real timestep, integrate it here and we can take the average later + d->last_sync_timestamp += le->timestamp; + d->last_sync_length += le->length; + d->last_sync_count++; +} + +static void ProcessStateChange(Disambiguator_data_t *d, const LightcapElement *le, enum LighthouseState new_state) { + SurviveContext *ctx = d->so->ctx; + + // Leaving a sync ... + if (LS_Params[d->state].is_sweep == 0) { + if (d->last_sync_count > 0) { + // Use the average of the captured pulse to adjust where we are modulo against. + // This lets us handle drift in any of the timing chararacteristics + LightcapElement lastSync = get_last_sync(d); + d->mod_offset = SolveForMod_Offset(d, d->state, &lastSync); + + // Figure out if it looks more like it has data or doesn't. We need this for OOX + int lengthData = ACODE_TIMING(LS_Params[d->state].acode | DATA_BIT); + int lengthNoData = ACODE_TIMING(LS_Params[d->state].acode); + bool hasData = abs(lengthData - lastSync.length) < abs(lengthNoData - lastSync.length); + int acode = LS_Params[d->state].acode; + if (hasData) { + acode |= DATA_BIT; + } + ctx->lightproc(d->so, -LS_Params[d->state].lh - 1, acode, 0, lastSync.timestamp, lastSync.length, + LS_Params[d->state].lh); + + // Store last sync time for sweep calculations + d->time_of_last_sync[LS_Params[d->state].lh] = lastSync.timestamp; + } + } else { + // Leaving a sweep ... + for (int i = 0; i < d->so->sensor_ct; i++) { + LightcapElement le = d->sweep_data[i]; + // Only care if we actually have data AND we have a time of last sync. We won't have the latter + // if we synced with the LH at cetain times. + if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0) { + int32_t offset_from = + timestamp_diff(le.timestamp + le.length / 2, d->time_of_last_sync[LS_Params[d->state].lh]); + + // Send the lightburst out. + assert(offset_from > 0); + d->so->ctx->lightproc(d->so, i, LS_Params[d->state].acode, offset_from, le.timestamp, le.length, + LS_Params[d->state].lh); + } + } + } + + SetState(d, le, new_state); +} + +static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { + int le_offset = le->timestamp > d->mod_offset + ? (le->timestamp - d->mod_offset + 10000) % LS_Params[LS_END].offset + : (0xFFFFFFFF - d->mod_offset + le->timestamp + 10000) % LS_Params[LS_END].offset; + + /** Find where this new element fits into our state machine. This can skip states if its been a while since + * its been able to process, or if a LH is missing. */ + enum LighthouseState new_state = LighthouseState_findByOffset(le_offset); + + if (d->state != new_state) { + // This processes the change -- think setting buffers, and sending OOTX / lightproc calls + ProcessStateChange(d, le, new_state); + } + + const LighthouseStateParameters *param = &LS_Params[d->state]; + if (param->is_sweep == 0) { + RunACodeCapture(param->acode, d, le); + } else if (le->length > d->sweep_data[le->sensor_id].length) { + // Note we only select the highest length one per sweep. Also, we bundle everything up and send it later all at + // once. + // so that we can do this filtering. Might not be necessary? + d->sweep_data[le->sensor_id] = *le; + } +} + +void DisambiguatorStateBased(SurviveObject *so, const LightcapElement *le) { + SurviveContext *ctx = so->ctx; + + // Note, this happens if we don't have config yet -- just bail + if (so->sensor_ct == 0) { + return; + } + + if (so->disambiguator_data == NULL) { + DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct); + Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(LightcapElement) * so->sensor_ct); + d->so = so; + so->disambiguator_data = d; + } + + Disambiguator_data_t *d = so->disambiguator_data; + // It seems like the first few hundred lightcapelements are missing a ton of data; let it stabilize. + if (d->stabalize < 200) { + d->stabalize++; + return; + } + + if (d->state == LS_UNKNOWN) { + enum LighthouseState new_state = AttemptFindState(d, le); + if (new_state != LS_UNKNOWN) { + d->confidence = 0; + + int le_offset = (le->timestamp - d->mod_offset) % LS_Params[LS_END].offset; + enum LighthouseState new_state1 = LighthouseState_findByOffset(le_offset); + SetState(d, le, new_state1); + DEBUG_TB("Locked onto state %d(%d, %d) at %u", new_state, new_state1, le_offset, d->mod_offset); + } + } else { + PropagateState(d, le); + } +} + +REGISTER_LINKTIME(DisambiguatorStateBased); diff --git a/src/survive_vive.c b/src/survive_vive.c index 91f25af..3c60b2a 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1708,16 +1708,6 @@ int survive_vive_close( SurviveContext * ctx, void * driver ) return 0; } -void init_SurviveObject(SurviveObject* so) { - so->acc_scale = NULL; - so->acc_bias = NULL; - so->gyro_scale = NULL; - so->gyro_bias = NULL; - so->haptic = NULL; - so->PoserData = NULL; - so->disambiguator_data = NULL; -} - int DriverRegHTCVive( SurviveContext * ctx ) { const char *playback_dir = survive_configs(ctx, "playback", SC_GET, ""); |