refactor: 데모 브랜치 패키지 통일 com.example.medilightv2android → com.medithings.vesiscan
사용자앱 (feature/tab-navigation) 과 패키지 이름 일치. namespace + applicationId 둘 다 com.medithings.vesiscan 로 변경 (기존 데모 앱은 재설치 필요). ## 변경 범위 - Kotlin 148 파일: package + import 문 (714 occurrences) - 디렉토리 이동: com/example/medilightv2android → com/medithings/vesiscan (main, test, androidTest 각각) - app/build.gradle.kts: namespace, applicationId - docs/FLAVOR_DEMO_STABLE.md: 참조 갱신 - V41DetectorCH4Test: BvDispatchResult.methodChosen → method (dto field name fix) ## 주의 - applicationId 가 바뀌므로 기존 데모 앱 (com.example.medilightv2android.demo) 은 Android 관점에서 다른 앱으로 취급 — 재설치 시 PIN/설정 초기화됨. - Fresh install 권장. 기존 앱 (com.example...) 은 별도로 uninstall 필요. Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
@@ -0,0 +1,614 @@
|
||||
/*
|
||||
* Alignment Advisor V2 — port of vesiscan_test/alignment.py.
|
||||
*
|
||||
* 새 알고리즘 (한 팀장 / 대표님 회의 결정사항):
|
||||
* ─ method_d 기반 urine_len 으로 정렬 판단
|
||||
* ─ 2-phase: V (CH3 검출 → 머리방향) → L (CH4/CH5 urine_len 균형)
|
||||
* ─ |u4 − u5| ≤ LAT_TOL(8 samples ≈ 16mm) 이면 STOP
|
||||
* ─ ulen 큰 쪽으로 이동 (= 큰 쪽이 방광 중심)
|
||||
* ─ 둘 다 미검출 → 좌우 PROBE 모드 (hill-climbing 으로 최대점 추적)
|
||||
*
|
||||
* 입력은 raw 6채널 (각 길이 100, UShort/Int 어느 쪽이든 toDouble() 가능).
|
||||
* RollingAligner 가 N=accumK(default 10) 프레임 평균 후 한 번씩 commit.
|
||||
*
|
||||
* UI 측에서는 `push(raw6)` 호출 → AdvisorState.action 확인 → directionIcon/Hint
|
||||
* 매핑. 위치 이동 직후엔 `reset()` 으로 누적 버퍼 비우기 권장.
|
||||
*
|
||||
* 기존 PlacementGuide computePlacementGuide(...) 와 병행 — V1/V2 토글로 분기.
|
||||
*/
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import com.medithings.vesiscan.R
|
||||
import com.medithings.vesiscan.walldetect.MethodDResult
|
||||
import com.medithings.vesiscan.walldetect.MethodDRunner
|
||||
import com.medithings.vesiscan.walldetect.algo.methodd.MethodDParams
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.max
|
||||
|
||||
/** action 상수. MOVE_DOWN 은 4-stage guide 의 set 회복 (잃은 center channel 다시 찾기) 용. */
|
||||
enum class AlignAction { MOVE_UP, MOVE_DOWN, MOVE_LEFT, MOVE_RIGHT, PROBE_LR, STOP }
|
||||
|
||||
/** RollingAligner.push() 결과. */
|
||||
data class AdvisorState(
|
||||
val state: String, // 'accum' 또는 'commit'
|
||||
val action: AlignAction?,
|
||||
val msg: String,
|
||||
/** Localized message string resource id (null if no localized message). */
|
||||
val msgKey: Int? = null,
|
||||
/** Args for msgKey (formatted into string). */
|
||||
val msgArgs: List<Any> = emptyList(),
|
||||
val dets: List<MethodDResult?>?,
|
||||
val summary: AlignSummary?,
|
||||
val accumProgress: Int = 0, // 'accum' 일 때 (현재/총)
|
||||
val accumTarget: Int = 0,
|
||||
)
|
||||
|
||||
data class AlignSummary(
|
||||
val ch3: Boolean,
|
||||
val u4: Int?,
|
||||
val u5: Int?,
|
||||
val chN: Int,
|
||||
/** Per-channel detection (size 6). center=[0,1,2,3], lateral=[4,5]. */
|
||||
val detected: BooleanArray = BooleanArray(6),
|
||||
/** 2026-07-02: 각 채널 chord (angle-corrected mm). CENTER_OPTIMIZE CV 계산용.
|
||||
* chord[ch] = (post - ant) × cos(angle) × distancePerSample. null 이면 미검출. */
|
||||
val chords: Array<Double?> = Array(6) { null },
|
||||
)
|
||||
|
||||
object AlignmentConstants {
|
||||
const val CH3 = 3 // 치골쪽 최하단 center 채널
|
||||
const val LAT_TOL = 8 // 좌우 균형 허용 |u4-u5| (samples, ≈16mm)
|
||||
val CENTER_CH = intArrayOf(0, 1, 2, 3)
|
||||
val LATERAL_CH = intArrayOf(4, 5)
|
||||
|
||||
// 02bed02 (4494098 VBTWD201): cross-channel ant tie-break — config_6ch.py 매칭.
|
||||
/** Cross-channel 전벽 tie-break: top1/top2 score 비율 < TIEBREAK_RATIO 이면 동률 → 합의 z 로 선택.
|
||||
* 1.3 = 명확한 동률(top1/top2 1.1~1.2)만 잡고 압승(>=1.5)은 측정값 보존. */
|
||||
const val ANT_TIEBREAK_RATIO = 1.3
|
||||
/** Near-field 가드: ant 가 이웃 합의 z 보다 이 값 이상 "얕은" (probe 쪽) 경우 entry 복합체
|
||||
* 아티팩트로 보고 후보 교체. 깊은(urine 쪽) 이탈은 dome 곡률로 정상이라 허용. 8mm ≈ 4 sample. */
|
||||
const val ANT_NEARFIELD_TOL_MM = 8.0
|
||||
|
||||
// 2026-07-02: CENTER_OPTIMIZE phase — CH0~CH3 chord CV 로 최적 vertical 위치 판정.
|
||||
/** CV 통과 임계 — V1 (computePlacementGuide) GREEN 조건 실증값 참조. */
|
||||
const val CV_THRESHOLD_STRICT = 0.15
|
||||
/** 방향 힌트 활성 임계 — CV 이 이상이면 사용자에게 up/down 힌트. */
|
||||
const val CV_THRESHOLD_LOOSE = 0.20
|
||||
/** CENTER_OPTIMIZE 통과 요구 연속 commit 수 (hysteresis). */
|
||||
const val CENTER_OPT_CONFIRM_STREAK = 3
|
||||
}
|
||||
|
||||
/** stateless 단일 스냅샷 advisor — alignment.py:alignment_advice(). */
|
||||
object AlignmentAdvice {
|
||||
fun advise(ch3: Boolean, u4: Int?, u5: Int?, latTol: Int = AlignmentConstants.LAT_TOL): Pair<AlignAction, String> {
|
||||
if (!ch3) return Pair(AlignAction.MOVE_UP, "↑ 머리방향으로 이동 (ch3 미검출)")
|
||||
if (u4 != null && u5 != null) {
|
||||
val d = abs(u4 - u5)
|
||||
if (d <= latTol) return Pair(AlignAction.STOP, "■ 정렬 완료 — 정지 (|Δ|=$d)")
|
||||
return if (u4 < u5)
|
||||
Pair(AlignAction.MOVE_RIGHT, "→ 우측으로 이동 (ch4 $u4 < ch5 $u5, |Δ|=$d)")
|
||||
else
|
||||
Pair(AlignAction.MOVE_LEFT, "← 좌측으로 이동 (ch5 $u5 < ch4 $u4, |Δ|=$d)")
|
||||
}
|
||||
if (u4 == null && u5 == null)
|
||||
return Pair(AlignAction.PROBE_LR, "↔ 좌우로 천천히 이동해 lateral 탐색 (ch4·ch5 미검출)")
|
||||
return if (u4 == null) Pair(AlignAction.MOVE_RIGHT, "→ 우측으로 이동 (ch4 미검출)")
|
||||
else Pair(AlignAction.MOVE_LEFT, "← 좌측으로 이동 (ch5 미검출)")
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 2-phase stateful guide — Python `AlignGuide`.
|
||||
* (RollingAligner 는 단일 commit 마다 stateless advice 만 사용하지만, 향후
|
||||
* imbal hill-climbing 모드에서 쓸 수 있도록 같이 포팅.)
|
||||
*/
|
||||
class AlignGuide(private val latTol: Int = AlignmentConstants.LAT_TOL) {
|
||||
private enum class Phase { V, L }
|
||||
private var phase = Phase.V
|
||||
private var imbal: Int? = null
|
||||
private var u: Int? = null
|
||||
private var dir: AlignAction? = null
|
||||
private var triedBoth = false
|
||||
|
||||
fun reset() {
|
||||
phase = Phase.V
|
||||
imbal = null; u = null; dir = null; triedBoth = false
|
||||
}
|
||||
|
||||
fun step(summary: AlignSummary): Pair<AlignAction, String> {
|
||||
if (phase == Phase.V) {
|
||||
if (!summary.ch3) return Pair(AlignAction.MOVE_UP, "ch3 미검출 — 머리방향 이동")
|
||||
phase = Phase.L
|
||||
}
|
||||
return lateral(summary)
|
||||
}
|
||||
|
||||
private fun lateral(s: AlignSummary): Pair<AlignAction, String> {
|
||||
val u4 = s.u4
|
||||
val u5 = s.u5
|
||||
if (u4 != null && u5 != null) {
|
||||
val curImbal = abs(u4 - u5)
|
||||
if (curImbal <= latTol) return Pair(AlignAction.STOP, "좌우 균형 |Δulen|=$curImbal<=$latTol")
|
||||
if (dir == null) {
|
||||
dir = if (u4 < u5) AlignAction.MOVE_RIGHT else AlignAction.MOVE_LEFT
|
||||
} else if (imbal != null && curImbal > imbal!!) {
|
||||
// 악화 → 반대로
|
||||
dir = if (dir == AlignAction.MOVE_LEFT) AlignAction.MOVE_RIGHT else AlignAction.MOVE_LEFT
|
||||
}
|
||||
imbal = curImbal
|
||||
return Pair(dir!!, "불균형 |Δulen|=$curImbal 감소 방향")
|
||||
}
|
||||
// 미검출 (probe 모드)
|
||||
val cur = max(u4 ?: 0, u5 ?: 0)
|
||||
if (dir == null) {
|
||||
dir = AlignAction.MOVE_LEFT
|
||||
u = cur
|
||||
return Pair(AlignAction.MOVE_LEFT, "lateral 미검출 — 좌측 probe")
|
||||
}
|
||||
if (cur > (u ?: 0)) {
|
||||
u = cur
|
||||
return Pair(dir!!, "ulen 증가($cur) — ${dir!!.name} 계속")
|
||||
}
|
||||
if (!triedBoth && dir == AlignAction.MOVE_LEFT) {
|
||||
dir = AlignAction.MOVE_RIGHT
|
||||
triedBoth = true
|
||||
u = cur
|
||||
return Pair(AlignAction.MOVE_RIGHT, "좌측 미개선 — 우측 probe")
|
||||
}
|
||||
return Pair(AlignAction.STOP, "ulen 최대(${u ?: 0}) 위치 정지")
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* 4-stage stateful guide — 사용자 설계 (2026-06-16).
|
||||
*
|
||||
* 단계:
|
||||
* 1. INITIAL_ACCUM — 첫 10 cycle warm-up, 안내 X (msg 만)
|
||||
* 2. VERTICAL_CLIMB — ch3 검출까지 ↑
|
||||
* 3. CH3_STABILIZE — ch3 검출 후 10 cycle 또 누적 (자세 확정)
|
||||
* 4. LR_BALANCE — current center set 유지하며 좌우 균형
|
||||
* set 잃으면 회복 (lost ch3 → ↑, else → ↓)
|
||||
*
|
||||
* Stage 4 의 bestCenterSet 은 ever-best — set 이 한 번 ch3+ch2+ch1 까지 갔다가
|
||||
* ch1 사라져도, 목표는 여전히 ch3+ch2+ch1. 회복 후 좌우 계속.
|
||||
*/
|
||||
class AlignGuide4Stage(
|
||||
private val latTol: Int = AlignmentConstants.LAT_TOL,
|
||||
private val initialTarget: Int = 10,
|
||||
private val stabilizeTarget: Int = 10,
|
||||
/** VERTICAL_CLIMB 에서 CH3_STABILIZE 로 넘어가기 위한 ch3 연속 commit 수. */
|
||||
private val verticalHitRequired: Int = 3,
|
||||
/** LR_BALANCE 반전 deadband (samples). */
|
||||
private val lrReverseDeadband: Int = 3,
|
||||
/** LR_BALANCE 반전 요구 연속 commit 수. */
|
||||
private val lrReverseConsecutive: Int = 2,
|
||||
/**
|
||||
* LR_BALANCE 의 `lost` 감지 hysteresis — N 연속 commit 에서 같은 채널이
|
||||
* 빠져있어야 진짜 "회복 필요" 로 인정. 1 cycle noise 깜빡임에 화살표
|
||||
* 흔들리는 현상 차단 (2026-06-30 실측 분석 결과).
|
||||
*/
|
||||
private val lostStreakRequired: Int = 3,
|
||||
/** FINAL_CONFIRM 누적 프레임 수 — flowchart 의 \"마지막 측정 5회 반복\". */
|
||||
val finalConfirmTarget: Int = 5,
|
||||
) {
|
||||
// 2026-07-02: CENTER_OPTIMIZE phase 추가 — CH0~CH3 chord CV 로 최적 vertical 위치 판정.
|
||||
enum class Phase4 {
|
||||
INITIAL_ACCUM, VERTICAL_CLIMB, CH3_STABILIZE,
|
||||
CENTER_OPTIMIZE, // ← 신규
|
||||
LR_BALANCE, FINAL_CONFIRM
|
||||
}
|
||||
|
||||
data class Out(
|
||||
val action: AlignAction?,
|
||||
val msg: String,
|
||||
val phase: Phase4,
|
||||
val progress: Int = 0,
|
||||
/** Localized string resource id for user-facing UI (msg is dev/debug). */
|
||||
val msgKey: Int? = null,
|
||||
/** Args for msgKey formatting. */
|
||||
val msgArgs: List<Any> = emptyList(),
|
||||
)
|
||||
|
||||
var phase: Phase4 = Phase4.INITIAL_ACCUM
|
||||
private set
|
||||
var bestCenterSet: Set<Int> = emptySet()
|
||||
private set
|
||||
private var initialCount = 0
|
||||
private var verticalHitStreak = 0
|
||||
private var stabilizeCount = 0
|
||||
private var lateralDir: AlignAction? = null
|
||||
private var lateralImbal: Int? = null
|
||||
private var lateralWorseStreak = 0
|
||||
private var finalConfirmCount = 0 // FINAL_CONFIRM 누적 카운터
|
||||
private var lostStreak = 0 // LR_BALANCE 채널 lost hysteresis
|
||||
private var centerOptimizeStreak = 0 // CENTER_OPTIMIZE CV 통과 연속 카운터
|
||||
|
||||
fun reset() {
|
||||
phase = Phase4.INITIAL_ACCUM
|
||||
bestCenterSet = emptySet()
|
||||
initialCount = 0
|
||||
verticalHitStreak = 0
|
||||
stabilizeCount = 0
|
||||
lateralDir = null
|
||||
lateralImbal = null
|
||||
lateralWorseStreak = 0
|
||||
finalConfirmCount = 0
|
||||
lostStreak = 0
|
||||
centerOptimizeStreak = 0
|
||||
}
|
||||
|
||||
fun step(s: AlignSummary): Out {
|
||||
val curSet = (0..3).filter { it < s.detected.size && s.detected[it] }.toSet()
|
||||
|
||||
// === Phase 1: INITIAL_ACCUM ===
|
||||
if (phase == Phase4.INITIAL_ACCUM) {
|
||||
initialCount++
|
||||
if (initialCount < initialTarget) {
|
||||
return Out(null, "측정 준비 중... (${initialCount}/${initialTarget})",
|
||||
phase, initialCount,
|
||||
msgKey = R.string.align_hold_still,
|
||||
msgArgs = listOf(initialCount, initialTarget))
|
||||
}
|
||||
phase = Phase4.VERTICAL_CLIMB
|
||||
}
|
||||
|
||||
// === Phase 2: VERTICAL_CLIMB ===
|
||||
if (phase == Phase4.VERTICAL_CLIMB) {
|
||||
if (!s.ch3) {
|
||||
verticalHitStreak = 0 // 연속 카운터 리셋 — false detection 거부
|
||||
return Out(AlignAction.MOVE_UP, "↑ 위로 조금씩 올리세요 (ch3 미검출)",
|
||||
phase, 0,
|
||||
msgKey = R.string.align_move_up)
|
||||
}
|
||||
verticalHitStreak++
|
||||
if (verticalHitStreak < verticalHitRequired) {
|
||||
// ch3 일시 검출 — verticalHitRequired 회 연속 확인 전까지 사용자 정지.
|
||||
// (현재 위치가 진짜 ch3 인지 평균이 만든 false echo 인지 확인 중)
|
||||
return Out(AlignAction.STOP,
|
||||
"✓ ch3 일시 검출 — 확인 중 (${verticalHitStreak}/${verticalHitRequired})",
|
||||
phase, verticalHitStreak,
|
||||
msgKey = R.string.align_hold_position,
|
||||
msgArgs = listOf(verticalHitStreak, verticalHitRequired))
|
||||
}
|
||||
phase = Phase4.CH3_STABILIZE
|
||||
stabilizeCount = 0
|
||||
bestCenterSet = curSet
|
||||
}
|
||||
|
||||
// === Phase 3: CH3_STABILIZE ===
|
||||
if (phase == Phase4.CH3_STABILIZE) {
|
||||
stabilizeCount++
|
||||
if (curSet.size > bestCenterSet.size) bestCenterSet = curSet
|
||||
if (!s.ch3) {
|
||||
// ch3 잃었으면 → VERTICAL_CLIMB 회귀
|
||||
phase = Phase4.VERTICAL_CLIMB
|
||||
stabilizeCount = 0
|
||||
return Out(AlignAction.MOVE_UP, "↑ ch3 다시 잃음 — 위로",
|
||||
phase, 0,
|
||||
msgKey = R.string.align_recover_up)
|
||||
}
|
||||
if (stabilizeCount < stabilizeTarget) {
|
||||
return Out(AlignAction.STOP,
|
||||
"✓ ch3 검출! 위치 유지 (${stabilizeCount}/${stabilizeTarget})",
|
||||
phase, stabilizeCount,
|
||||
msgKey = R.string.align_hold_position,
|
||||
msgArgs = listOf(stabilizeCount, stabilizeTarget))
|
||||
}
|
||||
// 2026-07-02: CH3_STABILIZE 통과 → CENTER_OPTIMIZE (CV 로 4채널 균등 확인)
|
||||
phase = Phase4.CENTER_OPTIMIZE
|
||||
centerOptimizeStreak = 0
|
||||
}
|
||||
|
||||
// === Phase 4a: CENTER_OPTIMIZE (신규) — CH0~CH3 chord CV 로 최적 vertical 정밀 조정 ===
|
||||
// 전제: CH3 는 이미 안정 검출됨. 4채널 chord CV 가 낮으면 (0.15 이하) 정렬 완료로 인정.
|
||||
// CV 높으면: chord[3] > chord[0] 이면 probe 가 high → MOVE_DOWN,
|
||||
// chord[0] > chord[3] 이면 probe 가 low → MOVE_UP.
|
||||
if (phase == Phase4.CENTER_OPTIMIZE) {
|
||||
val center4Detected = (0..3).all { it < s.detected.size && s.detected[it] }
|
||||
if (!s.ch3) {
|
||||
// CH3 잃으면 CH3_STABILIZE 로 복귀
|
||||
phase = Phase4.CH3_STABILIZE
|
||||
stabilizeCount = 0
|
||||
centerOptimizeStreak = 0
|
||||
return Out(AlignAction.MOVE_UP, "↑ ch3 재확인 필요",
|
||||
phase, 0, msgKey = R.string.align_recover_up)
|
||||
}
|
||||
if (!center4Detected) {
|
||||
// CH0~2 중 일부 미검출 → probe 를 위로 올려 CH0 잡기
|
||||
centerOptimizeStreak = 0
|
||||
val missing = (0..3).filter { !s.detected[it] }
|
||||
val missingLabel = missing.joinToString(",") { "ch$it" }
|
||||
return Out(AlignAction.MOVE_UP,
|
||||
"↑ 조금 더 위로 ($missingLabel 미검출)", phase, 0,
|
||||
msgKey = R.string.align_move_up)
|
||||
}
|
||||
// 4채널 모두 검출됨 → chord CV 계산 (summarize 에서 angle-corrected mm 값 저장됨)
|
||||
val chords = DoubleArray(4) { ch -> s.chords[ch] ?: 0.0 }
|
||||
val chordMean = chords.average()
|
||||
if (chordMean <= 0.1) {
|
||||
phase = Phase4.VERTICAL_CLIMB
|
||||
verticalHitStreak = 0
|
||||
centerOptimizeStreak = 0
|
||||
return Out(AlignAction.MOVE_UP, "↑ 재정렬 필요", phase, 0,
|
||||
msgKey = R.string.align_move_up)
|
||||
}
|
||||
val variance = chords.map { (it - chordMean) * (it - chordMean) }.average()
|
||||
val chordStd = kotlin.math.sqrt(variance)
|
||||
val cv = chordStd / chordMean
|
||||
|
||||
if (cv <= AlignmentConstants.CV_THRESHOLD_STRICT) {
|
||||
centerOptimizeStreak++
|
||||
if (centerOptimizeStreak >= AlignmentConstants.CENTER_OPT_CONFIRM_STREAK) {
|
||||
bestCenterSet = (0..3).toSet()
|
||||
phase = Phase4.LR_BALANCE
|
||||
return Out(AlignAction.STOP,
|
||||
"✓ 최적 vertical 확인 (CV=${"%.2f".format(cv)}) — 좌우 균형 확인",
|
||||
phase, 0,
|
||||
msgKey = R.string.align_hold_position,
|
||||
msgArgs = listOf(centerOptimizeStreak, AlignmentConstants.CENTER_OPT_CONFIRM_STREAK))
|
||||
}
|
||||
return Out(AlignAction.STOP,
|
||||
"✓ 최적 근접 (CV=${"%.2f".format(cv)}) — 확인 중 " +
|
||||
"(${centerOptimizeStreak}/${AlignmentConstants.CENTER_OPT_CONFIRM_STREAK})",
|
||||
phase, centerOptimizeStreak,
|
||||
msgKey = R.string.align_hold_position,
|
||||
msgArgs = listOf(centerOptimizeStreak, AlignmentConstants.CENTER_OPT_CONFIRM_STREAK))
|
||||
}
|
||||
// CV 너무 큼 → 방향 힌트
|
||||
centerOptimizeStreak = 0
|
||||
val ch0Chord = chords[0]
|
||||
val ch3Chord = chords[3]
|
||||
return if (ch3Chord > ch0Chord) {
|
||||
Out(AlignAction.MOVE_DOWN,
|
||||
"↓ 조금 아래로 (CV=${"%.2f".format(cv)}, CH3 극대)", phase, 0,
|
||||
msgKey = R.string.align_recover_down_short)
|
||||
} else {
|
||||
Out(AlignAction.MOVE_UP,
|
||||
"↑ 조금 위로 (CV=${"%.2f".format(cv)}, CH0 극대)", phase, 0,
|
||||
msgKey = R.string.align_recover_up_short)
|
||||
}
|
||||
}
|
||||
|
||||
// === Phase 4b (기존 4): LR_BALANCE ===
|
||||
if (phase == Phase4.LR_BALANCE) {
|
||||
if (curSet.size > bestCenterSet.size) bestCenterSet = curSet
|
||||
val setLabel = bestCenterSet.sortedDescending().joinToString("+") { "ch$it" }
|
||||
val lost = bestCenterSet - curSet
|
||||
if (lost.isNotEmpty()) {
|
||||
// 2026-06-30 hysteresis: N 연속 commit 에서 같은 채널이 lost 여야 진짜 회복 모드.
|
||||
// 1 cycle 노이즈 깜빡임 (실측 CH1=15%, CH3=12% flap) 에 화살표 흔들리는 현상 차단.
|
||||
lostStreak++
|
||||
if (lostStreak >= lostStreakRequired) {
|
||||
val recoveryDir = if (3 in lost) AlignAction.MOVE_UP else AlignAction.MOVE_DOWN
|
||||
val lostLabel = lost.sortedDescending().joinToString(",") { "ch$it" }
|
||||
val arrow = if (recoveryDir == AlignAction.MOVE_UP) "↑" else "↓"
|
||||
val recoveryKey = if (recoveryDir == AlignAction.MOVE_UP)
|
||||
R.string.align_recover_up_short else R.string.align_recover_down_short
|
||||
return Out(recoveryDir, "$arrow $lostLabel 잃음 — 위치 회복", phase, 0,
|
||||
msgKey = recoveryKey)
|
||||
}
|
||||
// streak 미달 → noise 로 보고 STOP 유지.
|
||||
return Out(AlignAction.STOP,
|
||||
"✓ 그대로 측정 가능 (noise check $lostStreak/$lostStreakRequired)", phase, 0,
|
||||
msgKey = R.string.align_complete)
|
||||
} else {
|
||||
lostStreak = 0 // full set 복귀 — streak 리셋
|
||||
}
|
||||
|
||||
// full set 유지 중 — 좌우 균형 (deadband + consecutive 악화 로 oscillation 차단)
|
||||
val u4 = s.u4
|
||||
val u5 = s.u5
|
||||
if (u4 != null && u5 != null) {
|
||||
val imbal = kotlin.math.abs(u4 - u5)
|
||||
if (imbal <= latTol) {
|
||||
// 균형 도달 → FINAL_CONFIRM 으로 전이. RollingAligner 가 buf clear → 5 fresh 프레임 누적.
|
||||
phase = Phase4.FINAL_CONFIRM
|
||||
finalConfirmCount = 0
|
||||
return Out(AlignAction.STOP,
|
||||
"✓ 균형 도달 — 최종 확인 중 (0/${finalConfirmTarget})", phase, 0,
|
||||
msgKey = R.string.align_hold_position,
|
||||
msgArgs = listOf(0, finalConfirmTarget))
|
||||
}
|
||||
if (lateralDir == null) {
|
||||
lateralDir = if (u4 < u5) AlignAction.MOVE_RIGHT else AlignAction.MOVE_LEFT
|
||||
lateralWorseStreak = 0
|
||||
} else if (lateralImbal != null) {
|
||||
val significantlyWorse = imbal > lateralImbal!! + lrReverseDeadband
|
||||
if (significantlyWorse) {
|
||||
lateralWorseStreak++
|
||||
if (lateralWorseStreak >= lrReverseConsecutive) {
|
||||
lateralDir = if (lateralDir == AlignAction.MOVE_LEFT)
|
||||
AlignAction.MOVE_RIGHT else AlignAction.MOVE_LEFT
|
||||
lateralWorseStreak = 0
|
||||
}
|
||||
} else {
|
||||
lateralWorseStreak = 0
|
||||
}
|
||||
}
|
||||
lateralImbal = imbal
|
||||
val arrow = if (lateralDir == AlignAction.MOVE_LEFT) "←" else "→"
|
||||
val sideKey = if (lateralDir == AlignAction.MOVE_LEFT)
|
||||
R.string.align_left else R.string.align_right
|
||||
return Out(lateralDir!!, "$arrow $setLabel 유지 + 좌우 균형 (|Δ|=$imbal)",
|
||||
phase, 0,
|
||||
msgKey = sideKey)
|
||||
}
|
||||
return Out(AlignAction.PROBE_LR, "↔ $setLabel 유지 + 좌우 천천히 (lateral 미검출)",
|
||||
phase, 0,
|
||||
msgKey = R.string.align_probe_lr)
|
||||
}
|
||||
|
||||
// === Phase 5: FINAL_CONFIRM ===
|
||||
// RollingAligner 가 진입 시 buf 를 clear → 5 fresh 프레임 누적 후 imbal 재평가.
|
||||
if (phase == Phase4.FINAL_CONFIRM) {
|
||||
// center set 유지 확인 — 잃었으면 LR_BALANCE 회귀.
|
||||
val lost = bestCenterSet - curSet
|
||||
if (lost.isNotEmpty()) {
|
||||
val recoveryDir = if (3 in lost) AlignAction.MOVE_UP else AlignAction.MOVE_DOWN
|
||||
val arrow = if (recoveryDir == AlignAction.MOVE_UP) "↑" else "↓"
|
||||
phase = Phase4.LR_BALANCE
|
||||
finalConfirmCount = 0
|
||||
val recoveryKey = if (recoveryDir == AlignAction.MOVE_UP)
|
||||
R.string.align_recover_up_short else R.string.align_recover_down_short
|
||||
return Out(recoveryDir, "$arrow 채널 잃음 — 재정렬", phase, 0,
|
||||
msgKey = recoveryKey)
|
||||
}
|
||||
|
||||
finalConfirmCount++
|
||||
|
||||
// 누적 중 — 5 프레임 모일 때까지 정지 안내
|
||||
if (finalConfirmCount < finalConfirmTarget) {
|
||||
return Out(AlignAction.STOP,
|
||||
"✓ 최종 확인 중 (${finalConfirmCount}/${finalConfirmTarget})",
|
||||
phase, finalConfirmCount,
|
||||
msgKey = R.string.align_hold_position,
|
||||
msgArgs = listOf(finalConfirmCount, finalConfirmTarget))
|
||||
}
|
||||
|
||||
// 5 프레임 도달 — imbal 재평가
|
||||
val u4 = s.u4; val u5 = s.u5
|
||||
if (u4 == null || u5 == null) {
|
||||
phase = Phase4.LR_BALANCE
|
||||
finalConfirmCount = 0
|
||||
return Out(AlignAction.PROBE_LR, "lateral 미검출 — 좌우 재확인", phase, 0,
|
||||
msgKey = R.string.align_probe_lr)
|
||||
}
|
||||
val imbal = kotlin.math.abs(u4 - u5)
|
||||
val setLabel = bestCenterSet.sortedDescending().joinToString("+") { "ch$it" }
|
||||
return if (imbal <= latTol) {
|
||||
// 최종 확인 통과 → 진짜 STOP
|
||||
Out(AlignAction.STOP,
|
||||
"■ 정렬 완료 ($setLabel, 확인 OK, |Δ|=$imbal)", phase, finalConfirmTarget,
|
||||
msgKey = R.string.align_complete)
|
||||
} else {
|
||||
// 재평가 실패 → LR_BALANCE 회귀하여 추가 조정
|
||||
phase = Phase4.LR_BALANCE
|
||||
finalConfirmCount = 0
|
||||
val arrow = if (u4 < u5) "→" else "←"
|
||||
val sideKey = if (u4 < u5) R.string.align_right else R.string.align_left
|
||||
Out(if (u4 < u5) AlignAction.MOVE_RIGHT else AlignAction.MOVE_LEFT,
|
||||
"$arrow 재조정 필요 — 좌우 균형 (|Δ|=$imbal)", phase, 0,
|
||||
msgKey = sideKey)
|
||||
}
|
||||
}
|
||||
|
||||
// 도달 불가 (defensive)
|
||||
return Out(AlignAction.STOP, "unknown phase: $phase", phase, 0)
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* k-프레임 누적 + commit 한 번 — Python `RollingAligner`.
|
||||
*
|
||||
* push(raw6) 호출 시:
|
||||
* - buf.size < accumK 인 동안: state='accum'
|
||||
* - 충분히 모이면: 평균 → method_d → summarize → advice → state='commit'
|
||||
*
|
||||
* UI 는 commit 일 때만 화살표 갱신, accum 일 때는 "측정 중… N/K" 표시.
|
||||
*/
|
||||
class RollingAligner(
|
||||
private val latTol: Int = AlignmentConstants.LAT_TOL,
|
||||
/** INITIAL_ACCUM / VERTICAL_CLIMB / CH3_STABILIZE 단계 window 크기. flowchart 의 \"10회 측정 평균\". */
|
||||
private val accumKVertical: Int = 10,
|
||||
/** LR_BALANCE 단계 window 크기. flowchart 의 \"5회 측정 평균\" (빠른 반응). */
|
||||
private val accumKLateral: Int = 5,
|
||||
/** FINAL_CONFIRM 단계 window 크기. flowchart 의 \"마지막 5회 반복 측정\". */
|
||||
private val accumKConfirm: Int = 5,
|
||||
private val methodDParams: MethodDParams = MethodDParams.DEFAULT,
|
||||
) {
|
||||
private val buf = ArrayDeque<List<DoubleArray>>()
|
||||
private val guide4 = AlignGuide4Stage(latTol = latTol,
|
||||
initialTarget = accumKVertical, stabilizeTarget = accumKVertical,
|
||||
finalConfirmTarget = accumKConfirm)
|
||||
private var lastSeenPhase: AlignGuide4Stage.Phase4? = null
|
||||
/** 마지막 commit 결과. */
|
||||
var lastCommit: AdvisorState? = null
|
||||
private set
|
||||
val phase: AlignGuide4Stage.Phase4 get() = guide4.phase
|
||||
val bestCenterSet: Set<Int> get() = guide4.bestCenterSet
|
||||
|
||||
/** UI 그래프에서 N-cycle 평균 신호를 보여주기 위해 마지막 avg 노출. */
|
||||
var lastAvg: List<DoubleArray>? = null
|
||||
private set
|
||||
|
||||
fun reset() {
|
||||
buf.clear()
|
||||
guide4.reset()
|
||||
lastSeenPhase = null
|
||||
lastAvg = null
|
||||
}
|
||||
|
||||
/** 단일 채널 길이 100 가정. raw6.size == 6 이어야 함. */
|
||||
fun push(raw6: List<DoubleArray>): AdvisorState {
|
||||
// Phase 전이 감지 — FINAL_CONFIRM 진입 시 buf clear 로 fresh 5 프레임 누적 보장.
|
||||
// (flowchart \"마지막 측정 및 확인 — 5회 반복 측정\")
|
||||
val curPhase = guide4.phase
|
||||
if (lastSeenPhase != null && lastSeenPhase != AlignGuide4Stage.Phase4.FINAL_CONFIRM
|
||||
&& curPhase == AlignGuide4Stage.Phase4.FINAL_CONFIRM) {
|
||||
buf.clear()
|
||||
}
|
||||
|
||||
// Phase 별 동적 window 크기.
|
||||
val windowSize = when (curPhase) {
|
||||
AlignGuide4Stage.Phase4.LR_BALANCE -> accumKLateral
|
||||
AlignGuide4Stage.Phase4.FINAL_CONFIRM -> accumKConfirm
|
||||
else -> accumKVertical
|
||||
}
|
||||
|
||||
buf.addLast(raw6)
|
||||
while (buf.size > windowSize) buf.removeFirst()
|
||||
|
||||
val nCh = raw6.size
|
||||
val avg = List(nCh) { ch ->
|
||||
val len = buf.first()[ch].size
|
||||
DoubleArray(len) { i ->
|
||||
var sum = 0.0
|
||||
for (frame in buf) sum += frame[ch][i]
|
||||
sum / buf.size
|
||||
}
|
||||
}
|
||||
lastAvg = avg
|
||||
val dets = MethodDRunner.detectMultichannel(avg, methodDParams)
|
||||
val summary = summarize(dets)
|
||||
val out = guide4.step(summary)
|
||||
lastSeenPhase = guide4.phase
|
||||
|
||||
// accum state — INITIAL_ACCUM 의 첫 N 프레임 + FINAL_CONFIRM 누적 중.
|
||||
val isAccumulating = out.phase == AlignGuide4Stage.Phase4.INITIAL_ACCUM ||
|
||||
(out.phase == AlignGuide4Stage.Phase4.FINAL_CONFIRM &&
|
||||
out.progress in 1 until accumKConfirm)
|
||||
val state = if (isAccumulating) "accum" else "commit"
|
||||
val effectiveTarget = when (out.phase) {
|
||||
AlignGuide4Stage.Phase4.FINAL_CONFIRM -> accumKConfirm
|
||||
else -> accumKVertical
|
||||
}
|
||||
val st = AdvisorState(
|
||||
state = state, action = out.action, msg = out.msg,
|
||||
msgKey = out.msgKey, msgArgs = out.msgArgs,
|
||||
dets = dets, summary = summary,
|
||||
accumProgress = if (state == "accum") out.progress else effectiveTarget,
|
||||
accumTarget = effectiveTarget,
|
||||
)
|
||||
lastCommit = st
|
||||
return st
|
||||
}
|
||||
|
||||
private fun summarize(dets: List<MethodDResult?>): AlignSummary {
|
||||
fun u(i: Int) = if (i < dets.size) dets[i]?.urineLen else null
|
||||
val ch3 = dets.size > AlignmentConstants.CH3 && dets[AlignmentConstants.CH3] != null
|
||||
val chN = AlignmentConstants.CENTER_CH.count { it < dets.size && dets[it] != null }
|
||||
val detected = BooleanArray(6) { i -> i < dets.size && dets[i] != null }
|
||||
// 2026-07-02: per-channel chord (angle-corrected mm) — CENTER_OPTIMIZE CV 계산용
|
||||
val chords = Array<Double?>(6) { ch ->
|
||||
val d = if (ch < dets.size) dets[ch] else null
|
||||
if (d == null) null
|
||||
else {
|
||||
val urineLen = kotlin.math.max(0, d.post - d.ant - 1).toDouble()
|
||||
val angleRad = Math.toRadians(PiezoHW.degreeAll[ch])
|
||||
urineLen * kotlin.math.cos(angleRad) * PiezoHW.distancePerSample
|
||||
}
|
||||
}
|
||||
return AlignSummary(ch3 = ch3, u4 = u(4), u5 = u(5), chN = chN,
|
||||
detected = detected, chords = chords)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,268 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.max
|
||||
import kotlin.math.min
|
||||
import kotlin.math.roundToInt
|
||||
|
||||
/**
|
||||
* 후면반사 탐지+제거 시스템 — Python span_utils.py + pipeline.py 1:1 포팅
|
||||
*
|
||||
* 흐름: build_candidate → consensus → is_reflection_only → resolve → suppress → rerun → choose
|
||||
*/
|
||||
object BackReflection {
|
||||
|
||||
private const val MIN_BACK_REFLECTION_PROM = 50.0
|
||||
private const val POST_MATCH_TOL = 3
|
||||
private const val OUTLIER_TOL = 4
|
||||
private const val SHARED_TOL = 3
|
||||
private const val MIN_CHANNELS = 3
|
||||
private const val RADIUS = 6
|
||||
private const val EXTEND = 20
|
||||
|
||||
// ── Valley helpers (50.0 rise early-stop) ──
|
||||
|
||||
private fun findLeftValleyValue(x: DoubleArray, peakIdx: Int, maxDist: Int = 20): Double {
|
||||
var v = x[peakIdx]
|
||||
for (i in (peakIdx - 1) downTo max(0, peakIdx - maxDist)) {
|
||||
val cur = x[i]
|
||||
if (cur < v) v = cur
|
||||
else if (cur > v + 50.0) break
|
||||
}
|
||||
return v
|
||||
}
|
||||
|
||||
private fun findRightValleyValue(x: DoubleArray, peakIdx: Int, maxDist: Int = 20): Double {
|
||||
var v = x[peakIdx]
|
||||
for (i in (peakIdx + 1) until min(x.size, peakIdx + maxDist + 1)) {
|
||||
val cur = x[i]
|
||||
if (cur < v) v = cur
|
||||
else if (cur > v + 50.0) break
|
||||
}
|
||||
return v
|
||||
}
|
||||
|
||||
// ── Strongest significant peak ──
|
||||
|
||||
private fun strongestSignificantPeak(x: DoubleArray, lo: Int, hi: Int, minProm: Double = MIN_BACK_REFLECTION_PROM): Int? {
|
||||
if (lo >= hi || lo < 0 || hi > x.size) return null
|
||||
val seg: DoubleArray = x.sliceArray(lo until hi)
|
||||
val peaks: List<Int> = PiezoEchoAnalyzer.shared.findPeaks1D(seg)
|
||||
if (peaks.isEmpty()) return null
|
||||
|
||||
var bestIdx: Int? = null
|
||||
var bestAmp = Double.NEGATIVE_INFINITY
|
||||
for (p: Int in peaks) {
|
||||
val gp = p + lo
|
||||
val leftV = findLeftValleyValue(x, gp)
|
||||
val rightV = findRightValleyValue(x, gp)
|
||||
val prom = x[gp] - min(leftV, rightV)
|
||||
if (prom < minProm) continue
|
||||
if (x[gp] > bestAmp) { bestAmp = x[gp]; bestIdx = gp }
|
||||
}
|
||||
return bestIdx
|
||||
}
|
||||
|
||||
// ── find_back_reflection_idx (legacy, 전체 범위) ──
|
||||
|
||||
fun findBackReflectionIdx(tgcSg: DoubleArray, postMaxIdx: Int, minSearchIdx: Int = 40, extend: Int = EXTEND): Int {
|
||||
val n = tgcSg.size
|
||||
val searchEnd = min(n, postMaxIdx + extend)
|
||||
if (minSearchIdx >= searchEnd) return postMaxIdx
|
||||
val seg: DoubleArray = tgcSg.sliceArray(minSearchIdx until searchEnd)
|
||||
val peaks: List<Int> = PiezoEchoAnalyzer.shared.findPeaks1D(seg)
|
||||
if (peaks.isEmpty()) return postMaxIdx
|
||||
val globalPeaks: List<Int> = peaks.map { p -> p + minSearchIdx }
|
||||
return globalPeaks.maxByOrNull { idx -> tgcSg[idx] } ?: postMaxIdx
|
||||
}
|
||||
|
||||
// ── find_back_reflection_after_post ──
|
||||
|
||||
fun findBackReflectionAfterPost(tgcSg: DoubleArray, postIdx: Int, postMaxIdx: Int, extend: Int = EXTEND): Int? {
|
||||
val searchStart = postIdx + 1
|
||||
val searchEnd = min(tgcSg.size, postMaxIdx + extend)
|
||||
if (searchStart >= searchEnd) return null
|
||||
return strongestSignificantPeak(tgcSg, searchStart, searchEnd)
|
||||
}
|
||||
|
||||
// ── find_back_reflection_near_idx ──
|
||||
|
||||
fun findBackReflectionNearIdx(tgcSg: DoubleArray, centerIdx: Int, postIdx: Int, postMaxIdx: Int, radius: Int = RADIUS, extend: Int = EXTEND): Int? {
|
||||
val n = tgcSg.size
|
||||
val lo = max(postIdx + 1, centerIdx - radius)
|
||||
val hi = min(n, min(postMaxIdx + extend, centerIdx + radius + 1))
|
||||
if (lo >= hi) return null
|
||||
return strongestSignificantPeak(tgcSg, lo, hi)
|
||||
}
|
||||
|
||||
// ── build_back_reflection_candidate ──
|
||||
|
||||
fun buildCandidate(tgcSg: DoubleArray, postIdx: Int, postMaxIdx: Int, postMatchTol: Int = POST_MATCH_TOL): Int? {
|
||||
val globalBr = findBackReflectionIdx(tgcSg, postMaxIdx)
|
||||
if (abs(globalBr - postIdx) <= postMatchTol) return null // 후위벽≈전체BR → 별도 BR 없음
|
||||
return findBackReflectionAfterPost(tgcSg, postIdx, postMaxIdx)
|
||||
}
|
||||
|
||||
// ── consensus_back_reflection_idx ──
|
||||
|
||||
private const val MIN_VOTES = 3
|
||||
private const val AMP_RATIO = 1.5
|
||||
|
||||
fun consensusIdx(candidates: List<Int?>, outlierTol: Int = OUTLIER_TOL, minVotes: Int = MIN_VOTES): Int? {
|
||||
val vals = candidates.filterNotNull()
|
||||
if (vals.size < minVotes) return null
|
||||
val sorted = vals.sorted()
|
||||
val med = sorted[sorted.size / 2].toDouble()
|
||||
val inliers = vals.filter { abs(it - med) <= outlierTol }
|
||||
val finalVals = inliers.ifEmpty { vals }
|
||||
val finalSorted = finalVals.sorted()
|
||||
return finalSorted[finalSorted.size / 2]
|
||||
}
|
||||
|
||||
// ── is_suspicious_post_peak ──
|
||||
|
||||
fun isSuspiciousPostPeak(tgcSg: DoubleArray, postIdx: Int, antIdx: Int? = null, sharedIdx: Int? = null, sharedTol: Int = 2, ampRatio: Double = AMP_RATIO): Boolean {
|
||||
val n = tgcSg.size
|
||||
if (postIdx < 0 || postIdx >= n) return false
|
||||
var suspicious = false
|
||||
if (antIdx != null && antIdx in 0 until n && tgcSg[postIdx] > tgcSg[antIdx] * ampRatio) suspicious = true
|
||||
if (sharedIdx != null && abs(postIdx - sharedIdx) <= sharedTol) suspicious = true
|
||||
return suspicious
|
||||
}
|
||||
|
||||
// ── select_back_reflection_idx ──
|
||||
|
||||
fun selectIdx(tgcSg: DoubleArray, postIdx: Int, postMaxIdx: Int, antIdx: Int? = null, sharedIdx: Int? = null, channelCandidate: Int? = null): Int? {
|
||||
val suspiciousPost = isSuspiciousPostPeak(tgcSg, postIdx, antIdx = antIdx, sharedIdx = sharedIdx)
|
||||
|
||||
var local: Int? = null
|
||||
if (sharedIdx != null) {
|
||||
local = findBackReflectionNearIdx(tgcSg, sharedIdx, postIdx, postMaxIdx)
|
||||
}
|
||||
|
||||
if (suspiciousPost && sharedIdx != null) {
|
||||
val postDist = abs(postIdx - sharedIdx)
|
||||
val localDist = local?.let { abs(it - sharedIdx) }
|
||||
val candDist = channelCandidate?.let { abs(it - sharedIdx) }
|
||||
var nearestOther: Int? = localDist
|
||||
if (candDist != null) nearestOther = if (nearestOther == null) candDist else min(nearestOther, candDist)
|
||||
if (nearestOther == null || postDist < nearestOther) return postIdx
|
||||
}
|
||||
|
||||
if (local != null) return local
|
||||
if (channelCandidate != null) return channelCandidate
|
||||
if (suspiciousPost) return postIdx
|
||||
return null
|
||||
}
|
||||
|
||||
// ── resolve_back_reflection_indices ──
|
||||
|
||||
data class ResolveResult(val brIdx: Int?, val suppressIdx: Int?)
|
||||
|
||||
fun resolveIndices(tgcSg: DoubleArray, postIdx: Int, postMaxIdx: Int, antIdx: Int? = null, sharedIdx: Int? = null, channelCandidate: Int? = null): ResolveResult {
|
||||
val br = selectIdx(tgcSg, postIdx, postMaxIdx, antIdx = antIdx, sharedIdx = sharedIdx, channelCandidate = channelCandidate)
|
||||
?: return ResolveResult(null, null)
|
||||
val suppressIdx = sharedIdx ?: br
|
||||
return ResolveResult(br, suppressIdx)
|
||||
}
|
||||
|
||||
// ── is_reflection_only_multichannel ──
|
||||
|
||||
fun isReflectionOnlyMultichannel(
|
||||
tgcSignals: List<DoubleArray>,
|
||||
wallPairs: List<Pair<Int?, Int?>>,
|
||||
sharedIdx: Int? = null,
|
||||
sharedTol: Int = SHARED_TOL,
|
||||
minChannels: Int = MIN_CHANNELS,
|
||||
ampRatio: Double = AMP_RATIO
|
||||
): Boolean {
|
||||
data class Valid(val i: Int, val ant: Int, val post: Int)
|
||||
val valid = wallPairs.mapIndexedNotNull { i, (ant, post) ->
|
||||
if (ant != null && post != null) Valid(i, ant, post) else null
|
||||
}
|
||||
if (valid.size < minChannels) return false
|
||||
val ratio = ampRatio
|
||||
|
||||
// 조건 A: shared_idx 기반
|
||||
var effectiveShared = sharedIdx
|
||||
if (effectiveShared == null) {
|
||||
val postVals = valid.map { it.post }.sorted()
|
||||
val med = postVals[postVals.size / 2].toDouble()
|
||||
val inliers = valid.filter { abs(it.post - med) <= sharedTol }
|
||||
if (inliers.size >= minChannels) effectiveShared = med.roundToInt()
|
||||
}
|
||||
if (effectiveShared != null) {
|
||||
if (valid.all { abs(it.post - effectiveShared!!) <= sharedTol && tgcSignals[it.i][it.post] > tgcSignals[it.i][it.ant] * ratio }) return true
|
||||
}
|
||||
|
||||
// 조건 B: post 클러스터
|
||||
val postVals = valid.map { it.post }.sorted()
|
||||
val medPost = postVals[postVals.size / 2].toDouble()
|
||||
val inlierTriples = valid.filter { abs(it.post - medPost) <= sharedTol }
|
||||
if (inlierTriples.size >= minChannels) {
|
||||
if (inlierTriples.all { tgcSignals[it.i][it.post] > tgcSignals[it.i][it.ant] * ratio }) return true
|
||||
}
|
||||
return false
|
||||
}
|
||||
|
||||
// ── choose_rerun_or_initial_result ──
|
||||
|
||||
fun chooseResult(
|
||||
initial: LowEchoResult?, rerun: LowEchoResult?,
|
||||
brIdx: Int?, suppressIdx: Int?,
|
||||
minGap: Int, minBrSep: Int = 0, minInitialUrineLen: Int = 0
|
||||
): LowEchoResult? {
|
||||
if (rerun != null) return rerun
|
||||
if (canFallbackToInitial(initial, brIdx, suppressIdx, minGap, minBrSep, minInitialUrineLen)) return initial
|
||||
return null
|
||||
}
|
||||
|
||||
private fun canFallbackToInitial(initial: LowEchoResult?, brIdx: Int?, suppressIdx: Int?, minGap: Int, minBrSep: Int, minInitialUrineLen: Int): Boolean {
|
||||
if (initial == null || brIdx == null || suppressIdx == null) return false
|
||||
if (abs(brIdx - initial.post) < minBrSep) return false
|
||||
if ((suppressIdx - initial.post) < minGap) return false
|
||||
if (initial.urineLen < minInitialUrineLen) return false
|
||||
return true
|
||||
}
|
||||
|
||||
// ── suppress_back_reflection (valley early-stop 50.0) ──
|
||||
|
||||
fun suppress(sg: DoubleArray, backRefIdx: Int, searchMargin: Int = 10): DoubleArray {
|
||||
val cleaned = sg.copyOf()
|
||||
val n = sg.size
|
||||
if (backRefIdx >= n) return cleaned
|
||||
|
||||
val lo = max(0, backRefIdx - min(searchMargin, 3))
|
||||
val hi = min(n, backRefIdx + searchMargin + 1)
|
||||
var actualPeak = lo
|
||||
for (i in lo until hi) if (sg[i] > sg[actualPeak]) actualPeak = i
|
||||
|
||||
// Left valley with 50.0 rise early-stop
|
||||
var leftValley = actualPeak
|
||||
var leftBest = sg[actualPeak]
|
||||
for (i in (actualPeak - 1) downTo max(0, actualPeak - searchMargin)) {
|
||||
val cur = sg[i]
|
||||
if (cur < leftBest) { leftBest = cur; leftValley = i }
|
||||
else if (cur > leftBest + 50.0) break
|
||||
}
|
||||
|
||||
// Right valley with 50.0 rise early-stop
|
||||
var rightValley = actualPeak
|
||||
var rightBest = sg[actualPeak]
|
||||
for (i in (actualPeak + 1) until min(n, actualPeak + searchMargin + 1)) {
|
||||
val cur = sg[i]
|
||||
if (cur < rightBest) { rightBest = cur; rightValley = i }
|
||||
else if (cur > rightBest + 50.0) break
|
||||
}
|
||||
|
||||
if (rightValley > leftValley) {
|
||||
val leftVal = sg[leftValley]
|
||||
val rightVal = sg[rightValley]
|
||||
val length = rightValley - leftValley
|
||||
for (i in 0..length) {
|
||||
cleaned[leftValley + i] = leftVal + (rightVal - leftVal) * i.toDouble() / length
|
||||
}
|
||||
}
|
||||
return cleaned
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import kotlin.math.abs
|
||||
|
||||
/**
|
||||
* R-fit trend tracker — watches per-frame sphere-fit radius and classifies:
|
||||
* RISING — sliding toward bladder centre (R increasing)
|
||||
* PEAK — at / near session maximum R
|
||||
* FALLING — sliding past centre (R decreasing)
|
||||
* NOISE — fluctuating without clear trend
|
||||
* IDLE — no valid fit (too few channels)
|
||||
*
|
||||
* Ported from CKLaw BladderSeekRTracker.kt.
|
||||
*/
|
||||
class BladderSeekRTracker(
|
||||
private val historySize: Int = 8,
|
||||
private val peakToleranceMm: Double = 1.5,
|
||||
private val noiseDeltaMm: Double = 0.4,
|
||||
) {
|
||||
enum class Trend { RISING, PEAK, FALLING, NOISE, IDLE }
|
||||
|
||||
data class Status(
|
||||
val currentR: Double?,
|
||||
val bestR: Double,
|
||||
val trend: Trend,
|
||||
val improvementMm: Double,
|
||||
val hintEn: String,
|
||||
)
|
||||
|
||||
private val history = ArrayDeque<Double>()
|
||||
var bestR: Double = 0.0
|
||||
private set
|
||||
|
||||
fun reset() {
|
||||
history.clear()
|
||||
bestR = 0.0
|
||||
}
|
||||
|
||||
fun submit(r: Double?): Status {
|
||||
if (r == null || r.isNaN() || r <= 0.0) {
|
||||
return Status(null, bestR, Trend.IDLE, 0.0,
|
||||
"Not enough channels — check signal")
|
||||
}
|
||||
history.addLast(r)
|
||||
while (history.size > historySize) history.removeFirst()
|
||||
if (r > bestR) bestR = r
|
||||
|
||||
val trend = computeTrend(r)
|
||||
val gap = r - bestR
|
||||
val en = when (trend) {
|
||||
Trend.RISING -> "Getting closer (R=${"%.0f".format(r)}mm)"
|
||||
Trend.PEAK -> "At peak (R=${"%.0f".format(r)}mm)"
|
||||
Trend.FALLING -> "Past peak — reverse (best ${"%.0f".format(bestR)}mm)"
|
||||
Trend.NOISE -> "Searching (R=${"%.0f".format(r)}mm)"
|
||||
Trend.IDLE -> ""
|
||||
}
|
||||
return Status(r, bestR, trend, gap, en)
|
||||
}
|
||||
|
||||
private fun computeTrend(currentR: Double): Trend {
|
||||
if (history.size < 3) return Trend.NOISE
|
||||
if (bestR - currentR < peakToleranceMm) return Trend.PEAK
|
||||
val prev = history.toList().dropLast(1).takeLast(3)
|
||||
val prevMean = prev.average()
|
||||
val delta = currentR - prevMean
|
||||
return when {
|
||||
abs(delta) < noiseDeltaMm -> Trend.NOISE
|
||||
delta > 0 -> Trend.RISING
|
||||
else -> Trend.FALLING
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,190 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import kotlin.math.cos
|
||||
import kotlin.math.sin
|
||||
import kotlin.math.sqrt
|
||||
|
||||
/**
|
||||
* Sagittal-plane circle fit + coronal lateral offset + argmax.
|
||||
*
|
||||
* Ported from CKLaw BladderSphereSeek.kt, simplified to use PiezoHW
|
||||
* geometry directly (no SphereFit2Step / Geometry dependencies).
|
||||
*
|
||||
* Ch0..Ch3 wall points are projected to the YZ plane (sagittal slice)
|
||||
* and fitted with a 2D algebraic circle (Kasa method). The fit radius
|
||||
* serves as real-time alignment quality — maximised when probe is
|
||||
* centred over the bladder.
|
||||
*/
|
||||
object BladderSphereSeek {
|
||||
|
||||
data class FitResult(
|
||||
val radiusMm: Double,
|
||||
val centerY: Double, // along body axis (cranial+, caudal-)
|
||||
val centerZ: Double, // depth from probe surface
|
||||
val residualStdMm: Double,
|
||||
val nPoints: Int, // 3..8 wall points
|
||||
val confidence: Float, // 0..1
|
||||
)
|
||||
|
||||
data class CoronalResult(
|
||||
val lateralOffsetMm: Double,
|
||||
val nChannels: Int,
|
||||
val ch4MidXMm: Double?,
|
||||
val ch5MidXMm: Double?,
|
||||
val confidence: Float,
|
||||
)
|
||||
|
||||
/**
|
||||
* Convert wall sample index to YZ coordinates (mm) for a given channel.
|
||||
* Y = sensor_z + depth * sin(theta) (along body, cranial+)
|
||||
* Z = depth * cos(theta) (perpendicular depth into body)
|
||||
*/
|
||||
private fun wallIdxToYZ(ch: Int, sampleIdx: Int): DoubleArray {
|
||||
val hw = PiezoHW
|
||||
val depth = sampleIdx * hw.distancePerSample + hw.delayOffsetMm
|
||||
val theta = hw.degreeAll[ch] * Math.PI / 180.0
|
||||
val y = hw.sensorZMmAll[ch] + depth * sin(theta)
|
||||
val z = depth * cos(theta)
|
||||
return doubleArrayOf(y, z)
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert wall sample index to X coordinate (mm) for lateral channels.
|
||||
* X = sensor_x + depth * sin(theta_lr)
|
||||
*/
|
||||
private fun wallIdxToX(ch: Int, sampleIdx: Int): Double {
|
||||
val hw = PiezoHW
|
||||
val depth = sampleIdx * hw.distancePerSample + hw.delayOffsetMm
|
||||
val thetaLR = hw.degreeLRAll[ch] * Math.PI / 180.0
|
||||
return hw.sensorXMmAll[ch] + depth * sin(thetaLR)
|
||||
}
|
||||
|
||||
/**
|
||||
* 2D circle fit (Kasa algebraic method) on YZ wall points from Ch0..Ch3.
|
||||
*
|
||||
* @param walls list of 6 Pair(antIdx, postIdx)?, null for undetected channels
|
||||
* @param detected list of 6 booleans
|
||||
*/
|
||||
fun fitSagittalCircle(
|
||||
walls: List<Pair<Int, Int>?>,
|
||||
detected: List<Boolean>,
|
||||
): FitResult? {
|
||||
val pts = mutableListOf<DoubleArray>()
|
||||
for (ch in 0..3) {
|
||||
if (!detected.getOrElse(ch) { false }) continue
|
||||
val w = walls.getOrNull(ch) ?: continue
|
||||
pts.add(wallIdxToYZ(ch, w.first)) // ant
|
||||
pts.add(wallIdxToYZ(ch, w.second)) // post
|
||||
}
|
||||
if (pts.size < 3) return null
|
||||
|
||||
// Kasa circle fit: minimize algebraic distance
|
||||
// Solve [y^2+z^2, y, z, 1] * [1, -2cy, -2cz, cy^2+cz^2-r^2]^T = 0
|
||||
val n = pts.size
|
||||
var syy = 0.0; var sy = 0.0; var sz = 0.0
|
||||
var syz = 0.0; var szz = 0.0; var s1 = n.toDouble()
|
||||
var syyy = 0.0; var syzz = 0.0; var syyz = 0.0; var szzz = 0.0
|
||||
var sd = 0.0; var sdy = 0.0; var sdz = 0.0
|
||||
|
||||
for (p in pts) {
|
||||
val y = p[0]; val z = p[1]
|
||||
val d = y * y + z * z
|
||||
sy += y; sz += z; syy += y * y; szz += z * z; syz += y * z
|
||||
sd += d; sdy += d * y; sdz += d * z
|
||||
}
|
||||
|
||||
// Normal equations for Kasa fit
|
||||
val a11 = syy; val a12 = syz; val a13 = sy
|
||||
val a21 = syz; val a22 = szz; val a23 = sz
|
||||
val a31 = sy; val a32 = sz; val a33 = s1
|
||||
val b1 = sdy; val b2 = sdz; val b3 = sd
|
||||
|
||||
// Solve 3x3 system via Cramer's rule
|
||||
val det = a11 * (a22 * a33 - a23 * a32) -
|
||||
a12 * (a21 * a33 - a23 * a31) +
|
||||
a13 * (a21 * a32 - a22 * a31)
|
||||
if (kotlin.math.abs(det) < 1e-12) return null
|
||||
|
||||
val cy = (b1 * (a22 * a33 - a23 * a32) -
|
||||
a12 * (b2 * a33 - a23 * b3) +
|
||||
a13 * (b2 * a32 - a22 * b3)) / det / 2.0
|
||||
val cz = (a11 * (b2 * a33 - a23 * b3) -
|
||||
b1 * (a21 * a33 - a23 * a31) +
|
||||
a13 * (a21 * b3 - b2 * a31)) / det / 2.0
|
||||
val c3 = (a11 * (a22 * b3 - b2 * a32) -
|
||||
a12 * (a21 * b3 - b2 * a31) +
|
||||
b1 * (a21 * a32 - a22 * a31)) / det
|
||||
|
||||
val rSq = cy * cy + cz * cz + c3
|
||||
if (rSq <= 0) return null
|
||||
val r = sqrt(rSq)
|
||||
|
||||
// Residual: std of (|p - center| - r)
|
||||
var sumResidSq = 0.0
|
||||
for (p in pts) {
|
||||
val dist = sqrt((p[0] - cy) * (p[0] - cy) + (p[1] - cz) * (p[1] - cz))
|
||||
val resid = dist - r
|
||||
sumResidSq += resid * resid
|
||||
}
|
||||
val residStd = sqrt(sumResidSq / n)
|
||||
|
||||
// Sanity: anatomically possible range
|
||||
if (r < 15.0 || r > 120.0) return null
|
||||
if (residStd > 5.0) return null
|
||||
|
||||
val nFactor = ((n - 3).coerceAtLeast(0).toFloat() / 5f).coerceIn(0f, 1f)
|
||||
val residFactor = (1.0 - (residStd / 5.0).coerceIn(0.0, 1.0)).toFloat()
|
||||
val conf = (0.4f * nFactor + 0.6f * residFactor).coerceIn(0f, 1f)
|
||||
|
||||
return FitResult(r, cy, cz, residStd, n, conf)
|
||||
}
|
||||
|
||||
/**
|
||||
* Among Ch0..Ch3, returns the index of the channel with the longest chord.
|
||||
* 0 = centred over bladder, 1-3 = increasingly off-centre.
|
||||
* Returns null if any of Ch0..Ch3 is not detected.
|
||||
*/
|
||||
fun sagittalArgMaxCh(
|
||||
walls: List<Pair<Int, Int>?>,
|
||||
detected: List<Boolean>,
|
||||
): Int? {
|
||||
var bestCh = -1
|
||||
var bestLen = -1
|
||||
for (ch in 0..3) {
|
||||
if (!detected.getOrElse(ch) { false }) return null
|
||||
val w = walls.getOrNull(ch) ?: return null
|
||||
val len = w.second - w.first
|
||||
if (len > bestLen) { bestLen = len; bestCh = ch }
|
||||
}
|
||||
return if (bestCh >= 0) bestCh else null
|
||||
}
|
||||
|
||||
/**
|
||||
* Coronal lateral offset from Ch4/Ch5 chord midpoints.
|
||||
* Returns null if neither Ch4 nor Ch5 detected.
|
||||
*/
|
||||
fun computeCoronalLateral(
|
||||
walls: List<Pair<Int, Int>?>,
|
||||
detected: List<Boolean>,
|
||||
): CoronalResult? {
|
||||
var ch4Mid: Double? = null
|
||||
var ch5Mid: Double? = null
|
||||
for (ch in 4..5) {
|
||||
if (!detected.getOrElse(ch) { false }) continue
|
||||
val w = walls.getOrNull(ch) ?: continue
|
||||
val xAnt = wallIdxToX(ch, w.first)
|
||||
val xPost = wallIdxToX(ch, w.second)
|
||||
val midX = (xAnt + xPost) / 2.0
|
||||
if (ch == 4) ch4Mid = midX else ch5Mid = midX
|
||||
}
|
||||
val xs = listOfNotNull(ch4Mid, ch5Mid)
|
||||
if (xs.isEmpty()) return null
|
||||
return CoronalResult(
|
||||
lateralOffsetMm = xs.average(),
|
||||
nChannels = xs.size,
|
||||
ch4MidXMm = ch4Mid,
|
||||
ch5MidXMm = ch5Mid,
|
||||
confidence = if (xs.size == 2) 1.0f else 0.5f,
|
||||
)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,93 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.sqrt
|
||||
|
||||
/**
|
||||
* 센서 미부착 감지 — detachment_detection.py 1:1 포팅
|
||||
*
|
||||
* ADC 신호의 [20:80] 구간에서 baseline([80:100] median)을 빼고,
|
||||
* mean(|y|)과 std(y)가 모두 threshold 미만이면 미부착으로 판정.
|
||||
*
|
||||
* 미부착 시 LED state 4 (DETACH_WARNING) 전송.
|
||||
*/
|
||||
object DetachmentDetection {
|
||||
|
||||
const val WINDOW_START = 20
|
||||
const val WINDOW_END = 80
|
||||
const val BASELINE_WINDOW_START = 80
|
||||
const val BASELINE_WINDOW_END = 100
|
||||
const val THR_MEAN = 30.0
|
||||
const val THR_STD = 30.0
|
||||
const val N_CENTER_CH = 4
|
||||
|
||||
data class DetachmentResult(
|
||||
val isDetached: Boolean,
|
||||
val meanAbs: Double,
|
||||
val stdVal: Double,
|
||||
val meanAbsPerCh: DoubleArray,
|
||||
val stdPerCh: DoubleArray
|
||||
)
|
||||
|
||||
/**
|
||||
* 4채널(CH0~CH3) ADC 신호에서 미부착 여부 판정.
|
||||
*
|
||||
* @param signals 6채널 ADC 데이터 (List<List<UShort>>)
|
||||
* @return DetachmentResult
|
||||
*/
|
||||
fun detect(signals: List<List<UShort>>): DetachmentResult {
|
||||
if (signals.size < N_CENTER_CH) {
|
||||
return DetachmentResult(true, 0.0, 0.0, DoubleArray(N_CENTER_CH), DoubleArray(N_CENTER_CH))
|
||||
}
|
||||
|
||||
val nSample = signals[0].size
|
||||
val ws = WINDOW_START.coerceAtMost(nSample)
|
||||
val we = WINDOW_END.coerceAtMost(nSample)
|
||||
val bs = BASELINE_WINDOW_START.coerceAtMost(nSample)
|
||||
val be = BASELINE_WINDOW_END.coerceAtMost(nSample)
|
||||
|
||||
if (we <= ws || be <= bs) {
|
||||
return DetachmentResult(true, 0.0, 0.0, DoubleArray(N_CENTER_CH), DoubleArray(N_CENTER_CH))
|
||||
}
|
||||
|
||||
val meanAbsPerCh = DoubleArray(N_CENTER_CH)
|
||||
val stdPerCh = DoubleArray(N_CENTER_CH)
|
||||
|
||||
for (ch in 0 until N_CENTER_CH) {
|
||||
val raw = signals[ch]
|
||||
if (raw.size < be) {
|
||||
meanAbsPerCh[ch] = 0.0
|
||||
stdPerCh[ch] = 0.0
|
||||
continue
|
||||
}
|
||||
|
||||
// Baseline = median of [bs:be]
|
||||
val baselineSlice = (bs until be).map { raw[it].toDouble() }.sorted()
|
||||
val baseline = baselineSlice[baselineSlice.size / 2]
|
||||
|
||||
// y = signal[ws:we] - baseline
|
||||
val y = DoubleArray(we - ws) { raw[ws + it].toDouble() - baseline }
|
||||
|
||||
// mean(|y|)
|
||||
meanAbsPerCh[ch] = y.map { abs(it) }.average()
|
||||
|
||||
// std(y)
|
||||
val mean = y.average()
|
||||
stdPerCh[ch] = sqrt(y.map { (it - mean) * (it - mean) }.average())
|
||||
}
|
||||
|
||||
val meanAbs = meanAbsPerCh.average()
|
||||
val stdVal = stdPerCh.average()
|
||||
|
||||
// "and" rule: 둘 다 threshold 미만이면 미부착
|
||||
val isDetached = meanAbs < THR_MEAN && stdVal < THR_STD
|
||||
|
||||
return DetachmentResult(
|
||||
isDetached = isDetached,
|
||||
meanAbs = meanAbs,
|
||||
stdVal = stdVal,
|
||||
meanAbsPerCh = meanAbsPerCh,
|
||||
stdPerCh = stdPerCh
|
||||
)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,203 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
/**
|
||||
* Green Zone 전체 파이프라인의 공유 상수 — Single Source of Truth.
|
||||
*
|
||||
* UrinAI, PeakDetection, Scoring 모두 이 상수를 참조.
|
||||
* 파일 간 임계값 불일치 방지.
|
||||
*
|
||||
* 원본: xBench/GreenZoneConstants.kt (Charles KWON)
|
||||
* 포팅일: 2026-04-17
|
||||
*
|
||||
* ⚠ 미세 조정 시 이 파일만 수정하면 전체 파이프라인에 반영됨.
|
||||
* 각 상수의 의미와 영향 범위를 아래 주석 참고.
|
||||
*/
|
||||
enum class DetectionMethod { METHOD_A, METHOD_B, METHOD_C, METHOD_D }
|
||||
enum class PlacementGuideMode { SIMPLE, BOUNDARY, SWEEP }
|
||||
/**
|
||||
* BV 계산 방법.
|
||||
* - FRUSTUM : legacy analyzer walls + estimateBladderVolume6ch (Tanaka frustum)
|
||||
* - V41 : Method C 의 SphereFit (methodCBvMl 직접 사용)
|
||||
* - METHOD_D : Method D walls (cross-channel ant tie-break + OS-CFAR) + estimateBladderVolume6ch
|
||||
* → Python `for_app_share` 임상 검증 pipeline 와 1:1 정렬 (2026-06-30).
|
||||
*
|
||||
* 사용 지침 (phantom 측정 vs 임상):
|
||||
* - 구형 phantom (QC/calibration) : V41 (SphereFit) 권장. METHOD_D 는 lr_ratio < 1.0
|
||||
* 영역에서 BV underestimate 가능 (대칭 phantom 검증 시 -38% 오차 사례).
|
||||
* - 임상 인체 측정 : METHOD_D 권장. 인체 방광은 비대칭이므로 lr_ratio
|
||||
* [0.5, 1.5] 자유 범위가 anatomically 올바름. Python algorithm = appshare 팀
|
||||
* reference 동작과 정확 일치 (lr floor / shrinkage 의도적으로 없음).
|
||||
* - METHOD_D 에 phantom-style lr floor (1.0) 를 추가하지 말 것. 사용자 결정
|
||||
* 2026-06-30: "phantom QC 는 V41 로, 인체는 Python 1:1".
|
||||
*/
|
||||
enum class BvMethod { FRUSTUM, V41, METHOD_D }
|
||||
|
||||
/** Sensor alignment 알고리즘 — V1=기존 (computePlacementGuide), V2=신규 (alignment.py 포팅). */
|
||||
enum class AlignmentAlgo { V1, V2 }
|
||||
|
||||
object GreenZoneConstants {
|
||||
|
||||
@Volatile var postMaxIdx: Int = 80
|
||||
|
||||
@Volatile var detectionMethod: DetectionMethod = DetectionMethod.METHOD_C
|
||||
@Volatile var bvMethod: BvMethod = BvMethod.V41
|
||||
|
||||
/**
|
||||
* lr_ratio 강제 override (algorithm 팀 최신 표준: piezophantomtest 4830e7c).
|
||||
* - null : computed (기존 Python `compute_lr_ratio` 알고리즘)
|
||||
* - Double : 강제 값 (algorithm 팀 default = 1.0)
|
||||
*
|
||||
* Phantom 실측 검증: computed lr=0.68 → BV -38%. lr=1.0 fixed → -1.6% (APP 일치).
|
||||
* 6채널 hardware LR 분해능 한계로 computed lr 신뢰도 낮음. Dev toggle 전환 가능.
|
||||
*/
|
||||
@Volatile var lrRatioOverride: Double? = 1.0
|
||||
|
||||
@Volatile var placementGuideMode: PlacementGuideMode = PlacementGuideMode.SIMPLE
|
||||
@Volatile var alignmentAlgo: AlignmentAlgo = AlignmentAlgo.V1 // default V1 (일반 사용자). V2 (Method D) 는 clinical alignment session 전용.
|
||||
|
||||
/** Posture pitch 임계값 (deg). |pitch| > 이 값 → LYING, ≤ 이 값 → NON_LYING. dev 모드에서 조정 가능. */
|
||||
@Volatile var pitchLyingThreshold: Float = 25f
|
||||
|
||||
/** Posture acc magnitude gate min (g). 이 값 미만이면 classifier UNKNOWN. */
|
||||
@Volatile var postureAccMagMin: Float = 0.8f
|
||||
/** Posture acc magnitude gate max (g). 이 값 초과면 classifier UNKNOWN. */
|
||||
@Volatile var postureAccMagMax: Float = 1.2f
|
||||
|
||||
/**
|
||||
* Posture polling 간격 (ms). PiezoMonitoring 에서 msp 보내는 주기.
|
||||
* 6/30 변경: mtb (1Hz × 15sample) → msp (5Hz × 1sample) 로 전환. piezo burst 가청음 제거.
|
||||
* Walking detector 의 1.5초 window 안에 7-8 sample 확보 위해 200ms 주기.
|
||||
* dev 모드에서 조정 가능.
|
||||
*/
|
||||
@Volatile var posturePollIntervalMs: Long = 200L
|
||||
|
||||
// ── Walking detector 임계값 (flowchart 4-2 / 5-2) ─────────────────
|
||||
/** Walking 인정 최소 pitch (deg). |pitch| > 이 값 이라야 walking 후보. */
|
||||
@Volatile var walkingMinPitchDeg: Float = 45f
|
||||
/** Walking 시작 — gyro 이동평균 (wmean) 임계 (dps). 실측: stand-up + 자세조정
|
||||
* motion 이 5~10 dps 지속. 진짜 보행은 50+ dps. 10 으로 완화하여 짧은 보행도 잡음. */
|
||||
@Volatile var walkingStartWmean: Float = 10f
|
||||
/** Walking 시작 — gyro 이동표준편차 (wstd) 임계 (dps). */
|
||||
@Volatile var walkingStartWstd: Float = 2f
|
||||
/** Walking 시작 — 두 임계 모두 만족한 채 지속해야 하는 최소 시간 (ms). 실측: stand-up
|
||||
* motion 이 3-5초 → 6초 로 차단. 실제 보행은 무한 지속이라 6초 이상 자동 만족. */
|
||||
@Volatile var walkingStartHoldMs: Long = 6000L
|
||||
/** Walking 유지 — gyro wmean 임계 (dps). 시작보다 낮음. */
|
||||
@Volatile var walkingKeepWmean: Float = 2f
|
||||
/** Walking 유지 — wmean 임계 미달 후 종료까지 grace 시간 (ms). 미만 지속이 이 값 넘으면 walking 종료. */
|
||||
@Volatile var walkingKeepHoldMs: Long = 2000L
|
||||
/** Walking detector sliding window (ms). 이 시간 분량 sample 평균/표준편차. */
|
||||
@Volatile var walkingWindowMs: Long = 1500L
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 신호 범위
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
|
||||
/** raw ADC 중 사용할 샘플 수.
|
||||
* Kotlin 원본: 90. VesiScan maa 응답은 100 samples 전송하지만 끝부분 10개는
|
||||
* CRC/noise 가능성 → 앞 90개만 사용.
|
||||
* 영향: UrinAI 탐색 범위, PeakDetection 탐색 범위 */
|
||||
const val useSamples: Int = 90
|
||||
|
||||
/** 초기 ringdown 스킵 (센서 근접 반사 무시).
|
||||
* idx 0~2는 피에조 소자 직접 반사 → 항상 높은 값 → 액체로 오판 방지.
|
||||
* 영향: UrinAI.fw 탐색 시작점, PeakDetection.fw 탐색 시작점 */
|
||||
const val ringSkip: Int = 3
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 액체(소변) 판정 임계 — 12-bit ADC (0~4095)
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
|
||||
/** Low-echo detection 임계값 (denoised 신호 기준).
|
||||
* denoised ≤ 이 값 → low-echo (소변 후보).
|
||||
* UrinAI의 liquidThrLoose(raw 기준)와 다름 — 여기는 TVD+SG 적용 후 신호 기준.
|
||||
* Python 원본: 1150 (low_echo_detection_method_b.py LOW_ECHO_AMP, 6ch 버전)
|
||||
* 영향: PiezoEchoAnalyzer → low-echo span 탐지, 벽 찾기의 기반 */
|
||||
@Volatile var lowEchoAmp: Float = 1250f
|
||||
|
||||
/** 구조 이진화 임계값: raw < 이 값 → 액체(liquid), ≥ → 조직(tissue).
|
||||
* 낮출수록 엄격 (더 확실한 액체만 인정), 높일수록 관대.
|
||||
* Kotlin 원본: 1400.
|
||||
* 영향: UrinAI.binarize → fw/bw 탐색의 기반 */
|
||||
const val liquidThrLoose: Float = 1400f
|
||||
|
||||
/** 순수 액체 확인용 엄격 임계값: raw < 이 값 → "확실한 소변".
|
||||
* liquidThrLoose보다 낮아야 함.
|
||||
* 이 기준으로 연속 액체 구간(liquidRun) 측정.
|
||||
* Kotlin 원본: 1100. VesiScan 실측에서 팬텀 low-echo가 1100~1400 범위.
|
||||
* 1100→run=0, 1300→run=2 (미달). 1400으로 상향.
|
||||
* liquidThrLoose(1400)와 동일하게 설정 — 팬텀에서는 loose/strict 구분 불필요.
|
||||
* ⚠ 인체 측정 시 loose > strict 으로 재분리 필요할 수 있음
|
||||
* 영향: UrinAI.maxRun 계산 → 최종 판정의 핵심 */
|
||||
const val liquidThrStrict: Float = 1400f
|
||||
|
||||
/** 최소 연속 순수 액체 길이.
|
||||
* maxRun ≥ 이 값이어야 "소변 있음" 판정.
|
||||
* 소방광(50mL) 대응: chord≈24 중 순수 액체 5+ 필요.
|
||||
* Kotlin 원본: 5.
|
||||
* 영향: UrinAI 최종 판정 (detected 조건 1/3) */
|
||||
const val minLiquidRun: Int = 5
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 벽 검출 범위
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
|
||||
/** bw - fw 최소 (chord 최소 길이).
|
||||
* 소방광 한계: 15 → 30mL(chord≈20) 이상 검출 가능.
|
||||
* 50mL(chord≈24): margin 9 ✓
|
||||
* 30mL(chord≈20): margin 5 ⚠
|
||||
* 25mL 이하: 검출 불가 (임상적으로 PVR 50mL+ 의미 있는 범위)
|
||||
* Kotlin 원본: 15.
|
||||
* 영향: UrinAI 최종 판정 (detected 조건 2/3), PeakDetection bw 탐색 하한 */
|
||||
const val minChord: Int = 15
|
||||
|
||||
/** bw - fw 최대 (이론적 상한).
|
||||
* USE_SAMPLES(90)가 실질 제한.
|
||||
* Kotlin 원본: 80.
|
||||
* 영향: PeakDetection bw 탐색 상한 */
|
||||
const val maxChord: Int = 80
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 품질
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
|
||||
/** (wallPeak - lumenFloor) / wallPeak 최소.
|
||||
* 벽과 소변 사이 대비가 이 값 이상이어야 유효.
|
||||
* 높일수록 엄격 (선명한 벽 요구), 낮출수록 관대.
|
||||
* Kotlin 원본: 0.15.
|
||||
* 영향: UrinAI 최종 판정 (detected 조건 3/3) */
|
||||
const val contrastMin: Float = 0.15f
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// Green Zone Finder
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
|
||||
/** score ≥ 이 값 → center lock (Green Zone 확정).
|
||||
* Scoring.THR_EXCELLENT(75)보다 낮아 빠른 UX 확보.
|
||||
* Kotlin 원본: 70.
|
||||
* 영향: Placement Guide에서 Good 판정 기준 */
|
||||
const val lockThreshold: Int = 70
|
||||
|
||||
/** 최대 기억 측정 수 (히스토리).
|
||||
* Kotlin 원본: 20. */
|
||||
const val maxHistory: Int = 20
|
||||
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// 물리 상수 (교정용)
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
|
||||
/** ADC 샘플링 레이트 (Hz). VesiScan HW 사양. */
|
||||
const val fs: Double = 400_000.0
|
||||
|
||||
/** 기준 조직 음속 (m/s). */
|
||||
const val cRef: Double = 1540.0
|
||||
|
||||
/** 교정용 팬텀 직경 (mm). V=530mL 구. */
|
||||
const val dPhantomMm: Double = 100.406
|
||||
|
||||
/** 팬텀 용적 (mL). */
|
||||
const val vPhantomMl: Double = 530.0
|
||||
|
||||
/** 팬텀 반지름 (mm). */
|
||||
const val rPhantomMm: Double = 50.203
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import com.medithings.vesiscan.ble.ImuSample
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.atan2
|
||||
import kotlin.math.sqrt
|
||||
|
||||
/**
|
||||
* IMU 샘플 묶음(한 mtb 응답 = 15샘플)에서 자세/안정성 지표 추출.
|
||||
*
|
||||
* 가정: VBT 센서의 Accel z축이 환자 몸 바깥 방향(센서 면 → 위).
|
||||
* 평평하게 누운 환자(Supine)에 부착 시 Az ≈ -1g (또는 +1g, 부착 면에 따라).
|
||||
*
|
||||
* 향후 임상 데이터로 calibration 필요.
|
||||
*/
|
||||
object ImuAnalysis {
|
||||
|
||||
/** 한 응답 안의 모든 샘플 평균 Accel vector */
|
||||
fun meanAccel(samples: List<ImuSample>): Triple<Float, Float, Float>? {
|
||||
if (samples.isEmpty()) return null
|
||||
var sx = 0f; var sy = 0f; var sz = 0f
|
||||
for (s in samples) { sx += s.ax; sy += s.ay; sz += s.az }
|
||||
val n = samples.size
|
||||
return Triple(sx / n, sy / n, sz / n)
|
||||
}
|
||||
|
||||
/** 한 응답 안의 모든 샘플 평균 Gyro vector */
|
||||
fun meanGyro(samples: List<ImuSample>): Triple<Float, Float, Float>? {
|
||||
if (samples.isEmpty()) return null
|
||||
var sx = 0f; var sy = 0f; var sz = 0f
|
||||
for (s in samples) { sx += s.gx; sy += s.gy; sz += s.gz }
|
||||
val n = samples.size
|
||||
return Triple(sx / n, sy / n, sz / n)
|
||||
}
|
||||
|
||||
/**
|
||||
* 흔들림 강도 — Gyro 표준편차의 RMS. 정지 시 ≈ 0~5 dps, 흔드는 중 ≈ 50+ dps.
|
||||
*/
|
||||
fun shakeRmsDps(samples: List<ImuSample>): Float {
|
||||
if (samples.size < 2) return 0f
|
||||
val mean = meanGyro(samples) ?: return 0f
|
||||
var sum = 0f
|
||||
for (s in samples) {
|
||||
val dx = s.gx - mean.first
|
||||
val dy = s.gy - mean.second
|
||||
val dz = s.gz - mean.third
|
||||
sum += dx * dx + dy * dy + dz * dz
|
||||
}
|
||||
return sqrt(sum / samples.size)
|
||||
}
|
||||
|
||||
/**
|
||||
* 자세 추정 (간이 — Accel만 사용).
|
||||
* 평균 Accel vector의 방향 → 어느 축이 중력에 가까운지 판정.
|
||||
*
|
||||
* 반환: "Supine" / "Sitting" / "Standing" / "Tilted" / "Moving"
|
||||
*
|
||||
* 임시 기준 (임상 데이터로 보정 필요):
|
||||
* - |Az| 가장 큼 + Az > 0.7g: 환자 몸 누움 (Supine, 센서면 위 향함)
|
||||
* - |Az| 가장 큼 + Az < -0.7g: 센서 뒤집힘
|
||||
* - |Ay| 가장 큼: Sitting (몸 세움)
|
||||
* - |Ax| 가장 큼: Standing 또는 옆 기울기
|
||||
* - 기타: Tilted
|
||||
* - shakeRmsDps > 30: Moving (자세 판정 불가)
|
||||
*/
|
||||
fun estimatePosture(samples: List<ImuSample>): String {
|
||||
if (samples.isEmpty()) return "—"
|
||||
if (shakeRmsDps(samples) > 30f) return "Moving"
|
||||
val (ax, ay, az) = meanAccel(samples) ?: return "—"
|
||||
val axAbs = abs(ax); val ayAbs = abs(ay); val azAbs = abs(az)
|
||||
return when {
|
||||
azAbs >= axAbs && azAbs >= ayAbs && azAbs > 0.7f ->
|
||||
if (az > 0) "Supine" else "Supine (flip)"
|
||||
ayAbs >= axAbs && ayAbs > 0.6f -> "Sitting"
|
||||
axAbs > 0.6f -> "Standing"
|
||||
else -> "Tilted"
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Tilt 각도 (도) — 센서 z축이 중력에 대해 얼마나 기울었는지.
|
||||
* 0° = z축이 정확히 중력 방향, 90° = z축이 수평.
|
||||
*/
|
||||
fun tiltDegrees(samples: List<ImuSample>): Float {
|
||||
val (ax, ay, az) = meanAccel(samples) ?: return 0f
|
||||
val mag = sqrt(ax * ax + ay * ay + az * az)
|
||||
if (mag < 0.1f) return 0f // 자유낙하/이상치
|
||||
val tiltRad = atan2(sqrt(ax * ax + ay * ay), abs(az))
|
||||
return (tiltRad * 180.0 / Math.PI).toFloat()
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,78 @@
|
||||
/*
|
||||
* Posture classifier — flowchart `Posture classification` 기반 (2026-06-22).
|
||||
*
|
||||
* 단순화: 3 state (SUPINE/SITTING/STANDING) → **2 state (LYING / NON_LYING)**.
|
||||
* - 낙상 예방 목적상 sitting/standing 구분 불필요. "누웠나, 일어났나" 만 중요.
|
||||
*
|
||||
* 단계:
|
||||
* 1. Acc magnitude 게이트: 0.8 ≤ |a| ≤ 1.2 → motion/bad sample 거부
|
||||
* 2. Gyro magnitude 게이트: |g| ≤ 9.16 dps → 회전 중 sample 거부
|
||||
* 3. Pitch 계산: atan(ax / sqrt(ay² + az²)) × 180/π
|
||||
* 4. 분류 (|pitch| 기준, threshold = GreenZoneConstants.pitchLyingThreshold):
|
||||
* > threshold → LYING (누움)
|
||||
* ≤ threshold → NON_LYING (앉음/섬)
|
||||
* threshold default = 25°, dev 모드 설정에서 변경 가능.
|
||||
*/
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import com.medithings.vesiscan.ble.ImuSample
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.atan
|
||||
import kotlin.math.sqrt
|
||||
|
||||
enum class Posture { UNKNOWN, LYING, NON_LYING, WALKING }
|
||||
|
||||
object ImuPostureClassifier {
|
||||
|
||||
// Acc gate 임계값은 GreenZoneConstants.postureAccMagMin/Max 사용 (dev 조절 가능).
|
||||
// Gyro gate 비활성화 — 일부 디바이스의 gyro calibration bias (-25 dps 등) 가
|
||||
// flowchart 임계(9.16) 보다 커서 정지 상태에서도 fail → 분류 영구 불가.
|
||||
// accel magnitude gate 가 이미 큰 모션 거름. gyro 게이트 제거.
|
||||
// 단 dev 모드 디버그 위해 magnitude 는 계속 계산해 로그/Result 에 노출.
|
||||
|
||||
/** 자세 + 진단 정보 (UI/log용). */
|
||||
data class Result(
|
||||
val posture: Posture,
|
||||
val pitchDeg: Float,
|
||||
val accMag: Float,
|
||||
val gyroMagDps: Float,
|
||||
val reason: String,
|
||||
)
|
||||
|
||||
fun classify(samples: List<ImuSample>): Result {
|
||||
if (samples.isEmpty()) {
|
||||
return Result(Posture.UNKNOWN, 0f, 0f, 0f, "no samples")
|
||||
}
|
||||
|
||||
var sumAx = 0f; var sumAy = 0f; var sumAz = 0f
|
||||
var sumGx = 0f; var sumGy = 0f; var sumGz = 0f
|
||||
for (s in samples) {
|
||||
sumAx += s.ax; sumAy += s.ay; sumAz += s.az
|
||||
sumGx += s.gx; sumGy += s.gy; sumGz += s.gz
|
||||
}
|
||||
val n = samples.size
|
||||
val ax = sumAx / n; val ay = sumAy / n; val az = sumAz / n
|
||||
val gx = sumGx / n; val gy = sumGy / n; val gz = sumGz / n
|
||||
|
||||
val accMag = sqrt(ax * ax + ay * ay + az * az)
|
||||
val gyroMag = sqrt(gx * gx + gy * gy + gz * gz)
|
||||
|
||||
// Pitch 는 acc gate 통과 여부와 무관하게 항상 계산 (walking detector 가 사용).
|
||||
val pitchPre = (atan(ax / sqrt(ay * ay + az * az)) * 180.0 / Math.PI).toFloat()
|
||||
|
||||
if (accMag < GreenZoneConstants.postureAccMagMin || accMag > GreenZoneConstants.postureAccMagMax) {
|
||||
return Result(Posture.UNKNOWN, pitchPre, accMag, gyroMag,
|
||||
"acc mag out of range (%.2f)".format(accMag))
|
||||
}
|
||||
// Gyro gate 제거 — bias 큰 디바이스에서 영구 fail 문제 (위 주석 참조)
|
||||
|
||||
val absPitch = abs(pitchPre)
|
||||
val thr = GreenZoneConstants.pitchLyingThreshold
|
||||
|
||||
// Pitch 의미: AX 가 중력과 일치할수록 큰 값 (≈90°), AZ 가 중력과 일치하면 작은 값 (≈0°).
|
||||
// 사용자 직관: 센서 평평하게 둠 (AZ up) = LYING, 세움 (AX up) = NON_LYING (upright).
|
||||
// → 작은 pitch → LYING, 큰 pitch → NON_LYING
|
||||
val posture = if (absPitch <= thr) Posture.LYING else Posture.NON_LYING
|
||||
return Result(posture, pitchPre, accMag, gyroMag, "ok")
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,206 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import com.medithings.vesiscan.models.NirsConstants
|
||||
import com.medithings.vesiscan.models.NirsMagResponse
|
||||
import com.medithings.vesiscan.models.NirsOxyResult
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.ln
|
||||
import kotlin.math.max
|
||||
import kotlin.math.pow
|
||||
|
||||
class NirsCalcEngine {
|
||||
|
||||
// Gain Calibration
|
||||
private var gainValues: IntArray = intArrayOf()
|
||||
val hasGain: Boolean get() = gainValues.size >= 24
|
||||
|
||||
fun setGainFromMag(response: NirsMagResponse) {
|
||||
gainValues = response.gainValues
|
||||
}
|
||||
|
||||
fun clearGain() {
|
||||
gainValues = intArrayOf()
|
||||
}
|
||||
|
||||
// AGC DAC -> Software Amplification
|
||||
private fun agcGain(dacValue: Int): Double {
|
||||
return 10.0.pow(((dacValue.toDouble() * 3.3 / 4096.0 * -80.0) + 88.0) / 20.0)
|
||||
}
|
||||
|
||||
// DeGain Calculation
|
||||
fun degain(rawLed: IntArray): DoubleArray {
|
||||
if (rawLed.size < 24 || !hasGain) return DoubleArray(24) { 0.0 }
|
||||
|
||||
val result = DoubleArray(24)
|
||||
for (i in 0 until 24) {
|
||||
val raw = rawLed[i].toDouble()
|
||||
val sag = gainValues[i]
|
||||
val pkg = i / 6
|
||||
val wl = i % 6
|
||||
|
||||
val softAmp = agcGain(sag)
|
||||
val hAmp = NirsConstants.hardwareAmp[pkg]
|
||||
val spResponse = NirsConstants.spectralResponse[wl]
|
||||
val dpf = NirsConstants.dpf[wl]
|
||||
|
||||
val denominator = softAmp * hAmp
|
||||
if (denominator > 0) {
|
||||
var dg = raw / denominator
|
||||
dg /= spResponse
|
||||
dg *= dpf
|
||||
result[i] = dg
|
||||
}
|
||||
}
|
||||
return result
|
||||
}
|
||||
|
||||
// Butterworth IIR Filter
|
||||
private var filterState: Array<DoubleArray> = emptyArray()
|
||||
private var filterInitialized = false
|
||||
var isFilterEnabled = true
|
||||
|
||||
fun resetFilter() {
|
||||
filterState = Array(24) { doubleArrayOf(0.0, 0.0, 0.0, 0.0) }
|
||||
filterInitialized = false
|
||||
}
|
||||
|
||||
private fun applyFilter(x: Double, state: DoubleArray): Double {
|
||||
val b = NirsConstants.filterB
|
||||
val a = NirsConstants.filterA
|
||||
|
||||
val y = b[0] * x + b[1] * state[0] + b[2] * state[1] - a[1] * state[2] - a[2] * state[3]
|
||||
|
||||
state[1] = state[0]
|
||||
state[0] = x
|
||||
state[3] = state[2]
|
||||
state[2] = y
|
||||
|
||||
return y
|
||||
}
|
||||
|
||||
fun filterDeGain(degainValues: DoubleArray): DoubleArray {
|
||||
if (degainValues.size < 24) return degainValues
|
||||
|
||||
if (filterState.isEmpty()) resetFilter()
|
||||
|
||||
if (!filterInitialized) {
|
||||
for (i in 0 until 24) {
|
||||
val v = degainValues[i]
|
||||
filterState[i] = doubleArrayOf(v, v, v, v)
|
||||
}
|
||||
filterInitialized = true
|
||||
}
|
||||
|
||||
val filtered = DoubleArray(24)
|
||||
for (i in 0 until 24) {
|
||||
filtered[i] = applyFilter(degainValues[i], filterState[i])
|
||||
}
|
||||
return filtered
|
||||
}
|
||||
|
||||
// Oxygenation Calculation
|
||||
fun calcOxygenation(degainValues: DoubleArray): NirsOxyResult {
|
||||
if (degainValues.size < 24) return NirsOxyResult.zero
|
||||
|
||||
val c = NirsConstants
|
||||
val muA = DoubleArray(c.numWavelengths)
|
||||
|
||||
for (i in 0 until c.numWavelengths) {
|
||||
val i2cm = degainValues[i]
|
||||
val i5cm = degainValues[18 + i]
|
||||
|
||||
val i2norm = i2cm / (c.ledPower[0] / c.referencePower)
|
||||
val i5norm = i5cm / (c.ledPower[3] / c.referencePower)
|
||||
|
||||
val mix = c.srsMixNear * i2norm + c.srsMixFar * i5norm
|
||||
|
||||
if (mix > 0) {
|
||||
val slope = -ln(mix) / c.srsPathLength
|
||||
muA[i] = -slope
|
||||
}
|
||||
}
|
||||
|
||||
val solution = solveNormalEq6x3(muA)
|
||||
|
||||
var hbO2 = max(0.0, solution.first) * c.oxyScale
|
||||
var hb = max(0.0, solution.second) * c.oxyScale
|
||||
|
||||
val tHb = hbO2 + hb
|
||||
val stO2 = if (tHb > 0) (hbO2 / tHb) * 100.0 else 0.0
|
||||
|
||||
return NirsOxyResult(hbO2 = hbO2, hb = hb, tHb = tHb, stO2 = stO2)
|
||||
}
|
||||
|
||||
// Least Squares Solver (6x3)
|
||||
private fun solveNormalEq6x3(muA: DoubleArray): Triple<Double, Double, Double> {
|
||||
val eps = arrayOf(
|
||||
NirsConstants.extinctionHbO2,
|
||||
NirsConstants.extinctionHb,
|
||||
NirsConstants.extinctionH2O
|
||||
)
|
||||
|
||||
val n = Array(3) { DoubleArray(3) }
|
||||
for (i in 0 until 3) {
|
||||
for (j in 0 until 3) {
|
||||
var sum = 0.0
|
||||
for (k in 0 until 6) sum += eps[i][k] * eps[j][k]
|
||||
n[i][j] = sum
|
||||
}
|
||||
}
|
||||
|
||||
val b = DoubleArray(3)
|
||||
for (i in 0 until 3) {
|
||||
var sum = 0.0
|
||||
for (k in 0 until 6) sum += eps[i][k] * muA[k]
|
||||
b[i] = sum
|
||||
}
|
||||
|
||||
val det = n[0][0] * (n[1][1] * n[2][2] - n[1][2] * n[2][1]) -
|
||||
n[0][1] * (n[1][0] * n[2][2] - n[1][2] * n[2][0]) +
|
||||
n[0][2] * (n[1][0] * n[2][1] - n[1][1] * n[2][0])
|
||||
|
||||
if (abs(det) < 1e-12) return Triple(0.0, 0.0, 0.0)
|
||||
|
||||
val inv = Array(3) { DoubleArray(3) }
|
||||
inv[0][0] = (n[1][1] * n[2][2] - n[1][2] * n[2][1]) / det
|
||||
inv[0][1] = -(n[0][1] * n[2][2] - n[0][2] * n[2][1]) / det
|
||||
inv[0][2] = (n[0][1] * n[1][2] - n[0][2] * n[1][1]) / det
|
||||
inv[1][0] = -(n[1][0] * n[2][2] - n[1][2] * n[2][0]) / det
|
||||
inv[1][1] = (n[0][0] * n[2][2] - n[0][2] * n[2][0]) / det
|
||||
inv[1][2] = -(n[0][0] * n[1][2] - n[0][2] * n[1][0]) / det
|
||||
inv[2][0] = (n[1][0] * n[2][1] - n[1][1] * n[2][0]) / det
|
||||
inv[2][1] = -(n[0][0] * n[2][1] - n[0][1] * n[2][0]) / det
|
||||
inv[2][2] = (n[0][0] * n[1][1] - n[0][1] * n[1][0]) / det
|
||||
|
||||
val c = DoubleArray(3) { i ->
|
||||
inv[i][0] * b[0] + inv[i][1] * b[1] + inv[i][2] * b[2]
|
||||
}
|
||||
|
||||
return Triple(c[0], c[1], c[2])
|
||||
}
|
||||
|
||||
// Full Processing Pipeline
|
||||
data class ProcessResult(
|
||||
val degain: DoubleArray,
|
||||
val degainFiltered: DoubleArray,
|
||||
val oxy: NirsOxyResult,
|
||||
val oxyFiltered: NirsOxyResult
|
||||
)
|
||||
|
||||
fun process(rawLed: IntArray): ProcessResult {
|
||||
val dg = degain(rawLed)
|
||||
val oxyRaw = calcOxygenation(dg)
|
||||
|
||||
val dgf: DoubleArray
|
||||
val oxyF: NirsOxyResult
|
||||
if (isFilterEnabled) {
|
||||
dgf = filterDeGain(dg)
|
||||
oxyF = calcOxygenation(dgf)
|
||||
} else {
|
||||
dgf = dg
|
||||
oxyF = oxyRaw
|
||||
}
|
||||
|
||||
return ProcessResult(dg, dgf, oxyRaw, oxyF)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import androidx.compose.runtime.mutableIntStateOf
|
||||
import androidx.compose.runtime.mutableStateListOf
|
||||
import androidx.compose.runtime.mutableStateOf
|
||||
import com.medithings.vesiscan.models.NirsMagResponse
|
||||
import com.medithings.vesiscan.models.NirsMcjPacket
|
||||
import com.medithings.vesiscan.models.NirsConstants
|
||||
import com.medithings.vesiscan.BuildConfig
|
||||
import com.medithings.vesiscan.models.NirsOxyResult
|
||||
|
||||
class NirsManager private constructor() {
|
||||
companion object {
|
||||
val instance: NirsManager by lazy { NirsManager() }
|
||||
}
|
||||
|
||||
// State
|
||||
val isStreaming = mutableStateOf(false)
|
||||
val isCalibrated = mutableStateOf(false)
|
||||
val isFilterEnabled = mutableStateOf(true)
|
||||
|
||||
// Current Values
|
||||
val currentOxy = mutableStateOf(NirsOxyResult.zero)
|
||||
val sampleCount = mutableIntStateOf(0)
|
||||
|
||||
// Chart History
|
||||
val oxyHbO2History = mutableStateListOf<Double>()
|
||||
val oxyHbHistory = mutableStateListOf<Double>()
|
||||
|
||||
// Internal
|
||||
val calcEngine = NirsCalcEngine()
|
||||
private val maxPoints = NirsConstants.maxHistoryPoints
|
||||
|
||||
// Calibration
|
||||
fun setCalibrationGain(response: NirsMagResponse) {
|
||||
calcEngine.setGainFromMag(response)
|
||||
isCalibrated.value = true
|
||||
}
|
||||
|
||||
// Process mcj Packet
|
||||
fun processMcjPacket(packet: NirsMcjPacket) {
|
||||
val result = calcEngine.process(packet.ledValues)
|
||||
currentOxy.value = result.oxyFiltered
|
||||
sampleCount.intValue++
|
||||
|
||||
if (BuildConfig.DEBUG && (sampleCount.intValue <= 3 || sampleCount.intValue % 50 == 0)) {
|
||||
android.util.Log.d("NirsManager", "Sample #${sampleCount.intValue}: " +
|
||||
"hasGain=${calcEngine.hasGain}, " +
|
||||
"led[0..5]=${packet.ledValues.take(6)}, " +
|
||||
"degain[0..2]=${result.degain.take(3).map { "%.2f".format(it) }}, " +
|
||||
"oxy=StO2:${"%.1f".format(result.oxyFiltered.stO2)} HbO2:${"%.1f".format(result.oxyFiltered.hbO2)} HHb:${"%.1f".format(result.oxyFiltered.hb)}")
|
||||
}
|
||||
|
||||
appendTrimming(oxyHbO2History, result.oxyFiltered.hbO2)
|
||||
appendTrimming(oxyHbHistory, result.oxyFiltered.hb)
|
||||
}
|
||||
|
||||
// Controls
|
||||
fun toggleFilter() {
|
||||
isFilterEnabled.value = !isFilterEnabled.value
|
||||
calcEngine.isFilterEnabled = isFilterEnabled.value
|
||||
if (isFilterEnabled.value) calcEngine.resetFilter()
|
||||
}
|
||||
|
||||
fun clearData() {
|
||||
oxyHbO2History.clear()
|
||||
oxyHbHistory.clear()
|
||||
currentOxy.value = NirsOxyResult.zero
|
||||
sampleCount.intValue = 0
|
||||
calcEngine.resetFilter()
|
||||
}
|
||||
|
||||
fun startStreaming() {
|
||||
isStreaming.value = true
|
||||
calcEngine.resetFilter()
|
||||
}
|
||||
|
||||
fun stopStreaming() {
|
||||
isStreaming.value = false
|
||||
}
|
||||
|
||||
fun reset() {
|
||||
stopStreaming()
|
||||
clearData()
|
||||
isCalibrated.value = false
|
||||
calcEngine.clearGain()
|
||||
}
|
||||
|
||||
// Helpers
|
||||
private fun appendTrimming(list: MutableList<Double>, value: Double) {
|
||||
list.add(value)
|
||||
while (list.size > maxPoints) list.removeAt(0)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,855 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import com.medithings.vesiscan.walldetect.core.WdConfig
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.cos
|
||||
import kotlin.math.hypot
|
||||
import kotlin.math.min
|
||||
import kotlin.math.max
|
||||
import kotlin.math.sin
|
||||
import kotlin.math.sqrt
|
||||
|
||||
// ==========================================================================
|
||||
// Bladder Volume Estimation — Multi-Channel Frustum + Cap (5ch vertical)
|
||||
//
|
||||
// 1:1 port of PiezoBVEstimator.swift / bv_estimation.py (algo-test branch)
|
||||
// Only "traditional" mode is used (coord/hybrid disabled in Python too)
|
||||
// ==========================================================================
|
||||
|
||||
// ── Hardware Parameters (5ch vertical array) ──
|
||||
|
||||
/**
|
||||
* Probe geometry — probe model별 고정
|
||||
*/
|
||||
object PiezoHW {
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
// Device Presets — 6ch 기기 3가지 버전 (Snell's law 굴절 보정 적용)
|
||||
// config_6ch.py 1:1 포팅
|
||||
// ═══════════════════════════════════════════════════════════
|
||||
|
||||
enum class DevicePreset {
|
||||
LEGACY_5CH,
|
||||
V0, // All 0° (BLE 테스트용)
|
||||
V1, // Max 20° housing → Snell's law refraction
|
||||
V2 // Max 30° housing → Snell's law refraction
|
||||
}
|
||||
|
||||
var activePreset: DevicePreset = DevicePreset.V0
|
||||
|
||||
fun autoDetectPreset(deviceName: String) {
|
||||
if (deviceName.startsWith("VBT") && deviceName.length >= 4) {
|
||||
// VBT...n0x → 뒤에서 3번째 글자(n)가 각도 타입
|
||||
// n=0 → V0, n=2 → V1, n=3 → V2
|
||||
val n = deviceName[deviceName.length - 3]
|
||||
activePreset = when (n) {
|
||||
'0' -> DevicePreset.V0
|
||||
'2' -> DevicePreset.V1
|
||||
'3' -> DevicePreset.V2
|
||||
else -> DevicePreset.V1 // 알 수 없으면 V1 기본
|
||||
}
|
||||
} else if (deviceName.startsWith("2025MEDIP")) {
|
||||
activePreset = DevicePreset.LEGACY_5CH
|
||||
}
|
||||
android.util.Log.d("PiezoHW", "autoDetectPreset: '$deviceName' → ${activePreset.name}")
|
||||
}
|
||||
|
||||
val centerCh = intArrayOf(0, 1, 2, 3)
|
||||
val lateralCh = intArrayOf(4, 5)
|
||||
val lateralNeighbors = intArrayOf(1, 2)
|
||||
|
||||
// 6채널 전체 z 좌표 (CH0~CH5)
|
||||
val sensorZMmAll: DoubleArray
|
||||
get() = when (activePreset) {
|
||||
DevicePreset.LEGACY_5CH -> doubleArrayOf(22.0, 16.5, 11.0, 5.5, 0.0, 0.0)
|
||||
DevicePreset.V0 -> doubleArrayOf(21.0, 14.0, 7.0, 0.0, 10.5, 10.5)
|
||||
DevicePreset.V1 -> doubleArrayOf(20.1, 12.8, 7.0, 0.0, 9.9, 9.9)
|
||||
DevicePreset.V2 -> doubleArrayOf(19.3, 13.0, 6.7, 0.0, 9.85, 9.85)
|
||||
}
|
||||
|
||||
// 6채널 전체 x 좌표
|
||||
val sensorXMmAll: DoubleArray
|
||||
get() = when (activePreset) {
|
||||
DevicePreset.LEGACY_5CH -> doubleArrayOf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
else -> doubleArrayOf(0.0, 0.0, 0.0, 0.0, -10.0, 10.0)
|
||||
}
|
||||
|
||||
// 6채널 전체 SI 빔 각도 (Snell's law 굴절 후)
|
||||
val degreeAll: DoubleArray
|
||||
get() = when (activePreset) {
|
||||
DevicePreset.LEGACY_5CH -> doubleArrayOf(0.0, -2.2, -4.4, -6.6, -8.8, 0.0)
|
||||
DevicePreset.V0 -> doubleArrayOf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
DevicePreset.V1 -> doubleArrayOf(6.89, 0.0, -6.89, -13.66, -6.87, -6.87)
|
||||
DevicePreset.V2 -> doubleArrayOf(0.0, -6.89, -13.66, -20.20, -6.87, -6.87)
|
||||
}
|
||||
|
||||
// 6채널 전체 LR 빔 각도 (Snell's law 굴절 후)
|
||||
val degreeLRAll: DoubleArray
|
||||
get() = when (activePreset) {
|
||||
DevicePreset.LEGACY_5CH -> doubleArrayOf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
DevicePreset.V0 -> doubleArrayOf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
DevicePreset.V1 -> doubleArrayOf(0.0, 0.0, 0.0, 0.0, -3.42, 3.42)
|
||||
DevicePreset.V2 -> doubleArrayOf(0.0, 0.0, 0.0, 0.0, -3.42, 3.42)
|
||||
}
|
||||
|
||||
// BV용 center 채널만 (CH0~CH3)
|
||||
val sensorZMm: DoubleArray get() = centerCh.map { sensorZMmAll[it] }.toDoubleArray()
|
||||
val sensorXMm: DoubleArray get() = centerCh.map { sensorXMmAll[it] }.toDoubleArray()
|
||||
val degree: DoubleArray get() = centerCh.map { degreeAll[it] }.toDoubleArray()
|
||||
val degreeLR: DoubleArray get() = centerCh.map { degreeLRAll[it] }.toDoubleArray()
|
||||
|
||||
/** Acoustic calibration (런타임 조절 가능).
|
||||
* Setter는 walldetect.core.WdConfig.DPS_DEFAULT 도 함께 동기화한다.
|
||||
* V41Detector / Geometry / AnatomicalGate 가 WdConfig 측 상수를 사용하기
|
||||
* 때문에 한 곳만 바꾸면 BV 두 경로 (6ch estimate vs V41 dispatch / gate) 가
|
||||
* 서로 다른 dps 로 계산되어 측정이 어긋난다. */
|
||||
// `.also` 로 초기값을 walldetect.core.WdConfig 측에도 동시에 반영 — 시작
|
||||
// 시점부터 두 경로의 dps 가 일치한다.
|
||||
// 02bed02 (config_6ch.py): DISTANCE_PER_SAMPLE 1.981 → 1.936 (HW 샘플링레이트 변경).
|
||||
// 우리 default 도 1.968 → 1.936 으로 정렬 (FW VBTFW118+ 기준).
|
||||
@Volatile private var _distancePerSample: Double = 1.936.also { WdConfig.DPS_DEFAULT = it }
|
||||
var distancePerSample: Double
|
||||
get() = _distancePerSample
|
||||
set(value) {
|
||||
_distancePerSample = value
|
||||
WdConfig.DPS_DEFAULT = value
|
||||
}
|
||||
const val delayOffsetMm: Double = 6.85
|
||||
|
||||
/** Volume model */
|
||||
val areaK: Double = Math.PI / 4.0
|
||||
const val defaultLrRatio: Double = 1.0
|
||||
|
||||
// lr_ratio fallbacks
|
||||
const val lrRatioNoDetection: Double = 1.0
|
||||
const val lrRatioInvalid: Double = 1.2
|
||||
const val lrPrior: Double = 1.2
|
||||
|
||||
val presetName: String
|
||||
get() = when (activePreset) {
|
||||
DevicePreset.LEGACY_5CH -> "Legacy 5ch"
|
||||
DevicePreset.V0 -> "V0 (all 0°)"
|
||||
DevicePreset.V1 -> "V1 (max 20°, Snell)"
|
||||
DevicePreset.V2 -> "V2 (max 30°, Snell)"
|
||||
}
|
||||
}
|
||||
|
||||
// ── Center Wall Repair (6ch) ──
|
||||
|
||||
/**
|
||||
* 6ch center 채널(CH0~CH3) 패턴을 BV 계산용으로 보정.
|
||||
* - gap 1개: 선형 보간
|
||||
* - gap 2개+: 위쪽 그룹 버리고 아래쪽만 사용
|
||||
* Port of bv_estimation.py _repair_center_walls_for_6ch (#21 merge)
|
||||
*/
|
||||
private fun repairCenterWallsFor6ch(
|
||||
centerWalls: List<Pair<Int, Int>?>
|
||||
): List<Pair<Int, Int>?> {
|
||||
val result = centerWalls.toMutableList()
|
||||
val valid = result.indices.filter { i ->
|
||||
val w = result[i]; w != null
|
||||
}
|
||||
if (valid.size < 2) return result
|
||||
|
||||
for (k in 0 until valid.size - 1) {
|
||||
val prevIdx = valid[k]
|
||||
val nextIdx = valid[k + 1]
|
||||
val gap = nextIdx - prevIdx - 1
|
||||
if (gap <= 0) continue
|
||||
if (gap == 1) {
|
||||
val pw = result[prevIdx]!!
|
||||
val nw = result[nextIdx]!!
|
||||
val ant = ((pw.first + nw.first) / 2.0 + 0.5).toInt()
|
||||
val post = ((pw.second + nw.second) / 2.0 + 0.5).toInt()
|
||||
result[prevIdx + 1] = Pair(ant, post)
|
||||
continue
|
||||
}
|
||||
// gap >= 2: drop top group
|
||||
for (i in 0 until nextIdx) result[i] = null
|
||||
return result
|
||||
}
|
||||
return result
|
||||
}
|
||||
|
||||
// ── LR Ratio Computation (x-z ellipse fitting, #21 merge) ──
|
||||
|
||||
/**
|
||||
* x-z 평면 타원 피팅 기반 LR/AP ratio.
|
||||
* CH4/CH5의 SI 높이에서 center(CH1,CH2) 벽 좌표를 보간하고,
|
||||
* lateral 벽 좌표와 합쳐 6개 경계점으로 x-z 평면 타원 피팅.
|
||||
* lr_ratio = LR 반축 / AP 반축.
|
||||
*/
|
||||
fun computeLrRatio(
|
||||
centerWalls: List<Pair<Int, Int>?>,
|
||||
lateralWalls: List<Pair<Int, Int>?>,
|
||||
maxRatio: Double = 1.0,
|
||||
sensorZMm: DoubleArray = PiezoHW.sensorZMmAll
|
||||
): Double {
|
||||
val hw = PiezoHW
|
||||
val neighbors = hw.lateralNeighbors
|
||||
val availableNbrs = neighbors.filter { ni ->
|
||||
ni < centerWalls.size && centerWalls[ni] != null
|
||||
}
|
||||
if (availableNbrs.size < 2) return hw.lrRatioNoDetection
|
||||
|
||||
// Lateral wall points (x-z plane)
|
||||
val latPtsX = mutableListOf<Double>()
|
||||
val latPtsZ = mutableListOf<Double>()
|
||||
val latSiPositions = mutableListOf<Double>()
|
||||
val latChords = mutableListOf<Double>()
|
||||
|
||||
for ((li, lCh) in hw.lateralCh.withIndex()) {
|
||||
if (li >= lateralWalls.size) continue
|
||||
val w = lateralWalls[li] ?: continue
|
||||
val (dNear, dFar) = segmentToDistancesMm(w.first, w.second)
|
||||
val alpha = hw.degreeAll[lCh] * Math.PI / 180.0
|
||||
val beta = hw.degreeLRAll[lCh] * Math.PI / 180.0
|
||||
val sx = hw.sensorXMmAll[lCh]
|
||||
for (d in listOf(dNear, dFar)) {
|
||||
latPtsX.add(sx + d * cos(alpha) * sin(beta))
|
||||
latPtsZ.add(d * cos(alpha) * cos(beta))
|
||||
}
|
||||
latSiPositions.add(sensorZMm[lCh])
|
||||
latChords.add(abs(dFar - dNear))
|
||||
}
|
||||
if (latPtsX.isEmpty()) return hw.lrRatioNoDetection
|
||||
val targetSi = latSiPositions.average()
|
||||
|
||||
// Center wall z-coords at neighbor channels, interpolated to lateral SI height
|
||||
val nbrSi = mutableListOf<Double>()
|
||||
val nbrZAnt = mutableListOf<Double>()
|
||||
val nbrZPost = mutableListOf<Double>()
|
||||
for (ni in availableNbrs) {
|
||||
val w = centerWalls[ni]!!
|
||||
val ch = hw.centerCh[ni]
|
||||
val (dNear, dFar) = segmentToDistancesMm(w.first, w.second)
|
||||
val th = hw.degreeAll[ch] * Math.PI / 180.0
|
||||
nbrSi.add(sensorZMm[ch])
|
||||
nbrZAnt.add(dNear * cos(th))
|
||||
nbrZPost.add(dFar * cos(th))
|
||||
}
|
||||
|
||||
val zAntCenter: Double
|
||||
val zPostCenter: Double
|
||||
if (nbrSi.size >= 2) {
|
||||
val w = if (abs(nbrSi[0] - nbrSi[1]) > 1e-6)
|
||||
((targetSi - nbrSi[1]) / (nbrSi[0] - nbrSi[1])).coerceIn(0.0, 1.0) else 0.5
|
||||
zAntCenter = nbrZAnt[1] + w * (nbrZAnt[0] - nbrZAnt[1])
|
||||
zPostCenter = nbrZPost[1] + w * (nbrZPost[0] - nbrZPost[1])
|
||||
} else {
|
||||
zAntCenter = nbrZAnt[0]
|
||||
zPostCenter = nbrZPost[0]
|
||||
}
|
||||
|
||||
// 6 boundary points: 2 center (x=0) + 4 lateral
|
||||
val xw = (listOf(0.0, 0.0) + latPtsX).toDoubleArray()
|
||||
val zw = (listOf(zAntCenter, zPostCenter) + latPtsZ).toDoubleArray()
|
||||
|
||||
// Normalized ellipse fit: α·x̂² + β·ẑ² + γ·x̂ + δ·ẑ = 1
|
||||
val xm = xw.average(); val xs = std(xw) + 1e-12
|
||||
val zm = zw.average(); val zs = std(zw) + 1e-12
|
||||
val xn = DoubleArray(xw.size) { (xw[it] - xm) / xs }
|
||||
val zn = DoubleArray(zw.size) { (zw[it] - zm) / zs }
|
||||
|
||||
val sol = solveEllipseLSQ(xn, zn, xw.size) ?: return hw.lrRatioInvalid
|
||||
val (alphaF, betaF, _, _) = sol.let { Triple(it[0], it[1], Pair(it[2], it[3])) }
|
||||
.let { doubleArrayOf(sol[0], sol[1], sol[2], sol[3]) }
|
||||
|
||||
if (sol[0] <= 1e-12 || sol[1] <= 1e-12) return hw.lrRatioInvalid
|
||||
val rVal = 1.0 + sol[2] * sol[2] / (4.0 * sol[0]) + sol[3] * sol[3] / (4.0 * sol[1])
|
||||
if (rVal <= 0) return hw.lrRatioInvalid
|
||||
|
||||
val bLr = sqrt(rVal / sol[0]) * xs // LR 반축 (mm)
|
||||
val aAp = sqrt(rVal / sol[1]) * zs // AP 반축 (mm)
|
||||
if (aAp <= 0) return hw.lrRatioInvalid
|
||||
|
||||
val lrRaw = max(bLr / aAp, 1.0)
|
||||
|
||||
// Shrinkage toward prior
|
||||
val dCenterMean = abs(zPostCenter - zAntCenter)
|
||||
if (dCenterMean <= 0) return hw.lrRatioInvalid
|
||||
val avgRatio = if (latChords.isNotEmpty()) latChords.average() / dCenterMean else 1.0
|
||||
val confidence = ((1.0 - avgRatio) / 0.10).coerceIn(0.0, 1.0)
|
||||
val result = hw.lrPrior + (lrRaw - hw.lrPrior) * confidence
|
||||
return result.coerceAtLeast(1.0)
|
||||
}
|
||||
|
||||
// ── Result ──
|
||||
|
||||
data class BVResult(
|
||||
val volumeMl: Double,
|
||||
val volumeMm3: Double,
|
||||
|
||||
// Per-channel (입력 순서, valid만)
|
||||
val validChannels: List<Int>,
|
||||
val dAntMm: DoubleArray,
|
||||
val dPostMm: DoubleArray,
|
||||
val dMm: DoubleArray, // 단면 직경 (cos 보정)
|
||||
val sMm2: DoubleArray, // 단면적
|
||||
|
||||
// Sorted by y (bottom → top)
|
||||
val sortedChannels: List<Int>,
|
||||
val sortedYMm: DoubleArray,
|
||||
val sortedSMm2: DoubleArray,
|
||||
|
||||
// Frustum / caps
|
||||
val vFrustumMm3: DoubleArray,
|
||||
val vCoreMm3: Double,
|
||||
val vBottomMm3: Double,
|
||||
val vTopMm3: Double,
|
||||
val bottomHMm: Double,
|
||||
val topHMm: Double,
|
||||
val bottomKind: String,
|
||||
val topKind: String,
|
||||
|
||||
// Parameters used
|
||||
val lrRatio: Double,
|
||||
val distancePerSample: Double,
|
||||
val delayOffsetMm: Double
|
||||
) {
|
||||
override fun equals(other: Any?): Boolean {
|
||||
if (this === other) return true
|
||||
if (other !is BVResult) return false
|
||||
return volumeMl == other.volumeMl && volumeMm3 == other.volumeMm3
|
||||
&& validChannels == other.validChannels
|
||||
}
|
||||
|
||||
override fun hashCode(): Int = 31 * volumeMl.hashCode() + volumeMm3.hashCode()
|
||||
}
|
||||
|
||||
// ── Helpers ──
|
||||
|
||||
/** Sample index → distance (mm) */
|
||||
private fun sampleToMm(
|
||||
idx: Double,
|
||||
dps: Double = PiezoHW.distancePerSample,
|
||||
offset: Double = PiezoHW.delayOffsetMm
|
||||
): Double = offset + idx * dps
|
||||
|
||||
/** (ant, post) sample indices → (d_near, d_far) mm */
|
||||
private fun segmentToDistancesMm(
|
||||
ant: Int, post: Int,
|
||||
dps: Double = PiezoHW.distancePerSample,
|
||||
offset: Double = PiezoHW.delayOffsetMm
|
||||
): Pair<Double, Double> {
|
||||
val d1 = sampleToMm(ant.toDouble(), dps, offset)
|
||||
val d2 = sampleToMm(post.toDouble(), dps, offset)
|
||||
return Pair(min(d1, d2), max(d1, d2))
|
||||
}
|
||||
|
||||
/** 4×4 least squares: M^T M x = M^T 1 */
|
||||
private fun solveEllipseLSQ(px: DoubleArray, py: DoubleArray, n: Int): DoubleArray? {
|
||||
// Build 4×4 normal equations: (M^T M) params = M^T ones
|
||||
// M columns: [x², y², x, y]
|
||||
val mtm = Array(4) { DoubleArray(4) }
|
||||
val mtb = DoubleArray(4)
|
||||
|
||||
for (k in 0 until n) {
|
||||
val x = px[k]; val y = py[k]
|
||||
val row = doubleArrayOf(x * x, y * y, x, y)
|
||||
for (i in 0 until 4) {
|
||||
for (j in 0 until 4) mtm[i][j] += row[i] * row[j]
|
||||
mtb[i] += row[i] // RHS = 1
|
||||
}
|
||||
}
|
||||
return solve4x4(mtm, mtb)
|
||||
}
|
||||
|
||||
private fun solve4x4(A: Array<DoubleArray>, b: DoubleArray): DoubleArray? {
|
||||
val a = Array(4) { A[it].copyOf() }
|
||||
val bb = b.copyOf()
|
||||
for (col in 0 until 4) {
|
||||
var maxRow = col; var maxVal = abs(a[col][col])
|
||||
for (row in (col + 1) until 4) {
|
||||
if (abs(a[row][col]) > maxVal) { maxVal = abs(a[row][col]); maxRow = row }
|
||||
}
|
||||
if (maxVal < 1e-12) return null
|
||||
if (maxRow != col) {
|
||||
val tmpA = a[col]; a[col] = a[maxRow]; a[maxRow] = tmpA
|
||||
val tmpB = bb[col]; bb[col] = bb[maxRow]; bb[maxRow] = tmpB
|
||||
}
|
||||
for (row in (col + 1) until 4) {
|
||||
val factor = a[row][col] / a[col][col]
|
||||
for (j in col until 4) a[row][j] -= factor * a[col][j]
|
||||
bb[row] -= factor * bb[col]
|
||||
}
|
||||
}
|
||||
val x = DoubleArray(4)
|
||||
for (i in 3 downTo 0) {
|
||||
var sum = bb[i]
|
||||
for (j in (i + 1) until 4) sum -= a[i][j] * x[j]
|
||||
if (abs(a[i][i]) < 1e-12) return null
|
||||
x[i] = sum / a[i][i]
|
||||
}
|
||||
return x
|
||||
}
|
||||
|
||||
// ── NEW BV lib (piezophantomtest 4830e7c) helpers ─────────────────────────
|
||||
|
||||
/** posterior arc chord-대비 최대 수직 이탈 (sagitta, mm). Python `_sagitta_mm`. */
|
||||
private fun sagittaMm(yp: DoubleArray, zp: DoubleArray): Double {
|
||||
if (yp.size < 3) return 0.0
|
||||
val order = (0 until yp.size).sortedBy { yp[it] }
|
||||
val y = DoubleArray(yp.size) { yp[order[it]] }
|
||||
val z = DoubleArray(zp.size) { zp[order[it]] }
|
||||
val p0y = y[0]; val p0z = z[0]
|
||||
val dY = y[y.size - 1] - p0y
|
||||
val dZ = z[z.size - 1] - p0z
|
||||
val ln = hypot(dY, dZ)
|
||||
if (ln < 1e-6) return 0.0
|
||||
val nY = -dZ / ln; val nZ = dY / ln
|
||||
var m = 0.0
|
||||
for (i in 1 until y.size - 1) {
|
||||
val d = abs((y[i] - p0y) * nY + (z[i] - p0z) * nZ)
|
||||
if (d > m) m = d
|
||||
}
|
||||
return m
|
||||
}
|
||||
|
||||
/**
|
||||
* posterior arc 원 fit (Kåsa algebraic) + sagitta shrinkage → R_eff.
|
||||
* Python `_shrink_si_radius`. 실패 시 null.
|
||||
*/
|
||||
private fun shrinkSiRadius(
|
||||
yWallPost: DoubleArray, zWallPost: DoubleArray,
|
||||
cApPrior: Double?,
|
||||
sNoise: Double = BOTTOM_CAP_SAGITTA_NOISE_MM,
|
||||
): Double? {
|
||||
val n = yWallPost.size
|
||||
if (n < 3 || cApPrior == null || cApPrior <= 0) return null
|
||||
val ata = Array(3) { DoubleArray(3) }
|
||||
val atb = DoubleArray(3)
|
||||
for (i in 0 until n) {
|
||||
val row = doubleArrayOf(yWallPost[i], zWallPost[i], 1.0)
|
||||
val r = yWallPost[i] * yWallPost[i] + zWallPost[i] * zWallPost[i]
|
||||
for (a in 0 until 3) {
|
||||
for (b in 0 until 3) ata[a][b] += row[a] * row[b]
|
||||
atb[a] += row[a] * r
|
||||
}
|
||||
}
|
||||
val sol = solve3x3(ata, atb) ?: return null
|
||||
val yc = sol[0] / 2.0
|
||||
val zc = sol[1] / 2.0
|
||||
val rFit = sqrt(max(sol[2] + yc * yc + zc * zc, 1e-9))
|
||||
val s = sagittaMm(yWallPost, zWallPost)
|
||||
val w = (s * s) / (s * s + sNoise * sNoise)
|
||||
val kappa = w / rFit + (1.0 - w) / cApPrior
|
||||
return if (kappa > 1e-9) 1.0 / kappa else cApPrior
|
||||
}
|
||||
|
||||
/** R 구에서 base a 인 minor 구면 캡 높이. Python `_minor_cap_height`. */
|
||||
private fun minorCapHeight(rEff: Double, aBase: Double): Double {
|
||||
val a = min(aBase, rEff)
|
||||
return rEff - sqrt(max(rEff * rEff - a * a, 0.0))
|
||||
}
|
||||
|
||||
/** 구면 캡 부피 (lr 보정). V = π·h²·(3R−h)/3 · lr. Python `_spherical_cap_volume`. */
|
||||
private fun sphericalCapVolume(rEff: Double, h: Double, lrRatio: Double): Double =
|
||||
Math.PI * h * h * (3.0 * rEff - h) / 3.0 * lrRatio
|
||||
|
||||
/**
|
||||
* 벽 인셋: (ant + f·(ls-ant), post - f·(post-le)) → 벽을 lumen 내부로 살짝 밀어 넣기.
|
||||
* Python `_apply_lumen_inset_one`. frac=0 이면 (ant, post) 그대로.
|
||||
* ls, le 없으면 (ant, post) 그대로.
|
||||
*/
|
||||
fun applyLumenInsetOne(
|
||||
ant: Int, post: Int, ls: Int?, le: Int?, frac: Double = LUMEN_INSET_FRAC,
|
||||
): Pair<Int, Int> {
|
||||
if (frac <= 0 || ls == null || le == null) return Pair(ant, post)
|
||||
val ai = kotlin.math.round(ant + frac * (ls - ant)).toInt()
|
||||
val pi = kotlin.math.round(post - frac * (post - le)).toInt()
|
||||
return Pair(ai, pi)
|
||||
}
|
||||
|
||||
/** Python `BOTTOM_CAP_SAGITTA_NOISE_MM` (bv_estimation.py:895). */
|
||||
const val BOTTOM_CAP_SAGITTA_NOISE_MM: Double = 4.0
|
||||
|
||||
/** Python `LUMEN_INSET_FRAC` (config_6ch.py:41). */
|
||||
const val LUMEN_INSET_FRAC: Double = 0.15
|
||||
|
||||
// ── Core BV Computation ──
|
||||
|
||||
/**
|
||||
* Traditional frustum BV 추정 (cos 보정 + SI 축 기반)
|
||||
* 1:1 port of _bv_core(mode="traditional")
|
||||
*/
|
||||
/**
|
||||
* 6ch BV 추정 — center 4채널(CH0~CH3) + lateral 2채널(CH4/CH5)에서 lr_ratio 계산
|
||||
* config_6ch.py / bv_estimation.py estimate_bladder_volume_6ch 1:1 포팅
|
||||
*/
|
||||
fun estimateBladderVolume6ch(
|
||||
allWalls: List<Pair<Int, Int>?>, // 6채널 전체 (ant, post), null = invalid
|
||||
): BVResult? {
|
||||
var centerWalls = PiezoHW.centerCh.map { if (it < allWalls.size) allWalls[it] else null }
|
||||
centerWalls = repairCenterWallsFor6ch(centerWalls)
|
||||
val lateralWalls = PiezoHW.lateralCh.map { if (it < allWalls.size) allWalls[it] else null }
|
||||
// 2026-07-01: algorithm 팀 최신 표준 (piezophantomtest 4830e7c) — lr_ratio_override=1.0
|
||||
// Phantom 검증: computed lr 은 6채널 HW 한계로 -38% 오차. Dev panel 로 전환 가능.
|
||||
val lrRatio = com.medithings.vesiscan.managers.GreenZoneConstants.lrRatioOverride
|
||||
?: computeLrRatio(centerWalls, lateralWalls, sensorZMm = PiezoHW.sensorZMmAll)
|
||||
return estimateBladderVolume(
|
||||
walls = centerWalls,
|
||||
lrRatio = lrRatio,
|
||||
edgeCh = 3,
|
||||
edgeRefChannels = listOf(1, 2)
|
||||
)
|
||||
}
|
||||
|
||||
fun estimateBladderVolume(
|
||||
walls: List<Pair<Int, Int>?>,
|
||||
distancePerSample: Double = PiezoHW.distancePerSample,
|
||||
delayOffsetMm: Double = PiezoHW.delayOffsetMm,
|
||||
sensorZMm: DoubleArray = PiezoHW.sensorZMm,
|
||||
degreeDeg: DoubleArray = PiezoHW.degree,
|
||||
areaK: Double = PiezoHW.areaK,
|
||||
lrRatio: Double = PiezoHW.defaultLrRatio,
|
||||
applyAngleCorrection: Boolean = true,
|
||||
edgeCh: Int = walls.size - 1,
|
||||
edgeRefChannels: List<Int> = listOf(walls.size - 2, walls.size - 3)
|
||||
): BVResult? {
|
||||
|
||||
// 1) Edge channel filter — cap 방식 결정용 플래그
|
||||
var edgeIsShort = false
|
||||
if (edgeCh < walls.size) {
|
||||
val wEdge = walls[edgeCh]
|
||||
if (wEdge != null) {
|
||||
val lEdge = wEdge.second - wEdge.first
|
||||
val refLens = mutableListOf<Int>()
|
||||
for (ci in edgeRefChannels) {
|
||||
if (ci in walls.indices) {
|
||||
val w = walls[ci]
|
||||
if (w != null) refLens.add(w.second - w.first)
|
||||
}
|
||||
}
|
||||
val minRef = refLens.minOrNull()
|
||||
if (minRef != null && lEdge.toDouble() < 0.9 * minRef.toDouble()) {
|
||||
edgeIsShort = true
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 2) 유효 채널 추출 + sample → mm
|
||||
val validChannels = mutableListOf<Int>()
|
||||
val dAnt = mutableListOf<Double>()
|
||||
val dPost = mutableListOf<Double>()
|
||||
|
||||
for ((i, w) in walls.withIndex()) {
|
||||
if (w == null) continue
|
||||
validChannels.add(i)
|
||||
val (dNear, dFar) = segmentToDistancesMm(w.first, w.second, distancePerSample, delayOffsetMm)
|
||||
dAnt.add(dNear)
|
||||
dPost.add(dFar)
|
||||
}
|
||||
|
||||
// 1채널 케이스: 구(sphere) 부피 (Python _single_channel_bv)
|
||||
if (validChannels.size < 2) {
|
||||
if (validChannels.size == 1) {
|
||||
val ch = validChannels[0]
|
||||
val theta = degreeDeg[ch] * Math.PI / 180.0
|
||||
val dRaw = dPost[0] - dAnt[0]
|
||||
val D = if (applyAngleCorrection) dRaw * cos(theta) else dRaw
|
||||
val R = D / 2.0
|
||||
val bvMm3 = (4.0 / 3.0) * Math.PI * R * R * R
|
||||
val S = areaK * D * D * lrRatio
|
||||
val yMid = sensorZMm[ch] + (dAnt[0] + dPost[0]) / 2.0 * sin(theta)
|
||||
return BVResult(
|
||||
volumeMl = bvMm3 / 1000.0, volumeMm3 = bvMm3,
|
||||
validChannels = validChannels, dAntMm = dAnt.toDoubleArray(), dPostMm = dPost.toDoubleArray(),
|
||||
dMm = doubleArrayOf(D), sMm2 = doubleArrayOf(S),
|
||||
sortedChannels = listOf(ch), sortedYMm = doubleArrayOf(yMid), sortedSMm2 = doubleArrayOf(S),
|
||||
vFrustumMm3 = doubleArrayOf(), vCoreMm3 = 0.0,
|
||||
vBottomMm3 = bvMm3, vTopMm3 = 0.0,
|
||||
bottomHMm = R, topHMm = 0.0,
|
||||
bottomKind = "sphere", topKind = "sphere",
|
||||
lrRatio = lrRatio, distancePerSample = distancePerSample, delayOffsetMm = delayOffsetMm
|
||||
)
|
||||
}
|
||||
return null
|
||||
}
|
||||
var n = validChannels.size
|
||||
|
||||
// 2-1) Post median outlier 제거 (#21 merge)
|
||||
if (n >= 3) {
|
||||
val posts = dPost.toDoubleArray()
|
||||
val med = posts.sorted()[posts.size / 2]
|
||||
val postTol = max(med * 0.25, 5.0 * distancePerSample)
|
||||
val keep = (0 until n).filter { abs(posts[it] - med) <= postTol }
|
||||
if (keep.size >= 2 && keep.size < n) {
|
||||
val newValid = keep.map { validChannels[it] }.toMutableList()
|
||||
val newDAnt = keep.map { dAnt[it] }.toMutableList()
|
||||
val newDPost = keep.map { dPost[it] }.toMutableList()
|
||||
validChannels.clear(); validChannels.addAll(newValid)
|
||||
dAnt.clear(); dAnt.addAll(newDAnt)
|
||||
dPost.clear(); dPost.addAll(newDPost)
|
||||
n = validChannels.size
|
||||
}
|
||||
}
|
||||
|
||||
// theta, sensor_z for valid channels
|
||||
val theta = DoubleArray(n) { degreeDeg[validChannels[it]] * Math.PI / 180.0 }
|
||||
val sensZ = DoubleArray(n) { sensorZMm[validChannels[it]] }
|
||||
|
||||
// 3) 단면 직경 (traditional: cos 보정)
|
||||
val lRaw = DoubleArray(n) { dPost[it] - dAnt[it] }
|
||||
val D = DoubleArray(n) { if (applyAngleCorrection) lRaw[it] * cos(theta[it]) else lRaw[it] }
|
||||
|
||||
// 4) 단면적 + AP 반지름
|
||||
val S = DoubleArray(n) { areaK * D[it] * D[it] * lrRatio }
|
||||
val aCap = DoubleArray(n) { D[it] / 2.0 }
|
||||
|
||||
// 5) Midpoint + 벽 좌표 (y-z 평면, 타원 피팅용)
|
||||
val dMid = DoubleArray(n) { (dAnt[it] + dPost[it]) / 2.0 }
|
||||
val y = DoubleArray(n) { sensZ[it] + dMid[it] * sin(theta[it]) }
|
||||
val yWallAnt = DoubleArray(n) { sensZ[it] + dAnt[it] * sin(theta[it]) }
|
||||
val zWallAnt = DoubleArray(n) { dAnt[it] * cos(theta[it]) }
|
||||
val yWallPost = DoubleArray(n) { sensZ[it] + dPost[it] * sin(theta[it]) }
|
||||
val zWallPost = DoubleArray(n) { dPost[it] * cos(theta[it]) }
|
||||
|
||||
// 6) y 오름차순 정렬
|
||||
val order = (0 until n).sortedBy { y[it] }
|
||||
val yS = DoubleArray(order.size) { y[order[it]] }
|
||||
val sS = DoubleArray(order.size) { S[order[it]] }
|
||||
val aCapS = DoubleArray(order.size) { aCap[order[it]] }
|
||||
val sortedCh = order.map { validChannels[it] }
|
||||
|
||||
// 7) y-z 평면 타원 피팅 + 반복 outlier 제거 (#22 merge)
|
||||
val allYw = DoubleArray(2 * n) { i -> if (i < n) yWallAnt[i] else yWallPost[i - n] }
|
||||
val allZw = DoubleArray(2 * n) { i -> if (i < n) zWallAnt[i] else zWallPost[i - n] }
|
||||
val nPts = allYw.size
|
||||
var capKind = "fallback"
|
||||
var z0Ellipse: Double? = null
|
||||
var cApPrior: Double? = null // shrink_bottom_cap 용 AP 반축
|
||||
var hCapBot = aCapS[0]
|
||||
var hCapTop = aCapS[n - 1]
|
||||
val ellipseCostThr = 0.5 // 점당 평균 잔차 임계
|
||||
|
||||
// 타원 피팅 helper: 성공 시 (y0, z0, bSi, aAp, residuals) 반환
|
||||
fun fitEllipsePts(ywF: DoubleArray, zwF: DoubleArray): Array<Any>? {
|
||||
val cnt = ywF.size
|
||||
val ym = ywF.average(); val ysS = std(ywF) + 1e-12
|
||||
val zm = zwF.average(); val zsS = std(zwF) + 1e-12
|
||||
val yn = DoubleArray(cnt) { (ywF[it] - ym) / ysS }
|
||||
val zn = DoubleArray(cnt) { (zwF[it] - zm) / zsS }
|
||||
val sol = solveEllipseLSQ(yn, zn, cnt) ?: return null
|
||||
if (sol[0] <= 1e-12 || sol[1] <= 1e-12) return null
|
||||
val rr = 1.0 + sol[2] * sol[2] / (4.0 * sol[0]) + sol[3] * sol[3] / (4.0 * sol[1])
|
||||
if (rr <= 0) return null
|
||||
val y0 = (-sol[2] / (2.0 * sol[0])) * ysS + ym
|
||||
val z0 = (-sol[3] / (2.0 * sol[1])) * zsS + zm
|
||||
val bSi = sqrt(rr / sol[0]) * ysS
|
||||
val aAp = sqrt(rr / sol[1]) * zsS
|
||||
val resid = DoubleArray(cnt) {
|
||||
abs(((ywF[it] - y0) / bSi) * ((ywF[it] - y0) / bSi) +
|
||||
((zwF[it] - z0) / aAp) * ((zwF[it] - z0) / aAp) - 1.0)
|
||||
}
|
||||
return arrayOf(y0, z0, bSi, aAp, resid)
|
||||
}
|
||||
|
||||
if (nPts >= 5) {
|
||||
val keep = BooleanArray(nPts) { true }
|
||||
var fit = fitEllipsePts(allYw, allZw)
|
||||
|
||||
if (fit != null) {
|
||||
var y0 = fit[0] as Double; var z0 = fit[1] as Double
|
||||
var bSi = fit[2] as Double; var aAp = fit[3] as Double
|
||||
var resid = fit[4] as DoubleArray
|
||||
var meanRes = resid.average()
|
||||
|
||||
// 반복 outlier 제거: worst 점 하나씩, 최소 5점 유지
|
||||
while (meanRes > ellipseCostThr && keep.count { it } > 5) {
|
||||
val worstLocal = resid.indices.maxByOrNull { resid[it] } ?: break
|
||||
val activeIndices = keep.indices.filter { keep[it] }
|
||||
keep[activeIndices[worstLocal]] = false
|
||||
val keptY = keep.indices.filter { keep[it] }.map { allYw[it] }.toDoubleArray()
|
||||
val keptZ = keep.indices.filter { keep[it] }.map { allZw[it] }.toDoubleArray()
|
||||
val fit2 = fitEllipsePts(keptY, keptZ) ?: break
|
||||
val meanRes2 = (fit2[4] as DoubleArray).average()
|
||||
if (meanRes2 < meanRes) {
|
||||
y0 = fit2[0] as Double; z0 = fit2[1] as Double
|
||||
bSi = fit2[2] as Double; aAp = fit2[3] as Double
|
||||
resid = fit2[4] as DoubleArray; meanRes = meanRes2
|
||||
} else break
|
||||
}
|
||||
|
||||
// 품질 판정: 평균 잔차 ≤ threshold
|
||||
if (meanRes <= ellipseCostThr) {
|
||||
z0Ellipse = z0
|
||||
// b_si 상한: a_ap × 1.3 (해부학적 SI/AP 비율 제한)
|
||||
if (bSi > aAp * 1.3) bSi = aAp * 1.3
|
||||
hCapBot = max(0.0, yS[0] - (y0 - bSi))
|
||||
hCapTop = max(0.0, (y0 + bSi) - yS[n - 1])
|
||||
hCapBot = min(hCapBot, aCapS[0])
|
||||
hCapTop = min(hCapTop, aCapS[n - 1])
|
||||
cApPrior = aAp
|
||||
capKind = "ellipse"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// shrink_bottom_cap=true (Python default) — posterior arc 원 fit + sagitta shrink 로
|
||||
// R_eff 공유 곡률 산출. 성공 시 both caps 높이를 정규화 (자유 b_si 제거).
|
||||
val rEffCap: Double? =
|
||||
if (cApPrior != null) shrinkSiRadius(yWallPost, zWallPost, cApPrior) else null
|
||||
if (rEffCap != null) {
|
||||
hCapBot = minorCapHeight(rEffCap, aCapS[0])
|
||||
hCapTop = minorCapHeight(rEffCap, aCapS[n - 1])
|
||||
}
|
||||
|
||||
// Top cap 상한: 미검출 상위 채널의 빔 y 좌표로 제한
|
||||
val nTotalCh = sensorZMm.size
|
||||
val topSortedCh = sortedCh.last()
|
||||
if (n < nTotalCh && topSortedCh > 0) {
|
||||
val upperCh = topSortedCh - 1
|
||||
val upperTheta = degreeDeg[upperCh] * Math.PI / 180.0
|
||||
val yBeamUpper = if (z0Ellipse != null && abs(cos(upperTheta)) > 1e-6) {
|
||||
val dAtZ0 = z0Ellipse!! / cos(upperTheta)
|
||||
sensorZMm[upperCh] + dAtZ0 * sin(upperTheta)
|
||||
} else {
|
||||
sensorZMm[upperCh].toDouble()
|
||||
}
|
||||
val hTopLimit = yBeamUpper - yS[n - 1]
|
||||
if (hTopLimit > 0 && hCapTop > hTopLimit) hCapTop = hTopLimit
|
||||
}
|
||||
|
||||
// 8) Core frustum (traditional: h = dy)
|
||||
val dy = DoubleArray(n - 1) { yS[it + 1] - yS[it] }
|
||||
val vFrustum = DoubleArray(n - 1) {
|
||||
(dy[it] / 3.0) * (sS[it] + sS[it + 1] + sqrt(sS[it] * sS[it + 1]))
|
||||
}
|
||||
val vCore = vFrustum.sum()
|
||||
|
||||
// 9) Caps — Bottom: spherical cap (shrink 시 R_eff 공유), Top: cone
|
||||
val vBottom: Double
|
||||
val vTop: Double
|
||||
val bottomKind: String
|
||||
val topKind: String
|
||||
if (rEffCap != null) {
|
||||
vBottom = sphericalCapVolume(rEffCap, hCapBot, lrRatio)
|
||||
vTop = sS[n - 1] * hCapTop / 3.0
|
||||
bottomKind = "shrink sphere"
|
||||
topKind = "shrink cone"
|
||||
} else {
|
||||
// fallback (ellipse fit 실패): 기존 hemisphere 공식.
|
||||
vBottom = sS[0] * hCapBot / 2.0 + Math.PI * hCapBot * hCapBot * hCapBot / 6.0
|
||||
vTop = sS[n - 1] * hCapTop / 3.0
|
||||
bottomKind = "$capKind sphere"
|
||||
topKind = "$capKind cone"
|
||||
}
|
||||
|
||||
// 10) 합산
|
||||
val bvMm3 = vCore + vBottom + vTop
|
||||
|
||||
return BVResult(
|
||||
volumeMl = bvMm3 / 1000.0,
|
||||
volumeMm3 = bvMm3,
|
||||
validChannels = validChannels,
|
||||
dAntMm = dAnt.toDoubleArray(),
|
||||
dPostMm = dPost.toDoubleArray(),
|
||||
dMm = D,
|
||||
sMm2 = S,
|
||||
sortedChannels = sortedCh,
|
||||
sortedYMm = yS,
|
||||
sortedSMm2 = sS,
|
||||
vFrustumMm3 = vFrustum,
|
||||
vCoreMm3 = vCore,
|
||||
vBottomMm3 = vBottom,
|
||||
vTopMm3 = vTop,
|
||||
bottomHMm = hCapBot,
|
||||
topHMm = hCapTop,
|
||||
bottomKind = bottomKind,
|
||||
topKind = topKind,
|
||||
lrRatio = lrRatio,
|
||||
distancePerSample = distancePerSample,
|
||||
delayOffsetMm = delayOffsetMm
|
||||
)
|
||||
}
|
||||
|
||||
// ── Math Utilities ──
|
||||
|
||||
/**
|
||||
* Degree 2 polynomial fit: returns Triple(c2, c1, c0) where f(x) = c2*x² + c1*x + c0
|
||||
* Least squares via normal equations (Vandermonde)
|
||||
*/
|
||||
private fun polyfit2(x: DoubleArray, y: DoubleArray): Triple<Double, Double, Double> {
|
||||
val n = x.size
|
||||
if (n < 3) return Triple(0.0, 0.0, y.firstOrNull() ?: 0.0)
|
||||
|
||||
// Build normal equations for Ax = b where A is Vandermonde [x^0, x^1, x^2]
|
||||
val sx = DoubleArray(5)
|
||||
val sy = DoubleArray(3)
|
||||
for (i in 0 until n) {
|
||||
val xi = x[i]
|
||||
val yi = y[i]
|
||||
var xp = 1.0
|
||||
for (k in 0 until 5) {
|
||||
sx[k] += xp
|
||||
xp *= xi
|
||||
}
|
||||
sy[0] += yi
|
||||
sy[1] += yi * xi
|
||||
sy[2] += yi * xi * xi
|
||||
}
|
||||
|
||||
// 3x3 system
|
||||
val a = arrayOf(
|
||||
doubleArrayOf(sx[0], sx[1], sx[2]),
|
||||
doubleArrayOf(sx[1], sx[2], sx[3]),
|
||||
doubleArrayOf(sx[2], sx[3], sx[4])
|
||||
)
|
||||
val b = doubleArrayOf(sy[0], sy[1], sy[2])
|
||||
|
||||
val sol = solve3x3(a, b) ?: return Triple(0.0, 0.0, y.firstOrNull() ?: 0.0)
|
||||
return Triple(sol[2], sol[1], sol[0]) // (c2, c1, c0)
|
||||
}
|
||||
|
||||
/** Evaluate degree 2 polynomial: c2*x² + c1*x + c0 */
|
||||
private fun polyval2(c: Triple<Double, Double, Double>, x: Double): Double =
|
||||
c.first * x * x + c.second * x + c.third
|
||||
|
||||
/** Solve 3x3 linear system via Gaussian elimination with partial pivoting */
|
||||
private fun solve3x3(A: Array<DoubleArray>, b: DoubleArray): DoubleArray? {
|
||||
val a = Array(3) { A[it].copyOf() }
|
||||
val bb = b.copyOf()
|
||||
|
||||
for (col in 0 until 3) {
|
||||
// Partial pivoting
|
||||
var maxRow = col
|
||||
var maxVal = abs(a[col][col])
|
||||
for (row in (col + 1) until 3) {
|
||||
if (abs(a[row][col]) > maxVal) {
|
||||
maxVal = abs(a[row][col])
|
||||
maxRow = row
|
||||
}
|
||||
}
|
||||
if (maxVal < 1e-12) return null
|
||||
if (maxRow != col) {
|
||||
val tmpA = a[col]; a[col] = a[maxRow]; a[maxRow] = tmpA
|
||||
val tmpB = bb[col]; bb[col] = bb[maxRow]; bb[maxRow] = tmpB
|
||||
}
|
||||
|
||||
// Eliminate
|
||||
for (row in (col + 1) until 3) {
|
||||
val factor = a[row][col] / a[col][col]
|
||||
for (j in col until 3) a[row][j] -= factor * a[col][j]
|
||||
bb[row] -= factor * bb[col]
|
||||
}
|
||||
}
|
||||
|
||||
// Back substitution
|
||||
val x = DoubleArray(3)
|
||||
for (i in 2 downTo 0) {
|
||||
var sum = bb[i]
|
||||
for (j in (i + 1) until 3) sum -= a[i][j] * x[j]
|
||||
if (abs(a[i][i]) < 1e-12) return null
|
||||
x[i] = sum / a[i][i]
|
||||
}
|
||||
return x
|
||||
}
|
||||
|
||||
/** Standard deviation */
|
||||
private fun std(arr: DoubleArray): Double {
|
||||
val n = arr.size.toDouble()
|
||||
if (n <= 0) return 0.0
|
||||
val mean = arr.sum() / n
|
||||
val variance = arr.sumOf { (it - mean).let { d -> d * d } } / n
|
||||
return sqrt(variance)
|
||||
}
|
||||
@@ -0,0 +1,916 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import com.medithings.vesiscan.ble.PiezoChannelData
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.max
|
||||
import kotlin.math.min
|
||||
import kotlin.math.sqrt
|
||||
|
||||
// ── Result Types ──
|
||||
|
||||
/**
|
||||
* Low-echo 기반 urine region 탐지 결과 (원은지 B방식 포팅)
|
||||
*/
|
||||
data class LowEchoResult(
|
||||
val ant: Int, // 전벽 index
|
||||
val post: Int, // 후벽 index
|
||||
val lowStart: Int, // low-echo span 시작
|
||||
val lowEnd: Int, // low-echo span 끝 (inclusive)
|
||||
val lowMean: Double, // low-echo 구간 평균 진폭
|
||||
val urineLen: Int, // post - ant
|
||||
val score: Double, // low_depth * urine_len
|
||||
val innerPeaks: List<Int> // (ant, post) 내부 local peak indices
|
||||
) {
|
||||
/** 방광 직경 (mm) = urine_len × distancePerSample */
|
||||
val diameterMm: Double
|
||||
get() = urineLen.toDouble() * PiezoConstants.distancePerSample
|
||||
|
||||
/** 방광 용적 (ml) = K × (D_cm)³ (DrBench 공식, 팬텀 캘리브레이션) */
|
||||
val volumeMl: Double
|
||||
get() = PiezoConstants.volumeMl(diameterMm)
|
||||
|
||||
/** 전벽 깊이 (mm) */
|
||||
val antDepthMm: Double
|
||||
get() = PiezoConstants.distanceMm(ant)
|
||||
|
||||
/** 후벽 깊이 (mm) */
|
||||
val postDepthMm: Double
|
||||
get() = PiezoConstants.distanceMm(post)
|
||||
}
|
||||
|
||||
/**
|
||||
* 채널별 분석 결과
|
||||
*/
|
||||
data class ChannelAnalysisResult(
|
||||
val channel: Int,
|
||||
val result: LowEchoResult?,
|
||||
val rawSignal: DoubleArray,
|
||||
val denoisedSignal: DoubleArray
|
||||
) {
|
||||
val isValid: Boolean get() = result != null
|
||||
|
||||
override fun equals(other: Any?): Boolean {
|
||||
if (this === other) return true
|
||||
if (other !is ChannelAnalysisResult) return false
|
||||
return channel == other.channel && result == other.result
|
||||
}
|
||||
|
||||
override fun hashCode(): Int = 31 * channel + (result?.hashCode() ?: 0)
|
||||
}
|
||||
|
||||
/**
|
||||
* 전체 측정 분석 결과
|
||||
*/
|
||||
data class PiezoAnalysisResult(
|
||||
val channels: List<ChannelAnalysisResult>,
|
||||
val volumeMl: Double,
|
||||
val confidence: Double, // 0~100
|
||||
val validChannelCount: Int,
|
||||
val method: String // "single", "multi_avg", "multi_weighted"
|
||||
) {
|
||||
val bestChannel: ChannelAnalysisResult?
|
||||
get() = channels.filter { it.isValid }
|
||||
.maxByOrNull { it.result?.score ?: 0.0 }
|
||||
}
|
||||
|
||||
// ── PiezoConstants (referenced by LowEchoResult) ──
|
||||
|
||||
/**
|
||||
* Physical constants for Piezo ultrasound echo measurement.
|
||||
* Mirrors iOS PiezoConstants, delegates distancePerSample/delayOffsetMm to PiezoHW.
|
||||
*/
|
||||
object PiezoConstants {
|
||||
const val adcMaxValue: Int = 4095
|
||||
const val adcVrefMv: Double = 3300.0
|
||||
|
||||
const val sampleRateMhz: Double = 0.5375
|
||||
const val sampleIntervalUs: Double = 1.86
|
||||
const val soundSpeedMmUs: Double = 1.54
|
||||
|
||||
val distancePerSample: Double get() = PiezoHW.distancePerSample
|
||||
val delayOffsetMm: Double get() = PiezoHW.delayOffsetMm
|
||||
|
||||
const val volumeK: Double = 1.76
|
||||
|
||||
fun distanceMm(index: Int): Double =
|
||||
index.toDouble() * distancePerSample + delayOffsetMm
|
||||
|
||||
fun voltageMv(adcValue: Int): Double =
|
||||
adcValue.toDouble() / adcMaxValue.toDouble() * adcVrefMv
|
||||
|
||||
fun volumeMl(diameterMm: Double): Double {
|
||||
val dCm = diameterMm / 10.0
|
||||
return volumeK * dCm * dCm * dCm
|
||||
}
|
||||
}
|
||||
|
||||
// ── Analyzer ──
|
||||
|
||||
/**
|
||||
* Piezo 초음파 에코 분석기 — 원은지 연구원 B방식 알고리즘 포팅
|
||||
* Pipeline: Raw ADC → TVD Denoise → SG Smooth → Low-Echo Detection → Volume
|
||||
*/
|
||||
class PiezoEchoAnalyzer private constructor() {
|
||||
|
||||
companion object {
|
||||
val shared = PiezoEchoAnalyzer()
|
||||
}
|
||||
|
||||
// ── Parameters (노트북 검증값) ──
|
||||
|
||||
// TVD
|
||||
val tvdWeight: Double = 0.06
|
||||
val tvdMaxIter: Int = 200
|
||||
val tvdTol: Double = 2e-4
|
||||
val tvdTau: Double = 0.25
|
||||
|
||||
// Savitzky-Golay (window=5, poly=2 → 고정 계수)
|
||||
val sgCoeffs: DoubleArray = doubleArrayOf(-3.0/35.0, 12.0/35.0, 17.0/35.0, 12.0/35.0, -3.0/35.0)
|
||||
|
||||
// Low Echo Detection
|
||||
val lowEchoAmpDefault: Double get() = GreenZoneConstants.lowEchoAmp.toDouble()
|
||||
val lowMinLen: Int = 3
|
||||
val mergeGapMax: Int = 3
|
||||
val peakSearchWin: Int = 20
|
||||
val postMaxIdx: Int get() = GreenZoneConstants.postMaxIdx
|
||||
val minPeakMargin: Double = 30.0
|
||||
val minUrineLen: Int = 3
|
||||
val postRefineTolRatio: Double = 0.05
|
||||
val maxRealisticVolumeMl: Double = 1500.0
|
||||
val valleyStopRise: Double = 50.0
|
||||
val edgeDistDecay: Double = 0.12
|
||||
|
||||
// ── Public API ──
|
||||
|
||||
/** 단일 채널 raw ADC → 용적 분석 (crash-safe) */
|
||||
fun analyzeChannel(rawADC: List<UShort>, channel: Int = 0): ChannelAnalysisResult {
|
||||
val raw = DoubleArray(rawADC.size) { rawADC[it].toDouble() }
|
||||
if (raw.size < 10) {
|
||||
return ChannelAnalysisResult(channel = channel, result = null, rawSignal = raw, denoisedSignal = raw.copyOf())
|
||||
}
|
||||
return try {
|
||||
val denoised = denoise(raw)
|
||||
val result = detectLowEcho(raw = raw, denoised = denoised)
|
||||
ChannelAnalysisResult(channel = channel, result = result, rawSignal = raw, denoisedSignal = denoised)
|
||||
} catch (_: Exception) {
|
||||
ChannelAnalysisResult(channel = channel, result = null, rawSignal = raw, denoisedSignal = raw.copyOf())
|
||||
}
|
||||
}
|
||||
|
||||
/** 다채널 파이프라인 — Python pipeline.py detect_walls_multichannel 1:1 포팅
|
||||
* denoise → TGC(전체 채널 통합) → initial detect → BR consensus → resolve → suppress → rerun → choose
|
||||
*/
|
||||
fun analyzeMultiChannelWithTgc(channelData: List<PiezoChannelData>): List<ChannelAnalysisResult> {
|
||||
val raws = channelData.map { DoubleArray(it.buffer.size) { i -> it.buffer[i].toDouble() } }
|
||||
val denoised = raws.map { denoise(it) }
|
||||
|
||||
// TGC 전체 채널 통합 (Python preprocess_multichannel)
|
||||
val preprocessed = applyTgcMultichannel(denoised)
|
||||
|
||||
// 채널별 각도 보정 (Python _make_angle_kwargs_b)
|
||||
val beamAngles = PiezoHW.degreeAll
|
||||
fun channelOtsuRatio(chIdx: Int): Double {
|
||||
if (chIdx >= beamAngles.size) return otsuRatio
|
||||
return otsuRatio * kotlin.math.cos(beamAngles[chIdx] * Math.PI / 180.0)
|
||||
}
|
||||
|
||||
// 1) Initial detect + BR candidate
|
||||
val initialResults = mutableListOf<LowEchoResult?>()
|
||||
val brCandidates = mutableListOf<Int?>()
|
||||
for (i in channelData.indices) {
|
||||
val r = detectLowEcho(raw = raws[i], denoised = preprocessed[i], otsuRatioOverride = channelOtsuRatio(channelData[i].channel))
|
||||
initialResults.add(r)
|
||||
brCandidates.add(if (r != null) BackReflection.buildCandidate(preprocessed[i], r.post, postMaxIdx) else null)
|
||||
}
|
||||
|
||||
// 2) Consensus + reflection_only
|
||||
val sharedBrIdx = BackReflection.consensusIdx(brCandidates)
|
||||
val wallPairs = initialResults.map { r -> if (r != null) Pair<Int?, Int?>(r.ant, r.post) else Pair(null, null) }
|
||||
val reflectionOnly = BackReflection.isReflectionOnlyMultichannel(preprocessed, wallPairs, sharedIdx = sharedBrIdx)
|
||||
|
||||
// 3) Per-channel: resolve → suppress → rerun → choose
|
||||
return channelData.mapIndexed { i, ch ->
|
||||
try {
|
||||
val r = initialResults[i]
|
||||
var suppressIdx: Int? = null
|
||||
|
||||
if (r == null) {
|
||||
if (reflectionOnly && sharedBrIdx != null) suppressIdx = sharedBrIdx
|
||||
else return@mapIndexed ChannelAnalysisResult(ch.channel, null, raws[i], preprocessed[i])
|
||||
} else {
|
||||
val resolved = BackReflection.resolveIndices(preprocessed[i], r.post, postMaxIdx, antIdx = r.ant, sharedIdx = sharedBrIdx, channelCandidate = brCandidates[i])
|
||||
suppressIdx = resolved.suppressIdx
|
||||
if (suppressIdx == null && reflectionOnly && sharedBrIdx != null) suppressIdx = sharedBrIdx
|
||||
}
|
||||
|
||||
if (suppressIdx == null) {
|
||||
return@mapIndexed ChannelAnalysisResult(ch.channel, r, raws[i], preprocessed[i])
|
||||
}
|
||||
|
||||
val cleaned = BackReflection.suppress(preprocessed[i], suppressIdx)
|
||||
val r2 = detectLowEcho(raw = raws[i], denoised = cleaned, otsuSignal = preprocessed[i], dynamicPostMax = suppressIdx, otsuRatioOverride = channelOtsuRatio(ch.channel))
|
||||
val chosen = BackReflection.chooseResult(r, r2, brIdx = BackReflection.selectIdx(preprocessed[i], r?.post ?: 0, postMaxIdx, antIdx = r?.ant, sharedIdx = sharedBrIdx, channelCandidate = brCandidates[i]), suppressIdx = suppressIdx, minGap = 8)
|
||||
ChannelAnalysisResult(ch.channel, chosen, raws[i], cleaned)
|
||||
} catch (_: Exception) {
|
||||
ChannelAnalysisResult(ch.channel, null, raws[i], preprocessed[i])
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/** TGC 전체 채널 통합 처리 (Python preprocess_multichannel) */
|
||||
fun applyTgcMultichannel(denoisedChannels: List<DoubleArray>): List<DoubleArray> {
|
||||
if (denoisedChannels.isEmpty()) return denoisedChannels
|
||||
val n = denoisedChannels[0].size
|
||||
val nCh = denoisedChannels.size
|
||||
|
||||
// Log 압축 → 채널별 slope 계산
|
||||
val slopes = DoubleArray(nCh)
|
||||
for (ch in 0 until nCh) {
|
||||
val logSig = DoubleArray(n) { kotlin.math.ln(max(denoisedChannels[ch][it], 1.0)) }
|
||||
val x = DoubleArray(n) { it.toDouble() }
|
||||
var sumX = 0.0; var sumY = 0.0; var sumXY = 0.0; var sumX2 = 0.0
|
||||
for (i in 0 until n) { sumX += x[i]; sumY += logSig[i]; sumXY += x[i] * logSig[i]; sumX2 += x[i] * x[i] }
|
||||
slopes[ch] = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX)
|
||||
}
|
||||
|
||||
// 채널별 보상 적용
|
||||
return denoisedChannels.mapIndexed { ch, signal ->
|
||||
val slope = slopes[ch]
|
||||
if (slope >= 0) return@mapIndexed signal.copyOf()
|
||||
val absSlope = abs(slope)
|
||||
val slopeThresh = 3.0; val slopeMax = 15.0; val ratioMin = 0.1
|
||||
val ratio = if (absSlope < slopeThresh) 1.0
|
||||
else max(ratioMin, 1.0 - (1.0 - ratioMin) * (absSlope - slopeThresh) / (slopeMax - slopeThresh))
|
||||
val targetSlope = slope * ratio
|
||||
val x = DoubleArray(n) { it.toDouble() }
|
||||
DoubleArray(n) { signal[it] + (targetSlope - slope) * x[it] }
|
||||
}
|
||||
}
|
||||
|
||||
/** 다채널 분석 → 최종 용적 */
|
||||
fun analyzeMultiChannel(channelData: List<PiezoChannelData>): PiezoAnalysisResult {
|
||||
val results = channelData.map { ch ->
|
||||
analyzeChannel(ch.buffer, ch.channel)
|
||||
}
|
||||
|
||||
// Step 1: Get valid results (algorithm found urine region)
|
||||
val validResults = results.filter { it.isValid }
|
||||
|
||||
// Step 2: Filter out unrealistic volumes (container wall artifacts)
|
||||
val realisticResults = validResults.filter { (it.result?.volumeMl ?: 0.0) <= maxRealisticVolumeMl }
|
||||
|
||||
// Step 3: Outlier removal — if 3+ results, remove > 2× median
|
||||
var filtered = realisticResults
|
||||
if (filtered.size >= 3) {
|
||||
val vols = filtered.mapNotNull { it.result?.volumeMl }.sorted()
|
||||
val median = vols[vols.size / 2]
|
||||
filtered = filtered.filter { cr ->
|
||||
val v = cr.result?.volumeMl ?: return@filter false
|
||||
v <= median * 2.5 && v >= median * 0.3
|
||||
}
|
||||
}
|
||||
|
||||
val usedCount = filtered.size
|
||||
val volumeMl: Double
|
||||
val method: String
|
||||
val confidence: Double
|
||||
|
||||
if (usedCount == 0) {
|
||||
volumeMl = 0.0
|
||||
method = "none"
|
||||
confidence = 0.0
|
||||
} else if (usedCount == 1) {
|
||||
volumeMl = filtered[0].result!!.volumeMl
|
||||
method = "single"
|
||||
confidence = min(100.0, filtered[0].result!!.score / 100.0)
|
||||
} else {
|
||||
// Median-based: use median volume (robust against remaining outliers)
|
||||
val vols = filtered.mapNotNull { it.result?.volumeMl }.sorted()
|
||||
val median = vols[vols.size / 2]
|
||||
volumeMl = median
|
||||
method = "median_${usedCount}ch"
|
||||
confidence = min(100.0, usedCount.toDouble() / results.size.toDouble() * 100.0)
|
||||
}
|
||||
|
||||
return PiezoAnalysisResult(
|
||||
channels = results,
|
||||
volumeMl = volumeMl,
|
||||
confidence = confidence,
|
||||
validChannelCount = usedCount,
|
||||
method = method
|
||||
)
|
||||
}
|
||||
|
||||
// ── Denoising Pipeline ──
|
||||
|
||||
var useSgFilter: Boolean = true
|
||||
|
||||
fun denoise(signal: DoubleArray): DoubleArray {
|
||||
return if (useSgFilter) sgSmooth(signal) else signal.copyOf()
|
||||
}
|
||||
|
||||
/** 1D Total Variation Denoising — Chambolle (2004) dual projected gradient */
|
||||
fun tvDenoiseChambolle1D(signal: DoubleArray): DoubleArray {
|
||||
val n = signal.size
|
||||
if (n < 2 || tvdWeight <= 0) return signal.copyOf()
|
||||
// Skip if signal is flat (all same values)
|
||||
val mn = signal.min()
|
||||
val mx = signal.max()
|
||||
if (mx - mn < 1.0) return signal.copyOf()
|
||||
|
||||
val p = DoubleArray(n - 1)
|
||||
var uPrev = signal.copyOf()
|
||||
val tauW = tvdTau / tvdWeight
|
||||
|
||||
for (iter in 0 until tvdMaxIter) {
|
||||
// u = x + weight * div(p)
|
||||
val divP = DoubleArray(n)
|
||||
divP[0] = p[0]
|
||||
for (i in 1 until (n - 1)) {
|
||||
divP[i] = p[i] - p[i - 1]
|
||||
}
|
||||
divP[n - 1] = -p[n - 2]
|
||||
|
||||
val u = DoubleArray(n) { signal[it] + tvdWeight * divP[it] }
|
||||
|
||||
// grad(u) = diff(u) and dual update
|
||||
var maxDiff = 0.0
|
||||
for (i in 0 until (n - 1)) {
|
||||
val gradU = u[i + 1] - u[i]
|
||||
p[i] = (p[i] + tauW * gradU) / (1.0 + tauW * abs(gradU))
|
||||
maxDiff = max(maxDiff, abs(u[i] - uPrev[i]))
|
||||
}
|
||||
maxDiff = max(maxDiff, abs(u[n - 1] - uPrev[n - 1]))
|
||||
|
||||
if (maxDiff < tvdTol) break
|
||||
uPrev = u
|
||||
}
|
||||
|
||||
// Final reconstruction
|
||||
val divP = DoubleArray(n)
|
||||
divP[0] = p[0]
|
||||
for (i in 1 until (n - 1)) {
|
||||
divP[i] = p[i] - p[i - 1]
|
||||
}
|
||||
divP[n - 1] = -p[n - 2]
|
||||
|
||||
return DoubleArray(n) { signal[it] + tvdWeight * divP[it] }
|
||||
}
|
||||
|
||||
/** Savitzky-Golay 5-tap FIR smoothing (window=5, poly=2) */
|
||||
fun sgSmooth(signal: DoubleArray): DoubleArray {
|
||||
val n = signal.size
|
||||
if (n < 5) return signal.copyOf()
|
||||
|
||||
val out = DoubleArray(n)
|
||||
|
||||
// Inner: convolution with fixed coefficients [-3, 12, 17, 12, -3]/35
|
||||
for (i in 2 until (n - 2)) {
|
||||
out[i] = sgCoeffs[0] * signal[i - 2] +
|
||||
sgCoeffs[1] * signal[i - 1] +
|
||||
sgCoeffs[2] * signal[i] +
|
||||
sgCoeffs[3] * signal[i + 1] +
|
||||
sgCoeffs[4] * signal[i + 2]
|
||||
}
|
||||
|
||||
// Edge: polynomial fit for first/last 2 samples
|
||||
// Left edge: fit poly2 to signal[0..4], evaluate at 0,1
|
||||
val leftCoeffs = polyFit2(signal.sliceArray(0 until 5))
|
||||
out[0] = evalPoly2(leftCoeffs, -2.0)
|
||||
out[1] = evalPoly2(leftCoeffs, -1.0)
|
||||
|
||||
// Right edge: fit poly2 to signal[n-5..n-1], evaluate at n-2,n-1
|
||||
val rightCoeffs = polyFit2(signal.sliceArray((n - 5) until n))
|
||||
out[n - 2] = evalPoly2(rightCoeffs, 1.0)
|
||||
out[n - 1] = evalPoly2(rightCoeffs, 2.0)
|
||||
|
||||
return out
|
||||
}
|
||||
|
||||
// Quadratic LS fit to 5 points centered at 0: x = [-2,-1,0,1,2]
|
||||
// Returns Triple(c0, c1, c2) where f(t) = c0 + c1*t + c2*t²
|
||||
private fun polyFit2(y: DoubleArray): Triple<Double, Double, Double> {
|
||||
val c0 = (-3*y[0] + 12*y[1] + 17*y[2] + 12*y[3] - 3*y[4]) / 35.0
|
||||
val c1 = (-2*y[0] - y[1] + y[3] + 2*y[4]) / 10.0
|
||||
val c2 = (2*y[0] - y[1] - 2*y[2] - y[3] + 2*y[4]) / 14.0
|
||||
return Triple(c0, c1, c2)
|
||||
}
|
||||
|
||||
private fun evalPoly2(c: Triple<Double, Double, Double>, t: Double): Double =
|
||||
c.first + c.second * t + c.third * t * t
|
||||
|
||||
// ── Low Echo Detection ──
|
||||
|
||||
var useAdaptiveThreshold: Boolean = true
|
||||
var lastOtsuThreshold: Double = 0.0; private set
|
||||
var lastOtsuSeparability: Double = 0.0; private set
|
||||
|
||||
val otsuRatio: Double = 0.9
|
||||
val minScore: Double = 3000.0
|
||||
|
||||
/** 단일 1D 채널 → urine region 탐지 (Otsu-only, Python 동기화) */
|
||||
fun detectLowEcho(raw: DoubleArray, denoised: DoubleArray, otsuSignal: DoubleArray? = null, dynamicPostMax: Int? = null, lowEchoAmpOverride: Double? = null, otsuRatioOverride: Double? = null): LowEchoResult? {
|
||||
val sg = denoised
|
||||
if (sg.size < 10) return null
|
||||
val effectivePostMax = dynamicPostMax ?: postMaxIdx
|
||||
|
||||
if (lowEchoAmpOverride != null) {
|
||||
lastOtsuThreshold = lowEchoAmpOverride
|
||||
return detectLowEchoCore(sg = sg, threshold = lowEchoAmpOverride, effectivePostMax = effectivePostMax)
|
||||
}
|
||||
|
||||
if (!useAdaptiveThreshold) {
|
||||
lastOtsuThreshold = lowEchoAmpDefault
|
||||
return detectLowEchoCore(sg = sg, threshold = lowEchoAmpDefault, effectivePostMax = effectivePostMax)
|
||||
}
|
||||
|
||||
// Otsu-only: 단일 시도 (Python detect_low_echo_method_b 동일)
|
||||
val effectiveRatio = otsuRatioOverride ?: otsuRatio
|
||||
val otsuInput = otsuSignal ?: sg
|
||||
val (thr, sep) = otsu1dWithSeparability(otsuInput, otsuInput.size)
|
||||
lastOtsuThreshold = thr * effectiveRatio
|
||||
lastOtsuSeparability = sep
|
||||
return detectLowEchoCore(sg = sg, threshold = lastOtsuThreshold, effectivePostMax = effectivePostMax)
|
||||
}
|
||||
|
||||
// ── TGC (Time Gain Compensation) ──
|
||||
|
||||
fun applyTgc(signal: DoubleArray): DoubleArray {
|
||||
val n = signal.size
|
||||
if (n < 5) return signal.copyOf()
|
||||
|
||||
// Log 압축 후 linear fit (Python fit_attenuation_lines 동일)
|
||||
val logSignal = DoubleArray(n) { kotlin.math.ln(max(signal[it], 1.0)) }
|
||||
val x = DoubleArray(n) { it.toDouble() }
|
||||
var sumX = 0.0; var sumY = 0.0; var sumXY = 0.0; var sumX2 = 0.0
|
||||
for (i in 0 until n) { sumX += x[i]; sumY += logSignal[i]; sumXY += x[i] * logSignal[i]; sumX2 += x[i] * x[i] }
|
||||
val slope = (n * sumXY - sumX * sumY) / (n * sumX2 - sumX * sumX)
|
||||
|
||||
if (slope >= 0) return signal.copyOf()
|
||||
|
||||
val absSlope = abs(slope)
|
||||
val slopeThresh = 3.0; val slopeMax = 15.0; val ratioMin = 0.1
|
||||
val ratio = if (absSlope < slopeThresh) 1.0
|
||||
else max(ratioMin, 1.0 - (1.0 - ratioMin) * (absSlope - slopeThresh) / (slopeMax - slopeThresh))
|
||||
|
||||
val targetSlope = slope * ratio
|
||||
val compensation = DoubleArray(n) { (targetSlope - slope) * x[it] }
|
||||
return DoubleArray(n) { signal[it] + compensation[it] }
|
||||
}
|
||||
|
||||
// ── Back Reflection Detection & Suppression ──
|
||||
|
||||
fun findBackReflectionIdx(tgcSg: DoubleArray, postMaxIdx: Int, minSearchIdx: Int = 40, extend: Int = 20): Int {
|
||||
val n = tgcSg.size
|
||||
val searchEnd = min(n, postMaxIdx + extend)
|
||||
if (minSearchIdx >= searchEnd) return postMaxIdx
|
||||
|
||||
val seg = tgcSg.sliceArray(minSearchIdx until searchEnd)
|
||||
val peaks = findPeaks1D(seg)
|
||||
if (peaks.isEmpty()) return postMaxIdx
|
||||
|
||||
val globalPeaks = peaks.map { it + minSearchIdx }
|
||||
return globalPeaks.maxByOrNull { tgcSg[it] } ?: postMaxIdx
|
||||
}
|
||||
|
||||
fun suppressBackReflection(sg: DoubleArray, backRefIdx: Int, searchMargin: Int = 10): DoubleArray {
|
||||
val cleaned = sg.copyOf()
|
||||
val n = sg.size
|
||||
if (backRefIdx >= n) return cleaned
|
||||
|
||||
val lo = max(0, backRefIdx - min(searchMargin, 3))
|
||||
val hi = min(n, backRefIdx + searchMargin + 1)
|
||||
var actualPeak = lo
|
||||
for (i in lo until hi) if (sg[i] > sg[actualPeak]) actualPeak = i
|
||||
|
||||
// left valley
|
||||
val valleyLoStart = max(0, actualPeak - searchMargin)
|
||||
var leftValley = valleyLoStart
|
||||
for (i in valleyLoStart..actualPeak) if (sg[i] < sg[leftValley]) leftValley = i
|
||||
|
||||
// right valley
|
||||
val valleyHiEnd = min(n, actualPeak + searchMargin + 1)
|
||||
var rightValley = actualPeak
|
||||
for (i in actualPeak until valleyHiEnd) if (sg[i] < sg[rightValley]) rightValley = i
|
||||
|
||||
if (rightValley > leftValley) {
|
||||
val leftVal = sg[leftValley]
|
||||
val rightVal = sg[rightValley]
|
||||
val length = rightValley - leftValley
|
||||
for (i in 0..length) {
|
||||
cleaned[leftValley + i] = leftVal + (rightVal - leftVal) * i.toDouble() / length
|
||||
}
|
||||
}
|
||||
return cleaned
|
||||
}
|
||||
|
||||
/** 다채널 후면 반사 위치 합의 (median) */
|
||||
fun findBackReflectionMultichannel(tgcSignals: List<DoubleArray>, postMaxIdx: Int): Int {
|
||||
val detected = mutableListOf<Int>()
|
||||
for (tgc in tgcSignals) {
|
||||
val idx = findBackReflectionIdx(tgc, postMaxIdx)
|
||||
if (idx < postMaxIdx) detected.add(idx)
|
||||
}
|
||||
if (detected.isEmpty()) return postMaxIdx
|
||||
detected.sort()
|
||||
return detected[detected.size / 2]
|
||||
}
|
||||
|
||||
/**
|
||||
* 1D Otsu threshold + separability
|
||||
*/
|
||||
data class OtsuResult(val threshold: Double, val separability: Double)
|
||||
|
||||
fun otsu1dWithSeparability(values: DoubleArray, limit: Int = values.size, nBins: Int = 64): OtsuResult {
|
||||
val n = min(values.size, limit)
|
||||
if (n == 0) return OtsuResult(0.0, 0.0)
|
||||
|
||||
var vMin = values[0]; var vMax = values[0]
|
||||
for (i in 1 until n) {
|
||||
if (values[i] < vMin) vMin = values[i]
|
||||
if (values[i] > vMax) vMax = values[i]
|
||||
}
|
||||
if (vMax == vMin) return OtsuResult(vMin, 0.0)
|
||||
|
||||
val binWidth = (vMax - vMin) / nBins
|
||||
val hist = IntArray(nBins)
|
||||
for (i in 0 until n) {
|
||||
val bin = ((values[i] - vMin) / binWidth).toInt().coerceIn(0, nBins - 1)
|
||||
hist[bin]++
|
||||
}
|
||||
|
||||
val centers = DoubleArray(nBins) { vMin + (it + 0.5) * binWidth }
|
||||
val total = n.toDouble()
|
||||
val p = DoubleArray(nBins) { hist[it] / total }
|
||||
|
||||
val cumP = DoubleArray(nBins)
|
||||
val cumMP = DoubleArray(nBins)
|
||||
cumP[0] = p[0]; cumMP[0] = p[0] * centers[0]
|
||||
for (i in 1 until nBins) {
|
||||
cumP[i] = cumP[i - 1] + p[i]
|
||||
cumMP[i] = cumMP[i - 1] + p[i] * centers[i]
|
||||
}
|
||||
val totalM = cumMP[nBins - 1]
|
||||
|
||||
var bestSigma = -1.0
|
||||
var bestIdx = 0
|
||||
for (t in 0 until nBins - 1) {
|
||||
val w0 = cumP[t]; val w1 = 1.0 - w0
|
||||
if (w0 < 1e-6 || w1 < 1e-6) continue
|
||||
val m0 = cumMP[t] / w0
|
||||
val m1 = (totalM - cumMP[t]) / w1
|
||||
val sigmaB = w0 * w1 * (m0 - m1) * (m0 - m1)
|
||||
if (sigmaB > bestSigma) {
|
||||
bestSigma = sigmaB
|
||||
bestIdx = t
|
||||
}
|
||||
}
|
||||
// Separability: σ_b² / σ_total²
|
||||
var totalVar = 0.0
|
||||
for (i in 0 until nBins) totalVar += p[i] * (centers[i] - totalM) * (centers[i] - totalM)
|
||||
val separability = if (totalVar > 1e-12) bestSigma / totalVar else 0.0
|
||||
|
||||
return OtsuResult(centers[bestIdx], separability)
|
||||
}
|
||||
|
||||
fun otsu1d(values: DoubleArray, limit: Int = values.size, nBins: Int = 64): Double {
|
||||
return otsu1dWithSeparability(values, limit, nBins).threshold
|
||||
}
|
||||
|
||||
/**
|
||||
* Low-echo 탐지 핵심 — Python _detect_core 1:1 포팅 (비대칭 score + FP 필터)
|
||||
*/
|
||||
private fun detectLowEchoCore(sg: DoubleArray, threshold: Double, effectivePostMax: Int = postMaxIdx): LowEchoResult? {
|
||||
if (sg.size < 10) return null
|
||||
|
||||
val lowMask = BooleanArray(sg.size) { sg[it] <= threshold }
|
||||
val rawSpans = contiguousTrueSpans(lowMask).filter { it.second - it.first + 1 >= lowMinLen }
|
||||
|
||||
val gapPeakThr = threshold + 5.0
|
||||
val spans = mergeCloseSpansWithWallCheck(rawSpans, maxGap = mergeGapMax, signal = sg, gapPeakThr = gapPeakThr)
|
||||
val firstSpan = spans.firstOrNull() ?: return null
|
||||
|
||||
// refine_right_edge
|
||||
val (s0, e0) = refineRightEdge(rawSpans, firstSpan, lowMinLen)
|
||||
val s = max(0, s0)
|
||||
val e = min(sg.size - 1, e0)
|
||||
if (s > e) return null
|
||||
|
||||
val lowSlice = safeSlice(sg, from = s, to = e) ?: return null
|
||||
if (lowSlice.isEmpty()) return null
|
||||
val lowMean = lowSlice.sum() / lowSlice.size.toDouble()
|
||||
val peakMin = max(lowMean + minPeakMargin, threshold)
|
||||
|
||||
// POST 먼저, ANT 나중 (Python 순서)
|
||||
var post = selectWallByProminence(sg = sg, edge = e, searchWin = peakSearchWin,
|
||||
peakMin = peakMin, side = WallSide.POST, otherEdge = s) ?: return null
|
||||
var ant = selectWallByProminence(sg = sg, edge = s, searchWin = peakSearchWin,
|
||||
peakMin = peakMin, side = WallSide.ANT, otherEdge = e)
|
||||
// ant fallback: low-echo 오른쪽 경계와 post 사이 거리만큼 왼쪽 반사
|
||||
if (ant == null && post != null) ant = max(0, s - (post - e))
|
||||
if (ant == null && s > 0) ant = s - 1
|
||||
if (ant == null) return null
|
||||
// sg[ant] < threshold 또는 sg[post] < threshold → reject
|
||||
if (sg[ant] < threshold || sg[post] < threshold) return null
|
||||
|
||||
if (post > effectivePostMax) {
|
||||
val backHalfEdge = (s + e) / 2
|
||||
post = selectWallByProminence(
|
||||
sg = sg, edge = backHalfEdge,
|
||||
searchWin = effectivePostMax - backHalfEdge,
|
||||
peakMin = peakMin, side = WallSide.POST, otherEdge = null
|
||||
) ?: return null
|
||||
}
|
||||
|
||||
val antH = sg[ant]
|
||||
val postH = sg[post]
|
||||
val antProm = antH - findRightValley(sg, ant)
|
||||
val postProm = postH - findLeftValley(sg, post)
|
||||
|
||||
val urineLen = post - ant - 1
|
||||
if (urineLen < minUrineLen) return null
|
||||
|
||||
// FP 필터: post prominence 최소
|
||||
if (postProm < minPeakMargin) return null
|
||||
|
||||
// 비대칭 score (ant_reliability)
|
||||
val antDepth = max(0.0, antH - lowMean)
|
||||
val postDepth = max(0.0, postH - lowMean)
|
||||
val antReliability = (antProm / max(postProm, 1e-6)).coerceIn(0.25, 1.0)
|
||||
val lowDepth = ((antReliability * antDepth) + postDepth) / (1.0 + antReliability)
|
||||
val score = lowDepth * urineLen.toDouble()
|
||||
|
||||
if (score < minScore) return null
|
||||
|
||||
return LowEchoResult(
|
||||
ant = ant, post = post,
|
||||
lowStart = s, lowEnd = e, lowMean = lowMean,
|
||||
urineLen = urineLen, score = score,
|
||||
innerPeaks = findInnerPeaks(sg, left = ant, right = post)
|
||||
)
|
||||
}
|
||||
|
||||
/** 병합 span 우측 트리밍 (Python _refine_right_edge_from_merged_components) */
|
||||
private fun refineRightEdge(rawSpans: List<Pair<Int, Int>>, mergedSpan: Pair<Int, Int>, lowMinLen: Int): Pair<Int, Int> {
|
||||
val (s, e) = mergedSpan
|
||||
val components = rawSpans.filter { it.first >= s && it.second <= e }
|
||||
if (components.size <= 1) return mergedSpan
|
||||
val (firstS, firstE) = components[0]
|
||||
val (secondS, _) = components[1]
|
||||
val spanLen = e - s + 1
|
||||
if ((secondS - s) >= (spanLen / 2) && (firstE - s + 1) >= lowMinLen) return Pair(firstS, firstE)
|
||||
return mergedSpan
|
||||
}
|
||||
|
||||
// ── Prominence-based Wall Selection (Python 동기화: ANT=nearest, POST=edge_score) ──
|
||||
|
||||
private val maxPeakCandidates = 3
|
||||
private enum class WallSide { ANT, POST }
|
||||
|
||||
/** valley with descending-first logic (Python _find_right_valley) */
|
||||
private fun findRightValley(sg: DoubleArray, peakIdx: Int, maxDist: Int = 20): Double {
|
||||
val n = sg.size
|
||||
var v = sg[peakIdx]
|
||||
var descending = false
|
||||
for (i in (peakIdx + 1) until min(n, peakIdx + maxDist + 1)) {
|
||||
val cur = sg[i]
|
||||
if (cur <= v) { v = cur; descending = true; continue }
|
||||
if (descending) break
|
||||
if (cur > v + valleyStopRise) break
|
||||
}
|
||||
return v
|
||||
}
|
||||
|
||||
private fun findLeftValley(sg: DoubleArray, peakIdx: Int, maxDist: Int = 20): Double {
|
||||
var v = sg[peakIdx]
|
||||
var descending = false
|
||||
for (i in (peakIdx - 1) downTo max(0, peakIdx - maxDist)) {
|
||||
val cur = sg[i]
|
||||
if (cur <= v) { v = cur; descending = true; continue }
|
||||
if (descending) break
|
||||
if (cur > v + valleyStopRise) break
|
||||
}
|
||||
return v
|
||||
}
|
||||
|
||||
/**
|
||||
* Wall peak 선택 — Python _select_wall_by_prominence 1:1 포팅
|
||||
* ANT: 가장 가까운 outer peak (prominence는 tie-breaker)
|
||||
* POST: edge_score = prom / (1 + decay * dist)
|
||||
*/
|
||||
private fun selectWallByProminence(
|
||||
sg: DoubleArray, edge: Int, searchWin: Int,
|
||||
peakMin: Double, side: WallSide, otherEdge: Int?
|
||||
): Int? {
|
||||
val n = sg.size
|
||||
if (n <= 0 || edge < 0 || edge >= n) return null
|
||||
|
||||
val leftLo: Int
|
||||
val rightHi: Int
|
||||
if (side == WallSide.ANT) {
|
||||
leftLo = max(0, edge - searchWin)
|
||||
var rh = min(n - 1, edge + searchWin)
|
||||
if (otherEdge != null) rh = min(rh, otherEdge)
|
||||
rightHi = rh
|
||||
} else {
|
||||
var ll = max(0, edge - searchWin)
|
||||
if (otherEdge != null) ll = max(ll, otherEdge)
|
||||
leftLo = ll
|
||||
rightHi = min(n - 1, edge + searchWin)
|
||||
}
|
||||
|
||||
var leftCandidates = listOf<Int>()
|
||||
if (edge > leftLo) {
|
||||
val leftEnd = min(edge + 1, n)
|
||||
val seg = safeSlice(sg, from = leftLo, to = leftEnd - 1)
|
||||
if (seg != null) {
|
||||
val pks = findPeaks1D(seg)
|
||||
leftCandidates = pks.map { it + leftLo }.filter { it < edge }.sortedBy { abs(it - edge) }.take(maxPeakCandidates)
|
||||
}
|
||||
}
|
||||
|
||||
var rightCandidates = listOf<Int>()
|
||||
if (rightHi >= edge) {
|
||||
val rightStart = max(edge - 1, 0)
|
||||
val seg = safeSlice(sg, from = rightStart, to = rightHi)
|
||||
if (seg != null) {
|
||||
val pks = findPeaks1D(seg)
|
||||
rightCandidates = pks.map { it + rightStart }.filter { it >= edge }.sortedBy { abs(it - edge) }.take(maxPeakCandidates)
|
||||
}
|
||||
}
|
||||
|
||||
// Score candidates
|
||||
data class Scored(val p: Int, val prom: Double, val dist: Int, val sidePref: Int, val edgeScore: Double)
|
||||
fun scoreGroup(group: List<Int>, groupName: String): List<Scored> {
|
||||
return group.mapNotNull { p ->
|
||||
val valley = if (side == WallSide.ANT) findRightValley(sg, p) else findLeftValley(sg, p)
|
||||
val prom = sg[p] - valley
|
||||
if (sg[p] < peakMin) return@mapNotNull null
|
||||
val dist = abs(p - edge)
|
||||
val sidePref = if (groupName == "outer") 0 else 1
|
||||
val es = prom / (1.0 + edgeDistDecay * dist)
|
||||
Scored(p, prom, dist, sidePref, es)
|
||||
}
|
||||
}
|
||||
|
||||
val (outerCands, innerCands) = if (side == WallSide.ANT)
|
||||
Pair(leftCandidates, rightCandidates) else Pair(rightCandidates, leftCandidates)
|
||||
val outerScored = scoreGroup(outerCands, "outer")
|
||||
val innerScored = scoreGroup(innerCands, "inner")
|
||||
|
||||
// ANT: 가장 가까운 outer peak (distance 우선, prominence tie-break)
|
||||
if (side == WallSide.ANT) {
|
||||
if (outerScored.isNotEmpty()) {
|
||||
return outerScored.sortedWith(compareBy<Scored> { it.dist }.thenByDescending { it.prom }.thenBy { it.p }).first().p
|
||||
}
|
||||
return null
|
||||
}
|
||||
|
||||
// POST: edge_score 최대
|
||||
val allScored = outerScored + innerScored
|
||||
if (allScored.isEmpty()) return null
|
||||
return allScored.sortedWith(compareByDescending<Scored> { it.edgeScore }.thenByDescending { it.prom }.thenBy { it.dist }.thenBy { it.sidePref }.thenBy { it.p }).first().p
|
||||
}
|
||||
|
||||
// ── Span Utilities ──
|
||||
|
||||
fun findPeaks1D(x: DoubleArray, height: Double? = null): List<Int> {
|
||||
val n = x.size
|
||||
if (n < 3) return emptyList()
|
||||
|
||||
val peaks = mutableListOf<Int>()
|
||||
var i = 1
|
||||
while (i < n - 1) {
|
||||
if (x[i - 1] < x[i]) {
|
||||
var j = i
|
||||
while (j < n - 1 && x[j + 1] == x[j]) j++
|
||||
if (j < n - 1 && x[j + 1] < x[j]) {
|
||||
peaks.add((i + j) / 2)
|
||||
}
|
||||
i = j + 1
|
||||
} else {
|
||||
i++
|
||||
}
|
||||
}
|
||||
|
||||
return if (height != null) {
|
||||
peaks.filter { x[it] >= height }
|
||||
} else {
|
||||
peaks
|
||||
}
|
||||
}
|
||||
|
||||
fun contiguousTrueSpans(mask: BooleanArray): List<Pair<Int, Int>> {
|
||||
val spans = mutableListOf<Pair<Int, Int>>()
|
||||
var start: Int? = null
|
||||
for ((i, m) in mask.withIndex()) {
|
||||
if (m && start == null) {
|
||||
start = i
|
||||
} else if (!m && start != null) {
|
||||
spans.add(Pair(start, i - 1))
|
||||
start = null
|
||||
}
|
||||
}
|
||||
if (start != null) {
|
||||
spans.add(Pair(start, mask.size - 1))
|
||||
}
|
||||
return spans
|
||||
}
|
||||
|
||||
fun mergeCloseSpans(spans: List<Pair<Int, Int>>, maxGap: Int): List<Pair<Int, Int>> {
|
||||
return mergeCloseSpansWithWallCheck(spans, maxGap = maxGap, signal = null, gapPeakThr = 0.0)
|
||||
}
|
||||
|
||||
/** 병합 시 gap 내부에 벽 후보(gapPeakThr 초과 peak)가 있으면 병합하지 않음 */
|
||||
fun mergeCloseSpansWithWallCheck(
|
||||
spans: List<Pair<Int, Int>>, maxGap: Int,
|
||||
signal: DoubleArray?, gapPeakThr: Double
|
||||
): List<Pair<Int, Int>> {
|
||||
if (spans.isEmpty()) return emptyList()
|
||||
val ordered = spans.sortedBy { it.first }
|
||||
val merged = mutableListOf(ordered[0])
|
||||
for (i in 1 until ordered.size) {
|
||||
val (s, e) = ordered[i]
|
||||
val prevEnd = merged[merged.size - 1].second
|
||||
val gap = s - prevEnd - 1
|
||||
if (gap <= maxGap) {
|
||||
// gap < 3은 무조건 병합 (Python peak_check_min_gap=3)
|
||||
var hasWallPeak = false
|
||||
if (gap >= 3 && signal != null && gapPeakThr > 0) {
|
||||
val gapStart = prevEnd + 1
|
||||
val gapEnd = s - 1
|
||||
if (gapStart <= gapEnd) {
|
||||
val gapSlice = safeSlice(signal, from = gapStart, to = gapEnd)
|
||||
if (gapSlice != null) {
|
||||
val peaks = findPeaks1D(gapSlice, height = gapPeakThr)
|
||||
if (peaks.isNotEmpty()) hasWallPeak = true
|
||||
// Also check endpoints
|
||||
if (!hasWallPeak) {
|
||||
if ((gapSlice.firstOrNull() ?: 0.0) > gapPeakThr) hasWallPeak = true
|
||||
if ((gapSlice.lastOrNull() ?: 0.0) > gapPeakThr) hasWallPeak = true
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (hasWallPeak) {
|
||||
// Wall candidate in gap — don't merge
|
||||
merged.add(Pair(s, e))
|
||||
} else {
|
||||
merged[merged.size - 1] = Pair(merged[merged.size - 1].first, max(prevEnd, e))
|
||||
}
|
||||
} else {
|
||||
merged.add(Pair(s, e))
|
||||
}
|
||||
}
|
||||
return merged
|
||||
}
|
||||
|
||||
fun nearestPeak(signal: DoubleArray, center: Int, left: Int, right: Int, minHeight: Double): Int? {
|
||||
if (signal.isEmpty()) return null
|
||||
val l = max(0, left)
|
||||
val r = min(signal.size - 1, right)
|
||||
val local = safeSlice(signal, from = l, to = r) ?: return null
|
||||
val pkLocal = findPeaks1D(local, height = minHeight)
|
||||
val candidates = (pkLocal.map { it + l }).toMutableSet()
|
||||
|
||||
// Endpoint correction
|
||||
if (l == r) {
|
||||
if (signal[l] >= minHeight) candidates.add(l)
|
||||
} else {
|
||||
if (signal[l] >= minHeight && signal[l] >= signal[l + 1]) candidates.add(l)
|
||||
if (signal[r] >= minHeight && signal[r] >= signal[r - 1]) candidates.add(r)
|
||||
}
|
||||
|
||||
if (candidates.isEmpty()) return null
|
||||
|
||||
val sorted = candidates.sorted()
|
||||
// Find nearest to center, tie-break by amplitude
|
||||
var bestIdx = sorted[0]
|
||||
var bestDist = abs(sorted[0] - center)
|
||||
for (c in sorted) {
|
||||
val dist = abs(c - center)
|
||||
if (dist < bestDist || (dist == bestDist && signal[c] > signal[bestIdx])) {
|
||||
bestIdx = c
|
||||
bestDist = dist
|
||||
}
|
||||
}
|
||||
return bestIdx
|
||||
}
|
||||
|
||||
fun findInnerPeaks(signal: DoubleArray, left: Int, right: Int): List<Int> {
|
||||
val l = left + 1
|
||||
val r = right - 1
|
||||
val local = safeSlice(signal, from = l, to = r) ?: return emptyList()
|
||||
return findPeaks1D(local).map { it + l }
|
||||
}
|
||||
|
||||
// ── Safe Array Slicing ──
|
||||
|
||||
/** Safe inclusive range slice — returns null if bounds are invalid */
|
||||
private fun safeSlice(arr: DoubleArray, from: Int, to: Int): DoubleArray? {
|
||||
val l = max(0, from)
|
||||
val r = min(arr.size - 1, to)
|
||||
if (l > r || arr.isEmpty()) return null
|
||||
return arr.sliceArray(l..r)
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,329 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.max
|
||||
import kotlin.math.min
|
||||
import kotlin.math.sqrt
|
||||
|
||||
/**
|
||||
* Method A — TVD → SG → Derivative + Plateau-first wall expansion.
|
||||
* 1:1 port of low_echo_detection_method_a.py (원은지 연구원)
|
||||
*
|
||||
* Pipeline:
|
||||
* 1. Condat(2013) 1D TVD
|
||||
* 2. SG smooth
|
||||
* 3. Composite signal y = sg + α|d1| + β|d2|
|
||||
* 4. Sliding plateau score
|
||||
* 5. Plateau span detection + merge
|
||||
* 6. Prominence-based wall selection from plateau edges
|
||||
*/
|
||||
class PiezoEchoAnalyzerA private constructor() {
|
||||
|
||||
companion object {
|
||||
val shared = PiezoEchoAnalyzerA()
|
||||
}
|
||||
|
||||
// ── Parameters (notebook 실측 튜닝값) ──
|
||||
val tvLambda: Double = 3.0
|
||||
val alpha: Double = 1.0
|
||||
val beta: Double = 1.0
|
||||
val postMaxIdx: Int get() = GreenZoneConstants.postMaxIdx
|
||||
val plateauMinLen: Int = 3
|
||||
val plateauMergeGap: Int = 5
|
||||
val scoreWin: Int = 7
|
||||
val plateauQ: Double = 0.5
|
||||
val edgeDistDecay: Double = 0.15
|
||||
val peakSearchWin: Int = 20
|
||||
val minWallProminence: Double = 3.0
|
||||
val minWallContrastRatio: Double = 1.2
|
||||
val minUrineLen: Int = 3
|
||||
val minEffectiveDist: Int = 3
|
||||
val valleyStopRise: Double = 5.0
|
||||
|
||||
var lastOtsuThreshold: Double = 0.0; private set
|
||||
|
||||
// ── Public API ──
|
||||
|
||||
fun analyzeChannel(rawADC: List<UShort>, channel: Int = 0): ChannelAnalysisResult {
|
||||
val raw = DoubleArray(rawADC.size) { rawADC[it].toDouble() }
|
||||
if (raw.size < 10) {
|
||||
return ChannelAnalysisResult(channel, null, raw, raw.copyOf())
|
||||
}
|
||||
return try {
|
||||
val sg = PiezoEchoAnalyzer.shared.denoise(raw)
|
||||
// 각도 보정: min_wall_contrast_ratio × cos(θ)
|
||||
val beamAngles = PiezoHW.degreeAll
|
||||
val angleFactor = if (channel < beamAngles.size) kotlin.math.cos(beamAngles[channel] * Math.PI / 180.0) else 1.0
|
||||
val result = detectMethodA(raw, sg, contrastRatioOverride = minWallContrastRatio * angleFactor)
|
||||
ChannelAnalysisResult(channel, result, raw, sg)
|
||||
} catch (_: Exception) {
|
||||
ChannelAnalysisResult(channel, null, raw, raw.copyOf())
|
||||
}
|
||||
}
|
||||
|
||||
fun detectMethodA(raw: DoubleArray, sg: DoubleArray, contrastRatioOverride: Double? = null): LowEchoResult? {
|
||||
val effectiveContrastRatio = contrastRatioOverride ?: minWallContrastRatio
|
||||
if (sg.size < 10) return null
|
||||
|
||||
// 1) Composite signal y = sg + α|d1| + β|d2|
|
||||
val d1 = DoubleArray(sg.size - 1) { sg[it + 1] - sg[it] }
|
||||
val d2 = DoubleArray(sg.size - 2) { sg[it + 2] - 2 * sg[it + 1] + sg[it] }
|
||||
val L = d2.size
|
||||
if (L < 10) return null
|
||||
val y = DoubleArray(L) { sg[it] + alpha * abs(d1[it]) + beta * abs(d2[it]) }
|
||||
|
||||
// 2) Plateau score — threshold from full array (np.quantile 보간)
|
||||
val platScore = slidingScores1d(sg, scoreWin)
|
||||
val scoreThr = npQuantile(platScore, plateauQ)
|
||||
val limit = min(platScore.size, postMaxIdx + 1)
|
||||
|
||||
// 3) Plateau spans + merge (postMaxIdx 이내만)
|
||||
var plateauSpans = findPlateauSpans(platScore, limit, scoreThr, plateauMinLen)
|
||||
if (plateauSpans.size > 1) {
|
||||
val merged = mutableListOf(plateauSpans[0])
|
||||
for (i in 1 until plateauSpans.size) {
|
||||
val (s, e) = plateauSpans[i]
|
||||
val (ps, pe) = merged.last()
|
||||
if (s - pe <= plateauMergeGap) {
|
||||
merged[merged.size - 1] = Pair(ps, e)
|
||||
} else {
|
||||
merged.add(Pair(s, e))
|
||||
}
|
||||
}
|
||||
plateauSpans = merged
|
||||
}
|
||||
|
||||
// 4) Plateau-first matching
|
||||
for ((spanStart, spanEnd) in plateauSpans) {
|
||||
val leftLo = max(0, spanStart - peakSearchWin)
|
||||
val rightHi = min(sg.size, spanEnd + peakSearchWin + 1)
|
||||
|
||||
val platMean = if (spanEnd >= spanStart) {
|
||||
var sum = 0.0
|
||||
for (i in spanStart..spanEnd) sum += sg[i]
|
||||
sum / (spanEnd - spanStart + 1)
|
||||
} else continue
|
||||
|
||||
if (platMean <= 0) continue
|
||||
|
||||
// Contrast check
|
||||
var leftMax = 0.0
|
||||
for (i in leftLo..min(spanStart, sg.size - 1)) if (sg[i] > leftMax) leftMax = sg[i]
|
||||
var rightMax = 0.0
|
||||
for (i in spanEnd until rightHi) if (sg[i] > rightMax) rightMax = sg[i]
|
||||
if (leftMax / platMean < effectiveContrastRatio || rightMax / platMean < effectiveContrastRatio) continue
|
||||
|
||||
// Find peaks
|
||||
val leftPks = findPeaksInRange(sg, leftLo, spanStart).filter { it < spanStart }
|
||||
val rightPks = findPeaksInRange(sg, spanEnd, rightHi - 1).filter { it > spanEnd && it <= postMaxIdx }
|
||||
if (leftPks.isEmpty() || rightPks.isEmpty()) continue
|
||||
|
||||
// Score left candidates
|
||||
var bestLeftScore = -1.0; var bestLeft = -1
|
||||
for (p in leftPks) {
|
||||
val prom = sg[p] - findRightValley(sg, p)
|
||||
if (prom < minWallProminence) continue
|
||||
val dist = spanStart - p
|
||||
val effDist = max(dist, minEffectiveDist)
|
||||
val score = prom / (1.0 + edgeDistDecay * effDist)
|
||||
if (score > bestLeftScore) { bestLeftScore = score; bestLeft = p }
|
||||
}
|
||||
|
||||
// Score right candidates
|
||||
var bestRightScore = -1.0; var bestRight = -1
|
||||
for (p in rightPks) {
|
||||
val prom = sg[p] - findLeftValley(sg, p)
|
||||
if (prom < minWallProminence) continue
|
||||
val dist = p - spanEnd
|
||||
val effDist = max(dist, minEffectiveDist)
|
||||
val score = prom / (1.0 + edgeDistDecay * effDist)
|
||||
if (score > bestRightScore) { bestRightScore = score; bestRight = p }
|
||||
}
|
||||
|
||||
if (bestLeft < 0 || bestRight < 0 || bestRight <= bestLeft) continue
|
||||
val urineLen = bestRight - bestLeft - 1
|
||||
if (urineLen < minUrineLen) continue
|
||||
|
||||
val lowSlice = sg.sliceArray(spanStart..spanEnd)
|
||||
val lowMean = lowSlice.average()
|
||||
|
||||
return LowEchoResult(
|
||||
ant = bestLeft, post = bestRight,
|
||||
lowStart = spanStart, lowEnd = spanEnd,
|
||||
lowMean = lowMean, urineLen = urineLen,
|
||||
score = (sg[bestLeft] + sg[bestRight]).toDouble(),
|
||||
innerPeaks = PiezoEchoAnalyzer.shared.findInnerPeaks(sg, bestLeft, bestRight)
|
||||
)
|
||||
}
|
||||
return null
|
||||
}
|
||||
|
||||
// ── Sliding plateau score ──
|
||||
|
||||
private fun slidingScores1d(x: DoubleArray, win: Int): DoubleArray {
|
||||
val w = if (win % 2 == 0) win + 1 else win
|
||||
val half = w / 2
|
||||
val T = x.size
|
||||
val flat = DoubleArray(T)
|
||||
val slope = DoubleArray(T)
|
||||
val low = DoubleArray(T)
|
||||
|
||||
// Edge padding (mode="edge") — Python np.pad 동일
|
||||
val xp = DoubleArray(T + 2 * half)
|
||||
for (i in 0 until half) xp[i] = x[0]
|
||||
for (i in 0 until T) xp[i + half] = x[i]
|
||||
for (i in 0 until half) xp[T + half + i] = x[T - 1]
|
||||
|
||||
val tt = DoubleArray(w) { (it - half).toDouble() }
|
||||
var ttSqSum = 0.0
|
||||
for (t in tt) ttSqSum += t * t
|
||||
ttSqSum += 1e-12
|
||||
|
||||
for (i in 0 until T) {
|
||||
val ww = DoubleArray(w) { xp[i + it] }
|
||||
var sum = 0.0; var sqSum = 0.0
|
||||
for (v in ww) { sum += v; sqSum += v * v }
|
||||
val mean = sum / w
|
||||
flat[i] = sqrt(max(0.0, sqSum / w - mean * mean))
|
||||
|
||||
val sorted = ww.sorted()
|
||||
low[i] = npQuantile(sorted, 0.2)
|
||||
|
||||
var slopeNum = 0.0
|
||||
for (j in 0 until w) {
|
||||
slopeNum += tt[j] * (ww[j] - mean)
|
||||
}
|
||||
slope[i] = abs(slopeNum / ttSqSum)
|
||||
}
|
||||
|
||||
val rzLow = robustZ(low)
|
||||
val rzFlat = robustZ(flat)
|
||||
val rzSlope = robustZ(slope)
|
||||
return DoubleArray(T) { rzLow[it] + rzFlat[it] + rzSlope[it] }
|
||||
}
|
||||
|
||||
private fun robustZ(a: DoubleArray): DoubleArray {
|
||||
val sorted = a.sorted().toDoubleArray()
|
||||
val n = sorted.size
|
||||
val med = if (n % 2 == 0) (sorted[n / 2 - 1] + sorted[n / 2]) / 2.0 else sorted[n / 2]
|
||||
val deviations = DoubleArray(n) { abs(a[it] - med) }
|
||||
val devSorted = deviations.sorted().toDoubleArray()
|
||||
val madMed = if (n % 2 == 0) (devSorted[n / 2 - 1] + devSorted[n / 2]) / 2.0 else devSorted[n / 2]
|
||||
val mad = madMed + 1e-12
|
||||
return DoubleArray(n) { (a[it] - med) / (1.4826 * mad) }
|
||||
}
|
||||
|
||||
// ── Plateau span detection ──
|
||||
|
||||
private fun findPlateauSpans(score: DoubleArray, limit: Int, thr: Double, minLen: Int): MutableList<Pair<Int, Int>> {
|
||||
val spans = mutableListOf<Pair<Int, Int>>()
|
||||
var runStart: Int? = null
|
||||
for (i in 0 until limit) {
|
||||
if (score[i] < thr) {
|
||||
if (runStart == null) runStart = i
|
||||
} else {
|
||||
if (runStart != null && (i - runStart) >= minLen) {
|
||||
spans.add(Pair(runStart, i - 1))
|
||||
}
|
||||
runStart = null
|
||||
}
|
||||
}
|
||||
if (runStart != null && (limit - runStart) >= minLen) {
|
||||
spans.add(Pair(runStart, limit - 1))
|
||||
}
|
||||
return spans
|
||||
}
|
||||
|
||||
// ── Cross-validation (채널간 벽 보정) ──
|
||||
// 1:1 port of _cross_validate_walls()
|
||||
|
||||
private val crossValMaxDev = 5
|
||||
private val crossValMaxGradient = 3
|
||||
|
||||
fun crossValidateWalls(
|
||||
walls: List<Pair<Int, Int>?>,
|
||||
centerCh: List<Int> = listOf(0, 1, 2, 3)
|
||||
): List<Pair<Int, Int>?> {
|
||||
val corrected = walls.toMutableList()
|
||||
|
||||
// 1단계: 글로벌 중앙값 기반
|
||||
val ants = corrected.mapNotNull { it?.first }
|
||||
val posts = corrected.mapNotNull { it?.second }
|
||||
if (ants.size >= 3) {
|
||||
val medAnt = ants.sorted()[ants.size / 2]
|
||||
val medPost = posts.sorted()[posts.size / 2]
|
||||
for (i in corrected.indices) {
|
||||
val w = corrected[i] ?: continue
|
||||
val a = if (abs(w.first - medAnt) >= crossValMaxDev) medAnt else w.first
|
||||
val p = if (abs(w.second - medPost) >= crossValMaxDev) medPost else w.second
|
||||
corrected[i] = Pair(a, p)
|
||||
}
|
||||
}
|
||||
|
||||
// 2단계: 인접 center 채널 smoothness
|
||||
val validCenter = centerCh.filter { it < corrected.size && corrected[it] != null }
|
||||
if (validCenter.size < 3) return corrected
|
||||
|
||||
for (field in listOf("ant", "post")) {
|
||||
val vals = validCenter.map { if (field == "ant") corrected[it]!!.first else corrected[it]!!.second }
|
||||
for (j in vals.indices) {
|
||||
val neighbors = mutableListOf<Int>()
|
||||
if (j > 0) neighbors.add(vals[j - 1])
|
||||
if (j < vals.size - 1) neighbors.add(vals[j + 1])
|
||||
if (neighbors.isEmpty()) continue
|
||||
val neighborMean = neighbors.average()
|
||||
if (abs(vals[j] - neighborMean) > crossValMaxGradient) {
|
||||
val newVal = Math.round(neighborMean).toInt()
|
||||
val chIdx = validCenter[j]
|
||||
val w = corrected[chIdx]!!
|
||||
corrected[chIdx] = if (field == "ant") Pair(newVal, w.second) else Pair(w.first, newVal)
|
||||
}
|
||||
}
|
||||
}
|
||||
return corrected
|
||||
}
|
||||
|
||||
// ── np.quantile 보간 (linear interpolation, numpy default) ──
|
||||
private fun npQuantile(data: DoubleArray, q: Double): Double {
|
||||
if (data.isEmpty()) return 0.0
|
||||
val sorted = data.sorted().toDoubleArray()
|
||||
return npQuantile(sorted.toList(), q)
|
||||
}
|
||||
|
||||
private fun npQuantile(sorted: List<Double>, q: Double): Double {
|
||||
if (sorted.isEmpty()) return 0.0
|
||||
val n = sorted.size
|
||||
val idx = q * (n - 1)
|
||||
val lo = idx.toInt().coerceIn(0, n - 1)
|
||||
val hi = (lo + 1).coerceAtMost(n - 1)
|
||||
val frac = idx - lo
|
||||
return sorted[lo] + frac * (sorted[hi] - sorted[lo])
|
||||
}
|
||||
|
||||
// ── Peak / Valley helpers ──
|
||||
|
||||
private fun findPeaksInRange(sg: DoubleArray, from: Int, to: Int): List<Int> {
|
||||
val lo = max(0, from); val hi = min(sg.size - 1, to)
|
||||
if (hi - lo < 2) return emptyList()
|
||||
val seg = sg.sliceArray(lo..hi)
|
||||
return PiezoEchoAnalyzer.shared.findPeaks1D(seg).map { it + lo }
|
||||
}
|
||||
|
||||
private fun findRightValley(sg: DoubleArray, peakIdx: Int, maxDist: Int = 20): Double {
|
||||
var v = sg[peakIdx]
|
||||
for (i in (peakIdx + 1) until min(sg.size, peakIdx + maxDist)) {
|
||||
if (sg[i] < v) v = sg[i]
|
||||
else if (sg[i] > v + valleyStopRise) break
|
||||
}
|
||||
return v
|
||||
}
|
||||
|
||||
private fun findLeftValley(sg: DoubleArray, peakIdx: Int, maxDist: Int = 20): Double {
|
||||
var v = sg[peakIdx]
|
||||
for (i in (peakIdx - 1) downTo max(0, peakIdx - maxDist)) {
|
||||
if (sg[i] < v) v = sg[i]
|
||||
else if (sg[i] > v + valleyStopRise) break
|
||||
}
|
||||
return v
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,197 @@
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
/**
|
||||
* UrinAI (UrinGateKeeper) — 소변(액체) 존재 판정 엔진.
|
||||
*
|
||||
* 역할: raw ADC 신호에서 "방광(액체)이 있는가?" → true/false 반환.
|
||||
* true일 때만 벽 검출 엔진(PiezoEchoAnalyzer)이 실행됨.
|
||||
*
|
||||
* 원리: 이진 임계 기법
|
||||
* raw < LIQUID_THR_LOOSE → 액체 / raw ≥ LIQUID_THR_LOOSE → 조직
|
||||
* 연속 액체 구간 + contrast 검증 → 생물학적 판정
|
||||
*
|
||||
* 1:1 port of UrinAI.swift / UrinAI.kt (Charles KWON)
|
||||
*/
|
||||
object UrinAI {
|
||||
|
||||
// ── Result ──
|
||||
|
||||
data class Result(
|
||||
/** 핵심: 소변 존재 여부 */
|
||||
val detected: Boolean,
|
||||
/** lumen 시작점 (액체 진입) */
|
||||
val fw: Int,
|
||||
/** posterior wall peak */
|
||||
val bw: Int,
|
||||
/** 연속 순수 액체 최대 길이 */
|
||||
val liquidRun: Int,
|
||||
/** wall-lumen 대비 (0~1) */
|
||||
val contrast: Float,
|
||||
/** 원본 raw (그래프 표시용) */
|
||||
val rawSignal: List<UShort>,
|
||||
/** 이진 마스크 (true=액체) */
|
||||
val binaryMask: List<Boolean>,
|
||||
/** 순수 액체 구간들 */
|
||||
val urineZones: List<IntRange>,
|
||||
/** anterior wall 평가 가능 여부 */
|
||||
val antWallAvailable: Boolean
|
||||
)
|
||||
|
||||
// ── Detection ──
|
||||
|
||||
/** 소변 존재 판정 */
|
||||
fun detect(rawAdc: List<UShort>): Result {
|
||||
if (rawAdc.isEmpty()) {
|
||||
return emptyResult(rawCopy = emptyList(), binary = emptyList())
|
||||
}
|
||||
|
||||
val n = minOf(rawAdc.size, GreenZoneConstants.useSamples)
|
||||
val raw = FloatArray(n) { rawAdc[it].toFloat() }
|
||||
val rawCopy = rawAdc.take(n)
|
||||
|
||||
val binarizeThr = GreenZoneConstants.liquidThrLoose
|
||||
val inUrineThr = GreenZoneConstants.liquidThrStrict
|
||||
val minLiquidRun = GreenZoneConstants.minLiquidRun
|
||||
val minDepth = GreenZoneConstants.minChord
|
||||
val contrastMin = GreenZoneConstants.contrastMin
|
||||
val ringSkip = GreenZoneConstants.ringSkip
|
||||
|
||||
// 1. Binarize (LIQUID_THR_LOOSE 기준)
|
||||
val binary = List(n) { raw[it] < binarizeThr }
|
||||
|
||||
// 2. fw = 첫 번째 연속 액체 (3샘플 이상, ringdown skip)
|
||||
var fw = -1
|
||||
for (i in ringSkip until (n - 2)) {
|
||||
if (binary[i] && binary[i + 1] && binary[i + 2]) {
|
||||
fw = i
|
||||
break
|
||||
}
|
||||
}
|
||||
if (fw < 0) {
|
||||
return emptyResult(rawCopy = rawCopy, binary = binary)
|
||||
}
|
||||
|
||||
// 3. bw = fw 이후 가장 강한 조직 그룹의 peak
|
||||
// Two-pass: 엄격(3샘플) → 실패 시 완화(2샘플, 소방광 대응)
|
||||
var bw = findBw(fw = fw, binary = binary, raw = raw, minGroupLen = 3)
|
||||
|
||||
if (bw < 0 || bw - fw < minDepth) {
|
||||
val bw2 = findBw(fw = fw, binary = binary, raw = raw, minGroupLen = 2)
|
||||
if (bw2 >= 0 && bw2 - fw >= minDepth) {
|
||||
bw = bw2
|
||||
}
|
||||
}
|
||||
|
||||
if (bw < 0) {
|
||||
return emptyResult(rawCopy = rawCopy, binary = binary, fw = fw)
|
||||
}
|
||||
if (bw - fw < minDepth) {
|
||||
return emptyResult(rawCopy = rawCopy, binary = binary, fw = fw, bw = bw)
|
||||
}
|
||||
|
||||
// 4. 순수 액체 연속 구간 (strict threshold)
|
||||
var maxRun = 0
|
||||
var run = 0
|
||||
var zoneStart = -1
|
||||
val zones = mutableListOf<IntRange>()
|
||||
|
||||
for (j in fw until bw) {
|
||||
if (raw[j] < inUrineThr) {
|
||||
if (zoneStart < 0) zoneStart = j
|
||||
run += 1
|
||||
} else {
|
||||
if (run > 0) {
|
||||
if (run > maxRun) maxRun = run
|
||||
zones.add(zoneStart until j)
|
||||
}
|
||||
run = 0
|
||||
zoneStart = -1
|
||||
}
|
||||
}
|
||||
if (run > 0 && zoneStart >= 0) {
|
||||
if (run > maxRun) maxRun = run
|
||||
zones.add(zoneStart until bw)
|
||||
}
|
||||
|
||||
// 5. Contrast
|
||||
val antWallAvailable = fw > ringSkip + 2
|
||||
|
||||
var antWallPeak = raw[fw]
|
||||
for (j in maxOf(ringSkip, fw - 5) until fw) {
|
||||
if (raw[j] > antWallPeak) antWallPeak = raw[j]
|
||||
}
|
||||
|
||||
val wallPeak = maxOf(antWallPeak, raw[bw])
|
||||
|
||||
var lumenFloor = raw[fw + 1]
|
||||
for (j in (fw + 2) until bw) {
|
||||
if (raw[j] < lumenFloor) lumenFloor = raw[j]
|
||||
}
|
||||
|
||||
val contrast: Float = if (wallPeak > 0) (wallPeak - lumenFloor) / wallPeak else 0f
|
||||
|
||||
// 6. 판정
|
||||
val detected = maxRun >= minLiquidRun
|
||||
&& (bw - fw) >= minDepth
|
||||
&& contrast >= contrastMin
|
||||
|
||||
return Result(
|
||||
detected = detected,
|
||||
fw = fw, bw = bw,
|
||||
liquidRun = maxRun, contrast = contrast,
|
||||
rawSignal = rawCopy, binaryMask = binary,
|
||||
urineZones = zones,
|
||||
antWallAvailable = antWallAvailable
|
||||
)
|
||||
}
|
||||
|
||||
// ── Private ──
|
||||
|
||||
/** fw 이후 가장 강한 조직 그룹의 peak index */
|
||||
private fun findBw(fw: Int, binary: List<Boolean>, raw: FloatArray, minGroupLen: Int): Int {
|
||||
val n = raw.size
|
||||
var bw = -1
|
||||
var maxGroupSum = 0f
|
||||
var i = fw + 1
|
||||
|
||||
// fw 이후 liquid 구간 건너뛰기
|
||||
while (i < n && binary[i]) i++
|
||||
|
||||
while (i < n) {
|
||||
if (!binary[i]) {
|
||||
val gs = i
|
||||
var gSum = 0f
|
||||
while (i < n && !binary[i]) {
|
||||
gSum += raw[i]
|
||||
i++
|
||||
}
|
||||
val ge = i - 1
|
||||
if (ge - gs + 1 >= minGroupLen && gSum > maxGroupSum) {
|
||||
maxGroupSum = gSum
|
||||
// peak index within group
|
||||
var peakIdx = gs
|
||||
for (k in (gs + 1)..ge) {
|
||||
if (raw[k] > raw[peakIdx]) peakIdx = k
|
||||
}
|
||||
bw = peakIdx
|
||||
}
|
||||
} else {
|
||||
i++
|
||||
}
|
||||
}
|
||||
return bw
|
||||
}
|
||||
|
||||
/** 실패 시 빈 결과 */
|
||||
private fun emptyResult(
|
||||
rawCopy: List<UShort>, binary: List<Boolean>,
|
||||
fw: Int = 0, bw: Int = 0
|
||||
): Result = Result(
|
||||
detected = false,
|
||||
fw = fw, bw = bw,
|
||||
liquidRun = 0, contrast = 0f,
|
||||
rawSignal = rawCopy, binaryMask = binary,
|
||||
urineZones = emptyList(),
|
||||
antWallAvailable = false
|
||||
)
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
/*
|
||||
* WakeupDetector — LYING → NON_LYING 전이 감지 (낙상 예방용 기상 알람).
|
||||
*
|
||||
* 로직:
|
||||
* - 사용자가 LYING 상태로 들어가면 startTimestamp 기록
|
||||
* - LYING 으로 lyingDurationSec(default 180) 이상 유지 후 NON_LYING 으로 전환 → 기상 인정
|
||||
* - 기상 인정 시 onWakeup(elapsedSec) 콜백 호출 (한 번)
|
||||
* - 알람 발동 후 다시 LYING 으로 돌아가야 재무장 (재발동 가능)
|
||||
*
|
||||
* UNKNOWN 상태는 무시 (직전 상태 유지). 자세 분류 게이트 실패 시 잠깐 ?_ 떠도 detector 영향 X.
|
||||
*
|
||||
* 사용:
|
||||
* val detector = WakeupDetector(lyingDurationSec = 180)
|
||||
* detector.onWakeup = { elapsedSec -> showAlarm() }
|
||||
* // 매 posture 갱신 시:
|
||||
* detector.update(currentPosture, System.currentTimeMillis())
|
||||
*/
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
class WakeupDetector(
|
||||
/** 알람 발동을 위해 직전 LYING 상태로 유지되어야 하는 최소 시간 (초). */
|
||||
var lyingDurationSec: Int = 180,
|
||||
) {
|
||||
/** 일어남 감지 시 호출 — argument 는 직전 LYING 유지 시간 (초). */
|
||||
var onWakeup: ((elapsedSec: Long) -> Unit)? = null
|
||||
|
||||
private var lastPosture: Posture = Posture.UNKNOWN
|
||||
private var lyingStartMs: Long = 0L // 현재 LYING streak 시작 시각 (0 = LYING 아님)
|
||||
private var armed: Boolean = true // false 면 알람 1회 발동 후 LYING 복귀 대기
|
||||
|
||||
/** 외부 reset (Settings 토글 변경 등). */
|
||||
fun reset() {
|
||||
lastPosture = Posture.UNKNOWN
|
||||
lyingStartMs = 0L
|
||||
armed = true
|
||||
}
|
||||
|
||||
/** 현재 자세 + 현재 시각으로 갱신. UNKNOWN 은 직전 상태 유지. */
|
||||
fun update(posture: Posture, nowMs: Long = System.currentTimeMillis()) {
|
||||
if (posture == Posture.UNKNOWN) return
|
||||
|
||||
val prev = lastPosture
|
||||
lastPosture = posture
|
||||
|
||||
when {
|
||||
// LYING 진입 — start timestamp 기록 + 재무장
|
||||
posture == Posture.LYING && prev != Posture.LYING -> {
|
||||
lyingStartMs = nowMs
|
||||
armed = true
|
||||
}
|
||||
// LYING 유지 — 그대로
|
||||
posture == Posture.LYING -> { /* keep streak */ }
|
||||
// LYING → NON_LYING 전이 — 알람 판정
|
||||
posture == Posture.NON_LYING && prev == Posture.LYING -> {
|
||||
val elapsedMs = nowMs - lyingStartMs
|
||||
val elapsedSec = elapsedMs / 1000
|
||||
if (armed && lyingStartMs > 0 && elapsedSec >= lyingDurationSec) {
|
||||
armed = false
|
||||
onWakeup?.invoke(elapsedSec)
|
||||
}
|
||||
lyingStartMs = 0L
|
||||
}
|
||||
// NON_LYING 유지 — 그대로
|
||||
else -> { /* nothing */ }
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,152 @@
|
||||
/*
|
||||
* WalkingDetector — flowchart Path B (4-2 / 5-2) 구현.
|
||||
*
|
||||
* 알고리즘:
|
||||
* 1. Sliding window (walkingWindowMs, default 1.5초) 동안 IMU gyro sample 누적
|
||||
* 2. window 의 gyro magnitude 평균 (wmean) 과 표준편차 (wstd) 계산
|
||||
* 3. 상태 머신:
|
||||
* IDLE (not walking):
|
||||
* 조건: |pitch| > walkingMinPitchDeg
|
||||
* AND wmean > walkingStartWmean (default 4)
|
||||
* AND wstd > walkingStartWstd (default 2)
|
||||
* → 조건 만족이 walkingStartHoldMs (default 1초) 이상 지속 → WALKING
|
||||
* WALKING:
|
||||
* 조건: |pitch| > walkingMinPitchDeg
|
||||
* AND wmean > walkingKeepWmean (default 2)
|
||||
* → 조건 미달이 walkingKeepHoldMs (default 2초) 이상 지속 → IDLE
|
||||
*
|
||||
* 입력: 매 mtb cycle (≈1초 주기) 마다 IMU sample 리스트 + 현재 pitch + 시각
|
||||
* 출력: 현재 walking 상태 (Boolean) + 진단 정보
|
||||
*/
|
||||
package com.medithings.vesiscan.managers
|
||||
|
||||
import com.medithings.vesiscan.ble.ImuSample
|
||||
import kotlin.math.abs
|
||||
import kotlin.math.sqrt
|
||||
|
||||
class WalkingDetector {
|
||||
|
||||
/** 진단 결과. UI/log 용. */
|
||||
data class Snapshot(
|
||||
val isWalking: Boolean,
|
||||
val wmean: Float,
|
||||
val wstd: Float,
|
||||
val pitchOk: Boolean,
|
||||
val samplesInWindow: Int,
|
||||
val reason: String,
|
||||
)
|
||||
|
||||
private data class TsGyro(val tsMs: Long, val magDps: Float)
|
||||
|
||||
private val buffer = ArrayDeque<TsGyro>()
|
||||
private var walking = false
|
||||
/** IDLE 에서 시작 조건 처음 만족된 시각. null 이면 미만족 / 끊김. */
|
||||
private var startCondSinceMs: Long? = null
|
||||
/** WALKING 에서 유지 조건 마지막 만족 시각. */
|
||||
private var keepCondLastOkMs: Long? = null
|
||||
/** 직전 cycle 의 pitchOk — false→true 전이 시 buffer reset (sit-to-stand burst 차단). */
|
||||
private var lastPitchOk: Boolean = false
|
||||
|
||||
val isWalking: Boolean get() = walking
|
||||
|
||||
fun reset() {
|
||||
buffer.clear()
|
||||
walking = false
|
||||
startCondSinceMs = null
|
||||
keepCondLastOkMs = null
|
||||
lastPitchOk = false
|
||||
}
|
||||
|
||||
/**
|
||||
* 매 cycle 마다 호출. samples = 이번 cycle 에 들어온 IMU sample 리스트.
|
||||
* pitchDeg = 같은 cycle 의 pitch (ImuPostureClassifier 가 계산한 값).
|
||||
*/
|
||||
fun update(samples: List<ImuSample>, pitchDeg: Float, nowMs: Long): Snapshot {
|
||||
val minPitch = GreenZoneConstants.walkingMinPitchDeg
|
||||
val pitchOk = abs(pitchDeg) > minPitch
|
||||
|
||||
// Pitch false→true 전이 (sit-to-stand 등) — buffer 비움.
|
||||
// 이유: 일어남 burst 가 pitch<45° 일 때 발생 → window 에 누적 → pitch 가 통과한
|
||||
// 순간 wmean 이 이미 큰 값 → 즉시 start cond 만족 → 가짜 walking trigger.
|
||||
// pitch 가 true 가 된 직후엔 새 데이터만으로 measure 하도록.
|
||||
if (pitchOk && !lastPitchOk && !walking) {
|
||||
buffer.clear()
|
||||
startCondSinceMs = null
|
||||
}
|
||||
lastPitchOk = pitchOk
|
||||
|
||||
// 1) sample buffer 에 추가 (timestamp 단순 분배 — cycle 내 균등 간격 가정)
|
||||
val windowMs = GreenZoneConstants.walkingWindowMs
|
||||
if (samples.isNotEmpty()) {
|
||||
// cycle 내 각 sample 의 timestamp 를 nowMs 기준으로 역산 (≈ ICM ODR 50Hz → 20ms 간격)
|
||||
val sampleGapMs = 20L
|
||||
val startTs = nowMs - (samples.size - 1) * sampleGapMs
|
||||
for ((i, s) in samples.withIndex()) {
|
||||
val mag = sqrt(s.gx * s.gx + s.gy * s.gy + s.gz * s.gz)
|
||||
buffer.addLast(TsGyro(startTs + i * sampleGapMs, mag))
|
||||
}
|
||||
}
|
||||
// 2) window 밖 sample 제거
|
||||
val cutoff = nowMs - windowMs
|
||||
while (buffer.isNotEmpty() && buffer.first().tsMs < cutoff) buffer.removeFirst()
|
||||
|
||||
// 3) wmean, wstd 계산
|
||||
val n = buffer.size
|
||||
if (n < 2) {
|
||||
return Snapshot(walking, 0f, 0f, false, n, "buffer too small")
|
||||
}
|
||||
var sum = 0f
|
||||
for (b in buffer) sum += b.magDps
|
||||
val wmean = sum / n
|
||||
var sqSum = 0f
|
||||
for (b in buffer) {
|
||||
val d = b.magDps - wmean
|
||||
sqSum += d * d
|
||||
}
|
||||
val wstd = sqrt(sqSum / n)
|
||||
|
||||
// 4) state machine (pitchOk 은 위에서 이미 계산됨)
|
||||
if (!walking) {
|
||||
// IDLE → WALKING 검사 (4-2)
|
||||
val cond = pitchOk &&
|
||||
wmean > GreenZoneConstants.walkingStartWmean &&
|
||||
wstd > GreenZoneConstants.walkingStartWstd
|
||||
if (cond) {
|
||||
if (startCondSinceMs == null) startCondSinceMs = nowMs
|
||||
val held = nowMs - startCondSinceMs!!
|
||||
if (held >= GreenZoneConstants.walkingStartHoldMs) {
|
||||
walking = true
|
||||
keepCondLastOkMs = nowMs
|
||||
return Snapshot(true, wmean, wstd, pitchOk, n,
|
||||
"WALKING start (held ${held}ms)")
|
||||
}
|
||||
return Snapshot(false, wmean, wstd, pitchOk, n,
|
||||
"walking start cond met for ${held}ms")
|
||||
} else {
|
||||
startCondSinceMs = null
|
||||
return Snapshot(false, wmean, wstd, pitchOk, n,
|
||||
"idle (pitchOk=$pitchOk wmean=${"%.1f".format(wmean)} wstd=${"%.1f".format(wstd)})")
|
||||
}
|
||||
} else {
|
||||
// WALKING → IDLE 검사 (5-2)
|
||||
val cond = pitchOk && wmean > GreenZoneConstants.walkingKeepWmean
|
||||
if (cond) {
|
||||
keepCondLastOkMs = nowMs
|
||||
return Snapshot(true, wmean, wstd, pitchOk, n,
|
||||
"WALKING keep (wmean=${"%.1f".format(wmean)})")
|
||||
} else {
|
||||
// 조건 미달 — grace 시간 체크
|
||||
val sinceOk = nowMs - (keepCondLastOkMs ?: nowMs)
|
||||
if (sinceOk >= GreenZoneConstants.walkingKeepHoldMs) {
|
||||
walking = false
|
||||
startCondSinceMs = null
|
||||
keepCondLastOkMs = null
|
||||
return Snapshot(false, wmean, wstd, pitchOk, n,
|
||||
"WALKING end (grace ${sinceOk}ms exceeded)")
|
||||
}
|
||||
return Snapshot(true, wmean, wstd, pitchOk, n,
|
||||
"WALKING grace (${sinceOk}ms / ${GreenZoneConstants.walkingKeepHoldMs}ms)")
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user