Tweaked thresholds, played with single sweep solver

2 files changed, 171 insertions, 21 deletions

diff --git a/src/poser_sba.c b/src/poser_sba.c
index 44af9c1..a29dffe 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -22,6 +22,12 @@ typedef struct {
 	SurviveObject *so;
 } sba_context;
 
+typedef struct {
+    sba_context hdr;
+    int acode;
+    int lh;
+} sba_context_single_sweep;
+
 void metric_function(int j, int i, double *aj, double *xij, void *adata) {
 	sba_context *ctx = (sba_context *)(adata);
 	SurviveObject *so = ctx->so;
@@ -51,6 +57,26 @@ size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *
 	return measCount;
 }
 
+size_t construct_input_from_scene_single_sweep(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene,
+                                  char *vmask, double *meas, int acode, int lh) {
+    size_t rtn = 0;
+
+    for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
+        const uint32_t *data_timecode = scene->timecode[sensor][lh];
+        if (pdl->timecode - data_timecode[acode & 1] <= SurviveSensorActivations_default_tolerance) {
+            double *a = scene->angles[sensor][lh];
+            vmask[sensor * NUM_LIGHTHOUSES + lh] = 1;
+            meas[rtn++] = a[acode & 0x1];
+        } else {
+            vmask[sensor * NUM_LIGHTHOUSES + lh] = 0;
+
+        }
+    }
+
+    return rtn;
+}
+
+
 size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene,
 								  char *vmask, double *meas) {
 	size_t rtn = 0;
@@ -89,6 +115,30 @@ void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_po
 }
 void *GetDriver(const char *name);
 
+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.Pos, &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_with_config(so->ctx, &ctx->hdr.calibration_config, lh, camera, xyz, out);
+    *xij = out[acode];
+}
+
 void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 	SurvivePose obj = *(SurvivePose *)bi;
 	int sensor_idx = j >> 1;
@@ -109,13 +159,116 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 	survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij);
 }
 
+
+
+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*/,
+                                        double max_reproj_error /* = 0.005*/) {
+    double *covx = 0;
+
+    char *vmask = alloca(sizeof(char) * so->sensor_ct);
+    double *meas = alloca(sizeof(double) * so->sensor_ct);
+    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 && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
+            SurviveContext *ctx = so->ctx;
+            SV_INFO("Can't solve for position with just %lu measurements", meas_size);
+            failure_count = 0;
+        }
+        return -1;
+    }
+    failure_count = 0;
+
+    SurvivePose soLocation = so->OutPose;
+    bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]);
+
+    {
+        const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
+        PoserCB driver = (PoserCB)GetDriver(subposer);
+        SurviveContext *ctx = so->ctx;
+        if (driver) {
+            PoserData hdr = pdl->hdr;
+            memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
+            pdl->hdr.pt = hdr.pt;
+            pdl->hdr.rawposeproc = sba_set_position;
+
+            sba_set_position_t locations = {};
+            pdl->hdr.userdata = &locations;
+            driver(so, &pdl->hdr);
+            pdl->hdr = hdr;
+
+            if (locations.hasInfo == false) {
+
+                return -1;
+            } else if (locations.hasInfo) {
+                soLocation = locations.poses;
+            }
+        } else {
+            SV_INFO("Not using a seed poser for SBA; results will likely be way off");
+        }
+    }
+
+    double opts[SBA_OPTSSZ] = {};
+    double info[SBA_INFOSZ] = {};
+
+    sba_context_single_sweep ctx = {
+            .hdr = { options, &pdl->hdr, so },
+            .acode = acode,
+            .lh = lh
+    };
+
+    opts[0] = SBA_INIT_MU;
+    opts[1] = SBA_STOP_THRESH;
+    opts[2] = SBA_STOP_THRESH;
+    opts[3] = SBA_STOP_THRESH;
+    opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size();
+    opts[4] = 0.0;
+
+    int status = sba_str_levmar(1, // Number of 3d points
+                                0, // Number of 3d points to fix in spot
+                                so->sensor_ct, vmask,
+                                soLocation.Pos, // Reads as the full pose though
+                                7,				// pnp -- SurvivePose
+                                meas,			// x* -- measurement data
+                                0,				// cov data
+                                1,				// mnp -- 2 points per image
+                                str_metric_function_single_sweep,
+                                0,				// jacobia of metric_func
+                                &ctx,			// user data
+                                max_iterations, // Max iterations
+                                0,				// verbosity
+                                opts,			// options
+                                info);			// info
+
+    if (status > 0) {
+        quatnormalize(soLocation.Rot, soLocation.Rot);
+        PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation);
+
+        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) {
+            SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size);
+            SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
+            cnt = 0;
+        }
+    }
+
+    return info[1] / meas_size * 2;
+}
+
+
 static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so,
 										SurviveSensorActivations *scene, int max_iterations /* = 50*/,
 										double max_reproj_error /* = 0.005*/) {
 	double *covx = 0;
 
-	char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
-	double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
+	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_from_scene(so, pdl, scene, vmask, meas);
 
 	static int failure_count = 500;
@@ -125,8 +278,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
 			SV_INFO("Can't solve for position with just %lu measurements", meas_size);
 			failure_count = 0;
 		}
-		free(vmask);
-		free(meas);
 		return -1;
 	}
 	failure_count = 0;
@@ -150,8 +301,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
 			pdl->hdr = hdr;
 
 			if (locations.hasInfo == false) {
-				free(vmask);
-				free(meas);
 
 				return -1;
 			} else if (locations.hasInfo) {
@@ -206,9 +355,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
 		}
 	}
 
-	free(vmask);
-	free(meas);
-
 	return info[1] / meas_size * 2;
 }
 
@@ -216,8 +362,8 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 					  int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
 	double *covx = 0;
 
-	char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
-	double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
+	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);
 
 	SurvivePose camera_params[2] = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose};
@@ -282,9 +428,6 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
 	// Docs say info[0] should be divided by meas; I don't buy it really...
 	// std::cerr << info[0] / meas.size() * 2 << " original reproj error" << std::endl;
 
-	free(vmask);
-	free(meas);
-
 	{
 		SurviveContext *ctx = so->ctx;
 		// Docs say info[0] should be divided by meas; I don't buy it really...
@@ -299,11 +442,18 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 	switch (pd->pt) {
 	case POSERDATA_LIGHT: {
 		SurviveSensorActivations *scene = &so->activations;
-
 		PoserDataLight *lightData = (PoserDataLight *)pd;
-
-		survive_calibration_config config = *survive_calibration_default_config();
-		FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5);
+/*
+        static int last_acode = -1;
+        static int last_lh = -1;
+        if(last_lh != lightData->lh || last_acode != lightData->acode) {
+        */
+            survive_calibration_config config = *survive_calibration_default_config();
+            FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5);
+        /*}
+        last_lh = lightData->lh;
+        last_acode = lightData->acode;
+         */
 		return 0;
 	}
 	case POSERDATA_FULL_SCENE: {
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index 42de833..d14bbec 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -3,7 +3,8 @@
 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];
-	return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance);
+	return !(timecode_now - data_timecode[0] > tolerance ||
+			 timecode_now - data_timecode[1] > tolerance);
 }
 
 void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData) {
@@ -15,5 +16,4 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa
 	*data_timecode = lightData->timecode;
 }
 
-// Roughly 31ms at a 48mhz clock rate
-uint32_t SurviveSensorActivations_default_tolerance = 500000;
\ No newline at end of file
+uint32_t SurviveSensorActivations_default_tolerance = (uint32_t) (48000000/*mhz*/ * (16.7 * 2/*ms*/) / 1000);
\ No newline at end of file