diff options
Diffstat (limited to 'src/poser_sba.c')
-rw-r--r-- | src/poser_sba.c | 318 |
1 files changed, 158 insertions, 160 deletions
diff --git a/src/poser_sba.c b/src/poser_sba.c index a29dffe..b5962f8 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -20,20 +20,25 @@ typedef struct { survive_calibration_config calibration_config; PoserData *pdfs; SurviveObject *so; + SurvivePose obj_pose; + SurvivePose camera_params[2]; } sba_context; typedef struct { - sba_context hdr; - int acode; - int lh; + 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; - survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, - &so->sensor_locations[i * 3], xij); + SurvivePose obj2world = ctx->obj_pose; + FLT sensorInWorld[3] = {}; + ApplyPoseToPoint(sensorInWorld, obj2world.Pos, &so->sensor_locations[i * 3]); + survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld, + xij); } size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, char *vmask, double *meas) { @@ -57,26 +62,25 @@ 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; +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; + return rtn; } - size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask, double *meas) { size_t rtn = 0; @@ -97,9 +101,10 @@ size_t construct_input_from_scene(const SurviveObject *so, PoserDataLight *pdl, return rtn; } -void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user) { - SurvivePose *poses = (SurvivePose *)(user); - poses[lighthouse] = *pose; +void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, SurvivePose *obj_pose, void *user) { + sba_context *ctx = (sba_context *)user; + ctx->camera_params[lighthouse] = *pose; + ctx->obj_pose = *obj_pose; } typedef struct { @@ -116,27 +121,27 @@ 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; + 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; + 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); + 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]); + 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; + // 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]; + 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) { @@ -159,108 +164,100 @@ 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 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 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*/, @@ -283,7 +280,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose failure_count = 0; SurvivePose soLocation = so->OutPose; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]); + bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); @@ -358,6 +355,7 @@ static double run_sba_find_3d_structure(survive_calibration_config options, Pose return info[1] / meas_size * 2; } +// 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*/) { double *covx = 0; @@ -366,9 +364,10 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd 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}; + sba_context sbactx = {options, &pdfs->hdr, so, .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + .obj_pose = so->OutPose}; - if (true || so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0) { + { const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; @@ -378,7 +377,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions pdfs->hdr.pt = hdr.pt; pdfs->hdr.lighthouseposeproc = sba_set_cameras; - pdfs->hdr.userdata = camera_params; + pdfs->hdr.userdata = &sbactx; driver(so, &pdfs->hdr); pdfs->hdr = hdr; } else { @@ -395,8 +394,6 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd double opts[SBA_OPTSSZ] = {}; double info[SBA_INFOSZ] = {}; - sba_context ctx = {options, &pdfs->hdr, so}; - opts[0] = SBA_INIT_MU; opts[1] = SBA_STOP_THRESH; opts[2] = SBA_STOP_THRESH; @@ -408,22 +405,23 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd NUM_LIGHTHOUSES, // Number of cameras -- 2 lighthouses 0, // Number of cameras to not modify vmask, // boolean vis mask - (double *)&camera_params[0], // camera parameters + (double *)&sbactx.camera_params[0], // camera parameters sizeof(SurvivePose) / sizeof(double), // The number of floats that are in a camera param meas, // 2d points for 3d objs covx, // covariance of measurement. Null sets to identity 2, // 2 points per image metric_function, 0, // jacobia of metric_func - &ctx, // user data + &sbactx, // user data max_iterations, // Max iterations 0, // verbosity opts, // options info); // info if (status >= 0) { - PoserData_lighthouse_pose_func(&pdfs->hdr, so, 0, &camera_params[0]); - PoserData_lighthouse_pose_func(&pdfs->hdr, so, 1, &camera_params[1]); + 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); } // 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; @@ -443,17 +441,17 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_LIGHT: { SurviveSensorActivations *scene = &so->activations; PoserDataLight *lightData = (PoserDataLight *)pd; -/* - 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; - */ + /* + 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: { |