From 8d269a56805d4abd3f912b789cbd9b3b3f8925c0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 20 Mar 2018 15:52:23 -0600 Subject: Added functionality to standardize where found lighthouses go --- src/poser_epnp.c | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) (limited to 'src/poser_epnp.c') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 193e30c..cf4294f 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -90,13 +90,13 @@ static int survive_standardize_calibration(SurviveObject* so, } */ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) { - SurvivePose object2world = {}; + SurvivePose object2world = {//.Rot = { 0.7325378, 0.4619398, 0.1913417, 0.4619398} + .Rot = {1.}}; const FLT up[3] = {0, 0, 1}; - quatfrom2vectors(object2world.Rot, so->activations.accel, up); - - FLT tx[4][4]; - quattomatrix(tx, object2world.Rot); + SurvivePose actual; + // quatfrom2vectors(object2world.Rot, so->activations.accel, up); + // quatfrom2vectors(object2world.Rot, up, so->activations.accel); SurvivePose additionalTx = {0}; @@ -122,27 +122,26 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) } SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true); - + FLT euler[3]; + quattoeuler(euler, lighthouse2object.Rot); SurvivePose lighthouse2world = {}; // Lighthouse is now a tx from camera -> object ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos); - if (quatmagnitude(additionalTx.Rot) == 0) { + if (false && quatmagnitude(additionalTx.Rot) == 0) { SurvivePose desiredPose = lighthouse2world; desiredPose.Pos[0] = 0.; quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos); } - SurvivePose finalTx = {}; - ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos); + SurvivePose finalTx = lighthouse2world; + // ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &finalTx); + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &finalTx, &object2world); epnp_dtor(&pnp); } - so->OutPose = object2world; - return 0; } -- cgit v1.2.3