From 49bade6bbcb351900f627f066b14a6826f6d7984 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 15:59:08 -0600 Subject: Removed printfs, fixed a segfault --- Makefile | 2 +- data_recorder.c | 3 ++- src/epnp/epnp.c | 32 +----------------------- src/epnp/opencv_shim.c | 2 +- src/poser_epnp.c | 67 +++++++++++++++++++++++++++++++++----------------- test.c | 1 + 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); -- cgit v1.2.3