From a3684c90ab0e0fb6bb54dccedf3f1d0fe2f4c44a Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 1 Feb 2022 21:45:53 -0700 Subject: [PATCH] Small fixes in mpfit --- src/poser_mpfit.c | 16 ++++++++++------ src/survive_optimizer.c | 2 +- src/survive_process.c | 2 +- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index 8bfa4d1..05d71a7 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -92,6 +92,7 @@ typedef struct MPFITData { FLT record_reprojection_error; FLT obj_up_variance, lh_up_variance, calibration_stationary_obj_up_variance, stationary_obj_up_variance; + FLT sensor_variance_cal; bool model_velocity; bool globalDataAvailable; struct survive_async_optimizer *async_optimizer; @@ -100,6 +101,7 @@ typedef struct MPFITData { } MPFITData; STRUCT_CONFIG_SECTION(MPFITData) +STRUCT_CONFIG_ITEM("mpfit-cal-sensor-variance", "Light sensor variance for calibration", 1e-4, t->sensor_variance_cal) STRUCT_CONFIG_ITEM("mpfit-model-velocity", "Model velocity in non mpfit process", true, t->model_velocity) STRUCT_CONFIG_ITEM("mpfit-record-reprojection-error", "", 0, t->record_reprojection_error) STRUCT_CONFIG_ITEM("mpfit-object-up-variance", @@ -821,7 +823,7 @@ bool solve_global_scene(struct SurviveContext *ctx, MPFITData *d, PoserDataGloba survive_optimizer_measurement *meas = survive_optimizer_emplace_meas(&mpfitctx, survive_optimizer_measurement_type_light); meas->light.object = i; - meas->variance = d->sensor_variance; + meas->variance = d->sensor_variance_cal; meas->light.value = gss->scenes[i].meas[j].value; meas->light.lh = gss->scenes[i].meas[j].lh; meas->light.axis = gss->scenes[i].meas[j].axis; @@ -970,9 +972,10 @@ bool solve_global_scene(struct SurviveContext *ctx, MPFITData *d, PoserDataGloba return false; } else { - SV_INFO("MPFIT success %f/%10.10f/%7.7f (%d measurements, %d, %s, trace %7.7f)", result.orignorm, - result.bestnorm, sensor_error, (int)mpfitctx.measurementsCnt, res, survive_optimizer_error(res), - cn_trace(&R) / R.rows); + SV_INFO("MPFIT success %f/%10.10f/%7.7f (%d measurements, %d, %s, %d iters, up err %7.7f, trace %7.7f)", + result.orignorm, result.bestnorm, sensor_error, (int)mpfitctx.measurementsCnt, res, + survive_optimizer_error(res), result.niter, + mpfitctx.stats.object_up_error / mpfitctx.stats.object_up_error_cnt, cn_trace(&R) / R.rows); SurvivePose *opt_cameras = survive_optimizer_get_camera(&mpfitctx); SurvivePose cameras[NUM_GEN2_LIGHTHOUSES] = {0}; @@ -1033,8 +1036,9 @@ bool solve_global_scene(struct SurviveContext *ctx, MPFITData *d, PoserDataGloba quatrotatevector(err_up, p->Rot, gss->scenes[i].accel); normalize3d(err_up, err_up); - SV_VERBOSE(10, "Solved scene with pose (%s) " SurvivePose_format " %5.4f", mpfitctx.sos[i]->codename, - SURVIVE_POSE_EXPAND(*p), fabs(err_up[2] - 1)); + SV_VERBOSE(10, "Solved scene with pose (%s) " SurvivePose_format " %5.4f " Point3_format, + mpfitctx.sos[i]->codename, SURVIVE_POSE_EXPAND(*p), fabs(err_up[2] - 1), + LINMATH_VEC3_EXPAND(err_up)); if (!quatiszero(p->Rot)) { survive_recording_write_to_output(ctx->recptr, "SPHERE %s_%d %f %d " Point3_format "\n", diff --git a/src/survive_optimizer.c b/src/survive_optimizer.c index 8803f2b..e5e5b58 100644 --- a/src/survive_optimizer.c +++ b/src/survive_optimizer.c @@ -929,7 +929,7 @@ static int mpfunc(int m, int n, FLT *p, FLT *deviates, FLT **derivs, void *priva } } - mpfunc_ctx->stats.object_up_error += deviates[meas_idx]; + mpfunc_ctx->stats.object_up_error += deviates[meas_idx] * deviates[meas_idx]; mpfunc_ctx->stats.object_up_error_cnt ++; break; } diff --git a/src/survive_process.c b/src/survive_process.c index 2e3cd67..1d1bf10 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -110,7 +110,7 @@ static inline void calculate_external2world(SurviveContext *ctx) { } else { KabschPoses(&ctx->private_members->external2world, externalLH, bsds, num_poses); // Kabsch(&ctx->private_members->external2world, ptsExtLH.data, ptsWorldLH.data, num_pairs); - SV_VERBOSE(100, "external2world " SurvivePose_format, + SV_VERBOSE(110, "external2world " SurvivePose_format, SURVIVE_POSE_EXPAND(ctx->private_members->external2world)); survive_recording_write_to_output(ctx->recptr, "EXTERNAL_TO_WORLD " SurvivePose_format "\n", SURVIVE_POSE_EXPAND(ctx->private_members->external2world)); }