From f60bed509a7e416c155bcd35d5151bca65eaa190 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 11:20:34 -0600 Subject: IMU research --- Makefile | 3 +- include/libsurvive/survive_imu.h | 27 ++++++++++++ redist/linmath.h | 2 +- src/poser_imu.c | 31 ++++++++++++++ src/survive_imu.c | 89 ++++++++++++++++++++++++++++++++++++++++ 5 files changed, 150 insertions(+), 2 deletions(-) create mode 100644 include/libsurvive/survive_imu.h create mode 100644 src/poser_imu.c create mode 100644 src/survive_imu.c diff --git a/Makefile b/Makefile index a6d139b..de74589 100644 --- a/Makefile +++ b/Makefile @@ -39,7 +39,8 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o + +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_imu.o src/poser_imu.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h new file mode 100644 index 0000000..323cb6a --- /dev/null +++ b/include/libsurvive/survive_imu.h @@ -0,0 +1,27 @@ +#ifndef _SURVIVE_IMU_H +#define _SURVIVE_IMU_H + +#include "poser.h" +#include "survive_types.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct { + FLT updir[3]; + LinmathVec3d current_velocity; + PoserDataIMU last_data; + SurvivePose pose; + +} SurviveIMUTracker; + +void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, SurvivePose *pose); +void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data); + +#ifdef __cplusplus +}; +#endif + +#endif diff --git a/redist/linmath.h b/redist/linmath.h index cacb1c6..5d5bed2 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -49,7 +49,7 @@ typedef FLT LinmathQuat[4]; // This is the [wxyz] quaternion, in wxyz format. typedef FLT LinmathPoint3d[3]; -typedef FLT linmathVec3d[3]; +typedef FLT LinmathVec3d[3]; typedef struct LinmathPose { LinmathPoint3d Pos; diff --git a/src/poser_imu.c b/src/poser_imu.c new file mode 100644 index 0000000..8ca716b --- /dev/null +++ b/src/poser_imu.c @@ -0,0 +1,31 @@ +#include +#include + +#include +#include + +int PoserIMU(SurviveObject *so, PoserData *pd) { + PoserType pt = pd->pt; + SurviveContext *ctx = so->ctx; + SurviveIMUTracker *dd = so->PoserData; + + if (!dd) { + so->PoserData = dd = malloc(sizeof(SurviveIMUTracker)); + *dd = (SurviveIMUTracker){}; + } + + switch (pt) { + case POSERDATA_IMU: { + PoserDataIMU *imu = (PoserDataIMU *)pd; + + survive_imu_tracker_integrate(so, dd, imu); + + PoserData_poser_raw_pose_func(pd, so, -1, &dd->pose); + + return 0; + } + } + return -1; +} + +REGISTER_LINKTIME(PoserIMU); diff --git a/src/survive_imu.c b/src/survive_imu.c new file mode 100644 index 0000000..6fb3076 --- /dev/null +++ b/src/survive_imu.c @@ -0,0 +1,89 @@ +#include "survive_imu.h" +#include "linmath.h" +#include "survive_internal.h" +#include + +void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, SurvivePose *pose) { tracker->pose = *pose; } + +static const int imu_calibration_iterations = 100; + +static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const LinmathVec3d accel) { + quatrotatevector(rAcc, pose->Rot, accel); + scale3d(rAcc, rAcc, 2.); + LinmathVec3d G = {0, 0, -1}; + add3d(rAcc, rAcc, G); +} +static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d vel, double time_diff, + const PoserDataIMU *pIMU) { + SurvivePose result = *pose; + + FLT acc_mul = time_diff * time_diff / 2; + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, pIMU->accel); + scale3d(rAcc, rAcc, acc_mul); + + LinmathVec3d gyro; + + for (int i = 0; i < 3; i++) { + result.Pos[i] += time_diff * vel[i] + rAcc[i] * 9.8; + gyro[i] = time_diff / 2 * pIMU->gyro[i]; + } + + LinmathEulerAngle curr, next; + quattoeuler(curr, pose->Rot); + add3d(next, curr, gyro); + quatfromeuler(result.Rot, next); + + return result; +} + +static void iterate_velocity(LinmathVec3d result, const SurvivePose *pose, const LinmathVec3d vel, double time_diff, + PoserDataIMU *pIMU) { + scale3d(result, vel, .99999); + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, pIMU->accel); + scale3d(rAcc, rAcc, time_diff); + add3d(result, result, rAcc); +} + +void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { + if (tracker->last_data.timecode == 0) { + if (tracker->last_data.datamask == imu_calibration_iterations) { + tracker->last_data = *data; + return; + } + + tracker->last_data.datamask++; + + tracker->updir[0] += data->accel[0] / imu_calibration_iterations; + tracker->updir[1] += data->accel[1] / imu_calibration_iterations; + tracker->updir[2] += data->accel[2] / imu_calibration_iterations; + return; + } + + for (int i = 0; i < 3; i++) { + tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; + } + + FLT up[3] = {0, 0, 1}; + FLT pose_up[3] = {0, 0, 1}; + quatrotatevector(pose_up, tracker->pose.Rot, tracker->updir); + + FLT time_diff = (data->timecode - tracker->last_data.timecode) / (FLT)so->timebase_hz; + + SurvivePose t_next = iterate_position(&tracker->pose, tracker->current_velocity, time_diff, data); + + LinmathVec3d v_next; + iterate_velocity(v_next, &tracker->pose, tracker->current_velocity, time_diff, data); + + tracker->pose = t_next; + scale3d(tracker->current_velocity, v_next, 1); + + tracker->last_data = *data; + + FLT tmp[3]; + ApplyPoseToPoint(tmp, &tracker->pose, up); + + printf("[%f, %f, %f] [%f, %f, %f]\n", tracker->pose.Pos[0], tracker->pose.Pos[1], tracker->pose.Pos[2], tmp[0], + tmp[1], tmp[2]); +} -- cgit v1.2.3 From dcf5d7a482e022e762a656253017ebbc721d8a83 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 22 Mar 2018 12:06:41 -0600 Subject: Progress on IMU tracking --- include/libsurvive/survive_imu.h | 6 ++++-- src/poser_imu.c | 2 ++ src/survive_default_devices.c | 9 ++++----- src/survive_imu.c | 38 +++++++++++++++++++++++++++++++++----- src/survive_vive.c | 39 +++++++++++++++++++++++---------------- 5 files changed, 66 insertions(+), 28 deletions(-) diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 323cb6a..a37a4df 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -11,13 +11,15 @@ extern "C" { typedef struct { FLT updir[3]; - LinmathVec3d current_velocity; + LinmathVec3d current_velocity; // Velocity in world frame PoserDataIMU last_data; SurvivePose pose; + SurvivePose lastGT; + uint32_t lastGTTime; } SurviveIMUTracker; -void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, SurvivePose *pose); +void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose); void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data); #ifdef __cplusplus diff --git a/src/poser_imu.c b/src/poser_imu.c index 8ca716b..02ff8e9 100644 --- a/src/poser_imu.c +++ b/src/poser_imu.c @@ -22,6 +22,8 @@ int PoserIMU(SurviveObject *so, PoserData *pd) { PoserData_poser_raw_pose_func(pd, so, -1, &dd->pose); + // if(magnitude3d(dd->pose.Pos) > 1) + // SV_ERROR("IMU drift"); return 0; } } diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 6b65752..2e47b9e 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -144,11 +144,10 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { FLT *values = NULL; if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { so->acc_bias = values; - so->acc_bias[0] *= .125; // XXX Wat? Observed by CNL. Biasing - // by more than this seems to hose - // things. - so->acc_bias[1] *= .125; - so->acc_bias[2] *= .125; + const FLT bias_units = 1. / 1000.; // I deeply suspect bias is in milligravities -JB + so->acc_bias[0] *= bias_units; + so->acc_bias[1] *= bias_units; + so->acc_bias[2] *= bias_units; } } if (jsoneq(ct0conf, tk, "acc_scale") == 0) { diff --git a/src/survive_imu.c b/src/survive_imu.c index 6fb3076..6667d2f 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -3,15 +3,23 @@ #include "survive_internal.h" #include -void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, SurvivePose *pose) { tracker->pose = *pose; } +void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose) { + tracker->pose = *pose; + + for (int i = 0; i < 3; i++) + tracker->current_velocity[i] = pose->Pos[i] - tracker->lastGT.Pos[i]; + + tracker->lastGTTime = timecode; + tracker->lastGT = *pose; +} static const int imu_calibration_iterations = 100; static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const LinmathVec3d accel) { quatrotatevector(rAcc, pose->Rot, accel); - scale3d(rAcc, rAcc, 2.); LinmathVec3d G = {0, 0, -1}; add3d(rAcc, rAcc, G); + scale3d(rAcc, rAcc, 9.8); } static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d vel, double time_diff, const PoserDataIMU *pIMU) { @@ -20,12 +28,16 @@ static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d FLT acc_mul = time_diff * time_diff / 2; LinmathVec3d rAcc = {0}; RotateAccel(rAcc, pose, pIMU->accel); + + fprintf(stderr, "r %f %f %f %f\n", pIMU->accel[0], pIMU->accel[1], pIMU->accel[2], quatmagnitude(pIMU->accel)); + fprintf(stderr, "i %f %f %f %f\n", rAcc[0], rAcc[1], rAcc[2], quatmagnitude(rAcc)); + scale3d(rAcc, rAcc, acc_mul); LinmathVec3d gyro; for (int i = 0; i < 3; i++) { - result.Pos[i] += time_diff * vel[i] + rAcc[i] * 9.8; + result.Pos[i] += time_diff * vel[i] + rAcc[i]; gyro[i] = time_diff / 2 * pIMU->gyro[i]; } @@ -39,7 +51,7 @@ static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d static void iterate_velocity(LinmathVec3d result, const SurvivePose *pose, const LinmathVec3d vel, double time_diff, PoserDataIMU *pIMU) { - scale3d(result, vel, .99999); + scale3d(result, vel, 1.); LinmathVec3d rAcc = {0}; RotateAccel(rAcc, pose, pIMU->accel); scale3d(rAcc, rAcc, time_diff); @@ -50,6 +62,10 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker if (tracker->last_data.timecode == 0) { if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; + tracker->pose.Rot[0] = 1.; + + const FLT up[3] = {0, 0, 1}; + quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); return; } @@ -65,7 +81,15 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; } - FLT up[3] = {0, 0, 1}; + const FLT up[3] = {0, 0, 1}; + LinmathQuat upRot, wouldbeUp; + LinmathVec3d rup; + quatrotatevector(rup, tracker->pose.Rot, up); + quatfrom2vectors(upRot, rup, data->accel); + + quatrotateabout(wouldbeUp, upRot, tracker->pose.Rot); + quatslerp(tracker->pose.Rot, tracker->pose.Rot, wouldbeUp, .1); + FLT pose_up[3] = {0, 0, 1}; quatrotatevector(pose_up, tracker->pose.Rot, tracker->updir); @@ -77,6 +101,10 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker iterate_velocity(v_next, &tracker->pose, tracker->current_velocity, time_diff, data); tracker->pose = t_next; + + fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1], + tracker->current_velocity[2]); + scale3d(tracker->current_velocity, v_next, 1); tracker->last_data = *data; diff --git a/src/survive_vive.c b/src/survive_vive.c index d431207..91f4b1f 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1485,26 +1485,33 @@ void survive_data_cb( SurviveUSBInterface * si ) obj->oldcode = code; //XXX XXX BIG TODO!!! Actually recal gyro data. - FLT agm[9] = { acceldata[0].v, acceldata[1].v, acceldata[2].v, - acceldata[3].v, acceldata[4].v, acceldata[5].v, - 0,0,0 }; - - agm[0]*=(float)(1./8192.0); - agm[1]*=(float)(1./8192.0); - agm[2]*=(float)(1./8192.0); - calibrate_acc(obj, agm); + FLT agm[9] = {acceldata[0].v, + acceldata[1].v, + acceldata[2].v, + acceldata[3].v, + acceldata[4].v, + acceldata[5].v, + 0, + 0, + 0}; //1G for accelerometer, from MPU6500 datasheet //this can change if the firmware changes the sensitivity. + // When coming off of USB, these values are in units of .5g -JB + agm[0] *= (float)(2. / 8192.0); + agm[1] *= (float)(2. / 8192.0); + agm[2] *= (float)(2. / 8192.0); + calibrate_acc(obj, agm); - - agm[3]*=(float)((1./32.768)*(3.14159/180.)); - agm[4]*=(float)((1./32.768)*(3.14159/180.)); - agm[5]*=(float)((1./32.768)*(3.14159/180.)); - calibrate_gyro(obj, agm+3); - - //65.5 deg/s for gyroscope, from MPU6500 datasheet - //1000 deg/s for gyroscope, from MPU6500 datasheet + // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits + // FLT deg_per_sec = 250; + // FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.; + FLT DEGREES_TO_RADS = 3.14159 / 180.; + FLT conv = 1. / 10. * DEGREES_TO_RADS; + // calibrate_gyro(obj, agm+3); + agm[3] *= conv; + agm[4] *= conv; + agm[5] *= conv; ctx->imuproc( obj, 3, agm, timecode, code ); } -- cgit v1.2.3 From 3272ffe5245c6f39f93572d4059f35953dc38faa Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 22 Mar 2018 12:42:11 -0600 Subject: Integrated IMU into SBA --- src/poser_sba.c | 11 +++++++++++ src/survive_imu.c | 3 ++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/poser_sba.c b/src/poser_sba.c index d1677d1..1f604ff 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -7,6 +7,7 @@ #include "poser.h" #include +#include #include "assert.h" #include "linmath.h" @@ -445,6 +446,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd typedef struct SBAData { int last_acode; int last_lh; + SurviveIMUTracker tracker; } SBAData; int PoserSBA(SurviveObject *so, PoserData *pd) { @@ -470,6 +472,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->last_acode = lightData->acode; } + if (error < 0) { + PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose); + // SV_INFO("using imu"); + } else { + survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); + } return 0; } case POSERDATA_FULL_SCENE: { @@ -481,6 +489,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // std::cerr << "Average reproj error: " << error << std::endl; return 0; } + case POSERDATA_IMU: { + survive_imu_tracker_integrate(so, &d->tracker, (PoserDataIMU *)pd); + } // INTENTIONAL FALLTHROUGH default: { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); diff --git a/src/survive_imu.c b/src/survive_imu.c index 6667d2f..5197266 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -7,7 +7,8 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, tracker->pose = *pose; for (int i = 0; i < 3; i++) - tracker->current_velocity[i] = pose->Pos[i] - tracker->lastGT.Pos[i]; + tracker->current_velocity[i] = + (pose->Pos[i] - tracker->lastGT.Pos[i]) / (timecode - tracker->lastGTTime) * 48000000.; tracker->lastGTTime = timecode; tracker->lastGT = *pose; -- cgit v1.2.3 From 50b026760144e76a2e69babe34e6dee0a6a3c9c0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 07:26:31 +0000 Subject: Added reset cntr for sba --- src/poser_sba.c | 55 ++++++++++++++++++++++++++++++++++++------------------- src/survive_imu.c | 8 ++++---- 2 files changed, 40 insertions(+), 23 deletions(-) diff --git a/src/poser_sba.c b/src/poser_sba.c index 1f604ff..72359da 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -31,6 +31,14 @@ typedef struct { int lh; } sba_context_single_sweep; +typedef struct SBAData { + int last_acode; + int last_lh; + int failures_to_reset; + int failures_to_reset_cntr; + SurviveIMUTracker tracker; +} SBAData; + void metric_function(int j, int i, double *aj, double *xij, void *adata) { sba_context *ctx = (sba_context *)(adata); SurviveObject *so = ctx->so; @@ -182,7 +190,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) { if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { SurviveContext *ctx = so->ctx; - SV_INFO("Can't solve for position with just %lu measurements", meas_size); + SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); failure_count = 0; } return -1; @@ -254,7 +262,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config // Docs say info[0] should be divided by meas; I don't buy it really... static int cnt = 0; if (cnt++ > 1000 || meas_size < 8) { - SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (unsigned int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); cnt = 0; } @@ -263,7 +271,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config return info[1] / meas_size * 2; } -static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, +static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, SurviveSensorActivations *scene, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; @@ -276,7 +284,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 7) { if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { SurviveContext *ctx = so->ctx; - SV_INFO("Can't solve for position with just %lu measurements", meas_size); + SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); failure_count = 0; } return -1; @@ -286,10 +294,13 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose SurvivePose soLocation = so->OutPose; bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; + if(d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { + SurviveContext *ctx = so->ctx; + SV_INFO("Must rerun seed poser"); const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); - SurviveContext *ctx = so->ctx; + if (driver) { PoserData hdr = pdl->hdr; memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions @@ -341,6 +352,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose info); // info if (status > 0) { + d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); } @@ -350,7 +362,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose // Docs say info[0] should be divided by meas; I don't buy it really... static int cnt = 0; if (cnt++ > 1000 || meas_size < 8) { - SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); cnt = 0; } @@ -436,22 +448,19 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd { SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... - SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); } return info[1] / meas_size * 2; } -typedef struct SBAData { - int last_acode; - int last_lh; - SurviveIMUTracker tracker; -} SBAData; - int PoserSBA(SurviveObject *so, PoserData *pd) { if (so->PoserData == 0) { so->PoserData = calloc(1, sizeof(SBAData)); + SBAData *d = so->PoserData; + d->failures_to_reset_cntr = 0; + d->failures_to_reset = 30; } SBAData *d = so->PoserData; SurviveContext *ctx = so->ctx; @@ -467,30 +476,38 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { FLT error = -1; if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { survive_calibration_config config = *survive_calibration_default_config(); - error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5); + error = run_sba_find_3d_structure(d, config, lightData, so, scene, 50, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } if (error < 0) { - PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose); - // SV_INFO("using imu"); + if(d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; } else { - survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); + survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); } + return 0; } case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); survive_calibration_config config = *survive_calibration_default_config(); - SV_INFO("Running sba with %lu", survive_calibration_config_index(&config)); + SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config)); double error = run_sba(config, pdfs, so, 50, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } case POSERDATA_IMU: { - survive_imu_tracker_integrate(so, &d->tracker, (PoserDataIMU *)pd); + + PoserDataIMU * imu = (PoserDataIMU*)pd; + survive_imu_tracker_integrate(so, &d->tracker, imu); + + if (ctx->calptr && ctx->calptr->stage < 5) { + } else { + PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose); + } } // INTENTIONAL FALLTHROUGH default: { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); diff --git a/src/survive_imu.c b/src/survive_imu.c index 5197266..297bbac 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -30,8 +30,8 @@ static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d LinmathVec3d rAcc = {0}; RotateAccel(rAcc, pose, pIMU->accel); - fprintf(stderr, "r %f %f %f %f\n", pIMU->accel[0], pIMU->accel[1], pIMU->accel[2], quatmagnitude(pIMU->accel)); - fprintf(stderr, "i %f %f %f %f\n", rAcc[0], rAcc[1], rAcc[2], quatmagnitude(rAcc)); + //fprintf(stderr, "r %f %f %f %f\n", pIMU->accel[0], pIMU->accel[1], pIMU->accel[2], quatmagnitude(pIMU->accel)); + //fprintf(stderr, "i %f %f %f %f\n", rAcc[0], rAcc[1], rAcc[2], quatmagnitude(rAcc)); scale3d(rAcc, rAcc, acc_mul); @@ -103,8 +103,8 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->pose = t_next; - fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1], - tracker->current_velocity[2]); + //fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1], + //tracker->current_velocity[2]); scale3d(tracker->current_velocity, v_next, 1); -- cgit v1.2.3 From 2b63278497130d01b1fbc7e6a94b6ad8e32ab4dd Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 14:07:23 +0000 Subject: Made IMU use optional --- src/poser_sba.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/poser_sba.c b/src/poser_sba.c index 72359da..efdeeef 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -32,11 +32,12 @@ typedef struct { } sba_context_single_sweep; typedef struct SBAData { - int last_acode; - int last_lh; + int last_acode; + int last_lh; int failures_to_reset; - int failures_to_reset_cntr; - SurviveIMUTracker tracker; + int failures_to_reset_cntr; + SurviveIMUTracker tracker; + bool useIMU; } SBAData; void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -297,7 +298,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o if(d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { SurviveContext *ctx = so->ctx; - SV_INFO("Must rerun seed poser"); + SV_INFO("Running seed poser"); const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); @@ -484,7 +485,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (error < 0) { if(d->failures_to_reset_cntr > 0) d->failures_to_reset_cntr--; - } else { + } else if(d->useIMU) { survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); } @@ -502,10 +503,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_IMU: { PoserDataIMU * imu = (PoserDataIMU*)pd; - survive_imu_tracker_integrate(so, &d->tracker, imu); - if (ctx->calptr && ctx->calptr->stage < 5) { - } else { + } else if(d->useIMU){ + survive_imu_tracker_integrate(so, &d->tracker, imu); PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose); } } // INTENTIONAL FALLTHROUGH -- cgit v1.2.3 From db41a20170bb7f77959b9901a31582ad2ba93db7 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:30:47 -0600 Subject: Madgwick code integrated --- include/libsurvive/survive_imu.h | 11 +- src/poser_sba.c | 18 ++-- src/survive_imu.c | 217 ++++++++++++++++++++++++++++++--------- src/survive_vive.c | 2 +- 4 files changed, 188 insertions(+), 60 deletions(-) diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index a37a4df..e7a3d90 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -9,14 +9,23 @@ extern "C" { #endif +struct SurviveIMUTracker_p; + typedef struct { FLT updir[3]; - LinmathVec3d current_velocity; // Velocity in world frame + FLT accel_scale_bias; + + LinmathVec3d current_velocity; // Velocity in world frame + LinmathVec3d current_velocity_lp; // Velocity in world frame PoserDataIMU last_data; SurvivePose pose; + LinmathPoint3d pos_lp; SurvivePose lastGT; uint32_t lastGTTime; + + float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki + } SurviveIMUTracker; void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose); diff --git a/src/poser_sba.c b/src/poser_sba.c index 1dbc820..a23eb0f 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -272,12 +272,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive info); // info if (currentPositionValid) { - FLT distp[3]; - sub3d(distp, so->OutPose.Pos, soLocation.Pos); - FLT distance = magnitude3d(distp); - ; - if (distance > 1.) - status = -1; + // FLT distp[3]; + // sub3d(distp, so->OutPose.Pos, soLocation.Pos); + // FLT distance = magnitude3d(distp); + + // if (distance > 1.) + // status = -1; } if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; @@ -296,7 +296,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } } - return info[1] / meas_size * 2; + return status; // info[1] / meas_size * 2; } // Optimizes for LH position assuming object is posed at 0 @@ -396,12 +396,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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->useIMU = true; d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); - d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 0.001); + d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->so = so; diff --git a/src/survive_imu.c b/src/survive_imu.c index 297bbac..cdf254b 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -3,13 +3,130 @@ #include "survive_internal.h" #include +//--------------------------------------------------------------------------------------------------- +// Definitions + +#define sampleFreq 240.0f // sample frequency in Hz +#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain +#define twoKiDef (2.0f * 0.0f) // 2 * integral gain + +//--------------------------------------------------------------------------------------------------- +// Function declarations + +float invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long *)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} +//--------------------------------------------------------------------------------------------------- +// IMU algorithm update +// From http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +static void MahonyAHRSupdateIMU(SurviveIMUTracker *tracker, float gx, float gy, float gz, float ax, float ay, + float az) { + float recipNorm; + float halfvx, halfvy, halfvz; + float halfex, halfey, halfez; + float qa, qb, qc; + + const float twoKp = twoKpDef; // 2 * proportional gain (Kp) + const float twoKi = twoKiDef; // 2 * integral gain (Ki) + + float q0 = tracker->pose.Rot[0]; + float q1 = tracker->pose.Rot[1]; + float q2 = tracker->pose.Rot[2]; + float q3 = tracker->pose.Rot[3]; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity and vector perpendicular to magnetic flux + halfvx = q1 * q3 - q0 * q2; + halfvy = q0 * q1 + q2 * q3; + halfvz = q0 * q0 - 0.5f + q3 * q3; + + // Error is sum of cross product between estimated and measured direction of gravity + halfex = (ay * halfvz - az * halfvy); + halfey = (az * halfvx - ax * halfvz); + halfez = (ax * halfvy - ay * halfvx); + + // Compute and apply integral feedback if enabled + if (twoKi > 0.0f) { + tracker->integralFBx += twoKi * halfex * (1.0f / sampleFreq); // tracker->integral error scaled by Ki + tracker->integralFBy += twoKi * halfey * (1.0f / sampleFreq); + tracker->integralFBz += twoKi * halfez * (1.0f / sampleFreq); + gx += tracker->integralFBx; // apply tracker->integral feedback + gy += tracker->integralFBy; + gz += tracker->integralFBz; + } else { + tracker->integralFBx = 0.0f; // prevent tracker->integral windup + tracker->integralFBy = 0.0f; + tracker->integralFBz = 0.0f; + } + + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + } + + // Integrate rate of change of quaternion + gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors + gy *= (0.5f * (1.0f / sampleFreq)); + gz *= (0.5f * (1.0f / sampleFreq)); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + tracker->pose.Rot[0] = q0; + tracker->pose.Rot[1] = q1; + tracker->pose.Rot[2] = q2; + tracker->pose.Rot[3] = q3; +} + +static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) { + uint32_t diff = 0; + if (most_recent > least_recent) { + diff = most_recent - least_recent; + } else { + diff = least_recent - most_recent; + } + + if (diff > 0xFFFFFFFF / 2) + return 0x7FFFFFFF / 2 - diff; + return diff; +} + void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose) { tracker->pose = *pose; - for (int i = 0; i < 3; i++) - tracker->current_velocity[i] = - (pose->Pos[i] - tracker->lastGT.Pos[i]) / (timecode - tracker->lastGTTime) * 48000000.; + for (int i = 0; i < 3; i++) { + tracker->current_velocity_lp[i] = tracker->current_velocity[i] = 0; + tracker->pos_lp[i] = 0; + } + //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; + tracker->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0; tracker->lastGTTime = timecode; tracker->lastGT = *pose; } @@ -20,53 +137,54 @@ static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const Linmat quatrotatevector(rAcc, pose->Rot, accel); LinmathVec3d G = {0, 0, -1}; add3d(rAcc, rAcc, G); - scale3d(rAcc, rAcc, 9.8); + scale3d(rAcc, rAcc, 9.8066); + FLT m = magnitude3d(rAcc); } -static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d vel, double time_diff, - const PoserDataIMU *pIMU) { - SurvivePose result = *pose; +static void iterate_position(SurviveIMUTracker *tracker, double time_diff, const PoserDataIMU *pIMU, FLT *out) { + const SurvivePose *pose = &tracker->pose; + const FLT *vel = tracker->current_velocity; - FLT acc_mul = time_diff * time_diff / 2; - LinmathVec3d rAcc = {0}; - RotateAccel(rAcc, pose, pIMU->accel); + for (int i = 0; i < 3; i++) + out[i] = pose->Pos[i]; - //fprintf(stderr, "r %f %f %f %f\n", pIMU->accel[0], pIMU->accel[1], pIMU->accel[2], quatmagnitude(pIMU->accel)); - //fprintf(stderr, "i %f %f %f %f\n", rAcc[0], rAcc[1], rAcc[2], quatmagnitude(rAcc)); + FLT acc_mul = time_diff * time_diff / 2; + LinmathVec3d acc; + scale3d(acc, pIMU->accel, tracker->accel_scale_bias); + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, acc); scale3d(rAcc, rAcc, acc_mul); - LinmathVec3d gyro; - for (int i = 0; i < 3; i++) { - result.Pos[i] += time_diff * vel[i] + rAcc[i]; - gyro[i] = time_diff / 2 * pIMU->gyro[i]; + out[i] += time_diff * (vel[i] - tracker->current_velocity_lp[i]); // + rAcc[i]; } - - LinmathEulerAngle curr, next; - quattoeuler(curr, pose->Rot); - add3d(next, curr, gyro); - quatfromeuler(result.Rot, next); - - return result; } -static void iterate_velocity(LinmathVec3d result, const SurvivePose *pose, const LinmathVec3d vel, double time_diff, - PoserDataIMU *pIMU) { +static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, double time_diff, PoserDataIMU *pIMU) { + const SurvivePose *pose = &tracker->pose; + const FLT *vel = tracker->current_velocity; scale3d(result, vel, 1.); + + LinmathVec3d acc; + scale3d(acc, pIMU->accel, tracker->accel_scale_bias); + LinmathVec3d rAcc = {0}; - RotateAccel(rAcc, pose, pIMU->accel); + RotateAccel(rAcc, pose, acc); + // fprintf(stderr, "%f\n", time_diff); scale3d(rAcc, rAcc, time_diff); add3d(result, result, rAcc); } void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { if (tracker->last_data.timecode == 0) { + if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; tracker->pose.Rot[0] = 1.; const FLT up[3] = {0, 0, 1}; quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); + tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir); return; } @@ -82,37 +200,38 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; } - const FLT up[3] = {0, 0, 1}; - LinmathQuat upRot, wouldbeUp; - LinmathVec3d rup; - quatrotatevector(rup, tracker->pose.Rot, up); - quatfrom2vectors(upRot, rup, data->accel); + float gx = data->gyro[0], gy = data->gyro[1], gz = data->gyro[2]; + float ax = data->accel[0], ay = data->accel[1], az = data->accel[2]; - quatrotateabout(wouldbeUp, upRot, tracker->pose.Rot); - quatslerp(tracker->pose.Rot, tracker->pose.Rot, wouldbeUp, .1); + MahonyAHRSupdateIMU(tracker, gx, gy, gz, ax, ay, az); - FLT pose_up[3] = {0, 0, 1}; - quatrotatevector(pose_up, tracker->pose.Rot, tracker->updir); + FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz; - FLT time_diff = (data->timecode - tracker->last_data.timecode) / (FLT)so->timebase_hz; + if (tick_difference(data->timecode, tracker->lastGTTime) < 3200000 * 3) { + FLT next[3]; + iterate_position(tracker, time_diff, data, next); - SurvivePose t_next = iterate_position(&tracker->pose, tracker->current_velocity, time_diff, data); + LinmathVec3d v_next, lp_add; + iterate_velocity(v_next, tracker, time_diff, data); - LinmathVec3d v_next; - iterate_velocity(v_next, &tracker->pose, tracker->current_velocity, time_diff, data); + scale3d(tracker->current_velocity, v_next, 1); - tracker->pose = t_next; + FLT v_alpha = 1; + FLT p_alpha = 1; + scale3d(tracker->current_velocity_lp, tracker->current_velocity_lp, v_alpha); - //fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1], - //tracker->current_velocity[2]); + scale3d(lp_add, v_next, 1 - v_alpha); + add3d(tracker->current_velocity_lp, tracker->current_velocity_lp, lp_add); - scale3d(tracker->current_velocity, v_next, 1); + LinmathPoint3d p_add; + scale3d(p_add, next, 1 - p_alpha); + scale3d(tracker->pos_lp, tracker->pos_lp, p_alpha); + add3d(tracker->pos_lp, tracker->pos_lp, p_add); - tracker->last_data = *data; - - FLT tmp[3]; - ApplyPoseToPoint(tmp, &tracker->pose, up); + for (int i = 0; i < 3; i++) + tracker->pose.Pos[i] = next[i] - tracker->pos_lp[i]; + // scale3d(tracker->pose.Pos, next, 1); + } - printf("[%f, %f, %f] [%f, %f, %f]\n", tracker->pose.Pos[0], tracker->pose.Pos[1], tracker->pose.Pos[2], tmp[0], - tmp[1], tmp[2]); + tracker->last_data = *data; } diff --git a/src/survive_vive.c b/src/survive_vive.c index 53f6d42..1ffb737 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1508,7 +1508,7 @@ void survive_data_cb( SurviveUSBInterface * si ) // FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.; FLT DEGREES_TO_RADS = 3.14159 / 180.; FLT conv = 1. / 10. * DEGREES_TO_RADS; - // calibrate_gyro(obj, agm+3); + calibrate_gyro(obj, agm + 3); agm[3] *= conv; agm[4] *= conv; agm[5] *= conv; -- cgit v1.2.3 From fe025b0ff6bfb440da7cec8f388fa951910a86f0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:40:29 -0600 Subject: Removed high pass filter --- include/libsurvive/survive_imu.h | 2 -- src/poser_sba.c | 3 ++- src/survive_imu.c | 25 ++++--------------------- 3 files changed, 6 insertions(+), 24 deletions(-) diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index e7a3d90..124ad7e 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -16,10 +16,8 @@ typedef struct { FLT accel_scale_bias; LinmathVec3d current_velocity; // Velocity in world frame - LinmathVec3d current_velocity_lp; // Velocity in world frame PoserDataIMU last_data; SurvivePose pose; - LinmathPoint3d pos_lp; SurvivePose lastGT; uint32_t lastGTTime; diff --git a/src/poser_sba.c b/src/poser_sba.c index a23eb0f..f6b1131 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -396,7 +396,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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->useIMU = true; + 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); d->sensor_time_window = @@ -411,6 +411,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second); SV_INFO("\tsba-time-window: %d", d->sensor_time_window); SV_INFO("\tsba-max-error: %f", d->max_error); + SV_INFO("\tsba-use-imu: %d", d->useIMU); } SBAData *d = so->PoserData; switch (pd->pt) { diff --git a/src/survive_imu.c b/src/survive_imu.c index cdf254b..e49da3e 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -121,8 +121,7 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, tracker->pose = *pose; for (int i = 0; i < 3; i++) { - tracker->current_velocity_lp[i] = tracker->current_velocity[i] = 0; - tracker->pos_lp[i] = 0; + tracker->current_velocity[i] = 0; } //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; @@ -156,7 +155,7 @@ static void iterate_position(SurviveIMUTracker *tracker, double time_diff, const scale3d(rAcc, rAcc, acc_mul); for (int i = 0; i < 3; i++) { - out[i] += time_diff * (vel[i] - tracker->current_velocity_lp[i]); // + rAcc[i]; + out[i] += time_diff * vel[i] + rAcc[i]; } } @@ -170,7 +169,6 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do LinmathVec3d rAcc = {0}; RotateAccel(rAcc, pose, acc); - // fprintf(stderr, "%f\n", time_diff); scale3d(rAcc, rAcc, time_diff); add3d(result, result, rAcc); } @@ -211,26 +209,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker FLT next[3]; iterate_position(tracker, time_diff, data, next); - LinmathVec3d v_next, lp_add; + LinmathVec3d v_next; iterate_velocity(v_next, tracker, time_diff, data); scale3d(tracker->current_velocity, v_next, 1); - - FLT v_alpha = 1; - FLT p_alpha = 1; - scale3d(tracker->current_velocity_lp, tracker->current_velocity_lp, v_alpha); - - scale3d(lp_add, v_next, 1 - v_alpha); - add3d(tracker->current_velocity_lp, tracker->current_velocity_lp, lp_add); - - LinmathPoint3d p_add; - scale3d(p_add, next, 1 - p_alpha); - scale3d(tracker->pos_lp, tracker->pos_lp, p_alpha); - add3d(tracker->pos_lp, tracker->pos_lp, p_add); - - for (int i = 0; i < 3; i++) - tracker->pose.Pos[i] = next[i] - tracker->pos_lp[i]; - // scale3d(tracker->pose.Pos, next, 1); + scale3d(tracker->pose.Pos, next, 1); } tracker->last_data = *data; -- cgit v1.2.3