From be3fa4562f9578472de1ded5588df8dc502898c6 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 21 Mar 2018 00:53:46 -0600 Subject: Tweaked thresholds, played with single sweep solver --- src/poser_sba.c | 186 ++++++++++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 168 insertions(+), 18 deletions(-) (limited to 'src/poser_sba.c') diff --git a/src/poser_sba.c b/src/poser_sba.c index 44af9c1..a29dffe 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -22,6 +22,12 @@ typedef struct { SurviveObject *so; } sba_context; +typedef struct { + sba_context hdr; + int acode; + int lh; +} sba_context_single_sweep; + void metric_function(int j, int i, double *aj, double *xij, void *adata) { sba_context *ctx = (sba_context *)(adata); SurviveObject *so = ctx->so; @@ -51,6 +57,26 @@ 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) { size_t rtn = 0; @@ -89,6 +115,30 @@ 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) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + + sba_context_single_sweep *ctx = (sba_context_single_sweep *)(adata); + SurviveObject *so = ctx->hdr.so; + int lh = ctx->lh; + int acode = ctx->acode; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + quatnormalize(obj.Rot, obj.Rot); + FLT xyz[3]; + ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]); + + // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; + 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); + *xij = out[acode]; +} + void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { SurvivePose obj = *(SurvivePose *)bi; int sensor_idx = j >> 1; @@ -109,13 +159,116 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { 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 %lu measurements", 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 %lu meas", (info[0] / meas_size * 2), meas_size); + SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); + cnt = 0; + } + } + + return info[1] / meas_size * 2; +} + + static double run_sba_find_3d_structure(survive_calibration_config options, PoserDataLight *pdl, SurviveObject *so, SurviveSensorActivations *scene, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; - char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); - double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); + 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); static int failure_count = 500; @@ -125,8 +278,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose SV_INFO("Can't solve for position with just %lu measurements", meas_size); failure_count = 0; } - free(vmask); - free(meas); return -1; } failure_count = 0; @@ -150,8 +301,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose pdl->hdr = hdr; if (locations.hasInfo == false) { - free(vmask); - free(meas); return -1; } else if (locations.hasInfo) { @@ -206,9 +355,6 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose } } - free(vmask); - free(meas); - return info[1] / meas_size * 2; } @@ -216,8 +362,8 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; - char *vmask = malloc(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); - double *meas = malloc(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); + 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); SurvivePose camera_params[2] = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}; @@ -282,9 +428,6 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd // Docs say info[0] should be divided by meas; I don't buy it really... // std::cerr << info[0] / meas.size() * 2 << " original reproj error" << std::endl; - free(vmask); - free(meas); - { SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... @@ -299,11 +442,18 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { switch (pd->pt) { case POSERDATA_LIGHT: { SurviveSensorActivations *scene = &so->activations; - PoserDataLight *lightData = (PoserDataLight *)pd; - - survive_calibration_config config = *survive_calibration_default_config(); - FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5); +/* + static int last_acode = -1; + static int last_lh = -1; + if(last_lh != lightData->lh || last_acode != lightData->acode) { + */ + survive_calibration_config config = *survive_calibration_default_config(); + FLT error = run_sba_find_3d_structure(config, lightData, so, scene, 50, .5); + /*} + last_lh = lightData->lh; + last_acode = lightData->acode; + */ return 0; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3