From 9dc7a7ed589db16cc040c7eeeae0343977f3f885 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 07:35:41 -0600 Subject: Added proper initialization to imu tracker --- include/libsurvive/survive_imu.h | 3 +++ src/survive_imu.c | 9 +++++++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 8a86425..11635aa 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -4,6 +4,7 @@ #include "poser.h" #include "survive_types.h" #include +#include #ifdef __cplusplus extern "C" { @@ -12,6 +13,8 @@ extern "C" { struct SurviveIMUTracker_p; typedef struct { + bool is_initialized; + FLT updir[3]; FLT accel_scale_bias; diff --git a/src/survive_imu.c b/src/survive_imu.c index 36d1aeb..8f4266a 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -174,7 +174,7 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do } void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { - if (tracker->last_data.timecode == 0) { + if (!tracker->is_initialized) { tracker->pose.Rot[0] = 1.; if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; @@ -182,7 +182,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker const FLT up[3] = {0, 0, 1}; quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir); - + tracker->is_initialized = true; return; } @@ -233,6 +233,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker, SurvivePose *pose, const FLT *R) { + if (!tracker->is_initialized) { + tracker->pose = *pose; + return; + } + // Kalman filter assuming: // F -> Identity // H -> Identity -- cgit v1.3.1 From 5d43c7f5541b91f29aa3138a8a460f48198e11d0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 07:36:23 -0600 Subject: Fixed terrible bug in SBA which slowed it way down --- src/poser_sba.c | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/src/poser_sba.c b/src/poser_sba.c index fcf4f2e..23e03fc 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -399,7 +399,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->failures_to_reset_cntr = 0; d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); d->successes_to_reset_cntr = 0; - d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100); + d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, -1); d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1); d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); @@ -435,27 +435,28 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->last_lh = lightData->lh; d->last_acode = lightData->acode; - } - if (error < 0) { - if (d->failures_to_reset_cntr > 0) - d->failures_to_reset_cntr--; - } else { - if (d->useIMU) { - FLT var_meters = 0.5; - FLT var_quat = error + .05; - FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, - error * var_quat, error * var_quat, error * var_quat}; - - survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); - estimate = d->tracker.pose; + + if (error < 0) { + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; } + else { + if (d->useIMU) { + FLT var_meters = 0.5; + FLT var_quat = error + .05; + FLT var[7] = { error * var_meters, error * var_meters, error * var_meters, error * var_quat, + error * var_quat, error * var_quat, error * var_quat }; + + survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); + estimate = d->tracker.pose; + } - PoserData_poser_pose_func(&lightData->hdr, so, &estimate); - if (d->successes_to_reset_cntr > 0) - d->successes_to_reset_cntr--; + PoserData_poser_pose_func(&lightData->hdr, so, &estimate); + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; + } } - return 0; } case POSERDATA_FULL_SCENE: { -- cgit v1.3.1 From c1f59615f9639bae75cd0e1a7cffe118ac6a7b41 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 07:37:50 -0600 Subject: Updated winbuild to include imu tracker --- winbuild/calibrate/calibrate.vcxproj | 82 ++++++++++++++++++++++++++ winbuild/data_recorder/data_recorder.vcxproj | 78 ++++++++++++++++++++++++ winbuild/libsurvive.sln | 3 + winbuild/libsurvive/libsurvive.vcxproj | 71 +++++++++++++++++++++- winbuild/libsurvive/libsurvive.vcxproj.filters | 3 + winbuild/test/test.vcxproj | 78 ++++++++++++++++++++++++ 6 files changed, 313 insertions(+), 2 deletions(-) diff --git a/winbuild/calibrate/calibrate.vcxproj b/winbuild/calibrate/calibrate.vcxproj index d0b1c48..feb72fd 100644 --- a/winbuild/calibrate/calibrate.vcxproj +++ b/winbuild/calibrate/calibrate.vcxproj @@ -17,6 +17,14 @@ Release x64 + + RelWDebInfo + Win32 + + + RelWDebInfo + x64 + 15.0 @@ -39,6 +47,13 @@ true MultiByte + + Application + false + v141 + true + MultiByte + Application true @@ -52,6 +67,13 @@ true MultiByte + + Application + false + v141 + true + MultiByte + @@ -63,12 +85,18 @@ + + + + + + true @@ -80,9 +108,15 @@ false + + false + false + + false + @@ -152,6 +186,30 @@ true + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) + + + Console + true + true + + + setupapi.lib;dbghelp.lib;%(AdditionalDependencies) + true + + + true + + Level3 @@ -176,6 +234,30 @@ true + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) + + + Console + true + true + + + setupapi.lib;dbghelp.lib;%(AdditionalDependencies) + true + + + true + + diff --git a/winbuild/data_recorder/data_recorder.vcxproj b/winbuild/data_recorder/data_recorder.vcxproj index e63d474..9f3f86a 100644 --- a/winbuild/data_recorder/data_recorder.vcxproj +++ b/winbuild/data_recorder/data_recorder.vcxproj @@ -17,6 +17,14 @@ Release x64 + + RelWDebInfo + Win32 + + + RelWDebInfo + x64 + 15.0 @@ -39,6 +47,13 @@ true MultiByte + + Application + false + v141 + true + MultiByte + Application true @@ -52,6 +67,13 @@ true MultiByte + + Application + false + v141 + true + MultiByte + @@ -63,12 +85,18 @@ + + + + + + true @@ -80,9 +108,15 @@ false + + false + false + + false + @@ -143,6 +177,28 @@ true + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + true + true + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + + + true + + Level3 @@ -165,6 +221,28 @@ true + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + true + true + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + + + true + + {435cfd2c-8724-42ee-8fde-71ef7d2c6b2f} diff --git a/winbuild/libsurvive.sln b/winbuild/libsurvive.sln index b525975..3a8b2da 100644 --- a/winbuild/libsurvive.sln +++ b/winbuild/libsurvive.sln @@ -55,4 +55,7 @@ Global GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {5C1AB415-DC8F-42C0-B2E0-7300D0C79E09} + EndGlobalSection EndGlobal diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 4b76c99..520279c 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -17,6 +17,14 @@ Release x64 + + RelWDebInfo + Win32 + + + RelWDebInfo + x64 + 15.0 @@ -39,6 +47,13 @@ true MultiByte + + StaticLibrary + false + v141 + true + MultiByte + DynamicLibrary true @@ -46,7 +61,14 @@ MultiByte - StaticLibrary + DynamicLibrary + false + v141 + true + MultiByte + + + DynamicLibrary false v141 true @@ -64,12 +86,18 @@ + + + + + + $(IncludePath) @@ -132,6 +160,24 @@ + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) + ..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) + + + Windows + true + true + + + Level3 @@ -140,13 +186,33 @@ MaxSpeed true true - USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;NDEBUG;_LIB;%(PreprocessorDefinitions) + FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions) ..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) Windows true true + DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies) + + + + + + Level3 + + + MaxSpeed + true + true + FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions) + ..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) + + + Windows + true + true + DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies) @@ -184,6 +250,7 @@ + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 1f98c68..d06f083 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -135,6 +135,9 @@ Source Files + + Source Files + diff --git a/winbuild/test/test.vcxproj b/winbuild/test/test.vcxproj index 8df96d3..ae130b4 100644 --- a/winbuild/test/test.vcxproj +++ b/winbuild/test/test.vcxproj @@ -17,6 +17,14 @@ Release x64 + + RelWDebInfo + Win32 + + + RelWDebInfo + x64 + 15.0 @@ -39,6 +47,13 @@ true Unicode + + Application + false + v141 + true + Unicode + Application true @@ -52,6 +67,13 @@ true Unicode + + Application + false + v141 + true + Unicode + @@ -63,12 +85,18 @@ + + + + + + true @@ -81,9 +109,15 @@ false + + false + false + + false + @@ -144,6 +178,28 @@ true + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + true + true + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + + + true + + Level3 @@ -166,6 +222,28 @@ true + + + Level3 + + + MaxSpeed + true + true + USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + ..\..\windows;..\..\include\libsurvive;..\..\redist; + + + Console + true + true + setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies) + true + + + true + + {435cfd2c-8724-42ee-8fde-71ef7d2c6b2f} -- cgit v1.3.1 From b792a65dbfbb304927400ca6c754195d92089b61 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 08:46:50 -0600 Subject: Fixed 32 bit linker issue --- include/libsurvive/survive.h | 3 +-- src/survive.c | 2 +- winbuild/libsurvive/libsurvive.vcxproj | 30 ++++++++++++++++++++++++------ 3 files changed, 26 insertions(+), 9 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 65343b7..c1bb52c 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -323,8 +323,7 @@ void RegisterDriver(const char *name, void *data); #ifdef _MSC_VER #define REGISTER_LINKTIME(func) \ - __pragma(comment(linker, "/export:REGISTER" #func)); \ - void REGISTER##func() { RegisterDriver(#func, &func); } + SURVIVE_EXPORT void REGISTER##func() { RegisterDriver(#func, &func); } #else #define REGISTER_LINKTIME(func) \ void __attribute__((constructor)) REGISTER##func() { RegisterDriver(#func, &func); } diff --git a/src/survive.c b/src/survive.c index b024bbb..9e750f9 100644 --- a/src/survive.c +++ b/src/survive.c @@ -106,7 +106,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { static int did_manual_driver_registration = 0; if (did_manual_driver_registration == 0) { #define MANUAL_DRIVER_REGISTRATION(func) \ - int func(SurviveObject *so, PoserData *pd); \ + int func(SurviveObject *so, PoserData *pd); \ RegisterDriver(#func, &func); MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow) diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 520279c..fb1b9a5 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -41,14 +41,14 @@ MultiByte - StaticLibrary + DynamicLibrary false v141 true MultiByte - StaticLibrary + DynamicLibrary false v141 true @@ -117,7 +117,7 @@ Windows DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies) true - $(IntDir)Test.txt + libsurvive.map true @@ -134,11 +134,15 @@ Level3 Disabled - USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions) + FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions) ..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) Windows + DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies) + true + true + libsurvive.map @@ -150,13 +154,17 @@ MaxSpeed true true - USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) + FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions) ..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) Windows true true + DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies) + true + true + libsurvive.map @@ -168,13 +176,17 @@ MaxSpeed true true - USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions) + FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions) ..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories) Windows true true + DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies) + true + true + libsurvive.map @@ -194,6 +206,9 @@ true true DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies) + true + true + libsurvive.map @@ -213,6 +228,9 @@ true true DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies) + true + true + libsurvive.map -- cgit v1.3.1