Enabled and then cleaned up warnings

8 files changed, 41 insertions, 74 deletions

diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c
index ff82fd4..057ac3d 100644
--- a/src/poser_general_optimizer.c
+++ b/src/poser_general_optimizer.c
@@ -37,6 +37,10 @@ bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error) {
 		d->failures_to_reset_cntr = d->failures_to_reset;
 		return true;
 	}
+
+	if (d->failures_to_reset_cntr > 0)
+		d->failures_to_reset_cntr--;
+
 	return false;
 }
 
diff --git a/src/poser_sba.c b/src/poser_sba.c
index 8c9e328..07d8f25 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -17,6 +17,7 @@
 #include "string.h"
 #include "survive_cal.h"
 #include "survive_config.h"
+#include "survive_kalman.h"
 #include "survive_reproject.h"
 
 typedef struct {
@@ -26,12 +27,6 @@ typedef struct {
 	SurvivePose camera_params[2];
 } sba_context;
 
-typedef struct {
-	sba_context hdr;
-	int acode;
-	int lh;
-} sba_context_single_sweep;
-
 typedef struct SBAData {
 	GeneralOptimizerData opt;
 
@@ -44,7 +39,9 @@ typedef struct SBAData {
 	int use_jacobian_function;
 	int required_meas;
 
+	survive_kpose_t kpose;
 	SurviveIMUTracker tracker;
+
 	bool useIMU;
 
 	struct {
@@ -89,6 +86,8 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv
 	size_t rtn = 0;
 	SurviveObject *so = d->opt.so;
 
+	// fprintf(stderr, "#");
+
 	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)) {
@@ -96,24 +95,26 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv
 				// FLT a[2];
 				// survive_apply_bsd_calibration(so->ctx, lh, _a, a);
 				vmask[sensor * NUM_LIGHTHOUSES + lh] = 1;
-
 				if (cov) {
 					*(cov++) = d->sensor_variance +
-							   abs(pdl->timecode - scene->timecode[sensor][lh][0]) * d->sensor_variance_per_second /
-								   (double)so->timebase_hz;
+							   abs((int32_t)pdl->timecode - (int32_t)scene->timecode[sensor][lh][0]) *
+								   d->sensor_variance_per_second / (double)so->timebase_hz;
 					*(cov++) = 0;
 					*(cov++) = 0;
 					*(cov++) = d->sensor_variance +
-							   abs(pdl->timecode - scene->timecode[sensor][lh][1]) * d->sensor_variance_per_second /
-								   (double)so->timebase_hz;
+							   abs((int32_t)pdl->timecode - (int32_t)scene->timecode[sensor][lh][1]) *
+								   d->sensor_variance_per_second / (double)so->timebase_hz;
 				}
 				meas[rtn++] = a[0];
 				meas[rtn++] = a[1];
+				// fprintf(stderr, "%.04f %.04f ", a[0], a[1]);
 			} else {
 				vmask[sensor * NUM_LIGHTHOUSES + lh] = 0;
+				// fprintf(stderr, "%.06f %.06f ", sensor, lh, -2,-2);
 			}
 		}
 	}
+	// fprintf(stderr, "\n");
 	return rtn;
 }
 
@@ -126,43 +127,6 @@ void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, S
 		ctx->obj_pose = LinmathPose_Identity;
 }
 
-typedef struct {
-	bool hasInfo;
-	SurvivePose poses;
-} sba_set_position_t;
-
-static void sba_set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) {
-	sba_set_position_t *user = _user;
-	assert(user->hasInfo == false);
-	user->hasInfo = 1;
-	user->poses = *new_pose;
-}
-void *GetDriver(const char *name);
-
-static void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) {
-	SurvivePose obj = *(SurvivePose *)bi;
-	int sensor_idx = j >> 1;
-
-	sba_context_single_sweep *ctx = (sba_context_single_sweep *)(adata);
-	SurviveObject *so = ctx->hdr.so;
-	int lh = ctx->lh;
-	int acode = ctx->acode;
-
-	assert(lh < 2);
-	assert(sensor_idx < so->sensor_ct);
-
-	quatnormalize(obj.Rot, obj.Rot);
-	FLT xyz[3];
-	ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]);
-
-	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
-	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
-
-	FLT out[2];
-	survive_reproject_from_pose(so->ctx, lh, camera, xyz, out);
-	*xij = out[acode];
-}
-
 static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 	SurvivePose obj = *(SurvivePose *)bi;
 	int sensor_idx = j >> 1;
@@ -174,7 +138,7 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada
 	assert(lh < 2);
 	assert(sensor_idx < so->sensor_ct);
 
-	// quatnormalize(obj.Rot, obj.Rot);
+	quatnormalize(obj.Rot, obj.Rot);
 
 	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
@@ -193,7 +157,7 @@ static void str_metric_function_jac(int j, int i, double *bi, double *xij, void
 	assert(lh < 2);
 	assert(sensor_idx < so->sensor_ct);
 
-	// quatnormalize(obj.Rot, obj.Rot);
+	quatnormalize(obj.Rot, obj.Rot);
 
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
 	survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh],
@@ -208,8 +172,9 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 
 	char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
 	double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
-	double *cov =
-		d->sensor_variance_per_second > 0. ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES) : 0;
+	double *cov = (d->sensor_variance_per_second > 0. && d->sensor_variance)
+					  ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES)
+					  : 0;
 	size_t meas_size = construct_input_from_scene(d, pdl, scene, vmask, meas, cov);
 
 	static int failure_count = 500;
@@ -271,9 +236,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 		quatnormalize(soLocation.Rot, soLocation.Rot);
 		*out = soLocation;
 		rtn = info[1] / meas_size * 2;
-	} else
-
-	{
+	} else {
 		SurviveContext *ctx = so->ctx;
 		// Docs say info[0] should be divided by meas; I don't buy it really...
 		if (error_failure) {
@@ -418,6 +381,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 			if (error < 0) {
 
 			} else {
+				quatnormalize(estimate.Rot, estimate.Rot);
+
 				if (d->useIMU) {
 					FLT var_meters = 0.5;
 					FLT var_quat = error + .05;
@@ -428,6 +393,11 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 					estimate = d->tracker.pose;
 				}
 
+				LinmathVec3d pvar = {.1, .1, .1};
+				FLT rvar = .01;
+				survive_kpose_integrate_pose(&d->kpose, lightData->timecode, &estimate, pvar, rvar);
+				estimate = d->kpose.state.pose;
+
 				PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
 			}
 		}
diff --git a/src/survive.c b/src/survive.c
index 58566c1..ce17295 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -581,7 +581,7 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in
 
 #include <zlib.h>
 
-int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int inlen, char *output, int outlen) {
+int survive_simple_inflate(struct SurviveContext *ctx, const uint8_t *input, int inlen, uint8_t *output, int outlen) {
 	z_stream zs; // Zlib stream.  May only be used by configuration at beginning and by USB thread periodically.
 	memset(&zs, 0, sizeof(zs));
 	inflateInit(&zs); /// Consider checking error
@@ -614,4 +614,4 @@ const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose;
 
 int8_t survive_object_sensor_ct(SurviveObject *so) { return so->sensor_ct; }
 const FLT *survive_object_sensor_locations(SurviveObject *so) { return so->sensor_locations; }
-const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; }
\ No newline at end of file
+const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; }
diff --git a/src/survive_cal.c b/src/survive_cal.c
index faf2ac7..ad62d22 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -417,7 +417,6 @@ static void handle_calibration( struct SurviveCalData *cd )
 	mkdir( "calinfo", 0755 );
 #endif
 	int sen, axis, lh;
-	FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES];
 
 	//Just to get it out of the way early, we'll calculate the sync-pulse-lengths here.
 	FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" );
@@ -620,7 +619,6 @@ static void handle_calibration( struct SurviveCalData *cd )
 			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];
-			fsd.synctimes[i][j] = temp_syncs[i][j];
 		}
 
 		int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd );
diff --git a/src/survive_imu.c b/src/survive_imu.c
index 3622b2d..f866caf 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -66,18 +66,6 @@ static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_rece
 	return diff;
 }
 
-void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose) {
-	tracker->pose = *pose;
-
-	for (int i = 0; i < 3; i++) {
-		tracker->current_velocity[i] = 0;
-	}
-	//(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.;
-
-	tracker->lastGTTime = timecode;
-	tracker->lastGT = *pose;
-}
-
 static const int imu_calibration_iterations = 100;
 
 static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const LinmathVec3d accel) {
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index dc5c0d4..e83b2fc 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -4,6 +4,9 @@
 bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance,
 										  uint32_t timecode_now, uint32_t idx, int lh) {
 	const uint32_t *data_timecode = self->timecode[idx][lh];
+	if (self->lengths[idx][lh][0] == 0 || self->lengths[idx][lh][1] == 0)
+		return false;
+
 	return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance);
 }
 
@@ -26,7 +29,7 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa
 
 	*angle = lightData->angle;
 	*data_timecode = lightData->timecode;
-	*length = lightData->length * 48000000;
+	*length = (uint32_t)(lightData->length * 48000000);
 }
 
 uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000) + 5000;
diff --git a/src/survive_turveybiguator.c b/src/survive_turveybiguator.c
index 8dbce46..5b10e63 100644
--- a/src/survive_turveybiguator.c
+++ b/src/survive_turveybiguator.c
@@ -155,8 +155,8 @@ static uint8_t remove_outliers(SurviveObject *so) {
 
 	//	uint8_t removed_outliers = 0;
 
-	uint32_t d1 = abs(*min - mean);
-	uint32_t d2 = abs(*max - mean);
+	uint32_t d1 = mean - *min;
+	uint32_t d2 = *max - mean;
 
 	if (d1 > d2) {
 		if (d1 > tau_test) {
@@ -199,6 +199,7 @@ static void handle_lightcap2_process_sweep_data(SurviveObject *so) {
 	{
 		unsigned int longest_pulse = 0;
 		unsigned int timestamp_of_longest_pulse = 0;
+		(void)timestamp_of_longest_pulse;
 		for (int i = 0; i < SENSORS_PER_OBJECT; i++) {
 			if (lcd->sweep.sweep_len[i] > longest_pulse) {
 				longest_pulse = lcd->sweep.sweep_len[i];
@@ -216,6 +217,7 @@ static void handle_lightcap2_process_sweep_data(SurviveObject *so) {
 
 			{
 				static int counts[SENSORS_PER_OBJECT][2] = {0};
+				(void)counts;
 
 				//				if (lcd->per_sweep.activeLighthouse == 0 && !allZero)
 				if (lcd->per_sweep.activeLighthouse > -1 && !allZero) {
diff --git a/src/survive_vive.c b/src/survive_vive.c
index d9cbc3e..88bc065 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -961,6 +961,8 @@ void calibrate_acc(SurviveObject* so, FLT* agm) {
 		agm[1] *= so->acc_scale[1];
 		agm[2] *= so->acc_scale[2];
 	}
+
+	fprintf(stderr, "%f\t%f\t%f\n", agm[0], agm[1], agm[2]);
 }
 
 void calibrate_gyro(SurviveObject* so, FLT* agm) {