Make gss prefer system with set lh positions; avoid rotations
@@ -791,7 +791,7 @@ bool solve_global_scene(struct SurviveContext *ctx, MPFITData *d, PoserDataGloba
.objectUpVectorVariance = d->stationary_obj_up_variance,
.disableVelocity = true,
.covarAllParams = true,
- .nofilter = false};
+ .nofilter = scenes_cnt < 8};
SURVIVE_OPTIMIZER_SETUP_STACK_BUFFERS(mpfitctx, 0);
int useJacobians = 1;
@@ -867,19 +867,22 @@ bool solve_global_scene(struct SurviveContext *ctx, MPFITData *d, PoserDataGloba
if (worldEstablishedLh != -1) {
//start =
//mpfitctx.mp_parameters_info[survive_optimizer_get_camera_index(&mpfitctx) + 7 * worldEstablishedLh].fixed = true;
- /*
+
survive_optimizer_measurement* meas = survive_optimizer_emplace_meas(&mpfitctx,
survive_optimizer_measurement_type_camera_position); meas->camera_pos.camera = worldEstablishedLh;
meas->variance = 1;
+
copy3d(meas->camera_pos.pos, survive_get_lighthouse_position(ctx, worldEstablishedLh)->Pos);
mpfitctx.mp_parameters_info[bestObjForCal* 7].fixed = true;
SV_VERBOSE(10, "Locking lh %d to " Point3_format, worldEstablishedLh,
LINMATH_VEC3_EXPAND(meas->camera_pos.pos));
- */
+
+ /*
for (int i = 0; i < 3; i++) {
- mpfitctx.mp_parameters_info[survive_optimizer_get_camera_index(&mpfitctx) + 7 * worldEstablishedLh + i]
- .fixed = true;
+ mpfitctx.mp_parameters_info[survive_optimizer_get_camera_index(&mpfitctx) + 7 * worldEstablishedLh +
+ i] .fixed = true;
}
+ */
} else {
if (quatiszero(survive_optimizer_get_pose(&mpfitctx)[bestObjForCal].Rot)) {
const FLT up[3] = {0, 0, 1};
@@ -933,6 +936,15 @@ bool solve_global_scene(struct SurviveContext *ctx, MPFITData *d, PoserDataGloba
survive_optimizer_get_camera(&mpfitctx)[i] = initial_guess;
SV_VERBOSE(10, "No estimate for %d", i);
}
+ } else {
+ int pidx = survive_optimizer_get_camera_index(&mpfitctx) + i * 7;
+ for (int j = 0; j < 3; j++) {
+ survive_optimizer_measurement *meas =
+ survive_optimizer_emplace_meas(&mpfitctx, survive_optimizer_measurement_type_parameters_bias);
+ meas->variance = 5;
+ meas->parameter_bias.parameter_index = pidx + j;
+ meas->parameter_bias.expected_value = survive_optimizer_get_camera(&mpfitctx)[i].Pos[j];
+ }
}
}
