From 3272ffe5245c6f39f93572d4059f35953dc38faa Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 22 Mar 2018 12:42:11 -0600 Subject: Integrated IMU into SBA --- src/poser_sba.c | 11 +++++++++++ 1 file changed, 11 insertions(+) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index d1677d1..1f604ff 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -7,6 +7,7 @@ #include "poser.h" #include +#include #include "assert.h" #include "linmath.h" @@ -445,6 +446,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd typedef struct SBAData { int last_acode; int last_lh; + SurviveIMUTracker tracker; } SBAData; int PoserSBA(SurviveObject *so, PoserData *pd) { @@ -470,6 +472,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->last_acode = lightData->acode; } + if (error < 0) { + PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose); + // SV_INFO("using imu"); + } else { + survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); + } return 0; } case POSERDATA_FULL_SCENE: { @@ -481,6 +489,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // std::cerr << "Average reproj error: " << error << std::endl; return 0; } + case POSERDATA_IMU: { + survive_imu_tracker_integrate(so, &d->tracker, (PoserDataIMU *)pd); + } // INTENTIONAL FALLTHROUGH default: { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); -- cgit v1.2.3 From 50b026760144e76a2e69babe34e6dee0a6a3c9c0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 07:26:31 +0000 Subject: Added reset cntr for sba --- src/poser_sba.c | 55 ++++++++++++++++++++++++++++++++++++------------------- 1 file changed, 36 insertions(+), 19 deletions(-) (limited to 'src/poser_sba.c') 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"); -- cgit v1.2.3 From 2b63278497130d01b1fbc7e6a94b6ad8e32ab4dd Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 23 Mar 2018 14:07:23 +0000 Subject: Made IMU use optional --- src/poser_sba.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index 72359da..efdeeef 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -32,11 +32,12 @@ typedef struct { } sba_context_single_sweep; typedef struct SBAData { - int last_acode; - int last_lh; + int last_acode; + int last_lh; int failures_to_reset; - int failures_to_reset_cntr; - SurviveIMUTracker tracker; + int failures_to_reset_cntr; + SurviveIMUTracker tracker; + bool useIMU; } SBAData; void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -297,7 +298,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o if(d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { SurviveContext *ctx = so->ctx; - SV_INFO("Must rerun seed poser"); + SV_INFO("Running seed poser"); const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); @@ -484,7 +485,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (error < 0) { if(d->failures_to_reset_cntr > 0) d->failures_to_reset_cntr--; - } else { + } else if(d->useIMU) { survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); } @@ -502,10 +503,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_IMU: { PoserDataIMU * imu = (PoserDataIMU*)pd; - survive_imu_tracker_integrate(so, &d->tracker, imu); - if (ctx->calptr && ctx->calptr->stage < 5) { - } else { + } else if(d->useIMU){ + survive_imu_tracker_integrate(so, &d->tracker, imu); PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose); } } // INTENTIONAL FALLTHROUGH -- cgit v1.2.3 From db41a20170bb7f77959b9901a31582ad2ba93db7 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:30:47 -0600 Subject: Madgwick code integrated --- src/poser_sba.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index 1dbc820..a23eb0f 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -272,12 +272,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive info); // info if (currentPositionValid) { - FLT distp[3]; - sub3d(distp, so->OutPose.Pos, soLocation.Pos); - FLT distance = magnitude3d(distp); - ; - if (distance > 1.) - status = -1; + // FLT distp[3]; + // sub3d(distp, so->OutPose.Pos, soLocation.Pos); + // FLT distance = magnitude3d(distp); + + // if (distance > 1.) + // status = -1; } if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; @@ -296,7 +296,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } } - return info[1] / meas_size * 2; + return status; // info[1] / meas_size * 2; } // Optimizes for LH position assuming object is posed at 0 @@ -396,12 +396,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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); - + d->useIMU = true; 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, 0.001); + 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; -- cgit v1.2.3 From fe025b0ff6bfb440da7cec8f388fa951910a86f0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:40:29 -0600 Subject: Removed high pass filter --- src/poser_sba.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index a23eb0f..f6b1131 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -396,7 +396,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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); - d->useIMU = true; + 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 = @@ -411,6 +411,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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-use-imu: %d", d->useIMU); } SBAData *d = so->PoserData; switch (pd->pt) { -- cgit v1.2.3