From 47c7fb15182700fb403894f65beaf143a7fad6ab Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 12:23:48 -0600 Subject: Tweaked how reproject / calibate interact --- include/libsurvive/survive.h | 10 +- include/libsurvive/survive_reproject.h | 37 +---- src/poser_epnp.c | 6 +- src/poser_sba.c | 41 +++-- src/survive.c | 3 +- src/survive_cal.c | 10 +- src/survive_reproject.c | 230 +++++++-------------------- tools/findoptimalconfig/findoptimalconfig.cc | 72 +-------- 8 files changed, 97 insertions(+), 312 deletions(-) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index e435752..669821d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -185,6 +185,13 @@ enum SurviveCalFlag { SVCal_All = SVCal_Gib | SVCal_Curve | SVCal_Tilt | SVCal_Phase }; +typedef struct survive_calibration_config { + enum SurviveCalFlag use_flag; + FLT phase_scale, tilt_scale, curve_scale, gib_scale; +} survive_calibration_config; + +survive_calibration_config survive_calibration_config_ctor(); + struct SurviveContext { text_feedback_func faultfunction; text_feedback_func notefunction; @@ -222,8 +229,7 @@ struct SurviveContext { ButtonQueue buttonQueue; void *user_ptr; - enum SurviveCalFlag calibration_flag; - struct survive_calibration_config *calibration_config; + struct survive_calibration_config calibration_config; }; void survive_verify_FLT_size( diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index c90d39c..6546e66 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -9,35 +9,12 @@ extern "C" { #endif -typedef struct { - bool enable[2]; - bool invert[2]; - bool swap; -} survive_calibration_options_config; - -typedef struct survive_calibration_config { - - survive_calibration_options_config phase, tilt, curve, gibMag, gibPhase; - - bool gibUseSin; - -} survive_calibration_config; - -void survive_calibration_options_config_apply(const survive_calibration_options_config *option, const FLT *input, - FLT *output); - -const survive_calibration_config *survive_calibration_default_config(const SurviveContext *ctx); - -size_t survive_calibration_config_max_idx(); - -survive_calibration_config survive_calibration_config_create_from_idx(size_t v); - -size_t survive_calibration_config_index(const survive_calibration_config *config); - void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out); void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); // This is given a lighthouse -- in the same system as stored in BaseStationData, and // a 3d point and finds what the effective 'angle' value for a given lighthouse syste @@ -45,18 +22,12 @@ void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, cons // While this is typically opposite of what we want to do -- we want to find the 3d // position from a 2D coordinate, this is helpful since the minimization of reprojection // error is a core mechanism to many types of solvers. -void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *point3d, FLT *out); void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *point3d, FLT *out); -void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, - const FLT *point3d, FLT *out); - -void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *config); - -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, FLT *out); +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out); void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out); #ifdef __cplusplus diff --git a/src/poser_epnp.c b/src/poser_epnp.c index c05450a..2cbd9c1 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -72,9 +72,9 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) for (size_t i = 0; i < so->sensor_ct; i++) { FLT *lengths = pdfs->lengths[i][lh]; - FLT *_ang = pdfs->angles[i][lh]; - FLT ang[2]; - survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); + FLT *ang = pdfs->angles[i][lh]; + // FLT ang[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; diff --git a/src/poser_sba.c b/src/poser_sba.c index 4a4ed8f..49854f2 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -18,7 +18,6 @@ #include "survive_reproject.h" typedef struct { - survive_calibration_config calibration_config; PoserData *pdfs; SurviveObject *so; SurvivePose obj_pose; @@ -58,8 +57,8 @@ static void metric_function(int j, int i, double *aj, double *xij, void *adata) SurvivePose obj2world = ctx->obj_pose; FLT sensorInWorld[3] = {0}; ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]); - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, - xij); + survive_calibration_config cfg = so->ctx->calibration_config; + survive_reproject_from_pose_with_config(so->ctx, &cfg, j, (SurvivePose *)aj, sensorInWorld, xij); } static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { @@ -73,9 +72,9 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, continue; } - double *_angles = pdfs->angles[sensor][lh]; - double angles[2]; - survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); + double *angles = pdfs->angles[sensor][lh]; + // double angles[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; meas[measCount++] = angles[0]; @@ -93,9 +92,9 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { - double *_a = scene->angles[sensor][lh]; - FLT a[2]; - survive_apply_bsd_calibration(so->ctx, lh, _a, a); + const double *a = scene->angles[sensor][lh]; + // FLT a[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _a, a); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; if (cov) { @@ -160,7 +159,7 @@ static void str_metric_function_single_sweep(int j, int i, double *bi, double *x SurvivePose *camera = &so->ctx->bsd[lh].Pose; FLT out[2]; - survive_reproject_from_pose_with_config(so->ctx, &ctx->hdr.calibration_config, lh, camera, xyz, out); + survive_reproject_from_pose(so->ctx, lh, camera, xyz, out); *xij = out[acode]; } @@ -181,12 +180,11 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); + survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij); } -static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl, - SurviveSensorActivations *scene, int max_iterations /* = 50*/, - double max_reproj_error /* = 0.005*/) { +static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, + int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; SurviveObject *so = d->so; @@ -246,7 +244,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; - sba_context ctx = {options, &pdl->hdr, so}; + sba_context ctx = {&pdl->hdr, so}; opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; @@ -300,15 +298,15 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so, - int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { +static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, + double max_reproj_error /* = 0.005*/) { double *covx = 0; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); size_t meas_size = construct_input(so, pdfs, vmask, meas); - sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + sba_context sbactx = {&pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, .obj_pose = so->OutPose}; { @@ -424,8 +422,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // only process sweeps FLT error = -1; if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { - survive_calibration_config config = *survive_calibration_default_config(ctx); - error = run_sba_find_3d_structure(d, config, lightData, scene, 100, .5); + error = run_sba_find_3d_structure(d, lightData, scene, 100, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } @@ -443,9 +440,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - survive_calibration_config config = *survive_calibration_default_config(ctx); - SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config)); - double error = run_sba(config, pdfs, so, 100, .005); + double error = run_sba(pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } diff --git a/src/survive.c b/src/survive.c index 63ad2ba..2a7aad1 100644 --- a/src/survive.c +++ b/src/survive.c @@ -219,7 +219,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { ctx->configfunction = survive_default_htc_config_process; ctx->rawposeproc = survive_default_raw_pose_process; - ctx->calibration_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); + ctx->calibration_config = survive_calibration_config_ctor(); + ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All); return ctx; } diff --git a/src/survive_cal.c b/src/survive_cal.c index e094e7b..3015b68 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -13,12 +13,13 @@ #include "survive_internal.h" #include "survive_reproject.h" +#include +#include #include +#include #include #include #include -#include -#include #include "survive_config.h" @@ -618,9 +619,8 @@ static void handle_calibration( struct SurviveCalData *cd ) } fsd.lengths[i][j][0] = cd->avglens[dataindex+0]; fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; - // fsd.angles[i][j][0] = cd->avgsweeps[dataindex+0]; - // fsd.angles[i][j][1] = cd->avgsweeps[dataindex+1]; - survive_apply_bsd_calibration(ctx, lh, &cd->avgsweeps[dataindex], fsd.angles[i][j]); + fsd.angles[i][j][0] = cd->avgsweeps[dataindex + 0]; + fsd.angles[i][j][1] = cd->avgsweeps[dataindex + 1]; fsd.synctimes[i][j] = temp_syncs[i][j]; } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 845f30a..ee9704f 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -4,75 +4,6 @@ #include #include -static void survive_calibration_options_config_normalize( - survive_calibration_options_config *option) { - if (!option->enable[0]) - option->invert[0] = false; - if (!option->enable[1]) - option->invert[1] = false; - if (!option->enable[0] && !option->enable[1]) - option->swap = false; -} - -void survive_calibration_options_config_apply( - const survive_calibration_options_config *option, const FLT *input, - FLT *output) { - FLT tmp[2]; // In case they try to do in place - for (int i = 0; i < 2; i++) { - tmp[i] = option->enable[i] * (option->invert[i] ? -1 : 1) * - input[i ^ option->swap]; - } - for (int i = 0; i < 2; i++) { - output[i] = tmp[i]; - } -} - -survive_calibration_config -survive_calibration_config_create_from_idx(size_t v) { - survive_calibration_config config; - memset(&config, 0, sizeof(config)); - - bool *_this = (bool *)&config; - - for (size_t i = 0; i < sizeof(config); i++) { - _this[i] = (bool)(v & 1); - v = v >> 1; - } - - survive_calibration_options_config_normalize(&config.phase); - survive_calibration_options_config_normalize(&config.tilt); - survive_calibration_options_config_normalize(&config.curve); - survive_calibration_options_config_normalize(&config.gibMag); - - config.gibPhase.enable[0] = config.gibMag.enable[0]; - config.gibPhase.enable[1] = config.gibMag.enable[1]; - - survive_calibration_options_config_normalize(&config.gibPhase); - - if (!config.gibPhase.enable[0] && !config.gibPhase.enable[1]) - config.gibUseSin = false; - - return config; -} - -size_t -survive_calibration_config_index(const survive_calibration_config *config) { - bool *_this = (bool *)config; - size_t v = 0; - for (size_t i = 0; i < sizeof(*config); i++) { - v = (v | _this[sizeof(*config) - i - 1]); - v = v << 1; - } - v = v >> 1; - return v; -} - -static FLT gibf(bool useSin, FLT v) { - if (useSin) - return sin(v); - return cos(v); -} - void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *pt, FLT *out) { LinmathQuat invq; @@ -89,116 +20,43 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv FLT x = -t_pt[0] / -t_pt[2]; FLT y = t_pt[1] / -t_pt[2]; - double ang_x = atan(x); - double ang_y = atan(y); - - double phase[2]; - survive_calibration_options_config_apply(&config->phase, bsd->fcal.phase, phase); - double tilt[2]; - survive_calibration_options_config_apply(&config->tilt, bsd->fcal.tilt, tilt); - double curve[2]; - survive_calibration_options_config_apply(&config->curve, bsd->fcal.curve, curve); - double gibPhase[2]; - survive_calibration_options_config_apply(&config->gibPhase, bsd->fcal.gibpha, gibPhase); - double gibMag[2]; - survive_calibration_options_config_apply(&config->gibMag, bsd->fcal.gibmag, gibMag); - - out[0] = ang_x + phase[0] + (tilt[0]) * ang_y + curve[0] * ang_y * ang_y + - gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0]; - out[1] = ang_y + phase[1] + (tilt[1]) * ang_x + curve[1] * ang_x * ang_x + - gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1]; -} -void survive_reproject_from_pose_with_config(const SurviveContext *ctx, const survive_calibration_config *config, - int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { - const BaseStationData *bsd = &ctx->bsd[lighthouse]; - survive_reproject_from_pose_with_bsd(bsd, config, pose, pt, out); -} - -void survive_reproject_with_config(const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, - const FLT *point3d, FLT *out) { - survive_reproject_from_pose_with_config(ctx, config, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); -} - -void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, - const SurvivePose *pose, FLT *pt, FLT *out) { - survive_reproject_from_pose_with_config(ctx, survive_calibration_default_config(ctx), lighthouse, pose, pt, out); -} + double ang[] = {atan(x), atan(y)}; -void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, - FLT *out) { - survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, - point3d, out); -} + const FLT *phase = bsd->fcal.phase; + const FLT *curve = bsd->fcal.curve; + const FLT *tilt = bsd->fcal.tilt; + const FLT *gibPhase = bsd->fcal.gibpha; + const FLT *gibMag = bsd->fcal.gibmag; + enum SurviveCalFlag f = config->use_flag; -const survive_calibration_config *survive_calibration_default_config(const SurviveContext *_ctx) { - SurviveContext *ctx = (SurviveContext *)_ctx; - if (ctx->calibration_config == 0) { - size_t idx = survive_configi(ctx, "default-cal-conf", SC_GET, 0); - ctx->calibration_config = malloc(sizeof(survive_calibration_config)); - memset(ctx->calibration_config, 0, sizeof(survive_calibration_config)); - *ctx->calibration_config = survive_calibration_config_create_from_idx(idx); - } - return ctx->calibration_config; -} + for (int axis = 0; axis < 2; axis++) { + int opp_axis = axis == 0 ? 1 : 0; -size_t survive_calibration_config_max_idx() { - survive_calibration_config cfg; - memset(&cfg, 0x1, sizeof(survive_calibration_config)); - return survive_calibration_config_index(&cfg); -} + out[axis] = ang[axis]; -static void survive_calibration_options_config_fprint(FILE *file, const survive_calibration_options_config *self) { - fprintf(file, "\t"); - if (!self->enable[0] && !self->enable[1]) { - fprintf(file, "disabled"); - return; + if (f & SVCal_Phase) + out[axis] -= config->phase_scale * phase[axis]; + if (f & SVCal_Tilt) + out[axis] -= (config->tilt_scale * tilt[axis]) * ang[opp_axis]; + if (f & SVCal_Curve) + out[axis] -= config->curve_scale * curve[axis] * ang[opp_axis] * ang[opp_axis]; + if (f & SVCal_Gib) + out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; } - fprintf(file, "swap: %d\n", self->swap); - for (int i = 0; i < 2; i++) { - if (self->enable[i]) { - fprintf(file, "\tinvert[%d]: %d", i, self->invert[i]); - } else { - fprintf(file, "\t%d: disabled", i); - } - } } -void survive_calibration_config_fprint(FILE *file, const survive_calibration_config *self) { - fprintf(file, "Index: %ld\n", survive_calibration_config_index(self)); - - fprintf(file, "Phase: \n"); - survive_calibration_options_config_fprint(file, &self->phase); - fprintf(file, "\n"); - - fprintf(file, "Tilt: \n"); - survive_calibration_options_config_fprint(file, &self->tilt); - fprintf(file, "\n"); - - fprintf(file, "Curve: \n"); - survive_calibration_options_config_fprint(file, &self->curve); - fprintf(file, "\n"); - - fprintf(file, "gibPhase: \n"); - survive_calibration_options_config_fprint(file, &self->gibPhase); - fprintf(file, "\n"); - - fprintf(file, "gibMag: \n"); - survive_calibration_options_config_fprint(file, &self->gibMag); - fprintf(file, "\n"); - - fprintf(file, "gibUseSin: %d\n", self->gibUseSin); -} - -void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum SurviveCalFlag f, const FLT *in, - FLT *out) { +void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config, + const FLT *in, FLT *out) { const BaseStationCal *cal = &ctx->bsd[lh].fcal; - out[0] = in[0] + cal->phase[0]; - out[1] = in[1] + cal->phase[1]; - - FLT tilt_scale = 10; - FLT curve_scale = 10000; - FLT gib_scale = 10; + out[0] = in[0] + config->phase_scale * cal->phase[0]; + out[1] = in[1] + config->phase_scale * cal->phase[1]; + + enum SurviveCalFlag f = config->use_flag; + FLT phase_scale = config->phase_scale; + FLT tilt_scale = config->tilt_scale; + FLT curve_scale = config->curve_scale; + FLT gib_scale = config->gib_scale; const int iterations = 4; for (int i = 0; i < iterations; i++) { FLT last_out[2] = {out[0], out[1]}; @@ -207,17 +65,39 @@ void survive_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, enum Sur int oj = j == 0 ? 1 : 0; out[j] = in[j]; if (!last_iteration || (f & SVCal_Phase)) - out[j] += (cal->phase[j]); + out[j] += phase_scale * cal->phase[j]; if (!last_iteration || (f & SVCal_Tilt)) - out[j] += tan(cal->tilt[j] / tilt_scale) * last_out[oj]; + out[j] += (tilt_scale * cal->tilt[j]) * last_out[oj]; if (!last_iteration || (f & SVCal_Curve)) - out[j] += (cal->curve[j] / curve_scale) * last_out[oj] * last_out[oj]; + out[j] += (cal->curve[j] * curve_scale) * last_out[oj] * last_out[oj]; if (!last_iteration || (f & SVCal_Gib)) - out[j] += cos(cal->gibpha[j] + last_out[j]) * cal->gibmag[1] / gib_scale; + out[j] += sin(cal->gibpha[j] + last_out[j]) * cal->gibmag[j] * gib_scale; } } } +void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, const SurvivePose *pose, FLT *pt, + FLT *out) { + survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], &ctx->calibration_config, pose, pt, out); +} + +void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, FLT *out) { + survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose, point3d, out); +} + +survive_calibration_config survive_calibration_config_ctor() { + return (survive_calibration_config){.use_flag = SVCal_All, + .phase_scale = 1., + .tilt_scale = 1. / 10000., + .curve_scale = 1. / 1000., + .gib_scale = -1. / 10.}; +} + void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { - survive_apply_bsd_calibration_by_flag(ctx, lh, ctx->calibration_flag, in, out); + survive_apply_bsd_calibration_by_flag(ctx, lh, &ctx->calibration_config, in, out); +} + +void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, + int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out) { + return survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out); } diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc index 874ed84..88a43eb 100644 --- a/tools/findoptimalconfig/findoptimalconfig.cc +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -11,35 +11,6 @@ #include #include -std::ostream &operator<<(std::ostream &o, const survive_calibration_options_config &self) { - o << "\t"; - if (!self.enable[0] && !self.enable[1]) { - o << "disabled"; - return o; - } - - o << "swap: " << self.swap << std::endl; - for (int i = 0; i < 2; i++) { - if (self.enable[i]) { - o << "\tinvert[" << i << "]: " << self.invert[i]; - } else { - o << "\t" << i << ": disabled"; - } - } - return o; -} - -std::ostream &operator<<(std::ostream &o, const survive_calibration_config &self) { - o << "Index: " << survive_calibration_config_index(&self) << std::endl; - o << "Phase: " << std::endl << self.phase << std::endl; - o << "Tilt: " << std::endl << self.tilt << std::endl; - o << "Curve: " << std::endl << self.curve << std::endl; - o << "gibPhase: " << std::endl << self.gibPhase << std::endl; - o << "gibMag: " << std::endl << self.gibMag << std::endl; - o << "gibUseSin: " << self.gibUseSin << std::endl; - return o; -} - struct SBAData { int last_acode = -1; int last_lh = -1; @@ -279,9 +250,7 @@ double find_optimal_cal(SurviveContext *ctx, const survive_calibration_config &c double opts[SBA_OPTSSZ] = {0}; double info[SBA_INFOSZ] = {0}; - BaseStationCal cal[2] = {}; - - opts[0] = SBA_INIT_MU; + survive_calibration_config opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; opts[2] = SBA_STOP_THRESH; opts[3] = SBA_STOP_THRESH; @@ -402,49 +371,12 @@ int main(int argc, char **argv) { while (survive_poll(ctx) == 0) { } - survive_calibration_config config = {}; - // config.tilt.enable[0] = config.tilt.enable[1] = 1; - // config.curve.enable[0] = config.curve.enable[1] = 1; - config.phase.enable[0] = config.phase.enable[1] = 1; - // config.gibPhase.enable[0] = config.gibPhase.enable[1] = 1; - // config.gibMag.enable[0] = config.gibMag.enable[1] = 1; + survive_calibration_config config = survive_calibration_config_ctor(); find_optimal_cal(ctx, config, data); - for (int j = 0; j < sections.size(); j++) { - auto &range = sections[j]; - for (size_t _i = 0; _i < (1 << range.first); _i++) { - int i = (_i << range.second); - survive_calibration_config config = survive_calibration_config_create_from_idx(i); - if (i == survive_calibration_config_index(&config)) { - double error = find_avg_reproj_error(ctx, config, data); - errors[j][i] += error; - } - } - std::cerr << "Finished grouping " << j << std::endl; - } - survive_close(ctx); } - for (int i = 0; i < errors.size(); i++) { - std::cout << "Grouping " << i << std::endl; - auto compFunctor = [](std::pair elem1, std::pair elem2) { - if (elem1.second == elem2.second) - return elem1.first < elem2.first; - return elem1.second < elem2.second; - }; - - std::set, typeof(compFunctor)> set(errors[i].begin(), errors[i].end(), compFunctor); - - for (auto err : set) { - survive_calibration_config config = survive_calibration_config_create_from_idx(err.first); - if (err.first == survive_calibration_config_index(&config)) { - double error = err.second; - std::cout << "Config " << err.first << " " << error << std::endl; - std::cout << config << std::endl; - } - } - } return 0; } -- cgit v1.2.3