diff options
33 files changed, 4307 insertions, 349 deletions
@@ -20,6 +20,7 @@ simple_pose_test calinfo/ *.log *.png +*.sage.py # Windows specific *.dll @@ -2,12 +2,12 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test CC?=gcc - CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic #-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses CFLAGS_RELEASE:=-Iinclude/libsurvive -fPIC -msse2 -ftree-vectorize -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #LDFLAGS:=-L/usr/local/lib -lpthread -lusb-1.0 -lz -lm -flto -g + LDFLAGS:=-L/usr/local/lib -lpthread -lz -llapacke -lcblas -lm -flto -g #---------- @@ -41,7 +41,7 @@ ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o src/survive_api.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.o src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_statebased_disambiguator.o src/poser_charlesrefine.o src/survive_imu.o src/poser_imu.o src/poser_general_optimizer.o src/poser_mpfit.o redist/mpfit/mpfit.o src/survive_api.o #If you want to use HIDAPI on Linux. #CFLAGS:=$(CFLAGS) -DHIDAPI diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 52dcc0d..93af4c0 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -317,8 +317,7 @@ void RegisterDriver(const char *name, void *data); #ifdef _MSC_VER #define REGISTER_LINKTIME(func) \ - __pragma(comment(linker, "/export:REGISTER" #func)); \ - void REGISTER##func() { RegisterDriver(#func, &func); } + SURVIVE_EXPORT void REGISTER##func() { RegisterDriver(#func, &func); } #else #define REGISTER_LINKTIME(func) \ void __attribute__((constructor)) REGISTER##func() { RegisterDriver(#func, &func); } diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 8a86425..4d03038 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -3,6 +3,7 @@ #include "poser.h" #include "survive_types.h" +#include <stdbool.h> #include <stdint.h> #ifdef __cplusplus @@ -12,6 +13,8 @@ extern "C" { struct SurviveIMUTracker_p; typedef struct { + bool is_initialized; + FLT updir[3]; FLT accel_scale_bias; @@ -24,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/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 bafdbe8..e46bbd4 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -9,7 +9,7 @@ extern "C" { #ifndef LINMATH_EXPORT #ifdef _WIN32 -#define LINMATH_EXPORT __declspec(dllexport) +#define LINMATH_EXPORT extern __declspec(dllexport) #else #define LINMATH_EXPORT __attribute__((visibility("default"))) #endif @@ -101,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); @@ -110,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 06a9901..075f6e1 100644 --- a/simple_pose_test.c +++ b/simple_pose_test.c @@ -27,12 +27,34 @@ void HandleKey( int keycode, int bDown ) } } +float viewX = 0.7853975; +float viewY = 0.7853975; +int down, downx, downy; void HandleButton( int x, int y, int button, int bDown ) { + if (button == 1) { + if (bDown) { + down = 1; + downx = x; + downy = y; + } else { + down = 0; + } + } } 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; + downx = x; + downy = y; + } } void HandleDestroy() @@ -41,12 +63,13 @@ void HandleDestroy() FLT hpos[3]; FLT hpos2[3]; +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, "HMD") != 0) - return; + // if (strcmp(so->codename, "WW0") != 0) + // return; // print the pose; /* double qw = quat[0]; @@ -72,6 +95,8 @@ 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}; + ApplyPoseToPoint(hposx, pose, hposinx); fflush(stdout); } @@ -126,10 +151,15 @@ void *GUIThread(void*v) CNFGGetDimensions( &screenx, &screeny ); int x, y; - float eye[3] = { sin(TimeSinceStart)*4, cos(TimeSinceStart)*4, 4}; + 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 }; - + 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); tdSetViewport( -1, -1, 1, 1, screenx, screeny ); tdMode( tdPROJECTION ); @@ -142,6 +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( 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_charlesrefine.c b/src/poser_charlesrefine.c index 52a5f54..1749d8e 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -13,7 +13,7 @@ #include <string.h> #include <survive_imu.h> -#define MAX_PT_PER_SWEEP 32 +#define MAX_PT_PER_SWEEP SENSORS_PER_OBJECT typedef struct { int sweepaxis; @@ -23,6 +23,11 @@ typedef struct { FLT angles_at_pts[MAX_PT_PER_SWEEP]; SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; + + LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2]; + SurvivePose InteralPoseUsedForCalc; // Super high speed vibratey and terrible. + float MixingConfidence[NUM_LIGHTHOUSES][2]; + int ptsweep; SurviveIMUTracker tracker; @@ -30,17 +35,58 @@ typedef struct { int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { CharlesPoserData *dd = so->PoserData; - if (!dd) + if (!dd) { so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1); + SurvivePose object_pose_out; + memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); + memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); + so->PoseConfidence = 1.0; + PoserData_poser_pose_func(pd, so, &object_pose_out); + } SurviveSensorActivations *scene = &so->activations; switch (pd->pt) { case POSERDATA_IMU: { // Really should use this... - PoserDataIMU *imu = (PoserDataIMU *)pd; + PoserDataIMU *imuData = (PoserDataIMU *)pd; - survive_imu_tracker_integrate(so, &dd->tracker, imu); - PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + // TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose *object_pose = &so->OutPose; + imuData->gyro[0] *= 1. / 240.; + imuData->gyro[1] *= 1. / 240.; + imuData->gyro[2] *= 1. / 240.; + quatfromeuler(applymotion, imuData->gyro); + // printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + + LinmathQuat InvertQuat; + quatgetreciprocal(InvertQuat, object_pose->Rot); + + // Apply a tiny tug to re-right headset based on the gravity vector. + LinmathVec3d reright = {0, 0, 1}; + LinmathVec3d normup; + normalize3d(normup, imuData->accel); + LinmathVec3d correct_diff; + quatrotatevector(reright, InvertQuat, reright); + sub3d(correct_diff, normup, reright); + scale3d(correct_diff, correct_diff, -0.001); // This is the coefficient applying the drag. + add3d(correct_diff, correct_diff, reright); + LinmathQuat reright_quat; + normalize3d(correct_diff, correct_diff); + quatfrom2vectors(reright_quat, reright, correct_diff); + + SurvivePose object_pose_out; + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion); // Contribution from Gyro + quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, reright_quat); // Contribution from Accelerometer + quatnormalize(object_pose_out.Rot, object_pose_out.Rot); + + copy3d(object_pose_out.Pos, object_pose->Pos); + PoserData_poser_pose_func(pd, so, &object_pose_out); + quatcopy(dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); + + // PoserDataIMU *imu = (PoserDataIMU *)pd; + // survive_imu_tracker_integrate(so, &dd->tracker, imu); + // PoserData_poser_pose_func(pd, so, &dd->tracker.pose); return 0; } @@ -56,7 +102,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { FLT angle = ld->angle; int sensor_id = ld->sensor_id; int axis = dd->sweepaxis; - const SurvivePose *object_pose = &so->OutPose; + // const SurvivePose *object_pose = &so->OutPose; + dd->sweeplh = lhid; // FOR NOW, drop LH1. @@ -70,7 +117,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { FLT sensor_position_worldspace[3]; // XXX Once I saw this get pretty wild (When in playback) // I had to invert the values of sensor_inpos. Not sure why. - ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + ApplyPoseToPoint(sensor_position_worldspace, &dd->InteralPoseUsedForCalc, sensor_inpos); // printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], // sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); @@ -109,7 +156,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { dd->quantity_errors[i] = dist; dd->angles_at_pts[i] = angle; dd->sensor_ids[i] = sensor_id; - memcpy(&dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose)); + memcpy(&dd->object_pose_at_hit[i], &dd->InteralPoseUsedForCalc, sizeof(SurvivePose)); dd->ptsweep++; } @@ -126,8 +173,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { int lhid = dd->sweeplh; int axis = dd->sweepaxis; int pts = dd->ptsweep; - const SurvivePose *object_pose = - &so->OutPose; // XXX TODO Should pull pose from approximate time when LHs were scanning it. + // const SurvivePose *object_pose = + // &so->OutPose; // XXX TODO Should pull pose from approximate time when LHs were scanning it. BaseStationData *bsd = &so->ctx->bsd[lhid]; SurvivePose *lh_pose = &bsd->Pose; @@ -143,11 +190,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define HIT_QUALITY_BASELINE \ 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 // Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 8.0 // Converges even as high as 10.0 and doesn't explode. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 7.00 // Converges even as high as 10.0 and doesn't explode. #define CORRECT_ROTATION_COEFFICIENT \ - 1.0 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. -#define ROTATIONAL_CORRECTION_MAXFORCE 0.10 + 0.2 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. +#define ROTATIONAL_CORRECTION_MAXFORCE 0.01 // Step 1: Determine standard of deviation, and average in order to // drop points that are likely in error. @@ -238,16 +285,33 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { zoom *= CORRECT_TELESCOPTION_COEFFICIENT; FLT veccamalong[3]; - sub3d(veccamalong, lh_pose->Pos, object_pose->Pos); + sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); normalize3d(veccamalong, veccamalong); scale3d(veccamalong, veccamalong, zoom); add3d(vec_correct, veccamalong, vec_correct); } - SurvivePose object_pose_out; - add3d(object_pose_out.Pos, vec_correct, object_pose->Pos); +#if 0 + LinmathPoint3d LastDelta; + sub3d( LastDelta, dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos ); + //Compare with "vec_correct" + + LinmathPoint3d DeltaDelta; + sub3d( DeltaDelta, vec_correct, LastDelta ); + + + //SurvivePose object_pose_out; - quatcopy(object_pose_out.Rot, object_pose->Rot); + memcpy( dd->MixingPositions[lhid][axis], vec_correct, sizeof( vec_correct ) ); + + LinmathPoint3d system_average_adjust = { 0, 0, 0 }; + + printf( "%f %f %f + %f %f %f\n", vec_correct[0], vec_correct[1], vec_correct[2], dd->InteralPoseUsedForCalc.Pos[0], dd->InteralPoseUsedForCalc.Pos[1], dd->InteralPoseUsedForCalc.Pos[2] ); + +#endif + add3d(dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos); + + // quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot); // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. // If we were able to determine likliehood of a hit in the sweep instead of afterward @@ -308,25 +372,50 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } // printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); // Apply our corrective quaternion to the output. - quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, correction); - quatnormalize(object_pose_out.Rot, object_pose_out.Rot); - } - - // Janky need to do this somewhere else... This initializes the pose estimator. - if (so->PoseConfidence < .01) { - memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); - so->PoseConfidence = 1.0; + quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction); + quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); } - // PoserData_poser_pose_func(pd, so, &object_pose_out); - FLT var_meters = 0.5; - FLT error = 0.0001; - 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}; + memcpy(dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos, + sizeof(dd->InteralPoseUsedForCalc.Pos)); + dd->MixingConfidence[lhid][axis] = (validpoints) ? ((validpoints > 1) ? 1.0 : 0.5) : 0; - survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var); - PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + // Box filter all of the guesstimations, and emit the new guess. + { + FLT MixedAmount = 0; + LinmathPoint3d MixedPosition = {0, 0, 0}; + int l = 0, a = 0; + if (lhid == 0 && axis == 0) + for (l = 0; l < NUM_LIGHTHOUSES; l++) + for (a = 0; a < 2; a++) + dd->MixingConfidence[l][a] -= 0.1; + + for (l = 0; l < NUM_LIGHTHOUSES; l++) + for (a = 0; a < 2; a++) { + LinmathPoint3d MixThis = {0, 0, 0}; + FLT Confidence = dd->MixingConfidence[l][a]; + if (Confidence < 0) + Confidence = 0; + scale3d(MixThis, dd->MixingPositions[l][a], Confidence); + add3d(MixedPosition, MixedPosition, MixThis); + MixedAmount += Confidence; + // printf( "%f ", Confidence ); + } + scale3d(MixedPosition, MixedPosition, 1. / MixedAmount); + // printf( "%f\n", MixedAmount ); + SurvivePose object_pose_out; + quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot); + copy3d(object_pose_out.Pos, MixedPosition); + PoserData_poser_pose_func(pd, so, &object_pose_out); + } + // FLT var_meters = 0.5; + // FLT error = 0.00001; + // 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, l->timecode, &dd->tracker, &object_pose_out, var); + // PoserData_poser_pose_func(pd, so, &dd->tracker.pose); dd->ptsweep = 0; } 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 fcf4f2e..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, 100); + + 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) { @@ -435,27 +414,23 @@ 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 { - 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; - } + if (error < 0) { - PoserData_poser_pose_func(&lightData->hdr, so, &estimate); - if (d->successes_to_reset_cntr > 0) - d->successes_to_reset_cntr--; - } + } 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}; + + 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_FULL_SCENE: { @@ -465,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 b024bbb..362adbc 100644 --- a/src/survive.c +++ b/src/survive.c @@ -114,6 +114,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { MANUAL_DRIVER_REGISTRATION(PoserDummy) MANUAL_DRIVER_REGISTRATION(PoserEPNP) MANUAL_DRIVER_REGISTRATION(PoserSBA) + MANUAL_DRIVER_REGISTRATION(PoserCharlesRefine) + MANUAL_DRIVER_REGISTRATION(PoserMPFIT) MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive) MANUAL_DRIVER_REGISTRATION(DriverRegPlayback) diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 2e47b9e..4fa3284 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -144,10 +144,6 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { FLT *values = NULL; if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { so->acc_bias = values; - const FLT bias_units = 1. / 1000.; // I deeply suspect bias is in milligravities -JB - so->acc_bias[0] *= bias_units; - so->acc_bias[1] *= bias_units; - so->acc_bias[2] *= bias_units; } } if (jsoneq(ct0conf, tk, "acc_scale") == 0) { @@ -174,6 +170,51 @@ 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; + } + 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; + } + } 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. + { + // 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 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.); + + // 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.); + } + char fname[64]; sprintf(fname, "calinfo/%s_points.csv", so->codename); diff --git a/src/survive_imu.c b/src/survive_imu.c index 36d1aeb..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; } @@ -174,7 +122,7 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do } void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { - if (tracker->last_data.timecode == 0) { + if (!tracker->is_initialized) { tracker->pose.Rot[0] = 1.; if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; @@ -182,7 +130,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker const FLT up[3] = {0, 0, 1}; quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir); - + tracker->is_initialized = true; return; } @@ -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; @@ -233,6 +178,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker, SurvivePose *pose, const FLT *R) { + if (!tracker->is_initialized) { + tracker->pose = *pose; + return; + } + // Kalman filter assuming: // F -> Identity // H -> Identity @@ -265,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 8d8f0ed..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) { 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 1ffb737..0fae736 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1233,31 +1233,15 @@ 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; - //XXX XXX BIG TODO!!! Actually recal gyro data. - FLT agm[9] = { readdata[1], readdata[2], readdata[3], - readdata[4], readdata[5], readdata[6], - 0,0,0 }; - -// if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]); + 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)); calibrate_acc(w, agm); - - //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... - agm[0]*=(1.0f/255.0f); - agm[1]*=(1.0f/255.0f); - agm[2]*=(1.0f/255.0f); - calibrate_gyro(w, agm+3); - - //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... - agm[3]*=(1.0f/255.0f); - agm[4]*=(1.0f/255.0f); - agm[5]*=(1.0f/255.0f); - w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); - - int16_t * k = (int16_t *)readdata+1; - //printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] ); readdata += 13; qty -= 13; + type &= ~0xe8; if( qty ) { @@ -1495,24 +1479,8 @@ void survive_data_cb( SurviveUSBInterface * si ) 0, 0}; - //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 - agm[0] *= (float)(2. / 8192.0); - agm[1] *= (float)(2. / 8192.0); - agm[2] *= (float)(2. / 8192.0); calibrate_acc(obj, agm); - - // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits - // FLT deg_per_sec = 250; - // FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.; - FLT DEGREES_TO_RADS = 3.14159 / 180.; - FLT conv = 1. / 10. * DEGREES_TO_RADS; calibrate_gyro(obj, agm + 3); - agm[3] *= conv; - agm[4] *= conv; - agm[5] *= conv; - ctx->imuproc( obj, 3, agm, timecode, code ); } } 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/winbuild/calibrate/calibrate.vcxproj b/winbuild/calibrate/calibrate.vcxproj index d0b1c48..feb72fd 100644 --- a/winbuild/calibrate/calibrate.vcxproj +++ b/winbuild/calibrate/calibrate.vcxproj @@ -17,6 +17,14 @@ <Configuration>Release</Configuration> <Platform>x64</Platform> </ProjectConfiguration> + <ProjectConfiguration Include="RelWDebInfo|Win32"> + <Configuration>RelWDebInfo</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="RelWDebInfo|x64"> + <Configuration>RelWDebInfo</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> </ItemGroup> <PropertyGroup Label="Globals"> <VCProjectVersion>15.0</VCProjectVersion> @@ -39,6 +47,13 @@ <WholeProgramOptimization>true</WholeProgramOptimization> <CharacterSet>MultiByte</CharacterSet> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration"> <ConfigurationType>Application</ConfigurationType> <UseDebugLibraries>true</UseDebugLibraries> @@ -52,6 +67,13 @@ <WholeProgramOptimization>true</WholeProgramOptimization> <CharacterSet>MultiByte</CharacterSet> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> <ImportGroup Label="ExtensionSettings"> </ImportGroup> @@ -63,12 +85,18 @@ <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> + <ImportGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'" Label="PropertySheets"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> + <ImportGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'" Label="PropertySheets"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> <PropertyGroup Label="UserMacros" /> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> <LinkIncremental>true</LinkIncremental> @@ -80,9 +108,15 @@ <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <LinkIncremental>false</LinkIncremental> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <LinkIncremental>false</LinkIncremental> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> <ClCompile> <PrecompiledHeader> @@ -152,6 +186,30 @@ <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> </ProjectReference> </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalLibraryDirectories> + </AdditionalLibraryDirectories> + <AdditionalDependencies>setupapi.lib;dbghelp.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <ClCompile> <WarningLevel>Level3</WarningLevel> @@ -176,6 +234,30 @@ <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> </ProjectReference> </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;RUNTIME_SYMNUMX;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalLibraryDirectories> + </AdditionalLibraryDirectories> + <AdditionalDependencies>setupapi.lib;dbghelp.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> <ItemGroup> <ClCompile Include="..\..\calibrate.c" /> <ClCompile Include="..\..\redist\CNFG3D.c" /> diff --git a/winbuild/data_recorder/data_recorder.vcxproj b/winbuild/data_recorder/data_recorder.vcxproj index e63d474..9f3f86a 100644 --- a/winbuild/data_recorder/data_recorder.vcxproj +++ b/winbuild/data_recorder/data_recorder.vcxproj @@ -17,6 +17,14 @@ <Configuration>Release</Configuration> <Platform>x64</Platform> </ProjectConfiguration> + <ProjectConfiguration Include="RelWDebInfo|Win32"> + <Configuration>RelWDebInfo</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="RelWDebInfo|x64"> + <Configuration>RelWDebInfo</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> </ItemGroup> <PropertyGroup Label="Globals"> <VCProjectVersion>15.0</VCProjectVersion> @@ -39,6 +47,13 @@ <WholeProgramOptimization>true</WholeProgramOptimization> <CharacterSet>MultiByte</CharacterSet> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration"> <ConfigurationType>Application</ConfigurationType> <UseDebugLibraries>true</UseDebugLibraries> @@ -52,6 +67,13 @@ <WholeProgramOptimization>true</WholeProgramOptimization> <CharacterSet>MultiByte</CharacterSet> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> <ImportGroup Label="ExtensionSettings"> </ImportGroup> @@ -63,12 +85,18 @@ <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> + <ImportGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'" Label="PropertySheets"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> + <ImportGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'" Label="PropertySheets"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> <PropertyGroup Label="UserMacros" /> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <LinkIncremental>true</LinkIncremental> @@ -80,9 +108,15 @@ <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <LinkIncremental>false</LinkIncremental> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <LinkIncremental>false</LinkIncremental> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <ClCompile> <PrecompiledHeader> @@ -143,6 +177,28 @@ <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> </ProjectReference> </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalDependencies>setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <ClCompile> <WarningLevel>Level3</WarningLevel> @@ -165,6 +221,28 @@ <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> </ProjectReference> </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalDependencies>setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> <ItemGroup> <ProjectReference Include="..\libsurvive\libsurvive.vcxproj"> <Project>{435cfd2c-8724-42ee-8fde-71ef7d2c6b2f}</Project> diff --git a/winbuild/libsurvive/libsurvive.vcxproj b/winbuild/libsurvive/libsurvive.vcxproj index 85e9f5a..620e4e6 100644 --- a/winbuild/libsurvive/libsurvive.vcxproj +++ b/winbuild/libsurvive/libsurvive.vcxproj @@ -17,6 +17,14 @@ <Configuration>Release</Configuration> <Platform>x64</Platform> </ProjectConfiguration> + <ProjectConfiguration Include="RelWDebInfo|Win32"> + <Configuration>RelWDebInfo</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="RelWDebInfo|x64"> + <Configuration>RelWDebInfo</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> </ItemGroup> <PropertyGroup Label="Globals"> <VCProjectVersion>15.0</VCProjectVersion> @@ -33,7 +41,14 @@ <CharacterSet>MultiByte</CharacterSet> </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> - <ConfigurationType>StaticLibrary</ConfigurationType> + <ConfigurationType>DynamicLibrary</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'" Label="Configuration"> + <ConfigurationType>DynamicLibrary</ConfigurationType> <UseDebugLibraries>false</UseDebugLibraries> <PlatformToolset>v141</PlatformToolset> <WholeProgramOptimization>true</WholeProgramOptimization> @@ -46,7 +61,14 @@ <CharacterSet>MultiByte</CharacterSet> </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration"> - <ConfigurationType>StaticLibrary</ConfigurationType> + <ConfigurationType>DynamicLibrary</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'" Label="Configuration"> + <ConfigurationType>DynamicLibrary</ConfigurationType> <UseDebugLibraries>false</UseDebugLibraries> <PlatformToolset>v141</PlatformToolset> <WholeProgramOptimization>true</WholeProgramOptimization> @@ -64,12 +86,18 @@ <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> + <ImportGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'" Label="PropertySheets"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> + <ImportGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'" Label="PropertySheets"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> <PropertyGroup Label="UserMacros" /> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <IncludePath>$(IncludePath)</IncludePath> @@ -89,7 +117,7 @@ <SubSystem>Windows</SubSystem> <AdditionalDependencies>DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies)</AdditionalDependencies> <GenerateMapFile>true</GenerateMapFile> - <MapFileName>$(IntDir)Test.txt</MapFileName> + <MapFileName>libsurvive.map</MapFileName> <MapExports>true</MapExports> <ModuleDefinitionFile> </ModuleDefinitionFile> @@ -106,11 +134,15 @@ </PrecompiledHeader> <WarningLevel>Level3</WarningLevel> <Optimization>Disabled</Optimization> - <PreprocessorDefinitions>USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <PreprocessorDefinitions>FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions)</PreprocessorDefinitions> <AdditionalIncludeDirectories>..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> </ClCompile> <Link> <SubSystem>Windows</SubSystem> + <AdditionalDependencies>DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateMapFile>true</GenerateMapFile> + <MapExports>true</MapExports> + <MapFileName>libsurvive.map</MapFileName> </Link> <ProjectReference /> </ItemDefinitionGroup> @@ -122,13 +154,39 @@ <Optimization>MaxSpeed</Optimization> <FunctionLevelLinking>true</FunctionLevelLinking> <IntrinsicFunctions>true</IntrinsicFunctions> - <PreprocessorDefinitions>USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <PreprocessorDefinitions>FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Windows</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalDependencies>DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateMapFile>true</GenerateMapFile> + <MapExports>true</MapExports> + <MapFileName>libsurvive.map</MapFileName> + </Link> + <ProjectReference /> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions)</PreprocessorDefinitions> <AdditionalIncludeDirectories>..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> </ClCompile> <Link> <SubSystem>Windows</SubSystem> <EnableCOMDATFolding>true</EnableCOMDATFolding> <OptimizeReferences>true</OptimizeReferences> + <AdditionalDependencies>DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateMapFile>true</GenerateMapFile> + <MapExports>true</MapExports> + <MapFileName>libsurvive.map</MapFileName> </Link> <ProjectReference /> </ItemDefinitionGroup> @@ -140,13 +198,39 @@ <Optimization>MaxSpeed</Optimization> <FunctionLevelLinking>true</FunctionLevelLinking> <IntrinsicFunctions>true</IntrinsicFunctions> - <PreprocessorDefinitions>USE_DOUBLE;RUNTIME_SYMNUM;RUNTIME_SYMNUMX;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;NDEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <PreprocessorDefinitions>FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions)</PreprocessorDefinitions> <AdditionalIncludeDirectories>..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> </ClCompile> <Link> <SubSystem>Windows</SubSystem> <EnableCOMDATFolding>true</EnableCOMDATFolding> <OptimizeReferences>true</OptimizeReferences> + <AdditionalDependencies>DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateMapFile>true</GenerateMapFile> + <MapExports>true</MapExports> + <MapFileName>libsurvive.map</MapFileName> + </Link> + <ProjectReference /> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>FLT=double;USE_DOUBLE;MANUAL_REGISTRATION;NOZLIB;_CRT_SECURE_NO_WARNINGS;HIDAPI;WINDOWS;_DEBUG;_LIB;HAVE_LAPACK_CONFIG_H;LAPACK_COMPLEX_STRUCTURE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\winbuild;..\..\include\libsurvive;..\..\redist;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Windows</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalDependencies>DbgHelp.lib;SetupAPI.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateMapFile>true</GenerateMapFile> + <MapExports>true</MapExports> + <MapFileName>libsurvive.map</MapFileName> </Link> <ProjectReference /> </ItemDefinitionGroup> @@ -160,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" /> @@ -170,10 +255,13 @@ <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" /> @@ -185,6 +273,7 @@ <ClCompile Include="..\..\src\survive_default_devices.c" /> <ClCompile Include="..\..\src\survive_disambiguator.c" /> <ClCompile Include="..\..\src\survive_driverman.c" /> + <ClCompile Include="..\..\src\survive_imu.c" /> <ClCompile Include="..\..\src\survive_playback.c" /> <ClCompile Include="..\..\src\survive_process.c" /> <ClCompile Include="..\..\src\survive_reproject.c" /> @@ -205,11 +294,13 @@ <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 61cec4c..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"> @@ -137,6 +137,21 @@ </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> @@ -194,8 +209,14 @@ <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> diff --git a/winbuild/test/test.vcxproj b/winbuild/test/test.vcxproj index 8df96d3..ae130b4 100644 --- a/winbuild/test/test.vcxproj +++ b/winbuild/test/test.vcxproj @@ -17,6 +17,14 @@ <Configuration>Release</Configuration> <Platform>x64</Platform> </ProjectConfiguration> + <ProjectConfiguration Include="RelWDebInfo|Win32"> + <Configuration>RelWDebInfo</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="RelWDebInfo|x64"> + <Configuration>RelWDebInfo</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> </ItemGroup> <PropertyGroup Label="Globals"> <VCProjectVersion>15.0</VCProjectVersion> @@ -39,6 +47,13 @@ <WholeProgramOptimization>true</WholeProgramOptimization> <CharacterSet>Unicode</CharacterSet> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>Unicode</CharacterSet> + </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration"> <ConfigurationType>Application</ConfigurationType> <UseDebugLibraries>true</UseDebugLibraries> @@ -52,6 +67,13 @@ <WholeProgramOptimization>true</WholeProgramOptimization> <CharacterSet>Unicode</CharacterSet> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v141</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>Unicode</CharacterSet> + </PropertyGroup> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> <ImportGroup Label="ExtensionSettings"> </ImportGroup> @@ -63,12 +85,18 @@ <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> + <ImportGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'" Label="PropertySheets"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> </ImportGroup> + <ImportGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'" Label="PropertySheets"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> <PropertyGroup Label="UserMacros" /> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <LinkIncremental>true</LinkIncremental> @@ -81,9 +109,15 @@ <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <LinkIncremental>false</LinkIncremental> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <LinkIncremental>false</LinkIncremental> </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'"> + <LinkIncremental>false</LinkIncremental> + </PropertyGroup> <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> <ClCompile> <PrecompiledHeader> @@ -144,6 +178,28 @@ <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> </ProjectReference> </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalDependencies>setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> <ClCompile> <WarningLevel>Level3</WarningLevel> @@ -166,6 +222,28 @@ <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> </ProjectReference> </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='RelWDebInfo|x64'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>USE_DOUBLE;HIDAPI;_CRT_SECURE_NO_WARNINGS;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\windows;..\..\include\libsurvive;..\..\redist;</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalDependencies>setupapi.lib;dbghelp.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + <ProjectReference> + <UseLibraryDependencyInputs>true</UseLibraryDependencyInputs> + </ProjectReference> + </ItemDefinitionGroup> <ItemGroup> <ProjectReference Include="..\libsurvive\libsurvive.vcxproj"> <Project>{435cfd2c-8724-42ee-8fde-71ef7d2c6b2f}</Project> |