From 17e06b4822e60ccca865e77b45bf2b52a26852f2 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 14:42:53 +0000 Subject: Made picking posers more consistent / configposer can use short name --- src/survive.c | 22 ++++++++++++++-------- src/survive_cal.c | 26 ++------------------------ src/survive_internal.h | 3 ++- 3 files changed, 18 insertions(+), 33 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index e6bf69f..a8e7b94 100644 --- a/src/survive.c +++ b/src/survive.c @@ -199,7 +199,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { return ctx; } -static void *setup_func_by_name(SurviveContext *ctx, const char *name, const char *configname, const char *configdef) { +void *GetDriverByConfig(SurviveContext *ctx, const char *name, const char *configname, const char *configdef, + int verbose) { const char *Preferred = survive_configs(ctx, configname, SC_SETCONFIG, configdef); const char *DriverName = 0; const char *picked = 0; @@ -207,12 +208,14 @@ static void *setup_func_by_name(SurviveContext *ctx, const char *name, const cha void *func = 0; int prefixLen = strlen(name); - SV_INFO("Available %s:", name); + if (verbose > 1) + SV_INFO("Available %s:", name); while ((DriverName = GetDriverNameMatching(name, i++))) { void *p = GetDriver(DriverName); bool match = strcmp(DriverName, Preferred) == 0 || strcmp(DriverName + prefixLen, Preferred) == 0; - SV_INFO("\t%c%s", match ? '*' : ' ', DriverName + prefixLen); + if (verbose > 1) + SV_INFO("\t%c%s", match ? '*' : ' ', DriverName + prefixLen); if (!func || match) { func = p; picked = (DriverName + prefixLen); @@ -221,7 +224,10 @@ static void *setup_func_by_name(SurviveContext *ctx, const char *name, const cha if (!func) { SV_ERROR("Error. Cannot find any valid %s.", name); } - SV_INFO("Totals %d %ss. Using %s.", i - 1, name, picked); + if (verbose > 1) + SV_INFO("Totals %d %ss.", i - 1, name); + if (verbose > 0) + SV_INFO("Using %s for %s", name, configname); return func; } @@ -230,6 +236,8 @@ int survive_startup(SurviveContext *ctx) { int r = 0; int i = 0; + survive_install_recording(ctx); + // initialize the button queue memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue)); ctx->buttonQueue.buttonservicesem = OGCreateSema(); @@ -237,15 +245,13 @@ int survive_startup(SurviveContext *ctx) { // start the thread to process button data ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); - PoserCB PreferredPoserCB = setup_func_by_name(ctx, "Poser", "defaultposer", "PoserTurveyTori"); - ctx->lightcapfunction = setup_func_by_name(ctx, "Disambiguator", "disambiguator", "Turvey"); + PoserCB PreferredPoserCB = GetDriverByConfig(ctx, "Poser", "defaultposer", "TurveyTori", 2); + ctx->lightcapfunction = GetDriverByConfig(ctx, "Disambiguator", "disambiguator", "Turvey", 2); const char *DriverName; i = 0; - survive_install_recording(ctx); - while ((DriverName = GetDriverNameMatching("DriverReg", i++))) { DeviceDriver dd = GetDriver(DriverName); SV_INFO("Loading driver %s (%p) (%d)", DriverName, dd, i); diff --git a/src/survive_cal.c b/src/survive_cal.c index 3367aa0..c94bd0d 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -184,30 +184,8 @@ void survive_cal_install( struct SurviveContext * ctx ) } const char * DriverName; -// const char * PreferredPoser = survive_configs(ctx, "configposer", "PoserCharlesSlow"); - const char * PreferredPoser = survive_configs(ctx, "configposer", SC_SETCONFIG, "PoserTurveyTori"); - - SV_INFO( "Trying to load poser %s for cal.", PreferredPoser ); - PoserCB SelectedPoserCB = 0; - const char * SelectedPoserName = 0; - i = 0; - while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) ) - { - PoserCB p = GetDriver( DriverName ); - if( !SelectedPoserCB ) - { - SelectedPoserCB = p; - SelectedPoserName = DriverName; - } - int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0; - if( ThisPoser ) - { - SelectedPoserCB = p; - SelectedPoserName = DriverName; - } - } - cd->ConfigPoserFn = SelectedPoserCB; - SV_INFO( "Got config poser: %s (%p)", SelectedPoserName, cd->ConfigPoserFn ); + cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "TurveyTori", 0); + ootx_packet_clbk = ootx_packet_clbk_d; ctx->calptr = cd; diff --git a/src/survive_internal.h b/src/survive_internal.h index 86b119f..9120f41 100644 --- a/src/survive_internal.h +++ b/src/survive_internal.h @@ -16,7 +16,8 @@ void * GetDriver( const char * name ); const char * GetDriverNameMatching( const char * prefix, int place ); void ListDrivers(); - +void *GetDriverByConfig(SurviveContext *ctx, const char *name, const char *configname, const char *configdef, + int verbose); #endif -- cgit v1.3.1 From d475a433ec40f335fa0bffdf774bac4c69d7fc10 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 14:43:32 +0000 Subject: Made calibration installation driveable from the command line; also turns on if there is no calibration --- data_recorder.c | 4 ---- src/survive.c | 25 +++++++++++++++++++++++++ src/survive_cal.c | 3 +++ 3 files changed, 28 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/data_recorder.c b/data_recorder.c index eef233d..427a953 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -17,10 +17,6 @@ int main(int argc, char **argv) { } survive_startup(ctx); - if (survive_configi(ctx, "calibrate", SC_GET, 1)) { - SV_INFO("Installing calibration"); - survive_cal_install(ctx); - } while (survive_poll(ctx) == 0) { } diff --git a/src/survive.c b/src/survive.c index a8e7b94..3f3b844 100644 --- a/src/survive.c +++ b/src/survive.c @@ -269,6 +269,31 @@ int survive_startup(SurviveContext *ctx) { ctx->state = SURVIVE_RUNNING; + int calibrateMandatory = survive_configi(ctx, "calibrate", SC_GET, 0); + int calibrateForbidden = survive_configi(ctx, "no-calibrate", SC_GET, 0); + if (calibrateMandatory && calibrateForbidden) { + SV_INFO("Contradictory settings --calibrate and --no-calibrate specified. Switching to normal behavior."); + calibrateMandatory = calibrateForbidden = 0; + } + + if (!calibrateForbidden) { + bool isCalibrated = true; + for (int i = 0; i < ctx->activeLighthouses; i++) { + isCalibrated &= ctx->bsd[i].PositionSet; + } + + if (!isCalibrated) { + SV_INFO("Uncalibrated configuration detected. Attaching calibration. Please don't move tracked objects for " + "the duration of calibration. Pass '--no-calibrate' to skip calibration"); + } + + bool doCalibrate = isCalibrated == false || calibrateMandatory; + + if (doCalibrate) { + survive_cal_install(ctx); + } + } + return 0; } diff --git a/src/survive_cal.c b/src/survive_cal.c index c94bd0d..218f5c1 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -116,6 +116,9 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int void survive_cal_install( struct SurviveContext * ctx ) { + if (ctx->calptr) + return; + int i; struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) ); -- cgit v1.3.1 From 8c247ac2aaa5e6de4c52cae05e9b5aec736b3e70 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 07:26:31 +0000 Subject: Added reset cntr for sba --- src/poser_sba.c | 47 ++++++++++++++++++++++++++++++----------------- 1 file changed, 30 insertions(+), 17 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index d1677d1..d36c264 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -30,6 +30,13 @@ typedef struct { int lh; } sba_context_single_sweep; +typedef struct SBAData { + int last_acode; + int last_lh; + int failures_to_reset; + int failures_to_reset_cntr; +} SBAData; + void metric_function(int j, int i, double *aj, double *xij, void *adata) { sba_context *ctx = (sba_context *)(adata); SurviveObject *so = ctx->so; @@ -181,7 +188,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) { if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { SurviveContext *ctx = so->ctx; - SV_INFO("Can't solve for position with just %lu measurements", meas_size); + SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); failure_count = 0; } return -1; @@ -253,7 +260,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config // Docs say info[0] should be divided by meas; I don't buy it really... static int cnt = 0; if (cnt++ > 1000 || meas_size < 8) { - SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f 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; } @@ -262,9 +269,9 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config return info[1] / meas_size * 2; } -static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, - SurviveSensorActivations *scene, int max_iterations /* = 50*/, - double max_reproj_error /* = 0.005*/) { +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*/) { double *covx = 0; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); @@ -275,7 +282,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 7) { if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { SurviveContext *ctx = so->ctx; - SV_INFO("Can't solve for position with just %lu measurements", meas_size); + SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); failure_count = 0; } return -1; @@ -285,10 +292,12 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose SurvivePose soLocation = so->OutPose; bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; - { + if (d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { + SurviveContext *ctx = so->ctx; + SV_INFO("Must rerun seed poser"); 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 @@ -340,6 +349,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose info); // info if (status > 0) { + d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); } @@ -349,7 +359,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose // Docs say info[0] should be divided by meas; I don't buy it really... static int cnt = 0; if (cnt++ > 1000 || meas_size < 8) { - SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); cnt = 0; } @@ -435,21 +445,19 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd { SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... - SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); } return info[1] / meas_size * 2; } -typedef struct SBAData { - int last_acode; - int last_lh; -} SBAData; - int PoserSBA(SurviveObject *so, PoserData *pd) { if (so->PoserData == 0) { so->PoserData = calloc(1, sizeof(SBAData)); + SBAData *d = so->PoserData; + d->failures_to_reset_cntr = 0; + d->failures_to_reset = 30; } SBAData *d = so->PoserData; SurviveContext *ctx = so->ctx; @@ -465,18 +473,23 @@ 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(config, lightData, so, scene, 50, .5); + error = run_sba_find_3d_structure(d, config, lightData, so, scene, 50, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } + if (error < 0) { + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; + } + return 0; } case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); survive_calibration_config config = *survive_calibration_default_config(); - SV_INFO("Running sba with %lu", survive_calibration_config_index(&config)); + SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config)); double error = run_sba(config, pdfs, so, 50, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; -- cgit v1.3.1 From 5e8633ecc6f1309b5be2dd86b61e8a7b0a9f5ecb Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 24 Mar 2018 00:10:24 -0600 Subject: Added success counter to sba too --- src/poser_sba.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index d36c264..c01cc61 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -35,6 +35,8 @@ typedef struct SBAData { int last_lh; int failures_to_reset; int failures_to_reset_cntr; + int successes_to_reset; + int successes_to_reset_cntr; } SBAData; void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -292,9 +294,9 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o SurvivePose soLocation = so->OutPose; bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; - if (d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { + if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { SurviveContext *ctx = so->ctx; - SV_INFO("Must rerun seed poser"); + // SV_INFO("Must rerun seed poser"); const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); @@ -310,11 +312,12 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o pdl->hdr = hdr; if (locations.hasInfo == false) { - return -1; } else if (locations.hasInfo) { soLocation = locations.poses; } + + d->successes_to_reset_cntr = d->successes_to_reset; } else { SV_INFO("Not using a seed poser for SBA; results will likely be way off"); } @@ -457,7 +460,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { so->PoserData = calloc(1, sizeof(SBAData)); SBAData *d = so->PoserData; d->failures_to_reset_cntr = 0; - d->failures_to_reset = 30; + d->failures_to_reset = 5; + d->successes_to_reset_cntr = 0; + d->successes_to_reset = 20; } SBAData *d = so->PoserData; SurviveContext *ctx = so->ctx; @@ -481,6 +486,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (error < 0) { if (d->failures_to_reset_cntr > 0) d->failures_to_reset_cntr--; + } else { + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; } return 0; -- cgit v1.3.1 From 378b6e58d4cb0e9873062cfabefa271e43794bd6 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 16:19:22 +0000 Subject: Broadcast lighthouse position at startup --- src/survive.c | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 3f3b844..1786d45 100644 --- a/src/survive.c +++ b/src/survive.c @@ -294,6 +294,13 @@ int survive_startup(SurviveContext *ctx) { } } + // If lighthouse positions are known, broadcast them + for (int i = 0; i < ctx->activeLighthouses; i++) { + if(ctx->bsd[i].PositionSet) { + ctx->lighthouseposeproc(ctx, i, &ctx->bsd[i].Pose, 0); + } + } + return 0; } -- cgit v1.3.1 From ef7f9229a23e3ade8c6b5c33cbfaa684b80909b2 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 24 Mar 2018 03:44:02 -0400 Subject: add option to log lightdata to separate file. --- src/survive_disambiguator.c | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_disambiguator.c b/src/survive_disambiguator.c index 39b23b6..cf89068 100644 --- a/src/survive_disambiguator.c +++ b/src/survive_disambiguator.c @@ -1,3 +1,16 @@ #include "survive.h" +#include +#include -void handle_lightcap(SurviveObject *so, LightcapElement *le) { so->ctx->lightcapfunction(so, le); } +//#define LOG_LIGHTDATA + +void handle_lightcap(SurviveObject *so, LightcapElement *le) +{ +#ifdef LOG_LIGHTDATA + static FILE * flog; + static double start = 0; + if( !flog ) { flog = fopen( "lightcap.txt", "wb" ); start = OGGetAbsoluteTime(); } + fprintf( flog, "%.6f %2d %4d %9d\n", OGGetAbsoluteTime()-start, le->sensor_id, le->length, le->timestamp ); +#endif + so->ctx->lightcapfunction(so, le); +} -- cgit v1.3.1 From 0d323e4a42bcfa70150c77f45f6f465f84666e31 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 24 Mar 2018 09:50:40 -0600 Subject: Made playback / record process raw light data --- src/poser.c | 6 ++++- src/survive_disambiguator.c | 3 +++ src/survive_playback.c | 55 ++++++++++++++++++++++++++++++++++++++++----- src/survive_playback.h | 2 +- 4 files changed, 58 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/poser.c b/src/poser.c index 3fb4fe8..2cfe28d 100644 --- a/src/poser.c +++ b/src/poser.c @@ -45,7 +45,11 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui // 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); + if (quatmagnitude(so->activations.accel)) { + quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); + } else { + object2objUp.Rot[0] = 1.0; + } // Calculate the pose of the lighthouse in this space ApplyPoseToPose(&lighthouse2objUp, &object2objUp, &lighthouse2obj); diff --git a/src/survive_disambiguator.c b/src/survive_disambiguator.c index cf89068..6c19475 100644 --- a/src/survive_disambiguator.c +++ b/src/survive_disambiguator.c @@ -1,11 +1,14 @@ #include "survive.h" + #include #include +#include "survive_playback.h" //#define LOG_LIGHTDATA void handle_lightcap(SurviveObject *so, LightcapElement *le) { + survive_recording_lightcap(so, le); #ifdef LOG_LIGHTDATA static FILE * flog; static double start = 0; diff --git a/src/survive_playback.c b/src/survive_playback.c index 9261bb5..172614b 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -16,6 +16,7 @@ typedef struct SurviveRecordingData { bool alwaysWriteStdOut; + bool writeRawLight; FILE *output_file; } SurviveRecordingData; @@ -96,6 +97,16 @@ void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, in angle, lh); } +void survive_recording_lightcap(SurviveObject *so, LightcapElement *le) { + SurviveRecordingData *recordingData = so->ctx->recptr; + if (recordingData == 0) + return; + + if (recordingData->writeRawLight) { + write_to_output(recordingData, "%s C %d %u %u\n", so->codename, le->sensor_id, le->timestamp, le->length); + } +} + void survive_recording_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { SurviveRecordingData *recordingData = so->ctx->recptr; @@ -133,6 +144,7 @@ void survive_recording_light_process(struct SurviveObject *so, int sensor_id, in LH_Axis = "Y"; break; } + write_to_output(recordingData, "%s %s %s %d %d %d %u %u %u\n", so->codename, LH_ID, LH_Axis, sensor_id, acode, timeinsweep, timecode, length, lh); } @@ -154,6 +166,7 @@ struct SurvivePlaybackData { FLT time_factor; double next_time_us; + bool hasRawLight; }; typedef struct SurvivePlaybackData SurvivePlaybackData; @@ -190,8 +203,31 @@ static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) { return 0; } -static int parse_and_run_lightcode(const char *line, - SurvivePlaybackData *driver) { +static int parse_and_run_rawlight(const char *line, SurvivePlaybackData *driver) { + driver->hasRawLight = 1; + + char dev[10]; + char op[10]; + LightcapElement le; + int rr = sscanf(line, "%s %s %hhu %u %hu\n", dev, op, &le.sensor_id, &le.timestamp, &le.length); + + SurviveObject *so = survive_get_so_by_name(driver->ctx, dev); + if (!so) { + static bool display_once = false; + SurviveContext *ctx = driver->ctx; + if (display_once == false) { + SV_ERROR("Could not find device named %s from lineno %d\n", dev, driver->lineno); + } + display_once = true; + + return -1; + } + + handle_lightcap(so, &le); + return 0; +} + +static int parse_and_run_lightcode(const char *line, SurvivePlaybackData *driver) { char lhn[10]; char axn[10]; char dev[10]; @@ -206,8 +242,7 @@ static int parse_and_run_lightcode(const char *line, &length, &lh); if (rr != 9) { - fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n", - driver->lineno, rr, line); + fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line); return -1; } @@ -263,13 +298,19 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { char dev[10]; char op[10]; - if (sscanf(line, "%8s %8s", dev, op) < 2) + if (sscanf(line, "%8s %8s", dev, op) < 2) { + free(line); return 0; + } - if ((op[0] != 'R' && op[0] != 'L' && op[0] != 'I') || op[1] != 0) + if (op[1] != 0) { return 0; + } switch (op[0]) { + case 'C': + parse_and_run_rawlight(line, driver); + break; case 'L': case 'R': parse_and_run_lightcode(line, driver); @@ -319,6 +360,8 @@ void survive_install_recording(SurviveContext *ctx) { if (record_to_stdout) { SV_INFO("Recording to stdout"); } + + ctx->recptr->writeRawLight = survive_configi(ctx, "record-rawlight", SC_GET, 1); } } diff --git a/src/survive_playback.h b/src/survive_playback.h index 52638a9..c895120 100644 --- a/src/survive_playback.h +++ b/src/survive_playback.h @@ -5,7 +5,7 @@ void survive_recording_config_process(SurviveObject *so, char *ct0conf, int len) void survive_recording_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, SurvivePose *obj); - +void survive_recording_lightcap(SurviveObject *so, LightcapElement *le); void survive_recording_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); void survive_recording_info_process(SurviveContext *ctx, const char *fault); void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, int acode, uint32_t timecode, FLT length, -- cgit v1.3.1 From b45e58d9f75747e8727ac6cd0594a921e945c7e2 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 24 Mar 2018 10:03:43 -0600 Subject: Fixed issue(s) which would write to the config file when it wasn't needed --- data_recorder.c | 2 +- src/survive_playback.c | 8 ++++---- src/survive_vive.c | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/data_recorder.c b/data_recorder.c index 427a953..968ae17 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -11,7 +11,7 @@ int main(int argc, char **argv) { if (ctx == 0) // implies -help or similiar return 0; - const char *dataout_file = survive_configs(ctx, "record", SC_SETCONFIG, ""); + const char *dataout_file = survive_configs(ctx, "record", SC_GET, ""); if (strlen(dataout_file) == 0) { survive_configi(ctx, "record-stdout", SC_SET | SC_OVERRIDE, 1); } diff --git a/src/survive_playback.c b/src/survive_playback.c index 172614b..e4321bc 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -342,8 +342,8 @@ static int playback_close(struct SurviveContext *ctx, void *_driver) { } void survive_install_recording(SurviveContext *ctx) { - const char *dataout_file = survive_configs(ctx, "record", SC_SETCONFIG, ""); - int record_to_stdout = survive_configi(ctx, "record-stdout", SC_SETCONFIG, 0); + const char *dataout_file = survive_configs(ctx, "record", SC_GET, ""); + int record_to_stdout = survive_configi(ctx, "record-stdout", SC_GET, 0); if (strlen(dataout_file) > 0 || record_to_stdout) { ctx->recptr = calloc(1, sizeof(struct SurviveRecordingData)); @@ -366,7 +366,7 @@ void survive_install_recording(SurviveContext *ctx) { } int DriverRegPlayback(SurviveContext *ctx) { - const char *playback_file = survive_configs(ctx, "playback", SC_SETCONFIG, ""); + const char *playback_file = survive_configs(ctx, "playback", SC_GET, ""); if (strlen(playback_file) == 0) { return 0; @@ -375,7 +375,7 @@ int DriverRegPlayback(SurviveContext *ctx) { SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData)); sp->ctx = ctx; sp->playback_dir = playback_file; - sp->time_factor = survive_configf(ctx, "playback-factor", SC_SETCONFIG, 1.f); + sp->time_factor = survive_configf(ctx, "playback-factor", SC_GET, 1.f); printf("%s\n", playback_file); diff --git a/src/survive_vive.c b/src/survive_vive.c index d431207..91f25af 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1720,7 +1720,7 @@ void init_SurviveObject(SurviveObject* so) { int DriverRegHTCVive( SurviveContext * ctx ) { - const char *playback_dir = survive_configs(ctx, "playback", SC_SETCONFIG, ""); + const char *playback_dir = survive_configs(ctx, "playback", SC_GET, ""); if(strlen(playback_dir) != 0) { SV_INFO("Playback is active; disabling USB driver"); return 0; -- cgit v1.3.1 From d5c42c4951261c401c5f9148ce2d6bb1402ce75b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 24 Mar 2018 10:13:23 -0600 Subject: Some minor fixups around command line processing --- calibrate.c | 11 +++++++---- calibrate_client.c | 3 +++ include/libsurvive/survive.h | 15 +++++++++++---- simple_pose_test.c | 3 +++ src/survive.c | 5 ++++- test.c | 3 +++ 6 files changed, 31 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/calibrate.c b/calibrate.c index e0426f7..569c5cb 100644 --- a/calibrate.c +++ b/calibrate.c @@ -393,9 +393,6 @@ char * const * gargv; void * SurviveThread(void *jnk) { - ctx = survive_init( gargc, gargv ); - - uint8_t i =0; for (i=0;i<32;++i) { sensor_name[i] = malloc(3); @@ -433,8 +430,14 @@ int main( int argc, char ** argv ) { gargc = argc; gargv = argv; + + ctx = survive_init(gargc, gargv); + if (ctx == 0) { // Implies --help or similiar + return -1; + } + // Create the survive thread - OGCreateThread( SurviveThread, 0 ); + OGCreateThread(SurviveThread, 0); // Wait for the survive thread to load while(!SurviveThreadLoaded){ OGUSleep(100); } diff --git a/calibrate_client.c b/calibrate_client.c index 43ab821..4471acf 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -162,6 +162,9 @@ int main( int argc, char ** argv ) */ ctx = survive_init( argc, argv ); + if (ctx == 0) { // Implies --help or similiar + return -1; + } survive_install_light_fn( ctx, my_light_process ); survive_install_imu_fn( ctx, my_imu_process ); diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 0b85bf3..b1c32cd 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -202,11 +202,18 @@ struct SurviveContext void survive_verify_FLT_size(uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. - - - - SurviveContext * survive_init_internal( int argc, char * const * argv ); + +/** + * Call survive_init to get a populated SurviveContext pointer. + * + * This also sets up a number of configuration values based on command line + * arguments. Pass 0, 0 to this function if you specifically do not want + * command line processing. + * + * Note that this function _can_ return null based on command line arguments, + * notably if -h was passed in. + */ static inline SurviveContext * survive_init( int argc, char * const * argv ) { survive_verify_FLT_size(sizeof(FLT)); diff --git a/simple_pose_test.c b/simple_pose_test.c index 34c15de..0772abb 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -158,6 +158,9 @@ int main( int argc, char ** argv ) double Start = OGGetAbsoluteTime(); ctx = survive_init( argc, argv ); + if (ctx == 0) { // Implies --help or similiar + return -1; + } //survive_install_button_fn(ctx, testprog_button_process); survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); diff --git a/src/survive.c b/src/survive.c index 1786d45..807e82f 100644 --- a/src/survive.c +++ b/src/survive.c @@ -177,7 +177,10 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { fprintf(stderr, " -p [poser] - use a specific defaultposer.\n"); fprintf(stderr, " -l [lighthouse count] - use a specific number of lighthoses.\n"); fprintf(stderr, " -c [config file] - set config file\n"); - fprintf(stderr, " -p [lighthouse count] - use a specific number of lighthoses.\n"); + fprintf(stderr, " --record [log file] - Write all events to the given record file.\n"); + fprintf(stderr, " --playback [log file] - Read events from the given file instead of USB devices.\n"); + fprintf(stderr, " --playback-factor [f] - Time factor of playback -- 1 is run at the same timing as " + "original, 0 is run as fast as possible.\n"); return 0; } diff --git a/test.c b/test.c index 8e620bc..ad7aaa0 100644 --- a/test.c +++ b/test.c @@ -142,6 +142,9 @@ int main( int argc, char ** argv ) double Start = OGGetAbsoluteTime(); ctx = survive_init( argc, argv ); + if (ctx == 0) { // Implies --help or similiar + return -1; + } survive_install_button_fn(ctx, testprog_button_process); survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); -- cgit v1.3.1 From c17ac29dcdb617f5d9d960e432a628747e0e63df Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 24 Mar 2018 10:26:53 -0600 Subject: Added support for flags on command-line --- src/survive.c | 19 ++++++++++++++----- src/survive_playback.c | 4 +--- 2 files changed, 15 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index 807e82f..73d6474 100644 --- a/src/survive.c +++ b/src/survive.c @@ -159,11 +159,18 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { } if (vartoupdate) { - if (av + 1 == argvend) { - fprintf(stderr, "Error: expected parameter after %s\n", *av); - showhelp = 1; + const char *name = *av + 2; // Skip the '--'; + bool flagArgument = (av + 1 == argvend) || av[1][0] == '-'; + + if (flagArgument) { + bool value = strncmp("no-", name, 3) != 0; + if (value == 0) { + name += 3; // Skip "no-" + } + survive_configi(ctx, name, SC_OVERRIDE | SC_SET, value); } else { - survive_configs(ctx, *av + 2, SC_OVERRIDE | SC_SET, *(av + 1)); + const char *value = *(av + 1); + survive_configs(ctx, name, SC_OVERRIDE | SC_SET, value); av++; } } @@ -273,7 +280,7 @@ int survive_startup(SurviveContext *ctx) { ctx->state = SURVIVE_RUNNING; int calibrateMandatory = survive_configi(ctx, "calibrate", SC_GET, 0); - int calibrateForbidden = survive_configi(ctx, "no-calibrate", SC_GET, 0); + int calibrateForbidden = survive_configi(ctx, "calibrate", SC_GET, 1) == 0; if (calibrateMandatory && calibrateForbidden) { SV_INFO("Contradictory settings --calibrate and --no-calibrate specified. Switching to normal behavior."); calibrateMandatory = calibrateForbidden = 0; @@ -288,6 +295,8 @@ int survive_startup(SurviveContext *ctx) { if (!isCalibrated) { SV_INFO("Uncalibrated configuration detected. Attaching calibration. Please don't move tracked objects for " "the duration of calibration. Pass '--no-calibrate' to skip calibration"); + } else { + SV_INFO("Calibration requested. Previous calibration will be overwritten."); } bool doCalibrate = isCalibrated == false || calibrateMandatory; diff --git a/src/survive_playback.c b/src/survive_playback.c index e4321bc..dc9a330 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -350,7 +350,7 @@ void survive_install_recording(SurviveContext *ctx) { ctx->recptr->output_file = fopen(dataout_file, "w"); if (ctx->recptr->output_file == 0 && !record_to_stdout) { - SV_INFO("Could not open %s for writing\n", dataout_file); + SV_INFO("Could not open %s for writing", dataout_file); free(ctx->recptr); ctx->recptr = 0; return; @@ -377,8 +377,6 @@ int DriverRegPlayback(SurviveContext *ctx) { sp->playback_dir = playback_file; sp->time_factor = survive_configf(ctx, "playback-factor", SC_GET, 1.f); - printf("%s\n", playback_file); - sp->playback_file = fopen(playback_file, "r"); if (sp->playback_file == 0) { fprintf(stderr, "Could not open playback events file %s", -- cgit v1.3.1 From c47826bb1171083377309e356168b46cb3621df7 Mon Sep 17 00:00:00 2001 From: dpeter99 Date: Sun, 25 Mar 2018 21:06:27 +0200 Subject: Started the VS project update --- .gitignore | 1 + src/poser.c | 5 ++++- src/poser_charlesslow.c | 2 +- src/poser_daveortho.c | 2 +- src/poser_epnp.c | 4 ++-- src/poser_sba.c | 22 +++++++++++----------- src/poser_turveytori.c | 2 +- src/survive_cal.c | 2 +- src/survive_playback.c | 8 ++++---- winbuild/calibrate/calibrate.vcxproj | 1 + winbuild/libsurvive/libsurvive.vcxproj | 7 ++++++- winbuild/libsurvive/libsurvive.vcxproj.filters | 21 ++++++++++++++++++--- 12 files changed, 51 insertions(+), 26 deletions(-) (limited to 'src') diff --git a/.gitignore b/.gitignore index e5f098b..712fde2 100644 --- a/.gitignore +++ b/.gitignore @@ -11,6 +11,7 @@ winbuild/calibrate/calinfo/* winbuild/calibrate/config.json winbuild/calibrate/x64/* winbuild/libsurvive/x64/* +winbuild/data_recorder/x64/* winbuild/.vs/* *.user *.ipch diff --git a/src/poser.c b/src/poser.c index 2cfe28d..1c638f8 100644 --- a/src/poser.c +++ b/src/poser.c @@ -3,6 +3,9 @@ #include #include +#define _USE_MATH_DEFINES // for C +#include + void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { if (poser_data->rawposeproc) { poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata); @@ -44,7 +47,7 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui ApplyPoseToPose(&lighthouse2obj, &arb2object, &lighthouse2arb); // Now find the space with the same origin, but rotated so that gravity is up - SurvivePose lighthouse2objUp = {}, object2objUp = {}; + SurvivePose lighthouse2objUp = {0}, object2objUp = {0}; if (quatmagnitude(so->activations.accel)) { quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); } else { diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index c7d9033..bc6683a 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -53,7 +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 = {}; + SurvivePose additionalTx = {0}; int lh, cycle; FLT dz, dy, dx; diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index c922b2e..c47bceb 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -127,7 +127,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) PoserDataFullScene * fs = (PoserDataFullScene*)pd; int LH_ID; - SurvivePose alignLh0ToXAxis = {}; + SurvivePose alignLh0ToXAxis = {0}; for( LH_ID = 0; LH_ID < 2; LH_ID++ ) { int i; diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 401ea2a..f5fa127 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -13,7 +13,7 @@ #include "stdio.h" static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool cameraToWorld) { - SurvivePose rtn = {}; + SurvivePose rtn = {0}; // std::cerr << "Solving for " << cal_imagePoints.size() << " correspondents" << std::endl; if (pnp->number_of_correspondences <= 3) { SurviveContext *ctx = so->ctx; @@ -128,7 +128,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { SurvivePose pose = solve_correspondence(so, &pnp, false); - SurvivePose txPose = {}; + 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]; diff --git a/src/poser_sba.c b/src/poser_sba.c index c01cc61..a1fdea6 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -44,7 +44,7 @@ void metric_function(int j, int i, double *aj, double *xij, void *adata) { SurviveObject *so = ctx->so; SurvivePose obj2world = ctx->obj_pose; - FLT sensorInWorld[3] = {}; + FLT sensorInWorld[3] = {0}; ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]); survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, xij); @@ -210,7 +210,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config pdl->hdr.pt = hdr.pt; pdl->hdr.rawposeproc = sba_set_position; - sba_set_position_t locations = {}; + sba_set_position_t locations = {0}; pdl->hdr.userdata = &locations; driver(so, &pdl->hdr); pdl->hdr = hdr; @@ -226,8 +226,8 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config } } - double opts[SBA_OPTSSZ] = {}; - double info[SBA_INFOSZ] = {}; + double opts[SBA_OPTSSZ] = {0}; + double info[SBA_INFOSZ] = {0}; sba_context_single_sweep ctx = {.hdr = {options, &pdl->hdr, so}, .acode = acode, .lh = lh}; @@ -306,7 +306,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o pdl->hdr.pt = hdr.pt; pdl->hdr.rawposeproc = sba_set_position; - sba_set_position_t locations = {}; + sba_set_position_t locations = {0}; pdl->hdr.userdata = &locations; driver(so, &pdl->hdr); pdl->hdr = hdr; @@ -323,8 +323,8 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o } } - double opts[SBA_OPTSSZ] = {}; - double info[SBA_INFOSZ] = {}; + double opts[SBA_OPTSSZ] = {0}; + double info[SBA_INFOSZ] = {0}; sba_context ctx = {options, &pdl->hdr, so}; @@ -399,7 +399,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd } else { SV_INFO("Not using a seed poser for SBA; results will likely be way off"); for (int i = 0; i < 2; i++) { - so->ctx->bsd[i].Pose = (SurvivePose){}; + so->ctx->bsd[i].Pose = (SurvivePose){0}; so->ctx->bsd[i].Pose.Rot[0] = 1.; } } @@ -407,8 +407,8 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd // PoserCharlesSlow(so, (PoserData *)pdfs); } - double opts[SBA_OPTSSZ] = {}; - double info[SBA_INFOSZ] = {}; + double opts[SBA_OPTSSZ] = {0}; + double info[SBA_INFOSZ] = {0}; opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; @@ -435,7 +435,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd info); // info if (status >= 0) { - SurvivePose additionalTx = {}; + SurvivePose additionalTx = {0}; 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); } else { diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 2d3f802..035fca7 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1443,7 +1443,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob lighthousePose.Pos[1] = refinedEstimateGd.y; lighthousePose.Pos[2] = refinedEstimateGd.z; - SurvivePose assumedObj = {}; + SurvivePose assumedObj = {0}; FLT negZ[3] = {0, 0, 1}; quatfrom2vectors(assumedObj.Rot, toriData->down, negZ); diff --git a/src/survive_cal.c b/src/survive_cal.c index 218f5c1..25e43b9 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -599,7 +599,7 @@ static void handle_calibration( struct SurviveCalData *cd ) for( obj = 0; obj < cd->numPoseObjects; obj++ ) { int i, j; - PoserDataFullScene fsd = {}; + PoserDataFullScene fsd = {0}; fsd.hdr.pt = POSERDATA_FULL_SCENE; for( j = 0; j < NUM_LIGHTHOUSES; j++ ) for( i = 0; i < SENSORS_PER_OBJECT; i++ ) diff --git a/src/survive_playback.c b/src/survive_playback.c index dc9a330..de26f73 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -6,7 +6,7 @@ #include #include -#include +//#include #include "survive_config.h" #include "survive_default_devices.h" @@ -274,7 +274,7 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { if (driver->next_time_us == 0) { char *buffer; size_t n = 0; - ssize_t r = getdelim(&line, &n, ' ', f); + int r = getdelim(&line, &n, ' ', f); if (r <= 0) return 0; @@ -292,7 +292,7 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { char *buffer; size_t n = 0; - ssize_t r = getline(&line, &n, f); + int r = getline(&line, &n, f); if (r <= 0) return 0; @@ -397,7 +397,7 @@ int DriverRegPlayback(SurviveContext *ctx) { while (!feof(sp->playback_file) && !ferror(sp->playback_file)) { char *line = 0; size_t n; - ssize_t r = getline(&line, &n, sp->playback_file); + int r = getline(&line, &n, sp->playback_file); if (r <= 0) continue; diff --git a/winbuild/calibrate/calibrate.vcxproj b/winbuild/calibrate/calibrate.vcxproj index 7a62c14..4392e49 100644 --- a/winbuild/calibrate/calibrate.vcxproj +++ b/winbuild/calibrate/calibrate.vcxproj @@ -111,6 +111,7 @@ Disabled USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) ..\..\windows;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) + true Console diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 725243a..3ce9dd3 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -150,6 +150,7 @@ + @@ -157,11 +158,15 @@ + - + + + + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 4c8c3c5..e96e220 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -39,9 +39,6 @@ Source Files - - Source Files - Source Files @@ -93,6 +90,24 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + -- cgit v1.3.1 From 7b6361a55e47dace4b1cfe36d8dba00a96424ad5 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 13:15:59 -0600 Subject: Made playback work --- src/survive_playback.c | 16 ++- winbuild/getdelim.c | 149 +++++++++++++++++++++++++ winbuild/libsurvive/libsurvive.vcxproj | 2 + winbuild/libsurvive/libsurvive.vcxproj.filters | 6 + 4 files changed, 167 insertions(+), 6 deletions(-) create mode 100644 winbuild/getdelim.c (limited to 'src') diff --git a/src/survive_playback.c b/src/survive_playback.c index de26f73..43a3c0b 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -6,7 +6,6 @@ #include #include -//#include #include "survive_config.h" #include "survive_default_devices.h" @@ -14,6 +13,14 @@ #include "os_generic.h" #include "stdarg.h" +#ifdef _WIN32 +typedef long ssize_t; +#define SSIZE_MAX LONG_MAX + +ssize_t getdelim(char ** lineptr, size_t * n, int delimiter, FILE *stream); +ssize_t getline(char **lineptr, size_t * n, FILE *stream); +#endif + typedef struct SurviveRecordingData { bool alwaysWriteStdOut; bool writeRawLight; @@ -267,14 +274,12 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { FILE *f = driver->playback_file; if (f && !feof(f) && !ferror(f)) { - int i; driver->lineno++; char *line; if (driver->next_time_us == 0) { - char *buffer; size_t n = 0; - int r = getdelim(&line, &n, ' ', f); + ssize_t r = getdelim(&line, &n, ' ', f); if (r <= 0) return 0; @@ -290,9 +295,8 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { return 0; driver->next_time_us = 0; - char *buffer; size_t n = 0; - int r = getline(&line, &n, f); + ssize_t r = getline(&line, &n, f); if (r <= 0) return 0; diff --git a/winbuild/getdelim.c b/winbuild/getdelim.c new file mode 100644 index 0000000..ca808ab --- /dev/null +++ b/winbuild/getdelim.c @@ -0,0 +1,149 @@ +/* Implementations of the getdelim() and getline() functions from POSIX 2008, + just in case your libc doesn't have them. + + getdelim() reads from a stream until a specified delimiter is encountered. + getline() reads from a stream until a newline is encountered. + + See: http://pubs.opengroup.org/onlinepubs/9699919799/functions/getdelim.html + + NOTE: It is always the caller's responsibility to free the line buffer, even + when an error occurs. + + Copyright (c) 2011 James E. Ingram + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + + +#include +#include +#include +#include + + +#if __STDC_VERSION__ >= 199901L +/* restrict is a keyword */ +#else +# define restrict +#endif + + +#ifndef _POSIX_SOURCE +typedef long ssize_t; +#define SSIZE_MAX LONG_MAX +#endif + + +ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, + FILE *restrict stream); +ssize_t getline(char **restrict lineptr, size_t *restrict n, + FILE *restrict stream); + + + +#define _GETDELIM_GROWBY 128 /* amount to grow line buffer by */ +#define _GETDELIM_MINLEN 4 /* minimum line buffer size */ + + +ssize_t getdelim(char **restrict lineptr, size_t *restrict n, int delimiter, + FILE *restrict stream) +{ + char *buf, *pos; + int c; + ssize_t bytes; + + if (lineptr == NULL || n == NULL) { + errno = EINVAL; + return -1; + } + if (stream == NULL) { + errno = EBADF; + return -1; + } + + /* resize (or allocate) the line buffer if necessary */ + buf = *lineptr; + if (buf == NULL || *n < _GETDELIM_MINLEN) { + buf = realloc(*lineptr, _GETDELIM_GROWBY); + if (buf == NULL) { + /* ENOMEM */ + return -1; + } + *n = _GETDELIM_GROWBY; + *lineptr = buf; + } + + /* read characters until delimiter is found, end of file is reached, or an + error occurs. */ + bytes = 0; + pos = buf; + while ((c = getc(stream)) != EOF) { + if (bytes + 1 >= SSIZE_MAX) { + errno = EOVERFLOW; + return -1; + } + bytes++; + if (bytes >= *n - 1) { + buf = realloc(*lineptr, *n + _GETDELIM_GROWBY); + if (buf == NULL) { + /* ENOMEM */ + return -1; + } + *n += _GETDELIM_GROWBY; + pos = buf + bytes - 1; + *lineptr = buf; + } + + *pos++ = (char) c; + if (c == delimiter) { + break; + } + } + + if (ferror(stream) || (feof(stream) && (bytes == 0))) { + /* EOF, or an error from getc(). */ + return -1; + } + + *pos = '\0'; + return bytes; +} + + +ssize_t getline(char **restrict lineptr, size_t *restrict n, + FILE *restrict stream) +{ + return getdelim(lineptr, n, '\n', stream); +} + + +#ifdef _TEST_GETDELIM + +/* TODO: this isn't a very extensive test. */ +int main(void) +{ + char *line = NULL; + size_t n = 0; + while (getline(&line, &n, stdin) > 0) { + printf("%s", line); + } + return 0; +} + +#endif diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 3ce9dd3..bf1ae2d 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -163,12 +163,14 @@ + + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index e96e220..d2dc6ba 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -108,6 +108,12 @@ Source Files + + Source Files + + + Source Files + -- cgit v1.3.1 From 287fc6886057e1773d572b2058222b38fd11122f Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:19:05 -0600 Subject: Fixed out of array stepping in charles-biguator --- src/survive_charlesbiguator.c | 35 +++++++++++++++++++++-------------- 1 file changed, 21 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/survive_charlesbiguator.c b/src/survive_charlesbiguator.c index fbba888..83b3681 100644 --- a/src/survive_charlesbiguator.c +++ b/src/survive_charlesbiguator.c @@ -1,6 +1,7 @@ //<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License. // #include "survive_internal.h" +#include #include /* for sqrt */ #include #include @@ -15,12 +16,16 @@ static int32_t decode_acode(uint32_t length, int32_t main_divisor) { if (acode & 1) return -1; - return (acode >> 1) - 6; + int32_t rtn = (acode >> 1) - 6; + if (rtn > 7 || rtn < 0) { + return -1; + } + return rtn; } //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ///////The charles disambiguator. Don't use this, mostly here for -///debugging./////////////////////////////////////////////////////// +/// debugging./////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// static void HandleOOTX(SurviveContext *ctx, SurviveObject *so) { @@ -169,21 +174,23 @@ void DisambiguatorCharles(SurviveObject *so, LightcapElement *le) { int32_t main_divisor = so->timebase_hz / 384000; // 125 @ 48 MHz. int acode = decode_acode(so->last_sync_length[0], main_divisor); - int whichlh; - if (acode < 0) - whichlh = 1; - else - whichlh = (acode >> 2); - int32_t dl = so->last_sync_time[whichlh]; - if (!so->did_handle_ootx) - HandleOOTX(ctx, so); + // If acode isn't right; don't even think of emitting an event + if (acode >= 0) { + int whichlh = (acode >> 2); + assert(whichlh <= 1); + int32_t dl = so->last_sync_time[whichlh]; - int32_t offset_from = le->timestamp - dl + le->length / 2; + if (!so->did_handle_ootx) + HandleOOTX(ctx, so); - // Make sure pulse is in valid window - if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time) { - ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh); + int32_t offset_from = le->timestamp - dl + le->length / 2; + + // Make sure pulse is in valid window + if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time && + offset_from > so->pulse_in_clear_time) { + ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh); + } } } else { // printf( "FAIL %d %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time - -- cgit v1.3.1 From 801e17d2c52c21adad5eff63265c1aaea2255b1b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:22:01 -0600 Subject: Added additional parameters to EPNP and SBA, made it so that degenerate poses didn't occur as easily in EPNP --- src/poser_epnp.c | 16 ++++++++++++++-- src/poser_sba.c | 25 ++++++++++++++++--------- 2 files changed, 30 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 401ea2a..ea1e735 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -27,6 +27,12 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer CvMat R = cvMat(3, 3, CV_64F, r); CvMat T = cvMat(3, 1, CV_64F, rtn.Pos); + + // Super degenerate inputs will project us basically right in the camera. Detect and reject + if (magnitude3d(rtn.Pos) < 0.25) { + return rtn; + } + // Requested output is camera -> world, so invert if (cameraToWorld) { FLT tmp[3]; @@ -81,7 +87,10 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) } SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0); + + if (quatmagnitude(lighthouse2object.Rot) != 0.0) { + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0); + } epnp_dtor(&pnp); } @@ -123,8 +132,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); add_correspondences(so, &pnp, scene, lightData); + 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 > 4) { + if (pnp.number_of_correspondences > required_meas) { SurvivePose pose = solve_correspondence(so, &pnp, false); diff --git a/src/poser_sba.c b/src/poser_sba.c index c01cc61..226f387 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -37,6 +37,9 @@ typedef struct SBAData { int failures_to_reset_cntr; int successes_to_reset; int successes_to_reset_cntr; + + int required_meas; + } SBAData; void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -175,7 +178,7 @@ 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*/, @@ -187,7 +190,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config size_t meas_size = construct_input_from_scene_single_sweep(so, pdl, scene, vmask, meas, acode, lh); static int failure_count = 500; - if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) { + if (so->ctx->bsd[0].PositionSet == 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); @@ -261,7 +264,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... static int cnt = 0; - if (cnt++ > 1000 || meas_size < 8) { + 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; @@ -270,7 +273,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config 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*/) { @@ -281,7 +284,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas); static int failure_count = 500; - if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 7) { + 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); @@ -361,7 +364,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... static int cnt = 0; - if (cnt++ > 1000 || meas_size < 8) { + if (cnt++ > 1000 || meas_size < d->required_meas) { SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); cnt = 0; @@ -456,16 +459,20 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd } int PoserSBA(SurviveObject *so, PoserData *pd) { + SurviveContext *ctx = so->ctx; if (so->PoserData == 0) { so->PoserData = calloc(1, sizeof(SBAData)); SBAData *d = so->PoserData; d->failures_to_reset_cntr = 0; - d->failures_to_reset = 5; + d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); d->successes_to_reset_cntr = 0; - d->successes_to_reset = 20; + d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 1); + + d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); + + SV_INFO("Initializing SBA with %d required measurements", d->required_meas); } SBAData *d = so->PoserData; - SurviveContext *ctx = so->ctx; switch (pd->pt) { case POSERDATA_LIGHT: { // No poses if calibration is ongoing -- cgit v1.3.1 From 31c9dcb9f783e4920f675010833739f9a3783eea Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:24:19 -0600 Subject: Made degenerate poses call SV_ERROR --- src/poser.c | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src') diff --git a/src/poser.c b/src/poser.c index 2cfe28d..7dcdc33 100644 --- a/src/poser.c +++ b/src/poser.c @@ -1,6 +1,7 @@ #include "math.h" #include #include +#include #include void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { @@ -18,6 +19,11 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui } else { const FLT up[3] = {0, 0, 1}; + if (quatmagnitude(lighthouse_pose->Rot) == 0) { + SurviveContext *ctx = so->ctx; + SV_ERROR("Pose func called with invalid pose."); + } + // 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 -- cgit v1.3.1 From d9ecb4d321bfa04b5d67fb501be0cd9c46140775 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 25 Mar 2018 21:29:32 -0600 Subject: Added tool to calculate various things about length of received pulses --- include/libsurvive/survive.h | 1 + src/survive_sensor_activations.c | 4 +- tools/lightlengthparams/Makefile | 15 ++++ tools/lightlengthparams/lightlengthparams.c | 112 ++++++++++++++++++++++++++++ 4 files changed, 131 insertions(+), 1 deletion(-) create mode 100644 tools/lightlengthparams/Makefile create mode 100644 tools/lightlengthparams/lightlengthparams.c (limited to 'src') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index b1c32cd..8a1474b 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -16,6 +16,7 @@ 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 + uint32_t lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks FLT accel[3]; FLT gyro[3]; diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index 4d1801c..e42b50e 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -21,9 +21,11 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa int axis = (lightData->acode & 1); uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis]; FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis]; + uint32_t *length = &self->lengths[lightData->sensor_id][lightData->lh][axis]; *angle = lightData->angle; *data_timecode = lightData->timecode; + *length = lightData->length * 48000000; } -uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); \ No newline at end of file +uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); diff --git a/tools/lightlengthparams/Makefile b/tools/lightlengthparams/Makefile new file mode 100644 index 0000000..b743f4f --- /dev/null +++ b/tools/lightlengthparams/Makefile @@ -0,0 +1,15 @@ +all : lightlengthparams + +SRT:=../.. + +LIBSURVIVE:=$(SRT)/lib/libsurvive.so + +CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE +LDFLAGS:=-lm -lpthread -llapacke -lcblas + +lightlengthparams : lightlengthparams.c $(LIBSURVIVE) + gcc $(CFLAGS) -o $@ $^ $(LDFLAGS) + +clean : + rm -rf lightlengthparams + diff --git a/tools/lightlengthparams/lightlengthparams.c b/tools/lightlengthparams/lightlengthparams.c new file mode 100644 index 0000000..3b28b3f --- /dev/null +++ b/tools/lightlengthparams/lightlengthparams.c @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include + +uint32_t current_timecode; + +void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, + uint32_t lighthouse) { + current_timecode = timecode; + survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); +} + +void raw_pose_process(SurviveObject *so, uint8_t _lh, SurvivePose *pose2w) { + survive_default_raw_pose_process(so, _lh, pose2w); + + /*printf("Pose: "); + for(int i = 0;i < 7;i++) { + printf("%f, ", pose2w->Pos[i]); + }; + printf("\n"); + */ + for (int lh = 0; lh < 2; lh++) { + SurvivePose pose2lh = {}; + SurvivePose w2lh = {}; + InvertPose(&w2lh, &so->ctx->bsd[lh].Pose); + ApplyPoseToPose(&pose2lh, &w2lh, pose2w); + + SurviveSensorActivations *scene = &so->activations; + for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) { + if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance / 2, + current_timecode, sensor_idx, lh)) { + uint32_t *lengths = scene->lengths[sensor_idx][lh]; + + const FLT *sensor_location = so->sensor_locations + 3 * sensor_idx; + const FLT *sensor_normals = so->sensor_normals + 3 * sensor_idx; + + LinmathPoint3d sensor = {}, sensorN = {}; + + scale3d(sensorN, sensor_normals, .01); + add3d(sensorN, sensor_location, sensor_normals); + + /* + printf("\n"); + printf("%f, %f, %f\n", sensor_location[0], sensor_location[1], sensor_location[2]); + printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]); + + FLT m[4][4]; + PoseToMatrix((FLT*)m, &pose2lh); + + for(int i = 0;i < 7;i++) { + printf("%f, ", pose2lh.Pos[i]); + }; + printf("\n"); + for(int i = 0;i < 4;i++) { + for(int j = 0;j < 4;j++) { + printf("%f, ", m[i][j]); + } + printf("\n"); + } + printf("\n"); + */ + ApplyPoseToPoint(sensor, &pose2lh, sensor_location); + ApplyPoseToPoint(sensorN, &pose2lh, sensorN); + + // printf("%f, %f, %f\n", sensor[0], sensor[1], sensor[2]); + // printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]); + FLT dist = magnitude3d(sensor); + LinmathVec3d zout = {0, 0, -dist}; + LinmathQuat r; + quatfrom2vectors(r, sensor, zout); + quatrotatevector(sensorN, r, sensorN); + sub3d(sensorN, sensorN, zout); + + FLT dot = dot3d(sensor, sensorN); + if (dist > 20 || dist < 0.25) + continue; + + FLT angs[2] = {}; + for (int axis = 0; axis < 2; axis++) { + angs[axis] = cos(atan2(fabs(sensorN[axis ? 0 : 1]), sensorN[2])); + } + FLT area = angs[0] * angs[1]; + + printf("%u\t%u\t%lu\t%f\t%f\t%f\t%f\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", current_timecode, lh, + sensor_idx, dist, angs[0], angs[1], area, lengths[0], lengths[1], dot, sensor[0], sensor[1], + sensor[2], sensorN[0], sensorN[1], sensorN[2]); + + // assert(angs[0] >= 0); + } + } + } +} + +int main(int argc, char **argv) { + SurviveContext *ctx = survive_init(argc, argv); + if (ctx == 0) // implies -help or similiar + return 0; + + printf("timecode\tlh\tsensor_idx\tdistance\tnorm_x\tnorm_y\tarea\tlength_x\tlength_y\tdot\tx\ty\tz\tnx\tny\tnz\n"); + + survive_startup(ctx); + + survive_install_raw_pose_fn(ctx, raw_pose_process); + survive_install_light_fn(ctx, light_process); + while (survive_poll(ctx) == 0) { + } + + survive_close(ctx); + return 0; +} -- cgit v1.3.1 From 18e717642be3af3b9f72b630dcad68ca17c32dc9 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 13:50:41 -0600 Subject: Made SBA calibrate with 1lh --- src/poser.c | 3 ++- src/poser_sba.c | 9 +++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/poser.c b/src/poser.c index 7dcdc33..ea5812e 100644 --- a/src/poser.c +++ b/src/poser.c @@ -21,7 +21,8 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui if (quatmagnitude(lighthouse_pose->Rot) == 0) { SurviveContext *ctx = so->ctx; - SV_ERROR("Pose func called with invalid pose."); + SV_INFO("Pose func called with invalid pose."); + return; } // Assume that the space solved for is valid but completely arbitrary. We are going to do a few things: diff --git a/src/poser_sba.c b/src/poser_sba.c index 226f387..82fbd56 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -439,8 +439,13 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd if (status >= 0) { 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); + for (int i = 0; i < NUM_LIGHTHOUSES; 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); + } + } + } else { SurviveContext *ctx = so->ctx; SV_INFO("SBA was unable to run %d", status); -- cgit v1.3.1 From f73076dd1703601e5e14ab740cd26d29e0b9593d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 15:16:52 -0600 Subject: Made EPNP actually use both LH --- src/poser_epnp.c | 54 +++++++++++++++++++++++++++++++----------------------- 1 file changed, 31 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index c294c0c..632737b 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 < 2; 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; -- cgit v1.3.1 From a2eef0e9d90a196f86dece20c6346c55af3bdbc0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 16:47:11 -0600 Subject: Made SBA and EPNP follow availableLighthouses --- src/poser_epnp.c | 2 +- src/poser_sba.c | 13 +++++++++---- src/survive_process.c | 4 +++- 3 files changed, 13 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 632737b..7e86542 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -125,7 +125,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { SurvivePose posers[2]; int meas[2] = {0, 0}; - for (int lh = 0; lh < 2; lh++) { + 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); diff --git a/src/poser_sba.c b/src/poser_sba.c index 069e1d0..8fa8c08 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -190,6 +190,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config 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; @@ -284,8 +285,12 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas); 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; @@ -421,7 +426,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 +444,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); 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); -- cgit v1.3.1 From f3877f7b13c4f19ac101a0c6c7d4122a46db9a76 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 12:35:42 -0600 Subject: Initial pass --- Makefile | 4 +- src/survive_tb_disambiguator.c | 143 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 145 insertions(+), 2 deletions(-) create mode 100644 src/survive_tb_disambiguator.c (limited to 'src') diff --git a/Makefile b/Makefile index f95ca61..e0c5306 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 -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable 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_tb_disambiguator.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c new file mode 100644 index 0000000..5d79429 --- /dev/null +++ b/src/survive_tb_disambiguator.c @@ -0,0 +1,143 @@ +// +#include "survive_internal.h" +#include +#include /* for sqrt */ +#include +#include +#include + +#define NUM_HISTORY 3 + +enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 }; + +typedef struct { + LightcapElement history[NUM_HISTORY]; + enum LightcapClassification classifications[NUM_HISTORY]; + int idx; +} SensorHistory_t; + +typedef struct { + LightcapElement last_sync; + SensorHistory_t histories[]; +} Disambiguator_data_t; + +Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) { + return calloc(sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct, 1); +} + +static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { + if (recent > prior) + return recent - prior; + return (0xFFFFFFFF - prior) + recent; +} + +#define LOWER_SYNC_TIME 2250 +#define UPPER_SYNC_TIME 6750 + +static int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; } + +static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHistory_t *history, + const LightcapElement *le) { + bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME; + + if (clearlyNotSync) { + return LCC_SWEEP; + } + + uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp); + uint32_t split_time = 399840; // 8.33ms in 48mhz + uint32_t jitter_allowance = 4000; + + // If we are ~8.33ms ahead of the last sync; we are a sync + if (d->last_sync.length > 0 && time_diff_last_sync < (split_time + jitter_allowance) && + time_diff_last_sync > (split_time - jitter_allowance)) { + return LCC_SYNC; + } + + if (d->last_sync.length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) { + return LCC_SWEEP; + } + + int prevIdx = circle_buffer_get(history->idx, -1); + uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); + + // We don't have recent data; unclear + if (time_diff > split_time - jitter_allowance) { + fprintf(stderr, "Time diff too high %d\n", time_diff); + return LCC_UNKNOWN; + } + + switch (history->classifications[prevIdx]) { + case LCC_SWEEP: + return LCC_SYNC; + } + fprintf(stderr, "last not sweep\n"); + return LCC_UNKNOWN; +} + +static enum LightcapClassification update_histories(Disambiguator_data_t *d, const LightcapElement *le) { + SensorHistory_t *history = &d->histories[le->sensor_id]; + + enum LightcapClassification classification = classify(d, history, le); + + history->classifications[history->idx] = classification; + history->history[history->idx] = *le; + history->idx = (history->idx + 1) % NUM_HISTORY; + + return classification; +} + +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; +} + +void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { + SurviveContext *ctx = so->ctx; + if (so->disambiguator_data == NULL) { + SV_INFO("Initializing Disambiguator Data"); + so->disambiguator_data = Disambiguator_data_t_ctor(so); + } + + Disambiguator_data_t *d = so->disambiguator_data; + + SensorHistory_t *history = &d->histories[le->sensor_id]; + int prevIdx = circle_buffer_get(history->idx, -1); + uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); + enum LightcapClassification classification = update_histories(d, le); + + SV_INFO("Classification %d\t%d\t%d\t%u\t%u(%.02fms)", classification, le->sensor_id, le->length, le->timestamp, + time_diff, (double)time_diff / 48000.0); + if (classification == LCC_SYNC) { + uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp); + if (time_diff_last_sync > (0xFFFFFFFF / 2)) + time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync; + d->last_sync = *le; + int acode = find_acode(le->length); + SV_INFO("acode: %d 0x%x a:%d d:%d s:%d (%.02fms)", le->length, acode, acode & 1, (bool)(acode & 2), + (bool)(acode & 4), (double)time_diff_last_sync / 48000.); + assert(acode != -1); + } +} + +REGISTER_LINKTIME(DisambiguatorTimeBased); -- cgit v1.3.1 From 3963b2bfe6954bf647d112b2a435910baa5529b2 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 19:22:54 -0600 Subject: Fixed text in menu text --- src/survive.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') 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; } -- cgit v1.3.1 From 0d10ca10f10a7c16828ae88ebd9a55fde5938efe Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 26 Mar 2018 23:20:31 -0600 Subject: Added notes before major overhaul --- src/survive_tb_disambiguator.c | 219 ++++++++++++++++++++++++++++++++--------- src/survive_vive.c | 10 -- 2 files changed, 173 insertions(+), 56 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index 5d79429..b4dd0ea 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -8,6 +8,46 @@ #define NUM_HISTORY 3 +/** + * The lighthouses go in the following order: + * + * Ticks State + * 0 ACode 0b1x0 (4) + * 20 000 ACode 0b0x0 (0) + * LH A X Sweep + * 400 000 ACode 0b1x1 (5) + * 420 000 ACode 0b0x1 (1) + * LH A Y SWEEP + * 800 000 ACode 0b0x0 (0) + * 820 000 ACode 0b1x0 (4) + * LH B X Sweep + * 1 200 000 ACode 0b0x1 (1) + * 1 220 000 ACode 0b1x1 (5) + * LH B Y SWEEP + * 1 600 000 < REPEAT > + * + * NOTE: Obviously you cut the data bit out for this + */ + +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 +}; + enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 }; typedef struct { @@ -17,12 +57,19 @@ typedef struct { } SensorHistory_t; typedef struct { - LightcapElement last_sync; + uint64_t last_sync_timestamp; + uint64_t last_sync_length; + int last_sync_count; + bool lastWasSync; + + uint32_t mod_offset; + LighthouseState state; + SensorHistory_t histories[]; } Disambiguator_data_t; Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) { - return calloc(sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct, 1); + return calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); } static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { @@ -31,11 +78,67 @@ static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { return (0xFFFFFFFF - prior) + recent; } +static int find_acode(uint32_t pulseLen) { + const static int offset = 0; + if (pulseLen < 2200 + 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; +} + #define LOWER_SYNC_TIME 2250 #define UPPER_SYNC_TIME 6750 static int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; } +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; + +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}; +} + +static uint32_t next_sync_expected(Disambiguator_data_t *d) { + int acode = find_acode(get_last_sync(d).length); + if (acode & SKIP_BIT) + return get_last_sync(d).timestamp + 20000; + return get_last_sync(d).timestamp + 399840; +} + static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHistory_t *history, const LightcapElement *le) { bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME; @@ -44,17 +147,24 @@ static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHisto return LCC_SWEEP; } - uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp); + uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp); uint32_t split_time = 399840; // 8.33ms in 48mhz - uint32_t jitter_allowance = 4000; + uint32_t jitter_allowance = 20000; // If we are ~8.33ms ahead of the last sync; we are a sync - if (d->last_sync.length > 0 && time_diff_last_sync < (split_time + jitter_allowance) && - time_diff_last_sync > (split_time - jitter_allowance)) { + if (get_last_sync(d).length > 0 && abs(timestamp_diff(le->timestamp, next_sync_expected(d))) < jitter_allowance) { return LCC_SYNC; } - if (d->last_sync.length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) { + LightcapElement last_sync = get_last_sync(d); + if (get_last_sync(d).length > 0 && overlaps(&last_sync, le)) { + return LCC_SYNC; + } + + if (le->length > 2000) + return LCC_SYNC; + + if (get_last_sync(d).length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) { return LCC_SWEEP; } @@ -62,7 +172,7 @@ static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHisto uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); // We don't have recent data; unclear - if (time_diff > split_time - jitter_allowance) { + if (time_diff > split_time) { fprintf(stderr, "Time diff too high %d\n", time_diff); return LCC_UNKNOWN; } @@ -79,43 +189,22 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con SensorHistory_t *history = &d->histories[le->sensor_id]; enum LightcapClassification classification = classify(d, history, le); - history->classifications[history->idx] = classification; history->history[history->idx] = *le; history->idx = (history->idx + 1) % NUM_HISTORY; - return classification; } -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; -} - void DisambiguatorTimeBased(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) { - SV_INFO("Initializing Disambiguator Data"); + SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct); so->disambiguator_data = Disambiguator_data_t_ctor(so); } @@ -126,18 +215,56 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); enum LightcapClassification classification = update_histories(d, le); - SV_INFO("Classification %d\t%d\t%d\t%u\t%u(%.02fms)", classification, le->sensor_id, le->length, le->timestamp, - time_diff, (double)time_diff / 48000.0); + uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp); + if (time_diff_last_sync > (0xFFFFFFFF / 2)) + time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync; + + LightcapElement lastSync = get_last_sync(d); + int acode = find_acode(lastSync.length); + if (classification == LCC_SYNC) { - uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp); - if (time_diff_last_sync > (0xFFFFFFFF / 2)) - time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync; - d->last_sync = *le; - int acode = find_acode(le->length); - SV_INFO("acode: %d 0x%x a:%d d:%d s:%d (%.02fms)", le->length, acode, acode & 1, (bool)(acode & 2), - (bool)(acode & 4), (double)time_diff_last_sync / 48000.); - assert(acode != -1); + LightcapElement lastSync = get_last_sync(d); + if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { + + if (lastSync.length) { + int acode = find_acode(lastSync.length); + SV_INFO("%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", + timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., + timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, + (bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1)); + assert(acode != -1); + } + 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; + + lastSync = get_last_sync(d); + // SV_INFO("acode building: %u %u %u", lastSync.length, lastSync.timestamp, lastSync.length + + // lastSync.timestamp); + } else { + if (d->lastWasSync) { + if (lastSync.length) { + int acode = find_acode(lastSync.length); + SV_INFO("start acode: %d 0x%x a:%d d:%d s:%d (%d)", lastSync.length, acode, acode & 1, + (bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1)); + assert(acode != -1); + } + } + d->lastWasSync = false; } + + if (classification == LCC_SWEEP) + SV_INFO("%.02fms Classification %d\t%d\t%d\t%u\t%u\t(%.02fms)\ttime since last sync: %.02fms a:%d d:%d s:%d", + (double)le->timestamp / 48000, classification, le->sensor_id, le->length, le->timestamp, time_diff, + (double)time_diff / 48000.0, (double)time_diff_last_sync / 48000., acode & 1, (bool)(acode & 2), + (bool)(acode & 4)); } REGISTER_LINKTIME(DisambiguatorTimeBased); 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, ""); -- cgit v1.3.1 From 0a4722c50ddfe12e59aba9c79ec1988f9b82f997 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 00:16:13 -0600 Subject: Find state works well --- src/survive_tb_disambiguator.c | 166 ++++++++++++++++++++++++++++------------- 1 file changed, 114 insertions(+), 52 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index b4dd0ea..dda55ff 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -30,7 +30,7 @@ */ enum LighthouseState { - LS_UNKNOWN = 0 + LS_UNKNOWN = 0, LS_WaitLHA_ACode4 = 1, LS_WaitLHA_ACode0, @@ -48,6 +48,40 @@ enum LighthouseState { LS_END }; +int LighthouseState_offset(enum LighthouseState s) { + int mini_jump = 20000; + int big_jump = 360000; + switch (s) { + case LS_WaitLHA_ACode4: + return 0; + case LS_WaitLHA_ACode0: + return mini_jump; + case LS_SweepAX: + return 2 * mini_jump; + case LS_WaitLHA_ACode5: + return 2 * mini_jump + big_jump; + case LS_WaitLHA_ACode1: + return 3 * mini_jump + big_jump; + case LS_SweepAY: + return 4 * mini_jump + big_jump; + case LS_WaitLHB_ACode0: + return 4 * mini_jump + 2 * big_jump; + case LS_WaitLHB_ACode4: + return 5 * mini_jump + 2 * big_jump; + case LS_SweepBX: + return 6 * mini_jump + 2 * big_jump; + case LS_WaitLHB_ACode1: + return 6 * mini_jump + 3 * big_jump; + case LS_WaitLHB_ACode5: + return 7 * mini_jump + 3 * big_jump; + case LS_SweepBY: + return 8 * mini_jump + 3 * big_jump; + case LS_END: + return 8 * mini_jump + 4 * big_jump; + } + return -1; +} + enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 }; typedef struct { @@ -57,19 +91,30 @@ typedef struct { } SensorHistory_t; typedef struct { + SurviveObject *so; + /** This part of the structure is general use when we know our state */ + uint32_t mod_offset; + enum LighthouseState state; + int confidence; + + /** This rest of the structure is dedicated to finding a state when we are unknown */ + + int encoded_acodes; + /* Keep running average of sync signals as they come in */ uint64_t last_sync_timestamp; uint64_t last_sync_length; int last_sync_count; - bool lastWasSync; - - uint32_t mod_offset; - LighthouseState state; + bool lastWasSync; SensorHistory_t histories[]; + } Disambiguator_data_t; Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) { - return calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); + Disambiguator_data_t *rtn = calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); + rtn->so = so; + + return rtn; } static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { @@ -195,45 +240,48 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con return classification; } -void DisambiguatorTimeBased(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; - } +static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapElement *le) { + SurviveContext *ctx = d->so->ctx; + LightcapElement lastSync = get_last_sync(d); + int acode = find_acode(lastSync.length); + SV_INFO("!!%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", + timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., + timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, (bool)(acode & 2), + (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); + + if (acode > 0) { + d->encoded_acodes &= 0xFFFF; + d->encoded_acodes = (d->encoded_acodes << 8) | (acode & (SKIP_BIT | AXIS_BIT)); + SV_INFO("%x", d->encoded_acodes); + switch (d->encoded_acodes) { + case (5 << 16) | (4 << 8) | 0: + return d->state = LS_SweepAX - 1; + case (0 << 16) | (5 << 8) | 1: + return d->state = LS_SweepAY - 1; + case (1 << 16) | (0 << 8) | 4: + return d->state = LS_SweepBX - 1; + case (4 << 16) | (1 << 8) | 5: + return d->state = LS_SweepBY - 1; + } - if (so->disambiguator_data == NULL) { - SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct); - so->disambiguator_data = Disambiguator_data_t_ctor(so); + } else { + d->encoded_acodes = 0; } - Disambiguator_data_t *d = so->disambiguator_data; + return LS_UNKNOWN; +} - SensorHistory_t *history = &d->histories[le->sensor_id]; - int prevIdx = circle_buffer_get(history->idx, -1); - uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); +static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) { enum LightcapClassification classification = update_histories(d, le); - uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp); - if (time_diff_last_sync > (0xFFFFFFFF / 2)) - time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync; - - LightcapElement lastSync = get_last_sync(d); - int acode = find_acode(lastSync.length); - if (classification == LCC_SYNC) { LightcapElement lastSync = get_last_sync(d); + if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { + enum LighthouseState new_state = EndSync(d, le); + if (new_state != LS_UNKNOWN) + return new_state; - if (lastSync.length) { - int acode = find_acode(lastSync.length); - SV_INFO("%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", - timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., - timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, - (bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1)); - assert(acode != -1); - } d->last_sync_timestamp = le->timestamp; d->last_sync_length = le->length; d->last_sync_count = 1; @@ -244,27 +292,41 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } d->lastWasSync = true; - - lastSync = get_last_sync(d); - // SV_INFO("acode building: %u %u %u", lastSync.length, lastSync.timestamp, lastSync.length + - // lastSync.timestamp); } else { - if (d->lastWasSync) { - if (lastSync.length) { - int acode = find_acode(lastSync.length); - SV_INFO("start acode: %d 0x%x a:%d d:%d s:%d (%d)", lastSync.length, acode, acode & 1, - (bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1)); - assert(acode != -1); - } - } d->lastWasSync = false; } - if (classification == LCC_SWEEP) - SV_INFO("%.02fms Classification %d\t%d\t%d\t%u\t%u\t(%.02fms)\ttime since last sync: %.02fms a:%d d:%d s:%d", - (double)le->timestamp / 48000, classification, le->sensor_id, le->length, le->timestamp, time_diff, - (double)time_diff / 48000.0, (double)time_diff_last_sync / 48000., acode & 1, (bool)(acode & 2), - (bool)(acode & 4)); + return LS_UNKNOWN; +} + +void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { d->state = LS_UNKNOWN; } + +void DisambiguatorTimeBased(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) { + SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct); + so->disambiguator_data = Disambiguator_data_t_ctor(so); + } + + Disambiguator_data_t *d = so->disambiguator_data; + + if (d->state == LS_UNKNOWN) { + enum LighthouseState new_state = AttemptFindState(d, le); + if (new_state != LS_UNKNOWN) { + d->confidence = 0; + d->mod_offset = (le->timestamp % LighthouseState_offset(LS_END)) - LighthouseState_offset(new_state); + d->state = new_state; + SV_INFO("Locked onto state %d at %d", new_state, d->mod_offset); + } + } else { + PropagateState(d, le); + } } REGISTER_LINKTIME(DisambiguatorTimeBased); -- cgit v1.3.1 From 0ff8f72a30a4c659815696074f672569d02e7718 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 08:15:39 -0600 Subject: State machine sorta works; but times are backwards?! --- src/survive_tb_disambiguator.c | 112 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 108 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index dda55ff..e09d3da 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -29,6 +29,10 @@ * NOTE: Obviously you cut the data bit out for this */ +// 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, @@ -48,6 +52,63 @@ enum LighthouseState { LS_END }; +void LighthouseState_Parameterize(enum LighthouseState s, int *acode, int *lh, int *axis, int *window) { + *lh = *axis = *acode = -1; + switch (s) { + case LS_WaitLHB_ACode4: + case LS_WaitLHA_ACode4: + case LS_WaitLHB_ACode0: + case LS_WaitLHA_ACode0: + case LS_WaitLHB_ACode5: + case LS_WaitLHA_ACode5: + case LS_WaitLHB_ACode1: + case LS_WaitLHA_ACode1: + *window = PULSE_WINDOW; + break; + case LS_SweepAX: + case LS_SweepAY: + case LS_SweepBX: + case LS_SweepBY: + *window = CAPTURE_WINDOW; + break; + } + + switch (s) { + case LS_WaitLHB_ACode4: + case LS_WaitLHA_ACode4: + *acode = 4; + break; + case LS_WaitLHB_ACode0: + case LS_WaitLHA_ACode0: + *acode = 0; + break; + case LS_WaitLHB_ACode5: + case LS_WaitLHA_ACode5: + *acode = 5; + break; + case LS_WaitLHB_ACode1: + case LS_WaitLHA_ACode1: + *acode = 1; + break; + case LS_SweepAX: + *axis = 0; + *lh = 0; + break; + case LS_SweepAY: + *axis = 1; + *lh = 0; + break; + case LS_SweepBX: + *axis = 0; + *lh = 1; + break; + case LS_SweepBY: + *axis = 1; + *lh = 1; + break; + } +} + int LighthouseState_offset(enum LighthouseState s) { int mini_jump = 20000; int big_jump = 360000; @@ -95,8 +156,9 @@ typedef struct { /** This part of the structure is general use when we know our state */ uint32_t mod_offset; enum LighthouseState state; + uint32_t last_state_transition_time; int confidence; - + uint32_t last_seen_time; /** This rest of the structure is dedicated to finding a state when we are unknown */ int encoded_acodes; @@ -299,7 +361,47 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh return LS_UNKNOWN; } -void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { d->state = LS_UNKNOWN; } +static void SetState(Disambiguator_data_t *d, const LightcapElement *le, enum LighthouseState new_state) { + + SurviveContext *ctx = d->so->ctx; + SV_INFO("State transition %d -> %d at %u(%.03f)", d->state, new_state, le->timestamp, + timestamp_diff(d->last_state_transition_time, le->timestamp) / 480000.); + + d->state = new_state; + if (d->state >= LS_END) + d->state = 1; + d->last_state_transition_time = le->timestamp; +} + +static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); +static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { + int acode = find_acode(le->length); + SurviveContext *ctx = d->so->ctx; + + SV_INFO("acode %d %d 0x%x", target_acode, le->length, acode); + + if (target_acode != (acode & (SKIP_BIT | AXIS_BIT))) + SetState(d, le, LS_UNKNOWN); +} + +static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { + int acode, lh, axis, window; + LighthouseState_Parameterize(d->state, &acode, &lh, &axis, &window); + + SurviveContext *ctx = d->so->ctx; + SV_INFO("param %u %d %d %d", le->timestamp, acode, le->length, window + d->last_state_transition_time); + + if (le->timestamp < d->last_state_transition_time + window) { + if (acode != -1) { + RunACodeCapture(acode, d, le); + } else { + // RunLightDataCapture(lh, axis, d, le); + } + } else { + SetState(d, le, d->state + 1); + PropagateState(d, le); + } +} void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { SurviveContext *ctx = so->ctx; @@ -315,14 +417,16 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } Disambiguator_data_t *d = so->disambiguator_data; + assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); + d->last_seen_time = le->timestamp; if (d->state == LS_UNKNOWN) { enum LighthouseState new_state = AttemptFindState(d, le); if (new_state != LS_UNKNOWN) { d->confidence = 0; d->mod_offset = (le->timestamp % LighthouseState_offset(LS_END)) - LighthouseState_offset(new_state); - d->state = new_state; - SV_INFO("Locked onto state %d at %d", new_state, d->mod_offset); + SetState(d, le, new_state); + SV_INFO("Locked onto state %d at %u", new_state, d->mod_offset); } } else { PropagateState(d, le); -- cgit v1.3.1 From 7bff842d6f01ea3a855d478734212ae0379f58e4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 08:53:20 -0600 Subject: Made state finder tolerant to one lh --- src/survive_tb_disambiguator.c | 79 +++++++++++++++++++++++++++--------------- 1 file changed, 51 insertions(+), 28 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index e09d3da..86cc1d3 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -12,17 +12,17 @@ * The lighthouses go in the following order: * * Ticks State - * 0 ACode 0b1x0 (4) - * 20 000 ACode 0b0x0 (0) + * 0 ACode 0b1x0 (4) <--- B + * 20 000 ACode 0b0x0 (0) <--- A/c * LH A X Sweep - * 400 000 ACode 0b1x1 (5) - * 420 000 ACode 0b0x1 (1) + * 400 000 ACode 0b1x1 (5) <--- B + * 420 000 ACode 0b0x1 (1) <--- A/c * LH A Y SWEEP - * 800 000 ACode 0b0x0 (0) - * 820 000 ACode 0b1x0 (4) + * 800 000 ACode 0b0x0 (0) <--- B + * 820 000 ACode 0b1x0 (4) <--- A/c * LH B X Sweep - * 1 200 000 ACode 0b0x1 (1) - * 1 220 000 ACode 0b1x1 (5) + * 1 200 000 ACode 0b0x1 (1) <--- B + * 1 220 000 ACode 0b1x1 (5) <--- A/c * LH B Y SWEEP * 1 600 000 < REPEAT > * @@ -302,6 +302,31 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con return classification; } +#define ACODE(s, d, a) ((s << 2) | (d << 1) | a) +#define SWEEP 0xFF + +static enum LighthouseState CheckEncodedAcode(Disambiguator_data_t *d, uint8_t newByte) { + SurviveContext *ctx = d->so->ctx; + d->encoded_acodes &= 0xFF; + d->encoded_acodes = (d->encoded_acodes << 8) | newByte; //(acode & (SKIP_BIT | AXIS_BIT)); + SV_INFO("0x%x", d->encoded_acodes); + + switch (d->encoded_acodes) { + case (ACODE(0, 1, 0) << 8) | SWEEP: + return LS_SweepAX + 1; + case (ACODE(0, 1, 1) << 8) | SWEEP: + return LS_SweepAY + 1; + case (SWEEP << 8) | (ACODE(0, 1, 1)): + return LS_SweepBX + 1; + case (SWEEP << 8) | (ACODE(1, 1, 0)): + return LS_SweepBY + 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) { SurviveContext *ctx = d->so->ctx; LightcapElement lastSync = get_last_sync(d); @@ -312,24 +337,10 @@ static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapEleme (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); if (acode > 0) { - d->encoded_acodes &= 0xFFFF; - d->encoded_acodes = (d->encoded_acodes << 8) | (acode & (SKIP_BIT | AXIS_BIT)); - SV_INFO("%x", d->encoded_acodes); - switch (d->encoded_acodes) { - case (5 << 16) | (4 << 8) | 0: - return d->state = LS_SweepAX - 1; - case (0 << 16) | (5 << 8) | 1: - return d->state = LS_SweepAY - 1; - case (1 << 16) | (0 << 8) | 4: - return d->state = LS_SweepBX - 1; - case (4 << 16) | (1 << 8) | 5: - return d->state = LS_SweepBY - 1; - } - + return CheckEncodedAcode(d, (acode | DATA_BIT)); } else { d->encoded_acodes = 0; } - return LS_UNKNOWN; } @@ -340,9 +351,17 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh LightcapElement lastSync = get_last_sync(d); if (d->lastWasSync == false || overlaps(&lastSync, le) == false) { - enum LighthouseState new_state = EndSync(d, le); - if (new_state != LS_UNKNOWN) - return new_state; + if (d->lastWasSync && timestamp_diff(lastSync.timestamp, le->timestamp) > 30000) { + // Missed a sweep window; clear encoded values. + d->encoded_acodes = 0; + } + + enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le); + + if (new_state != LS_UNKNOWN) { + fprintf(stderr, "new state: %d\n", new_state); + } + // return new_state; d->last_sync_timestamp = le->timestamp; d->last_sync_length = le->length; @@ -355,6 +374,10 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh d->lastWasSync = true; } else { + if (d->lastWasSync) { + enum LighthouseState new_state = EndSync(d, le); + fprintf(stderr, "Sweep start\n\n"); + } d->lastWasSync = false; } @@ -417,7 +440,7 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } Disambiguator_data_t *d = so->disambiguator_data; - assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); + // assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); d->last_seen_time = le->timestamp; if (d->state == LS_UNKNOWN) { @@ -425,7 +448,7 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { if (new_state != LS_UNKNOWN) { d->confidence = 0; d->mod_offset = (le->timestamp % LighthouseState_offset(LS_END)) - LighthouseState_offset(new_state); - SetState(d, le, new_state); + // SetState(d, le, new_state); SV_INFO("Locked onto state %d at %u", new_state, d->mod_offset); } } else { -- cgit v1.3.1 From d292e120dc997b4dad8d6d8a10ed86045a5f3e94 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 16:20:41 -0600 Subject: More or less works, just doesn't output --- src/survive_tb_disambiguator.c | 298 ++++++++++++++++++++++++----------------- 1 file changed, 172 insertions(+), 126 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index 86cc1d3..8bb4617 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -8,6 +8,8 @@ #define NUM_HISTORY 3 +//#define DEBUG_TB(...) SV_INFO(__VA_ARGS__) +#define DEBUG_TB(...) /** * The lighthouses go in the following order: * @@ -52,94 +54,60 @@ enum LighthouseState { LS_END }; -void LighthouseState_Parameterize(enum LighthouseState s, int *acode, int *lh, int *axis, int *window) { - *lh = *axis = *acode = -1; - switch (s) { - case LS_WaitLHB_ACode4: - case LS_WaitLHA_ACode4: - case LS_WaitLHB_ACode0: - case LS_WaitLHA_ACode0: - case LS_WaitLHB_ACode5: - case LS_WaitLHA_ACode5: - case LS_WaitLHB_ACode1: - case LS_WaitLHA_ACode1: - *window = PULSE_WINDOW; - break; - case LS_SweepAX: - case LS_SweepAY: - case LS_SweepBX: - case LS_SweepBY: - *window = CAPTURE_WINDOW; - break; - } - - switch (s) { - case LS_WaitLHB_ACode4: - case LS_WaitLHA_ACode4: - *acode = 4; - break; - case LS_WaitLHB_ACode0: - case LS_WaitLHA_ACode0: - *acode = 0; - break; - case LS_WaitLHB_ACode5: - case LS_WaitLHA_ACode5: - *acode = 5; - break; - case LS_WaitLHB_ACode1: - case LS_WaitLHA_ACode1: - *acode = 1; - break; - case LS_SweepAX: - *axis = 0; - *lh = 0; - break; - case LS_SweepAY: - *axis = 1; - *lh = 0; - break; - case LS_SweepBX: - *axis = 0; - *lh = 1; - break; - case LS_SweepBY: - *axis = 1; - *lh = 1; - break; - } -} +typedef struct { int acode, lh, axis, window, offset; } LighthouseStateParameters; + +const LighthouseStateParameters LS_Params[LS_END + 1] = { + {.acode = -1, .lh = -1, .axis = -1, .window = -1}, + + {.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 + {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 + {.acode = -1, + .lh = 0, + .axis = 0, + .window = CAPTURE_WINDOW, + .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 40000 + + {.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 + {.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 + {.acode = -1, + .lh = 0, + .axis = 1, + .window = CAPTURE_WINDOW, + .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 440000 + + {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 + {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 + {.acode = -1, + .lh = 1, + .axis = 0, + .window = CAPTURE_WINDOW, + .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 840000 + + {.acode = 1, + .lh = 1, + .axis = 1, + .window = PULSE_WINDOW, + .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1200000 + {.acode = 5, + .lh = 0, + .axis = 1, + .window = PULSE_WINDOW, + .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1220000 + {.acode = -1, + .lh = 1, + .axis = 1, + .window = CAPTURE_WINDOW, + .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1240000 + + {.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW} // 1600000 +}; -int LighthouseState_offset(enum LighthouseState s) { - int mini_jump = 20000; - int big_jump = 360000; - switch (s) { - case LS_WaitLHA_ACode4: - return 0; - case LS_WaitLHA_ACode0: - return mini_jump; - case LS_SweepAX: - return 2 * mini_jump; - case LS_WaitLHA_ACode5: - return 2 * mini_jump + big_jump; - case LS_WaitLHA_ACode1: - return 3 * mini_jump + big_jump; - case LS_SweepAY: - return 4 * mini_jump + big_jump; - case LS_WaitLHB_ACode0: - return 4 * mini_jump + 2 * big_jump; - case LS_WaitLHB_ACode4: - return 5 * mini_jump + 2 * big_jump; - case LS_SweepBX: - return 6 * mini_jump + 2 * big_jump; - case LS_WaitLHB_ACode1: - return 6 * mini_jump + 3 * big_jump; - case LS_WaitLHB_ACode5: - return 7 * mini_jump + 3 * big_jump; - case LS_SweepBY: - return 8 * mini_jump + 3 * big_jump; - case LS_END: - return 8 * mini_jump + 4 * big_jump; +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; } @@ -166,7 +134,7 @@ typedef struct { uint64_t last_sync_timestamp; uint64_t last_sync_length; int last_sync_count; - + int stabalize; bool lastWasSync; SensorHistory_t histories[]; @@ -186,8 +154,8 @@ static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { } static int find_acode(uint32_t pulseLen) { - const static int offset = 0; - if (pulseLen < 2200 + offset) + const static int offset = 50; + if (pulseLen < 2500 + offset) return -1; if (pulseLen < 3000 + offset) @@ -302,24 +270,46 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con return classification; } +#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].acode >= 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) { SurviveContext *ctx = d->so->ctx; d->encoded_acodes &= 0xFF; d->encoded_acodes = (d->encoded_acodes << 8) | newByte; //(acode & (SKIP_BIT | AXIS_BIT)); - SV_INFO("0x%x", d->encoded_acodes); + DEBUG_TB("0x%x", d->encoded_acodes); + LightcapElement lastSync = get_last_sync(d); switch (d->encoded_acodes) { case (ACODE(0, 1, 0) << 8) | SWEEP: - return LS_SweepAX + 1; + d->mod_offset = SolveForMod_Offset(d, LS_SweepAX - 1, &lastSync); + + return (LS_SweepAX + 1); case (ACODE(0, 1, 1) << 8) | SWEEP: - return LS_SweepAY + 1; + d->mod_offset = SolveForMod_Offset(d, LS_SweepAY - 1, &lastSync); + + return (LS_SweepAY + 1); case (SWEEP << 8) | (ACODE(0, 1, 1)): - return LS_SweepBX + 1; + d->mod_offset = SolveForMod_Offset(d, LS_WaitLHB_ACode1, &lastSync); + + return (LS_WaitLHB_ACode1 + 1); case (SWEEP << 8) | (ACODE(1, 1, 0)): - return LS_SweepBY + 1; + d->mod_offset = SolveForMod_Offset(d, LS_WaitLHA_ACode4, &lastSync); + + return (LS_WaitLHA_ACode4 + 1); } return LS_UNKNOWN; @@ -331,10 +321,10 @@ static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapEleme SurviveContext *ctx = d->so->ctx; LightcapElement lastSync = get_last_sync(d); int acode = find_acode(lastSync.length); - SV_INFO("!!%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", - timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., - timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, (bool)(acode & 2), - (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); + DEBUG_TB("!!%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", + timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., + timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, (bool)(acode & 2), + (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); if (acode > 0) { return CheckEncodedAcode(d, (acode | DATA_BIT)); @@ -347,21 +337,29 @@ static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapEleme static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) { enum LightcapClassification classification = update_histories(d, le); + static uint32_t start = 0; + if (start == 0) + start = le->timestamp; + SurviveContext *ctx = d->so->ctx; + DEBUG_TB("%d(%.03f) %d Incoming %u %u", (le->timestamp - start), (le->timestamp - start) / 48000., classification, + le->timestamp, le->length); + if (classification == LCC_SYNC) { LightcapElement lastSync = get_last_sync(d); 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. + SurviveContext *ctx = d->so->ctx; + // DEBUG_TB("Missed sweep window."); d->encoded_acodes = 0; } enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le); - if (new_state != LS_UNKNOWN) { - fprintf(stderr, "new state: %d\n", new_state); - } - // return new_state; + if (new_state != LS_UNKNOWN) + return new_state; d->last_sync_timestamp = le->timestamp; d->last_sync_length = le->length; @@ -376,7 +374,8 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh } else { if (d->lastWasSync) { enum LighthouseState new_state = EndSync(d, le); - fprintf(stderr, "Sweep start\n\n"); + if (new_state != LS_UNKNOWN) + return new_state; } d->lastWasSync = false; } @@ -384,45 +383,84 @@ static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const Ligh return LS_UNKNOWN; } -static void SetState(Disambiguator_data_t *d, const LightcapElement *le, enum LighthouseState new_state) { +static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le, + enum LighthouseState new_state) { SurviveContext *ctx = d->so->ctx; - SV_INFO("State transition %d -> %d at %u(%.03f)", d->state, new_state, le->timestamp, - timestamp_diff(d->last_state_transition_time, le->timestamp) / 480000.); + if (new_state >= LS_END) + new_state = 1; + + d->encoded_acodes = 0; + DEBUG_TB("State transition %d -> %d at %u(%.03f)", d->state, new_state, le->timestamp, + timestamp_diff(d->last_state_transition_time, le->timestamp) / 480000.); d->state = new_state; - if (d->state >= LS_END) - d->state = 1; d->last_state_transition_time = le->timestamp; + + d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0; + + 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) { + if (le->length < 100) + return; + int acode = find_acode(le->length); SurviveContext *ctx = d->so->ctx; - SV_INFO("acode %d %d 0x%x", target_acode, le->length, acode); + 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); + uint32_t error = time_error_d0 > time_error_d1 ? time_error_d1 : time_error_d0; - if (target_acode != (acode & (SKIP_BIT | AXIS_BIT))) - SetState(d, le, LS_UNKNOWN); + DEBUG_TB("acode %d %d 0x%x (%d)", target_acode, le->length, acode, error); + if (error > 1250) { + if (d->confidence-- == 0) { + SetState(d, le, LS_UNKNOWN); + assert(false); + } + return; + } + + if (d->confidence < 100) + d->confidence++; + d->last_sync_timestamp += le->timestamp; + d->last_sync_length += le->length; + d->last_sync_count++; } static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { - int acode, lh, axis, window; - LighthouseState_Parameterize(d->state, &acode, &lh, &axis, &window); + 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; + enum LighthouseState new_state = LighthouseState_findByOffset(le_offset); SurviveContext *ctx = d->so->ctx; - SV_INFO("param %u %d %d %d", le->timestamp, acode, le->length, window + d->last_state_transition_time); - - if (le->timestamp < d->last_state_transition_time + window) { - if (acode != -1) { - RunACodeCapture(acode, d, le); - } else { - // RunLightDataCapture(lh, axis, d, le); + DEBUG_TB("new %u %d %d %d %d", le->timestamp, le->length, le_offset, LS_Params[d->state].offset, + LS_Params[new_state].offset); + + if (d->state != new_state) { + if (d->last_sync_count > 0 && LS_Params[d->state].acode >= 0) { + LightcapElement lastSync = get_last_sync(d); + uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); + DEBUG_TB("New mod offset diff %d", (int)d->mod_offset - (int)mo); + d->mod_offset = mo; } + + SetState(d, le, new_state); + } + const LighthouseStateParameters *param = &LS_Params[d->state]; + + DEBUG_TB("param %u %d %d %d %d %d", le->timestamp, param->acode, le->length, le_offset, new_state, + LS_Params[d->state].offset); + + if (param->acode != -1) { + RunACodeCapture(param->acode, d, le); } else { - SetState(d, le, d->state + 1); - PropagateState(d, le); + DEBUG_TB("Logic for sweep %d", le->length); + // assert( le->length < 2200); + // RunLightDataCapture(lh, axis, d, le); } } @@ -435,21 +473,29 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } if (so->disambiguator_data == NULL) { - SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct); + DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct); so->disambiguator_data = Disambiguator_data_t_ctor(so); } Disambiguator_data_t *d = so->disambiguator_data; // assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); + if (d->stabalize < 500) { + d->stabalize++; + return; + } + d->last_seen_time = le->timestamp; if (d->state == LS_UNKNOWN) { enum LighthouseState new_state = AttemptFindState(d, le); if (new_state != LS_UNKNOWN) { + LightcapElement lastSync = get_last_sync(d); d->confidence = 0; - d->mod_offset = (le->timestamp % LighthouseState_offset(LS_END)) - LighthouseState_offset(new_state); - // SetState(d, le, new_state); - SV_INFO("Locked onto state %d at %u", new_state, d->mod_offset); + + 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); -- cgit v1.3.1 From 2e4625f76a44359fcc77b9131b18919703bba1be Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 27 Mar 2018 17:17:45 -0600 Subject: Added hooks; seems like it works but is noiser than other disambiguators? --- src/survive_tb_disambiguator.c | 110 +++++++++++++++++++++-------------------- 1 file changed, 56 insertions(+), 54 deletions(-) (limited to 'src') diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index 8bb4617..a807012 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -54,53 +54,34 @@ enum LighthouseState { LS_END }; -typedef struct { int acode, lh, axis, window, offset; } LighthouseStateParameters; +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 = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 0 - {.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 20000 - {.acode = -1, - .lh = 0, - .axis = 0, - .window = CAPTURE_WINDOW, - .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW}, // 40000 - - {.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW, .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 400000 - {.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW, .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 420000 - {.acode = -1, - .lh = 0, - .axis = 1, - .window = CAPTURE_WINDOW, - .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW}, // 440000 - - {.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW, .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 800000 - {.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW, .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 820000 - {.acode = -1, - .lh = 1, - .axis = 0, - .window = CAPTURE_WINDOW, - .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW}, // 840000 - - {.acode = 1, - .lh = 1, - .axis = 1, - .window = PULSE_WINDOW, - .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1200000 - {.acode = 5, - .lh = 0, - .axis = 1, - .window = PULSE_WINDOW, - .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1220000 - {.acode = -1, - .lh = 1, - .axis = 1, - .window = CAPTURE_WINDOW, - .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW}, // 1240000 + {.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 = 0, .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 = 1, .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++) { @@ -121,14 +102,16 @@ typedef struct { typedef struct { SurviveObject *so; + /** This part of the structure is general use when we know our state */ uint32_t mod_offset; enum LighthouseState state; uint32_t last_state_transition_time; int confidence; uint32_t last_seen_time; - /** This rest of the structure is dedicated to finding a state when we are unknown */ + LightcapElement sweep_data[SENSORS_PER_OBJECT]; + /** This rest of the structure is dedicated to finding a state when we are unknown */ int encoded_acodes; /* Keep running average of sync signals as they come in */ uint64_t last_sync_timestamp; @@ -276,7 +259,7 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con #define SWEEP 0xFF static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState state, const LightcapElement *le) { - assert(LS_Params[state].acode >= 0); // Doesn't work for sweep data + 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)); @@ -398,10 +381,16 @@ static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElem d->last_state_transition_time = le->timestamp; d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0; + memset(d->sweep_data, 0, sizeof(LightcapElement) * SENSORS_PER_OBJECT); return new_state; } +static void RunLightDataCapture(Disambiguator_data_t *d, const LightcapElement *le) { + if (le->length > d->sweep_data[le->sensor_id].length) + d->sweep_data[le->sensor_id] = *le; +} + static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { if (le->length < 100) @@ -416,10 +405,11 @@ static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const Lig DEBUG_TB("acode %d %d 0x%x (%d)", target_acode, le->length, acode, error); if (error > 1250) { - if (d->confidence-- == 0) { + if (d->confidence < 3) { SetState(d, le, LS_UNKNOWN); assert(false); } + d->confidence -= 3; return; } @@ -441,26 +431,41 @@ static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { LS_Params[new_state].offset); if (d->state != new_state) { - if (d->last_sync_count > 0 && LS_Params[d->state].acode >= 0) { - LightcapElement lastSync = get_last_sync(d); - uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); - DEBUG_TB("New mod offset diff %d", (int)d->mod_offset - (int)mo); - d->mod_offset = mo; + if (LS_Params[d->state].is_sweep == 0) { + if (d->last_sync_count > 0) { + LightcapElement lastSync = get_last_sync(d); + uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); + DEBUG_TB("New mod offset diff %d", (int)d->mod_offset - (int)mo); + d->mod_offset = mo; + int acode = find_acode(lastSync.length); + assert((acode | DATA_BIT) == (LS_Params[d->state].acode | DATA_BIT)); + ctx->lightproc(d->so, -LS_Params[d->state].lh - 1, acode, 0, lastSync.timestamp, lastSync.length, + LS_Params[d->state].lh); + } + } else { + for (int i = 0; i < SENSORS_PER_OBJECT; i++) { + if (d->sweep_data[i].length > 0) { + d->so->ctx->lightproc( + d->so, i, LS_Params[d->state].acode, + timestamp_diff(d->sweep_data[i].timestamp, d->mod_offset + LS_Params[d->state].offset), + d->sweep_data[i].timestamp, d->sweep_data[i].length, LS_Params[d->state].lh); + } + } } SetState(d, le, new_state); } - const LighthouseStateParameters *param = &LS_Params[d->state]; + const LighthouseStateParameters *param = &LS_Params[d->state]; DEBUG_TB("param %u %d %d %d %d %d", le->timestamp, param->acode, le->length, le_offset, new_state, LS_Params[d->state].offset); - if (param->acode != -1) { + if (param->is_sweep == 0) { RunACodeCapture(param->acode, d, le); } else { DEBUG_TB("Logic for sweep %d", le->length); // assert( le->length < 2200); - // RunLightDataCapture(lh, axis, d, le); + RunLightDataCapture(d, le); } } @@ -478,14 +483,11 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { } Disambiguator_data_t *d = so->disambiguator_data; - // assert(d->last_seen_time < le->timestamp || d->last_seen_time - le->timestamp > 0x8FFFFFFF); - if (d->stabalize < 500) { d->stabalize++; return; } - d->last_seen_time = le->timestamp; if (d->state == LS_UNKNOWN) { enum LighthouseState new_state = AttemptFindState(d, le); if (new_state != LS_UNKNOWN) { -- cgit v1.3.1 From ad76877aa0f75c30c35d4d6e0f5434828e36cba7 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 00:11:10 -0600 Subject: Finished! --- Makefile | 2 +- src/survive_tb_disambiguator.c | 72 ++++++++++++++++++++++-------------------- 2 files changed, 39 insertions(+), 35 deletions(-) (limited to 'src') diff --git a/Makefile b/Makefile index e0c5306..878d9c1 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 -O0 -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 -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-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 diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c index a807012..078743a 100644 --- a/src/survive_tb_disambiguator.c +++ b/src/survive_tb_disambiguator.c @@ -65,11 +65,11 @@ const LighthouseStateParameters LS_Params[LS_END + 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 = 0, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW,.is_sweep = 1}, // 40000 + {.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 = 1, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW,.is_sweep = 1}, // 440000 + {.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 @@ -102,6 +102,7 @@ typedef struct { typedef struct { SurviveObject *so; + uint32_t time_of_last_sync[NUM_LIGHTHOUSES]; /** This part of the structure is general use when we know our state */ uint32_t mod_offset; @@ -113,6 +114,7 @@ typedef struct { /** This rest of the structure is dedicated to finding a state when we are unknown */ int encoded_acodes; + /* Keep running average of sync signals as they come in */ uint64_t last_sync_timestamp; uint64_t last_sync_length; @@ -123,13 +125,6 @@ typedef struct { } Disambiguator_data_t; -Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) { - Disambiguator_data_t *rtn = calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); - rtn->so = so; - - return rtn; -} - static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) { if (recent > prior) return recent - prior; @@ -161,9 +156,6 @@ static int find_acode(uint32_t pulseLen) { return -1; } -#define LOWER_SYNC_TIME 2250 -#define UPPER_SYNC_TIME 6750 - static int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; } static bool overlaps(const LightcapElement *a, const LightcapElement *b) { @@ -180,6 +172,9 @@ 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}; @@ -386,11 +381,6 @@ static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElem return new_state; } -static void RunLightDataCapture(Disambiguator_data_t *d, const LightcapElement *le) { - if (le->length > d->sweep_data[le->sensor_id].length) - d->sweep_data[le->sensor_id] = *le; -} - static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le); static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) { if (le->length < 100) @@ -407,7 +397,8 @@ static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const Lig if (error > 1250) { if (d->confidence < 3) { SetState(d, le, LS_UNKNOWN); - assert(false); + // assert(false); + SV_INFO("WARNING: Disambiguator got lost; refinding state."); } d->confidence -= 3; return; @@ -431,24 +422,37 @@ static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { LS_Params[new_state].offset); if (d->state != new_state) { + static uint64_t sync2syncs[LS_END] = {0}; + static uint64_t sync2syncsCnt[LS_END] = {0}; + if (LS_Params[d->state].is_sweep == 0) { if (d->last_sync_count > 0) { LightcapElement lastSync = get_last_sync(d); uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); - DEBUG_TB("New mod offset diff %d", (int)d->mod_offset - (int)mo); + DEBUG_TB("New mod offset diff %d %u", (int)d->mod_offset - (int)mo, mo); d->mod_offset = mo; - int acode = find_acode(lastSync.length); - assert((acode | DATA_BIT) == (LS_Params[d->state].acode | DATA_BIT)); + + 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); + d->time_of_last_sync[LS_Params[d->state].lh] = lastSync.timestamp; } } else { for (int i = 0; i < SENSORS_PER_OBJECT; i++) { - if (d->sweep_data[i].length > 0) { - d->so->ctx->lightproc( - d->so, i, LS_Params[d->state].acode, - timestamp_diff(d->sweep_data[i].timestamp, d->mod_offset + LS_Params[d->state].offset), - d->sweep_data[i].timestamp, d->sweep_data[i].length, LS_Params[d->state].lh); + LightcapElement le = d->sweep_data[i]; + 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]); + 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); } } } @@ -457,15 +461,12 @@ static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { } const LighthouseStateParameters *param = &LS_Params[d->state]; - DEBUG_TB("param %u %d %d %d %d %d", le->timestamp, param->acode, le->length, le_offset, new_state, - LS_Params[d->state].offset); - if (param->is_sweep == 0) { RunACodeCapture(param->acode, d, le); } else { - DEBUG_TB("Logic for sweep %d", le->length); - // assert( le->length < 2200); - RunLightDataCapture(d, le); + if (le->length > d->sweep_data[le->sensor_id].length) { + d->sweep_data[le->sensor_id] = *le; + } } } @@ -479,11 +480,14 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) { if (so->disambiguator_data == NULL) { DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct); - so->disambiguator_data = Disambiguator_data_t_ctor(so); + Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct); + d->so = so; + so->disambiguator_data = d; } Disambiguator_data_t *d = so->disambiguator_data; - if (d->stabalize < 500) { + // It seems like the first few hundred lightcapelements are missing a ton of data; let it stabilize. + if (d->stabalize < 200) { d->stabalize++; return; } -- cgit v1.3.1 From bf92b059271e7be0db6f91660ad8abd1c71e8e73 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 00:59:14 -0600 Subject: Cleanup + Comments --- Makefile | 4 +- src/survive_statebased_disambiguator.c | 469 ++++++++++++++++++++++++++++++ src/survive_tb_disambiguator.c | 511 --------------------------------- 3 files changed, 471 insertions(+), 513 deletions(-) create mode 100644 src/survive_statebased_disambiguator.c delete mode 100644 src/survive_tb_disambiguator.c (limited to 'src') diff --git a/Makefile b/Makefile index 878d9c1..ee55a2d 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 -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses +CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -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 src/survive_tb_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/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 +#include /* for sqrt */ +#include +#include +#include + +//#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_tb_disambiguator.c b/src/survive_tb_disambiguator.c deleted file mode 100644 index 078743a..0000000 --- a/src/survive_tb_disambiguator.c +++ /dev/null @@ -1,511 +0,0 @@ -// -#include "survive_internal.h" -#include -#include /* for sqrt */ -#include -#include -#include - -#define NUM_HISTORY 3 - -//#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 - */ - -// 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; -} - -enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 }; - -typedef struct { - LightcapElement history[NUM_HISTORY]; - enum LightcapClassification classifications[NUM_HISTORY]; - int idx; -} SensorHistory_t; - -typedef struct { - SurviveObject *so; - uint32_t time_of_last_sync[NUM_LIGHTHOUSES]; - - /** This part of the structure is general use when we know our state */ - uint32_t mod_offset; - enum LighthouseState state; - uint32_t last_state_transition_time; - int confidence; - uint32_t last_seen_time; - LightcapElement sweep_data[SENSORS_PER_OBJECT]; - - /** This rest of the structure is dedicated to finding a state when we are unknown */ - int encoded_acodes; - - /* Keep running average of sync signals as they come in */ - uint64_t last_sync_timestamp; - uint64_t last_sync_length; - int last_sync_count; - int stabalize; - bool lastWasSync; - SensorHistory_t histories[]; - -} 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 int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; } - -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}; -} - -static uint32_t next_sync_expected(Disambiguator_data_t *d) { - int acode = find_acode(get_last_sync(d).length); - if (acode & SKIP_BIT) - return get_last_sync(d).timestamp + 20000; - return get_last_sync(d).timestamp + 399840; -} - -static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHistory_t *history, - const LightcapElement *le) { - bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME; - - if (clearlyNotSync) { - return LCC_SWEEP; - } - - uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp); - uint32_t split_time = 399840; // 8.33ms in 48mhz - uint32_t jitter_allowance = 20000; - - // If we are ~8.33ms ahead of the last sync; we are a sync - if (get_last_sync(d).length > 0 && abs(timestamp_diff(le->timestamp, next_sync_expected(d))) < jitter_allowance) { - return LCC_SYNC; - } - - LightcapElement last_sync = get_last_sync(d); - if (get_last_sync(d).length > 0 && overlaps(&last_sync, le)) { - return LCC_SYNC; - } - - if (le->length > 2000) - return LCC_SYNC; - - if (get_last_sync(d).length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) { - return LCC_SWEEP; - } - - int prevIdx = circle_buffer_get(history->idx, -1); - uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp); - - // We don't have recent data; unclear - if (time_diff > split_time) { - fprintf(stderr, "Time diff too high %d\n", time_diff); - return LCC_UNKNOWN; - } - - switch (history->classifications[prevIdx]) { - case LCC_SWEEP: - return LCC_SYNC; - } - fprintf(stderr, "last not sweep\n"); - return LCC_UNKNOWN; -} - -static enum LightcapClassification update_histories(Disambiguator_data_t *d, const LightcapElement *le) { - SensorHistory_t *history = &d->histories[le->sensor_id]; - - enum LightcapClassification classification = classify(d, history, le); - history->classifications[history->idx] = classification; - history->history[history->idx] = *le; - history->idx = (history->idx + 1) % NUM_HISTORY; - return classification; -} - -#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) { - SurviveContext *ctx = d->so->ctx; - d->encoded_acodes &= 0xFF; - d->encoded_acodes = (d->encoded_acodes << 8) | newByte; //(acode & (SKIP_BIT | AXIS_BIT)); - DEBUG_TB("0x%x", d->encoded_acodes); - LightcapElement lastSync = get_last_sync(d); - - 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) { - SurviveContext *ctx = d->so->ctx; - LightcapElement lastSync = get_last_sync(d); - int acode = find_acode(lastSync.length); - DEBUG_TB("!!%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)", - timestamp_diff(le->timestamp, lastSync.timestamp) / 48000., - timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1, (bool)(acode & 2), - (bool)(acode & 4), acode & (SKIP_BIT | AXIS_BIT)); - - if (acode > 0) { - return CheckEncodedAcode(d, (acode | DATA_BIT)); - } else { - d->encoded_acodes = 0; - } - return LS_UNKNOWN; -} - -static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) { - enum LightcapClassification classification = update_histories(d, le); - - static uint32_t start = 0; - if (start == 0) - start = le->timestamp; - SurviveContext *ctx = d->so->ctx; - DEBUG_TB("%d(%.03f) %d Incoming %u %u", (le->timestamp - start), (le->timestamp - start) / 48000., classification, - le->timestamp, le->length); - - if (classification == LCC_SYNC) { - LightcapElement lastSync = get_last_sync(d); - - 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. - SurviveContext *ctx = d->so->ctx; - // DEBUG_TB("Missed sweep window."); - d->encoded_acodes = 0; - } - - enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le); - - if (new_state != LS_UNKNOWN) - return new_state; - - 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 (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; - DEBUG_TB("State transition %d -> %d at %u(%.03f)", d->state, new_state, le->timestamp, - timestamp_diff(d->last_state_transition_time, le->timestamp) / 480000.); - - d->state = new_state; - d->last_state_transition_time = le->timestamp; - - d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0; - memset(d->sweep_data, 0, sizeof(LightcapElement) * SENSORS_PER_OBJECT); - - 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) { - if (le->length < 100) - return; - - int acode = find_acode(le->length); - SurviveContext *ctx = d->so->ctx; - - 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); - uint32_t error = time_error_d0 > time_error_d1 ? time_error_d1 : time_error_d0; - - DEBUG_TB("acode %d %d 0x%x (%d)", target_acode, le->length, acode, error); - if (error > 1250) { - if (d->confidence < 3) { - SetState(d, le, LS_UNKNOWN); - // assert(false); - SV_INFO("WARNING: Disambiguator got lost; refinding state."); - } - d->confidence -= 3; - return; - } - - if (d->confidence < 100) - d->confidence++; - d->last_sync_timestamp += le->timestamp; - d->last_sync_length += le->length; - d->last_sync_count++; -} - -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; - - enum LighthouseState new_state = LighthouseState_findByOffset(le_offset); - SurviveContext *ctx = d->so->ctx; - DEBUG_TB("new %u %d %d %d %d", le->timestamp, le->length, le_offset, LS_Params[d->state].offset, - LS_Params[new_state].offset); - - if (d->state != new_state) { - static uint64_t sync2syncs[LS_END] = {0}; - static uint64_t sync2syncsCnt[LS_END] = {0}; - - if (LS_Params[d->state].is_sweep == 0) { - if (d->last_sync_count > 0) { - LightcapElement lastSync = get_last_sync(d); - uint32_t mo = SolveForMod_Offset(d, d->state, &lastSync); - DEBUG_TB("New mod offset diff %d %u", (int)d->mod_offset - (int)mo, mo); - d->mod_offset = mo; - - 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); - d->time_of_last_sync[LS_Params[d->state].lh] = lastSync.timestamp; - } - } else { - for (int i = 0; i < SENSORS_PER_OBJECT; i++) { - LightcapElement le = d->sweep_data[i]; - 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]); - 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); - } - - 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) { - d->sweep_data[le->sensor_id] = *le; - } - } -} - -void DisambiguatorTimeBased(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(SensorHistory_t) * 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) { - LightcapElement lastSync = get_last_sync(d); - 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(DisambiguatorTimeBased); -- cgit v1.3.1 From b371fd666300279ef9a519d9d2bf0cbbdbffee1c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 10:35:09 -0600 Subject: Added ability to specify covariance of meas --- src/poser_sba.c | 182 ++++++++++++++++---------------------------------------- 1 file changed, 52 insertions(+), 130 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index 8fa8c08..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,111 +176,18 @@ 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; bool hasAllBSDs = true; @@ -349,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 @@ -359,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); @@ -479,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) { @@ -494,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; } -- cgit v1.3.1 From 91f0cab811e983da63ea49f6e24afae283138a1c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 20:36:05 -0600 Subject: Functional C# in windows --- bindings/cs/Demo/App.config | 6 ++ bindings/cs/Demo/Demo.csproj | 86 +++++++++++++++++++++ bindings/cs/Demo/Program.cs | 32 ++++++++ bindings/cs/Demo/Properties/AssemblyInfo.cs | 36 +++++++++ bindings/cs/LibSurviveBinding.sln | 54 +++++++++++-- bindings/cs/LibSurviveBinding/Binding.cs | 33 -------- bindings/cs/LibSurviveBinding/Config_entry.cs | 31 -------- .../cs/LibSurviveBinding/LibSurviveBinding.csproj | 18 ----- bindings/cs/LibSurviveBinding/LightcapElement.cs | 6 -- bindings/cs/LibSurviveBinding/Program.cs | 62 --------------- bindings/cs/LibSurviveBinding/SurviveContext.cs | 44 ----------- bindings/cs/LibSurviveBinding/SurviveObject.cs | 6 -- bindings/cs/LibSurviveBinding/SurvivePose.cs | 6 -- bindings/cs/LibSurviveBinding/config_group.cs | 14 ---- bindings/cs/libsurvive.net/SurviveContext.cs | 90 ++++++++++++++++++++++ bindings/cs/libsurvive.net/cfunctions.cs | 84 ++++++++++++++++++++ bindings/cs/libsurvive.net/libsurvive.net.csproj | 7 ++ calibrate.c | 2 - include/libsurvive/survive.h | 63 ++++++++------- redist/symbol_enumerator.c | 5 +- src/survive.c | 5 ++ src/survive_playback.c | 2 +- winbuild/libsurvive/libsurvive.def | 2 + winbuild/libsurvive/libsurvive.vcxproj | 5 +- winbuild/libsurvive/libsurvive.vcxproj.filters | 3 + 25 files changed, 441 insertions(+), 261 deletions(-) create mode 100644 bindings/cs/Demo/App.config create mode 100644 bindings/cs/Demo/Demo.csproj create mode 100644 bindings/cs/Demo/Program.cs create mode 100644 bindings/cs/Demo/Properties/AssemblyInfo.cs delete mode 100644 bindings/cs/LibSurviveBinding/Binding.cs delete mode 100644 bindings/cs/LibSurviveBinding/Config_entry.cs delete mode 100644 bindings/cs/LibSurviveBinding/LibSurviveBinding.csproj delete mode 100644 bindings/cs/LibSurviveBinding/LightcapElement.cs delete mode 100644 bindings/cs/LibSurviveBinding/Program.cs delete mode 100644 bindings/cs/LibSurviveBinding/SurviveContext.cs delete mode 100644 bindings/cs/LibSurviveBinding/SurviveObject.cs delete mode 100644 bindings/cs/LibSurviveBinding/SurvivePose.cs delete mode 100644 bindings/cs/LibSurviveBinding/config_group.cs create mode 100644 bindings/cs/libsurvive.net/SurviveContext.cs create mode 100644 bindings/cs/libsurvive.net/cfunctions.cs create mode 100644 bindings/cs/libsurvive.net/libsurvive.net.csproj (limited to 'src') diff --git a/bindings/cs/Demo/App.config b/bindings/cs/Demo/App.config new file mode 100644 index 0000000..731f6de --- /dev/null +++ b/bindings/cs/Demo/App.config @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/bindings/cs/Demo/Demo.csproj b/bindings/cs/Demo/Demo.csproj new file mode 100644 index 0000000..f347a5e --- /dev/null +++ b/bindings/cs/Demo/Demo.csproj @@ -0,0 +1,86 @@ + + + + + Debug + AnyCPU + {D3A43A2F-03B7-4A10-928F-686790036838} + Exe + Demo + Demo + v4.6.1 + 512 + true + publish\ + true + Disk + false + Foreground + 7 + Days + false + false + true + 0 + 1.0.0.%2a + false + false + true + + + AnyCPU + true + full + false + ..\x64\Debug\ + DEBUG;TRACE + prompt + 4 + false + + + AnyCPU + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + + + + + + + + + + + + + + + + + + + {da1b12d7-a121-4d40-b363-4893e4f88b8a} + libsurvive.net + + + + + False + Microsoft .NET Framework 4.6.1 %28x86 and x64%29 + true + + + False + .NET Framework 3.5 SP1 + false + + + + \ No newline at end of file diff --git a/bindings/cs/Demo/Program.cs b/bindings/cs/Demo/Program.cs new file mode 100644 index 0000000..1644360 --- /dev/null +++ b/bindings/cs/Demo/Program.cs @@ -0,0 +1,32 @@ +using libsurvive; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Text; +using System.Threading.Tasks; + +namespace Demo +{ + internal class MyHandler : SurviveContext + { + public MyHandler() + { + } + + public MyHandler(string[] args) : base(args) + { + } + } + class Program + { + static void Main(string[] args) + { + MyHandler handler = new MyHandler(args); + + while (handler.Poll() == 0) { + } + + } + + } +} diff --git a/bindings/cs/Demo/Properties/AssemblyInfo.cs b/bindings/cs/Demo/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..2c6a6c7 --- /dev/null +++ b/bindings/cs/Demo/Properties/AssemblyInfo.cs @@ -0,0 +1,36 @@ +using System.Reflection; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("Demo")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("Demo")] +[assembly: AssemblyCopyright("Copyright © 2018")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("d3a43a2f-03b7-4a10-928f-686790036838")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Build and Revision Numbers +// by using the '*' as shown below: +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] diff --git a/bindings/cs/LibSurviveBinding.sln b/bindings/cs/LibSurviveBinding.sln index 539470c..b2e2f11 100644 --- a/bindings/cs/LibSurviveBinding.sln +++ b/bindings/cs/LibSurviveBinding.sln @@ -1,20 +1,62 @@  Microsoft Visual Studio Solution File, Format Version 12.00 # Visual Studio 15 -VisualStudioVersion = 15.0.27428.2015 +VisualStudioVersion = 15.0.27130.2003 MinimumVisualStudioVersion = 10.0.40219.1 -Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "LibSurviveBinding", "LibSurviveBinding\LibSurviveBinding.csproj", "{3DEFC8F7-C61E-4BC2-B743-E39453C6BA10}" +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Demo", "Demo\Demo.csproj", "{D3A43A2F-03B7-4A10-928F-686790036838}" +EndProject +Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "libsurvive.net", "libsurvive.net\libsurvive.net.csproj", "{DA1B12D7-A121-4D40-B363-4893E4F88B8A}" + ProjectSection(ProjectDependencies) = postProject + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F} = {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libsurvive", "..\..\winbuild\libsurvive\libsurvive.vcxproj", "{435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}" EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU + Debug|x64 = Debug|x64 + Debug|x86 = Debug|x86 Release|Any CPU = Release|Any CPU + Release|x64 = Release|x64 + Release|x86 = Release|x86 EndGlobalSection GlobalSection(ProjectConfigurationPlatforms) = postSolution - {3DEFC8F7-C61E-4BC2-B743-E39453C6BA10}.Debug|Any CPU.ActiveCfg = Debug|Any CPU - {3DEFC8F7-C61E-4BC2-B743-E39453C6BA10}.Debug|Any CPU.Build.0 = Debug|Any CPU - {3DEFC8F7-C61E-4BC2-B743-E39453C6BA10}.Release|Any CPU.ActiveCfg = Release|Any CPU - {3DEFC8F7-C61E-4BC2-B743-E39453C6BA10}.Release|Any CPU.Build.0 = Release|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Debug|Any CPU.Build.0 = Debug|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Debug|x64.ActiveCfg = Debug|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Debug|x64.Build.0 = Debug|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Debug|x86.ActiveCfg = Debug|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Debug|x86.Build.0 = Debug|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Release|Any CPU.ActiveCfg = Release|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Release|Any CPU.Build.0 = Release|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Release|x64.ActiveCfg = Release|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Release|x64.Build.0 = Release|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Release|x86.ActiveCfg = Release|Any CPU + {D3A43A2F-03B7-4A10-928F-686790036838}.Release|x86.Build.0 = Release|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Debug|Any CPU.Build.0 = Debug|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Debug|x64.ActiveCfg = Debug|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Debug|x64.Build.0 = Debug|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Debug|x86.ActiveCfg = Debug|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Debug|x86.Build.0 = Debug|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Release|Any CPU.ActiveCfg = Release|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Release|Any CPU.Build.0 = Release|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Release|x64.ActiveCfg = Release|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Release|x64.Build.0 = Release|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Release|x86.ActiveCfg = Release|Any CPU + {DA1B12D7-A121-4D40-B363-4893E4F88B8A}.Release|x86.Build.0 = Release|Any CPU + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Debug|Any CPU.ActiveCfg = Debug|x64 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Debug|Any CPU.Build.0 = Debug|x64 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Debug|x64.ActiveCfg = Debug|x64 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Debug|x64.Build.0 = Debug|x64 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Debug|x86.ActiveCfg = Debug|Win32 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Debug|x86.Build.0 = Debug|Win32 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|Any CPU.ActiveCfg = Release|Win32 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x64.ActiveCfg = Release|x64 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x64.Build.0 = Release|x64 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x86.ActiveCfg = Release|Win32 + {435CFD2C-8724-42EE-8FDE-71EF7D2C6B2F}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/bindings/cs/LibSurviveBinding/Binding.cs b/bindings/cs/LibSurviveBinding/Binding.cs deleted file mode 100644 index f4f0652..0000000 --- a/bindings/cs/LibSurviveBinding/Binding.cs +++ /dev/null @@ -1,33 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Text; - -namespace LibSurviveBinding -{ - class Binding - { - /* - typedef int (* htc_config_func) (SurviveObject* so, char* ct0conf, int len); - typedef void (* text_feedback_func) (SurviveContext* ctx, const char* fault ); - typedef void (* light_process_func) (SurviveObject* so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); - typedef void (* imu_process_func) (SurviveObject* so, int mask, FLT* accelgyro, uint32_t timecode, int id); - 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* lighthouse_pose, - SurvivePose* object_pose); - */ - } - - public delegate int htc_config_func(IntPtr so, char ct0conf, int len); - public delegate void text_feedback_func(IntPtr ctx, string fault); - public delegate void light_process_func(IntPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse); - public delegate void imu_process_func(IntPtr so, int mask, double accelgyro, UInt32 timecode, int id); - public delegate void angle_process_func(IntPtr so, int sensor_id, int acode, UInt32 timecode, double length, double angle, UInt32 lh); - public delegate void button_process_func(IntPtr so, byte eventType, byte buttonId, byte axis1Id, UInt16 axis1Val, byte axis2Id, UInt16 axis2Val); - public delegate void raw_pose_func(IntPtr so, byte lighthouse, IntPtr pose); - public delegate void lighthouse_pose_func(IntPtr ctx, byte lighthouse, IntPtr lighthouse_pose, - IntPtr object_pose); - public delegate void handle_lightcap_func (IntPtr so, IntPtr le); - -} diff --git a/bindings/cs/LibSurviveBinding/Config_entry.cs b/bindings/cs/LibSurviveBinding/Config_entry.cs deleted file mode 100644 index 85a6701..0000000 --- a/bindings/cs/LibSurviveBinding/Config_entry.cs +++ /dev/null @@ -1,31 +0,0 @@ -namespace LibSurviveBinding -{ - - // internal class Config_entry - // { - // char[] tag; - // cval_type type; - // /** - // union { - //uint32_t i; - // FLT f; - // } - - // numeric; - // **/ - // char* data; - // uint32_t elements; - - // } - - // public enum cval_type - // { - // CONFIG_UNKNOWN = 0, - // CONFIG_FLOAT = 1, - // CONFIG_UINT32 = 2, - // CONFIG_STRING = 3, - // CONFIG_FLOAT_ARRAY = 4, - // } - - -} \ No newline at end of file diff --git a/bindings/cs/LibSurviveBinding/LibSurviveBinding.csproj b/bindings/cs/LibSurviveBinding/LibSurviveBinding.csproj deleted file mode 100644 index 646c16d..0000000 --- a/bindings/cs/LibSurviveBinding/LibSurviveBinding.csproj +++ /dev/null @@ -1,18 +0,0 @@ - - - - Exe - netcoreapp2.0 - - - - - Always - - - - - - - - diff --git a/bindings/cs/LibSurviveBinding/LightcapElement.cs b/bindings/cs/LibSurviveBinding/LightcapElement.cs deleted file mode 100644 index ca51fb9..0000000 --- a/bindings/cs/LibSurviveBinding/LightcapElement.cs +++ /dev/null @@ -1,6 +0,0 @@ -namespace LibSurviveBinding -{ - public class LightcapElement - { - } -} \ No newline at end of file diff --git a/bindings/cs/LibSurviveBinding/Program.cs b/bindings/cs/LibSurviveBinding/Program.cs deleted file mode 100644 index b6d0d8f..0000000 --- a/bindings/cs/LibSurviveBinding/Program.cs +++ /dev/null @@ -1,62 +0,0 @@ -using System; -using System.Runtime.InteropServices; - -namespace LibSurVive -{ - class Program - { - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - static extern IntPtr survive_init_internal(int argc, string[] args); - - public delegate void raw_pose_func(IntPtr so, byte lighthouse, IntPtr pose); - public delegate void lighthouse_pose_func(IntPtr ctx, byte lighthouse, IntPtr lighthouse_pose, IntPtr object_pose); - public delegate void light_process_func( IntPtr so, int sensor_id, int acode, int timeinsweep, - UInt32 timecode, UInt32 length, UInt32 lighthouse); - - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - static extern void survive_install_raw_pose_fn(IntPtr ctx, raw_pose_func fbp); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - static extern void survive_install_lighthouse_pose_fn(IntPtr ctx, lighthouse_pose_func fbp); - - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - static extern void survive_install_light_fn(IntPtr ctx, light_process_func fbp); - - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - static extern int survive_startup(IntPtr ctx); - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - static extern void survive_cal_install(IntPtr ctx); - - [DllImport("libsurvive", CallingConvention = CallingConvention.StdCall)] - static extern int survive_poll(IntPtr ctx); - - static void Main(string[] args) - { - IntPtr context = survive_init_internal(args.Length, args); - - survive_install_lighthouse_pose_fn(context, LighthousPos); - survive_install_raw_pose_fn(context, PositionUpdate); - survive_install_light_fn(context, LightUpdate); - - survive_startup(context); - survive_cal_install(context); - - while(survive_poll(context) == 0) {} - - } - - public static void LightUpdate( IntPtr so, int sensor_id, int acode, int timeinsweep, - UInt32 timecode, UInt32 length, UInt32 lighthouse) { - Console.WriteLine(timeinsweep); - } - - public static void PositionUpdate(IntPtr so, byte lighthouse, IntPtr pose) - { - Console.WriteLine(pose); - } - - public static void LighthousPos(IntPtr ctx, byte lighthouse, IntPtr lighthouse_pose, IntPtr object_pose) - { - - } - } -} diff --git a/bindings/cs/LibSurviveBinding/SurviveContext.cs b/bindings/cs/LibSurviveBinding/SurviveContext.cs deleted file mode 100644 index 2ba9b7c..0000000 --- a/bindings/cs/LibSurviveBinding/SurviveContext.cs +++ /dev/null @@ -1,44 +0,0 @@ -namespace LibSurviveBinding -{ - /* - public struct SurviveContext - { - text_feedback_func faultfunction; - text_feedback_func notefunction; - light_process_func lightproc; - imu_process_func imuproc; - angle_process_func angleproc; - button_process_func buttonproc; - raw_pose_func rawposeproc; - lighthouse_pose_func lighthouseposeproc; - htc_config_func configfunction; - handle_lightcap_func lightcapfunction; - - Config_group global_config_values; - Config_group* lh_config; //lighthouse configs - Config_group* temporary_config_values; //Set per-session, from command-line. Not saved but override global_config_values - - //Calibration data: - int activeLighthouses; - BaseStationData bsd[NUM_LIGHTHOUSES]; - SurviveCalData* calptr; //If and only if the calibration subsystem is attached. - struct SurviveRecordingData *recptr; // Iff recording is attached - SurviveObject** objs; - int objs_ct; - - void** drivers; - DeviceDriverCb* driverpolls; - DeviceDriverCb* drivercloses; - DeviceDriverMagicCb* drivermagics; - int driver_ct; - - SurviveState state; - - void* buttonservicethread; - ButtonQueue buttonQueue; - - void* user_ptr; - - } - */ -} \ No newline at end of file diff --git a/bindings/cs/LibSurviveBinding/SurviveObject.cs b/bindings/cs/LibSurviveBinding/SurviveObject.cs deleted file mode 100644 index bb9e3cd..0000000 --- a/bindings/cs/LibSurviveBinding/SurviveObject.cs +++ /dev/null @@ -1,6 +0,0 @@ -namespace LibSurviveBinding -{ - public class SurviveObject - { - } -} \ No newline at end of file diff --git a/bindings/cs/LibSurviveBinding/SurvivePose.cs b/bindings/cs/LibSurviveBinding/SurvivePose.cs deleted file mode 100644 index 9808d89..0000000 --- a/bindings/cs/LibSurviveBinding/SurvivePose.cs +++ /dev/null @@ -1,6 +0,0 @@ -namespace LibSurviveBinding -{ - public class SurvivePose - { - } -} \ No newline at end of file diff --git a/bindings/cs/LibSurviveBinding/config_group.cs b/bindings/cs/LibSurviveBinding/config_group.cs deleted file mode 100644 index 8a2f00e..0000000 --- a/bindings/cs/LibSurviveBinding/config_group.cs +++ /dev/null @@ -1,14 +0,0 @@ -using System; - -namespace LibSurviveBinding -{ - /* - internal class Config_group - { - Config_entry config_entries; - UInt16 used_entries; - UInt16 max_entries; - - } - */ -} \ No newline at end of file diff --git a/bindings/cs/libsurvive.net/SurviveContext.cs b/bindings/cs/libsurvive.net/SurviveContext.cs new file mode 100644 index 0000000..1fb8c11 --- /dev/null +++ b/bindings/cs/libsurvive.net/SurviveContext.cs @@ -0,0 +1,90 @@ +using System; + +namespace libsurvive +{ + using SurviveContextPtr = IntPtr; + using SurviveObjectPtr = IntPtr; + using SurvivePosePtr = IntPtr; + + public class SurviveContext : IDisposable + { + protected IntPtr ctx; + + public void Dispose() + { + cfunctions.survive_close(ctx); + ctx = IntPtr.Zero; + } + + public SurviveContext() : this(null) { } + + public SurviveContext(string[] args) + { + string[] newArgs = new string[args.Length + 1]; + newArgs[0] = System.Reflection.Assembly.GetEntryAssembly().FullName; + Array.Copy(args, 0, newArgs, 1, args.Length); + + ctx = cfunctions.survive_init_internal(newArgs.Length, newArgs); + + cfunctions.survive_install_raw_pose_fn(ctx, PoseEvent); + cfunctions.survive_install_light_fn(ctx, LightEvent); + cfunctions.survive_install_lighthouse_pose_fn(ctx, LightHouseEvent); + cfunctions.survive_install_angle_fn(ctx, AngleEvent); + cfunctions.survive_install_button_fn(ctx, ButtonEvent); + cfunctions.survive_install_htc_config_fn(ctx, HTCConfigEvent); + cfunctions.survive_install_imu_fn(ctx, IMUEvent); + cfunctions.survive_install_error_fn(ctx, ErrorEvent); + cfunctions.survive_install_info_fn(ctx, InfoEvent); + } + + protected void InfoEvent(SurviveObjectPtr ctx, string fault) + { + Console.Out.WriteLine(fault); + } + + protected void ErrorEvent(SurviveObjectPtr ctx, string fault) + { + Console.Error.WriteLine(fault); + } + + protected void IMUEvent(SurviveObjectPtr so, int mask, double[] accelgyro, uint timecode, int id) + { + cfunctions.survive_default_imu_process(so, mask, accelgyro, timecode, id); + } + + protected int HTCConfigEvent(SurviveObjectPtr so, string ct0conf, int len) + { + return cfunctions.survive_default_htc_config_process(so, ct0conf, len); + } + + protected void ButtonEvent(SurviveObjectPtr so, byte eventType, byte buttonId, byte axis1Id, ushort axis1Val, byte axis2Id, ushort axis2Val) + { + cfunctions.survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val); + } + + protected void AngleEvent(SurviveObjectPtr so, int sensor_id, int acode, uint timecode, double length, double angle, uint lh) + { + cfunctions.survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh); + } + + protected void LightHouseEvent(SurviveObjectPtr ctx, byte lighthouse, ref SurvivePose lighthouse_pose, ref SurvivePose object_pose) + { + cfunctions.survive_default_lighthouse_pose_process(ctx, lighthouse, ref lighthouse_pose, ref object_pose); + } + + protected void LightEvent(SurviveObjectPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse) + { + cfunctions.survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); + } + + protected void PoseEvent(IntPtr so, byte lighthouse, ref SurvivePose pose) + { + cfunctions.survive_default_raw_pose_process(so, lighthouse, ref pose); + } + + public int Poll() + { + return cfunctions.survive_poll(ctx); + } + } +} \ No newline at end of file diff --git a/bindings/cs/libsurvive.net/cfunctions.cs b/bindings/cs/libsurvive.net/cfunctions.cs new file mode 100644 index 0000000..f9d88f0 --- /dev/null +++ b/bindings/cs/libsurvive.net/cfunctions.cs @@ -0,0 +1,84 @@ +using System; +using System.Collections.Generic; +using System.Runtime.InteropServices; +using System.Text; + +namespace libsurvive +{ + using SurviveContextPtr = IntPtr; + using SurviveObjectPtr = IntPtr; + using SurvivePosePtr = IntPtr; + + [StructLayout(LayoutKind.Sequential)] + public struct SurvivePose + { + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)] + public double[] Pos; // Position in the form xyz + [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)] + public double[] Rot; // Quaternion in the form wxyz + } + + class cfunctions + { + +#pragma warning disable IDE1006 // Naming Styles + [DllImport("libsurvive")] + public static extern SurviveContextPtr survive_init_internal(int argc, string[] args); + [DllImport("libsurvive")] + public static extern SurviveContextPtr survive_close(SurviveContextPtr ctx); + [DllImport("libsurvive")] + public static extern int survive_poll(SurviveContextPtr ctx); + [DllImport("libsurvive")] + public static extern int survive_startup(SurviveContextPtr ctx); + + [DllImport("libsurvive")] + public static extern void survive_install_htc_config_fn(SurviveContextPtr ctx, htc_config_func fbp); + [DllImport("libsurvive")] + public static extern void survive_install_info_fn(SurviveContextPtr ctx, text_feedback_func fbp); + [DllImport("libsurvive")] + public static extern void survive_install_error_fn(SurviveContextPtr ctx, text_feedback_func fbp); + [DllImport("libsurvive")] + public static extern void survive_install_imu_fn(SurviveContextPtr ctx, imu_process_func fbp); + [DllImport("libsurvive")] + public static extern void survive_install_angle_fn(SurviveContextPtr ctx, angle_process_func fbp); + [DllImport("libsurvive")] + public static extern void survive_install_button_fn(SurviveContextPtr ctx, button_process_func fbp); + [DllImport("libsurvive")] + public static extern void survive_install_raw_pose_fn(SurviveContextPtr ctx, raw_pose_func fbp); + [DllImport("libsurvive")] + public static extern void survive_install_lighthouse_pose_fn(SurviveContextPtr ctx, lighthouse_pose_func fbp); + [DllImport("libsurvive")] + public static extern void survive_install_light_fn(SurviveContextPtr ctx, light_process_func fbp); + [DllImport("libsurvive")] + public static extern void survive_cal_install(SurviveContextPtr ctx); + + [DllImport("libsurvive")] + public static extern void survive_default_light_process(SurviveObjectPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lh); + [DllImport("libsurvive")] + public static extern void survive_default_imu_process(SurviveObjectPtr so, int mode, double[] accelgyro, UInt32 timecode, int id); + [DllImport("libsurvive")] + public static extern void survive_default_angle_process(SurviveObjectPtr so, int sensor_id, int acode, UInt32 timecode, double length, double angle, UInt32 lh); + [DllImport("libsurvive")] + public static extern void survive_default_button_process(SurviveObjectPtr so, byte eventType, byte buttonId, byte axis1Id, UInt16 axis1Val, byte axis2Id, UInt16 axis2Val); + [DllImport("libsurvive")] + public static extern void survive_default_raw_pose_process(SurviveObjectPtr so, byte lighthouse, ref SurvivePose pose); + [DllImport("libsurvive")] + public static extern void survive_default_lighthouse_pose_process(SurviveContextPtr ctx, byte lighthouse, ref SurvivePose lh_pose, + ref SurvivePose obj_pose); + [DllImport("libsurvive")] + public static extern int survive_default_htc_config_process(SurviveObjectPtr so, string ct0conf, int len); + +#pragma warning restore IDE1006 // Naming Styles + } + + public delegate int htc_config_func(SurviveObjectPtr so, string ct0conf, int len); + public delegate void text_feedback_func(SurviveContextPtr ctx, string fault); + public delegate void light_process_func(SurviveObjectPtr so, int sensor_id, int acode, int timeinsweep, UInt32 timecode, UInt32 length, UInt32 lighthouse); + public delegate void imu_process_func(SurviveObjectPtr so, int mask, double[] accelgyro, UInt32 timecode, int id); + public delegate void angle_process_func(SurviveObjectPtr so, int sensor_id, int acode, UInt32 timecode, double length, double angle, UInt32 lh); + public delegate void button_process_func(SurviveObjectPtr so, byte eventType, byte buttonId, byte axis1Id, UInt16 axis1Val, byte axis2Id, UInt16 axis2Val); + public delegate void raw_pose_func(SurviveObjectPtr so, byte lighthouse, ref SurvivePose pose); + public delegate void lighthouse_pose_func(SurviveContextPtr ctx, byte lighthouse, ref SurvivePose lighthouse_pose, + ref SurvivePose object_pose); + +} diff --git a/bindings/cs/libsurvive.net/libsurvive.net.csproj b/bindings/cs/libsurvive.net/libsurvive.net.csproj new file mode 100644 index 0000000..eac3503 --- /dev/null +++ b/bindings/cs/libsurvive.net/libsurvive.net.csproj @@ -0,0 +1,7 @@ + + + + netstandard1.6 + + + diff --git a/calibrate.c b/calibrate.c index 569c5cb..e21c310 100644 --- a/calibrate.c +++ b/calibrate.c @@ -404,8 +404,6 @@ void * SurviveThread(void *jnk) survive_install_angle_fn( ctx, my_angle_process ); survive_startup( ctx ); - - survive_cal_install( ctx ); if( !ctx ) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 8a1474b..85078f7 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -10,6 +10,13 @@ extern "C" { #endif + +#ifdef _WIN32 +#define SURVIVE_EXPORT __declspec(dllexport) +#else +#define SURVIVE_EXPORT +#endif + /** * This struct encodes what the last effective angles seen on a sensor were, and when they occured. */ @@ -203,7 +210,7 @@ struct SurviveContext void survive_verify_FLT_size(uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. -SurviveContext * survive_init_internal( int argc, char * const * argv ); +SURVIVE_EXPORT SurviveContext * survive_init_internal( int argc, char * const * argv ); /** * Call survive_init to get a populated SurviveContext pointer. @@ -224,20 +231,20 @@ static inline SurviveContext * survive_init( int argc, char * const * argv ) //For any of these, you may pass in 0 for the function pointer to use default behavior. //In general unless you are doing wacky things like recording or playing back data, you won't need to use this. -void survive_install_htc_config_fn( SurviveContext *ctx, htc_config_func fbp ); -void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ); -void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); -void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); -void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); -void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); -void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp); -void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp); -void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp); -int survive_startup( SurviveContext * ctx ); -int survive_poll( SurviveContext * ctx ); -void survive_close( SurviveContext * ctx ); - -SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); +SURVIVE_EXPORT void survive_install_htc_config_fn( SurviveContext *ctx, htc_config_func fbp ); +SURVIVE_EXPORT void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ); +SURVIVE_EXPORT void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); +SURVIVE_EXPORT void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); +SURVIVE_EXPORT void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); +SURVIVE_EXPORT void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); +SURVIVE_EXPORT void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp); +SURVIVE_EXPORT void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp); +SURVIVE_EXPORT void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp); +SURVIVE_EXPORT int survive_startup( SurviveContext * ctx ); +SURVIVE_EXPORT int survive_poll( SurviveContext * ctx ); +SURVIVE_EXPORT void survive_close( SurviveContext * ctx ); + +SURVIVE_EXPORT SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); //Utilitiy functions. int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); @@ -249,30 +256,30 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d #define SC_OVERRIDE 2 //Set, to new default value. #define SC_SETCONFIG 4 //Set, both in-memory and config file. Use in conjunction with SC_OVERRIDE. -FLT survive_configf( SurviveContext * ctx, const char *tag, char flags, FLT def ); -uint32_t survive_configi( SurviveContext * ctx, const char *tag, char flags, uint32_t def ); -const char * survive_configs( SurviveContext * ctx, const char *tag, char flags, const char *def ); +SURVIVE_EXPORT FLT survive_configf( SurviveContext * ctx, const char *tag, char flags, FLT def ); +SURVIVE_EXPORT uint32_t survive_configi( SurviveContext * ctx, const char *tag, char flags, uint32_t def ); +SURVIVE_EXPORT const char * survive_configs( SurviveContext * ctx, const char *tag, char flags, const char *def ); //Install the calibrator. -void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. +SURVIVE_EXPORT void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. // Read back a human-readable string description of the calibration status -int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length ); +SURVIVE_EXPORT int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length ); // Induce haptic feedback -int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount); +SURVIVE_EXPORT int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount); //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); -void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); -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 *lh_pose, +SURVIVE_EXPORT void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); +SURVIVE_EXPORT void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); +SURVIVE_EXPORT void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); +SURVIVE_EXPORT 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); +SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); +SURVIVE_EXPORT 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); +SURVIVE_EXPORT int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); diff --git a/redist/symbol_enumerator.c b/redist/symbol_enumerator.c index fcb3727..58bbfaa 100644 --- a/redist/symbol_enumerator.c +++ b/redist/symbol_enumerator.c @@ -60,7 +60,7 @@ BOOL WINAPI SymCleanup( HANDLE hProcess ); -BOOL CALLBACK __cdecl mycb( +BOOL mycb( PSYMBOL_INFO pSymInfo, ULONG SymbolSize, PVOID UserContext @@ -71,10 +71,11 @@ BOOL CALLBACK __cdecl mycb( int EnumerateSymbols( SymEnumeratorCallback cb ) { - HANDLE proc = GetCurrentProcess(); + HANDLE proc = GetCurrentProcess(); if( !SymInitialize( proc, 0, 1 ) ) return -1; if( !SymEnumSymbols( proc, 0, "*!*", &mycb, (void*)cb ) ) { + fprintf(stderr, "SymEnumSymbols returned %d\n", GetLastError()); SymCleanup(proc); return -2; } diff --git a/src/survive.c b/src/survive.c index 899d206..f46f128 100644 --- a/src/survive.c +++ b/src/survive.c @@ -111,6 +111,11 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { MANUAL_DRIVER_REGISTRATION(PoserDaveOrtho) MANUAL_DRIVER_REGISTRATION(PoserDummy) MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive) + MANUAL_DRIVER_REGISTRATION(DriverRegPlayback) + + MANUAL_DRIVER_REGISTRATION(DisambiguatorCharles) + MANUAL_DRIVER_REGISTRATION(DisambiguatorStateBased) + MANUAL_DRIVER_REGISTRATION(DisambiguatorTurvey) #endif SurviveContext *ctx = calloc(1, sizeof(SurviveContext)); diff --git a/src/survive_playback.c b/src/survive_playback.c index 43a3c0b..c73dd2f 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -275,7 +275,7 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { if (f && !feof(f) && !ferror(f)) { driver->lineno++; - char *line; + char *line = 0; if (driver->next_time_us == 0) { size_t n = 0; diff --git a/winbuild/libsurvive/libsurvive.def b/winbuild/libsurvive/libsurvive.def index fb2f05e..3a50ce6 100644 --- a/winbuild/libsurvive/libsurvive.def +++ b/winbuild/libsurvive/libsurvive.def @@ -10,6 +10,7 @@ EXPORTS survive_install_light_fn survive_install_imu_fn survive_install_angle_fn + survive_install_htc_config_fn survive_send_magic survive_cal_install survive_cal_get_status @@ -26,3 +27,4 @@ EXPORTS survive_default_lighthouse_pose_process quatnormalize quatrotatevector + \ No newline at end of file diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index acf8500..6268d63 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -27,7 +27,7 @@ - StaticLibrary + DynamicLibrary true v141 MultiByte @@ -77,7 +77,7 @@ Level3 Disabled - FLT=double;USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;%(PreprocessorDefinitions) + FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;%(PreprocessorDefinitions) ..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) @@ -169,6 +169,7 @@ + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 717038c..5387108 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -105,6 +105,9 @@ Source Files + + Source Files + -- cgit v1.3.1 From 08cc0afc797d2225cf23fbc785e6a28cc8667285 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 28 Mar 2018 21:52:00 -0600 Subject: Nuget packaged up dependencies --- redist/minimal_opencv.c | 172 ++++++++++++++++++++++++- src/epnp/epnp.c | 2 +- src/poser_sba.c | 1 + src/survive.c | 5 +- src/survive_cal.c | 2 +- winbuild/calibrate/calibrate.vcxproj | 11 ++ winbuild/calibrate/calibrate.vcxproj.filters | 3 + winbuild/data_recorder/data_recorder.vcxproj | 1 + winbuild/libsurvive/libsurvive.vcxproj | 33 ++++- winbuild/libsurvive/libsurvive.vcxproj.filters | 37 ++++++ winbuild/libsurvive/packages.config | 5 + winbuild/test/test.vcxproj | 4 +- 12 files changed, 265 insertions(+), 11 deletions(-) create mode 100644 winbuild/libsurvive/packages.config (limited to 'src') diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c index 50eb7df..e35e31d 100644 --- a/redist/minimal_opencv.c +++ b/redist/minimal_opencv.c @@ -1,5 +1,3 @@ -//#include "/home/justin/source/CLAPACK/INCLUDE/f2c.h" -//#include "/home/justin/source/CLAPACK/INCLUDE/clapack.h" #include #include @@ -10,8 +8,176 @@ #include "string.h" #include +#include + //#define DEBUG_PRINT +#ifdef _WIN3211 +#include "cblas.h" + +int CBLAS_CallFromC; +int RowMajorStrg; +void cblas_xerbla(int info, const char *rout, const char *form, ...) +{ + extern int RowMajorStrg; + char empty[1] = ""; + va_list argptr; + + va_start(argptr, form); + + if (RowMajorStrg) + { + if (strstr(rout, "gemm") != 0) + { + if (info == 5) info = 4; + else if (info == 4) info = 5; + else if (info == 11) info = 9; + else if (info == 9) info = 11; + } + else if (strstr(rout, "symm") != 0 || strstr(rout, "hemm") != 0) + { + if (info == 5) info = 4; + else if (info == 4) info = 5; + } + else if (strstr(rout, "trmm") != 0 || strstr(rout, "trsm") != 0) + { + if (info == 7) info = 6; + else if (info == 6) info = 7; + } + else if (strstr(rout, "gemv") != 0) + { + if (info == 4) info = 3; + else if (info == 3) info = 4; + } + else if (strstr(rout, "gbmv") != 0) + { + if (info == 4) info = 3; + else if (info == 3) info = 4; + else if (info == 6) info = 5; + else if (info == 5) info = 6; + } + else if (strstr(rout, "ger") != 0) + { + if (info == 3) info = 2; + else if (info == 2) info = 3; + else if (info == 8) info = 6; + else if (info == 6) info = 8; + } + else if ((strstr(rout, "her2") != 0 || strstr(rout, "hpr2") != 0) + && strstr(rout, "her2k") == 0) + { + if (info == 8) info = 6; + else if (info == 6) info = 8; + } + } + if (info) + fprintf(stderr, "Parameter %d to routine %s was incorrect\n", info, rout); + vfprintf(stderr, form, argptr); + va_end(argptr); + if (info && !info) + F77_xerbla(empty, &info); /* Force link of our F77 error handler */ + exit(-1); +} +void cblas_dgemm(const CBLAS_LAYOUT layout, const CBLAS_TRANSPOSE TransA, + const CBLAS_TRANSPOSE TransB, const int M, const int N, + const int K, const double alpha, const double *A, + const int lda, const double *B, const int ldb, + const double beta, double *C, const int ldc) +{ + char TA, TB; +#ifdef F77_CHAR + F77_CHAR F77_TA, F77_TB; +#else +#define F77_TA &TA +#define F77_TB &TB +#endif + +#ifdef F77_INT + F77_INT F77_M = M, F77_N = N, F77_K = K, F77_lda = lda, F77_ldb = ldb; + F77_INT F77_ldc = ldc; +#else +#define F77_M M +#define F77_N N +#define F77_K K +#define F77_lda lda +#define F77_ldb ldb +#define F77_ldc ldc +#endif + + RowMajorStrg = 0; + CBLAS_CallFromC = 1; + + if (layout == CblasColMajor) + { + if (TransA == CblasTrans) TA = 'T'; + else if (TransA == CblasConjTrans) TA = 'C'; + else if (TransA == CblasNoTrans) TA = 'N'; + else + { + cblas_xerbla(2, "cblas_dgemm", "Illegal TransA setting, %d\n", TransA); + CBLAS_CallFromC = 0; + RowMajorStrg = 0; + return; + } + + if (TransB == CblasTrans) TB = 'T'; + else if (TransB == CblasConjTrans) TB = 'C'; + else if (TransB == CblasNoTrans) TB = 'N'; + else + { + cblas_xerbla(3, "cblas_dgemm", "Illegal TransB setting, %d\n", TransB); + CBLAS_CallFromC = 0; + RowMajorStrg = 0; + return; + } + +#ifdef F77_CHAR + F77_TA = C2F_CHAR(&TA); + F77_TB = C2F_CHAR(&TB); +#endif + + F77_dgemm(F77_TA, F77_TB, &F77_M, &F77_N, &F77_K, &alpha, A, + &F77_lda, B, &F77_ldb, &beta, C, &F77_ldc); + } + else if (layout == CblasRowMajor) + { + RowMajorStrg = 1; + if (TransA == CblasTrans) TB = 'T'; + else if (TransA == CblasConjTrans) TB = 'C'; + else if (TransA == CblasNoTrans) TB = 'N'; + else + { + cblas_xerbla(2, "cblas_dgemm", "Illegal TransA setting, %d\n", TransA); + CBLAS_CallFromC = 0; + RowMajorStrg = 0; + return; + } + if (TransB == CblasTrans) TA = 'T'; + else if (TransB == CblasConjTrans) TA = 'C'; + else if (TransB == CblasNoTrans) TA = 'N'; + else + { + cblas_xerbla(2, "cblas_dgemm", "Illegal TransB setting, %d\n", TransB); + CBLAS_CallFromC = 0; + RowMajorStrg = 0; + return; + } +#ifdef F77_CHAR + F77_TA = C2F_CHAR(&TA); + F77_TB = C2F_CHAR(&TB); +#endif + + F77_dgemm(F77_TA, F77_TB, &F77_N, &F77_M, &F77_K, &alpha, B, + &F77_ldb, A, &F77_lda, &beta, C, &F77_ldc); + } + else cblas_xerbla(1, "cblas_dgemm", "Illegal layout setting, %d\n", layout); + CBLAS_CallFromC = 0; + RowMajorStrg = 0; + return; +} +#endif + + int cvRound(float f) { return roundf(f); } #define CV_Error(code, msg) assert(0 && msg); // cv::error( code, msg, CV_Func, __FILE__, __LINE__ ) @@ -27,7 +193,7 @@ void cvCopyTo(const CvMat *srcarr, CvMat *dstarr) { void cvGEMM(const CvMat *src1, const CvMat *src2, double alpha, const CvMat *src3, double beta, CvMat *dst, int tABC) { lapack_int rows1 = src1->rows; lapack_int cols1 = src1->cols; - + lapack_int rows2 = src2->rows; lapack_int cols2 = src2->cols; diff --git a/src/epnp/epnp.c b/src/epnp/epnp.c index 4b888aa..53a1d48 100644 --- a/src/epnp/epnp.c +++ b/src/epnp/epnp.c @@ -134,7 +134,7 @@ void epnp_choose_control_points(epnp *self) { // Take C1, C2, and C3 from PCA on the reference points: CvMat *PW0 = cvCreateMat(self->number_of_correspondences, 3, CV_64F); - double pw0tpw0[3 * 3] = {}, dc[3], uct[3 * 3]; + double pw0tpw0[3 * 3] = {0}, dc[3], uct[3 * 3]; CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0); CvMat DC = cvMat(3, 1, CV_64F, dc); CvMat UCt = cvMat(3, 3, CV_64F, uct); diff --git a/src/poser_sba.c b/src/poser_sba.c index 0ad38ac..df28a2d 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -4,6 +4,7 @@ #endif #include +#include #include "poser.h" #include diff --git a/src/survive.c b/src/survive.c index f46f128..23b1e4c 100644 --- a/src/survive.c +++ b/src/survive.c @@ -110,6 +110,9 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow) MANUAL_DRIVER_REGISTRATION(PoserDaveOrtho) MANUAL_DRIVER_REGISTRATION(PoserDummy) + MANUAL_DRIVER_REGISTRATION(PoserEPNP) + MANUAL_DRIVER_REGISTRATION(PoserSBA) + MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive) MANUAL_DRIVER_REGISTRATION(DriverRegPlayback) @@ -260,7 +263,7 @@ int survive_startup(SurviveContext *ctx) { // start the thread to process button data ctx->buttonservicethread = OGCreateThread(button_servicer, ctx); - PoserCB PreferredPoserCB = GetDriverByConfig(ctx, "Poser", "defaultposer", "TurveyTori", 2); + PoserCB PreferredPoserCB = GetDriverByConfig(ctx, "Poser", "defaultposer", "SBA", 2); ctx->lightcapfunction = GetDriverByConfig(ctx, "Disambiguator", "disambiguator", "Turvey", 2); const char *DriverName; diff --git a/src/survive_cal.c b/src/survive_cal.c index 25e43b9..2fc1896 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -187,7 +187,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } const char * DriverName; - cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "TurveyTori", 0); + cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "SBA", 0); ootx_packet_clbk = ootx_packet_clbk_d; diff --git a/winbuild/calibrate/calibrate.vcxproj b/winbuild/calibrate/calibrate.vcxproj index 51519e6..d0b1c48 100644 --- a/winbuild/calibrate/calibrate.vcxproj +++ b/winbuild/calibrate/calibrate.vcxproj @@ -75,6 +75,7 @@ true + $(LibraryPath) false @@ -190,7 +191,17 @@ {435cfd2c-8724-42ee-8fde-71ef7d2c6b2f} + + + + + + + This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}. + + + \ No newline at end of file diff --git a/winbuild/calibrate/calibrate.vcxproj.filters b/winbuild/calibrate/calibrate.vcxproj.filters index dae64dc..1498023 100644 --- a/winbuild/calibrate/calibrate.vcxproj.filters +++ b/winbuild/calibrate/calibrate.vcxproj.filters @@ -36,4 +36,7 @@ Header Files + + + \ No newline at end of file diff --git a/winbuild/data_recorder/data_recorder.vcxproj b/winbuild/data_recorder/data_recorder.vcxproj index 59a1e77..e63d474 100644 --- a/winbuild/data_recorder/data_recorder.vcxproj +++ b/winbuild/data_recorder/data_recorder.vcxproj @@ -72,6 +72,7 @@ true + $(VC_LibraryPath_x64);$(WindowsSDK_LibraryPath_x64);$(NETFXKitsDir)Lib\um\x64 true diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 6268d63..2451c22 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -56,6 +56,7 @@ + @@ -70,14 +71,17 @@ - + + $(IncludePath) + $(LibraryPath) + Level3 Disabled - FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;%(PreprocessorDefinitions) + FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions) ..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) @@ -89,6 +93,10 @@ $(MSBuildProjectDirectory)\libsurvive.def + + libblas.lib;liblapacke.lib;liblapack.lib%(AdditionalDependencies) + %(AdditionalLibraryDirectories) + @@ -149,14 +157,23 @@ + + + + + + + + + @@ -183,8 +200,11 @@ + + + @@ -193,8 +213,13 @@ + - - + + + This project references NuGet package(s) that are missing on this computer. Use NuGet Package Restore to download them. For more information, see http://go.microsoft.com/fwlink/?LinkID=322105. The missing file is {0}. + + + \ No newline at end of file diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 5387108..ffddee9 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -108,6 +108,33 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -152,8 +179,18 @@ Source Files + + Source Files + + + Header Files + + + Source Files + + \ No newline at end of file diff --git a/winbuild/libsurvive/packages.config b/winbuild/libsurvive/packages.config new file mode 100644 index 0000000..c283d9d --- /dev/null +++ b/winbuild/libsurvive/packages.config @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/winbuild/test/test.vcxproj b/winbuild/test/test.vcxproj index e6ee3fb..5ceff3d 100644 --- a/winbuild/test/test.vcxproj +++ b/winbuild/test/test.vcxproj @@ -72,6 +72,8 @@ true + $(LibraryPath) + $(VC_ReferencesPath_x64); true @@ -93,7 +95,7 @@ Console - setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;libblas.lib;liblapacke.lib;liblapack.lib;%(AdditionalDependencies) true UseFastLinkTimeCodeGeneration -- cgit v1.3.1 From 033111aa59de4a0277308efbc4405dd96c26f8f8 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 29 Mar 2018 16:11:15 -0600 Subject: Added config value to disable attempts at calibration --- src/survive_process.c | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/survive_process.c b/src/survive_process.c index ad7ce97..97fbc46 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -48,15 +48,19 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode FLT angle = (timeinsweep - so->timecenter_ticks) * (1./so->timecenter_ticks * 3.14159265359/2.0); //Need to now do angle correction. -#if 1 - BaseStationData * bsd = &ctx->bsd[base_station]; + static int use_bsd_cal = -1; + if(use_bsd_cal == -1) { + use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1); + } + if(use_bsd_cal) { + BaseStationData * bsd = &ctx->bsd[base_station]; - //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 - angle += bsd->fcalphase[axis]; -// angle += bsd->fcaltilt[axis] * predicted_angle(axis1); + //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 + angle += bsd->fcalphase[axis]; + // angle += bsd->fcaltilt[axis] * predicted_angle(axis1); - //TODO!!! -#endif + //TODO!!! + } FLT length_sec = length / (FLT)so->timebase_hz; ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh); -- cgit v1.3.1 From 443af4e8fb770d074f1ce729a40687f79d548a50 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 30 Mar 2018 09:43:20 -0600 Subject: Made is so that if rawlight data was available, angles don't playback --- src/survive_playback.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive_playback.c b/src/survive_playback.c index c73dd2f..6789a66 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -317,7 +317,8 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { break; case 'L': case 'R': - parse_and_run_lightcode(line, driver); + if (driver->hasRawLight == false) + parse_and_run_lightcode(line, driver); break; case 'I': parse_and_run_imu(line, driver); -- cgit v1.3.1 From 52bcccaea5de63a4de4b3df17236507455776409 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 30 Mar 2018 09:50:24 -0600 Subject: Added accessors for language bindings --- include/libsurvive/survive.h | 5 +++++ src/survive.c | 5 +++++ 2 files changed, 10 insertions(+) (limited to 'src') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 85078f7..ed43ddc 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -119,6 +119,11 @@ struct SurviveObject int tsl; }; +// These exports are mostly for language binding against +SURVIVE_EXPORT const char *survive_object_codename(SurviveObject *so); +SURVIVE_EXPORT int8_t survive_object_sensor_ct(SurviveObject *so); +SURVIVE_EXPORT const FLT *survive_object_sensor_locations(SurviveObject *so); +SURVIVE_EXPORT const FLT *survive_object_sensor_normals(SurviveObject *so); struct BaseStationData { diff --git a/src/survive.c b/src/survive.c index 23b1e4c..b359669 100644 --- a/src/survive.c +++ b/src/survive.c @@ -551,4 +551,9 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in return len; } +const char *survive_object_codename(SurviveObject *so) { return so->codename; } +int8_t survive_object_sensor_ct(SurviveObject *so) { return so->sensor_ct; } +const FLT *survive_object_sensor_locations(SurviveObject *so) { return so->sensor_locations; } +const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; } + #endif -- cgit v1.3.1 From 053750ef27c996822512f75f55c0110f573eccd6 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 30 Mar 2018 12:33:11 -0600 Subject: Made manual driver registration only ever happen once --- src/survive.c | 41 +++++++++++++++++++++++------------------ 1 file changed, 23 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index b359669..a15e0ed 100644 --- a/src/survive.c +++ b/src/survive.c @@ -36,8 +36,8 @@ static void survivefault(struct SurviveContext *ctx, const char *fault) { } static void survivenote(struct SurviveContext *ctx, const char *fault) { - survive_recording_info_process(ctx, fault); - fprintf(stderr, "Info: %s\n", fault); + survive_recording_info_process(ctx, fault); + fprintf(stderr, "Info: %s\n", fault); } static void *button_servicer(void *context) { @@ -86,8 +86,9 @@ void survive_verify_FLT_size(uint32_t user_size) { if (sizeof(FLT) != user_size) { fprintf(stderr, "FLT type incompatible; the shared library libsurvive has FLT size %lu vs user program %u\n", (unsigned long)sizeof(FLT), user_size); - fprintf(stderr, "Add '#define FLT %s' before including survive.h or recompile the shared library with the " - "appropriate flag. \n", + fprintf(stderr, + "Add '#define FLT %s' before including survive.h or recompile the shared library with the " + "appropriate flag. \n", sizeof(FLT) == sizeof(double) ? "double" : "float"); exit(-1); } @@ -101,24 +102,28 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { } #endif #ifdef MANUAL_REGISTRATION -// note: this manual registration is currently only in use on builds using Visual Studio. + // note: this manual registration is currently only in use on builds using Visual Studio. + static int did_manual_driver_registration = 0; + if (did_manual_driver_registration == 0) { #define MANUAL_DRIVER_REGISTRATION(func) \ int func(SurviveObject *so, PoserData *pd); \ RegisterDriver(#func, &func); - MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow) - MANUAL_DRIVER_REGISTRATION(PoserDaveOrtho) - MANUAL_DRIVER_REGISTRATION(PoserDummy) - MANUAL_DRIVER_REGISTRATION(PoserEPNP) - MANUAL_DRIVER_REGISTRATION(PoserSBA) + MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow) + MANUAL_DRIVER_REGISTRATION(PoserDaveOrtho) + MANUAL_DRIVER_REGISTRATION(PoserDummy) + MANUAL_DRIVER_REGISTRATION(PoserEPNP) + MANUAL_DRIVER_REGISTRATION(PoserSBA) - MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive) - MANUAL_DRIVER_REGISTRATION(DriverRegPlayback) + MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive) + MANUAL_DRIVER_REGISTRATION(DriverRegPlayback) - MANUAL_DRIVER_REGISTRATION(DisambiguatorCharles) - MANUAL_DRIVER_REGISTRATION(DisambiguatorStateBased) - MANUAL_DRIVER_REGISTRATION(DisambiguatorTurvey) + MANUAL_DRIVER_REGISTRATION(DisambiguatorCharles) + MANUAL_DRIVER_REGISTRATION(DisambiguatorStateBased) + MANUAL_DRIVER_REGISTRATION(DisambiguatorTurvey) + did_manual_driver_registration = 1; + } #endif SurviveContext *ctx = calloc(1, sizeof(SurviveContext)); @@ -316,9 +321,9 @@ int survive_startup(SurviveContext *ctx) { // If lighthouse positions are known, broadcast them for (int i = 0; i < ctx->activeLighthouses; i++) { - if(ctx->bsd[i].PositionSet) { - ctx->lighthouseposeproc(ctx, i, &ctx->bsd[i].Pose, 0); - } + if (ctx->bsd[i].PositionSet) { + ctx->lighthouseposeproc(ctx, i, &ctx->bsd[i].Pose, 0); + } } return 0; -- cgit v1.3.1 From c4f0028c0123f7e957db05c3486c16e58388a27a Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 31 Mar 2018 23:35:30 -0600 Subject: Noise filtering in state based dis --- src/survive_statebased_disambiguator.c | 39 +++++++++++++++++++++++++--------- 1 file changed, 29 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/survive_statebased_disambiguator.c b/src/survive_statebased_disambiguator.c index 4aa47ba..6b217ee 100644 --- a/src/survive_statebased_disambiguator.c +++ b/src/survive_statebased_disambiguator.c @@ -343,7 +343,7 @@ static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const Lig if (d->confidence < penalty) { SurviveContext *ctx = d->so->ctx; SetState(d, le, LS_UNKNOWN); - SV_INFO("WARNING: Disambiguator got lost; refinding state."); + SV_INFO("WARNING: Disambiguator got lost; refinding state for %s", d->so->codename); } d->confidence -= penalty; return; @@ -385,22 +385,40 @@ static void ProcessStateChange(Disambiguator_data_t *d, const LightcapElement *l } } else { // Leaving a sweep ... + size_t avg_length = 0; + size_t cnt = 0; + 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); + avg_length += le.length; + cnt++; + } + } + if (cnt > 0) { + double var = 1.5; + size_t minl = (1 / var) * (avg_length + cnt / 2) / cnt; + size_t maxl = var * (avg_length + cnt / 2) / cnt; + + 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 certain times. + if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0 && le.length >= minl && + le.length <= maxl) { + 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. + if (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); } @@ -421,7 +439,8 @@ static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) { 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) { + } else if (le->length > d->sweep_data[le->sensor_id].length && + le->length < 7000 /*anything above 10k seems to be bullshit?*/) { // 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? -- cgit v1.3.1 From 22d40fb360fdb65da7916fb87f9b199f4f401f05 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 07:52:00 -0600 Subject: overhal to calibration --- include/libsurvive/survive.h | 308 ++++++++++++++------------- include/libsurvive/survive_reproject.h | 35 +-- src/poser_epnp.c | 10 +- src/poser_sba.c | 30 ++- src/survive.c | 2 + src/survive_cal.c | 26 +-- src/survive_config.c | 22 +- src/survive_process.c | 9 +- src/survive_reproject.c | 129 ++++++++--- src/survive_sensor_activations.c | 3 +- tools/findoptimalconfig/Makefile | 3 +- tools/findoptimalconfig/findoptimalconfig.cc | 160 ++++++++++++-- 12 files changed, 496 insertions(+), 241 deletions(-) (limited to 'src') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index ed43ddc..e435752 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -10,11 +10,10 @@ extern "C" { #endif - #ifdef _WIN32 #define SURVIVE_EXPORT __declspec(dllexport) #else -#define SURVIVE_EXPORT +#define SURVIVE_EXPORT __attribute__((visibility("default"))) #endif /** @@ -32,6 +31,7 @@ typedef struct { struct PoserDataLight; struct PoserDataIMU; + /** * Adds a lightData packet to the table. */ @@ -53,69 +53,68 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, */ extern uint32_t SurviveSensorActivations_default_tolerance; -//DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks -//for optimization on 32- and 64-bit systems. +// DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks +// for optimization on 32- and 64-bit systems. -struct SurviveObject -{ - SurviveContext * ctx; +struct SurviveObject { + SurviveContext *ctx; - char codename[4]; //3 letters, null-terminated. Currently HMD, WM0, WM1. - char drivername[4]; //3 letters for driver. Currently "HTC" - void *driver; + char codename[4]; // 3 letters, null-terminated. Currently HMD, WM0, WM1. + char drivername[4]; // 3 letters for driver. Currently "HTC" + void *driver; int32_t buttonmask; int16_t axis1; int16_t axis2; int16_t axis3; - int8_t charge; - int8_t charging:1; - int8_t ison:1; - int8_t additional_flags:6; - - //Pose Information, also "poser" field. - FLT PoseConfidence; //0..1 - SurvivePose OutPose; //Final pose? (some day, one can dream!) - SurvivePose FromLHPose[NUM_LIGHTHOUSES]; //Filled out by poser, contains computed position from each lighthouse. - void * PoserData; //Initialized to zero, configured by poser, can be anything the poser wants. + int8_t charge; + int8_t charging : 1; + int8_t ison : 1; + int8_t additional_flags : 6; + + // Pose Information, also "poser" field. + FLT PoseConfidence; // 0..1 + SurvivePose OutPose; // Final pose? (some day, one can dream!) + SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. + void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. PoserCB PoserFn; - //Device-specific information about the location of the sensors. This data will be used by the poser. - int8_t sensor_ct; // sensor count - FLT * sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor - FLT * sensor_normals;// size is nrlocations*3. cointains normal vector for each sensor - - //Timing sensitive data (mostly for disambiguation) - int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) - int32_t timecenter_ticks; //200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full sweep length) - int32_t pulsedist_max_ticks; //500,000 for normal vive hardware. (guessed) - int32_t pulselength_min_sync; //2,200 for normal vive hardware. (guessed) - int32_t pulse_in_clear_time; //35,000 for normal vive hardware. (guessed) - int32_t pulse_max_for_sweep; //1,800 for normal vive hardware. (guessed) - int32_t pulse_synctime_offset; //20,000 for normal vive hardware. (guessed) - int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) - - //Flood info, for calculating which laser is currently sweeping. - void * disambiguator_data; - int8_t oldcode; - int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. - int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. + // Device-specific information about the location of the sensors. This data will be used by the poser. + int8_t sensor_ct; // sensor count + FLT *sensor_locations; // size is sensor_ct*3. Contains x,y,z values for each sensor + FLT *sensor_normals; // size is nrlocations*3. cointains normal vector for each sensor + + // Timing sensitive data (mostly for disambiguation) + int32_t timebase_hz; // 48,000,000 for normal vive hardware. (checked) + int32_t timecenter_ticks; // 200,000 for normal vive hardware. (checked) (This doubles-up as 2x this = full + // sweep length) + int32_t pulsedist_max_ticks; // 500,000 for normal vive hardware. (guessed) + int32_t pulselength_min_sync; // 2,200 for normal vive hardware. (guessed) + int32_t pulse_in_clear_time; // 35,000 for normal vive hardware. (guessed) + int32_t pulse_max_for_sweep; // 1,800 for normal vive hardware. (guessed) + int32_t pulse_synctime_offset; // 20,000 for normal vive hardware. (guessed) + int32_t pulse_synctime_slack; // 5,000 for normal vive hardware. (guessed) + + // Flood info, for calculating which laser is currently sweeping. + void *disambiguator_data; + int8_t oldcode; + int8_t sync_set_number; // 0 = master, 1 = slave, -1 = fault. + int8_t did_handle_ootx; // If unset, will send lightcap data for sync pulses next time a sensor is hit. uint32_t last_sync_time[NUM_LIGHTHOUSES]; uint32_t last_sync_length[NUM_LIGHTHOUSES]; uint32_t recent_sync_time; - uint32_t last_lighttime; //May be a 24- or 32- bit number depending on what device. - + uint32_t last_lighttime; // May be a 24- or 32- bit number depending on what device. - FLT* acc_bias; // size is FLT*3. contains x,y,z - FLT* acc_scale; // size is FLT*3. contains x,y,z - FLT* gyro_bias; // size is FLT*3. contains x,y,z - FLT* gyro_scale; // size is FLT*3. contains x,y,z + FLT *acc_bias; // size is FLT*3. contains x,y,z + FLT *acc_scale; // size is FLT*3. contains x,y,z + FLT *gyro_bias; // size is FLT*3. contains x,y,z + FLT *gyro_scale; // size is FLT*3. contains x,y,z haptic_func haptic; SurviveSensorActivations activations; - //Debug + // Debug int tsl; }; @@ -125,34 +124,37 @@ SURVIVE_EXPORT int8_t survive_object_sensor_ct(SurviveObject *so); SURVIVE_EXPORT const FLT *survive_object_sensor_locations(SurviveObject *so); SURVIVE_EXPORT const FLT *survive_object_sensor_normals(SurviveObject *so); -struct BaseStationData -{ - uint8_t PositionSet:1; +typedef struct BaseStationCal { + FLT phase[2]; + FLT tilt[2]; + FLT curve[2]; + FLT gibpha[2]; + FLT gibmag[2]; +} BaseStationCal; + +struct BaseStationData { + uint8_t PositionSet : 1; SurvivePose Pose; - uint8_t OOTXSet:1; + uint8_t OOTXSet : 1; uint32_t BaseStationID; - FLT fcalphase[2]; - FLT fcaltilt[2]; - FLT fcalcurve[2]; - FLT fcalgibpha[2]; - FLT fcalgibmag[2]; - int8_t accel[3]; //"Up" vector + + BaseStationCal fcal; + + int8_t accel[3]; //"Up" vector + uint8_t mode; }; struct config_group; #define BUTTON_QUEUE_MAX_LEN 32 - - // note: buttonId and axisId are 1-indexed values. // a value of 0 for an id means that no data is present in that value // additionally, when x and y values are both present in axis data, // axis1 will be x, axis2 will be y. -typedef struct -{ - uint8_t isPopulated; //probably can remove this given the semaphore in the parent struct. helps with debugging +typedef struct { + uint8_t isPopulated; // probably can remove this given the semaphore in the parent struct. helps with debugging uint8_t eventType; uint8_t buttonId; uint8_t axis1Id; @@ -162,20 +164,28 @@ typedef struct SurviveObject *so; } ButtonQueueEntry; -typedef struct -{ - uint8_t nextReadIndex; //init to 0 +typedef struct { + uint8_t nextReadIndex; // init to 0 uint8_t nextWriteIndex; // init to 0 - void* buttonservicesem; + void *buttonservicesem; ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN]; } ButtonQueue; typedef enum { SURVIVE_STOPPED = 0, SURVIVE_RUNNING, SURVIVE_CLOSING, SURVIVE_STATE_MAX } SurviveState; struct SurviveRecordingData; +struct survive_calibration_config; + +enum SurviveCalFlag { + SVCal_None = 0, + SVCal_Phase = 1, + SVCal_Tilt = 2, + SVCal_Curve = 4, + SVCal_Gib = 8, + SVCal_All = SVCal_Gib | SVCal_Curve | SVCal_Tilt | SVCal_Phase +}; -struct SurviveContext -{ +struct SurviveContext { text_feedback_func faultfunction; text_feedback_func notefunction; light_process_func lightproc; @@ -187,35 +197,39 @@ struct SurviveContext htc_config_func configfunction; handle_lightcap_func lightcapfunction; - struct config_group* global_config_values; - struct config_group* lh_config; //lighthouse configs - struct config_group* temporary_config_values; //Set per-session, from command-line. Not saved but override global_config_values + struct config_group *global_config_values; + struct config_group *lh_config; // lighthouse configs + struct config_group + *temporary_config_values; // Set per-session, from command-line. Not saved but override global_config_values - //Calibration data: + // Calibration data: int activeLighthouses; BaseStationData bsd[NUM_LIGHTHOUSES]; - SurviveCalData * calptr; //If and only if the calibration subsystem is attached. + SurviveCalData *calptr; // If and only if the calibration subsystem is attached. struct SurviveRecordingData *recptr; // Iff recording is attached - SurviveObject ** objs; + SurviveObject **objs; int objs_ct; - void ** drivers; - DeviceDriverCb * driverpolls; - DeviceDriverCb * drivercloses; - DeviceDriverMagicCb * drivermagics; + void **drivers; + DeviceDriverCb *driverpolls; + DeviceDriverCb *drivercloses; + DeviceDriverMagicCb *drivermagics; int driver_ct; SurviveState state; - void* buttonservicethread; + void *buttonservicethread; ButtonQueue buttonQueue; void *user_ptr; + enum SurviveCalFlag calibration_flag; + struct survive_calibration_config *calibration_config; }; -void survive_verify_FLT_size(uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. +void survive_verify_FLT_size( + uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. -SURVIVE_EXPORT SurviveContext * survive_init_internal( int argc, char * const * argv ); +SURVIVE_EXPORT SurviveContext *survive_init_internal(int argc, char *const *argv); /** * Call survive_init to get a populated SurviveContext pointer. @@ -227,98 +241,106 @@ SURVIVE_EXPORT SurviveContext * survive_init_internal( int argc, char * const * * Note that this function _can_ return null based on command line arguments, * notably if -h was passed in. */ -static inline SurviveContext * survive_init( int argc, char * const * argv ) -{ +static inline SurviveContext *survive_init(int argc, char *const *argv) { survive_verify_FLT_size(sizeof(FLT)); return survive_init_internal(argc, argv); } - -//For any of these, you may pass in 0 for the function pointer to use default behavior. -//In general unless you are doing wacky things like recording or playing back data, you won't need to use this. -SURVIVE_EXPORT void survive_install_htc_config_fn( SurviveContext *ctx, htc_config_func fbp ); -SURVIVE_EXPORT void survive_install_info_fn( SurviveContext * ctx, text_feedback_func fbp ); -SURVIVE_EXPORT void survive_install_error_fn( SurviveContext * ctx, text_feedback_func fbp ); -SURVIVE_EXPORT void survive_install_light_fn( SurviveContext * ctx, light_process_func fbp ); -SURVIVE_EXPORT void survive_install_imu_fn( SurviveContext * ctx, imu_process_func fbp ); -SURVIVE_EXPORT void survive_install_angle_fn( SurviveContext * ctx, angle_process_func fbp ); -SURVIVE_EXPORT void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp); -SURVIVE_EXPORT void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp); +// For any of these, you may pass in 0 for the function pointer to use default behavior. +// In general unless you are doing wacky things like recording or playing back data, you won't need to use this. +SURVIVE_EXPORT void survive_install_htc_config_fn(SurviveContext *ctx, htc_config_func fbp); +SURVIVE_EXPORT void survive_install_info_fn(SurviveContext *ctx, text_feedback_func fbp); +SURVIVE_EXPORT void survive_install_error_fn(SurviveContext *ctx, text_feedback_func fbp); +SURVIVE_EXPORT void survive_install_light_fn(SurviveContext *ctx, light_process_func fbp); +SURVIVE_EXPORT void survive_install_imu_fn(SurviveContext *ctx, imu_process_func fbp); +SURVIVE_EXPORT void survive_install_angle_fn(SurviveContext *ctx, angle_process_func fbp); +SURVIVE_EXPORT void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp); +SURVIVE_EXPORT void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp); SURVIVE_EXPORT void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp); -SURVIVE_EXPORT int survive_startup( SurviveContext * ctx ); -SURVIVE_EXPORT int survive_poll( SurviveContext * ctx ); -SURVIVE_EXPORT void survive_close( SurviveContext * ctx ); - -SURVIVE_EXPORT SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name ); +SURVIVE_EXPORT int survive_startup(SurviveContext *ctx); +SURVIVE_EXPORT int survive_poll(SurviveContext *ctx); +SURVIVE_EXPORT void survive_close(SurviveContext *ctx); -//Utilitiy functions. -int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, char * output, int outlen ); -int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen ); +SURVIVE_EXPORT SurviveObject *survive_get_so_by_name(SurviveContext *ctx, const char *name); -//These functions search both the stored-general and temporary sections for a parameter and return it. -#define SC_GET 0 //Get, only. -#define SC_SET 1 //Set, if not present -#define SC_OVERRIDE 2 //Set, to new default value. -#define SC_SETCONFIG 4 //Set, both in-memory and config file. Use in conjunction with SC_OVERRIDE. +// Utilitiy functions. +int survive_simple_inflate(SurviveContext *ctx, const char *input, int inlen, char *output, int outlen); +int survive_send_magic(SurviveContext *ctx, int magic_code, void *data, int datalen); -SURVIVE_EXPORT FLT survive_configf( SurviveContext * ctx, const char *tag, char flags, FLT def ); -SURVIVE_EXPORT uint32_t survive_configi( SurviveContext * ctx, const char *tag, char flags, uint32_t def ); -SURVIVE_EXPORT const char * survive_configs( SurviveContext * ctx, const char *tag, char flags, const char *def ); +// These functions search both the stored-general and temporary sections for a parameter and return it. +#define SC_GET 0 // Get, only. +#define SC_SET 1 // Set, if not present +#define SC_OVERRIDE 2 // Set, to new default value. +#define SC_SETCONFIG 4 // Set, both in-memory and config file. Use in conjunction with SC_OVERRIDE. +SURVIVE_EXPORT FLT survive_configf(SurviveContext *ctx, const char *tag, char flags, FLT def); +SURVIVE_EXPORT uint32_t survive_configi(SurviveContext *ctx, const char *tag, char flags, uint32_t def); +SURVIVE_EXPORT const char *survive_configs(SurviveContext *ctx, const char *tag, char flags, const char *def); -//Install the calibrator. -SURVIVE_EXPORT void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if not already done so. +// Install the calibrator. +SURVIVE_EXPORT void survive_cal_install(SurviveContext *ctx); // XXX This will be removed if not already done so. // Read back a human-readable string description of the calibration status -SURVIVE_EXPORT int survive_cal_get_status( SurviveContext * ctx, char * description, int description_length ); +SURVIVE_EXPORT int survive_cal_get_status(SurviveContext *ctx, char *description, int description_length); // Induce haptic feedback -SURVIVE_EXPORT int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount); - -//Call these from your callback if overridden. -//Accept higher-level data. -SURVIVE_EXPORT void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); -SURVIVE_EXPORT void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); -SURVIVE_EXPORT void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); -SURVIVE_EXPORT 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); +SURVIVE_EXPORT int survive_haptic(SurviveObject *so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, + uint16_t repeatCount); + +// Call these from your callback if overridden. +// Accept higher-level data. +SURVIVE_EXPORT void survive_default_light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, + uint32_t timecode, uint32_t length, uint32_t lh); +SURVIVE_EXPORT void survive_default_imu_process(SurviveObject *so, int mode, FLT *accelgyro, uint32_t timecode, int id); +SURVIVE_EXPORT void survive_default_angle_process(SurviveObject *so, int sensor_id, int acode, uint32_t timecode, + FLT length, FLT angle, uint32_t lh); +SURVIVE_EXPORT 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); SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); -SURVIVE_EXPORT void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, - SurvivePose *obj_pose); -SURVIVE_EXPORT int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); - - +SURVIVE_EXPORT void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, + SurvivePose *lh_pose, SurvivePose *obj_pose); +SURVIVE_EXPORT int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); ////////////////////// Survive Drivers //////////////////////////// -void RegisterDriver(const char * name, void * data); +void RegisterDriver(const char *name, void *data); #ifdef _MSC_VER -#define REGISTER_LINKTIME( func ) \ - __pragma(comment(linker,"/export:REGISTER"#func));\ +#define REGISTER_LINKTIME(func) \ + __pragma(comment(linker, "/export:REGISTER" #func)); \ void REGISTER##func() { RegisterDriver(#func, &func); } #else -#define REGISTER_LINKTIME( func ) \ +#define REGISTER_LINKTIME(func) \ void __attribute__((constructor)) REGISTER##func() { RegisterDriver(#func, &func); } #endif - - ///////////////////////// General stuff for writing drivers /////// -//For device drivers to call. This actually attaches them. -int survive_add_object( SurviveContext * ctx, SurviveObject * obj ); -void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic ); - -//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. -void handle_lightcap( SurviveObject * so, LightcapElement * le ); - -#define SV_INFO( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->notefunction( ctx, stbuff ); } -#define SV_ERROR( ... ) { char stbuff[1024]; sprintf( stbuff, __VA_ARGS__ ); ctx->faultfunction( ctx, stbuff ); } -#define SV_KILL() exit(0) //XXX This should likely be re-defined. +// For device drivers to call. This actually attaches them. +int survive_add_object(SurviveContext *ctx, SurviveObject *obj); +void survive_add_driver(SurviveContext *ctx, void *payload, DeviceDriverCb poll, DeviceDriverCb close, + DeviceDriverMagicCb magic); + +// This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. +void handle_lightcap(SurviveObject *so, LightcapElement *le); + +#define SV_INFO(...) \ + { \ + char stbuff[1024]; \ + sprintf(stbuff, __VA_ARGS__); \ + ctx->notefunction(ctx, stbuff); \ + } +#define SV_ERROR(...) \ + { \ + char stbuff[1024]; \ + sprintf(stbuff, __VA_ARGS__); \ + ctx->faultfunction(ctx, stbuff); \ + } +#define SV_KILL() exit(0) // XXX This should likely be re-defined. #ifdef __cplusplus }; #endif #endif - diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index 961db2a..c90d39c 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -1,6 +1,8 @@ #pragma once + #include "survive.h" #include +#include #include #ifdef __cplusplus @@ -13,7 +15,7 @@ typedef struct { bool swap; } survive_calibration_options_config; -typedef struct { +typedef struct survive_calibration_config { survive_calibration_options_config phase, tilt, curve, gibMag, gibPhase; @@ -21,10 +23,10 @@ typedef struct { } survive_calibration_config; -void survive_calibration_options_config_apply( - const survive_calibration_options_config *option, const FLT *input, - FLT *output); -const survive_calibration_config *survive_calibration_default_config(); +void survive_calibration_options_config_apply(const survive_calibration_options_config *option, const FLT *input, + FLT *output); + +const survive_calibration_config *survive_calibration_default_config(const SurviveContext *ctx); size_t survive_calibration_config_max_idx(); @@ -32,10 +34,9 @@ survive_calibration_config survive_calibration_config_create_from_idx(size_t v); size_t survive_calibration_config_index(const survive_calibration_config *config); -void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, - FLT *out); -void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, - const SurvivePose *pose, FLT *point3d, +void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out); + +void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); // This is given a lighthouse -- in the same system as stored in BaseStationData, and @@ -44,9 +45,19 @@ void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, // While this is typically opposite of what we want to do -- we want to find the 3d // position from a 2D coordinate, this is helpful since the minimization of reprojection // error is a core mechanism to many types of solvers. -void survive_reproject_from_pose_with_config( - const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out); +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out); + +void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, + const SurvivePose *pose, const FLT *point3d, FLT *out); + +void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, + const FLT *point3d, FLT *out); + +void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *config); + +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, FLT *out); +void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out); #ifdef __cplusplus } diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 7e86542..c05450a 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -6,6 +6,7 @@ #include #include +#include #include "epnp/epnp.h" #include "linmath.h" @@ -71,7 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) for (size_t i = 0; i < so->sensor_ct; i++) { FLT *lengths = pdfs->lengths[i][lh]; - FLT *ang = pdfs->angles[i][lh]; + FLT *_ang = pdfs->angles[i][lh]; + FLT ang[2]; + survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; @@ -103,7 +106,10 @@ static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActiv for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) { if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timecode, sensor_idx, lh)) { - double *angles = scene->angles[sensor_idx][lh]; + FLT *_angles = scene->angles[sensor_idx][lh]; + FLT angles[2]; + survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); + epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0], so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2], tan(angles[0]), tan(angles[1])); diff --git a/src/poser_sba.c b/src/poser_sba.c index df28a2d..4a4ed8f 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -40,6 +40,8 @@ typedef struct SBAData { int successes_to_reset; int successes_to_reset_cntr; + FLT max_error; + FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; @@ -71,7 +73,9 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, continue; } - double *angles = pdfs->angles[sensor][lh]; + double *_angles = pdfs->angles[sensor][lh]; + double angles[2]; + survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; meas[measCount++] = angles[0]; @@ -89,7 +93,9 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { - double *a = scene->angles[sensor][lh]; + double *_a = scene->angles[sensor][lh]; + FLT a[2]; + survive_apply_bsd_calibration(so->ctx, lh, _a, a); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; if (cov) { @@ -273,7 +279,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o if (distance > 1.) status = -1; } - if (status > 0) { + if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); @@ -283,7 +289,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o 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) { + if (cnt++ > 1000 || meas_size < d->required_meas || (info[1] / meas_size * 2) > d->max_error) { SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); cnt = 0; @@ -389,11 +395,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->failures_to_reset_cntr = 0; d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); d->successes_to_reset_cntr = 0; - d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 1); + d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100); d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); - - d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, 1600000 * 4); + d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); + d->sensor_time_window = + survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); 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; @@ -403,6 +410,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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); + SV_INFO("\tsba-max-error: %f", d->max_error); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -416,8 +424,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // only process sweeps 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, scene, 50, .5); + survive_calibration_config config = *survive_calibration_default_config(ctx); + error = run_sba_find_3d_structure(d, config, lightData, scene, 100, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } @@ -435,9 +443,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - survive_calibration_config config = *survive_calibration_default_config(); + survive_calibration_config config = *survive_calibration_default_config(ctx); SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config)); - double error = run_sba(config, pdfs, so, 50, .005); + double error = run_sba(config, pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } diff --git a/src/survive.c b/src/survive.c index a15e0ed..63ad2ba 100644 --- a/src/survive.c +++ b/src/survive.c @@ -219,6 +219,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->configfunction = survive_default_htc_config_process; ctx->rawposeproc = survive_default_raw_pose_process; + ctx->calibration_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); + return ctx; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 2fc1896..e094e7b 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -61,19 +61,20 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) //print_lighthouse_info_v6(&v6); b->BaseStationID = v6.id; - b->fcalphase[0] = v6.fcal_0_phase; - b->fcalphase[1] = v6.fcal_1_phase; - b->fcaltilt[0] = tan(v6.fcal_0_tilt); - b->fcaltilt[1] = tan(v6.fcal_1_tilt); //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18 - b->fcalcurve[0] = v6.fcal_0_curve; - b->fcalcurve[1] = v6.fcal_1_curve; - b->fcalgibpha[0] = v6.fcal_0_gibphase; - b->fcalgibpha[1] = v6.fcal_1_gibphase; - b->fcalgibmag[0] = v6.fcal_0_gibmag; - b->fcalgibmag[1] = v6.fcal_1_gibmag; + b->fcal.phase[0] = v6.fcal_0_phase; + b->fcal.phase[1] = v6.fcal_1_phase; + b->fcal.tilt[0] = (v6.fcal_0_tilt); + b->fcal.tilt[1] = (v6.fcal_1_tilt); // XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18 + b->fcal.curve[0] = v6.fcal_0_curve; + b->fcal.curve[1] = v6.fcal_1_curve; + b->fcal.gibpha[0] = v6.fcal_0_gibphase; + b->fcal.gibpha[1] = v6.fcal_1_gibphase; + b->fcal.gibmag[0] = v6.fcal_0_gibmag; + b->fcal.gibmag[1] = v6.fcal_1_gibmag; b->accel[0] = v6.accel_dir_x; b->accel[1] = v6.accel_dir_y; b->accel[2] = v6.accel_dir_z; + b->mode = v6.mode_current; b->OOTXSet = 1; config_set_lighthouse(ctx->lh_config,b,id); @@ -617,8 +618,9 @@ static void handle_calibration( struct SurviveCalData *cd ) } fsd.lengths[i][j][0] = cd->avglens[dataindex+0]; fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; - fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; - fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + // fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; + // fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + survive_apply_bsd_calibration(ctx, lh, &cd->avgsweeps[dataindex], fsd.angles[i][j]); fsd.synctimes[i][j] = temp_syncs[i][j]; } diff --git a/src/survive_config.c b/src/survive_config.c index 7b68ae4..d67cd8e 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -119,12 +119,13 @@ void config_read_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8 FLT defaults[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; bsd->BaseStationID = config_read_uint32(cg, "id", 0); + bsd->mode = config_read_uint32(cg, "mode", 0); config_read_float_array(cg, "pose", &bsd->Pose.Pos[0], defaults, 7); - config_read_float_array(cg, "fcalphase", bsd->fcalphase, defaults, 2); - config_read_float_array(cg, "fcaltilt", bsd->fcaltilt, defaults, 2); - config_read_float_array(cg, "fcalcurve", bsd->fcalcurve, defaults, 2); - config_read_float_array(cg, "fcalgibpha", bsd->fcalgibpha, defaults, 2); - config_read_float_array(cg, "fcalgibmag", bsd->fcalgibmag, defaults, 2); + config_read_float_array(cg, "fcalphase", bsd->fcal.phase, defaults, 2); + config_read_float_array(cg, "fcaltilt", bsd->fcal.tilt, defaults, 2); + config_read_float_array(cg, "fcalcurve", bsd->fcal.curve, defaults, 2); + config_read_float_array(cg, "fcalgibpha", bsd->fcal.gibpha, defaults, 2); + config_read_float_array(cg, "fcalgibmag", bsd->fcal.gibmag, defaults, 2); bsd->PositionSet = config_read_uint32(cg, "PositionSet", 0); } @@ -132,12 +133,13 @@ void config_set_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8_ config_group *cg = lh_config + idx; config_set_uint32(cg, "index", idx); config_set_uint32(cg, "id", bsd->BaseStationID); + config_set_uint32(cg, "mode", bsd->mode); config_set_float_a(cg, "pose", &bsd->Pose.Pos[0], 7); - config_set_float_a(cg, "fcalphase", bsd->fcalphase, 2); - config_set_float_a(cg, "fcaltilt", bsd->fcaltilt, 2); - config_set_float_a(cg, "fcalcurve", bsd->fcalcurve, 2); - config_set_float_a(cg, "fcalgibpha", bsd->fcalgibpha, 2); - config_set_float_a(cg, "fcalgibmag", bsd->fcalgibmag, 2); + config_set_float_a(cg, "fcalphase", bsd->fcal.phase, 2); + config_set_float_a(cg, "fcaltilt", bsd->fcal.tilt, 2); + config_set_float_a(cg, "fcalcurve", bsd->fcal.curve, 2); + config_set_float_a(cg, "fcalgibpha", bsd->fcal.gibpha, 2); + config_set_float_a(cg, "fcalgibmag", bsd->fcal.gibmag, 2); config_set_uint32(cg, "PositionSet", bsd->PositionSet); } diff --git a/src/survive_process.c b/src/survive_process.c index 97fbc46..62459f2 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -51,14 +51,17 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode static int use_bsd_cal = -1; if(use_bsd_cal == -1) { use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1); + if (use_bsd_cal == 0) { + SV_INFO("Not using BSD calibration values"); + } } if(use_bsd_cal) { BaseStationData * bsd = &ctx->bsd[base_station]; - //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 - angle += bsd->fcalphase[axis]; + // XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 + // angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis]; // angle += bsd->fcaltilt[axis] * predicted_angle(axis1); - + //TODO!!! } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index eabdb07..845f30a 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -1,6 +1,7 @@ #include "survive_reproject.h" #include <../redist/linmath.h> #include +#include #include static void survive_calibration_options_config_normalize( @@ -72,9 +73,8 @@ static FLT gibf(bool useSin, FLT v) { return cos(v); } -void survive_reproject_from_pose_with_config( - const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { +void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, + const SurvivePose *pose, const FLT *pt, FLT *out) { LinmathQuat invq; quatgetreciprocal(invq, pose->Rot); @@ -92,33 +92,36 @@ void survive_reproject_from_pose_with_config( double ang_x = atan(x); double ang_y = atan(y); - const BaseStationData *bsd = &ctx->bsd[lighthouse]; double phase[2]; - survive_calibration_options_config_apply(&config->phase, bsd->fcalphase, - phase); + survive_calibration_options_config_apply(&config->phase, bsd->fcal.phase, phase); double tilt[2]; - survive_calibration_options_config_apply(&config->tilt, bsd->fcaltilt, - tilt); + survive_calibration_options_config_apply(&config->tilt, bsd->fcal.tilt, tilt); double curve[2]; - survive_calibration_options_config_apply(&config->curve, bsd->fcalcurve, - curve); + survive_calibration_options_config_apply(&config->curve, bsd->fcal.curve, curve); double gibPhase[2]; - survive_calibration_options_config_apply(&config->gibPhase, bsd->fcalgibpha, - gibPhase); + survive_calibration_options_config_apply(&config->gibPhase, bsd->fcal.gibpha, gibPhase); double gibMag[2]; - survive_calibration_options_config_apply(&config->gibMag, bsd->fcalgibmag, - gibMag); + survive_calibration_options_config_apply(&config->gibMag, bsd->fcal.gibmag, gibMag); - out[0] = ang_x + phase[0] + tan(tilt[0]) * y + curve[0] * y * y + + out[0] = ang_x + phase[0] + (tilt[0]) * ang_y + curve[0] * ang_y * ang_y + gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0]; - out[1] = ang_y + phase[1] + tan(tilt[1]) * x + curve[1] * x * x + + out[1] = ang_y + phase[1] + (tilt[1]) * ang_x + curve[1] * ang_x * ang_x + gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1]; } +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { + const BaseStationData *bsd = &ctx->bsd[lighthouse]; + survive_reproject_from_pose_with_bsd(bsd, config, pose, pt, out); +} + +void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, + const FLT *point3d, FLT *out) { + survive_reproject_from_pose_with_config(ctx, config, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); +} void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *pt, FLT *out) { - survive_reproject_from_pose_with_config( - ctx, survive_calibration_default_config(), lighthouse, pose, pt, out); + survive_reproject_from_pose_with_config(ctx, survive_calibration_default_config(ctx), lighthouse, pose, pt, out); } void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, @@ -127,14 +130,15 @@ void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, point3d, out); } -const survive_calibration_config *survive_calibration_default_config() { - static survive_calibration_config *def = 0; - if (def == 0) { - def = malloc(sizeof(survive_calibration_config)); - memset(def, 0, sizeof(survive_calibration_config)); - *def = survive_calibration_config_create_from_idx(0); +const survive_calibration_config *survive_calibration_default_config(const SurviveContext *_ctx) { + SurviveContext *ctx = (SurviveContext *)_ctx; + if (ctx->calibration_config == 0) { + size_t idx = survive_configi(ctx, "default-cal-conf", SC_GET, 0); + ctx->calibration_config = malloc(sizeof(survive_calibration_config)); + memset(ctx->calibration_config, 0, sizeof(survive_calibration_config)); + *ctx->calibration_config = survive_calibration_config_create_from_idx(idx); } - return def; + return ctx->calibration_config; } size_t survive_calibration_config_max_idx() { @@ -142,3 +146,78 @@ size_t survive_calibration_config_max_idx() { memset(&cfg, 0x1, sizeof(survive_calibration_config)); return survive_calibration_config_index(&cfg); } + +static void survive_calibration_options_config_fprint(FILE *file, const survive_calibration_options_config *self) { + fprintf(file, "\t"); + if (!self->enable[0] && !self->enable[1]) { + fprintf(file, "disabled"); + return; + } + + fprintf(file, "swap: %d\n", self->swap); + for (int i = 0; i < 2; i++) { + if (self->enable[i]) { + fprintf(file, "\tinvert[%d]: %d", i, self->invert[i]); + } else { + fprintf(file, "\t%d: disabled", i); + } + } +} + +void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *self) { + fprintf(file, "Index: %ld\n", survive_calibration_config_index(self)); + + fprintf(file, "Phase: \n"); + survive_calibration_options_config_fprint(file, &self->phase); + fprintf(file, "\n"); + + fprintf(file, "Tilt: \n"); + survive_calibration_options_config_fprint(file, &self->tilt); + fprintf(file, "\n"); + + fprintf(file, "Curve: \n"); + survive_calibration_options_config_fprint(file, &self->curve); + fprintf(file, "\n"); + + fprintf(file, "gibPhase: \n"); + survive_calibration_options_config_fprint(file, &self->gibPhase); + fprintf(file, "\n"); + + fprintf(file, "gibMag: \n"); + survive_calibration_options_config_fprint(file, &self->gibMag); + fprintf(file, "\n"); + + fprintf(file, "gibUseSin: %d\n", self->gibUseSin); +} + +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, + FLT *out) { + const BaseStationCal *cal = &ctx->bsd[lh].fcal; + out[0] = in[0] + cal->phase[0]; + out[1] = in[1] + cal->phase[1]; + + FLT tilt_scale = 10; + FLT curve_scale = 10000; + FLT gib_scale = 10; + const int iterations = 4; + for (int i = 0; i < iterations; i++) { + FLT last_out[2] = {out[0], out[1]}; + bool last_iteration = i == iterations - 1; + for (int j = 0; j < 2; j++) { + int oj = j == 0 ? 1 : 0; + out[j] = in[j]; + if (!last_iteration || (f & SVCal_Phase)) + out[j] += (cal->phase[j]); + if (!last_iteration || (f & SVCal_Tilt)) + out[j] += tan(cal->tilt[j] / tilt_scale) * last_out[oj]; + if (!last_iteration || (f & SVCal_Curve)) + out[j] += (cal->curve[j] / curve_scale) * last_out[oj] * last_out[oj]; + if (!last_iteration || (f & SVCal_Gib)) + out[j] += cos(cal->gibpha[j] + last_out[j]) * cal->gibmag[1] / gib_scale; + } + } +} + +void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { + survive_apply_bsd_calibration_by_flag(ctx, lh, ctx->calibration_flag, in, out); +} diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index e42b50e..dc5c0d4 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -1,3 +1,4 @@ +#include #include bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance, @@ -28,4 +29,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa *length = lightData->length * 48000000; } -uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); +uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000) + 5000; diff --git a/tools/findoptimalconfig/Makefile b/tools/findoptimalconfig/Makefile index 5e14814..f174812 100644 --- a/tools/findoptimalconfig/Makefile +++ b/tools/findoptimalconfig/Makefile @@ -7,7 +7,8 @@ LIBSURVIVE:=$(SRT)/lib/libsurvive.so CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE #-fsanitize=address -fsanitize=undefined LDFLAGS:=-lm -lpthread -llapacke -lcblas -findoptimalconfig : findoptimalconfig.cc $(LIBSURVIVE) +findoptimalconfig : findoptimalconfig.cc $(LIBSURVIVE) + cd ../..;make g++ $(CFLAGS) -o $@ $^ $(LDFLAGS) clean : diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc index 543958b..874ed84 100644 --- a/tools/findoptimalconfig/findoptimalconfig.cc +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -9,6 +9,8 @@ #include #include +#include + std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self) { o << "\t"; if (!self.enable[0] && !self.enable[1]) { @@ -49,7 +51,7 @@ struct SBAData { FLT sensor_variance = 1.; FLT sensor_variance_per_second = 0; - int sensor_time_window = 1600000; + int sensor_time_window = SurviveSensorActivations_default_tolerance; int required_meas = 8; }; @@ -57,10 +59,12 @@ struct SBAData { struct PlaybackDataInput { SurviveObject *so = nullptr; SurvivePose position; - + uint32_t timestamp; std::vector vmask; std::vector meas, cov; - PlaybackDataInput(SurviveObject *so, const SurvivePose &position) : so(so), position(position) { + SurviveSensorActivations activations; + PlaybackDataInput(SurviveObject *so, const SurvivePose &position) + : so(so), position(position), activations(so->activations) { int32_t sensor_count = so->sensor_ct; vmask.resize(sensor_count * NUM_LIGHTHOUSES); cov.resize(4 * sensor_count * NUM_LIGHTHOUSES); @@ -74,6 +78,7 @@ struct PlaybackDataInput { }; struct PlaybackData { + SurviveObject *so = nullptr; BaseStationData bsd[2]; std::vector inputs; }; @@ -116,15 +121,24 @@ void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse); } +SurvivePose lastPose = {}; void raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, pose); PlaybackData *d = (PlaybackData *)so->ctx->user_ptr; + d->so = so; d->inputs.emplace_back(so, *pose); auto &input = d->inputs.back(); + input.timestamp = timestamp; int meas = construct_input_from_scene(so, timestamp, &input.vmask.front(), &input.meas.front(), &input.cov.front()); input.shrink(meas / 2); - if (meas / 2 < 12) + + double dist = 0; + if (d->inputs.empty() == false) { + dist = dist3d(pose->Pos, lastPose.Pos); + } + if (meas / 2 < 8 || dist > .00009) d->inputs.pop_back(); + lastPose = *pose; } void lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) { @@ -166,7 +180,6 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl SurviveObject *so = data.so; SurvivePose soLocation = data.position; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; @@ -191,6 +204,102 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl str_metric_function, 0, // jacobia of metric_func &_ctx, // user data + 100, // Max iterations + 0, // verbosity + opts, // options + info); // info + + int meas_size = data.meas.size() / 2; + if (meas_size == 0) + return 0; + + { + 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) { + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); + SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); + cnt = 0; + } + } + assert(!isinf(info[1])); + return info[1] / meas_size * 2; +} + +struct optimal_cal_ctx { + std::vector sensors; + SurviveContext *ctx; + survive_calibration_config config; +}; + +static void metric_function(int j, int i, double *aj, double *xij, void *adata) { + optimal_cal_ctx *ctx = (optimal_cal_ctx *)(adata); + + FLT sensorInWorld[3] = {ctx->sensors[i * 3 + 0], ctx->sensors[i * 3 + 1], ctx->sensors[i * 3 + 2]}; + + BaseStationData bsd = ctx->ctx->bsd[j]; + bsd.fcal = *(BaseStationCal *)aj; + + survive_reproject_from_pose_with_bsd(&bsd, &ctx->config, &ctx->ctx->bsd[j].Pose, sensorInWorld, xij); +} + +double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) { + optimal_cal_ctx _ctx; + std::vector vmask; + std::vector cov, meas; + _ctx.ctx = ctx; + _ctx.config = config; + for (auto &in : data.inputs) { + for (size_t sensor = 0; sensor < in.so->sensor_ct; sensor++) { + FLT p[3]; + ApplyPoseToPoint(p, &in.position, &data.so->sensor_locations[sensor * 3]); + _ctx.sensors.emplace_back(p[0]); + _ctx.sensors.emplace_back(p[1]); + _ctx.sensors.emplace_back(p[2]); + for (size_t lh = 0; lh < 1; lh++) { + auto scene = &in.activations; + if (SurviveSensorActivations_isPairValid(scene, settings.sensor_time_window, in.timestamp, sensor, + lh)) { + double *a = scene->angles[sensor][lh]; + vmask.emplace_back(1); //[sensor * NUM_LIGHTHOUSES + lh] = 1; + + meas.emplace_back(a[0]); + meas.emplace_back(a[1]); + } else { + vmask.emplace_back(0); + } + } + } + } + + double *covx = 0; + SurviveObject *so = data.so; + + double opts[SBA_OPTSSZ] = {0}; + double info[SBA_INFOSZ] = {0}; + + BaseStationCal cal[2] = {}; + + 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_mot_levmar(data.inputs.size() * so->sensor_ct, // number of 3d points + 1, // Number of cameras -- 2 lighthouses + 0, // Number of cameras to not modify + &vmask[0], // boolean vis mask + (double *)cal, // camera parameters + 2, // sizeof(BaseStationCal) / sizeof(FLT), + &meas[0], // 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 50, // Max iterations 0, // verbosity opts, // options @@ -200,7 +309,7 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl } else { assert(false); } - int meas_size = data.meas.size() / 2; + int meas_size = _ctx.sensors.size() / 2; if (meas_size == 0) return 0; @@ -215,17 +324,16 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl } } assert(!isinf(info[1])); + std::cerr << "Used " << meas_size << " measurements" << std::endl; + + double *_cal = (double *)cal; + for (int i = 0; i < sizeof(BaseStationCal) / sizeof(FLT); i++) + std::cerr << _cal[2 * i] << ", " << _cal[2 * i + 1] << " = " << (info[1] / meas_size * 2) << std::endl; + return info[1] / meas_size * 2; } - double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackDataInput &data) { - auto vmask = &data.vmask.front(); - auto cov = &data.cov.front(); - auto meas = &data.meas.front(); - double err = 0; - size_t cnt = 0; - - err += sba_opt(ctx, config, data); + return sba_opt(ctx, config, data); /* for (size_t sensor = 0; sensor < data.so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { @@ -246,7 +354,6 @@ double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_conf } } }*/ - return err; } double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) { @@ -259,10 +366,10 @@ double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_conf int main(int argc, char **argv) { std::vector> sections = { - {28, 0}, // phase - // { 5, 5 }, // tilt - // { 5, 10 }, // curve - // { 11, 15 } // gibs + useSin + {5, 0}, // phase + //{ 5, 5 }, // tilt + //{ 5, 10 }, // curve + //{ 11, 15 } // gibs + useSin }; for (int i = 1; i < argc; i++) { @@ -279,7 +386,9 @@ int main(int argc, char **argv) { "--defaultposer", "SBA", "--sba-required-meas", - "12", + "8", + "--sba-max-error", + ".1", "--playback", argv[i]}; @@ -293,10 +402,19 @@ int main(int argc, char **argv) { while (survive_poll(ctx) == 0) { } + survive_calibration_config config = {}; + // config.tilt.enable[0] = config.tilt.enable[1] = 1; + // config.curve.enable[0] = config.curve.enable[1] = 1; + config.phase.enable[0] = config.phase.enable[1] = 1; + // config.gibPhase.enable[0] = config.gibPhase.enable[1] = 1; + // config.gibMag.enable[0] = config.gibMag.enable[1] = 1; + + find_optimal_cal(ctx, config, data); + for (int j = 0; j < sections.size(); j++) { auto &range = sections[j]; for (size_t _i = 0; _i < (1 << range.first); _i++) { - int i = _i << range.second; + int i = (_i << range.second); survive_calibration_config config = survive_calibration_config_create_from_idx(i); if (i == survive_calibration_config_index(&config)) { double error = find_avg_reproj_error(ctx, config, data); -- cgit v1.3.1 From 47c7fb15182700fb403894f65beaf143a7fad6ab Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 12:23:48 -0600 Subject: Tweaked how reproject / calibate interact --- include/libsurvive/survive.h | 10 +- include/libsurvive/survive_reproject.h | 37 +---- src/poser_epnp.c | 6 +- src/poser_sba.c | 41 +++-- src/survive.c | 3 +- src/survive_cal.c | 10 +- src/survive_reproject.c | 230 +++++++-------------------- tools/findoptimalconfig/findoptimalconfig.cc | 72 +-------- 8 files changed, 97 insertions(+), 312 deletions(-) (limited to 'src') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e435752..669821d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -185,6 +185,13 @@ enum SurviveCalFlag { SVCal_All = SVCal_Gib | SVCal_Curve | SVCal_Tilt | SVCal_Phase }; +typedef struct survive_calibration_config { + enum SurviveCalFlag use_flag; + FLT phase_scale, tilt_scale, curve_scale, gib_scale; +} survive_calibration_config; + +survive_calibration_config survive_calibration_config_ctor(); + struct SurviveContext { text_feedback_func faultfunction; text_feedback_func notefunction; @@ -222,8 +229,7 @@ struct SurviveContext { ButtonQueue buttonQueue; void *user_ptr; - enum SurviveCalFlag calibration_flag; - struct survive_calibration_config *calibration_config; + struct survive_calibration_config calibration_config; }; void survive_verify_FLT_size( diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index c90d39c..6546e66 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -9,35 +9,12 @@ extern "C" { #endif -typedef struct { - bool enable[2]; - bool invert[2]; - bool swap; -} survive_calibration_options_config; - -typedef struct survive_calibration_config { - - survive_calibration_options_config phase, tilt, curve, gibMag, gibPhase; - - bool gibUseSin; - -} survive_calibration_config; - -void survive_calibration_options_config_apply(const survive_calibration_options_config *option, const FLT *input, - FLT *output); - -const survive_calibration_config *survive_calibration_default_config(const SurviveContext *ctx); - -size_t survive_calibration_config_max_idx(); - -survive_calibration_config survive_calibration_config_create_from_idx(size_t v); - -size_t survive_calibration_config_index(const survive_calibration_config *config); - void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out); void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); // This is given a lighthouse -- in the same system as stored in BaseStationData, and // a 3d point and finds what the effective 'angle' value for a given lighthouse syste @@ -45,18 +22,12 @@ void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, cons // While this is typically opposite of what we want to do -- we want to find the 3d // position from a 2D coordinate, this is helpful since the minimization of reprojection // error is a core mechanism to many types of solvers. -void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out); void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *point3d, FLT *out); -void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, - const FLT *point3d, FLT *out); - -void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *config); - -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, FLT *out); +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out); void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out); #ifdef __cplusplus diff --git a/src/poser_epnp.c b/src/poser_epnp.c index c05450a..2cbd9c1 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -72,9 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) for (size_t i = 0; i < so->sensor_ct; i++) { FLT *lengths = pdfs->lengths[i][lh]; - FLT *_ang = pdfs->angles[i][lh]; - FLT ang[2]; - survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); + FLT *ang = pdfs->angles[i][lh]; + // FLT ang[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; diff --git a/src/poser_sba.c b/src/poser_sba.c index 4a4ed8f..49854f2 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -18,7 +18,6 @@ #include "survive_reproject.h" typedef struct { - survive_calibration_config calibration_config; PoserData *pdfs; SurviveObject *so; SurvivePose obj_pose; @@ -58,8 +57,8 @@ static void metric_function(int j, int i, double *aj, double *xij, void *adata) SurvivePose obj2world = ctx->obj_pose; FLT sensorInWorld[3] = {0}; ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]); - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, - xij); + survive_calibration_config cfg = so->ctx->calibration_config; + survive_reproject_from_pose_with_config(so->ctx, &cfg, j, (SurvivePose *)aj, sensorInWorld, xij); } static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { @@ -73,9 +72,9 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, continue; } - double *_angles = pdfs->angles[sensor][lh]; - double angles[2]; - survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); + double *angles = pdfs->angles[sensor][lh]; + // double angles[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; meas[measCount++] = angles[0]; @@ -93,9 +92,9 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { - double *_a = scene->angles[sensor][lh]; - FLT a[2]; - survive_apply_bsd_calibration(so->ctx, lh, _a, a); + const double *a = scene->angles[sensor][lh]; + // FLT a[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _a, a); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; if (cov) { @@ -160,7 +159,7 @@ static void str_metric_function_single_sweep(int j, int i, double *bi, double *x SurvivePose *camera = &so->ctx->bsd[lh].Pose; FLT out[2]; - survive_reproject_from_pose_with_config(so->ctx, &ctx->hdr.calibration_config, lh, camera, xyz, out); + survive_reproject_from_pose(so->ctx, lh, camera, xyz, out); *xij = out[acode]; } @@ -181,12 +180,11 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); + survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij); } -static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl, - SurviveSensorActivations *scene, int max_iterations /* = 50*/, - double max_reproj_error /* = 0.005*/) { +static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, + int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; SurviveObject *so = d->so; @@ -246,7 +244,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; - sba_context ctx = {options, &pdl->hdr, so}; + sba_context ctx = {&pdl->hdr, so}; opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; @@ -300,15 +298,15 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o } // 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*/) { +static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, + double max_reproj_error /* = 0.005*/) { double *covx = 0; 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(so, pdfs, vmask, meas); - sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + sba_context sbactx = {&pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, .obj_pose = so->OutPose}; { @@ -424,8 +422,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // only process sweeps FLT error = -1; if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { - survive_calibration_config config = *survive_calibration_default_config(ctx); - error = run_sba_find_3d_structure(d, config, lightData, scene, 100, .5); + error = run_sba_find_3d_structure(d, lightData, scene, 100, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } @@ -443,9 +440,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - survive_calibration_config config = *survive_calibration_default_config(ctx); - SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config)); - double error = run_sba(config, pdfs, so, 100, .005); + double error = run_sba(pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } diff --git a/src/survive.c b/src/survive.c index 63ad2ba..2a7aad1 100644 --- a/src/survive.c +++ b/src/survive.c @@ -219,7 +219,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->configfunction = survive_default_htc_config_process; ctx->rawposeproc = survive_default_raw_pose_process; - ctx->calibration_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); + ctx->calibration_config = survive_calibration_config_ctor(); + ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); return ctx; } diff --git a/src/survive_cal.c b/src/survive_cal.c index e094e7b..3015b68 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -13,12 +13,13 @@ #include "survive_internal.h" #include "survive_reproject.h" +#include +#include #include +#include #include #include #include -#include -#include #include "survive_config.h" @@ -618,9 +619,8 @@ static void handle_calibration( struct SurviveCalData *cd ) } fsd.lengths[i][j][0] = cd->avglens[dataindex+0]; fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; - // fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; - // fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; - survive_apply_bsd_calibration(ctx, lh, &cd->avgsweeps[dataindex], fsd.angles[i][j]); + fsd.angles[i][j][0] = cd->avgsweeps[dataindex + 0]; + fsd.angles[i][j][1] = cd->avgsweeps[dataindex + 1]; fsd.synctimes[i][j] = temp_syncs[i][j]; } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 845f30a..ee9704f 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -4,75 +4,6 @@ #include #include -static void survive_calibration_options_config_normalize( - survive_calibration_options_config *option) { - if (!option->enable[0]) - option->invert[0] = false; - if (!option->enable[1]) - option->invert[1] = false; - if (!option->enable[0] && !option->enable[1]) - option->swap = false; -} - -void survive_calibration_options_config_apply( - const survive_calibration_options_config *option, const FLT *input, - FLT *output) { - FLT tmp[2]; // In case they try to do in place - for (int i = 0; i < 2; i++) { - tmp[i] = option->enable[i] * (option->invert[i] ? -1 : 1) * - input[i ^ option->swap]; - } - for (int i = 0; i < 2; i++) { - output[i] = tmp[i]; - } -} - -survive_calibration_config -survive_calibration_config_create_from_idx(size_t v) { - survive_calibration_config config; - memset(&config, 0, sizeof(config)); - - bool *_this = (bool *)&config; - - for (size_t i = 0; i < sizeof(config); i++) { - _this[i] = (bool)(v & 1); - v = v >> 1; - } - - survive_calibration_options_config_normalize(&config.phase); - survive_calibration_options_config_normalize(&config.tilt); - survive_calibration_options_config_normalize(&config.curve); - survive_calibration_options_config_normalize(&config.gibMag); - - config.gibPhase.enable[0] = config.gibMag.enable[0]; - config.gibPhase.enable[1] = config.gibMag.enable[1]; - - survive_calibration_options_config_normalize(&config.gibPhase); - - if (!config.gibPhase.enable[0] && !config.gibPhase.enable[1]) - config.gibUseSin = false; - - return config; -} - -size_t -survive_calibration_config_index(const survive_calibration_config *config) { - bool *_this = (bool *)config; - size_t v = 0; - for (size_t i = 0; i < sizeof(*config); i++) { - v = (v | _this[sizeof(*config) - i - 1]); - v = v << 1; - } - v = v >> 1; - return v; -} - -static FLT gibf(bool useSin, FLT v) { - if (useSin) - return sin(v); - return cos(v); -} - void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *pt, FLT *out) { LinmathQuat invq; @@ -89,116 +20,43 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv FLT x = -t_pt[0] / -t_pt[2]; FLT y = t_pt[1] / -t_pt[2]; - double ang_x = atan(x); - double ang_y = atan(y); - - double phase[2]; - survive_calibration_options_config_apply(&config->phase, bsd->fcal.phase, phase); - double tilt[2]; - survive_calibration_options_config_apply(&config->tilt, bsd->fcal.tilt, tilt); - double curve[2]; - survive_calibration_options_config_apply(&config->curve, bsd->fcal.curve, curve); - double gibPhase[2]; - survive_calibration_options_config_apply(&config->gibPhase, bsd->fcal.gibpha, gibPhase); - double gibMag[2]; - survive_calibration_options_config_apply(&config->gibMag, bsd->fcal.gibmag, gibMag); - - out[0] = ang_x + phase[0] + (tilt[0]) * ang_y + curve[0] * ang_y * ang_y + - gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0]; - out[1] = ang_y + phase[1] + (tilt[1]) * ang_x + curve[1] * ang_x * ang_x + - gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1]; -} -void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { - const BaseStationData *bsd = &ctx->bsd[lighthouse]; - survive_reproject_from_pose_with_bsd(bsd, config, pose, pt, out); -} - -void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, - const FLT *point3d, FLT *out) { - survive_reproject_from_pose_with_config(ctx, config, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); -} - -void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, - const SurvivePose *pose, FLT *pt, FLT *out) { - survive_reproject_from_pose_with_config(ctx, survive_calibration_default_config(ctx), lighthouse, pose, pt, out); -} + double ang[] = {atan(x), atan(y)}; -void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, - FLT *out) { - survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, - point3d, out); -} + const FLT *phase = bsd->fcal.phase; + const FLT *curve = bsd->fcal.curve; + const FLT *tilt = bsd->fcal.tilt; + const FLT *gibPhase = bsd->fcal.gibpha; + const FLT *gibMag = bsd->fcal.gibmag; + enum SurviveCalFlag f = config->use_flag; -const survive_calibration_config *survive_calibration_default_config(const SurviveContext *_ctx) { - SurviveContext *ctx = (SurviveContext *)_ctx; - if (ctx->calibration_config == 0) { - size_t idx = survive_configi(ctx, "default-cal-conf", SC_GET, 0); - ctx->calibration_config = malloc(sizeof(survive_calibration_config)); - memset(ctx->calibration_config, 0, sizeof(survive_calibration_config)); - *ctx->calibration_config = survive_calibration_config_create_from_idx(idx); - } - return ctx->calibration_config; -} + for (int axis = 0; axis < 2; axis++) { + int opp_axis = axis == 0 ? 1 : 0; -size_t survive_calibration_config_max_idx() { - survive_calibration_config cfg; - memset(&cfg, 0x1, sizeof(survive_calibration_config)); - return survive_calibration_config_index(&cfg); -} + out[axis] = ang[axis]; -static void survive_calibration_options_config_fprint(FILE *file, const survive_calibration_options_config *self) { - fprintf(file, "\t"); - if (!self->enable[0] && !self->enable[1]) { - fprintf(file, "disabled"); - return; + if (f & SVCal_Phase) + out[axis] -= config->phase_scale * phase[axis]; + if (f & SVCal_Tilt) + out[axis] -= (config->tilt_scale * tilt[axis]) * ang[opp_axis]; + if (f & SVCal_Curve) + out[axis] -= config->curve_scale * curve[axis] * ang[opp_axis] * ang[opp_axis]; + if (f & SVCal_Gib) + out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; } - fprintf(file, "swap: %d\n", self->swap); - for (int i = 0; i < 2; i++) { - if (self->enable[i]) { - fprintf(file, "\tinvert[%d]: %d", i, self->invert[i]); - } else { - fprintf(file, "\t%d: disabled", i); - } - } } -void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *self) { - fprintf(file, "Index: %ld\n", survive_calibration_config_index(self)); - - fprintf(file, "Phase: \n"); - survive_calibration_options_config_fprint(file, &self->phase); - fprintf(file, "\n"); - - fprintf(file, "Tilt: \n"); - survive_calibration_options_config_fprint(file, &self->tilt); - fprintf(file, "\n"); - - fprintf(file, "Curve: \n"); - survive_calibration_options_config_fprint(file, &self->curve); - fprintf(file, "\n"); - - fprintf(file, "gibPhase: \n"); - survive_calibration_options_config_fprint(file, &self->gibPhase); - fprintf(file, "\n"); - - fprintf(file, "gibMag: \n"); - survive_calibration_options_config_fprint(file, &self->gibMag); - fprintf(file, "\n"); - - fprintf(file, "gibUseSin: %d\n", self->gibUseSin); -} - -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, - FLT *out) { +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out) { const BaseStationCal *cal = &ctx->bsd[lh].fcal; - out[0] = in[0] + cal->phase[0]; - out[1] = in[1] + cal->phase[1]; - - FLT tilt_scale = 10; - FLT curve_scale = 10000; - FLT gib_scale = 10; + out[0] = in[0] + config->phase_scale * cal->phase[0]; + out[1] = in[1] + config->phase_scale * cal->phase[1]; + + enum SurviveCalFlag f = config->use_flag; + FLT phase_scale = config->phase_scale; + FLT tilt_scale = config->tilt_scale; + FLT curve_scale = config->curve_scale; + FLT gib_scale = config->gib_scale; const int iterations = 4; for (int i = 0; i < iterations; i++) { FLT last_out[2] = {out[0], out[1]}; @@ -207,17 +65,39 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum Sur int oj = j == 0 ? 1 : 0; out[j] = in[j]; if (!last_iteration || (f & SVCal_Phase)) - out[j] += (cal->phase[j]); + out[j] += phase_scale * cal->phase[j]; if (!last_iteration || (f & SVCal_Tilt)) - out[j] += tan(cal->tilt[j] / tilt_scale) * last_out[oj]; + out[j] += (tilt_scale * cal->tilt[j]) * last_out[oj]; if (!last_iteration || (f & SVCal_Curve)) - out[j] += (cal->curve[j] / curve_scale) * last_out[oj] * last_out[oj]; + out[j] += (cal->curve[j] * curve_scale) * last_out[oj] * last_out[oj]; if (!last_iteration || (f & SVCal_Gib)) - out[j] += cos(cal->gibpha[j] + last_out[j]) * cal->gibmag[1] / gib_scale; + out[j] += sin(cal->gibpha[j] + last_out[j]) * cal->gibmag[j] * gib_scale; } } } +void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *pt, + FLT *out) { + survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], &ctx->calibration_config, pose, pt, out); +} + +void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out) { + survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); +} + +survive_calibration_config survive_calibration_config_ctor() { + return (survive_calibration_config){.use_flag = SVCal_All, + .phase_scale = 1., + .tilt_scale = 1. / 10000., + .curve_scale = 1. / 1000., + .gib_scale = -1. / 10.}; +} + void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { - survive_apply_bsd_calibration_by_flag(ctx, lh, ctx->calibration_flag, in, out); + survive_apply_bsd_calibration_by_flag(ctx, lh, &ctx->calibration_config, in, out); +} + +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out) { + return survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out); } diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc index 874ed84..88a43eb 100644 --- a/tools/findoptimalconfig/findoptimalconfig.cc +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -11,35 +11,6 @@ #include #include -std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self) { - o << "\t"; - if (!self.enable[0] && !self.enable[1]) { - o << "disabled"; - return o; - } - - o << "swap: " << self.swap << std::endl; - for (int i = 0; i < 2; i++) { - if (self.enable[i]) { - o << "\tinvert[" << i << "]: " << self.invert[i]; - } else { - o << "\t" << i << ": disabled"; - } - } - return o; -} - -std::ostream &operator<<(std::ostream &o, const survive_calibration_config &self) { - o << "Index: " << survive_calibration_config_index(&self) << std::endl; - o << "Phase: " << std::endl << self.phase << std::endl; - o << "Tilt: " << std::endl << self.tilt << std::endl; - o << "Curve: " << std::endl << self.curve << std::endl; - o << "gibPhase: " << std::endl << self.gibPhase << std::endl; - o << "gibMag: " << std::endl << self.gibMag << std::endl; - o << "gibUseSin: " << self.gibUseSin << std::endl; - return o; -} - struct SBAData { int last_acode = -1; int last_lh = -1; @@ -279,9 +250,7 @@ double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &c double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; - BaseStationCal cal[2] = {}; - - opts[0] = SBA_INIT_MU; + survive_calibration_config opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; opts[2] = SBA_STOP_THRESH; opts[3] = SBA_STOP_THRESH; @@ -402,49 +371,12 @@ int main(int argc, char **argv) { while (survive_poll(ctx) == 0) { } - survive_calibration_config config = {}; - // config.tilt.enable[0] = config.tilt.enable[1] = 1; - // config.curve.enable[0] = config.curve.enable[1] = 1; - config.phase.enable[0] = config.phase.enable[1] = 1; - // config.gibPhase.enable[0] = config.gibPhase.enable[1] = 1; - // config.gibMag.enable[0] = config.gibMag.enable[1] = 1; + survive_calibration_config config = survive_calibration_config_ctor(); find_optimal_cal(ctx, config, data); - for (int j = 0; j < sections.size(); j++) { - auto &range = sections[j]; - for (size_t _i = 0; _i < (1 << range.first); _i++) { - int i = (_i << range.second); - survive_calibration_config config = survive_calibration_config_create_from_idx(i); - if (i == survive_calibration_config_index(&config)) { - double error = find_avg_reproj_error(ctx, config, data); - errors[j][i] += error; - } - } - std::cerr << "Finished grouping " << j << std::endl; - } - survive_close(ctx); } - for (int i = 0; i < errors.size(); i++) { - std::cout << "Grouping " << i << std::endl; - auto compFunctor = [](std::pair elem1, std::pair elem2) { - if (elem1.second == elem2.second) - return elem1.first < elem2.first; - return elem1.second < elem2.second; - }; - - std::set, typeof(compFunctor)> set(errors[i].begin(), errors[i].end(), compFunctor); - - for (auto err : set) { - survive_calibration_config config = survive_calibration_config_create_from_idx(err.first); - if (err.first == survive_calibration_config_index(&config)) { - double error = err.second; - std::cout << "Config " << err.first << " " << error << std::endl; - std::cout << config << std::endl; - } - } - } return 0; } -- cgit v1.3.1 From aea08a70a033cc0aef0998267fadb54af5fb2c69 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 15:37:16 -0600 Subject: More optimization of scale params for calibration --- include/libsurvive/survive.h | 2 +- src/survive_reproject.c | 15 ++++---- tools/findoptimalconfig/findoptimalconfig.cc | 54 ++++++++++++++-------------- 3 files changed, 35 insertions(+), 36 deletions(-) (limited to 'src') diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 669821d..7248b1c 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -186,8 +186,8 @@ enum SurviveCalFlag { }; typedef struct survive_calibration_config { - enum SurviveCalFlag use_flag; FLT phase_scale, tilt_scale, curve_scale, gib_scale; + enum SurviveCalFlag use_flag; } survive_calibration_config; survive_calibration_config survive_calibration_config_ctor(); diff --git a/src/survive_reproject.c b/src/survive_reproject.c index ee9704f..751abc0 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -19,7 +19,7 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv FLT x = -t_pt[0] / -t_pt[2]; FLT y = t_pt[1] / -t_pt[2]; - + double xy[] = {x, y}; double ang[] = {atan(x), atan(y)}; const FLT *phase = bsd->fcal.phase; @@ -37,9 +37,9 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv if (f & SVCal_Phase) out[axis] -= config->phase_scale * phase[axis]; if (f & SVCal_Tilt) - out[axis] -= (config->tilt_scale * tilt[axis]) * ang[opp_axis]; + out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis]; if (f & SVCal_Curve) - out[axis] -= config->curve_scale * curve[axis] * ang[opp_axis] * ang[opp_axis]; + out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis]; if (f & SVCal_Gib) out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; } @@ -60,6 +60,7 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct s const int iterations = 4; for (int i = 0; i < iterations; i++) { FLT last_out[2] = {out[0], out[1]}; + FLT tlast_out[2] = {tan(out[0]), tan(out[1])}; bool last_iteration = i == iterations - 1; for (int j = 0; j < 2; j++) { int oj = j == 0 ? 1 : 0; @@ -67,9 +68,9 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct s if (!last_iteration || (f & SVCal_Phase)) out[j] += phase_scale * cal->phase[j]; if (!last_iteration || (f & SVCal_Tilt)) - out[j] += (tilt_scale * cal->tilt[j]) * last_out[oj]; + out[j] += tan(tilt_scale * cal->tilt[j]) * tlast_out[oj]; if (!last_iteration || (f & SVCal_Curve)) - out[j] += (cal->curve[j] * curve_scale) * last_out[oj] * last_out[oj]; + out[j] += (cal->curve[j] * curve_scale) * tlast_out[oj] * tlast_out[oj]; if (!last_iteration || (f & SVCal_Gib)) out[j] += sin(cal->gibpha[j] + last_out[j]) * cal->gibmag[j] * gib_scale; } @@ -88,8 +89,8 @@ void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, survive_calibration_config survive_calibration_config_ctor() { return (survive_calibration_config){.use_flag = SVCal_All, .phase_scale = 1., - .tilt_scale = 1. / 10000., - .curve_scale = 1. / 1000., + .tilt_scale = 1. / 10., + .curve_scale = 1. / 10., .gib_scale = -1. / 10.}; } diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc index 88a43eb..b94590f 100644 --- a/tools/findoptimalconfig/findoptimalconfig.cc +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -200,35 +200,36 @@ double sba_opt(SurviveContext *ctx, const survive_calibration_config &config, Pl struct optimal_cal_ctx { std::vector sensors; + std::vector lighthouses; SurviveContext *ctx; - survive_calibration_config config; }; static void metric_function(int j, int i, double *aj, double *xij, void *adata) { optimal_cal_ctx *ctx = (optimal_cal_ctx *)(adata); FLT sensorInWorld[3] = {ctx->sensors[i * 3 + 0], ctx->sensors[i * 3 + 1], ctx->sensors[i * 3 + 2]}; + int lh = ctx->lighthouses[i]; + BaseStationData bsd = ctx->ctx->bsd[lh]; + survive_calibration_config cfg = *(survive_calibration_config *)aj; - BaseStationData bsd = ctx->ctx->bsd[j]; - bsd.fcal = *(BaseStationCal *)aj; - - survive_reproject_from_pose_with_bsd(&bsd, &ctx->config, &ctx->ctx->bsd[j].Pose, sensorInWorld, xij); + survive_reproject_from_pose_with_bsd(&bsd, &cfg, &ctx->ctx->bsd[lh].Pose, sensorInWorld, xij); } -double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &config, PlaybackData &data) { +double find_optimal_cal(SurviveContext *ctx, PlaybackData &data) { optimal_cal_ctx _ctx; std::vector vmask; std::vector cov, meas; _ctx.ctx = ctx; - _ctx.config = config; for (auto &in : data.inputs) { for (size_t sensor = 0; sensor < in.so->sensor_ct; sensor++) { FLT p[3]; ApplyPoseToPoint(p, &in.position, &data.so->sensor_locations[sensor * 3]); - _ctx.sensors.emplace_back(p[0]); - _ctx.sensors.emplace_back(p[1]); - _ctx.sensors.emplace_back(p[2]); - for (size_t lh = 0; lh < 1; lh++) { + for (size_t lh = 0; lh < 2; lh++) { + _ctx.sensors.emplace_back(p[0]); + _ctx.sensors.emplace_back(p[1]); + _ctx.sensors.emplace_back(p[2]); + _ctx.lighthouses.emplace_back(lh); + auto scene = &in.activations; if (SurviveSensorActivations_isPairValid(scene, settings.sensor_time_window, in.timestamp, sensor, lh)) { @@ -250,22 +251,25 @@ double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &c double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; - survive_calibration_config opts[0] = SBA_INIT_MU; + survive_calibration_config config = {0}; + config.use_flag = SVCal_All; + + 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_mot_levmar(data.inputs.size() * so->sensor_ct, // number of 3d points - 1, // Number of cameras -- 2 lighthouses - 0, // Number of cameras to not modify - &vmask[0], // boolean vis mask - (double *)cal, // camera parameters - 2, // sizeof(BaseStationCal) / sizeof(FLT), - &meas[0], // 2d points for 3d objs - covx, // covariance of measurement. Null sets to identity - 2, // 2 points per image + int status = sba_mot_levmar(data.inputs.size() * so->sensor_ct * NUM_LIGHTHOUSES, // number of 3d points + 1, // Number of cameras -- 2 lighthouses + 0, // Number of cameras to not modify + &vmask[0], // boolean vis mask + (double *)&config, // camera parameters + 4, // sizeof(BaseStationCal) / sizeof(FLT), + &meas[0], // 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 @@ -295,10 +299,6 @@ double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &c assert(!isinf(info[1])); std::cerr << "Used " << meas_size << " measurements" << std::endl; - double *_cal = (double *)cal; - for (int i = 0; i < sizeof(BaseStationCal) / sizeof(FLT); i++) - std::cerr << _cal[2 * i] << ", " << _cal[2 * i + 1] << " = " << (info[1] / meas_size * 2) << std::endl; - return info[1] / meas_size * 2; } double find_avg_reproj_error(SurviveContext *ctx, const survive_calibration_config &config, PlaybackDataInput &data) { @@ -371,9 +371,7 @@ int main(int argc, char **argv) { while (survive_poll(ctx) == 0) { } - survive_calibration_config config = survive_calibration_config_ctor(); - - find_optimal_cal(ctx, config, data); + find_optimal_cal(ctx, data); survive_close(ctx); } -- cgit v1.3.1 From 1724abef15a4090640bd82ba408681438316de7e Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 16:04:05 -0600 Subject: Made calibration on other posers use calibration data --- .gitignore | 1 + include/libsurvive/survive_reproject.h | 14 +++++++++++--- src/poser_charlesslow.c | 28 +++++++++++++++++++--------- src/poser_daveortho.c | 17 ++++++++++------- src/poser_epnp.c | 6 +++--- src/poser_sba.c | 2 -- src/poser_turveytori.c | 18 ++++++++++++++---- src/survive_reproject.c | 6 +++--- tools/showreproject/showreproject.cc | 2 +- 9 files changed, 62 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/.gitignore b/.gitignore index 8cfd2be..f39d866 100644 --- a/.gitignore +++ b/.gitignore @@ -19,6 +19,7 @@ simple_pose_test *.csv calinfo/ *.log +*.png # Windows specific *.dll diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index 6546e66..e4f21d0 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -17,8 +17,9 @@ void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct s int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); // This is given a lighthouse -- in the same system as stored in BaseStationData, and -// a 3d point and finds what the effective 'angle' value for a given lighthouse syste +// a 3d point and finds what the effective 'angle' value for a given lighthouse system // would be. +// // While this is typically opposite of what we want to do -- we want to find the 3d // position from a 2D coordinate, this is helpful since the minimization of reprojection // error is a core mechanism to many types of solvers. @@ -26,10 +27,17 @@ void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct s void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *point3d, FLT *out); -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, - const FLT *in, FLT *out); +// This is given input from the light sensors and approximates the idealized version of them +// by incorporating the calibration data from the lighthouse. In theory, it's an approximation +// but in practice in converges pretty quickly and to a good degree of accuracy. +// That said, all things being equal, it is better to compare reprojection to raw incoming +// data if you are looking to minimize that error. void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out); +// Same as above, but lets you specify the configuration. Used internally and also in some tools +void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out); + #ifdef __cplusplus } #endif diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index bc6683a..b44225e 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -1,12 +1,13 @@ +#include "linmath.h" #include "survive_cal.h" +#include +#include #include -#include -#include "linmath.h" -#include #include #include -#include -#include +#include +#include +#include typedef struct { @@ -255,9 +256,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2; if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; + FLT out[2] = {}; + survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); + //Find out where our ray shoots forth from. - FLT ax = fs->angles[p][lh][0]; - FLT ay = fs->angles[p][lh][1]; + FLT ax = out[0]; + FLT ay = out[1]; + //NOTE: Inputs may never be output with cross product. //Create a fictitious normalized ray. Imagine the lighthouse is pointed //straight in the +z direction, this is the lighthouse ray to the point. @@ -338,8 +343,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue; //Find out where our ray shoots forth from. - FLT ax = fs->angles[p][lh][0]; - FLT ay = fs->angles[p][lh][1]; + FLT out[2] = {}; + survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out); + + // Find out where our ray shoots forth from. + FLT ax = out[0]; + FLT ay = out[1]; + FLT RayShootOut[3] = { sin(ax), sin(ay), 0 }; RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) ); diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index c47bceb..9cdab45 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -1,12 +1,13 @@ +#include "linmath.h" #include "survive_cal.h" +#include +#include #include -#include -#include "linmath.h" -#include #include #include -#include -#include +#include +#include +#include // Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s @@ -139,8 +140,10 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) //Load all our valid points into something the LHFinder can use. if( fs->lengths[i][LH_ID][0] > 0 ) { - S_in[0][max_hits] = fs->angles[i][LH_ID][0]; - S_in[1][max_hits] = fs->angles[i][LH_ID][1]; + FLT out[2]; + survive_apply_bsd_calibration(ctx, LH_ID, fs->angles[i][LH_ID], out); + S_in[0][max_hits] = out[0]; + S_in[1][max_hits] = out[1]; X_in[0][max_hits] = so->sensor_locations[i*3+0]; X_in[1][max_hits] = so->sensor_locations[i*3+1]; X_in[2][max_hits] = so->sensor_locations[i*3+2]; diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 2cbd9c1..c05450a 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -72,9 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) for (size_t i = 0; i < so->sensor_ct; i++) { FLT *lengths = pdfs->lengths[i][lh]; - FLT *ang = pdfs->angles[i][lh]; - // FLT ang[2]; - // survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); + FLT *_ang = pdfs->angles[i][lh]; + FLT ang[2]; + survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; diff --git a/src/poser_sba.c b/src/poser_sba.c index 49854f2..bd7d520 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -73,8 +73,6 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, } double *angles = pdfs->angles[sensor][lh]; - // double angles[2]; - // survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; meas[measCount++] = angles[0]; diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 035fca7..4628207 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -13,6 +13,8 @@ #include #else #include //for alloca +#include + #endif @@ -1786,8 +1788,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; - to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + FLT out[2]; + survive_apply_bsd_calibration(ctx, 0, fs->angles[i][0], out); + + to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) sensorCount++; } @@ -1822,8 +1828,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.x = point[0]; to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; - to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + + FLT out[2]; + survive_apply_bsd_calibration(ctx, lh, fs->angles[i][lh], out); + + to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; } } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 751abc0..0eaceb2 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -46,8 +46,8 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv } -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, - const FLT *in, FLT *out) { +void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out) { const BaseStationCal *cal = &ctx->bsd[lh].fcal; out[0] = in[0] + config->phase_scale * cal->phase[0]; out[1] = in[1] + config->phase_scale * cal->phase[1]; @@ -95,7 +95,7 @@ survive_calibration_config survive_calibration_config_ctor() { } void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { - survive_apply_bsd_calibration_by_flag(ctx, lh, &ctx->calibration_config, in, out); + survive_apply_bsd_calibration_by_config(ctx, lh, &ctx->calibration_config, in, out); } void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 89d67c4..caa7a66 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -176,7 +176,7 @@ void drawbsds(SurviveContext *ctx) { FLT out[2]; auto config = survive_calibration_config_ctor(); config.use_flag = f; - survive_apply_bsd_calibration_by_flag(ctx, lh, &config, in, out); + survive_apply_bsd_calibration_by_config(ctx, lh, &config, in, out); double ex = out[0] - in[0]; double ey = out[1] - in[1]; if (f == SVCal_All) { -- cgit v1.3.1