aboutsummaryrefslogtreecommitdiff
path: root/src/poser_sba.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/poser_sba.c')
-rw-r--r--src/poser_sba.c318
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: {