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 --- src/poser_epnp.c | 67 +++++++++++++++++++++++++++++++++++++------------------- 1 file changed, 44 insertions(+), 23 deletions(-) (limited to 'src/poser_epnp.c') 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)); -- cgit v1.2.3