Skip to content

Revert TrackerResetsHandler & add comments #1748

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
ButterscotchV wants to merge 2 commits into
base: main
Choose a base branch
from
+57 −50
Open

Revert TrackerResetsHandler & add comments #1748

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
61c99e0
Revert TrackerResetsHandler & add comments
ButterscotchV Feb 14, 2026
67d4b5a
Also revert mounting tests
ButterscotchV Feb 14, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
101 changes: 54 additions & 47 deletions server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,6 @@ class TrackerResetsHandler(val tracker: Tracker) {
Math.PI.toFloat(),
0f,
).toQuaternion()
private val QuarterPitch = Quaternion.rotationAroundXAxis(FastMath.HALF_PI)

private var driftAmount = 0f
private var averagedDriftQuat = Quaternion.IDENTITY
private var rotationSinceReset = Quaternion.IDENTITY
Expand Down Expand Up @@ -54,6 +52,16 @@ class TrackerResetsHandler(val tracker: Tracker) {

// Reference adjustment quats

/**
* Gyro fix is set by full reset. This sets the current y rotation to 0, correcting
* for initial yaw rotation and the rotation incurred by mounting orientation. This
* is a local offset in rotation and does not affect the axes of rotation.
*
* This rotation is only used to compute [attachmentFix], otherwise [yawFix] would
* correct for the same rotation.
*/
private var gyroFix = Quaternion.IDENTITY

/**
* Attachment fix is set by full reset. This sets the current x and z rotations to
* 0, correcting for initial pitch and roll rotation. This is a global offset in
Expand Down Expand Up @@ -180,9 +188,12 @@ class TrackerResetsHandler(val tracker: Tracker) {
/**
* Get the reference adjusted accel.
*/
// All IMU axis corrections are inverse to undo `adjustToReference` after local yaw offsets are added
// Order is VERY important here! Please be extremely careful! >~>
fun getReferenceAdjustedAccel(rawRot: Quaternion, accel: Vector3): Vector3 = (adjustToReference(rawRot) * (attachmentFix * mountingOrientation * mountRotFix * tposeDownFix).inv()).sandwich(accel)
// TODO: Make this actually adjusted to the corrected IMU heading. The current
// implementation for heading correction doesn't appear to be correct and may simply
// make acceleration worse, so I'm just leaving this until we work that out. The
// output of this will be world space, but with an unknown offset to heading (yaw).
// - Butterscotch
fun getReferenceAdjustedAccel(rawRot: Quaternion, accel: Vector3): Vector3 = rawRot.sandwich(accel)

/**
* Converts raw or filtered rotation into reference- and
Expand All @@ -191,17 +202,27 @@ class TrackerResetsHandler(val tracker: Tracker) {
*/
private fun adjustToReference(rotation: Quaternion): Quaternion {
var rot = rotation
// Correct for global pitch/roll offset
rot *= attachmentFix
// Correct for global yaw offset without affecting local yaw so we can change this
// later without invalidating local yaw offset corrections
// Align heading axis with bone space (order invariant with other yaw-only
// operations, but should generally be done alongside attitude alignment)
if (!tracker.isHmd || tracker.trackerPosition != TrackerPosition.HEAD) {
rot = mountingOrientation.inv() * rot * mountingOrientation
rot *= mountingOrientation
}
rot = mountRotFix.inv() * rot * mountRotFix
// T-pose global correction
// IMU space heading correction assuming manual orientation is correct
rot = gyroFix * rot
// Align attitude axes with bone space
rot *= attachmentFix
// Secondary heading axis alignment with bone space for automatic mounting,
// this comes after attitude axes' alignment so it may be changed later without
// also changing the attitude axes' alignment quaternion
// Note: Prepare for unforeseen consequences when applying an inverse amount of
// heading correction corresponding to the axis alignment quaternion
rot = mountRotFix.inv() * (rot * mountRotFix)
// More attitude axes alignment specifically for the t-pose configuration, this
// probably shouldn't be a separate variable from attachmentFix?
rot *= tposeDownFix
// Align local yaw with reference
// More IMU heading correction, but in bone space now? This is really weird and
// bad, don't do this in the future
// TODO: Make this in IMU space, where drift actually occurs - Butterscotch
rot = yawFix * rot
rot = constraintFix * rot
return rot
Expand All @@ -211,6 +232,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
* Converts raw or filtered rotation into zero-reference-adjusted by
* applying quaternions produced after full reset and yaw reset only
*/
// This is essentially just adjustToReference but aligning to quaternion identity
// rather than to the bone.
private fun adjustToIdentity(rotation: Quaternion): Quaternion {
var rot = rotation
rot = gyroFixNoMounting * rot
Expand Down Expand Up @@ -265,23 +288,15 @@ class TrackerResetsHandler(val tracker: Tracker) {
lastResetQuaternion = oldRot

// Adjust raw rotation to mountingOrientation
val rotation = tracker.getRawRotation()
val mountingAdjustedRotation = tracker.getRawRotation() * mountingOrientation

// Gyrofix
val gyroFix = if (tracker.allowMounting || (tracker.trackerPosition == TrackerPosition.HEAD && !tracker.isHmd)) {
if (tracker.isComputed) {
fixGyroscope(rotation)
if (tracker.allowMounting || (tracker.trackerPosition == TrackerPosition.HEAD && !tracker.isHmd)) {
gyroFix = if (tracker.isComputed) {
fixGyroscope(tracker.getRawRotation())
} else {
if (tracker.trackerPosition.isFoot()) {
// Feet are rotated by 90 deg pitch, this means we're relying on IMU rotation
// to be set correctly here.
fixGyroscope(rotation * tposeDownFix * QuarterPitch)
} else {
fixGyroscope(rotation * tposeDownFix)
}
fixGyroscope(mountingAdjustedRotation * tposeDownFix)
}
} else {
Quaternion.IDENTITY
}

// Mounting for computed trackers
Expand All @@ -296,15 +311,15 @@ class TrackerResetsHandler(val tracker: Tracker) {
if (resetHmdPitch) {
// Reset the HMD's pitch if it's assigned to head and resetHmdPitch is true
// Get rotation without yaw (make sure to use the raw rotation directly!)
val rotBuf = getYawQuaternion(rotation).inv() * rotation
val rotBuf = getYawQuaternion(tracker.getRawRotation()).inv() * tracker.getRawRotation()
// Isolate pitch
Quaternion(rotBuf.w, -rotBuf.x, 0f, 0f).unit()
} else {
// Don't reset the HMD at all
Quaternion.IDENTITY
}
} else {
(gyroFix * rotation).inv()
fixAttachment(mountingAdjustedRotation)
}

// Rotate attachmentFix by 180 degrees as a workaround for t-pose (down)
Expand All @@ -316,7 +331,7 @@ class TrackerResetsHandler(val tracker: Tracker) {

// Don't adjust yaw if head and computed
if (tracker.trackerPosition != TrackerPosition.HEAD || !tracker.isComputed) {
yawFix = gyroFix * reference.project(Vector3.POS_Y).unit()
yawFix = fixYaw(mountingAdjustedRotation, reference)
tracker.yawResetSmoothing.reset()
}

Expand Down Expand Up @@ -360,7 +375,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
lastResetQuaternion = oldRot

val yawFixOld = yawFix
yawFix = fixYaw(tracker.getRawRotation(), reference)
yawFix = fixYaw(tracker.getRawRotation() * mountingOrientation, reference)
tracker.yawResetSmoothing.reset()

makeIdentityAdjustmentQuatsYaw()
Expand Down Expand Up @@ -399,9 +414,9 @@ class TrackerResetsHandler(val tracker: Tracker) {
constraintFix = Quaternion.IDENTITY

// Get the current calibrated rotation
var rotBuf = adjustToDrift(tracker.getRawRotation())
var rotBuf = adjustToDrift(tracker.getRawRotation() * mountingOrientation)
rotBuf = gyroFix * rotBuf
rotBuf *= attachmentFix
rotBuf = mountingOrientation.inv() * rotBuf * mountingOrientation
rotBuf = yawFix * rotBuf

// Adjust buffer to reference
Expand Down Expand Up @@ -457,22 +472,14 @@ class TrackerResetsHandler(val tracker: Tracker) {
mountRotFix = Quaternion.IDENTITY
}

// EulerOrder.YXZ is actually better for gyroscope fix, as it can get yaw at any roll.
// Consequentially, instead of the roll being limited, the pitch is limited to
// 90 degrees from the yaw plane. This means trackers may be mounted upside down
// or with incorrectly configured IMU rotation, but we will need to compensate for
// the pitch.
private fun fixGyroscope(sensorRotation: Quaternion): Quaternion = getYawQuaternion(sensorRotation, EulerOrder.YXZ).inv()
private fun fixGyroscope(sensorRotation: Quaternion): Quaternion = getYawQuaternion(sensorRotation).inv()

private fun fixAttachment(sensorRotation: Quaternion): Quaternion = (gyroFix * sensorRotation).inv()

private fun fixYaw(sensorRotation: Quaternion, reference: Quaternion): Quaternion {
var rot = sensorRotation * attachmentFix
// We need to fix the global yaw offset for the euler yaw calculation
if (!tracker.isHmd || tracker.trackerPosition != TrackerPosition.HEAD) {
rot = mountingOrientation.inv() * rot * mountingOrientation
}
rot = mountRotFix.inv() * rot * mountRotFix
// TODO: Get diff from ref to rot, use euler angle (YZX) yaw as output.
// This prevents pitch and roll from affecting the alignment.
var rot = gyroFix * sensorRotation
rot *= attachmentFix
rot = mountRotFix.inv() * (rot * mountRotFix)
rot = getYawQuaternion(rot)
return rot.inv() * reference.project(Vector3.POS_Y).unit()
}
Expand All @@ -483,7 +490,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
// In both cases, the isolated yaw value changes
// with the tracker's roll when pointing forward.
// calling twinNearest() makes sure this rotation has the wanted polarity (+-).
private fun getYawQuaternion(rot: Quaternion, order: EulerOrder = EulerOrder.YZX): Quaternion = EulerAngles(order, 0f, rot.toEulerAngles(order).y, 0f).toQuaternion().twinNearest(rot)
private fun getYawQuaternion(rot: Quaternion): Quaternion = EulerAngles(EulerOrder.YZX, 0f, rot.toEulerAngles(EulerOrder.YZX).y, 0f).toQuaternion().twinNearest(rot)

private fun makeIdentityAdjustmentQuatsFull() {
val sensorRotation = tracker.getRawRotation()
Expand Down
6 changes: 3 additions & 3 deletions server/core/src/test/java/dev/slimevr/unit/SkeletonResetTests.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,13 +93,13 @@ class SkeletonResetTests {
TrackerPosition.HIP,
TrackerPosition.LEFT_LOWER_LEG,
TrackerPosition.RIGHT_LOWER_LEG,
-> mountRot
-> mountRot * Quaternion.SLIMEVR.FRONT

TrackerPosition.LEFT_UPPER_LEG,
TrackerPosition.RIGHT_UPPER_LEG,
-> mountRot * Quaternion.SLIMEVR.FRONT
-> mountRot

else -> mountRot * Quaternion.SLIMEVR.FRONT
else -> mountRot
}
val actualMounting = tracker.resetsHandler.mountRotFix

Expand Down