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 --- include/libsurvive/survive.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'include') 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 ); -- cgit v1.3.1 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(-) (limited to 'include') 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.3.1 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(-) (limited to 'include') 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.3.1 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 (limited to 'include') 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.3.1 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(-) (limited to 'include') 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.3.1