aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-04-10 23:47:11 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-04-10 23:47:11 -0600
commite6b487cacfd57792b9cdcde30489faf19f2a4a77 (patch)
tree2435f0302849919aa5001961a95daff693a16a12
parent30cfb87ec0d95e0cb8a671cf8f2327b4204927ed (diff)
parentd7c88592a5450a65f5359e23d87122a04d019503 (diff)
downloadlibsurvive-e6b487cacfd57792b9cdcde30489faf19f2a4a77.tar.gz
libsurvive-e6b487cacfd57792b9cdcde30489faf19f2a4a77.tar.bz2
Merge branch 'master' into simple_api
-rw-r--r--.gitignore1
-rw-r--r--Makefile4
-rw-r--r--include/libsurvive/survive.h3
-rw-r--r--include/libsurvive/survive_imu.h5
-rw-r--r--include/libsurvive/survive_reproject.h8
-rw-r--r--redist/linmath.c85
-rw-r--r--redist/linmath.h4
-rw-r--r--redist/mpfit/DISCLAIMER77
-rw-r--r--redist/mpfit/mpfit.c2289
-rw-r--r--redist/mpfit/mpfit.h192
-rw-r--r--simple_pose_test.c42
-rw-r--r--src/poser_charlesrefine.c157
-rw-r--r--src/poser_epnp.c2
-rw-r--r--src/poser_general_optimizer.c101
-rw-r--r--src/poser_general_optimizer.h29
-rw-r--r--src/poser_mpfit.c283
-rw-r--r--src/poser_sba.c189
-rw-r--r--src/survive.c2
-rw-r--r--src/survive_default_devices.c49
-rw-r--r--src/survive_imu.c150
-rw-r--r--src/survive_reproject.c70
-rw-r--r--src/survive_reproject.generated.h215
-rwxr-xr-xsrc/survive_vive.c42
-rw-r--r--tools/findoptimalconfig/findoptimalconfig.cc7
-rw-r--r--tools/generate_reprojection_functions/Makefile20
-rw-r--r--tools/generate_reprojection_functions/check_generated.c127
-rw-r--r--tools/generate_reprojection_functions/reprojection_functions.sage131
-rw-r--r--tools/showreproject/showreproject.cc6
-rw-r--r--winbuild/calibrate/calibrate.vcxproj82
-rw-r--r--winbuild/data_recorder/data_recorder.vcxproj78
-rw-r--r--winbuild/libsurvive/libsurvive.vcxproj103
-rw-r--r--winbuild/libsurvive/libsurvive.vcxproj.filters25
-rw-r--r--winbuild/test/test.vcxproj78
33 files changed, 4307 insertions, 349 deletions
diff --git a/.gitignore b/.gitignore
index f39d866..4e90b84 100644
--- a/.gitignore
+++ b/.gitignore
@@ -20,6 +20,7 @@ simple_pose_test
calinfo/
*.log
*.png
+*.sage.py
# Windows specific
*.dll
diff --git a/Makefile b/Makefile
index 7532dce..800ca92 100644
--- a/Makefile
+++ b/Makefile
@@ -2,12 +2,12 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test
CC?=gcc
-
CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic #-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>