From 4034cd07ea404df0cb572fbabe9c8ea99f8ebf8f Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 3 May 2018 13:01:31 -0600 Subject: Hacked in tilt / curve metric space stuff --- src/survive_reproject.c | 22 +++++++++------------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/src/survive_reproject.c b/src/survive_reproject.c index ae946fe..23958e4 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -43,7 +43,6 @@ void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const Linmath FLT x = -t_pt[0] / -t_pt[2]; FLT y = t_pt[1] / -t_pt[2]; double xy[] = {x, y}; - double ang[] = {atan(x), atan(y)}; const FLT *phase = bsd->fcal.phase; const FLT *curve = bsd->fcal.curve; @@ -52,6 +51,9 @@ void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const Linmath const FLT *gibMag = bsd->fcal.gibmag; enum SurviveCalFlag f = config->use_flag; + double ang[] = {atan2(-t_pt[0] - (tilt[0] + curve[0] * t_pt[1]) * t_pt[1], -t_pt[2]), + atan2(t_pt[1] - (tilt[1] + curve[1] * t_pt[0]) * t_pt[0], -t_pt[2])}; + for (int axis = 0; axis < 2; axis++) { int opp_axis = axis == 0 ? 1 : 0; @@ -59,10 +61,6 @@ void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const Linmath if (f & SVCal_Phase) out[axis] -= config->phase_scale * phase[axis]; - if (f & SVCal_Tilt) - out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis]; - if (f & SVCal_Curve) - out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis]; if (f & SVCal_Gib) out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; } @@ -84,7 +82,6 @@ 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 xy[] = {x, y}; - double ang[] = {atan(x), atan(y)}; const FLT *phase = bsd->fcal.phase; const FLT *curve = bsd->fcal.curve; @@ -92,6 +89,9 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv const FLT *gibPhase = bsd->fcal.gibpha; const FLT *gibMag = bsd->fcal.gibmag; enum SurviveCalFlag f = config->use_flag; + + double ang[] = {atan2(-t_pt[0] - (tilt[0] + curve[0] * t_pt[1]) * t_pt[1], -t_pt[2]), + atan2(t_pt[1] - (tilt[1] + curve[1] * t_pt[0]) * t_pt[0], -t_pt[2])}; for (int axis = 0; axis < 2; axis++) { int opp_axis = axis == 0 ? 1 : 0; @@ -100,10 +100,6 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv if (f & SVCal_Phase) out[axis] -= config->phase_scale * phase[axis]; - if (f & SVCal_Tilt) - out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis]; - if (f & SVCal_Curve) - out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis]; if (f & SVCal_Gib) out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; } @@ -152,9 +148,9 @@ void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d, survive_calibration_config survive_calibration_config_ctor() { return (survive_calibration_config){.use_flag = SVCal_All, .phase_scale = 1., - .tilt_scale = 1. / 10., - .curve_scale = 1. / 10., - .gib_scale = -1. / 10.}; + .tilt_scale = 1. / 1., + .curve_scale = 1. / 1., + .gib_scale = -1. / 1.}; } void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) { -- cgit v1.2.3