Skip to content

Add tests that define & prove our calibration math #1768

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

Draft
ButterscotchV wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Add tests that define & prove our calibration math #1768

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
7398479
Rename TrackerTestUtils methods & add quat/vec
ButterscotchV Feb 24, 2026
7a09d31
Reset theory tests
ButterscotchV Mar 1, 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
57 changes: 29 additions & 28 deletions server/core/src/test/java/dev/slimevr/unit/MountingResetTests.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,9 +4,10 @@ import com.jme3.math.FastMath
import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
import dev.slimevr.tracking.trackers.Tracker
import dev.slimevr.tracking.trackers.udp.IMUType
import dev.slimevr.unit.TrackerTestUtils.assertAnglesApproxEqual
import dev.slimevr.unit.TrackerTestUtils.deg
import dev.slimevr.unit.TrackerTestUtils.yaw
import dev.slimevr.unit.TrackerTestUtils.assertAngleEquals
import dev.slimevr.unit.TrackerTestUtils.degYaw
import dev.slimevr.unit.TrackerTestUtils.radToDeg
import dev.slimevr.unit.TrackerTestUtils.radYaw
import io.github.axisangles.ktmath.EulerAngles
import io.github.axisangles.ktmath.EulerOrder
import io.github.axisangles.ktmath.Quaternion
Expand All @@ -26,7 +27,7 @@ class MountingResetTests {
fun testResetAndMounting(): List<DynamicTest> = TrackerTestUtils.directions.flatMap { e ->
TrackerTestUtils.directions.map { m ->
DynamicTest.dynamicTest(
"Full and Mounting Reset Test of Tracker (Expected: ${deg(e)}, reference: ${deg(m)})",
"Full and Mounting Reset Test of Tracker (Expected: ${degYaw(e)}, reference: ${degYaw(m)})",
) {
checkResetMounting(e, m)
}
Expand Down Expand Up @@ -56,12 +57,12 @@ class MountingResetTests {
tracker.setRotation(trackerRot)
tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)

val expectedYaw = yaw(expected)
val resultYaw = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw = radYaw(expected)
val resultYaw = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw,
resultYaw,
"Resulting mounting yaw after full reset is not equal to reference yaw (${deg(expectedYaw)} vs ${deg(resultYaw)})",
message = "Resulting mounting yaw after full reset is not equal to reference yaw (${radToDeg(expectedYaw)} vs ${radToDeg(resultYaw)})",
)

// Apply full reset and mounting plus offset
Expand All @@ -73,12 +74,12 @@ class MountingResetTests {
// it needs to be applied twice
tracker.resetsHandler.resetMounting(reference * reference)

val expectedYaw2 = yaw(expected)
val resultYaw2 = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw2 = radYaw(expected)
val resultYaw2 = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw2,
resultYaw2,
"Resulting mounting yaw after full reset with offset is not equal to reference yaw (${deg(expectedYaw2)} vs ${deg(resultYaw2)})",
message = "Resulting mounting yaw after full reset with offset is not equal to reference yaw (${radToDeg(expectedYaw2)} vs ${radToDeg(resultYaw2)})",
)

// Apply yaw reset and mounting
Expand All @@ -88,12 +89,12 @@ class MountingResetTests {
tracker.setRotation(trackerRot)
tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)

val expectedYaw3 = yaw(expected)
val resultYaw3 = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw3 = radYaw(expected)
val resultYaw3 = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw3,
resultYaw3,
"Resulting mounting yaw after yaw reset is not equal to reference yaw (${deg(expectedYaw3)} vs ${deg(resultYaw3)})",
message = "Resulting mounting yaw after yaw reset is not equal to reference yaw (${radToDeg(expectedYaw3)} vs ${radToDeg(resultYaw3)})",
)

// Apply yaw reset and mounting plus offset
Expand All @@ -106,12 +107,12 @@ class MountingResetTests {
// it needs to be applied twice
tracker.resetsHandler.resetMounting(reference * reference)

val expectedYaw4 = yaw(expected)
val resultYaw4 = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw4 = radYaw(expected)
val resultYaw4 = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw3,
resultYaw3,
"Resulting mounting yaw after yaw reset with offset is not equal to reference yaw (${deg(expectedYaw4)} vs ${deg(resultYaw4)})",
message = "Resulting mounting yaw after yaw reset with offset is not equal to reference yaw (${radToDeg(expectedYaw4)} vs ${radToDeg(resultYaw4)})",
)
}

Expand Down Expand Up @@ -141,23 +142,23 @@ class MountingResetTests {
tracker.setRotation(trackerRot)
tracker.resetsHandler.resetMounting(Quaternion.IDENTITY)

val expectedYaw = yaw(expected)
val resultYaw = yaw(tracker.resetsHandler.mountRotFix)
assertAnglesApproxEqual(
val expectedYaw = radYaw(expected)
val resultYaw = radYaw(tracker.resetsHandler.mountRotFix)
assertAngleEquals(
expectedYaw,
resultYaw,
"Resulting mounting yaw after full reset is not equal to reference yaw (${deg(expectedYaw)} vs ${deg(resultYaw)})",
message = "Resulting mounting yaw after full reset is not equal to reference yaw (${radToDeg(expectedYaw)} vs ${radToDeg(resultYaw)})",
)

tracker.setRotation(reference * reference)
tracker.resetsHandler.resetYaw(reference)

val expectedYaw2 = yaw(reference)
val resultYaw2 = yaw(tracker.getRotation())
assertAnglesApproxEqual(
val expectedYaw2 = radYaw(reference)
val resultYaw2 = radYaw(tracker.getRotation())
assertAngleEquals(
expectedYaw2,
resultYaw2,
"Resulting rotation after yaw reset is not equal to reference yaw (${deg(expectedYaw2)} vs ${deg(resultYaw2)})",
message = "Resulting rotation after yaw reset is not equal to reference yaw (${radToDeg(expectedYaw2)} vs ${radToDeg(resultYaw2)})",
)
}
}
231 changes: 231 additions & 0 deletions server/core/src/test/java/dev/slimevr/unit/ResetTheoryTests.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,231 @@
package dev.slimevr.unit

import com.jme3.math.FastMath
import dev.slimevr.unit.TrackerTestUtils.assertAngleEquals
import dev.slimevr.unit.TrackerTestUtils.assertQuatEquals
import dev.slimevr.unit.TrackerTestUtils.assertQuatNotEquals
import dev.slimevr.unit.TrackerTestUtils.quatApproxEqual
import io.github.axisangles.ktmath.EulerAngles
import io.github.axisangles.ktmath.EulerOrder
import io.github.axisangles.ktmath.Quaternion
import org.junit.jupiter.api.DynamicTest
import org.junit.jupiter.api.TestFactory
import kotlin.math.abs
import kotlin.test.assertEquals
import kotlin.test.assertNotEquals

// These technically don't test any implemented code, but serves as a definitive proof
// and learning material for our "session calibration" math and logic.
class ResetTheoryTests {
@TestFactory
fun makeRawOrientationTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
roughAttitude.flatMap { aA ->
roughHeading.map { hA ->
DynamicTest.dynamicTest(
"testMakeOrientation( hC: $hC, aA: $aA, hA: $hA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testMakeRawOrientation(Quaternion.IDENTITY, hC, aA, hA)
}
}
}
}

/**
* We're trying to prove stuff here using this, so let's at least prove that we can
* make a raw orientation and then bring it back to the bone frame of reference.
*/
fun testMakeRawOrientation(
boneOrientation: Quaternion,
headingCorrect: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
) {
val rawOrientation = makeRawOrientation(boneOrientation, headingCorrect, attitudeAlign, headingAlign)
val newBoneOrientation = headingCorrect * rawOrientation * attitudeAlign * headingAlign
// Now that we re-applied the calibrations, let's see if it matches!
assertQuatEquals(boneOrientation, newBoneOrientation)
}

@TestFactory
fun headingCorrectTimingTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
roughAttitude.flatMap { aA ->
roughHeading.map { hA ->
DynamicTest.dynamicTest(
"testHeadingCorrectTiming( hC: $hC, aA: $aA, hA: $hA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testHeadingCorrectTiming(Quaternion.IDENTITY, hC, aA, hA)
}
}
}
}

/**
* It doesn't actually matter *when* you add heading correction, just as long as
* it's on the left side.
*/
fun testHeadingCorrectTiming(
rawOrientation: Quaternion,
headingCorrect: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
) {
val boneOrientationA = headingCorrect * rawOrientation * attitudeAlign * headingAlign
val boneOrientationB = headingCorrect * (rawOrientation * attitudeAlign * headingAlign)
val boneOrientationC = headingCorrect * (rawOrientation * attitudeAlign) * headingAlign
assertQuatEquals(boneOrientationA, boneOrientationB)
assertQuatEquals(boneOrientationA, boneOrientationC)
}

@TestFactory
fun headingCorrectAttitudeAlignTests(): List<DynamicTest> = roughHeading.flatMap { hC ->
roughAttitude.map { aA ->
DynamicTest.dynamicTest(
"testHeadingCorrectAttitudeAlign( hC: $hC, aA: $aA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testHeadingCorrectAttitudeAlign(Quaternion.IDENTITY, hC, aA)
}
}
}

/**
* Heading correction also does not affect the calculation of the attitude alignment
* using euler angles.
*/
fun testHeadingCorrectAttitudeAlign(
rawOrientation: Quaternion,
headingCorrect: Quaternion,
attitudeAlign: Quaternion,
) {
val boneOrientationA = (rawOrientation * attitudeAlign).toEulerAngles(EulerOrder.YZX)
val boneOrientationB = (headingCorrect * rawOrientation * attitudeAlign).toEulerAngles(EulerOrder.YZX)
assertAngleEquals(boneOrientationA.x, boneOrientationB.x, FastMath.ZERO_TOLERANCE)
assertAngleEquals(boneOrientationA.z, boneOrientationB.z, FastMath.ZERO_TOLERANCE)
// We can also show that we're calculating the right attitude alignment.
val attitudeAlignEul = attitudeAlign.toEulerAngles(EulerOrder.YZX)
assertAngleEquals(attitudeAlignEul.x, boneOrientationA.x, FastMath.ZERO_TOLERANCE)
assertAngleEquals(attitudeAlignEul.z, boneOrientationA.z, FastMath.ZERO_TOLERANCE)
}

@TestFactory
fun attitudeHeadingAlignDependenceTests(): List<DynamicTest> {
// Order doesn't matter if the attitude alignment has no attitude.
return roughAttitude.filterNot { FastMath.isApproxZero(it.x) && FastMath.isApproxZero(it.z) }.flatMap { aA ->
// Same for if heading alignment is the quaternion identity.
roughHeading.filterNot { quatApproxEqual(it, Quaternion.IDENTITY) }.map { hA ->
DynamicTest.dynamicTest(
"testAttitudeHeadingAlignDependence( aA: $aA, hA: $hA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testAttitudeHeadingAlignDependence(Quaternion.IDENTITY, aA, hA)
}
}
}
}

/**
* It *does* matter what order you apply attitude and heading alignment.
*/
fun testAttitudeHeadingAlignDependence(
rawOrientation: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
) {
val boneOrientationA = rawOrientation * attitudeAlign * headingAlign
val boneOrientationB = rawOrientation * headingAlign * attitudeAlign
assertQuatNotEquals(boneOrientationA, boneOrientationB)
}

@TestFactory
fun attitudeHeadingAlignOrderTests(): List<DynamicTest> {
// We're not proving anything if both attitude axes are of equal magnitude.
return roughAttitude.filterNot { FastMath.isApproxEqual(abs(it.x), abs(it.z)) }.flatMap { aA ->
roughHeading.map { hA ->
DynamicTest.dynamicTest(
"testAttitudeHeadingAlignOrder( aA: $aA, hA: $hA )",
) {
// We can just use identity for the target orientation as only the
// calibration quaternions themselves matter.
testAttitudeHeadingAlignOrder(Quaternion.IDENTITY, aA, hA)
}
}
}
}

/**
* If we want to modify heading alignment but keep a constant attitude alignment,
* we need to apply heading alignment *after* attitude.
*/
fun testAttitudeHeadingAlignOrder(
rawOrientation: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
) {
// Perpendicular heading alignment (rotated by 90 deg), makes it easy to check
// our results.
val headingAlignB = headingAlign * Quaternion.rotationAroundYAxis(FastMath.HALF_PI)

// We must also apply an inverse of the heading alignment to make our
// quaternions comparable; we just want to affect the axes, not add to the
// orientation.
val boneOrientationA = headingAlign.inv() * (rawOrientation * attitudeAlign * headingAlign)
val boneOrientationB = headingAlignB.inv() * (rawOrientation * attitudeAlign * headingAlignB)
assertEquals(abs(boneOrientationA.x), abs(boneOrientationB.z), FastMath.ZERO_TOLERANCE)
assertEquals(abs(boneOrientationA.z), abs(boneOrientationB.x), FastMath.ZERO_TOLERANCE)

// Since it's required for this test, we can also show that by applying the
// inverse of heading alignment as a heading correction, we can retain the same
// heading orientation despite changing alignment. By doing this, we remove
// dependence between correction and alignment; they can resolve to definitive
// values.
assertEquals(boneOrientationA.y, boneOrientationB.y, FastMath.ZERO_TOLERANCE)

// We can also show that this does not work when heading alignment comes before
// attitude alignment.
val boneOrientationC = headingAlign.inv() * (rawOrientation * headingAlign * attitudeAlign)
val boneOrientationD = headingAlignB.inv() * (rawOrientation * headingAlignB * attitudeAlign)
assertNotEquals(abs(boneOrientationC.x), abs(boneOrientationD.z), FastMath.ZERO_TOLERANCE)
assertNotEquals(abs(boneOrientationC.z), abs(boneOrientationD.x), FastMath.ZERO_TOLERANCE)
}

companion object {
// 5 steps
val roughStep = -180..180 step 72

// 12 steps
val fineStep = -180..180 step 30

// Will not work when we don't know the heading correction or heading alignment,
// we will need euler angles to calculate those, and it needs to sacrifice one
// axis for euler angles to work
val roughAttitude = roughStep.flatMap { x ->
roughStep.map { z ->
EulerAngles(EulerOrder.YZX, x * FastMath.DEG_TO_RAD, 0f, z * FastMath.DEG_TO_RAD).toQuaternion()
}
}

val roughHeading = roughStep.map { y ->
Quaternion.rotationAroundYAxis(y * FastMath.DEG_TO_RAD)
}

/**
* Reverse calibration to get a raw orientation from a bone orientation.
*/
// We reverse the order of headingAlign and attitudeAlign here since our
// attitude alignment is within the raw heading frame of reference, so we must
// bring the orientation back into that frame of reference first. Whatever is
// applied last must be taken off first.
fun makeRawOrientation(
boneOrientation: Quaternion,
headingCorrect: Quaternion,
attitudeAlign: Quaternion,
headingAlign: Quaternion,
): Quaternion = headingCorrect.inv() * boneOrientation * headingAlign.inv() * attitudeAlign.inv()
}
}
12 changes: 6 additions & 6 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 @@ -3,7 +3,7 @@ package dev.slimevr.unit
import com.jme3.math.FastMath
import dev.slimevr.tracking.processor.HumanPoseManager
import dev.slimevr.tracking.trackers.TrackerPosition
import dev.slimevr.unit.TrackerTestUtils.assertAnglesApproxEqual
import dev.slimevr.unit.TrackerTestUtils.assertAngleEquals
import dev.slimevr.unit.TrackerTestUtils.quatApproxEqual
import io.github.axisangles.ktmath.EulerAngles
import io.github.axisangles.ktmath.EulerOrder
Expand Down Expand Up @@ -57,8 +57,8 @@ class SkeletonResetTests {
hpm.resetTrackersYaw(resetSource)

for (tracker in trackers.set) {
val yaw = TrackerTestUtils.yaw(tracker.getRotation())
assertAnglesApproxEqual(0f, yaw, "\"${tracker.name}\" did not reset to the reference rotation.")
val yaw = TrackerTestUtils.radYaw(tracker.getRotation())
assertAngleEquals(0f, yaw, message = "\"${tracker.name}\" did not reset to the reference rotation.")
}
}

Expand Down Expand Up @@ -104,9 +104,9 @@ class SkeletonResetTests {
val actualMounting = tracker.resetsHandler.mountRotFix

// Make sure yaw matches
val expectedY = TrackerTestUtils.yaw(expectedMounting)
val actualY = TrackerTestUtils.yaw(actualMounting)
assertAnglesApproxEqual(expectedY, actualY, "\"${tracker.name}\" did not reset to the reference rotation.")
val expectedY = TrackerTestUtils.radYaw(expectedMounting)
val actualY = TrackerTestUtils.radYaw(actualMounting)
assertAngleEquals(expectedY, actualY, message = "\"${tracker.name}\" did not reset to the reference rotation.")

// X and Z components should be zero for mounting
assert(FastMath.isApproxZero(actualMounting.x)) {
Expand Down
Loading