aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-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.c163
-rw-r--r--src/survive.c2
-rw-r--r--src/survive_imu.c141
-rw-r--r--src/survive_reproject.c70
-rw-r--r--src/survive_reproject.generated.h213
9 files changed, 810 insertions, 194 deletions
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 7e922e7..851ce29 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
case POSERDATA_LIGHT: {
PoserDataLight *lightData = (PoserDataLight *)pd;
- SurvivePose posers[2];
+ SurvivePose posers[2] = { 0 };
int meas[2] = {0, 0};
for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) {
if (so->ctx->bsd[lh].PositionSet) {
diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c
new file mode 100644
index 0000000..b3c4e65
--- /dev/null
+++ b/src/poser_general_optimizer.c
@@ -0,0 +1,101 @@
+#include "poser_general_optimizer.h"
+#include "string.h"
+
+#include <malloc.h>
+#include <assert.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..81d94bf
--- /dev/null
+++ b/src/poser_general_optimizer.h
@@ -0,0 +1,29 @@
+#include <survive.h>
+#include <stdlib.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..439d151
--- /dev/null
+++ b/src/poser_mpfit.c
@@ -0,0 +1,283 @@
+#ifndef USE_DOUBLE
+#define FLT double
+#define USE_DOUBLE
+#endif
+
+#include <malloc.h>
+
+#include "mpfit/mpfit.h"
+#include "poser.h"
+#include <survive.h>
+#include <survive_imu.h>
+
+#include "assert.h"
+#include "linmath.h"
+#include "math.h"
+#include "poser_general_optimizer.h"
+#include "string.h"
+#include "survive_cal.h"
+#include "survive_config.h"
+#include "survive_reproject.h"
+
+typedef struct {
+ SurviveObject *so;
+ FLT *pts3d;
+ int *lh;
+ FLT *meas;
+ SurvivePose camera_params[2];
+} mpfit_context;
+
+typedef struct MPFITData {
+ GeneralOptimizerData opt;
+
+ int last_acode;
+ int last_lh;
+
+ int sensor_time_window;
+ int use_jacobian_function;
+ int required_meas;
+
+ SurviveIMUTracker tracker;
+ bool useIMU;
+
+ struct {
+ int meas_failures;
+ } stats;
+} MPFITData;
+
+static size_t construct_input_from_scene(MPFITData *d, PoserDataLight *pdl, SurviveSensorActivations *scene,
+ double *meas, double *sensors, int *lhs) {
+ size_t rtn = 0;
+ SurviveObject *so = d->opt.so;
+
+ for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
+ for (size_t lh = 0; lh < 2; lh++) {
+ if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) {
+ const double *a = scene->angles[sensor][lh];
+
+ sensors[rtn * 3 + 0] = so->sensor_locations[sensor * 3 + 0];
+ sensors[rtn * 3 + 1] = so->sensor_locations[sensor * 3 + 1];
+ sensors[rtn * 3 + 2] = so->sensor_locations[sensor * 3 + 2];
+ meas[rtn * 2 + 0] = a[0];
+ meas[rtn * 2 + 1] = a[1];
+ lhs[rtn] = lh;
+ rtn++;
+ }
+ }
+ }
+ return rtn;
+}
+
+static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
+ SurvivePose obj = *(SurvivePose *)bi;
+ int sensor_idx = j >> 1;
+ int lh = j & 1;
+
+ mpfit_context *ctx = (mpfit_context *)(adata);
+ SurviveObject *so = ctx->so;
+
+ assert(lh < 2);
+ assert(sensor_idx < so->sensor_ct);
+
+ // quatnormalize(obj.Rot, obj.Rot);
+
+ // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
+ SurvivePose *camera = &so->ctx->bsd[lh].Pose;
+ survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh],
+ &so->ctx->calibration_config);
+}
+
+static void str_metric_function_jac(int j, int i, double *bi, double *xij, void *adata) {
+ SurvivePose obj = *(SurvivePose *)bi;
+ int sensor_idx = j >> 1;
+ int lh = j & 1;
+
+ mpfit_context *ctx = (mpfit_context *)(adata);
+ SurviveObject *so = ctx->so;
+
+ assert(lh < 2);
+ assert(sensor_idx < so->sensor_ct);
+
+ // quatnormalize(obj.Rot, obj.Rot);
+
+ SurvivePose *camera = &so->ctx->bsd[lh].Pose;
+ survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh],
+ &so->ctx->calibration_config);
+}
+
+int mpfunc(int m, int n, double *p, double *deviates, double **derivs, void *private) {
+ mpfit_context *mpfunc_ctx = private;
+
+ SurvivePose *pose = (SurvivePose *)p;
+
+ for (int i = 0; i < m / 2; i++) {
+ FLT out[2];
+ survive_reproject_full(out, pose, mpfunc_ctx->pts3d + i * 3, &mpfunc_ctx->camera_params[mpfunc_ctx->lh[i]],
+ &mpfunc_ctx->so->ctx->bsd[mpfunc_ctx->lh[i]], &mpfunc_ctx->so->ctx->calibration_config);
+ assert(!isnan(out[0]));
+ assert(!isnan(out[1]));
+ deviates[i * 2 + 0] = out[0] - mpfunc_ctx->meas[i * 2 + 0];
+ deviates[i * 2 + 1] = out[1] - mpfunc_ctx->meas[i * 2 + 1];
+
+ if (derivs) {
+ FLT out[7 * 2];
+ survive_reproject_full_jac_obj_pose(
+ out, pose, mpfunc_ctx->pts3d + i * 3, &mpfunc_ctx->camera_params[mpfunc_ctx->lh[i]],
+ &mpfunc_ctx->so->ctx->bsd[mpfunc_ctx->lh[i]], &mpfunc_ctx->so->ctx->calibration_config);
+
+ for (int j = 0; j < n; j++) {
+ derivs[j][i * 2 + 0] = out[j];
+ derivs[j][i * 2 + 1] = out[j + 7];
+ }
+ }
+ }
+
+ return 0;
+}
+
+static double run_mpfit_find_3d_structure(MPFITData *d, PoserDataLight *pdl, SurviveSensorActivations *scene,
+ int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/,
+ SurvivePose *out) {
+ double *covx = 0;
+ SurviveObject *so = d->opt.so;
+
+ mpfit_context mpfitctx = {.so = so,
+ .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose},
+ .meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES),
+ .lh = alloca(sizeof(int) * so->sensor_ct * NUM_LIGHTHOUSES),
+ .pts3d = alloca(sizeof(double) * 3 * so->sensor_ct * NUM_LIGHTHOUSES)};
+
+ size_t meas_size = 2 * construct_input_from_scene(d, pdl, scene, mpfitctx.meas, mpfitctx.pts3d, mpfitctx.lh);
+
+ static int failure_count = 500;
+ bool hasAllBSDs = true;
+ for (int lh = 0; lh < so->ctx->activeLighthouses; lh++)
+ hasAllBSDs &= so->ctx->bsd[lh].PositionSet;
+
+ if (!hasAllBSDs || meas_size < d->required_meas) {
+ if (hasAllBSDs && failure_count++ == 500) {
+ SurviveContext *ctx = so->ctx;
+ SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
+ failure_count = 0;
+ }
+ if (meas_size < d->required_meas) {
+ d->stats.meas_failures++;
+ }
+ return -1;
+ }
+ failure_count = 0;
+
+ SurvivePose soLocation = { 0 };
+
+ if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) {
+ return -1;
+ }
+
+ mp_result result = { 0 };
+ mp_par pars[7] = { 0 };
+
+ const bool debug_jacobian = false;
+ if (d->use_jacobian_function) {
+ for (int i = 0; i < 7; i++) {
+ if (debug_jacobian) {
+ pars[i].side = 1;
+ pars[i].deriv_debug = 1;
+ } else {
+ pars[i].side = 3;
+ }
+ }
+ }
+
+ int res = mpfit(mpfunc, meas_size, 7, soLocation.Pos, pars, 0, &mpfitctx, &result);
+
+ double rtn = -1;
+ bool status_failure = res <= 0;
+ bool error_failure = !general_optimizer_data_record_success(&d->opt, result.bestnorm);
+ if (!status_failure && !error_failure) {
+ quatnormalize(soLocation.Rot, soLocation.Rot);
+ *out = soLocation;
+ rtn = result.bestnorm;
+ }
+
+ return rtn;
+}
+
+int PoserMPFIT(SurviveObject *so, PoserData *pd) {
+ SurviveContext *ctx = so->ctx;
+ if (so->PoserData == 0) {
+ so->PoserData = calloc(1, sizeof(MPFITData));
+ MPFITData *d = so->PoserData;
+
+ general_optimizer_data_init(&d->opt, so);
+ d->useIMU = (bool)survive_configi(ctx, "mpfit-use-imu", SC_GET, 1);
+ d->required_meas = survive_configi(ctx, "mpfit-required-meas", SC_GET, 8);
+
+ d->sensor_time_window =
+ survive_configi(ctx, "mpfit-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2);
+ d->use_jacobian_function = survive_configi(ctx, "mpfit-use-jacobian-function", SC_GET, 1.0);
+
+ SV_INFO("Initializing MPFIT:");
+ SV_INFO("\tmpfit-required-meas: %d", d->required_meas);
+ SV_INFO("\tmpfit-time-window: %d", d->sensor_time_window);
+ SV_INFO("\tmpfit-use-imu: %d", d->useIMU);
+ SV_INFO("\tmpfit-use-jacobian-function: %d", d->use_jacobian_function);
+ }
+ MPFITData *d = so->PoserData;
+ switch (pd->pt) {
+ case POSERDATA_LIGHT: {
+ // No poses if calibration is ongoing
+ if (ctx->calptr && ctx->calptr->stage < 5)
+ return 0;
+ SurviveSensorActivations *scene = &so->activations;
+ PoserDataLight *lightData = (PoserDataLight *)pd;
+ SurvivePose estimate;
+
+ // only process sweeps
+ FLT error = -1;
+ if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) {
+ error = run_mpfit_find_3d_structure(d, lightData, scene, 100, .5, &estimate);
+
+ d->last_lh = lightData->lh;
+ d->last_acode = lightData->acode;
+
+ if (error > 0) {
+ if (d->useIMU) {
+ FLT var_meters = 0.5;
+ FLT var_quat = error + .05;
+ FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat,
+ error * var_quat, error * var_quat, error * var_quat};
+
+ survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var);
+ estimate = d->tracker.pose;
+ }
+
+ PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
+ }
+ }
+ return 0;
+ }
+
+ case POSERDATA_DISASSOCIATE: {
+ SV_INFO("MPFIT stats:");
+ SV_INFO("\tmeas failures %d", d->stats.meas_failures);
+ general_optimizer_data_dtor(&d->opt);
+ free(d);
+ so->PoserData = 0;
+ return 0;
+ }
+ case POSERDATA_IMU: {
+
+ PoserDataIMU *imu = (PoserDataIMU *)pd;
+ if (ctx->calptr && ctx->calptr->stage < 5) {
+ } else if (d->useIMU) {
+ survive_imu_tracker_integrate(so, &d->tracker, imu);
+ PoserData_poser_pose_func(pd, so, &d->tracker.pose);
+ }
+
+ general_optimizer_data_record_imu(&d->opt, imu);
+ }
+ }
+ return -1;
+}
+
+REGISTER_LINKTIME(PoserMPFIT);
diff --git a/src/poser_sba.c b/src/poser_sba.c
index 23e03fc..5166951 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;
-
- 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);
+ SurvivePose soLocation = { 0 };
- 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,25 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
if (so->PoserData == 0) {
so->PoserData = calloc(1, sizeof(SBAData));
SBAData *d = so->PoserData;
- d->failures_to_reset_cntr = 0;
- d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
- d->successes_to_reset_cntr = 0;
- d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, -1);
+
+ general_optimizer_data_init(&d->opt, so);
d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1);
d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
- d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001);
+
d->sensor_time_window =
survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2);
d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0);
d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0);
- d->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) {
@@ -438,8 +418,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
if (error < 0) {
- if (d->failures_to_reset_cntr > 0)
- d->failures_to_reset_cntr--;
+
}
else {
if (d->useIMU) {
@@ -453,8 +432,6 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
}
PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
- if (d->successes_to_reset_cntr > 0)
- d->successes_to_reset_cntr--;
}
}
return 0;
@@ -466,22 +443,24 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
// std::cerr << "Average reproj error: " << error << std::endl;
return 0;
}
+ case POSERDATA_DISASSOCIATE: {
+ SV_INFO("SBA stats:");
+ SV_INFO("\tmeas failures %d", d->stats.meas_failures);
+ general_optimizer_data_dtor(&d->opt);
+ free(d);
+ so->PoserData = 0;
+ return 0;
+ }
case POSERDATA_IMU: {
PoserDataIMU * imu = (PoserDataIMU*)pd;
if (ctx->calptr && ctx->calptr->stage < 5) {
- } else if(d->useIMU){
- survive_imu_tracker_integrate(so, &d->tracker, imu);
- PoserData_poser_pose_func(pd, so, &d->tracker.pose);
+ } else if (d->useIMU) {
+ survive_imu_tracker_integrate(so, &d->tracker, imu);
+ PoserData_poser_pose_func(pd, so, &d->tracker.pose);
}
- } // INTENTIONAL FALLTHROUGH
- default: {
- const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");
- PoserCB driver = (PoserCB)GetDriver(subposer);
- if (driver) {
- return driver(so, pd);
- }
- break;
+
+ general_optimizer_data_record_imu(&d->opt, imu);
}
}
return -1;
diff --git a/src/survive.c b/src/survive.c
index 9e750f9..1ab09da 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -114,6 +114,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
MANUAL_DRIVER_REGISTRATION(PoserDummy)
MANUAL_DRIVER_REGISTRATION(PoserEPNP)
MANUAL_DRIVER_REGISTRATION(PoserSBA)
+ MANUAL_DRIVER_REGISTRATION(PoserCharlesRefine)
+ MANUAL_DRIVER_REGISTRATION(PoserMPFIT)
MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive)
MANUAL_DRIVER_REGISTRATION(DriverRegPlayback)
diff --git a/src/survive_imu.c b/src/survive_imu.c
index 8f4266a..9cd0322 100644
--- a/src/survive_imu.c
+++ b/src/survive_imu.c
@@ -1,107 +1,56 @@
#include "survive_imu.h"
#include "linmath.h"
+#include "math.h"
#include "survive_internal.h"
+#include <memory.h>
#include <survive_imu.h>
-//---------------------------------------------------------------------------------------------------
-// Definitions
+// Mahoney is due to https://hal.archives-ouvertes.fr/hal-00488376/document
+// See also http://www.olliw.eu/2013/imu-data-fusing/#chapter41 and
+// http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+static void mahony_ahrs(SurviveIMUTracker *tracker, LinmathVec3d _gyro, LinmathVec3d _accel) {
+ LinmathVec3d gyro;
+ memcpy(gyro, _gyro, 3 * sizeof(FLT));
-#define sampleFreq 240.0f // sample frequency in Hz
-#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain
-#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
+ LinmathVec3d accel;
+ memcpy(accel, _accel, 3 * sizeof(FLT));
-//---------------------------------------------------------------------------------------------------
-// Function declarations
+ const FLT sample_f = 240;
+ const FLT prop_gain = .5;
+ const FLT int_gain = 0;
-float invSqrt(float x) {
- float halfx = 0.5f * x;
- float y = x;
- long i = *(long *)&y;
- i = 0x5f3759df - (i >> 1);
- y = *(float *)&i;
- y = y * (1.5f - (halfx * y * y));
- return y;
-}
-//---------------------------------------------------------------------------------------------------
-// IMU algorithm update
-// From http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
-static void MahonyAHRSupdateIMU(SurviveIMUTracker *tracker, float gx, float gy, float gz, float ax, float ay,
- float az) {
- float recipNorm;
- float halfvx, halfvy, halfvz;
- float halfex, halfey, halfez;
- float qa, qb, qc;
-
- const float twoKp = twoKpDef; // 2 * proportional gain (Kp)
- const float twoKi = twoKiDef; // 2 * integral gain (Ki)
-
- float q0 = tracker->pose.Rot[0];
- float q1 = tracker->pose.Rot[1];
- float q2 = tracker->pose.Rot[2];
- float q3 = tracker->pose.Rot[3];
-
- // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
- if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
-
- // Normalise accelerometer measurement
- recipNorm = invSqrt(ax * ax + ay * ay + az * az);
- ax *= recipNorm;
- ay *= recipNorm;
- az *= recipNorm;
-
- // Estimated direction of gravity and vector perpendicular to magnetic flux
- halfvx = q1 * q3 - q0 * q2;
- halfvy = q0 * q1 + q2 * q3;
- halfvz = q0 * q0 - 0.5f + q3 * q3;
-
- // Error is sum of cross product between estimated and measured direction of gravity
- halfex = (ay * halfvz - az * halfvy);
- halfey = (az * halfvx - ax * halfvz);
- halfez = (ax * halfvy - ay * halfvx);
-
- // Compute and apply integral feedback if enabled
- if (twoKi > 0.0f) {
- tracker->integralFBx += twoKi * halfex * (1.0f / sampleFreq); // tracker->integral error scaled by Ki
- tracker->integralFBy += twoKi * halfey * (1.0f / sampleFreq);
- tracker->integralFBz += twoKi * halfez * (1.0f / sampleFreq);
- gx += tracker->integralFBx; // apply tracker->integral feedback
- gy += tracker->integralFBy;
- gz += tracker->integralFBz;
- } else {
- tracker->integralFBx = 0.0f; // prevent tracker->integral windup
- tracker->integralFBy = 0.0f;
- tracker->integralFBz = 0.0f;
+ FLT *q = tracker->pose.Rot;
+
+ FLT mag_accel = magnitude3d(accel);
+
+ if (mag_accel != 0.0) {
+ scale3d(accel, accel, 1. / mag_accel);
+
+ // Equiv of q^-1 * G
+ LinmathVec3d v = {q[1] * q[3] - q[0] * q[2], q[0] * q[1] + q[2] * q[3], q[0] * q[0] - 0.5 + q[3] * q[3]};
+
+ LinmathVec3d error;
+ cross3d(error, accel, v);
+
+ if (int_gain > 0.0f) {
+ LinmathVec3d fb_correction;
+ scale3d(fb_correction, error, int_gain * 2. / sample_f);
+ add3d(tracker->integralFB, tracker->integralFB, fb_correction);
+ add3d(gyro, gyro, tracker->integralFB);
}
- // Apply proportional feedback
- gx += twoKp * halfex;
- gy += twoKp * halfey;
- gz += twoKp * halfez;
+ scale3d(error, error, prop_gain * 2.);
+ add3d(gyro, gyro, error);
}
- // Integrate rate of change of quaternion
- gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
- gy *= (0.5f * (1.0f / sampleFreq));
- gz *= (0.5f * (1.0f / sampleFreq));
- qa = q0;
- qb = q1;
- qc = q2;
- q0 += (-qb * gx - qc * gy - q3 * gz);
- q1 += (qa * gx + qc * gz - q3 * gy);
- q2 += (qa * gy - qb * gz + q3 * gx);
- q3 += (qa * gz + qb * gy - qc * gx);
-
- // Normalise quaternion
- recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
- q0 *= recipNorm;
- q1 *= recipNorm;
- q2 *= recipNorm;
- q3 *= recipNorm;
-
- tracker->pose.Rot[0] = q0;
- tracker->pose.Rot[1] = q1;
- tracker->pose.Rot[2] = q2;
- tracker->pose.Rot[3] = q3;
+ scale3d(gyro, gyro, 0.5 / sample_f);
+
+ LinmathQuat correction = {
+ (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]), (q[0] * gyro[0] + q[2] * gyro[2] - q[3] * gyro[1]),
+ (q[0] * gyro[1] - q[1] * gyro[2] + q[3] * gyro[0]), (q[0] * gyro[2] + q[1] * gyro[1] - q[2] * gyro[0])};
+
+ quatadd(q, q, correction);
+ quatnormalize(q, q);
}
static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) {
@@ -125,7 +74,6 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode,
}
//(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.;
- tracker->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0;
tracker->lastGTTime = timecode;
tracker->lastGT = *pose;
}
@@ -198,10 +146,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker
tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90;
}
- float gx = data->gyro[0], gy = data->gyro[1], gz = data->gyro[2];
- float ax = data->accel[0], ay = data->accel[1], az = data->accel[2];
-
- MahonyAHRSupdateIMU(tracker, gx, gy, gz, ax, ay, az);
+ mahony_ahrs(tracker, data->gyro, data->accel);
FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz;
@@ -270,4 +215,4 @@ void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timec
tracker->lastGTTime = timecode;
tracker->lastGT = tracker->pose;
-} \ No newline at end of file
+}
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index d6ba70b..921386b 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -1,8 +1,72 @@
#include "survive_reproject.h"
-#include <../redist/linmath.h>
+#include "survive_reproject.generated.h"
#include <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..e184775
--- /dev/null
+++ b/src/survive_reproject.generated.h
@@ -0,0 +1,213 @@
+ // 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;
+}
+