aboutsummaryrefslogtreecommitdiff
path: root/src/poser_sba.c
diff options
context:
space:
mode:
authorJustin Berger <jdavidberger@gmail.com>2018-03-23 07:26:31 +0000
committerJustin Berger <jdavidberger@gmail.com>2018-03-23 07:26:31 +0000
commit50b026760144e76a2e69babe34e6dee0a6a3c9c0 (patch)
tree75290f9ba4932ced6b45f2c25739e213837ce75b /src/poser_sba.c
parent3272ffe5245c6f39f93572d4059f35953dc38faa (diff)
downloadlibsurvive-50b026760144e76a2e69babe34e6dee0a6a3c9c0.tar.gz
libsurvive-50b026760144e76a2e69babe34e6dee0a6a3c9c0.tar.bz2
Added reset cntr for sba
Diffstat (limited to 'src/poser_sba.c')
-rw-r--r--src/poser_sba.c55
1 files changed, 36 insertions, 19 deletions
diff --git a/src/poser_sba.c b/src/poser_sba.c
index 1f604ff..72359da 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -31,6 +31,14 @@ typedef struct {
int lh;
} sba_context_single_sweep;
+typedef struct SBAData {
+ int last_acode;
+ int last_lh;
+ int failures_to_reset;
+ int failures_to_reset_cntr;
+ SurviveIMUTracker tracker;
+} SBAData;
+
void metric_function(int j, int i, double *aj, double *xij, void *adata) {
sba_context *ctx = (sba_context *)(adata);
SurviveObject *so = ctx->so;
@@ -182,7 +190,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 8) {
if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
SurviveContext *ctx = so->ctx;
- SV_INFO("Can't solve for position with just %lu measurements", meas_size);
+ SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
failure_count = 0;
}
return -1;
@@ -254,7 +262,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
// Docs say info[0] should be divided by meas; I don't buy it really...
static int cnt = 0;
if (cnt++ > 1000 || meas_size < 8) {
- SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size);
+ SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (unsigned int)meas_size);
SV_INFO("%f cur reproj error", (info[1] / meas_size * 2));
cnt = 0;
}
@@ -263,7 +271,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config
return info[1] / meas_size * 2;
}
-static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so,
+static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so,
SurviveSensorActivations *scene, int max_iterations /* = 50*/,
double max_reproj_error /* = 0.005*/) {
double *covx = 0;
@@ -276,7 +284,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < 7) {
if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) {
SurviveContext *ctx = so->ctx;
- SV_INFO("Can't solve for position with just %lu measurements", meas_size);
+ SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size);
failure_count = 0;
}
return -1;
@@ -286,10 +294,13 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
SurvivePose soLocation = so->OutPose;
bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0;
+ if(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);
- SurviveContext *ctx = so->ctx;
+
if (driver) {
PoserData hdr = pdl->hdr;
memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
@@ -341,6 +352,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
info); // info
if (status > 0) {
+ d->failures_to_reset_cntr = d->failures_to_reset;
quatnormalize(soLocation.Rot, soLocation.Rot);
PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation);
}
@@ -350,7 +362,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose
// Docs say info[0] should be divided by meas; I don't buy it really...
static int cnt = 0;
if (cnt++ > 1000 || meas_size < 8) {
- SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size);
+ 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;
}
@@ -436,22 +448,19 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd
{
SurviveContext *ctx = so->ctx;
// Docs say info[0] should be divided by meas; I don't buy it really...
- SV_INFO("%f original reproj error for %lu meas", (info[0] / meas_size * 2), meas_size);
+ 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));
}
return info[1] / meas_size * 2;
}
-typedef struct SBAData {
- int last_acode;
- int last_lh;
- SurviveIMUTracker tracker;
-} SBAData;
-
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 = 30;
}
SBAData *d = so->PoserData;
SurviveContext *ctx = so->ctx;
@@ -467,30 +476,38 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
FLT error = -1;
if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) {
survive_calibration_config config = *survive_calibration_default_config();
- error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5);
+ error = run_sba_find_3d_structure(d, config, lightData, so, scene, 50, .5);
d->last_lh = lightData->lh;
d->last_acode = lightData->acode;
}
if (error < 0) {
- PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose);
- // SV_INFO("using imu");
+ if(d->failures_to_reset_cntr > 0)
+ d->failures_to_reset_cntr--;
} else {
- survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose);
+ survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose);
}
+
return 0;
}
case POSERDATA_FULL_SCENE: {
SurviveContext *ctx = so->ctx;
PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd);
survive_calibration_config config = *survive_calibration_default_config();
- SV_INFO("Running sba with %lu", survive_calibration_config_index(&config));
+ SV_INFO("Running sba with %u", (int)survive_calibration_config_index(&config));
double error = run_sba(config, pdfs, so, 50, .005);
// std::cerr << "Average reproj error: " << error << std::endl;
return 0;
}
case POSERDATA_IMU: {
- survive_imu_tracker_integrate(so, &d->tracker, (PoserDataIMU *)pd);
+
+ PoserDataIMU * imu = (PoserDataIMU*)pd;
+ survive_imu_tracker_integrate(so, &d->tracker, imu);
+
+ if (ctx->calptr && ctx->calptr->stage < 5) {
+ } else {
+ PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose);
+ }
} // INTENTIONAL FALLTHROUGH
default: {
const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP");