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.2.3 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.2.3 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 --- src/survive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') 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) -- cgit v1.2.3