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 --- include/libsurvive/poser.h | 7 +++-- include/libsurvive/survive.h | 3 +- include/libsurvive/survive_types.h | 3 +- src/poser.c | 59 ++++++++++++++++++++++++++++++++++---- src/poser_charlesslow.c | 3 +- src/poser_epnp.c | 23 +++++++-------- src/poser_sba.c | 29 ++++++++++--------- src/poser_turveytori.c | 24 ++++++++++------ src/survive_process.c | 7 +++-- test.c | 4 +-- 10 files changed, 112 insertions(+), 50 deletions(-) diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 9667f1a..6b5b09d 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -18,7 +18,8 @@ typedef enum PoserType_t } PoserType; typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user); -typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user); +typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose, void *user); typedef struct { @@ -29,7 +30,9 @@ typedef struct } PoserData; void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); -void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); +void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, + /* OUTPARAM */ SurvivePose *objUp2world, SurvivePose *lighthouse_poses, + SurvivePose *object_pose); typedef struct PoserDataIMU { PoserData hdr; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index a3639dc..747d076 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -259,7 +259,8 @@ void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh ); void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); -void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose); +void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, + SurvivePose *obj_pose); int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 5f5dd1f..7253df0 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -53,7 +53,8 @@ typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); typedef void (*raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose); -typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose); +typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose); typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount); diff --git a/src/poser.c b/src/poser.c index ec7865e..e52edd2 100644 --- a/src/poser.c +++ b/src/poser.c @@ -1,7 +1,7 @@ +#include "math.h" +#include #include - -#include "poser.h" -#include "survive_internal.h" +#include void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { if (poser_data->rawposeproc) { @@ -11,10 +11,57 @@ void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uin } } -void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { +void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, + SurvivePose *objUp2world, SurvivePose *lighthouse_pose, SurvivePose *object_pose) { if (poser_data->lighthouseposeproc) { - poser_data->lighthouseposeproc(so, lighthouse, pose, poser_data->userdata); + poser_data->lighthouseposeproc(so, lighthouse, lighthouse_pose, object_pose, poser_data->userdata); } else { - so->ctx->lighthouseposeproc(so->ctx, lighthouse, pose); + const FLT up[3] = {0, 0, 1}; + + // Assume that the space solved for is valid but completely arbitrary. We are going to do a few things: + // a) Using the gyro data, normalize it so that gravity is pushing straight down along Z + // c) Assume the object is at origin + // b) Place the first lighthouse on the X axis by rotating around Z + // + // This calibration setup has the benefit that as long as the user is calibrating on the same flat surface, + // calibration results will be roughly identical between all posers no matter the orientation the object is + // lying + // in. + // + // We might want to go a step further and affix the first lighthouse in a given pose that preserves up so that + // it doesn't matter where on that surface the object is. + + SurvivePose object2arb = *object_pose; + SurvivePose lighthouse2arb = *lighthouse_pose; + + // Start by just moving from whatever arbitrary space into object space. + SurvivePose arb2object; + InvertPose(arb2object.Pos, object2arb.Pos); + + SurvivePose lighthouse2obj; + ApplyPoseToPose(lighthouse2obj.Pos, arb2object.Pos, lighthouse2arb.Pos); + + // 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); + + // Purposefully only set this once. It should only depend on the first (calculated) lighthouse + if (quatmagnitude(objUp2world->Rot) == 0) { + // Find what angle we need to rotate about Z by to get to 90 degrees. + FLT ang = atan2(lighthouse2objUp.Pos[1], lighthouse2objUp.Pos[0]); + FLT euler[3] = {0, 0, M_PI / 2. - ang}; + + quatfromeuler(objUp2world->Rot, euler); + } + + // 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); + + so->ctx->lighthouseposeproc(so->ctx, lighthouse, &lighthouse2world, &obj2world); } } diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c index 3b781f4..c7d9033 100644 --- a/src/poser_charlesslow.c +++ b/src/poser_charlesslow.c @@ -53,6 +53,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) printf( "%f %f %f\n", hmd_points[p*3+0], hmd_points[p*3+1], hmd_points[p*3+2] ); } + SurvivePose additionalTx = {}; int lh, cycle; FLT dz, dy, dx; @@ -174,7 +175,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd ) quatrotateabout(tmp, lighthousePose.Rot, rt); memcpy(lighthousePose.Rot, tmp, sizeof(FLT) * 4); - PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose); + PoserData_lighthouse_pose_func(pd, so, lh, &additionalTx, &lighthousePose, 0); #define ALT_COORDS #ifdef ALT_COORDS 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; } diff --git a/src/poser_sba.c b/src/poser_sba.c index 0dcc38c..fa71dd2 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -20,13 +20,15 @@ typedef struct { survive_calibration_config calibration_config; PoserData *pdfs; SurviveObject *so; + SurvivePose obj_pose; + SurvivePose camera_params[2]; } sba_context; void metric_function(int j, int i, double *aj, double *xij, void *adata) { sba_context *ctx = (sba_context *)(adata); SurviveObject *so = ctx->so; - SurvivePose obj2world = so->OutPose; + SurvivePose obj2world = ctx->obj_pose; FLT sensorInWorld[3] = {}; ApplyPoseToPoint(sensorInWorld, obj2world.Pos, &so->sensor_locations[i * 3]); survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, @@ -74,9 +76,10 @@ size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, return rtn; } -void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user) { - SurvivePose *poses = (SurvivePose *)(user); - poses[lighthouse] = *pose; +void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose, void *user) { + sba_context *ctx = (sba_context *)user; + ctx->camera_params[lighthouse] = *pose; + ctx->obj_pose = *obj_pose; } typedef struct { @@ -225,9 +228,10 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); size_t meas_size = construct_input(so, pdfs, vmask, meas); - SurvivePose camera_params[2] = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}; + sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + .obj_pose = so->OutPose}; - if (true || so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0) { + { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; @@ -237,7 +241,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions pdfs->hdr.pt = hdr.pt; pdfs->hdr.lighthouseposeproc = sba_set_cameras; - pdfs->hdr.userdata = camera_params; + pdfs->hdr.userdata = &sbactx; driver(so, &pdfs->hdr); pdfs->hdr = hdr; } else { @@ -254,8 +258,6 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd double opts[SBA_OPTSSZ] = {}; double info[SBA_INFOSZ] = {}; - sba_context ctx = {options, &pdfs->hdr, so}; - opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; opts[2] = SBA_STOP_THRESH; @@ -267,22 +269,23 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd NUM_LIGHTHOUSES, // Number of cameras -- 2 lighthouses 0, // Number of cameras to not modify vmask, // boolean vis mask - (double *)&camera_params[0], // camera parameters + (double *)&sbactx.camera_params[0], // camera parameters sizeof(SurvivePose) / sizeof(double), // The number of floats that are in a camera param meas, // 2d points for 3d objs covx, // covariance of measurement. Null sets to identity 2, // 2 points per image metric_function, 0, // jacobia of metric_func - &ctx, // user data + &sbactx, // user data max_iterations, // Max iterations 0, // verbosity opts, // options info); // info if (status >= 0) { - PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &camera_params[0]); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &camera_params[1]); + SurvivePose additionalTx = {}; + PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &additionalTx, &sbactx.camera_params[0], &sbactx.obj_pose); + PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &additionalTx, &sbactx.camera_params[1], &sbactx.obj_pose); } // Docs say info[0] should be divided by meas; I don't buy it really... // std::cerr << info[0] / meas.size() * 2 << " original reproj error" << std::endl; diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index db8d638..2d3f802 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1235,7 +1235,7 @@ void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh) } static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, PoserData *pd, - char doLogOutput, const int lh, const int setLhCalibration) { + char doLogOutput, SurvivePose *additionalTx, const int lh, const int setLhCalibration) { ToriData *toriData = so->PoserData; //printf("Solving for Lighthouse\n"); @@ -1443,7 +1443,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob lighthousePose.Pos[1] = refinedEstimateGd.y; lighthousePose.Pos[2] = refinedEstimateGd.z; - PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose); + SurvivePose assumedObj = {}; + FLT negZ[3] = {0, 0, 1}; + quatfrom2vectors(assumedObj.Rot, toriData->down, negZ); + + PoserData_lighthouse_pose_func(pd, so, lh, additionalTx, &lighthousePose, &assumedObj); } @@ -1528,7 +1532,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob return refinedEstimateGd; } -static void QuickPose(SurviveObject *so, PoserData *pd, int lh) { +static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalTx, int lh) { ToriData * td = so->PoserData; @@ -1622,7 +1626,7 @@ static void QuickPose(SurviveObject *so, PoserData *pd, int lh) { // SolveForLighthouse(pos, quat, to, so, 0, lh, 0); //} - SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, lh, 0); + SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, lh, 0); //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); if (so->ctx->rawposeproc) @@ -1693,6 +1697,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) } int axis = l->acode & 0x1; //printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode ); + + SurvivePose additionalTx; if ((td->lastAxis[l->lh] != (l->acode & 0x1)) ) { @@ -1705,7 +1711,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 4 == 0) - QuickPose(so, poserData, 0); + QuickPose(so, poserData, &additionalTx, 0); } if (1 == l->lh && axis) // only once per full cycle... { @@ -1715,7 +1721,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 4 == 0) - QuickPose(so, poserData, 1); + QuickPose(so, poserData, &additionalTx, 1); } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; @@ -1755,7 +1761,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT axis[3]; angleaxisfrom2vect(&angle, axis, td->down, negZ); //angleaxisfrom2vect(&angle, &axis, negZ, td->down); - + SurvivePose additionalTx; { int sensorCount = 0; @@ -1790,7 +1796,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT pos[3], quat[4]; - SolveForLighthouse(pos, quat, to, so, poserData, 0, 0, 1); + SolveForLighthouse(pos, quat, to, so, poserData, 0, &additionalTx, 0, 1); } { int sensorCount = 0; @@ -1826,7 +1832,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT pos[3], quat[4]; - SolveForLighthouse(pos, quat, to, so, poserData, 0, 1, 1); + SolveForLighthouse(pos, quat, to, so, poserData, 0, &additionalTx, 1, 1); } diff --git a/src/survive_process.c b/src/survive_process.c index f315884..a49b632 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -115,9 +115,10 @@ void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, Sur so->FromLHPose[lighthouse] = *pose; } -void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { - if (pose) { - ctx->bsd[lighthouse].Pose = *pose; +void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose) { + if (lighthouse_pose) { + ctx->bsd[lighthouse].Pose = *lighthouse_pose; ctx->bsd[lighthouse].PositionSet = 1; } else { ctx->bsd[lighthouse].PositionSet = 0; diff --git a/test.c b/test.c index ea60a14..8e620bc 100644 --- a/test.c +++ b/test.c @@ -84,8 +84,8 @@ void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t butt } } -void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { - survive_default_lighthouse_pose_process(ctx, lighthouse, pose); +void testprog_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose, obj_pose); printf("Lighthouse: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); -- cgit v1.2.3