diff options
| -rw-r--r-- | src/poser_epnp.c | 2 | ||||
| -rw-r--r-- | src/poser_sba.c | 13 | ||||
| -rw-r--r-- | src/survive_process.c | 4 |
3 files changed, 13 insertions, 6 deletions
diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 632737b..7e86542 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -125,7 +125,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { SurvivePose posers[2]; int meas[2] = {0, 0}; - for (int lh = 0; lh < 2; lh++) { + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { if (so->ctx->bsd[lh].PositionSet) { epnp pnp = {.fu = 1, .fv = 1}; epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct); diff --git a/src/poser_sba.c b/src/poser_sba.c index 069e1d0..8fa8c08 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -190,6 +190,7 @@ static double run_sba_find_3d_structure_single_sweep(survive_calibration_config 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 < d->required_meas) { if (so->ctx->bsd[0].PositionSet && so->ctx->bsd[1].PositionSet && failure_count++ == 500) { SurviveContext *ctx = so->ctx; @@ -284,8 +285,12 @@ static double run_sba_find_3d_structure(SBAData *d, survive_calibration_config o size_t meas_size = construct_input_from_scene(so, pdl, scene, vmask, meas); static int failure_count = 500; - if (so->ctx->bsd[0].PositionSet == 0 || so->ctx->bsd[1].PositionSet == 0 || meas_size < d->required_meas) { - 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; @@ -421,7 +426,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 @@ -439,7 +444,7 @@ static double run_sba(survive_calibration_config options, PoserDataFullScene *pd if (status >= 0) { SurvivePose additionalTx = {0}; - for (int i = 0; i < NUM_LIGHTHOUSES; i++) { + 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); diff --git a/src/survive_process.c b/src/survive_process.c index 6136148..ad7ce97 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -80,7 +80,9 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode .lh = lh, }; - SurviveSensorActivations_add(&so->activations, &l); + // Simulate the use of only one lighthouse in playback mode. + if (lh < ctx->activeLighthouses) + SurviveSensorActivations_add(&so->activations, &l); survive_recording_angle_process(so, sensor_id, acode, timecode, length, angle, lh); |