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 --- src/survive_imu.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) (limited to 'src') 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(-) (limited to 'src') 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 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(-) (limited to 'src') 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