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 --- src/poser_epnp.c | 65 ++++++++++++++++++++++++++++++++++++++-- src/poser_sba.c | 11 +++++-- src/survive_process.c | 28 +++++++++-------- src/survive_sensor_activations.c | 11 +++++++ 4 files changed, 96 insertions(+), 19 deletions(-) (limited to 'src') 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]; -- cgit v1.2.3