From 8ec5d7facc76e05859444335adbdbfd2bbbad852 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 18 Mar 2018 23:15:54 -0600 Subject: Added code to cancel out object rotation when calibrating --- include/libsurvive/poser.h | 3 +- include/libsurvive/survive.h | 7 +++++ src/poser_epnp.c | 65 ++++++++++++++++++++++++++++++++++++++-- src/poser_sba.c | 11 +++++-- src/survive_process.c | 28 +++++++++-------- src/survive_sensor_activations.c | 11 +++++++ tools/viz/survive_viewer.js | 18 +++++++---- 7 files changed, 116 insertions(+), 27 deletions(-) diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 6c74c52..9667f1a 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -31,8 +31,7 @@ typedef struct 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); -typedef struct -{ +typedef struct PoserDataIMU { PoserData hdr; uint8_t datamask; //0 = accel present, 1 = gyro present, 2 = mag present. FLT accel[3]; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 180d83c..a3639dc 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -16,14 +16,21 @@ extern "C" { typedef struct { FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes (Angles in LH space) uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks + + FLT accel[3]; + FLT gyro[3]; + FLT mag[3]; } SurviveSensorActivations; struct PoserDataLight; +struct PoserDataIMU; /** * Adds a lightData packet to the table. */ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData); +void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData); + /** * Returns true iff both angles for the given sensor and lighthouse were seen at most `tolerance` ticks before the given * `timecode_now`. diff --git a/src/poser_epnp.c b/src/poser_epnp.c index dfe86ff..193e30c 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -56,8 +56,49 @@ 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 = {}; + const FLT up[3] = {0, 0, 1}; + + quatfrom2vectors(object2world.Rot, so->activations.accel, up); + + FLT tx[4][4]; + quattomatrix(tx, object2world.Rot); + + SurvivePose additionalTx = {0}; for (int lh = 0; lh < 2; lh++) { epnp pnp = {.fu = 1, .fv = 1}; @@ -80,11 +121,28 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) continue; } - SurvivePose lighthouse = solve_correspondence(so, &pnp, true); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &lighthouse); + SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true); + + 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); + + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &finalTx); epnp_dtor(&pnp); } + + so->OutPose = object2world; + return 0; } @@ -103,6 +161,8 @@ static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActiv } int PoserEPNP(SurviveObject *so, PoserData *pd) { + + SurviveSensorActivations *scene = &so->activations; switch (pd->pt) { case POSERDATA_IMU: { // Really should use this... @@ -110,7 +170,6 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { return 0; } case POSERDATA_LIGHT: { - SurviveSensorActivations *scene = &so->activations; PoserDataLight *lightData = (PoserDataLight *)pd; SurvivePose posers[2]; diff --git a/src/poser_sba.c b/src/poser_sba.c index 44af9c1..0dcc38c 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -26,8 +26,11 @@ void metric_function(int j, int i, double *aj, double *xij, void *adata) { sba_context *ctx = (sba_context *)(adata); SurviveObject *so = ctx->so; - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, - &so->sensor_locations[i * 3], xij); + SurvivePose obj2world = so->OutPose; + 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, + xij); } size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { @@ -109,6 +112,7 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); } +// Optimizes for object assuming given LH static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, SurviveSensorActivations *scene, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { @@ -132,7 +136,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose failure_count = 0; SurvivePose soLocation = so->OutPose; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]); + bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); @@ -212,6 +216,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose return info[1] / meas_size * 2; } +// Optimizes for LH position assuming object is posed at 0 static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; diff --git a/src/survive_process.c b/src/survive_process.c index b697f4a..f315884 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -132,19 +132,21 @@ int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len } void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id ) { - if( so->PoserFn ) - { - PoserDataIMU imu = { - .hdr = - { - .pt = POSERDATA_IMU, - }, - .datamask = mask, - .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]}, - .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]}, - .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]}, - .timecode = timecode, - }; + PoserDataIMU imu = { + .hdr = + { + .pt = POSERDATA_IMU, + }, + .datamask = mask, + .accel = {accelgyromag[0], accelgyromag[1], accelgyromag[2]}, + .gyro = {accelgyromag[3], accelgyromag[4], accelgyromag[5]}, + .mag = {accelgyromag[6], accelgyromag[7], accelgyromag[8]}, + .timecode = timecode, + }; + + SurviveSensorActivations_add_imu(&so->activations, &imu); + + if (so->PoserFn) { so->PoserFn( so, (PoserData *)&imu ); } } diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index 42de833..dce353c 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -6,6 +6,17 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance); } +void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct PoserDataIMU *imuData) { + for (int i = 0; i < 3; i++) { + self->accel[i] = .98 * self->accel[i] + .02 * imuData->accel[i]; + } + for (int i = 0; i < 3; i++) { + self->gyro[i] = .98 * self->gyro[i] + .02 * imuData->gyro[i]; + } + for (int i = 0; i < 3; i++) { + self->mag[i] = .98 * self->mag[i] + .02 * imuData->mag[i]; + } +} void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData) { int axis = (lightData->acode & 1); uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis]; diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 3aa2cf7..ab6a200 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -280,18 +280,21 @@ $(function() { create_object(obj); } else if (obj.type === "imu") { if (objs[obj.tracker]) { - if (!downAxes[obj.tracker]) { + if (!downAxes[obj.tracker] && objs[obj.tracker]) { downAxes[obj.tracker] = new THREE.Geometry(); - downAxes[obj.tracker].vertices.push( - new THREE.Vector3(0, 0, 0), - new THREE.Vector3(obj.accelgyro[0], obj.accelgyro[1], obj.accelgyro[2])); + downAxes[obj.tracker].vertices.push(new THREE.Vector3(0, 0, 0), new THREE.Vector3(0, 0, 0)); var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff})); - objs[obj.tracker].add(line); - } else { + scene.add(line); + } + + if (objs[obj.tracker].position) { var q = obj.accelgyro; + + downAxes[obj.tracker].vertices[0] = objs[obj.tracker].position; downAxes[obj.tracker].vertices[1].fromArray(q); + downAxes[obj.tracker].vertices[1].add(objs[obj.tracker].position); downAxes[obj.tracker].verticesNeedUpdate = true; } } @@ -382,6 +385,9 @@ init() { var skyBoxMaterial = new THREE.MeshBasicMaterial({color : 0x888888, side : THREE.BackSide}); var skyBox = new THREE.Mesh(skyBoxGeometry, skyBoxMaterial); scene.add(skyBox); + + var axes = new THREE.AxesHelper(5); + scene.add(axes); } function animate() { -- cgit v1.2.3