From a9d9441a2490091f723b3385204c98b917662609 Mon Sep 17 00:00:00 2001 From: Mark Stalzer Date: Fri, 13 Mar 2026 07:52:33 -0400 Subject: [PATCH] poser_mpfit: skip non-finite optical angles instead of passing to solver Corrupt optical angles (bad FPGA timestamps during USB disturbances) can reach construct_input_from_scene() as NaN or Inf. Passing a non-finite value into the MPFIT solver causes it to produce a garbage pose which then assert-crashes downstream (observed as assert(!isnan(lhs[lh].Rot[0]))). Add an isfinite() check before emplacing each measurement. Non-finite angles are skipped with a one-time stderr warning. One dropped measurement has negligible effect on the pose solve; crashing does not. Co-Authored-By: Claude Sonnet 4.6 --- src/poser_mpfit.c | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index 4ce6e014..13a9477d 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -187,6 +187,21 @@ static size_t construct_input_from_scene(const MPFITData *d, survive_long_timeco if (isReadingValue) { const FLT *a = scene->angles[sensor][lh]; + /* Guard against non-finite angles from corrupted optical data + * (bad FPGA timestamps during USB disturbances). Passing NaN + * into the MPFIT solver causes it to produce a garbage pose + * that can then assert-crash downstream. One dropped measurement + * has negligible effect on the pose solve. */ + if (!isfinite(a[axis])) { + static int warned = 0; + if (!warned) { + fprintf(stderr, "[libsurvive WARN] poser_mpfit: NaN optical angle sensor %d lh %d axis %d; suppressing further\n", + (int)sensor, (int)lh, (int)axis); + warned = 1; + } + continue; + } + survive_optimizer_measurement *meas = survive_optimizer_emplace_meas(mpfitctx, survive_optimizer_measurement_type_light);