From e5cc00096e7b0700c75160c62f598a8af412c082 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 16 Mar 2018 10:39:26 -0600 Subject: Refactored how the persistence (now called sensor_activations) worked --- .gitignore | 8 ++++++-- Makefile | 6 ++++-- include/libsurvive/poser.h | 3 +-- include/libsurvive/survive.h | 34 ++++++++++++++++++++++++++++++++-- include/libsurvive/survive_types.h | 2 ++ src/persistent_scene.c | 36 ------------------------------------ src/persistent_scene.h | 32 -------------------------------- src/poser_epnp.c | 32 ++++++++++++++------------------ src/survive_process.c | 37 +++++++++++++++++++------------------ src/survive_sensor_activations.c | 19 +++++++++++++++++++ 10 files changed, 97 insertions(+), 112 deletions(-) delete mode 100644 src/persistent_scene.c delete mode 100644 src/persistent_scene.h create mode 100644 src/survive_sensor_activations.c diff --git a/.gitignore b/.gitignore index 99adf7f..e5f098b 100644 --- a/.gitignore +++ b/.gitignore @@ -19,9 +19,13 @@ winbuild/.vs/libsurvive/v15/.suo winbuild/calibrate.exe winbuild/calibrate.def *~ -*/#*# +*/\#*\# +\#*\# *.so calinfo calibrate_client calibrate -test \ No newline at end of file +test +.idea +*.json +*.csv \ No newline at end of file diff --git a/Makefile b/Makefile index 99d29a8..034fbd0 100644 --- a/Makefile +++ b/Makefile @@ -35,7 +35,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/persistent_scene.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o #If you want to use HIDAPI on Linux. @@ -102,7 +102,9 @@ calibrate_tcc : $(LIBSURVIVE_C) tcc -DRUNTIME_SYMNUM $(CFLAGS) -o $@ $^ $(LDFLAGS) calibrate.c redist/os_generic.c $(DRAWFUNCTIONS) redist/symbol_enumerator.c clean : - rm -rf *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so test_minimal_cv test_epnp test_epnp_ocv calibrate_client redist/*.o redist/*~ + rm -rf *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so test_minimal_cv test_epnp test_epnp_ocv calibrate_client redist/*.o redist/*~ tools/data_server/data_server tools/lighthousefind/lighthousefind tools/lighthousefind_tori/lighthousefind-tori tools/plot_lighthouse/plot_lighthouse tools/process_rawcap/process_to_points redist/jsmntest redist/lintest + + diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index eb0de3b..6c74c52 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -41,8 +41,7 @@ typedef struct uint32_t timecode; //In object-local ticks. } PoserDataIMU; -typedef struct -{ +typedef struct PoserDataLight { PoserData hdr; int sensor_id; int acode; //OOTX Code associated with this sweep. bit 1 indicates vertical(1) or horizontal(0) sweep diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 7a0ed1e..7a9ec64 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -1,14 +1,43 @@ #ifndef _SURVIVE_H #define _SURVIVE_H -#include -#include "survive_types.h" #include "poser.h" +#include "survive_types.h" +#include +#include #ifdef __cplusplus extern "C" { #endif +/** + * This struct encodes what the last effective angles seen on a sensor were, and when they occured. + */ +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 +} SurviveSensorActivations; + +struct PoserDataLight; +/** + * Adds a lightData packet to the table. + */ +void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDataLight *lightData); + +/** + * Returns true iff both angles for the given sensor and lighthouse were seen at most `tolerance` ticks before the given + * `timecode_now`. + */ +bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance, + uint32_t timecode_now, uint32_t sensor_idx, int lh); + +/** + * Default tolerance that gives a somewhat accuate representation of current state. + * + * Don't rely on this to be a given value. + */ +extern uint32_t SurviveSensorActivations_default_tolerance; + //DANGER: This structure may be redefined. Note that it is logically split into 64-bit chunks //for optimization on 32- and 64-bit systems. @@ -70,6 +99,7 @@ struct SurviveObject haptic_func haptic; + SurviveSensorActivations activations; //Debug int tsl; }; diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index ce0baac..3c7dc7f 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -1,6 +1,8 @@ #ifndef _SURVIVE_TYPES_H #define _SURVIVE_TYPES_H +#include "stdint.h" + #ifdef __cplusplus extern "C" { #endif diff --git a/src/persistent_scene.c b/src/persistent_scene.c deleted file mode 100644 index cfc389f..0000000 --- a/src/persistent_scene.c +++ /dev/null @@ -1,36 +0,0 @@ -#include "persistent_scene.h" -#include "linmath.h" -#include -#include -#include - -bool PersistentScene_isStillValid(const PersistentScene *self, uint32_t timecode_now, uint32_t idx, int lh) { - const uint32_t *data_timecode = self->timecode[idx][lh]; - return !(timecode_now - data_timecode[0] > self->tolerance || timecode_now - data_timecode[1] > self->tolerance); -} - -void PersistentScene_add(PersistentScene *self, SurviveObject *so, PoserDataLight *lightData) { - int axis = (lightData->acode & 1); - uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis]; - FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis]; - - *angle = lightData->angle; - *data_timecode = lightData->timecode; -} - -void PersistentScene_ForEachCorrespondence(PersistentScene *self, PersistentScene_ForEachCorrespondence_fn fn, - SurviveObject *so, uint32_t timecode_now, void *user) { - for (int lh = 0; lh < NUM_LIGHTHOUSES; lh++) { - for (size_t i = 0; i < so->sensor_ct; i++) { - if (PersistentScene_isStillValid(self, timecode_now, i, lh)) { - double *pts = self->angles[i][lh]; - fn(so, lh, i, pts, user); - } - } - } -} - -void PersistentScene_ctor(PersistentScene *self) { - memset(self, 0, sizeof(PersistentScene)); - self->tolerance = 1500000; -} diff --git a/src/persistent_scene.h b/src/persistent_scene.h deleted file mode 100644 index 07d9056..0000000 --- a/src/persistent_scene.h +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once -#include "persistent_scene.h" -#include "stdbool.h" - -#ifndef USE_DOUBLE -#define FLT double -#define USE_DOUBLE -#endif - -#include "linmath.h" -#include - -typedef struct { - uint32_t tolerance; - - // If "lengths[...]" < 0, means not a valid piece of sweep information. - FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes (Angles in LH space) - uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; - - PoserDataIMU lastimu; - -} PersistentScene; - -typedef void (*PersistentScene_ForEachCorrespondence_fn)(SurviveObject *so, int lh, int sensor_idx, FLT *angles, - void *); -void PersistentScene_ForEachCorrespondence(PersistentScene *self, PersistentScene_ForEachCorrespondence_fn fn, - SurviveObject *so, uint32_t timecode_now, void *user); - -void PersistentScene_add(PersistentScene *self, SurviveObject *so, PoserDataLight *lightData); - -bool PersistentScene_isStillValid(const PersistentScene *self, uint32_t timecode_now, uint32_t idx, int lh); -void PersistentScene_ctor(PersistentScene *self); diff --git a/src/poser_epnp.c b/src/poser_epnp.c index e416456..40a2dd0 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -1,4 +1,3 @@ -#include "persistent_scene.h" #ifndef USE_DOUBLE #define FLT double @@ -91,17 +90,18 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) return 0; } -struct add_correspondence_for_lh { - epnp *pnp; - int lh; -}; - -void add_correspondence_for_lh(SurviveObject *so, int lh, int sensor_idx, FLT *angles, void *_user) { - struct add_correspondence_for_lh *user = (struct add_correspondence_for_lh *)_user; - if (user->lh == lh) - epnp_add_correspondence(user->pnp, so->sensor_locations[sensor_idx * 3 + 0], - so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2], - tan(angles[0]), tan(angles[1])); +static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActivations *scene, + const PoserDataLight *lightData) { + int lh = lightData->lh; + for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) { + if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, lightData->timecode, + sensor_idx, lh)) { + double *angles = scene->angles[sensor_idx][lh]; + epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0], + so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2], + tan(angles[0]), tan(angles[1])); + } + } } int PoserEPNP(SurviveObject *so, PoserData *pd) { @@ -112,19 +112,15 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { return 0; } case POSERDATA_LIGHT: { - static PersistentScene _scene = {.tolerance = 1500000}; - PersistentScene *scene = &_scene; + SurviveSensorActivations *scene = &so->activations; PoserDataLight *lightData = (PoserDataLight *)pd; - PersistentScene_add(scene, so, lightData); - int lh = lightData->lh; if (so->ctx->bsd[lh].PositionSet) { epnp pnp = {.fu = 1, .fv = 1}; epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); - struct add_correspondence_for_lh user = {.lh = lh, .pnp = &pnp}; - PersistentScene_ForEachCorrespondence(scene, add_correspondence_for_lh, so, lightData->timecode, &user); + add_correspondences(so, &pnp, scene, lightData); if (pnp.number_of_correspondences > 4) { diff --git a/src/survive_process.c b/src/survive_process.c index fe6dc45..c9225e4 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -45,25 +45,26 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh) { SurviveContext * ctx = so->ctx; - if( ctx->calptr ) - { - survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh ); - } - if( so->PoserFn ) - { - PoserDataLight l = { - .hdr = - { - .pt = POSERDATA_LIGHT, - }, - .sensor_id = sensor_id, - .acode = acode, - .timecode = timecode, - .length = length, - .angle = angle, - .lh = lh, - }; + PoserDataLight l = { + .hdr = + { + .pt = POSERDATA_LIGHT, + }, + .sensor_id = sensor_id, + .acode = acode, + .timecode = timecode, + .length = length, + .angle = angle, + .lh = lh, + }; + + SurviveSensorActivations_add(&so->activations, &l); + + if (ctx->calptr) { + survive_cal_angle(so, sensor_id, acode, timecode, length, angle, lh); + } + if (so->PoserFn) { so->PoserFn( so, (PoserData *)&l ); } } diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c new file mode 100644 index 0000000..6daded4 --- /dev/null +++ b/src/survive_sensor_activations.c @@ -0,0 +1,19 @@ +#include + +bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance, + uint32_t timecode_now, uint32_t idx, int lh) { + const uint32_t *data_timecode = self->timecode[idx][lh]; + return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance); +} + +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]; + FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis]; + + *angle = lightData->angle; + *data_timecode = lightData->timecode; +} + +// Roughly 31ms at a 48mhz clock rate +uint32_t SurviveSensorActivations_default_tolerance = 1500000; \ No newline at end of file -- cgit v1.2.3