diff options
41 files changed, 4347 insertions, 356 deletions
@@ -20,6 +20,7 @@ simple_pose_test calinfo/ *.log *.png +*.sage.py # Windows specific *.dll @@ -42,7 +42,6 @@ else LIBSURVIVE_C:=$(POSERS) $(REDISTS) $(LIBSURVIVE_CORE) $(SBA) $(MINIMAL_NEEDED) $(AUX_NEEDED) endif - LIBSURVIVE_O:=$(LIBSURVIVE_C:%.c=$(OBJDIR)/%.o) LIBSURVIVE_D:=$(LIBSURVIVE_C:%.c=$(OBJDIR)/%.d) -include $(LIBSURVIVE_D) @@ -74,7 +73,6 @@ ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif - ifdef LINUX_USE_HIDAPI CFLAGS:=$(CFLAGS) -DHIDAPI REDISTS:=$(REDISTS) redist/hid-linux.o @@ -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) @@ -144,6 +145,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 diff --git a/api_example.c b/api_example.c new file mode 100644 index 0000000..1f06740 --- /dev/null +++ b/api_example.c @@ -0,0 +1,35 @@ +#include <stdio.h> +#include <string.h> +#include <survive_api.h> +#include <os_generic.h> + +int main(int argc, char **argv) { + SurviveSimpleContext *actx = survive_simple_init(argc, argv); + if (actx == 0) // implies -help or similiar + return 0; + + survive_simple_start_thread(actx); + + while (survive_simple_is_running(actx)) { + OGUSleep(30000); + + SurvivePose pose; + + 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 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_simple_close(actx); + return 0; +} 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..32d58de --- /dev/null +++ b/bindings/python/example.py @@ -0,0 +1,13 @@ +import libsurvive +import sys + +actx = libsurvive.SimpleContext(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..653eb60 --- /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_simple_init.argtypes = (ctypes.c_int, LP_LP_c_char) # argc, argv +_libsurvive.survive_simple_init.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_simple_get_first_object.argtypes = [ctypes.c_void_p] +_libsurvive.survive_simple_get_first_object.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_simple_object_name.argtypes = [ ctypes.c_void_p ] +_libsurvive.survive_simple_object_name.restype = ctypes.c_char_p + +_libsurvive.survive_simple_is_running.argtypes = [ctypes.c_void_p] +_libsurvive.survive_simple_is_running.restype = ctypes.c_bool + +_libsurvive.survive_simple_start_thread.argtypes = [ctypes.c_void_p] +_libsurvive.survive_simple_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_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 SimpleObject: + ptr = 0 + def __init__(self, ptr): + self.ptr = ptr + + def Name(self): + return _libsurvive.survive_simple_object_name(self.ptr) + + def Pose(self): + pose = SurvivePose() + time = _libsurvive.survive_simple_object_get_latest_pose(self.ptr, pose) + return (pose, time) + +class SimpleContext: + 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_simple_init(argc, argv) + + self.objects = [] + curr = _libsurvive.survive_simple_get_first_object(self.ptr); + while curr: + 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_simple_is_running(self.ptr) + + def NextUpdated(self): + ptr = _libsurvive.survive_simple_get_next_updated(self.ptr) + if ptr: + return SimpleObject(ptr) + return None + diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index c1bb52c..66993df 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); @@ -333,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/include/libsurvive/survive_api.h b/include/libsurvive/survive_api.h new file mode 100644 index 0000000..64d1271 --- /dev/null +++ b/include/libsurvive/survive_api.h @@ -0,0 +1,63 @@ +#include "survive_types.h" +#include <stdbool.h> + +#ifdef __cplusplus +extern "C" { +#endif + +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 +} +#endif diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 11635aa..4d03038 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -3,8 +3,8 @@ #include "poser.h" #include "survive_types.h" -#include <stdint.h> #include <stdbool.h> +#include <stdint.h> #ifdef __cplusplus extern "C" { @@ -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/include/libsurvive/survive_reproject.h b/include/libsurvive/survive_reproject.h index e4f21d0..05aa7d9 100644 --- a/include/libsurvive/survive_reproject.h +++ b/include/libsurvive/survive_reproject.h @@ -16,6 +16,14 @@ 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/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 367c391..7fa5e0f 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -4,6 +4,14 @@ #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" { #endif @@ -38,6 +46,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 +55,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/redist/linmath.c b/redist/linmath.c index 01ae2b0..f4c3635 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -5,31 +5,31 @@ #include <math.h> #include <string.h> -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,28 @@ 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 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; 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 +215,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 +243,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 +287,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 +319,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 +351,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 +382,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 +417,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 +428,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 +479,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 +515,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 +523,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 +543,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 +587,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 +611,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..e46bbd4 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) +#define LINMATH_EXPORT extern __declspec(dllexport) #else #define LINMATH_EXPORT __attribute__((visibility("default"))) #endif +#endif // Yes, I know it's kind of arbitrary. #define DEFAULT_EPSILON 0.001 @@ -99,6 +101,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); @@ -108,6 +111,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); 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 <math.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> + +/* 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<n; j++) { + for (i=0; i<n; i++) { + printf("%f ", r[j*ldr+i]); + } + printf("\n"); + } +#endif + + tolr = tol * fabs(r[0]); + l = -1; + for (k = 0; k < n; k++) { + kk = k * ldr + k; + if (fabs(r[kk]) <= tolr) + break; + + r[kk] = one / r[kk]; + for (j = 0; j < k; j++) { + kj = k * ldr + j; + temp = r[kk] * r[kj]; + r[kj] = zero; + + k0 = k * ldr; + j0 = j * ldr; + for (i = 0; i <= j; i++) { + r[k0 + i] += (-temp * r[j0 + i]); + } + } + l = k; + } + + /* + * Form the full upper triangle of the inverse of (r transpose)*r + * in the full upper triangle of r + */ + + if (l >= 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<n; j++) { + for (i=0; i<n; i++) { + printf("%f ", r[j*ldr+i]); + } + printf("\n"); + } +#endif + + return 0; +} diff --git a/redist/mpfit/mpfit.h b/redist/mpfit/mpfit.h new file mode 100644 index 0000000..6a39ff1 --- /dev/null +++ b/redist/mpfit/mpfit.h @@ -0,0 +1,192 @@ +/* + * MINPACK-1 Least Squares Fitting Library + * + * Original public domain version by B. Garbow, K. Hillstrom, J. More' + * (Argonne National Laboratory, MINPACK project, March 1980) + * + * 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) + */ + +/* Header file defining constants, data structures and functions of + mpfit library + $Id: mpfit.h,v 1.16 2016/06/02 19:14:16 craigm Exp $ +*/ + +#ifndef MPFIT_H +#define MPFIT_H + +/* This is a C library. Allow compilation with a C++ compiler */ +#ifdef __cplusplus +extern "C" { +#endif + +/* MPFIT version string */ +#define MPFIT_VERSION "1.3" + +/* Definition of a parameter constraint structure */ +struct mp_par_struct { + int fixed; /* 1 = fixed; 0 = free */ + int limited[2]; /* 1 = low/upper limit; 0 = no limit */ + double limits[2]; /* lower/upper limit boundary value */ + + char *parname; /* Name of parameter, or 0 for none */ + double step; /* Step size for finite difference */ + double relstep; /* Relative step size for finite difference */ + int side; /* Sidedness of finite difference derivative + 0 - one-sided derivative computed automatically + 1 - one-sided derivative (f(x+h) - f(x) )/h + -1 - one-sided derivative (f(x) - f(x-h))/h + 2 - two-sided derivative (f(x+h) - f(x-h))/(2*h) + 3 - user-computed analytical derivatives + */ + int deriv_debug; /* Derivative debug mode: 1 = Yes; 0 = No; + + If yes, compute both analytical and numerical + derivatives and print them to the console for + comparison. + + NOTE: when debugging, do *not* set side = 3, + but rather to the kind of numerical derivative + you want to compare the user-analytical one to + (0, 1, -1, or 2). + */ + double deriv_reltol; /* Relative tolerance for derivative debug + printout */ + double deriv_abstol; /* Absolute tolerance for derivative debug + printout */ +}; + +/* Just a placeholder - do not use!! */ +typedef void (*mp_iterproc)(void); + +/* Definition of MPFIT configuration structure */ +struct mp_config_struct { + /* NOTE: the user may set the value explicitly; OR, if the passed + value is zero, then the "Default" value will be substituted by + mpfit(). */ + double ftol; /* Relative chi-square convergence criterium Default: 1e-10 */ + double xtol; /* Relative parameter convergence criterium Default: 1e-10 */ + double gtol; /* Orthogonality convergence criterium Default: 1e-10 */ + double epsfcn; /* Finite derivative step size Default: MP_MACHEP0 */ + double stepfactor; /* Initial step bound Default: 100.0 */ + double covtol; /* Range tolerance for covariance calculation Default: 1e-14 */ + int maxiter; /* Maximum number of iterations. If maxiter == MP_NO_ITER, + then basic error checking is done, and parameter + errors/covariances are estimated based on input + parameter values, but no fitting iterations are done. + Default: 200 + */ +#define MP_NO_ITER (-1) /* No iterations, just checking */ + int maxfev; /* Maximum number of function evaluations, or 0 for no limit + Default: 0 (no limit) */ + int nprint; /* Default: 1 */ + int douserscale; /* Scale variables by user values? + 1 = yes, user scale values in diag; + 0 = no, variables scaled internally (Default) */ + int nofinitecheck; /* Disable check for infinite quantities from user? + 0 = do not perform check (Default) + 1 = perform check + */ + mp_iterproc iterproc; /* Placeholder pointer - must set to 0 */ +}; + +/* Definition of results structure, for when fit completes */ +struct mp_result_struct { + double bestnorm; /* Final chi^2 */ + double orignorm; /* Starting value of chi^2 */ + int niter; /* Number of iterations */ + int nfev; /* Number of function evaluations */ + int status; /* Fitting status code */ + + int npar; /* Total number of parameters */ + int nfree; /* Number of free parameters */ + int npegged; /* Number of pegged parameters */ + int nfunc; /* Number of residuals (= num. of data points) */ + + double *resid; /* Final residuals + nfunc-vector, or 0 if not desired */ + double *xerror; /* Final parameter uncertainties (1-sigma) + npar-vector, or 0 if not desired */ + double *covar; /* Final parameter covariance matrix + npar x npar array, or 0 if not desired */ + char version[20]; /* MPFIT version string */ +}; + +/* Convenience typedefs */ +typedef struct mp_par_struct mp_par; +typedef struct mp_config_struct mp_config; +typedef struct mp_result_struct mp_result; + +/* Enforce type of fitting function */ +typedef int (*mp_func)(int m, /* Number of functions (elts of fvec) */ + int n, /* Number of variables (elts of x) */ + double *x, /* I - Parameters */ + double *fvec, /* O - function values */ + double **dvec, /* O - function derivatives (optional)*/ + void *private_data); /* I/O - function private data*/ + +/* Error codes */ +#define MP_ERR_INPUT (0) /* General input parameter error */ +#define MP_ERR_NAN (-16) /* User function produced non-finite values */ +#define MP_ERR_FUNC (-17) /* No user function was supplied */ +#define MP_ERR_NPOINTS (-18) /* No user data points were supplied */ +#define MP_ERR_NFREE (-19) /* No free parameters */ +#define MP_ERR_MEMORY (-20) /* Memory allocation error */ +#define MP_ERR_INITBOUNDS (-21) /* Initial values inconsistent w constraints*/ +#define MP_ERR_BOUNDS (-22) /* Initial constraints inconsistent */ +#define MP_ERR_PARAM (-23) /* General input parameter error */ +#define MP_ERR_DOF (-24) /* Not enough degrees of freedom */ + +/* Potential success status codes */ +#define MP_OK_CHI (1) /* Convergence in chi-square value */ +#define MP_OK_PAR (2) /* Convergence in parameter value */ +#define MP_OK_BOTH (3) /* Both MP_OK_PAR and MP_OK_CHI hold */ +#define MP_OK_DIR (4) /* Convergence in orthogonality */ +#define MP_MAXITER (5) /* Maximum number of iterations reached */ +#define MP_FTOL (6) /* ftol is too small; no further improvement*/ +#define MP_XTOL (7) /* xtol is too small; no further improvement*/ +#define MP_GTOL (8) /* gtol is too small; no further improvement*/ + +/* Double precision numeric constants */ +#define MP_MACHEP0 2.2204460e-16 +#define MP_DWARF 2.2250739e-308 +#define MP_GIANT 1.7976931e+308 + +#if 0 +/* Float precision */ +#define MP_MACHEP0 1.19209e-07 +#define MP_DWARF 1.17549e-38 +#define MP_GIANT 3.40282e+38 +#endif + +#define MP_RDWARF (sqrt(MP_DWARF * 1.5) * 10) +#define MP_RGIANT (sqrt(MP_GIANT) * 0.1) + +/* External function prototype declarations */ +extern int mpfit(mp_func funct, int m, int npar, double *xall, mp_par *pars, mp_config *config, void *private_data, + mp_result *result); + +/* C99 uses isfinite() instead of finite() */ +#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L +#define mpfinite(x) isfinite(x) + +/* Microsoft C uses _finite(x) instead of finite(x) */ +#elif defined(_MSC_VER) && _MSC_VER +#include <float.h> +#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/simple_pose_test.c b/simple_pose_test.c index 5f7f5e2..075f6e1 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -32,16 +32,12 @@ float viewY = 0.7853975; int down, downx, downy; void HandleButton( int x, int y, int button, int bDown ) { - if( button == 1 ) - { - if( bDown ) - { + if (button == 1) { + if (bDown) { down = 1; downx = x; downy = y; - } - else - { + } else { down = 0; } } @@ -49,12 +45,13 @@ void HandleButton( int x, int y, int button, int bDown ) void HandleMotion( int x, int y, int mask ) { - if( down ) - { - viewX += (x-downx)/100.0; - viewY -= (y-downy)/100.0; - if( viewY < 0.01 ) viewY = 0.01; - if( viewY > 3.14 ) viewY = 3.14; + if (down) { + viewX += (x - downx) / 100.0; + viewY -= (y - downy) / 100.0; + if (viewY < 0.01) + viewY = 0.01; + if (viewY > 3.14) + viewY = 3.14; downx = x; downy = y; } @@ -71,8 +68,8 @@ FLT hposx[3]; void testprog_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { survive_default_raw_pose_process(so, timecode, pose); -// if (strcmp(so->codename, "WW0") != 0) -// return; + // if (strcmp(so->codename, "WW0") != 0) + // return; // print the pose; /* double qw = quat[0]; @@ -98,7 +95,7 @@ void testprog_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose hpos[2] = pose->Pos[2]; FLT hposin[3] = { 0, 0, 1 }; ApplyPoseToPoint(hpos2, pose, hposin); - FLT hposinx[3] = { .1, 0, 0 }; + FLT hposinx[3] = {.1, 0, 0}; ApplyPoseToPoint(hposx, pose, hposinx); fflush(stdout); @@ -154,14 +151,15 @@ void *GUIThread(void*v) CNFGGetDimensions( &screenx, &screeny ); int x, y; - float eye[3] = { 3*sin(viewX)*sin(viewY), 3*cos(viewX)*sin(viewY), 3*cos(viewY)}; //Create a 2-rotation with Z primarily up. - //float up[3] = { 0, cos(viewY), sin(viewY)}; //Create a 2-rotation with Z primarily up. + float eye[3] = {3 * sin(viewX) * sin(viewY), 3 * cos(viewX) * sin(viewY), + 3 * cos(viewY)}; // Create a 2-rotation with Z primarily up. + // float up[3] = { 0, cos(viewY), sin(viewY)}; //Create a 2-rotation with Z primarily up. float at[3] = { 0,0, 0 }; - float up[3] = { 0,0, 1.0 }; + float up[3] = {0, 0, 1.0}; float right[3]; - tdCross( up, eye, right ); - tdCross( eye, right, up ); //Have to make sure we're making a coordinate frame for lookat. - tdNormalizeSelf( right ); + tdCross(up, eye, right); + tdCross(eye, right, up); // Have to make sure we're making a coordinate frame for lookat. + tdNormalizeSelf(right); tdSetViewport( -1, -1, 1, 1, screenx, screeny ); tdMode( tdPROJECTION ); @@ -174,7 +172,8 @@ void *GUIThread(void*v) CNFGColor( 0x00ffff ); DrawLineSegment( hpos[0], hpos[1], hpos[2], hpos2[0], hpos2[1], hpos2[2] ); CNFGColor( 0xff00ff ); DrawLineSegment( hpos[0], hpos[1], hpos[2], hpos[0], hpos[1], hpos[2] ); - CNFGColor( 0xffff00 ); DrawLineSegment( hpos[0], hpos[1], hpos[2], hposx[0], hposx[1], hposx[2] ); + CNFGColor(0xffff00); + DrawLineSegment(hpos[0], hpos[1], hpos[2], hposx[0], hposx[1], hposx[2]); CNFGColor( 0x0000ff ); DrawLineSegment( 0, 0, 0, 1, 0, 0 ); CNFGColor( 0xff0000 ); DrawLineSegment( 0, 0, 0, 0, 1, 0 ); CNFGColor( 0x00ff00 ); DrawLineSegment( 0, 0, 0, 0, 0, 1 ); diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 7e922e7..90fab65 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 new file mode 100644 index 0000000..ff82fd4 --- /dev/null +++ b/src/poser_general_optimizer.c @@ -0,0 +1,101 @@ +#include "poser_general_optimizer.h" +#include "string.h" + +#include <assert.h> +#include <malloc.h> +#include <stdio.h> + +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..6d4d906 --- /dev/null +++ b/src/poser_general_optimizer.h @@ -0,0 +1,29 @@ +#include <stdlib.h> +#include <survive.h> + +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_mpfit.c b/src/poser_mpfit.c new file mode 100644 index 0000000..45e2c69 --- /dev/null +++ b/src/poser_mpfit.c @@ -0,0 +1,283 @@ +#ifndef USE_DOUBLE +#define FLT double +#define USE_DOUBLE +#endif + +#include <malloc.h> + +#include "mpfit/mpfit.h" +#include "poser.h" +#include <survive.h> +#include <survive_imu.h> + +#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 = {0}; + + if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { + return -1; + } + + 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++) { + 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); diff --git a/src/poser_sba.c b/src/poser_sba.c index 23e03fc..8c9e328 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,26 +33,23 @@ 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; - + int use_jacobian_function; int required_meas; SurviveIMUTracker tracker; bool useIMU; - SurviveObject *so; + struct { + int meas_failures; + } stats; } SBAData; static void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -89,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++) { @@ -176,20 +174,37 @@ 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, 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); @@ -208,40 +223,17 @@ 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; - SurvivePose soLocation = so->OutPose; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; + SurvivePose soLocation = {0}; - 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); - - 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; - - if (locations.hasInfo == false) { - return -1; - } else if (locations.hasInfo) { - soLocation = locations.poses; - } - - d->successes_to_reset_cntr = d->successes_to_reset; - } else { - 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}; @@ -265,38 +257,28 @@ 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 - - if (currentPositionValid) { - // FLT distp[3]; - // sub3d(distp, so->OutPose.Pos, soLocation.Pos); - // FLT distance = magnitude3d(distp); - - // if (distance > 1.) - // status = -1; - } + 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 double rtn = -1; - if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { - d->failures_to_reset_cntr = d->failures_to_reset; + bool status_failure = status <= 0; + bool error_failure = !general_optimizer_data_record_success(&d->opt, (info[1] / meas_size * 2)); + if (!status_failure && !error_failure) { quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; - } + } else { 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)); } } @@ -316,11 +298,11 @@ 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"); + const char *subposer = survive_configs(so->ctx, "seed-poser", SC_GET, "PoserEPNP"); + PoserCB driver = (PoserCB)GetDriver(subposer); 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; @@ -396,27 +378,24 @@ 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->so = so; + d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); 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); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -436,25 +415,20 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->last_lh = lightData->lh; d->last_acode = lightData->acode; - if (error < 0) { - if (d->failures_to_reset_cntr > 0) - d->failures_to_reset_cntr--; - } - else { + + } else { 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 }; + FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, + error * var_quat, error * var_quat, error * var_quat}; survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); estimate = d->tracker.pose; } PoserData_poser_pose_func(&lightData->hdr, so, &estimate); - if (d->successes_to_reset_cntr > 0) - d->successes_to_reset_cntr--; } } return 0; @@ -466,22 +440,24 @@ 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("\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); + } else if (d->useIMU) { + survive_imu_tracker_integrate(so, &d->tracker, imu); + PoserData_poser_pose_func(pd, so, &d->tracker.pose); } - } // 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); - } - break; + + general_optimizer_data_record_imu(&d->opt, imu); } } return -1; diff --git a/src/survive.c b/src/survive.c index 9e750f9..bb1179e 100644 --- a/src/survive.c +++ b/src/survive.c @@ -106,7 +106,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { static int did_manual_driver_registration = 0; if (did_manual_driver_registration == 0) { #define MANUAL_DRIVER_REGISTRATION(func) \ - int func(SurviveObject *so, PoserData *pd); \ + int func(SurviveObject *so, PoserData *pd); \ RegisterDriver(#func, &func); MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow) @@ -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) @@ -402,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_api.c b/src/survive_api.c new file mode 100644 index 0000000..a39524b --- /dev/null +++ b/src/survive_api.c @@ -0,0 +1,165 @@ +#include "survive_api.h" +#include "os_generic.h" +#include "survive.h" +#include "stdio.h" + +struct SurviveSimpleObject { + struct SurviveSimpleContext *actx; + + enum SurviveSimpleObject_type { SurviveSimpleObject_LIGHTHOUSE, SurviveSimpleObject_OBJECT } type; + + union { + int lighthouse; + struct SurviveObject* so; + } data; + + char name[32]; + bool has_update; +}; + +struct SurviveSimpleContext { + SurviveContext* ctx; + + bool running; + og_thread_t thread; + og_mutex_t poll_mutex; + + size_t object_ct; + struct SurviveSimpleObject objects[]; +}; + +static void pose_fn(SurviveObject *so, uint32_t timecode, SurvivePose *pose) { + struct SurviveSimpleContext *actx = so->ctx->user_ptr; + OGLockMutex(actx->poll_mutex); + survive_default_raw_pose_process(so, timecode, pose); + + 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 SurviveSimpleContext *actx = ctx->user_ptr; + OGLockMutex(actx->poll_mutex); + survive_default_lighthouse_pose_process(ctx, lighthouse, lighthouse_pose, object_pose); + + actx->objects[lighthouse].has_update = true; + + OGUnlockMutex(actx->poll_mutex); +} + +struct SurviveSimpleContext *survive_simple_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 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; + intptr_t i = 0; + for (i = 0; i < ctx->activeLighthouses; i++) { + struct SurviveSimpleObject *obj = &actx->objects[i]; + obj->data.lighthouse = i; + obj->type = SurviveSimpleObject_LIGHTHOUSE; + obj->actx = actx; + obj->has_update = ctx->bsd[i].PositionSet; + snprintf(obj->name, 32, "LH%ld", i); + } + for (; i < object_ct; i++) { + struct SurviveSimpleObject *obj = &actx->objects[i]; + int so_idx = i - ctx->activeLighthouses; + obj->data.so = ctx->objs[so_idx]; + obj->type = SurviveSimpleObject_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; +} + +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_simple_stop_thread(actx); + } + + survive_close(actx->ctx); +} + +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_simple_is_running(struct SurviveSimpleContext *actx) { return actx->running; } +void survive_simple_start_thread(struct SurviveSimpleContext *actx) { + actx->running = true; + actx->thread = OGCreateThread(__simple_thread, actx); +} + +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 SurviveSimpleObject *survive_simple_get_first_object(struct SurviveSimpleContext *actx) { + return actx->objects; +} + +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; + return &actx->objects[i]; + } + } + return 0; +} + +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 SurviveSimpleObject_LIGHTHOUSE: { + if(pose) + *pose = sao->actx->ctx->bsd[sao->data.lighthouse].Pose; + break; + } + case SurviveSimpleObject_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_simple_object_name(const SurviveSimpleObject *sao) { return sao->name; } diff --git a/src/survive_cal.c b/src/survive_cal.c index 4d26bfb..faf2ac7 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_default_devices.c b/src/survive_default_devices.c index 954c47a..4fa3284 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -170,49 +170,49 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { } } - - //Handle device-specific sacling. - if( strcmp( so->codename, "HMD" ) == 0 ) - { - if( so->acc_scale ) - { - so->acc_scale[0] *= -1./8192.0; - so->acc_scale[1] *= -1./8192.0; - so->acc_scale[2] *= 1./8192.0; + // Handle device-specific sacling. + if (strcmp(so->codename, "HMD") == 0) { + if (so->acc_scale) { + so->acc_scale[0] *= -1. / 8192.0; + so->acc_scale[1] *= -1. / 8192.0; + so->acc_scale[2] *= 1. / 8192.0; } - if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 2./1000. ); //Odd but seems right. - if( so->gyro_scale ) - { + if (so->acc_bias) + scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Odd but seems right. + if (so->gyro_scale) { so->gyro_scale[0] *= -0.000065665; so->gyro_scale[1] *= -0.000065665; - so->gyro_scale[2] *= 0.000065665; + so->gyro_scale[2] *= 0.000065665; } - } - else if( memcmp( so->codename, "WM", 2 ) == 0 ) - { - if( so->acc_scale ) scale3d( so->acc_scale, so->acc_scale, 2./8192.0 ); - if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 2./1000. ); //Need to verify. - if( so->gyro_scale ) scale3d( so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8 ); //??! 1.8 feels right but why?! + } else if (memcmp(so->codename, "WM", 2) == 0) { + if (so->acc_scale) + scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0); + if (so->acc_bias) + scale3d(so->acc_bias, so->acc_bias, 2. / 1000.); // Need to verify. + if (so->gyro_scale) + scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8); //??! 1.8 feels right but why?! int j; for (j = 0; j < so->sensor_ct; j++) { so->sensor_locations[j * 3 + 0] *= 1.0; } - } - else //Verified on WW, Need to verify on Tracker. + } else // Verified on WW, Need to verify on Tracker. { - //1G for accelerometer, from MPU6500 datasheet - //this can change if the firmware changes the sensitivity. + // 1G for accelerometer, from MPU6500 datasheet + // this can change if the firmware changes the sensitivity. // When coming off of USB, these values are in units of .5g -JB - if( so->acc_scale ) scale3d( so->acc_scale, so->acc_scale, 2./8192.0 ); + if (so->acc_scale) + scale3d(so->acc_scale, so->acc_scale, 2. / 8192.0); - //If any other device, we know we at least need this. + // If any other device, we know we at least need this. // I deeply suspect bias is in milligravities -JB - if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 1./1000. ); + if (so->acc_bias) + scale3d(so->acc_bias, so->acc_bias, 1. / 1000.); // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits // FLT deg_per_sec = 250; - if( so->gyro_scale )scale3d( so->gyro_scale, so->gyro_scale, 3.14159 / 1800. ); + if (so->gyro_scale) + scale3d(so->gyro_scale, so->gyro_scale, 3.14159 / 1800.); } char fname[64]; diff --git a/src/survive_imu.c b/src/survive_imu.c index 8f4266a..3622b2d 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 <memory.h> #include <survive_imu.h> -//--------------------------------------------------------------------------------------------------- -// 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; @@ -237,7 +182,7 @@ void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timec tracker->pose = *pose; return; } - + // Kalman filter assuming: // F -> Identity // H -> Identity @@ -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 +} diff --git a/src/survive_reproject.c b/src/survive_reproject.c index d6ba70b..ae946fe 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 <math.h> -#include <stdio.h> -#include <string.h> + +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) { @@ -99,5 +163,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/src/survive_reproject.generated.h b/src/survive_reproject.generated.h new file mode 100644 index 0000000..6d834f7 --- /dev/null +++ b/src/survive_reproject.generated.h @@ -0,0 +1,215 @@ +// NOTE: Auto-generated code; see tools/generate_reprojection_functions +#include <math.h> +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 = (lh_qi * lh_qi); + FLT x5 = (lh_qj * lh_qj); + FLT x6 = x4 + x5; + 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; + 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 = (obj_qi * obj_qi); + FLT x23 = (obj_qj * obj_qj); + FLT x24 = x22 + x23; + 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; + 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 = 1. / (x44 * x44); + FLT x53 = 4 * curve_0 * curve_scale * x51 * x52 * x8; + FLT x54 = x11 * x52; + FLT x55 = (x51 * x51); + FLT x56 = curve_0 * x55; + 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 = (x62 * x62); + 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/src/survive_vive.c b/src/survive_vive.c index 83bc977..d9cbc3e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1233,10 +1233,10 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset. { propset |= 2; - FLT agm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + FLT agm[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; int j; - for( j = 0; j < 6; j++ ) - agm[j] = (int16_t)(readdata[j*2+1] | (readdata[j*2+2]<<8)); + for (j = 0; j < 6; j++) + agm[j] = (int16_t)(readdata[j * 2 + 1] | (readdata[j * 2 + 2] << 8)); calibrate_acc(w, agm); calibrate_gyro(w, agm+3); w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); @@ -1660,6 +1660,7 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int if( len < 0 ) { + survive_remove_object(ctx, so); return len; } 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 <vector> #include <sba/sba.h> -#include <survive_reproject.h> 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); 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..446e94f --- /dev/null +++ b/tools/generate_reprojection_functions/check_generated.c @@ -0,0 +1,127 @@ +#include "survive_reproject.full.generated.h" +#include <libsurvive/survive.h> +#include <libsurvive/survive_reproject.h> +#include <math.h> +#include <os_generic.h> + +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 <math.h>") + +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))) diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 98dd5f0..3eea83d 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; @@ -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'; 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 @@ <body> <div id="ThreeJS" style="z-index: 1; position: absolute; left:0px; top:0px"></div> <div id="cam-control" style="z-index: 2;border:1px solid white;position:absolute"> - <input type="checkbox" id="trails">Trails</input><br/> + <input type="checkbox" id="trails" checked>Trails</input><br/> + <input type="checkbox" id="fpv" checked>FPV</input><br/> <button id="toggleBtn"> Toggle 2D View </button> 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); +} diff --git a/winbuild/libsurvive.sln b/winbuild/libsurvive.sln index 3a8b2da..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,11 +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 = {5C1AB415-DC8F-42C0-B2E0-7300D0C79E09} + SolutionGuid = {EA7B8D68-188A-4190-B011-8A7F4EC52C38} EndGlobalSection EndGlobal diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index fb1b9a5..620e4e6 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -244,6 +244,7 @@ <ClCompile Include="..\..\redist\json_helpers.c" /> <ClCompile Include="..\..\redist\linmath.c" /> <ClCompile Include="..\..\redist\minimal_opencv.c" /> + <ClCompile Include="..\..\redist\mpfit\mpfit.c" /> <ClCompile Include="..\..\redist\puff.c" /> <ClCompile Include="..\..\redist\sba\sba_chkjac.c" /> <ClCompile Include="..\..\redist\sba\sba_crsm.c" /> @@ -254,14 +255,18 @@ <ClCompile Include="..\..\src\epnp\epnp.c" /> <ClCompile Include="..\..\src\ootx_decoder.c" /> <ClCompile Include="..\..\src\poser.c" /> + <ClCompile Include="..\..\src\poser_charlesrefine.c" /> <ClCompile Include="..\..\src\poser_charlesslow.c" /> <ClCompile Include="..\..\src\poser_daveortho.c" /> <ClCompile Include="..\..\src\poser_dummy.c" /> <ClCompile Include="..\..\src\poser_epnp.c" /> + <ClCompile Include="..\..\src\poser_general_optimizer.c" /> + <ClCompile Include="..\..\src\poser_mpfit.c" /> <ClCompile Include="..\..\src\poser_octavioradii.c" /> <ClCompile Include="..\..\src\poser_sba.c" /> <ClCompile Include="..\..\src\poser_turveytori.c" /> <ClCompile Include="..\..\src\survive.c" /> + <ClCompile Include="..\..\src\survive_api.c" /> <ClCompile Include="..\..\src\survive_cal.c" /> <ClCompile Include="..\..\src\survive_charlesbiguator.c" /> <ClCompile Include="..\..\src\survive_config.c" /> @@ -282,17 +287,20 @@ <ItemGroup> <ClInclude Include="..\..\include\libsurvive\poser.h" /> <ClInclude Include="..\..\include\libsurvive\survive.h" /> + <ClInclude Include="..\..\include\libsurvive\survive_api.h" /> <ClInclude Include="..\..\include\libsurvive\survive_types.h" /> <ClInclude Include="..\..\redist\crc32.h" /> <ClInclude Include="..\..\redist\jsmn.h" /> <ClInclude Include="..\..\redist\json_helpers.h" /> <ClInclude Include="..\..\redist\linmath.h" /> <ClInclude Include="..\..\redist\minimal_opencv.h" /> + <ClInclude Include="..\..\redist\mpfit\mpfit.h" /> <ClInclude Include="..\..\redist\os_generic.h" /> <ClInclude Include="..\..\redist\sba\sba.h" /> <ClInclude Include="..\..\redist\symbol_enumerator.h" /> <ClInclude Include="..\..\src\epnp\epnp.h" /> <ClInclude Include="..\..\src\ootx_decoder.h" /> + <ClInclude Include="..\..\src\poser_general_optimizer.h" /> <ClInclude Include="..\..\src\survive_cal.h" /> <ClInclude Include="..\..\src\survive_config.h" /> <ClInclude Include="..\..\src\survive_default_devices.h" /> diff --git a/winbuild/libsurvive/libsurvive.vcxproj.filters b/winbuild/libsurvive/libsurvive.vcxproj.filters index d06f083..b48377e 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj.filters +++ b/winbuild/libsurvive/libsurvive.vcxproj.filters @@ -1,4 +1,4 @@ -<?xml version="1.0" encoding="utf-8"?> +<?xml version="1.0" encoding="utf-8"?> <Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> <ItemGroup> <Filter Include="Source Files"> @@ -135,9 +135,24 @@ <ClCompile Include="..\..\redist\sba\sba_chkjac.c"> <Filter>Source Files</Filter> </ClCompile> + <ClCompile Include="..\..\src\survive_api.c"> + <Filter>Source Files</Filter> + </ClCompile> <ClCompile Include="..\..\src\survive_imu.c"> <Filter>Source Files</Filter> </ClCompile> + <ClCompile Include="..\..\redist\mpfit\mpfit.c"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\src\poser_general_optimizer.c"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\src\poser_mpfit.c"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\src\poser_charlesrefine.c"> + <Filter>Source Files</Filter> + </ClCompile> </ItemGroup> <ItemGroup> <ClInclude Include="..\..\src\ootx_decoder.h"> @@ -191,8 +206,17 @@ <ClInclude Include="..\..\redist\sba\sba.h"> <Filter>Source Files</Filter> </ClInclude> + <ClInclude Include="..\..\include\libsurvive\survive_api.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\redist\mpfit\mpfit.h"> + <Filter>Source Files</Filter> + </ClInclude> + <ClInclude Include="..\..\src\poser_general_optimizer.h"> + <Filter>Source Files</Filter> + </ClInclude> </ItemGroup> <ItemGroup> <None Include="packages.config" /> </ItemGroup> -</Project>
\ No newline at end of file +</Project> |