From 5d43c7f5541b91f29aa3138a8a460f48198e11d0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 07:36:23 -0600 Subject: Fixed terrible bug in SBA which slowed it way down --- src/poser_sba.c | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index fcf4f2e..23e03fc 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -399,7 +399,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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, 100); + d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, -1); 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); @@ -435,27 +435,28 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->last_lh = lightData->lh; d->last_acode = lightData->acode; - } - if (error < 0) { - if (d->failures_to_reset_cntr > 0) - d->failures_to_reset_cntr--; - } else { - if (d->useIMU) { - FLT var_meters = 0.5; - FLT var_quat = error + .05; - FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, - error * var_quat, error * var_quat, error * var_quat}; - - survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); - estimate = d->tracker.pose; + + if (error < 0) { + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; } + else { + if (d->useIMU) { + FLT var_meters = 0.5; + FLT var_quat = error + .05; + FLT var[7] = { error * var_meters, error * var_meters, error * var_meters, error * var_quat, + error * var_quat, error * var_quat, error * var_quat }; + + survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); + estimate = d->tracker.pose; + } - PoserData_poser_pose_func(&lightData->hdr, so, &estimate); - if (d->successes_to_reset_cntr > 0) - d->successes_to_reset_cntr--; + PoserData_poser_pose_func(&lightData->hdr, so, &estimate); + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; + } } - return 0; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3 From 5b07e56094f2f765d679ac06bc07d4081338bb29 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 09:53:26 -0600 Subject: Added statistics to sba --- src/poser_sba.c | 29 ++++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index 23e03fc..827e420 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -51,6 +51,13 @@ typedef struct SBAData { SurviveIMUTracker tracker; bool useIMU; + struct { + int runs; + int poser_seed_runs; + int meas_failures; + int error_failures; + } stats; + SurviveObject *so; } SBAData; @@ -208,6 +215,9 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); failure_count = 0; } + if (meas_size < d->required_meas) { + d->stats.meas_failures++; + } return -1; } failure_count = 0; @@ -231,6 +241,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive pdl->hdr.userdata = &locations; driver(so, &pdl->hdr); pdl->hdr = hdr; + d->stats.poser_seed_runs++; if (locations.hasInfo == false) { return -1; @@ -256,6 +267,7 @@ 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, @@ -287,6 +299,8 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; + } else if ((info[1] / meas_size * 2) >= d->max_error) { + d->stats.error_failures++; } { @@ -466,13 +480,22 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // std::cerr << "Average reproj error: " << error << std::endl; return 0; } + 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); + free(d); + so->PoserData = 0; + 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_pose_func(pd, so, &d->tracker.pose); + } else if (d->useIMU) { + survive_imu_tracker_integrate(so, &d->tracker, imu); + PoserData_poser_pose_func(pd, so, &d->tracker.pose); } } // INTENTIONAL FALLTHROUGH default: { -- cgit v1.2.3 From bb0d186e967421c48ba781759063dca165f69df2 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 10:15:23 -0600 Subject: Detailed messages about sba failures --- src/poser_sba.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index 827e420..1eefc80 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -294,23 +294,23 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } double rtn = -1; - if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { + bool status_failure = status <= 0; + bool error_failure = (info[1] / meas_size * 2) >= d->max_error; + 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 ((info[1] / meas_size * 2) >= d->max_error) { + } else if (error_failure) { d->stats.error_failures++; } { SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... - static int cnt = 0; - if (cnt++ > 1000 || meas_size < d->required_meas || (info[1] / meas_size * 2) > d->max_error) { - // 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; + if (error_failure) { + 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)); } } -- cgit v1.2.3 From d46271513e6f789af0e82d4ed6628abe21e96a92 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 15:54:07 -0600 Subject: Added jacobian to sba, ~2x speed improvement --- src/poser_sba.c | 41 ++++++++++++++++++++++++++++++----------- 1 file changed, 30 insertions(+), 11 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index 1eefc80..f101018 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -45,7 +45,7 @@ typedef struct SBAData { FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; - + int use_jacobian_function; int required_meas; SurviveIMUTracker tracker; @@ -183,13 +183,30 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada assert(lh < 2); assert(sensor_idx < so->sensor_ct); - quatnormalize(obj.Rot, obj.Rot); - FLT xyz[3]; - ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); + // quatnormalize(obj.Rot, obj.Rot); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; - survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij); + survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); +} + +static void str_metric_function_jac(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + sba_context *ctx = (sba_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + // quatnormalize(obj.Rot, obj.Rot); + + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); } static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, @@ -277,12 +294,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive cov, // cov data 2, // mnp -- 2 points per image str_metric_function, - 0, // jacobia of metric_func - &ctx, // user data - max_iterations, // Max iterations - 0, // verbosity - opts, // options - info); // info + d->use_jacobian_function ? str_metric_function_jac : 0, // jacobia of metric_func + &ctx, // user data + max_iterations, // Max iterations + 0, // verbosity + opts, // options + info); // info if (currentPositionValid) { // FLT distp[3]; @@ -421,6 +438,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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; SV_INFO("Initializing SBA:"); @@ -431,6 +449,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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); } SBAData *d = so->PoserData; switch (pd->pt) { -- cgit v1.2.3 From 538b7c32bfbd6460c61a080c2697365934b81938 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 16:16:14 -0600 Subject: Made picking the seed poser use cmd line --- src/poser_sba.c | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index f101018..1f79986 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -59,6 +59,8 @@ typedef struct SBAData { } stats; SurviveObject *so; + + PoserCB seed_poser; } SBAData; static void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -241,12 +243,11 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive 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"); - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); + PoserCB driver = d->seed_poser; if (driver) { PoserData hdr = pdl->hdr; @@ -267,7 +268,8 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } d->successes_to_reset_cntr = d->successes_to_reset; - } else { + } else if (seed_warning == false) { + seed_warning = true; SV_INFO("Not using a seed poser for SBA; results will likely be way off"); } } @@ -335,7 +337,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, +static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; @@ -347,11 +349,9 @@ static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_itera .obj_pose = so->OutPose}; { - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); + PoserCB driver = d->seed_poser; SurviveContext *ctx = so->ctx; if (driver) { - SV_INFO("Using %s seed poser for SBA", subposer); PoserData hdr = pdfs->hdr; memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions pdfs->hdr.pt = hdr.pt; @@ -441,6 +441,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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); @@ -450,6 +453,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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) { @@ -495,7 +499,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - double error = run_sba(pdfs, so, 100, .005); + double error = run_sba(d, pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } @@ -518,10 +522,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } } // INTENTIONAL FALLTHROUGH default: { - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); - if (driver) { - return driver(so, pd); + if (d->seed_poser) { + return d->seed_poser(so, pd); } break; } -- cgit v1.2.3 From 9601f596fe545692ed39f939cf846f9883ea188c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 16:26:41 -0600 Subject: Revered what config poser was doing for SBA --- src/poser_sba.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index 1f79986..347b600 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -337,7 +337,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, +static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; @@ -349,7 +349,9 @@ static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, i .obj_pose = so->OutPose}; { - PoserCB driver = d->seed_poser; + const char *subposer = survive_configs(so->ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); + + PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; if (driver) { PoserData hdr = pdfs->hdr; @@ -499,7 +501,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - double error = run_sba(d, pdfs, so, 100, .005); + double error = run_sba(pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } -- cgit v1.2.3 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_sba.c | 103 ++++++++++---------------------------------------------- 1 file changed, 18 insertions(+), 85 deletions(-) (limited to 'src/poser_sba.c') 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 From 7f4385538d6b19ea5dd3f00c4a0eb49385798031 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Apr 2018 09:33:40 -0600 Subject: Made new posers work in windows --- src/poser_sba.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index 4c7fcd1..5166951 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -230,7 +230,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } failure_count = 0; - SurvivePose soLocation = {}; + SurvivePose soLocation = { 0 }; if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { return -1; -- cgit v1.2.3