From 7d1d930dcb99559eee95fc8a94cc68d12a968353 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 16:08:00 -0600 Subject: Added simple API --- api_example.c | 33 +++++ include/libsurvive/survive.h | 34 +++-- include/libsurvive/survive_api.h | 30 +++++ include/libsurvive/survive_types.h | 19 ++- src/survive_api.c | 167 +++++++++++++++++++++++++ src/survive_cal.c | 1 - src/survive_reproject.c | 2 +- winbuild/libsurvive.sln | 23 ++++ winbuild/libsurvive/libsurvive.vcxproj | 2 + winbuild/libsurvive/libsurvive.vcxproj.filters | 6 + 10 files changed, 289 insertions(+), 28 deletions(-) create mode 100644 api_example.c create mode 100644 include/libsurvive/survive_api.h create mode 100644 src/survive_api.c diff --git a/api_example.c b/api_example.c new file mode 100644 index 0000000..f2d9bd6 --- /dev/null +++ b/api_example.c @@ -0,0 +1,33 @@ +#include +#include +#include +#include + +int main(int argc, char **argv) { + SurviveAsyncContext *actx = survive_asyc_init(argc, argv); + if (actx == 0) // implies -help or similiar + return 0; + + survive_async_start_thread(actx); + + while (survive_async_is_running(actx)) { + OGUSleep(30000); + + SurvivePose pose; + + for (const SurviveAsyncObject* it = survive_async_get_first_tracked(actx); it != 0; it = survive_async_get_next_tracked(actx, it)) { + uint32_t timecode = survive_async_object_get_latest_pose(it, &pose); + printf("%s (%u): %f %f %f %f %f %f %f\n", survive_async_object_name(it), timecode, + pose.Pos[0], pose.Pos[1], pose.Pos[2], pose.Rot[0], pose.Rot[1], pose.Rot[2], pose.Rot[3]); + } + + OGUSleep(30000); + for (const SurviveAsyncObject* it = survive_async_get_next_updated(actx); it != 0; it = survive_async_get_next_updated(actx)) { + printf("%s changed since last checked\n", survive_async_object_name(it)); + } + + } + + survive_async_close(actx); + return 0; +} diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 65343b7..52dcc0d 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -10,19 +10,13 @@ extern "C" { #endif -#ifdef _WIN32 -#define SURVIVE_EXPORT __declspec(dllexport) -#else -#define SURVIVE_EXPORT __attribute__((visibility("default"))) -#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 - uint32_t lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks + survive_timecode timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks + survive_timecode lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // Timecode per axis in ticks FLT accel[3]; FLT gyro[3]; @@ -43,15 +37,15 @@ void SurviveSensorActivations_add_imu(SurviveSensorActivations *self, struct Pos * 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); +bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, survive_timecode tolerance, + survive_timecode 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; +extern survive_timecode 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. @@ -75,7 +69,7 @@ struct SurviveObject { // Pose Information, also "poser" field. FLT PoseConfidence; // 0..1 SurvivePose OutPose; // Final pose? (some day, one can dream!) - uint32_t OutPose_timecode; + survive_timecode OutPose_timecode; SurvivePose FromLHPose[NUM_LIGHTHOUSES]; // Filled out by poser, contains computed position from each lighthouse. void *PoserData; // Initialized to zero, configured by poser, can be anything the poser wants. PoserCB PoserFn; @@ -101,11 +95,11 @@ struct SurviveObject { int8_t oldcode; int8_t sync_set_number; // 0 = master, 1 = slave, -1 = fault. int8_t did_handle_ootx; // If unset, will send lightcap data for sync pulses next time a sensor is hit. - uint32_t last_sync_time[NUM_LIGHTHOUSES]; - uint32_t last_sync_length[NUM_LIGHTHOUSES]; - uint32_t recent_sync_time; + survive_timecode last_sync_time[NUM_LIGHTHOUSES]; + survive_timecode last_sync_length[NUM_LIGHTHOUSES]; + survive_timecode recent_sync_time; - uint32_t last_lighttime; // May be a 24- or 32- bit number depending on what device. + survive_timecode last_lighttime; // May be a 24- or 32- bit number depending on what device. FLT *acc_bias; // size is FLT*3. contains x,y,z FLT *acc_scale; // size is FLT*3. contains x,y,z @@ -305,14 +299,14 @@ SURVIVE_EXPORT int survive_haptic(SurviveObject *so, uint8_t reserved, uint16_t // Call these from your callback if overridden. // Accept higher-level data. SURVIVE_EXPORT void survive_default_light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, - uint32_t timecode, uint32_t length, uint32_t lh); -SURVIVE_EXPORT void survive_default_imu_process(SurviveObject *so, int mode, FLT *accelgyro, uint32_t timecode, int id); -SURVIVE_EXPORT void survive_default_angle_process(SurviveObject *so, int sensor_id, int acode, uint32_t timecode, + survive_timecode timecode, survive_timecode length, uint32_t lh); +SURVIVE_EXPORT void survive_default_imu_process(SurviveObject *so, int mode, FLT *accelgyro, survive_timecode timecode, int id); +SURVIVE_EXPORT void survive_default_angle_process(SurviveObject *so, int sensor_id, int acode, survive_timecode timecode, FLT length, FLT angle, uint32_t lh); SURVIVE_EXPORT void survive_default_button_process(SurviveObject *so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); -SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose); +SURVIVE_EXPORT void survive_default_raw_pose_process(SurviveObject *so, survive_timecode timecode, SurvivePose *pose); SURVIVE_EXPORT void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose, SurvivePose *obj_pose); SURVIVE_EXPORT int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len); diff --git a/include/libsurvive/survive_api.h b/include/libsurvive/survive_api.h new file mode 100644 index 0000000..30e30ac --- /dev/null +++ b/include/libsurvive/survive_api.h @@ -0,0 +1,30 @@ +#include "survive_types.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + + struct SurviveAsyncContext; + typedef struct SurviveAsyncContext SurviveAsyncContext; + + SURVIVE_EXPORT SurviveAsyncContext *survive_asyc_init(int argc, char *const *argv); + SURVIVE_EXPORT void survive_async_close(SurviveAsyncContext* actx); + + SURVIVE_EXPORT void survive_async_start_thread(SurviveAsyncContext* actx); + SURVIVE_EXPORT int survive_async_stop_thread(SurviveAsyncContext* actx); + SURVIVE_EXPORT bool survive_async_is_running(SurviveAsyncContext* actx); + + struct SurviveAsyncObject; + typedef struct SurviveAsyncObject SurviveAsyncObject; + + SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_first_tracked(SurviveAsyncContext* actx); + SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_next_tracked(SurviveAsyncContext* actx, const SurviveAsyncObject* curr); + SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_next_updated(SurviveAsyncContext* actx); + + SURVIVE_EXPORT survive_timecode survive_async_object_get_latest_pose(const SurviveAsyncObject* sao, SurvivePose* pose); + SURVIVE_EXPORT const char* survive_async_object_name(SurviveAsyncObject* sao); + +#ifdef __cplusplus +} +#endif diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 367c391..b9d145f 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -4,6 +4,12 @@ #include "linmath.h" #include "stdint.h" +#ifdef _WIN32 +#define SURVIVE_EXPORT __declspec(dllexport) +#else +#define SURVIVE_EXPORT __attribute__((visibility("default"))) +#endif + #ifdef __cplusplus extern "C" { #endif @@ -38,6 +44,8 @@ typedef LinmathPose SurvivePose; #define BUTTON_EVENT_BUTTON_UP 2 #define BUTTON_EVENT_AXIS_CHANGED 3 +typedef uint32_t survive_timecode; + typedef struct SurviveObject SurviveObject; typedef struct SurviveContext SurviveContext; typedef struct BaseStationData BaseStationData; @@ -45,13 +53,12 @@ typedef struct SurviveCalData SurviveCalData; //XXX Warning: This may be remov typedef int (*htc_config_func)(SurviveObject *so, char *ct0conf, int len); typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault ); -typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse); -typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id ); -typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh); +typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, survive_timecode timecode, survive_timecode length, uint32_t lighthouse); +typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, survive_timecode timecode, int id ); +typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, survive_timecode timecode, FLT length, FLT angle, uint32_t lh); typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val); -typedef void (*pose_func)(SurviveObject *so, uint32_t timecode, SurvivePose *pose); -typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, - SurvivePose *object_pose); +typedef void (*pose_func)(SurviveObject *so, survive_timecode timecode, SurvivePose *pose); +typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, SurvivePose *object_pose); // For lightcap, etc. Don't change this structure at all. Regular vive is dependent on it being exactly as-is. // When you write drivers, you can use this to send survive lightcap data. diff --git a/src/survive_api.c b/src/survive_api.c new file mode 100644 index 0000000..9883c9f --- /dev/null +++ b/src/survive_api.c @@ -0,0 +1,167 @@ +#include "survive_api.h" +#include "os_generic.h" +#include "survive.h" +#include "stdio.h" + +struct SurviveAsyncObject { + struct SurviveAsyncContext* actx; + + enum SurviveAsyncObject_type { + SurviveAsyncObject_LIGHTHOUSE, + SurviveAsyncObject_OBJECT + } type; + + union { + int lighthosue; + struct SurviveObject* so; + } data; + + char name[32]; + bool has_update; +}; + +struct SurviveAsyncContext { + SurviveContext* ctx; + + bool running; + og_thread_t thread; + og_mutex_t poll_mutex; + + size_t object_ct; + struct SurviveAsyncObject objects[]; +}; + +static void pose_fn(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { + survive_default_raw_pose_process(so, timecode, pose); + + struct SurviveAsyncContext* actx = so->ctx->user_ptr; + int idx = so->user_ptr; + actx->objects[idx].has_update = true; +} +static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, + SurvivePose *object_pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); + + struct SurviveAsyncContext* actx = ctx->user_ptr; + actx->objects[lighthouse].has_update = true; +} + +struct SurviveAsyncContext *survive_asyc_init(int argc, char *const *argv) { + SurviveContext* ctx = survive_init(argc, argv); + if (ctx == 0) + return 0; + + survive_startup(ctx); + + int object_ct = ctx->activeLighthouses + ctx->objs_ct; + struct SurviveAsyncContext * actx = calloc(1, sizeof(struct SurviveAsyncContext) + sizeof(struct SurviveAsyncObject) * object_ct ); + actx->object_ct = object_ct; + actx->ctx = ctx; + actx->poll_mutex = OGCreateMutex(); + ctx->user_ptr = actx; + int i = 0; + for (i = 0; i < ctx->activeLighthouses; i++) { + struct SurviveAsyncObject* obj = &actx->objects[i]; + obj->data.lighthosue = i; + obj->type = SurviveAsyncObject_LIGHTHOUSE; + obj->actx = actx; + obj->has_update = ctx->bsd[i].PositionSet; + snprintf(obj->name, 32, "LH%d", i); + } + for (; i < object_ct; i++) { + struct SurviveAsyncObject* obj = &actx->objects[i]; + int so_idx = i - ctx->activeLighthouses; + obj->data.so = ctx->objs[so_idx]; + obj->type = SurviveAsyncObject_OBJECT; + obj->actx = actx; + obj->data.so->user_ptr = (void*)i; + snprintf(obj->name, 32, "%s", obj->data.so->codename); + } + + survive_install_pose_fn(ctx, pose_fn); + survive_install_lighthouse_pose_fn(ctx, lh_fn); + return actx; +} + +void survive_async_close(struct SurviveAsyncContext* actx) { + if (actx->running) { + survive_async_stop_thread(actx); + } + + survive_close(actx->ctx); +} + +static inline void* __async_thread(void* _actx) { + struct SurviveAsyncContext* actx = _actx; + int error = 0; + while (actx->running && error == 0) { + OGLockMutex(actx->poll_mutex); + error = survive_poll(actx->ctx); + OGUnlockMutex(actx->poll_mutex); + } + actx->running = false; + return (void*)error; +} +bool survive_async_is_running(struct SurviveAsyncContext* actx) { + return actx->running; +} +void survive_async_start_thread(struct SurviveAsyncContext* actx) { + actx->running = true; + actx->thread = OGCreateThread(__async_thread, actx); +} +int survive_async_stop_thread(struct SurviveAsyncContext* actx) { + actx->running = false; + int error = (int)OGJoinThread(actx->thread); + if (error != 0) { + SurviveContext* ctx = actx->ctx; + SV_INFO("Warning: Loope exited with error %d", error); + } + return error; +} + +const struct SurviveAsyncObject* survive_async_get_next_tracked(struct SurviveAsyncContext* actx, const struct SurviveAsyncObject* curr) { + const struct SurviveAsyncObject* next = curr + 1; + if (next >= actx->objects + actx->object_ct) + return 0; + return next; +} + +const struct SurviveAsyncObject* survive_async_get_first_tracked(struct SurviveAsyncContext* actx) { + return actx->objects; +} + +const struct SurviveAsyncObject* survive_async_get_next_updated(struct SurviveAsyncContext* actx) { + for (int i = 0; i < actx->object_ct; i++) { + if (actx->objects[i].has_update) { + actx->objects[i].has_update = false; + return &actx->objects[i]; + } + } + return 0; +} + +uint32_t survive_async_object_get_latest_pose(const struct SurviveAsyncObject* sao, SurvivePose* pose) { + uint32_t timecode = 0; + OGLockMutex(sao->actx->poll_mutex); + + switch (sao->type) { + case SurviveAsyncObject_LIGHTHOUSE: { + if(pose) + *pose = sao->actx->ctx->bsd[sao->data.lighthosue].Pose; + break; + } + case SurviveAsyncObject_OBJECT: + if(pose) + *pose = sao->data.so->OutPose; + timecode = sao->data.so->OutPose_timecode; + break; + } + + OGUnlockMutex(sao->actx->poll_mutex); + return timecode; +} + +const char * survive_async_object_name(const SurviveAsyncObject * sao) +{ + return sao->name; +} diff --git a/src/survive_cal.c b/src/survive_cal.c index 3015b68..36e0a31 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -188,7 +188,6 @@ void survive_cal_install( struct SurviveContext * ctx ) } } - const char * DriverName; cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "SBA", 0); ootx_packet_clbk = ootx_packet_clbk_d; diff --git a/src/survive_reproject.c b/src/survive_reproject.c index d6ba70b..8d8f0ed 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -99,5 +99,5 @@ void survive_apply_bsd_calibration(SurviveContext *ctx, int lh, const FLT *in, F void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out) { - return survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out); + survive_reproject_from_pose_with_bsd(&ctx->bsd[lighthouse], config, pose, point3d, out); } diff --git a/winbuild/libsurvive.sln b/winbuild/libsurvive.sln index b525975..1872a5a 100644 --- a/winbuild/libsurvive.sln +++ b/winbuild/libsurvive.sln @@ -11,6 +11,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "data_recorder", "data_recor EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "test", "test\test.vcxproj", "{EF083B79-F1D7-408A-9902-502B9A0639E0}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "simple_pose_test", "simple_pose_test\simple_pose_test.vcxproj", "{308708B5-DDC9-49EE-BF31-C83E187AFE15}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "api_example", "api_example\api_example.vcxproj", "{545316AE-4E07-49DD-A2B8-7A2DE9676EFF}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|x64 = Debug|x64 @@ -51,8 +55,27 @@ Global {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x64.Build.0 = Release|x64 {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x86.ActiveCfg = Release|Win32 {EF083B79-F1D7-408A-9902-502B9A0639E0}.Release|x86.Build.0 = Release|Win32 + {308708B5-DDC9-49EE-BF31-C83E187AFE15}.Debug|x64.ActiveCfg = Debug|x64 + {308708B5-DDC9-49EE-BF31-C83E187AFE15}.Debug|x64.Build.0 = Debug|x64 + {308708B5-DDC9-49EE-BF31-C83E187AFE15}.Debug|x86.ActiveCfg = Debug|Win32 + {308708B5-DDC9-49EE-BF31-C83E187AFE15}.Debug|x86.Build.0 = Debug|Win32 + {308708B5-DDC9-49EE-BF31-C83E187AFE15}.Release|x64.ActiveCfg = Release|x64 + {308708B5-DDC9-49EE-BF31-C83E187AFE15}.Release|x64.Build.0 = Release|x64 + {308708B5-DDC9-49EE-BF31-C83E187AFE15}.Release|x86.ActiveCfg = Release|Win32 + {308708B5-DDC9-49EE-BF31-C83E187AFE15}.Release|x86.Build.0 = Release|Win32 + {545316AE-4E07-49DD-A2B8-7A2DE9676EFF}.Debug|x64.ActiveCfg = Debug|x64 + {545316AE-4E07-49DD-A2B8-7A2DE9676EFF}.Debug|x64.Build.0 = Debug|x64 + {545316AE-4E07-49DD-A2B8-7A2DE9676EFF}.Debug|x86.ActiveCfg = Debug|Win32 + {545316AE-4E07-49DD-A2B8-7A2DE9676EFF}.Debug|x86.Build.0 = Debug|Win32 + {545316AE-4E07-49DD-A2B8-7A2DE9676EFF}.Release|x64.ActiveCfg = Release|x64 + {545316AE-4E07-49DD-A2B8-7A2DE9676EFF}.Release|x64.Build.0 = Release|x64 + {545316AE-4E07-49DD-A2B8-7A2DE9676EFF}.Release|x86.ActiveCfg = Release|Win32 + {545316AE-4E07-49DD-A2B8-7A2DE9676EFF}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {EA7B8D68-188A-4190-B011-8A7F4EC52C38} + EndGlobalSection EndGlobal diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 4b76c99..85e9f5a 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -178,6 +178,7 @@ + @@ -197,6 +198,7 @@ + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index 1f98c68..61cec4c 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -135,6 +135,9 @@ Source Files + + Source Files + @@ -188,6 +191,9 @@ Source Files + + Header Files + -- cgit v1.2.3 From be7593dc945ed4025f81c6876760807310a67319 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 22:56:40 -0600 Subject: Minor cleanup --- Makefile | 2 +- include/libsurvive/survive_api.h | 7 ++++--- include/libsurvive/survive_types.h | 2 ++ redist/linmath.h | 2 ++ src/survive_api.c | 4 ++-- 5 files changed, 11 insertions(+), 6 deletions(-) diff --git a/Makefile b/Makefile index c5763cb..8493f18 100644 --- a/Makefile +++ b/Makefile @@ -40,7 +40,7 @@ ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o src/survive_api.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/include/libsurvive/survive_api.h b/include/libsurvive/survive_api.h index 30e30ac..aacd47a 100644 --- a/include/libsurvive/survive_api.h +++ b/include/libsurvive/survive_api.h @@ -8,7 +8,7 @@ extern "C" { struct SurviveAsyncContext; typedef struct SurviveAsyncContext SurviveAsyncContext; - SURVIVE_EXPORT SurviveAsyncContext *survive_asyc_init(int argc, char *const *argv); + SURVIVE_EXPORT SurviveAsyncContext *survive_async_init(int argc, char *const *argv); SURVIVE_EXPORT void survive_async_close(SurviveAsyncContext* actx); SURVIVE_EXPORT void survive_async_start_thread(SurviveAsyncContext* actx); @@ -22,8 +22,9 @@ extern "C" { SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_next_tracked(SurviveAsyncContext* actx, const SurviveAsyncObject* curr); SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_next_updated(SurviveAsyncContext* actx); - SURVIVE_EXPORT survive_timecode survive_async_object_get_latest_pose(const SurviveAsyncObject* sao, SurvivePose* pose); - SURVIVE_EXPORT const char* survive_async_object_name(SurviveAsyncObject* sao); + SURVIVE_EXPORT survive_timecode survive_async_object_get_latest_pose(const SurviveAsyncObject *sao, + SurvivePose *OUTPUT); + SURVIVE_EXPORT const char *survive_async_object_name(const SurviveAsyncObject *sao); #ifdef __cplusplus } diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index b9d145f..7fa5e0f 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -4,11 +4,13 @@ #include "linmath.h" #include "stdint.h" +#ifndef SURVIVE_EXPORT #ifdef _WIN32 #define SURVIVE_EXPORT __declspec(dllexport) #else #define SURVIVE_EXPORT __attribute__((visibility("default"))) #endif +#endif #ifdef __cplusplus extern "C" { diff --git a/redist/linmath.h b/redist/linmath.h index e268e96..bafdbe8 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -7,11 +7,13 @@ extern "C" { #endif +#ifndef LINMATH_EXPORT #ifdef _WIN32 #define LINMATH_EXPORT __declspec(dllexport) #else #define LINMATH_EXPORT __attribute__((visibility("default"))) #endif +#endif // Yes, I know it's kind of arbitrary. #define DEFAULT_EPSILON 0.001 diff --git a/src/survive_api.c b/src/survive_api.c index 9883c9f..2aaf31a 100644 --- a/src/survive_api.c +++ b/src/survive_api.c @@ -35,7 +35,7 @@ static void pose_fn(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { survive_default_raw_pose_process(so, timecode, pose); struct SurviveAsyncContext* actx = so->ctx->user_ptr; - int idx = so->user_ptr; + int idx = (int)so->user_ptr; actx->objects[idx].has_update = true; } static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, @@ -46,7 +46,7 @@ static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthou actx->objects[lighthouse].has_update = true; } -struct SurviveAsyncContext *survive_asyc_init(int argc, char *const *argv) { +struct SurviveAsyncContext *survive_async_init(int argc, char *const *argv) { SurviveContext* ctx = survive_init(argc, argv); if (ctx == 0) return 0; -- cgit v1.2.3 From 8992810b626e58e1dd1ed61b3b41f20db60dddf4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 00:50:05 -0600 Subject: Added basic documentation --- include/libsurvive/survive_api.h | 40 ++++++++++++++++++++++++++++++++++++---- 1 file changed, 36 insertions(+), 4 deletions(-) diff --git a/include/libsurvive/survive_api.h b/include/libsurvive/survive_api.h index aacd47a..52eca8d 100644 --- a/include/libsurvive/survive_api.h +++ b/include/libsurvive/survive_api.h @@ -8,22 +8,54 @@ extern "C" { struct SurviveAsyncContext; typedef struct SurviveAsyncContext SurviveAsyncContext; + /*** + * Initialize a new instance of an async context -- mirrors survive_init + * @return Pointer to the async context + */ SURVIVE_EXPORT SurviveAsyncContext *survive_async_init(int argc, char *const *argv); + + /** + * Close all devices and free all memory associated with the given context + */ SURVIVE_EXPORT void survive_async_close(SurviveAsyncContext* actx); + /** + * Start the background thread which processes images. + */ SURVIVE_EXPORT void survive_async_start_thread(SurviveAsyncContext* actx); - SURVIVE_EXPORT int survive_async_stop_thread(SurviveAsyncContext* actx); - SURVIVE_EXPORT bool survive_async_is_running(SurviveAsyncContext* actx); + + /** + * @return true iff the background thread is still running + */ + SURVIVE_EXPORT bool survive_async_is_running(SurviveAsyncContext *actx); struct SurviveAsyncObject; typedef struct SurviveAsyncObject SurviveAsyncObject; + /** + * Get the first known object. Note that the first n objects are the lighthouses; for however many you are + * configured for. + */ SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_first_tracked(SurviveAsyncContext* actx); + /** + * Get the next known object from a current one. + */ SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_next_tracked(SurviveAsyncContext* actx, const SurviveAsyncObject* curr); - SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_next_updated(SurviveAsyncContext* actx); + /** + * Gets the next object which has been updated since we last looked at it with this function + */ + SURVIVE_EXPORT const SurviveAsyncObject *survive_async_get_next_updated(SurviveAsyncContext *actx); + + /** + * Gets the pose of a given object + */ SURVIVE_EXPORT survive_timecode survive_async_object_get_latest_pose(const SurviveAsyncObject *sao, - SurvivePose *OUTPUT); + SurvivePose *pose); + + /** + * Gets the null terminated name of the object. + */ SURVIVE_EXPORT const char *survive_async_object_name(const SurviveAsyncObject *sao); #ifdef __cplusplus -- cgit v1.2.3 From 229a2cb44d1462200ffa14fe925cd91796e99989 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 09:36:10 -0600 Subject: Dramatically reduced the scope of locking --- src/survive_api.c | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/survive_api.c b/src/survive_api.c index 2aaf31a..bb1f090 100644 --- a/src/survive_api.c +++ b/src/survive_api.c @@ -32,18 +32,23 @@ struct SurviveAsyncContext { }; static void pose_fn(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { + struct SurviveAsyncContext *actx = so->ctx->user_ptr; + OGLockMutex(actx->poll_mutex); survive_default_raw_pose_process(so, timecode, pose); - struct SurviveAsyncContext* actx = so->ctx->user_ptr; int idx = (int)so->user_ptr; actx->objects[idx].has_update = true; + OGUnlockMutex(actx->poll_mutex); } static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, SurvivePose *object_pose) { + struct SurviveAsyncContext *actx = ctx->user_ptr; + OGLockMutex(actx->poll_mutex); survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); - struct SurviveAsyncContext* actx = ctx->user_ptr; actx->objects[lighthouse].has_update = true; + + OGUnlockMutex(actx->poll_mutex); } struct SurviveAsyncContext *survive_async_init(int argc, char *const *argv) { @@ -95,9 +100,7 @@ static inline void* __async_thread(void* _actx) { struct SurviveAsyncContext* actx = _actx; int error = 0; while (actx->running && error == 0) { - OGLockMutex(actx->poll_mutex); error = survive_poll(actx->ctx); - OGUnlockMutex(actx->poll_mutex); } actx->running = false; return (void*)error; -- cgit v1.2.3 From d438c5b16f42700c8ff541269759d6c585959729 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 09:37:18 -0600 Subject: Fixed api demo --- api_example.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/api_example.c b/api_example.c index f2d9bd6..515c4e6 100644 --- a/api_example.c +++ b/api_example.c @@ -4,7 +4,7 @@ #include int main(int argc, char **argv) { - SurviveAsyncContext *actx = survive_asyc_init(argc, argv); + SurviveAsyncContext *actx = survive_async_init(argc, argv); if (actx == 0) // implies -help or similiar return 0; -- cgit v1.2.3 From d6d310fdd13c11382f37faca6a0c20b361ae9c40 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 4 Apr 2018 10:42:51 -0600 Subject: Added python binding --- Makefile | 5 +-- bindings/python/Makefile | 7 ++++ bindings/python/example.py | 13 +++++++ bindings/python/libsurvive.py | 83 +++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 106 insertions(+), 2 deletions(-) create mode 100644 bindings/python/Makefile create mode 100644 bindings/python/example.py create mode 100644 bindings/python/libsurvive.py diff --git a/Makefile b/Makefile index 8493f18..7532dce 100644 --- a/Makefile +++ b/Makefile @@ -2,12 +2,13 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test CC?=gcc -CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses + +CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic #-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses CFLAGS_RELEASE:=-Iinclude/libsurvive -fPIC -msse2 -ftree-vectorize -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #LDFLAGS:=-L/usr/local/lib -lpthread -lusb-1.0 -lz -lm -flto -g -LDFLAGS:=-L/usr/local/lib -lpthread -lz -lm -flto -g +LDFLAGS:=-L/usr/local/lib -lpthread -lz -llapacke -lcblas -lm -flto -g #---------- # Platform specific changes to CFLAGS/LDFLAGS diff --git a/bindings/python/Makefile b/bindings/python/Makefile new file mode 100644 index 0000000..d3f846e --- /dev/null +++ b/bindings/python/Makefile @@ -0,0 +1,7 @@ +_libsurvive.so: ../../lib/libsurvive.so + cp ../../lib/libsurvive.so _libsurvive.so + +../../lib/libsurvive.so: .always + cd ../.. && make clean all + +.always: diff --git a/bindings/python/example.py b/bindings/python/example.py new file mode 100644 index 0000000..5454fbc --- /dev/null +++ b/bindings/python/example.py @@ -0,0 +1,13 @@ +import libsurvive +import sys + +actx = libsurvive.AsyncContext(sys.argv) + +for obj in actx.Objects(): + print(obj.Name()) + +while actx.Running(): + updated = actx.NextUpdated() + if updated: + print(updated.Name(), updated.Pose()) + diff --git a/bindings/python/libsurvive.py b/bindings/python/libsurvive.py new file mode 100644 index 0000000..1cf9b67 --- /dev/null +++ b/bindings/python/libsurvive.py @@ -0,0 +1,83 @@ +import ctypes + +_libsurvive = ctypes.CDLL('./_libsurvive.so') +LP_c_char = ctypes.POINTER(ctypes.c_char) +LP_LP_c_char = ctypes.POINTER(LP_c_char) + +_libsurvive.survive_async_init.argtypes = (ctypes.c_int, LP_LP_c_char) # argc, argv +_libsurvive.survive_async_init.restype = ctypes.c_void_p + +_libsurvive.survive_async_get_next_tracked.argtypes = [ctypes.c_void_p, ctypes.c_void_p] +_libsurvive.survive_async_get_next_tracked.restype = ctypes.c_void_p + +_libsurvive.survive_async_get_first_tracked.argtypes = [ctypes.c_void_p] +_libsurvive.survive_async_get_first_tracked.restype = ctypes.c_void_p + +_libsurvive.survive_async_get_next_updated.argtypes = [ctypes.c_void_p] +_libsurvive.survive_async_get_next_updated.restype = ctypes.c_void_p + +_libsurvive.survive_async_object_name.argtypes = [ ctypes.c_void_p ] +_libsurvive.survive_async_object_name.restype = ctypes.c_char_p + +_libsurvive.survive_async_is_running.argtypes = [ctypes.c_void_p] +_libsurvive.survive_async_is_running.restype = ctypes.c_bool + +_libsurvive.survive_async_start_thread.argtypes = [ctypes.c_void_p] +_libsurvive.survive_async_start_thread.restype = None + +class SurvivePose(ctypes.Structure): + _fields_ = [ + ('Pos', ctypes.c_double * 3), + ('Rot', ctypes.c_double * 4) + ] + def __repr__(self): + return '[{0} {1} {2}], [{3} {4} {5} {6}]'.format(self.Pos[0],self.Pos[1],self.Pos[2],self.Rot[0],self.Rot[1],self.Rot[2],self.Rot[3]) + + +_libsurvive.survive_async_object_get_latest_pose.argtypes = [ctypes.c_void_p, ctypes.POINTER(SurvivePose)] +_libsurvive.survive_async_object_get_latest_pose.restype = ctypes.c_uint + +class AsyncObject: + ptr = 0 + def __init__(self, ptr): + self.ptr = ptr + + def Name(self): + return _libsurvive.survive_async_object_name(self.ptr) + + def Pose(self): + pose = SurvivePose() + time = _libsurvive.survive_async_object_get_latest_pose(self.ptr, pose) + return (pose, time) + +class AsyncContext: + ptr = 0 + objects = [] + + def __init__(self, args): + argc = len(args) + argv = (LP_c_char * (argc + 1))() + for i, arg in enumerate(args): + enc_arg = arg.encode('utf-8') + argv[i] = ctypes.create_string_buffer(enc_arg) + self.ptr = _libsurvive.survive_async_init(argc, argv) + + self.objects = [] + curr = _libsurvive.survive_async_get_first_tracked(self.ptr); + while curr: + self.objects.append(AsyncObject(curr)) + curr = _libsurvive.survive_async_get_next_tracked(self.ptr, curr); + _libsurvive.survive_async_start_thread(self.ptr) + + def Objects(self): + return self.objects + + def Running(self): + return _libsurvive.survive_async_is_running(self.ptr) + + def NextUpdated(self): + ptr = _libsurvive.survive_async_get_next_updated(self.ptr) + if ptr: + return AsyncObject(ptr) + return None + -- cgit v1.2.3 From 9e1883922de980c01e60bde10c1e67261752afa6 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 5 Apr 2018 07:56:01 -0600 Subject: Fixed typo; naming suggestions --- api_example.c | 3 ++- include/libsurvive/survive_api.h | 10 +++++----- src/survive_api.c | 11 ++++++----- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/api_example.c b/api_example.c index 515c4e6..b0c9e4c 100644 --- a/api_example.c +++ b/api_example.c @@ -15,7 +15,8 @@ int main(int argc, char **argv) { SurvivePose pose; - for (const SurviveAsyncObject* it = survive_async_get_first_tracked(actx); it != 0; it = survive_async_get_next_tracked(actx, it)) { + for (const SurviveAsyncObject *it = survive_async_get_first_object(actx); it != 0; + it = survive_async_get_next_object(actx, it)) { uint32_t timecode = survive_async_object_get_latest_pose(it, &pose); printf("%s (%u): %f %f %f %f %f %f %f\n", survive_async_object_name(it), timecode, pose.Pos[0], pose.Pos[1], pose.Pos[2], pose.Rot[0], pose.Rot[1], pose.Rot[2], pose.Rot[3]); diff --git a/include/libsurvive/survive_api.h b/include/libsurvive/survive_api.h index 52eca8d..fc4ce2c 100644 --- a/include/libsurvive/survive_api.h +++ b/include/libsurvive/survive_api.h @@ -20,7 +20,7 @@ extern "C" { SURVIVE_EXPORT void survive_async_close(SurviveAsyncContext* actx); /** - * Start the background thread which processes images. + * Start the background thread which processes various inputs and produces deliverable data like position. */ SURVIVE_EXPORT void survive_async_start_thread(SurviveAsyncContext* actx); @@ -33,14 +33,14 @@ extern "C" { typedef struct SurviveAsyncObject SurviveAsyncObject; /** - * Get the first known object. Note that the first n objects are the lighthouses; for however many you are - * configured for. + * Get the first known object. Note that this also includes lighthouses */ - SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_first_tracked(SurviveAsyncContext* actx); + SURVIVE_EXPORT const SurviveAsyncObject *survive_async_get_first_object(SurviveAsyncContext *actx); /** * Get the next known object from a current one. */ - SURVIVE_EXPORT const SurviveAsyncObject* survive_async_get_next_tracked(SurviveAsyncContext* actx, const SurviveAsyncObject* curr); + SURVIVE_EXPORT const SurviveAsyncObject *survive_async_get_next_object(SurviveAsyncContext *actx, + const SurviveAsyncObject *curr); /** * Gets the next object which has been updated since we last looked at it with this function diff --git a/src/survive_api.c b/src/survive_api.c index bb1f090..e2c3d54 100644 --- a/src/survive_api.c +++ b/src/survive_api.c @@ -12,7 +12,7 @@ struct SurviveAsyncObject { } type; union { - int lighthosue; + int lighthouse; struct SurviveObject* so; } data; @@ -67,7 +67,7 @@ struct SurviveAsyncContext *survive_async_init(int argc, char *const *argv) { int i = 0; for (i = 0; i < ctx->activeLighthouses; i++) { struct SurviveAsyncObject* obj = &actx->objects[i]; - obj->data.lighthosue = i; + obj->data.lighthouse = i; obj->type = SurviveAsyncObject_LIGHTHOUSE; obj->actx = actx; obj->has_update = ctx->bsd[i].PositionSet; @@ -122,14 +122,15 @@ int survive_async_stop_thread(struct SurviveAsyncContext* actx) { return error; } -const struct SurviveAsyncObject* survive_async_get_next_tracked(struct SurviveAsyncContext* actx, const struct SurviveAsyncObject* curr) { +const struct SurviveAsyncObject *survive_async_get_next_object(struct SurviveAsyncContext *actx, + const struct SurviveAsyncObject *curr) { const struct SurviveAsyncObject* next = curr + 1; if (next >= actx->objects + actx->object_ct) return 0; return next; } -const struct SurviveAsyncObject* survive_async_get_first_tracked(struct SurviveAsyncContext* actx) { +const struct SurviveAsyncObject *survive_async_get_first_object(struct SurviveAsyncContext *actx) { return actx->objects; } @@ -150,7 +151,7 @@ uint32_t survive_async_object_get_latest_pose(const struct SurviveAsyncObject* s switch (sao->type) { case SurviveAsyncObject_LIGHTHOUSE: { if(pose) - *pose = sao->actx->ctx->bsd[sao->data.lighthosue].Pose; + *pose = sao->actx->ctx->bsd[sao->data.lighthouse].Pose; break; } case SurviveAsyncObject_OBJECT: -- cgit v1.2.3 From 0653df1da7364e75953eea85e92d23f2b41480be Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 5 Apr 2018 08:39:25 -0600 Subject: Fixed naming in python binding --- bindings/python/libsurvive.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/bindings/python/libsurvive.py b/bindings/python/libsurvive.py index 1cf9b67..e88657a 100644 --- a/bindings/python/libsurvive.py +++ b/bindings/python/libsurvive.py @@ -7,11 +7,11 @@ LP_LP_c_char = ctypes.POINTER(LP_c_char) _libsurvive.survive_async_init.argtypes = (ctypes.c_int, LP_LP_c_char) # argc, argv _libsurvive.survive_async_init.restype = ctypes.c_void_p -_libsurvive.survive_async_get_next_tracked.argtypes = [ctypes.c_void_p, ctypes.c_void_p] -_libsurvive.survive_async_get_next_tracked.restype = ctypes.c_void_p +_libsurvive.survive_async_get_next_object.argtypes = [ctypes.c_void_p, ctypes.c_void_p] +_libsurvive.survive_async_get_next_object.restype = ctypes.c_void_p -_libsurvive.survive_async_get_first_tracked.argtypes = [ctypes.c_void_p] -_libsurvive.survive_async_get_first_tracked.restype = ctypes.c_void_p +_libsurvive.survive_async_get_first_object.argtypes = [ctypes.c_void_p] +_libsurvive.survive_async_get_first_object.restype = ctypes.c_void_p _libsurvive.survive_async_get_next_updated.argtypes = [ctypes.c_void_p] _libsurvive.survive_async_get_next_updated.restype = ctypes.c_void_p @@ -63,10 +63,10 @@ class AsyncContext: self.ptr = _libsurvive.survive_async_init(argc, argv) self.objects = [] - curr = _libsurvive.survive_async_get_first_tracked(self.ptr); + curr = _libsurvive.survive_async_get_first_object(self.ptr); while curr: self.objects.append(AsyncObject(curr)) - curr = _libsurvive.survive_async_get_next_tracked(self.ptr, curr); + curr = _libsurvive.survive_async_get_next_object(self.ptr, curr); _libsurvive.survive_async_start_thread(self.ptr) def Objects(self): -- cgit v1.2.3 From 5b07e56094f2f765d679ac06bc07d4081338bb29 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 09:53:26 -0600 Subject: Added statistics to sba --- src/poser_sba.c | 29 ++++++++++++++++++++++++++--- tools/showreproject/showreproject.cc | 2 ++ 2 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/poser_sba.c b/src/poser_sba.c index 23e03fc..827e420 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -51,6 +51,13 @@ typedef struct SBAData { SurviveIMUTracker tracker; bool useIMU; + struct { + int runs; + int poser_seed_runs; + int meas_failures; + int error_failures; + } stats; + SurviveObject *so; } SBAData; @@ -208,6 +215,9 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); failure_count = 0; } + if (meas_size < d->required_meas) { + d->stats.meas_failures++; + } return -1; } failure_count = 0; @@ -231,6 +241,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive pdl->hdr.userdata = &locations; driver(so, &pdl->hdr); pdl->hdr = hdr; + d->stats.poser_seed_runs++; if (locations.hasInfo == false) { return -1; @@ -256,6 +267,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); opts[4] = 0.0; + d->stats.runs++; int status = sba_str_levmar(1, // Number of 3d points 0, // Number of 3d points to fix in spot NUM_LIGHTHOUSES * so->sensor_ct, vmask, @@ -287,6 +299,8 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; + } else if ((info[1] / meas_size * 2) >= d->max_error) { + d->stats.error_failures++; } { @@ -466,13 +480,22 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // std::cerr << "Average reproj error: " << error << std::endl; return 0; } + case POSERDATA_DISASSOCIATE: { + SV_INFO("SBA stats:"); + SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs); + SV_INFO("\tmeas failures %d", d->stats.meas_failures); + SV_INFO("\terror failures %d", d->stats.error_failures); + free(d); + so->PoserData = 0; + return 0; + } case POSERDATA_IMU: { PoserDataIMU * imu = (PoserDataIMU*)pd; if (ctx->calptr && ctx->calptr->stage < 5) { - } else if(d->useIMU){ - survive_imu_tracker_integrate(so, &d->tracker, imu); - PoserData_poser_pose_func(pd, so, &d->tracker.pose); + } else if (d->useIMU) { + survive_imu_tracker_integrate(so, &d->tracker, imu); + PoserData_poser_pose_func(pd, so, &d->tracker.pose); } } // INTENTIONAL FALLTHROUGH default: { diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 98dd5f0..755cb92 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -263,6 +263,8 @@ int main(int argc, char **argv) { cv::imshow(name, err[i]); } + survive_close(ctx1); + std::cerr << "Error: " << error / error_count << std::endl; int c = '\0'; -- cgit v1.2.3 From bb0d186e967421c48ba781759063dca165f69df2 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 10:15:23 -0600 Subject: Detailed messages about sba failures --- src/poser_sba.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/poser_sba.c b/src/poser_sba.c index 827e420..1eefc80 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -294,23 +294,23 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } double rtn = -1; - if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { + bool status_failure = status <= 0; + bool error_failure = (info[1] / meas_size * 2) >= d->max_error; + if (!status_failure && !error_failure) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; - } else if ((info[1] / meas_size * 2) >= d->max_error) { + } else if (error_failure) { d->stats.error_failures++; } { SurviveContext *ctx = so->ctx; // 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)); - cnt = 0; + if (error_failure) { + 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)); } } -- cgit v1.2.3 From 7aa84b0f423a94e15c180a0834114692b6ba4870 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 15:49:44 -0600 Subject: Libraries should link on the SO --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index c5763cb..48d5a98 100644 --- a/Makefile +++ b/Makefile @@ -2,12 +2,12 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test CC?=gcc -CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses +CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic #-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses CFLAGS_RELEASE:=-Iinclude/libsurvive -fPIC -msse2 -ftree-vectorize -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #LDFLAGS:=-L/usr/local/lib -lpthread -lusb-1.0 -lz -lm -flto -g -LDFLAGS:=-L/usr/local/lib -lpthread -lz -lm -flto -g +LDFLAGS:=-L/usr/local/lib -lpthread -lz -lm -flto -g -llapacke -lcblas -lm #---------- # Platform specific changes to CFLAGS/LDFLAGS -- cgit v1.2.3 From a2e00c099d28e541c0e1fa1bd4fa8d3e386ae089 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 15:50:49 -0600 Subject: Put compile inline hint for linmath functions --- redist/linmath.c | 78 ++++++++++++++++++++++++++++---------------------------- redist/linmath.h | 3 ++- 2 files changed, 41 insertions(+), 40 deletions(-) diff --git a/redist/linmath.c b/redist/linmath.c index 01ae2b0..50baeab 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -5,31 +5,31 @@ #include #include -void cross3d(FLT *out, const FLT *a, const FLT *b) { +inline void cross3d(FLT *out, const FLT *a, const FLT *b) { out[0] = a[1] * b[2] - a[2] * b[1]; out[1] = a[2] * b[0] - a[0] * b[2]; out[2] = a[0] * b[1] - a[1] * b[0]; } -void sub3d(FLT *out, const FLT *a, const FLT *b) { +inline void sub3d(FLT *out, const FLT *a, const FLT *b) { out[0] = a[0] - b[0]; out[1] = a[1] - b[1]; out[2] = a[2] - b[2]; } -void add3d(FLT *out, const FLT *a, const FLT *b) { +inline void add3d(FLT *out, const FLT *a, const FLT *b) { out[0] = a[0] + b[0]; out[1] = a[1] + b[1]; out[2] = a[2] + b[2]; } -void scale3d(FLT *out, const FLT *a, FLT scalar) { +inline void scale3d(FLT *out, const FLT *a, FLT scalar) { out[0] = a[0] * scalar; out[1] = a[1] * scalar; out[2] = a[2] * scalar; } -void normalize3d(FLT *out, const FLT *in) { +inline void normalize3d(FLT *out, const FLT *in) { FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]); out[0] = in[0] * r; out[1] = in[1] * r; @@ -56,7 +56,7 @@ int compare3d(const FLT *a, const FLT *b, FLT epsilon) { return 0; } -void copy3d(FLT *out, const FLT *in) { +inline void copy3d(FLT *out, const FLT *in) { out[0] = in[0]; out[1] = in[1]; out[2] = in[2]; @@ -83,7 +83,7 @@ FLT anglebetween3d(FLT *a, FLT *b) { } // algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/ -void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) { +inline void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) { // TODO: this really should be external. normalize3d(axis, axis); @@ -103,7 +103,7 @@ void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle) { outvec3[2] = w * (u * x + v * y + w * z) * (1 - c) + z * c + (-v * x + u * y) * s; } -void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest) { +inline void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest) { FLT v0[3]; FLT v1[3]; normalize3d(v0, src); @@ -136,7 +136,7 @@ void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest) { cross3d(axis, v1, v0); } -void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) { +inline void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) { // this way might be fine, too. // FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); // @@ -173,21 +173,21 @@ void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) { // Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) // Under the mit/X11 license. -void quatsetnone(LinmathQuat q) { +inline void quatsetnone(LinmathQuat q) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } -void quatcopy(LinmathQuat qout, const LinmathQuat qin) { +inline void quatcopy(LinmathQuat qout, const LinmathQuat qin) { qout[0] = qin[0]; qout[1] = qin[1]; qout[2] = qin[2]; qout[3] = qin[3]; } -void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler) { +inline void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler) { FLT X = euler[0] / 2.0f; // roll FLT Y = euler[1] / 2.0f; // pitch FLT Z = euler[2] / 2.0f; // yaw @@ -208,14 +208,14 @@ void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler) { quatnormalize(q, q); } -void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q) { +inline void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q) { // According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009) euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2])); euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1])); euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3])); } -void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians) { +inline void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians) { FLT v[3]; normalize3d(v, axis); @@ -236,12 +236,12 @@ FLT quatinvsqmagnitude(const LinmathQuat q) { return ((FLT)1.) / FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3])); } -void quatnormalize(LinmathQuat qout, const LinmathQuat qin) { +inline void quatnormalize(LinmathQuat qout, const LinmathQuat qin) { FLT imag = quatinvsqmagnitude(qin); quatscale(qout, qin, imag); } -void quattomatrix(FLT *matrix44, const LinmathQuat qin) { +inline void quattomatrix(FLT *matrix44, const LinmathQuat qin) { FLT q[4]; quatnormalize(q, qin); @@ -280,7 +280,7 @@ void quattomatrix(FLT *matrix44, const LinmathQuat qin) { matrix44[15] = 1; } -void quatfrommatrix33(FLT *q, const FLT *m) { +inline void quatfrommatrix33(FLT *q, const FLT *m) { FLT m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7], m22 = m[8]; FLT tr = m00 + m11 + m22; @@ -312,7 +312,7 @@ void quatfrommatrix33(FLT *q, const FLT *m) { } } -void quatfrommatrix(LinmathQuat q, const FLT *matrix44) { +inline void quatfrommatrix(LinmathQuat q, const FLT *matrix44) { // Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ FLT tr = matrix44[0] + matrix44[5] + matrix44[10]; @@ -344,7 +344,7 @@ void quatfrommatrix(LinmathQuat q, const FLT *matrix44) { } // Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ -void quattomatrix33(FLT *matrix33, const LinmathQuat qin) { +inline void quattomatrix33(FLT *matrix33, const LinmathQuat qin) { FLT q[4]; quatnormalize(q, qin); @@ -375,34 +375,34 @@ void quattomatrix33(FLT *matrix33, const LinmathQuat qin) { matrix33[8] = 1 - xx - yy; } -void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin) { +inline void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin) { qout[0] = qin[0]; qout[1] = -qin[1]; qout[2] = -qin[2]; qout[3] = -qin[3]; } -void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin) { +inline void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin) { FLT m = quatinvsqmagnitude(qin); quatgetconjugate(qout, qin); quatscale(qout, qout, m); } -void quatsub(LinmathQuat qout, const FLT *a, const FLT *b) { +inline void quatsub(LinmathQuat qout, const FLT *a, const FLT *b) { qout[0] = a[0] - b[0]; qout[1] = a[1] - b[1]; qout[2] = a[2] - b[2]; qout[3] = a[3] - b[3]; } -void quatadd(LinmathQuat qout, const FLT *a, const FLT *b) { +inline void quatadd(LinmathQuat qout, const FLT *a, const FLT *b) { qout[0] = a[0] + b[0]; qout[1] = a[1] + b[1]; qout[2] = a[2] + b[2]; qout[3] = a[3] + b[3]; } -void quatrotateabout(LinmathQuat qout, const LinmathQuat q1, const LinmathQuat q2) { +inline void quatrotateabout(LinmathQuat qout, const LinmathQuat q1, const LinmathQuat q2) { // NOTE: Does not normalize qout[0] = (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]); qout[1] = (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]); @@ -410,7 +410,7 @@ void quatrotateabout(LinmathQuat qout, const LinmathQuat q1, const LinmathQuat q qout[3] = (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0]); } -void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s) { +inline void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s) { qout[0] = qin[0] * s; qout[1] = qin[1] * s; qout[2] = qin[2] * s; @@ -421,26 +421,26 @@ FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb) { return (qa[0] * qb[0]) + (qa[1] * qb[1]) + (qa[2] * qb[2]) + (qa[3] * qb[3]); } -void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) { +inline void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) { outvec3[0] = (qa[0] * qb[1]) - (qa[1] * qb[0]) - (qa[2] * qb[3]) + (qa[3] * qb[2]); outvec3[1] = (qa[0] * qb[2]) + (qa[1] * qb[3]) - (qa[2] * qb[0]) - (qa[3] * qb[1]); outvec3[2] = (qa[0] * qb[3]) - (qa[1] * qb[2]) + (qa[2] * qb[1]) - (qa[3] * qb[0]); } -void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb) { +inline void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb) { q[0] = (qa[0] * qb[0]) - (qa[1] * qb[1]) - (qa[2] * qb[2]) - (qa[3] * qb[3]); q[1] = (qa[0] * qb[1]) + (qa[1] * qb[0]); q[2] = (qa[0] * qb[2]) + (qa[2] * qb[0]); q[3] = (qa[0] * qb[3]) + (qa[3] * qb[0]); } -void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) { +inline void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) { outvec3[0] = (qa[2] * qb[3]) - (qa[3] * qb[2]); outvec3[1] = (qa[3] * qb[1]) - (qa[1] * qb[3]); outvec3[2] = (qa[1] * qb[2]) - (qa[2] * qb[1]); } -void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t) { +inline void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t) { FLT an[4]; FLT bn[4]; quatnormalize(an, qa); @@ -472,7 +472,7 @@ void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t) } } -void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in) { +inline void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in) { // See: http://www.geeks3d.com/20141201/how-to-rotate-a-vertex-by-a-quaternion-in-glsl/ FLT tmp[3]; @@ -508,7 +508,7 @@ Matrix3x3 inverseM33(const Matrix3x3 mat) { return newMat; } -void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]) { +inline void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]) { FLT q[4]; quatfrom2vectors(q, v1, v2); @@ -516,7 +516,7 @@ void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]) quattomatrix33(&(m->val[0][0]), q); } -void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot) { +inline void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot) { out[0] = rot.val[0][0] * in[0] + rot.val[1][0] * in[1] + rot.val[2][0] * in[2]; out[1] = rot.val[0][1] * in[0] + rot.val[1][1] * in[1] + rot.val[2][1] * in[2]; out[2] = rot.val[0][2] * in[0] + rot.val[1][2] * in[1] + rot.val[2][2] * in[2]; @@ -536,7 +536,7 @@ If you call this with a dest vector that is close to the inverse of this vector, we will rotate 180 degrees around a generated axis if since in this case ANY axis of rotation is valid. */ -void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) { +inline void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) { // Based on Stan Melax's article in Game Programming Gems // Copy, since cannot modify local @@ -580,9 +580,9 @@ void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest) { } } -void matrix44copy(FLT *mout, const FLT *minm) { memcpy(mout, minm, sizeof(FLT) * 16); } +inline void matrix44copy(FLT *mout, const FLT *minm) { memcpy(mout, minm, sizeof(FLT) * 16); } -void matrix44transpose(FLT *mout, const FLT *minm) { +inline void matrix44transpose(FLT *mout, const FLT *minm) { mout[0] = minm[0]; mout[1] = minm[4]; mout[2] = minm[8]; @@ -604,24 +604,24 @@ void matrix44transpose(FLT *mout, const FLT *minm) { mout[15] = minm[15]; } -void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin) { +inline void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin) { quatrotatevector(pout, pose->Rot, pin); add3d(pout, pout, pose->Pos); } -void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose) { +inline void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose) { ApplyPoseToPoint(pout->Pos, lhs_pose, rhs_pose->Pos); quatrotateabout(pout->Rot, lhs_pose->Rot, rhs_pose->Rot); } -void InvertPose(LinmathPose *poseout, const LinmathPose *pose) { +inline void InvertPose(LinmathPose *poseout, const LinmathPose *pose) { quatgetreciprocal(poseout->Rot, pose->Rot); quatrotatevector(poseout->Pos, poseout->Rot, pose->Pos); scale3d(poseout->Pos, poseout->Pos, -1); } -void PoseToMatrix(FLT *matrix44, const LinmathPose *pose_in) { +inline void PoseToMatrix(FLT *matrix44, const LinmathPose *pose_in) { quattomatrix(matrix44, pose_in->Rot); /* diff --git a/redist/linmath.h b/redist/linmath.h index e268e96..60cc9d6 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -8,7 +8,7 @@ extern "C" { #endif #ifdef _WIN32 -#define LINMATH_EXPORT __declspec(dllexport) +#define LINMATH_EXPORT extern __declspec(dllexport) #else #define LINMATH_EXPORT __attribute__((visibility("default"))) #endif @@ -108,6 +108,7 @@ FLT quatmagnitude(const LinmathQuat q); FLT quatinvsqmagnitude(const LinmathQuat q); LINMATH_EXPORT void quatnormalize(LinmathQuat qout, const LinmathQuat qin); // Safe for in to be same as out. LINMATH_EXPORT void quattomatrix(FLT *matrix44, const LinmathQuat q); +LINMATH_EXPORT void quattomatrix33(FLT *matrix33, const LinmathQuat qin); LINMATH_EXPORT void quatfrommatrix(LinmathQuat q, const FLT *matrix44); LINMATH_EXPORT void quatfrommatrix33(LinmathQuat q, const FLT *matrix33); LINMATH_EXPORT void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin); -- cgit v1.2.3 From 347a479f84f124548e810d94d91175b6be43db1d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 15:52:22 -0600 Subject: modified showreproject tool to be less annoying --- tools/showreproject/showreproject.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 755cb92..8bb91a2 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -223,8 +223,8 @@ int main(int argc, char **argv) { } size_t showui = survive_configi(ctx1, "show-ui", SC_GET, 0); - - drawbsds(ctx1); + if(showui) + drawbsds(ctx1); int waitUpdate = 100; int SIZE = 1000; -- cgit v1.2.3 From d46271513e6f789af0e82d4ed6628abe21e96a92 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 15:54:07 -0600 Subject: Added jacobian to sba, ~2x speed improvement --- .gitignore | 1 + include/libsurvive/survive_reproject.h | 9 + src/poser_sba.c | 41 ++-- src/survive_reproject.c | 70 ++++++- src/survive_reproject.generated.h | 213 +++++++++++++++++++++ tools/generate_reprojection_functions/Makefile | 20 ++ .../check_generated.c | 128 +++++++++++++ .../reprojection_functions.sage | 131 +++++++++++++ 8 files changed, 599 insertions(+), 14 deletions(-) create mode 100644 src/survive_reproject.generated.h create mode 100644 tools/generate_reprojection_functions/Makefile create mode 100644 tools/generate_reprojection_functions/check_generated.c create mode 100644 tools/generate_reprojection_functions/reprojection_functions.sage diff --git a/.gitignore b/.gitignore index f39d866..4e90b84 100644 --- a/.gitignore +++ b/.gitignore @@ -20,6 +20,7 @@ simple_pose_test calinfo/ *.log *.png +*.sage.py # Windows specific *.dll diff --git a/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index e4f21d0..e7c1745 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -16,6 +16,15 @@ void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse, cons void survive_reproject_from_pose_with_config(const SurviveContext *ctx, struct survive_calibration_config *config, int lighthouse, const SurvivePose *pose, FLT *point3d, FLT *out); +void survive_reproject_full_jac_obj_pose(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config); + +void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config); + + // This is given a lighthouse -- in the same system as stored in BaseStationData, and // a 3d point and finds what the effective 'angle' value for a given lighthouse system // would be. diff --git a/src/poser_sba.c b/src/poser_sba.c index 1eefc80..f101018 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -45,7 +45,7 @@ typedef struct SBAData { FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; - + int use_jacobian_function; int required_meas; SurviveIMUTracker tracker; @@ -183,13 +183,30 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada assert(lh < 2); assert(sensor_idx < so->sensor_ct); - quatnormalize(obj.Rot, obj.Rot); - FLT xyz[3]; - ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); + // quatnormalize(obj.Rot, obj.Rot); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; - survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij); + survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); +} + +static void str_metric_function_jac(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + sba_context *ctx = (sba_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + // quatnormalize(obj.Rot, obj.Rot); + + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); } static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, @@ -277,12 +294,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive cov, // cov data 2, // mnp -- 2 points per image str_metric_function, - 0, // jacobia of metric_func - &ctx, // user data - max_iterations, // Max iterations - 0, // verbosity - opts, // options - info); // info + d->use_jacobian_function ? str_metric_function_jac : 0, // jacobia of metric_func + &ctx, // user data + max_iterations, // Max iterations + 0, // verbosity + opts, // options + info); // info if (currentPositionValid) { // FLT distp[3]; @@ -421,6 +438,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); + d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); d->so = so; SV_INFO("Initializing SBA:"); @@ -431,6 +449,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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); + SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function); } SBAData *d = so->PoserData; switch (pd->pt) { diff --git a/src/survive_reproject.c b/src/survive_reproject.c index d6ba70b..921386b 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -1,8 +1,72 @@ #include "survive_reproject.h" -#include <../redist/linmath.h> +#include "survive_reproject.generated.h" #include -#include -#include + +void survive_reproject_full_jac_obj_pose(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config) { + FLT phase_scale = config->use_flag & SVCal_Phase ? config->phase_scale : 0.; + FLT phase_0 = bsd->fcal.phase[0]; + FLT phase_1 = bsd->fcal.phase[1]; + + FLT tilt_scale = config->use_flag & SVCal_Tilt ? config->tilt_scale : 0.; + FLT tilt_0 = bsd->fcal.tilt[0]; + FLT tilt_1 = bsd->fcal.tilt[1]; + + FLT curve_scale = config->use_flag & SVCal_Curve ? config->curve_scale : 0.; + FLT curve_0 = bsd->fcal.curve[0]; + FLT curve_1 = bsd->fcal.curve[1]; + + FLT gib_scale = config->use_flag & SVCal_Gib ? config->gib_scale : 0; + FLT gibPhase_0 = bsd->fcal.gibpha[0]; + FLT gibPhase_1 = bsd->fcal.gibpha[1]; + FLT gibMag_0 = bsd->fcal.gibmag[0]; + FLT gibMag_1 = bsd->fcal.gibmag[1]; + + gen_reproject_jac_obj_p(out, obj_pose->Pos, obj_pt, lh2world->Pos, phase_scale, phase_0, phase_1, tilt_scale, + tilt_0, tilt_1, curve_scale, curve_0, curve_1, gib_scale, gibPhase_0, gibPhase_1, gibMag_0, + gibMag_1); +} + +void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config) { + LinmathVec3d world_pt; + ApplyPoseToPoint(world_pt, obj_pose, obj_pt); + + SurvivePose world2lh; + InvertPose(&world2lh, lh2world); + + LinmathPoint3d t_pt; + ApplyPoseToPoint(t_pt, &world2lh, world_pt); + + FLT x = -t_pt[0] / -t_pt[2]; + FLT y = t_pt[1] / -t_pt[2]; + double xy[] = {x, y}; + double ang[] = {atan(x), atan(y)}; + + const FLT *phase = bsd->fcal.phase; + const FLT *curve = bsd->fcal.curve; + const FLT *tilt = bsd->fcal.tilt; + const FLT *gibPhase = bsd->fcal.gibpha; + const FLT *gibMag = bsd->fcal.gibmag; + enum SurviveCalFlag f = config->use_flag; + + for (int axis = 0; axis < 2; axis++) { + int opp_axis = axis == 0 ? 1 : 0; + + out[axis] = ang[axis]; + + if (f & SVCal_Phase) + out[axis] -= config->phase_scale * phase[axis]; + if (f & SVCal_Tilt) + out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis]; + if (f & SVCal_Curve) + out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis]; + if (f & SVCal_Gib) + out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; + } +} void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *pt, FLT *out) { diff --git a/src/survive_reproject.generated.h b/src/survive_reproject.generated.h new file mode 100644 index 0000000..f4b499a --- /dev/null +++ b/src/survive_reproject.generated.h @@ -0,0 +1,213 @@ + // NOTE: Auto-generated code; see tools/generate_reprojection_functions +#include +static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT *sensor, const FLT *lh, FLT phase_scale, FLT phase_0, FLT phase_1, FLT tilt_scale, FLT tilt_0, FLT tilt_1, FLT curve_scale, FLT curve_0, FLT curve_1, FLT gib_scale, FLT gibPhase_0, FLT gibPhase_1, FLT gibMag_0, FLT gibMag_1) { + FLT obj_px = *(obj++); + FLT obj_py = *(obj++); + FLT obj_pz = *(obj++); + FLT obj_qw = *(obj++); + FLT obj_qi = *(obj++); + FLT obj_qj = *(obj++); + FLT obj_qk = *(obj++); + FLT sensor_x = *(sensor++); + FLT sensor_y = *(sensor++); + FLT sensor_z = *(sensor++); + FLT lh_px = *(lh++); + FLT lh_py = *(lh++); + FLT lh_pz = *(lh++); + FLT lh_qw = *(lh++); + FLT lh_qi = *(lh++); + FLT lh_qj = *(lh++); + FLT lh_qk = *(lh++); + FLT x0 = tan(tilt_0*tilt_scale); + FLT x1 = lh_qi*lh_qj; + FLT x2 = lh_qk*lh_qw; + FLT x3 = x1 - x2; + FLT x4 = pow(lh_qi, 2); + FLT x5 = pow(lh_qj, 2); + FLT x6 = x4 + x5; + FLT x7 = pow(lh_qk, 2); + FLT x8 = sqrt(pow(lh_qw, 2) + x6 + x7); + FLT x9 = lh_qi*lh_qk; + FLT x10 = lh_qj*lh_qw; + FLT x11 = x10 + x9; + FLT x12 = 2*lh_px*x8; + FLT x13 = lh_qj*lh_qk; + FLT x14 = lh_qi*lh_qw; + FLT x15 = x13 - x14; + FLT x16 = 2*lh_py*x8; + FLT x17 = 2*x8; + FLT x18 = x17*x6 - 1; + FLT x19 = obj_qi*obj_qk; + FLT x20 = obj_qj*obj_qw; + FLT x21 = sensor_z*(x19 + x20); + FLT x22 = pow(obj_qi, 2); + FLT x23 = pow(obj_qj, 2); + FLT x24 = x22 + x23; + FLT x25 = pow(obj_qk, 2); + FLT x26 = sqrt(pow(obj_qw, 2) + x24 + x25); + FLT x27 = 2*x26; + FLT x28 = obj_qi*obj_qj; + FLT x29 = obj_qk*obj_qw; + FLT x30 = sensor_y*(x28 - x29); + FLT x31 = x23 + x25; + FLT x32 = obj_px - sensor_x*(x27*x31 - 1) + x21*x27 + x27*x30; + FLT x33 = 2*x32*x8; + FLT x34 = sensor_x*(x28 + x29); + FLT x35 = obj_qj*obj_qk; + FLT x36 = obj_qi*obj_qw; + FLT x37 = sensor_z*(x35 - x36); + FLT x38 = x22 + x25; + FLT x39 = obj_py - sensor_y*(x27*x38 - 1) + x27*x34 + x27*x37; + FLT x40 = 2*x39*x8; + FLT x41 = sensor_y*(x35 + x36); + FLT x42 = sensor_x*(x19 - x20); + FLT x43 = obj_pz - sensor_z*(x24*x27 - 1) + x27*x41 + x27*x42; + FLT x44 = lh_pz*x18 - x11*x12 + x11*x33 - x15*x16 + x15*x40 - x18*x43; + FLT x45 = 1.0/x44; + FLT x46 = 2*x45*x8; + FLT x47 = x13 + x14; + FLT x48 = 2*lh_pz*x8; + FLT x49 = x17*(x4 + x7) - 1; + FLT x50 = 2*x43*x8; + FLT x51 = lh_py*x49 - x12*x3 + x3*x33 - x39*x49 - x47*x48 + x47*x50; + FLT x52 = pow(x44, -2); + FLT x53 = 4*curve_0*curve_scale*x51*x52*x8; + FLT x54 = x11*x52; + FLT x55 = pow(x51, 2); + FLT x56 = curve_0*x55; + FLT x57 = pow(x44, -3); + FLT x58 = 4*curve_scale*x11*x57*x8; + FLT x59 = x1 + x2; + FLT x60 = -x10 + x9; + FLT x61 = x17*(x5 + x7) - 1; + FLT x62 = lh_px*x61 - x16*x59 - x32*x61 + x40*x59 - x48*x60 + x50*x60; + FLT x63 = pow(x62, 2); + FLT x64 = 1.0/(x52*x63 + 1); + FLT x65 = x45*x61; + FLT x66 = 2*x54*x62*x8; + FLT x67 = x64*(x65 + x66); + FLT x68 = gibMag_0*gib_scale*cos(gibPhase_0 + atan(x45*x62)); + FLT x69 = x45*x49; + FLT x70 = 2*x15*x8; + FLT x71 = x51*x52; + FLT x72 = x70*x71; + FLT x73 = 4*curve_scale*x15*x57*x8; + FLT x74 = 2*x64; + FLT x75 = x45*x8; + FLT x76 = x74*(-x15*x52*x62*x8 + x59*x75); + FLT x77 = x46*x47; + FLT x78 = x18*x71; + FLT x79 = 2*curve_scale*x18*x57; + FLT x80 = x46*x60; + FLT x81 = x52*x62; + FLT x82 = x18*x81; + FLT x83 = x64*(x80 + x82); + FLT x84 = 2*x0; + FLT x85 = obj_qj*x26; + FLT x86 = sensor_x*x85; + FLT x87 = obj_qi*x26; + FLT x88 = sensor_y*x87; + FLT x89 = sensor_z*x24; + FLT x90 = 1.0/x26; + FLT x91 = obj_qw*x90; + FLT x92 = -x41*x91 - x42*x91 + x86 - x88 + x89*x91; + FLT x93 = 2*x8*x92; + FLT x94 = obj_qk*x26; + FLT x95 = sensor_y*x94; + FLT x96 = sensor_z*x85; + FLT x97 = sensor_x*x31; + FLT x98 = -x21*x91 - x30*x91 + x91*x97 + x95 - x96; + FLT x99 = 2*x8*x98; + FLT x100 = sensor_x*x94; + FLT x101 = sensor_z*x87; + FLT x102 = sensor_y*x38; + FLT x103 = x100 - x101 - x102*x91 + x34*x91 + x37*x91; + FLT x104 = x103*x49 + x3*x99 + x47*x93; + FLT x105 = x104*x45; + FLT x106 = 4*curve_0*curve_scale*x51*x52; + FLT x107 = -x103*x70 + x11*x99 - x18*x92; + FLT x108 = x107*x71; + FLT x109 = 4*curve_scale*x107*x57; + FLT x110 = 2*x59*x8; + FLT x111 = x103*x110 - x60*x93 + x61*x98; + FLT x112 = x74*(x107*x81 + x111*x45); + FLT x113 = sensor_y*x85; + FLT x114 = sensor_z*x94; + FLT x115 = obj_qi*x90; + FLT x116 = -x113 - x114 - x115*x21 - x115*x30 + x115*x97; + FLT x117 = 2*x116*x8; + FLT x118 = obj_qw*x26; + FLT x119 = sensor_y*x118; + FLT x120 = obj_qi*x27; + FLT x121 = -sensor_z*(x115*x24 + x120) + x100 + x115*x41 + x115*x42 + x119; + FLT x122 = 2*x121*x8; + FLT x123 = sensor_z*x118; + FLT x124 = -sensor_y*(x115*x38 + x120) + x115*x34 + x115*x37 - x123 + x86; + FLT x125 = x117*x3 - x122*x47 + x124*x49; + FLT x126 = x125*x45; + FLT x127 = x11*x117 + x121*x18 - x124*x70; + FLT x128 = x127*x71; + FLT x129 = 4*curve_0*curve_scale*x55*x57; + FLT x130 = x110*x124 + x116*x61 + x122*x60; + FLT x131 = x74*(x127*x81 + x130*x45); + FLT x132 = sensor_x*x87; + FLT x133 = obj_qj*x90; + FLT x134 = -x102*x133 + x114 + x132 + x133*x34 + x133*x37; + FLT x135 = obj_qj*x27; + FLT x136 = -sensor_x*(x133*x31 + x135) + x123 + x133*x21 + x133*x30 + x88; + FLT x137 = 2*x136*x8; + FLT x138 = sensor_x*x118; + FLT x139 = -sensor_z*(x133*x24 + x135) + x133*x41 + x133*x42 - x138 + x95; + FLT x140 = 2*x139*x8; + FLT x141 = -x134*x49 + x137*x3 + x140*x47; + FLT x142 = x141*x45; + FLT x143 = x11*x137 + x134*x70 - x139*x18; + FLT x144 = x143*x71; + FLT x145 = x110*x134 - x136*x61 + x140*x60; + FLT x146 = x74*(-x143*x81 + x145*x45); + FLT x147 = obj_qk*x90; + FLT x148 = x113 + x132 + x147*x41 + x147*x42 - x147*x89; + FLT x149 = 2*x148*x8; + FLT x150 = obj_qk*x27; + FLT x151 = -sensor_x*(x147*x31 + x150) + x101 - x119 + x147*x21 + x147*x30; + FLT x152 = 2*x151*x8; + FLT x153 = -sensor_y*(x147*x38 + x150) + x138 + x147*x34 + x147*x37 + x96; + FLT x154 = x149*x47 + x152*x3 - x153*x49; + FLT x155 = x154*x45; + FLT x156 = x11*x152 - x148*x18 + x153*x70; + FLT x157 = x156*x71; + FLT x158 = x110*x153 + x149*x60 - x151*x61; + FLT x159 = x74*(-x156*x81 + x158*x45); + FLT x160 = tan(tilt_1*tilt_scale); + FLT x161 = curve_1*x63; + FLT x162 = 1.0/(x52*x55 + 1); + FLT x163 = 2*x162; + FLT x164 = x163*(x3*x75 - x51*x54*x8); + FLT x165 = gibMag_1*gib_scale*cos(gibPhase_1 - atan(x45*x51)); + FLT x166 = 4*curve_1*curve_scale*x52*x62*x8; + FLT x167 = x162*(x69 + x72); + FLT x168 = x162*(x77 + x78); + FLT x169 = 2*x160*x45; + FLT x170 = 4*curve_1*curve_scale*x52*x62; + FLT x171 = 2*x160*x52*x62; + FLT x172 = x163*(-x105 + x108); + FLT x173 = 4*curve_1*curve_scale*x57*x63; + FLT x174 = x163*(-x126 + x128); + FLT x175 = x163*(x142 - x144); + FLT x176 = x163*(x155 - x157); + *(out++) = x0*x3*x46 - 2*x0*x51*x54*x8 - x3*x53 + x56*x58 + x67*x68 - x67; + *(out++) = 2*curve_0*curve_scale*x49*x51*x52 - x0*x69 - x0*x72 + x56*x73 - x68*x76 + x76; + *(out++) = x0*x77 + x0*x78 - x47*x53 - x56*x79 - x68*x83 + x83; + *(out++) = x104*x106 - x105*x84 + x108*x84 - x109*x56 - x112*x68 + x112; + *(out++) = x106*x125 - x126*x84 - x127*x129 + x128*x84 - x131*x68 + x131; + *(out++) = -x106*x141 + x129*x143 + x142*x84 - x144*x84 - x146*x68 + x146; + *(out++) = -x106*x154 + x129*x156 + x155*x84 - x157*x84 - x159*x68 + x159; + *(out++) = 2*curve_1*curve_scale*x52*x61*x62 + x160*x65 + x160*x66 + x161*x58 + x164*x165 - x164; + *(out++) = -x110*x160*x45 + x160*x52*x62*x70 + x161*x73 - x165*x167 - x166*x59 + x167; + *(out++) = -x160*x80 - x160*x82 - x161*x79 + x165*x168 - x166*x60 - x168; + *(out++) = -x107*x171 - x109*x161 - x111*x169 - x111*x170 + x165*x172 - x172; + *(out++) = -x127*x171 - x127*x173 - x130*x169 - x130*x170 + x165*x174 - x174; + *(out++) = x143*x171 + x143*x173 - x145*x169 - x145*x170 + x165*x175 - x175; + *(out++) = x156*x171 + x156*x173 - x158*x169 - x158*x170 + x165*x176 - x176; +} + diff --git a/tools/generate_reprojection_functions/Makefile b/tools/generate_reprojection_functions/Makefile new file mode 100644 index 0000000..79d05cb --- /dev/null +++ b/tools/generate_reprojection_functions/Makefile @@ -0,0 +1,20 @@ +all : check_generated + +SRT:=../.. + +LIBSURVIVE:=$(SRT)/lib/libsurvive.so + +CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O3 -g -DFLT=double -DUSE_DOUBLE # -fsanitize=address -fsanitize=undefined + +check_generated: check_generated.c ../../src/survive_reproject.generated.h survive_reproject.full.generated.h $(LIBSURVIVE) + cd ../..;make + gcc $(CFLAGS) -o $@ $^ $(LDFLAGS) -lm -lc -lgcc + +clean : + rm -rf check_generated + +../../src/survive_reproject.generated.h: reprojection_functions.sage + sage reprojection_functions.sage > ../../src/survive_reproject.generated.h + +survive_reproject.full.generated.h: reprojection_functions.sage + sage reprojection_functions.sage --full > survive_reproject.full.generated.h diff --git a/tools/generate_reprojection_functions/check_generated.c b/tools/generate_reprojection_functions/check_generated.c new file mode 100644 index 0000000..a139651 --- /dev/null +++ b/tools/generate_reprojection_functions/check_generated.c @@ -0,0 +1,128 @@ +#include "survive_reproject.full.generated.h" +#include +#include +#include +#include + + +void gen_survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config) { + FLT phase_scale = config->use_flag & SVCal_Phase ? config->phase_scale : 0.; + FLT phase_0 = bsd->fcal.phase[0]; + FLT phase_1 = bsd->fcal.phase[1]; + + FLT tilt_scale = config->use_flag & SVCal_Tilt ? config->tilt_scale : 0.; + FLT tilt_0 = bsd->fcal.tilt[0]; + FLT tilt_1 = bsd->fcal.tilt[1]; + + FLT curve_scale = config->use_flag & SVCal_Curve ? config->curve_scale : 0.; + FLT curve_0 = bsd->fcal.curve[0]; + FLT curve_1 = bsd->fcal.curve[1]; + + FLT gib_scale = config->use_flag & SVCal_Gib ? config->gib_scale : 0; + FLT gibPhase_0 = bsd->fcal.gibpha[0]; + FLT gibPhase_1 = bsd->fcal.gibpha[1]; + FLT gibMag_0 = bsd->fcal.gibmag[0]; + FLT gibMag_1 = bsd->fcal.gibmag[1]; + + gen_reproject(out, obj_pose->Pos, obj_pt, lh2world->Pos, phase_scale, phase_0, phase_1, tilt_scale, tilt_0, tilt_1, + curve_scale, curve_0, curve_1, gib_scale, gibPhase_0, gibPhase_1, gibMag_0, gibMag_1); +} + +double next_rand(double mx) { return (float)rand() / (float)(RAND_MAX / mx) - mx / 2.; } + +SurvivePose random_pose() { + SurvivePose rtn = {.Pos = {next_rand(10), next_rand(10), next_rand(10)}, + .Rot = {next_rand(1), next_rand(1), next_rand(1), next_rand(1)}}; + + quatnormalize(rtn.Rot, rtn.Rot); + return rtn; +} + +void random_point(FLT *out) { + out[0] = next_rand(10); + out[1] = next_rand(10); + out[2] = next_rand(10); +} + +void print_pose(const SurvivePose *pose) { + printf("[%f %f %f] [%f %f %f %f]\n", pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], + pose->Rot[2], pose->Rot[3]); +} + +void check_rotate_vector() { + SurvivePose obj = random_pose(); + FLT pt[3]; + random_point(pt); + + int cycles = 1000000000; + FLT gen_out[3], out[3]; + double start, stop; + start = OGGetAbsoluteTime(); + for (int i = 0; i < cycles; i++) { + gen_quat_rotate_vector(gen_out, obj.Rot, pt); + } + stop = OGGetAbsoluteTime(); + printf("gen: %f %f %f (%f)\n", gen_out[0], gen_out[1], gen_out[2], stop - start); + + start = OGGetAbsoluteTime(); + for (int i = 0; i < cycles; i++) { + quatrotatevector(out, obj.Rot, pt); + } + stop = OGGetAbsoluteTime(); + + printf("%f %f %f (%f)\n", out[0], out[1], out[2], stop - start); +} + +void check_invert() { + SurvivePose obj = random_pose(); + SurvivePose gen_inv, inv; + gen_invert_pose(gen_inv.Pos, obj.Pos); + InvertPose(&inv, &obj); + + print_pose(&gen_inv); + print_pose(&inv); +} + +void check_reproject() { + SurvivePose obj = random_pose(); + LinmathVec3d pt; + random_point(pt); + SurvivePose lh = random_pose(); + + survive_calibration_config config; + BaseStationData bsd; + for (int i = 0; i < 10; i++) + *((FLT *)&bsd.fcal.phase[0] + i) = next_rand(1); + + for (int i = 0; i < 4; i++) + *((FLT *)&config.phase_scale + i) = next_rand(1); + + config.use_flag = (enum SurviveCalFlag)0xff; + FLT out_pt[2] = {0}; + int cycles = 10000000; + + double start_gen = OGGetAbsoluteTime(); + for (int i = 0; i < cycles; i++) { + gen_survive_reproject_full(out_pt, &obj, pt, &lh, &bsd, &config); + } + double stop_gen = OGGetAbsoluteTime(); + printf("gen: %f %f (%f)\n", out_pt[0], out_pt[1], stop_gen - start_gen); + + double start_reproject = OGGetAbsoluteTime(); + for (int i = 0; i < cycles; i++) + survive_reproject_full(out_pt, &obj, pt, &lh, &bsd, &config); + double stop_reproject = OGGetAbsoluteTime(); + + printf("%f %f (%f)\n", out_pt[0], out_pt[1], stop_reproject - start_reproject); + out_pt[0] = out_pt[1] = 0; +} + +int main() { + check_rotate_vector(); + check_invert(); + check_reproject(); + + return 0; +} diff --git a/tools/generate_reprojection_functions/reprojection_functions.sage b/tools/generate_reprojection_functions/reprojection_functions.sage new file mode 100644 index 0000000..1ff5c25 --- /dev/null +++ b/tools/generate_reprojection_functions/reprojection_functions.sage @@ -0,0 +1,131 @@ +# -*- python -*- +from sympy.utilities.codegen import codegen +from sympy.printing import print_ccode +from sympy import cse, sqrt, sin, pprint, ccode +import types +import sys + +obj_qw,obj_qi,obj_qj,obj_qk=var('obj_qw,obj_qi,obj_qj,obj_qk') +obj_px,obj_py,obj_pz=var('obj_px,obj_py,obj_pz') + +lh_qw,lh_qi,lh_qj,lh_qk=var('lh_qw,lh_qi,lh_qj,lh_qk') +lh_px,lh_py,lh_pz=var('lh_px,lh_py,lh_pz') + +sensor_x,sensor_y,sensor_z=var('sensor_x,sensor_y,sensor_z') + +phase_scale=var('phase_scale') +tilt_scale=var('tilt_scale') +curve_scale=var('curve_scale') +gib_scale=var('gib_scale') + +phase_0,phase_1=var('phase_0, phase_1') +tilt_0,tilt_1=var('tilt_0, tilt_1') +curve_0,curve_1=var('curve_0, curve_1') +gibPhase_0,gibPhase_1=var('gibPhase_0, gibPhase_1') +gibMag_0,gibMag_1=var('gibMag_0, gibMag_1') + +def quatmagnitude(q): + qw,qi,qj,qk = q + return sqrt(qw*qw+qi*qi+qj*qj+qk*qk) + +def quatrotationmatrix(q): + qw,qi,qj,qk = q + s = quatmagnitude(q) + return matrix(SR, + [ [ 1 - 2 * s * (qj*qj + qk*qk), 2 * s*(qi*qj - qk*qw), 2*s*(qi*qk + qj*qw)], + [ 2*s*(qi*qj + qk*qw), 1 - 2*s*(qi*qi+qk*qk), 2*s*(qj*qk-qi*qw)], + [ 2*s*(qi*qk-qj*qw), 2*s*(qj*qk+qi*qw), 1-2*s*(qi*qi+qj*qj)] + ]) + +def quatrotatevector(q, pt): + qw,qi,qj,qk = q + x,y,z = pt + return quatrotationmatrix(q) * vector((x,y,z)) + +def quatgetreciprocal(q): + return [ q[0], -q[1], -q[2], -q[3] ] + +def apply_pose_to_pt(p, pt): + px,py,pz = p[0] + return quatrotatevector(p[1], pt) + vector((px,py,pz)) + +def invert_pose(p): + r = quatgetreciprocal(p[1]) + return ( -1 * quatrotatevector(r, p[0]), r) + +def reproject(p, pt, + lh_p, + phase_scale, phase_0, phase_1, + tilt_scale, tilt_0, tilt_1, + curve_scale, curve_0, curve_1, + gib_scale, gibPhase_0, gibPhase_1, gibMag_0, gibMag_1): + pt_in_world = apply_pose_to_pt( p, pt ) + pt_in_lh = apply_pose_to_pt( invert_pose(lh_p), pt_in_world) + xy = vector((pt_in_lh[0] / pt_in_lh[2], pt_in_lh[1] / -pt_in_lh[2])) + ang = vector((atan(xy[0]), atan(xy[1]))) + + return vector(( + ang[0] - phase_scale * phase_0 - tan(tilt_scale * tilt_0) * xy[1] - curve_scale * curve_0 * xy[1] * xy[1] - gib_scale * sin(gibPhase_0 + ang[0]) * gibMag_0, + ang[1] - phase_scale * phase_1 - tan(tilt_scale * tilt_1) * xy[0] - curve_scale * curve_1 * xy[0] * xy[0] - gib_scale * sin(gibPhase_1 + ang[1]) * gibMag_1 + )) + +obj_rot = (obj_qw,obj_qi,obj_qj,obj_qk) +obj_p = ((obj_px, obj_py, obj_pz), (obj_qw,obj_qi,obj_qj,obj_qk)) + +lh_p = ((lh_px, lh_py, lh_pz), (lh_qw,lh_qi,lh_qj,lh_qk)) +sensor_pt = (sensor_x,sensor_y,sensor_z) +#print( quatrotationmatrix(obj_rot) ) + +reproject_params = (obj_p, sensor_pt, lh_p, phase_scale, phase_0, phase_1, + tilt_scale, tilt_0, tilt_1, + curve_scale, curve_0, curve_1, + gib_scale, gibPhase_0, gibPhase_1, gibMag_0, gibMag_1) + +def flatten_args(bla): + output = [] + for item in bla: + output += flatten_args(item) if hasattr (item, "__iter__") else [item] + return output + +def generate_ccode(name, args, expressions): + flatten = [] + if isinstance(expressions, types.FunctionType): + expressions = expressions(*args) + + for col in expressions: + if hasattr(col, '_sympy_'): + flatten.append(col._sympy_()) + else: + for cell in col: + flatten.append(cell._sympy_()) + + cse_output = cse( flatten ) + cnt = 0 + arg_str = lambda (idx, a): ("const FLT *%s" % str(flatten_args(a)[0]).split('_', 1)[0] ) if isinstance(a, tuple) else ("FLT " + str(a)) + print("static inline void gen_%s(FLT* out, %s) {" % (name, ", ".join( map(arg_str, enumerate(args)) ))) + + for idx, a in enumerate(args): + if isinstance(a, tuple): + name = str(flatten_args(a)[0]).split('_', 1)[0] + for v in flatten_args(a): + print("\tFLT %s = *(%s++);" % (str(v), name)) + + for item in cse_output[0]: + if isinstance(item, tuple): + print("\tFLT %s = %s;" % (ccode(item[0]), ccode(item[1]))) + for item in cse_output[1]: + print("\t*(out++) = %s;" % ccode(item)) + print "}" + print "" + +#print(min_form) + +print(" // NOTE: Auto-generated code; see tools/generate_reprojection_functions ") +print("#include ") + +if len(sys.argv) > 1 and sys.argv[1] == "--full": + generate_ccode("quat_rotate_vector", [obj_rot, sensor_pt], quatrotatevector) + generate_ccode("invert_pose", [obj_p], invert_pose) + generate_ccode("reproject", reproject_params, reproject) + +generate_ccode("reproject_jac_obj_p", reproject_params, jacobian(reproject(*reproject_params), (obj_px, obj_py, obj_pz, obj_qw,obj_qi,obj_qj,obj_qk))) -- cgit v1.2.3 From 538b7c32bfbd6460c61a080c2697365934b81938 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 16:16:14 -0600 Subject: Made picking the seed poser use cmd line --- src/poser_sba.c | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/poser_sba.c b/src/poser_sba.c index f101018..1f79986 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -59,6 +59,8 @@ typedef struct SBAData { } stats; SurviveObject *so; + + PoserCB seed_poser; } SBAData; static void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -241,12 +243,11 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive SurvivePose soLocation = so->OutPose; bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; - + static bool seed_warning = false; if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { SurviveContext *ctx = so->ctx; // SV_INFO("Must rerun seed poser"); - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); + PoserCB driver = d->seed_poser; if (driver) { PoserData hdr = pdl->hdr; @@ -267,7 +268,8 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } d->successes_to_reset_cntr = d->successes_to_reset; - } else { + } else if (seed_warning == false) { + seed_warning = true; SV_INFO("Not using a seed poser for SBA; results will likely be way off"); } } @@ -335,7 +337,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, +static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; @@ -347,11 +349,9 @@ static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_itera .obj_pose = so->OutPose}; { - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); + PoserCB driver = d->seed_poser; SurviveContext *ctx = so->ctx; if (driver) { - SV_INFO("Using %s seed poser for SBA", subposer); PoserData hdr = pdfs->hdr; memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions pdfs->hdr.pt = hdr.pt; @@ -441,6 +441,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); d->so = so; + const char *subposer = survive_configs(ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); + d->seed_poser = (PoserCB)GetDriver(subposer); + SV_INFO("Initializing SBA:"); SV_INFO("\tsba-required-meas: %d", d->required_meas); SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance); @@ -450,6 +453,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SV_INFO("\tsba-successes-to-reset: %d", d->successes_to_reset); SV_INFO("\tsba-use-imu: %d", d->useIMU); SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function); + SV_INFO("\tsba-seed-poser: %s(%p)", subposer, d->seed_poser); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -495,7 +499,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - double error = run_sba(pdfs, so, 100, .005); + double error = run_sba(d, pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } @@ -518,10 +522,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } } // INTENTIONAL FALLTHROUGH default: { - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); - if (driver) { - return driver(so, pd); + if (d->seed_poser) { + return d->seed_poser(so, pd); } break; } -- cgit v1.2.3 From 9601f596fe545692ed39f939cf846f9883ea188c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 16:26:41 -0600 Subject: Revered what config poser was doing for SBA --- src/poser_sba.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/poser_sba.c b/src/poser_sba.c index 1f79986..347b600 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -337,7 +337,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, +static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; @@ -349,7 +349,9 @@ static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, i .obj_pose = so->OutPose}; { - PoserCB driver = d->seed_poser; + const char *subposer = survive_configs(so->ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); + + PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; if (driver) { PoserData hdr = pdfs->hdr; @@ -499,7 +501,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - double error = run_sba(d, pdfs, so, 100, .005); + double error = run_sba(pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } -- cgit v1.2.3 From 6363d21d6bfd60564830c410ea75f96b3bc1f129 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 16:46:22 -0600 Subject: Optimizations because vc++ is dumb --- src/survive_reproject.generated.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/survive_reproject.generated.h b/src/survive_reproject.generated.h index f4b499a..e184775 100644 --- a/src/survive_reproject.generated.h +++ b/src/survive_reproject.generated.h @@ -22,11 +22,11 @@ static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT * FLT x1 = lh_qi*lh_qj; FLT x2 = lh_qk*lh_qw; FLT x3 = x1 - x2; - FLT x4 = pow(lh_qi, 2); - FLT x5 = pow(lh_qj, 2); + FLT x4 = (lh_qi * lh_qi); + FLT x5 = (lh_qj * lh_qj); FLT x6 = x4 + x5; - FLT x7 = pow(lh_qk, 2); - FLT x8 = sqrt(pow(lh_qw, 2) + x6 + x7); + FLT x7 = (lh_qk * lh_qk); + FLT x8 = sqrt((lh_qw * lh_qw) + x6 + x7); FLT x9 = lh_qi*lh_qk; FLT x10 = lh_qj*lh_qw; FLT x11 = x10 + x9; @@ -40,11 +40,11 @@ static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT * FLT x19 = obj_qi*obj_qk; FLT x20 = obj_qj*obj_qw; FLT x21 = sensor_z*(x19 + x20); - FLT x22 = pow(obj_qi, 2); - FLT x23 = pow(obj_qj, 2); + FLT x22 = (obj_qi * obj_qi); + FLT x23 = (obj_qj * obj_qj); FLT x24 = x22 + x23; - FLT x25 = pow(obj_qk, 2); - FLT x26 = sqrt(pow(obj_qw, 2) + x24 + x25); + FLT x25 = (obj_qk * obj_qk); + FLT x26 = sqrt((obj_qw * obj_qw) + x24 + x25); FLT x27 = 2*x26; FLT x28 = obj_qi*obj_qj; FLT x29 = obj_qk*obj_qw; @@ -70,18 +70,18 @@ static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT * FLT x49 = x17*(x4 + x7) - 1; FLT x50 = 2*x43*x8; FLT x51 = lh_py*x49 - x12*x3 + x3*x33 - x39*x49 - x47*x48 + x47*x50; - FLT x52 = pow(x44, -2); + FLT x52 = 1. / (x44 * x44); FLT x53 = 4*curve_0*curve_scale*x51*x52*x8; FLT x54 = x11*x52; - FLT x55 = pow(x51, 2); + FLT x55 = (x51*x51); FLT x56 = curve_0*x55; - FLT x57 = pow(x44, -3); + FLT x57 = 1. / (x44*x44*x44); FLT x58 = 4*curve_scale*x11*x57*x8; FLT x59 = x1 + x2; FLT x60 = -x10 + x9; FLT x61 = x17*(x5 + x7) - 1; FLT x62 = lh_px*x61 - x16*x59 - x32*x61 + x40*x59 - x48*x60 + x50*x60; - FLT x63 = pow(x62, 2); + FLT x63 = (x62 * x62); FLT x64 = 1.0/(x52*x63 + 1); FLT x65 = x45*x61; FLT x66 = 2*x54*x62*x8; -- cgit v1.2.3 From e06e452445bdd3d61f393bde4206cd848a54444b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 22:51:57 -0600 Subject: Created common base for optimizer based posers --- Makefile | 4 +- src/poser_epnp.c | 2 +- src/poser_general_optimizer.c | 100 ++++++++++++++++++++++++++++++++++++++++ src/poser_general_optimizer.h | 29 ++++++++++++ src/poser_sba.c | 103 ++++++++---------------------------------- 5 files changed, 150 insertions(+), 88 deletions(-) create mode 100644 src/poser_general_optimizer.c create mode 100644 src/poser_general_optimizer.h diff --git a/Makefile b/Makefile index 48d5a98..5a4ec75 100644 --- a/Makefile +++ b/Makefile @@ -2,7 +2,7 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test CC?=gcc -CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic #-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses +CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic #-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses CFLAGS_RELEASE:=-Iinclude/libsurvive -fPIC -msse2 -ftree-vectorize -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm @@ -40,7 +40,7 @@ ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o src/poser_general_optimizer.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 7e922e7..615c01c 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { case POSERDATA_LIGHT: { PoserDataLight *lightData = (PoserDataLight *)pd; - SurvivePose posers[2]; + SurvivePose posers[2] = {}; int meas[2] = {0, 0}; for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { if (so->ctx->bsd[lh].PositionSet) { diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c new file mode 100644 index 0000000..313e378 --- /dev/null +++ b/src/poser_general_optimizer.c @@ -0,0 +1,100 @@ +#include "poser_general_optimizer.h" +#include "string.h" +#include +#include +#include + +void *GetDriver(const char *name); +void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) { + memset(d, 0, sizeof(*d)); + d->so = so; + + SurviveContext *ctx = so->ctx; + + d->failures_to_reset = survive_configi(ctx, "failures-to-reset", SC_GET, 1); + d->successes_to_reset = survive_configi(ctx, "successes-to-reset", SC_GET, -1); + d->max_error = survive_configf(ctx, "max-error", SC_GET, .0001); + + const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, "PoserEPNP"); + d->seed_poser = (PoserCB)GetDriver(subposer); + + SV_INFO("Initializing general optimizer:"); + SV_INFO("\tmax-error: %f", d->max_error); + SV_INFO("\tsuccesses-to-reset: %d", d->successes_to_reset); + SV_INFO("\tfailures-to-reset: %d", d->failures_to_reset); + SV_INFO("\tseed-poser: %s(%p)", subposer, d->seed_poser); +} +void general_optimizer_data_record_failure(GeneralOptimizerData *d) { + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; +} +bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error) { + d->stats.runs++; + if (d->max_error > error) { + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; + d->failures_to_reset_cntr = d->failures_to_reset; + return true; + } + return false; +} + +typedef struct { + bool hasInfo; + SurvivePose pose; +} set_position_t; + +static void set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) { + set_position_t *user = _user; + assert(user->hasInfo == false); + user->hasInfo = true; + user->pose = *new_pose; +} + +bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *_hdr, size_t len_hdr, + SurvivePose *soLocation) { + *soLocation = d->so->OutPose; + bool currentPositionValid = quatmagnitude(soLocation->Rot) != 0; + static bool seed_warning = false; + if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { + PoserCB driver = d->seed_poser; + SurviveContext *ctx = d->so->ctx; + if (driver) { + + PoserData *hdr = alloca(len_hdr); + memcpy(hdr, _hdr, len_hdr); + memset(hdr, 0, sizeof(PoserData)); // Clear callback functions + hdr->pt = _hdr->pt; + hdr->poseproc = set_position; + + set_position_t locations = {0}; + hdr->userdata = &locations; + driver(d->so, hdr); + d->stats.poser_seed_runs++; + + if (locations.hasInfo == false) { + return false; + } else if (locations.hasInfo) { + *soLocation = locations.pose; + } + + d->successes_to_reset_cntr = d->successes_to_reset; + } else if (seed_warning == false) { + seed_warning = true; + SV_INFO("Not using a seed poser for SBA; results will likely be way off"); + } + } + return true; +} + +void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu) { + if (d->seed_poser) { + d->seed_poser(d->so, &imu->hdr); + } +} + +void general_optimizer_data_dtor(GeneralOptimizerData *d) { + SurviveContext *ctx = d->so->ctx; + SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs); + SV_INFO("\terror failures %d", d->stats.error_failures); +} diff --git a/src/poser_general_optimizer.h b/src/poser_general_optimizer.h new file mode 100644 index 0000000..81d94bf --- /dev/null +++ b/src/poser_general_optimizer.h @@ -0,0 +1,29 @@ +#include +#include + +typedef struct GeneralOptimizerData { + int failures_to_reset; + int failures_to_reset_cntr; + int successes_to_reset; + int successes_to_reset_cntr; + + FLT max_error; + + struct { + int runs; + int poser_seed_runs; + int error_failures; + } stats; + + PoserCB seed_poser; + SurviveObject *so; +} GeneralOptimizerData; + +void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so); +void general_optimizer_data_dtor(GeneralOptimizerData *d); + +void general_optimizer_data_record_failure(GeneralOptimizerData *d); +bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error); +void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu); +bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *hdr, size_t len_hdr, + SurvivePose *p); diff --git a/src/poser_sba.c b/src/poser_sba.c index 347b600..4c7fcd1 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -13,6 +13,7 @@ #include "assert.h" #include "linmath.h" #include "math.h" +#include "poser_general_optimizer.h" #include "string.h" #include "survive_cal.h" #include "survive_config.h" @@ -32,16 +33,11 @@ typedef struct { } sba_context_single_sweep; typedef struct SBAData { + GeneralOptimizerData opt; + int last_acode; int last_lh; - int failures_to_reset; - int failures_to_reset_cntr; - int successes_to_reset; - int successes_to_reset_cntr; - - FLT max_error; - FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; @@ -52,15 +48,8 @@ typedef struct SBAData { bool useIMU; struct { - int runs; - int poser_seed_runs; int meas_failures; - int error_failures; } stats; - - SurviveObject *so; - - PoserCB seed_poser; } SBAData; static void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -98,7 +87,7 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask, double *meas, double *cov) { size_t rtn = 0; - SurviveObject *so = d->so; + SurviveObject *so = d->opt.so; for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { @@ -215,7 +204,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/, SurvivePose *out) { double *covx = 0; - SurviveObject *so = d->so; + SurviveObject *so = d->opt.so; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); @@ -241,37 +230,10 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } failure_count = 0; - SurvivePose soLocation = so->OutPose; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; - static bool seed_warning = false; - if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { - SurviveContext *ctx = so->ctx; - // SV_INFO("Must rerun seed poser"); - PoserCB driver = d->seed_poser; + SurvivePose soLocation = {}; - if (driver) { - PoserData hdr = pdl->hdr; - memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions - pdl->hdr.pt = hdr.pt; - pdl->hdr.poseproc = sba_set_position; - - sba_set_position_t locations = {0}; - pdl->hdr.userdata = &locations; - driver(so, &pdl->hdr); - pdl->hdr = hdr; - d->stats.poser_seed_runs++; - - if (locations.hasInfo == false) { - return -1; - } else if (locations.hasInfo) { - soLocation = locations.poses; - } - - d->successes_to_reset_cntr = d->successes_to_reset; - } else if (seed_warning == false) { - seed_warning = true; - SV_INFO("Not using a seed poser for SBA; results will likely be way off"); - } + if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { + return -1; } double opts[SBA_OPTSSZ] = {0}; @@ -286,7 +248,6 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); opts[4] = 0.0; - d->stats.runs++; int status = sba_str_levmar(1, // Number of 3d points 0, // Number of 3d points to fix in spot NUM_LIGHTHOUSES * so->sensor_ct, vmask, @@ -303,26 +264,14 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive opts, // options info); // info - if (currentPositionValid) { - // FLT distp[3]; - // sub3d(distp, so->OutPose.Pos, soLocation.Pos); - // FLT distance = magnitude3d(distp); - - // if (distance > 1.) - // status = -1; - } - double rtn = -1; bool status_failure = status <= 0; - bool error_failure = (info[1] / meas_size * 2) >= d->max_error; + bool error_failure = !general_optimizer_data_record_success(&d->opt, (info[1] / meas_size * 2)); if (!status_failure && !error_failure) { - d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; - } else if (error_failure) { - d->stats.error_failures++; - } + } else { SurviveContext *ctx = so->ctx; @@ -349,7 +298,7 @@ static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_itera .obj_pose = so->OutPose}; { - const char *subposer = survive_configs(so->ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); + const char *subposer = survive_configs(so->ctx, "seed-poser", SC_GET, "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; @@ -429,33 +378,25 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (so->PoserData == 0) { so->PoserData = calloc(1, sizeof(SBAData)); SBAData *d = so->PoserData; - d->failures_to_reset_cntr = 0; - d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); - d->successes_to_reset_cntr = 0; - d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, -1); + + general_optimizer_data_init(&d->opt, so); d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1); d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); - d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); + d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); - d->so = so; - const char *subposer = survive_configs(ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); - d->seed_poser = (PoserCB)GetDriver(subposer); SV_INFO("Initializing SBA:"); SV_INFO("\tsba-required-meas: %d", d->required_meas); SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance); 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); SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function); - SV_INFO("\tsba-seed-poser: %s(%p)", subposer, d->seed_poser); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -477,8 +418,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (error < 0) { - if (d->failures_to_reset_cntr > 0) - d->failures_to_reset_cntr--; + } else { if (d->useIMU) { @@ -492,8 +432,6 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } PoserData_poser_pose_func(&lightData->hdr, so, &estimate); - if (d->successes_to_reset_cntr > 0) - d->successes_to_reset_cntr--; } } return 0; @@ -507,9 +445,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } case POSERDATA_DISASSOCIATE: { SV_INFO("SBA stats:"); - SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs); SV_INFO("\tmeas failures %d", d->stats.meas_failures); - SV_INFO("\terror failures %d", d->stats.error_failures); + general_optimizer_data_dtor(&d->opt); free(d); so->PoserData = 0; return 0; @@ -522,12 +459,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { survive_imu_tracker_integrate(so, &d->tracker, imu); PoserData_poser_pose_func(pd, so, &d->tracker.pose); } - } // INTENTIONAL FALLTHROUGH - default: { - if (d->seed_poser) { - return d->seed_poser(so, pd); - } - break; + + general_optimizer_data_record_imu(&d->opt, imu); } } return -1; -- cgit v1.2.3 From 5985e89c8d7460f17ede727615bad795b3ab2c87 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Apr 2018 09:14:06 -0600 Subject: Added MPFIT based poser --- Makefile | 2 +- redist/mpfit/DISCLAIMER | 77 ++ redist/mpfit/mpfit.c | 2289 +++++++++++++++++++++++++++++++++++++++++++++++ redist/mpfit/mpfit.h | 192 ++++ src/poser_mpfit.c | 290 ++++++ 5 files changed, 2849 insertions(+), 1 deletion(-) create mode 100644 redist/mpfit/DISCLAIMER create mode 100644 redist/mpfit/mpfit.c create mode 100644 redist/mpfit/mpfit.h create mode 100644 src/poser_mpfit.c diff --git a/Makefile b/Makefile index 5a4ec75..cd10496 100644 --- a/Makefile +++ b/Makefile @@ -40,7 +40,7 @@ ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o src/poser_general_optimizer.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o src/poser_general_optimizer.o src/poser_mpfit.o redist/mpfit/mpfit.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/redist/mpfit/DISCLAIMER b/redist/mpfit/DISCLAIMER new file mode 100644 index 0000000..3e1b76f --- /dev/null +++ b/redist/mpfit/DISCLAIMER @@ -0,0 +1,77 @@ + +MPFIT: A MINPACK-1 Least Squares Fitting Library in C + +Original public domain version by B. Garbow, K. Hillstrom, J. More' + (Argonne National Laboratory, MINPACK project, March 1980) + Copyright (1999) University of Chicago + (see below) + +Tranlation to C Language by S. Moshier (moshier.net) + (no restrictions placed on distribution) + +Enhancements and packaging by C. Markwardt + (comparable to IDL fitting routine MPFIT + see http://cow.physics.wisc.edu/~craigm/idl/idl.html) + Copyright (C) 2003, 2004, 2006, 2007 Craig B. Markwardt + + This software is provided as is without any warranty whatsoever. + Permission to use, copy, modify, and distribute modified or + unmodified copies is granted, provided this copyright and disclaimer + are included unchanged. + + +Source code derived from MINPACK must have the following disclaimer +text provided. + +=========================================================================== +Minpack Copyright Notice (1999) University of Chicago. All rights reserved + +Redistribution and use in source and binary forms, with or +without modification, are permitted provided that the +following conditions are met: + +1. Redistributions of source code must retain the above +copyright notice, this list of conditions and the following +disclaimer. + +2. Redistributions in binary form must reproduce the above +copyright notice, this list of conditions and the following +disclaimer in the documentation and/or other materials +provided with the distribution. + +3. The end-user documentation included with the +redistribution, if any, must include the following +acknowledgment: + + "This product includes software developed by the + University of Chicago, as Operator of Argonne National + Laboratory. + +Alternately, this acknowledgment may appear in the software +itself, if and wherever such third-party acknowledgments +normally appear. + +4. WARRANTY DISCLAIMER. THE SOFTWARE IS SUPPLIED "AS IS" +WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT HOLDER, THE +UNITED STATES, THE UNITED STATES DEPARTMENT OF ENERGY, AND +THEIR EMPLOYEES: (1) DISCLAIM ANY WARRANTIES, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO ANY IMPLIED WARRANTIES +OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE +OR NON-INFRINGEMENT, (2) DO NOT ASSUME ANY LEGAL LIABILITY +OR RESPONSIBILITY FOR THE ACCURACY, COMPLETENESS, OR +USEFULNESS OF THE SOFTWARE, (3) DO NOT REPRESENT THAT USE OF +THE SOFTWARE WOULD NOT INFRINGE PRIVATELY OWNED RIGHTS, (4) +DO NOT WARRANT THAT THE SOFTWARE WILL FUNCTION +UNINTERRUPTED, THAT IT IS ERROR-FREE OR THAT ANY ERRORS WILL +BE CORRECTED. + +5. LIMITATION OF LIABILITY. IN NO EVENT WILL THE COPYRIGHT +HOLDER, THE UNITED STATES, THE UNITED STATES DEPARTMENT OF +ENERGY, OR THEIR EMPLOYEES: BE LIABLE FOR ANY INDIRECT, +INCIDENTAL, CONSEQUENTIAL, SPECIAL OR PUNITIVE DAMAGES OF +ANY KIND OR NATURE, INCLUDING BUT NOT LIMITED TO LOSS OF +PROFITS OR LOSS OF DATA, FOR ANY REASON WHATSOEVER, WHETHER +SUCH LIABILITY IS ASSERTED ON THE BASIS OF CONTRACT, TORT +(INCLUDING NEGLIGENCE OR STRICT LIABILITY), OR OTHERWISE, +EVEN IF ANY OF SAID PARTIES HAS BEEN WARNED OF THE +POSSIBILITY OF SUCH LOSS OR DAMAGES. diff --git a/redist/mpfit/mpfit.c b/redist/mpfit/mpfit.c new file mode 100644 index 0000000..1bfc92a --- /dev/null +++ b/redist/mpfit/mpfit.c @@ -0,0 +1,2289 @@ +/* + * MINPACK-1 Least Squares Fitting Library + * + * Original public domain version by B. Garbow, K. Hillstrom, J. More' + * (Argonne National Laboratory, MINPACK project, March 1980) + * See the file DISCLAIMER for copyright information. + * + * Tranlation to C Language by S. Moshier (moshier.net) + * + * Enhancements and packaging by C. Markwardt + * (comparable to IDL fitting routine MPFIT + * see http://cow.physics.wisc.edu/~craigm/idl/idl.html) + */ + +/* Main mpfit library routines (double precision) + $Id: mpfit.c,v 1.24 2013/04/23 18:37:38 craigm Exp $ + */ + +#include "mpfit.h" +#include +#include +#include +#include + +/* Forward declarations of functions in this module */ +static int mp_fdjac2(mp_func funct, int m, int n, int *ifree, int npar, double *x, double *fvec, double *fjac, + int ldfjac, double epsfcn, double *wa, void *priv, int *nfev, double *step, double *dstep, + int *dside, int *qulimited, double *ulimit, int *ddebug, double *ddrtol, double *ddatol, + double *wa2, double **dvecptr); +static void mp_qrfac(int m, int n, double *a, int lda, int pivot, int *ipvt, int lipvt, double *rdiag, double *acnorm, + double *wa); +static void mp_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, + double *wa); +static void mp_lmpar(int n, double *r, int ldr, int *ipvt, int *ifree, double *diag, double *qtb, double delta, + double *par, double *x, double *sdiag, double *wa1, double *wa2); +static double mp_enorm(int n, double *x); +static double mp_dmax1(double a, double b); +static double mp_dmin1(double a, double b); +static int mp_min0(int a, int b); +static int mp_covar(int n, double *r, int ldr, int *ipvt, double tol, double *wa); + +/* Macro to call user function */ +#define mp_call(funct, m, n, x, fvec, dvec, priv) (*(funct))(m, n, x, fvec, dvec, priv) + +/* Macro to safely allocate memory */ +#define mp_malloc(dest, type, size) \ + dest = (type *)malloc(sizeof(type) * size); \ + if (dest == 0) { \ + info = MP_ERR_MEMORY; \ + goto CLEANUP; \ + } else { \ + int _k; \ + for (_k = 0; _k < (size); _k++) \ + dest[_k] = 0; \ + } + +/* +* ********** +* +* subroutine mpfit +* +* the purpose of mpfit is to minimize the sum of the squares of +* m nonlinear functions in n variables by a modification of +* the levenberg-marquardt algorithm. the user must provide a +* subroutine which calculates the functions. the jacobian is +* then calculated by a finite-difference approximation. +* +* mp_funct funct - function to be minimized +* int m - number of data points +* int npar - number of fit parameters +* double *xall - array of n initial parameter values +* upon return, contains adjusted parameter values +* mp_par *pars - array of npar structures specifying constraints; +* or 0 (null pointer) for unconstrained fitting +* [ see README and mpfit.h for definition & use of mp_par] +* mp_config *config - pointer to structure which specifies the +* configuration of mpfit(); or 0 (null pointer) +* if the default configuration is to be used. +* See README and mpfit.h for definition and use +* of config. +* void *private - any private user data which is to be passed directly +* to funct without modification by mpfit(). +* mp_result *result - pointer to structure, which upon return, contains +* the results of the fit. The user should zero this +* structure. If any of the array values are to be +* returned, the user should allocate storage for them +* and assign the corresponding pointer in *result. +* Upon return, *result will be updated, and +* any of the non-null arrays will be filled. +* +* +* FORTRAN DOCUMENTATION BELOW +* +* +* the subroutine statement is +* +* subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, +* diag,mode,factor,nprint,info,nfev,fjac, +* ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4) +* +* where +* +* fcn is the name of the user-supplied subroutine which +* calculates the functions. fcn must be declared +* in an external statement in the user calling +* program, and should be written as follows. +* +* subroutine fcn(m,n,x,fvec,iflag) +* integer m,n,iflag +* double precision x(n),fvec(m) +* ---------- +* calculate the functions at x and +* return this vector in fvec. +* ---------- +* return +* end +* +* the value of iflag should not be changed by fcn unless +* the user wants to terminate execution of lmdif. +* in this case set iflag to a negative integer. +* +* m is a positive integer input variable set to the number +* of functions. +* +* n is a positive integer input variable set to the number +* of variables. n must not exceed m. +* +* x is an array of length n. on input x must contain +* an initial estimate of the solution vector. on output x +* contains the final estimate of the solution vector. +* +* fvec is an output array of length m which contains +* the functions evaluated at the output x. +* +* ftol is a nonnegative input variable. termination +* occurs when both the actual and predicted relative +* reductions in the sum of squares are at most ftol. +* therefore, ftol measures the relative error desired +* in the sum of squares. +* +* xtol is a nonnegative input variable. termination +* occurs when the relative error between two consecutive +* iterates is at most xtol. therefore, xtol measures the +* relative error desired in the approximate solution. +* +* gtol is a nonnegative input variable. termination +* occurs when the cosine of the angle between fvec and +* any column of the jacobian is at most gtol in absolute +* value. therefore, gtol measures the orthogonality +* desired between the function vector and the columns +* of the jacobian. +* +* maxfev is a positive integer input variable. termination +* occurs when the number of calls to fcn is at least +* maxfev by the end of an iteration. +* +* epsfcn is an input variable used in determining a suitable +* step length for the forward-difference approximation. this +* approximation assumes that the relative errors in the +* functions are of the order of epsfcn. if epsfcn is less +* than the machine precision, it is assumed that the relative +* errors in the functions are of the order of the machine +* precision. +* +* diag is an array of length n. if mode = 1 (see +* below), diag is internally set. if mode = 2, diag +* must contain positive entries that serve as +* multiplicative scale factors for the variables. +* +* mode is an integer input variable. if mode = 1, the +* variables will be scaled internally. if mode = 2, +* the scaling is specified by the input diag. other +* values of mode are equivalent to mode = 1. +* +* factor is a positive input variable used in determining the +* initial step bound. this bound is set to the product of +* factor and the euclidean norm of diag*x if nonzero, or else +* to factor itself. in most cases factor should lie in the +* interval (.1,100.). 100. is a generally recommended value. +* +* nprint is an integer input variable that enables controlled +* printing of iterates if it is positive. in this case, +* fcn is called with iflag = 0 at the beginning of the first +* iteration and every nprint iterations thereafter and +* immediately prior to return, with x and fvec available +* for printing. if nprint is not positive, no special calls +* of fcn with iflag = 0 are made. +* +* info is an integer output variable. if the user has +* terminated execution, info is set to the (negative) +* value of iflag. see description of fcn. otherwise, +* info is set as follows. +* +* info = 0 improper input parameters. +* +* info = 1 both actual and predicted relative reductions +* in the sum of squares are at most ftol. +* +* info = 2 relative error between two consecutive iterates +* is at most xtol. +* +* info = 3 conditions for info = 1 and info = 2 both hold. +* +* info = 4 the cosine of the angle between fvec and any +* column of the jacobian is at most gtol in +* absolute value. +* +* info = 5 number of calls to fcn has reached or +* exceeded maxfev. +* +* info = 6 ftol is too small. no further reduction in +* the sum of squares is possible. +* +* info = 7 xtol is too small. no further improvement in +* the approximate solution x is possible. +* +* info = 8 gtol is too small. fvec is orthogonal to the +* columns of the jacobian to machine precision. +* +* nfev is an integer output variable set to the number of +* calls to fcn. +* +* fjac is an output m by n array. the upper n by n submatrix +* of fjac contains an upper triangular matrix r with +* diagonal elements of nonincreasing magnitude such that +* +* t t t +* p *(jac *jac)*p = r *r, +* +* where p is a permutation matrix and jac is the final +* calculated jacobian. column j of p is column ipvt(j) +* (see below) of the identity matrix. the lower trapezoidal +* part of fjac contains information generated during +* the computation of r. +* +* ldfjac is a positive integer input variable not less than m +* which specifies the leading dimension of the array fjac. +* +* ipvt is an integer output array of length n. ipvt +* defines a permutation matrix p such that jac*p = q*r, +* where jac is the final calculated jacobian, q is +* orthogonal (not stored), and r is upper triangular +* with diagonal elements of nonincreasing magnitude. +* column j of p is column ipvt(j) of the identity matrix. +* +* qtf is an output array of length n which contains +* the first n elements of the vector (q transpose)*fvec. +* +* wa1, wa2, and wa3 are work arrays of length n. +* +* wa4 is a work array of length m. +* +* subprograms called +* +* user-supplied ...... fcn +* +* minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac +* +* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod +* +* argonne national laboratory. minpack project. march 1980. +* burton s. garbow, kenneth e. hillstrom, jorge j. more +* +* ********** */ + +int mpfit(mp_func funct, int m, int npar, double *xall, mp_par *pars, mp_config *config, void *private_data, + mp_result *result) { + mp_config conf; + int i, j, info, iflag, nfree, npegged, iter; + int qanylim = 0; + + int ij, jj, l; + double actred, delta, dirder, fnorm, fnorm1, gnorm, orignorm; + double par, pnorm, prered, ratio; + double sum, temp, temp1, temp2, temp3, xnorm, alpha; + static double one = 1.0; + static double p1 = 0.1; + static double p5 = 0.5; + static double p25 = 0.25; + static double p75 = 0.75; + static double p0001 = 1.0e-4; + static double zero = 0.0; + int nfev = 0; + + double *step = 0, *dstep = 0, *llim = 0, *ulim = 0; + int *pfixed = 0, *mpside = 0, *ifree = 0, *qllim = 0, *qulim = 0; + int *ddebug = 0; + double *ddrtol = 0, *ddatol = 0; + + double *fvec = 0, *qtf = 0; + double *x = 0, *xnew = 0, *fjac = 0, *diag = 0; + double *wa1 = 0, *wa2 = 0, *wa3 = 0, *wa4 = 0; + double **dvecptr = 0; + int *ipvt = 0; + + int ldfjac; + + /* Default configuration */ + conf.ftol = 1e-10; + conf.xtol = 1e-10; + conf.gtol = 1e-10; + conf.stepfactor = 100.0; + conf.nprint = 1; + conf.epsfcn = MP_MACHEP0; + conf.maxiter = 200; + conf.douserscale = 0; + conf.maxfev = 0; + conf.covtol = 1e-14; + conf.nofinitecheck = 0; + + if (config) { + /* Transfer any user-specified configurations */ + if (config->ftol > 0) + conf.ftol = config->ftol; + if (config->xtol > 0) + conf.xtol = config->xtol; + if (config->gtol > 0) + conf.gtol = config->gtol; + if (config->stepfactor > 0) + conf.stepfactor = config->stepfactor; + if (config->nprint >= 0) + conf.nprint = config->nprint; + if (config->epsfcn > 0) + conf.epsfcn = config->epsfcn; + if (config->maxiter > 0) + conf.maxiter = config->maxiter; + if (config->maxiter == MP_NO_ITER) + conf.maxiter = 0; + if (config->douserscale != 0) + conf.douserscale = config->douserscale; + if (config->covtol > 0) + conf.covtol = config->covtol; + if (config->nofinitecheck > 0) + conf.nofinitecheck = config->nofinitecheck; + conf.maxfev = config->maxfev; + } + + info = MP_ERR_INPUT; /* = 0 */ + iflag = 0; + nfree = 0; + npegged = 0; + + /* Basic error checking */ + if (funct == 0) { + return MP_ERR_FUNC; + } + + if ((m <= 0) || (xall == 0)) { + return MP_ERR_NPOINTS; + } + + if (npar <= 0) { + return MP_ERR_NFREE; + } + + fnorm = -1.0; + fnorm1 = -1.0; + xnorm = -1.0; + delta = 0.0; + + /* FIXED parameters? */ + mp_malloc(pfixed, int, npar); + if (pars) + for (i = 0; i < npar; i++) { + pfixed[i] = (pars[i].fixed) ? 1 : 0; + } + + /* Finite differencing step, absolute and relative, and sidedness of deriv */ + mp_malloc(step, double, npar); + mp_malloc(dstep, double, npar); + mp_malloc(mpside, int, npar); + mp_malloc(ddebug, int, npar); + mp_malloc(ddrtol, double, npar); + mp_malloc(ddatol, double, npar); + if (pars) + for (i = 0; i < npar; i++) { + step[i] = pars[i].step; + dstep[i] = pars[i].relstep; + mpside[i] = pars[i].side; + ddebug[i] = pars[i].deriv_debug; + ddrtol[i] = pars[i].deriv_reltol; + ddatol[i] = pars[i].deriv_abstol; + } + + /* Finish up the free parameters */ + nfree = 0; + mp_malloc(ifree, int, npar); + for (i = 0, j = 0; i < npar; i++) { + if (pfixed[i] == 0) { + nfree++; + ifree[j++] = i; + } + } + if (nfree == 0) { + info = MP_ERR_NFREE; + goto CLEANUP; + } + + if (pars) { + for (i = 0; i < npar; i++) { + if ((pars[i].limited[0] && (xall[i] < pars[i].limits[0])) || + (pars[i].limited[1] && (xall[i] > pars[i].limits[1]))) { + info = MP_ERR_INITBOUNDS; + goto CLEANUP; + } + if ((pars[i].fixed == 0) && pars[i].limited[0] && pars[i].limited[1] && + (pars[i].limits[0] >= pars[i].limits[1])) { + info = MP_ERR_BOUNDS; + goto CLEANUP; + } + } + + mp_malloc(qulim, int, nfree); + mp_malloc(qllim, int, nfree); + mp_malloc(ulim, double, nfree); + mp_malloc(llim, double, nfree); + + for (i = 0; i < nfree; i++) { + qllim[i] = pars[ifree[i]].limited[0]; + qulim[i] = pars[ifree[i]].limited[1]; + llim[i] = pars[ifree[i]].limits[0]; + ulim[i] = pars[ifree[i]].limits[1]; + if (qllim[i] || qulim[i]) + qanylim = 1; + } + } + + /* Sanity checking on input configuration */ + if ((npar <= 0) || (conf.ftol <= 0) || (conf.xtol <= 0) || (conf.gtol <= 0) || (conf.maxiter < 0) || + (conf.stepfactor <= 0)) { + info = MP_ERR_PARAM; + goto CLEANUP; + } + + /* Ensure there are some degrees of freedom */ + if (m < nfree) { + info = MP_ERR_DOF; + goto CLEANUP; + } + + /* Allocate temporary storage */ + mp_malloc(fvec, double, m); + mp_malloc(qtf, double, nfree); + mp_malloc(x, double, nfree); + mp_malloc(xnew, double, npar); + mp_malloc(fjac, double, m *nfree); + ldfjac = m; + mp_malloc(diag, double, npar); + mp_malloc(wa1, double, npar); + mp_malloc(wa2, double, npar); + mp_malloc(wa3, double, npar); + mp_malloc(wa4, double, m); + mp_malloc(ipvt, int, npar); + mp_malloc(dvecptr, double *, npar); + + /* Evaluate user function with initial parameter values */ + iflag = mp_call(funct, m, npar, xall, fvec, 0, private_data); + nfev += 1; + if (iflag < 0) { + goto CLEANUP; + } + + fnorm = mp_enorm(m, fvec); + orignorm = fnorm * fnorm; + + /* Make a new copy */ + for (i = 0; i < npar; i++) { + xnew[i] = xall[i]; + } + + /* Transfer free parameters to 'x' */ + for (i = 0; i < nfree; i++) { + x[i] = xall[ifree[i]]; + } + + /* Initialize Levelberg-Marquardt parameter and iteration counter */ + + par = 0.0; + iter = 1; + for (i = 0; i < nfree; i++) { + qtf[i] = 0; + } + +/* Beginning of the outer loop */ +OUTER_LOOP: + for (i = 0; i < nfree; i++) { + xnew[ifree[i]] = x[i]; + } + + /* XXX call iterproc */ + + /* Calculate the jacobian matrix */ + iflag = mp_fdjac2(funct, m, nfree, ifree, npar, xnew, fvec, fjac, ldfjac, conf.epsfcn, wa4, private_data, &nfev, + step, dstep, mpside, qulim, ulim, ddebug, ddrtol, ddatol, wa2, dvecptr); + if (iflag < 0) { + goto CLEANUP; + } + + /* Determine if any of the parameters are pegged at the limits */ + if (qanylim) { + for (j = 0; j < nfree; j++) { + int lpegged = (qllim[j] && (x[j] == llim[j])); + int upegged = (qulim[j] && (x[j] == ulim[j])); + sum = 0; + + /* If the parameter is pegged at a limit, compute the gradient + direction */ + if (lpegged || upegged) { + ij = j * ldfjac; + for (i = 0; i < m; i++, ij++) { + sum += fvec[i] * fjac[ij]; + } + } + /* If pegged at lower limit and gradient is toward negative then + reset gradient to zero */ + if (lpegged && (sum > 0)) { + ij = j * ldfjac; + for (i = 0; i < m; i++, ij++) + fjac[ij] = 0; + } + /* If pegged at upper limit and gradient is toward positive then + reset gradient to zero */ + if (upegged && (sum < 0)) { + ij = j * ldfjac; + for (i = 0; i < m; i++, ij++) + fjac[ij] = 0; + } + } + } + + /* Compute the QR factorization of the jacobian */ + mp_qrfac(m, nfree, fjac, ldfjac, 1, ipvt, nfree, wa1, wa2, wa3); + + /* + * on the first iteration and if mode is 1, scale according + * to the norms of the columns of the initial jacobian. + */ + if (iter == 1) { + if (conf.douserscale == 0) { + for (j = 0; j < nfree; j++) { + diag[ifree[j]] = wa2[j]; + if (wa2[j] == zero) { + diag[ifree[j]] = one; + } + } + } + + /* + * on the first iteration, calculate the norm of the scaled x + * and initialize the step bound delta. + */ + for (j = 0; j < nfree; j++) { + wa3[j] = diag[ifree[j]] * x[j]; + } + + xnorm = mp_enorm(nfree, wa3); + delta = conf.stepfactor * xnorm; + if (delta == zero) + delta = conf.stepfactor; + } + + /* + * form (q transpose)*fvec and store the first n components in + * qtf. + */ + for (i = 0; i < m; i++) { + wa4[i] = fvec[i]; + } + + jj = 0; + for (j = 0; j < nfree; j++) { + temp3 = fjac[jj]; + if (temp3 != zero) { + sum = zero; + ij = jj; + for (i = j; i < m; i++) { + sum += fjac[ij] * wa4[i]; + ij += 1; /* fjac[i+m*j] */ + } + temp = -sum / temp3; + ij = jj; + for (i = j; i < m; i++) { + wa4[i] += fjac[ij] * temp; + ij += 1; /* fjac[i+m*j] */ + } + } + fjac[jj] = wa1[j]; + jj += m + 1; /* fjac[j+m*j] */ + qtf[j] = wa4[j]; + } + + /* ( From this point on, only the square matrix, consisting of the + triangle of R, is needed.) */ + + if (conf.nofinitecheck) { + /* Check for overflow. This should be a cheap test here since FJAC + has been reduced to a (small) square matrix, and the test is + O(N^2). */ + int off = 0, nonfinite = 0; + + for (j = 0; j < nfree; j++) { + for (i = 0; i < nfree; i++) { + if (mpfinite(fjac[off + i]) == 0) + nonfinite = 1; + } + off += ldfjac; + } + + if (nonfinite) { + info = MP_ERR_NAN; + goto CLEANUP; + } + } + + /* + * compute the norm of the scaled gradient. + */ + gnorm = zero; + if (fnorm != zero) { + jj = 0; + for (j = 0; j < nfree; j++) { + l = ipvt[j]; + if (wa2[l] != zero) { + sum = zero; + ij = jj; + for (i = 0; i <= j; i++) { + sum += fjac[ij] * (qtf[i] / fnorm); + ij += 1; /* fjac[i+m*j] */ + } + gnorm = mp_dmax1(gnorm, fabs(sum / wa2[l])); + } + jj += m; + } + } + + /* + * test for convergence of the gradient norm. + */ + if (gnorm <= conf.gtol) + info = MP_OK_DIR; + if (info != 0) + goto L300; + if (conf.maxiter == 0) { + info = MP_MAXITER; + goto L300; + } + + /* + * rescale if necessary. + */ + if (conf.douserscale == 0) { + for (j = 0; j < nfree; j++) { + diag[ifree[j]] = mp_dmax1(diag[ifree[j]], wa2[j]); + } + } + +/* + * beginning of the inner loop. + */ +L200: + /* + * determine the levenberg-marquardt parameter. + */ + mp_lmpar(nfree, fjac, ldfjac, ipvt, ifree, diag, qtf, delta, &par, wa1, wa2, wa3, wa4); + /* + * store the direction p and x + p. calculate the norm of p. + */ + for (j = 0; j < nfree; j++) { + wa1[j] = -wa1[j]; + } + + alpha = 1.0; + if (qanylim == 0) { + /* No parameter limits, so just move to new position WA2 */ + for (j = 0; j < nfree; j++) { + wa2[j] = x[j] + wa1[j]; + } + + } else { + /* Respect the limits. If a step were to go out of bounds, then + * we should take a step in the same direction but shorter distance. + * The step should take us right to the limit in that case. + */ + for (j = 0; j < nfree; j++) { + int lpegged = (qllim[j] && (x[j] <= llim[j])); + int upegged = (qulim[j] && (x[j] >= ulim[j])); + int dwa1 = fabs(wa1[j]) > MP_MACHEP0; + + if (lpegged && (wa1[j] < 0)) + wa1[j] = 0; + if (upegged && (wa1[j] > 0)) + wa1[j] = 0; + + if (dwa1 && qllim[j] && ((x[j] + wa1[j]) < llim[j])) { + alpha = mp_dmin1(alpha, (llim[j] - x[j]) / wa1[j]); + } + if (dwa1 && qulim[j] && ((x[j] + wa1[j]) > ulim[j])) { + alpha = mp_dmin1(alpha, (ulim[j] - x[j]) / wa1[j]); + } + } + + /* Scale the resulting vector, advance to the next position */ + for (j = 0; j < nfree; j++) { + double sgnu, sgnl; + double ulim1, llim1; + + wa1[j] = wa1[j] * alpha; + wa2[j] = x[j] + wa1[j]; + + /* Adjust the output values. If the step put us exactly + * on a boundary, make sure it is exact. + */ + sgnu = (ulim[j] >= 0) ? (+1) : (-1); + sgnl = (llim[j] >= 0) ? (+1) : (-1); + ulim1 = ulim[j] * (1 - sgnu * MP_MACHEP0) - ((ulim[j] == 0) ? (MP_MACHEP0) : 0); + llim1 = llim[j] * (1 + sgnl * MP_MACHEP0) + ((llim[j] == 0) ? (MP_MACHEP0) : 0); + + if (qulim[j] && (wa2[j] >= ulim1)) { + wa2[j] = ulim[j]; + } + if (qllim[j] && (wa2[j] <= llim1)) { + wa2[j] = llim[j]; + } + } + } + + for (j = 0; j < nfree; j++) { + wa3[j] = diag[ifree[j]] * wa1[j]; + } + + pnorm = mp_enorm(nfree, wa3); + + /* + * on the first iteration, adjust the initial step bound. + */ + if (iter == 1) { + delta = mp_dmin1(delta, pnorm); + } + + /* + * evaluate the function at x + p and calculate its norm. + */ + for (i = 0; i < nfree; i++) { + xnew[ifree[i]] = wa2[i]; + } + + iflag = mp_call(funct, m, npar, xnew, wa4, 0, private_data); + nfev += 1; + if (iflag < 0) + goto L300; + + fnorm1 = mp_enorm(m, wa4); + + /* + * compute the scaled actual reduction. + */ + actred = -one; + if ((p1 * fnorm1) < fnorm) { + temp = fnorm1 / fnorm; + actred = one - temp * temp; + } + + /* + * compute the scaled predicted reduction and + * the scaled directional derivative. + */ + jj = 0; + for (j = 0; j < nfree; j++) { + wa3[j] = zero; + l = ipvt[j]; + temp = wa1[l]; + ij = jj; + for (i = 0; i <= j; i++) { + wa3[i] += fjac[ij] * temp; + ij += 1; /* fjac[i+m*j] */ + } + jj += m; + } + + /* Remember, alpha is the fraction of the full LM step actually + * taken + */ + + temp1 = mp_enorm(nfree, wa3) * alpha / fnorm; + temp2 = (sqrt(alpha * par) * pnorm) / fnorm; + prered = temp1 * temp1 + (temp2 * temp2) / p5; + dirder = -(temp1 * temp1 + temp2 * temp2); + + /* + * compute the ratio of the actual to the predicted + * reduction. + */ + ratio = zero; + if (prered != zero) { + ratio = actred / prered; + } + + /* + * update the step bound. + */ + + if (ratio <= p25) { + if (actred >= zero) { + temp = p5; + } else { + temp = p5 * dirder / (dirder + p5 * actred); + } + if (((p1 * fnorm1) >= fnorm) || (temp < p1)) { + temp = p1; + } + delta = temp * mp_dmin1(delta, pnorm / p1); + par = par / temp; + } else { + if ((par == zero) || (ratio >= p75)) { + delta = pnorm / p5; + par = p5 * par; + } + } + + /* + * test for successful iteration. + */ + if (ratio >= p0001) { + + /* + * successful iteration. update x, fvec, and their norms. + */ + for (j = 0; j < nfree; j++) { + x[j] = wa2[j]; + wa2[j] = diag[ifree[j]] * x[j]; + } + for (i = 0; i < m; i++) { + fvec[i] = wa4[i]; + } + xnorm = mp_enorm(nfree, wa2); + fnorm = fnorm1; + iter += 1; + } + + /* + * tests for convergence. + */ + if ((fabs(actred) <= conf.ftol) && (prered <= conf.ftol) && (p5 * ratio <= one)) { + info = MP_OK_CHI; + } + if (delta <= conf.xtol * xnorm) { + info = MP_OK_PAR; + } + if ((fabs(actred) <= conf.ftol) && (prered <= conf.ftol) && (p5 * ratio <= one) && (info == 2)) { + info = MP_OK_BOTH; + } + if (info != 0) { + goto L300; + } + + /* + * tests for termination and stringent tolerances. + */ + if ((conf.maxfev > 0) && (nfev >= conf.maxfev)) { + /* Too many function evaluations */ + info = MP_MAXITER; + } + if (iter >= conf.maxiter) { + /* Too many iterations */ + info = MP_MAXITER; + } + if ((fabs(actred) <= MP_MACHEP0) && (prered <= MP_MACHEP0) && (p5 * ratio <= one)) { + info = MP_FTOL; + } + if (delta <= MP_MACHEP0 * xnorm) { + info = MP_XTOL; + } + if (gnorm <= MP_MACHEP0) { + info = MP_GTOL; + } + if (info != 0) { + goto L300; + } + + /* + * end of the inner loop. repeat if iteration unsuccessful. + */ + if (ratio < p0001) + goto L200; + /* + * end of the outer loop. + */ + goto OUTER_LOOP; + +L300: + /* + * termination, either normal or user imposed. + */ + if (iflag < 0) { + info = iflag; + } + iflag = 0; + + for (i = 0; i < nfree; i++) { + xall[ifree[i]] = x[i]; + } + + if ((conf.nprint > 0) && (info > 0)) { + iflag = mp_call(funct, m, npar, xall, fvec, 0, private_data); + nfev += 1; + } + + /* Compute number of pegged parameters */ + npegged = 0; + if (pars) + for (i = 0; i < npar; i++) { + if ((pars[i].limited[0] && (pars[i].limits[0] == xall[i])) || + (pars[i].limited[1] && (pars[i].limits[1] == xall[i]))) { + npegged++; + } + } + + /* Compute and return the covariance matrix and/or parameter errors */ + if (result && (result->covar || result->xerror)) { + mp_covar(nfree, fjac, ldfjac, ipvt, conf.covtol, wa2); + + if (result->covar) { + /* Zero the destination covariance array */ + for (j = 0; j < (npar * npar); j++) + result->covar[j] = 0; + + /* Transfer the covariance array */ + for (j = 0; j < nfree; j++) { + for (i = 0; i < nfree; i++) { + result->covar[ifree[j] * npar + ifree[i]] = fjac[j * ldfjac + i]; + } + } + } + + if (result->xerror) { + for (j = 0; j < npar; j++) + result->xerror[j] = 0; + + for (j = 0; j < nfree; j++) { + double cc = fjac[j * ldfjac + j]; + if (cc > 0) + result->xerror[ifree[j]] = sqrt(cc); + } + } + } + + if (result) { + strcpy(result->version, MPFIT_VERSION); + result->bestnorm = mp_dmax1(fnorm, fnorm1); + result->bestnorm *= result->bestnorm; + result->orignorm = orignorm; + result->status = info; + result->niter = iter; + result->nfev = nfev; + result->npar = npar; + result->nfree = nfree; + result->npegged = npegged; + result->nfunc = m; + + /* Copy residuals if requested */ + if (result->resid) { + for (j = 0; j < m; j++) + result->resid[j] = fvec[j]; + } + } + +CLEANUP: + if (fvec) + free(fvec); + if (qtf) + free(qtf); + if (x) + free(x); + if (xnew) + free(xnew); + if (fjac) + free(fjac); + if (diag) + free(diag); + if (wa1) + free(wa1); + if (wa2) + free(wa2); + if (wa3) + free(wa3); + if (wa4) + free(wa4); + if (ipvt) + free(ipvt); + if (pfixed) + free(pfixed); + if (step) + free(step); + if (dstep) + free(dstep); + if (mpside) + free(mpside); + if (ddebug) + free(ddebug); + if (ddrtol) + free(ddrtol); + if (ddatol) + free(ddatol); + if (ifree) + free(ifree); + if (qllim) + free(qllim); + if (qulim) + free(qulim); + if (llim) + free(llim); + if (ulim) + free(ulim); + if (dvecptr) + free(dvecptr); + + return info; +} + +/************************fdjac2.c*************************/ + +static int mp_fdjac2(mp_func funct, int m, int n, int *ifree, int npar, double *x, double *fvec, double *fjac, + int ldfjac, double epsfcn, double *wa, void *priv, int *nfev, double *step, double *dstep, + int *dside, int *qulimited, double *ulimit, int *ddebug, double *ddrtol, double *ddatol, + double *wa2, double **dvec) { + /* + * ********** + * + * subroutine fdjac2 + * + * this subroutine computes a forward-difference approximation + * to the m by n jacobian matrix associated with a specified + * problem of m functions in n variables. + * + * the subroutine statement is + * + * subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) + * + * where + * + * fcn is the name of the user-supplied subroutine which + * calculates the functions. fcn must be declared + * in an external statement in the user calling + * program, and should be written as follows. + * + * subroutine fcn(m,n,x,fvec,iflag) + * integer m,n,iflag + * double precision x(n),fvec(m) + * ---------- + * calculate the functions at x and + * return this vector in fvec. + * ---------- + * return + * end + * + * the value of iflag should not be changed by fcn unless + * the user wants to terminate execution of fdjac2. + * in this case set iflag to a negative integer. + * + * m is a positive integer input variable set to the number + * of functions. + * + * n is a positive integer input variable set to the number + * of variables. n must not exceed m. + * + * x is an input array of length n. + * + * fvec is an input array of length m which must contain the + * functions evaluated at x. + * + * fjac is an output m by n array which contains the + * approximation to the jacobian matrix evaluated at x. + * + * ldfjac is a positive integer input variable not less than m + * which specifies the leading dimension of the array fjac. + * + * iflag is an integer variable which can be used to terminate + * the execution of fdjac2. see description of fcn. + * + * epsfcn is an input variable used in determining a suitable + * step length for the forward-difference approximation. this + * approximation assumes that the relative errors in the + * functions are of the order of epsfcn. if epsfcn is less + * than the machine precision, it is assumed that the relative + * errors in the functions are of the order of the machine + * precision. + * + * wa is a work array of length m. + * + * subprograms called + * + * user-supplied ...... fcn + * + * minpack-supplied ... dpmpar + * + * fortran-supplied ... dabs,dmax1,dsqrt + * + * argonne national laboratory. minpack project. march 1980. + * burton s. garbow, kenneth e. hillstrom, jorge j. more + * + ********** + */ + int i, j, ij; + int iflag = 0; + double eps, h, temp; + static double zero = 0.0; + int has_analytical_deriv = 0, has_numerical_deriv = 0; + int has_debug_deriv = 0; + + temp = mp_dmax1(epsfcn, MP_MACHEP0); + eps = sqrt(temp); + ij = 0; + ldfjac = 0; /* Prevent compiler warning */ + if (ldfjac) { + } /* Prevent compiler warning */ + + for (j = 0; j < npar; j++) + dvec[j] = 0; + + /* Initialize the Jacobian derivative matrix */ + for (j = 0; j < (n * m); j++) + fjac[j] = 0; + + /* Check for which parameters need analytical derivatives and which + need numerical ones */ + for (j = 0; j < n; j++) { /* Loop through free parameters only */ + if (dside && dside[ifree[j]] == 3 && ddebug[ifree[j]] == 0) { + /* Purely analytical derivatives */ + dvec[ifree[j]] = fjac + j * m; + has_analytical_deriv = 1; + } else if (dside && ddebug[ifree[j]] == 1) { + /* Numerical and analytical derivatives as a debug cross-check */ + dvec[ifree[j]] = fjac + j * m; + has_analytical_deriv = 1; + has_numerical_deriv = 1; + has_debug_deriv = 1; + } else { + has_numerical_deriv = 1; + } + } + + /* If there are any parameters requiring analytical derivatives, + then compute them first. */ + if (has_analytical_deriv) { + iflag = mp_call(funct, m, npar, x, wa, dvec, priv); + if (nfev) + *nfev = *nfev + 1; + if (iflag < 0) + goto DONE; + } + + if (has_debug_deriv) { + printf("FJAC DEBUG BEGIN\n"); + printf("# %10s %10s %10s %10s %10s %10s\n", "IPNT", "FUNC", "DERIV_U", "DERIV_N", "DIFF_ABS", "DIFF_REL"); + } + + /* Any parameters requiring numerical derivatives */ + if (has_numerical_deriv) + for (j = 0; j < n; j++) { /* Loop thru free parms */ + int dsidei = (dside) ? (dside[ifree[j]]) : (0); + int debug = ddebug[ifree[j]]; + double dr = ddrtol[ifree[j]], da = ddatol[ifree[j]]; + + /* Check for debugging */ + if (debug) { + printf("FJAC PARM %d\n", ifree[j]); + } + + /* Skip parameters already done by user-computed partials */ + if (dside && dsidei == 3) + continue; + + temp = x[ifree[j]]; + h = eps * fabs(temp); + if (step && step[ifree[j]] > 0) + h = step[ifree[j]]; + if (dstep && dstep[ifree[j]] > 0) + h = fabs(dstep[ifree[j]] * temp); + if (h == zero) + h = eps; + + /* If negative step requested, or we are against the upper limit */ + if ((dside && dsidei == -1) || + (dside && dsidei == 0 && qulimited && ulimit && qulimited[j] && (temp > (ulimit[j] - h)))) { + h = -h; + } + + x[ifree[j]] = temp + h; + iflag = mp_call(funct, m, npar, x, wa, 0, priv); + if (nfev) + *nfev = *nfev + 1; + if (iflag < 0) + goto DONE; + x[ifree[j]] = temp; + + if (dsidei <= 1) { + /* COMPUTE THE ONE-SIDED DERIVATIVE */ + if (!debug) { + /* Non-debug path for speed */ + for (i = 0; i < m; i++, ij++) { + fjac[ij] = (wa[i] - fvec[i]) / h; /* fjac[i+m*j] */ + } + } else { + /* Debug path for correctness */ + for (i = 0; i < m; i++, ij++) { + double fjold = fjac[ij]; + fjac[ij] = (wa[i] - fvec[i]) / h; /* fjac[i+m*j] */ + if ((da == 0 && dr == 0 && (fjold != 0 || fjac[ij] != 0)) || + ((da != 0 || dr != 0) && (fabs(fjold - fjac[ij]) > da + fabs(fjold) * dr))) { + printf(" %10d %10.4g %10.4g %10.4g %10.4g %10.4g\n", i, fvec[i], fjold, fjac[ij], + fjold - fjac[ij], (fjold == 0) ? (0) : ((fjold - fjac[ij]) / fjold)); + } + } + } /* end debugging */ + + } else { /* dside > 2 */ + /* COMPUTE THE TWO-SIDED DERIVATIVE */ + for (i = 0; i < m; i++) { + wa2[i] = wa[i]; + } + + /* Evaluate at x - h */ + x[ifree[j]] = temp - h; + iflag = mp_call(funct, m, npar, x, wa, 0, priv); + if (nfev) + *nfev = *nfev + 1; + if (iflag < 0) + goto DONE; + x[ifree[j]] = temp; + + /* Now compute derivative as (f(x+h) - f(x-h))/(2h) */ + if (!debug) { + /* Non-debug path for speed */ + for (i = 0; i < m; i++, ij++) { + fjac[ij] = (fjac[ij] - wa[i]) / (2 * h); /* fjac[i+m*j] */ + } + } else { + /* Debug path for correctness */ + for (i = 0; i < m; i++, ij++) { + double fjold = fjac[ij]; + fjac[ij] = (wa2[i] - wa[i]) / (2 * h); /* fjac[i+m*j] */ + if ((da == 0 && dr == 0 && (fjold != 0 || fjac[ij] != 0)) || + ((da != 0 || dr != 0) && (fabs(fjold - fjac[ij]) > da + fabs(fjold) * dr))) { + printf(" %10d %10.4g %10.4g %10.4g %10.4g %10.4g\n", i, fvec[i], fjold, fjac[ij], + fjold - fjac[ij], (fjold == 0) ? (0) : ((fjold - fjac[ij]) / fjold)); + } + } + } /* end debugging */ + + } /* if (dside > 2) */ + } /* if (has_numerical_derivative) */ + + if (has_debug_deriv) { + printf("FJAC DEBUG END\n"); + } + +DONE: + if (iflag < 0) + return iflag; + return 0; + /* + * last card of subroutine fdjac2. + */ +} + +/************************qrfac.c*************************/ + +static void mp_qrfac(int m, int n, double *a, int lda, int pivot, int *ipvt, int lipvt, double *rdiag, double *acnorm, + double *wa) { + /* + * ********** + * + * subroutine qrfac + * + * this subroutine uses householder transformations with column + * pivoting (optional) to compute a qr factorization of the + * m by n matrix a. that is, qrfac determines an orthogonal + * matrix q, a permutation matrix p, and an upper trapezoidal + * matrix r with diagonal elements of nonincreasing magnitude, + * such that a*p = q*r. the householder transformation for + * column k, k = 1,2,...,min(m,n), is of the form + * + * t + * i - (1/u(k))*u*u + * + * where u has zeros in the first k-1 positions. the form of + * this transformation and the method of pivoting first + * appeared in the corresponding linpack subroutine. + * + * the subroutine statement is + * + * subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) + * + * where + * + * m is a positive integer input variable set to the number + * of rows of a. + * + * n is a positive integer input variable set to the number + * of columns of a. + * + * a is an m by n array. on input a contains the matrix for + * which the qr factorization is to be computed. on output + * the strict upper trapezoidal part of a contains the strict + * upper trapezoidal part of r, and the lower trapezoidal + * part of a contains a factored form of q (the non-trivial + * elements of the u vectors described above). + * + * lda is a positive integer input variable not less than m + * which specifies the leading dimension of the array a. + * + * pivot is a logical input variable. if pivot is set true, + * then column pivoting is enforced. if pivot is set false, + * then no column pivoting is done. + * + * ipvt is an integer output array of length lipvt. ipvt + * defines the permutation matrix p such that a*p = q*r. + * column j of p is column ipvt(j) of the identity matrix. + * if pivot is false, ipvt is not referenced. + * + * lipvt is a positive integer input variable. if pivot is false, + * then lipvt may be as small as 1. if pivot is true, then + * lipvt must be at least n. + * + * rdiag is an output array of length n which contains the + * diagonal elements of r. + * + * acnorm is an output array of length n which contains the + * norms of the corresponding columns of the input matrix a. + * if this information is not needed, then acnorm can coincide + * with rdiag. + * + * wa is a work array of length n. if pivot is false, then wa + * can coincide with rdiag. + * + * subprograms called + * + * minpack-supplied ... dpmpar,enorm + * + * fortran-supplied ... dmax1,dsqrt,min0 + * + * argonne national laboratory. minpack project. march 1980. + * burton s. garbow, kenneth e. hillstrom, jorge j. more + * + * ********** + */ + int i, ij, jj, j, jp1, k, kmax, minmn; + double ajnorm, sum, temp; + static double zero = 0.0; + static double one = 1.0; + static double p05 = 0.05; + + lda = 0; /* Prevent compiler warning */ + lipvt = 0; /* Prevent compiler warning */ + if (lda) { + } /* Prevent compiler warning */ + if (lipvt) { + } /* Prevent compiler warning */ + + /* + * compute the initial column norms and initialize several arrays. + */ + ij = 0; + for (j = 0; j < n; j++) { + acnorm[j] = mp_enorm(m, &a[ij]); + rdiag[j] = acnorm[j]; + wa[j] = rdiag[j]; + if (pivot != 0) + ipvt[j] = j; + ij += m; /* m*j */ + } + /* + * reduce a to r with householder transformations. + */ + minmn = mp_min0(m, n); + for (j = 0; j < minmn; j++) { + if (pivot == 0) + goto L40; + /* + * bring the column of largest norm into the pivot position. + */ + kmax = j; + for (k = j; k < n; k++) { + if (rdiag[k] > rdiag[kmax]) + kmax = k; + } + if (kmax == j) + goto L40; + + ij = m * j; + jj = m * kmax; + for (i = 0; i < m; i++) { + temp = a[ij]; /* [i+m*j] */ + a[ij] = a[jj]; /* [i+m*kmax] */ + a[jj] = temp; + ij += 1; + jj += 1; + } + rdiag[kmax] = rdiag[j]; + wa[kmax] = wa[j]; + k = ipvt[j]; + ipvt[j] = ipvt[kmax]; + ipvt[kmax] = k; + + L40: + /* + * compute the householder transformation to reduce the + * j-th column of a to a multiple of the j-th unit vector. + */ + jj = j + m * j; + ajnorm = mp_enorm(m - j, &a[jj]); + if (ajnorm == zero) + goto L100; + if (a[jj] < zero) + ajnorm = -ajnorm; + ij = jj; + for (i = j; i < m; i++) { + a[ij] /= ajnorm; + ij += 1; /* [i+m*j] */ + } + a[jj] += one; + /* + * apply the transformation to the remaining columns + * and update the norms. + */ + jp1 = j + 1; + if (jp1 < n) { + for (k = jp1; k < n; k++) { + sum = zero; + ij = j + m * k; + jj = j + m * j; + for (i = j; i < m; i++) { + sum += a[jj] * a[ij]; + ij += 1; /* [i+m*k] */ + jj += 1; /* [i+m*j] */ + } + temp = sum / a[j + m * j]; + ij = j + m * k; + jj = j + m * j; + for (i = j; i < m; i++) { + a[ij] -= temp * a[jj]; + ij += 1; /* [i+m*k] */ + jj += 1; /* [i+m*j] */ + } + if ((pivot != 0) && (rdiag[k] != zero)) { + temp = a[j + m * k] / rdiag[k]; + temp = mp_dmax1(zero, one - temp * temp); + rdiag[k] *= sqrt(temp); + temp = rdiag[k] / wa[k]; + if ((p05 * temp * temp) <= MP_MACHEP0) { + rdiag[k] = mp_enorm(m - j - 1, &a[jp1 + m * k]); + wa[k] = rdiag[k]; + } + } + } + } + + L100: + rdiag[j] = -ajnorm; + } + /* + * last card of subroutine qrfac. + */ +} + +/************************qrsolv.c*************************/ + +static void mp_qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag, + double *wa) { + /* + * ********** + * + * subroutine qrsolv + * + * given an m by n matrix a, an n by n diagonal matrix d, + * and an m-vector b, the problem is to determine an x which + * solves the system + * + * a*x = b , d*x = 0 , + * + * in the least squares sense. + * + * this subroutine completes the solution of the problem + * if it is provided with the necessary information from the + * qr factorization, with column pivoting, of a. that is, if + * a*p = q*r, where p is a permutation matrix, q has orthogonal + * columns, and r is an upper triangular matrix with diagonal + * elements of nonincreasing magnitude, then qrsolv expects + * the full upper triangle of r, the permutation matrix p, + * and the first n components of (q transpose)*b. the system + * a*x = b, d*x = 0, is then equivalent to + * + * t t + * r*z = q *b , p *d*p*z = 0 , + * + * where x = p*z. if this system does not have full rank, + * then a least squares solution is obtained. on output qrsolv + * also provides an upper triangular matrix s such that + * + * t t t + * p *(a *a + d*d)*p = s *s . + * + * s is computed within qrsolv and may be of separate interest. + * + * the subroutine statement is + * + * subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa) + * + * where + * + * n is a positive integer input variable set to the order of r. + * + * r is an n by n array. on input the full upper triangle + * must contain the full upper triangle of the matrix r. + * on output the full upper triangle is unaltered, and the + * strict lower triangle contains the strict upper triangle + * (transposed) of the upper triangular matrix s. + * + * ldr is a positive integer input variable not less than n + * which specifies the leading dimension of the array r. + * + * ipvt is an integer input array of length n which defines the + * permutation matrix p such that a*p = q*r. column j of p + * is column ipvt(j) of the identity matrix. + * + * diag is an input array of length n which must contain the + * diagonal elements of the matrix d. + * + * qtb is an input array of length n which must contain the first + * n elements of the vector (q transpose)*b. + * + * x is an output array of length n which contains the least + * squares solution of the system a*x = b, d*x = 0. + * + * sdiag is an output array of length n which contains the + * diagonal elements of the upper triangular matrix s. + * + * wa is a work array of length n. + * + * subprograms called + * + * fortran-supplied ... dabs,dsqrt + * + * argonne national laboratory. minpack project. march 1980. + * burton s. garbow, kenneth e. hillstrom, jorge j. more + * + * ********** + */ + int i, ij, ik, kk, j, jp1, k, kp1, l, nsing; + double cosx, cotan, qtbpj, sinx, sum, tanx, temp; + static double zero = 0.0; + static double p25 = 0.25; + static double p5 = 0.5; + + /* + * copy r and (q transpose)*b to preserve input and initialize s. + * in particular, save the diagonal elements of r in x. + */ + kk = 0; + for (j = 0; j < n; j++) { + ij = kk; + ik = kk; + for (i = j; i < n; i++) { + r[ij] = r[ik]; + ij += 1; /* [i+ldr*j] */ + ik += ldr; /* [j+ldr*i] */ + } + x[j] = r[kk]; + wa[j] = qtb[j]; + kk += ldr + 1; /* j+ldr*j */ + } + + /* + * eliminate the diagonal matrix d using a givens rotation. + */ + for (j = 0; j < n; j++) { + /* + * prepare the row of d to be eliminated, locating the + * diagonal element using p from the qr factorization. + */ + l = ipvt[j]; + if (diag[l] == zero) + goto L90; + for (k = j; k < n; k++) + sdiag[k] = zero; + sdiag[j] = diag[l]; + /* + * the transformations to eliminate the row of d + * modify only a single element of (q transpose)*b + * beyond the first n, which is initially zero. + */ + qtbpj = zero; + for (k = j; k < n; k++) { + /* + * determine a givens rotation which eliminates the + * appropriate element in the current row of d. + */ + if (sdiag[k] == zero) + continue; + kk = k + ldr * k; + if (fabs(r[kk]) < fabs(sdiag[k])) { + cotan = r[kk] / sdiag[k]; + sinx = p5 / sqrt(p25 + p25 * cotan * cotan); + cosx = sinx * cotan; + } else { + tanx = sdiag[k] / r[kk]; + cosx = p5 / sqrt(p25 + p25 * tanx * tanx); + sinx = cosx * tanx; + } + /* + * compute the modified diagonal element of r and + * the modified element of ((q transpose)*b,0). + */ + r[kk] = cosx * r[kk] + sinx * sdiag[k]; + temp = cosx * wa[k] + sinx * qtbpj; + qtbpj = -sinx * wa[k] + cosx * qtbpj; + wa[k] = temp; + /* + * accumulate the tranformation in the row of s. + */ + kp1 = k + 1; + if (n > kp1) { + ik = kk + 1; + for (i = kp1; i < n; i++) { + temp = cosx * r[ik] + sinx * sdiag[i]; + sdiag[i] = -sinx * r[ik] + cosx * sdiag[i]; + r[ik] = temp; + ik += 1; /* [i+ldr*k] */ + } + } + } + L90: + /* + * store the diagonal element of s and restore + * the corresponding diagonal element of r. + */ + kk = j + ldr * j; + sdiag[j] = r[kk]; + r[kk] = x[j]; + } + /* + * solve the triangular system for z. if the system is + * singular, then obtain a least squares solution. + */ + nsing = n; + for (j = 0; j < n; j++) { + if ((sdiag[j] == zero) && (nsing == n)) + nsing = j; + if (nsing < n) + wa[j] = zero; + } + if (nsing < 1) + goto L150; + + for (k = 0; k < nsing; k++) { + j = nsing - k - 1; + sum = zero; + jp1 = j + 1; + if (nsing > jp1) { + ij = jp1 + ldr * j; + for (i = jp1; i < nsing; i++) { + sum += r[ij] * wa[i]; + ij += 1; /* [i+ldr*j] */ + } + } + wa[j] = (wa[j] - sum) / sdiag[j]; + } +L150: + /* + * permute the components of z back to components of x. + */ + for (j = 0; j < n; j++) { + l = ipvt[j]; + x[l] = wa[j]; + } + /* + * last card of subroutine qrsolv. + */ +} + +/************************lmpar.c*************************/ + +static void mp_lmpar(int n, double *r, int ldr, int *ipvt, int *ifree, double *diag, double *qtb, double delta, + double *par, double *x, double *sdiag, double *wa1, double *wa2) { + /* ********** + * + * subroutine lmpar + * + * given an m by n matrix a, an n by n nonsingular diagonal + * matrix d, an m-vector b, and a positive number delta, + * the problem is to determine a value for the parameter + * par such that if x solves the system + * + * a*x = b , sqrt(par)*d*x = 0 , + * + * in the least squares sense, and dxnorm is the euclidean + * norm of d*x, then either par is zero and + * + * (dxnorm-delta) .le. 0.1*delta , + * + * or par is positive and + * + * abs(dxnorm-delta) .le. 0.1*delta . + * + * this subroutine completes the solution of the problem + * if it is provided with the necessary information from the + * qr factorization, with column pivoting, of a. that is, if + * a*p = q*r, where p is a permutation matrix, q has orthogonal + * columns, and r is an upper triangular matrix with diagonal + * elements of nonincreasing magnitude, then lmpar expects + * the full upper triangle of r, the permutation matrix p, + * and the first n components of (q transpose)*b. on output + * lmpar also provides an upper triangular matrix s such that + * + * t t t + * p *(a *a + par*d*d)*p = s *s . + * + * s is employed within lmpar and may be of separate interest. + * + * only a few iterations are generally needed for convergence + * of the algorithm. if, however, the limit of 10 iterations + * is reached, then the output par will contain the best + * value obtained so far. + * + * the subroutine statement is + * + * subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag, + * wa1,wa2) + * + * where + * + * n is a positive integer input variable set to the order of r. + * + * r is an n by n array. on input the full upper triangle + * must contain the full upper triangle of the matrix r. + * on output the full upper triangle is unaltered, and the + * strict lower triangle contains the strict upper triangle + * (transposed) of the upper triangular matrix s. + * + * ldr is a positive integer input variable not less than n + * which specifies the leading dimension of the array r. + * + * ipvt is an integer input array of length n which defines the + * permutation matrix p such that a*p = q*r. column j of p + * is column ipvt(j) of the identity matrix. + * + * diag is an input array of length n which must contain the + * diagonal elements of the matrix d. + * + * qtb is an input array of length n which must contain the first + * n elements of the vector (q transpose)*b. + * + * delta is a positive input variable which specifies an upper + * bound on the euclidean norm of d*x. + * + * par is a nonnegative variable. on input par contains an + * initial estimate of the levenberg-marquardt parameter. + * on output par contains the final estimate. + * + * x is an output array of length n which contains the least + * squares solution of the system a*x = b, sqrt(par)*d*x = 0, + * for the output par. + * + * sdiag is an output array of length n which contains the + * diagonal elements of the upper triangular matrix s. + * + * wa1 and wa2 are work arrays of length n. + * + * subprograms called + * + * minpack-supplied ... dpmpar,mp_enorm,qrsolv + * + * fortran-supplied ... dabs,mp_dmax1,dmin1,dsqrt + * + * argonne national laboratory. minpack project. march 1980. + * burton s. garbow, kenneth e. hillstrom, jorge j. more + * + * ********** + */ + int i, iter, ij, jj, j, jm1, jp1, k, l, nsing; + double dxnorm, fp, gnorm, parc, parl, paru; + double sum, temp; + static double zero = 0.0; + /* static double one = 1.0; */ + static double p1 = 0.1; + static double p001 = 0.001; + + /* + * compute and store in x the gauss-newton direction. if the + * jacobian is rank-deficient, obtain a least squares solution. + */ + nsing = n; + jj = 0; + for (j = 0; j < n; j++) { + wa1[j] = qtb[j]; + if ((r[jj] == zero) && (nsing == n)) + nsing = j; + if (nsing < n) + wa1[j] = zero; + jj += ldr + 1; /* [j+ldr*j] */ + } + + if (nsing >= 1) { + for (k = 0; k < nsing; k++) { + j = nsing - k - 1; + wa1[j] = wa1[j] / r[j + ldr * j]; + temp = wa1[j]; + jm1 = j - 1; + if (jm1 >= 0) { + ij = ldr * j; + for (i = 0; i <= jm1; i++) { + wa1[i] -= r[ij] * temp; + ij += 1; + } + } + } + } + + for (j = 0; j < n; j++) { + l = ipvt[j]; + x[l] = wa1[j]; + } + /* + * initialize the iteration counter. + * evaluate the function at the origin, and test + * for acceptance of the gauss-newton direction. + */ + iter = 0; + for (j = 0; j < n; j++) + wa2[j] = diag[ifree[j]] * x[j]; + dxnorm = mp_enorm(n, wa2); + fp = dxnorm - delta; + if (fp <= p1 * delta) { + goto L220; + } + /* + * if the jacobian is not rank deficient, the newton + * step provides a lower bound, parl, for the zero of + * the function. otherwise set this bound to zero. + */ + parl = zero; + if (nsing >= n) { + for (j = 0; j < n; j++) { + l = ipvt[j]; + wa1[j] = diag[ifree[l]] * (wa2[l] / dxnorm); + } + jj = 0; + for (j = 0; j < n; j++) { + sum = zero; + jm1 = j - 1; + if (jm1 >= 0) { + ij = jj; + for (i = 0; i <= jm1; i++) { + sum += r[ij] * wa1[i]; + ij += 1; + } + } + wa1[j] = (wa1[j] - sum) / r[j + ldr * j]; + jj += ldr; /* [i+ldr*j] */ + } + temp = mp_enorm(n, wa1); + parl = ((fp / delta) / temp) / temp; + } + /* + * calculate an upper bound, paru, for the zero of the function. + */ + jj = 0; + for (j = 0; j < n; j++) { + sum = zero; + ij = jj; + for (i = 0; i <= j; i++) { + sum += r[ij] * qtb[i]; + ij += 1; + } + l = ipvt[j]; + wa1[j] = sum / diag[ifree[l]]; + jj += ldr; /* [i+ldr*j] */ + } + gnorm = mp_enorm(n, wa1); + paru = gnorm / delta; + if (paru == zero) + paru = MP_DWARF / mp_dmin1(delta, p1); + /* + * if the input par lies outside of the interval (parl,paru), + * set par to the closer endpoint. + */ + *par = mp_dmax1(*par, parl); + *par = mp_dmin1(*par, paru); + if (*par == zero) + *par = gnorm / dxnorm; + +/* + * beginning of an iteration. + */ +L150: + iter += 1; + /* + * evaluate the function at the current value of par. + */ + if (*par == zero) + *par = mp_dmax1(MP_DWARF, p001 * paru); + temp = sqrt(*par); + for (j = 0; j < n; j++) + wa1[j] = temp * diag[ifree[j]]; + mp_qrsolv(n, r, ldr, ipvt, wa1, qtb, x, sdiag, wa2); + for (j = 0; j < n; j++) + wa2[j] = diag[ifree[j]] * x[j]; + dxnorm = mp_enorm(n, wa2); + temp = fp; + fp = dxnorm - delta; + /* + * if the function is small enough, accept the current value + * of par. also test for the exceptional cases where parl + * is zero or the number of iterations has reached 10. + */ + if ((fabs(fp) <= p1 * delta) || ((parl == zero) && (fp <= temp) && (temp < zero)) || (iter == 10)) + goto L220; + /* + * compute the newton correction. + */ + for (j = 0; j < n; j++) { + l = ipvt[j]; + wa1[j] = diag[ifree[l]] * (wa2[l] / dxnorm); + } + jj = 0; + for (j = 0; j < n; j++) { + wa1[j] = wa1[j] / sdiag[j]; + temp = wa1[j]; + jp1 = j + 1; + if (jp1 < n) { + ij = jp1 + jj; + for (i = jp1; i < n; i++) { + wa1[i] -= r[ij] * temp; + ij += 1; /* [i+ldr*j] */ + } + } + jj += ldr; /* ldr*j */ + } + temp = mp_enorm(n, wa1); + parc = ((fp / delta) / temp) / temp; + /* + * depending on the sign of the function, update parl or paru. + */ + if (fp > zero) + parl = mp_dmax1(parl, *par); + if (fp < zero) + paru = mp_dmin1(paru, *par); + /* + * compute an improved estimate for par. + */ + *par = mp_dmax1(parl, *par + parc); + /* + * end of an iteration. + */ + goto L150; + +L220: + /* + * termination. + */ + if (iter == 0) + *par = zero; + /* + * last card of subroutine lmpar. + */ +} + +/************************enorm.c*************************/ + +static double mp_enorm(int n, double *x) { + /* + * ********** + * + * function enorm + * + * given an n-vector x, this function calculates the + * euclidean norm of x. + * + * the euclidean norm is computed by accumulating the sum of + * squares in three different sums. the sums of squares for the + * small and large components are scaled so that no overflows + * occur. non-destructive underflows are permitted. underflows + * and overflows do not occur in the computation of the unscaled + * sum of squares for the intermediate components. + * the definitions of small, intermediate and large components + * depend on two constants, rdwarf and rgiant. the main + * restrictions on these constants are that rdwarf**2 not + * underflow and rgiant**2 not overflow. the constants + * given here are suitable for every known computer. + * + * the function statement is + * + * double precision function enorm(n,x) + * + * where + * + * n is a positive integer input variable. + * + * x is an input array of length n. + * + * subprograms called + * + * fortran-supplied ... dabs,dsqrt + * + * argonne national laboratory. minpack project. march 1980. + * burton s. garbow, kenneth e. hillstrom, jorge j. more + * + * ********** + */ + int i; + double agiant, floatn, s1, s2, s3, xabs, x1max, x3max; + double ans, temp; + double rdwarf = MP_RDWARF; + double rgiant = MP_RGIANT; + static double zero = 0.0; + static double one = 1.0; + + s1 = zero; + s2 = zero; + s3 = zero; + x1max = zero; + x3max = zero; + floatn = n; + agiant = rgiant / floatn; + + for (i = 0; i < n; i++) { + xabs = fabs(x[i]); + if ((xabs > rdwarf) && (xabs < agiant)) { + /* + * sum for intermediate components. + */ + s2 += xabs * xabs; + continue; + } + + if (xabs > rdwarf) { + /* + * sum for large components. + */ + if (xabs > x1max) { + temp = x1max / xabs; + s1 = one + s1 * temp * temp; + x1max = xabs; + } else { + temp = xabs / x1max; + s1 += temp * temp; + } + continue; + } + /* + * sum for small components. + */ + if (xabs > x3max) { + temp = x3max / xabs; + s3 = one + s3 * temp * temp; + x3max = xabs; + } else { + if (xabs != zero) { + temp = xabs / x3max; + s3 += temp * temp; + } + } + } + /* + * calculation of norm. + */ + if (s1 != zero) { + temp = s1 + (s2 / x1max) / x1max; + ans = x1max * sqrt(temp); + return (ans); + } + if (s2 != zero) { + if (s2 >= x3max) + temp = s2 * (one + (x3max / s2) * (x3max * s3)); + else + temp = x3max * ((s2 / x3max) + (x3max * s3)); + ans = sqrt(temp); + } else { + ans = x3max * sqrt(s3); + } + return (ans); + /* + * last card of function enorm. + */ +} + +/************************lmmisc.c*************************/ + +static double mp_dmax1(double a, double b) { + if (a >= b) + return (a); + else + return (b); +} + +static double mp_dmin1(double a, double b) { + if (a <= b) + return (a); + else + return (b); +} + +static int mp_min0(int a, int b) { + if (a <= b) + return (a); + else + return (b); +} + +/************************covar.c*************************/ +/* +c ********** +c +c subroutine covar +c +c given an m by n matrix a, the problem is to determine +c the covariance matrix corresponding to a, defined as +c +c t +c inverse(a *a) . +c +c this subroutine completes the solution of the problem +c if it is provided with the necessary information from the +c qr factorization, with column pivoting, of a. that is, if +c a*p = q*r, where p is a permutation matrix, q has orthogonal +c columns, and r is an upper triangular matrix with diagonal +c elements of nonincreasing magnitude, then covar expects +c the full upper triangle of r and the permutation matrix p. +c the covariance matrix is then computed as +c +c t t +c p*inverse(r *r)*p . +c +c if a is nearly rank deficient, it may be desirable to compute +c the covariance matrix corresponding to the linearly independent +c columns of a. to define the numerical rank of a, covar uses +c the tolerance tol. if l is the largest integer such that +c +c abs(r(l,l)) .gt. tol*abs(r(1,1)) , +c +c then covar computes the covariance matrix corresponding to +c the first l columns of r. for k greater than l, column +c and row ipvt(k) of the covariance matrix are set to zero. +c +c the subroutine statement is +c +c subroutine covar(n,r,ldr,ipvt,tol,wa) +c +c where +c +c n is a positive integer input variable set to the order of r. +c +c r is an n by n array. on input the full upper triangle must +c contain the full upper triangle of the matrix r. on output +c r contains the square symmetric covariance matrix. +c +c ldr is a positive integer input variable not less than n +c which specifies the leading dimension of the array r. +c +c ipvt is an integer input array of length n which defines the +c permutation matrix p such that a*p = q*r. column j of p +c is column ipvt(j) of the identity matrix. +c +c tol is a nonnegative input variable used to define the +c numerical rank of a in the manner described above. +c +c wa is a work array of length n. +c +c subprograms called +c +c fortran-supplied ... dabs +c +c argonne national laboratory. minpack project. august 1980. +c burton s. garbow, kenneth e. hillstrom, jorge j. more +c +c ********** +*/ + +static int mp_covar(int n, double *r, int ldr, int *ipvt, double tol, double *wa) { + int i, ii, j, jj, k, l; + int kk, kj, ji, j0, k0, jj0; + int sing; + double one = 1.0, temp, tolr, zero = 0.0; + +/* + * form the inverse of r in the full upper triangle of r. + */ + +#if 0 + for (j=0; j= 0) { + for (k = 0; k <= l; k++) { + k0 = k * ldr; + + for (j = 0; j < k; j++) { + temp = r[k * ldr + j]; + + j0 = j * ldr; + for (i = 0; i <= j; i++) { + r[j0 + i] += temp * r[k0 + i]; + } + } + + temp = r[k0 + k]; + for (i = 0; i <= k; i++) { + r[k0 + i] *= temp; + } + } + } + + /* + * For the full lower triangle of the covariance matrix + * in the strict lower triangle or and in wa + */ + for (j = 0; j < n; j++) { + jj = ipvt[j]; + sing = (j > l); + j0 = j * ldr; + jj0 = jj * ldr; + for (i = 0; i <= j; i++) { + ji = j0 + i; + + if (sing) + r[ji] = zero; + ii = ipvt[i]; + if (ii > jj) + r[jj0 + ii] = r[ji]; + if (ii < jj) + r[ii * ldr + jj] = r[ji]; + } + wa[jj] = r[j0 + j]; + } + + /* + * Symmetrize the covariance matrix in r + */ + for (j = 0; j < n; j++) { + j0 = j * ldr; + for (i = 0; i < j; i++) { + r[j0 + i] = r[i * ldr + j]; + } + r[j0 + j] = wa[j]; + } + +#if 0 + for (j=0; j= 199901L +#define mpfinite(x) isfinite(x) + +/* Microsoft C uses _finite(x) instead of finite(x) */ +#elif defined(_MSC_VER) && _MSC_VER +#include +#define mpfinite(x) _finite(x) + +/* Default is to assume that compiler/library has finite() function */ +#else +#define mpfinite(x) finite(x) + +#endif + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* MPFIT_H */ diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c new file mode 100644 index 0000000..03ef522 --- /dev/null +++ b/src/poser_mpfit.c @@ -0,0 +1,290 @@ +#ifndef USE_DOUBLE +#define FLT double +#define USE_DOUBLE +#endif + +#include + +#include "mpfit/mpfit.h" +#include "poser.h" +#include +#include + +#include "assert.h" +#include "linmath.h" +#include "math.h" +#include "poser_general_optimizer.h" +#include "string.h" +#include "survive_cal.h" +#include "survive_config.h" +#include "survive_reproject.h" + +typedef struct { + SurviveObject *so; + FLT *pts3d; + int *lh; + FLT *meas; + SurvivePose camera_params[2]; +} mpfit_context; + +typedef struct MPFITData { + GeneralOptimizerData opt; + + int last_acode; + int last_lh; + + int sensor_time_window; + int use_jacobian_function; + int required_meas; + + SurviveIMUTracker tracker; + bool useIMU; + + struct { + int meas_failures; + } stats; +} MPFITData; + +static size_t construct_input_from_scene(MPFITData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, + double *meas, double *sensors, int *lhs) { + size_t rtn = 0; + SurviveObject *so = d->opt.so; + + for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { + for (size_t lh = 0; lh < 2; lh++) { + if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { + const double *a = scene->angles[sensor][lh]; + + sensors[rtn * 3 + 0] = so->sensor_locations[sensor * 3 + 0]; + sensors[rtn * 3 + 1] = so->sensor_locations[sensor * 3 + 1]; + sensors[rtn * 3 + 2] = so->sensor_locations[sensor * 3 + 2]; + meas[rtn * 2 + 0] = a[0]; + meas[rtn * 2 + 1] = a[1]; + lhs[rtn] = lh; + rtn++; + } + } + } + return rtn; +} + +static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + mpfit_context *ctx = (mpfit_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + // quatnormalize(obj.Rot, obj.Rot); + + // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); +} + +static void str_metric_function_jac(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + mpfit_context *ctx = (mpfit_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + // quatnormalize(obj.Rot, obj.Rot); + + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); +} + +int mpfunc(int m, int n, double *p, double *deviates, double **derivs, void *private) { + mpfit_context *mpfunc_ctx = private; + + SurvivePose *pose = (SurvivePose *)p; + + for (int i = 0; i < m / 2; i++) { + FLT out[2]; + survive_reproject_full(out, pose, mpfunc_ctx->pts3d + i * 3, &mpfunc_ctx->camera_params[mpfunc_ctx->lh[i]], + &mpfunc_ctx->so->ctx->bsd[mpfunc_ctx->lh[i]], &mpfunc_ctx->so->ctx->calibration_config); + assert(!isnan(out[0])); + assert(!isnan(out[1])); + deviates[i * 2 + 0] = out[0] - mpfunc_ctx->meas[i * 2 + 0]; + deviates[i * 2 + 1] = out[1] - mpfunc_ctx->meas[i * 2 + 1]; + + if (derivs) { + FLT out[7 * 2]; + survive_reproject_full_jac_obj_pose( + out, pose, mpfunc_ctx->pts3d + i * 3, &mpfunc_ctx->camera_params[mpfunc_ctx->lh[i]], + &mpfunc_ctx->so->ctx->bsd[mpfunc_ctx->lh[i]], &mpfunc_ctx->so->ctx->calibration_config); + + for (int j = 0; j < n; j++) { + derivs[j][i * 2 + 0] = out[j]; + derivs[j][i * 2 + 1] = out[j + 7]; + } + } + } + + return 0; +} + +static double run_mpfit_find_3d_structure(MPFITData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, + int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/, + SurvivePose *out) { + double *covx = 0; + SurviveObject *so = d->opt.so; + + mpfit_context mpfitctx = {.so = so, + .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + .meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES), + .lh = alloca(sizeof(int) * so->sensor_ct * NUM_LIGHTHOUSES), + .pts3d = alloca(sizeof(double) * 3 * so->sensor_ct * NUM_LIGHTHOUSES)}; + + size_t meas_size = 2 * construct_input_from_scene(d, pdl, scene, mpfitctx.meas, mpfitctx.pts3d, mpfitctx.lh); + + static int failure_count = 500; + bool hasAllBSDs = true; + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) + hasAllBSDs &= so->ctx->bsd[lh].PositionSet; + + if (!hasAllBSDs || meas_size < d->required_meas) { + if (hasAllBSDs && failure_count++ == 500) { + SurviveContext *ctx = so->ctx; + SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); + failure_count = 0; + } + if (meas_size < d->required_meas) { + d->stats.meas_failures++; + } + return -1; + } + failure_count = 0; + + SurvivePose soLocation = {}; + + if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { + return -1; + } + + mp_result result = {}; + mp_par pars[7] = {}; + pars[0].parname = "X"; + pars[1].parname = "Y"; + pars[2].parname = "Z"; + pars[3].parname = "w"; + pars[4].parname = "i"; + pars[5].parname = "j"; + pars[6].parname = "k"; + + const bool debug_jacobian = false; + if (d->use_jacobian_function) { + for (int i = 0; i < 7; i++) { + if (debug_jacobian) { + pars[i].side = 1; + pars[i].deriv_debug = 1; + } else { + pars[i].side = 3; + } + } + } + + int res = mpfit(mpfunc, meas_size, 7, soLocation.Pos, pars, 0, &mpfitctx, &result); + + double rtn = -1; + bool status_failure = res <= 0; + bool error_failure = !general_optimizer_data_record_success(&d->opt, result.bestnorm); + if (!status_failure && !error_failure) { + quatnormalize(soLocation.Rot, soLocation.Rot); + *out = soLocation; + rtn = result.bestnorm; + } + + return rtn; +} + +int PoserMPFIT(SurviveObject *so, PoserData *pd) { + SurviveContext *ctx = so->ctx; + if (so->PoserData == 0) { + so->PoserData = calloc(1, sizeof(MPFITData)); + MPFITData *d = so->PoserData; + + general_optimizer_data_init(&d->opt, so); + d->useIMU = (bool)survive_configi(ctx, "mpfit-use-imu", SC_GET, 1); + d->required_meas = survive_configi(ctx, "mpfit-required-meas", SC_GET, 8); + + d->sensor_time_window = + survive_configi(ctx, "mpfit-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); + d->use_jacobian_function = survive_configi(ctx, "mpfit-use-jacobian-function", SC_GET, 1.0); + + SV_INFO("Initializing MPFIT:"); + SV_INFO("\tmpfit-required-meas: %d", d->required_meas); + SV_INFO("\tmpfit-time-window: %d", d->sensor_time_window); + SV_INFO("\tmpfit-use-imu: %d", d->useIMU); + SV_INFO("\tmpfit-use-jacobian-function: %d", d->use_jacobian_function); + } + MPFITData *d = so->PoserData; + switch (pd->pt) { + case POSERDATA_LIGHT: { + // No poses if calibration is ongoing + if (ctx->calptr && ctx->calptr->stage < 5) + 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_mpfit_find_3d_structure(d, lightData, scene, 100, .5, &estimate); + + d->last_lh = lightData->lh; + d->last_acode = lightData->acode; + + if (error > 0) { + if (d->useIMU) { + 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); + } + } + return 0; + } + + case POSERDATA_DISASSOCIATE: { + SV_INFO("MPFIT stats:"); + SV_INFO("\tmeas failures %d", d->stats.meas_failures); + general_optimizer_data_dtor(&d->opt); + free(d); + so->PoserData = 0; + return 0; + } + case POSERDATA_IMU: { + + PoserDataIMU *imu = (PoserDataIMU *)pd; + if (ctx->calptr && ctx->calptr->stage < 5) { + } else if (d->useIMU) { + survive_imu_tracker_integrate(so, &d->tracker, imu); + PoserData_poser_pose_func(pd, so, &d->tracker.pose); + } + + general_optimizer_data_record_imu(&d->opt, imu); + } + } + return -1; +} + +REGISTER_LINKTIME(PoserMPFIT); -- cgit v1.2.3 From 7f4385538d6b19ea5dd3f00c4a0eb49385798031 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Apr 2018 09:33:40 -0600 Subject: Made new posers work in windows --- src/poser_epnp.c | 2 +- src/poser_general_optimizer.c | 3 ++- src/poser_mpfit.c | 15 ++++----------- src/poser_sba.c | 2 +- src/survive.c | 2 ++ winbuild/libsurvive/libsurvive.vcxproj | 6 ++++++ winbuild/libsurvive/libsurvive.vcxproj.filters | 18 ++++++++++++++++++ 7 files changed, 34 insertions(+), 14 deletions(-) diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 615c01c..851ce29 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { case POSERDATA_LIGHT: { PoserDataLight *lightData = (PoserDataLight *)pd; - SurvivePose posers[2] = {}; + SurvivePose posers[2] = { 0 }; int meas[2] = {0, 0}; for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { if (so->ctx->bsd[lh].PositionSet) { diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c index 313e378..b3c4e65 100644 --- a/src/poser_general_optimizer.c +++ b/src/poser_general_optimizer.c @@ -1,6 +1,7 @@ #include "poser_general_optimizer.h" #include "string.h" -#include + +#include #include #include diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index 03ef522..439d151 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -167,22 +167,15 @@ static double run_mpfit_find_3d_structure(MPFITData *d, PoserDataLight *pdl, Sur } failure_count = 0; - SurvivePose soLocation = {}; + SurvivePose soLocation = { 0 }; if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { return -1; } - mp_result result = {}; - mp_par pars[7] = {}; - pars[0].parname = "X"; - pars[1].parname = "Y"; - pars[2].parname = "Z"; - pars[3].parname = "w"; - pars[4].parname = "i"; - pars[5].parname = "j"; - pars[6].parname = "k"; - + mp_result result = { 0 }; + mp_par pars[7] = { 0 }; + const bool debug_jacobian = false; if (d->use_jacobian_function) { for (int i = 0; i < 7; i++) { diff --git a/src/poser_sba.c b/src/poser_sba.c index 4c7fcd1..5166951 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -230,7 +230,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } failure_count = 0; - SurvivePose soLocation = {}; + SurvivePose soLocation = { 0 }; if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { return -1; diff --git a/src/survive.c b/src/survive.c index 9e750f9..1ab09da 100644 --- a/src/survive.c +++ b/src/survive.c @@ -114,6 +114,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { MANUAL_DRIVER_REGISTRATION(PoserDummy) MANUAL_DRIVER_REGISTRATION(PoserEPNP) MANUAL_DRIVER_REGISTRATION(PoserSBA) + MANUAL_DRIVER_REGISTRATION(PoserCharlesRefine) + MANUAL_DRIVER_REGISTRATION(PoserMPFIT) MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive) MANUAL_DRIVER_REGISTRATION(DriverRegPlayback) diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index fb1b9a5..8a0aaf1 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -244,6 +244,7 @@ + @@ -254,10 +255,13 @@ + + + @@ -288,11 +292,13 @@ + + diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index d06f083..019631b 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -138,6 +138,18 @@ Source Files + + Source Files + + + Source Files + + + Source Files + + + Source Files + @@ -191,6 +203,12 @@ Source Files + + Source Files + + + Source Files + -- cgit v1.2.3 From 42462d9253229835800814519593d6af0b5fbfc4 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Apr 2018 09:06:00 -0600 Subject: Added utility function to linmath --- redist/linmath.c | 7 +++++++ redist/linmath.h | 1 + 2 files changed, 8 insertions(+) diff --git a/redist/linmath.c b/redist/linmath.c index 50baeab..f4c3635 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -173,6 +173,13 @@ inline void axisanglefromquat(FLT *angle, FLT *axis, FLT *q) { // Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman) // Under the mit/X11 license. +inline void quatset(LinmathQuat q, FLT w, FLT x, FLT y, FLT z) { + q[0] = w; + q[1] = x; + q[2] = y; + q[3] = z; +} + inline void quatsetnone(LinmathQuat q) { q[0] = 1; q[1] = 0; diff --git a/redist/linmath.h b/redist/linmath.h index 60cc9d6..29db5a9 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -99,6 +99,7 @@ LINMATH_EXPORT void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat); typedef FLT LinmathEulerAngle[3]; +LINMATH_EXPORT void quatset(LinmathQuat q, FLT w, FLT x, FLT y, FLT z); LINMATH_EXPORT void quatsetnone(LinmathQuat q); LINMATH_EXPORT void quatcopy(LinmathQuat q, const LinmathQuat qin); LINMATH_EXPORT void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler); -- cgit v1.2.3 From c5d07ca78a7d9a14c70b7bd6deaa7ccbbf48661c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Apr 2018 09:07:40 -0600 Subject: Updated tool to proper cb sig --- tools/findoptimalconfig/findoptimalconfig.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/tools/findoptimalconfig/findoptimalconfig.cc b/tools/findoptimalconfig/findoptimalconfig.cc index b94590f..265fd94 100644 --- a/tools/findoptimalconfig/findoptimalconfig.cc +++ b/tools/findoptimalconfig/findoptimalconfig.cc @@ -9,7 +9,6 @@ #include #include -#include struct SBAData { int last_acode = -1; @@ -93,8 +92,8 @@ 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) { - survive_default_raw_pose_process(so, lighthouse, pose); +void pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { + survive_default_raw_pose_process(so, timecode, pose); PlaybackData *d = (PlaybackData *)so->ctx->user_ptr; d->so = so; d->inputs.emplace_back(so, *pose); @@ -364,7 +363,7 @@ int main(int argc, char **argv) { auto ctx = survive_init(sizeof(args) / sizeof(args[0]), (char *const *)args); ctx->user_ptr = &data; - survive_install_raw_pose_fn(ctx, raw_pose_process); + survive_install_pose_fn(ctx, pose_process); survive_install_lighthouse_pose_fn(ctx, lighthouse_process); survive_install_light_fn(ctx, light_process); -- cgit v1.2.3 From d7c88592a5450a65f5359e23d87122a04d019503 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Apr 2018 21:58:25 -0600 Subject: New impl of mahoney imu filter --- include/libsurvive/survive_imu.h | 2 +- src/survive_imu.c | 141 ++++++++++++--------------------------- 2 files changed, 44 insertions(+), 99 deletions(-) diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 11635aa..508710a 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -27,7 +27,7 @@ typedef struct { FLT P[7]; // estimate variance - float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki + LinmathVec3d integralFB; } SurviveIMUTracker; diff --git a/src/survive_imu.c b/src/survive_imu.c index 8f4266a..9cd0322 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -1,107 +1,56 @@ #include "survive_imu.h" #include "linmath.h" +#include "math.h" #include "survive_internal.h" +#include #include -//--------------------------------------------------------------------------------------------------- -// Definitions +// Mahoney is due to https://hal.archives-ouvertes.fr/hal-00488376/document +// See also http://www.olliw.eu/2013/imu-data-fusing/#chapter41 and +// http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +static void mahony_ahrs(SurviveIMUTracker *tracker, LinmathVec3d _gyro, LinmathVec3d _accel) { + LinmathVec3d gyro; + memcpy(gyro, _gyro, 3 * sizeof(FLT)); -#define sampleFreq 240.0f // sample frequency in Hz -#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain -#define twoKiDef (2.0f * 0.0f) // 2 * integral gain + LinmathVec3d accel; + memcpy(accel, _accel, 3 * sizeof(FLT)); -//--------------------------------------------------------------------------------------------------- -// Function declarations + const FLT sample_f = 240; + const FLT prop_gain = .5; + const FLT int_gain = 0; -float invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long *)&y; - i = 0x5f3759df - (i >> 1); - y = *(float *)&i; - y = y * (1.5f - (halfx * y * y)); - return y; -} -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update -// From http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ -static void MahonyAHRSupdateIMU(SurviveIMUTracker *tracker, float gx, float gy, float gz, float ax, float ay, - float az) { - float recipNorm; - float halfvx, halfvy, halfvz; - float halfex, halfey, halfez; - float qa, qb, qc; - - const float twoKp = twoKpDef; // 2 * proportional gain (Kp) - const float twoKi = twoKiDef; // 2 * integral gain (Ki) - - float q0 = tracker->pose.Rot[0]; - float q1 = tracker->pose.Rot[1]; - float q2 = tracker->pose.Rot[2]; - float q3 = tracker->pose.Rot[3]; - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Estimated direction of gravity and vector perpendicular to magnetic flux - halfvx = q1 * q3 - q0 * q2; - halfvy = q0 * q1 + q2 * q3; - halfvz = q0 * q0 - 0.5f + q3 * q3; - - // Error is sum of cross product between estimated and measured direction of gravity - halfex = (ay * halfvz - az * halfvy); - halfey = (az * halfvx - ax * halfvz); - halfez = (ax * halfvy - ay * halfvx); - - // Compute and apply integral feedback if enabled - if (twoKi > 0.0f) { - tracker->integralFBx += twoKi * halfex * (1.0f / sampleFreq); // tracker->integral error scaled by Ki - tracker->integralFBy += twoKi * halfey * (1.0f / sampleFreq); - tracker->integralFBz += twoKi * halfez * (1.0f / sampleFreq); - gx += tracker->integralFBx; // apply tracker->integral feedback - gy += tracker->integralFBy; - gz += tracker->integralFBz; - } else { - tracker->integralFBx = 0.0f; // prevent tracker->integral windup - tracker->integralFBy = 0.0f; - tracker->integralFBz = 0.0f; + FLT *q = tracker->pose.Rot; + + FLT mag_accel = magnitude3d(accel); + + if (mag_accel != 0.0) { + scale3d(accel, accel, 1. / mag_accel); + + // Equiv of q^-1 * G + LinmathVec3d v = {q[1] * q[3] - q[0] * q[2], q[0] * q[1] + q[2] * q[3], q[0] * q[0] - 0.5 + q[3] * q[3]}; + + LinmathVec3d error; + cross3d(error, accel, v); + + if (int_gain > 0.0f) { + LinmathVec3d fb_correction; + scale3d(fb_correction, error, int_gain * 2. / sample_f); + add3d(tracker->integralFB, tracker->integralFB, fb_correction); + add3d(gyro, gyro, tracker->integralFB); } - // Apply proportional feedback - gx += twoKp * halfex; - gy += twoKp * halfey; - gz += twoKp * halfez; + scale3d(error, error, prop_gain * 2.); + add3d(gyro, gyro, error); } - // Integrate rate of change of quaternion - gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors - gy *= (0.5f * (1.0f / sampleFreq)); - gz *= (0.5f * (1.0f / sampleFreq)); - qa = q0; - qb = q1; - qc = q2; - q0 += (-qb * gx - qc * gy - q3 * gz); - q1 += (qa * gx + qc * gz - q3 * gy); - q2 += (qa * gy - qb * gz + q3 * gx); - q3 += (qa * gz + qb * gy - qc * gx); - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - tracker->pose.Rot[0] = q0; - tracker->pose.Rot[1] = q1; - tracker->pose.Rot[2] = q2; - tracker->pose.Rot[3] = q3; + scale3d(gyro, gyro, 0.5 / sample_f); + + LinmathQuat correction = { + (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]), (q[0] * gyro[0] + q[2] * gyro[2] - q[3] * gyro[1]), + (q[0] * gyro[1] - q[1] * gyro[2] + q[3] * gyro[0]), (q[0] * gyro[2] + q[1] * gyro[1] - q[2] * gyro[0])}; + + quatadd(q, q, correction); + quatnormalize(q, q); } static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) { @@ -125,7 +74,6 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, } //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; - tracker->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0; tracker->lastGTTime = timecode; tracker->lastGT = *pose; } @@ -198,10 +146,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; } - float gx = data->gyro[0], gy = data->gyro[1], gz = data->gyro[2]; - float ax = data->accel[0], ay = data->accel[1], az = data->accel[2]; - - MahonyAHRSupdateIMU(tracker, gx, gy, gz, ax, ay, az); + mahony_ahrs(tracker, data->gyro, data->accel); FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz; @@ -270,4 +215,4 @@ void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timec tracker->lastGTTime = timecode; tracker->lastGT = tracker->pose; -} \ No newline at end of file +} -- cgit v1.2.3 From 30cfb87ec0d95e0cb8a671cf8f2327b4204927ed Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Apr 2018 23:44:36 -0600 Subject: Renamed api; fixed various warnings --- api_example.c | 25 +++++----- include/libsurvive/survive_api.h | 104 +++++++++++++++++++-------------------- src/survive_api.c | 98 +++++++++++++++++------------------- 3 files changed, 111 insertions(+), 116 deletions(-) diff --git a/api_example.c b/api_example.c index b0c9e4c..1f06740 100644 --- a/api_example.c +++ b/api_example.c @@ -4,31 +4,32 @@ #include int main(int argc, char **argv) { - SurviveAsyncContext *actx = survive_async_init(argc, argv); + SurviveSimpleContext *actx = survive_simple_init(argc, argv); if (actx == 0) // implies -help or similiar return 0; - survive_async_start_thread(actx); + survive_simple_start_thread(actx); - while (survive_async_is_running(actx)) { + while (survive_simple_is_running(actx)) { OGUSleep(30000); SurvivePose pose; - for (const SurviveAsyncObject *it = survive_async_get_first_object(actx); it != 0; - it = survive_async_get_next_object(actx, it)) { - uint32_t timecode = survive_async_object_get_latest_pose(it, &pose); - printf("%s (%u): %f %f %f %f %f %f %f\n", survive_async_object_name(it), timecode, - pose.Pos[0], pose.Pos[1], pose.Pos[2], pose.Rot[0], pose.Rot[1], pose.Rot[2], pose.Rot[3]); + for (const SurviveSimpleObject *it = survive_simple_get_first_object(actx); it != 0; + it = survive_simple_get_next_object(actx, it)) { + uint32_t timecode = survive_simple_object_get_latest_pose(it, &pose); + printf("%s (%u): %f %f %f %f %f %f %f\n", survive_simple_object_name(it), timecode, pose.Pos[0], + pose.Pos[1], pose.Pos[2], pose.Rot[0], pose.Rot[1], pose.Rot[2], pose.Rot[3]); } OGUSleep(30000); - for (const SurviveAsyncObject* it = survive_async_get_next_updated(actx); it != 0; it = survive_async_get_next_updated(actx)) { - printf("%s changed since last checked\n", survive_async_object_name(it)); + for (const SurviveSimpleObject *it = survive_simple_get_next_updated(actx); it != 0; + it = survive_simple_get_next_updated(actx)) { + printf("%s changed since last checked\n", survive_simple_object_name(it)); } } - - survive_async_close(actx); + + survive_simple_close(actx); return 0; } diff --git a/include/libsurvive/survive_api.h b/include/libsurvive/survive_api.h index fc4ce2c..64d1271 100644 --- a/include/libsurvive/survive_api.h +++ b/include/libsurvive/survive_api.h @@ -5,58 +5,58 @@ extern "C" { #endif - struct SurviveAsyncContext; - typedef struct SurviveAsyncContext SurviveAsyncContext; - - /*** - * Initialize a new instance of an async context -- mirrors survive_init - * @return Pointer to the async context - */ - SURVIVE_EXPORT SurviveAsyncContext *survive_async_init(int argc, char *const *argv); - - /** - * Close all devices and free all memory associated with the given context - */ - SURVIVE_EXPORT void survive_async_close(SurviveAsyncContext* actx); - - /** - * Start the background thread which processes various inputs and produces deliverable data like position. - */ - SURVIVE_EXPORT void survive_async_start_thread(SurviveAsyncContext* actx); - - /** - * @return true iff the background thread is still running - */ - SURVIVE_EXPORT bool survive_async_is_running(SurviveAsyncContext *actx); - - struct SurviveAsyncObject; - typedef struct SurviveAsyncObject SurviveAsyncObject; - - /** - * Get the first known object. Note that this also includes lighthouses - */ - SURVIVE_EXPORT const SurviveAsyncObject *survive_async_get_first_object(SurviveAsyncContext *actx); - /** - * Get the next known object from a current one. - */ - SURVIVE_EXPORT const SurviveAsyncObject *survive_async_get_next_object(SurviveAsyncContext *actx, - const SurviveAsyncObject *curr); - - /** - * Gets the next object which has been updated since we last looked at it with this function - */ - SURVIVE_EXPORT const SurviveAsyncObject *survive_async_get_next_updated(SurviveAsyncContext *actx); - - /** - * Gets the pose of a given object - */ - SURVIVE_EXPORT survive_timecode survive_async_object_get_latest_pose(const SurviveAsyncObject *sao, - SurvivePose *pose); - - /** - * Gets the null terminated name of the object. - */ - SURVIVE_EXPORT const char *survive_async_object_name(const SurviveAsyncObject *sao); +struct SurviveSimpleContext; +typedef struct SurviveSimpleContext SurviveSimpleContext; + +/*** + * Initialize a new instance of an simple context -- mirrors survive_init + * @return Pointer to the simple context + */ +SURVIVE_EXPORT SurviveSimpleContext *survive_simple_init(int argc, char *const *argv); + +/** + * Close all devices and free all memory associated with the given context + */ +SURVIVE_EXPORT void survive_simple_close(SurviveSimpleContext *actx); + +/** + * Start the background thread which processes various inputs and produces deliverable data like position. + */ +SURVIVE_EXPORT void survive_simple_start_thread(SurviveSimpleContext *actx); + +/** + * @return true iff the background thread is still running + */ +SURVIVE_EXPORT bool survive_simple_is_running(SurviveSimpleContext *actx); + +struct SurviveSimpleObject; +typedef struct SurviveSimpleObject SurviveSimpleObject; + +/** + * Get the first known object. Note that this also includes lighthouses + */ +SURVIVE_EXPORT const SurviveSimpleObject *survive_simple_get_first_object(SurviveSimpleContext *actx); +/** + * Get the next known object from a current one. + */ +SURVIVE_EXPORT const SurviveSimpleObject *survive_simple_get_next_object(SurviveSimpleContext *actx, + const SurviveSimpleObject *curr); + +/** + * Gets the next object which has been updated since we last looked at it with this function + */ +SURVIVE_EXPORT const SurviveSimpleObject *survive_simple_get_next_updated(SurviveSimpleContext *actx); + +/** + * Gets the pose of a given object + */ +SURVIVE_EXPORT survive_timecode survive_simple_object_get_latest_pose(const SurviveSimpleObject *sao, + SurvivePose *pose); + +/** + * Gets the null terminated name of the object. + */ +SURVIVE_EXPORT const char *survive_simple_object_name(const SurviveSimpleObject *sao); #ifdef __cplusplus } diff --git a/src/survive_api.c b/src/survive_api.c index e2c3d54..a39524b 100644 --- a/src/survive_api.c +++ b/src/survive_api.c @@ -3,13 +3,10 @@ #include "survive.h" #include "stdio.h" -struct SurviveAsyncObject { - struct SurviveAsyncContext* actx; - - enum SurviveAsyncObject_type { - SurviveAsyncObject_LIGHTHOUSE, - SurviveAsyncObject_OBJECT - } type; +struct SurviveSimpleObject { + struct SurviveSimpleContext *actx; + + enum SurviveSimpleObject_type { SurviveSimpleObject_LIGHTHOUSE, SurviveSimpleObject_OBJECT } type; union { int lighthouse; @@ -20,7 +17,7 @@ struct SurviveAsyncObject { bool has_update; }; -struct SurviveAsyncContext { +struct SurviveSimpleContext { SurviveContext* ctx; bool running; @@ -28,21 +25,21 @@ struct SurviveAsyncContext { og_mutex_t poll_mutex; size_t object_ct; - struct SurviveAsyncObject objects[]; + struct SurviveSimpleObject objects[]; }; static void pose_fn(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { - struct SurviveAsyncContext *actx = so->ctx->user_ptr; + struct SurviveSimpleContext *actx = so->ctx->user_ptr; OGLockMutex(actx->poll_mutex); - survive_default_raw_pose_process(so, timecode, pose); + survive_default_raw_pose_process(so, timecode, pose); - int idx = (int)so->user_ptr; + intptr_t idx = (intptr_t)so->user_ptr; actx->objects[idx].has_update = true; OGUnlockMutex(actx->poll_mutex); } static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose, SurvivePose *object_pose) { - struct SurviveAsyncContext *actx = ctx->user_ptr; + struct SurviveSimpleContext *actx = ctx->user_ptr; OGLockMutex(actx->poll_mutex); survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); @@ -51,7 +48,7 @@ static void lh_fn(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthou OGUnlockMutex(actx->poll_mutex); } -struct SurviveAsyncContext *survive_async_init(int argc, char *const *argv) { +struct SurviveSimpleContext *survive_simple_init(int argc, char *const *argv) { SurviveContext* ctx = survive_init(argc, argv); if (ctx == 0) return 0; @@ -59,25 +56,26 @@ struct SurviveAsyncContext *survive_async_init(int argc, char *const *argv) { survive_startup(ctx); int object_ct = ctx->activeLighthouses + ctx->objs_ct; - struct SurviveAsyncContext * actx = calloc(1, sizeof(struct SurviveAsyncContext) + sizeof(struct SurviveAsyncObject) * object_ct ); + struct SurviveSimpleContext *actx = + calloc(1, sizeof(struct SurviveSimpleContext) + sizeof(struct SurviveSimpleObject) * object_ct); actx->object_ct = object_ct; actx->ctx = ctx; actx->poll_mutex = OGCreateMutex(); ctx->user_ptr = actx; - int i = 0; + intptr_t i = 0; for (i = 0; i < ctx->activeLighthouses; i++) { - struct SurviveAsyncObject* obj = &actx->objects[i]; + struct SurviveSimpleObject *obj = &actx->objects[i]; obj->data.lighthouse = i; - obj->type = SurviveAsyncObject_LIGHTHOUSE; + obj->type = SurviveSimpleObject_LIGHTHOUSE; obj->actx = actx; obj->has_update = ctx->bsd[i].PositionSet; - snprintf(obj->name, 32, "LH%d", i); + snprintf(obj->name, 32, "LH%ld", i); } for (; i < object_ct; i++) { - struct SurviveAsyncObject* obj = &actx->objects[i]; + struct SurviveSimpleObject *obj = &actx->objects[i]; int so_idx = i - ctx->activeLighthouses; obj->data.so = ctx->objs[so_idx]; - obj->type = SurviveAsyncObject_OBJECT; + obj->type = SurviveSimpleObject_OBJECT; obj->actx = actx; obj->data.so->user_ptr = (void*)i; snprintf(obj->name, 32, "%s", obj->data.so->codename); @@ -88,53 +86,52 @@ struct SurviveAsyncContext *survive_async_init(int argc, char *const *argv) { return actx; } -void survive_async_close(struct SurviveAsyncContext* actx) { +int survive_simple_stop_thread(struct SurviveSimpleContext *actx) { + actx->running = false; + intptr_t error = (intptr_t)OGJoinThread(actx->thread); + if (error != 0) { + SurviveContext *ctx = actx->ctx; + SV_INFO("Warning: Loope exited with error %ld", error); + } + return error; +} + +void survive_simple_close(struct SurviveSimpleContext *actx) { if (actx->running) { - survive_async_stop_thread(actx); + survive_simple_stop_thread(actx); } survive_close(actx->ctx); } -static inline void* __async_thread(void* _actx) { - struct SurviveAsyncContext* actx = _actx; - int error = 0; +static inline void *__simple_thread(void *_actx) { + struct SurviveSimpleContext *actx = _actx; + intptr_t error = 0; while (actx->running && error == 0) { error = survive_poll(actx->ctx); } actx->running = false; return (void*)error; } -bool survive_async_is_running(struct SurviveAsyncContext* actx) { - return actx->running; -} -void survive_async_start_thread(struct SurviveAsyncContext* actx) { +bool survive_simple_is_running(struct SurviveSimpleContext *actx) { return actx->running; } +void survive_simple_start_thread(struct SurviveSimpleContext *actx) { actx->running = true; - actx->thread = OGCreateThread(__async_thread, actx); -} -int survive_async_stop_thread(struct SurviveAsyncContext* actx) { - actx->running = false; - int error = (int)OGJoinThread(actx->thread); - if (error != 0) { - SurviveContext* ctx = actx->ctx; - SV_INFO("Warning: Loope exited with error %d", error); - } - return error; + actx->thread = OGCreateThread(__simple_thread, actx); } -const struct SurviveAsyncObject *survive_async_get_next_object(struct SurviveAsyncContext *actx, - const struct SurviveAsyncObject *curr) { - const struct SurviveAsyncObject* next = curr + 1; +const struct SurviveSimpleObject *survive_simple_get_next_object(struct SurviveSimpleContext *actx, + const struct SurviveSimpleObject *curr) { + const struct SurviveSimpleObject *next = curr + 1; if (next >= actx->objects + actx->object_ct) return 0; return next; } -const struct SurviveAsyncObject *survive_async_get_first_object(struct SurviveAsyncContext *actx) { +const struct SurviveSimpleObject *survive_simple_get_first_object(struct SurviveSimpleContext *actx) { return actx->objects; } -const struct SurviveAsyncObject* survive_async_get_next_updated(struct SurviveAsyncContext* actx) { +const struct SurviveSimpleObject *survive_simple_get_next_updated(struct SurviveSimpleContext *actx) { for (int i = 0; i < actx->object_ct; i++) { if (actx->objects[i].has_update) { actx->objects[i].has_update = false; @@ -144,17 +141,17 @@ const struct SurviveAsyncObject* survive_async_get_next_updated(struct SurviveAs return 0; } -uint32_t survive_async_object_get_latest_pose(const struct SurviveAsyncObject* sao, SurvivePose* pose) { +uint32_t survive_simple_object_get_latest_pose(const struct SurviveSimpleObject *sao, SurvivePose *pose) { uint32_t timecode = 0; OGLockMutex(sao->actx->poll_mutex); switch (sao->type) { - case SurviveAsyncObject_LIGHTHOUSE: { + case SurviveSimpleObject_LIGHTHOUSE: { if(pose) *pose = sao->actx->ctx->bsd[sao->data.lighthouse].Pose; break; } - case SurviveAsyncObject_OBJECT: + case SurviveSimpleObject_OBJECT: if(pose) *pose = sao->data.so->OutPose; timecode = sao->data.so->OutPose_timecode; @@ -165,7 +162,4 @@ uint32_t survive_async_object_get_latest_pose(const struct SurviveAsyncObject* s return timecode; } -const char * survive_async_object_name(const SurviveAsyncObject * sao) -{ - return sao->name; -} +const char *survive_simple_object_name(const SurviveSimpleObject *sao) { return sao->name; } -- cgit v1.2.3 From 5f429662488533ff4f4384b678ae738244972cc6 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Apr 2018 23:48:36 -0600 Subject: Updated naming of python binding --- bindings/python/example.py | 2 +- bindings/python/libsurvive.py | 56 +++++++++++++++++++++---------------------- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/bindings/python/example.py b/bindings/python/example.py index 5454fbc..32d58de 100644 --- a/bindings/python/example.py +++ b/bindings/python/example.py @@ -1,7 +1,7 @@ import libsurvive import sys -actx = libsurvive.AsyncContext(sys.argv) +actx = libsurvive.SimpleContext(sys.argv) for obj in actx.Objects(): print(obj.Name()) diff --git a/bindings/python/libsurvive.py b/bindings/python/libsurvive.py index e88657a..653eb60 100644 --- a/bindings/python/libsurvive.py +++ b/bindings/python/libsurvive.py @@ -4,26 +4,26 @@ _libsurvive = ctypes.CDLL('./_libsurvive.so') LP_c_char = ctypes.POINTER(ctypes.c_char) LP_LP_c_char = ctypes.POINTER(LP_c_char) -_libsurvive.survive_async_init.argtypes = (ctypes.c_int, LP_LP_c_char) # argc, argv -_libsurvive.survive_async_init.restype = ctypes.c_void_p +_libsurvive.survive_simple_init.argtypes = (ctypes.c_int, LP_LP_c_char) # argc, argv +_libsurvive.survive_simple_init.restype = ctypes.c_void_p -_libsurvive.survive_async_get_next_object.argtypes = [ctypes.c_void_p, ctypes.c_void_p] -_libsurvive.survive_async_get_next_object.restype = ctypes.c_void_p +_libsurvive.survive_simple_get_next_object.argtypes = [ctypes.c_void_p, ctypes.c_void_p] +_libsurvive.survive_simple_get_next_object.restype = ctypes.c_void_p -_libsurvive.survive_async_get_first_object.argtypes = [ctypes.c_void_p] -_libsurvive.survive_async_get_first_object.restype = ctypes.c_void_p +_libsurvive.survive_simple_get_first_object.argtypes = [ctypes.c_void_p] +_libsurvive.survive_simple_get_first_object.restype = ctypes.c_void_p -_libsurvive.survive_async_get_next_updated.argtypes = [ctypes.c_void_p] -_libsurvive.survive_async_get_next_updated.restype = ctypes.c_void_p +_libsurvive.survive_simple_get_next_updated.argtypes = [ctypes.c_void_p] +_libsurvive.survive_simple_get_next_updated.restype = ctypes.c_void_p -_libsurvive.survive_async_object_name.argtypes = [ ctypes.c_void_p ] -_libsurvive.survive_async_object_name.restype = ctypes.c_char_p +_libsurvive.survive_simple_object_name.argtypes = [ ctypes.c_void_p ] +_libsurvive.survive_simple_object_name.restype = ctypes.c_char_p -_libsurvive.survive_async_is_running.argtypes = [ctypes.c_void_p] -_libsurvive.survive_async_is_running.restype = ctypes.c_bool +_libsurvive.survive_simple_is_running.argtypes = [ctypes.c_void_p] +_libsurvive.survive_simple_is_running.restype = ctypes.c_bool -_libsurvive.survive_async_start_thread.argtypes = [ctypes.c_void_p] -_libsurvive.survive_async_start_thread.restype = None +_libsurvive.survive_simple_start_thread.argtypes = [ctypes.c_void_p] +_libsurvive.survive_simple_start_thread.restype = None class SurvivePose(ctypes.Structure): _fields_ = [ @@ -34,23 +34,23 @@ class SurvivePose(ctypes.Structure): return '[{0} {1} {2}], [{3} {4} {5} {6}]'.format(self.Pos[0],self.Pos[1],self.Pos[2],self.Rot[0],self.Rot[1],self.Rot[2],self.Rot[3]) -_libsurvive.survive_async_object_get_latest_pose.argtypes = [ctypes.c_void_p, ctypes.POINTER(SurvivePose)] -_libsurvive.survive_async_object_get_latest_pose.restype = ctypes.c_uint +_libsurvive.survive_simple_object_get_latest_pose.argtypes = [ctypes.c_void_p, ctypes.POINTER(SurvivePose)] +_libsurvive.survive_simple_object_get_latest_pose.restype = ctypes.c_uint -class AsyncObject: +class SimpleObject: ptr = 0 def __init__(self, ptr): self.ptr = ptr def Name(self): - return _libsurvive.survive_async_object_name(self.ptr) + return _libsurvive.survive_simple_object_name(self.ptr) def Pose(self): pose = SurvivePose() - time = _libsurvive.survive_async_object_get_latest_pose(self.ptr, pose) + time = _libsurvive.survive_simple_object_get_latest_pose(self.ptr, pose) return (pose, time) -class AsyncContext: +class SimpleContext: ptr = 0 objects = [] @@ -60,24 +60,24 @@ class AsyncContext: for i, arg in enumerate(args): enc_arg = arg.encode('utf-8') argv[i] = ctypes.create_string_buffer(enc_arg) - self.ptr = _libsurvive.survive_async_init(argc, argv) + self.ptr = _libsurvive.survive_simple_init(argc, argv) self.objects = [] - curr = _libsurvive.survive_async_get_first_object(self.ptr); + curr = _libsurvive.survive_simple_get_first_object(self.ptr); while curr: - self.objects.append(AsyncObject(curr)) - curr = _libsurvive.survive_async_get_next_object(self.ptr, curr); - _libsurvive.survive_async_start_thread(self.ptr) + self.objects.append(SimpleObject(curr)) + curr = _libsurvive.survive_simple_get_next_object(self.ptr, curr); + _libsurvive.survive_simple_start_thread(self.ptr) def Objects(self): return self.objects def Running(self): - return _libsurvive.survive_async_is_running(self.ptr) + return _libsurvive.survive_simple_is_running(self.ptr) def NextUpdated(self): - ptr = _libsurvive.survive_async_get_next_updated(self.ptr) + ptr = _libsurvive.survive_simple_get_next_updated(self.ptr) if ptr: - return AsyncObject(ptr) + return SimpleObject(ptr) return None -- cgit v1.2.3 From f0b2fbc707050b7c8b3e5e8310737ed5bfbc291e Mon Sep 17 00:00:00 2001 From: Eric Schleicher Date: Wed, 11 Apr 2018 19:15:11 -0700 Subject: Added info about possible dependencies requirement added section showing from blinking cursor to get libsurvive, compile and run calibrate, including what to do if you run into missing dependency. --- README.md | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/README.md b/README.md index 8a28bb0..70a5f03 100644 --- a/README.md +++ b/README.md @@ -144,6 +144,17 @@ The limiting factor for Vive viability on a given computer is the maximum availa To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time). # Getting Started +``` +git clone https://github.com/cnlohr/libsurvive.git && cd libsurvive +make + +# If you get and error complaining about lapacke.h, you may need to install the following dependencies +sudo apt-get install liblapacke-dev libopenblas-dev libatlas-base-dev + +# Create calibration files for connected HMDs, Trackers. +# See below for more detailed information about the configuration files that the calibration process. +./calibrate +``` ## General Information -- cgit v1.2.3 From 7acbf4e2a2cf564b1792d0b67f483fdd20e19b3e Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 13 Apr 2018 10:20:27 -0600 Subject: Added FPV and reference shapes --- tools/viz/index.html | 3 +- tools/viz/survive_viewer.js | 102 +++++++++++++++++++++++++++++++++++--------- 2 files changed, 84 insertions(+), 21 deletions(-) diff --git a/tools/viz/index.html b/tools/viz/index.html index 2987555..4d8d65d 100644 --- a/tools/viz/index.html +++ b/tools/viz/index.html @@ -12,7 +12,8 @@
- Trails
+ Trails
+ FPV
diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index c1d613d..26f77a6 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -8,7 +8,7 @@ var oldDrawTime = 0; var timecode = {}; var oldPoseTime = 0, poseCnt = 0; var oldPose = [0, 0, 0]; -var scene, camera, renderer, floor; +var scene, camera, renderer, floor, fpv_camera; $(function() { $("#toggleBtn").click(function() { $("#cam").toggle(); }); }); @@ -193,26 +193,26 @@ function create_tracked_object(info) { var trails; var MAX_LINE_POINTS = 100000; -$(function() { - $("#trails").change(function() { - if (this.checked) { - var geometry = new THREE.Geometry(); - var material = new THREE.LineBasicMaterial({color : 0x305ea8}); +function update_trails() { + if (this.checked) { + var geometry = new THREE.Geometry(); + var material = new THREE.LineBasicMaterial({color : 0x305ea8}); - for (i = 0; i < MAX_LINE_POINTS; i++) { - geometry.vertices.push(new THREE.Vector3(0, 0, 0)); - } - geometry.dynamic = true; + for (i = 0; i < MAX_LINE_POINTS; i++) { + geometry.vertices.push(new THREE.Vector3(0, 0, 0)); + } + geometry.dynamic = true; - trails = new THREE.Line(geometry, material); + trails = new THREE.Line(geometry, material); - scene.add(trails); - } else { - if (trails) - scene.remove(trails); - } - }); -}); + scene.add(trails); + } else { + if (trails) + scene.remove(trails); + } +} + +$(function() { $("#trails").change(update_trails); }); var survive_log_handlers = { "LH_POSE" : function(v) { @@ -256,7 +256,18 @@ var survive_log_handlers = { oldPose = obj.position; } - } + if ("HMD" === obj.tracker) { + var up = new THREE.Vector3(0, 1, 0); + var out = new THREE.Vector3(0, 0, 1); + + fpv_camera.up = up.applyQuaternion(objs[obj.tracker].quaternion); + var lookAt = out.applyQuaternion(objs[obj.tracker].quaternion); + lookAt.add(objs[obj.tracker].position); + + fpv_camera.position.set(obj.position[0], obj.position[1], obj.position[2]); + fpv_camera.lookAt(lookAt); + } + } }, "CONFIG" : function(v, tracker) { var configStr = v.slice(3).join(' '); @@ -389,11 +400,57 @@ init() { camera = new THREE.PerspectiveCamera(VIEW_ANGLE, ASPECT, NEAR, FAR); camera.up = new THREE.Vector3(0, 0, 1); + fpv_camera = new THREE.PerspectiveCamera(VIEW_ANGLE, ASPECT, .1, FAR); + scene.add(fpv_camera); + // add the camera to the scene scene.add(camera); camera.position.set(5, 2, 5.00); camera.lookAt(scene.position); + for (var z = 0; z < 5; z++) { + for (var i = -4; i < 5; i++) { + for (var j = 0; j < 5; j++) { + var size = .1; + var geometry = new THREE.BoxGeometry(size, size, size); + + var cube = new THREE.Mesh(geometry, material); + var x, y, zz = z, color; + switch (j) { + case 0: + x = i; + y = 5; + color = 0xff; + break; + case 1: + x = i; + y = -5; + color = 0xff00; + break; + case 2: + x = 5; + y = i; + color = 0xff0000; + break; + case 3: + x = -5; + y = i; + color = 0xffffff; + break; + case 4: + x = 2 * z - 5; + y = i; + zz = 5; + color = 0xffff00; + break; + } + var material = new THREE.MeshStandardMaterial({color : color}); + cube.position.set(x, y, zz); + scene.add(cube); + } + } + } + ////////////// // RENDERER // ////////////// @@ -432,6 +489,8 @@ init() { var axes = new THREE.AxesHelper(5); scene.add(axes); + + update_trails.call($("#trails")[0]); } function animate() { @@ -441,4 +500,7 @@ function animate() { redrawCanvas(timecode); } -function render() { renderer.render(scene, camera); } +function render() { + var use_fpv = $("#fpv")[0].checked; + renderer.render(scene, use_fpv ? fpv_camera : camera); +} -- cgit v1.2.3 From 9533ddefcf8af901ebc0ff0b2ac503b5af2523b8 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 13 Apr 2018 14:45:16 -0600 Subject: Remove SurviveObject if they fail to configure; prevents issues calibrating with HMD when controllers are off --- include/libsurvive/survive.h | 1 + src/survive.c | 25 +++++++++++++++++++++++++ src/survive_vive.c | 1 + 3 files changed, 27 insertions(+) diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 93af4c0..66993df 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -327,6 +327,7 @@ void RegisterDriver(const char *name, void *data); // For device drivers to call. This actually attaches them. int survive_add_object(SurviveContext *ctx, SurviveObject *obj); +void survive_remove_object(SurviveContext *ctx, SurviveObject *obj); void survive_add_driver(SurviveContext *ctx, void *payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic); diff --git a/src/survive.c b/src/survive.c index 362adbc..bb1179e 100644 --- a/src/survive.c +++ b/src/survive.c @@ -404,6 +404,31 @@ int survive_add_object(SurviveContext *ctx, SurviveObject *obj) { return 0; } +void survive_remove_object(SurviveContext *ctx, SurviveObject *obj) { + int obj_idx = 0; + for (obj_idx = 0; obj_idx < ctx->objs_ct; obj_idx++) { + if (ctx->objs[obj_idx] == obj) + break; + } + + if (obj_idx == ctx->objs_ct) { + SV_INFO("Warning: Tried to remove un-added object %p(%s)", obj, obj->codename); + return; + } + + // Swap the last item into this items slot; this assumes order doesn't matter in this list + if (obj_idx != ctx->objs_ct - 1) { + ctx->objs[obj_idx] = ctx->objs[ctx->objs_ct - 1]; + } + + ctx->objs_ct--; + + // Blank out the spot; but this is only really necessary for diagnostic reasons -- presumably no one will ever read + // past the end of the list + ctx->objs[ctx->objs_ct] = 0; + + free(obj); +} void survive_add_driver(SurviveContext *ctx, void *payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic) { int oldct = ctx->driver_ct; diff --git a/src/survive_vive.c b/src/survive_vive.c index 0fae736..d9cbc3e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1660,6 +1660,7 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int if( len < 0 ) { + survive_remove_object(ctx, so); return len; } -- cgit v1.2.3 From 39a63badbb5864314a9d9e18c0871718ac5d2912 Mon Sep 17 00:00:00 2001 From: jdavidberger Date: Sat, 14 Apr 2018 23:40:00 -0600 Subject: Update README.md Added MPFIT details --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 70a5f03..afafc7b 100644 --- a/README.md +++ b/README.md @@ -119,6 +119,7 @@ Poser | [poser_octavioradii.c](src/poser_octavioradii.c) | A potentially very fa Poser | [poser_turveytori.c](src/poser_turveytori.c) | A moderately fast, fairly high precision poser that works by determine the angle at the lighthouse between many sets of two sensors. Using the inscirbed angle theorom, each set defines a torus of possible locations of the lighthouse. Multiple sets define multiple tori, and this poser finds most likely location of the lighthouse using least-squares distance. Best suited for calibration, but is can be used for real-time tracking on a powerful system. | [@mwturvey](https://github.com/mwturvey) Poser | [poser_epnp.c](src/poser_epnp.c) | Reasonably fast and accurate calibration and tracker that uses the [EPNP algorithm](https://en.wikipedia.org/wiki/Perspective-n-Point#EPnP) to solve the perspective and points problem. Suitable for fast tracking, but does best with >5-6 sensor readings. | [@jdavidberger](https://github.com/jdavidberger) Poser | [poser_sba.c](src/poser_sba.c) (default) | Reasonably fast and accurate calibration and tracker but is dependent on a 'seed' poser to give it an initial estimate. This then performs [bundle adjustment](https://en.wikipedia.org/wiki/Bundle_adjustment) to minimize reprojection error given both ligthhouse readings. This has the benefit of greatly increasing accuracy by incorporating all the light data that is available. Set 'SBASeedPoser' config option to specify the seed poser; default is EPNP. | [@jdavidberger](https://github.com/jdavidberger) +Poser | [poser_mpfit.c](src/poser_mpfit.c) | Performs Levenberg-Marquardt using [MPFIT](https://www.physics.wisc.edu/~craigm/idl/cmpfit.html). Since SBA does basically the same thing, this poser gets nearly identical results to SBA. Overall it is a tad slower than SBA since SBA uses optimized lapack functions to solve Ax=b, but MPFIT has the distinction of not needing lapack at all since it's Ax=b solver is a minimal internal version. It also requires a seed poser. | [@jdavidberger](https://github.com/jdavidberger) Disambiguator | [survive_data.c](src/survive_charlesbiguator.c) | The old disambiguator - very fast, but slightly buggy. | [@cnlohr](https://github.com/cnlohr) Disambiguator | [survive_data.c](src/survive_turveybiguator.c) (default) | More complicated but much more robust disambiguator | [@mwturvey](https://github.com/mwturvey) Disambiguator | [survive_data.c](src/survive_statebased_disambiguator.c) | A fast disambiguator that was times the state shifts between pulses. Experimental. Made to allow tracking very close to the lighthouse | [@jdavidberger](https://github.com/jdavidberger) -- cgit v1.2.3