Removed printfs, fixed a segfault

6 files changed, 50 insertions, 57 deletions

diff --git a/Makefile b/Makefile
index b619970..2eb4859 100644
--- a/Makefile
+++ b/Makefile
@@ -36,7 +36,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 src/survive_reproject.o src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/epnp/opencv_shim.c
+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 src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/epnp/opencv_shim.c src/persistent_scene.o
 
 
 #If you want to use HIDAPI on Linux.
diff --git a/data_recorder.c b/data_recorder.c
index fbc9bdc..e835c73 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -67,7 +67,7 @@ void my_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose
 	write_to_output("LH_POSE %d %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, pose->Pos[0], pose->Pos[1],
 					pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]);
 }
-void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
+void my_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
 	survive_default_raw_pose_process(so, lighthouse, pose);
 	write_to_output("POSE %s %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", so->codename, pose->Pos[0], pose->Pos[1],
 					pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]);
@@ -182,6 +182,7 @@ void *SurviveThread(void *junk) {
 	survive_install_light_fn(ctx, my_light_process);
 	survive_install_imu_fn(ctx, my_imu_process);
 	survive_install_lighthouse_pose_fn(ctx, my_lighthouse_process);
+	survive_install_raw_pose_fn(ctx, my_raw_pose_process);
 	survive_cal_install(ctx);
 	if (!ctx) {
 		fprintf(stderr, "Fatal. Could not start\n");
diff --git a/src/epnp/epnp.c b/src/epnp/epnp.c
index 48b7d8a..4b888aa 100644
--- a/src/epnp/epnp.c
+++ b/src/epnp/epnp.c
@@ -79,12 +79,7 @@ void epnp_compute_rho(epnp *self, double *rho) {
 
 	CvMat cws = cvMat(4, 3, CV_64F, self->cws);
 	CvMat ccs = cvMat(4, 3, CV_64F, self->ccs);
-	printf("Rho:\n");
-	print_mat(&cws);
-	print_mat(&ccs);
-
 	CvMat pws = cvMat(self->maximum_number_of_correspondences, 3, CV_64F, self->pws);
-	print_mat(&pws);
 }
 
 void epnp_set_internal_parameters(epnp *self, double uc, double vc, double fu, double fv) {
@@ -149,13 +144,10 @@ void epnp_choose_control_points(epnp *self) {
 			PW0->data.db[3 * i + j] = self->pws[3 * i + j] - self->cws[0][j];
 
 	cvMulTransposed(PW0, &PW0tPW0, 1, 0, 1);
-	printf("PW0tPW0\n");
-	print_mat(&PW0tPW0);
 
 	cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
 	assert(UCt.data.db == uct);
-	print_mat(&DC);
-	print_mat(&UCt);
+
 	cvReleaseMat(&PW0);
 
 	for (int i = 1; i < 4; i++) {
@@ -174,19 +166,8 @@ void epnp_compute_barycentric_coordinates(epnp *self) {
 		for (int j = 1; j < 4; j++)
 			cc[3 * i + j - 1] = self->cws[j][i] - self->cws[0][i];
 
-	printf("CC_inv\n");
-	print_mat(&CC);
 	cvInvert(&CC, &CC_inv, 1);
 
-	/*    double gt[] = {
-	  -0.39443, 0.639333, 1.16496 ,
-	  -0.550589, -1.45206, 0.610476 ,
-	  3.54726, -0.682609, 1.57564
-	};
-	for(int i = 0;i < 9;i++) CC_inv.data.db[i] = gt[i];
-	*/
-	print_mat(&CC_inv);
-
 	double *ci = cc_inv;
 	for (int i = 0; i < self->number_of_correspondences; i++) {
 		double *pi = self->pws + 3 * i;
@@ -289,10 +270,7 @@ void find_betas_approx_1(const CvMat *L_6x10, const CvMat *Rho, double *betas) {
 		cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6));
 	}
 
-	print_mat(&L_6x4);
 	cvSolve(&L_6x4, Rho, &B4, CV_SVD);
-	print_mat(Rho);
-	print_mat(&B4);
 
 	assert(B4.data.db == b4);
 
@@ -512,8 +490,6 @@ double epnp_compute_pose(epnp *self, double R[3][3], double t[3]) {
 	for (int i = 0; i < self->number_of_correspondences; i++)
 		epnp_fill_M(self, M, 2 * i, self->alphas + 4 * i, self->us[2 * i], self->us[2 * i + 1]);
 
-	printf("M\n");
-	print_mat(M);
 
 	double mtm[12 * 12], d[12], ut[12 * 12];
 	CvMat MtM = cvMat(12, 12, CV_64F, mtm);
@@ -552,9 +528,6 @@ double epnp_compute_pose(epnp *self, double R[3][3], double t[3]) {
 		   for(int i = 0;i < 144;i++) ut[i] = gt[i];*/
 	assert(Ut.data.db == ut);
 
-	print_mat(&Ut);
-	print_mat(&D);
-
 	double l_6x10[6 * 10], rho[6];
 	CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
 	CvMat Rho = cvMat(6, 1, CV_64F, rho);
@@ -570,17 +543,14 @@ double epnp_compute_pose(epnp *self, double R[3][3], double t[3]) {
 	gauss_newton(&L_6x10, &Rho, Betas[1]);
 
 	rep_errors[1] = epnp_compute_R_and_t(self, ut, Betas[1], Rs[1], ts[1]);
-	printf("r1: %f\n", rep_errors[1]);
 
 	find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
 	gauss_newton(&L_6x10, &Rho, Betas[2]);
 	rep_errors[2] = epnp_compute_R_and_t(self, ut, Betas[2], Rs[2], ts[2]);
-	printf("r2: %f\n", rep_errors[2]);
 
 	epnp_find_betas_approx_3(self, &L_6x10, &Rho, Betas[3]);
 	gauss_newton(&L_6x10, &Rho, Betas[3]);
 	rep_errors[3] = epnp_compute_R_and_t(self, ut, Betas[3], Rs[3], ts[3]);
-	printf("r3: %f\n", rep_errors[3]);
 
 	int N = 1;
 	if (rep_errors[2] < rep_errors[1])
diff --git a/src/epnp/opencv_shim.c b/src/epnp/opencv_shim.c
index 4680b1f..d7886d0 100644
--- a/src/epnp/opencv_shim.c
+++ b/src/epnp/opencv_shim.c
@@ -388,7 +388,7 @@ void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr, int flags) {
 	lapack_int ulda = uarr ? uarr->cols : 1;
 	lapack_int plda = varr ? varr->cols : acols;
 
-	double *superb = malloc(MIN(arows, acols));
+	double *superb = malloc(sizeof(double) * MIN(arows, acols));
 	inf = LAPACKE_dgesvd(LAPACK_ROW_MAJOR, jobu, jobvt, arows, acols, aarr->data.db, acols, warr ? warr->data.db : 0,
 						 uarr ? uarr->data.db : 0, ulda, varr ? varr->data.db : 0, plda, superb);
 
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 4c3c8b7..21e61ae 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -1,4 +1,4 @@
-#include "PersistentScene.h"
+#include "persistent_scene.h"
 
 #ifndef USE_DOUBLE
 #define FLT double
@@ -89,6 +89,19 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
 	return 0;
 }
 
+struct add_correspondence_for_lh {
+	epnp *pnp;
+	int lh;
+};
+
+void add_correspondence_for_lh(SurviveObject *so, int lh, int sensor_idx, FLT *angles, void *_user) {
+	struct add_correspondence_for_lh *user = (struct add_correspondence_for_lh *)_user;
+	if (user->lh == lh)
+		epnp_add_correspondence(user->pnp, so->sensor_locations[sensor_idx * 3 + 0],
+								so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2],
+								tan(angles[0]), tan(angles[1]));
+}
+
 int PoserEPNP(SurviveObject *so, PoserData *pd) {
 	switch (pd->pt) {
 	case POSERDATA_IMU: {
@@ -97,28 +110,36 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 		return 0;
 	}
 	case POSERDATA_LIGHT: {
-		/*
-		PersistentScene* scene;
-		   PoserDataLight * lightData = pd;
-
-		   PersistentScene_add(scene, so, lightData);
-
-		   if (so->ctx->bsd[lh].PositionSet) {
-			  auto pose = solve_correspondence(so, cal_objectPoints, cal_imagePoints, false);
-
-			  SurvivePose txPose = {};
-			  quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos);
-			  for (int i = 0; i < 3; i++) {
-				  txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i];
-			  }
-			  quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot);
-
-			  // scene->integratePose(txPose, lightData->timecode);
-			  // txPose = scene->currentPose;
-			  PoserData_poser_raw_pose_func(pd, so, lh, &txPose);
-		  }
-		*/
-		return -1;
+		static PersistentScene _scene = {.tolerance = 1500000};
+		PersistentScene *scene = &_scene;
+		PoserDataLight *lightData = (PoserDataLight *)pd;
+
+		PersistentScene_add(scene, so, lightData);
+
+		int lh = lightData->lh;
+		if (so->ctx->bsd[lh].PositionSet) {
+			epnp pnp = {.fu = 1, .fv = 1};
+			epnp_set_maximum_number_of_correspondences(&pnp, so->nr_locations);
+
+			struct add_correspondence_for_lh user = {.lh = lh, .pnp = &pnp};
+			PersistentScene_ForEachCorrespondence(scene, add_correspondence_for_lh, so, lightData->timecode, &user);
+
+			if (pnp.number_of_correspondences > 4) {
+
+				SurvivePose pose = solve_correspondence(so, &pnp, false);
+
+				SurvivePose txPose = {};
+				quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos);
+				for (int i = 0; i < 3; i++) {
+					txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i];
+				}
+
+				quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot);
+				PoserData_poser_raw_pose_func(pd, so, lh, &txPose);
+			}
+		}
+
+		return 0;
 	}
 	case POSERDATA_FULL_SCENE: {
 		return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd));
diff --git a/test.c b/test.c
index d4b3d6f..aaf980f 100644
--- a/test.c
+++ b/test.c
@@ -145,6 +145,7 @@ int main()
 
 	survive_install_button_fn(ctx, testprog_button_process);
 	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+
 	survive_install_imu_fn(ctx, testprog_imu_process);
 	survive_install_lighthouse_pose_fn(ctx, testprog_lighthouse_process);