From be3886df1e118e2cfaf586de67f1a0584c631439 Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Thu, 5 Apr 2018 02:06:55 +0200 Subject: remove forced-source license notice --- LICENSE | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/LICENSE b/LICENSE index b706382..d618b98 100644 --- a/LICENSE +++ b/LICENSE @@ -1,16 +1,10 @@ -NOTICE: Portions of this project are tightly based off of OSVR-Vive-Libre - which is under a forced-source license. I have rewritten as much as I can - to avoid that, however, I may have made some mistakes. Magic numbers - have not been modified as they cannot and (thankfully) are exempt from - copyright law. - NOTICE: Based off of https://github.com/collabora/OSVR-Vive-Libre Originally Copyright 2016 Philipp Zabel Originally Copyright 2016 Lubosz Sarnecki Originally Copyright (C) 2013 Fredrik Hultin Originally Copyright (C) 2013 Jakob Bornecrantz -All other code is licensed under the MIT/x11 License. You may re-license the code under the GPL or LGPL licenses. +All code is licensed under the MIT/x11 License. You may re-license the code under the GPL or LGPL licenses. MIT License -- cgit v1.2.3 From b8447455fdce570e22094e2a71f2fa755724e95b Mon Sep 17 00:00:00 2001 From: cnlohr Date: Wed, 4 Apr 2018 21:25:29 -0400 Subject: Allow calibration with one lighthouse --- src/poser_epnp.c | 2 +- src/survive_cal.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/poser_epnp.c b/src/poser_epnp.c index eaa1659..7e922e7 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -66,7 +66,7 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) { SurvivePose additionalTx = {0}; - for (int lh = 0; lh < 2; lh++) { + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { epnp pnp = {.fu = 1, .fv = 1}; epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); diff --git a/src/survive_cal.c b/src/survive_cal.c index 3015b68..4d26bfb 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -81,7 +81,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) config_set_lighthouse(ctx->lh_config,b,id); lighthouses_completed++; - if (lighthouses_completed >= NUM_LIGHTHOUSES) { + if (lighthouses_completed >= ctx->activeLighthouses) { config_save(ctx, survive_configs(ctx, "configfile", SC_GET, "config.json")); } } @@ -225,7 +225,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int ootx_pump_bit( &cd->ootx_decoders[lhid], dbit ); } int i; - for( i = 0; i < NUM_LIGHTHOUSES; i++ ) + for( i = 0; i < ctx->activeLighthouses; i++ ) if( ctx->bsd[i].OOTXSet == 0 ) break; if( i == ctx->activeLighthouses ) cd->stage = 2; //TODO: Make this configuratble to allow single lighthouse. } -- cgit v1.2.3 From a2514e88dc6945a1cc817c09d251378bd924488e Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 21:06:28 -0600 Subject: Added a filter to trails --- tools/viz/survive_viewer.js | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index c8a7b23..c1d613d 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -7,6 +7,7 @@ var canvas; var oldDrawTime = 0; var timecode = {}; var oldPoseTime = 0, poseCnt = 0; +var oldPose = [0, 0, 0]; var scene, camera, renderer, floor; $(function() { $("#toggleBtn").click(function() { $("#cam").toggle(); }); }); @@ -242,14 +243,20 @@ var survive_log_handlers = { objs[obj.tracker].quaternion.set(obj.quat[1], obj.quat[2], obj.quat[3], obj.quat[0]); objs[obj.tracker].verticesNeedUpdate = true; - if (trails) { + var d = 0; + for(var i = 0; i < 3;i++) { + d += Math.pow(obj.position[i] - oldPose[i], 2.); + } + if (trails && Math.sqrt( d ) > .01) { trails.geometry.vertices.push(trails.geometry.vertices.shift()); // shift the array trails.geometry.vertices[MAX_LINE_POINTS - 1] = new THREE.Vector3(obj.position[0], obj.position[1], obj.position[2]); trails.geometry.verticesNeedUpdate = true; + oldPose = obj.position; } - } + + } }, "CONFIG" : function(v, tracker) { var configStr = v.slice(3).join(' '); -- cgit v1.2.3 From 48f791f98d9eda5948de4ef6930d0530b7ca633e Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 21:07:02 -0600 Subject: Added kalman to imu --- include/libsurvive/survive_imu.h | 5 ++++ src/poser_sba.c | 27 ++++++++++++++------ src/survive_imu.c | 49 ++++++++++++++++++++++++++++++++++++ tools/showreproject/showreproject.cc | 4 +-- 4 files changed, 76 insertions(+), 9 deletions(-) diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 124ad7e..8a86425 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -22,12 +22,17 @@ typedef struct { SurvivePose lastGT; uint32_t lastGTTime; + FLT P[7]; // estimate variance + float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki } SurviveIMUTracker; void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose); + void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data); +void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker, + SurvivePose *pose, const FLT *variance); #ifdef __cplusplus }; diff --git a/src/poser_sba.c b/src/poser_sba.c index f0d5645..d106d6c 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -186,7 +186,8 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada } static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, - int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { + int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/, + SurvivePose *out) { double *covx = 0; SurviveObject *so = d->so; @@ -279,10 +280,13 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive // if (distance > 1.) // status = -1; } + + double rtn = -1; if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); - PoserData_poser_pose_func(&pdl->hdr, so, &soLocation); + *out = soLocation; + rtn = info[1] / meas_size * 2; } { @@ -290,13 +294,13 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive // Docs say info[0] should be divided by meas; I don't buy it really... static int cnt = 0; if (cnt++ > 1000 || meas_size < d->required_meas || (info[1] / meas_size * 2) > d->max_error) { - SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); - SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); + // SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); + // SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); cnt = 0; } } - return status; // info[1] / meas_size * 2; + return rtn; } // Optimizes for LH position assuming object is posed at 0 @@ -421,11 +425,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { return 0; SurviveSensorActivations *scene = &so->activations; PoserDataLight *lightData = (PoserDataLight *)pd; + SurvivePose estimate; // only process sweeps FLT error = -1; if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { - error = run_sba_find_3d_structure(d, lightData, scene, 100, .5); + error = run_sba_find_3d_structure(d, lightData, scene, 100, .5, &estimate); d->last_lh = lightData->lh; d->last_acode = lightData->acode; @@ -436,8 +441,16 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->failures_to_reset_cntr--; } else { if (d->useIMU) { - survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); + FLT var_meters = 0.5; + FLT var_quat = error + .05; + FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, + error * var_quat, error * var_quat, error * var_quat}; + + survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); + estimate = d->tracker.pose; } + + PoserData_poser_pose_func(&lightData->hdr, so, &estimate); if (d->successes_to_reset_cntr > 0) d->successes_to_reset_cntr--; } diff --git a/src/survive_imu.c b/src/survive_imu.c index e49da3e..205e2c2 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -183,6 +183,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker const FLT up[3] = {0, 0, 1}; quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir); + return; } @@ -216,5 +217,53 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker scale3d(tracker->pose.Pos, next, 1); } + FLT var_meters = .000001; + FLT var_quat = .05; + const FLT Q[7] = {var_meters, var_meters, var_meters, var_quat, var_quat, var_quat, var_quat}; + + // Note that this implementation is somewhat truncated. Instead of modeling velocity and velocities + // covariance with position explicitly, we just square the variance for the position indexes. This + // gives more or less the same calculation without having to do matrix multiplication. + for (int i = 0; i < 3; i++) + tracker->P[i] = tracker->P[i] * tracker->P[i] + Q[i]; + for (int i = 3; i < 7; i++) + tracker->P[i] += Q[i]; + tracker->last_data = *data; } + +void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker, + SurvivePose *pose, const FLT *R) { + // Kalman filter assuming: + // F -> Identity + // H -> Identity + // Q / R / P -> Diagonal matrices; just treat them as such. This assumption might need some checking but it + // makes the # of calculations needed much smaller so we may be willing to tolerate some approximation here + + FLT *xhat = &tracker->pose.Pos[0]; + FLT *zk = &pose->Pos[0]; + + FLT yk[7]; + for (int i = 0; i < 7; i++) + yk[i] = zk[i] - xhat[i]; + + FLT sk[7]; + for (int i = 0; i < 7; i++) + sk[i] = R[i] + tracker->P[i]; + + FLT K[7]; + for (int i = 0; i < 7; i++) + K[i] = tracker->P[i] / sk[i]; + + for (int i = 0; i < 7; i++) + xhat[i] += K[i] * yk[i]; + for (int i = 0; i < 7; i++) + tracker->P[i] *= (1. - K[i]); + + FLT time_diff = tick_difference(timecode, tracker->lastGTTime) / (FLT)so->timebase_hz; + for (int i = 0; i < 3; i++) + tracker->current_velocity[i] = 0.5 * (tracker->pose.Pos[i] - tracker->lastGT.Pos[i]) / time_diff; + + tracker->lastGTTime = timecode; + tracker->lastGT = tracker->pose; +} \ No newline at end of file diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 8cde992..98dd5f0 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -134,7 +134,7 @@ void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, SurvivePose lastPose = {}; -void raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { +void raw_pose_process(SurviveObject *so, uint32_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, pose); auto d = dist3d(lastPose.Pos, pose->Pos); // std::cerr << d << std::endl; @@ -151,7 +151,7 @@ void lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *po SurviveContext *create(int argc, char **argv) { auto ctx = survive_init(argc, argv); - survive_install_raw_pose_fn(ctx, raw_pose_process); + survive_install_pose_fn(ctx, raw_pose_process); survive_install_lighthouse_pose_fn(ctx, lighthouse_process); survive_install_light_fn(ctx, light_process); -- cgit v1.2.3 From d83a2cd790bc390f7485b3e8b63166ee29151050 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 22:10:41 -0600 Subject: Added imu tracker to charlesrefine --- src/poser_charlesrefine.c | 31 +++++++++++++++++-------------- src/survive_imu.c | 3 +-- 2 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 388ba77..52a5f54 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -6,10 +6,12 @@ #include "epnp/epnp.h" #include "linmath.h" +#include "survive_cal.h" #include #include #include #include +#include #define MAX_PT_PER_SWEEP 32 @@ -22,6 +24,8 @@ typedef struct { SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; int ptsweep; + + SurviveIMUTracker tracker; } CharlesPoserData; int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { @@ -33,20 +37,10 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { switch (pd->pt) { case POSERDATA_IMU: { // Really should use this... - PoserDataIMU *imuData = (PoserDataIMU *)pd; + PoserDataIMU *imu = (PoserDataIMU *)pd; - // TODO: Actually do Madgwick's algorithm - LinmathQuat applymotion; - const SurvivePose *object_pose = &so->OutPose; - imuData->gyro[0] *= -0.0005; - imuData->gyro[1] *= -0.0005; - imuData->gyro[2] *= 0.0005; - quatfromeuler(applymotion, imuData->gyro); - // printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); - SurvivePose object_pose_out; - quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion); - copy3d(object_pose_out.Pos, object_pose->Pos); - PoserData_poser_pose_func(pd, so, &object_pose_out); + survive_imu_tracker_integrate(so, &dd->tracker, imu); + PoserData_poser_pose_func(pd, so, &dd->tracker.pose); return 0; } @@ -324,7 +318,16 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { so->PoseConfidence = 1.0; } - PoserData_poser_pose_func(pd, so, &object_pose_out); + // PoserData_poser_pose_func(pd, so, &object_pose_out); + FLT var_meters = 0.5; + FLT error = 0.0001; + FLT var_quat = error + .05; + FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, + error * var_quat, error * var_quat, error * var_quat}; + + survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var); + PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + dd->ptsweep = 0; } diff --git a/src/survive_imu.c b/src/survive_imu.c index 205e2c2..36d1aeb 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -175,10 +175,9 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { if (tracker->last_data.timecode == 0) { - + tracker->pose.Rot[0] = 1.; if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; - tracker->pose.Rot[0] = 1.; const FLT up[3] = {0, 0, 1}; quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); -- cgit v1.2.3 From f765f0e85cbd1bd00c7f5a18d6e7a4ada0db5918 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 22:15:41 -0600 Subject: more messages on sba start --- src/poser_sba.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/poser_sba.c b/src/poser_sba.c index d106d6c..fcf4f2e 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -415,6 +415,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second); SV_INFO("\tsba-time-window: %d", d->sensor_time_window); SV_INFO("\tsba-max-error: %f", d->max_error); + SV_INFO("\tsba-successes-to-reset: %d", d->successes_to_reset); SV_INFO("\tsba-use-imu: %d", d->useIMU); } SBAData *d = so->PoserData; -- cgit v1.2.3