diff --git a/server/core/src/main/java/dev/slimevr/reset/accel/AccelResetHandler.kt b/server/core/src/main/java/dev/slimevr/reset/accel/AccelResetHandler.kt new file mode 100644 index 000000000..71d0e607d --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/reset/accel/AccelResetHandler.kt @@ -0,0 +1,237 @@ +package dev.slimevr.reset.accel + +import dev.slimevr.tracking.trackers.Tracker +import dev.slimevr.util.AccelAccumulator +import io.eiren.util.logging.LogManager +import io.github.axisangles.ktmath.Vector3 +import java.util.Timer +import java.util.TimerTask +import java.util.concurrent.locks.ReentrantLock +import kotlin.concurrent.schedule +import kotlin.concurrent.thread +import kotlin.concurrent.withLock +import kotlin.time.Duration.Companion.seconds +import kotlin.time.DurationUnit +import kotlin.time.TimeSource + +// Handles recording and processing of acceleration-based session calibration +class AccelResetHandler(val timeSource: TimeSource.WithComparableMarks = TimeSource.Monotonic) { + var isRunning: Boolean = false + private set + var isDetecting: Boolean = false + private set + var isRecording: Boolean = false + private set + + private val recordingLock = ReentrantLock() + + private var hmd: Tracker? = null + private val trackers: MutableList = mutableListOf() + + private val timeoutTimer = Timer() + private var timerTask: TimerTask? = null + + private var recStartTime = timeSource.markNow() + + /** + * Starts the accel reset process. performing rest detection on the trackers + * provided to automatically control the recording period. + */ + fun start(hmd: Tracker, trackers: Iterable) = recordingLock.withLock { + // Maybe should throw IllegalStateException? Or just restart? + if (isRunning) return + + // Nothing to do + if (trackers.none()) return + + // Initialize our state + isRunning = true + this.hmd = hmd + + // Register our tracker event listener + for (tracker in trackers) { + val wrappedTracker = RecordingWrapper(tracker) + this.trackers.add(wrappedTracker) + tracker.accelTickCallback = { + onAccelData(wrappedTracker) + } + } + + // Start waiting for movement + isDetecting = true + timerTask?.cancel() + timerTask = timeoutTimer.schedule(START_TIMEOUT.inWholeMilliseconds) { + timeout() + } + + LogManager.info("[AccelResetHandler] Reset requested, detecting movement...") + } + + /** + * Handles rest detection and data collection. + */ + private fun onAccelData(tracker: RecordingWrapper) { + if (!isDetecting) return + + val sample = tracker.makeSample(timeSource.markNow(), hmd?.position ?: Vector3.NULL) + + // Rest detection + tracker.updateRestState(sample) + tracker.addRestSample(sample) + // TODO: This shouldn't be done like this + tracker.tracker.accelMountInProgress = isRecording && tracker.moving + + if (!isRecording) { + // We haven't started moving yet, don't start recording + if (!tracker.moving) return + + // Start recording + recordingLock.withLock { + // Race condition + if (isRecording) return@withLock + + // Dump rest detection into the recording on tracker threads + for (tracker in trackers) tracker.dumpRest = true + + isRecording = true + recStartTime = timeSource.markNow() + timerTask?.cancel() + timerTask = timeoutTimer.schedule(RECORD_TIMEOUT.inWholeMilliseconds) { + timeout() + } + + LogManager.info("[AccelResetHandler] Movement detected, recording started!") + } + } else if ( + timeSource.markNow() - recStartTime > MINIMUM_DURATION && + trackers.none { it.moving } + ) { + // We're recording, the minimum duration has passed, and no trackers are + // moving, therefore we can stop the recording and process it + recordingLock.withLock { + // Race condition + if (!isRecording) return + // Let's not block the tracker thread while processing + thread { + process() + } + } + return + } + + // Take the latest sample or dump the rest detection samples into the recording + if (!tracker.dumpRest) { + tracker.recording.add(sample) + } else { + tracker.recording.addAll(tracker.restDetect) + tracker.dumpRest = false + } + } + + /** + * Stops recording, processes the recorded data, then resets this handler. + */ + private fun process() { + stop() + + LogManager.info("[AccelResetHandler] Done recording, processing...") + + for (tracker in trackers) { + val firstSample = tracker.recording.first() + val lastSample = tracker.recording.last() + + // Compute the unbiased final velocity + val calibAccum = AccelAccumulator() + RecordingProcessor.processTimeline(calibAccum, tracker) + + // Assume the final velocity is zero (at rest), we can divide our unbiased + // final velocity (m/s) by the duration and get a static acceleration + // offset (m/s^2) + val duration = lastSample.time - firstSample.time + val bias = calibAccum.velocity / duration.toDouble(DurationUnit.SECONDS).toFloat() + + // Compute the biased final offset + val finalAccum = AccelAccumulator() + RecordingProcessor.processTimeline(finalAccum, tracker, accelBias = bias) + + // Compute the final offsets + val trackerOffset = finalAccum.offset + val trackerXZ = Vector3(trackerOffset.x, 0f, trackerOffset.z) + val hmdOffset = lastSample.hmdPos - firstSample.hmdPos + val hmdXZ = Vector3(hmdOffset.x, 0f, hmdOffset.z) + + // TODO: Fail on high error + + // Compute mounting to fix the yaw offset from tracker to HMD + val mountRot = RecordingProcessor.angle(trackerXZ.unit()) * + RecordingProcessor.angle(hmdXZ.unit()).inv() + + // Apply that mounting to the tracker + val resetsHandler = tracker.tracker.resetsHandler + val finalMounting = resetsHandler.mountingOrientation * resetsHandler.mountRotFix * mountRot + resetsHandler.mountRotFix *= mountRot + + LogManager.info( + "[Accel] Tracker ${tracker.tracker.id} (${tracker.tracker.trackerPosition?.designation}):\n" + + "Tracker offset: $trackerOffset\n" + + "HMD offset: $hmdOffset\n" + + "Error value (meters): ${trackerXZ.len() - hmdXZ.len()}\n" + + "Resulting mounting: $finalMounting", + ) + } + + clean() + } + + /** + * Stops recording without clearing the recorded data. + */ + private fun stop() = recordingLock.withLock { + // Cancel any pending timeouts + timerTask?.cancel() + timerTask = null + + isDetecting = false + isRecording = false + + // Unregister our tracker event listener + for (tracker in trackers) { + tracker.tracker.accelTickCallback = null + tracker.tracker.accelMountInProgress = false + } + } + + /** + * Immediately stops execution and resets this handler. + */ + private fun clean() { + stop() + + // Reset data storage + hmd = null + trackers.clear() + + isRunning = false + } + + /** + * Stops the accel reset process and resets this handler. + */ + fun cancel() { + clean() + } + + /** + * Indicates that the process has timed out, then resets this handler. + */ + private fun timeout() { + LogManager.warning("[AccelResetHandler] Reset timed out, aborting") + clean() + } + + companion object { + val START_TIMEOUT = 8.seconds + val MINIMUM_DURATION = 2.seconds + val RECORD_TIMEOUT = 8.seconds + } +} diff --git a/server/core/src/main/java/dev/slimevr/reset/accel/RecordingProcessor.kt b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingProcessor.kt new file mode 100644 index 000000000..df7c9858e --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingProcessor.kt @@ -0,0 +1,44 @@ +package dev.slimevr.reset.accel + +import dev.slimevr.util.AccelAccumulator +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import kotlin.math.atan2 +import kotlin.time.ComparableTimeMark +import kotlin.time.Duration +import kotlin.time.DurationUnit + +object RecordingProcessor { + fun accumSample( + accum: AccelAccumulator, + sample: RecordingSample, + lastSampleTime: ComparableTimeMark? = null, + accelBias: Vector3 = Vector3.NULL, + ): Duration { + val delta = lastSampleTime?.let { sample.time - it } ?: Duration.ZERO + accum.dataTick(sample.accel - accelBias, delta.toDouble(DurationUnit.SECONDS).toFloat()) + + return delta + } + + fun processTimeline( + accum: AccelAccumulator, + wrapper: RecordingWrapper, + lastSampleTime: ComparableTimeMark? = null, + accelBias: Vector3 = Vector3.NULL, + ): ComparableTimeMark? { + var lastTime = lastSampleTime + + for (sample in wrapper.recording) { + accumSample(accum, sample, lastTime, accelBias) + lastTime = sample.time + } + + return lastTime + } + + fun angle(vector: Vector3): Quaternion { + val yaw = atan2(vector.x, vector.z) + return Quaternion.rotationAroundYAxis(yaw) + } +} diff --git a/server/core/src/main/java/dev/slimevr/reset/accel/RecordingSample.kt b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingSample.kt new file mode 100644 index 000000000..fcd40a3b0 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingSample.kt @@ -0,0 +1,14 @@ +package dev.slimevr.reset.accel + +import io.github.axisangles.ktmath.Quaternion +import io.github.axisangles.ktmath.Vector3 +import kotlin.time.ComparableTimeMark + +data class RecordingSample( + val time: ComparableTimeMark, + // Tracker + val accel: Vector3, + val rot: Quaternion, + // HMD + val hmdPos: Vector3, +) diff --git a/server/core/src/main/java/dev/slimevr/reset/accel/RecordingWrapper.kt b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingWrapper.kt new file mode 100644 index 000000000..00596ab36 --- /dev/null +++ b/server/core/src/main/java/dev/slimevr/reset/accel/RecordingWrapper.kt @@ -0,0 +1,63 @@ +package dev.slimevr.reset.accel + +import dev.slimevr.autobone.StatsCalculator +import dev.slimevr.tracking.trackers.Tracker +import io.github.axisangles.ktmath.Vector3 +import org.apache.commons.collections4.queue.CircularFifoQueue +import kotlin.time.ComparableTimeMark +import kotlin.time.Duration.Companion.milliseconds + +data class RecordingWrapper(val tracker: Tracker, var moving: Boolean = false) { + // Buffer for performing rest detection + val restDetect = CircularFifoQueue(8) + + // List capacity assuming ~10 seconds at 100 TPS + val recording: MutableList = ArrayList(1024) + + // Whether to dump our rest detection into the recording on the next sample + var dumpRest = false + + fun makeSample(time: ComparableTimeMark, hmdPos: Vector3): RecordingSample = RecordingSample( + time, + tracker.getAcceleration(), + tracker.getRotation(), + hmdPos, + ) + + fun addRestSample(sample: RecordingSample): Boolean { + // Collect samples for rest detection at a constant-ish rate if possible + return if (moving && restDetect.isNotEmpty()) { + val lastSampleTime = restDetect.last().time + // Try to have TPS at a lower rate + if (sample.time - lastSampleTime > REST_INTERVAL) { + restDetect.add(sample) + } else { + false + } + } else { + restDetect.add(sample) + } + } + + fun updateRestState(new: RecordingSample): Boolean { + if (restDetect.size < 4) return moving + + val stats = StatsCalculator() + for (sample in restDetect) { + stats.addValue(sample.accel.len()) + } + + // Conditions to start or remain moving + // TODO: Add rotation as a rest metric + moving = if (moving) { + stats.mean >= 0.1f || stats.standardDeviation >= 0.2f + } else { + stats.mean >= 0.3f || new.accel.len() - stats.mean >= 0.6f + } + return moving + } + + companion object { + val REST_INTERVAL = 100.milliseconds + } +} diff --git a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt index 2baef0bcd..d157bc0d4 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.kt @@ -3,6 +3,7 @@ package dev.slimevr.tracking.processor.skeleton import dev.slimevr.VRServer import dev.slimevr.config.MountingMethods import dev.slimevr.config.StayAlignedConfig +import dev.slimevr.reset.accel.AccelResetHandler import dev.slimevr.tracking.processor.Bone import dev.slimevr.tracking.processor.BoneType import dev.slimevr.tracking.processor.Constraint @@ -217,6 +218,8 @@ class HumanSkeleton( var ikSolver = IKSolver(headBone) var userHeightCalibration: UserHeightCalibration? = null + val accelResetHandler = AccelResetHandler() + // Stay Aligned var trackerSkeleton = TrackerSkeleton(this) var stayAlignedConfig = StayAlignedConfig() @@ -1621,6 +1624,29 @@ class HumanSkeleton( return } + // TODO: Make this not dumb + if (headTracker?.resetsHandler?.stepMounting == true || + trackersToReset.any { it?.resetsHandler?.stepMounting == true } + ) { + headTracker?.let { hmd -> + // Make sure we have HMD position + if (!hmd.hasPosition) { + return@let + } + + // Start step mounting + accelResetHandler.start( + hmd, + trackersToReset.filterNotNull().filter { + it.allowMounting && (bodyParts.isEmpty() || bodyParts.contains(it.trackerPosition?.bodyPart)) + }, + ) + return + } + LogManager.info("[HumanSkeleton] Reset: mounting ($resetSourceName) failed, HMD is not available") + return + } + // Resets the mounting orientation of the trackers with the HMD as reference. var referenceRotation = IDENTITY headTracker?.let { diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt index 8e6495b2b..a40b65627 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt @@ -1,9 +1,7 @@ package dev.slimevr.tracking.trackers import dev.slimevr.VRServer -import dev.slimevr.autobone.StatsCalculator import dev.slimevr.config.TrackerConfig -import dev.slimevr.filtering.CircularArrayList import dev.slimevr.tracking.processor.stayaligned.trackers.StayAlignedTrackerState import dev.slimevr.tracking.trackers.TrackerPosition.Companion.getByDesignation import dev.slimevr.tracking.trackers.udp.IMUType @@ -11,12 +9,8 @@ import dev.slimevr.tracking.trackers.udp.MagnetometerStatus import dev.slimevr.tracking.trackers.udp.TrackerDataType import dev.slimevr.util.InterpolationHandler import io.eiren.util.BufferedTimer -import io.eiren.util.collections.FastList -import io.eiren.util.logging.LogManager import io.github.axisangles.ktmath.Quaternion import io.github.axisangles.ktmath.Vector3 -import kotlin.math.abs -import kotlin.math.atan2 import kotlin.properties.Delegates const val TIMEOUT_MS = 2_000L @@ -182,6 +176,7 @@ class Tracker @JvmOverloads constructor( // Currently only used for accel resets, to add anything else, consider using a // subscribable event listener instead var accelTickCallback: ((tracker: Tracker) -> Unit)? = null + var accelMountInProgress = false init { // IMPORTANT: Look here for the required states of inputs @@ -197,18 +192,6 @@ class Tracker @JvmOverloads constructor( // require(device != null && _trackerNum == null) { // "If ${::device.name} exists, then ${::trackerNum.name} must not be null" // } - /* - if (!isInternal && isImu()) { - csv = File("C:/Users/Butterscotch/Desktop/Tracker Accel", "tracker_$id.csv") - csvOut = csv.writer() - - LogManager.info("Starting recording (probably)") - csvOut.write("Time (ms),Acceleration X,Acceleration Y,Acceleration Z,Acceleration Magnitude,Velocity X,Velocity Y,Velocity Z,Velocity Magnitude,Position X,Position Y,Position Z,HMD Position X,HMD Position Y,HMD Position Z\n") - } else { - csv = null - csvOut = null - } - */ } /** @@ -288,82 +271,6 @@ class Tracker @JvmOverloads constructor( stayAligned.update() } - val minDur = 2000L - var startTime = System.currentTimeMillis() - - data class AccelSample(val time: Long, val accel: Vector3, val hmdPos: Vector3) - data class AccelTimeline(val resting: Boolean, val samples: FastList = FastList()) - - var lastFrameRest = true - var curFrameRest = true - - val lastSamples = CircularArrayList(8) - var curTimeline: AccelTimeline? = null - - var accelMountInProgress = false - - fun accumSample(accum: AccelAccumulator, sample: AccelSample, lastSampleTime: Long = -1, accelBias: Vector3 = Vector3.NULL): Float { - val delta = if (lastSampleTime >= 0) { - (sample.time - lastSampleTime) / 1000f - } else { - 0f - } - accum.dataTick(sample.accel - accelBias, delta) - - return delta - } - - fun processTimeline(accum: AccelAccumulator, timeline: AccelTimeline, lastSampleTime: Long = -1, accelBias: Vector3 = Vector3.NULL, action: (accum: AccelAccumulator, sample: AccelSample, delta: Float) -> Unit = { _, _, _ -> }): Long { - // If -1, assume we are at the start - var lastTime = lastSampleTime - - for (sample in timeline.samples) { - val delta = accumSample(accum, sample, lastTime, accelBias) - action(accum, sample, delta) - lastTime = sample.time - } - - return lastTime - } - - fun processRest(accum: AccelAccumulator, timeline: AccelTimeline, lastSampleTime: Long = -1): Pair { - val sampleCount = timeline.samples.size.toFloat() - var avgY = Vector3.NULL - - val lastTime = processTimeline(accum, timeline, lastSampleTime) { accum, _, _ -> - avgY += accum.velocity / sampleCount - } - - return Pair(lastTime, avgY) - } - - fun writeTimeline(accum: AccelAccumulator, timeline: AccelTimeline, lastSampleTime: Long = -1, accelBias: Vector3 = Vector3.NULL): Long { - // Accel position is only the offset, so let's make the HMD an offset too - val initHmd = timeline.samples.first().hmdPos - - val time = processTimeline(accum, timeline, lastSampleTime, accelBias) { accum, sample, _ -> - val time = sample.time - val accel = accum.acceleration - val vel = accum.velocity - val pos = accum.offset - val hmd = sample.hmdPos - initHmd - - // csvOut?.write("$time,${accel.x},${accel.y},${accel.z},${accel.len()},${vel.x},${vel.y},${vel.z},${vel.len()},${pos.x},${pos.y},${pos.z},${hmd.x},${hmd.y},${hmd.z}\n") - } - - return time - } - - fun angle(vector: Vector3): Quaternion { - val yaw = atan2(vector.x, vector.z) - return Quaternion.rotationAroundYAxis(yaw) - } - - fun startMounting() { - accelMountInProgress = true - startTime = System.currentTimeMillis() - } - /** * Tells the tracker that it received new data * NOTE: Use only when rotation is received @@ -374,122 +281,6 @@ class Tracker @JvmOverloads constructor( if (trackRotDirection) { filteringHandler.dataTick(getAdjustedRotation()) } - - if (accelMountInProgress) { - lastFrameRest = curFrameRest - - val accel = getAcceleration() - val accelLen = accel.len() - val hmdPos = if (VRServer.instanceInitialized) { - VRServer.instance.humanPoseManager.skeleton.headTracker?.position ?: Vector3.NULL - } else { - Vector3.NULL - } - val sample = AccelSample(timeAtLastUpdate - startTime, accel, hmdPos) - - // Ensure a minimum sample size, assume resting at start - if (lastSamples.size >= 4) { - val stats = StatsCalculator() - for (sample in lastSamples) { - stats.addValue(sample.accel.len()) - } - - curFrameRest = if (curFrameRest) { - stats.mean < 0.3f && accelLen - stats.mean < 0.6f - } else { - stats.mean < 0.1f && stats.standardDeviation < 0.2f && sample.time >= minDur - } - } - - // On rest state change - if (curFrameRest != lastFrameRest) { - if (curFrameRest) { - LogManager.info("[Accel] Tracker $id (${trackerPosition?.designation}) is now eepy.") - - curTimeline?.let { move -> - val firstSample = move.samples.first() - val lastSample = move.samples.last() - - val calibAccum = AccelAccumulator() - processTimeline(calibAccum, move) - - val moveTime = lastSample.time - firstSample.time - val postAvg = calibAccum.velocity - - // Assume the velocity at the end is the resting velocity - val slope = postAvg / (moveTime / 1000f) - // LogManager.info("moveTime: $moveTime\npostAvg: $postAvg\nslope: $slope") - - val outAccum = AccelAccumulator() - processTimeline(outAccum, move, accelBias = slope) - - // We need to compare offsets of HMD and tracker - val hmdOff = lastSample.hmdPos - firstSample.hmdPos - val trackerOff = outAccum.offset - - val hmd = Vector3(hmdOff.x, 0f, hmdOff.z) - val tracker = Vector3(trackerOff.x, 0f, trackerOff.z) - - val hmdRot = angle(hmd.unit()) - val trackerRot = angle(tracker.unit()) - val mountRot = trackerRot * hmdRot.inv() - - val mountVec = (resetsHandler.mountingOrientation * resetsHandler.mountRotFix * mountRot).inv().sandwich(Vector3.POS_Z) - val mountText = if (abs(mountVec.z) > abs(mountVec.x)) { - if (mountVec.z < 0f) { - "front" - } else { - "back" - } - } else { - if (mountVec.x > 0f) { - "right" - } else { - "left" - } - } - - LogManager.info("[Accel] Tracker $id (${trackerPosition?.designation}):\nTracker: $trackerOff\nHmd: $hmdOff\nErr: ${tracker.len() - hmd.len()}\nResult: $mountVec ($mountText)") - resetsHandler.mountRotFix *= mountRot - accelMountInProgress = false - } - curTimeline = null - } else { - LogManager.info("[Accel] Tracker $id (${trackerPosition?.designation}) now has zoomies!") - - // Cycle timeline - curTimeline = AccelTimeline(false) - for (sample in lastSamples) { - curTimeline?.samples?.add(sample) - } - } - - // Flush rest detection - lastSamples.clear() - } - - // Moving avg accel for rest detection - if (lastSamples.size == lastSamples.capacity()) { - lastSamples.removeLast() - } - - // Collect samples for rest detection at a constant-ish rate if possible - if (curFrameRest) { - lastSamples.add(sample) - } else { - // Collect the latest samples when moving - curTimeline?.samples?.add(sample) - - if (lastSamples.isNotEmpty()) { - // Try to have TPS at a lower rate - if (sample.time - lastSamples.first().time > 100) { - lastSamples.add(sample) - } - } else { - lastSamples.add(sample) - } - } - } } /** diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index c75013ac5..8c511c7c1 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -407,11 +407,6 @@ class TrackerResetsHandler(val tracker: Tracker) { return } - if (stepMounting) { - tracker.startMounting() - return - } - constraintFix = Quaternion.IDENTITY // Get the current calibrated rotation diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/AccelAccumulator.kt b/server/core/src/main/java/dev/slimevr/util/AccelAccumulator.kt similarity index 93% rename from server/core/src/main/java/dev/slimevr/tracking/trackers/AccelAccumulator.kt rename to server/core/src/main/java/dev/slimevr/util/AccelAccumulator.kt index 68e6cfdd1..9ad0c816a 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/AccelAccumulator.kt +++ b/server/core/src/main/java/dev/slimevr/util/AccelAccumulator.kt @@ -1,4 +1,4 @@ -package dev.slimevr.tracking.trackers +package dev.slimevr.util import com.jme3.system.NanoTimer import io.github.axisangles.ktmath.Vector3