aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-03-16 10:39:26 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-03-16 10:39:26 -0600
commite5cc00096e7b0700c75160c62f598a8af412c082 (patch)
treeea341f6bf2af0609d888c5910a15e323cd24be73
parentfc575131ccf05c605ba567b95851976fad7c5444 (diff)
downloadlibsurvive-e5cc00096e7b0700c75160c62f598a8af412c082.tar.gz
libsurvive-e5cc00096e7b0700c75160c62f598a8af412c082.tar.bz2
Refactored how the persistence (now called sensor_activations) worked
-rw-r--r--.gitignore8
-rw-r--r--Makefile6
-rw-r--r--include/libsurvive/poser.h3
-rw-r--r--include/libsurvive/survive.h34
-rw-r--r--include/libsurvive/survive_types.h2
-rw-r--r--src/persistent_scene.c36
-rw-r--r--src/persistent_scene.h32
-rw-r--r--src/poser_epnp.c32
-rw-r--r--src/survive_process.c37
-rw-r--r--src/survive_sensor_activations.c19
10 files changed, 97 insertions, 112 deletions
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 <stdint.h>
-#include "survive_types.h"
#include "poser.h"
+#include "survive_types.h"
+#include <stdbool.h>
+#include <stdint.h>
#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 <stdlib.h>
-#include <string.h>
-#include <survive.h>
-
-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 <survive.h>
-
-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 <survive.h>
+
+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