From 4e3ac12016cc489c818709412acd293cac54cb78 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Fri, 17 Mar 2017 11:37:30 -0400 Subject: reusable function for decoding acode --- src/survive_data.c | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 4a2cfb6..d658e18 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -5,6 +5,19 @@ #include #include + +int32_t decode_acode(uint32_t length, int32_t main_divisor) { + //+50 adds a small offset and seems to help always get it right. + //Check the +50 in the future to see how well this works on a variety of hardware. + + int32_t acode = (length+main_divisor+50)/(main_divisor*2); + if( acode & 1 ) return -1; + + return (acode>>1) - 6; +} + +} + //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 ) { @@ -112,17 +125,13 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) int32_t acode_array[2] = { - (so->last_sync_length[0]+main_divisor+50)/(main_divisor*2), //+50 adds a small offset and seems to help always get it right. - (so->last_sync_length[1]+main_divisor+50)/(main_divisor*2), //Check the +50 in the future to see how well this works on a variety of hardware. + decode_acode(so->last_sync_length[0],main_divisor), + decode_acode(so->last_sync_length[1],main_divisor) }; //XXX: TODO: Capture error count here. - if( acode_array[0] & 1 ) return; - if( acode_array[1] & 1 ) return; - - acode_array[0] = (acode_array[0]>>1) - 6; - acode_array[1] = (acode_array[1]>>1) - 6; - + if( acode_array[0] < 0 ) return; + if( acode_array[1] < 0 ) return; int acode = acode_array[0]; -- cgit v1.2.3 From acd7b7c0ecc7482ca60cdefeb277543212a293f5 Mon Sep 17 00:00:00 2001 From: geekmaster Date: Fri, 17 Mar 2017 18:34:30 -0500 Subject: Update README.md Fix typos. --- README.md | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 45ac38f..fcde197 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ I say "high-performance" really this project is based tightly off of OSVR-Vive-L 2. Put it under an open-source instead of a force-source license. (GPL to MIT/X11) 3. Write it in C. 4. Avoid extra layers where convenient. -5. (long shot) Make the vive vivable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.] +5. (long shot) Make the vive viable for use with Intel Integrated Graphics systems. [It works with HD4000 using DisplayPort. See "Intel Integrated Graphics" section below.] Will ~~I~~ we succeed? Probably not. @@ -72,8 +72,6 @@ I may or may not read data from the Vive regarding configuration. If I do, it w ## Intel Integrated Graphics -The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). +The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the Vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time). - -but HDMI will require non-certified drivers to allow custom resolutions (due to WHQL restrictions). Haswell (Iris) graphics and later can use the new intel beta drivers to allow custom resolutions.] -- cgit v1.2.3 From 129677f041dc87689e73d85ef737501be59b58ca Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 01:01:49 -0400 Subject: Fix mixed usage of "struct" and typedef'd struct, and fix compile error on gcc/Linux. --- src/survive_vive.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index a9b295f..91efc2c 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -141,10 +141,10 @@ 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 , 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 ); +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ); #ifdef HIDAPI void * HAPIReceiver( void * v ) @@ -177,8 +177,8 @@ void * HAPIReceiver( void * v ) #else static void handle_transfer(struct libusb_transfer* transfer) { - struct SurviveUSBInterface * iface = transfer->user_data; - struct SurviveContext * ctx = iface->ctx; + SurviveUSBInterface * iface = transfer->user_data; + SurviveContext * ctx = iface->ctx; if( transfer->status != LIBUSB_TRANSFER_COMPLETED ) { @@ -307,9 +307,9 @@ static inline int hid_get_feature_report_timeout(USBHANDLE device, uint16_t ifac return -1; } -int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1, struct SurviveObject * tr0 , struct SurviveObject * ww0 ) +int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *wm0, SurviveObject * wm1, SurviveObject * tr0 , SurviveObject * ww0 ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; #ifdef HIDAPI if( !GlobalRXUSBMutx ) { @@ -488,10 +488,10 @@ int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, s return 0; } -int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) +int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen ) { int r; - struct SurviveViveData * sv = drv; + SurviveViveData * sv = drv; printf( "*CALLING %p %p\n", ctx, sv ); //XXX TODO: Handle haptics, etc. @@ -583,7 +583,7 @@ int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_c return 0; } -void survive_vive_usb_close( struct SurviveViveData * sv ) +void survive_vive_usb_close( SurviveViveData * sv ) { int i; #ifdef HIDAPI @@ -608,7 +608,7 @@ void survive_vive_usb_close( struct SurviveViveData * sv ) #endif } -int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) +int survive_vive_usb_poll( SurviveContext * ctx, void * v ) { #ifdef HIDAPI OGUnlockMutex( GlobalRXUSBMutx ); @@ -616,11 +616,11 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) OGUnlockMutex( GlobalRXUSBMutx ); return 0; #else - struct SurviveViveData * sv = v; + SurviveViveData * sv = v; int r = libusb_handle_events( sv->usbctx ); if( r ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; SV_ERROR( "Libusb poll failed." ); } return r; @@ -629,9 +629,9 @@ int survive_vive_usb_poll( struct SurviveContext * ctx, void * v ) } -int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic ) +int survive_get_config( char ** config, SurviveViveData * sv, int devno, int iface, int send_extra_magic ) { - struct SurviveContext * ctx = sv->ctx; + SurviveContext * ctx = sv->ctx; int ret, count = 0, size = 0; uint8_t cfgbuff[64]; uint8_t compressed_data[8192]; @@ -749,7 +749,7 @@ int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, #define POP2 (*(((uint16_t*)((readdata+=2)-2)))) #define POP4 (*(((uint32_t*)((readdata+=4)-4)))) -static void handle_watchman( struct SurviveObject * w, uint8_t * readdata ) +static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { uint8_t startread[29]; @@ -1015,7 +1015,7 @@ void survive_data_cb( SurviveUSBInterface * si ) { case USB_IF_HMD: { - struct SurviveObject * headset = obj; + SurviveObject * headset = obj; readdata+=2; headset->buttonmask = POP1; //Lens headset->axis2 = POP2; //Lens Separation -- cgit v1.2.3 From af81a4dbe0e58e94b61b11c14b21a2decc15d823 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 13:15:22 -0400 Subject: Fix unititialized array. --- src/survive_vive.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index 91efc2c..c8e2b55 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -920,7 +920,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) { uint8_t *marked; marked = alloca(nrtime); - memset( marked, 0, sizeof( marked ) ); + memset( marked, 0, sizeof( nrtime ) ); int i, parpl = 0; timecount--; int timepl = 0; @@ -935,6 +935,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) while( marked[timepl] ) timepl++; if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. uint32_t endtime = times[timepl++]; + int end = timepl + adv; if( end > timecount ) { fault = 4; goto end; } //end referencing off list if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used. @@ -978,7 +979,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) end: { SurviveContext * ctx = w->ctx; - SV_INFO( "Light decoding fault: %d\n", fault ); + SV_INFO( "Light decoding fault: %d", fault ); } } } -- cgit v1.2.3 From f976a532970afecce640ddfba88a6f1b04ab34b8 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 13:21:13 -0400 Subject: Wow... Actually initilize to right size. --- src/survive_vive.c | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/survive_vive.c b/src/survive_vive.c index c8e2b55..f6465b2 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -771,7 +771,7 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) qty-=2; int propset = 0; int doimu = 0; - + int i; if( (type & 0xf0) == 0xf0 ) { @@ -916,11 +916,12 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) LightcapElement les[10]; int lese = 0; //les's end + //Second, go through all LEDs and extract the lightevent from them. { uint8_t *marked; marked = alloca(nrtime); - memset( marked, 0, sizeof( nrtime ) ); + memset( marked, 0, nrtime ); int i, parpl = 0; timecount--; int timepl = 0; @@ -933,6 +934,17 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) led >>= 3; while( marked[timepl] ) timepl++; + +#ifdef DEBUG_WATCHMAN + int i; + printf( "TP %d TC: %d : ", timepl, timecount ); + for( i = 0; i < nrtime; i++ ) + { + printf( "%d", marked[i] ); + } + printf( "\n" ); +#endif + if( timepl > timecount ) { fault = 3; goto end; } //Ran off max of list. uint32_t endtime = times[timepl++]; -- cgit v1.2.3 From 91a6bb98e0531bda74efc682223de14a1787fede Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:20:30 -0400 Subject: Fix histograms --- src/survive_cal.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/survive_cal.c b/src/survive_cal.c index 0eb9446..9dc3af2 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -272,6 +272,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin if( sensors_visible < MIN_SENSORS_VISIBLE_PER_LH_FOR_CAL ) { //printf( "Dev %d, LH %d not enough visible points found.\n", i, j ); + reset_calibration( cd ); cd->found_common = 0; return; } @@ -440,7 +441,7 @@ static void handle_calibration( struct SurviveCalData *cd ) FLT stddevlen = 0; #define HISTOGRAMSIZE 31 - #define HISTOGRAMBINANG 0.00001 //TODO: Tune + #define HISTOGRAMBINANG ((3.14159)/400000.0) //TODO: Tune int histo[HISTOGRAMSIZE]; memset( histo, 0, sizeof( histo ) ); -- cgit v1.2.3 From 67ccb935ef12d9ab25f8223dea5208b96ae5052f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:20:50 -0400 Subject: increase time for integrating points. --- src/survive_cal.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/survive_cal.h b/src/survive_cal.h index 3d62051..701dc60 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -35,7 +35,9 @@ void survive_cal_angle( SurviveObject * so, int sensor_id, int acode, uint32_t t #define MAX_SENSORS_TO_CAL 96 #define MIN_PTS_BEFORE_CAL 24 -#define DRPTS 128 + + +#define DRPTS 1024 //Number of samples required in collection phase. #define POSE_OBJECTS 3 -- cgit v1.2.3 From 0f94e39d96fbc2744e61b85d2884250d0783eb11 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 18 Mar 2017 15:21:24 -0400 Subject: Add more possible debugging if turned on. --- src/survive_data.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 5c5a5e9..0789203 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -8,17 +8,31 @@ //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 ) { - SurviveContext * ctx = so->ctx; + SurviveContext * ctx = so->ctx; //int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time; - //if( so->codename[0] != 'H' ) - - if( le->sensor_id > SENSORS_PER_OBJECT ) { return; } +#if 0 + if( so->codename[0] == 'H' ) + { + static int lt; + static int last; + if( le->length > 1000 ) + { + int dl = le->timestamp - lt; + lt = le->timestamp; + if( dl > 10000 || dl < -10000 ) + printf( "+++%s %3d %5d %9d ", so->codename, le->sensor_id, le->length, dl ); + if( dl > 100000 ) printf(" \n" ); + } + last=le->length; + } +#endif + so->tsl = le->timestamp; if( le->length < 20 ) return; ///Assuming 20 is an okay value for here. -- cgit v1.2.3 From d4e7bc4037f9e0b4af3f11940b53520eb9f82ff6 Mon Sep 17 00:00:00 2001 From: Joshua Allen Date: Sat, 18 Mar 2017 20:25:06 -0400 Subject: fix compile --- src/survive_data.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/survive_data.c b/src/survive_data.c index 16a22e9..ffeacff 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -16,8 +16,6 @@ int32_t decode_acode(uint32_t length, int32_t main_divisor) { return (acode>>1) - 6; } -} - //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 ) { -- cgit v1.2.3 From aaaa45dcd79e1d9ebe04ec428422fa29d1961488 Mon Sep 17 00:00:00 2001 From: CNLohr Date: Sun, 19 Mar 2017 15:34:24 -0400 Subject: Closes #42 with logo. --- README.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index fcde197..7b89c2d 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,10 @@ # libsurvive -**WARNING PROJECT IN EXPERIMENTAL PHASE** +![Logo](https://cloud.githubusercontent.com/assets/2748168/24084003/9095c98a-0cb8-11e7-88a3-575f9f4c7bb4.png) -Discord: https://discordapp.com/invite/7QbCAGS +An Open-Source tool for working with lighthouse-based trakcing data, including support for the HTC Vive, which is still in the experimental phase. + +Most of the development is discussed on Discord. Join the chat and discussion here: https://discordapp.com/invite/7QbCAGS ## Livestream collection | Note | Youtube URL | Run time | -- cgit v1.2.3 From a229dec7ee46873e0b590fdbd34feb29112e4f0d Mon Sep 17 00:00:00 2001 From: CNLohr Date: Sun, 19 Mar 2017 16:55:54 -0400 Subject: Update README.md --- README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/README.md b/README.md index 7b89c2d..db1d6ce 100644 --- a/README.md +++ b/README.md @@ -77,3 +77,9 @@ I may or may not read data from the Vive regarding configuration. If I do, it w The limiting factor for Vive viability on a given computer is the maximum available pixel clock frequency, and frequency limitations of the HDMI port, and HDMI and DisplayPort video cables. DisplayPort can support higher frequencies than HDMI, on Ivy Bridge HD4000 graphics. In fact, the Vive works with HD4000 graphics using DisplayPort, with native EDID resolution (2160x1200@90Hz). To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time). + + + +## Addendum and notes + +Thanks to Mr. Fault for our logo! -- cgit v1.2.3 From 8253e3c3adbb54ebc3d3c8e02c017e900a83edb0 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Mon, 20 Mar 2017 01:42:32 -0400 Subject: log synctimes. --- Makefile | 6 +++++ calibrate.c | 2 +- include/libsurvive/poser.h | 1 + src/survive_cal.c | 56 +++++++++++++++++++++++++++++++++++++++++++++- src/survive_cal.h | 3 +++ src/survive_data.c | 10 ++++++++- src/survive_process.c | 3 +++ 7 files changed, 78 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index a642877..1899c4c 100644 --- a/Makefile +++ b/Makefile @@ -32,6 +32,12 @@ LIBSURVIVE_CORE:=$(LIBSURVIVE_CORE) LIBSURVIVE_O:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) LIBSURVIVE_C:=$(LIBSURVIVE_O:.o=.c) + +#If you want to use HIDAPI on Linux. +#CFLAGS:=$(CFLAGS) -DHIDAPI +#REDISTS:=$(REDISTS) redist/hid-linux.o +#LDFLAGS:=$(LDFLAGS) -ludev + #Useful Preprocessor Directives: # -DUSE_DOUBLE = use double instead of float for most operations. # -DNOZLIB = use puff.c diff --git a/calibrate.c b/calibrate.c index 726a8cc..f27131a 100644 --- a/calibrate.c +++ b/calibrate.c @@ -56,7 +56,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int // if( timeinsweep < 0 ) return; survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length ); if( sensor_id < 0 ) return; - if( acode == -1 ) return; + if( acode < 0 ) return; //return; int jumpoffset = sensor_id; if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32; diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 98c926e..cf11e0c 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -45,6 +45,7 @@ typedef struct //If "lengths[...]" < 0, means not a valid piece of sweep information. 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]; PoserDataIMU lastimu; } PoserDataFullScene; diff --git a/src/survive_cal.c b/src/survive_cal.c index 9dc3af2..04931cc 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -196,6 +196,21 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int if( i == NUM_LIGHTHOUSES ) cd->stage = 2; //If all lighthouses have their OOTX set, move on. } break; + case 3: //Look for light sync lengths. + { + if( acode >= -2 ) break; + 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; + + cd->all_sync_times[sensor_id][lh][cd->all_sync_counts[sensor_id][lh]++] = length; + break; + } + } } @@ -333,6 +348,8 @@ static void reset_calibration( struct SurviveCalData * cd ) cd->found_common = 0; cd->times_found_common = 0; cd->stage = 2; + + memset( cd->all_sync_counts, 0, sizeof( cd->all_sync_counts ) ); } static void handle_calibration( struct SurviveCalData *cd ) @@ -358,9 +375,45 @@ static void handle_calibration( struct SurviveCalData *cd ) #else mkdir( "calinfo", 0755 ); #endif + int sen, axis, lh; + FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; + + //Just to get it out of the way early, we'll calculate the sync-pulse-lengths here. + FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" ); + + for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) + for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) + { + int count = cd->all_sync_counts[sen][lh]; + int i; + double totaltime; + + totaltime = 0; + + if( count < 20 ) continue; + for( i = 0; i < count; i++ ) + { + totaltime += cd->all_sync_times[sen][lh][i]; + } + FLT avg = totaltime/count; + + double stddev = 0.0; + for( i = 0; i < count; i++ ) + { + stddev += (cd->all_sync_times[sen][lh][i] - avg)*(cd->all_sync_times[sen][lh][i] - avg); + } + stddev /= count; + + fprintf( sync_time_info, "%d %d %f %d %f\n", sen, lh, totaltime/count, count, stddev ); + } + + fclose( sync_time_info ); + + + + FILE * hists = fopen( "calinfo/histograms.csv", "w" ); FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" ); - int sen, axis, lh; for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ ) for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ ) for( axis = 0; axis < 2; axis++ ) @@ -526,6 +579,7 @@ static void handle_calibration( struct SurviveCalData *cd ) fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; + fsd.synctimes[i][j] = temp_syncs[i][j]; } int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd ); diff --git a/src/survive_cal.h b/src/survive_cal.h index 701dc60..c64b8f9 100644 --- a/src/survive_cal.h +++ b/src/survive_cal.h @@ -56,6 +56,9 @@ struct SurviveCalData int8_t found_common; int8_t times_found_common; + FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS]; + int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES]; + //For camfind (4+) //Index is calculated with: int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis; FLT avgsweeps[MAX_CAL_PT_DAT]; diff --git a/src/survive_data.c b/src/survive_data.c index ffeacff..8c5c646 100644 --- a/src/survive_data.c +++ b/src/survive_data.c @@ -69,7 +69,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) { int is_new_pulse = delta > so->pulselength_min_sync /*1500*/ + last_sync_length; - //printf("m sync %d %d %d %d\n", le->sensor_id, so->last_sync_time[ssn], le->timestamp, delta); + so->did_handle_ootx = 0; @@ -114,6 +114,14 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le ) } } } + + //Extra tidbit for storing length-of-sync-pulses. + { + 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 ); + } } diff --git a/src/survive_process.c b/src/survive_process.c index 8dc849a..9295638 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -16,6 +16,9 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length ); } + //We don't use sync times, yet. + if( acode < -1 ) return; + if( base_station > NUM_LIGHTHOUSES ) return; //No loner need sync information past this point. -- cgit v1.2.3 From 3694605b23b1019488844bf448d461eacc69077f Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 22 Mar 2017 00:12:31 -0400 Subject: Update RawDraw to version that can at least support OpenGL if we want it. --- redist/CNFGFunctions.c | 82 ++++++++++++++++++++++++++++++++++++++++++++++++++ redist/CNFGFunctions.h | 7 ++++- redist/CNFGWinDriver.c | 71 ++++++++++++++++++++++++++++++++++++++++--- redist/CNFGXDriver.c | 74 ++++++++++++++++++++++++++++++++++++++++----- 4 files changed, 221 insertions(+), 13 deletions(-) diff --git a/redist/CNFGFunctions.c b/redist/CNFGFunctions.c index 947456f..e93eb6f 100644 --- a/redist/CNFGFunctions.c +++ b/redist/CNFGFunctions.c @@ -270,3 +270,85 @@ void CNFGDrawTextbox( int x, int y, const char * text, int textsize ) CNFGPenY = y + textsize; CNFGDrawText( text, textsize ); } + + +#ifdef CNFGOGL + +#include + +uint32_t CNFGColor( uint32_t RGB ) +{ + unsigned char red = RGB & 0xFF; + unsigned char grn = ( RGB >> 8 ) & 0xFF; + unsigned char blu = ( RGB >> 16 ) & 0xFF; + glColor3ub( red, grn, blu ); +} + +void CNFGClearFrame() +{ + short w, h; + unsigned char red = CNFGBGColor & 0xFF; + unsigned char grn = ( CNFGBGColor >> 8 ) & 0xFF; + unsigned char blu = ( CNFGBGColor >> 16 ) & 0xFF; + glClearColor( red/255.0, grn/255.0, blu/255.0, 1.0 ); + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + CNFGGetDimensions( &w, &h ); + glMatrixMode( GL_PROJECTION ); + glLoadIdentity(); + glViewport( 0, 0, w, h ); + glOrtho( 0, w, h, 0, 1, -1 ); + glMatrixMode( GL_MODELVIEW ); + glLoadIdentity(); +} + + +void CNFGTackSegment( short x1, short y1, short x2, short y2 ) +{ + if( x1 == x2 && y1 == y2 ) + { + glBegin( GL_POINTS ); + glVertex2f( x1+.5, y1+.5 ); + glEnd(); + } + else + { + glBegin( GL_LINES ); + glVertex2f( x1+.5, y1+.5 ); + glVertex2f( x2+.5, y2+.5 ); + glEnd(); + } +} + +void CNFGTackPixel( short x1, short y1 ) +{ + glBegin( GL_POINTS ); + glVertex2f( x1, y1 ); + glEnd(); +} + +void CNFGTackRectangle( short x1, short y1, short x2, short y2 ) +{ + glBegin( GL_QUADS ); + glVertex2f( x1, y1 ); + glVertex2f( x2, y1 ); + glVertex2f( x2, y2 ); + glVertex2f( x1, y2 ); + glEnd(); +} + +void CNFGTackPoly( RDPoint * points, int verts ) +{ + int i; + glBegin( GL_TRIANGLE_FAN ); + glVertex2f( points[0].x, points[0].y ); + for( i = 1; i < verts; i++ ) + { + glVertex2f( points[i].x, points[i].y ); + } + glEnd(); +} + +void CNFGInternalResize( short x, short y ) { } + + +#endif diff --git a/redist/CNFGFunctions.h b/redist/CNFGFunctions.h index 9ecb1bd..179a20b 100644 --- a/redist/CNFGFunctions.h +++ b/redist/CNFGFunctions.h @@ -1,4 +1,4 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. +//Copyright (c) 2011, 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. #ifndef _DRAWFUCNTIONS_H #define _DRAWFUCNTIONS_H @@ -49,6 +49,11 @@ void HandleDestroy(); //Internal function for resizing rasterizer for rasterizer-mode. void CNFGInternalResize( short x, short y ); //don't call this. +//Not available on all systems. Use The OGL portion with care. +#ifdef CNFGOGL +void CNFGSetVSync( int vson ); +void * CNFGGetExtension( const char * extname ); +#endif #ifdef __cplusplus }; diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c index a029419..c5da925 100644 --- a/redist/CNFGWinDriver.c +++ b/redist/CNFGWinDriver.c @@ -13,7 +13,6 @@ static HWND lsHWND; static HDC lsWindowHDC; static HDC lsHDC; - #ifdef RASTERIZER #include "CNFGRasterizer.h" @@ -32,14 +31,34 @@ static void InternalHandleResize(); #endif +#ifdef CNFGOGL +#include +static HGLRC hRC=NULL; +static void InternalHandleResize() { } +void CNFGSwapBuffers() +{ + SwapBuffers(lsWindowHDC); +} +#endif + void CNFGGetDimensions( short * x, short * y ) { + static int lastx, lasty; + RECT window; + GetClientRect( lsHWND, &window ); + bufferx = ( window.right - window.left); + buffery = ( window.bottom - window.top); + if( bufferx != lastx || buffery != lasty ) + { + lastx = bufferx; + lasty = buffery; + InternalHandleResize(); + } *x = bufferx; *y = buffery; } - void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) { RECT r; @@ -126,9 +145,49 @@ void CNFGSetup( const char * name_of_window, int width, int height ) hInstance, NULL); //no parameters to pass - lsWindowHDC = GetDC( lsHWND ); +#ifdef CNFGOGL + //From NeHe + static PIXELFORMATDESCRIPTOR pfd = + { + sizeof(PIXELFORMATDESCRIPTOR), + 1, + PFD_DRAW_TO_WINDOW | + PFD_SUPPORT_OPENGL | + PFD_DOUBLEBUFFER, + PFD_TYPE_RGBA, + 32, + 0, 0, 0, 0, 0, 0, + 0, + 0, + 0, + 0, 0, 0, 0, + 16, + 0, + 0, + PFD_MAIN_PLANE, + 0, + 0, 0, 0 + }; + GLuint PixelFormat = ChoosePixelFormat( lsWindowHDC, &pfd ); + if( !SetPixelFormat( lsWindowHDC, PixelFormat, &pfd ) ) + { + MessageBox( 0, "Could not create PFD for OpenGL Context\n", 0, 0 ); + exit( -1 ); + } + if (!(hRC=wglCreateContext(lsWindowHDC))) // Are We Able To Get A Rendering Context? + { + MessageBox( 0, "Could not create OpenGL Context\n", 0, 0 ); + exit( -1 ); + } + if(!wglMakeCurrent(lsWindowHDC,hRC)) // Try To Activate The Rendering Context + { + MessageBox( 0, "Could not current OpenGL Context\n", 0, 0 ); + exit( -1 ); + } +#endif + lsHDC = CreateCompatibleDC( lsWindowHDC ); lsBitmap = CreateCompatibleBitmap( lsWindowHDC, bufferx, buffery ); SelectObject( lsHDC, lsBitmap ); @@ -182,6 +241,7 @@ void CNFGHandleInput() } } +#ifndef CNFGOGL #ifndef RASTERIZER @@ -237,8 +297,7 @@ void CNFGClearFrame() DeleteObject( lsClearBrush ); lsClearBrush = CreateSolidBrush( CNFGBGColor ); SelectObject( lsHDC, lsClearBrush ); - - FillRect( lsHDC, &r, lsClearBrush ); + FillRect( lsHDC, &r, lsClearBrush); } void CNFGTackPoly( RDPoint * points, int verts ) @@ -287,3 +346,5 @@ void CNFGSwapBuffers() void CNFGInternalResize( short bufferx, short buffery ) { } #endif +#endif + diff --git a/redist/CNFGXDriver.c b/redist/CNFGXDriver.c index 8a8904a..ebaed91 100644 --- a/redist/CNFGXDriver.c +++ b/redist/CNFGXDriver.c @@ -1,4 +1,4 @@ -//Copyright (c) 2011 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. +//Copyright (c) 2011, 2017 <>< Charles Lohr - Under the MIT/x11 or NewBSD License you choose. //portions from //http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html @@ -25,6 +25,17 @@ Window CNFGWindow; Pixmap CNFGPixmap; GC CNFGGC; GC CNFGWindowGC; +Visual * CNFGVisual; + + +#ifdef CNFGOGL +#include +#include + +GLXContext CNFGCtx; +void * CNFGGetExtension( const char * extname ) { return glXGetProcAddressARB((const GLubyte *) extname); } +#endif + int FullScreen = 0; void CNFGGetDimensions( short * x, short * y ) @@ -86,7 +97,7 @@ void CNFGSetupFullscreen( const char * WindowName, int screen_no ) exit( 1 ); } - Visual * visual = DefaultVisual(CNFGDisplay, screen); + CNFGVisual = DefaultVisual(CNFGDisplay, screen); CNFGWinAtt.depth = DefaultDepth(CNFGDisplay, screen); if (XineramaQueryExtension(CNFGDisplay, &a, &b ) && @@ -117,7 +128,9 @@ void CNFGSetupFullscreen( const char * WindowName, int screen_no ) CNFGWindow = XCreateWindow(CNFGDisplay, XRootWindow(CNFGDisplay, screen), xpos, ypos, CNFGWinAtt.width, CNFGWinAtt.height, - 0, CNFGWinAtt.depth, InputOutput, visual, CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, &setwinattr); + 0, CNFGWinAtt.depth, InputOutput, CNFGVisual, + CWBorderPixel | CWEventMask | CWOverrideRedirect | CWSaveUnder, + &setwinattr); XMapWindow(CNFGDisplay, CNFGWindow); XSetInputFocus( CNFGDisplay, CNFGWindow, RevertToParent, CurrentTime ); @@ -155,7 +168,31 @@ void CNFGSetup( const char * WindowName, int w, int h ) XGetWindowAttributes( CNFGDisplay, RootWindow(CNFGDisplay, 0), &CNFGWinAtt ); int depth = CNFGWinAtt.depth; - CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CopyFromParent, 0, 0 ); + int screen = DefaultScreen(CNFGDisplay); + CNFGVisual = DefaultVisual(CNFGDisplay, screen); + +#ifdef CNFGOGL + int attribs[] = { GLX_RGBA, + GLX_DOUBLEBUFFER, + GLX_RED_SIZE, 1, + GLX_GREEN_SIZE, 1, + GLX_BLUE_SIZE, 1, + GLX_DEPTH_SIZE, 1, + None }; + XVisualInfo * vis = glXChooseVisual(CNFGDisplay, screen, attribs); + CNFGVisual = vis->visual; + depth = vis->depth; + CNFGCtx = glXCreateContext( CNFGDisplay, vis, NULL, True ); +#endif + + XSetWindowAttributes attr; + attr.background_pixel = 0; + attr.border_pixel = 0; + attr.colormap = XCreateColormap( CNFGDisplay, RootWindow(CNFGDisplay, 0), CNFGVisual, AllocNone); + attr.event_mask = StructureNotifyMask | ExposureMask | KeyPressMask; + int mask = CWBackPixel | CWBorderPixel | CWColormap | CWEventMask; + + CNFGWindow = XCreateWindow(CNFGDisplay, RootWindow(CNFGDisplay, 0), 1, 1, w, h, 0, depth, InputOutput, CNFGVisual, mask, &attr ); XMapWindow(CNFGDisplay, CNFGWindow); XFlush(CNFGDisplay); @@ -163,7 +200,12 @@ void CNFGSetup( const char * WindowName, int w, int h ) Atom WM_DELETE_WINDOW = XInternAtom( CNFGDisplay, "WM_DELETE_WINDOW", False ); XSetWMProtocols( CNFGDisplay, CNFGWindow, &WM_DELETE_WINDOW, 1 ); + XSelectInput( CNFGDisplay, CNFGWindow, KeyPressMask | KeyReleaseMask | ButtonPressMask | ButtonReleaseMask | ExposureMask | PointerMotionMask ); + +#ifdef CNFGOGL + glXMakeCurrent( CNFGDisplay, CNFGWindow, CNFGCtx ); +#endif } void CNFGHandleInput() @@ -226,7 +268,6 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) if( !xi ) { int screen = DefaultScreen(CNFGDisplay); - Visual * visual = DefaultVisual(CNFGDisplay, screen); depth = DefaultDepth(CNFGDisplay, screen)/8; // xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); // lw = w; @@ -236,7 +277,7 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) if( lw != w || lh != h ) { if( xi ) free( xi ); - xi = XCreateImage(CNFGDisplay, DefaultVisual( CNFGDisplay, DefaultScreen(CNFGDisplay) ), depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); + xi = XCreateImage(CNFGDisplay, CNFGVisual, depth*8, ZPixmap, 0, (char*)data, w, h, 32, w*4 ); lw = w; lh = h; } @@ -247,7 +288,25 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) } -#ifndef RASTERIZER +#ifdef CNFGOGL + +void CNFGSetVSync( int vson ) +{ + void (*glfn)( int ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalMESA" ); if( glfn ) glfn( vson ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalSGI" ); if( glfn ) glfn( vson ); + glfn = (void (*)( int ))CNFGGetExtension( "glXSwapIntervalEXT" ); if( glfn ) glfn( vson ); +} + +void CNFGSwapBuffers() +{ + glFlush(); + glFinish(); + glXSwapBuffers( CNFGDisplay, CNFGWindow ); +} +#endif + +#if !defined( RASTERIZER ) && !defined( CNFGOGL) uint32_t CNFGColor( uint32_t RGB ) @@ -302,3 +361,4 @@ void CNFGInternalResize( short x, short y ) { } #include "CNFGRasterizer.h" #endif + -- cgit v1.2.3 From c72b1dd68a653dff2be9915750d2f6535a6db3bd Mon Sep 17 00:00:00 2001 From: CNLohr Date: Wed, 22 Mar 2017 04:44:15 -0400 Subject: Fix Windows compile if OGL is turned on. --- redist/CNFGFunctions.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/redist/CNFGFunctions.c b/redist/CNFGFunctions.c index e93eb6f..25e9298 100644 --- a/redist/CNFGFunctions.c +++ b/redist/CNFGFunctions.c @@ -274,6 +274,10 @@ void CNFGDrawTextbox( int x, int y, const char * text, int textsize ) #ifdef CNFGOGL +#ifdef _MSC_VER +#include +#pragma comment( lib, "OpenGL32.lib" ) +#endif #include uint32_t CNFGColor( uint32_t RGB ) -- cgit v1.2.3 From 39ef5af74702c8825a82f65cf68e6af875a814ee Mon Sep 17 00:00:00 2001 From: ultramn Date: Thu, 23 Mar 2017 00:41:22 -0400 Subject: Added a dynamic plotting tool for OrthoSolve. Charles added orthogonalization to the rotation matrix. --- dave/AffineSolve | Bin 65424 -> 40104 bytes dave/AffineSolve.c | 1241 +++++++++++++++++++++++++------------------------ dave/Makefile | 20 +- dave/OrthoPlot.c | 451 ++++++++++++++++++ dave/dclapack_test.c | 2 +- dave/fileutil.c | 133 ++++++ dave/fileutil.h | 35 ++ src/poser_daveortho.c | 85 +++- src/survive_cal.h | 2 +- src/survive_data.c | 2 +- 10 files changed, 1356 insertions(+), 615 deletions(-) create mode 100644 dave/OrthoPlot.c create mode 100644 dave/fileutil.c create mode 100644 dave/fileutil.h diff --git a/dave/AffineSolve b/dave/AffineSolve index 98a9590..7d51b34 100755 Binary files a/dave/AffineSolve and b/dave/AffineSolve differ diff --git a/dave/AffineSolve.c b/dave/AffineSolve.c index 4fba56b..685062e 100644 --- a/dave/AffineSolve.c +++ b/dave/AffineSolve.c @@ -11,10 +11,13 @@ #include #include "dclapack.h" #include +#define RegisterDriver(a,b) +#include "poser_daveortho.c" #define LH_ID 0 #define NUM_HMD 32 +#define INDIR "full_test_triangle_on_floor/" -#define MAX_POINTS 128 +#define MAX_POINTS SENSORS_PER_OBJECT //#define _ABS(a) ( (a)<=0 ? -(a) : (a) ) #define _SIGN(a) ( (a)<=0 ? -1.0f : 1.0f ) #define RANDF ( (float)rand() / (float)RAND_MAX ) @@ -31,7 +34,7 @@ float hmd_pos[NUM_HMD][3]; void ReadHmdPoints() { int i; - FILE *fin = fopen("HMD_points.csv","r"); + FILE *fin = fopen(INDIR "HMD_points.csv","r"); if (fin==NULL) { printf("ERROR: could not open HMD_points.csv for reading\n"); exit(1); @@ -52,7 +55,7 @@ void ReadPtinfo() for (i=0; i TOO_SMALL) { + float scaleRot = stepSizeRot / gradSizeRot; + for (j=0; j<3; j++) { + for (k=0; k<3; k++) { + T[j][k] += scaleRot * conj[j][k]; + } + } + stepSizeRot *= falloff; + } + + if (gradSizePos > TOO_SMALL) { + float scalePos = stepSizePos / gradSizePos; + for (j=0; j<3; j++) { + T[j][3] += scalePos * conj[j][3]; + } + stepSizePos *= falloff; + } + + // Constrain the gradient (such that scaling is one) + if (constrain) + { + // Measure the scales + float len[3] = {0.0, 0.0, 0.0}; + for (j=0; j<3; j++) { + double lenSq = 0.0; + for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } + len[j] = sqrt(lenSq); + } - // Calculate the error term - FLOAT xy_dot = xy_dot2 + x_y*y_y; - FLOAT yz_dot = yz_dot2 + y_y*z_y; - FLOAT zx_dot = zx_dot2 + z_y*x_y; - FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot); - - // Calculate the handedness - FLOAT cx,cy,cz; - CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); - FLOAT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); - - // If we are the best right-handed frame so far - //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } - if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } - z_y = -z_y; + // How far off is the scale? + float xzLen = 0.5 * (len[0] + len[2]); + if (xzLen > TOO_SMALL) { + float inv_xzLen = 1.0 / xzLen; + for (j=0; j<3; j++) { + T[3][j] *= inv_xzLen; + } + } + + // Rescale the thing + for (j=0; j<3; j++) + { + if (len[j] > TOO_SMALL) { + float inv_len = 1.0 / len[j]; + for (k=0; k<3; k++) { T[j][k] *= inv_len; } + } } - y_y = -y_y; } - x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ + float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); + printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); +} + +int main() +{ + int i,j,k,sen,axis; + + // Read the data files + ReadHmdPoints(); + ReadPtinfo(); //------------------------- - // A test version of the rescaling to the proper length + // Package the lighthouse data for "AffineSolve" //------------------------- - FLOAT ydist2; - FLOAT bestBestErr = 9999.0; - FLOAT bestYdist = 0; - for (ydist2=ydist-0.1; ydist2 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } - z_y = -z_y; - } - y_y = -y_y; - } - x_y = -x_y; - } - printf("ydist2 %f bestErr %f\n",ydist2,bestErr); + for (sen=0; sen 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + if ( i == 0 && j == 1 && k == 0) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } + z_y = -z_y; } -/* } else { - // Calculate "beta" for Fletcher Reeves method - float beta = gradDot / prevGradDot; -//printf("gradDot %f prevGradDot %f beta %f\n", gradDot, prevGradDot, beta); - - // Update the conjugate - for (j=0; j<3; j++) { - for (k=0; k<4; k++) { - conj[j][k] = beta*conj[j][k] - de_dT[j][k]; - } - } + y_y = -y_y; } + x_y = -x_y; + } + printf("bestErr %f\n", bestErr); */ -// PRINT_MAT(de_dT,4,4); -// exit(1); + //------------------------- + // A test version of the rescaling to the proper length + //------------------------- + FLOAT ydist2; + FLOAT bestBestErr = 9999.0; + FLOAT bestYdist = 0; + for (ydist2=ydist-0.1; ydist2 TOO_SMALL) { - float scaleRot = stepSizeRot / gradSizeRot; - for (j=0; j<3; j++) { - for (k=0; k<3; k++) { - T[j][k] += scaleRot * conj[j][k]; - } - } - stepSizeRot *= falloff; - } - - if (gradSizePos > TOO_SMALL) { - float scalePos = stepSizePos / gradSizePos; - for (j=0; j<3; j++) { - T[j][3] += scalePos * conj[j][3]; - } - stepSizePos *= falloff; - } - - // Constrain the gradient (such that scaling is one) - if (constrain) - { - // Measure the scales - float len[3] = {0.0, 0.0, 0.0}; - for (j=0; j<3; j++) { - double lenSq = 0.0; - for (k=0; k<3; k++) { lenSq += (double)T[j][k] * (double)T[j][k]; } - len[j] = sqrt(lenSq); - } - - // How far off is the scale? - float xzLen = 0.5 * (len[0] + len[2]); - if (xzLen > TOO_SMALL) { - float inv_xzLen = 1.0 / xzLen; - for (j=0; j<3; j++) { - T[3][j] *= inv_xzLen; - } - } + // we know the distance into (or out of) the camera for the z axis, + // but we don't know which direction . . . + FLOAT x_y = sqrt(1.0 - x2[0][0]*x2[0][0] - x2[2][0]*x2[2][0]); + FLOAT y_y = sqrt(1.0 - y2[0][0]*y2[0][0] - y2[2][0]*y2[2][0]); + FLOAT z_y = sqrt(1.0 - z2[0][0]*z2[0][0] - z2[2][0]*z2[2][0]); + + // Exhaustively flip the minus sign of the z axis until we find the right one . . . + FLOAT bestErr = 9999.0; + FLOAT xy_dot2 = x2[0][0]*y2[0][0] + x2[2][0]*y2[2][0]; + FLOAT yz_dot2 = y2[0][0]*z2[0][0] + y2[2][0]*z2[2][0]; + FLOAT zx_dot2 = z2[0][0]*x2[0][0] + z2[2][0]*x2[2][0]; + for (i=0;i<2;i++) { + for (j=0;j<2;j++) { + for(k=0;k<2;k++) { - // Rescale the thing - for (j=0; j<3; j++) - { - if (len[j] > TOO_SMALL) { - float inv_len = 1.0 / len[j]; - for (k=0; k<3; k++) { T[j][k] *= inv_len; } + // Calculate the error term + FLOAT xy_dot = xy_dot2 + x_y*y_y; + FLOAT yz_dot = yz_dot2 + y_y*z_y; + FLOAT zx_dot = zx_dot2 + z_y*x_y; + FLOAT err = _ABS(xy_dot) + _ABS(yz_dot) + _ABS(zx_dot); + + // Calculate the handedness + FLOAT cx,cy,cz; + CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]); + FLOAT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0]; +// printf("err %f hand %f\n", err, hand); + + // If we are the best right-handed frame so far + if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } + z_y = -z_y; } + y_y = -y_y; } + x_y = -x_y; + } + printf("ydist2 %f bestErr %f\n",ydist2,bestErr); + + if (bestErr < bestBestErr) { + memcpy(x,x2,3*sizeof(FLOAT)); + memcpy(y,y2,3*sizeof(FLOAT)); + memcpy(z,z2,3*sizeof(FLOAT)); + bestBestErr = bestErr; + bestYdist = ydist2; } } - float dist = sqrt(T[0][3]*T[0][3] + T[1][3]*T[1][3] + T[2][3]*T[2][3]); - printf("AffineSolve: pos: %f %f %f dist: %f\n", T[0][3], T[1][3], T[2][3], dist); -} + ydist = bestYdist; -int main() -{ - int i,j,k,sen,axis; - - // Read the data files - ReadHmdPoints(); - ReadPtinfo(); +/* + for (i=0; i // Standard Header For Most Programs +#include +#include +#include +#include "os_generic.h" +#include "linmath.h" +#include "fileutil.h" + +#ifdef __APPLE__ +#include // The GL Header File +#include // The GL Utility Toolkit (Glut) Header +#else +#include +#include +#endif +#ifdef __linux__ +#include +#endif + +#define RegisterDriver(a,b) +#include "poser_daveortho.c" + + +// Required to set up a window +#define WIDTH 1280 +#define HEIGHT 1280 +#define FULLSCREEN 0 +int keys[256]; // Regular keyboard keys +int sp_keys[256]; // Special keyboard keycodes (GLUT specific) + +#define LH_ID 0 +#define NUM_HMD 32 +#define INDIR "dave/full_test_triangle_on_floor/" +#define MAX_POINTS SENSORS_PER_OBJECT + +#define PI 3.1415926535897932386264 +#define MOVESPEED 1.0 +#define ROTSPEED 5.0 + +// View space +float posx=0.0f; +float posy=0.0f; +float posz=0.0f; +float rotx=0.0f; +float roty=0.0f; +float rotz=0.0f; + +// Data for the "fake" ortho solve formula +float Tortho[4][4]; // OUTPUT: 4x4 transformation matrix +FLOAT S_out[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT S_in[2][MAX_POINTS]; // INPUT: array of screenspace points +FLOAT X_in[3][MAX_POINTS]; // INPUT: array of offsets +int nPoints=0; + +//-------------------------------------------------------------------- +// +//-------------------------------------------------------------------- + +void DrawGrid( + float minX, float maxX, + float minY, float maxY, + float minZ, float maxZ, + float stepX, float stepY, float stepZ); + +void DrawCoordinateSystem( + float x, float y, float z, + float qx, float qy, float qz, float qr); + + +float hmd_pos[NUM_HMD][3]; +void ReadHmdPoints() +{ + int i; + FILE *fin = fopen(INDIR "HMD_points.csv","r"); + if (fin==NULL) { + printf("ERROR: could not open " INDIR "HMD_points.csv for reading\n"); + exit(1); + } + + for (i=0; i= read_frameno-6 && + read_hmdAngleViewed[sweepy][i] >= read_frameno-6 && + hmdAngles[sweepx][i]!=-9999.0 && hmdAngles[sweepy][i]!=-9999.0) + { + S_in[0][nPoints] = hmdAngles[sweepy][i]; + S_in[1][nPoints] = hmdAngles[sweepx][i]; + X_in[0][nPoints] = hmd_pos[i][0]; + X_in[1][nPoints] = hmd_pos[i][1]; + X_in[2][nPoints] = hmd_pos[i][2]; +printf("i %d S %f %f X %f %f %f frno %d %d currfr %d\n", + i, S_in[0][nPoints], S_in[1][nPoints], + X_in[0][nPoints], X_in[1][nPoints], X_in[2][nPoints], + read_hmdAngleViewed[sweepx][i], read_hmdAngleViewed[sweepy][i], read_frameno); + nPoints++; + } + } + + read_frameno++; + + //-------------------------------------------------- + // Run the "OrthoSolve" and then the "AffineSolve" + //-------------------------------------------------- + + // Run OrthoSolve + OrthoSolve( + Tortho, // OUTPUT: 4x4 transformation matrix + S_out, // OUTPUT: array of output screenspace points + S_in, // INPUT: array of screenspace points + X_in, // INPUT: array of offsets + nPoints); + printf( "POS: %f %f %f\n", Tortho[0][3], Tortho[1][3], Tortho[2][3]); + + + //------------------------ + // Draw the inputs + //------------------------ + glPointSize(3.0); + + // Draw the input points + glColor3f(1.0,0.5,0.5); + glBegin(GL_POINTS); + for (i=0; i #include diff --git a/dave/fileutil.c b/dave/fileutil.c new file mode 100644 index 0000000..04dc241 --- /dev/null +++ b/dave/fileutil.c @@ -0,0 +1,133 @@ +#include "fileutil.h" +#include +#include + +#define PI 3.14159265358979323846264 + + +og_mutex_t read_mutex; +og_thread_t read_thread; +double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +int read_frameno=0; + +static FILE *fopen_orDie(const char *path, const char *flag) +{ + FILE *f = fopen(path, flag); + if (f == NULL) { + printf("ERROR could not oepn %s for %s\n", path, flag); + exit(1); + } + return f; +} + +static void SeekToken(FILE *f, const char *keyword) +{ + char token[4096]; + do { + fscanf(f, "%s", token); + } while( strcmp(token,keyword)!=0 && !feof(f) ); +} + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal) +{ + FILE *f = fopen_orDie(path,"r"); + SeekToken(f, "POS:"); + fscanf(f, "%f %f %f\n", x, y, z); + SeekToken(f, "QUAT:"); + fscanf(f, "%f %f %f %f\n", qreal, qi, qj, qk); + fclose (f); +} + +void LoadHmdProcessedDataAngles( + const char *path, + double angles[NUM_SWEEP][NUM_HMD]) +{ + int i,j; + char type[256]; + char sweep[256]; + int id; + int nSweep; + double ang; + double d1,d2,d3; // revisit these numbers later + + // Initialize all of the angles to -9999 + for (i=0; iNUM_HMD) { continue; } + + OGLockMutex (read_mutex); + read_hmdAngles[sweepId][id] = angle; + OGUnlockMutex(read_mutex); + read_hmdAngleViewed[sweepId][id] = read_frameno; + } + } +} + + diff --git a/dave/fileutil.h b/dave/fileutil.h new file mode 100644 index 0000000..e5da244 --- /dev/null +++ b/dave/fileutil.h @@ -0,0 +1,35 @@ +#ifndef _fileutil_h_ +#define _fileutil_h_ + +#include +#include "os_generic.h" + +void LoadLighthousePos( + const char *path, + float *x, float *y, float *z, + float *qi, float *qj, float *qk, float *qreal); + + +// first 32 are hmd, next 24 wm0 next 24 wm1 +#define NUM_HMD 80 +#define NUM_SWEEP 4 +#define SWEEP_LX 0 +#define SWEEP_LY 1 +#define SWEEP_RX 2 +#define SWEEP_RY 3 +void LoadHmdProcessedDataAngles( + const char *path, + double angle[NUM_SWEEP][NUM_HMD]); + + +extern og_mutex_t read_mutex; +extern og_thread_t read_thread; +extern double read_hmdAngles[NUM_SWEEP][NUM_HMD]; +extern int read_hmdAngleViewed[NUM_SWEEP][NUM_HMD]; +extern int read_frameno; +void *ThreadReadHmtAngles(void *junk); + + +#endif // __fileutil_h_ + + diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index e81e154..80f65a9 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -265,7 +265,8 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); // FLT ydist1 = 1.0 / uhat_len; //0.25*PI / uhat_len; // FLT ydist2 = 1.0 / rhat_len; //0.25*PI / rhat_len; FLT ydist = 1.0 / urhat_len; - //printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); + printf("ydist %f\n", ydist); +// printf("ydist1 %f ydist2 %f ydist %f\n", ydist1, ydist2, ydist); //-------------------- // Rescale the axies to be of the proper length @@ -282,7 +283,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( x_y != x_y ) x_y = 0; if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; -/* + // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; FLT xy_dot2 = x[0][0]*y[0][0] + x[2][0]*y[2][0]; @@ -302,7 +303,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x[0][0],x_y,x[2][0],y[0][0],y_y,y[2][0]); FLT hand = cx*z[0][0] + cy*z_y + cz*z[2][0]; - printf("err %f hand %f\n", err, hand); +// printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far //if (hand > 0 && err < bestErr) { x[1][0]=x_y; y[1][0]=y_y; z[1][0]=z_y; bestErr=err; } @@ -313,9 +314,9 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } x_y = -x_y; } - printf("bestErr %f\n", bestErr); -*/ +// printf("bestErr %f\n", bestErr); +/* //------------------------- // A test version of the rescaling to the proper length //------------------------- @@ -338,7 +339,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); if( y_y != y_y ) y_y = 0; if( z_y != z_y ) z_y = 0; - printf( "---> %f %f %f\n", x_y, y_y, z_y ); +// printf( "---> %f %f %f\n", x_y, y_y, z_y ); // Exhaustively flip the minus sign of the z axis until we find the right one . . . FLT bestErr = 9999.0; @@ -359,7 +360,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); FLT cx,cy,cz; CrossProduct(cx,cy,cz,x2[0][0],x_y,x2[2][0],y2[0][0],y_y,y2[2][0]); FLT hand = cx*z2[0][0] + cy*z_y + cz*z2[2][0]; - printf("err %f hand %f\n", err, hand); + //printf("err %f hand %f\n", err, hand); // If we are the best right-handed frame so far if (hand > 0 && err < bestErr) { x2[1][0]=x_y; y2[1][0]=y_y; z2[1][0]=z_y; bestErr=err; } @@ -380,7 +381,7 @@ printf("rhat %f %f (len %f)\n", rhat[0][0], rhat[1][0], rhat_len); } } ydist = bestYdist; - +*/ /* for (i=0; itimebase_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 ); + //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 ); } } -- cgit v1.2.3