From 8a0831f2b9e458d8f1976c27f7865166ad8de6a4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 09:11:54 -0600 Subject: Moved pose to linmath to support stronger typing --- src/poser.c | 10 ++++---- src/poser_daveortho.c | 4 ++-- src/poser_epnp.c | 62 +++---------------------------------------------- src/poser_sba.c | 6 ++--- src/survive_reproject.c | 6 ++--- 5 files changed, 16 insertions(+), 72 deletions(-) (limited to 'src') diff --git a/src/poser.c b/src/poser.c index 1ed63da..3fb4fe8 100644 --- a/src/poser.c +++ b/src/poser.c @@ -38,17 +38,17 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui // Start by just moving from whatever arbitrary space into object space. SurvivePose arb2object; - InvertPose(arb2object.Pos, object2arb.Pos); + InvertPose(&arb2object, &object2arb); SurvivePose lighthouse2obj; - ApplyPoseToPose(lighthouse2obj.Pos, arb2object.Pos, lighthouse2arb.Pos); + ApplyPoseToPose(&lighthouse2obj, &arb2object, &lighthouse2arb); // Now find the space with the same origin, but rotated so that gravity is up SurvivePose lighthouse2objUp = {}, object2objUp = {}; quatfrom2vectors(object2objUp.Rot, so->activations.accel, up); // Calculate the pose of the lighthouse in this space - ApplyPoseToPose(lighthouse2objUp.Pos, object2objUp.Pos, lighthouse2obj.Pos); + ApplyPoseToPose(&lighthouse2objUp, &object2objUp, &lighthouse2obj); // Purposefully only set this once. It should only depend on the first (calculated) lighthouse if (quatmagnitude(objUp2world->Rot) == 0) { @@ -61,8 +61,8 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui // Find find the poses that map to the above SurvivePose obj2world, lighthouse2world; - ApplyPoseToPose(obj2world.Pos, objUp2world->Pos, object2objUp.Pos); - ApplyPoseToPose(lighthouse2world.Pos, objUp2world->Pos, lighthouse2objUp.Pos); + ApplyPoseToPose(&obj2world, objUp2world, &object2objUp); + ApplyPoseToPose(&lighthouse2world, objUp2world, &lighthouse2objUp); so->ctx->lighthouseposeproc(so->ctx, lighthouse, &lighthouse2world, &obj2world); } diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c index a7ff691..c922b2e 100644 --- a/src/poser_daveortho.c +++ b/src/poser_daveortho.c @@ -105,7 +105,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) SurvivePose lh2world = so->ctx->bsd[lhid].Pose; SurvivePose obj2world; - ApplyPoseToPose(obj2world.Pos, lh2world.Pos, objpose.Pos); + ApplyPoseToPose(&obj2world, &lh2world, &objpose); PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world); if (0) { @@ -195,7 +195,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd ) */ SurvivePose poseout; - InvertPose(poseout.Pos, objpose.Pos); + InvertPose(&poseout, &objpose); printf("INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2], objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]); diff --git a/src/poser_epnp.c b/src/poser_epnp.c index cf4294f..401ea2a 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -39,13 +39,13 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer cvGEMM(&R, &Tmp, -1, 0, 0, &T, 0); } - FLT tmp[4]; + LinmathQuat tmp; quatfrommatrix33(tmp, r[0]); // Typical camera applications have Z facing forward; the vive is contrarian and has Z going out of the // back of the lighthouse. Think of this as a rotation on the Y axis a full 180 degrees -- the quat for that is // [0 0x 1y 0z] - const FLT rt[4] = {0, 0, 1, 0}; + const LinmathQuat rt = {0, 0, 1, 0}; quatrotateabout(rtn.Rot, tmp, rt); if (!cameraToWorld) { // We have to pre-multiply the rt transform here, which means we have to also offset our position by @@ -56,50 +56,9 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer return rtn; } -/* -static int survive_standardize_calibration(SurviveObject* so, - FLT upvec[3], - SurvivePose* _object2world, - SurvivePose* _lighthouses2world0, - SurvivePose* _lighthouses2world1) { - SurvivePose object2world = {}; - SurvivePose lighthouses2world0 = *_lighthouses2world0; - SurvivePose lighthouses2world1 = *_lighthouses2world1; - - const FLT up[3] = {0, 0, 1}; - quatfrom2vectors(object2world.Rot, so->activations.accel, _object2world->Pos); - - FLT tx[4][4]; - quattomatrix(tx, object2world.Rot); - SurvivePose additionalTx = {0}; - - SurvivePose lighthouse2world = {}; - // Lighthouse is now a tx from camera -> object - ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos); - - if(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); - -} -*/ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) { - SurvivePose object2world = {//.Rot = { 0.7325378, 0.4619398, 0.1913417, 0.4619398} - .Rot = {1.}}; - const FLT up[3] = {0, 0, 1}; - - SurvivePose actual; - // quatfrom2vectors(object2world.Rot, so->activations.accel, up); - // quatfrom2vectors(object2world.Rot, up, so->activations.accel); - SurvivePose additionalTx = {0}; - for (int lh = 0; lh < 2; lh++) { epnp pnp = {.fu = 1, .fv = 1}; epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); @@ -122,22 +81,7 @@ 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 (false && quatmagnitude(additionalTx.Rot) == 0) { - SurvivePose desiredPose = lighthouse2world; - desiredPose.Pos[0] = 0.; - - quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos); - } - SurvivePose finalTx = lighthouse2world; - // ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos); - - PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &finalTx, &object2world); + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0); epnp_dtor(&pnp); } diff --git a/src/poser_sba.c b/src/poser_sba.c index b5962f8..870f541 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -36,7 +36,7 @@ void metric_function(int j, int i, double *aj, double *xij, void *adata) { SurvivePose obj2world = ctx->obj_pose; FLT sensorInWorld[3] = {}; - ApplyPoseToPoint(sensorInWorld, obj2world.Pos, &so->sensor_locations[i * 3]); + ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]); survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, xij); } @@ -134,7 +134,7 @@ void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, voi quatnormalize(obj.Rot, obj.Rot); FLT xyz[3]; - ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]); + ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; @@ -157,7 +157,7 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { quatnormalize(obj.Rot, obj.Rot); FLT xyz[3]; - ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]); + ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; diff --git a/src/survive_reproject.c b/src/survive_reproject.c index 7bfc7d7..eabdb07 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -75,13 +75,13 @@ static FLT gibf(bool useSin, FLT v) { void survive_reproject_from_pose_with_config( const SurviveContext *ctx, const survive_calibration_config *config, int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) { - FLT invq[4]; + LinmathQuat invq; quatgetreciprocal(invq, pose->Rot); - FLT tvec[3]; + LinmathPoint3d tvec; quatrotatevector(tvec, invq, pose->Pos); - FLT t_pt[3]; + LinmathPoint3d t_pt; quatrotatevector(t_pt, invq, pt); for (int i = 0; i < 3; i++) t_pt[i] = t_pt[i] - tvec[i]; -- cgit v1.2.3