fixed issue with step counter

This commit is contained in:
harine
2026-09-07 17:37:11 +08:00
parent 5fe424316e
commit 41b567929a
3 changed files with 230 additions and 9 deletions
@@ -58,12 +58,16 @@ import kotlin.math.sqrt
*
* @param minSpacingMs Minimum time, in milliseconds, between two accepted peaks for the same foot.
* @param minProminenceRatio Minimum required peak prominence, as a fraction of torso scale.
* @param maxFrameJumpRatio Maximum single-frame ankle-y movement, as a
* fraction of torso scale, still trusted as real motion rather than
* a detection glitch -- see the outlier gate in [update].
* @param stillnessWindowMs How long, in milliseconds, hip position must stay put to count as a held stance.
* @param stillnessRatio Maximum hip position drift, as a fraction of torso scale, still considered "still".
*/
class LiveStepDetector(
private val minSpacingMs: Long = 300L,
private val minProminenceRatio: Float = 0.15f,
private val maxFrameJumpRatio: Float = 0.25f,
private val stillnessWindowMs: Long = 600L,
private val stillnessRatio: Float = 0.05f
) {
@@ -102,6 +106,15 @@ class LiveStepDetector(
private var lastRightAnkleRaw: LandmarkPoint? = null
private var lastHipMid: Pair<Float, Float>? = null
// Last raw ankle-y actually fed to the peak trackers, per foot --
// distinct from lastLeftAnkleRaw/lastRightAnkleRaw above, which record
// *every* frame's reading (glitched or not) so the stall check keeps
// working. These only advance past a sample that clears the outlier
// gate in [update], so one glitched frame can't drag the reference
// point away from real motion and mask the next frame's genuine jump.
private var lastGoodLeftAnkleY: Float? = null
private var lastGoodRightAnkleY: Float? = null
/**
* @brief Feeds one frame's pose data into the detector, updating step
* count/stillness state and returning what happened this call.
@@ -147,16 +160,35 @@ class LiveStepDetector(
// busy) risks flattening the peak we're trying to detect into
// nothing. The heavier smoothing is still right for PoseFrame's
// stored/displayed values -- just not for finding the peak itself.
//
// Each reading is first checked against isPlausibleJump: confirmed
// against a real device trace where torso scale was small (~45-79px,
// a distant/small subject) and single-frame ankle-y jumps of
// 15-88px showed up dozens of times -- physically implausible
// movement in a single ~30-60ms analysis frame at that scale (the
// same trace's genuine footfalls only ever moved ~10-12px total
// across several frames). Those jumps are momentary landmark
// detection glitches, not real motion, and fed the live counter to
// 27 "steps" in 24 seconds. A glitched sample is skipped entirely
// rather than reset anything -- lastGoodLeftAnkleY/lastGoodRightAnkleY
// only advance past a trusted reading, so the next frame is still
// compared against real motion instead of the glitch.
frame.leftAnkleRaw?.let { ankle ->
leftFoot.update(frame.timestampMs, ankle.y, scale)?.let { confirmedAtMs ->
stepCount++
newSteps.add(StepEvent(confirmedAtMs, Foot.LEFT, stepCount))
if (isPlausibleJump(lastGoodLeftAnkleY, ankle.y, scale)) {
lastGoodLeftAnkleY = ankle.y
leftFoot.update(frame.timestampMs, ankle.y, scale)?.let { confirmedAtMs ->
stepCount++
newSteps.add(StepEvent(confirmedAtMs, Foot.LEFT, stepCount))
}
}
}
frame.rightAnkleRaw?.let { ankle ->
rightFoot.update(frame.timestampMs, ankle.y, scale)?.let { confirmedAtMs ->
stepCount++
newSteps.add(StepEvent(confirmedAtMs, Foot.RIGHT, stepCount))
if (isPlausibleJump(lastGoodRightAnkleY, ankle.y, scale)) {
lastGoodRightAnkleY = ankle.y
rightFoot.update(frame.timestampMs, ankle.y, scale)?.let { confirmedAtMs ->
stepCount++
newSteps.add(StepEvent(confirmedAtMs, Foot.RIGHT, stepCount))
}
}
}
@@ -194,6 +226,25 @@ class LiveStepDetector(
lastLeftAnkleRaw = null
lastRightAnkleRaw = null
lastHipMid = null
lastGoodLeftAnkleY = null
lastGoodRightAnkleY = null
}
/**
* @brief Whether a new ankle-y reading is plausible real motion given
* the last trusted reading for that same foot, rather than a
* one-frame detection glitch.
* @param lastGoodY The last reading that itself passed this check, or
* null if none yet established (nothing to compare against).
* @param newY This frame's raw ankle-y reading.
* @param scale Current best-known torso length in pixels, or null if
* none established yet (nothing to scale the check by).
* @return true if there's no reference to compare against yet, or the
* movement is within [maxFrameJumpRatio] of torso scale.
*/
private fun isPlausibleJump(lastGoodY: Float?, newY: Float, scale: Float?): Boolean {
if (lastGoodY == null || scale == null || scale <= 0f) return true
return kotlin.math.abs(newY - lastGoodY) <= scale * maxFrameJumpRatio
}
/**
@@ -259,6 +310,14 @@ private enum class TrackingMode { SEEKING_PEAK, SEEKING_VALLEY }
* about it looks like a drop until the foot actually lifts again) while
* still rejecting pure jitter that never clears the threshold either way.
*
* Single-frame detection glitches (a momentary implausible ankle-y jump)
* are filtered out *before* they ever reach this tracker -- see the
* isPlausibleJump gate in [LiveStepDetector.update] -- rather than handled
* here, since a real footfall's prominence (confirmed against a real
* device trace: as little as ~10-12px at that recording's torso scale) can
* be smaller than a single glitched frame's jump, so no prominence
* threshold on its own can tell the two apart.
*
* @param minSpacingMs Minimum time, in milliseconds, between two accepted peaks.
* @param minProminenceRatio Minimum required peak prominence, as a fraction of torso scale.
*/