diff options
Diffstat (limited to 'src/poser_sba.c')
| -rw-r--r-- | src/poser_sba.c | 19 |
1 files changed, 18 insertions, 1 deletions
diff --git a/src/poser_sba.c b/src/poser_sba.c index bd7d520..1dbc820 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -3,11 +3,12 @@ #define USE_DOUBLE #endif -#include <sba/sba.h> #include <malloc.h> +#include <sba/sba.h> #include "poser.h" #include <survive.h> +#include <survive_imu.h> #include "assert.h" #include "linmath.h" @@ -47,6 +48,9 @@ typedef struct SBAData { int required_meas; + SurviveIMUTracker tracker; + bool useIMU; + SurviveObject *so; } SBAData; @@ -421,6 +425,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { FLT error = -1; if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { error = run_sba_find_3d_structure(d, lightData, scene, 100, .5); + d->last_lh = lightData->lh; d->last_acode = lightData->acode; } @@ -429,6 +434,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (d->failures_to_reset_cntr > 0) d->failures_to_reset_cntr--; } else { + if (d->useIMU) { + survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); + } if (d->successes_to_reset_cntr > 0) d->successes_to_reset_cntr--; } @@ -442,6 +450,15 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // std::cerr << "Average reproj error: " << error << std::endl; return 0; } + case POSERDATA_IMU: { + + PoserDataIMU * imu = (PoserDataIMU*)pd; + if (ctx->calptr && ctx->calptr->stage < 5) { + } 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 default: { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); |