Tweaked how reproject / calibate interact

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 <assert.h>
+#include <linmath.h>
 #include <math.h>
+#include <poser.h>
 #include <string.h>
 #include <sys/stat.h>
 #include <sys/types.h>
-#include <linmath.h>
-#include <assert.h>
 
 #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 <stdio.h>
 #include <string.h>
 
-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 <sba/sba.h>
 #include <survive_reproject.h>
 
-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<size_t, double> elem1, std::pair<size_t, double> elem2) {
-			if (elem1.second == elem2.second)
-				return elem1.first < elem2.first;
-			return elem1.second < elem2.second;
-		};
-
-		std::set<std::pair<size_t, double>, 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;
 }