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 --- 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 +++++++++++++++++++ 5 files changed, 52 insertions(+), 104 deletions(-) delete mode 100644 src/persistent_scene.c delete mode 100644 src/persistent_scene.h create mode 100644 src/survive_sensor_activations.c (limited to 'src') 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