diff options
author | Justin Berger <j.david.berger@gmail.com> | 2018-03-25 21:22:01 -0600 |
---|---|---|
committer | Justin Berger <j.david.berger@gmail.com> | 2018-03-25 21:29:41 -0600 |
commit | 801e17d2c52c21adad5eff63265c1aaea2255b1b (patch) | |
tree | 898b86edd0a188d032af416facff49491849b525 /src/poser_epnp.c | |
parent | 287fc6886057e1773d572b2058222b38fd11122f (diff) | |
download | libsurvive-801e17d2c52c21adad5eff63265c1aaea2255b1b.tar.gz libsurvive-801e17d2c52c21adad5eff63265c1aaea2255b1b.tar.bz2 |
Added additional parameters to EPNP and SBA, made it so that degenerate poses didn't occur as easily in EPNP
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 401ea2a..ea1e735 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); |