From 86bbd12c3889db3290e22ff61934ca1a218ce114 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 27 Apr 2018 21:42:05 -0400 Subject: New config system --- src/poser_general_optimizer.c | 13 +- src/poser_sba.c | 23 ++-- src/survive.c | 92 +++++++++++---- src/survive_cal.c | 7 +- src/survive_config.c | 267 +++++++++++++++++++++++++++++++++--------- src/survive_config.h | 17 ++- src/survive_driver_dummy.c | 5 +- src/survive_playback.c | 12 +- src/survive_vive.c | 1 + 9 files changed, 338 insertions(+), 99 deletions(-) (limited to 'src') diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c index 057ac3d..186b359 100644 --- a/src/poser_general_optimizer.c +++ b/src/poser_general_optimizer.c @@ -5,6 +5,11 @@ #include #include +STATIC_CONFIG_ITEM( CONFIG_MAX_ERROR, "max-error", 'f', "Maximum error permitted by poser_general_optimizer", .0001 ); +STATIC_CONFIG_ITEM( CONFIG_FAIL_TO_RESET, "failures-to-reset", 'i', "Failures needed before seed poser is re-run", 1 ); +STATIC_CONFIG_ITEM( CONFIG_SUC_TO_RESET, "successes-to-reset", 'i', "[TODO: I don't know what this does]", -1 ); +STATIC_CONFIG_ITEM( CONFIG_SEED_POSER, "seed-poser", 's', "Poser to be used to seed optimizer", "PoserEPNP" ); + void *GetDriver(const char *name); void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) { memset(d, 0, sizeof(*d)); @@ -12,11 +17,11 @@ void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) { SurviveContext *ctx = so->ctx; - d->failures_to_reset = survive_configi(ctx, "failures-to-reset", SC_GET, 1); - d->successes_to_reset = survive_configi(ctx, "successes-to-reset", SC_GET, -1); - d->max_error = survive_configf(ctx, "max-error", SC_GET, .0001); + survive_attach_config( ctx, "failures-to-reset", &d->failures_to_reset, 'i' ); + survive_attach_config( ctx, "successes-to-reset", &d->successes_to_reset, 'i' ); + survive_attach_config( ctx, "max-error", &d->max_error, 'f' ); - const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, "PoserEPNP"); + const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, 0 ); d->seed_poser = (PoserCB)GetDriver(subposer); SV_INFO("Initializing general optimizer:"); diff --git a/src/poser_sba.c b/src/poser_sba.c index ed19c1d..21dd74f 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -20,6 +20,14 @@ #include "survive_kalman.h" #include "survive_reproject.h" + +STATIC_CONFIG_ITEM( SBA_USE_IMU, "sba-use-imu", 'i', "[TODO: I don't know what this does]", 1 ); +STATIC_CONFIG_ITEM( SBA_REQUIRED_MEAS, "sba-required-meas", 'i', "[TODO: I don't know what this does]", 8 ); +STATIC_CONFIG_ITEM( SBA_TIME_WINDOW, "sba-time-window", 'i', "[TODO: I don't know what this does]", (int)SurviveSensorActivations_default_tolerance ); +STATIC_CONFIG_ITEM( SBA_SENSOR_VARIANCE_PER_SEC, "sba-sensor-variance-per-sec", 'f', "[TODO: I don't know what this does]", 10.0 ); +STATIC_CONFIG_ITEM( SBA_SENSOR_VARIANCE, "sba-sensor-variance", 'f', "[TODO: I don't know what this does]", 1.0 ); +STATIC_CONFIG_ITEM( SBA_USE_JACOBIAN_FUNCTION, "sba-use-jacobian-function", 'i', "Poser to be used to seed optimizer", 1); + typedef struct { PoserData *pdfs; SurviveObject *so; @@ -343,14 +351,13 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SBAData *d = so->PoserData; general_optimizer_data_init(&d->opt, so); - d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1); - d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); - - 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, 10.0); - d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); - d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); + + survive_attach_config( ctx, "sba-use-imu", &d->useIMU, 'i' ); + survive_attach_config( ctx, "sba-required-meas", &d->required_meas, 'i' ); + survive_attach_config( ctx, "sba-time-window", &d->sensor_time_window, 'i' ); + survive_attach_config( ctx, "sba-sensor-variance-per-sec", &d->sensor_variance_per_second, 'f' ); + survive_attach_config( ctx, "sba-sensor-variance", &d->sensor_variance, 'f' ); + survive_attach_config( ctx, "sba-use-jacobian-function", &d->use_jacobian_function, 'i' ); SV_INFO("Initializing SBA:"); SV_INFO("\tsba-required-meas: %d", d->required_meas); diff --git a/src/survive.c b/src/survive.c index 20a2a1b..2bdefca 100644 --- a/src/survive.c +++ b/src/survive.c @@ -16,6 +16,10 @@ #define z_const const #endif +STATIC_CONFIG_ITEM( CONFIG_FILE, "configfile", 's', "Default configuration file", "config.json" ); +STATIC_CONFIG_ITEM( CONFIG_D_CALI, "disable-calibrate", 'i', "Enables or disables calibration", 0 ); +STATIC_CONFIG_ITEM( CONFIG_F_CALI, "force-calibrate", 'i', "Forces calibration even if one exists.", 0 ); + #ifdef RUNTIME_SYMNUM #include static int did_runtime_symnum; @@ -93,6 +97,20 @@ void survive_verify_FLT_size(uint32_t user_size) { } } +static void PrintMatchingDrivers( const char * prefix, const char * matchingparam ) +{ + int i = 0; + char stringmatch[128]; + snprintf( stringmatch, 127, "%s%s", prefix, matchingparam?matchingparam:"" ); + const char * DriverName; + while ((DriverName = GetDriverNameMatching(stringmatch, i++))) + { + printf( "%s ", DriverName+strlen(prefix) ); + } +} + + + SurviveContext *survive_init_internal(int argc, char *const *argv) { int i; @@ -133,8 +151,6 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->state = SURVIVE_STOPPED; - survive_config_populate_ctx( ctx ); - ctx->global_config_values = malloc(sizeof(config_group)); ctx->temporary_config_values = malloc(sizeof(config_group)); ctx->lh_config = malloc(sizeof(config_group) * NUM_LIGHTHOUSES); @@ -148,6 +164,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { // Process command-line parameters. char *const *av = argv + 1; char *const *argvend = argv + argc; + int list_for_autocomplete = 0; + const char * autocomplete_match[3] = { 0, 0, 0}; int showhelp = 0; for (; av < argvend; av++) { if ((*av)[0] != '-') @@ -168,6 +186,13 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { case 'l': vartoupdate = "lighthousecount"; break; + case 'm': + if( av + 1 < argvend ) autocomplete_match[0] = *(av+1); + if( av + 2 < argvend ) autocomplete_match[1] = *(av+2); + if( av + 3 < argvend ) autocomplete_match[2] = *(av+3); + list_for_autocomplete = 1; + av = argvend; //Eject immediately after processing -m + break; case 'c': vartoupdate = "configfile"; break; @@ -196,33 +221,54 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { } } } - if (showhelp) { - // Can't use SV_ERROR here since we don't have a context to send to yet. - fprintf(stderr, "libsurvive - usage:\n"); - fprintf(stderr, " --[parameter] [value] - sets parameter\n"); - fprintf(stderr, " -h - shows help.\n"); - 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, " --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"); - } - config_read(ctx, survive_configs(ctx, "configfile", SC_GET, "config.json")); ctx->activeLighthouses = survive_configi(ctx, "lighthousecount", SC_SETCONFIG, 2); config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0); config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1); + ctx->faultfunction = survivefault; + ctx->notefunction = survivenote; + + if( list_for_autocomplete ) + { + const char * lastparam = (autocomplete_match[2]==0)?autocomplete_match[1]:autocomplete_match[2]; + const char * matchingparam = (autocomplete_match[2]==0)?0:autocomplete_match[1]; + //First see if any of the parameters perfectly match a config item, if so print some help. + //fprintf( stderr, "!!! %s !!! %s !!!\n", lastparam, matchingparam ); + + const char * checkconfig = matchingparam; + if( matchingparam == 0 ) checkconfig = lastparam; + + if( checkconfig && strlen( checkconfig ) > 2 && survive_print_help_for_parameter( checkconfig+2 ) ) + { + exit(0); + } + + if( strstr( lastparam, "poser" ) ) PrintMatchingDrivers( "Poser", matchingparam ); + else if( strstr( lastparam, "disambiguator" ) ) PrintMatchingDrivers( "Disambiguator", matchingparam ); + else + { + printf( "-h -m -p -l -c " ); + survive_print_known_configs( ctx, 0 ); + } + printf( "\n" ); + exit( 0 ); + } + if( showhelp ) { - survive_print_known_configs( ctx ); + // Can't use SV_ERROR here since we don't have a context to send to yet. + fprintf(stderr, "libsurvive - usage:\n"); + fprintf(stderr, " -h - shows help.\n"); + fprintf(stderr, " -m - list parameters, for autocomplete." ); + 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, "Additional --[parameter] [value] - sets generic parameters...\n"); + survive_print_known_configs( ctx, 1 ); return 0; } - ctx->faultfunction = survivefault; - ctx->notefunction = survivenote; ctx->lightproc = survive_default_light_process; ctx->imuproc = survive_default_imu_process; ctx->angleproc = survive_default_angle_process; @@ -319,10 +365,10 @@ int survive_startup(SurviveContext *ctx) { ctx->state = SURVIVE_RUNNING; - int calibrateMandatory = survive_configi(ctx, "calibrate", SC_GET, 0); - int calibrateForbidden = survive_configi(ctx, "calibrate", SC_GET, 1) == 0; + int calibrateMandatory = survive_configi(ctx, "force-calibrate", SC_GET, 0); + int calibrateForbidden = survive_configi(ctx, "disable-calibrate", SC_GET, 1) == 0; if (calibrateMandatory && calibrateForbidden) { - SV_INFO("Contradictory settings --calibrate and --no-calibrate specified. Switching to normal behavior."); + SV_INFO("Contradictory settings --force-calibrate and --disable-calibrate specified. Switching to normal behavior."); calibrateMandatory = calibrateForbidden = 0; } @@ -334,7 +380,7 @@ 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"); + "the duration of calibration. Pass '--disable-calibrate' to skip calibration"); } else { SV_INFO("Calibration requested. Previous calibration will be overwritten."); } diff --git a/src/survive_cal.c b/src/survive_cal.c index ad62d22..cf65166 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -27,6 +27,11 @@ int mkdir(const char *); #endif + +STATIC_CONFIG_ITEM( REQ_TRACK_FOR_CAL, "requiredtrackersforcal", 's', "Which devices will be used, i.e. HMD,WM0,WM1", "" ); +STATIC_CONFIG_ITEM( ALLOW_TRACK_FOR_CAL, "allowalltrackersforcal", 'i', "Allow use of additional connected devices for calibration", 0 ); +STATIC_CONFIG_ITEM( CONFIG_POSER, "configposer", 's', "Poser used for calibration step", "SBA" ); + #define PTS_BEFORE_COMMON 32 #define NEEDED_COMMON_POINTS 10 #define MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL 4 @@ -145,7 +150,7 @@ void survive_cal_install( struct SurviveContext * ctx ) // basically, libsurvive will detect whatever they have plugged in and start using that. // const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1"); const char *RequiredTrackersForCal = survive_configs(ctx, "requiredtrackersforcal", SC_SETCONFIG, ""); - const uint32_t AllowAllTrackersForCal = survive_configi(ctx, "allowalltrackersforcal", SC_SETCONFIG, 1); + const uint32_t AllowAllTrackersForCal = survive_configi(ctx, "allowalltrackersforcal", SC_SETCONFIG, 0); size_t requiredTrackersFound = 0; for (int j=0; j < ctx->objs_ct; j++) diff --git a/src/survive_config.c b/src/survive_config.c index 92c0125..23433c4 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -28,28 +28,32 @@ struct static_conf_t }; static struct static_conf_t static_configs[MAX_SHORTHAND_CONFIGS]; +int nr_static_configs; -void survive_config_bind_variable( char vt, int * variable, const char * name, const char * description, ... ) +void survive_config_bind_variable( char vt, const char * name, const char * description, ... ) { va_list ap; va_start(ap, description); int i; struct static_conf_t * config; - for( i = 0; i < MAX_SHORTHAND_CONFIGS; i++ ) + for( i = 0; i < nr_static_configs; i++ ) { config = &static_configs[i]; - if( !config->name || strcmp( config->name, name ) == 0 ) break; + if( strcmp( config->name, name ) == 0 ) break; } + config = &static_configs[i]; if( i == MAX_SHORTHAND_CONFIGS ) { fprintf( stderr, "Fatal: Too many static configuration items. Please recompile with a higher MAX_STATIC_CONFIGS\n" ); exit( -1 ); } + if( i == nr_static_configs ) nr_static_configs++; + if( !config->description ) config->description = description; if( !config->name ) config->name = name; if( config->type && config->type != vt ) { - fprintf( stderr, "Fatal: Internal error on variable %s. Types disagree.\n", name ); + fprintf( stderr, "Fatal: Internal error on variable %s. Types disagree [%c/%c].\n", name, config->type, vt ); exit( -2 ); } config->type = vt; @@ -61,50 +65,113 @@ void survive_config_bind_variable( char vt, int * variable, const char * name, c default: fprintf( stderr, "Fatal: Internal error on variable %s. Unknown type %c\n", name, vt ); } - *variable = i; } -void survive_config_populate_ctx( SurviveContext * ctx ) +int survive_print_help_for_parameter( const char * tomap ) { int i; - struct static_conf_t * config; - for( i = 0; i < MAX_SHORTHAND_CONFIGS; i++ ) + for( i = 0; i < nr_static_configs; i++ ) { - config = &static_configs[i]; - switch( config->type ) + struct static_conf_t * sc = &static_configs[i]; + if( strcmp( sc->name, tomap ) == 0 ) { - case 'i': ctx->shorthand_configs[i].i = config->data_default.i; - case 'f': ctx->shorthand_configs[i].f = config->data_default.f; - case 's': ctx->shorthand_configs[i].s = config->data_default.s; + char sthelp[160]; + snprintf( sthelp, 159, " %s: %s [%c]", sc->name, sc->description, sc->type ); + fprintf( stderr, "\0337\033[1A\033[1000D%s\0338", sthelp, strlen( sthelp )); + return 1; } } + return 0; } -void survive_print_known_configs( SurviveContext * ctx ) +static void PrintConfigGroup(config_group * grp, const char ** chkval, int * cvs, int verbose ) { - int i; - struct static_conf_t * config; - for( i = 0; i < MAX_SHORTHAND_CONFIGS; i++ ) + int i, j; + for( i = 0; i < grp->used_entries; i++ ) { - config = &static_configs[i]; - if( !config->name ) break; - switch( config->type ) + config_entry * ce = &grp->config_entries[i]; + for( j = 0; j < *cvs; j++ ) + { + if( strcmp( chkval[j], ce->tag ) == 0 ) break; + } + if( j == *cvs ) { - case 'i': printf( "%10d %20s %s\n", config->data_default.i, config->name, config->description ); break; - case 'f': printf( "%10f %20s %s\n", config->data_default.f, config->name, config->description ); break; - case 's': printf( "%10s %20s %s\n", config->data_default.s, config->name, config->description ); break; + if( verbose ) + { + char stobuf[128]; + switch( ce->type ) + { + case CONFIG_UINT32: snprintf( stobuf, 127, " --%s %d", ce->tag, ce->numeric.i ); break; + case CONFIG_FLOAT: snprintf( stobuf, 127, " --%s %f", ce->tag, ce->numeric.f ); break; + case CONFIG_STRING: snprintf( stobuf, 127, " --%s %s", ce->tag, ce->data ); break; + case CONFIG_FLOAT_ARRAY: printf( "[FA] %20s", ce->tag ); break; + } + + int namepad = 40 - strlen( stobuf ); + printf( "%s%*c", stobuf, namepad, (ce->type==CONFIG_FLOAT)?'f':(ce->type==CONFIG_UINT32)?'i':(ce->type==CONFIG_STRING)?'s':'.' ); + + //Try to get description from the static tags. + for( j = 0; j < nr_static_configs; j++ ) + { + if( strcmp( static_configs[j].name, ce->tag ) == 0 ) + { + printf( " %s", static_configs[j].description ); + } + } + printf( "\n" ); + } + else + { + printf( "--%s ", ce->tag ); + } + chkval[*cvs] = ce->tag; + (*cvs)++; } } - - //XXX TODO: Maybe this should print out the actual config values after being updated from the rest of the config system? - //struct config_group *global_config_values; - //struct config_group *temporary_config_values; // Set per-session, from command-line. Not saved but override global_config_values } +void survive_print_known_configs( SurviveContext * ctx, int verbose ) +{ + int i; - - - + const char * checked_values[256]; + int cvs = 0; + memset( checked_values, 0, sizeof( checked_values ) ); + PrintConfigGroup( ctx->temporary_config_values, checked_values, &cvs, verbose ); + PrintConfigGroup( ctx->global_config_values, checked_values, &cvs, verbose ); + int j; + for( j = 0; j < nr_static_configs; j++ ) + { + for( i = 0; i < cvs; i++ ) + { + if( strcmp( static_configs[j].name, checked_values[i] ) == 0 ) + break; + } + if( i == cvs ) + { + struct static_conf_t * config = &static_configs[j]; + const char * name = config->name; + + if( verbose ) + { + char stobuf[128]; + switch( config->type ) + { + case 'i': snprintf( stobuf, 127, " --%s %d", name, config->data_default.i ); break; + case 'f': snprintf( stobuf, 127, " --%s %f", name, config->data_default.f ); break; + case 's': snprintf( stobuf, 127, " --%s %s", name, config->data_default.s ); break; + case 'a': snprintf( stobuf, 127, "[FA] %25s %s\n", config->name, config->description ); break; + } + int namepad = 40 - strlen( stobuf ); + printf( "%s%*c %s\n", stobuf, namepad, config->type, config->description ); + } + else + { + printf( "--%s ", name ); + } + } + } +} const FLT *config_set_float_a(config_group *cg, const char *tag, const FLT *values, uint8_t count); @@ -113,7 +180,7 @@ void init_config_entry(config_entry *ce) { ce->tag = NULL; ce->type = CONFIG_UNKNOWN; ce->elements = 0; - ce->shorthand_place = -1; + ce->update_list = 0; } void destroy_config_entry(config_entry *ce) { @@ -313,20 +380,6 @@ config_entry *next_unused_entry(config_group *cg, const char * tag) { cg->used_entries++; - - int i; - struct static_conf_t * config; - for( i = 0; i < MAX_SHORTHAND_CONFIGS; i++ ) - { - config = &static_configs[i]; - if( !config->name ) break; - if( strcmp( config->name, tag ) == 0 ) - { - cv->shorthand_place = i; - break; - } - } - return cv; } @@ -344,7 +397,8 @@ const char *config_set_str(config_group *cg, const char *tag, const char *value) } cv->type = CONFIG_STRING; - if( cv->shorthand_place >= 0 ) cg->ctx->shorthand_configs[cv->shorthand_place].s = value; + update_list_t * t = cv->update_list; + while( t ) { *((const char **)t->value) = value; t = t->next; } return value; } @@ -358,7 +412,8 @@ const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32 cv->numeric.i = value; cv->type = CONFIG_UINT32; - if( cv->shorthand_place >= 0 ) cg->ctx->shorthand_configs[cv->shorthand_place].i = value; + update_list_t * t = cv->update_list; + while( t ) { *((uint32_t*)t->value) = value; t = t->next; } return value; } @@ -372,7 +427,9 @@ const FLT config_set_float(config_group *cg, const char *tag, const FLT value) { cv->numeric.f = value; cv->type = CONFIG_FLOAT; - if( cv->shorthand_place >= 0 ) cg->ctx->shorthand_configs[cv->shorthand_place].f = value; + + update_list_t * t = cv->update_list; + while( t ) { *((FLT*)t->value) = value; t = t->next; } return value; } @@ -610,6 +667,21 @@ FLT survive_configf(SurviveContext *ctx, const char *tag, char flags, FLT def) { } } + + int i; + if( !(flags & SC_OVERRIDE) ) + { + for( i = 0; i < nr_static_configs; i++ ) + { + struct static_conf_t * sc = &static_configs[i]; + if( strcmp( tag, sc->name ) == 0 ) + { + def = sc->data_default.f; + } + } + } + + // If override is flagged, or, we can't find the variable, ,continue on. if (flags & SC_SETCONFIG) { config_set_float(ctx->temporary_config_values, tag, def); @@ -629,6 +701,21 @@ uint32_t survive_configi(SurviveContext *ctx, const char *tag, char flags, uint3 } } + uint32_t statictimedef = def; + int i; + if( !(flags & SC_OVERRIDE) ) + { + for( i = 0; i < nr_static_configs; i++ ) + { + struct static_conf_t * sc = &static_configs[i]; + if( strcmp( tag, sc->name ) == 0 ) + { + def = sc->data_default.i; + } + } + } + + // If override is flagged, or, we can't find the variable, ,continue on. if (flags & SC_SETCONFIG) { config_set_uint32(ctx->temporary_config_values, tag, def); @@ -647,15 +734,87 @@ const char *survive_configs(SurviveContext *ctx, const char *tag, char flags, co return cv->data; } - // If override is flagged, or, we can't find the variable, ,continue on. - if (flags & SC_SETCONFIG) { - config_set_str(ctx->temporary_config_values, tag, def); - config_set_str(ctx->global_config_values, tag, def); - } else if (flags & SC_SET) { - config_set_str(ctx->temporary_config_values, tag, def); + int i; + char foundtype = 0; + const char * founddata = def; + for( i = 0; i < nr_static_configs; i++ ) + { + struct static_conf_t * sc = &static_configs[i]; + if( !sc ) break; + if( strcmp( tag, sc->name ) == 0 ) + { + founddata = sc->data_default.s; + foundtype = sc->type; + if( !(flags & SC_OVERRIDE) ) + { + def = founddata; + } + } + } + + if( !foundtype || foundtype == 's' ) + { + // If override is flagged, or, we can't find the variable, ,continue on. + if (flags & SC_SETCONFIG) { + config_set_str(ctx->temporary_config_values, tag, def); + config_set_str(ctx->global_config_values, tag, def); + } else if (flags & SC_SET) { + config_set_str(ctx->temporary_config_values, tag, def); + } else { + return founddata; + } + } + else if( foundtype == 'i' ) + { + survive_configi( ctx, tag, flags, atoi( def ) ); + } + else if( foundtype == 'f' ) + { + survive_configf( ctx, tag, flags, atof( def ) ); } return def; } +SURVIVE_EXPORT void survive_attach_config(SurviveContext *ctx, const char *tag, void * var, char type ) +{ + config_entry *cv = sc_search(ctx, tag); + if( !cv ) + { + SV_ERROR( "Configuration item %s not initialized.\n", tag ); + return; + } + update_list_t ** ul = &cv->update_list; + while( *ul ) + { + if( (*ul)->value == var ) return; + ul = &((*ul)->next); + } + update_list_t * t = *ul = malloc( sizeof( update_list_t ) ); + t->next = 0; + t->value = var; +} + +SURVIVE_EXPORT void survive_detach_config(SurviveContext *ctx, const char *tag, void * var ) +{ + config_entry *cv = sc_search(ctx, tag); + if( !cv ) + { + SV_ERROR( "Configuration item %s not initialized.\n", tag ); + return; + } + + update_list_t ** ul = &cv->update_list; + while( *ul ) + { + if( (*ul)->value == var ) + { + update_list_t * v = *ul; + *ul = (*ul)->next; + free( v ); + } + ul = &((*ul)->next); + } +} + diff --git a/src/survive_config.h b/src/survive_config.h index 37e904c..3f823b0 100644 --- a/src/survive_config.h +++ b/src/survive_config.h @@ -16,6 +16,14 @@ typedef enum { CONFIG_FLOAT_ARRAY = 4, } cval_type; +struct update_list_t_s +{ + void * value; + struct update_list_t_s * next; +}; + +typedef struct update_list_t_s update_list_t; + typedef struct { char *tag; @@ -26,7 +34,8 @@ typedef struct { } numeric; char *data; uint32_t elements; - int shorthand_place; + + update_list_t * update_list; } config_entry; typedef struct config_group { @@ -63,8 +72,10 @@ uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t de const char* config_read_str(config_group *cg, const char *tag, const char *def); //These are for the internal non-function configuration system. -void survive_config_bind_variable( char vt, int * variable, const char * name, const char * description, ... ); -void survive_print_known_configs(); +void survive_config_bind_variable( char vt, const char * name, const char * description, ... ); +void survive_print_known_configs( SurviveContext * ctx, int verbose ); void survive_config_populate_ctx( SurviveContext * ctx ); +int survive_print_help_for_parameter( const char * tomap ); + #endif diff --git a/src/survive_driver_dummy.c b/src/survive_driver_dummy.c index 01d0482..e305b33 100644 --- a/src/survive_driver_dummy.c +++ b/src/survive_driver_dummy.c @@ -8,7 +8,7 @@ #include "survive_config.h" #include "os_generic.h" -STATIC_CONFIG_ITEM_I( DUMMY_DRIVER_ENABLE, "dummy-driver-enable", "Load a dummy driver for testing.", 0 ); +STATIC_CONFIG_ITEM( DUMMY_DRIVER_ENABLE, "dummy-driver-enable", 'i', "Load a dummy driver for testing.", 0 ); struct SurviveDriverDummy { SurviveContext * ctx; @@ -55,7 +55,8 @@ int dummy_haptic( SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint int DriverRegDummy(SurviveContext *ctx) { - if( !GCONFIGI(DUMMY_DRIVER_ENABLE) ) return 0; + int enable = survive_configi( ctx, "dummy-driver-enable", SC_GET, 9 ); + if( !enable ) return 0; SurviveDriverDummy *sp = calloc(1, sizeof(SurviveDriverDummy)); sp->ctx = ctx; diff --git a/src/survive_playback.c b/src/survive_playback.c index a5c4519..df922e2 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -21,8 +21,9 @@ ssize_t getdelim(char **lineptr, size_t *n, int delimiter, FILE *stream); ssize_t getline(char **lineptr, size_t *n, FILE *stream); #endif - -STATIC_CONFIG_ITEM_F( PLAYBACK_FACTOR, "playback-factor", "Time factor of playback -- 1 is run at the same timing as original, 0 is run as fast as possible.", 1.0f ); +STATIC_CONFIG_ITEM( RECORD, "record", 's', "File to record to if you wish to make a recording.", "" ); +STATIC_CONFIG_ITEM( PLAYBACK, "playback", 's', "File to be used for playback if playing a recording.", "" ); +STATIC_CONFIG_ITEM( PLAYBACK_FACTOR, "playback-factor", 'f', "Time factor of playback -- 1 is run at the same timing as original, 0 is run as fast as possible.", 1.0f ); typedef struct SurviveRecordingData { @@ -177,6 +178,7 @@ struct SurvivePlaybackData { int lineno; double next_time_us; + FLT playback_factor; bool hasRawLight; }; typedef struct SurvivePlaybackData SurvivePlaybackData; @@ -295,7 +297,7 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) { line = 0; } - if (driver->next_time_us * GCONFIGF( PLAYBACK_FACTOR ) > timestamp_in_us()) + if (driver->next_time_us * driver->playback_factor > timestamp_in_us()) return 0; driver->next_time_us = 0; @@ -394,7 +396,9 @@ int DriverRegPlayback(SurviveContext *ctx) { return -1; } - SV_INFO("Using playback file '%s' with timefactor of %f", playback_file, GCONFIGF( PLAYBACK_FACTOR ) ); + survive_attach_config( ctx, "playback-factor", &sp->playback_factor, 'f' ); + + SV_INFO("Using playback file '%s' with timefactor of %f", playback_file, sp->playback_factor ); SurviveObject *hmd = survive_create_hmd(ctx, "Playback", sp); SurviveObject *wm0 = survive_create_wm0(ctx, "Playback", sp, 0); SurviveObject *wm1 = survive_create_wm1(ctx, "Playback", sp, 0); diff --git a/src/survive_vive.c b/src/survive_vive.c index 493215a..37bec7e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -40,6 +40,7 @@ #endif #endif + struct SurviveViveData; const short vidpids[] = { -- cgit v1.2.3 From cb06bbdba6cefc042f32985000f1ef03aafdb9a9 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 23 Jun 2018 14:04:06 -0400 Subject: Fix config in master. --- src/poser_general_optimizer.c | 6 +++--- src/poser_sba.c | 14 +++++++------- src/poser_sba.h | 16 ---------------- src/survive.c | 4 ++-- src/survive_config.c | 41 +++++++++++++++++++++++++++++++++++++---- src/survive_playback.c | 2 +- 6 files changed, 50 insertions(+), 33 deletions(-) delete mode 100644 src/poser_sba.h (limited to 'src') diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c index 186b359..6f181a3 100644 --- a/src/poser_general_optimizer.c +++ b/src/poser_general_optimizer.c @@ -17,9 +17,9 @@ void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) { SurviveContext *ctx = so->ctx; - survive_attach_config( ctx, "failures-to-reset", &d->failures_to_reset, 'i' ); - survive_attach_config( ctx, "successes-to-reset", &d->successes_to_reset, 'i' ); - survive_attach_config( ctx, "max-error", &d->max_error, 'f' ); + survive_attach_configf( ctx, "max-error", &d->max_error ); + survive_attach_configi( ctx, "failures-to-reset", &d->failures_to_reset ); + survive_attach_configi( ctx, "successes-to-reset", &d->successes_to_reset ); const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, 0 ); d->seed_poser = (PoserCB)GetDriver(subposer); diff --git a/src/poser_sba.c b/src/poser_sba.c index 21dd74f..b049c80 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -50,7 +50,7 @@ typedef struct SBAData { survive_kpose_t kpose; SurviveIMUTracker tracker; - bool useIMU; + int useIMU; struct { int meas_failures; @@ -352,12 +352,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { general_optimizer_data_init(&d->opt, so); - survive_attach_config( ctx, "sba-use-imu", &d->useIMU, 'i' ); - survive_attach_config( ctx, "sba-required-meas", &d->required_meas, 'i' ); - survive_attach_config( ctx, "sba-time-window", &d->sensor_time_window, 'i' ); - survive_attach_config( ctx, "sba-sensor-variance-per-sec", &d->sensor_variance_per_second, 'f' ); - survive_attach_config( ctx, "sba-sensor-variance", &d->sensor_variance, 'f' ); - survive_attach_config( ctx, "sba-use-jacobian-function", &d->use_jacobian_function, 'i' ); + survive_attach_configi( ctx, "sba-use-imu", &d->useIMU ); + survive_attach_configi( ctx, "sba-required-meas", &d->required_meas ); + survive_attach_configi( ctx, "sba-time-window", &d->sensor_time_window ); + survive_attach_configf( ctx, "sba-sensor-variance-per-sec", &d->sensor_variance_per_second ); + survive_attach_configf( ctx, "sba-sensor-variance", &d->sensor_variance ); + survive_attach_configi( ctx, "sba-use-jacobian-function", &d->use_jacobian_function ); SV_INFO("Initializing SBA:"); SV_INFO("\tsba-required-meas: %d", d->required_meas); diff --git a/src/poser_sba.h b/src/poser_sba.h deleted file mode 100644 index 8a49122..0000000 --- a/src/poser_sba.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -#pragma once -#include -#define FLT double -#include "survive_reproject.h" -#include -#include - -struct SurviveObject; - -int sba_bruteforce_config_solver_cb(SurviveObject *so, PoserData *pd); -int sba_solver_poser_cb(SurviveObject *so, PoserData *pd); - -std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self); -std::ostream &operator<<(std::ostream &o, const survive_calibration_config &self); \ No newline at end of file diff --git a/src/survive.c b/src/survive.c index 2bdefca..7ee1992 100644 --- a/src/survive.c +++ b/src/survive.c @@ -156,8 +156,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->lh_config = malloc(sizeof(config_group) * NUM_LIGHTHOUSES); // initdata - init_config_group(ctx->global_config_values, 10, ctx); - init_config_group(ctx->temporary_config_values, 20, ctx); + init_config_group(ctx->global_config_values, 30, ctx); + init_config_group(ctx->temporary_config_values, 30, ctx); for( i = 0; i < NUM_LIGHTHOUSES; i++ ) init_config_group(&ctx->lh_config[i], 10, ctx); diff --git a/src/survive_config.c b/src/survive_config.c index 23433c4..5ee4a84 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -77,7 +77,7 @@ int survive_print_help_for_parameter( const char * tomap ) { char sthelp[160]; snprintf( sthelp, 159, " %s: %s [%c]", sc->name, sc->description, sc->type ); - fprintf( stderr, "\0337\033[1A\033[1000D%s\0338", sthelp, strlen( sthelp )); + fprintf( stderr, "\0337\033[1A\033[1000D%s\0338", sthelp ); return 1; } } @@ -776,13 +776,31 @@ const char *survive_configs(SurviveContext *ctx, const char *tag, char flags, co return def; } -SURVIVE_EXPORT void survive_attach_config(SurviveContext *ctx, const char *tag, void * var, char type ) +static void survive_attach_config(SurviveContext *ctx, const char *tag, void * var, char type ) { config_entry *cv = sc_search(ctx, tag); if( !cv ) { - SV_ERROR( "Configuration item %s not initialized.\n", tag ); - return; + //Check to see if there is a static config with this value. + if( type == 'i' ) + { + *((int*)var) = survive_configi( ctx, tag, SC_SET, 0); + } + if( type == 'f' ) + { + *((FLT*)var) = survive_configf( ctx, tag, SC_SET, 0); + } + if( type == 's' ) + { + const char * cv = survive_configs( ctx, tag, SC_SET, 0); + memcpy( var, cv, strlen(cv) ); + } + cv = sc_search(ctx, tag); + if( !cv ) + { + SV_ERROR( "Configuration item %s not initialized.\n", tag ); + return; + } } update_list_t ** ul = &cv->update_list; while( *ul ) @@ -795,6 +813,21 @@ SURVIVE_EXPORT void survive_attach_config(SurviveContext *ctx, const char *tag, t->value = var; } +SURVIVE_EXPORT void survive_attach_configi(SurviveContext *ctx, const char *tag, int * var ) +{ + survive_attach_config( ctx, tag, var, 'i' ); +} + +SURVIVE_EXPORT void survive_attach_configf(SurviveContext *ctx, const char *tag, FLT * var ) +{ + survive_attach_config( ctx, tag, var, 'f' ); +} + +SURVIVE_EXPORT void survive_attach_configs(SurviveContext *ctx, const char *tag, char * var ) +{ + survive_attach_config( ctx, tag, var, 's' ); +} + SURVIVE_EXPORT void survive_detach_config(SurviveContext *ctx, const char *tag, void * var ) { config_entry *cv = sc_search(ctx, tag); diff --git a/src/survive_playback.c b/src/survive_playback.c index ebf7e3f..c616aea 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -401,7 +401,7 @@ int DriverRegPlayback(SurviveContext *ctx) { return -1; } - survive_attach_config( ctx, "playback-factor", &sp->playback_factor, 'f' ); + survive_attach_configf( ctx, "playback-factor", &sp->playback_factor ); SV_INFO("Using playback file '%s' with timefactor of %f", playback_file, sp->playback_factor ); SurviveObject *hmd = survive_create_hmd(ctx, "Playback", sp); -- cgit v1.2.3 From e8b268f4c0116b135400ef6cf3dc939481de119b Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 23 Jun 2018 14:27:27 -0400 Subject: Fix units for vive, but not switching scales to incremental. --- src/survive_default_devices.c | 15 +++++++++++---- src/survive_vive.c | 2 +- 2 files changed, 12 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 05dd10d..ea38d55 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -6,6 +6,9 @@ #include "json_helpers.h" +#define HMD_IMU_HZ 1000.0f +#define VIVE_DEFAULT_IMU_HZ 250.0f + static SurviveObject * survive_create_device(SurviveContext *ctx, const char *driver_name, void *driver, const char *device_name, haptic_func fn) { @@ -17,7 +20,7 @@ survive_create_device(SurviveContext *ctx, const char *driver_name, memcpy(device->drivername, driver_name, strlen(driver_name)); device->timebase_hz = 48000000; - device->imu_freq = 250.f; + device->imu_freq = VIVE_DEFAULT_IMU_HZ; device->haptic = fn; return device; @@ -167,16 +170,20 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { if (strcmp(so->codename, "HMD") == 0) { if (so->acc_scale) { scale3d(so->acc_scale, so->acc_scale, 1. / 8192.0); + //Invert X and Y at least on my vive. + so->acc_scale[1] *= -1; + so->acc_scale[0] *= -1; } + so->imu_freq = HMD_IMU_HZ; if (so->acc_bias) scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Odd but seems right. if (so->gyro_scale) { FLT deg_per_sec = 500; scale3d(so->gyro_scale, so->gyro_scale, deg_per_sec / (1 << 15) * LINMATHPI / 180.); - // scale3d(so->gyro_scale, so->gyro_scale, 0.000065665); + //Invert X and Y at least on my vive. + so->gyro_scale[0] *= -1; + so->gyro_scale[1] *= -1; } - - so->imu_freq = 1000.f; } else if (memcmp(so->codename, "WM", 2) == 0) { if (so->acc_scale) scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0); diff --git a/src/survive_vive.c b/src/survive_vive.c index 37bec7e..b20052d 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -553,7 +553,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo //From actual steam. if (sv->udev[USB_DEV_HMD]) { - static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01 }; r = update_feature_report( sv->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } -- cgit v1.2.3 From 217adf4a3fd5621b6499195a882989b24d9ad615 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 23 Jun 2018 14:28:20 -0400 Subject: Fix my poser's use. --- src/poser_charlesrefine.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index b14d882..43558d6 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -55,9 +55,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //TODO: Actually do Madgwick's algorithm LinmathQuat applymotion; const SurvivePose * object_pose = &so->OutPose; - imuData->gyro[0] *= 1./1000.; - imuData->gyro[1] *= 1./1000.; - imuData->gyro[2] *= 1./1000.; + imuData->gyro[0] *= 1./so->imu_freq; + imuData->gyro[1] *= 1./so->imu_freq; + imuData->gyro[2] *= 1./so->imu_freq; quatfromeuler( applymotion, imuData->gyro ); //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); -- cgit v1.2.3 From b2151e0c8917258f5a9659296b6df5381645428e Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 23 Jun 2018 14:30:17 -0400 Subject: Remove extra reprojection info --- src/poser_charlesrefine.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 43558d6..7cd27e3 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -421,6 +421,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { int l = 0, a = 0; if( lhid == 0 && axis == 0 ) for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) dd->MixingConfidence[l][a] -= 0.1; +#if 0 for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) { LinmathPoint3d MixThis = { 0, 0, 0 }; @@ -432,8 +433,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //printf( "%f ", Confidence ); } scale3d( MixedPosition, MixedPosition, 1./MixedAmount ); - printf( "Reprojection disagreements:" ); +#endif for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) { printf( "%f ", dist3d( dd->MixingPositions[l][a], MixedPosition ) ); -- cgit v1.2.3 From e21c1f96ad67015d7051eeeaf5e1e9183626922a Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 23 Jun 2018 16:28:35 -0400 Subject: Significant improvements to the CharlesRefine trcker, to consider confidence of tracking solution. --- src/poser_charlesrefine.c | 181 +++++++++++++++++++++++++++++++--------------- 1 file changed, 121 insertions(+), 60 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 7cd27e3..1b392a3 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -20,7 +20,7 @@ typedef struct { int sweeplh; FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; // Value is actually normalized, not just normal to sweep plane. - FLT quantity_errors[MAX_PT_PER_SWEEP]; + FLT quantity_errors[MAX_PT_PER_SWEEP]; //Dot product of error offset. FLT angles_at_pts[MAX_PT_PER_SWEEP]; SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; @@ -32,6 +32,8 @@ typedef struct { int ptsweep; SurviveIMUTracker tracker; + + SurvivePose * lastlhp; } CharlesPoserData; int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { @@ -42,7 +44,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); - so->PoseConfidence = 1.0; + so->PoseConfidence = 0.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -103,7 +105,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { BaseStationData *bsd = &so->ctx->bsd[ld->lh]; if (!bsd->PositionSet) break; - SurvivePose *lhp = &bsd->Pose; + SurvivePose *lhp = dd->lastlhp = &bsd->Pose; FLT inangle = ld->angle; int sensor_id = ld->sensor_id; int axis = dd->sweepaxis; @@ -195,10 +197,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { case POSERDATA_SYNC: { PoserDataLight *l = (PoserDataLight *)pd; int lhid = l->lh; - // you can get sweepaxis and sweeplh. if (dd->ptsweep) { int i; + int applied_corrections = 0; + int normal_faults = 0; int lhid = dd->sweeplh; int axis = dd->sweepaxis; int pts = dd->ptsweep; @@ -224,6 +227,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define CORRECT_ROTATION_COEFFICIENT \ 0.2 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.01 +#define MINIMUM_CONFIDENCE_TO_CORRECT_POSITION 0.3 // Step 1: Determine standard of deviation, and average in order to // drop points that are likely in error. @@ -260,6 +264,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // Step 2: Determine average lateral error. // We can actually always perform this operation. Even with only one point. + if ( so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) { FLT avg_err[3] = {0, 0, 0}; // Positional error. for (i = 0; i < pts; i++) { @@ -284,15 +289,24 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // two points to make sure the error on this isn't unusually high? // If calculated error is unexpectedly high, then we should probably // Not apply the transform. - scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); - add3d(vec_correct, vec_correct, avg_err); + + if( ( magnitude3d( avg_err ) < 0.4 || so->PoseConfidence < 0.6 ) ) + { + scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); + add3d(vec_correct, vec_correct, avg_err); + applied_corrections++; + } + else + { + so->PoseConfidence *= 0.9; + } } // Step 3: Control telecoption from lighthouse. // we need to find out what the weighting is to determine "zoom" - if (validpoints > 1) // Can't correct "zoom" with only one point. + if (validpoints > 1 && so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) // Can't correct "zoom" with only one point. { FLT zoom = 0.0; FLT rmsang = 0.0; @@ -315,11 +329,20 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { zoom *= CORRECT_TELESCOPTION_COEFFICIENT; - FLT veccamalong[3]; - sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); - normalize3d(veccamalong, veccamalong); - scale3d(veccamalong, veccamalong, zoom); - add3d(vec_correct, veccamalong, vec_correct); + //Don't apply completely wild zoom's unless our confidence is awful. + if( ( zoom < 0.4 || so->PoseConfidence < 0.6 ) && ( so->PoseConfidence > 0.3 ) ) + { + FLT veccamalong[3]; + sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); + normalize3d(veccamalong, veccamalong); + scale3d(veccamalong, veccamalong, zoom); + add3d(vec_correct, veccamalong, vec_correct); + applied_corrections++; + } + else + { + so->PoseConfidence *= 0.9; + } } @@ -343,9 +366,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #endif add3d(dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos); - - - //quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot); + //XXX TODO + // ?: Fuse accelerometer. // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. // If we were able to determine likliehood of a hit in the sweep instead of afterward @@ -356,53 +378,84 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { for (i = 0; i < pts; i++) { if (!ptvalid[i]) continue; - FLT dist = dd->quantity_errors[i] - avgerr; + + FLT dist = dd->quantity_errors[i] - avgerr; //Relative dot-product error. FLT angle = dd->angles_at_pts[i]; int sensor_id = dd->sensor_ids[i]; - FLT *normal = dd->normal_at_errors[i]; - const SurvivePose *object_pose_at_hit = &dd->object_pose_at_hit[i]; - const FLT *sensor_inpos = &so->sensor_locations[sensor_id * 3]; + FLT * normal = dd->normal_at_errors[i]; + FLT * sensornormal = &so->sensor_normals[sensor_id*3]; + SurvivePose * lhp = dd->lastlhp; + const SurvivePose * object_pose_at_hit = &dd->object_pose_at_hit[i]; + const FLT * sensor_inpos = &so->sensor_locations[sensor_id * 3]; LinmathQuat world_to_object_space; quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); - FLT correction_in_object_space[3]; // The amount across the surface of the object the rotation - // should happen. - - quatrotatevector(correction_in_object_space, world_to_object_space, normal); - dist *= CORRECT_ROTATION_COEFFICIENT; - if (dist > ROTATIONAL_CORRECTION_MAXFORCE) - dist = ROTATIONAL_CORRECTION_MAXFORCE; - if (dist < -ROTATIONAL_CORRECTION_MAXFORCE) - dist = -ROTATIONAL_CORRECTION_MAXFORCE; - - // Now, we have a "tug" vector in object-local space. Need to apply the torque. - FLT vector_from_center_of_object[3]; - normalize3d(vector_from_center_of_object, sensor_inpos); - // scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); - // vector_from_center_of_object[2]*=-1; - // vector_from_center_of_object[1]*=-1; - // vector_from_center_of_object[0]*=-1; - // vector_from_center_of_object - scale3d(vector_from_center_of_object, vector_from_center_of_object, 1); - - FLT new_vector_in_object_space[3]; - // printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], - // object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); - // printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], - // vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], - // correction_in_object_space[1], correction_in_object_space[2], dist ); - scale3d(correction_in_object_space, correction_in_object_space, -dist); - add3d(new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space); - - normalize3d(new_vector_in_object_space, new_vector_in_object_space); - - LinmathQuat corrective_quaternion; - quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space); - quatrotateabout(correction, correction, corrective_quaternion); - // printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], - // vector_from_center_of_object[1], vector_from_center_of_object[2], - // correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], - // corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); + + //First, check to see if this hit is a sensor that is facing the lighthouse. + { + LinmathPoint3d vector_to_lighthouse; + sub3d( vector_to_lighthouse, lhp->Pos, object_pose_at_hit->Pos ); //Get vector in world space. + normalize3d( vector_to_lighthouse, vector_to_lighthouse ); + quatrotatevector( vector_to_lighthouse, world_to_object_space, vector_to_lighthouse ); + float facingness = dot3d( sensornormal, vector_to_lighthouse ); + if( facingness < 0 ) + { + //This is an impossible sensor hit. + so->PoseConfidence *= 0.8; + + //If our pose confidence is low, apply a torque. + if( so->PoseConfidence < 0.6 ) + { + LinmathPoint3d rotateaxis; + cross3d( rotateaxis, vector_to_lighthouse, sensornormal ); + LinmathQuat correction; + quatfromaxisangle(correction, rotateaxis, facingness*.2 ); + quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction); + quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); + normal_faults ++; + } + } + } + + //Apply the normal tug. + { + + FLT correction_in_object_space[3]; // The amount across the surface of the object the rotation + // should happen. + + quatrotatevector(correction_in_object_space, world_to_object_space, normal); + dist *= CORRECT_ROTATION_COEFFICIENT; + if (dist > ROTATIONAL_CORRECTION_MAXFORCE) + dist = ROTATIONAL_CORRECTION_MAXFORCE; + if (dist < -ROTATIONAL_CORRECTION_MAXFORCE) + dist = -ROTATIONAL_CORRECTION_MAXFORCE; + + // Now, we have a "tug" vector in object-local space. Need to apply the torque. + FLT vector_from_center_of_object[3]; + normalize3d(vector_from_center_of_object, sensor_inpos); + // scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); + // vector_from_center_of_object[2]*=-1; + // vector_from_center_of_object[1]*=-1; + // vector_from_center_of_object[0]*=-1; + // vector_from_center_of_object + scale3d(vector_from_center_of_object, vector_from_center_of_object, 1); + + FLT new_vector_in_object_space[3]; + // printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], + // object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); + // printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], + // vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], + // correction_in_object_space[1], correction_in_object_space[2], dist ); + scale3d(correction_in_object_space, correction_in_object_space, -dist); + add3d(new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space); + + normalize3d(new_vector_in_object_space, new_vector_in_object_space); + + LinmathQuat corrective_quaternion; + quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space); + quatrotateabout(correction, correction, corrective_quaternion); + } + } // printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); // Apply our corrective quaternion to the output. @@ -420,8 +473,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathPoint3d MixedPosition = { 0, 0, 0 }; int l = 0, a = 0; if( lhid == 0 && axis == 0 ) for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) dd->MixingConfidence[l][a] -= 0.1; - -#if 0 for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) { LinmathPoint3d MixThis = { 0, 0, 0 }; @@ -433,13 +484,14 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //printf( "%f ", Confidence ); } scale3d( MixedPosition, MixedPosition, 1./MixedAmount ); +#if 0 printf( "Reprojection disagreements:" ); -#endif for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) { printf( "%f ", dist3d( dd->MixingPositions[l][a], MixedPosition ) ); } printf( "\n" ); +#endif //printf( "%f\n", MixedAmount ); SurvivePose object_pose_out; @@ -457,6 +509,15 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // PoserData_poser_pose_func(pd, so, &dd->tracker.pose); dd->ptsweep = 0; + + if( validpoints > 1 && applied_corrections > 1 && !normal_faults) + { + so->PoseConfidence += (1-so->PoseConfidence)*.05; + } + else if( validpoints > 1 && so->PoseConfidence < 0.5 && !normal_faults ) + { + so->PoseConfidence += (1-so->PoseConfidence)*.05; + } } dd->sweepaxis = l->acode & 1; -- cgit v1.2.3 From 663198c1d4adeba3a5df5d008d8416b2f26f3a39 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Jun 2018 00:29:17 -0400 Subject: Significant improvements to the charles refine. --- src/poser_charlesrefine.c | 227 +++++++++++++++++++++++++++++++--------------- 1 file changed, 152 insertions(+), 75 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 1b392a3..09e6bbf 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -1,5 +1,3 @@ -// Driver works, but you _must_ start it near the origin looking in +Z. - #include #include #include @@ -15,6 +13,30 @@ #define MAX_PT_PER_SWEEP SENSORS_PER_OBJECT + + +// Tunable parameters: +#define MIN_HIT_QUALITY 0.5 // Determines which hits to cull. +#define HIT_QUALITY_BASELINE \ + 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm + +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response) +#define CORRECT_TELESCOPTION_COEFFICIENT 7.00 // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) +#define CORRECT_ROTATION_COEFFICIENT \ + 0.2 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define ROTATIONAL_CORRECTION_MAXFORCE 0.01 +#define MINIMUM_CONFIDENCE_TO_CORRECT_POSITION 0.2 + + +#define POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU 0.4 //0.9 + + +#define MAX_JUMP_DISTANCE 1000 //0.4 + + +//Grand todo: +// Update global "Up" vector from LH's PoV based on "Up" from the HMD. + typedef struct { int sweepaxis; int sweeplh; @@ -25,8 +47,18 @@ typedef struct { SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; + LinmathPoint3d imuvel; + LinmathPoint3d imu_accel_zero; + LinmathQuat imu_up_correct; + LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2]; - SurvivePose InteralPoseUsedForCalc; //Super high speed vibratey and terrible. + LinmathPoint3d mixed_output; + + //Super high speed vibratey and terrible. + //Also, big deal: this does NOT have the "re-righting" vector + //from the accelerometer applied to it. + SurvivePose InteralPoseUsedForCalc; + FLT MixingConfidence[NUM_LIGHTHOUSES][2]; FLT last_angle_lh_axis[NUM_LIGHTHOUSES][2]; int ptsweep; @@ -44,6 +76,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); + memcpy(&dd->imu_up_correct, &LinmathQuat_Identity, sizeof(LinmathQuat_Identity) ); so->PoseConfidence = 0.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -51,49 +84,104 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurviveSensorActivations *scene = &so->activations; switch (pd->pt) { case POSERDATA_IMU: { + float imu_time = 1./ so->imu_freq; // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; + SurvivePose object_pose_out; + LinmathQuat object_to_world_with_imu_up_correct; + LinmathQuat world_space_to_object_space_quat; - //TODO: Actually do Madgwick's algorithm - LinmathQuat applymotion; - const SurvivePose * object_pose = &so->OutPose; - imuData->gyro[0] *= 1./so->imu_freq; - imuData->gyro[1] *= 1./so->imu_freq; - imuData->gyro[2] *= 1./so->imu_freq; - quatfromeuler( applymotion, imuData->gyro ); - //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); - - LinmathQuat InvertQuat; - quatgetreciprocal(InvertQuat, object_pose->Rot); - - //Apply a tiny tug to re-right headset based on the gravity vector. - LinmathVec3d reright = { 0, 0, 1 }; - LinmathVec3d normup; - normalize3d( normup, imuData->accel ); - LinmathVec3d correct_diff; - quatrotatevector(reright, InvertQuat, reright); - sub3d( correct_diff, normup, reright ); - scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. - add3d( correct_diff, correct_diff, reright ); - LinmathQuat reright_quat; - normalize3d( correct_diff, correct_diff ); - quatfrom2vectors( reright_quat, reright, correct_diff ); + quatrotateabout( object_to_world_with_imu_up_correct, dd->InteralPoseUsedForCalc.Rot, dd->imu_up_correct ); + quatgetreciprocal(world_space_to_object_space_quat, object_to_world_with_imu_up_correct ); + { + LinmathQuat applymotion; + imuData->gyro[0] *= imu_time; + imuData->gyro[1] *= imu_time; + imuData->gyro[2] *= imu_time; + quatfromeuler( applymotion, imuData->gyro ); + + quatrotateabout(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot, applymotion ); //Contribution from Gyro + quatnormalize(object_pose_out.Rot, object_pose_out.Rot); + quatcopy( dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); + } - SurvivePose object_pose_out; - quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); //Contribution from Gyro - quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, reright_quat); //Contribution from Accelerometer - quatnormalize(object_pose_out.Rot, object_pose_out.Rot); + //This will be overwritten by the accelerometer updates. + //We do this here in case we want to use it later. + copy3d( object_pose_out.Pos, dd->InteralPoseUsedForCalc.Pos ); + // Do accelerometer based stuff. + if( so->PoseConfidence > POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU ) { - copy3d( object_pose_out.Pos, object_pose->Pos ); - PoserData_poser_pose_func(pd, so, &object_pose_out); - quatcopy( dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); + //Step 1: Use the accelerometer's "up" to figure out how we can re-right world space to be the correct "up" vector. + //Apply a tiny tug to the imu_up_correct with the up vector.. + { + LinmathVec3d reright = { 0, 0, 1 }; + LinmathVec3d normup; + normalize3d( normup, imuData->accel ); + LinmathVec3d correct_diff; + quatrotatevector(reright, world_space_to_object_space_quat, reright); + sub3d( correct_diff, normup, reright ); + scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. XXX THIS MUST CHANGE. + add3d( correct_diff, correct_diff, reright ); + normalize3d( correct_diff, correct_diff ); + LinmathQuat reright_quat; + quatfrom2vectors( reright_quat, reright, correct_diff ); + //Push to correct "Up" a little bit. + quatrotateabout(dd->imu_up_correct, dd->imu_up_correct, reright_quat); + quatnormalize(dd->imu_up_correct, dd->imu_up_correct); + } + + //Update position as a function from the IMU... + if( 0 ) { + LinmathVec3d expected_up = { 0, 0, 1 }; + LinmathVec3d acceleration; + scale3d( acceleration, imuData->accel, 1.0 ); + quatrotatevector( acceleration, dd->imu_up_correct, acceleration ); + + printf( "ACCEL %f %f %f\n", acceleration[0], acceleration[1], acceleration[2] ); + sub3d( acceleration, acceleration, dd->imu_accel_zero ); //IMU Accel Zero is in object-local space. + + quatrotatevector( acceleration, dd->InteralPoseUsedForCalc.Rot, acceleration ); + + printf( "ACCEL %f %f %f %f %f %f %f %f %f\n", acceleration[0],acceleration[1],acceleration[2], expected_up[0], expected_up[1], expected_up[2], + dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2]); + + sub3d( acceleration, acceleration, expected_up ); + + scale3d( acceleration, acceleration, 9.8 * imu_time ); + + //Acceleration is now in global space. - //PoserDataIMU *imu = (PoserDataIMU *)pd; - //survive_imu_tracker_integrate(so, &dd->tracker, imu); - //PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + add3d( dd->imuvel, dd->imuvel, acceleration ); + + //IMUVel is the estimated object motion, in world space, but it will be pulled in the direction of the + //Faulty IMU bias. + + LinmathVec3d updatepos; + scale3d( updatepos, dd->imuvel, 1.0 ); + scale3d( updatepos, updatepos, imu_time ); + + + //printf( "ACCEL: %10f %10f %10f %10f %10f %10f %10f %10f %10f\n", + // acceleration[0], acceleration[1], acceleration[2], + // dd->imuvel[0],dd->imuvel[1],dd->imuvel[2], + // dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2] ); + + //Tricky, tug the imuvel toward zero otherwise it will contine to drift. + scale3d( dd->imuvel, dd->imuvel, 0.9999 ); + + //Update actual location. + add3d( dd->InteralPoseUsedForCalc.Pos, dd->InteralPoseUsedForCalc.Pos, updatepos ); + add3d( dd->mixed_output, dd->mixed_output, updatepos ); + + } + } + + copy3d( object_pose_out.Pos, dd->mixed_output ); + quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); + PoserData_poser_pose_func(pd, so, &object_pose_out); return 0; } @@ -217,18 +305,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { FLT vec_correct[3] = {0., 0., 0.}; FLT avgang = 0.0; -// Tunable parameters: -#define MIN_HIT_QUALITY 0.5 // Determines which hits to cull. -#define HIT_QUALITY_BASELINE \ - 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm - -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 7.00 // Converges even as high as 10.0 and doesn't explode. -#define CORRECT_ROTATION_COEFFICIENT \ - 0.2 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. -#define ROTATIONAL_CORRECTION_MAXFORCE 0.01 -#define MINIMUM_CONFIDENCE_TO_CORRECT_POSITION 0.3 - // Step 1: Determine standard of deviation, and average in order to // drop points that are likely in error. { @@ -262,6 +338,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { avgerr /= validpoints; } + // Step 2: Determine average lateral error. // We can actually always perform this operation. Even with only one point. if ( so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) @@ -290,7 +367,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // If calculated error is unexpectedly high, then we should probably // Not apply the transform. - if( ( magnitude3d( avg_err ) < 0.4 || so->PoseConfidence < 0.6 ) ) + if( ( magnitude3d( avg_err ) < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) { scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); add3d(vec_correct, vec_correct, avg_err); @@ -330,7 +407,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { zoom *= CORRECT_TELESCOPTION_COEFFICIENT; //Don't apply completely wild zoom's unless our confidence is awful. - if( ( zoom < 0.4 || so->PoseConfidence < 0.6 ) && ( so->PoseConfidence > 0.3 ) ) + if( ( zoom < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) { FLT veccamalong[3]; sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); @@ -346,28 +423,24 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } -#if 0 - LinmathPoint3d LastDelta; - sub3d( LastDelta, dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos ); - //Compare with "vec_correct" - - LinmathPoint3d DeltaDelta; - sub3d( DeltaDelta, vec_correct, LastDelta ); - - - //SurvivePose object_pose_out; - - memcpy( dd->MixingPositions[lhid][axis], vec_correct, sizeof( vec_correct ) ); + //Tricky: Update position here, and back-correct imuvel based on correction. + if( 0 ) { //XXX XXX TODO Position update + LinmathPoint3d vecc; + scale3d( vecc, vec_correct, 0.01 ); + add3d( dd->imuvel, dd->imuvel, vecc ); - LinmathPoint3d system_average_adjust = { 0, 0, 0 }; - - printf( "%f %f %f + %f %f %f\n", vec_correct[0], vec_correct[1], vec_correct[2], dd->InteralPoseUsedForCalc.Pos[0], dd->InteralPoseUsedForCalc.Pos[1], dd->InteralPoseUsedForCalc.Pos[2] ); - -#endif - add3d(dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos); + if( so->PoseConfidence > POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU ) + { + scale3d( vecc, vec_correct, .01 ); + LinmathQuat world_to_object_space; + quatgetreciprocal(world_to_object_space, dd->InteralPoseUsedForCalc.Rot); + quatrotatevector( vecc, world_to_object_space, vecc ); + add3d( dd->imu_accel_zero, dd->imu_accel_zero, vecc ); + printf( "ACCELV: %f %f %f %f %f %f\n", vecc[0], vecc[1], vecc[2], dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2] ); + } + } - //XXX TODO - // ?: Fuse accelerometer. + add3d( dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos); // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. // If we were able to determine likliehood of a hit in the sweep instead of afterward @@ -398,13 +471,13 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { normalize3d( vector_to_lighthouse, vector_to_lighthouse ); quatrotatevector( vector_to_lighthouse, world_to_object_space, vector_to_lighthouse ); float facingness = dot3d( sensornormal, vector_to_lighthouse ); - if( facingness < 0 ) + if( facingness < -.1 ) { //This is an impossible sensor hit. so->PoseConfidence *= 0.8; //If our pose confidence is low, apply a torque. - if( so->PoseConfidence < 0.6 ) + if( so->PoseConfidence < 0.8 ) { LinmathPoint3d rotateaxis; cross3d( rotateaxis, vector_to_lighthouse, sensornormal ); @@ -497,6 +570,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); copy3d( object_pose_out.Pos, MixedPosition ); + + copy3d( dd->mixed_output, object_pose_out.Pos ); + quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); PoserData_poser_pose_func(pd, so, &object_pose_out); } // FLT var_meters = 0.5; @@ -512,11 +588,12 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { if( validpoints > 1 && applied_corrections > 1 && !normal_faults) { - so->PoseConfidence += (1-so->PoseConfidence)*.05; + so->PoseConfidence += (1-so->PoseConfidence)*.04; } else if( validpoints > 1 && so->PoseConfidence < 0.5 && !normal_faults ) { - so->PoseConfidence += (1-so->PoseConfidence)*.05; + printf( "Push\n" ); + so->PoseConfidence += (1-so->PoseConfidence)*.01; } } -- cgit v1.2.3 From fe153d9def29387f4ad9712bd364bf8d9fdc5776 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Jun 2018 06:32:29 +0000 Subject: some more tweaks to the charlesrefine poser --- src/poser_charlesrefine.c | 58 ++++++++++++++++++++++------------------------- 1 file changed, 27 insertions(+), 31 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 09e6bbf..dbc7087 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -21,19 +21,15 @@ 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm #define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response) -#define CORRECT_TELESCOPTION_COEFFICIENT 7.00 // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) +#define CORRECT_TELESCOPTION_COEFFICIENT 7.0 // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) #define CORRECT_ROTATION_COEFFICIENT \ 0.2 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.01 -#define MINIMUM_CONFIDENCE_TO_CORRECT_POSITION 0.2 +#define MINIMUM_CONFIDENCE_TO_CORRECT_POSITION 0.02 // 0.2 +#define POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU .9 //0.9 +#define MAX_JUMP_DISTANCE 0.5 //0.4 -#define POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU 0.4 //0.9 - - -#define MAX_JUMP_DISTANCE 1000 //0.4 - - //Grand todo: // Update global "Up" vector from LH's PoV based on "Up" from the HMD. @@ -123,64 +119,64 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathVec3d correct_diff; quatrotatevector(reright, world_space_to_object_space_quat, reright); sub3d( correct_diff, normup, reright ); - scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. XXX THIS MUST CHANGE. + scale3d( correct_diff, correct_diff, -0.01 ); //This is the coefficient applying the drag. XXX THIS MUST CHANGE. add3d( correct_diff, correct_diff, reright ); normalize3d( correct_diff, correct_diff ); LinmathQuat reright_quat; quatfrom2vectors( reright_quat, reright, correct_diff ); //Push to correct "Up" a little bit. - quatrotateabout(dd->imu_up_correct, dd->imu_up_correct, reright_quat); - quatnormalize(dd->imu_up_correct, dd->imu_up_correct); + + + quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, reright_quat); + + //quatrotateabout(dd->imu_up_correct, dd->imu_up_correct, reright_quat); + //quatnormalize(dd->imu_up_correct, dd->imu_up_correct); } //Update position as a function from the IMU... - if( 0 ) { + if(0 ) { LinmathVec3d expected_up = { 0, 0, 1 }; LinmathVec3d acceleration; scale3d( acceleration, imuData->accel, 1.0 ); - quatrotatevector( acceleration, dd->imu_up_correct, acceleration ); - - printf( "ACCEL %f %f %f\n", acceleration[0], acceleration[1], acceleration[2] ); + //quatrotatevector( acceleration, dd->imu_up_correct, acceleration ); sub3d( acceleration, acceleration, dd->imu_accel_zero ); //IMU Accel Zero is in object-local space. - quatrotatevector( acceleration, dd->InteralPoseUsedForCalc.Rot, acceleration ); - - printf( "ACCEL %f %f %f %f %f %f %f %f %f\n", acceleration[0],acceleration[1],acceleration[2], expected_up[0], expected_up[1], expected_up[2], - dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2]); - sub3d( acceleration, acceleration, expected_up ); - scale3d( acceleration, acceleration, 9.8 * imu_time ); + LinmathVec3d recalv; + scale3d( recalv, acceleration, 0.01 ); + LinmathQuat invrr; + quatgetreciprocal( invrr, dd->InteralPoseUsedForCalc.Rot ); + quatrotatevector( recalv, invrr, recalv ); + add3d( dd->imu_accel_zero, dd->imu_accel_zero, recalv ); - //Acceleration is now in global space. + printf( "%s %f %f %f %f %f %f\n", so->codename, dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2], acceleration[0], acceleration[1], acceleration[2] ); + scale3d( acceleration, acceleration, 9.8 * imu_time ); add3d( dd->imuvel, dd->imuvel, acceleration ); //IMUVel is the estimated object motion, in world space, but it will be pulled in the direction of the //Faulty IMU bias. LinmathVec3d updatepos; + scale3d( updatepos, dd->imuvel, 1.0 ); scale3d( updatepos, updatepos, imu_time ); - - //printf( "ACCEL: %10f %10f %10f %10f %10f %10f %10f %10f %10f\n", - // acceleration[0], acceleration[1], acceleration[2], - // dd->imuvel[0],dd->imuvel[1],dd->imuvel[2], - // dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2] ); - //Tricky, tug the imuvel toward zero otherwise it will contine to drift. scale3d( dd->imuvel, dd->imuvel, 0.9999 ); //Update actual location. add3d( dd->InteralPoseUsedForCalc.Pos, dd->InteralPoseUsedForCalc.Pos, updatepos ); add3d( dd->mixed_output, dd->mixed_output, updatepos ); - } } + quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); + quatnormalize(dd->imu_up_correct, dd->imu_up_correct); copy3d( object_pose_out.Pos, dd->mixed_output ); quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); + quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); PoserData_poser_pose_func(pd, so, &object_pose_out); return 0; @@ -465,7 +461,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); //First, check to see if this hit is a sensor that is facing the lighthouse. - { + if( so->PoseConfidence < 0.9 ) { LinmathPoint3d vector_to_lighthouse; sub3d( vector_to_lighthouse, lhp->Pos, object_pose_at_hit->Pos ); //Get vector in world space. normalize3d( vector_to_lighthouse, vector_to_lighthouse ); @@ -573,6 +569,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { copy3d( dd->mixed_output, object_pose_out.Pos ); quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); + quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); PoserData_poser_pose_func(pd, so, &object_pose_out); } // FLT var_meters = 0.5; @@ -592,7 +589,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } else if( validpoints > 1 && so->PoseConfidence < 0.5 && !normal_faults ) { - printf( "Push\n" ); so->PoseConfidence += (1-so->PoseConfidence)*.01; } } -- cgit v1.2.3 From 7d5b1db0edf0ec43f13293de1526206721c3494f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Jun 2018 13:41:51 -0400 Subject: Fix base calibration values. --- src/survive_default_devices.c | 10 ++++++---- src/survive_vive.c | 14 ++++++++------ 2 files changed, 14 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index ea38d55..2d63297 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -174,9 +174,11 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { so->acc_scale[1] *= -1; so->acc_scale[0] *= -1; } - so->imu_freq = HMD_IMU_HZ; if (so->acc_bias) - scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Odd but seems right. + scale3d(so->acc_bias, so->acc_bias, 1000.0 ); // Odd but seems right. + + so->imu_freq = HMD_IMU_HZ; + if (so->gyro_scale) { FLT deg_per_sec = 500; scale3d(so->gyro_scale, so->gyro_scale, deg_per_sec / (1 << 15) * LINMATHPI / 180.); @@ -188,7 +190,7 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { if (so->acc_scale) scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0); if (so->acc_bias) - scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Need to verify. + scale3d(so->acc_bias, so->acc_bias, 1000.); // Need to verify. FLT deg_per_sec = 2000; if (so->gyro_scale) @@ -209,7 +211,7 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { // If any other device, we know we at least need this. // I deeply suspect bias is in milligravities -JB if (so->acc_bias) - scale3d(so->acc_bias, so->acc_bias, 1. / 1000.); + scale3d(so->acc_bias, so->acc_bias, 1000.); // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits FLT deg_per_sec = 2000; diff --git a/src/survive_vive.c b/src/survive_vive.c index b20052d..a83c6a1 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -491,6 +491,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] ); return -9; } + usleep(20000); } SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces ); @@ -894,12 +895,6 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa } SV_INFO( "Got config data length %d", count ); - - char fstname[128]; - sprintf( fstname, "calinfo/%d.json.gz", devno ); - FILE * f = fopen( fstname, "wb" ); - fwrite( compressed_data, count, 1, f ); - fclose( f ); int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 ); if( len <= 0 ) @@ -910,6 +905,13 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa *config = malloc( len + 1 ); memcpy( *config, uncompressed_data, len ); + + char fstname[128]; + sprintf( fstname, "calinfo/%d.json", devno ); + FILE * f = fopen( fstname, "wb" ); + fwrite( uncompressed_data, len, 1, f ); + fclose( f ); + return len; } -- cgit v1.2.3 From 8ae8f5aa50ef7472a486ec6f3688e7dfb0c9dcab Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Jun 2018 20:23:34 -0400 Subject: Refactor --- src/poser_charlesrefine.c | 342 +++++++++++++++++++++++++++------------------- src/survive_vive.c | 3 +- 2 files changed, 206 insertions(+), 139 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index dbc7087..3e09925 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -21,7 +21,7 @@ 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm #define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response) -#define CORRECT_TELESCOPTION_COEFFICIENT 7.0 // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) +#define CORRECT_TELESCOPTION_COEFFICIENT 7.0f // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) #define CORRECT_ROTATION_COEFFICIENT \ 0.2 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.01 @@ -43,9 +43,10 @@ typedef struct { SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; - LinmathPoint3d imuvel; - LinmathPoint3d imu_accel_zero; - LinmathQuat imu_up_correct; +// LinmathPoint3d imuvel; +// LinmathPoint3d imu_accel_zero; +// int did_zero_imu; +// LinmathPoint3d imu_up_correct; LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2]; LinmathPoint3d mixed_output; @@ -64,6 +65,43 @@ typedef struct { SurvivePose * lastlhp; } CharlesPoserData; + +void AdjustRotation( SurviveObject *so, LinmathQuat adjustment, int is_imu, int is_coarse ) +{ + CharlesPoserData *dd = so->PoserData; + + quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, adjustment ); + quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); + + //XXX TODO: Calibrate the gyroscope using the lightcap. + + //Always update the accelerometer zero based on the gyro. + //quatrotatevector( dd->imu_accel_zero, applygyro, dd->imu_accel_zero ); + //Always update the system-corrective quat based on the gyro. + //quatrotateabout( dd->imu_up_correct, dd->imu_up_correct, applygyro ); +} + +void AdjustPosition( SurviveObject * so, LinmathPoint3d adjustment, int is_imu ) +{ + CharlesPoserData *dd = so->PoserData; + + add3d( dd->InteralPoseUsedForCalc.Pos, adjustment, dd->InteralPoseUsedForCalc.Pos); + add3d( dd->mixed_output, adjustment, dd->mixed_output); +} + +//Emits "dd->mixed_output" for position and dd->InteralPoseUsedForCalc.Rot for rotation. +void EmitPose( SurviveObject *so, PoserData *pd ) +{ + CharlesPoserData *dd = so->PoserData; + + SurvivePose object_pose_out; + copy3d( object_pose_out.Pos, dd->mixed_output ); + quatcopy( object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); + PoserData_poser_pose_func(pd, so, &object_pose_out); +} + + + int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { CharlesPoserData *dd = so->PoserData; if (!dd) @@ -72,7 +110,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); - memcpy(&dd->imu_up_correct, &LinmathQuat_Identity, sizeof(LinmathQuat_Identity) ); + //memcpy(&dd->imu_up_correct, &LinmathQuat_Identity, sizeof(LinmathQuat_Identity) ); so->PoseConfidence = 0.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -87,26 +125,26 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathQuat object_to_world_with_imu_up_correct; LinmathQuat world_space_to_object_space_quat; - quatrotateabout( object_to_world_with_imu_up_correct, dd->InteralPoseUsedForCalc.Rot, dd->imu_up_correct ); - quatgetreciprocal(world_space_to_object_space_quat, object_to_world_with_imu_up_correct ); - + //quatrotateabout( object_to_world_with_imu_up_correct, dd->InteralPoseUsedForCalc.Rot, dd->imu_up_correct ); + //quatgetreciprocal(world_space_to_object_space_quat, object_to_world_with_imu_up_correct ); { - LinmathQuat applymotion; + LinmathQuat applygyro; imuData->gyro[0] *= imu_time; imuData->gyro[1] *= imu_time; imuData->gyro[2] *= imu_time; - quatfromeuler( applymotion, imuData->gyro ); - - quatrotateabout(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot, applymotion ); //Contribution from Gyro - quatnormalize(object_pose_out.Rot, object_pose_out.Rot); - quatcopy( dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); + quatfromeuler( applygyro, imuData->gyro ); + AdjustRotation( so, applygyro, 1, 0 ); } + EmitPose( so, pd ); + +#if 0 //This will be overwritten by the accelerometer updates. //We do this here in case we want to use it later. copy3d( object_pose_out.Pos, dd->InteralPoseUsedForCalc.Pos ); + // Do accelerometer based stuff. if( so->PoseConfidence > POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU ) { @@ -119,22 +157,43 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathVec3d correct_diff; quatrotatevector(reright, world_space_to_object_space_quat, reright); sub3d( correct_diff, normup, reright ); - scale3d( correct_diff, correct_diff, -0.01 ); //This is the coefficient applying the drag. XXX THIS MUST CHANGE. + scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. XXX THIS MUST CHANGE. add3d( correct_diff, correct_diff, reright ); normalize3d( correct_diff, correct_diff ); LinmathQuat reright_quat; quatfrom2vectors( reright_quat, reright, correct_diff ); //Push to correct "Up" a little bit. - - quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, reright_quat); + //Do this if we want to use the IMU's up to help out the libsurvive estimate of "up" + // quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, reright_quat); - //quatrotateabout(dd->imu_up_correct, dd->imu_up_correct, reright_quat); - //quatnormalize(dd->imu_up_correct, dd->imu_up_correct); + // quatrotateabout(dd->imu_up_correct, dd->imu_up_correct, reright_quat); + // quatnormalize(dd->imu_up_correct, dd->imu_up_correct); } + //Update position as a function from the IMU... - if(0 ) { + if( 1 ) { + LinmathVec3d acceleration; + scale3d( acceleration, imuData->accel, 1.0 ); + + if( !dd->did_zero_imu ) + { + copy3d( dd->imu_accel_zero, acceleration ); + dd->did_zero_imu = 1; + } + else + { + LinmathVec3d recalingval; + scale3d( recalingval, acceleration, 0.0001 ); + scale3d( dd->imu_accel_zero, dd->imu_accel_zero, 0.9999 ); + add3d( dd->imu_accel_zero, dd->imu_accel_zero, recalingval ); + } + + sub3d( acceleration, acceleration, dd->imu_accel_zero ); + scale3d( acceleration, acceleration, 9.8 * imu_time ); + printf( "ACCEL %f %f %f\n", PFTHREE( acceleration ) ); +#if 0 LinmathVec3d expected_up = { 0, 0, 1 }; LinmathVec3d acceleration; scale3d( acceleration, imuData->accel, 1.0 ); @@ -169,6 +228,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //Update actual location. add3d( dd->InteralPoseUsedForCalc.Pos, dd->InteralPoseUsedForCalc.Pos, updatepos ); add3d( dd->mixed_output, dd->mixed_output, updatepos ); +#endif } } @@ -178,7 +238,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); PoserData_poser_pose_func(pd, so, &object_pose_out); - +#endif return 0; } case POSERDATA_LIGHT: { @@ -298,127 +358,131 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { int validpoints = 0; int ptvalid[MAX_PT_PER_SWEEP]; FLT avgerr = 0.0; - FLT vec_correct[3] = {0., 0., 0.}; FLT avgang = 0.0; - // Step 1: Determine standard of deviation, and average in order to - // drop points that are likely in error. { - // Calculate average - FLT avgerr_orig = 0.0; - FLT stddevsq = 0.0; - for (i = 0; i < pts; i++) - avgerr_orig += dd->quantity_errors[i]; - avgerr_orig /= pts; - - // Calculate standard of deviation. - for (i = 0; i < pts; i++) { - FLT diff = dd->quantity_errors[i] - avgerr_orig; - stddevsq += diff * diff; - } - stddevsq /= pts; - - for (i = 0; i < pts; i++) { - FLT err = dd->quantity_errors[i]; - FLT diff = err - avgerr_orig; - diff *= diff; - int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0; - ptvalid[i] = isptvalid; - if (isptvalid) { - avgang += dd->angles_at_pts[i]; - avgerr += err; - validpoints++; + FLT vec_correct[3] = {0., 0., 0.}; + // Step 1: Determine standard of deviation, and average in order to + // drop points that are likely in error. + { + // Calculate average + FLT avgerr_orig = 0.0; + FLT stddevsq = 0.0; + for (i = 0; i < pts; i++) + avgerr_orig += dd->quantity_errors[i]; + avgerr_orig /= pts; + + // Calculate standard of deviation. + for (i = 0; i < pts; i++) { + FLT diff = dd->quantity_errors[i] - avgerr_orig; + stddevsq += diff * diff; + } + stddevsq /= pts; + + for (i = 0; i < pts; i++) { + FLT err = dd->quantity_errors[i]; + FLT diff = err - avgerr_orig; + diff *= diff; + int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0; + ptvalid[i] = isptvalid; + if (isptvalid) { + avgang += dd->angles_at_pts[i]; + avgerr += err; + validpoints++; + } } + avgang /= validpoints; + avgerr /= validpoints; } - avgang /= validpoints; - avgerr /= validpoints; - } - // Step 2: Determine average lateral error. - // We can actually always perform this operation. Even with only one point. - if ( so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) - { - FLT avg_err[3] = {0, 0, 0}; // Positional error. - for (i = 0; i < pts; i++) { - if (!ptvalid[i]) - continue; - FLT *nrm = dd->normal_at_errors[i]; - FLT err = dd->quantity_errors[i]; - avg_err[0] = avg_err[0] + nrm[0] * err; - avg_err[1] = avg_err[1] + nrm[1] * err; - avg_err[2] = avg_err[2] + nrm[2] * err; - } + // Step 2: Determine average lateral error. + // We can actually always perform this operation. Even with only one point. + if ( so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) + { + FLT avg_err[3] = {0, 0, 0}; // Positional error. + for (i = 0; i < pts; i++) { + if (!ptvalid[i]) + continue; + FLT *nrm = dd->normal_at_errors[i]; + FLT err = dd->quantity_errors[i]; + avg_err[0] = avg_err[0] + nrm[0] * err; + avg_err[1] = avg_err[1] + nrm[1] * err; + avg_err[2] = avg_err[2] + nrm[2] * err; + } - // NOTE: The "avg_err" is not geometrically centered. This is actually - // probably okay, since if you have sevearl data points to one side, you - // can probably trust that more. - scale3d(avg_err, avg_err, 1. / validpoints); + // NOTE: The "avg_err" is not geometrically centered. This is actually + // probably okay, since if you have sevearl data points to one side, you + // can probably trust that more. + scale3d(avg_err, avg_err, 1. / validpoints); - // We have "Average error" now. A vector in worldspace. - // This can correct for lateral error, but not distance from camera. + // We have "Average error" now. A vector in worldspace. + // This can correct for lateral error, but not distance from camera. - // XXX TODO: Should we check to see if we only have one or - // two points to make sure the error on this isn't unusually high? - // If calculated error is unexpectedly high, then we should probably - // Not apply the transform. + // XXX TODO: Should we check to see if we only have one or + // two points to make sure the error on this isn't unusually high? + // If calculated error is unexpectedly high, then we should probably + // Not apply the transform. - if( ( magnitude3d( avg_err ) < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) - { - scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); - add3d(vec_correct, vec_correct, avg_err); - applied_corrections++; - } - else - { - so->PoseConfidence *= 0.9; + if( ( magnitude3d( avg_err ) < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) + { + scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); + add3d(vec_correct, vec_correct, avg_err); + applied_corrections++; + } + else + { + so->PoseConfidence *= 0.9; + } } - } - // Step 3: Control telecoption from lighthouse. - // we need to find out what the weighting is to determine "zoom" - if (validpoints > 1 && so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) // Can't correct "zoom" with only one point. - { - FLT zoom = 0.0; - FLT rmsang = 0.0; - for (i = 0; i < pts; i++) { - if (!ptvalid[i]) - continue; - FLT delang = dd->angles_at_pts[i] - avgang; - FLT delerr = dd->quantity_errors[i] - avgerr; - if (axis) - delang *= -1; // Flip sign on alternate axis because it's measured backwards. - zoom += delerr * delang; - rmsang += delang * delang; - } + // Step 3: Control telecoption from lighthouse. + // we need to find out what the weighting is to determine "zoom" + if (validpoints > 1 && so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) // Can't correct "zoom" with only one point. + { + FLT zoom = 0.0; + FLT rmsang = 0.0; + for (i = 0; i < pts; i++) { + if (!ptvalid[i]) + continue; + FLT delang = dd->angles_at_pts[i] - avgang; + FLT delerr = dd->quantity_errors[i] - avgerr; + if (axis) + delang *= -1; // Flip sign on alternate axis because it's measured backwards. + zoom += delerr * delang; + rmsang += delang * delang; + } - // Control into or outof lighthouse. - // XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to - // lighthouse. - // This is a questionable step. - zoom /= sqrt(rmsang); + // Control into or outof lighthouse. + // XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to + // lighthouse. + // This is a questionable step. + zoom /= sqrt(rmsang); - zoom *= CORRECT_TELESCOPTION_COEFFICIENT; + zoom *= CORRECT_TELESCOPTION_COEFFICIENT; - //Don't apply completely wild zoom's unless our confidence is awful. - if( ( zoom < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) - { - FLT veccamalong[3]; - sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); - normalize3d(veccamalong, veccamalong); - scale3d(veccamalong, veccamalong, zoom); - add3d(vec_correct, veccamalong, vec_correct); - applied_corrections++; - } - else - { - so->PoseConfidence *= 0.9; + //Don't apply completely wild zoom's unless our confidence is awful. + if( ( zoom < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) + { + FLT veccamalong[3]; + sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); + normalize3d(veccamalong, veccamalong); + scale3d(veccamalong, veccamalong, zoom); + add3d(vec_correct, veccamalong, vec_correct); + applied_corrections++; + } + else + { + so->PoseConfidence *= 0.9; + } } + AdjustPosition( so, vec_correct, 0 ); } +#if 0 //Tricky: Update position here, and back-correct imuvel based on correction. if( 0 ) { //XXX XXX TODO Position update LinmathPoint3d vecc; @@ -431,12 +495,12 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathQuat world_to_object_space; quatgetreciprocal(world_to_object_space, dd->InteralPoseUsedForCalc.Rot); quatrotatevector( vecc, world_to_object_space, vecc ); - add3d( dd->imu_accel_zero, dd->imu_accel_zero, vecc ); - printf( "ACCELV: %f %f %f %f %f %f\n", vecc[0], vecc[1], vecc[2], dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2] ); + //add3d( dd->imu_accel_zero, dd->imu_accel_zero, vecc ); + //printf( "ACCELV: %f %f %f %f %f %f\n", vecc[0], vecc[1], vecc[2], dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2] ); } } +#endif - add3d( dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos); // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. // If we were able to determine likliehood of a hit in the sweep instead of afterward @@ -460,7 +524,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathQuat world_to_object_space; quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); - //First, check to see if this hit is a sensor that is facing the lighthouse. + //4A: First, check to see if this hit is a sensor that is facing the lighthouse. + //This is for coarse corrections early on in the calibration. + //If one of these happens it means the orientation/pose is totally impossible. if( so->PoseConfidence < 0.9 ) { LinmathPoint3d vector_to_lighthouse; sub3d( vector_to_lighthouse, lhp->Pos, object_pose_at_hit->Pos ); //Get vector in world space. @@ -479,9 +545,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { cross3d( rotateaxis, vector_to_lighthouse, sensornormal ); LinmathQuat correction; quatfromaxisangle(correction, rotateaxis, facingness*.2 ); - quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction); - quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); normal_faults ++; + AdjustRotation( so, correction, 0, 1 ); } } } @@ -528,14 +593,12 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } // printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); // Apply our corrective quaternion to the output. - quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction); - quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); + AdjustRotation( so, correction, 0, 0 ); } memcpy( dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos, sizeof( dd->InteralPoseUsedForCalc.Pos ) ); dd->MixingConfidence[lhid][axis] = (validpoints)?((validpoints>1)?1.0:0.5):0; - //Box filter all of the guesstimations, and emit the new guess. { FLT MixedAmount = 0; @@ -552,7 +615,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { MixedAmount += Confidence; //printf( "%f ", Confidence ); } - scale3d( MixedPosition, MixedPosition, 1./MixedAmount ); + scale3d( dd->mixed_output, MixedPosition, 1./MixedAmount ); #if 0 printf( "Reprojection disagreements:" ); for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) @@ -561,16 +624,19 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } printf( "\n" ); #endif + EmitPose( so, pd ); //printf( "%f\n", MixedAmount ); - SurvivePose object_pose_out; - quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); - copy3d( object_pose_out.Pos, MixedPosition ); - - copy3d( dd->mixed_output, object_pose_out.Pos ); - quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); - quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); - PoserData_poser_pose_func(pd, so, &object_pose_out); + + // SurvivePose object_pose_out; + // quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); + // copy3d( object_pose_out.Pos, MixedPosition ); + + // copy3d( dd->mixed_output, object_pose_out.Pos ); + // quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); + // quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); + // PoserData_poser_pose_func(pd, so, &object_pose_out); + } // FLT var_meters = 0.5; // FLT error = 0.00001; diff --git a/src/survive_vive.c b/src/survive_vive.c index a83c6a1..0c8f583 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -491,10 +491,11 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] ); return -9; } - usleep(20000); } SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces ); + + usleep(100000); } libusb_free_device_list( devs, 1 ); -- cgit v1.2.3 From ae8c269a9760cfd3bf7727485506df5ad2aacc69 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Jun 2018 20:34:38 -0400 Subject: Remove unused code. --- src/poser_charlesrefine.c | 185 +--------------------------------------------- 1 file changed, 1 insertion(+), 184 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 3e09925..5375d9c 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -43,11 +43,6 @@ typedef struct { SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; -// LinmathPoint3d imuvel; -// LinmathPoint3d imu_accel_zero; -// int did_zero_imu; -// LinmathPoint3d imu_up_correct; - LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2]; LinmathPoint3d mixed_output; @@ -61,7 +56,6 @@ typedef struct { int ptsweep; SurviveIMUTracker tracker; - SurvivePose * lastlhp; } CharlesPoserData; @@ -110,7 +104,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); - //memcpy(&dd->imu_up_correct, &LinmathQuat_Identity, sizeof(LinmathQuat_Identity) ); + memcpy(&dd->imu_up_correct, &LinmathPoint3d_Identity, sizeof(LinmathPoint3d_Identity) ); so->PoseConfidence = 0.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -125,9 +119,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathQuat object_to_world_with_imu_up_correct; LinmathQuat world_space_to_object_space_quat; - //quatrotateabout( object_to_world_with_imu_up_correct, dd->InteralPoseUsedForCalc.Rot, dd->imu_up_correct ); - //quatgetreciprocal(world_space_to_object_space_quat, object_to_world_with_imu_up_correct ); - { LinmathQuat applygyro; imuData->gyro[0] *= imu_time; @@ -138,107 +129,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } EmitPose( so, pd ); - -#if 0 - //This will be overwritten by the accelerometer updates. - //We do this here in case we want to use it later. - copy3d( object_pose_out.Pos, dd->InteralPoseUsedForCalc.Pos ); - - - // Do accelerometer based stuff. - if( so->PoseConfidence > POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU ) { - - //Step 1: Use the accelerometer's "up" to figure out how we can re-right world space to be the correct "up" vector. - //Apply a tiny tug to the imu_up_correct with the up vector.. - { - LinmathVec3d reright = { 0, 0, 1 }; - LinmathVec3d normup; - normalize3d( normup, imuData->accel ); - LinmathVec3d correct_diff; - quatrotatevector(reright, world_space_to_object_space_quat, reright); - sub3d( correct_diff, normup, reright ); - scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. XXX THIS MUST CHANGE. - add3d( correct_diff, correct_diff, reright ); - normalize3d( correct_diff, correct_diff ); - LinmathQuat reright_quat; - quatfrom2vectors( reright_quat, reright, correct_diff ); - //Push to correct "Up" a little bit. - - //Do this if we want to use the IMU's up to help out the libsurvive estimate of "up" - // quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, reright_quat); - - // quatrotateabout(dd->imu_up_correct, dd->imu_up_correct, reright_quat); - // quatnormalize(dd->imu_up_correct, dd->imu_up_correct); - } - - - //Update position as a function from the IMU... - if( 1 ) { - LinmathVec3d acceleration; - scale3d( acceleration, imuData->accel, 1.0 ); - - if( !dd->did_zero_imu ) - { - copy3d( dd->imu_accel_zero, acceleration ); - dd->did_zero_imu = 1; - } - else - { - LinmathVec3d recalingval; - scale3d( recalingval, acceleration, 0.0001 ); - scale3d( dd->imu_accel_zero, dd->imu_accel_zero, 0.9999 ); - add3d( dd->imu_accel_zero, dd->imu_accel_zero, recalingval ); - } - - sub3d( acceleration, acceleration, dd->imu_accel_zero ); - scale3d( acceleration, acceleration, 9.8 * imu_time ); - printf( "ACCEL %f %f %f\n", PFTHREE( acceleration ) ); -#if 0 - LinmathVec3d expected_up = { 0, 0, 1 }; - LinmathVec3d acceleration; - scale3d( acceleration, imuData->accel, 1.0 ); - //quatrotatevector( acceleration, dd->imu_up_correct, acceleration ); - sub3d( acceleration, acceleration, dd->imu_accel_zero ); //IMU Accel Zero is in object-local space. - quatrotatevector( acceleration, dd->InteralPoseUsedForCalc.Rot, acceleration ); - sub3d( acceleration, acceleration, expected_up ); - - LinmathVec3d recalv; - scale3d( recalv, acceleration, 0.01 ); - LinmathQuat invrr; - quatgetreciprocal( invrr, dd->InteralPoseUsedForCalc.Rot ); - quatrotatevector( recalv, invrr, recalv ); - add3d( dd->imu_accel_zero, dd->imu_accel_zero, recalv ); - - printf( "%s %f %f %f %f %f %f\n", so->codename, dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2], acceleration[0], acceleration[1], acceleration[2] ); - - scale3d( acceleration, acceleration, 9.8 * imu_time ); - add3d( dd->imuvel, dd->imuvel, acceleration ); - - //IMUVel is the estimated object motion, in world space, but it will be pulled in the direction of the - //Faulty IMU bias. - - LinmathVec3d updatepos; - - scale3d( updatepos, dd->imuvel, 1.0 ); - scale3d( updatepos, updatepos, imu_time ); - - //Tricky, tug the imuvel toward zero otherwise it will contine to drift. - scale3d( dd->imuvel, dd->imuvel, 0.9999 ); - - //Update actual location. - add3d( dd->InteralPoseUsedForCalc.Pos, dd->InteralPoseUsedForCalc.Pos, updatepos ); - add3d( dd->mixed_output, dd->mixed_output, updatepos ); -#endif - } - } - - quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); - quatnormalize(dd->imu_up_correct, dd->imu_up_correct); - copy3d( object_pose_out.Pos, dd->mixed_output ); - quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); - quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); - PoserData_poser_pose_func(pd, so, &object_pose_out); -#endif return 0; } case POSERDATA_LIGHT: { @@ -257,30 +147,18 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { dd->sweeplh = lhid; - // FOR NOW, drop LH1. - // if( lhid == 1 ) break; - - // const FLT * sensor_normal = &so->sensor_normals[senid*3]; - // FLT sensor_normal_worldspace[3]; - // ApplyPoseToPoint(sensor_normal_worldspace, object_pose, sensor_inpos); - const FLT *sensor_inpos = &so->sensor_locations[senid * 3]; FLT sensor_position_worldspace[3]; // XXX Once I saw this get pretty wild (When in playback) // I had to invert the values of sensor_inpos. Not sure why. ApplyPoseToPoint(sensor_position_worldspace, &dd->InteralPoseUsedForCalc, sensor_inpos); - // printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], - // sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); - // = sensor position, relative to lighthouse center. FLT sensorpos_rel_lh[3]; sub3d(sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos); // Next, define a normal in global space of the plane created by the sweep hit. // Careful that this must be normalized. FLT sweep_normal[3]; - - FLT inangles[2]; FLT outangles[2]; inangles[axis] = inangle; @@ -482,25 +360,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } -#if 0 - //Tricky: Update position here, and back-correct imuvel based on correction. - if( 0 ) { //XXX XXX TODO Position update - LinmathPoint3d vecc; - scale3d( vecc, vec_correct, 0.01 ); - add3d( dd->imuvel, dd->imuvel, vecc ); - - if( so->PoseConfidence > POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU ) - { - scale3d( vecc, vec_correct, .01 ); - LinmathQuat world_to_object_space; - quatgetreciprocal(world_to_object_space, dd->InteralPoseUsedForCalc.Rot); - quatrotatevector( vecc, world_to_object_space, vecc ); - //add3d( dd->imu_accel_zero, dd->imu_accel_zero, vecc ); - //printf( "ACCELV: %f %f %f %f %f %f\n", vecc[0], vecc[1], vecc[2], dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2] ); - } - } -#endif - // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. // If we were able to determine likliehood of a hit in the sweep instead of afterward @@ -566,20 +425,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // Now, we have a "tug" vector in object-local space. Need to apply the torque. FLT vector_from_center_of_object[3]; - normalize3d(vector_from_center_of_object, sensor_inpos); - // scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); - // vector_from_center_of_object[2]*=-1; - // vector_from_center_of_object[1]*=-1; - // vector_from_center_of_object[0]*=-1; - // vector_from_center_of_object scale3d(vector_from_center_of_object, vector_from_center_of_object, 1); FLT new_vector_in_object_space[3]; - // printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], - // object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); - // printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], - // vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], - // correction_in_object_space[1], correction_in_object_space[2], dist ); scale3d(correction_in_object_space, correction_in_object_space, -dist); add3d(new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space); @@ -591,7 +439,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } } - // printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); // Apply our corrective quaternion to the output. AdjustRotation( so, correction, 0, 0 ); } @@ -616,37 +463,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //printf( "%f ", Confidence ); } scale3d( dd->mixed_output, MixedPosition, 1./MixedAmount ); -#if 0 - printf( "Reprojection disagreements:" ); - for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) - { - printf( "%f ", dist3d( dd->MixingPositions[l][a], MixedPosition ) ); - } - printf( "\n" ); -#endif EmitPose( so, pd ); - - //printf( "%f\n", MixedAmount ); - - // SurvivePose object_pose_out; - // quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); - // copy3d( object_pose_out.Pos, MixedPosition ); - - // copy3d( dd->mixed_output, object_pose_out.Pos ); - // quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); - // quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); - // PoserData_poser_pose_func(pd, so, &object_pose_out); - } - // FLT var_meters = 0.5; - // FLT error = 0.00001; - // FLT var_quat = error + .05; - // FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, - // error * var_quat, error * var_quat, error * var_quat}; - // - // survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var); - // PoserData_poser_pose_func(pd, so, &dd->tracker.pose); - dd->ptsweep = 0; if( validpoints > 1 && applied_corrections > 1 && !normal_faults) @@ -660,7 +478,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } dd->sweepaxis = l->acode & 1; - // printf( "SYNC %d %p\n", l->acode, dd ); break; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3 From 6b10746b801169439f9337383f394e9cddfc9108 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Jun 2018 20:39:04 -0400 Subject: Fix refactor --- src/poser_charlesrefine.c | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 5375d9c..dea67c6 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -104,7 +104,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); - memcpy(&dd->imu_up_correct, &LinmathPoint3d_Identity, sizeof(LinmathPoint3d_Identity) ); + //memcpy(&dd->imu_up_correct, &LinmathQuat_Identity, sizeof(LinmathQuat_Identity) ); so->PoseConfidence = 0.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -143,7 +143,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { FLT inangle = ld->angle; int sensor_id = ld->sensor_id; int axis = dd->sweepaxis; - //const SurvivePose *object_pose = &so->OutPose; dd->sweeplh = lhid; @@ -227,8 +226,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { int lhid = dd->sweeplh; int axis = dd->sweepaxis; int pts = dd->ptsweep; - //const SurvivePose *object_pose = - // &so->OutPose; // XXX TODO Should pull pose from approximate time when LHs were scanning it. BaseStationData *bsd = &so->ctx->bsd[lhid]; SurvivePose *lh_pose = &bsd->Pose; @@ -359,8 +356,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { AdjustPosition( so, vec_correct, 0 ); } - - // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. // If we were able to determine likliehood of a hit in the sweep instead of afterward // we would actually be able to perform this on a per-hit basis. @@ -425,6 +420,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // Now, we have a "tug" vector in object-local space. Need to apply the torque. FLT vector_from_center_of_object[3]; + normalize3d(vector_from_center_of_object, sensor_inpos); scale3d(vector_from_center_of_object, vector_from_center_of_object, 1); FLT new_vector_in_object_space[3]; @@ -439,7 +435,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } } - // Apply our corrective quaternion to the output. AdjustRotation( so, correction, 0, 0 ); } @@ -464,7 +459,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } scale3d( dd->mixed_output, MixedPosition, 1./MixedAmount ); EmitPose( so, pd ); + } + dd->ptsweep = 0; if( validpoints > 1 && applied_corrections > 1 && !normal_faults) @@ -478,6 +475,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } dd->sweepaxis = l->acode & 1; + // printf( "SYNC %d %p\n", l->acode, dd ); break; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3 From adae79a87e31fa8c6cebfa44da42e8a24ad1dd11 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Jun 2018 22:07:14 -0400 Subject: Integrate the accelerometer even if not perfectly. --- src/poser_charlesrefine.c | 84 ++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 69 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index dea67c6..de732fe 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -20,8 +20,11 @@ #define HIT_QUALITY_BASELINE \ 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response) -#define CORRECT_TELESCOPTION_COEFFICIENT 7.0f // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) +#define LIGHTCAP_DESCALE 0.5 //DO NOT EXCEED 0.7 + +#define CORRECT_LATERAL_POSITION_COEFFICIENT LIGHTCAP_DESCALE // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response) +#define CORRECT_TELESCOPTION_COEFFICIENT (10.f*LIGHTCAP_DESCALE) // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) + #define CORRECT_ROTATION_COEFFICIENT \ 0.2 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.01 @@ -57,30 +60,43 @@ typedef struct { SurviveIMUTracker tracker; SurvivePose * lastlhp; + + + //Additional flags, used when we start to try to use the accelerometer + LinmathPoint3d average_accelerometer_up_vector_in_world_space; + LinmathPoint3d velocity_according_to_accelerometer; } CharlesPoserData; void AdjustRotation( SurviveObject *so, LinmathQuat adjustment, int is_imu, int is_coarse ) { CharlesPoserData *dd = so->PoserData; - + //LinmathQuat invert_adjust; + //quatgetreciprocal( invert_adjust, adjustment ); quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, adjustment ); quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); - //XXX TODO: Calibrate the gyroscope using the lightcap. - - //Always update the accelerometer zero based on the gyro. - //quatrotatevector( dd->imu_accel_zero, applygyro, dd->imu_accel_zero ); - //Always update the system-corrective quat based on the gyro. - //quatrotateabout( dd->imu_up_correct, dd->imu_up_correct, applygyro ); } -void AdjustPosition( SurviveObject * so, LinmathPoint3d adjustment, int is_imu ) +void AdjustPosition( SurviveObject * so, LinmathPoint3d adjustment, int is_imu, float descale ) { CharlesPoserData *dd = so->PoserData; add3d( dd->InteralPoseUsedForCalc.Pos, adjustment, dd->InteralPoseUsedForCalc.Pos); add3d( dd->mixed_output, adjustment, dd->mixed_output); + + if( descale > 0.0001 ) + { + LinmathPoint3d backflow; + scale3d( backflow, adjustment, 1./descale ); + CharlesPoserData *dd = so->PoserData; + //scale3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, 0.9 ); + + //XXX XXX XXX BIG TODO!!! If we are updating based on lightcap, we should back-adjust the acceleration. + //Also, figure out how to dampen velocity. + add3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, backflow ); + } + } //Emits "dd->mixed_output" for position and dd->InteralPoseUsedForCalc.Rot for rotation. @@ -90,7 +106,16 @@ void EmitPose( SurviveObject *so, PoserData *pd ) SurvivePose object_pose_out; copy3d( object_pose_out.Pos, dd->mixed_output ); + + //average_accelerometer_up_vector_in_world_space should be "up" + LinmathVec3d true_up = { 0, 0, 1 }; + LinmathVec3d dist_up; + normalize3d( dist_up, dd->average_accelerometer_up_vector_in_world_space ); //NOTE: Average vector probably won't be normalized. + LinmathQuat adjustment_from_rerighting_up; + quatfrom2vectors( adjustment_from_rerighting_up, dist_up, true_up ); + quatcopy( object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); + quatrotateabout( object_pose_out.Rot, adjustment_from_rerighting_up, dd->InteralPoseUsedForCalc.Rot ); PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -104,7 +129,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); - //memcpy(&dd->imu_up_correct, &LinmathQuat_Identity, sizeof(LinmathQuat_Identity) ); so->PoseConfidence = 0.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -115,9 +139,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { float imu_time = 1./ so->imu_freq; // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; - SurvivePose object_pose_out; - LinmathQuat object_to_world_with_imu_up_correct; - LinmathQuat world_space_to_object_space_quat; { LinmathQuat applygyro; @@ -128,6 +149,39 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { AdjustRotation( so, applygyro, 1, 0 ); } + { + LinmathPoint3d rotated_out; + quatrotatevector( rotated_out, dd->InteralPoseUsedForCalc.Rot, imuData->accel ); + + if( so->PoseConfidence > 0.9999 ) + { + LinmathPoint3d correction; + + //XXX Danger, will robinson. + //We are doing an IIR on the acceleration. Tests have shown THIS IS BAD. We should try to correct based on the lightcap data. + scale3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, .9999 ); + scale3d( correction, rotated_out, .0001 ); + add3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, correction ); + + LinmathPoint3d deviation; + sub3d( deviation, rotated_out, dd->average_accelerometer_up_vector_in_world_space ); + + LinmathPoint3d acc; + scale3d( acc, deviation, 9.8*imu_time ); + add3d( dd->velocity_according_to_accelerometer, acc, dd->velocity_according_to_accelerometer ); + scale3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, .999 ); //XXX Danger! We are doing an IIR on velocity. This is dangerous. + + LinmathPoint3d posdiff; + scale3d( posdiff, dd->velocity_according_to_accelerometer, imu_time ); + AdjustPosition( so, posdiff, 1, 0 ); + } + else + { + copy3d( dd->average_accelerometer_up_vector_in_world_space, rotated_out ); + scale3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, 0 ); + } + } + EmitPose( so, pd ); return 0; } @@ -353,7 +407,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { so->PoseConfidence *= 0.9; } } - AdjustPosition( so, vec_correct, 0 ); + AdjustPosition( so, vec_correct, 0, (applied_corrections==2)?LIGHTCAP_DESCALE:0 ); } // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. -- cgit v1.2.3 From 07196617669b844cbc4b15fbdaa5d0d1de6c7508 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 25 Jun 2018 02:30:08 +0000 Subject: nerf motion a little more. --- src/poser_charlesrefine.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index de732fe..4e6e34f 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -159,8 +159,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //XXX Danger, will robinson. //We are doing an IIR on the acceleration. Tests have shown THIS IS BAD. We should try to correct based on the lightcap data. - scale3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, .9999 ); - scale3d( correction, rotated_out, .0001 ); + scale3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, .999 ); + scale3d( correction, rotated_out, .001 ); add3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, correction ); LinmathPoint3d deviation; -- cgit v1.2.3 From fc9bdfa45e8ad1e4eeefc272db077a25af542a82 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 25 Jun 2018 00:56:58 -0400 Subject: Add blacklisting of devices, as well as improve feedback algorithm for charlesrefine. --- src/poser_charlesrefine.c | 16 +++++++++------- src/survive.c | 1 + src/survive_vive.c | 12 +++++++----- 3 files changed, 17 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 4e6e34f..a55bc97 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -20,7 +20,7 @@ #define HIT_QUALITY_BASELINE \ 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define LIGHTCAP_DESCALE 0.5 //DO NOT EXCEED 0.7 +#define LIGHTCAP_DESCALE 0.2 //DO NOT EXCEED 0.7 #define CORRECT_LATERAL_POSITION_COEFFICIENT LIGHTCAP_DESCALE // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response) #define CORRECT_TELESCOPTION_COEFFICIENT (10.f*LIGHTCAP_DESCALE) // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) @@ -85,16 +85,15 @@ void AdjustPosition( SurviveObject * so, LinmathPoint3d adjustment, int is_imu, add3d( dd->InteralPoseUsedForCalc.Pos, adjustment, dd->InteralPoseUsedForCalc.Pos); add3d( dd->mixed_output, adjustment, dd->mixed_output); - if( descale > 0.0001 ) + if( descale > 0.0001 ) //Coming from lightcap. { LinmathPoint3d backflow; - scale3d( backflow, adjustment, 1./descale ); + scale3d( backflow, adjustment, 1.0/descale ); CharlesPoserData *dd = so->PoserData; - //scale3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, 0.9 ); - - //XXX XXX XXX BIG TODO!!! If we are updating based on lightcap, we should back-adjust the acceleration. - //Also, figure out how to dampen velocity. + //XXX TODO figure out how to dampen velocity. add3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, backflow ); + scale3d( backflow, backflow, .001 ); + add3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, backflow ); } } @@ -149,6 +148,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { AdjustRotation( so, applygyro, 1, 0 ); } + + printf( "ACCEL %f %f %f\n", PFTHREE( imuData->accel ) ); + { LinmathPoint3d rotated_out; quatrotatevector( rotated_out, dd->InteralPoseUsedForCalc.Rot, imuData->accel ); diff --git a/src/survive.c b/src/survive.c index 7ee1992..920b9ec 100644 --- a/src/survive.c +++ b/src/survive.c @@ -16,6 +16,7 @@ #define z_const const #endif +STATIC_CONFIG_ITEM( BLACKLIST_DEVS, "blacklist-devs", 's', "List any devs (or substrings of devs) to blacklist.", "-" ); STATIC_CONFIG_ITEM( CONFIG_FILE, "configfile", 's', "Default configuration file", "config.json" ); STATIC_CONFIG_ITEM( CONFIG_D_CALI, "disable-calibrate", 'i', "Enables or disables calibration", 0 ); STATIC_CONFIG_ITEM( CONFIG_F_CALI, "force-calibrate", 'i', "Forces calibration even if one exists.", 0 ); diff --git a/src/survive_vive.c b/src/survive_vive.c index 0c8f583..07bda58 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -501,12 +501,14 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * libusb_free_device_list( devs, 1 ); #endif + const char * blacklist = survive_configs(ctx, "blacklist-devs", SC_GET, "-"); + //Add the drivers - this must happen BEFORE we actually attach interfaces. - if( sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } - if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } - if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } - if( sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } - if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } + if( !strstr( blacklist, "HMD" ) && sv->udev[USB_DEV_HMD_IMU_LH] ) { survive_add_object( ctx, hmd ); } + if( !strstr( blacklist, "WM0" ) && sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); } + if( !strstr( blacklist, "WM1" ) && sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); } + if( !strstr( blacklist, "TR0" ) && sv->udev[USB_DEV_TRACKER0] ) { survive_add_object( ctx, tr0 ); } + if( !strstr( blacklist, "WW0" ) && sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); } if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD, sv->udev[USB_DEV_HMD], 0x81, survive_data_cb, "Mainboard" ) ) { return -6; } if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } -- cgit v1.2.3 From a0f4e2b3265ae242772f7a69bf6e096a2def64b3 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 25 Jun 2018 01:06:08 -0400 Subject: Bump refine. --- src/poser_charlesrefine.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index a55bc97..d0d5b17 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -149,7 +149,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } - printf( "ACCEL %f %f %f\n", PFTHREE( imuData->accel ) ); + //printf( "ACCEL %f %f %f\n", PFTHREE( imuData->accel ) ); { LinmathPoint3d rotated_out; -- cgit v1.2.3 From 7a16dc316c23d55453174c56fbff89f043db3552 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 25 Jun 2018 05:56:05 +0000 Subject: add additional comments about where to go from here. --- src/poser_charlesrefine.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index d0d5b17..e95b621 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -13,6 +13,11 @@ #define MAX_PT_PER_SWEEP SENSORS_PER_OBJECT +/* + More notes to self: + try updating expected lighthouse position against each lighthouse with imu data and don't tie them to each other. + +*/ // Tunable parameters: @@ -155,7 +160,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathPoint3d rotated_out; quatrotatevector( rotated_out, dd->InteralPoseUsedForCalc.Rot, imuData->accel ); - if( so->PoseConfidence > 0.9999 ) + if( so->PoseConfidence > 0.6 ) { LinmathPoint3d correction; -- cgit v1.2.3