diff options
author | dpeter99 <dpeter99@gmail.com> | 2018-03-28 17:36:09 +0200 |
---|---|---|
committer | dpeter99 <dpeter99@gmail.com> | 2018-03-28 17:36:09 +0200 |
commit | 6f7759314baf66f69fa7a335a1f17c65e90661f2 (patch) | |
tree | 11a0e499a0ea83a5b7aa645ba05e5a87f6cc5070 /src/poser_epnp.c | |
parent | ef18541ac2e1e97de9f04f132ac7b2b1ba21e515 (diff) | |
parent | 4c373617220aca69a4acb7a32c12457a057f4e48 (diff) | |
download | libsurvive-6f7759314baf66f69fa7a335a1f17c65e90661f2.tar.gz libsurvive-6f7759314baf66f69fa7a335a1f17c65e90661f2.tar.bz2 |
Merge branch 'master' of https://github.com/dpeter99/libsurvive
Diffstat (limited to 'src/poser_epnp.c')
-rw-r--r-- | src/poser_epnp.c | 16 |
1 files changed, 14 insertions, 2 deletions
diff --git a/src/poser_epnp.c b/src/poser_epnp.c index f5fa127..c294c0c 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -27,6 +27,12 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer CvMat R = cvMat(3, 3, CV_64F, r); CvMat T = cvMat(3, 1, CV_64F, rtn.Pos); + + // Super degenerate inputs will project us basically right in the camera. Detect and reject + if (magnitude3d(rtn.Pos) < 0.25) { + return rtn; + } + // Requested output is camera -> world, so invert if (cameraToWorld) { FLT tmp[3]; @@ -81,7 +87,10 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) } SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0); + + if (quatmagnitude(lighthouse2object.Rot) != 0.0) { + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0); + } epnp_dtor(&pnp); } @@ -123,8 +132,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); add_correspondences(so, &pnp, scene, lightData); + static int required_meas = -1; + if (required_meas == -1) + required_meas = survive_configi(so->ctx, "epnp-required-meas", SC_GET, 4); - if (pnp.number_of_correspondences > 4) { + if (pnp.number_of_correspondences > required_meas) { SurvivePose pose = solve_correspondence(so, &pnp, false); |