Merge branch 'master' into master

5 files changed, 67 insertions, 28 deletions

diff --git a/src/poser.c b/src/poser.c
index 1c638f8..9a0de24 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -1,6 +1,7 @@
 #include "math.h"
 #include <linmath.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <survive.h>
 
 #define _USE_MATH_DEFINES // for C
@@ -21,6 +22,12 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui
 	} else {
 		const FLT up[3] = {0, 0, 1};
 
+		if (quatmagnitude(lighthouse_pose->Rot) == 0) {
+			SurviveContext *ctx = so->ctx;
+			SV_INFO("Pose func called with invalid pose.");
+			return;
+		}
+
 		// Assume that the space solved for is valid but completely arbitrary. We are going to do a few things:
 		// a) Using the gyro data, normalize it so that gravity is pushing straight down along Z
 		// c) Assume the object is at origin
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index f5fa127..c294c0c 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -27,6 +27,12 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer
 
 	CvMat R = cvMat(3, 3, CV_64F, r);
 	CvMat T = cvMat(3, 1, CV_64F, rtn.Pos);
+
+	// Super degenerate inputs will project us basically right in the camera. Detect and reject
+	if (magnitude3d(rtn.Pos) < 0.25) {
+		return rtn;
+	}
+
 	// Requested output is camera -> world, so invert
 	if (cameraToWorld) {
 		FLT tmp[3];
@@ -81,7 +87,10 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
 		}
 
 		SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true);
-		PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
+
+		if (quatmagnitude(lighthouse2object.Rot) != 0.0) {
+			PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
+		}
 
 		epnp_dtor(&pnp);
 	}
@@ -123,8 +132,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 				epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct);
 
 				add_correspondences(so, &pnp, scene, lightData);
+				static int required_meas = -1;
+				if (required_meas == -1)
+					required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4);
 
-				if (pnp.number_of_correspondences > 4) {
+				if (pnp.number_of_correspondences > required_meas) {
 
 					SurvivePose pose = solve_correspondence(so, &pnp, false);
 
diff --git a/src/poser_sba.c b/src/poser_sba.c
index a1fdea6..069e1d0 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -37,6 +37,9 @@ typedef struct SBAData {
 	int failures_to_reset_cntr;
 	int successes_to_reset;
 	int successes_to_reset_cntr;
+
+	int required_meas;
+
 } SBAData;
 
 void metric_function(int j, int i, double *aj, double *xij, void *adata) {
@@ -175,7 +178,7 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
 	survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij);
 }
-
+#if 0
 static double run_sba_find_3d_structure_single_sweep(survive_calibration_config options, PoserDataLight *pdl,
 													 SurviveObject *so, SurviveSensorActivations *scene, int acode,
 													 int lh, int max_iterations /* = 50*/,
@@ -187,7 +190,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
 	size_t meas_size = construct_input_from_scene_single_sweep(so, pdl, scene, vmask, meas, acode, lh);
 
 	static int failure_count = 500;
-	if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) {
+	if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < d->required_meas) {
 		if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
 			SurviveContext *ctx = so->ctx;
 			SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
@@ -261,7 +264,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
 		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 < 8) {
+		if (cnt++ > 1000 || meas_size < d->required_meas) {
 			SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (unsigned int)meas_size);
 			SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
 			cnt = 0;
@@ -270,7 +273,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
 
 	return info[1] / meas_size * 2;
 }
-
+#endif
 static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl,
 										SurviveObject *so, SurviveSensorActivations *scene,
 										int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
@@ -281,7 +284,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o
 	size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas);
 
 	static int failure_count = 500;
-	if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 7) {
+	if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < d->required_meas) {
 		if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
 			SurviveContext *ctx = so->ctx;
 			SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
@@ -361,7 +364,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 < 8) {
+		if (cnt++ > 1000 || meas_size < d->required_meas) {
 			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;
@@ -436,8 +439,12 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 
 	if (status >= 0) {
 		SurvivePose additionalTx = {0};
-		PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &additionalTx, &sbactx.camera_params[0], &sbactx.obj_pose);
-		PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &additionalTx, &sbactx.camera_params[1], &sbactx.obj_pose);
+		for (int i = 0; i < NUM_LIGHTHOUSES; i++) {
+			if (quatmagnitude(sbactx.camera_params[i].Rot) != 0) {
+				PoserData_lighthouse_pose_func(&pdfs->hdr, so, i, &additionalTx, &sbactx.camera_params[i],
+											   &sbactx.obj_pose);
+			}
+		}
 	} else {
 		SurviveContext *ctx = so->ctx;
 		SV_INFO("SBA was unable to run %d", status);
@@ -456,16 +463,20 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 }
 
 int PoserSBA(SurviveObject *so, PoserData *pd) {
+	SurviveContext *ctx = so->ctx;
 	if (so->PoserData == 0) {
 		so->PoserData = calloc(1, sizeof(SBAData));
 		SBAData *d = so->PoserData;
 		d->failures_to_reset_cntr = 0;
-		d->failures_to_reset = 5;
+		d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
 		d->successes_to_reset_cntr = 0;
-		d->successes_to_reset = 20;
+		d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 1);
+
+		d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
+
+		SV_INFO("Initializing SBA with %d required measurements", d->required_meas);
 	}
 	SBAData *d = so->PoserData;
-	SurviveContext *ctx = so->ctx;
 	switch (pd->pt) {
 	case POSERDATA_LIGHT: {
 		// No poses if calibration is ongoing
diff --git a/src/survive_charlesbiguator.c b/src/survive_charlesbiguator.c
index fbba888..83b3681 100644
--- a/src/survive_charlesbiguator.c
+++ b/src/survive_charlesbiguator.c
@@ -1,6 +1,7 @@
 //<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
 //
 #include "survive_internal.h"
+#include <assert.h>
 #include <math.h> /* for sqrt */
 #include <stdint.h>
 #include <string.h>
@@ -15,12 +16,16 @@ static int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	if (acode & 1)
 		return -1;
 
-	return (acode >> 1) - 6;
+	int32_t rtn = (acode >> 1) - 6;
+	if (rtn > 7 || rtn < 0) {
+		return -1;
+	}
+	return rtn;
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 ///////The charles disambiguator.  Don't use this, mostly here for
-///debugging.///////////////////////////////////////////////////////
+/// debugging.///////////////////////////////////////////////////////
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 static void HandleOOTX(SurviveContext *ctx, SurviveObject *so) {
@@ -169,21 +174,23 @@ void DisambiguatorCharles(SurviveObject *so, LightcapElement *le) {
 
 		int32_t main_divisor = so->timebase_hz / 384000; // 125 @ 48 MHz.
 		int acode = decode_acode(so->last_sync_length[0], main_divisor);
-		int whichlh;
-		if (acode < 0)
-			whichlh = 1;
-		else
-			whichlh = (acode >> 2);
-		int32_t dl = so->last_sync_time[whichlh];
 
-		if (!so->did_handle_ootx)
-			HandleOOTX(ctx, so);
+		// If acode isn't right; don't even think of emitting an event
+		if (acode >= 0) {
+			int whichlh = (acode >> 2);
+			assert(whichlh <= 1);
+			int32_t dl = so->last_sync_time[whichlh];
 
-		int32_t offset_from = le->timestamp - dl + le->length / 2;
+			if (!so->did_handle_ootx)
+				HandleOOTX(ctx, so);
 
-		// Make sure pulse is in valid window
-		if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time && offset_from > so->pulse_in_clear_time) {
-			ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh);
+			int32_t offset_from = le->timestamp - dl + le->length / 2;
+
+			// Make sure pulse is in valid window
+			if (offset_from < so->timecenter_ticks * 2 - so->pulse_in_clear_time &&
+				offset_from > so->pulse_in_clear_time) {
+				ctx->lightproc(so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh);
+			}
 		}
 	} else {
 		// printf( "FAIL %d   %d - %d = %d\n", le->length, so->last_photo_time, le->timestamp, so->last_photo_time -
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index 4d1801c..e42b50e 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -21,9 +21,11 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa
 	int axis = (lightData->acode & 1);
 	uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis];
 	FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis];
+	uint32_t *length = &self->lengths[lightData->sensor_id][lightData->lh][axis];
 
 	*angle = lightData->angle;
 	*data_timecode = lightData->timecode;
+	*length = lightData->length * 48000000;
 }
 
-uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000);
\ No newline at end of file
+uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000);