From 22d40fb360fdb65da7916fb87f9b199f4f401f05 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 1 Apr 2018 07:52:00 -0600 Subject: overhal to calibration --- src/poser_epnp.c | 10 ++- src/poser_sba.c | 30 +++++---- src/survive.c | 2 + src/survive_cal.c | 26 ++++---- src/survive_config.c | 22 ++++--- src/survive_process.c | 9 ++- src/survive_reproject.c | 129 +++++++++++++++++++++++++++++++-------- src/survive_sensor_activations.c | 3 +- 8 files changed, 167 insertions(+), 64 deletions(-) (limited to 'src') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 7e86542..c05450a 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -6,6 +6,7 @@ #include #include +#include #include "epnp/epnp.h" #include "linmath.h" @@ -71,7 +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 = pdfs->angles[i][lh]; + FLT ang[2]; + survive_apply_bsd_calibration(so->ctx, lh, _ang, ang); if (lengths[0] < 0 || lengths[1] < 0) continue; @@ -103,7 +106,10 @@ static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActiv for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) { if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, timecode, sensor_idx, lh)) { - double *angles = scene->angles[sensor_idx][lh]; + FLT *_angles = scene->angles[sensor_idx][lh]; + FLT angles[2]; + survive_apply_bsd_calibration(so->ctx, lh, _angles, angles); + epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0], so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2], tan(angles[0]), tan(angles[1])); diff --git a/src/poser_sba.c b/src/poser_sba.c index df28a2d..4a4ed8f 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -40,6 +40,8 @@ typedef struct SBAData { int successes_to_reset; int successes_to_reset_cntr; + FLT max_error; + FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; @@ -71,7 +73,9 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, continue; } - double *angles = pdfs->angles[sensor][lh]; + 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]; @@ -89,7 +93,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]; + 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) { @@ -273,7 +279,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o if (distance > 1.) status = -1; } - if (status > 0) { + if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); @@ -283,7 +289,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o 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) { + 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; @@ -389,11 +395,12 @@ 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, 1); + d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100); d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); - - d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, 1600000 * 4); + 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, 0.001); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->so = so; @@ -403,6 +410,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -416,8 +424,8 @@ 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(); - error = run_sba_find_3d_structure(d, config, lightData, scene, 50, .5); + survive_calibration_config config = *survive_calibration_default_config(ctx); + error = run_sba_find_3d_structure(d, config, lightData, scene, 100, .5); d->last_lh = lightData->lh; d->last_acode = lightData->acode; } @@ -435,9 +443,9 @@ 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(); + 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, 50, .005); + double error = run_sba(config, 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 a15e0ed..63ad2ba 100644 --- a/src/survive.c +++ b/src/survive.c @@ -219,6 +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); + return ctx; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 2fc1896..e094e7b 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -61,19 +61,20 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) //print_lighthouse_info_v6(&v6); b->BaseStationID = v6.id; - b->fcalphase[0] = v6.fcal_0_phase; - b->fcalphase[1] = v6.fcal_1_phase; - b->fcaltilt[0] = tan(v6.fcal_0_tilt); - b->fcaltilt[1] = tan(v6.fcal_1_tilt); //XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18 - b->fcalcurve[0] = v6.fcal_0_curve; - b->fcalcurve[1] = v6.fcal_1_curve; - b->fcalgibpha[0] = v6.fcal_0_gibphase; - b->fcalgibpha[1] = v6.fcal_1_gibphase; - b->fcalgibmag[0] = v6.fcal_0_gibmag; - b->fcalgibmag[1] = v6.fcal_1_gibmag; + b->fcal.phase[0] = v6.fcal_0_phase; + b->fcal.phase[1] = v6.fcal_1_phase; + b->fcal.tilt[0] = (v6.fcal_0_tilt); + b->fcal.tilt[1] = (v6.fcal_1_tilt); // XXX??? Is this right? See https://github.com/cnlohr/libsurvive/issues/18 + b->fcal.curve[0] = v6.fcal_0_curve; + b->fcal.curve[1] = v6.fcal_1_curve; + b->fcal.gibpha[0] = v6.fcal_0_gibphase; + b->fcal.gibpha[1] = v6.fcal_1_gibphase; + b->fcal.gibmag[0] = v6.fcal_0_gibmag; + b->fcal.gibmag[1] = v6.fcal_1_gibmag; b->accel[0] = v6.accel_dir_x; b->accel[1] = v6.accel_dir_y; b->accel[2] = v6.accel_dir_z; + b->mode = v6.mode_current; b->OOTXSet = 1; config_set_lighthouse(ctx->lh_config,b,id); @@ -617,8 +618,9 @@ 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]; + // 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.synctimes[i][j] = temp_syncs[i][j]; } diff --git a/src/survive_config.c b/src/survive_config.c index 7b68ae4..d67cd8e 100644 --- a/src/survive_config.c +++ b/src/survive_config.c @@ -119,12 +119,13 @@ void config_read_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8 FLT defaults[7] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; bsd->BaseStationID = config_read_uint32(cg, "id", 0); + bsd->mode = config_read_uint32(cg, "mode", 0); config_read_float_array(cg, "pose", &bsd->Pose.Pos[0], defaults, 7); - config_read_float_array(cg, "fcalphase", bsd->fcalphase, defaults, 2); - config_read_float_array(cg, "fcaltilt", bsd->fcaltilt, defaults, 2); - config_read_float_array(cg, "fcalcurve", bsd->fcalcurve, defaults, 2); - config_read_float_array(cg, "fcalgibpha", bsd->fcalgibpha, defaults, 2); - config_read_float_array(cg, "fcalgibmag", bsd->fcalgibmag, defaults, 2); + config_read_float_array(cg, "fcalphase", bsd->fcal.phase, defaults, 2); + config_read_float_array(cg, "fcaltilt", bsd->fcal.tilt, defaults, 2); + config_read_float_array(cg, "fcalcurve", bsd->fcal.curve, defaults, 2); + config_read_float_array(cg, "fcalgibpha", bsd->fcal.gibpha, defaults, 2); + config_read_float_array(cg, "fcalgibmag", bsd->fcal.gibmag, defaults, 2); bsd->PositionSet = config_read_uint32(cg, "PositionSet", 0); } @@ -132,12 +133,13 @@ void config_set_lighthouse(config_group *lh_config, BaseStationData *bsd, uint8_ config_group *cg = lh_config + idx; config_set_uint32(cg, "index", idx); config_set_uint32(cg, "id", bsd->BaseStationID); + config_set_uint32(cg, "mode", bsd->mode); config_set_float_a(cg, "pose", &bsd->Pose.Pos[0], 7); - config_set_float_a(cg, "fcalphase", bsd->fcalphase, 2); - config_set_float_a(cg, "fcaltilt", bsd->fcaltilt, 2); - config_set_float_a(cg, "fcalcurve", bsd->fcalcurve, 2); - config_set_float_a(cg, "fcalgibpha", bsd->fcalgibpha, 2); - config_set_float_a(cg, "fcalgibmag", bsd->fcalgibmag, 2); + config_set_float_a(cg, "fcalphase", bsd->fcal.phase, 2); + config_set_float_a(cg, "fcaltilt", bsd->fcal.tilt, 2); + config_set_float_a(cg, "fcalcurve", bsd->fcal.curve, 2); + config_set_float_a(cg, "fcalgibpha", bsd->fcal.gibpha, 2); + config_set_float_a(cg, "fcalgibmag", bsd->fcal.gibmag, 2); config_set_uint32(cg, "PositionSet", bsd->PositionSet); } diff --git a/src/survive_process.c b/src/survive_process.c index 97fbc46..62459f2 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -51,14 +51,17 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode static int use_bsd_cal = -1; if(use_bsd_cal == -1) { use_bsd_cal = survive_configi(ctx, "use-bsd-cal", SC_GET, 1); + if (use_bsd_cal == 0) { + SV_INFO("Not using BSD calibration values"); + } } if(use_bsd_cal) { BaseStationData * bsd = &ctx->bsd[base_station]; - //XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 - angle += bsd->fcalphase[axis]; + // XXX TODO: This seriously needs to be worked on. See: https://github.com/cnlohr/libsurvive/issues/18 + // angle += (use_bsd_cal == 2 ? -1 : 1) * bsd->fcal.phase[axis]; // angle += bsd->fcaltilt[axis] * predicted_angle(axis1); - + //TODO!!! } diff --git a/src/survive_reproject.c b/src/survive_reproject.c index eabdb07..845f30a 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -1,6 +1,7 @@ #include "survive_reproject.h" #include <../redist/linmath.h> #include +#include #include static void survive_calibration_options_config_normalize( @@ -72,9 +73,8 @@ static FLT gibf(bool useSin, FLT v) { return cos(v); } -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) { +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; quatgetreciprocal(invq, pose->Rot); @@ -92,33 +92,36 @@ void survive_reproject_from_pose_with_config( double ang_x = atan(x); double ang_y = atan(y); - const BaseStationData *bsd = &ctx->bsd[lighthouse]; double phase[2]; - survive_calibration_options_config_apply(&config->phase, bsd->fcalphase, - phase); + survive_calibration_options_config_apply(&config->phase, bsd->fcal.phase, phase); double tilt[2]; - survive_calibration_options_config_apply(&config->tilt, bsd->fcaltilt, - tilt); + survive_calibration_options_config_apply(&config->tilt, bsd->fcal.tilt, tilt); double curve[2]; - survive_calibration_options_config_apply(&config->curve, bsd->fcalcurve, - curve); + survive_calibration_options_config_apply(&config->curve, bsd->fcal.curve, curve); double gibPhase[2]; - survive_calibration_options_config_apply(&config->gibPhase, bsd->fcalgibpha, - gibPhase); + survive_calibration_options_config_apply(&config->gibPhase, bsd->fcal.gibpha, gibPhase); double gibMag[2]; - survive_calibration_options_config_apply(&config->gibMag, bsd->fcalgibmag, - gibMag); + survive_calibration_options_config_apply(&config->gibMag, bsd->fcal.gibmag, gibMag); - out[0] = ang_x + phase[0] + tan(tilt[0]) * y + curve[0] * y * y + + 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] + tan(tilt[1]) * x + curve[1] * x * x + + 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(), lighthouse, pose, pt, out); + survive_reproject_from_pose_with_config(ctx, survive_calibration_default_config(ctx), lighthouse, pose, pt, out); } void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, @@ -127,14 +130,15 @@ void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, point3d, out); } -const survive_calibration_config *survive_calibration_default_config() { - static survive_calibration_config *def = 0; - if (def == 0) { - def = malloc(sizeof(survive_calibration_config)); - memset(def, 0, sizeof(survive_calibration_config)); - *def = survive_calibration_config_create_from_idx(0); +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 def; + return ctx->calibration_config; } size_t survive_calibration_config_max_idx() { @@ -142,3 +146,78 @@ size_t survive_calibration_config_max_idx() { memset(&cfg, 0x1, sizeof(survive_calibration_config)); return survive_calibration_config_index(&cfg); } + +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; + } + + 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) { + 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; + const int iterations = 4; + for (int i = 0; i < iterations; i++) { + FLT last_out[2] = {out[0], out[1]}; + bool last_iteration = i == iterations - 1; + for (int j = 0; j < 2; j++) { + int oj = j == 0 ? 1 : 0; + out[j] = in[j]; + if (!last_iteration || (f & SVCal_Phase)) + out[j] += (cal->phase[j]); + if (!last_iteration || (f & SVCal_Tilt)) + out[j] += tan(cal->tilt[j] / tilt_scale) * last_out[oj]; + if (!last_iteration || (f & SVCal_Curve)) + 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; + } + } +} + +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); +} diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index e42b50e..dc5c0d4 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -1,3 +1,4 @@ +#include #include bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance, @@ -28,4 +29,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa *length = lightData->length * 48000000; } -uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000); +uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000) + 5000; -- cgit v1.2.3