aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-04-05 06:56:57 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-04-05 06:56:57 -0600
commit4b045e61ed6f3812cbf273ae279bfd7bc1ce029c (patch)
treee16ab505bfccc638f0ff288a8cca8c226a597767
parentd6d310fdd13c11382f37faca6a0c20b361ae9c40 (diff)
parentf765f0e85cbd1bd00c7f5a18d6e7a4ada0db5918 (diff)
downloadlibsurvive-4b045e61ed6f3812cbf273ae279bfd7bc1ce029c.tar.gz
libsurvive-4b045e61ed6f3812cbf273ae279bfd7bc1ce029c.tar.bz2
Merge branch 'master' into simple_api
-rw-r--r--LICENSE8
-rw-r--r--include/libsurvive/survive_imu.h5
-rw-r--r--src/poser_charlesrefine.c31
-rw-r--r--src/poser_epnp.c2
-rw-r--r--src/poser_sba.c28
-rwxr-xr-xsrc/survive_cal.c4
-rw-r--r--src/survive_imu.c52
-rw-r--r--tools/showreproject/showreproject.cc4
-rw-r--r--tools/viz/survive_viewer.js11
9 files changed, 108 insertions, 37 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 <lubosz.sarnecki@collabora.co.uk>
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
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_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 <math.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
+#include <survive_imu.h>
#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/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/poser_sba.c b/src/poser_sba.c
index f0d5645..fcf4f2e 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
@@ -411,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;
@@ -421,11 +426,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 +442,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_cal.c b/src/survive_cal.c
index 36e0a31..faf2ac7 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"));
}
}
@@ -224,7 +224,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.
}
diff --git a/src/survive_imu.c b/src/survive_imu.c
index e49da3e..36d1aeb 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -175,14 +175,14 @@ 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);
tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir);
+
return;
}
@@ -216,5 +216,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);
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(' ');