aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-04-08 22:51:57 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-04-09 09:14:39 -0600
commite06e452445bdd3d61f393bde4206cd848a54444b (patch)
treee3316a222a74f8cae5e43d70c22ede77a97ef0e2 /src
parent6363d21d6bfd60564830c410ea75f96b3bc1f129 (diff)
downloadlibsurvive-e06e452445bdd3d61f393bde4206cd848a54444b.tar.gz
libsurvive-e06e452445bdd3d61f393bde4206cd848a54444b.tar.bz2
Created common base for optimizer based posers
Diffstat (limited to 'src')
-rw-r--r--src/poser_epnp.c2
-rw-r--r--src/poser_general_optimizer.c100
-rw-r--r--src/poser_general_optimizer.h29
-rw-r--r--src/poser_sba.c103
4 files changed, 148 insertions, 86 deletions
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index 7e922e7..615c01c 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
case POSERDATA_LIGHT: {
PoserDataLight *lightData = (PoserDataLight *)pd;
- SurvivePose posers[2];
+ SurvivePose posers[2] = {};
int meas[2] = {0, 0};
for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) {
if (so->ctx->bsd[lh].PositionSet) {
diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c
new file mode 100644
index 0000000..313e378
--- /dev/null
+++ b/src/poser_general_optimizer.c
@@ -0,0 +1,100 @@
+#include "poser_general_optimizer.h"
+#include "string.h"
+#include <alloca.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_sba.c b/src/poser_sba.c
index 347b600..4c7fcd1 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -13,6 +13,7 @@
#include "assert.h"
#include "linmath.h"
#include "math.h"
+#include "poser_general_optimizer.h"
#include "string.h"
#include "survive_cal.h"
#include "survive_config.h"
@@ -32,16 +33,11 @@ typedef struct {
} sba_context_single_sweep;
typedef struct SBAData {
+ GeneralOptimizerData opt;
+
int last_acode;
int last_lh;
- int failures_to_reset;
- int failures_to_reset_cntr;
- int successes_to_reset;
- int successes_to_reset_cntr;
-
- FLT max_error;
-
FLT sensor_variance;
FLT sensor_variance_per_second;
int sensor_time_window;
@@ -52,15 +48,8 @@ typedef struct SBAData {
bool useIMU;
struct {
- int runs;
- int poser_seed_runs;
int meas_failures;
- int error_failures;
} stats;
-
- SurviveObject *so;
-
- PoserCB seed_poser;
} SBAData;
static void metric_function(int j, int i, double *aj, double *xij, void *adata) {
@@ -98,7 +87,7 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs,
static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask,
double *meas, double *cov) {
size_t rtn = 0;
- SurviveObject *so = d->so;
+ SurviveObject *so = d->opt.so;
for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) {
for (size_t lh = 0; lh < 2; lh++) {
@@ -215,7 +204,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/,
SurvivePose *out) {
double *covx = 0;
- SurviveObject *so = d->so;
+ SurviveObject *so = d->opt.so;
char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES);
double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES);
@@ -241,37 +230,10 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
}
failure_count = 0;
- SurvivePose soLocation = so->OutPose;
- bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0;
- static bool seed_warning = false;
- if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) {
- SurviveContext *ctx = so->ctx;
- // SV_INFO("Must rerun seed poser");
- PoserCB driver = d->seed_poser;
+ SurvivePose soLocation = {};
- if (driver) {
- PoserData hdr = pdl->hdr;
- memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
- pdl->hdr.pt = hdr.pt;
- pdl->hdr.poseproc = sba_set_position;
-
- sba_set_position_t locations = {0};
- pdl->hdr.userdata = &locations;
- driver(so, &pdl->hdr);
- pdl->hdr = hdr;
- d->stats.poser_seed_runs++;
-
- if (locations.hasInfo == false) {
- return -1;
- } else if (locations.hasInfo) {
- soLocation = locations.poses;
- }
-
- d->successes_to_reset_cntr = d->successes_to_reset;
- } else if (seed_warning == false) {
- seed_warning = true;
- SV_INFO("Not using a seed poser for SBA; results will likely be way off");
- }
+ if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) {
+ return -1;
}
double opts[SBA_OPTSSZ] = {0};
@@ -286,7 +248,6 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size();
opts[4] = 0.0;
- d->stats.runs++;
int status = sba_str_levmar(1, // Number of 3d points
0, // Number of 3d points to fix in spot
NUM_LIGHTHOUSES * so->sensor_ct, vmask,
@@ -303,26 +264,14 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
opts, // options
info); // info
- if (currentPositionValid) {
- // FLT distp[3];
- // sub3d(distp, so->OutPose.Pos, soLocation.Pos);
- // FLT distance = magnitude3d(distp);
-
- // if (distance > 1.)
- // status = -1;
- }
-
double rtn = -1;
bool status_failure = status <= 0;
- bool error_failure = (info[1] / meas_size * 2) >= d->max_error;
+ bool error_failure = !general_optimizer_data_record_success(&d->opt, (info[1] / meas_size * 2));
if (!status_failure && !error_failure) {
- d->failures_to_reset_cntr = d->failures_to_reset;
quatnormalize(soLocation.Rot, soLocation.Rot);
*out = soLocation;
rtn = info[1] / meas_size * 2;
- } else if (error_failure) {
- d->stats.error_failures++;
- }
+ } else
{
SurviveContext *ctx = so->ctx;
@@ -349,7 +298,7 @@ static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_itera
.obj_pose = so->OutPose};
{
- const char *subposer = survive_configs(so->ctx, "sba-seed-poser", SC_GET, "PoserEPNP");
+ const char *subposer = survive_configs(so->ctx, "seed-poser", SC_GET, "PoserEPNP");
PoserCB driver = (PoserCB)GetDriver(subposer);
SurviveContext *ctx = so->ctx;
@@ -429,33 +378,25 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
if (so->PoserData == 0) {
so->PoserData = calloc(1, sizeof(SBAData));
SBAData *d = so->PoserData;
- d->failures_to_reset_cntr = 0;
- d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1);
- d->successes_to_reset_cntr = 0;
- d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, -1);
+
+ general_optimizer_data_init(&d->opt, so);
d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1);
d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8);
- d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001);
+
d->sensor_time_window =
survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2);
d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0);
d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0);
d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0);
- d->so = so;
- const char *subposer = survive_configs(ctx, "sba-seed-poser", SC_GET, "PoserEPNP");
- d->seed_poser = (PoserCB)GetDriver(subposer);
SV_INFO("Initializing SBA:");
SV_INFO("\tsba-required-meas: %d", d->required_meas);
SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance);
SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second);
SV_INFO("\tsba-time-window: %d", d->sensor_time_window);
- SV_INFO("\tsba-max-error: %f", d->max_error);
- SV_INFO("\tsba-successes-to-reset: %d", d->successes_to_reset);
SV_INFO("\tsba-use-imu: %d", d->useIMU);
SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function);
- SV_INFO("\tsba-seed-poser: %s(%p)", subposer, d->seed_poser);
}
SBAData *d = so->PoserData;
switch (pd->pt) {
@@ -477,8 +418,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
if (error < 0) {
- if (d->failures_to_reset_cntr > 0)
- d->failures_to_reset_cntr--;
+
}
else {
if (d->useIMU) {
@@ -492,8 +432,6 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
}
PoserData_poser_pose_func(&lightData->hdr, so, &estimate);
- if (d->successes_to_reset_cntr > 0)
- d->successes_to_reset_cntr--;
}
}
return 0;
@@ -507,9 +445,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
}
case POSERDATA_DISASSOCIATE: {
SV_INFO("SBA stats:");
- SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs);
SV_INFO("\tmeas failures %d", d->stats.meas_failures);
- SV_INFO("\terror failures %d", d->stats.error_failures);
+ general_optimizer_data_dtor(&d->opt);
free(d);
so->PoserData = 0;
return 0;
@@ -522,12 +459,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
survive_imu_tracker_integrate(so, &d->tracker, imu);
PoserData_poser_pose_func(pd, so, &d->tracker.pose);
}
- } // INTENTIONAL FALLTHROUGH
- default: {
- if (d->seed_poser) {
- return d->seed_poser(so, pd);
- }
- break;
+
+ general_optimizer_data_record_imu(&d->opt, imu);
}
}
return -1;