Made calibration on other posers use calibration data

9 files changed, 62 insertions, 32 deletions

diff --git a/.gitignore b/.gitignore
index 8cfd2be..f39d866 100644
--- a/.gitignore
+++ b/.gitignore
@@ -19,6 +19,7 @@ simple_pose_test
 *.csv
 calinfo/
 *.log
+*.png
 
 # Windows specific
 *.dll
diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h
index 6546e66..e4f21d0 100644
--- a/include/libsurvive/survive_reproject.h
+++ b/include/libsurvive/survive_reproject.h
@@ -17,8 +17,9 @@ void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct s
 											 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
+// a 3d point and finds what the effective 'angle' value for a given lighthouse system
 // would be.
+//
 // 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.
@@ -26,10 +27,17 @@ void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct s
 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_apply_bsd_calibration_by_flag(SurviveContext *ctx, int lh, struct survive_calibration_config *config,
-										   const FLT *in, FLT *out);
+// This is given input from the light sensors and approximates the idealized version of them
+// by incorporating the calibration data from the lighthouse. In theory, it's an approximation
+// but in practice in converges pretty quickly and to a good degree of accuracy.
+// That said, all things being equal, it is better to compare reprojection to raw incoming
+// data if you are looking to minimize that error.
 void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out);
 
+// Same as above, but lets you specify the configuration. Used internally and also in some tools
+void survive_apply_bsd_calibration_by_config(SurviveContext *ctx, int lh, struct survive_calibration_config *config,
+											 const FLT *in, FLT *out);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
index bc6683a..b44225e 100644
--- a/src/poser_charlesslow.c
+++ b/src/poser_charlesslow.c
@@ -1,12 +1,13 @@
+#include "linmath.h"
 #include "survive_cal.h"
+#include <dclapack.h>
+#include <linmath.h>
 #include <math.h>
-#include <string.h>
-#include "linmath.h"
-#include <survive.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <dclapack.h>
-#include <linmath.h>
+#include <string.h>
+#include <survive.h>
+#include <survive_reproject.h>
 
 typedef struct
 {
@@ -255,9 +256,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr
 			int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
 			if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
 
+			FLT out[2] = {};
+			survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out);
+
 			//Find out where our ray shoots forth from.
-			FLT ax = fs->angles[p][lh][0];
-			FLT ay = fs->angles[p][lh][1];
+			FLT ax = out[0];
+			FLT ay = out[1];
+
 			//NOTE: Inputs may never be output with cross product.
 			//Create a fictitious normalized ray.  Imagine the lighthouse is pointed
 			//straight in the +z direction, this is the lighthouse ray to the point.
@@ -338,8 +343,13 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr
 		if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
 
 		//Find out where our ray shoots forth from.
-		FLT ax = fs->angles[p][lh][0];
-		FLT ay = fs->angles[p][lh][1];
+		FLT out[2] = {};
+		survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out);
+
+		// Find out where our ray shoots forth from.
+		FLT ax = out[0];
+		FLT ay = out[1];
+
 		FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
 		RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
 
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index c47bceb..9cdab45 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -1,12 +1,13 @@
+#include "linmath.h"
 #include "survive_cal.h"
+#include <dclapack.h>
+#include <linmath.h>
 #include <math.h>
-#include <string.h>
-#include "linmath.h"
-#include <survive.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <dclapack.h>
-#include <linmath.h>
+#include <string.h>
+#include <survive.h>
+#include <survive_reproject.h>
 
 // Dave talks about this poser here: https://www.youtube.com/watch?v=nSbEltdH9vM&feature=youtu.be&t=2h29m47s
 
@@ -139,8 +140,10 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 				//Load all our valid points into something the LHFinder can use.
 				if( fs->lengths[i][LH_ID][0] > 0 )
 				{
-					S_in[0][max_hits] = fs->angles[i][LH_ID][0];
-					S_in[1][max_hits] = fs->angles[i][LH_ID][1];
+					FLT out[2];
+					survive_apply_bsd_calibration(ctx, LH_ID, fs->angles[i][LH_ID], out);
+					S_in[0][max_hits] = out[0];
+					S_in[1][max_hits] = out[1];
 					X_in[0][max_hits] = so->sensor_locations[i*3+0];
 					X_in[1][max_hits] = so->sensor_locations[i*3+1];
 					X_in[2][max_hits] = so->sensor_locations[i*3+2];
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 2cbd9c1..c05450a 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 49854f2..bd7d520 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -73,8 +73,6 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs,
 			}
 
 			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];
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 035fca7..4628207 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -13,6 +13,8 @@
 #include <stdlib.h>
 #else
 #include <malloc.h> //for alloca
+#include <survive_reproject.h>
+
 #endif
 
 
@@ -1786,8 +1788,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					to->sensor[sensorCount].point.x = point[0];
 					to->sensor[sensorCount].point.y = point[1];
 					to->sensor[sensorCount].point.z = point[2];
-					to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
-					to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
+
+					FLT out[2];
+					survive_apply_bsd_calibration(ctx, 0, fs->angles[i][0], out);
+
+					to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2;   // lighthouse 0, angle 1 (vertical)
 
 					sensorCount++;
 				}
@@ -1822,8 +1828,12 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					to->sensor[sensorCount].point.x = point[0];
 					to->sensor[sensorCount].point.y = point[1];
 					to->sensor[sensorCount].point.z = point[2];
-					to->sensor[sensorCount].theta = fs->angles[i][lh][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
-					to->sensor[sensorCount].phi = fs->angles[i][lh][1] + LINMATHPI / 2; // lighthosue 0, angle 1 (vertical)
+
+					FLT out[2];
+					survive_apply_bsd_calibration(ctx, lh, fs->angles[i][lh], out);
+
+					to->sensor[sensorCount].theta = out[0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
+					to->sensor[sensorCount].phi = out[1] + LINMATHPI / 2;   // lighthosue 0, angle 1 (vertical)
 					sensorCount++;
 				}
 			}
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index 751abc0..0eaceb2 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -46,8 +46,8 @@ void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const surv
 
 }
 
-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_by_config(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] + config->phase_scale * cal->phase[0];
 	out[1] = in[1] + config->phase_scale * cal->phase[1];
@@ -95,7 +95,7 @@ survive_calibration_config survive_calibration_config_ctor() {
 }
 
 void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, FLT *out) {
-	survive_apply_bsd_calibration_by_flag(ctx, lh, &ctx->calibration_config, in, out);
+	survive_apply_bsd_calibration_by_config(ctx, lh, &ctx->calibration_config, in, out);
 }
 
 void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config,
diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc
index 89d67c4..caa7a66 100644
--- a/tools/showreproject/showreproject.cc
+++ b/tools/showreproject/showreproject.cc
@@ -176,7 +176,7 @@ void drawbsds(SurviveContext *ctx) {
 					FLT out[2];
 					auto config = survive_calibration_config_ctor();
 					config.use_flag = f;
-					survive_apply_bsd_calibration_by_flag(ctx, lh, &config, in, out);
+					survive_apply_bsd_calibration_by_config(ctx, lh, &config, in, out);
 					double ex = out[0] - in[0];
 					double ey = out[1] - in[1];
 					if (f == SVCal_All) {