From e1818d84e1ab0ae217d8035f8a12398f06508059 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 17 Mar 2017 16:32:18 -0700 Subject: Alternate disambiguator and calibration updates --- src/survive_cal.c | 50 ++++++++++++++++++++++++++-------- src/survive_cal.h | 6 +++-- src/survive_data.c | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 121 insertions(+), 13 deletions(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index 0eb9446..c36a48a 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -30,6 +30,11 @@ int mkdir(const char *); #define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4)) + //at stage 1, is when it branches to stage two or stage 7 + //stage 2 checks for the presence of two watchmen and an HMD visible to both lighthouses. + //Stage 3 collects a bunch of data for statistical averageing + //stage 4 does the calculation for the poses (NOT DONE!) + //Stage 5 = System Calibrate.d static void handle_calibration( struct SurviveCalData *cd ); @@ -117,17 +122,36 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->stage = 1; cd->ctx = ctx; - cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + cd->numPoseObjects = 0; - if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + for (int i=0; i < ctx->objs_ct; i++) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); - free( cd ); - return; + // This would be a place where we could conditionally decide if we + // want to include a certain device in the calibration routine. + if (1) + { + cd->poseobjects[i] = ctx->objs[i]; + cd->numPoseObjects++; + + SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename); + } } + // If we want to mandate that certain devices have been found + + + + //cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); + //cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); + //cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); + + //if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) + //{ + // SV_ERROR( "Error: cannot find all devices needed for calibration." ); + // free( cd ); + // return; + //} + //XXX TODO MWTourney, work on your code here. /* if( !cd->hmd ) @@ -185,7 +209,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( sensor_id < 0 ) { int lhid = -sensor_id-1; - if( lhid < NUM_LIGHTHOUSES && so->codename[0] == 'H' ) + // Take the OOTX data from the first device. + // TODO: Improve this so we'll watch all devices looking for OOTX data. Why limit ourselves to just one? + if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); @@ -193,7 +219,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int int i; for( i = 0; i < NUM_LIGHTHOUSES; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; - if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. + //if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! + if( i == 1 ) + cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; } @@ -498,11 +526,11 @@ static void handle_calibration( struct SurviveCalData *cd ) int obj; //Poses of lighthouses relative to objects. - SurvivePose objphl[POSE_OBJECTS][NUM_LIGHTHOUSES]; + SurvivePose objphl[MAX_POSE_OBJECTS][NUM_LIGHTHOUSES]; FILE * fobjp = fopen( "calinfo/objposes.csv", "w" ); - for( obj = 0; obj < POSE_OBJECTS; obj++ ) + for( obj = 0; obj < cd->numPoseObjects; obj++ ) { int i, j; PoserDataFullScene fsd; diff --git a/src/survive_cal.h b/src/survive_cal.h index 3d62051..8c488bd 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -37,7 +37,7 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MIN_PTS_BEFORE_CAL 24 #define DRPTS 128 -#define POSE_OBJECTS 3 +#define MAX_POSE_OBJECTS 10 #define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2) struct SurviveCalData @@ -64,7 +64,9 @@ struct SurviveCalData int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution. - SurviveObject * poseobjects[POSE_OBJECTS]; + SurviveObject * poseobjects[MAX_POSE_OBJECTS]; + + size_t numPoseObjects; PoserCB ConfigPoserFn; diff --git a/src/survive_data.c b/src/survive_data.c index 5c5a5e9..e6538d1 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,9 +5,87 @@ #include #include +int getAcodeFromSyncPulse(int pulseLen) +{ + if (pulseLen < 3125) return 0; + if (pulseLen < 3625) return 1; + if (pulseLen < 4125) return 2; + if (pulseLen < 4625) return 3; + if (pulseLen < 5125) return 4; + if (pulseLen < 5625) return 5; + if (pulseLen < 6125) return 6; + return 7; +} +void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) +{ + fprintf(stderr, "%d\n", le->length); + + if (le->timestamp - so->recent_sync_time < 24000) + { + // I do believe we are lighthouse B + so->last_sync_time[1] = so->recent_sync_time = le->timestamp; + so->last_sync_length[1] = le->length; + if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + { + so->sync_set_number = 1; + so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + } + } + else + { + // looks like this is the first sync pulse. Cool! + if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + { + so->sync_set_number = 1; + so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + } + } + + +// ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + + +// typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); + +} + +void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) +{ + +} + +void handle_lightcap2( SurviveObject * so, LightcapElement * le ) +{ + SurviveContext * ctx = so->ctx; + + if( le->sensor_id > SENSORS_PER_OBJECT ) + { + return; + } + + if (le->length > 6750) + { + // Should never get a reading so high. Odd. + return; + } + if (le->length >= 2750) + { + // Looks like a sync pulse, process it! + handle_lightcap2_sync(so, le); + } + + // must be a sweep pulse, process it! + handle_lightcap2_sweep(so, le); + +} + + //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode. void handle_lightcap( SurviveObject * so, LightcapElement * le ) { + handle_lightcap2(so,le); + return; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; -- cgit v1.2.3 From 472d05c2a356ec6d71669d67fb599e401a6f4a76 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 00:25:25 -0700 Subject: Updated disambiguator. Solid OOTX --- calibrate.c | 4 +- data_recorder.c | 4 +- include/libsurvive/survive.h | 3 +- src/survive_cal.c | 9 +- src/survive_data.c | 211 +++++++++++++++++++++++++++++++++++++++---- src/survive_process.c | 6 +- 6 files changed, 208 insertions(+), 29 deletions(-) diff --git a/calibrate.c b/calibrate.c index 726a8cc..0f2d6ac 100644 --- a/calibrate.c +++ b/calibrate.c @@ -51,10 +51,10 @@ int bufferpts[32*2*3][2]; char buffermts[32*128*3]; int buffertimeto[32*3][2]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { // if( timeinsweep < 0 ) return; - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( sensor_id < 0 ) return; if( acode == -1 ) return; //return; diff --git a/data_recorder.c b/data_recorder.c index 46f3427..6432f4f 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -43,9 +43,9 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( acode == -1 ) return; //return; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e04586c..c3b6a03 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -47,6 +47,7 @@ struct SurviveObject int32_t pulse_synctime_slack; //5,000 for normal vive hardware. (guessed) //Flood info, for calculating which laser is currently sweeping. + void * disambiguator_data; int8_t oldcode; int8_t sync_set_number; //0 = master, 1 = slave, -1 = fault. int8_t did_handle_ootx; //If unset, will send lightcap data for sync pulses next time a sensor is hit. @@ -129,7 +130,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , int lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/src/survive_cal.c b/src/survive_cal.c index c36a48a..51d12dd 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -210,7 +210,6 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int { int lhid = -sensor_id-1; // Take the OOTX data from the first device. - // TODO: Improve this so we'll watch all devices looking for OOTX data. Why limit ourselves to just one? if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; @@ -219,9 +218,9 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int int i; for( i = 0; i < NUM_LIGHTHOUSES; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; - //if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! - if( i == 1 ) - cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! + //if( i == 1 ) + //cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; } diff --git a/src/survive_data.c b/src/survive_data.c index e6538d1..2ef4940 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,7 +5,32 @@ #include #include -int getAcodeFromSyncPulse(int pulseLen) +typedef struct +{ + unsigned int sweep_time[SENSORS_PER_OBJECT]; + unsigned int sweep_len[SENSORS_PER_OBJECT]; +} lightcaps_sweep_data; +typedef struct +{ + lightcaps_sweep_data sweep; +} lightcap2_data; + +typedef struct +{ + int recent_sync_time; + int activeLighthouse; + int activeSweepStartTime; + int activeAcode; + + int lh_pulse_len[NUM_LIGHTHOUSES]; + int lh_start_time[NUM_LIGHTHOUSES]; + int current_lh; // used knowing which sync pulse we're looking at. + +} lightcap2_global_data; + +static lightcap2_global_data lcgd = { 0 }; + +int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) { if (pulseLen < 3125) return 0; if (pulseLen < 3625) return 1; @@ -16,48 +41,201 @@ int getAcodeFromSyncPulse(int pulseLen) if (pulseLen < 6125) return 6; return 7; } +void handle_lightcap2_process_sweep_data(SurviveObject *so) +{ + lightcap2_data *lcd = so->disambiguator_data; + + // look at all of the sensors we found, and process the ones that were hit. + // TODO: find the sensor(s) with the longest pulse length, and assume + // those are the "highest quality". Then, reject any pulses that are sufficiently + // different from those values, assuming that they are reflections. + { + unsigned int longest_pulse = 0; + unsigned int timestamp_of_longest_pulse = 0; + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] > longest_pulse) + { + longest_pulse = lcd->sweep.sweep_len[i]; + timestamp_of_longest_pulse = lcd->sweep.sweep_time[i]; + } + } + + for (int i = 0; i < SENSORS_PER_OBJECT; i++) + { + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it + { + int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + + if (offset_from < 380000 && offset_from > 70000) + { + int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]); + if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse. + { + so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); + } + } + } + } + } + // clear out sweep data (could probably limit this to only after a "first" sync. + // this is slightly more robust, so doing it here for now. + memset(&(((lightcap2_data*)so->disambiguator_data)->sweep), 0, sizeof(lightcaps_sweep_data)); +} void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) { - fprintf(stderr, "%d\n", le->length); + //fprintf(stderr, "%6.6d %4.4d \n", le->timestamp - so->recent_sync_time, le->length); + lightcap2_data *lcd = so->disambiguator_data; - if (le->timestamp - so->recent_sync_time < 24000) + //static unsigned int recent_sync_time = 0; + //static unsigned int recent_sync_count = -1; + //static unsigned int activeSweepStartTime; + + + // Process any sweep data we have + handle_lightcap2_process_sweep_data(so); + + int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); + + fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + // need to store up sync pulses, so we can take the earliest starting time for all sensors. + if (time_since_last_sync < 2400) { + lcgd.recent_sync_time = le->timestamp; + // it's the same sync pulse; + so->sync_set_number = 1; + //so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + so->recent_sync_time = le->timestamp; + + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcgd.activeLighthouse = lcgd.current_lh; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; + } + else + { + lcgd.activeLighthouse = -1; + lcgd.activeSweepStartTime = 0; + lcgd.activeAcode = 0; + } + } + else if (time_since_last_sync < 24000) + { + //recent_sync_count--; + lcgd.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - so->last_sync_time[1] = so->recent_sync_time = le->timestamp; - so->last_sync_length[1] = le->length; - if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + lcgd.current_lh = 1; + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + + //{ + //fprintf(stderr, "2"); + //so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. + + + //} + if (!(acode >> 2 & 1)) // if the skip bit is not set { - so->sync_set_number = 1; - so->ctx->lightproc(so, -2, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. + if (lcgd.activeLighthouse != -1) + { + // hmm, it appears we got two non-skip pulses at the same time. That should never happen + fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + } + lcgd.activeLighthouse = 1; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; } + } - else + else if (time_since_last_sync > 370000) { // looks like this is the first sync pulse. Cool! - if (le->length < 4625) // max non-skip pulse + 1/4 the difference in time between pulses. + + // first, send out the sync pulse data for the last round (for OOTX decoding { - so->sync_set_number = 1; - so->ctx->lightproc(so, -1, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. - } - } + if (lcgd.lh_pulse_len[0] != 0) + { + so->ctx->lightproc( + so, + -1, + handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]), + lcgd.lh_pulse_len[0], + lcgd.lh_start_time[0], + 0, + 0); + } + if (lcgd.lh_pulse_len[1] != 0) + { + so->ctx->lightproc( + so, + -2, + handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]), + lcgd.lh_pulse_len[1], + lcgd.lh_start_time[1], + 0, + 1); + } + } + // initialize here. + memset(&lcgd, 0, sizeof(lcgd)); + lcgd.activeLighthouse = -1; -// ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); -// typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); + lcgd.recent_sync_time = le->timestamp; + // I do believe we are lighthouse A + lcgd.current_lh = 0; + lcgd.lh_pulse_len[lcgd.current_lh] = le->length; + lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + + int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + //{ + // so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. + //} + + if (!(acode >> 2 & 1)) // if the skip bit is not set + { + lcgd.activeLighthouse = 0; + lcgd.activeSweepStartTime = le->timestamp; + lcgd.activeAcode = acode; + } + + } } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) { + lightcap2_data *lcd = so->disambiguator_data; + // If we see multiple "hits" on the sweep for a given sensor, + // assume that the longest (i.e. strongest signal) is most likely + // the non-reflected signal. + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) + { + lcd->sweep.sweep_len[le->sensor_id] = le->length; + lcd->sweep.sweep_time[le->sensor_id] = le->timestamp; + } } void handle_lightcap2( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; + if (so->disambiguator_data == NULL) + { + so->disambiguator_data = malloc(sizeof(lightcap2_data)); + memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); + } + if( le->sensor_id > SENSORS_PER_OBJECT ) { return; @@ -72,6 +250,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) { // Looks like a sync pulse, process it! handle_lightcap2_sync(so, le); + return; } // must be a sweep pulse, process it! diff --git a/src/survive_process.c b/src/survive_process.c index 8dc849a..6735f10 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,14 +6,14 @@ //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) { SurviveContext * ctx = so->ctx; - int base_station = acode >> 2; + int base_station = lh; int axis = acode & 1; if( ctx->calptr ) { - survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh); } if( base_station > NUM_LIGHTHOUSES ) return; -- cgit v1.2.3 From 7701d14440d5d005b1ec3513ff8646e2f6b101ef Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 23 Mar 2017 08:11:36 -0700 Subject: Cleanup --- src/survive_data.c | 27 +++++++++++---------------- 1 file changed, 11 insertions(+), 16 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 2ef4940..d92db36 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -69,8 +69,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (offset_from < 380000 && offset_from > 70000) { - int timeDelta = abs(timestamp_of_longest_pulse - lcd->sweep.sweep_time[i]); - if (timeDelta < 15000) // if this sweep point is within ~7 degrees of the point with the longest pulse. + if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) { so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); } @@ -97,14 +96,13 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); - fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); + //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { lcgd.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; - //so->ctx->lightproc(so, recent_sync_count, getAcodeFromSyncPulse(le->length), le->length, le->timestamp, le->length); // Don't think I got this quite right. so->recent_sync_time = le->timestamp; lcgd.lh_pulse_len[lcgd.current_lh] = le->length; @@ -126,7 +124,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) } else if (time_since_last_sync < 24000) { - //recent_sync_count--; lcgd.recent_sync_time = le->timestamp; // I do believe we are lighthouse B lcgd.current_lh = 1; @@ -135,12 +132,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); - //{ - //fprintf(stderr, "2"); - //so->ctx->lightproc(so, -2, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. - - - //} if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcgd.activeLighthouse != -1) @@ -197,9 +188,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); - //{ - // so->ctx->lightproc(so, -1, acode, le->length, le->timestamp, le->length); // Don't think I got this quite right. - //} if (!(acode >> 2 & 1)) // if the skip bit is not set { @@ -207,9 +195,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcgd.activeSweepStartTime = le->timestamp; lcgd.activeAcode = acode; } - } - } void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) @@ -219,6 +205,15 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) // If we see multiple "hits" on the sweep for a given sensor, // assume that the longest (i.e. strongest signal) is most likely // the non-reflected signal. + + if (le->length < 80) + { + // this is a low-quality read. Better to throw it out than to use it. + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + return; + } + fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { lcd->sweep.sweep_len[le->sensor_id] = le->length; -- cgit v1.2.3 From 280a6599fea76a7d2c16cfe0fcc5c8f37fde66de Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 09:47:38 -0700 Subject: More cleanup --- calibrate.c | 2 +- data_recorder.c | 2 +- include/libsurvive/survive.h | 2 +- include/libsurvive/survive_types.h | 2 +- src/survive_cal.c | 2 +- src/survive_cal.h | 2 +- src/survive_data.c | 96 +++++++++++++++++++------------------- src/survive_process.c | 2 +- src/survive_vive.c | 2 +- 9 files changed, 57 insertions(+), 55 deletions(-) diff --git a/calibrate.c b/calibrate.c index 0f2d6ac..82869e0 100644 --- a/calibrate.c +++ b/calibrate.c @@ -51,7 +51,7 @@ int bufferpts[32*2*3][2]; char buffermts[32*128*3]; int buffertimeto[32*3][2]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { // if( timeinsweep < 0 ) return; survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); diff --git a/data_recorder.c b/data_recorder.c index 6432f4f..04a219a 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -43,7 +43,7 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index c3b6a03..1165e9d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -130,7 +130,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //Call these from your callback if overridden. //Accept higher-level data. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , int lh); +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 1600e11..def30b8 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -28,7 +28,7 @@ typedef struct BaseStationData BaseStationData; typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be removed. Check at a later time for its defunctness. typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); diff --git a/src/survive_cal.c b/src/survive_cal.c index 51d12dd..dfa2e85 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -190,7 +190,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } -void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; diff --git a/src/survive_cal.h b/src/survive_cal.h index 8c488bd..9d7b3a9 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -29,7 +29,7 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri //void survive_cal_teardown( struct SurviveContext * ctx ); //Called from survive_default_light_process -void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ); +void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); #define MAX_SENSORS_TO_CAL 96 diff --git a/src/survive_data.c b/src/survive_data.c index d92db36..00e66f0 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -10,11 +10,6 @@ typedef struct unsigned int sweep_time[SENSORS_PER_OBJECT]; unsigned int sweep_len[SENSORS_PER_OBJECT]; } lightcaps_sweep_data; -typedef struct -{ - lightcaps_sweep_data sweep; -} lightcap2_data; - typedef struct { int recent_sync_time; @@ -28,7 +23,14 @@ typedef struct } lightcap2_global_data; -static lightcap2_global_data lcgd = { 0 }; +typedef struct +{ + lightcaps_sweep_data sweep; + lightcap2_global_data global; +} lightcap2_data; + + +//static lightcap2_global_data lcgd = { 0 }; int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) { @@ -65,13 +67,13 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - int offset_from = lcd->sweep.sweep_time[i] - lcgd.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) { - if (longest_pulse *10 / 8 < -lcd->sweep.sweep_len[i]) + if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { - so->ctx->lightproc(so, i, lcgd.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcgd.activeLighthouse); + so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); } } } @@ -94,54 +96,54 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // Process any sweep data we have handle_lightcap2_process_sweep_data(so); - int time_since_last_sync = (le->timestamp - lcgd.recent_sync_time); + int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time); //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcgd.activeLighthouse = lcgd.current_lh; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = lcd->global.current_lh; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } else { - lcgd.activeLighthouse = -1; - lcgd.activeSweepStartTime = 0; - lcgd.activeAcode = 0; + lcd->global.activeLighthouse = -1; + lcd->global.activeSweepStartTime = 0; + lcd->global.activeAcode = 0; } } else if (time_since_last_sync < 24000) { - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - lcgd.current_lh = 1; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.current_lh = 1; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - if (lcgd.activeLighthouse != -1) + if (lcd->global.activeLighthouse != -1) { // hmm, it appears we got two non-skip pulses at the same time. That should never happen fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } - lcgd.activeLighthouse = 1; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = 1; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } } @@ -151,49 +153,49 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcgd.lh_pulse_len[0] != 0) + if (lcd->global.lh_pulse_len[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[0]), - lcgd.lh_pulse_len[0], - lcgd.lh_start_time[0], + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]), + lcd->global.lh_pulse_len[0], + lcd->global.lh_start_time[0], 0, 0); } - if (lcgd.lh_pulse_len[1] != 0) + if (lcd->global.lh_pulse_len[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(lcgd.lh_pulse_len[1]), - lcgd.lh_pulse_len[1], - lcgd.lh_start_time[1], + handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]), + lcd->global.lh_pulse_len[1], + lcd->global.lh_start_time[1], 0, 1); } } // initialize here. - memset(&lcgd, 0, sizeof(lcgd)); - lcgd.activeLighthouse = -1; + memset(&lcd->global, 0, sizeof(lcd->global)); + lcd->global.activeLighthouse = -1; - lcgd.recent_sync_time = le->timestamp; + lcd->global.recent_sync_time = le->timestamp; // I do believe we are lighthouse A - lcgd.current_lh = 0; - lcgd.lh_pulse_len[lcgd.current_lh] = le->length; - lcgd.lh_start_time[lcgd.current_lh] = le->timestamp; + lcd->global.current_lh = 0; + lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; + lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcgd.activeLighthouse = 0; - lcgd.activeSweepStartTime = le->timestamp; - lcgd.activeAcode = acode; + lcd->global.activeLighthouse = 0; + lcd->global.activeSweepStartTime = le->timestamp; + lcd->global.activeAcode = acode; } } } @@ -383,8 +385,8 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t delta1 = so->last_sync_time[0] - so->recent_sync_time; int32_t delta2 = so->last_sync_time[1] - so->last_sync_time[0]; - ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0] ); - ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1] ); + ctx->lightproc( so, -1, acode_array[0], delta1, so->last_sync_time[0], so->last_sync_length[0], 0 ); + ctx->lightproc( so, -2, acode_array[1], delta2, so->last_sync_time[1], so->last_sync_length[1], 1 ); so->recent_sync_time = so->last_sync_time[1]; @@ -427,7 +429,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) //Make sure pulse is in valid window if( offset_from < 380000 && offset_from > 70000 ) { - ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, so->sync_set_number ); } } else diff --git a/src/survive_process.c b/src/survive_process.c index 6735f10..463481a 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -6,7 +6,7 @@ //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to //proper angles, then from there perform the rest of the solution. -void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, int lh) +void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { SurviveContext * ctx = so->ctx; int base_station = lh; diff --git a/src/survive_vive.c b/src/survive_vive.c index a9b295f..c04fc03 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -141,7 +141,7 @@ void survive_data_cb( SurviveUSBInterface * si ); //USB Subsystem void survive_usb_close( SurviveContext * t ); -int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 ); +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0, struct SurviveObject * ww0 ); int survive_usb_poll( SurviveContext * ctx ); int survive_get_config( char ** config, SurviveViveData * ctx, int devno, int iface, int send_extra_magic ); int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); -- cgit v1.2.3 From f33018188bf5bdf6f6b8af0d646e3f8c519d9d71 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 12:13:44 -0700 Subject: Added support for empty string in config.json & other cleanup. --- redist/json_helpers.c | 3 ++- src/survive_cal.c | 50 +++++++++++++++++++++----------------------------- src/survive_config.c | 10 +++++++++- src/survive_data.c | 2 +- 4 files changed, 33 insertions(+), 32 deletions(-) diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 0267932..3b5cc0d 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -117,7 +117,8 @@ char* load_file_to_mem(const char* path) { fseek( f, 0, SEEK_END ); int len = ftell( f ); fseek( f, 0, SEEK_SET ); - char * JSON_STRING = malloc( len ); + char * JSON_STRING = malloc( len + 1); + memset(JSON_STRING,0,len+1); fread( JSON_STRING, len, 1, f ); fclose( f ); return JSON_STRING; diff --git a/src/survive_cal.c b/src/survive_cal.c index 19eb3ca..f9ec595 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -124,50 +124,42 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->numPoseObjects = 0; - for (int i=0; i < ctx->objs_ct; i++) + const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" ); + const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 ); + size_t requiredTrackersFound = 0; + + for (int j=0; j < ctx->objs_ct; j++) { - // This would be a place where we could conditionally decide if we - // want to include a certain device in the calibration routine. - if (1) + // Add the tracker if we allow all trackers for calibration, or if it's in the list + // of required trackers. + int isRequiredTracker = strstr(RequiredTrackersForCal, ctx->objs[j]->codename) != NULL; + + if (isRequiredTracker) { - cd->poseobjects[i] = ctx->objs[i]; + requiredTrackersFound++; + } + + if (AllowAllTrackersForCal || isRequiredTracker) + { + cd->poseobjects[j] = ctx->objs[j]; cd->numPoseObjects++; - SV_INFO("Calibration is using %s", cd->poseobjects[i]->codename); + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); } + } // If we want to mandate that certain devices have been found - - - //cd->poseobjects[0] = survive_get_so_by_name( ctx, "HMD" ); - //cd->poseobjects[1] = survive_get_so_by_name( ctx, "WM0" ); - //cd->poseobjects[2] = survive_get_so_by_name( ctx, "WM1" ); - - //if( cd->poseobjects[0] == 0 || cd->poseobjects[1] == 0 || cd->poseobjects[2] == 0 ) - //{ - // SV_ERROR( "Error: cannot find all devices needed for calibration." ); - // free( cd ); - // return; - //} - -//XXX TODO MWTourney, work on your code here. -/* - if( !cd->hmd ) + if (strlen(RequiredTrackersForCal) > 0) { - cd->hmd = survive_get_so_by_name( ctx, "TR0" ); - - if( !cd->hmd ) + if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" ); + SV_ERROR( "Error: cannot find all devices needed for calibration." ); free( cd ); return; } - SV_INFO( "HMD not found, calibrating using Tracker" ); } -*/ - const char * DriverName; const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" ); diff --git a/src/survive_config.c b/src/survive_config.c index 0810280..005cfaf 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -175,7 +175,13 @@ const char* config_set_str(config_group *cg, const char *tag, const char* value) if (cv == NULL) cv = next_unused_entry(cg); sstrcpy(&(cv->tag), tag); - sstrcpy(&(cv->data), value); + + if (NULL != value){ + sstrcpy(&(cv->data), value); + } + else { + sstrcpy(&(cv->data), ""); + } cv->type = CONFIG_STRING; return value; @@ -357,9 +363,11 @@ void handle_tag_value(char* tag, char** values, uint8_t count) { print_json_value(tag,values,count); config_group* cg = cg_stack[cg_stack_head]; + if (NULL != *values){ if (parse_uint32(tag,values,count) > 0) return; //parse integers first, stricter rules if (parse_floats(tag,values,count) > 0) return; + } //should probably also handle string arrays config_set_str(cg,tag,values[0]); diff --git a/src/survive_data.c b/src/survive_data.c index ee180b1..da4e0a2 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -372,7 +372,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t main_divisor = so->timebase_hz / 384000; //125 @ 48 MHz. int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); - ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length ); + ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); } } -- cgit v1.2.3 From 3d86dda66d50d1e1955d4e0cd5f374e0aed1789f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:26:27 -0700 Subject: Fixed Windows USB Interface Enumeration --- src/survive_cal.c | 4 ++-- src/survive_data.c | 21 +++++++++++---------- src/survive_vive.c | 18 ++++++++++++++---- 3 files changed, 27 insertions(+), 16 deletions(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index f9ec595..81889ca 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -155,7 +155,7 @@ void survive_cal_install( struct SurviveContext * ctx ) { if (requiredTrackersFound != ((strlen(RequiredTrackersForCal) + 1) / 4)) { - SV_ERROR( "Error: cannot find all devices needed for calibration." ); + SV_ERROR( "Error: Did not find all devices required for calibration." ); free( cd ); return; } @@ -201,7 +201,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( sensor_id < 0 ) { int lhid = -sensor_id-1; - // Take the OOTX data from the first device. + // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) { uint8_t dbit = (acode & 2)>>1; diff --git a/src/survive_data.c b/src/survive_data.c index da4e0a2..ce263ed 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -208,13 +208,14 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le ) // assume that the longest (i.e. strongest signal) is most likely // the non-reflected signal. - if (le->length < 80) - { - // this is a low-quality read. Better to throw it out than to use it. - //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); - return; - } - fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //if (le->length < 80) + //{ + // // this is a low-quality read. Better to throw it out than to use it. + // //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + // return; + //} + //fprintf(stderr, "%2d %d\n", le->sensor_id, le->length); + //fprintf(stderr, "."); if (lcd->sweep.sweep_len[le->sensor_id] < le->length) { @@ -269,8 +270,8 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { void handle_lightcap( SurviveObject * so, LightcapElement * le ) { SurviveContext * ctx = so->ctx; -// handle_lightcap2(so,le); -// return; + handle_lightcap2(so,le); + return; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; @@ -373,7 +374,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int base_station = is_new_pulse; //printf( "%s %d %d %d\n", so->codename, le->sensor_id, so->sync_set_number, le->length ); ctx->lightproc( so, le->sensor_id, -3 - so->sync_set_number, 0, le->timestamp, le->length, base_station); - } + } } diff --git a/src/survive_vive.c b/src/survive_vive.c index f6465b2..bfe4926 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -340,7 +340,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * if (cur_dev->vendor_id == vendor_id && cur_dev->product_id == product_id) { - if( menum == enumid ) + if( cur_dev->interface_number == enumid ) { path_to_open = cur_dev->path; break; @@ -521,12 +521,21 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if (sv->udev[USB_DEV_W_WATCHMAN1]) { + //static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + static uint8_t vive_magic_power_on[5] = { 0x04 }; + //static uint8_t vive_magic_power_on[5] = { 0x04, 0x00, 0xc3, 0xe2, 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP]) + { + //static uint8_t vive_magic_enable_lighthouse[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } @@ -534,7 +543,8 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo for( int i = 0; i < 256; i++ ) { static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 }; - r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + //r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) ); SV_INFO( "UCR: %d", r ); if( r != sizeof( vive_controller_haptic_pulse ) ) return 5; OGUSleep( 1000 ); -- cgit v1.2.3 From c55c5928485da0432140be4befe5165c68ae4ab0 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 23 Mar 2017 16:46:04 -0700 Subject: Change disambiguator filter --- src/survive_data.c | 2 +- src/survive_vive.c | 45 +++++++++++++++++++++++++++++++++------------ 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index ce263ed..849b722 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -71,7 +71,7 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) if (offset_from < 380000 && offset_from > 70000) { - if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) + //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); } diff --git a/src/survive_vive.c b/src/survive_vive.c index bfe4926..f58fb79 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -47,6 +47,7 @@ const short vidpids[] = { 0x28de, 0x2012, 0, //Valve Watchman, USB connected #ifdef HIDAPI 0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap) + 0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap) 0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap) #endif }; //length MAX_USB_INTERFACES*2 @@ -60,6 +61,7 @@ const char * devnames[] = { "Wired Watchman 1", #ifdef HIDAPI "HMD Lightcap", + "Tracker 0 Lightcap", "Wired Watchman 1 Lightcap", #endif }; //length MAX_USB_INTERFACES @@ -74,8 +76,9 @@ const char * devnames[] = { #ifdef HIDAPI #define USB_DEV_LIGHTHOUSEB 6 -#define USB_DEV_W_WATCHMAN1_LIGHTCAP 7 -#define MAX_USB_DEVS 8 +#define USB_DEV_TRACKER0_LIGHTCAP 7 +#define USB_DEV_W_WATCHMAN1_LIGHTCAP 8 +#define MAX_USB_DEVS 9 #else #define MAX_USB_DEVS 6 #endif @@ -87,8 +90,9 @@ const char * devnames[] = { #define USB_IF_TRACKER0 4 #define USB_IF_W_WATCHMAN1 5 #define USB_IF_LIGHTCAP 6 -#define USB_IF_W_WATCHMAN1_LIGHTCAP 7 -#define MAX_INTERFACES 8 +#define USB_IF_TRACKER0_LIGHTCAP 7 +#define USB_IF_W_WATCHMAN1_LIGHTCAP 8 +#define MAX_INTERFACES 9 typedef struct SurviveUSBInterface SurviveUSBInterface; typedef struct SurviveViveData SurviveViveData; @@ -475,9 +479,11 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else if( sv->udev[USB_DEV_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #endif SV_INFO( "All enumerated devices attached." ); @@ -521,9 +527,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if (sv->udev[USB_DEV_W_WATCHMAN1]) { - //static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_power_on[5] = { 0x04 }; - //static uint8_t vive_magic_power_on[5] = { 0x04, 0x00, 0xc3, 0xe2, 0x04 }; r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } @@ -539,6 +543,23 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_power_on[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + if( r != sizeof( vive_magic_power_on ) ) return 5; + } + if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + #if 0 for( int i = 0; i < 256; i++ ) { @@ -551,12 +572,12 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo } #endif - if (sv->udev[USB_DEV_TRACKER0]) - { - static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; - r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); - if( r != sizeof( vive_magic_power_on ) ) return 5; - } + //if (sv->udev[USB_DEV_TRACKER0]) + //{ + // static uint8_t vive_magic_power_on[64] = { 0x04, 0x78, 0x29, 0x38 }; + // r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); + // if( r != sizeof( vive_magic_power_on ) ) return 5; + //} SV_INFO( "Powered unit on." ); } -- cgit v1.2.3 From 5404526ae8da8c5fdff81b8ee8120ffe73647747 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 24 Mar 2017 01:05:07 -0400 Subject: Dave's affine solve is getting close. --- dave/AffineSolve | Bin 40104 -> 40104 bytes redist/linmath.c | 2 +- src/poser_daveortho.c | 68 +++++++++++++++++++++++++++++++++++++------------- 3 files changed, 51 insertions(+), 19 deletions(-) diff --git a/dave/AffineSolve b/dave/AffineSolve index 7d51b34..bd93cbd 100755 Binary files a/dave/AffineSolve and b/dave/AffineSolve differ diff --git a/redist/linmath.c b/redist/linmath.c index eefcd5f..1724a13 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -215,7 +215,7 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 ) q[1] = (matrix44[9] - matrix44[6]) / S; q[2] = (matrix44[2] - matrix44[8]) / S; q[3] = (matrix44[4] - matrix44[1]) / S; - } else if ((matrix44[0] > matrix44[5])&(matrix44[0] > matrix44[10])) { + } else if ((matrix44[0] > matrix44[5])&&(matrix44[0] > matrix44[10])) { FLT S = FLT_SQRT(1.0 + matrix44[0] - matrix44[5] - matrix44[10]) * 2.; // S=4*qx q[0] = (matrix44[9] - matrix44[6]) / S; q[1] = 0.25f * S; diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index 80f65a9..906b21e 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -456,41 +456,73 @@ PRINT(ab,2,1); //------------------- // Orthogonalize the matrix //------------------- - - PRINT_MAT(T,4,4); -matrix44transpose(T2, T); -//matrix44copy(T2,T); - cross3d( &T2[0][0], &T2[1][0], &T2[2][0] ); - cross3d( &T2[2][0], &T2[0][0], &T2[1][0] ); - normalize3d( &T2[0][0], &T2[0][0] ); - normalize3d( &T2[1][0], &T2[1][0] ); - normalize3d( &T2[2][0], &T2[2][0] ); -//matrix44copy(T2,T); -matrix44transpose(T, T2); PRINT_MAT(T,4,4); +#if 1 +// matrix44transpose(T2, T); //Transpose so we are + matrix44copy(T2,T); + cross3d( &T2[1][0], &T2[0][0], &T2[2][0] ); + cross3d( &T2[2][0], &T2[1][0], &T2[0][0] ); //Replace axes in-place. + matrix44copy(T,T2); +// matrix44transpose(T, T2); + +#endif + + normalize3d( &T[0][0], &T[0][0] ); + normalize3d( &T[1][0], &T[1][0] ); + normalize3d( &T[2][0], &T[2][0] ); + //Change handedness + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; + +/* + //Check Orthogonality. Yep. It's orthogonal. + FLT tmp[3]; + cross3d( tmp, &T[0][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[1][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); + cross3d( tmp, &T[2][0], &T[0][0] ); + printf( "M3: %f\n", magnitude3d( tmp ) ); +*/ + +// PRINT_MAT(T,4,4); +#if 1 - //memcpy(T2,T,16*sizeof(float)); -// matrix44copy(T2,T); + //matrix44copy(T2,T); matrix44transpose(T2,T); + quatfrommatrix( quat, &T2[0][0] ); + printf( "QM: %f\n", quatmagnitude( quat ) ); quatnormalize(quatNorm,quat); quattoeuler(euler,quatNorm); - quattomatrix( T2, quatNorm ); + quattomatrix( &T2[0][0], quatNorm ); + + PRINT_MAT(T2,4,4); printf("rot %f %f %f len %f\n", euler[0], euler[1], euler[2], quatmagnitude(quat)); - PRINT(T,4,4); +// PRINT(T,4,4); // matrix44copy(temp,T2); matrix44transpose(temp,T2); - memcpy(T2,temp,16*sizeof(float)); + +// matrix44transpose(T2, temp); +// memcpy(T2,temp,16*sizeof(float)); for (i=0; i<3; i++) { for (j=0; j<3; j++) { - T[i][j] = T2[j][i]; + T[i][j] = temp[i][j]; } } - PRINT(T2,4,4); + +/* PRINT(T2,4,4); */ +#endif + + T[1][0]*=-1; + T[1][1]*=-1; + T[1][2]*=-1; /* -- cgit v1.2.3 From 9f6c7e87266c5a7d721d4d8e3ace82dc46207387 Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Fri, 24 Mar 2017 10:10:14 -0700 Subject: Fix linux build Successfully tested with a wired watchman on linux. Wired tracker now succeeds in providing calibration info Still not getting light info from tracker. --- calibrate_client.c | 11 +++++++---- src/survive_vive.c | 31 ++++++++++++++++++++++++++++++- 2 files changed, 37 insertions(+), 5 deletions(-) diff --git a/calibrate_client.c b/calibrate_client.c index 1a2ee41..1e18321 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -43,9 +43,9 @@ int bufferpts[32*2*3]; char buffermts[32*128*3]; int buffertimeto[32*3]; -void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length ) +void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh) { - survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh); if( acode == -1 ) return; //return; @@ -160,7 +160,7 @@ int main() // config_save("config.json"); */ - + ctx = survive_init( 1 ); survive_install_light_fn( ctx, my_light_process ); @@ -219,9 +219,12 @@ int main() so = wm0; if( strcmp( dev, "WM1" ) == 0 ) so = wm1; + uint32_t lh = 0; + if (lineptr[0] == 'r') + lh = 1; if( so ) - my_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); + my_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh ); break; } diff --git a/src/survive_vive.c b/src/survive_vive.c index f58fb79..c1bcb26 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -531,9 +531,10 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } + +#ifdef HIDAPI if (sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP]) { - //static uint8_t vive_magic_enable_lighthouse[64] = { 0x04, 0x78, 0x29, 0x38 }; static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; @@ -543,12 +544,27 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } +#else + if (sv->udev[USB_DEV_W_WATCHMAN1]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + +#endif + if (sv->udev[USB_DEV_TRACKER0]) { static uint8_t vive_magic_power_on[5] = { 0x04 }; r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } +#ifdef HIDAPI if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; @@ -559,6 +575,19 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } +#else + if (sv->udev[USB_DEV_TRACKER0]) + { + static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; + + static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) + r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; + } + +#endif #if 0 for( int i = 0; i < 256; i++ ) -- cgit v1.2.3 From a576703242adea11c012af5afdff38af84d22e2e Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 13:44:38 -0700 Subject: Adaptive Acode Offset Compensation --- calibrate.c | 2 +- src/survive_cal.c | 5 +-- src/survive_data.c | 114 +++++++++++++++++++++++++++++++---------------------- src/survive_vive.c | 30 ++++++++------ 4 files changed, 86 insertions(+), 65 deletions(-) diff --git a/calibrate.c b/calibrate.c index 62acbf6..6e2b5d1 100644 --- a/calibrate.c +++ b/calibrate.c @@ -59,7 +59,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int if( acode < 0 ) return; //return; int jumpoffset = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; + if( strcmp( so->codename, "WM0" ) == 0 || strcmp( so->codename, "WW0" ) == 0) jumpoffset += 32; else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; diff --git a/src/survive_cal.c b/src/survive_cal.c index 81889ca..22a8eff 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -200,6 +200,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int //Collecting OOTX data. if( sensor_id < 0 ) { + //fprintf(stderr, "%s\n", so->codename); int lhid = -sensor_id-1; // Take the OOTX data from the first device. (if using HMD, WM0, WM1 only, this will be HMD) if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0] ) @@ -210,9 +211,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int int i; for( i = 0; i < NUM_LIGHTHOUSES; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; - if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. <------- Revert This!!!!! - //if( i == 1 ) - //cd->stage = 2; //If all lighthouses have their OOTX set, move on. + if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse. } break; case 3: //Look for light sync lengths. diff --git a/src/survive_data.c b/src/survive_data.c index 849b722..4e2479a 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -21,26 +21,42 @@ typedef struct int lh_start_time[NUM_LIGHTHOUSES]; int current_lh; // used knowing which sync pulse we're looking at. +} lightcap2_per_sweep_data; + +typedef struct +{ + float acode_offset; } lightcap2_global_data; typedef struct { lightcaps_sweep_data sweep; + lightcap2_per_sweep_data per_sweep; lightcap2_global_data global; } lightcap2_data; //static lightcap2_global_data lcgd = { 0 }; -int handle_lightcap2_getAcodeFromSyncPulse(int pulseLen) +int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) { - if (pulseLen < 3125) return 0; - if (pulseLen < 3625) return 1; - if (pulseLen < 4125) return 2; - if (pulseLen < 4625) return 3; - if (pulseLen < 5125) return 4; - if (pulseLen < 5625) return 5; - if (pulseLen < 6125) return 6; + float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; + + int modifiedPulseLen = pulseLen - (int)oldOffset; + + float newOffset = (((pulseLen) + 250) % 500) - 250; + + ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1; + +//fprintf(stderr, " %f\n", oldOffset); +#define ACODE_OFFSET 0 + if (pulseLen < 3250 - ACODE_OFFSET) return 0; + if (pulseLen < 3750 - ACODE_OFFSET) return 1; + if (pulseLen < 4250 - ACODE_OFFSET) return 2; + if (pulseLen < 4750 - ACODE_OFFSET) return 3; + if (pulseLen < 5250 - ACODE_OFFSET) return 4; + if (pulseLen < 5750 - ACODE_OFFSET) return 5; + if (pulseLen < 6250 - ACODE_OFFSET) return 6; return 7; } void handle_lightcap2_process_sweep_data(SurviveObject *so) @@ -67,13 +83,13 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) { if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { - int offset_from = lcd->sweep.sweep_time[i] - lcd->global.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; + int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) { //if (longest_pulse *10 / 8 < lcd->sweep.sweep_len[i]) { - so->ctx->lightproc(so, i, lcd->global.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->global.activeLighthouse); + so->ctx->lightproc(so, i, lcd->per_sweep.activeAcode, offset_from, lcd->sweep.sweep_time[i], lcd->sweep.sweep_len[i], lcd->per_sweep.activeLighthouse); } } } @@ -96,54 +112,54 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // Process any sweep data we have handle_lightcap2_process_sweep_data(so); - int time_since_last_sync = (le->timestamp - lcd->global.recent_sync_time); + int time_since_last_sync = (le->timestamp - lcd->per_sweep.recent_sync_time); //fprintf(stderr, " %2d %8d %d\n", le->sensor_id, time_since_last_sync, le->length); // need to store up sync pulses, so we can take the earliest starting time for all sensors. if (time_since_last_sync < 2400) { - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // it's the same sync pulse; so->sync_set_number = 1; so->recent_sync_time = le->timestamp; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcd->global.activeLighthouse = lcd->global.current_lh; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } else { - lcd->global.activeLighthouse = -1; - lcd->global.activeSweepStartTime = 0; - lcd->global.activeAcode = 0; + lcd->per_sweep.activeLighthouse = -1; + lcd->per_sweep.activeSweepStartTime = 0; + lcd->per_sweep.activeAcode = 0; } } else if (time_since_last_sync < 24000) { - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse B - lcd->global.current_lh = 1; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.current_lh = 1; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - if (lcd->global.activeLighthouse != -1) + if (lcd->per_sweep.activeLighthouse != -1) { // hmm, it appears we got two non-skip pulses at the same time. That should never happen fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); } - lcd->global.activeLighthouse = 1; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = 1; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } } @@ -153,49 +169,50 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) // first, send out the sync pulse data for the last round (for OOTX decoding { - if (lcd->global.lh_pulse_len[0] != 0) + if (lcd->per_sweep.lh_pulse_len[0] != 0) { so->ctx->lightproc( so, -1, - handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[0]), - lcd->global.lh_pulse_len[0], - lcd->global.lh_start_time[0], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[0]), + lcd->per_sweep.lh_pulse_len[0], + lcd->per_sweep.lh_start_time[0], 0, 0); } - if (lcd->global.lh_pulse_len[1] != 0) + if (lcd->per_sweep.lh_pulse_len[1] != 0) { so->ctx->lightproc( so, -2, - handle_lightcap2_getAcodeFromSyncPulse(lcd->global.lh_pulse_len[1]), - lcd->global.lh_pulse_len[1], - lcd->global.lh_start_time[1], + handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_pulse_len[1]), + lcd->per_sweep.lh_pulse_len[1], + lcd->per_sweep.lh_start_time[1], 0, 1); } } + //fprintf(stderr, "************************************ Reinitializing Disambiguator!!!\n"); // initialize here. - memset(&lcd->global, 0, sizeof(lcd->global)); - lcd->global.activeLighthouse = -1; + memset(&lcd->per_sweep, 0, sizeof(lcd->per_sweep)); + lcd->per_sweep.activeLighthouse = -1; - lcd->global.recent_sync_time = le->timestamp; + lcd->per_sweep.recent_sync_time = le->timestamp; // I do believe we are lighthouse A - lcd->global.current_lh = 0; - lcd->global.lh_pulse_len[lcd->global.current_lh] = le->length; - lcd->global.lh_start_time[lcd->global.current_lh] = le->timestamp; + lcd->per_sweep.current_lh = 0; + lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; + lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(le->length); + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { - lcd->global.activeLighthouse = 0; - lcd->global.activeSweepStartTime = le->timestamp; - lcd->global.activeAcode = acode; + lcd->per_sweep.activeLighthouse = 0; + lcd->per_sweep.activeSweepStartTime = le->timestamp; + lcd->per_sweep.activeAcode = acode; } } } @@ -230,6 +247,7 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le ) if (so->disambiguator_data == NULL) { + fprintf(stderr, "Initializing Disambiguator Data\n"); so->disambiguator_data = malloc(sizeof(lightcap2_data)); memset(so->disambiguator_data, 0, sizeof(lightcap2_data)); } diff --git a/src/survive_vive.c b/src/survive_vive.c index c1bcb26..55e949a 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -479,6 +479,9 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + + // This is a HACK! But it works. Need to investigate further + sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; } if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; } #else @@ -564,18 +567,18 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo r = update_feature_report( sv->udev[USB_DEV_TRACKER0], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) ); if( r != sizeof( vive_magic_power_on ) ) return 5; } -#ifdef HIDAPI - if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) - { - static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); - if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; - - static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); - if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; - } -#else +//#ifdef HIDAPI +// if (sv->udev[USB_DEV_TRACKER0_LIGHTCAP]) +// { +// static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; +// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); +// if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; +// +// static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) +// r = update_feature_report( sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); +// if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; +// } +//#else if (sv->udev[USB_DEV_TRACKER0]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; @@ -587,7 +590,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } -#endif +//#endif #if 0 for( int i = 0; i < 256; i++ ) @@ -1186,6 +1189,7 @@ void survive_data_cb( SurviveUSBInterface * si ) break; } case USB_IF_W_WATCHMAN1_LIGHTCAP: + case USB_IF_TRACKER0_LIGHTCAP: { int i=0; for( i = 0; i < 7; i++ ) -- cgit v1.2.3 From f7d1c19e299df46087eb8693d2d0c0637d2e395f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 13:52:21 -0700 Subject: Fix Calibrate visualization to use proper lighthouse data --- calibrate.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/calibrate.c b/calibrate.c index 6e2b5d1..ee22abf 100644 --- a/calibrate.c +++ b/calibrate.c @@ -63,24 +63,24 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64; - if( acode == 0 || acode == 2 ) //data = 0 + if( acode % 2 == 0 && lh == 0) //data = 0 { bufferpts[jumpoffset*2+0][0] = (timeinsweep-100000)/500; buffertimeto[jumpoffset][0] = 0; } - if( acode == 1 || acode == 3 ) //data = 1 + if( acode % 2 == 1 && lh == 0 ) //data = 1 { bufferpts[jumpoffset*2+1][0] = (timeinsweep-100000)/500; buffertimeto[jumpoffset][0] = 0; } - if( acode == 4 || acode == 6 ) //data = 0 + if( acode % 2 == 0 && lh == 1 ) //data = 0 { bufferpts[jumpoffset*2+0][1] = (timeinsweep-100000)/500; buffertimeto[jumpoffset][1] = 0; } - if( acode == 5 || acode == 7 ) //data = 1 + if( acode % 2 == 1 && lh == 1 ) //data = 1 { bufferpts[jumpoffset*2+1][1] = (timeinsweep-100000)/500; buffertimeto[jumpoffset][1] = 0; -- cgit v1.2.3 From 4dc1d72785c660c206f8def9d8c8aa32289c2709 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:19:59 -0700 Subject: More cleanup & finishing genericization of calibrator --- calibrate.c | 4 ++-- include/libsurvive/survive.h | 2 +- include/libsurvive/survive_types.h | 2 +- redist/CNFGWinDriver.c | 2 +- redist/json_helpers.c | 2 +- src/survive_cal.c | 30 +++++++++++++++++++----------- src/survive_cal.h | 6 ++++-- src/survive_data.c | 6 +++--- src/survive_process.c | 6 +++--- src/survive_vive.c | 6 ++---- 10 files changed, 37 insertions(+), 29 deletions(-) diff --git a/calibrate.c b/calibrate.c index ee22abf..abf592a 100644 --- a/calibrate.c +++ b/calibrate.c @@ -99,9 +99,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3 } -void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh) { - survive_default_angle_process( so, sensor_id, acode, timecode, length, angle ); + survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh ); } char* sensor_name[32]; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 1165e9d..e13312d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -132,7 +132,7 @@ void survive_cal_install( SurviveContext * ctx ); //XXX This will be removed if //Accept higher-level data. void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh); void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id ); -void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); ////////////////////// Survive Drivers //////////////////////////// diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index def30b8..bfd0b1d 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -30,7 +30,7 @@ typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be remov typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); -typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); //Device drivers (prefix your drivers with "DriverReg") i.e. diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c index c5da925..b1c1eb0 100644 --- a/redist/CNFGWinDriver.c +++ b/redist/CNFGWinDriver.c @@ -232,7 +232,7 @@ void CNFGHandleInput() case WM_MBUTTONUP: HandleButton( (msg.lParam & 0xFFFF), (msg.lParam>>16) & 0xFFFF, 3, 0 ); break; case WM_KEYDOWN: case WM_KEYUP: - HandleKey( tolower( msg.wParam ), (msg.message==WM_KEYDOWN) ); + HandleKey( tolower( (int)(msg.wParam) ), (msg.message==WM_KEYDOWN) ); break; default: DispatchMessage(&msg); diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 3b5cc0d..29d48bd 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -174,7 +174,7 @@ void json_load_file(const char* path) { int16_t children = -1; - for (i=0; i<(int)items; i+=2) + for (i=0; i<(unsigned int)items; i+=2) { //increment i on each successful tag + values combination, not individual tokens jsmntok_t* tag_t = tokens+i; diff --git a/src/survive_cal.c b/src/survive_cal.c index 22a8eff..6c153b4 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -220,10 +220,13 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int else if( acode < -4 ) break; int lh = (-acode) - 3; - if( strcmp( so->codename, "WM0" ) == 0 ) - sensor_id += 32; - if( strcmp( so->codename, "WM1" ) == 0 ) - sensor_id += 64; + for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensor_id += i*32; + } + } cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; break; @@ -233,7 +236,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int } -void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) { struct SurviveContext * ctx = so->ctx; struct SurviveCalData * cd = ctx->calptr; @@ -241,14 +244,18 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( !cd ) return; int sensid = sensor_id; - if( strcmp( so->codename, "WM0" ) == 0 ) - sensid += 32; - if( strcmp( so->codename, "WM1" ) == 0 ) - sensid += 64; + + for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + { + if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) + { + sensid += i*32; + } + } if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return; - int lighthouse = acode>>2; + int lighthouse = lh; int axis = acode & 1; switch( cd->stage ) @@ -292,7 +299,8 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int min_peaks = PTS_BEFORE_COMMON; int i, j, k; cd->found_common = 1; - for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) + for( i = 0; i < cd->numPoseObjects; i++ ) + //for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ ) for( j = 0; j < NUM_LIGHTHOUSES; j++ ) { int sensors_visible = 0; diff --git a/src/survive_cal.h b/src/survive_cal.h index 8f4e4de..ae644d1 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -30,9 +30,11 @@ int survive_cal_get_status( SurviveContext * ctx, char * description, int descri //Called from survive_default_light_process void survive_cal_light( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); -void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ); +void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); -#define MAX_SENSORS_TO_CAL 96 +#define MAX_SENSORS_PER_DEVICE 32 +#define MAX_DEVICES_TO_CAL 3 +#define MAX_SENSORS_TO_CAL (MAX_SENSORS_PER_DEVICE * MAX_DEVICES_TO_CAL) #define MIN_PTS_BEFORE_CAL 24 diff --git a/src/survive_data.c b/src/survive_data.c index 4e2479a..9447104 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -25,7 +25,7 @@ typedef struct typedef struct { - float acode_offset; + double acode_offset; } lightcap2_global_data; typedef struct @@ -40,11 +40,11 @@ typedef struct int handle_lightcap2_getAcodeFromSyncPulse(SurviveObject * so, int pulseLen) { - float oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; + double oldOffset = ((lightcap2_data*)so->disambiguator_data)->global.acode_offset; int modifiedPulseLen = pulseLen - (int)oldOffset; - float newOffset = (((pulseLen) + 250) % 500) - 250; + double newOffset = (((pulseLen) + 250) % 500) - 250; ((lightcap2_data*)so->disambiguator_data)->global.acode_offset = oldOffset * 0.9 + newOffset * 0.1; diff --git a/src/survive_process.c b/src/survive_process.c index d4604d8..b58b344 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -37,16 +37,16 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode #endif FLT length_sec = length / (FLT)so->timebase_hz; - ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle ); + ctx->angleproc( so, sensor_id, acode, timecode, length_sec, angle, lh); } -void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh) { SurviveContext * ctx = so->ctx; if( ctx->calptr ) { - survive_cal_angle( so, sensor_id, acode, timecode, length, angle ); + survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh ); } if( so->PoserFn ) { diff --git a/src/survive_vive.c b/src/survive_vive.c index 55e949a..a5c731d 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -834,7 +834,6 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) qty-=2; int propset = 0; int doimu = 0; - int i; if( (type & 0xf0) == 0xf0 ) { @@ -911,10 +910,9 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) *readdata = type; //Put 'type' back on stack. uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to -//#define DEBUG_WATCHMAN #ifdef DEBUG_WATCHMAN printf( "_%s ", w->codename); - for( i = 0; i < qty; i++ ) + for(int i = 0; i < qty; i++ ) { printf( "%02x ", readdata[i] ); } @@ -1198,7 +1196,7 @@ void survive_data_cb( SurviveUSBInterface * si ) unsigned short *length = (unsigned short *)(&(readdata[2])); unsigned long *time = (unsigned long *)(&(readdata[4])); LightcapElement le; - le.sensor_id = POP2; + le.sensor_id = (uint8_t)POP2; le.length = POP2; le.timestamp = POP4; if( le.sensor_id == 0xff ) break; -- cgit v1.2.3 From a279a202768e1cae10f4fb651aad2b3b21db43ab Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 24 Mar 2017 15:36:43 -0700 Subject: remove min() --- src/survive_cal.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index 6c153b4..5414048 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -141,10 +141,17 @@ void survive_cal_install( struct SurviveContext * ctx ) if (AllowAllTrackersForCal || isRequiredTracker) { - cd->poseobjects[j] = ctx->objs[j]; - cd->numPoseObjects++; + if (MAX_DEVICES_TO_CAL < cd->numPoseObjects) + { + cd->poseobjects[j] = ctx->objs[j]; + cd->numPoseObjects++; - SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + SV_INFO("Calibration is using %s", cd->poseobjects[j]->codename); + } + else + { + SV_ERROR("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", cd->poseobjects[j]->codename); + } } } @@ -220,7 +227,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int else if( acode < -4 ) break; int lh = (-acode) - 3; - for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + for (int i=0; i < cd->numPoseObjects; i++) { if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) { @@ -234,6 +241,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int } + } void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) @@ -245,7 +253,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin int sensid = sensor_id; - for (int i=0; i < min(MAX_DEVICES_TO_CAL, cd->numPoseObjects); i++) + for (int i=0; i < cd->numPoseObjects; i++) { if( strcmp( so->codename, cd->poseobjects[i]->codename ) == 0 ) { -- cgit v1.2.3 From efe1b3e98866d45dd3bb5f8060712db152762edd Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Fri, 24 Mar 2017 16:00:20 -0700 Subject: Fixing linux --- calibrate_client.c | 4 ++-- src/survive_cal.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/calibrate_client.c b/calibrate_client.c index 1e18321..abfbabc 100644 --- a/calibrate_client.c +++ b/calibrate_client.c @@ -90,9 +90,9 @@ void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint3 } -void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle ) +void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ) { - survive_default_angle_process( so, sensor_id, acode, timecode, length, angle ); + survive_default_angle_process( so, sensor_id, acode, timecode, length, angle, lh ); } diff --git a/src/survive_cal.c b/src/survive_cal.c index 5414048..ae92bad 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -141,7 +141,7 @@ void survive_cal_install( struct SurviveContext * ctx ) if (AllowAllTrackersForCal || isRequiredTracker) { - if (MAX_DEVICES_TO_CAL < cd->numPoseObjects) + if (MAX_DEVICES_TO_CAL > cd->numPoseObjects) { cd->poseobjects[j] = ctx->objs[j]; cd->numPoseObjects++; @@ -150,7 +150,7 @@ void survive_cal_install( struct SurviveContext * ctx ) } else { - SV_ERROR("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", cd->poseobjects[j]->codename); + SV_INFO("Calibration is NOT using %s; device count exceeds MAX_DEVICES_TO_CAL", ctx->objs[j]->codename); } } -- cgit v1.2.3 From ce7fa5b134b1394f7bb9f4bddb1da9e83d792827 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 25 Mar 2017 21:58:51 -0700 Subject: Adding Tori Poser --- include/libsurvive/survive.h | 6 +- src/poser_turveytori.c | 871 +++++++++++++++++++++++++ winbuild/libsurvive/libsurvive.vcxproj | 1 + winbuild/libsurvive/libsurvive.vcxproj.filters | 3 + 4 files changed, 878 insertions(+), 3 deletions(-) create mode 100644 src/poser_turveytori.c diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e13312d..278bbca 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -32,9 +32,9 @@ struct SurviveObject PoserCB PoserFn; //Device-specific information about the location of the sensors. This data will be used by the poser. - int8_t nr_locations; - FLT * sensor_locations; - FLT * sensor_normals; + int8_t nr_locations; // sensor count + FLT * sensor_locations; // size is nr_locations*3. Contains x,y,z values for each sensor + FLT * sensor_normals;// size is nrlocations*3. cointains normal vector for each sensor //Timing sensitive data (mostly for disambiguation) int32_t timebase_hz; //48,000,000 for normal vive hardware. (checked) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c new file mode 100644 index 0000000..e9e5b7a --- /dev/null +++ b/src/poser_turveytori.c @@ -0,0 +1,871 @@ +#include +#include +#include +#include +#include +#include +#include "linmath.h" +#include +#include +#include + + +#define PointToFlts(x) ((FLT*)(x)) + +typedef struct +{ + FLT x; + FLT y; + FLT z; +} Point; + +void writePoint(FILE *file, double x, double y, double z, unsigned int rgb) {} +void updateHeader(FILE * file) {} +void writeAxes(FILE * file) {} +void drawLineBetweenPoints(FILE *file, Point a, Point b, unsigned int color) {} +void writePcdHeader(FILE * file) {} +void writePointCloud(FILE *f, Point *pointCloud, unsigned int Color) {} +void markPointWithStar(FILE *file, Point point, unsigned int color) {} + +typedef struct +{ + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. +} TrackedSensor; + +typedef struct +{ + size_t numSensors; + TrackedSensor sensor[0]; +} TrackedObject; + + +#ifndef M_PI +#define M_PI 3.14159265358979323846264338327 +#endif + +#define SQUARED(x) ((x)*(x)) + +typedef union +{ + struct + { + unsigned char Blue; + unsigned char Green; + unsigned char Red; + unsigned char Alpha; + }; + uint32_t long_value; +} RGBValue; + +static RGBValue RED = { .Red = 255,.Green = 0,.Blue = 0,.Alpha = 125 }; +static RGBValue GREEN = { .Red = 0,.Green = 255,.Blue = 0,.Alpha = 125 }; +static RGBValue BLUE = { .Red = 0,.Green = 0,.Blue = 255,.Alpha = 125 }; + +static const double WORLD_BOUNDS = 100; +#define MAX_TRACKED_POINTS 40 + +static const float DefaultPointsPerOuterDiameter = 60; + +typedef struct +{ + int something; + //Stuff +} ToriData; + + + + + + + +static FLT distance(Point a, Point b) +{ + FLT x = a.x - b.x; + FLT y = a.y - b.y; + FLT z = a.z - b.z; + return FLT_SQRT(x*x + y*y + z*z); +} + +Matrix3x3 GetRotationMatrixForTorus(Point a, Point b) +{ + Matrix3x3 result; + FLT v1[3] = { 0, 0, 1 }; + FLT v2[3] = { a.x - b.x, a.y - b.y, a.z - b.z }; + + normalize3d(v2, v2); + + rotation_between_vecs_to_m3(&result, v1, v2); + + // Useful for debugging... + //FLT v2b[3]; + //rotate_vec(v2b, v1, result); + + return result; +} + +typedef struct +{ + Point a; + Point b; + FLT angle; + FLT tanAngle; // tangent of angle + Matrix3x3 rotation; + Matrix3x3 invRotation; // inverse of rotation + +} PointsAndAngle; + + +Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin) +{ + Point q; + + double pf[3] = { p.x, p.y, p.z }; + q.x = rot.val[0][0] * p.x + rot.val[1][0] * p.y + rot.val[2][0] * p.z + newOrigin.x; + q.y = rot.val[0][1] * p.x + rot.val[1][1] * p.y + rot.val[2][1] * p.z + newOrigin.y; + q.z = rot.val[0][2] * p.x + rot.val[1][2] * p.y + rot.val[2][2] * p.z + newOrigin.z; + + return q; +} + +double angleFromPoints(Point p1, Point p2, Point center) +{ + Point v1, v2, v1norm, v2norm; + v1.x = p1.x - center.x; + v1.y = p1.y - center.y; + v1.z = p1.z - center.z; + + v2.x = p2.x - center.x; + v2.y = p2.y - center.y; + v2.z = p2.z - center.z; + + double v1mag = sqrt(v1.x * v1.x + v1.y * v1.y + v1.z * v1.z); + v1norm.x = v1.x / v1mag; + v1norm.y = v1.y / v1mag; + v1norm.z = v1.z / v1mag; + + double v2mag = sqrt(v2.x * v2.x + v2.y * v2.y + v2.z * v2.z); + v2norm.x = v2.x / v2mag; + v2norm.y = v2.y / v2mag; + v2norm.z = v2.z / v2mag; + + double res = v1norm.x * v2norm.x + v1norm.y * v2norm.y + v1norm.z * v2norm.z; + + double angle = acos(res); + + return angle; +} + +Point midpoint(Point a, Point b) +{ + Point m; + m.x = (a.x + b.x) / 2; + m.y = (a.y + b.y) / 2; + m.z = (a.z + b.z) / 2; + + return m; +} + +// What we're doing here is: +// * Given a point in space +// * And points and a lighthouse angle that implicitly define a torus +// * for that torus, what is the toroidal angle of the plane that will go through that point in space +// * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space? +void estimateToroidalAndPoloidalAngleOfPoint( + PointsAndAngle *pna, + Point point, + double *toroidalSin, + double *toroidalCos, + double *poloidalAngle, + double *poloidalSin) +{ + // We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from + // the tracked object coordinate system into the "easy" or "default" coordinate system of the torus. + // Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system. + Matrix3x3 rot = pna->invRotation; + Point origin; + origin.x = 0; + origin.y = 0; + origin.z = 0; + + Point m = midpoint(pna->a, pna->b); + + // in this new coordinate system, we'll rename all of the points we care about to have an "F" after them + // This will be their representation in the "friendly" coordinate system + Point pointF; + + // Okay, I lied a little above. In addition to the rotation matrix that we care about, there was also + // a translation that we did to move the origin. If we're going to get to the "friendly" coordinate system + // of the torus, we need to first undo the translation, then undo the rotation. Below, we're undoing the translation. + pointF.x = point.x - m.x; + pointF.y = point.y - m.y; + pointF.z = point.z - m.z; + + // now we'll undo the rotation part. + pointF = RotateAndTranslatePoint(pointF, rot, origin); + + // hooray, now pointF is in our more-friendly coordinate system. + + // Now, it's time to figure out the toroidal angle to that point. This should be pretty easy. + // We will "flatten" the z dimension to only look at the x and y values. Then, we just need to measure the + // angle between a vector to pointF and a vector along the x axis. + + FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x)); + + *toroidalSin = pointF.y / toroidalHyp; + + *toroidalCos = pointF.x / toroidalHyp; + + //*toroidalAngle = atan(pointF.y / pointF.x); + //if (pointF.x < 0) + //{ + // *toroidalAngle += M_PI; + //} + + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); + + //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001); + + // SCORE!! We've got the toroidal angle. We're half done! + + // Okay, what next...? Now, we will need to rotate the torus *again* to make it easy to + // figure out the poloidal angle. We should rotate the entire torus by the toroidal angle + // so that the point we're focusin on will lie on the x/z plane. We then should translate the + // torus so that the center of the poloidal circle is at the origin. At that point, it will + // be trivial to determine the poloidal angle-- it will be the angle on the xz plane of a + // vector from the origin to the point. + + // okay, instead of rotating the torus & point by the toroidal angle to get the point on + // the xz plane, we're going to take advantage of the radial symmetry of the torus + // (i.e. it's symmetric about the point we'd want to rotate it, so the rotation wouldn't + // change the torus at all). Therefore, we'll leave the torus as is, but we'll rotate the point + // This will only impact the x and y coordinates, and we'll use "G" as the postfix to represent + // this new coordinate system + + Point pointG; + pointG.z = pointF.z; + pointG.y = 0; + pointG.x = sqrt(SQUARED(pointF.x) + SQUARED(pointF.y)); + + // okay, that ended up being easier than I expected. Now that we have the point on the xZ plane, + // our next step will be to shift it down so that the center of the poloidal circle is at the origin. + // As you may have noticed, y has now gone to zero, and from here on out, we can basically treat + // this as a 2D problem. I think we're getting close... + + // I stole these lines from the torus generator. Gonna need the poloidal radius. + double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance. + double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); + double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); + + // The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. + // The shift along the X axis will be the toroidal radius. + + Point pointH; + pointH.z = pointG.z; + pointH.y = pointG.y; + pointH.x = pointG.x - toroidalRadius; + + // Okay, almost there. If we treat pointH as a vector on the XZ plane, if we get its angle, + // that will be the poloidal angle we're looking for. (crosses fingers) + + FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x)); + + *poloidalSin = pointH.z / poloidalHyp; + + + *poloidalAngle = atan(pointH.z / pointH.x); + if (pointH.x < 0) + { + *poloidalAngle += M_PI; + } + + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001); + //assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001); + + + + // Wow, that ended up being not so much code, but a lot of interesting trig. + // can't remember the last time I spent so much time working through each line of code. + + return; +} + +#define MAX_POINT_PAIRS 100 + +FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) +{ + FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta)); + FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + + return angle; +} + +// This provides a pretty good estimate of the angle above, probably better +// the further away the lighthouse is. But, it's not crazy-precise. +// It's main advantage is speed. +FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b) +{ + FLT p = (a->phi - b->phi); + FLT d = (a->theta - b->theta); + + FLT adjd = FLT_SIN((a->phi + b->phi) / 2); + FLT adjP = FLT_SIN((a->theta + b->theta) / 2); + FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd)); + return pythAngle; +} + +Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin) +{ + Point result; + + FLT distanceBetweenPoints = distance(pna->a, pna->b); + Point m = midpoint(pna->a, pna->b); + Matrix3x3 rot = pna->rotation; + + FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle); + FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2)); + + result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos; + result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin; + result.z = poloidalRadius*poloidalSin; + result = RotateAndTranslatePoint(result, rot, m); + + return result; +} + +FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) +{ + + double toroidalSin = 0; + double toroidalCos = 0; + double poloidalAngle = 0; + double poloidalSin = 0; + + estimateToroidalAndPoloidalAngleOfPoint( + pna, + pointIn, + &toroidalSin, + &toroidalCos, + &poloidalAngle, + &poloidalSin); + + Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin); + + FLT dist = distance(pointIn, torusPoint); + + // This is some voodoo black magic. This is here to solve the problem that the origin + // (which is near the center of all the tori) erroniously will rank as a good match. + // through a lot of empiracle testing on how to compensate for this, the "fudge factor" + // below ended up being the best fit. As simple as it is, I have a strong suspicion + // that there's some crazy complex thesis-level math that could be used to derive this + // but it works so we'll run with it. + // Note that this may be resulting in a skewing of the found location by several millimeters. + // it is not clear if this is actually removing existing skew (to get a more accurate value) + // or if it is introducing an undesirable skew. + double fudge = FLT_SIN((poloidalAngle - M_PI) / 2); + dist = dist / fudge; + + return dist; +} + +FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount) +{ + FLT fitness; + + FLT resultSum = 0; + + for (size_t i = 0; i < pnaCount; i++) + { + fitness = getPointFitnessForPna(pointIn, &(pna[i])); + resultSum += SQUARED(fitness); + } + + return 1 / FLT_SQRT(resultSum); +} + +Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision) +{ + Point result; + + Point tmpXplus = pointIn; + Point tmpXminus = pointIn; + tmpXplus.x = pointIn.x + precision; + tmpXminus.x = pointIn.x - precision; + result.x = getPointFitness(tmpXplus, pna, pnaCount) - getPointFitness(tmpXminus, pna, pnaCount); + + Point tmpYplus = pointIn; + Point tmpYminus = pointIn; + tmpYplus.y = pointIn.y + precision; + tmpYminus.y = pointIn.y - precision; + result.y = getPointFitness(tmpYplus, pna, pnaCount) - getPointFitness(tmpYminus, pna, pnaCount); + + Point tmpZplus = pointIn; + Point tmpZminus = pointIn; + tmpZplus.z = pointIn.z + precision; + tmpZminus.z = pointIn.z - precision; + result.z = getPointFitness(tmpZplus, pna, pnaCount) - getPointFitness(tmpZminus, pna, pnaCount); + + return result; +} + +Point getNormalizedVector(Point vectorIn, FLT desiredMagnitude) +{ + FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z)); + + FLT scale = desiredMagnitude / distanceIn; + + Point result = vectorIn; + + result.x *= scale; + result.y *= scale; + result.z *= scale; + + return result; +} + +Point getAvgPoints(Point a, Point b) +{ + Point result; + result.x = (a.x + b.x) / 2; + result.y = (a.y + b.y) / 2; + result.z = (a.z + b.z) / 2; + return result; +} + + +// This is modifies the basic gradient descent algorithm to better handle the shallow valley case, +// which appears to be typical of this convergence. +static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + Point lastPoint = initialEstimate; + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.2; g > 0.00001; g *= 0.99) + { + i++; + Point point1 = lastPoint; + // let's get 3 iterations of gradient descent here. + Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN1 = getNormalizedVector(gradient1, g); + + Point point2; + point2.x = point1.x + gradientN1.x; + point2.y = point1.y + gradientN1.y; + point2.z = point1.z + gradientN1.z; + + Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN2 = getNormalizedVector(gradient2, g); + + Point point3; + point3.x = point2.x + gradientN2.x; + point3.y = point2.y + gradientN2.y; + point3.z = point2.z + gradientN2.z; + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y }; + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + specialGradient = getNormalizedVector(specialGradient, g / 4); + + Point point4; + + point4.x = point3.x + specialGradient.x; + point4.y = point3.y + specialGradient.y; + point4.z = point3.z + specialGradient.z; + + FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + + if (newMatchFitness > lastMatchFitness) + { + if (logFile) + { + writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + } + + lastMatchFitness = newMatchFitness; + lastPoint = point4; +#ifdef TORI_DEBUG + printf("+"); +#endif + } + else + { +#ifdef TORI_DEBUG + printf("-"); +#endif + g *= 0.7; + + } + + + } + printf("\ni=%d\n", i); + + return lastPoint; +} + + +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1[3] = { 1, tan(theta), 0 }; + FLT t2[3] = { 1, tan(theta), 1 }; + + FLT tNorm[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNorm, t1, t2); + + // distance for this plane is d= fabs(A*x + B*y)/sqrt(A^2+B^2) (z term goes away since this plane is "vertical") + // where A is + //FLT d = + } +} + +static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + Point lastPoint = initialEstimate; + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.2; g > 0.00001; g *= 0.99) + { + i++; + Point point1 = lastPoint; + // let's get 3 iterations of gradient descent here. + Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN1 = getNormalizedVector(gradient1, g); + + Point point2; + point2.x = point1.x + gradientN1.x; + point2.y = point1.y + gradientN1.y; + point2.z = point1.z + gradientN1.z; + + Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); + Point gradientN2 = getNormalizedVector(gradient2, g); + + Point point3; + point3.x = point2.x + gradientN2.x; + point3.y = point2.y + gradientN2.y; + point3.z = point2.z + gradientN2.z; + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y }; + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + specialGradient = getNormalizedVector(specialGradient, g / 4); + + Point point4; + + point4.x = point3.x + specialGradient.x; + point4.y = point3.y + specialGradient.y; + point4.z = point3.z + specialGradient.z; + + FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + + if (newMatchFitness > lastMatchFitness) + { + if (logFile) + { + writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + } + + lastMatchFitness = newMatchFitness; + lastPoint = point4; +#ifdef TORI_DEBUG + printf("+"); +#endif + } + else + { +#ifdef TORI_DEBUG + printf("-"); +#endif + g *= 0.7; + + } + + + } + printf("\ni=%d\n", i); + + return lastPoint; +} + +void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) +{ + + // Step 1, create initial quaternion for guess. + // This should have the lighthouse directly facing the tracked object. + Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; + FLT theta = atan2(-lh.x, -lh.y); + FLT zAxis[3] = { 0, 0, 1 }; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + // not correcting for phi, but that's less important. + + // Step 2, optimize the quaternion to match the data. + +} + + +Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +{ + PointsAndAngle pna[MAX_POINT_PAIRS]; + + volatile size_t sizeNeeded = sizeof(pna); + + Point avgNorm = { 0 }; + + size_t pnaCount = 0; + for (unsigned int i = 0; i < obj->numSensors; i++) + { + for (unsigned int j = 0; j < i; j++) + { + if (pnaCount < MAX_POINT_PAIRS) + { + pna[pnaCount].a = obj->sensor[i].point; + pna[pnaCount].b = obj->sensor[j].point; + + pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]); + //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); + pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); + + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); + + pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); + pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation); + + + pnaCount++; + } + } + + avgNorm.x += obj->sensor[i].normal.x; + avgNorm.y += obj->sensor[i].normal.y; + avgNorm.z += obj->sensor[i].normal.z; + } + avgNorm.x = avgNorm.x / obj->numSensors; + avgNorm.y = avgNorm.y / obj->numSensors; + avgNorm.z = avgNorm.z / obj->numSensors; + + FLT avgNormF[3] = { avgNorm.x, avgNorm.y, avgNorm.z }; + + + FILE *logFile = NULL; + if (doLogOutput) + { + logFile = fopen("pointcloud2.pcd", "wb"); + writePcdHeader(logFile); + writeAxes(logFile); + } + + + // Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile); + + + // arbitrarily picking a value of 8 meters out to start from. + // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse + // since this is least likely to pick the incorrect "mirror" point that would send us + // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) + Point p1 = getNormalizedVector(avgNorm, 8); + + Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); + + FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z }; + + FLT a1 = anglebetween3d(pf1, avgNormF); + + if (a1 > M_PI / 2) + { + Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z }; + refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile); + + //FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z }; + + //FLT a2 = anglebetween3d(pf2, avgNormF); + + } + + FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); + + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); + printf("Distance is %f, Fitness is %f\n", distance, fitGd); + + if (logFile) + { + updateHeader(logFile); + fclose(logFile); + } + //fgetc(stdin); + return refinedEstimateGd; +} + + + + + + + + + + + + +int PoserTurveyTori( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + ToriData * dd = so->PoserData; + + if (!dd) + { + so->PoserData = dd = malloc(sizeof(ToriData)); + memset(dd, 0, sizeof(ToriData)); + } + + switch( pt ) + { + case POSERDATA_IMU: + { + PoserDataIMU * imu = (PoserDataIMU*)pd; + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + break; + } + case POSERDATA_LIGHT: + { + PoserDataLight * l = (PoserDataLight*)pd; + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + break; + } + case POSERDATA_FULL_SCENE: + { + TrackedObject *to; + + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //to->numSensors = so->nr_locations; + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + SolveForLighthouse(to, 0); + } + { + int sensorCount = 0; + int lh = 1; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + SolveForLighthouse(to, 0); + } + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + return 0; +} + + +REGISTER_LINKTIME( PoserTurveyTori ); + diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 643cff5..05fec8c 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -153,6 +153,7 @@ + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 0bb9d1b..8bb09b2 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -84,6 +84,9 @@ Source Files + + Source Files + -- cgit v1.2.3 From 17db4d9c369ee8633b05e1d19b6fbb3d61007598 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 09:34:08 -0700 Subject: Adding octavioradii OctavioRadii converges to the right radius!!!! --- src/poser_octavioradii.c | 542 +++++++++++++++++++++++++ src/poser_turveytori.c | 2 +- winbuild/libsurvive/libsurvive.vcxproj | 1 + winbuild/libsurvive/libsurvive.vcxproj.filters | 3 + 4 files changed, 547 insertions(+), 1 deletion(-) create mode 100644 src/poser_octavioradii.c diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c new file mode 100644 index 0000000..84d69fb --- /dev/null +++ b/src/poser_octavioradii.c @@ -0,0 +1,542 @@ +#include +#include +#include + +typedef struct +{ + int something; + //Stuff +} OctavioRadiiData; + +#include +#include +#include "linmath.h" +#include +#include +#include + +#define PTS 32 +#define MAX_CHECKS 40000 +#define MIN_HITS_FOR_VALID 10 + +FLT hmd_points[PTS * 3]; +FLT hmd_norms[PTS * 3]; +FLT hmd_point_angles[PTS * 2]; +int hmd_point_counts[PTS * 2]; +int best_hmd_target = 0; +int LoadData(char Camera, const char * FileData); + +//Values used for RunTest() +FLT LighthousePos[3] = { 0, 0, 0 }; +FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; + +FLT RunTest(int print); +void PrintOpti(); + +#define MAX_POINT_PAIRS 100 + +typedef struct +{ + FLT x; + FLT y; + FLT z; +} Point; + +typedef struct +{ + Point point; // location of the sensor on the tracked object; + Point normal; // unit vector indicating the normal for the sensor + double theta; // "horizontal" angular measurement from lighthouse radians + double phi; // "vertical" angular measurement from lighthouse in radians. +} TrackedSensor; + +typedef struct +{ + size_t numSensors; + TrackedSensor sensor[0]; +} TrackedObject; + +typedef struct +{ + unsigned char index1; + unsigned char index2; + FLT KnownDistance; +} PointPair; + +static FLT distance(Point a, Point b) +{ + FLT x = a.x - b.x; + FLT y = a.y - b.y; + FLT z = a.z - b.z; + return FLT_SQRT(x*x + y*y + z*z); +} + +typedef struct +{ + FLT HorizAngle; + FLT VertAngle; +} SensorAngles; + +#define SQUARED(x) ((x)*(x)) + +static FLT calculateFitnessOld(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + FLT estimatedDistanceBetweenPoints = + SQUARED(radii[pairs[i].index1]) + + SQUARED(radii[pairs[i].index2]) + - 2 * radii[pairs[i].index1] * radii[pairs[i].index2] + * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle) + * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle) + + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle); + + fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + } + + return FLT_SQRT(fitness); +} + +// angles is an array of angles between a sensor pair +// pairs is an array of point pairs +// radii is the guess at the radii of those angles +static FLT calculateFitnessOld2(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + // These are the vectors that represent the direction for the two points. + // TODO: optimize by precomputing the tangent. + FLT v1[3], v2[3], diff[3]; + + v1[0] = 1; + v2[0] = 1; + v1[1] = tan(angles[pairs[i].index1].HorizAngle); // can be precomputed + v2[1] = tan(angles[pairs[i].index2].HorizAngle); // can be precomputed + v1[2] = tan(angles[pairs[i].index1].VertAngle); // can be precomputed + v2[2] = tan(angles[pairs[i].index2].VertAngle); // can be precomputed + + // Now, normalize the vectors + normalize3d(v1, v1); // can be precomputed + normalize3d(v2, v2); // can be precomputed + + // Now, given the specified radii, find where the new points are + scale3d(v1, v1, radii[pairs[i].index1]); + scale3d(v2, v2, radii[pairs[i].index2]); + + // Cool, now find the vector between these two points + // TODO: optimize the following two funcs into one. + sub3d(diff, v1, v2); + + FLT distance = magnitude3d(diff); + + FLT t1 = magnitude3d(v1); + FLT t2 = magnitude3d(v2); + + + + FLT estimatedDistanceBetweenPoints = + + SQUARED(radii[pairs[i].index1]) + + SQUARED(radii[pairs[i].index2]) + - 2 * radii[pairs[i].index1] * radii[pairs[i].index2] + * FLT_SIN(angles[pairs[i].index1].HorizAngle) * FLT_SIN(angles[pairs[i].index2].HorizAngle) + * FLT_COS(angles[pairs[i].index1].VertAngle - angles[pairs[i].index2].VertAngle) + + FLT_COS(angles[pairs[i].index1].VertAngle) * FLT_COS(angles[pairs[i].index2].VertAngle); + + + //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + fitness += SQUARED(distance - pairs[i].KnownDistance); + } + + return FLT_SQRT(fitness); +} + +static FLT angleBetweenSensors(SensorAngles *a, SensorAngles *b) +{ + FLT angle = FLT_ACOS(FLT_COS(a->VertAngle - b->VertAngle)*FLT_COS(a->HorizAngle - b->HorizAngle)); + //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + + return angle; +} + +// angles is an array of angles between a sensor pair +// pairs is an array of point pairs +// radii is the guess at the radii of those angles +static FLT calculateFitness(SensorAngles *angles, FLT *radii, PointPair *pairs, size_t numPairs) +{ + FLT fitness = 0; + for (size_t i = 0; i < numPairs; i++) + { + + FLT angle = angleBetweenSensors(&angles[pairs[i].index1], &angles[pairs[i].index2]); + + // now we have a side-angle-side triangle, and we need to find the third side. + + // The Law of Cosines says: a^2 = b^2 + c^2 ? 2bc * cosA, + // where A is the angle opposite side a. + + // Transform this to: + // a = sqrt(b^2 + c^2 - 2bc * cosA) and we know the length of the missing side! + + FLT b2 = (SQUARED(radii[pairs[i].index1])); + FLT c2 = (SQUARED(radii[pairs[i].index2])); + FLT bc2 = (2 * radii[pairs[i].index1] * radii[pairs[i].index2]); + FLT cosA = (FLT_COS(angle)); + + FLT angleInDegrees = angle * 180 / LINMATHPI; + + FLT dist = sqrt( (SQUARED(radii[pairs[i].index1])) + + (SQUARED(radii[pairs[i].index2])) - + ( (2 * radii[pairs[i].index1] * radii[pairs[i].index2]) * + (FLT_COS(angle)))); + + + FLT fitnessAdder = SQUARED(dist - pairs[i].KnownDistance); + + if (isnan(fitnessAdder)) + { + int a = 0; + } + + //printf(" %2d %f\n", i, fitnessAdder); + + //fitness += SQUARED(estimatedDistanceBetweenPoints - pairs[i].KnownDistance); + fitness += SQUARED(dist - pairs[i].KnownDistance); + } + + //fitness = 1 / fitness; + return FLT_SQRT(fitness); +} + +#define MAX_RADII 32 + +// note gradientOut will be of the same degree as numRadii +static void getGradient(FLT *gradientOut, SensorAngles *angles, FLT *radii, size_t numRadii, PointPair *pairs, size_t numPairs, const FLT precision) +{ + FLT baseline = calculateFitness(angles, radii, pairs, numPairs); + + for (size_t i = 0; i < numRadii; i++) + { + FLT tmpPlus[MAX_RADII]; + memcpy(tmpPlus, radii, sizeof(*radii) * numRadii); + tmpPlus[i] += precision; + gradientOut[i] = -(calculateFitness(angles, tmpPlus, pairs, numPairs) - baseline); + } + + return; +} + +static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT desiredMagnitude) +{ + FLT distanceIn = 0; + + for (size_t i = 0; i < count; i++) + { + distanceIn += SQUARED(vectorToNormalize[i]); + } + distanceIn = FLT_SQRT(distanceIn); + + + FLT scale = desiredMagnitude / distanceIn; + + for (size_t i = 0; i < count; i++) + { + vectorToNormalize[i] *= scale; + } + + return; +} + + +static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) +{ + int i = 0; + FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs); + if (estimateOut != initialEstimate) + { + memcpy(estimateOut, initialEstimate, sizeof(*estimateOut) * numRadii); + } + + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.4; g > 0.00001; g *= 0.9999) + { + i++; + + + + FLT point1[MAX_RADII]; + memcpy(point1, estimateOut, sizeof(*point1) * numRadii); + + // let's get 3 iterations of gradient descent here. + FLT gradient1[MAX_RADII]; + getGradient(gradient1, angles, point1, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + normalizeAndMultiplyVector(gradient1, numRadii, g); + + FLT point2[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point2[i] = point1[i] + gradient1[i]; + } + FLT gradient2[MAX_RADII]; + getGradient(gradient2, angles, point2, numRadii, pairs, numPairs, g / 1000 /*somewhat arbitrary*/); + normalizeAndMultiplyVector(gradient2, numRadii, g); + + FLT point3[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point3[i] = point2[i] + gradient2[i]; + } + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + specialGradient[i] = point3[i] - point1[i]; + } + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + normalizeAndMultiplyVector(specialGradient, numRadii, g / 4); + + + FLT point4[MAX_RADII]; + for (size_t i = 0; i < numRadii; i++) + { + point4[i] = point3[i] + specialGradient[i]; + } + + + FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs); + + + if (newMatchFitness < lastMatchFitness) + { + //if (logFile) + //{ + // writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); + //} + + lastMatchFitness = newMatchFitness; + memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii); + +#ifdef RADII_DEBUG + printf("+ %d %0.9f (%0.9f) \n", i, newMatchFitness, g); +#endif + g = g * 1.05; + } + else + { +//#ifdef RADII_DEBUG + // printf("-"); + printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); +//#endif + // if it wasn't a match, back off on the distance we jump + g *= 0.7; + + } + +#ifdef RADII_DEBUG + FLT avg = 0; + FLT diffFromAvg[MAX_RADII]; + + for (size_t m = 0; m < numRadii; m++) + { + avg += estimateOut[m]; + } + avg = avg / numRadii; + + for (size_t m = 0; m < numRadii; m++) + { + diffFromAvg[m] = estimateOut[m] - avg;; + } + printf("[avg:%f] ", avg); + + for (size_t x = 0; x < numRadii; x++) + { + printf("%f, ", diffFromAvg[x]); + //printf("%f, ", estimateOut[x]); + } + printf("\n"); + + +#endif + + + } + printf("\ni=%d\n", i); +} + +void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +{ + FLT estimate[MAX_RADII]; + + for (size_t i = 0; i < MAX_RADII; i++) + { + estimate[i] = 2.2; + } + + SensorAngles angles[MAX_RADII]; + PointPair pairs[MAX_POINT_PAIRS]; + + size_t pairCount = 0; + + //obj->numSensors = 5; // TODO: HACK!!!! + + for (size_t i = 0; i < obj->numSensors; i++) + { + angles[i].HorizAngle = obj->sensor[i].theta; + angles[i].VertAngle = obj->sensor[i].phi; + } + + for (size_t i = 0; i < obj->numSensors - 1; i++) + { + for (size_t j = i + 1; j < obj->numSensors; j++) + { + pairs[pairCount].index1 = i; + pairs[pairCount].index2 = j; + pairs[pairCount].KnownDistance = distance(obj->sensor[i].point, obj->sensor[j].point); + pairCount++; + } + } + + + RefineEstimateUsingGradientDescentRadii(estimate, angles, estimate, obj->numSensors, pairs, pairCount, NULL); + + // we should now have an estimate of the radii. + + for (size_t i = 0; i < obj->numSensors; i++) + { + printf("radius[%d]: %f\n", i, estimate[i]); + } + // (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) + + return; +} + +int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) +{ + PoserType pt = pd->pt; + SurviveContext * ctx = so->ctx; + OctavioRadiiData * dd = so->PoserData; + + if( !dd ) so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + + switch( pt ) + { + case POSERDATA_IMU: + { + PoserDataIMU * imu = (PoserDataIMU*)pd; + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + break; + } + case POSERDATA_LIGHT: + { + PoserDataLight * l = (PoserDataLight*)pd; + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + break; + } + case POSERDATA_FULL_SCENE: + { + TrackedObject *to; + + PoserDataFullScene * fs = (PoserDataFullScene*)pd; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) + //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //to->numSensors = so->nr_locations; + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + Point position; + FLT orientation[4]; + + SolveForLighthouseRadii(&position, &orientation, to); + } + { + int sensorCount = 0; + int lh = 1; + + for (int i = 0; i < so->nr_locations; i++) + { + if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) + { + to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; + to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; + to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; + to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; + to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; + to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + sensorCount++; + } + } + + to->numSensors = sensorCount; + + Point position; + FLT orientation[4]; + + SolveForLighthouseRadii(&position, &orientation, to); + } + //printf( "Full scene data.\n" ); + break; + } + case POSERDATA_DISASSOCIATE: + { + free( dd ); + so->PoserData = 0; + //printf( "Need to disassociate.\n" ); + break; + } + } + return 0; +} + + +REGISTER_LINKTIME( PoserOctavioRadii ); + diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index e9e5b7a..4e602f3 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -299,7 +299,7 @@ void estimateToroidalAndPoloidalAngleOfPoint( FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b) { FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta)); - FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); + //FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta)); return angle; } diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 05fec8c..c794382 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -153,6 +153,7 @@ + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 8bb09b2..e7d44e2 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -87,6 +87,9 @@ Source Files + + Source Files + -- cgit v1.2.3 From 51b8cf7083d712eac730fe90f383abfabc7f2676 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 11:49:51 -0700 Subject: Tweaking --- src/poser_octavioradii.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 84d69fb..18d4026 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -350,7 +350,7 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a { //#ifdef RADII_DEBUG // printf("-"); - printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); + //printf("- %d %0.9f (%0.9f) [%0.9f] \n", i, newMatchFitness, g, estimateOut[0]); //#endif // if it wasn't a match, back off on the distance we jump g *= 0.7; @@ -394,7 +394,7 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj for (size_t i = 0; i < MAX_RADII; i++) { - estimate[i] = 2.2; + estimate[i] = 2.38; } SensorAngles angles[MAX_RADII]; -- cgit v1.2.3 From 5ef6668c3d2bc0a4e7c364119a99fcd6c6e43017 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 26 Mar 2017 19:08:44 -0700 Subject: Adding svd --- redist/svd.h | 450 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 450 insertions(+) create mode 100644 redist/svd.h diff --git a/redist/svd.h b/redist/svd.h new file mode 100644 index 0000000..41708da --- /dev/null +++ b/redist/svd.h @@ -0,0 +1,450 @@ +/************************************************************************** +** +** svd3 +** +** Quick singular value decomposition as described by: +** A. McAdams, A. Selle, R. Tamstorf, J. Teran and E. Sifakis, +** "Computing the Singular Value Decomposition of 3x3 matrices +** with minimal branching and elementary floating point operations", +** University of Wisconsin - Madison technical report TR1690, May 2011 +** +** OPTIMIZED CPU VERSION +** Implementation by: Eric Jang +** +** 13 Apr 2014 +** +** This file originally retrieved from: +** https://github.com/ericjang/svd3/blob/master/svd3.h 3/26/2017 +** +** Original licesnse is MIT per: +** https://github.com/ericjang/svd3/blob/master/LICENSE.md +** +** Ported from C++ to C by Mike Turvey. All modifications also released +** under an MIT license. +**************************************************************************/ + + +#ifndef SVD3_H +#define SVD3_H + +#define _gamma 5.828427124 // FOUR_GAMMA_SQUARED = sqrt(8)+3; +#define _cstar 0.923879532 // cos(pi/8) +#define _sstar 0.3826834323 // sin(p/8) +#define EPSILON 1e-6 + +#include + +/* This is a novel and fast routine for the reciprocal square root of an +IEEE float (single precision). +http://www.lomont.org/Math/Papers/2003/InvSqrt.pdf +http://playstation2-linux.com/download/p2lsd/fastrsqrt.pdf +http://www.beyond3d.com/content/articles/8/ +*/ +inline float rsqrt(float x) { + // int ihalf = *(int *)&x - 0x00800000; // Alternative to next line, + // float xhalf = *(float *)&ihalf; // for sufficiently large nos. + float xhalf = 0.5f*x; + int i = *(int *)&x; // View x as an int. + // i = 0x5f3759df - (i >> 1); // Initial guess (traditional). + i = 0x5f375a82 - (i >> 1); // Initial guess (slightly better). + x = *(float *)&i; // View i as float. + x = x*(1.5f - xhalf*x*x); // Newton step. + // x = x*(1.5008908 - xhalf*x*x); // Newton step for a balanced error. + return x; +} + +/* This is rsqrt with an additional step of the Newton iteration, for +increased accuracy. The constant 0x5f37599e makes the relative error +range from 0 to -0.00000463. +You can't balance the error by adjusting the constant. */ +inline float rsqrt1(float x) { + float xhalf = 0.5f*x; + int i = *(int *)&x; // View x as an int. + i = 0x5f37599e - (i >> 1); // Initial guess. + x = *(float *)&i; // View i as float. + x = x*(1.5f - xhalf*x*x); // Newton step. + x = x*(1.5f - xhalf*x*x); // Newton step again. + return x; +} + +inline float accurateSqrt(float x) +{ + return x * rsqrt1(x); +} + +inline void condSwap(bool c, float *X, float *Y) +{ + // used in step 2 + float Z = *X; + *X = c ? *Y : *X; + *Y = c ? Z : *Y; +} + +inline void condNegSwap(bool c, float *X, float *Y) +{ + // used in step 2 and 3 + float Z = -*X; + *X = c ? *Y : *X; + *Y = c ? Z : *Y; +} + +// matrix multiplication M = A * B +inline void multAB(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33) +{ + + *m11 = a11*b11 + a12*b21 + a13*b31; *m12 = a11*b12 + a12*b22 + a13*b32; *m13 = a11*b13 + a12*b23 + a13*b33; + *m21 = a21*b11 + a22*b21 + a23*b31; *m22 = a21*b12 + a22*b22 + a23*b32; *m23 = a21*b13 + a22*b23 + a23*b33; + *m31 = a31*b11 + a32*b21 + a33*b31; *m32 = a31*b12 + a32*b22 + a33*b32; *m33 = a31*b13 + a32*b23 + a33*b33; +} + +// matrix multiplication M = Transpose[A] * B +inline void multAtB(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33) +{ + *m11 = a11*b11 + a21*b21 + a31*b31; *m12 = a11*b12 + a21*b22 + a31*b32; *m13 = a11*b13 + a21*b23 + a31*b33; + *m21 = a12*b11 + a22*b21 + a32*b31; *m22 = a12*b12 + a22*b22 + a32*b32; *m23 = a12*b13 + a22*b23 + a32*b33; + *m31 = a13*b11 + a23*b21 + a33*b31; *m32 = a13*b12 + a23*b22 + a33*b32; *m33 = a13*b13 + a23*b23 + a33*b33; +} + +inline void quatToMat3(const float * qV, + float *m11, float *m12, float *m13, + float *m21, float *m22, float *m23, + float *m31, float *m32, float *m33 +) +{ + float w = qV[3]; + float x = qV[0]; + float y = qV[1]; + float z = qV[2]; + + float qxx = x*x; + float qyy = y*y; + float qzz = z*z; + float qxz = x*z; + float qxy = x*y; + float qyz = y*z; + float qwx = w*x; + float qwy = w*y; + float qwz = w*z; + + *m11 = 1 - 2 * (qyy + qzz); *m12 = 2 * (qxy - qwz); *m13 = 2 * (qxz + qwy); + *m21 = 2 * (qxy + qwz); *m22 = 1 - 2 * (qxx + qzz); *m23 = 2 * (qyz - qwx); + *m31 = 2 * (qxz - qwy); *m32 = 2 * (qyz + qwx); *m33 = 1 - 2 * (qxx + qyy); +} + +inline void approximateGivensQuaternion(float a11, float a12, float a22, float *ch, float *sh) +{ + /* + * Given givens angle computed by approximateGivensAngles, + * compute the corresponding rotation quaternion. + */ + *ch = 2 * (a11 - a22); + *sh = a12; + bool b = _gamma* (*sh)*(*sh) < (*ch)*(*ch); + // fast rsqrt function suffices + // rsqrt2 (https://code.google.com/p/lppython/source/browse/algorithm/HDcode/newCode/rsqrt.c?r=26) + // is even faster but results in too much error + float w = rsqrt((*ch)*(*ch) + (*sh)*(*sh)); + *ch = b ? w*(*ch) : (float)_cstar; + *sh = b ? w*(*sh) : (float)_sstar; +} + +inline void jacobiConjugation(const int x, const int y, const int z, + float *s11, + float *s21, float *s22, + float *s31, float *s32, float *s33, + float * qV) +{ + float ch, sh; + approximateGivensQuaternion(*s11, *s21, *s22, &ch, &sh); + + float scale = ch*ch + sh*sh; + float a = (ch*ch - sh*sh) / scale; + float b = (2 * sh*ch) / scale; + + // make temp copy of S + float _s11 = *s11; + float _s21 = *s21; float _s22 = *s22; + float _s31 = *s31; float _s32 = *s32; float _s33 = *s33; + + // perform conjugation S = Q'*S*Q + // Q already implicitly solved from a, b + *s11 = a*(a*_s11 + b*_s21) + b*(a*_s21 + b*_s22); + *s21 = a*(-b*_s11 + a*_s21) + b*(-b*_s21 + a*_s22); *s22 = -b*(-b*_s11 + a*_s21) + a*(-b*_s21 + a*_s22); + *s31 = a*_s31 + b*_s32; *s32 = -b*_s31 + a*_s32; *s33 = _s33; + + // update cumulative rotation qV + float tmp[3]; + tmp[0] = qV[0] * sh; + tmp[1] = qV[1] * sh; + tmp[2] = qV[2] * sh; + sh *= qV[3]; + + qV[0] *= ch; + qV[1] *= ch; + qV[2] *= ch; + qV[3] *= ch; + + // (x,y,z) corresponds to ((0,1,2),(1,2,0),(2,0,1)) + // for (p,q) = ((0,1),(1,2),(0,2)) + qV[z] += sh; + qV[3] -= tmp[z]; // w + qV[x] += tmp[y]; + qV[y] -= tmp[x]; + + // re-arrange matrix for next iteration + _s11 = *s22; + _s21 = *s32; _s22 = *s33; + _s31 = *s21; _s32 = *s31; _s33 = *s11; + *s11 = _s11; + *s21 = _s21; *s22 = _s22; + *s31 = _s31; *s32 = _s32; *s33 = _s33; + +} + +inline float dist2(float x, float y, float z) +{ + return x*x + y*y + z*z; +} + +// finds transformation that diagonalizes a symmetric matrix +inline void jacobiEigenanlysis( // symmetric matrix + float *s11, + float *s21, float *s22, + float *s31, float *s32, float *s33, + // quaternion representation of V + float * qV) +{ + qV[3] = 1; qV[0] = 0; qV[1] = 0; qV[2] = 0; // follow same indexing convention as GLM + for (int i = 0; i<4; i++) + { + // we wish to eliminate the maximum off-diagonal element + // on every iteration, but cycling over all 3 possible rotations + // in fixed order (p,q) = (1,2) , (2,3), (1,3) still retains + // asymptotic convergence + jacobiConjugation(0, 1, 2, s11, s21, s22, s31, s32, s33, qV); // p,q = 0,1 + jacobiConjugation(1, 2, 0, s11, s21, s22, s31, s32, s33, qV); // p,q = 1,2 + jacobiConjugation(2, 0, 1, s11, s21, s22, s31, s32, s33, qV); // p,q = 0,2 + } +} + + +inline void sortSingularValues(// matrix that we want to decompose + float *b11, float *b12, float *b13, + float *b21, float *b22, float *b23, + float *b31, float *b32, float *b33, + // sort V simultaneously + float *v11, float *v12, float *v13, + float *v21, float *v22, float *v23, + float *v31, float *v32, float *v33) +{ + float rho1 = dist2(*b11, *b21, *b31); + float rho2 = dist2(*b12, *b22, *b32); + float rho3 = dist2(*b13, *b23, *b33); + bool c; + c = rho1 < rho2; + condNegSwap(c, b11, b12); condNegSwap(c, v11, v12); + condNegSwap(c, b21, b22); condNegSwap(c, v21, v22); + condNegSwap(c, b31, b32); condNegSwap(c, v31, v32); + condSwap(c, &rho1, &rho2); + c = rho1 < rho3; + condNegSwap(c, b11, b13); condNegSwap(c, v11, v13); + condNegSwap(c, b21, b23); condNegSwap(c, v21, v23); + condNegSwap(c, b31, b33); condNegSwap(c, v31, v33); + condSwap(c, &rho1, &rho3); + c = rho2 < rho3; + condNegSwap(c, b12, b13); condNegSwap(c, v12, v13); + condNegSwap(c, b22, b23); condNegSwap(c, v22, v23); + condNegSwap(c, b32, b33); condNegSwap(c, v32, v33); +} + + +void QRGivensQuaternion(float a1, float a2, float *ch, float *sh) +{ + // a1 = pivot point on diagonal + // a2 = lower triangular entry we want to annihilate + float epsilon = (float)EPSILON; + float rho = accurateSqrt(a1*a1 + a2*a2); + + *sh = rho > epsilon ? a2 : 0; + *ch = fabsf(a1) + fmaxf(rho, epsilon); + bool b = a1 < 0; + condSwap(b, sh, ch); + float w = rsqrt((*ch)*(*ch) + (*sh)*(*sh)); + *ch *= w; + *sh *= w; +} + + +inline void QRDecomposition(// matrix that we want to decompose + float b11, float b12, float b13, + float b21, float b22, float b23, + float b31, float b32, float b33, + // output Q + float *q11, float *q12, float *q13, + float *q21, float *q22, float *q23, + float *q31, float *q32, float *q33, + // output R + float *r11, float *r12, float *r13, + float *r21, float *r22, float *r23, + float *r31, float *r32, float *r33) +{ + float ch1, sh1, ch2, sh2, ch3, sh3; + float a, b; + + // first givens rotation (ch,0,0,sh) + QRGivensQuaternion(b11, b21, &ch1, &sh1); + a = 1 - 2 * sh1*sh1; + b = 2 * ch1*sh1; + // apply B = Q' * B + *r11 = a*b11 + b*b21; *r12 = a*b12 + b*b22; *r13 = a*b13 + b*b23; + *r21 = -b*b11 + a*b21; *r22 = -b*b12 + a*b22; *r23 = -b*b13 + a*b23; + *r31 = b31; *r32 = b32; *r33 = b33; + + // second givens rotation (ch,0,-sh,0) + QRGivensQuaternion(*r11, *r31, &ch2, &sh2); + a = 1 - 2 * sh2*sh2; + b = 2 * ch2*sh2; + // apply B = Q' * B; + b11 = a*(*r11) + b*(*r31); b12 = a*(*r12) + b*(*r32); b13 = a*(*r13) + b*(*r33); + b21 = *r21; b22 = *r22; b23 = *r23; + b31 = -b*(*r11) + a*(*r31); b32 = -b*(*r12) + a*(*r32); b33 = -b*(*r13) + a*(*r33); + + // third givens rotation (ch,sh,0,0) + QRGivensQuaternion(b22, b32, &ch3, &sh3); + a = 1 - 2 * sh3*sh3; + b = 2 * ch3*sh3; + // R is now set to desired value + *r11 = b11; *r12 = b12; *r13 = b13; + *r21 = a*b21 + b*b31; *r22 = a*b22 + b*b32; *r23 = a*b23 + b*b33; + *r31 = -b*b21 + a*b31; *r32 = -b*b22 + a*b32; *r33 = -b*b23 + a*b33; + + // construct the cumulative rotation Q=Q1 * Q2 * Q3 + // the number of floating point operations for three quaternion multiplications + // is more or less comparable to the explicit form of the joined matrix. + // certainly more memory-efficient! + float sh12 = sh1*sh1; + float sh22 = sh2*sh2; + float sh32 = sh3*sh3; + + *q11 = (-1 + 2 * sh12)*(-1 + 2 * sh22); + *q12 = 4 * ch2*ch3*(-1 + 2 * sh12)*sh2*sh3 + 2 * ch1*sh1*(-1 + 2 * sh32); + *q13 = 4 * ch1*ch3*sh1*sh3 - 2 * ch2*(-1 + 2 * sh12)*sh2*(-1 + 2 * sh32); + + *q21 = 2 * ch1*sh1*(1 - 2 * sh22); + *q22 = -8 * ch1*ch2*ch3*sh1*sh2*sh3 + (-1 + 2 * sh12)*(-1 + 2 * sh32); + *q23 = -2 * ch3*sh3 + 4 * sh1*(ch3*sh1*sh3 + ch1*ch2*sh2*(-1 + 2 * sh32)); + + *q31 = 2 * ch2*sh2; + *q32 = 2 * ch3*(1 - 2 * sh22)*sh3; + *q33 = (-1 + 2 * sh22)*(-1 + 2 * sh32); +} + +void svd(// input A + float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // output U + float *u11, float *u12, float *u13, + float *u21, float *u22, float *u23, + float *u31, float *u32, float *u33, + // output S + float *s11, float *s12, float *s13, + float *s21, float *s22, float *s23, + float *s31, float *s32, float *s33, + // output V + float *v11, float *v12, float *v13, + float *v21, float *v22, float *v23, + float *v31, float *v32, float *v33) +{ + // normal equations matrix + float ATA11, ATA12, ATA13; + float ATA21, ATA22, ATA23; + float ATA31, ATA32, ATA33; + + multAtB(a11, a12, a13, a21, a22, a23, a31, a32, a33, + a11, a12, a13, a21, a22, a23, a31, a32, a33, + &ATA11, &ATA12, &ATA13, &ATA21, &ATA22, &ATA23, &ATA31, &ATA32, &ATA33); + + // symmetric eigenalysis + float qV[4]; + jacobiEigenanlysis(&ATA11, &ATA21, &ATA22, &ATA31, &ATA32, &ATA33, qV); + quatToMat3(qV, v11, v12, v13, v21, v22, v23, v31, v32, v33); + + float b11, b12, b13; + float b21, b22, b23; + float b31, b32, b33; + multAB(a11, a12, a13, a21, a22, a23, a31, a32, a33, + *v11, *v12, *v13, *v21, *v22, *v23, *v31, *v32, *v33, + &b11, &b12, &b13, &b21, &b22, &b23, &b31, &b32, &b33); + + // sort singular values and find V + sortSingularValues(&b11, &b12, &b13, &b21, &b22, &b23, &b31, &b32, &b33, + v11, v12, v13, v21, v22, v23, v31, v32, v33); + + // QR decomposition + QRDecomposition(b11, b12, b13, b21, b22, b23, b31, b32, b33, + u11, u12, u13, u21, u22, u23, u31, u32, u33, + s11, s12, s13, s21, s22, s23, s31, s32, s33 + ); +} + +/// polar decomposition can be reconstructed trivially from SVD result +// A = UP +void pd(float a11, float a12, float a13, + float a21, float a22, float a23, + float a31, float a32, float a33, + // output U + float *u11, float *u12, float *u13, + float *u21, float *u22, float *u23, + float *u31, float *u32, float *u33, + // output P + float *p11, float *p12, float *p13, + float *p21, float *p22, float *p23, + float *p31, float *p32, float *p33) +{ + float w11, w12, w13, w21, w22, w23, w31, w32, w33; + float s11, s12, s13, s21, s22, s23, s31, s32, s33; + float v11, v12, v13, v21, v22, v23, v31, v32, v33; + + svd(a11, a12, a13, a21, a22, a23, a31, a32, a33, + &w11, &w12, &w13, &w21, &w22, &w23, &w31, &w32, &w33, + &s11, &s12, &s13, &s21, &s22, &s23, &s31, &s32, &s33, + &v11, &v12, &v13, &v21, &v22, &v23, &v31, &v32, &v33); + + // P = VSV' + float t11, t12, t13, t21, t22, t23, t31, t32, t33; + multAB(v11, v12, v13, v21, v22, v23, v31, v32, v33, + s11, s12, s13, s21, s22, s23, s31, s32, s33, + &t11, &t12, &t13, &t21, &t22, &t23, &t31, &t32, &t33); + + multAB(t11, t12, t13, t21, t22, t23, t31, t32, t33, + v11, v21, v31, v12, v22, v32, v13, v23, v33, + p11, p12, p13, p21, p22, p23, p31, p32, p33); + + // U = WV' + multAB(w11, w12, w13, w21, w22, w23, w31, w32, w33, + v11, v21, v31, v12, v22, v32, v13, v23, v33, + u11, u12, u13, u21, u22, u23, u31, u32, u33); +} + +#endif \ No newline at end of file -- cgit v1.2.3 From ef039bb3ee8ce95f25e86baddbc4b3d5f21e6743 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 10:20:05 -0700 Subject: Fixing compiler warnings --- src/poser_octavioradii.c | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 18d4026..3893085 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -24,14 +24,11 @@ FLT hmd_norms[PTS * 3]; FLT hmd_point_angles[PTS * 2]; int hmd_point_counts[PTS * 2]; int best_hmd_target = 0; -int LoadData(char Camera, const char * FileData); //Values used for RunTest() FLT LighthousePos[3] = { 0, 0, 0 }; FLT LighthouseQuat[4] = { 1, 0, 0, 0 }; -FLT RunTest(int print); -void PrintOpti(); #define MAX_POINT_PAIRS 100 @@ -410,9 +407,9 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj angles[i].VertAngle = obj->sensor[i].phi; } - for (size_t i = 0; i < obj->numSensors - 1; i++) + for (unsigned char i = 0; i < obj->numSensors - 1; i++) { - for (size_t j = i + 1; j < obj->numSensors; j++) + for (unsigned char j = i + 1; j < obj->numSensors; j++) { pairs[pairCount].index1 = i; pairs[pairCount].index2 = j; @@ -426,7 +423,7 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (size_t i = 0; i < obj->numSensors; i++) + for (int i = 0; i < obj->numSensors; i++) { printf("radius[%d]: %f\n", i, estimate[i]); } @@ -494,7 +491,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) Point position; FLT orientation[4]; - SolveForLighthouseRadii(&position, &orientation, to); + SolveForLighthouseRadii(&position, orientation, to); } { int sensorCount = 0; @@ -521,7 +518,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) Point position; FLT orientation[4]; - SolveForLighthouseRadii(&position, &orientation, to); + SolveForLighthouseRadii(&position, orientation, to); } //printf( "Full scene data.\n" ); break; -- cgit v1.2.3 From f923e3c5ad03e7942eb46b67666807b738a1428a Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 10:59:26 -0700 Subject: completed fitness func for tori poser rotation --- src/poser_turveytori.c | 60 ++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 53 insertions(+), 7 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 4e602f3..37f79bb 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -535,23 +535,69 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) { + FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) { // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane - FLT t1[3] = { 1, tan(theta), 0 }; - FLT t2[3] = { 1, tan(theta), 1 }; + FLT t1H[3] = { 1, tan(theta), 0 }; + FLT t2H[3] = { 1, tan(theta), 1 }; - FLT tNorm[3]; + FLT tNormH[3]; // the normal is the cross of two vectors on the plane. - cross3d(tNorm, t1, t2); + cross3d(tNormH, t1H, t2H); - // distance for this plane is d= fabs(A*x + B*y)/sqrt(A^2+B^2) (z term goes away since this plane is "vertical") - // where A is - //FLT d = + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi)}; + FLT t2V[3] = { 1, 1, tan(phi)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); } + + fitness = FLT_SQRT(fitness); + + return fitness; } static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) -- cgit v1.2.3 From b9e9c89a46a91cbc69d7832d6f67e571723d11e6 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 12:48:40 -0700 Subject: Tori puzzle pieces in place, rotation not converging --- src/poser_turveytori.c | 197 +++++++++++++++++++++++++++++++++++++------------ 1 file changed, 149 insertions(+), 48 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 37f79bb..15961c8 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -412,7 +412,7 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci return result; } -Point getNormalizedVector(Point vectorIn, FLT desiredMagnitude) +Point getNormalizedAndScaledVector(Point vectorIn, FLT desiredMagnitude) { FLT distanceIn = sqrt(SQUARED(vectorIn.x) + SQUARED(vectorIn.y) + SQUARED(vectorIn.z)); @@ -464,7 +464,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, Point point1 = lastPoint; // let's get 3 iterations of gradient descent here. Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN1 = getNormalizedVector(gradient1, g); + Point gradientN1 = getNormalizedAndScaledVector(gradient1, g); Point point2; point2.x = point1.x + gradientN1.x; @@ -472,7 +472,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, point2.z = point1.z + gradientN1.z; Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN2 = getNormalizedVector(gradient2, g); + Point gradientN2 = getNormalizedAndScaledVector(gradient2, g); Point point3; point3.x = point2.x + gradientN2.x; @@ -491,7 +491,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // The second parameter to this function is very much a tunable parameter. Different values will result // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well // It's not clear what would be optimum here. - specialGradient = getNormalizedVector(specialGradient, g / 4); + specialGradient = getNormalizedAndScaledVector(specialGradient, g / 4); Point point4; @@ -531,6 +531,75 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *obj) +{ + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return fitness; +} + // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) @@ -541,8 +610,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) // first, get the normal of the plane for the horizonal sweep FLT theta = obj->sensor[i].theta; // make two vectors that lie on the plane - FLT t1H[3] = { 1, tan(theta), 0 }; - FLT t2H[3] = { 1, tan(theta), 1 }; + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; FLT tNormH[3]; @@ -556,8 +625,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) // first, get the normal of the plane for the horizonal sweep FLT phi = obj->sensor[i].phi; // make two vectors that lie on the plane - FLT t1V[3] = { 0, 1, tan(phi)}; - FLT t2V[3] = { 1, 1, tan(phi)}; + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; FLT tNormV[3]; @@ -600,11 +669,43 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) return fitness; } -static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) +void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitness(lhPoint, quaternion, obj); + + FLT tmp0plus[4]; + quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); + gradientOut[0] = RotationEstimateFitness(lhPoint, tmp0plus, obj) - baseFitness; + + FLT tmp1plus[4]; + quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); + gradientOut[1] = RotationEstimateFitness(lhPoint, tmp1plus, obj) - baseFitness; + + FLT tmp2plus[4]; + quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); + gradientOut[2] = RotationEstimateFitness(lhPoint, tmp2plus, obj) - baseFitness; + + FLT tmp3plus[4]; + quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); + gradientOut[3] = RotationEstimateFitness(lhPoint, tmp3plus, obj) - baseFitness; + + return; +} + +void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) +{ + quatnormalize(vectorToScale, vectorToScale); + quatscale(vectorToScale, vectorToScale, desiredMagnitude); + return; +} + +static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; - FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); - Point lastPoint = initialEstimate; + FLT lastMatchFitness = RotationEstimateFitness(lhPoint, initialEstimate, obj); + + quatcopy(rotOut, initialEstimate); // The values below are somewhat magic, and definitely tunable // The initial vlue of g will represent the biggest step that the gradient descent can take at first. @@ -622,23 +723,25 @@ static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, for (FLT g = 0.2; g > 0.00001; g *= 0.99) { i++; - Point point1 = lastPoint; + FLT point1[3]; + copy3d(point1, rotOut); // let's get 3 iterations of gradient descent here. - Point gradient1 = getGradient(point1, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN1 = getNormalizedVector(gradient1, g); + FLT gradient1[4]; + + getRotationGradient(gradient1, lhPoint, point1, obj, g/1000); + getNormalizedAndScaledRotationGradient(gradient1,g); - Point point2; - point2.x = point1.x + gradientN1.x; - point2.y = point1.y + gradientN1.y; - point2.z = point1.z + gradientN1.z; + FLT point2[4]; + quatadd(point2, gradient1, point1); + quatnormalize(point2,point2); - Point gradient2 = getGradient(point2, pna, pnaCount, g / 1000 /*somewhat arbitrary*/); - Point gradientN2 = getNormalizedVector(gradient2, g); + FLT gradient2[4]; + getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); + getNormalizedAndScaledRotationGradient(gradient2,g); - Point point3; - point3.x = point2.x + gradientN2.x; - point3.y = point2.y + gradientN2.y; - point3.z = point2.z + gradientN2.z; + FLT point3[4]; + quatadd(point3, gradient2, point2); + quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic @@ -647,48 +750,41 @@ static Point RefineRotationEstimate(Point initialEstimate, PointsAndAngle *pna, // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically // following *that* vector. As it turns out, this works *amazingly* well. - Point specialGradient = { .x = point3.x - point1.x,.y = point3.y - point1.y,.z = point3.y - point1.y }; + FLT specialGradient[4]; + quatsub(specialGradient,point3,point1); // The second parameter to this function is very much a tunable parameter. Different values will result // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well // It's not clear what would be optimum here. - specialGradient = getNormalizedVector(specialGradient, g / 4); + getNormalizedAndScaledRotationGradient(specialGradient,g/4); - Point point4; + FLT point4[4]; + quatadd(point4, specialGradient, point3); + quatnormalize(point4,point4); - point4.x = point3.x + specialGradient.x; - point4.y = point3.y + specialGradient.y; - point4.z = point3.z + specialGradient.z; - - FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); if (newMatchFitness > lastMatchFitness) { - if (logFile) - { - writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF); - } lastMatchFitness = newMatchFitness; - lastPoint = point4; -#ifdef TORI_DEBUG + quatcopy(rotOut, point4); +//#ifdef TORI_DEBUG printf("+"); -#endif +//#endif } else { -#ifdef TORI_DEBUG +//#ifdef TORI_DEBUG printf("-"); -#endif +//#endif g *= 0.7; } } - printf("\ni=%d\n", i); - - return lastPoint; + printf("\nRi=%d\n", i); } void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) @@ -698,12 +794,14 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[3] = { 0, 0, 1 }; - FLT quat1[4]; - quatfromaxisangle(quat1, zAxis, theta); + FLT zAxis[4] = { 0, 0, 1 ,0}; + //FLT quat1[4]; + //quatfromaxisangle(quat1, zAxis, theta); // not correcting for phi, but that's less important. + // Step 2, optimize the quaternion to match the data. + RefineRotationEstimate(rotOut, lh, zAxis, obj); } @@ -767,7 +865,7 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) // intentionally picking the direction of the average normal vector of the sensors that see the lighthouse // since this is least likely to pick the incorrect "mirror" point that would send us // back into the search for the correct point (see "if (a1 > M_PI / 2)" below) - Point p1 = getNormalizedVector(avgNorm, 8); + Point p1 = getNormalizedAndScaledVector(avgNorm, 8); Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile); @@ -792,6 +890,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); + if (logFile) { updateHeader(logFile); -- cgit v1.2.3 From ad5afae7e264ffa72ed16d2ec2c6f6090e7aa7b5 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 13:02:23 -0700 Subject: Adding operation for rotating about an axis --- redist/linmath.c | 19 +++++++++++++++++++ redist/linmath.h | 1 + 2 files changed, 20 insertions(+) diff --git a/redist/linmath.c b/redist/linmath.c index eefcd5f..0e06156 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -82,6 +82,25 @@ FLT anglebetween3d( FLT * a, FLT * b ) return FLT_ACOS(dot); } +// algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/ +void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) +{ + FLT s = FLT_SIN(angle); + FLT c = FLT_COS(angle); + + FLT u=axis[0]; + FLT v=axis[1]; + FLT w=axis[2]; + + FLT x=invec3[0]; + FLT y=invec3[1]; + FLT z=invec3[2]; + + outvec3[0] = u*(u*x + v*y + w*z)*(1-c) + x*c + (-w*y + v*z)*s; + outvec3[1] = v*(u*x + v*y + w*z)*(1-c) + y*c + ( w*x - u*z)*s; + outvec3[2] = w*(u*x + v*y + w*z)*(1-c) + z*c + (-v*x + u*y)*s; +} + /////////////////////////////////////QUATERNIONS////////////////////////////////////////// //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) //Under the mit/X11 license. diff --git a/redist/linmath.h b/redist/linmath.h index 4e0cb77..6f0bf60 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -70,6 +70,7 @@ FLT magnitude3d(const FLT * a ); FLT anglebetween3d( FLT * a, FLT * b ); +void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle); //Quaternion things... void quatsetnone( FLT * q ); -- cgit v1.2.3 From 9ead7de95621f1d7d59fed26fc7431344fdd9db4 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 13:58:04 -0700 Subject: Something is converging. --- src/poser_turveytori.c | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 15961c8..824fabb 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -646,7 +646,8 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) FLT sensor_in_lh_reference_frame[3]; sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); - quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + //quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. @@ -666,7 +667,7 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) fitness = FLT_SQRT(fitness); - return fitness; + return 1/fitness; } void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) @@ -693,10 +694,18 @@ void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, Track return; } +//void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) +//{ +// quatnormalize(vectorToScale, vectorToScale); +// quatscale(vectorToScale, vectorToScale, desiredMagnitude); +// return; +//} void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagnitude) { quatnormalize(vectorToScale, vectorToScale); quatscale(vectorToScale, vectorToScale, desiredMagnitude); + //vectorToScale[3] = desiredMagnitude; + return; } @@ -723,8 +732,8 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim for (FLT g = 0.2; g > 0.00001; g *= 0.99) { i++; - FLT point1[3]; - copy3d(point1, rotOut); + FLT point1[4]; + quatcopy(point1, rotOut); // let's get 3 iterations of gradient descent here. FLT gradient1[4]; @@ -733,7 +742,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point2[4]; quatadd(point2, gradient1, point1); - quatnormalize(point2,point2); + //quatnormalize(point2,point2); FLT gradient2[4]; getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); @@ -741,7 +750,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point3[4]; quatadd(point3, gradient2, point2); - quatnormalize(point3,point3); + //quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic @@ -760,7 +769,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point4[4]; quatadd(point4, specialGradient, point3); - quatnormalize(point4,point4); + //quatnormalize(point4,point4); FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); @@ -770,13 +779,13 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+"); + printf("+ %8.8f, %f\n", newMatchFitness, point4[3]); //#endif } else { //#ifdef TORI_DEBUG - printf("-"); + printf("- , %f\n", point4[3]); //#endif g *= 0.7; @@ -794,7 +803,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 ,0}; + FLT zAxis[4] = { 0, 0, 1 , theta}; //FLT quat1[4]; //quatfromaxisangle(quat1, zAxis, theta); // not correcting for phi, but that's less important. -- cgit v1.2.3 From e5c15af3af93356bb056624726e0b6068354690f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 14:20:29 -0700 Subject: Getting very close --- redist/linmath.c | 3 +++ src/poser_turveytori.c | 17 ++++++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/redist/linmath.c b/redist/linmath.c index 0e06156..69a70f6 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -85,6 +85,9 @@ FLT anglebetween3d( FLT * a, FLT * b ) // algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/ void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) { + // TODO: this really should be external. + normalize3d(axis, axis); + FLT s = FLT_SIN(angle); FLT c = FLT_COS(angle); diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 824fabb..d737723 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -779,8 +779,10 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+ %8.8f, %f\n", newMatchFitness, point4[3]); + printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif + g *= 1.03; + } else { @@ -796,6 +798,17 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim printf("\nRi=%d\n", i); } +static void WhereIsTheTrackedObject(FLT *rotation, Point lhPoint) +{ + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + + rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + + printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +} + + void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) { @@ -812,6 +825,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 2, optimize the quaternion to match the data. RefineRotationEstimate(rotOut, lh, zAxis, obj); + WhereIsTheTrackedObject(rotOut, lh); + } -- cgit v1.2.3 From e557b5128a32c99dcda0e6a9784a507258342301 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 14:36:54 -0700 Subject: Small tweak. Using axis/angle --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d737723..62deccf 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -729,7 +729,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. - for (FLT g = 0.2; g > 0.00001; g *= 0.99) + for (FLT g = 0.2; g > 0.000000001; g *= 0.99) { i++; FLT point1[4]; @@ -781,7 +781,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim //#ifdef TORI_DEBUG printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif - g *= 1.03; + g *= 1.02; } else -- cgit v1.2.3 From f86a7a9927c484f46768b14a5944a03863beee34 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 15:30:54 -0700 Subject: sucky angle estimators (quat & angle axis) --- src/poser_turveytori.c | 265 ++++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 242 insertions(+), 23 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 62deccf..da64850 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -602,7 +602,7 @@ FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *ob // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. -FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) +FLT RotationEstimateFitnessAxisAngle(Point lhPoint, FLT *quaternion, TrackedObject *obj) { FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) @@ -670,26 +670,121 @@ FLT RotationEstimateFitness(Point lhPoint, FLT *quaternion, TrackedObject *obj) return 1/fitness; } -void getRotationGradient(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +// interesting-- this is one place where we could use any sensors that are only hit by +// just an x or y axis to make our estimate better. TODO: bring that data to this fn. +FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj) { + FLT fitness = 0; + for (size_t i = 0; i < obj->numSensors; i++) + { + // first, get the normal of the plane for the horizonal sweep + FLT theta = obj->sensor[i].theta; + // make two vectors that lie on the plane + FLT t1H[3] = { 1, tan(theta-LINMATHPI/2), 0 }; + FLT t2H[3] = { 1, tan(theta-LINMATHPI/2), 1 }; + + FLT tNormH[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormH, t1H, t2H); + + normalize3d(tNormH, tNormH); + + // Now do the same for the vertical sweep + + // first, get the normal of the plane for the horizonal sweep + FLT phi = obj->sensor[i].phi; + // make two vectors that lie on the plane + FLT t1V[3] = { 0, 1, tan(phi-LINMATHPI/2)}; + FLT t2V[3] = { 1, 1, tan(phi-LINMATHPI/2)}; + + FLT tNormV[3]; + + // the normal is the cross of two vectors on the plane. + cross3d(tNormV, t1V, t2V); + + normalize3d(tNormV, tNormV); + + + // First, where is the sensor in the object's reference frame? + FLT sensor_in_obj_reference_frame[3] = {obj->sensor->point.x, obj->sensor->point.y, obj->sensor->point.z}; + // Where is the point, in the reference frame of the lighthouse? + // This has two steps, first we translate from the object's location being the + // origin to the lighthouse being the origin. + // And second, we apply the quaternion to rotate into the proper reference frame for the lighthouse. + + FLT sensor_in_lh_reference_frame[3]; + sub3d(sensor_in_lh_reference_frame, sensor_in_obj_reference_frame, (FLT[3]){lhPoint.x, lhPoint.y, lhPoint.z}); + + quatrotatevector(sensor_in_lh_reference_frame, quaternion, sensor_in_lh_reference_frame); + //rotatearoundaxis(sensor_in_lh_reference_frame, sensor_in_lh_reference_frame, quaternion, quaternion[3]); - FLT baseFitness = RotationEstimateFitness(lhPoint, quaternion, obj); + // now the we've got the location of the sensor in the lighthouses's reference frame, given lhPoint and quaternion inputs. + + // We need an arbitrary vector from the plane to the point. + // Since the plane goes through the origin, this is trivial. + // The sensor point itself is such a vector! + + // And go calculate the distances! + // TODO: don't need to ABS these because we square them below. + FLT dH = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormH)); + FLT dV = FLT_FABS(dot3d(sensor_in_lh_reference_frame, tNormV)); + + + fitness += SQUARED(dH); + fitness += SQUARED(dV); + } + + fitness = FLT_SQRT(fitness); + + return 1/fitness; +} + + +void getRotationGradientQuaternion(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitnessQuaternion(lhPoint, quaternion, obj); FLT tmp0plus[4]; quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); - gradientOut[0] = RotationEstimateFitness(lhPoint, tmp0plus, obj) - baseFitness; + gradientOut[0] = RotationEstimateFitnessQuaternion(lhPoint, tmp0plus, obj) - baseFitness; FLT tmp1plus[4]; quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); - gradientOut[1] = RotationEstimateFitness(lhPoint, tmp1plus, obj) - baseFitness; + gradientOut[1] = RotationEstimateFitnessQuaternion(lhPoint, tmp1plus, obj) - baseFitness; FLT tmp2plus[4]; quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); - gradientOut[2] = RotationEstimateFitness(lhPoint, tmp2plus, obj) - baseFitness; + gradientOut[2] = RotationEstimateFitnessQuaternion(lhPoint, tmp2plus, obj) - baseFitness; FLT tmp3plus[4]; quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); - gradientOut[3] = RotationEstimateFitness(lhPoint, tmp3plus, obj) - baseFitness; + gradientOut[3] = RotationEstimateFitnessQuaternion(lhPoint, tmp3plus, obj) - baseFitness; + + return; +} + +void getRotationGradientAxisAngle(FLT *gradientOut, Point lhPoint, FLT *quaternion, TrackedObject *obj, FLT precision) +{ + + FLT baseFitness = RotationEstimateFitnessAxisAngle(lhPoint, quaternion, obj); + + FLT tmp0plus[4]; + quatadd(tmp0plus, quaternion, (FLT[4]){precision, 0, 0, 0}); + gradientOut[0] = RotationEstimateFitnessAxisAngle(lhPoint, tmp0plus, obj) - baseFitness; + + FLT tmp1plus[4]; + quatadd(tmp1plus, quaternion, (FLT[4]){0, precision, 0, 0}); + gradientOut[1] = RotationEstimateFitnessAxisAngle(lhPoint, tmp1plus, obj) - baseFitness; + + FLT tmp2plus[4]; + quatadd(tmp2plus, quaternion, (FLT[4]){0, 0, precision, 0}); + gradientOut[2] = RotationEstimateFitnessAxisAngle(lhPoint, tmp2plus, obj) - baseFitness; + + FLT tmp3plus[4]; + quatadd(tmp3plus, quaternion, (FLT[4]){0, 0, 0, precision}); + gradientOut[3] = RotationEstimateFitnessAxisAngle(lhPoint, tmp3plus, obj) - baseFitness; return; } @@ -709,10 +804,20 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni return; } -static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) +static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) +{ + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + + rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + + printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +} + +static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) { int i = 0; - FLT lastMatchFitness = RotationEstimateFitness(lhPoint, initialEstimate, obj); + FLT lastMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, initialEstimate, obj); quatcopy(rotOut, initialEstimate); @@ -729,7 +834,7 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. - for (FLT g = 0.2; g > 0.000000001; g *= 0.99) + for (FLT g = 0.1; g > 0.000000001; g *= 0.99) { i++; FLT point1[4]; @@ -737,19 +842,26 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim // let's get 3 iterations of gradient descent here. FLT gradient1[4]; - getRotationGradient(gradient1, lhPoint, point1, obj, g/1000); + normalize3d(point1, point1); + + getRotationGradientAxisAngle(gradient1, lhPoint, point1, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient1,g); FLT point2[4]; quatadd(point2, gradient1, point1); //quatnormalize(point2,point2); + normalize3d(point1, point1); + FLT gradient2[4]; - getRotationGradient(gradient2, lhPoint, point2, obj, g/1000); + getRotationGradientAxisAngle(gradient2, lhPoint, point2, obj, g/10000); getNormalizedAndScaledRotationGradient(gradient2,g); FLT point3[4]; quatadd(point3, gradient2, point2); + + normalize3d(point1, point1); + //quatnormalize(point3,point3); // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? @@ -770,8 +882,9 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim FLT point4[4]; quatadd(point4, specialGradient, point3); //quatnormalize(point4,point4); + normalize3d(point1, point1); - FLT newMatchFitness = RotationEstimateFitness(lhPoint, point4, obj); + FLT newMatchFitness = RotationEstimateFitnessAxisAngle(lhPoint, point4, obj); if (newMatchFitness > lastMatchFitness) { @@ -797,18 +910,117 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim } printf("\nRi=%d\n", i); } - -static void WhereIsTheTrackedObject(FLT *rotation, Point lhPoint) +static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; - rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - + //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + quatrotatevector(objPoint, rotation, objPoint); printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } + +static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) +{ + int i = 0; + FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj); + + quatcopy(rotOut, initialEstimate); + + // The values below are somewhat magic, and definitely tunable + // The initial vlue of g will represent the biggest step that the gradient descent can take at first. + // bigger values may be faster, especially when the initial guess is wildly off. + // The downside to a bigger starting guess is that if we've picked a good guess at the local minima + // if there are other local minima, we may accidentally jump to such a local minima and get stuck there. + // That's fairly unlikely with the lighthouse problem, from expereince. + // The other downside is that if it's too big, we may have to spend a few iterations before it gets down + // to a size that doesn't jump us out of our minima. + // The terminal value of g represents how close we want to get to the local minima before we're "done" + // The change in value of g for each iteration is intentionally very close to 1. + // in fact, it probably could probably be 1 without any issue. The main place where g is decremented + // is in the block below when we've made a jump that results in a worse fitness than we're starting at. + // In those cases, we don't take the jump, and instead lower the value of g and try again. + for (FLT g = 0.1; g > 0.000000001; g *= 0.99) + { + i++; + FLT point1[4]; + quatcopy(point1, rotOut); + // let's get 3 iterations of gradient descent here. + FLT gradient1[4]; + + //normalize3d(point1, point1); + + getRotationGradientQuaternion(gradient1, lhPoint, point1, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient1,g); + + FLT point2[4]; + quatadd(point2, gradient1, point1); + quatnormalize(point2,point2); + + //normalize3d(point1, point1); + + FLT gradient2[4]; + getRotationGradientQuaternion(gradient2, lhPoint, point2, obj, g/10000); + getNormalizedAndScaledRotationGradient(gradient2,g); + + FLT point3[4]; + quatadd(point3, gradient2, point2); + + //normalize3d(point1, point1); + + quatnormalize(point3,point3); + + // remember that gradient descent has a tendency to zig-zag when it encounters a narrow valley? + // Well, solving the lighthouse problem presents a very narrow valley, and the zig-zag of a basic + // gradient descent is kinda horrible here. Instead, think about the shape that a zig-zagging + // converging gradient descent makes. Instead of using the gradient as the best indicator of + // the direction we should follow, we're looking at one side of the zig-zag pattern, and specifically + // following *that* vector. As it turns out, this works *amazingly* well. + + FLT specialGradient[4]; + quatsub(specialGradient,point3,point1); + + // The second parameter to this function is very much a tunable parameter. Different values will result + // in a different number of iterations before we get to the minimum. Numbers between 3-10 seem to work well + // It's not clear what would be optimum here. + getNormalizedAndScaledRotationGradient(specialGradient,g/4); + + FLT point4[4]; + quatadd(point4, specialGradient, point3); + quatnormalize(point4,point4); + //normalize3d(point1, point1); + + FLT newMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, point4, obj); + + if (newMatchFitness > lastMatchFitness) + { + + lastMatchFitness = newMatchFitness; + quatcopy(rotOut, point4); +//#ifdef TORI_DEBUG + //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); +//#endif + g *= 1.02; + printf("+"); + WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint); + } + else + { +//#ifdef TORI_DEBUG + //printf("- , %f\n", point4[3]); +//#endif + g *= 0.7; + printf("-"); + } + + + } + printf("\nRi=%d Fitness=%3f\n", i, lastMatchFitness); +} + + void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) { @@ -816,16 +1028,23 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // This should have the lighthouse directly facing the tracked object. Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z }; FLT theta = atan2(-lh.x, -lh.y); - FLT zAxis[4] = { 0, 0, 1 , theta}; - //FLT quat1[4]; - //quatfromaxisangle(quat1, zAxis, theta); + FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2}; + FLT quat1[4]; + quatfromaxisangle(quat1, zAxis, theta); + + //quatfrom2vectors(0,0) // not correcting for phi, but that's less important. - // Step 2, optimize the quaternion to match the data. - RefineRotationEstimate(rotOut, lh, zAxis, obj); + // Step 2, optimize the axis/ angle to match the data. + RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); + + WhereIsTheTrackedObjectAxisAngle(rotOut, lh); + + //// Step 2, optimize the quaternion to match the data. + //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); - WhereIsTheTrackedObject(rotOut, lh); + //WhereIsTheTrackedObjectQuaternion(rotOut, lh); } -- cgit v1.2.3 From d8c4b23789fd3aedb150144bed6beb286d1504a9 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 16:21:52 -0700 Subject: Reliable detection of orientation of lighthouse Lots of cruft. Lots of room for improving performance. --- src/poser_turveytori.c | 44 +++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index da64850..5c3350b 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -600,9 +600,47 @@ FLT RotationEstimateFitnessOld(Point lhPoint, FLT *quaternion, TrackedObject *ob return fitness; } +FLT RotationEstimateFitnessAxisAngle(Point lh, FLT *AxisAngle, TrackedObject *obj) +{ + // For this fitness calculator, we're going to use the rotation information to figure out where + // we expect to see the tracked object sensors, and we'll do a sum of squares to grade + // the quality of the guess formed by the AxisAngle; + + FLT fitness = 0; + + // for each point in the tracked object + for (int i=0; i< obj->numSensors; i++) + { + + + + // let's see... we need to figure out where this sensor should be in the LH reference frame. + FLT sensorLocation[3] = {obj->sensor[i].point.x-lh.x, obj->sensor[i].point.y-lh.y, obj->sensor[i].point.z-lh.z}; + + // And this puppy needs to be rotated... + + rotatearoundaxis(sensorLocation, sensorLocation, AxisAngle, AxisAngle[3]); + + // Now, the vector indicating the position of the sensor, as seen by the lighthouse is: + FLT realVectFromLh[3] = {1, tan(obj->sensor[i].theta - LINMATHPI/2), tan(obj->sensor[i].phi - LINMATHPI/2)}; + + // and the vector we're calculating given the rotation passed in is the same as the sensor location: + FLT calcVectFromLh[3] = {sensorLocation[0], sensorLocation[1], sensorLocation[2]}; + + FLT angleBetween = anglebetween3d( realVectFromLh, calcVectFromLh ); + + fitness += SQUARED(angleBetween); + } + + return 1/FLT_SQRT(fitness); +} + +// This figures out how far away from the scanned planes each point is, then does a sum of squares +// for the fitness. +// // interesting-- this is one place where we could use any sensors that are only hit by // just an x or y axis to make our estimate better. TODO: bring that data to this fn. -FLT RotationEstimateFitnessAxisAngle(Point lhPoint, FLT *quaternion, TrackedObject *obj) +FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, TrackedObject *obj) { FLT fitness = 0; for (size_t i = 0; i < obj->numSensors; i++) @@ -892,7 +930,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); + //printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif g *= 1.02; @@ -900,7 +938,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini else { //#ifdef TORI_DEBUG - printf("- , %f\n", point4[3]); + //printf("- , %f\n", point4[3]); //#endif g *= 0.7; -- cgit v1.2.3 From 788816a1c12499da9e419bde1978c056fe48cd4b Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 16:36:39 -0700 Subject: Adding sample config file --- useful_files/sample.config.json | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 useful_files/sample.config.json diff --git a/useful_files/sample.config.json b/useful_files/sample.config.json new file mode 100644 index 0000000..20a8f0c --- /dev/null +++ b/useful_files/sample.config.json @@ -0,0 +1,8 @@ +"_Comment0":"This file must be named config.json and placed in the same directory as the executable" +"_Comment1":"Valid Posers Are: PoserCharlesSlow, PoserDaveOrtho, PoserDummy, PoserOctavioRadii, PoserTurveyTori", +"DefaultPoser":"PoserTurveyTori", +"ConfigPoser":"PoserTurveyTori" +"_Comment2":"RequiredTrackersForCal takes a comma separated list of devices required for calibration to pass. Valid options are: HMD,WM0,WM1,TR0,WW0" +"RequiredTrackersForCal":"", +"_Comment3":"If set, AllowAllTrackersForCal will use all trackers for calibration. Otherwise only required trackers will be used." +"AllowAllTrackersForCal":"1", -- cgit v1.2.3 From 94795a17d2d108942d0326e95a1e59e61ab73340 Mon Sep 17 00:00:00 2001 From: Michael Turvey Date: Mon, 27 Mar 2017 16:39:14 -0700 Subject: Update sample.config.json --- useful_files/sample.config.json | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/useful_files/sample.config.json b/useful_files/sample.config.json index 20a8f0c..e12594a 100644 --- a/useful_files/sample.config.json +++ b/useful_files/sample.config.json @@ -1,8 +1,8 @@ -"_Comment0":"This file must be named config.json and placed in the same directory as the executable" +"_Comment0":"This file must be named config.json and placed in the same directory as the executable", "_Comment1":"Valid Posers Are: PoserCharlesSlow, PoserDaveOrtho, PoserDummy, PoserOctavioRadii, PoserTurveyTori", "DefaultPoser":"PoserTurveyTori", -"ConfigPoser":"PoserTurveyTori" -"_Comment2":"RequiredTrackersForCal takes a comma separated list of devices required for calibration to pass. Valid options are: HMD,WM0,WM1,TR0,WW0" +"ConfigPoser":"PoserTurveyTori", +"_Comment2":"RequiredTrackersForCal takes a comma separated list of devices required for calibration to pass. Valid options are: HMD,WM0,WM1,TR0,WW0", "RequiredTrackersForCal":"", -"_Comment3":"If set, AllowAllTrackersForCal will use all trackers for calibration. Otherwise only required trackers will be used." +"_Comment3":"If set, AllowAllTrackersForCal will use all trackers for calibration. Otherwise only required trackers will be used.", "AllowAllTrackersForCal":"1", -- cgit v1.2.3 From 6469d979046bf40c4ee76b89d3fc8311dcb16d33 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Mon, 27 Mar 2017 23:17:05 -0700 Subject: Use IMU to orient world coordinate system --- src/poser_turveytori.c | 80 ++++++++++++++++++++++++++++++++++---------------- 1 file changed, 54 insertions(+), 26 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5c3350b..5db2bc4 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -71,6 +71,7 @@ static const float DefaultPointsPerOuterDiameter = 60; typedef struct { + FLT down[3]; // populated by the IMU for posing int something; //Stuff } ToriData; @@ -1194,23 +1195,32 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) -int PoserTurveyTori( SurviveObject * so, PoserData * pd ) +int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) { - PoserType pt = pd->pt; + PoserType pt = poserData->pt; SurviveContext * ctx = so->ctx; - ToriData * dd = so->PoserData; + ToriData * pd = so->PoserData; - if (!dd) + + if (!pd) { - so->PoserData = dd = malloc(sizeof(ToriData)); - memset(dd, 0, sizeof(ToriData)); + so->PoserData = pd = malloc(sizeof(ToriData)); + memset(pd, 0, sizeof(ToriData)); } switch( pt ) { case POSERDATA_IMU: { - PoserDataIMU * imu = (PoserDataIMU*)pd; + PoserDataIMU * tmpImu = (PoserDataIMU*)pd; + + // store off data we can use for figuring out what direction is down when doing calibration. + if (tmpImu->datamask & 1) // accelerometer data is present + { + pd->down[0] = pd->down[0] * 0.98 + 0.2 * tmpImu->accel[0]; + pd->down[1] = pd->down[1] * 0.98 + 0.2 * tmpImu->accel[1]; + pd->down[2] = pd->down[2] * 0.98 + 0.2 * tmpImu->accel[2]; + } //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); break; } @@ -1228,30 +1238,40 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); - //FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; - //FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) - //FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + // if we rotate the internal reference frame of of the tracked object from having -z being arbitrary + // to being the down direction as defined by the accelerometer, then when we have come up + // with world coordinate system, it will have Z oriented correctly. + + // let's get the quaternion that represents this rotation. + FLT downQuat[4]; + FLT negZ[3] = { 0,0,-1 }; + quatfrom2vectors(downQuat, negZ, pd->down); - //to->numSensors = so->nr_locations; { int sensorCount = 0; + for (int i = 0; i < so->nr_locations; i++) { if (fs->lengths[i][0][0] != -1 && fs->lengths[i][0][1] != -1) //lh 0 { - to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; - to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; - to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; - to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; - to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; - to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; } } - to->numSensors = sensorCount; SolveForLighthouse(to, 0); @@ -1264,12 +1284,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) { if (fs->lengths[i][lh][0] != -1 && fs->lengths[i][lh][1] != -1) { - to->sensor[sensorCount].normal.x = so->sensor_normals[i * 3 + 0]; - to->sensor[sensorCount].normal.y = so->sensor_normals[i * 3 + 1]; - to->sensor[sensorCount].normal.z = so->sensor_normals[i * 3 + 2]; - to->sensor[sensorCount].point.x = so->sensor_locations[i * 3 + 0]; - to->sensor[sensorCount].point.y = so->sensor_locations[i * 3 + 1]; - to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + quatrotatevector(norm, downQuat, norm); + quatrotatevector(point, downQuat, point); + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) sensorCount++; @@ -1280,13 +1306,15 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) SolveForLighthouse(to, 0); } + + free(to); //printf( "Full scene data.\n" ); break; } case POSERDATA_DISASSOCIATE: { - free( dd ); - so->PoserData = 0; + free( pd ); + so->PoserData = NULL; //printf( "Need to disassociate.\n" ); break; } -- cgit v1.2.3 From b3bdcdb838ed57986f359b2181a624bfd1acbbf1 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 28 Mar 2017 10:15:36 -0700 Subject: Fix Tracker IMU --- src/poser_turveytori.c | 2 +- src/survive_data.c | 10 ++++++++-- src/survive_vive.c | 17 +---------------- 3 files changed, 10 insertions(+), 19 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5c3350b..df691e3 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1211,7 +1211,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * pd ) case POSERDATA_IMU: { PoserDataIMU * imu = (PoserDataIMU*)pd; - //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); break; } case POSERDATA_LIGHT: diff --git a/src/survive_data.c b/src/survive_data.c index 9447104..3eb5890 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -154,8 +154,14 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) { if (lcd->per_sweep.activeLighthouse != -1) { - // hmm, it appears we got two non-skip pulses at the same time. That should never happen - fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + static int pulseWarningCount=0; + + if (pulseWarningCount < 5) + { + pulseWarningCount++; + // hmm, it appears we got two non-skip pulses at the same time. That should never happen + fprintf(stderr, "WARNING: Two non-skip pulses received on the same cycle!\n"); + } } lcd->per_sweep.activeLighthouse = 1; lcd->per_sweep.activeSweepStartTime = le->timestamp; diff --git a/src/survive_vive.c b/src/survive_vive.c index a5c731d..757fa11 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1105,6 +1105,7 @@ void survive_data_cb( SurviveUSBInterface * si ) } case USB_IF_LIGHTHOUSE: case USB_IF_W_WATCHMAN1: + case USB_IF_TRACKER0: { int i; //printf( "%d -> ", size ); @@ -1156,22 +1157,6 @@ void survive_data_cb( SurviveUSBInterface * si ) } break; } - case USB_IF_TRACKER0: - { - SurviveObject * w = obj; - if( id == 32 ) - { - // TODO: Looks like this will need to be handle_tracker, since - // it appears the interface is sufficiently different. - // More work needd to reverse engineer it. - //handle_wired_watchman( w, readdata, size); - } - else - { - SV_INFO( "Unknown tracker code %d\n", id ); - } - break; - } case USB_IF_LIGHTCAP: { int i; -- cgit v1.2.3 From fba18d9de738fd07a0b6db944369127a6a66f0d8 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Tue, 28 Mar 2017 11:19:39 -0700 Subject: Add lh to poser light data --- include/libsurvive/poser.h | 3 ++- src/poser_turveytori.c | 38 ++++++++++++++++++++++++-------------- src/survive_process.c | 1 + 3 files changed, 27 insertions(+), 15 deletions(-) diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index cf11e0c..497b009 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -32,7 +32,8 @@ typedef struct { PoserType pt; int sensor_id; - int acode; //OOTX Code associated with this sweep. base_station = acode >> 2; axis = acode & 1; + int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep + int lh; //Lighthouse making this sweep uint32_t timecode; //In object-local ticks. FLT length; //In seconds FLT angle; //In radians from center of lighthouse. diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 177a16a..8b92860 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1199,42 +1199,51 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) { PoserType pt = poserData->pt; SurviveContext * ctx = so->ctx; - ToriData * pd = so->PoserData; + ToriData * td = so->PoserData; - if (!pd) + if (!td) { - so->PoserData = pd = malloc(sizeof(ToriData)); - memset(pd, 0, sizeof(ToriData)); + so->PoserData = td = malloc(sizeof(ToriData)); + memset(td, 0, sizeof(ToriData)); } switch( pt ) { case POSERDATA_IMU: { - PoserDataIMU * tmpImu = (PoserDataIMU*)pd; + PoserDataIMU * tmpImu = (PoserDataIMU*)poserData; // store off data we can use for figuring out what direction is down when doing calibration. - if (tmpImu->datamask & 1) // accelerometer data is present + //TODO: looks like the data mask isn't getting set correctly. + //if (tmpImu->datamask & 1) // accelerometer data is present { - pd->down[0] = pd->down[0] * 0.98 + 0.2 * tmpImu->accel[0]; - pd->down[1] = pd->down[1] * 0.98 + 0.2 * tmpImu->accel[1]; - pd->down[2] = pd->down[2] * 0.98 + 0.2 * tmpImu->accel[2]; + td->down[0] = td->down[0] * 0.98 + 0.02 * tmpImu->accel[0]; + td->down[1] = td->down[1] * 0.98 + 0.02 * tmpImu->accel[1]; + td->down[2] = td->down[2] * 0.98 + 0.02 * tmpImu->accel[2]; } - printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, imu->accel[0], imu->accel[1], imu->accel[2], imu->gyro[0], imu->gyro[1], imu->gyro[2] ); + //printf( "IMU:%s (%f %f %f) (%f %f %f)\n", so->codename, tmpImu->accel[0], tmpImu->accel[1], tmpImu->accel[2], tmpImu->gyro[0], tmpImu->gyro[1], tmpImu->gyro[2] ); + //printf( "Down: (%f %f %f)\n", td->down[0], td->down[1], td->down[2] ); break; } case POSERDATA_LIGHT: { - PoserDataLight * l = (PoserDataLight*)pd; + PoserDataLight * l = (PoserDataLight*)poserData; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + if (0 == l->lh) + { + if (l->acode & 0x1) + { + printf("%2d: %8f\n", l->sensor_id, l->angle); + } + } break; } case POSERDATA_FULL_SCENE: { TrackedObject *to; - PoserDataFullScene * fs = (PoserDataFullScene*)pd; + PoserDataFullScene * fs = (PoserDataFullScene*)poserData; to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); @@ -1245,7 +1254,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's get the quaternion that represents this rotation. FLT downQuat[4]; FLT negZ[3] = { 0,0,-1 }; - quatfrom2vectors(downQuat, negZ, pd->down); + //quatfrom2vectors(downQuat, negZ, td->down); + quatfrom2vectors(downQuat, td->down, negZ); { int sensorCount = 0; @@ -1313,7 +1323,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } case POSERDATA_DISASSOCIATE: { - free( pd ); + free( so->PoserData ); so->PoserData = NULL; //printf( "Need to disassociate.\n" ); break; diff --git a/src/survive_process.c b/src/survive_process.c index b58b344..3af2da9 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -57,6 +57,7 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode .timecode = timecode, .length = length, .angle = angle, + .lh = lh, }; so->PoserFn( so, (PoserData *)&l ); } -- cgit v1.2.3 From 3ca8ba3d69de226ae8835bb29e45c7bcd35793fe Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:39:47 -0700 Subject: Tori Poser Works! There's a ton of code cruft, and the algorithm is currently too slow. BUT I can track an object using only 1 lighthouse for tracking, at (I believe) an update rate of at least 7.5 HZ. By tracking, I know the position and orientation of the lighthouses relative to the tracked object, and I know the tracked object's location relative to the lighthouse. I don't have the orientation of the tracked object relative to the lighthouse yet, but that should be easy given the rest of the "knowns." --- src/poser_octavioradii.c | 198 +++++++++++++++++++++++++++++++++++++++++++++-- src/poser_turveytori.c | 174 +++++++++++++++++++++++++++++++++++++---- src/survive_data.c | 37 ++++++++- 3 files changed, 381 insertions(+), 28 deletions(-) diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c index 3893085..0d8674c 100644 --- a/src/poser_octavioradii.c +++ b/src/poser_octavioradii.c @@ -4,8 +4,12 @@ typedef struct { - int something; - //Stuff +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; + + int hitCount[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; } OctavioRadiiData; #include @@ -45,6 +49,7 @@ typedef struct Point normal; // unit vector indicating the normal for the sensor double theta; // "horizontal" angular measurement from lighthouse radians double phi; // "vertical" angular measurement from lighthouse in radians. + int id; } TrackedSensor; typedef struct @@ -382,10 +387,10 @@ static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *a } - printf("\ni=%d\n", i); + printf(" i=%d ", i); } -void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) +static void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObject *obj) { FLT estimate[MAX_RADII]; @@ -394,6 +399,12 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj estimate[i] = 2.38; } + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("%d, ", obj->sensor[i].id); + //} + SensorAngles angles[MAX_RADII]; PointPair pairs[MAX_POINT_PAIRS]; @@ -423,22 +434,119 @@ void SolveForLighthouseRadii(Point *objPosition, FLT *objOrientation, TrackedObj // we should now have an estimate of the radii. - for (int i = 0; i < obj->numSensors; i++) + //for (int i = 0; i < obj->numSensors; i++) + for (int i = 0; i < 1; i++) { printf("radius[%d]: %f\n", i, estimate[i]); } + // (FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile) return; } +static void QuickPose(SurviveObject *so) +{ + OctavioRadiiData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // //FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // //FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + //printf("%d[%d], ",i,td->hitCount[i][lh][0]); + + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if ((td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0)) + + + { + if (td->hitCount[i][lh][0] > 10 && td->hitCount[i][lh][1] > 10) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; + + + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + } + //printf("\n"); + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + FLT pos[3]; + FLT orient[4]; + SolveForLighthouseRadii(pos, orient, to); + } + + + } + + + free(to); + +} + + int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) { PoserType pt = pd->pt; SurviveContext * ctx = so->ctx; OctavioRadiiData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + if( !dd ) + { + so->PoserData = dd = malloc( sizeof(OctavioRadiiData) ); + memset(dd, 0, sizeof(OctavioRadiiData)); + } + switch( pt ) { @@ -451,9 +559,81 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)pd; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; + + //printf("%d ", l->sensor_id); + + //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); - break; - } + if ((dd->lastAxis[l->lh] != (l->acode & 0x1)) ) + { + int lastAxis = dd->lastAxis[l->lh]; + //printf("\n"); + //if (0 == l->lh) + // printf("or[%d,%d] ", l->lh,lastAxis); + + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int index = dd->angleIndex[l->lh][axis]; + if (dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] != 0) + { + //if (0 == l->lh) + // printf("%d ", i); + + dd->hitCount[i][l->lh][axis]++; + } + else + { + dd->hitCount[i][l->lh][axis] *= 0.5; + } + } + //if (0 == l->lh) + // printf("\n"); + //int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) + { + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + + + dd->angleIndex[l->lh][axis]++; + dd->angleIndex[l->lh][axis] = dd->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + dd->oldAngles[i][axis][l->lh][dd->angleIndex[l->lh][axis]] = 0; + } + + } + dd->lastAxis[l->lh] = axis; + } + + //if (0 == l->lh) + // printf("(%d) ", l->sensor_id); + + //FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + dd->oldAngles[l->sensor_id][axis][l->lh][dd->angleIndex[l->lh][axis]] = l->angle; + break; } case POSERDATA_FULL_SCENE: { TrackedObject *to; @@ -482,6 +662,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; sensorCount++; } } @@ -509,6 +690,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd ) to->sensor[sensorCount].point.z = so->sensor_locations[i * 3 + 2]; to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].id=i; sensorCount++; } } diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 8b92860..22098d0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -74,6 +74,11 @@ typedef struct FLT down[3]; // populated by the IMU for posing int something; //Stuff + +#define OLD_ANGLES_BUFF_LEN 3 + FLT oldAngles[SENSORS_PER_OBJECT][2][NUM_LIGHTHOUSES][OLD_ANGLES_BUFF_LEN]; // sensor, sweep axis, lighthouse, instance + int angleIndex[NUM_LIGHTHOUSES][2]; // index into circular buffer ahead. separate index for each axis. + int lastAxis[NUM_LIGHTHOUSES]; } ToriData; @@ -526,7 +531,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } - printf("\ni=%d\n", i); + printf(" i=%d ", i); return lastPoint; } @@ -845,12 +850,12 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], rotation[3]}; FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + printf("{%8.8f, %8.8f, %8.8f} ", objPoint[0], objPoint[1], objPoint[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -873,7 +878,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini // in fact, it probably could probably be 1 without any issue. The main place where g is decremented // is in the block below when we've made a jump that results in a worse fitness than we're starting at. // In those cases, we don't take the jump, and instead lower the value of g and try again. - for (FLT g = 0.1; g > 0.000000001; g *= 0.99) + for (FLT g = 0.1; g > 0.000000001 || i > 10000; g *= 0.99) { i++; FLT point1[4]; @@ -947,7 +952,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - printf("\nRi=%d\n", i); + printf(" Ri=%d ", i); } static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) { @@ -956,7 +961,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint) //rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); quatrotatevector(objPoint, rotation, objPoint); - printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); + printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); } @@ -1056,7 +1061,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in } - printf("\nRi=%d Fitness=%3f\n", i, lastMatchFitness); + printf("Ri=%3d Fitness=%3f ", i, lastMatchFitness); } @@ -1088,8 +1093,23 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } -Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) { + //printf("Solving for Lighthouse\n"); + + //printf("obj->numSensors = %d;\n", obj->numSensors); + + //for (int i=0; i < obj->numSensors; i++) + //{ + // printf("obj->sensor[%d].normal.x = %f;\n", i, obj->sensor[i].normal.x); + // printf("obj->sensor[%d].normal.y = %f;\n", i, obj->sensor[i].normal.y); + // printf("obj->sensor[%d].normal.z = %f;\n", i, obj->sensor[i].normal.z); + // printf("obj->sensor[%d].point.x = %f;\n", i, obj->sensor[i].point.x); + // printf("obj->sensor[%d].point.y = %f;\n", i, obj->sensor[i].point.y); + // printf("obj->sensor[%d].point.z = %f;\n", i, obj->sensor[i].point.z); + // printf("obj->sensor[%d].phi = %f;\n", i, obj->sensor[i].phi); + // printf("obj->sensor[%d].theta = %f;\n\n", i, obj->sensor[i].theta); + //} PointsAndAngle pna[MAX_POINT_PAIRS]; volatile size_t sizeNeeded = sizeof(pna); @@ -1168,13 +1188,17 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); - FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf("(%4.4f, %4.4f, %4.4f)\n", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z); - printf("Distance is %f, Fitness is %f\n", distance, fitGd); + printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - FLT rot[4]; - SolveForRotation(rot, obj, refinedEstimateGd); + if (fitGd > 5) + { + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); + //printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); + } if (logFile) { updateHeader(logFile); @@ -1191,7 +1215,82 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static void QuickPose(SurviveObject *so) +{ + ToriData * td = so->PoserData; + + + //for (int i=0; i < so->nr_locations; i++) + //{ + // FLT x0=td->oldAngles[i][0][0][td->angleIndex[0][0]]; + // FLT y0=td->oldAngles[i][1][0][td->angleIndex[0][1]]; + // FLT x1=td->oldAngles[i][0][1][td->angleIndex[1][0]]; + // FLT y1=td->oldAngles[i][1][1][td->angleIndex[1][1]]; + // //printf("%2d: %8.8f, %8.8f %8.8f, %8.8f \n", + // // i, + // // x0, + // // y0, + // // x1, + // // y1 + // // ); + // printf("%2d: %8.8f, %8.8f \n", + // i, + // x0, + // y0 + // ); + //} + //printf("\n"); + + TrackedObject *to; + + to = malloc(sizeof(TrackedObject) + (SENSORS_PER_OBJECT * sizeof(TrackedSensor))); + { + int sensorCount = 0; + + for (int i = 0; i < so->nr_locations; i++) + { + int lh = 0; + int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; + if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) + { + FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; + FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + + to->sensor[sensorCount].normal.x = norm[0]; + to->sensor[sensorCount].normal.y = norm[1]; + to->sensor[sensorCount].normal.z = norm[2]; + to->sensor[sensorCount].point.x = point[0]; + to->sensor[sensorCount].point.y = point[1]; + to->sensor[sensorCount].point.z = point[2]; + to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) + to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + + sensorCount++; + } + } + to->numSensors = sensorCount; + + if (sensorCount > 4) + { + SolveForLighthouse(to, 0); + printf("!\n"); + } + + + } + + + free(to); + +} @@ -1229,14 +1328,47 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) case POSERDATA_LIGHT: { PoserDataLight * l = (PoserDataLight*)poserData; + + if (l->lh >= NUM_LIGHTHOUSES || l->lh < 0) + { + // should never happen. Famous last words... + break; + } + int axis = l->acode & 0x1; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); - if (0 == l->lh) + if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { - if (l->acode & 0x1) + int foo = l->acode & 0x1; + //printf("%d", foo); + + + //if (axis) { - printf("%2d: %8f\n", l->sensor_id, l->angle); + if (0 == l->lh && axis) // only once per full cycle... + { + static unsigned int counter = 1; + + counter++; + + // let's just do this occasionally for now... + if (counter % 4 == 0) + QuickPose(so); + } + // axis changed, time to increment the circular buffer index. + td->angleIndex[l->lh][axis]++; + td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; + } + } + td->lastAxis[l->lh] = axis; } + + td->oldAngles[l->sensor_id][axis][l->lh][td->angleIndex[l->lh][axis]] = l->angle; break; } case POSERDATA_FULL_SCENE: @@ -1278,12 +1410,20 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].point.y = point[1]; to->sensor[sensorCount].point.z = point[2]; to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) - to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical) + to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) + + //printf("%2d: %8.8f, %8.8f \n", + // i, + // to->sensor[sensorCount].theta, + // to->sensor[sensorCount].phi + // ); + sensorCount++; } } to->numSensors = sensorCount; + SolveForLighthouse(to, 0); } { diff --git a/src/survive_data.c b/src/survive_data.c index 3eb5890..3f16c7c 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -79,10 +79,40 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) } } + int allZero = 1; + for (int q=0; q< 32; q++) + if (lcd->sweep.sweep_len[q] != 0) + allZero=0; + //if (!allZero) + // printf("a[%d]l[%d] ", lcd->per_sweep.activeAcode & 5, lcd->per_sweep.activeLighthouse); for (int i = 0; i < SENSORS_PER_OBJECT; i++) { + { + static int counts[SENSORS_PER_OBJECT][2] = {0}; + + if (lcd->per_sweep.activeLighthouse == 0 && !allZero) + { + if (lcd->sweep.sweep_len[i] != 0) + { + //printf("%d ", i); + //counts[i][lcd->per_sweep.activeAcode & 1] ++; + } + else + { + counts[i][lcd->per_sweep.activeAcode & 1] =0; + } + + //if (counts[i][0] > 10 && counts[i][1] > 10) + //{ + //printf("%d(%d,%d), ", i, counts[i][0], counts[i][1]); + //} + } + } + + if (lcd->sweep.sweep_len[i] != 0) // if the sensor was hit, process it { + int offset_from = lcd->sweep.sweep_time[i] - lcd->per_sweep.activeSweepStartTime + lcd->sweep.sweep_len[i] / 2; if (offset_from < 380000 && offset_from > 70000) @@ -94,6 +124,9 @@ void handle_lightcap2_process_sweep_data(SurviveObject *so) } } } + //if (!allZero) + // printf(" ..:..\n"); + } // clear out sweep data (could probably limit this to only after a "first" sync. // this is slightly more robust, so doing it here for now. @@ -108,6 +141,7 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) //static unsigned int recent_sync_count = -1; //static unsigned int activeSweepStartTime; + int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); // Process any sweep data we have handle_lightcap2_process_sweep_data(so); @@ -126,7 +160,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); if (!(acode >> 2 & 1)) // if the skip bit is not set { lcd->per_sweep.activeLighthouse = lcd->per_sweep.current_lh; @@ -148,8 +181,6 @@ void handle_lightcap2_sync(SurviveObject * so, LightcapElement * le ) lcd->per_sweep.lh_pulse_len[lcd->per_sweep.current_lh] = le->length; lcd->per_sweep.lh_start_time[lcd->per_sweep.current_lh] = le->timestamp; - int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); - if (!(acode >> 2 & 1)) // if the skip bit is not set { if (lcd->per_sweep.activeLighthouse != -1) -- cgit v1.2.3 From 9706cf9ee66134e5c37e8c860035d133b56dff66 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Wed, 29 Mar 2017 16:51:27 -0700 Subject: little bit o' spit 'n polish --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 22098d0..76dd7fe 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1190,7 +1190,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - if (fitGd > 5) + //if (fitGd > 5) { FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); @@ -1351,7 +1351,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 4 == 0) + if (counter % 1 == 0) QuickPose(so); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From a3837d79442e1845baf441b8f84b41da3e8d81bc Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 30 Mar 2017 11:07:39 -0700 Subject: Limit amount of time searching for best solution Sometimes the search algorithm would spend way too much time making a bunch of small improvements, when it would have been better to just stop and call it "good enough" --- src/poser_turveytori.c | 35 ++++++++++++++++++++++------------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 76dd7fe..47147b0 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -529,7 +529,16 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, } - + // from empiracle evidence, we're probably "good enough" at this point. + // So, even though we could still improve, we're likely to be improving + // very slowly, and we should just take what we've got and move on. + // This also seems to happen almost only when data is a little more "dirty" + // because the tracker is being rotated. + if (i > 120) + { + //printf("i got big"); + break; + } } printf(" i=%d ", i); @@ -950,7 +959,11 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - + if (i > 1000) + { + //printf("Ri got big"); + break; + } } printf(" Ri=%d ", i); } @@ -1188,18 +1201,14 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); - printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); + FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); + printf(" SensorCount(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + //printf("Distance is %f, Fitness is %f\n", distance, fitGd); - //if (fitGd > 5) - { - FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf("(%4.4f, %4.4f, %4.4f) Dist: %8.8f Fit:%4f ", refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance, fitGd); - //printf("Distance is %f, Fitness is %f\n", distance, fitGd); + FLT rot[4]; + SolveForRotation(rot, obj, refinedEstimateGd); - FLT rot[4]; - SolveForRotation(rot, obj, refinedEstimateGd); - } - if (logFile) + if (logFile) { updateHeader(logFile); fclose(logFile); @@ -1351,7 +1360,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) counter++; // let's just do this occasionally for now... - if (counter % 1 == 0) + if (counter % 2 == 0) QuickPose(so); } // axis changed, time to increment the circular buffer index. -- cgit v1.2.3 From 7ea900d3c0ce02bf5867e73dc2bbe3988207c477 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 30 Mar 2017 11:50:25 -0700 Subject: poser tweaks --- src/poser_turveytori.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 47147b0..5d25294 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -534,13 +534,13 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // very slowly, and we should just take what we've got and move on. // This also seems to happen almost only when data is a little more "dirty" // because the tracker is being rotated. - if (i > 120) + if (i > 900) { //printf("i got big"); break; } } - printf(" i=%d ", i); + printf(" i=%3d ", i); return lastPoint; } -- cgit v1.2.3 From c783f758751b9a64ca09b41e6417222a5e6cbff2 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 30 Mar 2017 22:56:19 -0700 Subject: Checking Smallest Angle --- src/poser_turveytori.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 5d25294..c2d7410 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1129,6 +1129,8 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) Point avgNorm = { 0 }; + FLT smallestAngle = 20.0; + size_t pnaCount = 0; for (unsigned int i = 0; i < obj->numSensors; i++) { @@ -1143,6 +1145,11 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) //pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]); pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle); + if (pna[pnaCount].angle < smallestAngle) + { + smallestAngle = pna[pnaCount].angle; + } + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); @@ -1202,7 +1209,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" SensorCount(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" sma(%d) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", smallestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From b562a8214508c74ed8094514816be012224abcd9 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 10:23:08 -0700 Subject: hacks that make tori more usable --- src/poser_turveytori.c | 68 ++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 57 insertions(+), 11 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index c2d7410..62cd12c 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -8,6 +8,11 @@ #include #include #include +#if defined(__FreeBSD__) || defined(__APPLE__) +#include +#else +#include //for alloca +#endif #define PointToFlts(x) ((FLT*)(x)) @@ -120,7 +125,8 @@ typedef struct FLT tanAngle; // tangent of angle Matrix3x3 rotation; Matrix3x3 invRotation; // inverse of rotation - + char ai; + char bi; } PointsAndAngle; @@ -378,21 +384,52 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna) return dist; } -FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount) +FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deubgPrint) { FLT fitness; FLT resultSum = 0; + FLT *fitnesses = alloca(sizeof(FLT) * pnaCount); + int i=0, j=0; + + FLT worstFitness = 0; for (size_t i = 0; i < pnaCount; i++) { fitness = getPointFitnessForPna(pointIn, &(pna[i])); - resultSum += SQUARED(fitness); + + if (worstFitness < fitness) + { + i = pna[i].ai; + j = pna[i].bi; + worstFitness = fitness; + } + + fitnesses[i] = fitness; + if (deubgPrint) + { + printf(" [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness); + } } + for (size_t i = 0; i < pnaCount; i++) + { + // TODO: This is an UGLY HACK!!! It is NOT ROBUST and MUST BE BETTER + // Right now, we're just throwing away the single worst fitness value + // this works frequently, but we REALLY need to do a better job of determing + // exactly when we should throw away a bad value. I'm thinking that decision + // alone could be a master's thesis, so lots of work to be done. + // This is just a stupid general approach that helps in a lot of cases, + // but is NOT suitable for long term use. + //if (pna[i].bi != i && pna[i].bi != j && pna[i].ai != i && pna[i].ai != j) + if (fitnesses[i] != worstFitness) + resultSum += SQUARED(fitnesses[i]); + } return 1 / FLT_SQRT(resultSum); } +// TODO: Use a central point instead of separate "minus" points for each axis. This will reduce +// the number of fitness calls by 1/3. Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT precision) { Point result; @@ -401,19 +438,19 @@ Point getGradient(Point pointIn, PointsAndAngle *pna, size_t pnaCount, FLT preci Point tmpXminus = pointIn; tmpXplus.x = pointIn.x + precision; tmpXminus.x = pointIn.x - precision; - result.x = getPointFitness(tmpXplus, pna, pnaCount) - getPointFitness(tmpXminus, pna, pnaCount); + result.x = getPointFitness(tmpXplus, pna, pnaCount, 0) - getPointFitness(tmpXminus, pna, pnaCount, 0); Point tmpYplus = pointIn; Point tmpYminus = pointIn; tmpYplus.y = pointIn.y + precision; tmpYminus.y = pointIn.y - precision; - result.y = getPointFitness(tmpYplus, pna, pnaCount) - getPointFitness(tmpYminus, pna, pnaCount); + result.y = getPointFitness(tmpYplus, pna, pnaCount, 0) - getPointFitness(tmpYminus, pna, pnaCount, 0); Point tmpZplus = pointIn; Point tmpZminus = pointIn; tmpZplus.z = pointIn.z + precision; tmpZminus.z = pointIn.z - precision; - result.z = getPointFitness(tmpZplus, pna, pnaCount) - getPointFitness(tmpZminus, pna, pnaCount); + result.z = getPointFitness(tmpZplus, pna, pnaCount, 0) - getPointFitness(tmpZminus, pna, pnaCount, 0); return result; } @@ -448,7 +485,7 @@ Point getAvgPoints(Point a, Point b) static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile) { int i = 0; - FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount); + FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount, 0); Point lastPoint = initialEstimate; // The values below are somewhat magic, and definitely tunable @@ -505,7 +542,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, point4.y = point3.y + specialGradient.y; point4.z = point3.z + specialGradient.z; - FLT newMatchFitness = getPointFitness(point4, pna, pnaCount); + FLT newMatchFitness = getPointFitness(point4, pna, pnaCount, 0); if (newMatchFitness > lastMatchFitness) { @@ -534,7 +571,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate, // very slowly, and we should just take what we've got and move on. // This also seems to happen almost only when data is a little more "dirty" // because the tracker is being rotated. - if (i > 900) + if (i > 120) { //printf("i got big"); break; @@ -1130,6 +1167,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) Point avgNorm = { 0 }; FLT smallestAngle = 20.0; + FLT largestAngle = 0; size_t pnaCount = 0; for (unsigned int i = 0; i < obj->numSensors; i++) @@ -1150,10 +1188,18 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) smallestAngle = pna[pnaCount].angle; } + if (pna[pnaCount].angle > largestAngle) + { + largestAngle = pna[pnaCount].angle; + } + double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta)); pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b); pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation); + pna[pnaCount].ai = i; + pna[pnaCount].bi = j; + pnaCount++; @@ -1206,10 +1252,10 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) } - FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount); + FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" sma(%d) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", smallestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" la(%f) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", largestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From 957945c223169073962b4987ee3bf73ab711d0a2 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 10:41:18 -0700 Subject: formatting changes --- src/poser_turveytori.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 62cd12c..3bb8d59 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -901,7 +901,7 @@ static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); - printf("{%8.8f, %8.8f, %8.8f} ", objPoint[0], objPoint[1], objPoint[2]); + printf("{% 08.8f, % 08.8f, % 08.8f} ", objPoint[0], objPoint[1], objPoint[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -996,7 +996,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini } - if (i > 1000) + if (i > 998) { //printf("Ri got big"); break; @@ -1255,7 +1255,7 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0); FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z)); - printf(" la(%f) SnsrCnt(%d) LhPos:(%4.4f, %4.4f, %4.4f) Dist: %8.8f ", largestAngle, obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); + printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); FLT rot[4]; -- cgit v1.2.3 From 4bcb16aae63e18085b73646a0c1dde94b10bb867 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 14:29:52 -0700 Subject: Wonky World View --- src/poser_turveytori.c | 103 ++++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 85 insertions(+), 18 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 3bb8d59..8b131b5 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -77,7 +77,6 @@ static const float DefaultPointsPerOuterDiameter = 60; typedef struct { FLT down[3]; // populated by the IMU for posing - int something; //Stuff #define OLD_ANGLES_BUFF_LEN 3 @@ -894,14 +893,15 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni return; } -static void WhereIsTheTrackedObjectAxisAngle(FLT *rotation, Point lhPoint) +static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) { - FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], rotation[3]}; - FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + posOut[0] = lhPoint.x; + posOut[1] = lhPoint.y; + posOut[2] = lhPoint.z; - rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + rotatearoundaxis(posOut, posOut, rotation, rotation[3]); - printf("{% 08.8f, % 08.8f, % 08.8f} ", objPoint[0], objPoint[1], objPoint[2]); + printf("{% 04.4f, % 04.4f, % 04.4f} ", posOut[0], posOut[1], posOut[2]); } static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj) @@ -1133,7 +1133,6 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 2, optimize the axis/ angle to match the data. RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj); - WhereIsTheTrackedObjectAxisAngle(rotOut, lh); //// Step 2, optimize the quaternion to match the data. //RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj); @@ -1143,7 +1142,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } -static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) +static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration) { //printf("Solving for Lighthouse\n"); @@ -1258,10 +1257,55 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance); //printf("Distance is %f, Fitness is %f\n", distance, fitGd); - FLT rot[4]; + FLT rot[4]; // this is axis/ angle rotation, not a quaternion! SolveForRotation(rot, obj, refinedEstimateGd); + FLT objPos[3]; + + WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); + + FLT rotQuat[4]; + + quatfromaxisangle(rotQuat, rot, rot[3]); + + //{ + FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z}; + + quatrotatevector(tmpPos, rotQuat, tmpPos); + //} + + if (setLhCalibration) + { + if (so->ctx->bsd[lh].PositionSet) + { + printf("Warning: resetting base station calibration data"); + } + + FLT invRot[4]; + quatgetreciprocal(invRot, rotQuat); + + so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x; + so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y; + so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z; + so->ctx->bsd[lh].Pose.Rot[0] = invRot[0]; + so->ctx->bsd[lh].Pose.Rot[1] = invRot[1]; + so->ctx->bsd[lh].Pose.Rot[2] = invRot[2]; + so->ctx->bsd[lh].Pose.Rot[3] = invRot[3]; + so->ctx->bsd[lh].PositionSet = 1; + } + + FLT wcPos[3]; // position in wold coordinates + + quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); - if (logFile) + wcPos[0] -= so->ctx->bsd[lh].Pose.Pos[0]; + wcPos[1] -= so->ctx->bsd[lh].Pose.Pos[1]; + wcPos[2] -= so->ctx->bsd[lh].Pose.Pos[2]; + + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + + //posOut = + + if (logFile) { updateHeader(logFile); fclose(logFile); @@ -1277,10 +1321,17 @@ static Point SolveForLighthouse(TrackedObject *obj, char doLogOutput) -static void QuickPose(SurviveObject *so) +static void QuickPose(SurviveObject *so, int lh) { + + ToriData * td = so->PoserData; + if (! so->ctx->bsd[lh].PositionSet) + { + // we don't know where we are! Augh!!! + return; + } //for (int i=0; i < so->nr_locations; i++) //{ @@ -1310,9 +1361,16 @@ static void QuickPose(SurviveObject *so) { int sensorCount = 0; + // TODO: remove, for debug purposes only! + FLT downQuat[4]; + FLT negZ[3] = { 0,0,-1 }; + quatfrom2vectors(downQuat, negZ, td->down); + //quatfrom2vectors(downQuat, td->down, negZ); + // end TODO + + for (int i = 0; i < so->nr_locations; i++) { - int lh = 0; int angleIndex0 = (td->angleIndex[lh][0] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; int angleIndex1 = (td->angleIndex[lh][1] + 1 + OLD_ANGLES_BUFF_LEN) % OLD_ANGLES_BUFF_LEN; if (td->oldAngles[i][0][lh][angleIndex0] != 0 && td->oldAngles[i][1][lh][angleIndex1] != 0) @@ -1320,6 +1378,10 @@ static void QuickPose(SurviveObject *so) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; + // TODO: remove these two lines!!! + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); + to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; to->sensor[sensorCount].normal.z = norm[2]; @@ -1342,7 +1404,9 @@ static void QuickPose(SurviveObject *so) if (sensorCount > 4) { - SolveForLighthouse(to, 0); + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, lh, 0); printf("!\n"); } @@ -1414,7 +1478,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 2 == 0) - QuickPose(so); + QuickPose(so, 0); } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; @@ -1485,8 +1549,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } to->numSensors = sensorCount; + FLT pos[3], quat[4]; - SolveForLighthouse(to, 0); + SolveForLighthouse(pos, quat, to, so, 0, 0, 1); } { int sensorCount = 0; @@ -1499,8 +1564,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - quatrotatevector(norm, downQuat, norm); - quatrotatevector(point, downQuat, point); + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; @@ -1516,7 +1581,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->numSensors = sensorCount; - SolveForLighthouse(to, 0); + FLT pos[3], quat[4]; + + SolveForLighthouse(pos, quat, to, so, 0, 1, 1); } free(to); -- cgit v1.2.3 From 198082258a62ab6313569e7950c0420baad6ddef Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:19:00 -0700 Subject: Sane World View The z=0 plane is now initially aligned to the device's z=0 plane during calibration. (works really well for the tracker, which is oriented with Z in the proper direction when sitting on a horizontal surface) --- src/poser_turveytori.c | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 8b131b5..d3dd2ff 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -895,9 +895,9 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint) { - posOut[0] = lhPoint.x; - posOut[1] = lhPoint.y; - posOut[2] = lhPoint.z; + posOut[0] = -lhPoint.x; + posOut[1] = -lhPoint.y; + posOut[2] = -lhPoint.z; rotatearoundaxis(posOut, posOut, rotation, rotation[3]); @@ -1273,8 +1273,12 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(tmpPos, rotQuat, tmpPos); //} + //static int foo = 0; + + //if (0 == foo) if (setLhCalibration) - { + { + //foo = 1; if (so->ctx->bsd[lh].PositionSet) { printf("Warning: resetting base station calibration data"); @@ -1297,20 +1301,22 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); - wcPos[0] -= so->ctx->bsd[lh].Pose.Pos[0]; - wcPos[1] -= so->ctx->bsd[lh].Pose.Pos[1]; - wcPos[2] -= so->ctx->bsd[lh].Pose.Pos[2]; + wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; + wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; + wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; - printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); + so->OutPose.Pos[0] = wcPos[0]; + so->OutPose.Pos[1] = wcPos[1]; + so->OutPose.Pos[2] = wcPos[2]; - //posOut = + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); if (logFile) { updateHeader(logFile); fclose(logFile); } - //fgetc(stdin); + return refinedEstimateGd; } @@ -1526,8 +1532,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] }; FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] }; - quatrotatevector(norm, downQuat, norm); - quatrotatevector(point, downQuat, point); + //quatrotatevector(norm, downQuat, norm); + //quatrotatevector(point, downQuat, point); to->sensor[sensorCount].normal.x = norm[0]; to->sensor[sensorCount].normal.y = norm[1]; -- cgit v1.2.3 From eb5fae1c2aa6007ec84fdf0c5405308c7ea274ca Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:30:35 -0700 Subject: Cleanup --- src/poser_turveytori.c | 58 ++++++++++++++++++-------------------------------- 1 file changed, 21 insertions(+), 37 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index d3dd2ff..6997aa7 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1367,12 +1367,12 @@ static void QuickPose(SurviveObject *so, int lh) { int sensorCount = 0; - // TODO: remove, for debug purposes only! - FLT downQuat[4]; - FLT negZ[3] = { 0,0,-1 }; - quatfrom2vectors(downQuat, negZ, td->down); + //// TODO: remove, for debug purposes only! + //FLT downQuat[4]; + //FLT negZ[3] = { 0,0,-1 }; + ////quatfrom2vectors(downQuat, negZ, td->down); //quatfrom2vectors(downQuat, td->down, negZ); - // end TODO + //// end TODO for (int i = 0; i < so->nr_locations; i++) @@ -1397,11 +1397,6 @@ static void QuickPose(SurviveObject *so, int lh) to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) - //printf("%2d: %8.8f, %8.8f \n", - // i, - // to->sensor[sensorCount].theta, - // to->sensor[sensorCount].phi - // ); sensorCount++; } @@ -1470,33 +1465,28 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { - int foo = l->acode & 0x1; - //printf("%d", foo); - //if (axis) + if (0 == l->lh && axis) // only once per full cycle... { - if (0 == l->lh && axis) // only once per full cycle... - { - static unsigned int counter = 1; + static unsigned int counter = 1; - counter++; + counter++; - // let's just do this occasionally for now... - if (counter % 2 == 0) - QuickPose(so, 0); - } - // axis changed, time to increment the circular buffer index. - td->angleIndex[l->lh][axis]++; - td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; - - // and clear out the data. - for (int i=0; i < SENSORS_PER_OBJECT; i++) - { - td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; - } + // let's just do this occasionally for now... + if (counter % 2 == 0) + QuickPose(so, 0); + } + // axis changed, time to increment the circular buffer index. + td->angleIndex[l->lh][axis]++; + td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN; + // and clear out the data. + for (int i=0; i < SENSORS_PER_OBJECT; i++) + { + td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0; } + td->lastAxis[l->lh] = axis; } @@ -1517,7 +1507,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's get the quaternion that represents this rotation. FLT downQuat[4]; - FLT negZ[3] = { 0,0,-1 }; + FLT negZ[3] = { 0,0,1 }; //quatfrom2vectors(downQuat, negZ, td->down); quatfrom2vectors(downQuat, td->down, negZ); @@ -1544,12 +1534,6 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal) to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical) - //printf("%2d: %8.8f, %8.8f \n", - // i, - // to->sensor[sensorCount].theta, - // to->sensor[sensorCount].phi - // ); - sensorCount++; } } -- cgit v1.2.3 From 66eb9b0508c6a5b1af5d1bc8fb5cac74573f8b20 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:36:36 -0700 Subject: Added rotation to object pose Haven't tested it yet, low confidence it's right, but the code is on the right track. --- src/poser_turveytori.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 6997aa7..c251040 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1301,6 +1301,9 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); + FLT newOrientation[4]; + quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot ); + wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; @@ -1309,6 +1312,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob so->OutPose.Pos[1] = wcPos[1]; so->OutPose.Pos[2] = wcPos[2]; + so->OutPose.Rot[0] = newOrientation[0]; + so->OutPose.Rot[1] = newOrientation[1]; + so->OutPose.Rot[2] = newOrientation[2]; + so->OutPose.Rot[3] = newOrientation[3]; + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); if (logFile) -- cgit v1.2.3 From 0e69fac4d8565926ac3cff6084c95f204d8e173f Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sat, 1 Apr 2017 23:37:03 -0700 Subject: Fix bug that prevented HMD from being added --- src/survive_vive.c | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 757fa11..5224728 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -68,14 +68,14 @@ const char * devnames[] = { #define USB_DEV_HMD 0 -#define USB_DEV_LIGHTHOUSE 1 +#define USB_DEV_HMD_IMU_LH 1 #define USB_DEV_WATCHMAN1 2 #define USB_DEV_WATCHMAN2 3 #define USB_DEV_TRACKER0 4 #define USB_DEV_W_WATCHMAN1 5 // Wired Watchman attached via USB #ifdef HIDAPI -#define USB_DEV_LIGHTHOUSEB 6 +#define USB_DEV_HMD_IMU_LHB 6 #define USB_DEV_TRACKER0_LIGHTCAP 7 #define USB_DEV_W_WATCHMAN1_LIGHTCAP 8 #define MAX_USB_DEVS 9 @@ -84,7 +84,7 @@ const char * devnames[] = { #endif #define USB_IF_HMD 0 -#define USB_IF_LIGHTHOUSE 1 +#define USB_IF_HMD_IMU_LH 1 #define USB_IF_WATCHMAN1 2 #define USB_IF_WATCHMAN2 3 #define USB_IF_TRACKER0 4 @@ -471,14 +471,14 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * #endif 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_LIGHTHOUSE] && AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; } + 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; } if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1, sv->udev[USB_DEV_WATCHMAN1], 0x81, survive_data_cb, "Watchman 1" ) ) { return -8; } if( sv->udev[USB_DEV_WATCHMAN2] && AttachInterface( sv, wm1, USB_IF_WATCHMAN2, sv->udev[USB_DEV_WATCHMAN2], 0x81, survive_data_cb, "Watchman 2")) { return -9; } if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0, sv->udev[USB_DEV_TRACKER0], 0x81, survive_data_cb, "Tracker 1")) { return -10; } if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1, sv->udev[USB_DEV_W_WATCHMAN1], 0x81, survive_data_cb, "Wired Watchman 1")) { return -11; } #ifdef HIDAPI //Tricky: use other interface for actual lightcap. XXX THIS IS NOT YET RIGHT!!! - if( sv->udev[USB_DEV_LIGHTHOUSEB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_LIGHTHOUSEB], 0x82, survive_data_cb, "Lightcap")) { return -12; } + if( sv->udev[USB_DEV_HMD_IMU_LHB] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LHB], 0x82, survive_data_cb, "Lightcap")) { return -12; } // This is a HACK! But it works. Need to investigate further sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64; @@ -517,14 +517,14 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo if( r != sizeof( vive_magic_power_on ) ) return 5; } - if (sv->udev[USB_DEV_LIGHTHOUSE]) + if (sv->udev[USB_DEV_HMD_IMU_LH]) { static uint8_t vive_magic_enable_lighthouse[5] = { 0x04 }; - r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); + r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) ); if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5; static uint8_t vive_magic_enable_lighthouse2[5] = { 0x07, 0x02 }; //Switch to 0x25 mode (able to get more light updates) - r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); + r = update_feature_report( sv->udev[USB_DEV_HMD_IMU_LH], 0, vive_magic_enable_lighthouse2, sizeof( vive_magic_enable_lighthouse2 ) ); if( r != sizeof( vive_magic_enable_lighthouse2 ) ) return 5; } @@ -1103,7 +1103,7 @@ void survive_data_cb( SurviveUSBInterface * si ) headset->ison = 1; break; } - case USB_IF_LIGHTHOUSE: + case USB_IF_HMD_IMU_LH: case USB_IF_W_WATCHMAN1: case USB_IF_TRACKER0: { @@ -1395,7 +1395,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) } //Next, pull out the config stuff. - if( sv->udev[USB_DEV_HMD] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } + if( sv->udev[USB_DEV_HMD_IMU_LH] && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); } if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); } if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); } if( sv->udev[USB_DEV_TRACKER0] && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); } @@ -1450,7 +1450,7 @@ int DriverRegHTCVive( SurviveContext * ctx ) */ //Add the drivers. - if( sv->udev[USB_DEV_HMD] ) { survive_add_object( ctx, hmd ); } + 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 ); } -- cgit v1.2.3