From 5e8633ecc6f1309b5be2dd86b61e8a7b0a9f5ecb Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 24 Mar 2018 00:10:24 -0600 Subject: Added success counter to sba too --- src/poser_sba.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index d36c264..c01cc61 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -35,6 +35,8 @@ typedef struct SBAData { int last_lh; int failures_to_reset; int failures_to_reset_cntr; + int successes_to_reset; + int successes_to_reset_cntr; } SBAData; void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -292,9 +294,9 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o SurvivePose soLocation = so->OutPose; bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; - if (d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { + 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"); + // SV_INFO("Must rerun seed poser"); const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); @@ -310,11 +312,12 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o pdl->hdr = hdr; if (locations.hasInfo == false) { - return -1; } else if (locations.hasInfo) { soLocation = locations.poses; } + + d->successes_to_reset_cntr = d->successes_to_reset; } else { SV_INFO("Not using a seed poser for SBA; results will likely be way off"); } @@ -457,7 +460,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { so->PoserData = calloc(1, sizeof(SBAData)); SBAData *d = so->PoserData; d->failures_to_reset_cntr = 0; - d->failures_to_reset = 30; + d->failures_to_reset = 5; + d->successes_to_reset_cntr = 0; + d->successes_to_reset = 20; } SBAData *d = so->PoserData; SurviveContext *ctx = so->ctx; @@ -481,6 +486,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (error < 0) { if (d->failures_to_reset_cntr > 0) d->failures_to_reset_cntr--; + } else { + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; } return 0; -- cgit v1.2.3