Added code to cancel out object rotation when calibrating

4 files changed, 96 insertions, 19 deletions

diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index dfe86ff..193e30c 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -56,8 +56,49 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer
 
 	return rtn;
 }
+/*
+static int survive_standardize_calibration(SurviveObject* so,
+										   FLT upvec[3],
+										   SurvivePose* _object2world,
+										   SurvivePose* _lighthouses2world0,
+										   SurvivePose* _lighthouses2world1) {
+	SurvivePose object2world = {};
+	SurvivePose lighthouses2world0 = *_lighthouses2world0;
+	SurvivePose lighthouses2world1 = *_lighthouses2world1;
 
+	const FLT up[3] = {0, 0, 1};
+	quatfrom2vectors(object2world.Rot, so->activations.accel, _object2world->Pos);
+
+	FLT tx[4][4];
+	quattomatrix(tx, object2world.Rot);
+
+	SurvivePose additionalTx = {0};
+
+	SurvivePose lighthouse2world = {};
+	// Lighthouse is now a tx from camera -> object
+	ApplyPoseToPose(lighthouse2world.Pos,  object2world.Pos, lighthouse2object.Pos);
+
+	if(quatmagnitude(additionalTx.Rot) == 0) {
+		SurvivePose desiredPose = lighthouse2world;
+		desiredPose.Pos[0] = 0.;
+
+		quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos);
+	}
+	SurvivePose finalTx = {};
+	ApplyPoseToPose(finalTx.Pos,  additionalTx.Pos, lighthouse2world.Pos);
+
+}
+*/
 static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) {
+	SurvivePose object2world = {};
+	const FLT up[3] = {0, 0, 1};
+
+	quatfrom2vectors(object2world.Rot, so->activations.accel, up);
+
+	FLT tx[4][4];
+	quattomatrix(tx, object2world.Rot);
+
+	SurvivePose additionalTx = {0};
 
 	for (int lh = 0; lh < 2; lh++) {
 		epnp pnp = {.fu = 1, .fv = 1};
@@ -80,11 +121,28 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
 			continue;
 		}
 
-		SurvivePose lighthouse = solve_correspondence(so, &pnp, true);
-		PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &lighthouse);
+		SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true);
+
+		SurvivePose lighthouse2world = {};
+		// Lighthouse is now a tx from camera -> object
+		ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos);
+
+		if (quatmagnitude(additionalTx.Rot) == 0) {
+			SurvivePose desiredPose = lighthouse2world;
+			desiredPose.Pos[0] = 0.;
+
+			quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos);
+		}
+		SurvivePose finalTx = {};
+		ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos);
+
+		PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &finalTx);
 
 		epnp_dtor(&pnp);
 	}
+
+	so->OutPose = object2world;
+
 	return 0;
 }
 
@@ -103,6 +161,8 @@ static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActiv
 }
 
 int PoserEPNP(SurviveObject *so, PoserData *pd) {
+
+	SurviveSensorActivations *scene = &so->activations;
 	switch (pd->pt) {
 	case POSERDATA_IMU: {
 		// Really should use this...
@@ -110,7 +170,6 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 		return 0;
 	}
 	case POSERDATA_LIGHT: {
-		SurviveSensorActivations *scene = &so->activations;
 		PoserDataLight *lightData = (PoserDataLight *)pd;
 
 		SurvivePose posers[2];
diff --git a/src/poser_sba.c b/src/poser_sba.c
index 44af9c1..0dcc38c 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -26,8 +26,11 @@ void metric_function(int j, int i, double *aj, double *xij, void *adata) {
 	sba_context *ctx = (sba_context *)(adata);
 	SurviveObject *so = ctx->so;
 
-	survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj,
-											&so->sensor_locations[i * 3], xij);
+	SurvivePose obj2world = so->OutPose;
+	FLT sensorInWorld[3] = {};
+	ApplyPoseToPoint(sensorInWorld, obj2world.Pos, &so->sensor_locations[i * 3]);
+	survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld,
+											xij);
 }
 
 size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) {
@@ -109,6 +112,7 @@ 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);
 }
 
+// Optimizes for object assuming given LH
 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*/) {
@@ -132,7 +136,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
 	failure_count = 0;
 
 	SurvivePose soLocation = so->OutPose;
-	bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]);
+	bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0;
 
 	{
 		const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
@@ -212,6 +216,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
 	return info[1] / meas_size * 2;
 }
 
+// Optimizes for LH position assuming object is posed at 0
 static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so,
 					  int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) {
 	double *covx = 0;
diff --git a/src/survive_process.c b/src/survive_process.c
index b697f4a..f315884 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -132,19 +132,21 @@ int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len
 }
 void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
-	if( so->PoserFn )
-	{
-		PoserDataIMU imu = {
-			.hdr =
-				{
-					.pt = POSERDATA_IMU,
-				},
-			.datamask = mask,
-			.accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]},
-			.gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]},
-			.mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]},
-			.timecode = timecode,
-		};
+	PoserDataIMU imu = {
+		.hdr =
+			{
+				.pt = POSERDATA_IMU,
+			},
+		.datamask = mask,
+		.accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]},
+		.gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]},
+		.mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]},
+		.timecode = timecode,
+	};
+
+	SurviveSensorActivations_add_imu(&so->activations, &imu);
+
+	if (so->PoserFn) {
 		so->PoserFn( so, (PoserData *)&imu );
 	}
 }
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
index 42de833..dce353c 100644
--- a/src/survive_sensor_activations.c
+++ b/src/survive_sensor_activations.c
@@ -6,6 +6,17 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self,
 	return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance);
 }
 
+void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData) {
+	for (int i = 0; i < 3; i++) {
+		self->accel[i] = .98 * self->accel[i] + .02 * imuData->accel[i];
+	}
+	for (int i = 0; i < 3; i++) {
+		self->gyro[i] = .98 * self->gyro[i] + .02 * imuData->gyro[i];
+	}
+	for (int i = 0; i < 3; i++) {
+		self->mag[i] = .98 * self->mag[i] + .02 * imuData->mag[i];
+	}
+}
 void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData) {
 	int axis = (lightData->acode & 1);
 	uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis];