Merge pull request #110 from cnlohr/reprojection

This adds reprojection functionality to the library

5 files changed, 249 insertions, 1 deletions

diff --git a/Makefile b/Makefile
index b4e65dd..332f59d 100644
--- a/Makefile
+++ b/Makefile
@@ -37,7 +37,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.
 ifeq ($(UNAME), Darwin)
 REDISTS:=$(REDISTS) redist/hid-osx.c
 endif
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o
 
 
 #If you want to use HIDAPI on Linux.
diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h
new file mode 100644
index 0000000..961db2a
--- /dev/null
+++ b/include/libsurvive/survive_reproject.h
@@ -0,0 +1,53 @@
+#pragma once
+#include "survive.h"
+#include <stdbool.h>
+#include <stdlib.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+	bool enable[2];
+	bool invert[2];
+	bool swap;
+} survive_calibration_options_config;
+
+typedef struct {
+
+	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();
+
+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);
+
+// 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
+// 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.
+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);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
index 080ad6a..96442b3 100644
--- a/src/poser_charlesslow.c
+++ b/src/poser_charlesslow.c
@@ -169,6 +169,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
 			copy3d( ctx->bsd[lh].Pose.Pos, LighthousePos );
 			quatcopy( ctx->bsd[lh].Pose.Rot, LighthouseQuat );			
 #define ALT_COORDS
+
 #ifdef ALT_COORDS
 			so->FromLHPose[lh].Pos[0] = LighthousePos[0];
 			so->FromLHPose[lh].Pos[1] = LighthousePos[1];
@@ -188,6 +189,11 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
 			so->FromLHPose[lh].Rot[2] = LighthouseQuat[2];
 			so->FromLHPose[lh].Rot[3] = LighthouseQuat[3];
 #endif
+
+			const FLT rt[4] = {0, 0, 1, 0};
+			FLT tmp[4];
+			quatrotateabout(tmp, so->ctx->bsd[lh].Pose.Rot, rt);
+			memcpy(so->ctx->bsd[lh].Pose.Rot, tmp, sizeof(FLT) * 4);
 		}
 
 		return 0;
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2ed0df0..696cce4 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -10,6 +10,8 @@
 
 #include "survive_cal.h"
 #include "survive_internal.h"
+#include "survive_reproject.h"
+
 #include <math.h>
 #include <string.h>
 #include <sys/stat.h>
@@ -634,6 +636,8 @@ static void handle_calibration( struct SurviveCalData *cd )
 			return;
 		}
 
+		int compute_reprojection_error = config_read_uint32(
+			ctx->global_config_values, "ComputeReprojectError", 0);
 
 		int lh;
 		for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
@@ -645,7 +649,48 @@ static void handle_calibration( struct SurviveCalData *cd )
 
 			fprintf( fobjp, "%f %f %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
 			fprintf( fobjp, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+
+			if (ctx->bsd[lh].PositionSet) {
+				config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0);
+				config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1);
+
+				if (compute_reprojection_error) {
+					FLT reproj_err = 0;
+					size_t cnt = 0;
+					SurviveObject *so = cd->poseobjects[obj];
+					for (size_t idx = 0; idx < so->nr_locations; idx++) {
+						FLT *lengths = fsd.lengths[idx][lh];
+						FLT *pt = fsd.angles[idx][lh];
+						if (lengths[0] < 0 || lengths[1] < 0)
+							continue;
+
+						cnt++;
+						FLT reproj_pt[2];
+						survive_reproject(ctx, lh, so->sensor_locations,
+										  reproj_pt);
+
+						FLT err = 0;
+						for (int dim = 0; dim < 2; dim++) {
+							err += (reproj_pt[dim] - pt[dim]) *
+								   (reproj_pt[dim] - pt[dim]);
+						}
+						reproj_err += sqrt(err);
+					}
+
+					// This represents the average distance we were off in our
+					// reprojection.
+					// Different libraries have slightly different variations on
+					// this theme,
+					// but this one has an intuitive meaning
+					reproj_err = (reproj_err / cnt);
+
+					SV_INFO("Reproject error was %.13g for lighthouse %d",
+							reproj_err, lh);
+				}
+			}
 		}
+
+		config_save(ctx, "config.json");
 	}
 	fclose( fobjp );
 
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
new file mode 100644
index 0000000..7bfc7d7
--- /dev/null
+++ b/src/survive_reproject.c
@@ -0,0 +1,144 @@
+#include "survive_reproject.h"
+#include <../redist/linmath.h>
+#include <math.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_config(
+	const SurviveContext *ctx, const survive_calibration_config *config,
+	int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) {
+	FLT invq[4];
+	quatgetreciprocal(invq, pose->Rot);
+
+	FLT tvec[3];
+	quatrotatevector(tvec, invq, pose->Pos);
+
+	FLT t_pt[3];
+	quatrotatevector(t_pt, invq, pt);
+	for (int i = 0; i < 3; i++)
+		t_pt[i] = t_pt[i] - tvec[i];
+
+	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);
+
+	const BaseStationData *bsd = &ctx->bsd[lighthouse];
+	double phase[2];
+	survive_calibration_options_config_apply(&config->phase, bsd->fcalphase,
+											 phase);
+	double tilt[2];
+	survive_calibration_options_config_apply(&config->tilt, bsd->fcaltilt,
+											 tilt);
+	double curve[2];
+	survive_calibration_options_config_apply(&config->curve, bsd->fcalcurve,
+											 curve);
+	double gibPhase[2];
+	survive_calibration_options_config_apply(&config->gibPhase, bsd->fcalgibpha,
+											 gibPhase);
+	double gibMag[2];
+	survive_calibration_options_config_apply(&config->gibMag, bsd->fcalgibmag,
+											 gibMag);
+
+	out[0] = ang_x + phase[0] + tan(tilt[0]) * y + curve[0] * y * y +
+			 gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0];
+	out[1] = ang_y + phase[1] + tan(tilt[1]) * x + curve[1] * x * x +
+			 gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1];
+}
+
+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);
+}
+
+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 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);
+	}
+	return def;
+}
+
+size_t survive_calibration_config_max_idx() {
+	survive_calibration_config cfg;
+	memset(&cfg, 0x1, sizeof(survive_calibration_config));
+	return survive_calibration_config_index(&cfg);
+}