From e06e452445bdd3d61f393bde4206cd848a54444b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 22:51:57 -0600 Subject: Created common base for optimizer based posers --- src/poser_epnp.c | 2 +- src/poser_general_optimizer.c | 100 ++++++++++++++++++++++++++++++++++++++++ src/poser_general_optimizer.h | 29 ++++++++++++ src/poser_sba.c | 103 ++++++++---------------------------------- 4 files changed, 148 insertions(+), 86 deletions(-) create mode 100644 src/poser_general_optimizer.c create mode 100644 src/poser_general_optimizer.h (limited to 'src') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 7e922e7..615c01c 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { case POSERDATA_LIGHT: { PoserDataLight *lightData = (PoserDataLight *)pd; - SurvivePose posers[2]; + SurvivePose posers[2] = {}; int meas[2] = {0, 0}; for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { if (so->ctx->bsd[lh].PositionSet) { diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c new file mode 100644 index 0000000..313e378 --- /dev/null +++ b/src/poser_general_optimizer.c @@ -0,0 +1,100 @@ +#include "poser_general_optimizer.h" +#include "string.h" +#include +#include +#include + +void *GetDriver(const char *name); +void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) { + memset(d, 0, sizeof(*d)); + d->so = so; + + SurviveContext *ctx = so->ctx; + + d->failures_to_reset = survive_configi(ctx, "failures-to-reset", SC_GET, 1); + d->successes_to_reset = survive_configi(ctx, "successes-to-reset", SC_GET, -1); + d->max_error = survive_configf(ctx, "max-error", SC_GET, .0001); + + const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, "PoserEPNP"); + d->seed_poser = (PoserCB)GetDriver(subposer); + + SV_INFO("Initializing general optimizer:"); + SV_INFO("\tmax-error: %f", d->max_error); + SV_INFO("\tsuccesses-to-reset: %d", d->successes_to_reset); + SV_INFO("\tfailures-to-reset: %d", d->failures_to_reset); + SV_INFO("\tseed-poser: %s(%p)", subposer, d->seed_poser); +} +void general_optimizer_data_record_failure(GeneralOptimizerData *d) { + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; +} +bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error) { + d->stats.runs++; + if (d->max_error > error) { + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; + d->failures_to_reset_cntr = d->failures_to_reset; + return true; + } + return false; +} + +typedef struct { + bool hasInfo; + SurvivePose pose; +} set_position_t; + +static void set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) { + set_position_t *user = _user; + assert(user->hasInfo == false); + user->hasInfo = true; + user->pose = *new_pose; +} + +bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *_hdr, size_t len_hdr, + SurvivePose *soLocation) { + *soLocation = d->so->OutPose; + bool currentPositionValid = quatmagnitude(soLocation->Rot) != 0; + static bool seed_warning = false; + if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { + PoserCB driver = d->seed_poser; + SurviveContext *ctx = d->so->ctx; + if (driver) { + + PoserData *hdr = alloca(len_hdr); + memcpy(hdr, _hdr, len_hdr); + memset(hdr, 0, sizeof(PoserData)); // Clear callback functions + hdr->pt = _hdr->pt; + hdr->poseproc = set_position; + + set_position_t locations = {0}; + hdr->userdata = &locations; + driver(d->so, hdr); + d->stats.poser_seed_runs++; + + if (locations.hasInfo == false) { + return false; + } else if (locations.hasInfo) { + *soLocation = locations.pose; + } + + d->successes_to_reset_cntr = d->successes_to_reset; + } else if (seed_warning == false) { + seed_warning = true; + SV_INFO("Not using a seed poser for SBA; results will likely be way off"); + } + } + return true; +} + +void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu) { + if (d->seed_poser) { + d->seed_poser(d->so, &imu->hdr); + } +} + +void general_optimizer_data_dtor(GeneralOptimizerData *d) { + SurviveContext *ctx = d->so->ctx; + SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs); + SV_INFO("\terror failures %d", d->stats.error_failures); +} diff --git a/src/poser_general_optimizer.h b/src/poser_general_optimizer.h new file mode 100644 index 0000000..81d94bf --- /dev/null +++ b/src/poser_general_optimizer.h @@ -0,0 +1,29 @@ +#include +#include + +typedef struct GeneralOptimizerData { + int failures_to_reset; + int failures_to_reset_cntr; + int successes_to_reset; + int successes_to_reset_cntr; + + FLT max_error; + + struct { + int runs; + int poser_seed_runs; + int error_failures; + } stats; + + PoserCB seed_poser; + SurviveObject *so; +} GeneralOptimizerData; + +void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so); +void general_optimizer_data_dtor(GeneralOptimizerData *d); + +void general_optimizer_data_record_failure(GeneralOptimizerData *d); +bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error); +void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu); +bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *hdr, size_t len_hdr, + SurvivePose *p); diff --git a/src/poser_sba.c b/src/poser_sba.c index 347b600..4c7fcd1 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -13,6 +13,7 @@ #include "assert.h" #include "linmath.h" #include "math.h" +#include "poser_general_optimizer.h" #include "string.h" #include "survive_cal.h" #include "survive_config.h" @@ -32,16 +33,11 @@ typedef struct { } sba_context_single_sweep; typedef struct SBAData { + GeneralOptimizerData opt; + int last_acode; int last_lh; - int failures_to_reset; - int failures_to_reset_cntr; - int successes_to_reset; - int successes_to_reset_cntr; - - FLT max_error; - FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; @@ -52,15 +48,8 @@ typedef struct SBAData { bool useIMU; struct { - int runs; - int poser_seed_runs; int meas_failures; - int error_failures; } stats; - - SurviveObject *so; - - PoserCB seed_poser; } SBAData; static void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -98,7 +87,7 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask, double *meas, double *cov) { size_t rtn = 0; - SurviveObject *so = d->so; + SurviveObject *so = d->opt.so; for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { @@ -215,7 +204,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/, SurvivePose *out) { double *covx = 0; - SurviveObject *so = d->so; + SurviveObject *so = d->opt.so; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); @@ -241,37 +230,10 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } failure_count = 0; - SurvivePose soLocation = so->OutPose; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; - static bool seed_warning = false; - if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { - SurviveContext *ctx = so->ctx; - // SV_INFO("Must rerun seed poser"); - PoserCB driver = d->seed_poser; + SurvivePose soLocation = {}; - if (driver) { - PoserData hdr = pdl->hdr; - memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions - pdl->hdr.pt = hdr.pt; - pdl->hdr.poseproc = sba_set_position; - - sba_set_position_t locations = {0}; - pdl->hdr.userdata = &locations; - driver(so, &pdl->hdr); - pdl->hdr = hdr; - d->stats.poser_seed_runs++; - - if (locations.hasInfo == false) { - return -1; - } else if (locations.hasInfo) { - soLocation = locations.poses; - } - - d->successes_to_reset_cntr = d->successes_to_reset; - } else if (seed_warning == false) { - seed_warning = true; - SV_INFO("Not using a seed poser for SBA; results will likely be way off"); - } + if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { + return -1; } double opts[SBA_OPTSSZ] = {0}; @@ -286,7 +248,6 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); opts[4] = 0.0; - d->stats.runs++; int status = sba_str_levmar(1, // Number of 3d points 0, // Number of 3d points to fix in spot NUM_LIGHTHOUSES * so->sensor_ct, vmask, @@ -303,26 +264,14 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive opts, // options info); // info - if (currentPositionValid) { - // FLT distp[3]; - // sub3d(distp, so->OutPose.Pos, soLocation.Pos); - // FLT distance = magnitude3d(distp); - - // if (distance > 1.) - // status = -1; - } - double rtn = -1; bool status_failure = status <= 0; - bool error_failure = (info[1] / meas_size * 2) >= d->max_error; + bool error_failure = !general_optimizer_data_record_success(&d->opt, (info[1] / meas_size * 2)); if (!status_failure && !error_failure) { - d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; - } else if (error_failure) { - d->stats.error_failures++; - } + } else { SurviveContext *ctx = so->ctx; @@ -349,7 +298,7 @@ static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_itera .obj_pose = so->OutPose}; { - const char *subposer = survive_configs(so->ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); + const char *subposer = survive_configs(so->ctx, "seed-poser", SC_GET, "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; @@ -429,33 +378,25 @@ 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 = 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, -1); + + general_optimizer_data_init(&d->opt, so); 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 = 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, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); - d->so = so; - const char *subposer = survive_configs(ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); - d->seed_poser = (PoserCB)GetDriver(subposer); SV_INFO("Initializing SBA:"); SV_INFO("\tsba-required-meas: %d", d->required_meas); SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance); 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-successes-to-reset: %d", d->successes_to_reset); SV_INFO("\tsba-use-imu: %d", d->useIMU); SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function); - SV_INFO("\tsba-seed-poser: %s(%p)", subposer, d->seed_poser); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -477,8 +418,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (error < 0) { - if (d->failures_to_reset_cntr > 0) - d->failures_to_reset_cntr--; + } else { if (d->useIMU) { @@ -492,8 +432,6 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } PoserData_poser_pose_func(&lightData->hdr, so, &estimate); - if (d->successes_to_reset_cntr > 0) - d->successes_to_reset_cntr--; } } return 0; @@ -507,9 +445,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } case POSERDATA_DISASSOCIATE: { SV_INFO("SBA stats:"); - SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs); SV_INFO("\tmeas failures %d", d->stats.meas_failures); - SV_INFO("\terror failures %d", d->stats.error_failures); + general_optimizer_data_dtor(&d->opt); free(d); so->PoserData = 0; return 0; @@ -522,12 +459,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { survive_imu_tracker_integrate(so, &d->tracker, imu); PoserData_poser_pose_func(pd, so, &d->tracker.pose); } - } // INTENTIONAL FALLTHROUGH - default: { - if (d->seed_poser) { - return d->seed_poser(so, pd); - } - break; + + general_optimizer_data_record_imu(&d->opt, imu); } } return -1; -- cgit v1.2.3