diff options
Diffstat (limited to 'src/poser_sba.c')
-rw-r--r-- | src/poser_sba.c | 306 |
1 files changed, 128 insertions, 178 deletions
diff --git a/src/poser_sba.c b/src/poser_sba.c index efdeeef..1dbc820 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -3,6 +3,7 @@ #define USE_DOUBLE #endif +#include <malloc.h> #include <sba/sba.h> #include "poser.h" @@ -18,7 +19,6 @@ #include "survive_reproject.h" typedef struct { - survive_calibration_config calibration_config; PoserData *pdfs; SurviveObject *so; SurvivePose obj_pose; @@ -32,26 +32,40 @@ typedef struct { } sba_context_single_sweep; typedef struct SBAData { - int last_acode; - int last_lh; - int failures_to_reset; - int failures_to_reset_cntr; - SurviveIMUTracker tracker; - bool useIMU; + 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 required_meas; + + SurviveIMUTracker tracker; + bool useIMU; + + SurviveObject *so; } SBAData; -void metric_function(int j, int i, double *aj, double *xij, void *adata) { +static void metric_function(int j, int i, double *aj, double *xij, void *adata) { sba_context *ctx = (sba_context *)(adata); SurviveObject *so = ctx->so; SurvivePose obj2world = ctx->obj_pose; - FLT sensorInWorld[3] = {}; + FLT sensorInWorld[3] = {0}; ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]); - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, - xij); + survive_calibration_config cfg = so->ctx->calibration_config; + survive_reproject_from_pose_with_config(so->ctx, &cfg, j, (SurvivePose *)aj, sensorInWorld, xij); } -size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { +static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { size_t measCount = 0; size_t size = so->sensor_ct * NUM_LIGHTHOUSES; // One set per lighthouse for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { @@ -72,35 +86,29 @@ size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char * return measCount; } -size_t construct_input_from_scene_single_sweep(const SurviveObject *so, PoserDataLight *pdl, - SurviveSensorActivations *scene, char *vmask, double *meas, int acode, - int lh) { - size_t rtn = 0; - - for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { - const uint32_t *data_timecode = scene->timecode[sensor][lh]; - if (pdl->timecode - data_timecode[acode & 1] <= SurviveSensorActivations_default_tolerance) { - double *a = scene->angles[sensor][lh]; - vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; - meas[rtn++] = a[acode & 0x1]; - } else { - vmask[sensor * NUM_LIGHTHOUSES + lh] = 0; - } - } - - return rtn; -} - -size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene, - char *vmask, double *meas) { +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; for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { - if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, pdl->timecode, - sensor, lh)) { - double *a = scene->angles[sensor][lh]; + if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { + const double *a = scene->angles[sensor][lh]; + // FLT a[2]; + // survive_apply_bsd_calibration(so->ctx, lh, _a, a); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; + + if (cov) { + *(cov++) = d->sensor_variance + + abs(pdl->timecode - scene->timecode[sensor][lh][0]) * d->sensor_variance_per_second / + (double)so->timebase_hz; + *(cov++) = 0; + *(cov++) = 0; + *(cov++) = d->sensor_variance + + abs(pdl->timecode - scene->timecode[sensor][lh][1]) * d->sensor_variance_per_second / + (double)so->timebase_hz; + } meas[rtn++] = a[0]; meas[rtn++] = a[1]; } else { @@ -125,7 +133,7 @@ typedef struct { SurvivePose poses; } sba_set_position_t; -void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) { +static void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) { sba_set_position_t *user = _user; assert(user->hasInfo == false); user->hasInfo = 1; @@ -133,7 +141,7 @@ void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_po } void *GetDriver(const char *name); -void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) { +static void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) { SurvivePose obj = *(SurvivePose *)bi; int sensor_idx = j >> 1; @@ -153,11 +161,11 @@ void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, voi SurvivePose *camera = &so->ctx->bsd[lh].Pose; FLT out[2]; - survive_reproject_from_pose_with_config(so->ctx, &ctx->hdr.calibration_config, lh, camera, xyz, out); + survive_reproject_from_pose(so->ctx, lh, camera, xyz, out); *xij = out[acode]; } -void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { +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; @@ -174,116 +182,27 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, lh, camera, xyz, xij); -} - -static double run_sba_find_3d_structure_single_sweep(survive_calibration_config options, PoserDataLight *pdl, - SurviveObject *so, SurviveSensorActivations *scene, int acode, - int lh, int max_iterations /* = 50*/, - double max_reproj_error /* = 0.005*/) { - double *covx = 0; - - char *vmask = alloca(sizeof(char) * so->sensor_ct); - double *meas = alloca(sizeof(double) * so->sensor_ct); - size_t meas_size = construct_input_from_scene_single_sweep(so, pdl, scene, vmask, meas, acode, lh); - - static int failure_count = 500; - 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 %u measurements", (unsigned int)meas_size); - failure_count = 0; - } - return -1; - } - failure_count = 0; - - SurvivePose soLocation = so->OutPose; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]); - - { - 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 - pdl->hdr.pt = hdr.pt; - pdl->hdr.rawposeproc = sba_set_position; - - sba_set_position_t locations = {}; - pdl->hdr.userdata = &locations; - driver(so, &pdl->hdr); - pdl->hdr = hdr; - - if (locations.hasInfo == false) { - - return -1; - } else if (locations.hasInfo) { - soLocation = locations.poses; - } - } else { - SV_INFO("Not using a seed poser for SBA; results will likely be way off"); - } - } - - double opts[SBA_OPTSSZ] = {}; - double info[SBA_INFOSZ] = {}; - - sba_context_single_sweep ctx = {.hdr = {options, &pdl->hdr, so}, .acode = acode, .lh = lh}; - - opts[0] = SBA_INIT_MU; - opts[1] = SBA_STOP_THRESH; - opts[2] = SBA_STOP_THRESH; - opts[3] = SBA_STOP_THRESH; - opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); - opts[4] = 0.0; - - int status = sba_str_levmar(1, // Number of 3d points - 0, // Number of 3d points to fix in spot - so->sensor_ct, vmask, - soLocation.Pos, // Reads as the full pose though - 7, // pnp -- SurvivePose - meas, // x* -- measurement data - 0, // cov data - 1, // mnp -- 2 points per image - str_metric_function_single_sweep, - 0, // jacobia of metric_func - &ctx, // user data - max_iterations, // Max iterations - 0, // verbosity - opts, // options - info); // info - - if (status > 0) { - quatnormalize(soLocation.Rot, soLocation.Rot); - PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); - - 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 < 8) { - 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; - } - } - - return info[1] / meas_size * 2; + survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij); } -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*/) { +static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, + int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; + SurviveObject *so = d->so; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); - size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas); + double *cov = + d->sensor_variance_per_second > 0. ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES) : 0; + size_t meas_size = construct_input_from_scene(d, pdl, scene, vmask, meas, cov); static int failure_count = 500; - 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) { + 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; @@ -295,10 +214,9 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o 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("Running seed poser"); + 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); @@ -308,26 +226,27 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o pdl->hdr.pt = hdr.pt; pdl->hdr.rawposeproc = sba_set_position; - sba_set_position_t locations = {}; + 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"); } } - double opts[SBA_OPTSSZ] = {}; - double info[SBA_INFOSZ] = {}; + double opts[SBA_OPTSSZ] = {0}; + double info[SBA_INFOSZ] = {0}; - sba_context ctx = {options, &pdl->hdr, so}; + sba_context ctx = {&pdl->hdr, so}; opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; @@ -342,7 +261,7 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o soLocation.Pos, // Reads as the full pose though 7, // pnp -- SurvivePose meas, // x* -- measurement data - 0, // cov data + cov, // cov data 2, // mnp -- 2 points per image str_metric_function, 0, // jacobia of metric_func @@ -352,8 +271,16 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o opts, // options info); // info - if (status > 0) { - d->failures_to_reset_cntr = d->failures_to_reset; + if (currentPositionValid) { + 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; quatnormalize(soLocation.Rot, soLocation.Rot); PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation); } @@ -362,8 +289,8 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o 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 < 8) { - SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); + 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; } @@ -373,15 +300,15 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(survive_calibration_config options, PoserDataFullScene *pdfs, SurviveObject *so, - int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { +static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, + double max_reproj_error /* = 0.005*/) { double *covx = 0; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); size_t meas_size = construct_input(so, pdfs, vmask, meas); - sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + sba_context sbactx = {&pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, .obj_pose = so->OutPose}; { @@ -400,7 +327,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd } else { SV_INFO("Not using a seed poser for SBA; results will likely be way off"); for (int i = 0; i < 2; i++) { - so->ctx->bsd[i].Pose = (SurvivePose){}; + so->ctx->bsd[i].Pose = (SurvivePose){0}; so->ctx->bsd[i].Pose.Rot[0] = 1.; } } @@ -408,8 +335,8 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd // PoserCharlesSlow(so, (PoserData *)pdfs); } - double opts[SBA_OPTSSZ] = {}; - double info[SBA_INFOSZ] = {}; + double opts[SBA_OPTSSZ] = {0}; + double info[SBA_INFOSZ] = {0}; opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; @@ -419,7 +346,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd opts[4] = 0.0; int status = sba_mot_levmar(so->sensor_ct, // number of 3d points - NUM_LIGHTHOUSES, // Number of cameras -- 2 lighthouses + so->ctx->activeLighthouses, // Number of cameras -- 2 lighthouses 0, // Number of cameras to not modify vmask, // boolean vis mask (double *)&sbactx.camera_params[0], // camera parameters @@ -436,9 +363,13 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd info); // info if (status >= 0) { - SurvivePose additionalTx = {}; - PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &additionalTx, &sbactx.camera_params[0], &sbactx.obj_pose); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &additionalTx, &sbactx.camera_params[1], &sbactx.obj_pose); + SurvivePose additionalTx = {0}; + for (int i = 0; i < so->ctx->activeLighthouses; i++) { + if (quatmagnitude(sbactx.camera_params[i].Rot) != 0) { + PoserData_lighthouse_pose_func(&pdfs->hdr, so, i, &additionalTx, &sbactx.camera_params[i], + &sbactx.obj_pose); + } + } } else { SurviveContext *ctx = so->ctx; SV_INFO("SBA was unable to run %d", status); @@ -457,14 +388,31 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd } int PoserSBA(SurviveObject *so, PoserData *pd) { + SurviveContext *ctx = so->ctx; 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; + 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); + + 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 = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); + d->so = so; + + 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); } SBAData *d = so->PoserData; - SurviveContext *ctx = so->ctx; switch (pd->pt) { case POSERDATA_LIGHT: { // No poses if calibration is ongoing @@ -476,17 +424,21 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // only process sweeps 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(d, config, lightData, so, scene, 50, .5); + error = run_sba_find_3d_structure(d, lightData, scene, 100, .5); + 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) { - survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; + } else { + if (d->useIMU) { + survive_imu_tracker_set_pose(&d->tracker, lightData->timecode, &so->OutPose); + } + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; } return 0; @@ -494,9 +446,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { 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 %u", (int)survive_calibration_config_index(&config)); - double error = run_sba(config, pdfs, so, 50, .005); + double error = run_sba(pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } |