diff options
| author | Justin Berger <j.david.berger@gmail.com> | 2018-03-22 12:42:11 -0600 |
|---|---|---|
| committer | Justin Berger <j.david.berger@gmail.com> | 2018-03-22 12:42:11 -0600 |
| commit | 3272ffe5245c6f39f93572d4059f35953dc38faa (patch) | |
| tree | 5dbca7448e303f86bb2c1180d5d44ac4ece03920 /src/poser_sba.c | |
| parent | dcf5d7a482e022e762a656253017ebbc721d8a83 (diff) | |
| download | libsurvive-3272ffe5245c6f39f93572d4059f35953dc38faa.tar.gz libsurvive-3272ffe5245c6f39f93572d4059f35953dc38faa.tar.bz2 | |
Integrated IMU into SBA
Diffstat (limited to 'src/poser_sba.c')
| -rw-r--r-- | src/poser_sba.c | 11 |
1 files changed, 11 insertions, 0 deletions
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 <survive.h> +#include <survive_imu.h> #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); |