Separate step mounting from Tracker

Author: ButterscotchV

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<RecordingWrapper> = 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<Tracker>) = 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
+	}
+}

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)
+	}
+}

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,
+)

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<RecordingSample>(8)
+
+	// List capacity assuming ~10 seconds at 100 TPS
+	val recording: MutableList<RecordingSample> = 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
+	}
+}