aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--Makefile4
-rw-r--r--redist/os_generic.h2
-rw-r--r--src/poser_epnp.c54
-rw-r--r--src/poser_sba.c193
-rw-r--r--src/survive.c4
-rw-r--r--src/survive_process.c4
-rw-r--r--src/survive_statebased_disambiguator.c469
-rwxr-xr-xsrc/survive_vive.c10
-rw-r--r--tools/viz/index.html1
-rw-r--r--tools/viz/survive_viewer.js4
-rw-r--r--useful_files/light_reading.md10
11 files changed, 581 insertions, 174 deletions
diff --git a/Makefile b/Makefile
index f95ca61..c73f139 100644
--- a/Makefile
+++ b/Makefile
@@ -2,7 +2,7 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test
CC?=gcc
-CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable
+CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses
CFLAGS_RELEASE:=-Iinclude/libsurvive -fPIC -msse2 -ftree-vectorize -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm
@@ -39,7 +39,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/minimal_ope
ifeq ($(UNAME), Darwin)
REDISTS:=$(REDISTS) redist/hid-osx.c
endif
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o
#If you want to use HIDAPI on Linux.
#CFLAGS:=$(CFLAGS) -DHIDAPI
diff --git a/redist/os_generic.h b/redist/os_generic.h
index 853440a..0d1c7e7 100644
--- a/redist/os_generic.h
+++ b/redist/os_generic.h
@@ -214,7 +214,7 @@ OSG_INLINE og_thread_t OGCreateThread(void *(routine)(void *), void *parameter)
return (og_thread_t)ret;
}
-static void *OGJoinThread(og_thread_t ot) {
+OSG_INLINE void *OGJoinThread(og_thread_t ot) {
void *retval;
if (!ot) {
return 0;
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, "");
diff --git a/tools/viz/index.html b/tools/viz/index.html
index 0221b41..2987555 100644
--- a/tools/viz/index.html
+++ b/tools/viz/index.html
@@ -26,6 +26,7 @@
position: absolute;
bottom: 20px;
overflow: auto;
+ white-space: pre-wrap;
background-color: rgba(0,200,200, .25);
z-index: 3;">
diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js
index 67e65f0..d7f80ad 100644
--- a/tools/viz/survive_viewer.js
+++ b/tools/viz/survive_viewer.js
@@ -130,7 +130,7 @@ function redrawCanvas(when) {
for (var key in angles) {
for (var lh = 0; lh < 2; lh++) {
- var bvalue = {"WW0" : "FF", "TR0" : "00"};
+ var bvalue = {"WW0" : "FF", "TR0" : "00", "HMD" : "88"};
ctx.strokeStyle = (lh === 0 ? "#FF00" : "#00FF") + bvalue[key];
if (angles[key][lh])
@@ -190,7 +190,7 @@ function create_tracked_object(info) {
}
var trails;
-var MAX_LINE_POINTS = 1000;
+var MAX_LINE_POINTS = 100000;
$(function() {
$("#trails").change(function() {
if (this.checked) {
diff --git a/useful_files/light_reading.md b/useful_files/light_reading.md
new file mode 100644
index 0000000..f8613c4
--- /dev/null
+++ b/useful_files/light_reading.md
@@ -0,0 +1,10 @@
+
+Here's an awesome course about VR that gives a ton of background and is great for anyone interested:
+https://stanford.edu/class/ee267/lectures/
+
+
+Nairol's discussion of the lighthouses used with the HTC Vive.
+https://github.com/nairol/LighthouseRedox/tree/master/docs
+
+This is probably what the parameters map to for the lens correction:
+https://docs.opencv.org/trunk/db/d58/group__calib3d__fisheye.html